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

[复制链接]
111527|549
手机看帖
扫描二维码
随时随地手机跟帖
zkcaptain|  楼主 | 2013-8-30 10:20 | 显示全部楼层 |阅读模式
本帖最后由 zkcaptain 于 2014-6-19 18:11 编辑

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

2014年6月16日更新:
PID+乱七八糟心得部分,内有完整代码和试飞视频,见该帖:https://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效果图,实物图就不上了,太丑,测试版,先追求调通得出姿态角:
3d.JPG

第二部分:软件
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、开始姿态融合,见下图:
111.JPG

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

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

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

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

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

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

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

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

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

第四部分:算法
第一步是校准,加速度计和陀螺仪我用的是MPU9150内部自校准,磁力计的校准采用如下方法:
磁力计校准.pdf (268.69 KB)
评论
哼唧哼唧 2020-4-9 09:59 回复TA
呜呜呜,楼主我用ahrs 怎么在-179到179度它还是有个波动啊?搞了一个月了自闭了 
评分
参与人数 9威望 +32 收起 理由
无为之益 + 3 很给力!
缺了一块 + 1 很给力!
liuwei4229743 + 1 赞一个!
banjiaozhang + 1 很给力!
whoyzf + 1 很给力!

查看全部评分

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

使用特权

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

使用特权

评论回复
gaochy1126| | 2013-8-31 00:04 | 显示全部楼层
楼主做的不错。

使用特权

评论回复
gaochy1126| | 2013-8-31 00:04 | 显示全部楼层
就是上位机自己写个就更好了。

使用特权

评论回复
gaochy1126| | 2013-8-31 00:05 | 显示全部楼层
今年的电子设计大赛就得用这个了。

使用特权

评论回复
taimuhanma| | 2013-9-1 01:45 | 显示全部楼层
楼主 代码呢? 谢谢

使用特权

评论回复
ZHOUXIANGJUN| | 2013-9-2 11:44 | 显示全部楼层
期待进展。。。

使用特权

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

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

使用特权

评论回复
zkcaptain|  楼主 | 2013-9-2 18:01 | 显示全部楼层
代码和图已补全,要的赶快下手啦。。

使用特权

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

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

使用特权

评论回复
zkcaptain|  楼主 | 2013-9-2 18:04 | 显示全部楼层
今年的电子大赛快开始了吧?明天?后天?

使用特权

评论回复
哈利| | 2013-9-2 20:02 | 显示全部楼层
哇塞,密切关注,楼主加油哦

使用特权

评论回复
loveliang1986| | 2013-9-3 23:05 | 显示全部楼层
密切关注楼主。

使用特权

评论回复
blackrui| | 2013-9-4 14:30 | 显示全部楼层
关注楼主  加油!

使用特权

评论回复
zkcaptain|  楼主 | 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即可。

使用特权

评论回复
cool_coder| | 2013-9-6 12:24 | 显示全部楼层
太棒了!

使用特权

评论回复
zdsine| | 2013-9-6 15:50 | 显示全部楼层
真心需要开源,向楼主学习了!

使用特权

评论回复
sedatefire| | 2013-9-6 23:55 | 显示全部楼层
看起来很专业的样子哦

使用特权

评论回复
sedatefire| | 2013-9-6 23:55 | 显示全部楼层
飞行,果然不是简单的

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则

64

主题

255

帖子

34

粉丝