In view of the importance of cascade PID in the pixhawk system, whether it is the compensation of errors, such as attitude calculation, or the realization of control, such as attitude control and position control, all rely on the PID of the cascade. pid will give an introduction, and will be followed by analysis, attitude control and position calculation and control. Their analysis will also be explained from the schematic block diagram and source code comments, that is, to communicate with you about their usual arrangement, and hope that the great god can take me to fly.
This part says three parts:
1. Introduction
of pid 2. PID parameter adjustment
3. Cascade pid
1. Introduction
of PID In industrial applications, PID and its derived algorithms are one of the most widely used algorithms, and are well-deserved universal algorithms. If you can master the design and implementation process of PID algorithms, for general R&D personnel, you should It is enough to deal with general research and development problems, and what is commendable is that among the control algorithms I have come into contact with, the PID control algorithm is the simplest and the control algorithm that best reflects the feedback idea, which can be described as a classic among the classics. The classics are not necessarily complicated. The classics are often simple and the simplest. The simple is not primitive, and the simple is not backward. It is simple to the point of beauty. Although many intelligent algorithms have evolved, such as ant colonies, neural networks, etc. If you are interested, you can read "Advanced PID Control" by Mr. Liu Jinkun, but in practical applications, cascade PID is still the main method because it is reliable.
Let's first look at the general form of the PID algorithm:
The process of PID is so simple that it can't be simpler. The controlled variable is controlled by the error signal , and the controller itself is the sum of the three links of proportional, integral and differential. Here we specify (at time t):
1. The input quantity is rin(t);
2. The output is rout(t);
3. The deviation is err(t)=rin(t)-rout(t);
The control law of pid is
1. Explain the principle of feedback control. It is not difficult to see from the above block diagram that PID control is actually a control process for deviation ;
2. If the deviation is 0, the proportional link does not work. Only when there is a deviation, the proportional link works.
3. The integral link is mainly used to eliminate the static error. The so-called static error is the difference between the output value and the set value after the system is stabilized. The integral link is actually the process of accumulating deviations, adding the accumulated error to the original value. system to offset the static difference caused by the system.
4. The differential signal reflects the change law of the deviation signal, or the change trend, and the advance adjustment is carried out according to the change trend of the deviation signal , thereby increasing the rapidity of the system.
The following will discretize the PID continuous system to facilitate implementation on the processor. Let's paste the formula for the continuous state again:
Assuming that the sampling interval is T, then at time KT:
Deviation err(K)=rin(K)-rout(K);
The integral part is expressed in the form of addition, that is, err(K)+err(K+1)+…;
The differential link is expressed in the form of slope, namely [err(K)-err(K-1)]/T;
Thus, the following PID discrete representation is formed:
Then u(K) can be expressed as:
As for the specific expressions of the three parameters Kp, Ki, and Kd, I think it can be easily introduced. I will save time here and will not describe them in detail.
In fact, so far, the basic discrete representation of PID has come out. The current form of expression belongs to positional PID, and another form of expression is incremental PID, which can be easily obtained from the above expression of U:
So:
这就是离散化PID的增量式表示方式，由公式可以看出，增量式的表达结果和最近三次的偏差有关，这样就大大提高了系统的稳定性。需要注意的是最终的输出结果应该为
u(K)+增量调节值;
PID的离散化过程基本思路就是这样，下面是将离散化的公式转换成为C语言，从而实现微控制器的控制作用。
那么如何用c语言进行表达？下面将对pid一种常见的形式进行c语言的描述，注意他们的演变过程，在pixhawk中也用到这其中的一些注意事项，如积分分离。
位置型PID的C语言实现：
第一步：定义PID变量结构体，代码如下：
struct _pid{
float SetSpeed; //定义设定值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_last; //定义上一个偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
float voltage; //定义电压值（控制执行器的变量）
float integral; //定义积分值
}pid;
控制算法中所需要用到的参数在一个结构体中统一定义，方便后面的使用。
第二部：初始化变量，代码如下：
void PID_init(){
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end \n");
}
统一初始化变量，尤其是Kp,Ki,Kd三个参数，调试过程当中，对于要求的控制效果，可以通过调节这三个量直接进行调节。
第三步：编写控制算法，代码如下：
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
pid.integral+=pid.err;//位置式pid是对积分的持续累加，容易造成积分饱和，是系统过调
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
注意：这里用了最基本的算法实现形式，没有考虑死区问题，没有设定上下限，只是对公式的一种直接的实现，后面的介绍当中还会逐渐的对此改进。
到此为止，PID的基本实现部分就初步完成了。下面是测试代码：
int main(){
printf("System begin \n");
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
增量型PID的C语言实现：
实现过程仍然是分为定义变量、初始化变量、实现控制算法函数、算法测试四个部分，
#include<stdio.h>
#include<stdlib.h>
struct _pid{
float SetSpeed; //定义设定值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_next; //定义上一个偏差值
float err_last; //定义最上前的偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
}pid;
void PID_init(){
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.err_next=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
float incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);//只和前后三次的误差值有关，也方便计算
pid.ActualSpeed+=incrementSpeed;
pid.err_last=pid.err_next;
pid.err_next=pid.err;
return pid.ActualSpeed;
}
int main(){
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
积分分离的PID控制算法C语言实现：
在普通PID控制中，引入积分环节的目的，主要是为了消除静差，提高控制精度。但是在启动、结束或大幅度增减设定时，短时间内系统输出有很大的偏差，会造成PID运算的积分积累，导致控制量超过执行机构可能允许的最大动作范围对应极限控制量，从而引起较大的超调，甚至是震荡，这是绝对不允许的。
为了克服这一问题，引入了积分分离的概念，其基本思路是当被控量与设定值偏差较大时，取消积分作用; 当被控量接近给定值时，引入积分控制，以消除静差，提高精度。其具体实现代码如下：
pid.Kp=0.2;
pid.Ki=0.04;
pid.Kd=0.2; //初始化过程
if(abs(pid.err)>200)
{
index=0;
}else{
index=1;
pid.integral+=pid.err;
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
//算法具体实现过程可参考上面的
抗积分饱和的PID控制算法C语言实现：
所谓的积分饱和现象是指如果系统存在一个方向的偏差，PID控制器的输出由于积分作用的不断累加而加大，从而导致执行机构达到极限位置，若控制器输出U(k)继续增大，执行器开度不可能再增大，此时计算机输出控制量超出了正常运行范围而进入饱和区。一旦系统出现反向偏差，u(k)逐渐从饱和区退出。进入饱和区越深则退出饱和区时间越长。在这段时间里，执行机构仍然停留在极限位置而不随偏差反向而立即做出相应的改变，这时系统就像失控一样，造成控制性能恶化，这种现象称为积分饱和现象或积分失控现象。
防止积分饱和的方法之一就是抗积分饱和法，该方法的思路是在计算u(k)时，首先判断上一时刻的控制量u(k-1)是否已经超出了极限范围： 如果u(k-1)>umax，则只累加负偏差; 如果u(k-1)<umin，则只累加正偏差。从而避免控制量长时间停留在饱和区。直接贴出代码，不懂的看看前面几节的介绍。
float PID_realize(float speed){
int index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if(pid.ActualSpeed>pid.umax) //灰色底色表示抗积分饱和的实现
{
if(abs(pid.err)>200) //蓝色标注为积分分离过程
{
index=0;
}else{
index=1;
if(pid.err<0)
{//如果超上限要嘛加负值要嘛就不加了，免得进入饱和区
pid.integral+=pid.err;
}
}
}else if(pid.ActualSpeed<pid.umin){
if(abs(pid.err)>200) //积分分离过程
{
index=0;
}else{
index=1;
if(pid.err>0)
{//如果超下限要嘛加正值要嘛就不加了免得进入饱和区
pid.integral+=pid.err;
}
}
}else{
if(abs(pid.err)>200) //积分分离过程
{
index=0;
}else{
index=1;
pid.integral+=pid.err;
}
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
变积分的PID控制算法C语言实现：
变积分PID可以看成是积分分离的PID算法的更一般的形式。在普通的PID控制算法中，由于积分系数ki是常数，所以在整个控制过程中，积分增量是不变的。但是，系统对于积分项的要求是，系统偏差大时，积分作用应该减弱甚至是全无，而在偏差小时，则应该加强。积分系数取大了会产生超调，甚至积分饱和，取小了又不能短时间内消除静差。因此，根据系统的偏差大小改变积分速度是有必要的。
变积分PID的基本思想是设法改变积分项的累加速度，使其与偏差大小相对应：偏差越大，积分越慢; 偏差越小，积分越快。
这里给积分系数前加上一个比例值index：
当abs(err)<180时，index=1;
当180<abs(err)<200时，index=（200-abs(err)）/20;
当abs(err)>200时，index=0;
最终的比例环节的比例系数值为ki*index;
float PID_realize(float speed){
float index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if(abs(pid.err)>200) //变积分过程
{
index=0.0;
}else if(abs(pid.err)<180){
index=1.0;
pid.integral+=pid.err;
}else{
index=(200-abs(pid.err))/20;
pid.integral+=pid.err;
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
最后给出大家专家系统中控制经验，自己理解吧。
反应系统性能的两个参数是系统误差e和误差变化律ec，
首先我们规定一个误差的极限值，假设为Mmax；规定一个误差的比较大的值，假设为Mmid；规定一个误差的较小值，假设为Mmin；
e*ec>0 误差在朝向误差绝对值增大的方向变化(可以理解成速度和加速度)
若此时 abs(e)>Mmid ：误差较大 强控制
若此时 abs(e)<Mmid ：误差绝对值本身并不是很大 一般的控制作用
e*ec<0 误差在朝向误差绝对值减小的方向变化
若此时 e*err(k-1)>0或者e=0 ：误差的绝对值向减小的方向变化，或者已经达到平衡状态，
此时保持控制器输出不变即可。
若此时e*err(k-1)<0 ： 误差处于极限状态。如果误差的绝对值>min，强控制 （调节幅度比较大） 如果此时误差绝对值较小，可以考虑实施较弱控制作用。
当abs（e）>Mmax时，说明误差的绝对值已经很大了，都应该考虑控制器的输入应按最大（或最小） 输出，以 达到迅速调整误差的效果，使误差绝对值以最大的速度减小。
当abs(e)<Mmin时，说明误差绝对值很小，此时加入积分，减小静态误差。
2、pid调参你怎么看
1）．PID调试一般原则
a.在输出不振荡时，增大比例增益P。
b.在输出不振荡时，减小积分时间常数Ti。
c.在输出不振荡时，增大微分时间常数Td。
2）．一般步骤
a.确定比例增益P ：确定比例增益P 时，首先去掉PID的积分项和微分项，一般是令Ti=0、Td=0（具体见PID的参数设定说明），使PID为纯比例调节。输入设定为系统允许的最大值的60%~70%，由0逐渐加大比例增益P，直至系统出现振荡；再反过来，从此时的比例增益P逐渐减小，直至系统振荡消失，记录此时的比例增益P，设定PID的比例增益P为当前值的60%~70%。比例增益P调试完成。
b.确定积分时间常数Ti比例增益P确定后，设定一个较大的积分时间常数Ti的初值，然后逐渐减小Ti，直至系统出现振荡，之后在反过来，逐渐加大Ti，直至系统振荡消失。记录此时的Ti，设定PID的积分时间常数Ti为当前值的150%~180%。积分时间常数Ti调试完成。
d.系统空载、带载联调，再对PID参数进行微调，直至满足要求：理想时间两个波，前高后低4比
3、串级pid简介
串级pid内外两环并联调节，这样的好处的是增加系统的稳定性，抗干扰。同时调节系统缓慢过度，注意外环都是本身误差，内环是速度，如位置控制外环是位置，内环是速度，是因为位置改变的实现是靠三个方向的速度积分出来的。同样姿态控制中，外环是角度差，内环是加速度，是因为角度的实现是靠角速度过渡来的，他们都是这样的一个过渡过程。实际中如果你追求响应的快捷，你也可以直接控制内环，或者直接控制姿态。
串级PID两个PID控制算法，只不过把他们串起来了(更精确的说是套起来)。那这么做有什么用？答案是，它增强了系统的抗干扰性(也就是增强稳定性)，因为有两个控制器控制飞行器，它会比单个控制器控制更多的变量，使得飞行器的适应能力更强。画出串级PID的原理框图，
在整定串级PID时的经验则是：先整定内环PID，再整定外环P。因为内环靠近输出，效果直接。
内环P：从小到大，拉动四轴越来越困难，越来越感觉到四轴在抵抗你的拉动；到比较大的数值时，四轴自己会高频震动，肉眼可见，此时拉扯它，它会快速的振荡几下，过几秒钟后稳定；继续增大，不用加人为干扰，自己发散翻机。
特别注意：只有内环P的时候，四轴会缓慢的往一个方向下掉，这属于正常现象。这就是系统角速度静差。
内环I：前述PID原理可以看出，积分只是用来消除静差，因此积分项系数个人觉得没必要弄的很大，因为这样做会降低系统稳定性。从小到大，四轴会定在一个位置不动，不再往下掉；继续增加I的值，四轴会不稳定，拉扯一下会自己发散。
特别注意：增加I的值，四轴的定角度能力很强，拉动他比较困难，似乎像是在钉钉子一样，但是一旦有强干扰，它就会发散。这是由于积分项太大，拉动一下积分速度快，给 的补偿非常大，因此很难拉动，给人一种很稳定的错觉。
内环D：这里的微分项D为标准的PID原理下的微分项，即本次误差-上次误差。在角速度环中的微分就是角加速度，原本四轴的震动就比较强烈，引起陀螺的值变化较大，此时做微分就更容易引入噪声。因此一般在这里可以适当做一些滑动滤波或者IIR滤波。从小到大，飞机的性能没有多大改变，只是回中的时候更加平稳；继续增加D的值，可以肉眼看到四轴在平衡位置高频震动(或者听到电机发出滋滋的声音)。前述已经说明D项属于辅助性项，因此如果机架的震动较大，D项可以忽略不加。
外环P：当内环PID全部整定完成后，飞机已经可以稳定在某一位置而不动了。此时内环P，从小到大，可以明显看到飞机从倾斜位置慢慢回中，用手拉扯它然后放手，它会慢速回中，达到平衡位置；继续增大P的值，用遥控器给不同的角度给定，可以看到飞机跟踪的速度和响应越来越快；继续增加P的值，飞机变得十分敏感，机动性能越来越强，有发散的趋势。
4、最后给你贴上pixhawk有关pid的源码，就是位置式的很简单，自己理解一下吧。需要说明的是位置式pid容易导致积分的饱和，所以在积分上过了很多处理。如在位置控制中，推力的积分量就是进行了饱和处理。
- __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
- {
- if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
- return pid->last_output;
- }
- float i, d;
- /* current error value */
- float error = sp - val;
- /* current error derivative */
- if (pid->mode == PID_MODE_DERIVATIV_CALC) {
- d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
- pid->error_previous = error;
- } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
- d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
- pid->error_previous = -val;
- } else if (pid->mode == PID_MODE_DERIVATIV_SET) {
- d = -val_dot;
- } else {
- d = 0.0f;
- }
- if (!isfinite(d)) {
- d = 0.0f;
- }
- /* calculate PD output */
- float output = (error * pid->kp) + (d * pid->kd);
- if (pid->ki > SIGMA) {
- // Calculate the error integral and check for saturation
- i = pid->integral + (error * dt);
- /* check for saturation */
- if (isfinite(i)) {
- if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
- fabsf(i) <= pid->integral_limit) {
- /* not saturated, use new integral value */
- pid->integral = i;
- }
- }
- /* add I component to output */
- output += pid->integral * pid->ki;
- }
- /* limit output */
- if (isfinite(output)) {
- if (pid->output_limit > SIGMA) {
- if (output > pid->output_limit) {
- output = pid->output_limit;
- } else if (output < -pid->output_limit) {
- output = -pid->output_limit;
- }
- }
- pid->last_output = output;
- }
- return pid->last_output;
- }
- __EXPORT void pid_reset_integral(PID_t *pid)
- {
- pid->integral = 0.0f;
- }