Pixhawk-姿态解算-互补滤波
版权声明：本文为博主原创文章，未经博主允许不得转载。
目录(?)[+]
根深方能叶茂 在等待的日子里，刻苦读书，谦卑做人，养得深根，日后才能枝叶茂盛。 --Better(根爷)
鉴于串级PID在pixhawk系统中的重要性，无论是误差的补偿，如姿态解算；还是控制的实现，如姿态控制，位置控制，靠的都是串级的pid，这里我们先对串级pid做一个介绍，后面会再接着分析，姿态的控制以及位置的解算和控制。他们的分析都还将从原理框图和源码注释上说明，就是把自己平时的一点整理与大家交流一下，也希望大神能带我飞。
这一部分说三部分内容：
1、pid的介绍
2、pid调参
3、串级pid
1、pid的介绍
在工业应用中PID及其衍生算法是应用最广泛的算法之一，是当之无愧的万能算法，如果能够熟练掌握PID算法的设计与实现过程，对于一般的研发人员来讲，应该是足够应对一般研发问题了，而难能可贵的是，在我所接触的控制算法当中，PID控制算法又是最简单，最能体现反馈思想的控制算法，可谓经典中的经典。经典的未必是复杂的，经典的东西常常是简单的，而且是最简单的，简单的不是原始的，简单的也不是落后的，简单到了美的程度。现在虽然已经演变出很多智能的算法，如蚁群，神经网络等，感兴趣可以看一下刘金琨老师的《先进pid控制》，但是在实际应用中还是以串级pid为主，因为它可靠。
先看看PID算法的一般形式：
PID的流程简单到了不能再简单的程度，通过误差信号控制被控量，而控制器本身就是比例、积分、微分三个环节的加和。这里我们规定（在t时刻）：
1.输入量为rin(t);
2.输出量为rout(t);
3.偏差量为err(t)=rin(t)-rout(t);
pid的控制规律为
1.说明一下反馈控制的原理，通过上面的框图不难看出，PID控制其实是对偏差的控制过程;
2.如果偏差为0,则比例环节不起作用，只有存在偏差时，比例环节才起作用。
3.积分环节主要是用来消除静差，所谓静差，就是系统稳定后输出值和设定值之间的差值，积分环节实际上就是偏差累计的过程，把累计的误差加到原有系统上以抵消系统造成的静差。
4.而微分信号则反应了偏差信号的变化规律，或者说是变化趋势，根据偏差信号的变化趋势来进行超前调节，从而增加了系统的快速性。
下面将对PID连续系统离散化，从而方便在处理器上实现。下面把连续状态的公式再贴一下：
假设采样间隔为T，则在第K T时刻：
偏差err(K)=rin(K)-rout(K);
积分环节用加和的形式表示，即err(K)+err(K+1)+……;
微分环节用斜率的形式表示，即[err(K)-err(K-1)]/T;
从而形成如下PID离散表示形式：
则u(K)可表示成为：
至于说Kp、Ki、Kd三个参数的具体表达式，我想可以轻松的推出了，这里节省时间，不再详细表示了。
其实到这里为止，PID的基本离散表示形式已经出来了。目前的这种表述形式属于位置型PID，另外一种表述方式为增量式PID，由U上述表达式可以轻易得到：
那么：
这就是离散化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
#include
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)
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
{
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=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)
e*ec<0 误差在朝向误差绝对值减小的方向变化
若此时 e*err(k-1)>0或者e=0 ：误差的绝对值向减小的方向变化，或者已经达到平衡状态，
此时保持控制器输出不变即可。
若此时e*err(k-1)<0 ： 误差处于极限状态。如果误差的绝对值>min，强控制 （调节幅度比较大） 如果此时误差绝对值较小，可以考虑实施较弱控制作用。
当abs（e）>Mmax时，说明误差的绝对值已经很大了，都应该考虑控制器的输入应按最大（或最小） 输出，以 达到迅速调整误差的效果，使误差绝对值以最大的速度减小。
当abs(e)
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;
- }
__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;
}