浅析MWC飞控PID算法

玩四轴快一年了，自制机架，改装云台，折腾很多，但终究是带着镣铐跳舞

所以最近开始研究算法，因为对Arduino略有了解，就选择了MWC作为学习的开始

先介绍下MWC （https://code.google.com/p/multiwii/）

MWC是一个基于Arduino的开源飞控，最早的版本很简单，一块Arduino + Wii手柄 ，后来逐渐发展，可以用各种Arduino板和传感器板组合成飞控

这是我自己用 Arduino Nano 和 GY-86 焊成的飞控（详见：http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=1043804）

成本不足100元，还可以再加一块Nano来连接GPS，实现定点悬停，制定路径飞行等功能，成本也不会超过200元

MWC的PID代码很短，只有几十行，黏贴如下

 //**** PITCH & ROLL & YAW PID ****  int16_t prop;  prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500);   for(axis=0;axis<3;axis++)   {    if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 )    {       errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis];           PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7;          PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);        errorAngleI[axis]     = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);       ITermACC              = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;    }    if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 )    {       if (abs(rcCommand[axis])<500)      {         error = (rcCommand[axis]<<6)/conf.P8[axis];       }      else       {                error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis];       }      error -= gyroData[axis];      PTermGYRO = rcCommand[axis];           errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);      if (abs(gyroData[axis])>640)       {        errorGyroI[axis] = 0;      }      ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6;                           }    if ( f.HORIZON_MODE && axis<2)     {      PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9;       ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;    }     else         {      if ( f.ANGLE_MODE && axis<2)       {        PTerm = PTermACC;        ITerm = ITermACC;      }       else ////for example       {        PTerm = PTermGYRO;        ITerm = ITermGYRO;      }    }        PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6;    delta          = gyroData[axis] - lastGyro[axis];      lastGyro[axis] = gyroData[axis];    deltaSum       = delta1[axis]+delta2[axis]+delta;    delta2[axis]   = delta1[axis];    delta1[axis]   = delta;       DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;    axisPID[axis] =  PTerm + ITerm - DTerm;  }

接下来请搬个板凳，偶们来简单说说PID算法
PID主要作用就是自稳，也就是你打杆让四轴倾斜，松杆它可以自己恢复水平


但是恢复的力道很有讲究，用拔萝卜打比方
比如说有个大萝卜，你用全身力气往后拔，但其实萝卜很松，一点力气就出来了，那你可能就会往后一屁股坐地上了
然后你遇到个小萝卜，吸取教训，用很小的力气拔，结果萝卜其实很紧，根本拔不出来
所以正确的拔法应该是边拔边感受，来确定合适的拔的力道


在四轴上，如果往左倾斜，然后让左边电机猛地加速，可能四轴就会先水平，然后继续往右，又不水平了
若是很缓和地加速，那就可能根本回不到水平


具体的数学就不说了，直接说怎么让PID理论编程飞控代码
这是wiki上pid最核心的式子，这些符号什么意思不用懂，我用中文给大家解释



大家都知道，调PID，就是调P，I，D三个参数，这三个参数具体的用处就是算出要用多大的力气拔萝卜
怎么算呢？分两步
一. 先要知道离四轴恢复水平还偏差多少
      比方说，现在左倾10度，水平是0度，那偏差就是10度
二. 套上面那个公式，偏差 * P + 偏差积分 *  I + 偏差微分 * D ，这就是要修正的量     积分和微分不用想得很复杂，实际MWC里面，积分就是做加法，微分就是做减法


好，接下来正式看代码，一丁点C语言基础就可以看懂

  int16_t prop;  prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500);

这两行定义了prop变量，之后要用，先不管

for(axis=0;axis<3;axis++)   {.............................................}

接下里是一个循环，循环3次分别对俯仰，横滚，和方向三个轴计算PID
自稳只涉及俯仰、横滚

/***********自稳PI部分***********/
先看俯仰横滚的PID算法

if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) 

这个if语句就是用来判断，现在是否在自稳的模式，如果是，执行自稳的PID计算

errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis];       PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7;    PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);

这三行计算之前wiki公式里的第一项（偏差 * P）
errorAngle：目前的倾角和想要恢复到的倾角的偏差 
第一行：通过传感器传回的数据和遥控数据算出偏差
第二行：偏差乘上P，再除以128 （注意：这里>>7是个位运算，其实就是除以128，只是这样可以算得更快）第三行：把上一行算出来的数和我们设置的D值的5倍作比较，不能比D的五倍大
注意，这里不用太纠结除以128，因为如果我们让P值大十倍，这里除以12.8，不是一样的嘛？

然后算之前wiki公式里的第二项（偏差积分 *  I）

errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);ITermACC  = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;

第一行就是积分，在这里也就是做加法（wiki公式中第二项是从0到t积分，也就是把从开始到现在的偏差都加起来，注意，偏差有正有负）

每次循环，第一行都会把之前算出来的errorAngle加到errorAngleI[axis]存起来，即完成了“偏差积分”
第二行就是把第一行算出来的“偏差积分”和 I 乘起来，再除以4096
/**********************/

第三项的计算是和第三个轴（方向，YAW）一起算的，之后再说

/***********特技与YAW PI 部分***********/

接下来就是第三个轴（方向，YAW）和特技模式的PID部分（特技模式即纯手动模式）
之所以方向和特技是用一个算法，是因为这两个都不需要自稳（推杆持续动作，松杆后不恢复）

和前面一样，先把偏差算出来

if (abs(rcCommand[axis])<500)      { ............................................}      else       {............................................ }

首先是一个if else语句，这个不用太在意，只是根据数据大小选择计算方式，节省内存，关键就只有下面这句
error = (rcCommand[axis]<<6)/conf.P8[axis]
和之前自稳的最大的区别就是，这里不再用传感器回传的angle来计算偏差，而是用遥控指令
所谓偏差，即是目前的状态和理想的状态的区别
比方说，我打方向杆，想让四轴左转10度，在四轴开始转之前，那这个偏差就是这个10度的遥控指令

然后是对error用陀螺仪数据做一个修正

error -= gyroData[axis];

再然后就是把pid公式中的三项算出来


特技模式PID中，pid公式第一项简单的用rcCommand[axis]来计算，没有乘一个系数

PTermGYRO = rcCommand[axis];

接下来算第二项

errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);

有没有很熟悉，和自稳PID一样，errorGyroI[axis]+error 把每次的偏差都加在一起
这句语句同时把偏差控制在（-16000，+16000）之间，防止过大
但是MWC论坛上有人把限制去掉了飞，也没有任何问题，估计只是保险起见吧

接下来是if语句，作用是：如果陀螺仪的数据过大，那么就把偏差人为地设定为0，防止偏差不断累积出现问题

if (abs(gyroData[axis])>640)       {        errorGyroI[axis] = 0;      }

然后还是老样子，偏差 * I 再除以系数，得出pid公式第二项

ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6;

至此，pid公式中的前两项就算好了
/**********************/




/***********微调PI部分***********/
不过如果是特技模式，还需要一点微调

PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;

微调用到prop，这个prop是利用遥控信号算出来的，可以看代码最开头两行

如果是自稳模式，那么就不用微调，直接赋值

PTerm = PTermACC;ITerm = ITermACC;

如果是第三个轴（方向，YAW），也不需要微调，直接赋值

PTerm = PTermGYRO;ITerm = ITermGYRO;

接下来一行是动态P，其实就是根据油门大小对P进行微调
效果是：油门比较大的时候，P会略小一点

PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6;

/***********D部分***********/
终于要算pid公式第三项了
来回忆一下，第三项是  偏差微分 * I


微分在这里就是做减法，所以核心其实就是第一行：

delta  = gyroData[axis] - lastGyro[axis]

偏差 等于 陀螺仪数据 减去 上一次的陀螺仪数据


接下来的几行只是把前三次测得的陀螺仪数据依次存储在 delta，delta1，delta2 里面而已
让前三次的陀螺仪数据都参与运算，效果会更好

lastGyro[axis] = gyroData[axis];deltaSum       = delta1[axis]+delta2[axis]+delta;delta2[axis]   = delta1[axis];delta1[axis]   = delta;

/**********************/


/***********大功告成部分***********/
最最最后，终于可以把pid公式算出来了
最终结果等于 偏差 * P + 偏差积分 *  I + 偏差微分 * D ，这里符号不用太在意，把D定义为负的，这里不就是加号了嘛

axisPID[axis] =  PTerm + ITerm - DTerm;

PID部分就是这么多了，之后就是把算出来的PID告诉电调来驱动电机，这一部分以后有机会再另开帖子吧

本人水平有限，有错误不妥之处，还请毫不吝惜地指出