鉴于串级PID在pixhawk系统中的重要性，无论是误差的补偿，如姿态解算；还是控制的实现，如姿态控制，位置控制，靠的都是串级的pid，这里我们先对串级pid做一个介绍，后面会再接着分析，姿态的控制以及位置的解算和控制。他们的分析都还将从原理框图和源码注释上说明，就是把自己平时的一点整理与大家交流一下，也希望大神能带我飞。
这一部分说三部分内容：
1、pid的介绍
2、pid调参
3、串级pid
4、pid与滤波的关系，这也是一个很有意思的问题，一个是从控制角度理解，一个是从滤波角度理解。这一个我只是一点理解，就在这里先说一点。pid中，i相当于低通滤波器，极限情况下理解：直流信号肯定会持续积分，反而高频的噪声正负叠加被屏蔽了，所以i是低通滤波器。而D是高通滤波器，同样极限情况下理解：直流信号微分为0，高频的噪声微分却有了值，所以D是高通滤波器，和我们平时说到的D太大容易放大噪声造成震动等效。
1、pid的介绍
在工业应用中PID及其衍生算法是应用最广泛的算法之一，是当之无愧的万能算法，如果能够熟练掌握PID算法的设计与实现过程，对于一般的研发人员来讲，应该是足够应对一般研发问题了，而难能可贵的是，在我所接触的控制算法当中，PID控制算法又是最简单，最能体现反馈思想的控制算法，可谓经典中的经典。经典的未必是复杂的，经典的东西常常是简单的，而且是最简单的，简单的不是原始的，简单的也不是落后的，简单到了美的程度。现在虽然已经演变出很多智能的算法，如蚁群，神经网络等，感兴趣可以看一下刘金琨老师的《先进pid控制》，但是在实际应用中还是以串级pid为主，因为它可靠。
先看看PID算法的一般形式：
PID的流程简单到了不能再简单的程度，通过误差信号控制被控量，而控制器本身就是比例、积分、微分三个环节的加和。这里我们规定（在t时刻）：
1. Enter the amount rin (t);
2. The output of rout (t);
3. The amount of deviation err (t) = rin (t) -rout (t);
Pid control law is
1. explain the principles of the feedback control, is easy to see by the above block diagram, PID control is a control process in fact for the deviation;
2. If the deviation is 0, the proportional component does not work, only when there is a deviation, the proportional element to function.
3. The integral part is mainly used to eliminate static errors, so-called static error, is the difference between the system stabilizes the output value and the set value, the cumulative integral part of the process is actually a deviation, the accumulated error is added to the original static system caused the system to offset the difference.
4. The differential signal is the reaction of a variation of the error signal, or a tendency to spark advance signal is adjusted according to the trend of the deviation, thereby increasing the rapidity of the system.
The following will be continuous system discrete PID, thus facilitating the realization on the processor. Here's the formula for a continuous state paste it:
It assumed that the sampling interval is T, the first time KT:
Deviation err (K) = rin (K) -rout (K);
Represented by the plus and integral part of the form, i.e., err (K) + err (K + 1) + ......;
Differentiating element expressed in the form of a slope, i.e., [err (K) -err (K-1)] / T;
Thereby forming a discrete representation of the PID are as follows:
Then u (K) can be expressed to be:
As for the specific expression of the three parameters Kp, Ki, Kd, I think we can easily launched, to save time here, not shown in detail.
In fact, here so far, the basic discrete PID representation has come out. This form of presentation is currently belongs positional PID, expressed another way incremental PID, U can be easily obtained from the above-mentioned expression:
Then:
This is an incremental representation of discrete PID can be seen from the formula, expressing the results of incremental and about the last three deviation, thus greatly improving the stability of the system. Note that the final output should be
u (K) + incremental adjustment value;
The basic idea PID discretization process is such that the following equation is discretized converted into C language, thereby achieving the control function for the microcontroller.
So how do you express in c language? Pid The following will describe a common form of c language, pay attention to their evolution, in pixhawk is also used in some of the considerations of which, such as integral separation.
PID position type of C language:
STEP 1: PID structure variables, as follows:
_pid {struct
a float SetSpeed; // set value defined
float ActualSpeed; // define the actual value of the
float err; // define deviation
float err_last; // offset value defined on a
float Kp, Ki, Kd; // defined ratio, integral, differential coefficient
float voltage; // define the voltage value (variable control of the actuators)
a float integral; // define integration value
} pid;
Control algorithm required parameters used in a uniformly defined structure, it is convenient later use.
The second part: initializing variables, as follows:
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;
}
Incremental PID of the C language:
Implementation process is still divided into the definition of variables, initialize variables, functions to achieve control algorithm, algorithm testing four parts,
#include<stdio.h>
#include<stdlib.h>
_pid {struct
float SetSpeed; // set value defined
float ActualSpeed; // define the actual value of the
float err; // define deviation
float err_next; // offset value defined on a
float err_last; // offset value defined in the most forward
float Kp, Ki, Kd; // definition of proportional, integral, differential coefficient
} 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);
// algorithm specific implementation may refer to the above
Anti-windup PID control algorithm C language:
The windup phenomenon is called if the system refers to the presence of a deviation in one direction , the output of the PID controller integral action due to the constant accumulation and increase, resulting in the limit position of the actuator, if the controller output U (k) continues to increase, the opening degree of the actuator can no longer increases, then the computer outputs a control amount beyond the normal operating range and into the saturated zone. Once the system has a reverse bias, u (k) is gradually withdrawn from the saturation region. The deeper into the saturated zone to exit the saturation zone longer. During this time, the actuator is still stuck in rather extreme position with a reverse bias and make the appropriate changes immediately, then the system just as out of control, resulting in deterioration of the control performance, a phenomenon known as saturation points or points out of control phenomenon.
One method of preventing windup of integration is an anti-saturation method, the idea of this method is the calculation of u (K), first determines whether a timing control amount u (k-1) is beyond the limits: if u ( k-1)> umax, only the negative bias accumulation; if u (k-1) <umin, only accumulated positive deviation. Control amount so as to avoid a long stay in the saturated zone. Direct posted the code, do not know look at the introduction of the previous sections.
float PID_realize(float speed){
int index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if (pid.ActualSpeed> pid.umax) // gray background shows that anti-saturation to achieve integral
{
if (abs (pid.err)> 200 ) // blue labeled integral separation process
{
index = 0;
} the else {
index =. 1;
IF (pid.err <0)
{// if it exceeds the upper limit negative thing to add either need no added value, and avoid entering the saturation region
pid.integral + = pid.err;
}
}
} The else IF (pid.ActualSpeed <pid.umin) {
IF (ABS (pid.err)> 200 is) integral separation //
{
index = 0;
} the else {
index =. 1;
IF (pid.err> 0 )
{// if it exceeds the lower limit value either need to add Well lest without entering the saturation region
pid.integral + = pid.err;
}
}
} {the else
IF (ABS (pid.err)> 200 is) integral separation // process
{
index = 0;
} the 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;
}
Finally, we expert system control experience, their understanding of it.
Two performance parameters of the reaction system is the system error e and error variation law EC ,
First, we set a limit of error is assumed to Mmax; provision of a relatively large error value is assumed to be Mmid; the smaller the value of a specified error is assumed to Mmin;
e * ec> 0 error absolute value error increases toward the direction of change (velocity and acceleration can be understood as)
At this time, if abs (e)> Mmid: control error is large and strong
At this time, if abs (e) <Mmid: absolute error itself is not large general control effect
e * ec <0 in the direction toward the error variation decreases the absolute value of the error
At this time, if e * err (k-1)> 0 or e = 0: the absolute value of the change in the direction of decreasing the error, or has reached an equilibrium state,
At this time can be kept constant controller output.
At this time, if e * err (k-1) <0: error in the limit state. If the absolute value of the error> min, strong control (adjustment range is relatively large) In this case, if the absolute value of the error is small, the control action may be considered weak embodiment.
When the abs (e)> Mmax, description has been great in the absolute value of error, should be considered the controller input shall be the maximum (or minimum) output, in order to achieve rapid adjustment error effect of making the maximum absolute error speed is reduced.
When the abs (e) <Mmin time, described small absolute error, at which time integral, reduced static error.
2, pid parameter adjustment for how you look
1). PID debugging general principles
a. When the output does not oscillate increase the proportional gain P.
b. When the output does not oscillate decrease the integral time constant Ti.
c. In the output does not oscillate increasing the derivative time constant Td.
(They cause any who had three metropolitan system shock.)
2). The general steps
. A proportional gain determined P: proportional gain P is determined, first remove the PID integral and derivative terms, is to make generally Ti = 0, Td = 0 (see PID parameter setting specific description), PID regulator so that the proportion of pure . Input set to 60% to 70% of the maximum allowed by the system, by a proportional gain P 0 is gradually increased, until the system oscillation occurs; then, in turn, gradually decreases from this point P is the proportional gain, until the disappearance of the oscillation system, the recording At this time, the proportional gain P, set the PID proportional gain P is 60% to 70% of the current value. P proportional gain commissioning.
b. Determine the integral time constant Ti proportional gain P is determined, setting a large initial value of the integral time constant Ti, and Ti decreases, until the system oscillation occurs, after the turn, Ti gradually increased, until the system oscillations disappear. At this time, the recording of Ti, set the PID integral time constant Ti of 150% ~ 180% of the current value. Integral time constant Ti commissioning.
c. Determine the integral time constant Td integral time constant Td is generally not set to 0. To set the same as the method of determining P and Ti, 30% of the oscillation is not taken.
d load system, FBI carrier tape, and then fine-tuning PID parameters, until answers: two waves over time, high to low ratio of 4
3, Cascade pid Profile
Cascade pid inner and outer rings of the shunt regulator, this advantage is to increase the stability of the system, interference. Slowly over while adjusting the system, the outer ring is noted that the error itself, the inner ring speed, position control such that the position of the outer ring, the inner ring speed, because the position change is achieved by integrating the speed out of the three directions. The same attitude control, the outer ring is the angle difference between the inner ring is the acceleration, because the angle is achieved by the transition to the angular velocity, they are such a transition. If you practice the pursuit of quick response, you can also directly control the inner loop, or direct control posture.
Cascade PID two PID control algorithm, just string them up (more precisely is set up). Then do what's the use? The answer is that it enhances the immunity system (that is, to enhance stability), because there are two controllers to control the aircraft, it will control more variables than a single controller, making the aircraft the ability to adapt more. Block diagram drawn Cascade PID,
Experience when tuning the Cascade PID is: First adjust the inner loop PID, and then setting the outer ring P. Because the inner ring near the output, the effect is immediate.
内环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;
}