|
|
本帖最后由 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上相关视频也不少
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)>>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告诉电调，有机会再另开帖子吧
本人水平有限，有错误不妥之处，还请指出
|
评分
-
查看全部评分
欢迎继续阅读楼主其他信息
|
/1 
发表于 2015-2-21 22:28
收藏
楼主
