查看: 81881|回复: 464
收起左侧

[小应用]GD32F103+MPU9150 四轴飞行器第一步:9DOF姿态融合

[复制链接]
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-8-30 10:20 | 显示全部楼层 |返回版面|阅读模式
本帖最后由 zkcaptain 于 2014-6-19 18:11 编辑

——————————————————————————————————————————————————————————————
请各位不要私信密我了,没空照顾那么多地方,就在帖子里回吧,我尽量回复,说的对其他人看了也受益,说的不对,其他人
也会指正,对大家都有好处。
——————————————————————————————————————————————————————————————

2014年6月16日更新:
PID+乱七八糟心得部分,内有完整代码和试飞视频,见该帖:http://bbs.21ic.com/forum.php?mo ... 1&extra=#pid4167670
---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

自己摸索着做的一个小应用,希望有朝一日能够飞起来,欢迎在电机、电调、算法、动力学等方面有研究的朋友一起讨论。

做四轴飞行器也是为了好玩,目前我只完成了第一步:基于加速度计、陀螺仪和磁力计的姿态融合,能够实时更新四元数,并由四元数解算出姿态角。接下来才是更重要的,比如上各种什么机架、电调、电机、螺旋桨之类的,然后写各种PID控制代码,系统整合以后还要调试各种参数,抗干扰,使系统稳定,最后还可能要加各种应用器件,目前对这方面了解的还不多。

算法参考自Madgwick写的AHRSUpdateIMUUpdate,简单有效,其中AHRSUpdate是融合了陀螺仪、加速度计和磁力计,而IMUUpdate只融合了陀螺仪和加速度计,但对于做四轴飞行器来说,IMUUpdate也够用了,不过我没发现有用AHRSUpdate的,并且我的一大半的时间都是花在AHRSUpdate这个算法上,直到现在也没用这个算法解算出来正确的四元数和欧拉角,什么方法都试过了,导致我现在都开始怀疑这个算法的正确性,希望用过的高手来指点迷径。

我现在用的算法:先用IMUUpdate算出PitchRoll,然后再结合互补滤波求出Yaw,其中求Yaw的公式参考自john800422

第一部分:硬件
1.传感器:MPU9150INVENSENSE公司的,单芯片内集成了加速度计、陀螺仪和磁力计,并且内置DMP用于姿态融合,不过只融合了加速度计和陀螺仪,具有自校准功能,价格比MPU6050贵很多,但是省PCB面积,省事,轴向重合度高。实际上就是把MPU6050和磁力计AK8975放在同一个芯片里,程序还是使用MPU6050的驱动);
2.MCUGD32F103CBGigadevice公司的,支持国产,与ST同型号的MCU 直接兼容,性价比更高,外接8M晶振,晶振远离传感器,避免干扰磁力计)
3.电源芯片:TLV70233DBVRTILDO,输入2-6V,输出3.3V,只需要外接2X7R无极性陶瓷电容)
4.串口:MAX3232(方便调试)
5.USB供电
上图:
图1:PCB的3D效果图,实物图就不上了,太丑,测试版,先追求调通得出姿态角:


第二部分:软件
1.使用keiluvision4.1.0,工具链:RealView MDK-ARM Version4.12
2.库文件:ST官方库STM32F10x_StdPeriph_Driver version V3.3.0
3.驱动:官方的MPU6050驱动inv_mpu.cinv_mpu_dmp_motion_driver.c
先看几个图,然后再说坐标轴的设定和算法部分。
上图:
图1:系统初始化,顺序从上到下依次是:初始化MPU、设置需要使用哪些传感器、设置陀螺仪测量范围(我设的是正负500度/s)、设置加速度计测量范围(我设的是正负4g)、配置fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、开启DMP、开始姿态融合,见下图:


2:由四元数求出的最终姿态角,其中Yaw为航向角,表示机头偏离正北方多少度,范围-180+180Pitch为俯仰角,表示机头正方向与水平线的夹角,范围-90+90Roll为翻滚角,表示机翼与水平线的夹角,范围:-180+180。下图为机身水平,且机头正北偏西37度左右的数据:


图3:下图为机翼水平,机头指向正北,且机头向下25度的数据:


图4:下图为机头指向正北,保持水平,且机翼的右翼向下倾斜23度的数据:


图5:看下欧拉角的奇异点,在奇异点处一个转动状态对应无穷多组自由度值,当物体转到这些奇异点附近,便没法求解。图中当Pitch+90度时,机体的姿态便没法控制,Roll的轴向发生了变化。如下图:


第三部分:如何确定自己的轴向
首先,轴向的定义跟初始化四元数和最后结算的欧拉角有关,跟IMUUpdate四元数更新算法无关,换句话说,不管你的轴向如何定义,IMUUpdate随便用,但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变,这个后面算法中有说。加速度计也好,陀螺仪也好,磁力计也好,他们的轴向都要满足右手定理,如下图:


再附上一段注释用于解释如何定义合理的轴向,以及如何正确旋转传感器的轴向,解释这么多其实就是说定义好的轴向要满足右手定理,如下图:


下图,旋转前是[x y z],旋转后就是[-y x z]:


下图是如何确定旋转角度的正方向,用右手握住坐标轴,拇指指向轴向的正方向,四个指头弯曲的方向就是旋转角度的正方向,在初始化四元数时,计算出的欧拉角的正方向也要满足这个条件:


我的程序使用的轴向如下图所示,未作任何改变:


第四部分:算法
第一步是校准,加速度计和陀螺仪我用的是MPU9150内部自校准,磁力计的校准采用如下方法:

第二步是初始化四元数,常见的轴向定义是绕x轴旋转是Roll,绕y轴旋转是Pitch,绕z轴旋转是Yaw,我的程序也是这样定义的,详细说明见程序,这里举个另外一种轴向定义来对初始化四元数进行说明,方便比较。
下面我们来定义绕x轴旋转是Pitch,绕y轴旋转是Roll,绕z轴旋转是Yaw,轴向的正方向如上图一样,不变。
先对加速度计和磁力计的数据进行处理,得到init_xx来供我们使用如下图:



然后通过公式计算出初始化的RollPitchYaw,注意加负号保证旋转角度的正方向,如下图:


然后由上面的欧拉角求出初始化四元数,这时要注意旋转顺序的不同,公式也不同,大部分旋转顺序是Z-Y-X,我的程序里也用的这个顺序,在这里我们按Z-X-Y的顺序来旋转,并得出求四元数的公式以做比较,其旋转矩阵:
q=qyaw*qpitch*qroll=
(cos(0.5*Yaw)+ksin(0.5*Yaw)) * (cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))
得出初始化四元数计算公式如下图所示:


其中i,j,k之间相乘的顺序不能随意变,在前的先计算,在后的后计算相乘的公式如下图:


至此初始化四元数完成。
第三步就是使用IMUUpdate算法了,用完以后再根据公式计算出欧拉角,此公式跟旋转顺序和旋转使用的轴向有关,我们的旋转顺序是Z-X-Y,且绕ZYaw,绕XPitch,绕YRoll,推到过程如下图:
首先得出3个方向余旋矩阵:
下图绕Z轴Yaw:


下图绕X轴pitch:


下图绕YRoll


然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw,如下图:

将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比,即可求出欧拉角,注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的,而下图的四元数姿态矩阵是固定的:


最后一步就是求出欧拉角,公式如下图:


至此,算法完成,参考如下:
//本程序的读写mpu9150部分是在作者_spetrel的基础上修改而来;
//本程序Yaw的互补滤波出自作者john800422;
//本程序采用的gyro accel融合算法出自作者Madgwick
在此感谢以上作者的无私分享和帮助!

附上自己的代码和AHRS IMU原始代码:



----------------------------------------------------------------------------------------------------------------------------
2013年11月28日更新-采用AHRSUpdate算法进行姿态融合:
这段时间一直没有太多时间搞这玩意,不过苦恼多日的AHRSUpdate算法问题终于解决了,该算法直接融合9轴数据,包括加速度计、陀螺仪和磁力计,其中0°
<yaw<360°
, -90°
<pitch<+90°, -180°<roll<180°,并且我定义:Yaw北偏西为正,pitch往上为正,roll“右翼”下沉为正。传感器坐标轴:绕y旋转是roll,绕x旋转为pitch,绕z旋转为yaw。代码里只有姿态更新,更新频率接近400Hz,这还只是跑72Mhz频率下的数据,想要跑108M可以自己修改。

说下改进:
1.由于IMUupdate算法只融合了加速度计和陀螺仪的数据,因此我用了互补滤波来融合磁力计以修正偏航角,但是这种互补滤波有个小问题,就是假如定义偏航角的范围是0-360度,那么当机头大概从北偏西1度转到北偏东364度时,机头会经过0度(360度)这个点,那么这时,yaw不会直接1-0-364这么变化,而是会被逆向积分从1-20-180-270-364这么转一圈,这是个不好的现象,实验了半天也没有解决,而AHRSupdate很好的解决了这个问题。
2.放弃了DMP输出,DMP输出会有部分数据丢失现象,输出频率只有200Hz,虽然也够用,但还是太低了,采用I2C直接读取raw数据,做了简单校准,下一步打算边做精确校准边搞PID控制,然后再加上加速度计的平滑滤波和磁力计的椭圆拟合校准,最好还有陀螺仪的自动零漂校准。
3.考虑到电机的转动频率是100Hz以上,因此我采用加速度计的低通滤波(DLPF)为98Hz,可以有效滤掉电机振动带来的影响,结合平滑滤波效果应该不错。

说下AHRSUpdate算法的缺点:
由于把磁力计的数据融合到了pitch和roll里面,那么当磁力计突然收到强磁场干扰时,会导致四轴失控。亲测过拿手机等物品靠近飞控板时,pitch、roll和yaw的数据有明显变化,变化程度跟物品具有的磁场强度成正比,不过应该是有算法可以滤掉这些固定的外部干扰的。
而IMUupdate只把磁力计融合进了yaw,那么即便有强磁场干扰也不会造成全方位的失控。

按照惯例,上代码,代码里有详细注释:

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册 手机登录

x

评分

参与人数 9威望 +32 收起 理由
无为之益 + 3 很给力!
缺了一块 + 1 很给力!
liuwei4229743 + 1 赞一个!
banjiaozhang + 1 很给力!
whoyzf + 1 很给力!

查看全部评分

     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-8-30 10:34 | 显示全部楼层 |返回版面
目前也在学习电机、电调、桨、电池、遥控等等型号的选择,大概思路了解了,具体都是怎么用的还不太清楚。

170

主题

8592

帖子

3万

积分

技术总监

发表于 2013-8-30 20:15 | 显示全部楼层 |返回版面
LZ做做来我买一个,对思轴有兴趣,不过自己开发太麻烦
     

362

主题

6664

帖子

2万

积分

版主

发表于 2013-8-31 00:04 | 显示全部楼层 |返回版面
楼主做的不错。
这个社会混好的两种人:一是有权有势,二是没脸没皮的。
     

362

主题

6664

帖子

2万

积分

版主

发表于 2013-8-31 00:04 | 显示全部楼层 |返回版面
就是上位机自己写个就更好了。
这个社会混好的两种人:一是有权有势,二是没脸没皮的。
     

362

主题

6664

帖子

2万

积分

版主

发表于 2013-8-31 00:05 | 显示全部楼层 |返回版面
今年的电子设计大赛就得用这个了。
这个社会混好的两种人:一是有权有势,二是没脸没皮的。

0

主题

1

帖子

3

积分

实习生

发表于 2013-9-1 01:45 | 显示全部楼层 |返回版面
楼主 代码呢? 谢谢

2

主题

7

帖子

21

积分

实习生

发表于 2013-9-2 11:44 | 显示全部楼层 |返回版面
期待进展。。。
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-9-2 18:01 | 显示全部楼层 |返回版面
lelee007 发表于 2013-8-30 20:15
LZ做做来我买一个,对思轴有兴趣,不过自己开发太麻烦

自己搞确实费劲,我都不知道什么时候能做出来。。
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-9-2 18:01 | 显示全部楼层 |返回版面
代码和图已补全,要的赶快下手啦。。
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-9-2 18:04 | 显示全部楼层 |返回版面
gaochy1126 发表于 2013-8-31 00:04
就是上位机自己写个就更好了。

上位机别人做的都很好了,我想的是先用串口助手看看数据,行了就拿别人的上位机来看漂亮的效果。。:lol
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-9-2 18:04 | 显示全部楼层 |返回版面
今年的电子大赛快开始了吧?明天?后天?

3

主题

75

帖子

229

积分

中级技术员

发表于 2013-9-2 20:02 | 显示全部楼层 |返回版面
哇塞,密切关注,楼主加油哦

0

主题

3

帖子

9

积分

实习生

发表于 2013-9-3 23:05 | 显示全部楼层 |返回版面
密切关注楼主。
     

116

主题

1916

帖子

6089

积分

版主

发表于 2013-9-4 08:09 | 显示全部楼层 |返回版面
关注,帖子写的不错!
上班,它是一种谋生手段;下班,它是一种娱乐方式!

0

主题

46

帖子

148

积分

中级技术员

发表于 2013-9-4 14:30 | 显示全部楼层 |返回版面
关注楼主  加油!
     

64

主题

255

帖子

286

积分

禁止发言

 楼主| 发表于 2013-9-5 09:33 | 显示全部楼层 |返回版面
关于PID控制中的2个参数Kp和Ki忘了说了,在这补上:

首先是理论:
PID算法中,需要整定的系数是Kp(比例系数),Ki(积分系数),Kd(微分系数)三个。这三个参数对系统性能的影响如下:
(1) 比例系数 Kp
① 对动态性能的影响 比例系数Kp加大,使系统的动作灵敏,速度加快,Kp偏大,振荡次数加多,调节时间加长。当Kp太大时,系统会趋于不稳定,若Kp太小,又会使系统的动作缓慢;
② 对稳态性能的影响 加大比例系数Kp,在系统稳定的情况下,可以减小静差,提高控制精度,但是加大Kp只是减少静差,不能完全消除。
(2) 积分系数 Ki
① 对动态性能的影响 积分系数Ki通常使系统的稳定性下降。Ki太大,系统将不稳定;Ki偏大,振荡次数较多;Ki太小,对系统性能的影响减少;而当Ki合适时,过渡特性比较理想;
② 对稳态性能的影响 积分系数能消除系统的静差,提高控制系统的控制精度。但是若Ki太小时,积分作用太弱,以致不能减小静差。
(3) 微分系数 Kd
微分控制可以改善动态特性,如超调量减少,调节时间缩短,允许加大比例控制,使静差减小,提高控制精度。但当Kd偏大或偏小时,超调量较大,调节时间较长,只有合适的时候,才可以得到比较满意的过渡过程。
对系数实行“先比例,后积分,再微分”的整定步骤。
(1) 首先只整定比例部分。即将比例系数由小到大,并观察相应的系统响应,直到得到反应快,超调小的响应。
(2) 加入积分环节。整定时首先置积分系数Ki一个较小的值,并将第(1)步中整定的比例系数略为缩小(例如缩小为原值的0.8倍),然后增大Ki,使在保持系统良好动态性能的情况下,静差得到消除。在此过程中,可根据响应的好坏反复改变比例系数与积分系数。
(3) 若使用比例积分调节器消除了静差,但动态过程经反复调整仍不能满意,则可加入微分环节。在整定时,可先置微分系数为0,在第一步的基础上,增大Kd,同时相应地改变比例系数。


以下是在没有起飞之前的一些个人经验:
一般kp我是在1-5之内来回调,太大了感觉角度会超调然后再回来,就是理论中的震荡,太小了感觉角度变化的慢,就是当四轴快速翻转到一定角度时,系统输出的角度还在慢慢的变化,没有很快到达实际的角度;
kp定下来之后再调ki,ki的话,我觉得0.001-0.5都可以,感觉上没什么影响,一般我是每隔0.1或0.3调1次,直到得出的角度比较理想为止。
在不起飞时,一般使用原代码默认的kp=2,ki=0.005即可。

33

主题

1441

帖子

4419

积分

中级工程师

发表于 2013-9-6 12:24 | 显示全部楼层 |返回版面
太棒了!
      

2

主题

189

帖子

577

积分

高级技术员

发表于 2013-9-6 15:50 | 显示全部楼层 |返回版面
真心需要开源,向楼主学习了!
     

4391

主题

5949

帖子

2030

积分

初级工程师

发表于 2013-9-6 23:55 | 显示全部楼层 |返回版面
看起来很专业的样子哦
我为我自己代言...
链接:一线研发之声系列汇集
链接:Sedatefire的笔记
我发心,待人以诚,处事以真,帮助菜鸟,凝听异见,成就自我..
您需要登录后才可以回帖 登录 | 注册 手机登录

本版积分规则

分享 快速回复 返回顶部 返回列表