PID相关的知识以及其c++实现

先感谢fc师兄的博客：https://blog.csdn.net/qq_28773183/article/details/79524766

前言：因为最近看的px4的源码中的控制部分就用的是PID，所以想自己打算用c++写一下pid，顺便复习一下。在师兄的博客中还学到了一些pid的改进型。

一、传统的PID（位置式PID）

输入的是我们的输入值（期望值），将与实际值做偏差后传给pid部分，然后传给作动器（比如电机什么的），然后再通过传感器将实际值表示出来在反馈回来。

初始时，反馈的信号为0，偏差量就是输入量。

离散化公式：u(k)=Kp*err(k)+Ki * ∑err(j) T+ Kd (err(k)−err(k−T))/T

注意：这里的∑err(j)不是前一次的err+此刻的err，而是从第一时刻的err一直累加到现在

程序实现：

#include <iostream>

using namespace std;

struct pid
{
float pid_setspeed;      /****期望速度****/
float pid_actualspeed;   /****实际速度****/
float Kp;
float Ki;
float Kd;
float err;               /**偏差**/
float last_err;          /**上一次的偏差**/
float voltage;           /**作动器输出的电压**/
float T;                 /**更新周期**/
float integral;
}_pid;

void pid_init();
float pid_run(float speed);

int main()
{
int count;
int n =1;

cin >> count ;
pid_init();

while(n<count)
{
float output = pid_run(200.0);

cout << n<<"."<<"输
4000
出的值为：" << " " << output << endl;
n++;
}

return 0;
}

void pid_init()
{
_pid.pid_setspeed = 0.0;
_pid.pid_actualspeed = 0.0;
_pid.Kd = 0.2;
_pid.Kp = 0.2;
_pid.Ki = 0.015;
_pid.err = 0.0;
_pid.last_err = 0.0;
_pid.voltage = 0.0;
_pid.T = 1.0 ;
_pid.integral = 0.0;
}

float pid_run(float speed)
{
_pid.pid_setspeed = speed;
_pid.err = _pid.pid_setspeed - _pid.pid_actualspeed;
_pid.integral = _pid.integral + _pid.err ;
_pid.voltage = _pid.err*_pid.Kp + _pid.integral*_pid.T*_pid.Ki + (_pid.err-          _pid.last_err)*_pid.Kd;
_pid.last_err = _pid.err;
_pid.pid_actualspeed = _pid.voltage*1.0;

return _pid.pid_actualspeed;

}

输出：

二、增量式PID

其实我通过查资料发现增量式pid本质上就是位置式PID。

直接写公式：Δu(k)=Kp*(err(k)−error(k−1))+ Ki err(k) T+Kd *(err(k)−2err(k−1)+err(k−2))/ T

所以通过公式求的是本时刻与上一时刻的pid输出的差。

因此传给作动器的输入是上一时刻的u（k），然后加上这里算的Δu(k)。

注意：有人会问这个u（k）怎么算，因为公式中并没有算呀，其实我们有一个初始状态时的u（k）=0，他加上Δu(k)不就是第一次的u（k），以后不断加不就得出来了吗？

程序（注意把逻辑理清楚）：

#include <iostream>

using namespace std;

struct pid
{
float setspeed;      /****期望速度****/
float actualspeed;   /****实际速度****/
float Kp;
float Ki;
float Kd;
float err;               /**偏差**/
float last_err;          /**上一次的偏差**/
float last_last_err;     /**上上一次的偏差**/
float voltage;           /**作动器输出的电压**/
float last_voltage;      /**上一时刻的电压**/
float delta_voltage;     /**电压增量**/
float T;                 /**更新周期**/
float integral;
}_pid;

void pid_init();
float pid_run(float speed);
float pid_run2(float speed);

int main()
{
int count;
int n =1;

cin >> count ;
pid_init();

//while(n<=2)
//{
//   float output = pid_run(200.0);

//   cout << n<<"."<<"输出的值为：" << " " << output << endl;
//   n++;

//}

while(n<count)
{
float output = pid_run2(200.0);
cout << n<<"."<<"输出的值为：" << " " << output << endl;
n++;

}

return 0;
}

void pid_init()
{
_pid.setspeed = 0.0;
_pid.actualspeed = 0.0;
_pid.Kd = 0.2;
_pid.Kp = 0.2;
_pid.Ki = 0.015;
_pid.err = 0.0;
_pid.last_err = 0.0;
_pid.voltage = 0.0;
_pid.T = 1.0 ;
_pid.integral = 0.0;
_pid.last_voltage = 0.0;
_pid.delta_voltage = 0.0;
_pid.last_last_err = 0.0;
}

float pid_run(float speed)
{
_pid.setspeed = speed;
_pid.err = _pid.setspeed - _pid.actualspeed;
_pid.integral = _pid.integral + _pid.err ;
_pid.voltage = _pid.err*_pid.Kp + _pid.integral*_pid.T*_pid.Ki + (_pid.err-_pid.last_err)*_pid.Kd;

_pid.last_last_err = _pid.last_err;
_pid.last_err = _pid.err;
_pid.last_voltage = _pid.voltage;
_pid.actualspeed = _pid.voltage*1.0;

return _pid.actualspeed;

}

float pid_run2(float speed)
{
_pid.setspeed = speed;
_pid.err = _pid.setspeed - _pid.actualspeed;
_pid.delta_voltage = (_pid.err-2*_pid.last_err+_pid.last_last_err)*_pid.Kd + _pid.err*_pid.T*_pid.Ki + (_pid.err-_pid.last_err)*_pid.Kp;

_pid.last_last_err = _pid.last_err;
_pid.last_err = _pid.err;
_pid.voltage = _pid.last_voltage + _pid.delta_voltage;
_pid.last_voltage = _pid.voltage;
_pid.actualspeed = _pid.voltage*1.0;

return _pid.actualspeed；
}

程序中注释掉的部分就是我刚刚提的问题，我之前以为初始的u（k）应先通过位置式算，其实后来我把他删了也可以。

三、积分分离

在普通 PID 控制中,引入积分环节的目的,主要是为了消除静差,提高控制精度。但是在启动、结束或大幅度增减设定时,短时间内系统输出有很大的偏差,会造成 PID 运算的积分积累,导致控制量超过执行机构可能允许的最大动作范围对应极限控制量,从而引起较大的超调,甚至是震荡,这是绝对不允许的。为了克服这一问题,引入了积分分离的概念,其基本思路是 当被控量与设定值偏差较大时,取消积分作用; 当被控量接近给定值时,引入积分控制,以消除静差,提高精度。

代码：

#include <iostream>
#include <cmath>
using namespace std;

struct pid
{
float pid_setspeed;      /****期望速度****/
float pid_actualspeed;   /****实际速度****/
float Kp;
float Ki;
float Kd;
float err;               /**偏差**/
float last_err;          /**上一次的偏差**/
float voltage;           /**作动器输出的电压**/
float T;                 /**更新周期**/
float integral;
}_pid;

void pid_init();
float pid_run(float speed);

int main()
{
int count;
int n =1;

cin >> count ;
pid_init();

while(n<count)
{
float output = pid_run(200.0);

cout << n<<"."<<"输出的值为：" << " " << output << endl;
n++;
}

return 0;
}

void pid_init()
{
_pid.pid_setspeed = 0.0;
_pid.pid_actualspeed = 0.0;
_pid.Kd = 0.2;
_pid.Kp = 0.2;
_pid.Ki = 0.04;
_pid.err = 0.0;
_pid.last_err = 0.0;
_pid.voltage = 0.0;
_pid.T = 1.0 ;
_pid.integral = 0.0;
}

float pid_run(float speed)
{
_pid.pid_setspeed = speed;
_pid.err = _pid.pid_setspeed - _pid.pid_actualspeed;

if(abs(_pid.err)>180)
{

}
else
{
_pid.integral = _pid.integral + _pid.err ;
}

_pid.voltage = _pid.err*_pid.Kp + _pid.integral*_pid.T*_pid.Ki + (_pid.err-_pid.last_err)*_pid.Kd;

_pid.last_err = _pid.err;
_pid.pid_actualspeed = _pid.voltage*1.0;

return _pid.pid_actualspeed;

}

四、抗积分饱和

所谓的积分饱和现象是指如果系统存在一个方向的偏差,PID 控制器的输出由于积分作用的不断累加而加大,从而导致执行机构达到极限位置,若控制器输出 U(k)继续增大,执行器开度不可能再增大,此时计算机输出控制量超出了正常运行范围而进入饱和区。一旦系统出现反向偏差,u(k)逐渐从饱和区退出。进入饱和区越深则退出饱和区时间越长。在这段时间里,执行机构仍然停留在极限位置而不随偏差反向而立即做出相应的改变,这时系统就像失控一样,造成控制性能恶化,这种现象称为积分饱和现象或积分失控现象。防止积分饱和的方法之一就是抗积分饱和法,该方法的思路是在计算 u(k)时,首先判断上一时刻的控制量 u(k-1)是否已经超出了极限范围: 如果u(k−1)>umaxu(k−1)>umax则只累加负偏差; 如果 u(k−1)