5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

标题: MWC 飞控 PID算法 浅析 [打印本页]
作者: do335maomao    时间: 2015-2-21 22:28
标题: MWC 飞控 PID算法 浅析
本帖最后由 do335maomao 于 2015-2-23 15:09 编辑

玩四轴快一年了，自制机架，改装云台，折腾很多，但终究是带着镣铐跳舞
所以最近开始研究算法，因为对Arduino略有了解，就选择了MWC作为学习的开始
我算是彻底放弃在坛子里排版了，明明编辑好了一保存又成原样了……嫌代码格式不好看的，麻烦移步http://blog.csdn.net/do335maomao/article/details/43899003

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

这是我自己用 Arduino Nano 和 GY-86 焊成的飞控（详见：http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=1043804）
成本不足100元，之后我又加了一块Nano来连接GPS，可以实现定点悬停等功能
若是只想有CC3D的功能，12块的Nano + 8块的MPU6050 就足够了，可见CC3D，QQ之类飞控，真的便宜嘛？也不见得

个人试飞下来，MWC的定高的确有问题，上下幅度能有一米
不过也看到有人能定得和APM一个水准，不知道是不是我调得不好

自稳方面还是很优秀的，MWC飞特技很顺手，YOUTUBE上相关视频也不少

(, 下载次数: 29)



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最核心的式子，这些符号什么意思不用懂，我用中文给大家解释
(, 下载次数: 25)

大家都知道，调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)>>9
ITerm = ((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告诉电调，有机会再另开帖子吧

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





























作者: 1263436446    时间: 2015-2-21 22:45
lz前排~~
作者: 伯努利    时间: 2015-2-21 23:02
lz第二排
作者: plaaf杰    时间: 2015-2-21 23:02
没有气压器定高自然不够好
作者: 白小淘    时间: 2015-2-21 23:03
前排
作者: hippojia76    时间: 2015-2-21 23:03
來迟了一歩
作者: zhanyiduo    时间: 2015-2-22 00:19
apm 本来就是基于arudino mega的。 所以两者差异应该只有算法了。
作者: xjkt    时间: 2015-2-22 00:31
虽然不懂，但这样的技术贴一定要顶。
作者: dongfang    时间: 2015-2-22 01:04
恭喜LZ，LZ和我一样是从MWC入门的。从技术上来讲，MWC的代码足够简单易懂，也已经实现了四轴飞行的大部分功能，我也喜欢MWC这种不受传感器限制的开源系统。

我现在在玩APM，但精力有限，已经不再看源码了。
作者: O斌LOVE斌O    时间: 2015-2-22 01:30
顶
作者: 快乐之神    时间: 2015-2-22 01:39
非常精辟   能不能分析一下气压计的代码呀   感谢楼主
作者: szhcs    时间: 2015-2-22 11:28
本帖最后由 szhcs 于 2015-2-22 11:31 编辑

顶，我也是用MWC飞特技，感觉还是MWC顺手。
PID的算法原理比较简单，但产生的效果和如何调试是还是有点难度的，我是实战派。
有兴趣可以看看我的这个帖子
http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=891184

作者: bluebore    时间: 2015-2-22 12:44
支持技术贴，我也打算从这个开始学，当然还在入门阶段~
作者: do335maomao    时间: 2015-2-22 12:54
szhcs 发表于 2015-2-22 11:28
顶，我也是用MWC飞特技，感觉还是MWC顺手。
PID的算法原理比较简单，但产生的效果和如何调试是还是有点难 ...

等完全吃透代码，打算首先加个像APM那样的飞行中调PID的功能，MWC调PID还是太不方便了

作者: wxjzy    时间: 2015-2-22 13:42
貌似很厉害的样纸
作者: leo2045    时间: 2015-2-23 00:02
大神！
作者: liubin200    时间: 2015-2-23 08:33
顶一个




欢迎光临 5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年） (http://5imx.com/) Powered by Discuz! X3.3