PID算法优化（以平衡小车代码为例）

                                                                PID控制

这篇文章是PID控制的进阶说明，如果没有看过PID控制（上）的读者，请先看看PID控制（上），以便更容易看懂这篇文章。关注微信公众号“电子搬砖师”即可阅读PID控制（上）。

文章分为两个部分讲解：PID算法优化，串级PID分析。

先讲PID算法优化，奉上最简单的角度PID算法：

以上PID算法是不是很简单？只要5行代码，只要调节P、I、D这3个系数，这个算法就可以让平衡车立起来！

我们现在开始对这最简单的算法做优化。

1、如果输入角度Angle是一个噪声比较大的值，可以对Angle做一个简单滤波，比如做个平滑滤波：

ErrorAngle=K*(ExpectAngle-Angle)+(1-K)* LastErrorAngle;

当然角度数据最好能在输入PID算法前就先做滤波了。

2、某些控制模型的误差绝对值大于一定值时可能需要停止运行，比如说平衡车倒地了，这时平衡车可能就不要再转轮了，不然可能产生意外。这时我们就可以判断平衡车角度误差绝对值是否大于限定值，如果大于的话我们就直接返回0，停止车轮滚动。如下：

if((abs)ErrorAngle>LimitAngle1) return 0;

3、如果误差绝对值大于一定值，积分项就会累积到很大的积分值，太大的积分值会使得系统更难控制，所以我们可以判断误差绝对值是否超标，过大的话就不积分，合规的话就可以积分，如下：

if((abs)ErrorAngle

4、以上还有一种处理积分的方式叫积分分离法，它指的是如果误差绝对值大于一定值，积分项干脆直接清零。这样做的好处是可以使控制模型更快回复到平衡状态，或者避免加剧控制模型的失控，如下：

if((abs)ErrorAngle>LimitAngle2) InteAngle=0;

5、由于过大的积分值会使系统难以控制或者回归稳定太慢，我们需要对积分做限制幅度处理，如下：

if(InteAngle>LimitInteAngle) InteAngle=LimitInteAngle;

if(InteAngle<-LimitInteAngle) InteAngle=-LimitInteAngle;

6、微分项往往是噪声比较大的项，如果微分噪声大我们也最好加个平滑滤波。本例中的角度微分项其实就是角速度，它可以用本次角度误差减去上次角度误差获得，也可以直接用陀螺仪测量得到的角速度。假设我们用的是前者，那对前者微分滤波如下：

DiffAngle=J*(ErrorAngle-LastErrorAngle)+(1-J)*LastDiffAngle;

7、执行机构的死区指的是PID算法给出一个控制数值到执行机构，执行机构可能会因为输入值较小而没反应。比如你给电机1V的电压，可能电机根本不会动，直到你给3V电压时，它才会动。这个3V电压就是该电机的死区。

死区通常可以用PID的积分项消除掉，但是有的地方不想用积分累积那么久才越过死区值，就可以在PID输出值上直接加上死区，如下：

PID_Out=P*ErrorAngle+I*InteAngle+D*DiffAngle;

if(PID_Out>0)        PID_Out+=DeadValue;

else if(PID_Out<0)  PID_Out+=-DeadValue;

8、有的执行机构没法接收过大的PID输出值，比如PWM占空比最大只能100%，而如果PID输出的值大于这个PWM上限，那就要将PID输出值限幅，如下：

if(PID_Out>LimitPID) PID_Out=LimitPID;

if(PID_Out

9、高深点的PID的P、I、D三个系数还可以不是固定的，而是随着误差值的变化而变化的，这样就可以实现将非线性控制模型给线性控制住。这种方式我也没有用过，不过它的简单实现方式可以列出，还是应用本例，有：

P*f(ErrorAngle)*ErrorAngle+I*g(ErrorAngle)*InteAngle+D*h(ErrorAngle)*DiffAngle;

大概的PID算法优化以上都已经给出了，读者不用把每一项优化都添加到自己的算法中。毕竟每个控制模型都不一样，都有自己各自需要优化的地方，不要拘泥于此。

现在开始讲串级PID，奉上串级PID的框图：

其中一个圈带+-号的是误差比较机构，E(S)是误差，G(S)是被控对象，H(S)是反馈机构，左边的PID属于外环，右边的PID属于内环。

回归到上一篇PID控制（上）中，那篇文章里面提到推箱子，箱子距离目的地越远就应该用越大的力气推，这个说法我当时是为了简单不绕提出的，但实际上是不对的。推力大会使得加速度大，如果摩擦力比较小，到达终点时推力为0只是说加速度为0，速度可能是非常大的，这个是不对的。真正正确的是距离和速度成比例，距离越远，速度越大，距离为0，速度为0，即推箱子的人应该控制的是速度而不是推力。

而速度要怎么控制呢？这就要靠串级PID算法吃饭了。

串级PID在无人机上用得很多，它具有比单级PID控制更稳定，更顺滑的优点。它和单级PID控制的区别是：无人机单级PID是直接计算输入角度和反馈角度的误差，然后利用PID算法直接算出PWM值控制电机转动；而串级PID是先计算输入角度和反馈角度的误差，然后利用外环PID算法计算出在此角度误差下无人机的角速度最应该是多少，接着再计算外环PID输出的角速度和无人机当前角速度的误差，将此误差再送入控制角速度的内环PID算法中计算，最终内环PID输出一个PWM值控制电机转动。

串级PID中的内环控制频率可能会比外环控制频率高，因为外环给内环一个角速度值，内环不可能瞬间达到这个角速度，它必须经过几个周期的调整才可能达到这个角速度值。但是很多无人机内外环控制频率是一致的，这个也没什么问题，因为虽然内环没法及时达到外环的要求，但是却是往外环要求的趋势走的，最终无人机姿态还是会稳定的。

前面提到积分是消除静差用的，那这里无人机外环传递给内环的只是一个角速度，而内环利用自己的PID算法会负责将这个角速度实现出来，那这样外环输出的只是一个数字而已，哪怕是0.01度/秒，内环也会帮它实现，这就不该存在静差了，为什么外环要加个积分呢？

这里要说明积分的另一个作用，就是减小调节时间。假设比例项是5，角度误差是2度，纯比例控制输出的值会是10度/秒，但是如果有积分项存在，输出就会是：5*2+I*积分，这个值会使角速度得到更快的变化。如果将外环的P和I系数调到一个合适值，虽然被控对象达到设定值时会过冲，然后反冲，如此震荡几次再稳定，但这个过程还是可以比仅有P系数的外环调节更快达到设定值。

以下是串级PID算法：

    /**************************角度环开始********************/

     //当前角度误差

     Error_Angle_roll = (Desire_angle_roll_SUM - w_and_angle.angle_roll);

     Error_Angle_pitch = (Desire_angle_pitch_SUM - w_and_angle.angle_pitch);

    

     Error_Angle_roll_I += Error_Angle_roll;

     Error_Angle_pitch_I += Error_Angle_pitch;

    

     //积分限幅

     if( Error_Angle_roll_I > 500 )

       Error_Angle_roll_I = 500;

     if( Error_Angle_roll_I < -500 )

       Error_Angle_roll_I = -500;

     if( Error_Angle_pitch_I > 500 )

       Error_Angle_pitch_I = 500;

     if( Error_Angle_pitch_I < -500 )

       Error_Angle_pitch_I = -500;

     /***********************进入速度环*************************/

     //外环的P 和 I产生期望角速度，作为角速度环的输入

     Desire_W_roll  = (Error_Angle_roll)*rollP_EXC + Error_Angle_roll_I*rollI_EXC;

     Desire_W_pitch = (Error_Angle_pitch)*pitchP_EXC + Error_Angle_pitch_I*pitchI_EXC;

    

     //由外环的P和I得到的输出减去此时的角速度，从而得到角速度误差

     Error_W_roll = Desire_W_roll - w_and_angle.w_roll;

     Error_W_pitch = Desire_W_pitch - w_and_angle.w_pitch;

    

     Error_W_roll_I += Error_W_roll;

     Error_W_pitch_I += Error_W_pitch;

      

     //积分限幅

     if( Error_W_roll_I > 500 )

       Error_W_roll_I = 500;

     if( Error_W_roll_I < -500 )

       Error_W_roll_I = -500;

     if( Error_W_pitch_I > 500 )

       Error_W_pitch_I = 500;

     if( Error_W_pitch_I < -500 )

       Error_W_pitch_I = -500;

    

     Now_W_roll_Err = Error_W_roll;

     Now_W_pitch_Err = Error_W_pitch;

     //速度环使用了PID

     PID_roll_out = -Error_W_roll*rollP_IN + Error_W_roll_I*rollI_IN

- (Now_W_roll_Err - Pre_W_roll_Err)*rollD_IN;

     PID_pitch_out = -Error_W_pitch*pitchP_IN + Error_W_pitch_I*pitchI_IN

- (Now_W_pitch_Err - Pre_W_pitch_Err)*pitchD_IN;

    

     //更新角速度误差

     Pre_W_roll_Err = Now_W_roll_Err;

     Pre_W_pitch_Err = Now_W_pitch_Err;

 

附一个PID参数整定的帖子，写得挺好的：

http://www.amobbs.com/thread-5554367-1-1.html