PID算法原理及模板讲解

很早都想写一栏关于PID算法的专栏，整个大学期间把谈恋爱的时间都拿来搞PID算法了（这样你们还不信我是真的搞PID的嘛。。）。为了学习PID算法买过平衡小车之家的平衡车（最后拆成玩具了），买过正点原子的minifly（卖掉了）。做过2015年电赛风力摆，研究过2017年电赛板球系统（机械结构太难搭了就写了写代码），准备了这么久就是为了在电赛上拿个国奖，结果2019年电赛把控制类和电源类结合到了一起（电磁炮），而且控制方式很幼稚，完全不用闭环控制。最后折腾了好几天做了个残次品，也是自己学艺不精，拿了个省一（河南赛区）。

出来实习接手的第一个项目是扫地机器人，也是利用PID驱动，结合上层H6芯片，LSLAM算法和路径规划，去完成室内导航的目的，冥冥之中自有天意~以前一直不写是怕自己写不好，现在随便了，就当是记录一下自己的PID学习历程，希望对需要的人有所帮助。

什么是PID算法？

本来不想写这一步的，感觉每个讲PID的都来一遍，有点啰嗦。但是想到我刚开始学PID的时候，也是对着什么是PID看了半天。这里分享一下自己的理解吧。

一个人在操场跑步，在起点开始跑，在终点停下。为什么人能停在终点呢？凭感觉吗？更多的还是用眼睛看吧，当眼睛看见终点线，给脑子说，“快到啦快到啦”，脑子给腿说，“慢点儿慢点儿”，腿慢了下来，到终点线的时候刚好停下。

这其中就蕴含了PID算法的思想，传感器采集数据（眼睛），反馈给MCU（大脑），大脑判断距离终点线的距离越来越近（误差越来越小），告诉腿（执行机构）快到了慢一点，腿（执行机构做出反应）慢了下来，到终点刚好停下（无超调量）。

这就是一个最基本的PID模型，无论其他多么复杂的PID算法，本质上都是基于这个模型来做的拓展和延申。官方一点儿就是，PID算法是对误差的处理，使误差逐渐趋近于0的一种算法。换句话说，就是PID算法会使得系统向期望的方向发展，这句话比较绕口，但是确实是对PID比较好的一个总结了。

P <—> 比例控制<—>对当前状态的处理<—>提高响应速度，过大则无静差
I <—> 微分控制<—>对过去状态的处理<—>用于减小静差
D <—> 积分控制<—>对将来状态的预测<—>用于抑制震荡

判断PID算法优劣的三种标准

①.最大超调量：响应曲线最大峰值与稳态值的差，是系统稳定性的重要标准
②.上升时间：响应曲线从原始状态出发，第一次到达输出稳态值所需的时间，是评估系统快速性的一个重要标准
③.静差：是被控量的稳定值与给定值之差，一般用于衡量系统的准确性。
本来想网上作图给大家看，嫌麻烦，直接上我手写的笔记：

可以看到当我们改变了PID的参数时，系统的响应速度和响应时间都会有所变换，所谓PID调参，就是找到一条最符合自己所需系统的曲线，仅此而已。顺着这个图，讲一下I环和D环的作用把。
P环很好理解，我们将误差乘以P值加在执行机构上面，当误差越小时P环所得值也就越小，给到执行机构的值也就越小，执行机构反应也就越慢，很符合这种闭环反馈的思想。但是当有一种情况，误差很小的时候P环得出的值给到执行机构上面不足以驱动执行机构工作，这时系统距离期望值仍有一段距离，该距离便被称为静差。
I环的作用是对过去状态的累加，本质上是对误差的积分，当系统存在静差时，I环会持续累加静差值，直到累加值大到足以驱动执行机构工作。这是I环的优点，可以有效地处理静差。但是I环本质上是对过去状态的处理，所以加上I环以后整个系统会变得有些不可控制，这里可以采用积分分离式的思想，只有当误差小于一定范围才开始进行进行误差的累加。如果是对稳定性要求比较高的系统，比如平衡小车直立环，就尽量不要用I环，单PD控制器控制即可。
D环，这个环比较的神奇，它可以预测将来，本质上是对误差的微分。也可以理解成对系统的阻尼，打个比方，在一个理想的环境下有个单摆一直在摆动，它摆动的波形是个正弦波形。如果不加阻尼那么它便可以一直摆动，但是现实中总会有各种各样的阻尼作用迫使单摆运动停下来。D环起的就是这个作用。D环一般运用在角度环比较多，因为你不可以进入弯道才想到转弯，而是在进入弯道之前就要有这个转弯的意识。做智能车的小伙伴们注意了！！

PID调参

PID调参其实蛮看经验的，并没有一个万能的标准或者公式，调参之前我们需要根据自己的系统估计参数大概的范围，从P开始调起。比如我们用PWM控制电机那么PWM的范围肯定知道吧，传感器采集数据一般是编码器，每隔一段时间对编码器采样一次便可以得到速度，得到的速度和期望的速度之间的差值我们也能估计对吧，算个大概让P环乘以误差得出的数足够驱动电机就够了。然后根据现象，先找对趋势，震荡太大就减小P值，响应太慢就增加P值。最终取到一个让系统有向期望方向变化的趋势，并且震荡在可接受的范围之内的P值就好了。
然后开始上I环，I环是对过去状态的处理，当系统的期望（希望传感器达到的值）与实际（传感器采集的值）过大时，I环的值很容易偏大，导致系统过冲，所以用I环我们一般会做一个积分分离式，即只有当误差小于一定的值，我们才开始累加偏差，否则清零即可。如果是做平衡车四轴之类的，I一般取P的1/200就好了，别的系统自己慢慢测试，找到一个比较理想的状态即可。注意上I环的时候可以把P环乘以0.8，经验之谈！
PI控制器可以满足大部分的控制系统了，并不是非要三个环全上才是好的。对于D环如超调量果我们对响应速度没有太大的要求，希望最大超调量尽可能地小或者没有，可以上个D环，响应慢点儿但是准确。
最后说一点，限幅是个好习惯，对积分累加限幅，对输出限幅都可以增加系统的稳定性，防止过冲等不良后果。

PID模板!!!

说了这么多，连个公式也没上。。其实是懒得操作，这里给大家一个更加福利的方式，直接上代码，做成了模板样式，改成自己需要的样式就能用。
位置式PID，即最原始的PID算法，做定量控制非常好用（比如控制直流减速电机旋转90°，舵机+陀螺仪做个摄像仪自稳器）代码如下:

#include "control.h"//先定义采集到的变量，一般是传感器采集的数据，用于误差的采集值填充。
float roll,pitch,raw;//在定义位置PID算法需要用到的变量。(variable是你自己采集的数据)
float err_variableA（e(k)）,err_variableA_prev（e(k)-1）,err_variableA_last(e(k)-2);
float kp_variableA,ki_variableA,kd_variableA;
float err_variableA_sum;
float PWMA;float err_variableB（e(k)）,err_variableB_prev（e(k)-1）,err_variableB_last(e(k)-2);
float kp_variableB,ki_variableB,kd_variableB;
float err_variableB_sum;
float PWMB;
......//定义我们的PID设定值。（即用测量值减去设定值，即为我们需要的err。）
u8 Set_variableA_angle = 0;
u8 Set_variableB_angle = 0;//在中断里面，我们去选择任务（通过蓝牙模块。）
int ****_IRQHandler(void) （可以为线中断，如MPU6050，也可以如定时器中断。）
{if(EXTI_GetITStatus(EXTI_Line4) != RESET){    TASK1_Start(); }EXTI_ClearITPendingBit(EXTI_Line4);
}void TASK1_Start()
{kp_variableA = 10 , ki_variableA = 0.01 , kd_variableA = 100;err_variableA = pitch - Set_variableA_angle ;err_variableA_sum += err_variableA ;if( err_variableA_sum  >  5000 ) err_variableA_sum  =  5000;if( err_variableA_sum  < -5000 ) err_variableA_sum  = -5000;PWMA = kp_variable * err_variableA +  ki_variable * err_variableA_sum + kd_variable * (err_variableA - err_variableA_prev);if(PWMA >  800) PWMA =   800;if(PWMA < -800) PWMA = - 800;err_variableB_last = err_variableA_prev ;err_variableA_prev = err_variableA;Motor1_Start(PWMA);//这一步就是看这个位置式PID的值是给到哪个驱动/电机上面，去操控元器件了。
}

这里因为我是是做风力摆时写的模板，直接用了MPU6050的自己的中断（根据采样率大概是5ms产生一次线中断），大家的工程里如果上RTOS的话可以开个任务扫描，没有的话就直接开个定时器。
为什么要这样做呢？是利用了高采样率把离散的数字信号趋近于模拟信号给到执行机构，这样做的话可以保证系统的连续性和稳定性。
整体的控制光看TASK1_Start()这个任务就好了，我在中断里加入了蓝牙的扫描，利用蓝牙改变期望去改变系统的运行状况。
在Motor1_Start（）这个任务里面其实就是改变PWM的占空比从而改变电机的转速，还要做一个正负的判断改变电机旋转的方向。

增量式PID，增量式PID是在原始的位置式PID数学推导过来的，它一般用在控制电机以恒定转速前进，反映到传感器上就是编码器的值能保持恒定，并且被外界影响较小。

float kp_variableA,ki_variableA,kd_variableA;
float err_variableA_sum;
float PWMA;
float err_variableB（e(k)）,err_variableB_prev（e(k)-1）,err_variableB_last(e(k)-2);
float kp_variableB,ki_variableB,kd_variableB;
float err_variableB_sum;
float PWMB;
......
//定义我们的PID设定值。（即用测量值减去设定值，即为我们需要的err。）
u8 Set_variableA_angle = 0;
u8 Set_variableB_angle = 0;u8 a = 0,b = 0,c = 0,d = 0,e = 0;//给蓝牙用的
extern u8 Question_Flag;//在中断里面，我们去选择任务（通过蓝牙模块。）
int ****_IRQHandler(void) （可以为线中断，如MPU6050，也可以如定时器中断。）
{if(EXTI_GetITStatus(EXTI_Line4) != RESET){TASK1_Start();}EXTI_ClearITPendingBit(EXTI_Line4);
}void TASK1_Start()
{kp_variableA = 10 , ki_variableA = 0.01 , kd_variableA = 100;err_variableA = pitch - Set_variableA_angle ;err_variableA_sum += err_variableA ;Sif( err_variableA_sum  >  5000 ) err_variableA_sum  =  5000;if( err_variableA_sum  < -5000 ) err_variableA_sum  = -5000; PWMA += kp_variableA * (err_variableA - err_variableA_prev) + ki_variableA * err_variableA + kd_variableA * (err_variableA - 2 * err_variableA_prev + err_variableA_last)if(PWMA >  800) PWMA =   800;if(PWMA < -800) PWMA = - 800；err_variableA_last = err_variableA_prev ;err_variableA_prev = err_variableA;Motor1_Start(PWMA);//这一步就是看这个增量式PID的值是给到哪个驱动/电机上面，去操控元器件了。
}

这就是增量式PID的模板，具体的数学推到其实挺简单的，有兴趣可以了解下。早PWMA中我们可以看到，增量式PID和本次误差，上次误差，上上次误差都有关系，所以增量式PID的系统更不容易被外界所干扰，能保持一种惯性。

串级PID，串级PID的本质是将一个环的输出作为另一个环的期望，这样的话可以保证内环的稳定性。串级PID一般用在两个环具有相关性的场景，比如说平衡小车或者四轴，改变其速度(外环)，那么我的倾斜角度（内环）的期望也会随之改变，整个系统还是在一个稳定的情况下前进后退左转右转。如果两个环没什么太大的关联性，并不建议用串行PID，就算调了个勉强能用的参也没什么意义。
下面上模板：

#include "control.h"
//因为我们要用两个环，外环为速度环，内环为角度环，速度环由编码器M法测出赋值给speed
//内环由mpu6050测出数据进行数据融合得出角度，从而得出倾斜角度赋值给angle
float speed,angle;
//定义一些PID算法需要用到的参数
float kp_speed,ki_speed,kd_speed;
float kp_angle,ki_angle,kd_angle;float err_speed,err_speed_prev,err_speed_last,err_speed_sum;
float err_angle,err_angle_prev,err_angle_last,err_angle_sum;float PWM_speed,PWM_angle;
u8 Set_speed_value = 0,Set_angle_value = 0;
//然后先写对应的中断函数（线中断/定时器中断）
u8 a = 0,b = 0,c = 0,d = 0,e = 0;extern u8 Question_Flag;int ****_IRQnHandler(void)
{if(****_GetITStatus(****_Line*) != RESET){TASK1_Start();}****_ClearITPendingBit(****_Line*);
}
//先写外环速度环PID控制函数
float Speed_PID()
{kp_speed = 10,ki_speed = 0.1,kd_speed = 100;err_speed = speed - Set_speed_value;err_speed_sum += err_speed;if(err_speed_sum >  5000) err_speed_sum =  5000;if(err_speed_sum < -5000) err_speed_sum = -5000;PWM_speed = kp_speed * err_speed + ki_speed * err_speed_sum + kd_speed * (err_speed - err_speed_last);if(PWM_speed >  800)  PWM_speed =  800;if(PWM_speed < -800)  PWM_speed = -800；err_speed_last = err_speed_prev;err_speed_prev = err_speed;return PWM_speed;
}
//在写内环角度环PID，注：
void TASK1_Start()
{float err_out_speed;kp_angle = 30,ki_angle = 0.01,kd_angle = 500;err_out_speed = Speed_PID();err_angle = angle - err_out_speed;err_angle_sum += err_angle;if(err_angle_sum >  5000)err_angle_sum =  5000;if(err_angle_sum < -5000)err_angle_sum = -5000;PWM_angle = kp_angle * err_angle + ki_angle * err_angle_sum + kd_angle * (err_angle - err_angle_last);if(PWM_angle >  800)  PWM_angle =  800;if(PWM_angle < -800)  PWM_angle = -800;err_angle_last = err_angle_prev;err_angle_prev = err_angle;Motor1_Start(PWM_angle);
}

至此，三个常用的模板已经给大家介绍完毕了，学习PID光看是没用的，自己去构建个系统，调调参数什么的。我刚开始学的时候，拿了个陀螺仪（MPU6050）拿了个舵机，不到一天就调出来了一个比较完美的自稳系统，后面会把以前参加比赛或者做的项目代码放出来一起学习！