pid调速 java

 一、理解PID

在大多数场合中，用“开关量”来控制一个物理量，就显得比较简单粗暴了。有时候，是无法保持稳定的。这时，就需要一种算法： 

• 它可以将需要控制的物理量带到目标附近
• 它可以“预见”这个量的变化趋势
• 它也可以消除因为散热、阻力等因素造成的静态误差

咱们知道，P，I，D是三种不同的调节作用，既可以单独使用（P，I，D），也可以两个两个用（PI，PD），也可以三个一起用（PID）。这三种作用有什么区别呢？

我们先只说PID控制器的三个最基本的参数：Kp,Ki,Kd

• Kp

P就是比例的意思。它的作用最明显，原理也最简单。我们先说这个：需要控制的量，比如水温，有它现在的『当前值』，也有我们期望的『目标值』。

• 当两者差距不大时，就让加热器“轻轻地”加热一下。
• 要是因为某些原因，温度降低了很多，就让加热器“稍稍用力”加热一下。
• 要是当前温度比目标温度低得多，就让加热器“开足马力”加热，尽快让水温到达目标附近。

这就是P的作用。实际写程序时，就让偏差（目标减去当前）与调节装置的“调节力度”，建立一个一次函数的关系，就可以实现最基本的“比例”控制了。Kp越大，调节作用越激进，Kp调小会让调节作用更保守。
要是你正在制作一个平衡车，有了P的作用，你会发现，平衡车在平衡角度附近来回“狂抖”，比较难稳住。如果已经到了这一步——恭喜你！离成功只差一小步了~

• Kd

D的作用更好理解一些，所以先说说D，最后说I。刚才我们有了P的作用。你不难发现，只有P好像不能让平衡车站起来，水温也控制得晃晃悠悠，好像整个系统不是特别稳定，总是在“抖动”。

你心里设想一个弹簧：现在在平衡位置上。拉它一下，然后松手。这时它会震荡起来。因为阻力很小，它可能会震荡很长时间，才会重新停在平衡位置。请想象一下：要是把上图所示的系统浸没在水里，同样拉它一下 ：这种情况下，重新停在平衡位置的时间就短得多。我们需要一个控制作用，让被控制的物理量的“变化速度”趋于0，即类似于“阻尼”的作用。因为，当比较接近目标时，P的控制作用就比较小了。越接近目标，P的作用越温柔。有很多内在的或者外部的因素，使控制量发生小范围的摆动。D的作用就是让物理量的速度趋于0，只要什么时候，这个量具有了速度，D就向相反的方向用力，尽力刹住这个变化。Kd参数越大，向速度相反方向刹车的力道就越强。
如果是平衡小车，加上P和D两种控制作用，如果参数调节合适，它应该可以站起来了~欢呼吧！等等，PID三兄弟好想还有一位。看起来PD就可以让物理量保持稳定，那还要I干嘛？因为我们忽视了一种重要的情况：

• Ki

以热水为例，假如有个人把我们的加热装置带到了非常冷的地方，开始烧水了。需要烧到50℃。在P的作用下，水温慢慢升高。直到升高到45℃时，他发现了一个不好的事情：天气太冷，水散热的速度，和P控制的加热的速度相等了。这可怎么办？

• P兄这样想：我和目标已经很近了，只需要轻轻加热就可以了。
• D兄这样想：加热和散热相等，温度没有波动，我好像不用调整什么。

于是，水温永远地停留在45℃，永远到不了50℃。作为一个人，根据常识，我们知道，应该进一步增加加热的功率。可是增加多少该如何计算呢？前辈科学家们想到的方法是真的巧妙。设置一个积分量。只要偏差存在，就不断地对偏差进行积分（累加），并反应在调节力度上。这样一来，即使45℃和50℃相差不太大，但是随着时间的推移，只要没达到目标温度，这个积分量就不断增加。系统就会慢慢意识到：还没有到达目标温度，该增加功率啦！到了目标温度后，假设温度没有波动，积分值就不会再变动。这时，加热功率仍然等于散热功率。但是，温度是稳稳的50℃。Ki的值越大，积分时乘的系数就越大，积分效果越明显。所以，I的作用就是，减小静态情况下的误差，让受控物理量尽可能接近目标值。I在使用时还有个问题：需要设定积分限制。防止在刚开始加热时，就把积分量积得太大，难以控制。

• 积分控制理解

PID控制器中的积分对应于下图中误差曲线 与坐标轴包围的面积（图中的灰色部分）。PID控制程序是周期性执行的，执行的周期称为采样周期。计算机的程序用下图中各矩形面积之和来近似精确的积分，图中的TS就是采样周期。每次PID运算时，在原来的积分值的基础上，增加一个与当前的误差值ev（n）成正比的微小部分。误差为负值时，积分的增量为负。

 积分运算示意图

二、PID公式及代码

PID系统分为两类：模拟PID（一般由硬件组成，比如电子原件）和数字PID（通过AD/DA，结合数字编程来实现）：

• 模拟PID：

• 数字PID：

位置式和增量式的主要区别：位置式在程序中要加积分上限，防止积分饱和；增量式对输出量做限制。

位置式

 

位置式代码

/*位置式PID算法，接口参数结构类型*/
typedef struct
{
    /*PID算法接口变量，用于给用户获取或修改PID算法的特性*/
    float Kp;     //比例系数
    float Ki;     //积分系数
    float Kd;     //微分系数
    float ErrorLim;//误差积分上限
    float ErrorNow;     //当前的误差
    float ControlOut;   //控制输出
    /*PID算法内部变量，其值不能修改*/
    float ErrorOld;
    float ErrorP;
    float ErrorI;
    float ErrorD;
} PID_AbsoluteType;
//右后轮控速
void PidSpeedControl_RightBack(void)
{
    if(Motion_State&0X09)PID_RightBack.ErrorNow = TargetSpeed1 - Speed_Right_Back;
    if(Motion_State&0X06)PID_RightBack.ErrorNow = TargetSpeed2 - Speed_Right_Back;
    PID_RightBack.Kp       = kp;
    PID_RightBack.Ki       = ki;
    PID_RightBack.Kd       = kd;
    PID_RightBack.ErrorLim = lim;
    PID_AbsoluteMode(&PID_RightBack);
    Right_Back_PWM = PID_RightBack.ControlOut;
}
//功能 ：位置式PID算法。公式 ：u(k) = Kp*e(k) + [Ki*e(k)+Pi(k-1)] + Kd*[e(k)-e(k-1)]
void PID_AbsoluteMode(PID_AbsoluteType* PID)
{
    if(PID->Kp < 0)               PID->Kp = -PID->Kp;
    if(PID->Ki < 0)                  PID->Ki = -PID->Ki;
    if(PID->Kd < 0)               PID->Kd = -PID->Kd;
    if(PID->ErrorLim < 0)   PID->ErrorLim = -PID->ErrorLim;
    PID->ErrorP = PID->ErrorNow;//读取现在的误差，用于Kp控制
    PID->ErrorI += PID->ErrorNow;//误差积分，用于ki控制
    if(PID->ErrorLim != 0)//微分上限和下限
    {
        if(PID->ErrorI > PID->ErrorLim)       PID->ErrorI = PID->ErrorLim;
        else if(PID->ErrorI < -PID->ErrorLim) PID->ErrorI = -PID->ErrorLim;
    }
    PID->ErrorD = PID->ErrorNow -PID->ErrorOld;//误差微分，用于Kd控制
    PID->ErrorOld = PID->ErrorNow;//保存现在的误差
    PID->ControlOut= PID->Kp * PID->ErrorP + PID->Ki * PID->ErrorI + PID->Kd * PID->ErrorD;//计算绝对式PID输出
}

增量式

 

增量式代码（这里没有设置输出上限）

/*增量式PID算法，接口参数结构类型*/
typedef struct 
{
 /*PID算法接口变量，用于给用户获取或修改PID算法的特性*/
 float kp;     //比例系数
 float ki;     //积分系数
 float kd;     //微分系数
 float errNow; //当前的误差
 float dCtrOut;//控制增量输出
 float  ctrOut;//控制输出
 /*PID算法内部变量，其值不能修改*/
 float errOld1;
 float errOld2;
}PID_IncrementType;
//左前轮控速
void PidSpeedControl_LeftFront(void)
{
    if(Motion_State&0X09)PID_LeftFront.errNow = TargetSpeed1 - Speed_Left_Front;
    if(Motion_State&0X06)PID_LeftFront.errNow = TargetSpeed2 - Speed_Left_Front;
    PID_LeftFront.kp       = kp;
    PID_LeftFront.ki       = ki;
    PID_LeftFront.kd       = kd;
    PID_IncrementMode(&PID_LeftFront);    //执行增量式PID算法
    Left_Front_PWM = PID_LeftFront.ctrOut;//读取控制值
}
//增量式PID算法
//公式：u(k) = u(k) + Kp*[e(k)-e(k-1)] + Ki*e(k) + Kd*[e(k)-2e(k-1)+e(k-2)]
void PID_IncrementMode(PID_IncrementType* PID)
{
 float dErrP, dErrI, dErrD;
 if(PID->kp < 0)  PID->kp = -PID->kp;
 if(PID->ki < 0)    PID->ki = -PID->ki;
 if(PID->kd < 0)  PID->kd = -PID->kd;
 dErrP = PID->errNow - PID->errOld1;
 dErrI = PID->errNow;
 dErrD = PID->errNow - 2 * PID->errOld1 + PID->errOld2;
 PID->errOld2 = PID->errOld1; //二阶误差微分
 PID->errOld1 = PID->errNow;  //一阶误差微分
 //增量式PID计算
 PID->dCtrOut = PID->kp * dErrP + PID->ki * dErrI + PID->kd * dErrD;
 if(PID->kp == 0 && PID->ki == 0 && PID->kd == 0)   PID->ctrOut = 0;
 else PID->ctrOut += PID->dCtrOut;
}

三、增量式PID计算公式的4个疑问与理解 

　PID就是对输入偏差进行比例积分微分运算，运算的叠加结果去控制执行机构。实践练习中，如何把这一原理转化为程序？为什么是用那几个error进行计算?

　　以下是我摘录的一段PID程序，我曾用其对智能车的速度进行闭环控制：

　　P：Proportional  比例

　　I：Integrating 积分

　　D：Differentiation 微分

　　Pwm_value：输出Pwm暂空比的值

　　Current_error：当前偏差  last_error：上次偏差   prev_error：上上次偏差

　　增量式PID计算公式： 

　　P=Kp*(current_error﹣last_error)；

　　D=Kd*(current_error﹣2*last_error﹢prev_error)；

　　I=Ki*current_error；

　　PID_add=Pwm_value+P﹢I﹢D；

一、为什么是PID_add=Pwm_value+（P﹢I﹢D）而不是PID_add=P+I+D？

如左图，有一个人前往目的地A，他用眼睛视觉传感器目测到距离目的地还有100m，即当前与目的地的偏差为100，他向双脚输出Δ=100J的能量，跑呀跑，10s之后，他又目测了一次，此时距离为40m，即current_error=40，他与10s前的偏差last_error=10对比，即current_error—last_error=—60，这是个负数，他意识到自己已经比较接近目的地，可以不用跑那么快，于是输出Δ=100+（—60）=40J的能量，40J的能量他刚好以4m/s的速度跑呀跑，10s之后，他发现已经到达目的点，此时current_error=0，大脑经过思考得出current_error—last_error=0—40=—40，两脚获得的能量Δ=40+（—40）=0，即他已经达到目的地，无需再跑。在刚才的叙述中，可知增量式P+I+D输出的是一个增量，将该增量与调节量相加后的到的才是最终输出量，P+I+D反应的是之前的输出量是在当前的状态中是该增加还是该减少。

 

 

 

二、纯比例控制P=Kp*(current_error﹣last_error)，怎样理解﹙current_error﹣last_error ﹚？

　　PID中纯比例控制就是把被控制量的偏差乘以一个系数作为调节器的输出，在增量式PID中，反映在程序上的，我们被控制量就是error，而实际上，例如在速度控制中error=目标速度﹣当前速度，所以明确目的：我们通过控制error趋近于0，最终使得当前速度趋近于目标速度。

如右图，假如考试时有这么一种题：函数经过时间Δt，由y1变化为y2时，问y增长的比例为多少？你很容易地得出答案：K=﹙y2-y1﹚/Δt；

 

以速度控制为例，若y为error，得右图，在时间t1到t2的过程中，我们可以得到输出控制量error变化的趋势为（current_error—last_error）/Δt。得到偏差的变化趋势后，乘以Kp使输出量与error相对变化。这个道理犹如模拟电子电路中，声音信号经过功放管放大输出的信号与输入信号相对应变化。

 

 

三、微分控制：

然而，通常情况下，我们的被控制量并非纯比例式地变化，如下图：

比例表示变化趋势，微分则表示变化趋势的变化率，映射到一个图像曲线中即为导数的变化！图3中若求曲线中x2至x1某点的斜率，当Δt足够小时，则可近似为（y2—y1）／Δt ，可知x3到x1导数的变化为﹛﹙y3—y2﹚—（y2—y1﹚﹜／Δt ＝﹙y3—2*y2﹢y1﹚／Δt 。将不同时间的y1、y2、y3映射为prev_error、last_error、current_error；则error变化趋势的变化为﹛﹙current_error—last_error﹚﹣﹙last_error—prev_error﹚﹜／Δt=﹛﹙current_error—2*last_error﹢prev_error﹚﹜／Δt，可得微分D=Kd*(current_error﹣2*last_error﹢prev_error)。 在系统中加入微分放映系统偏差信号的变化率，能预知偏差变化的趋势，具有超前控制作用，提前处理偏差。

 

 

 

 

四、积分控制：

积分控制可以消除偏差，体现在公式中较容易理解，当前的偏差差经过系数Ki的放大后映射为输出控制量，即I=Ki*current_error。P只要前后偏差之差为0，即current_error—last_current=0，则不进行调节，D只要前后偏差变化率为0，即(current_error﹣2*last_error﹢prev_error)=0，则不进行调节。而对于积分只要偏差存在，调节就始终进行，因此积分可以消除误差度，但在在某些情况下，一定范围内的误差是允许的，而如果此时积分调节始终存在，可能会导致系统稳定性下降，如右图，此时可通过弱化积分系数Ki使系统稳定。