发新帖我要提问
123
返回列表

【2月26日领奖】 GD32MCU.com网站上线啦!快来寻宝赢大礼!

[复制链接]
楼主: 21小跑堂
手机看帖
扫描二维码
随时随地手机跟帖
mmbs| | 2014-2-26 17:00 | 显示全部楼层

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


使用特权

评论回复
mmbs| | 2014-2-26 17:00 | 显示全部楼层

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


使用特权

评论回复
mmbs| | 2014-2-26 17:01 | 显示全部楼层

第三部分:如何确定自己的轴向

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


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


使用特权

评论回复
mmbs| | 2014-2-26 17:01 | 显示全部楼层

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

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

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


使用特权

评论回复
mmbs| | 2014-2-26 17:01 | 显示全部楼层

第四部分:算法

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

第二步是初始化四元数,常见的轴向定义是绕x轴旋转是Roll,绕y轴旋转是Pitch,绕z轴旋转是Yaw,我的程序中也有这样的定义,不过被我注释掉了,这里举个另外一种轴向定义来对初始化四元数进行说明,方便比较,也是我目前正在用的轴定义。

下面我们来定义绕x轴旋转是Pitch,绕y轴旋转是Roll,绕z轴旋转是Yaw,轴向的正方向如上图一样,不变。

先对加速度计和磁力计的数据进行处理,得到init_xx来供我们使用如下图:

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


使用特权

评论回复
mmbs| | 2014-2-26 17:01 | 显示全部楼层

其中Yaw的正方向未必对,可以自己去验证下,具体参考公式见附件-ST电子罗盘计算Yaw

然后由上面的欧拉角求出初始化四元数,这时要注意旋转顺序的不同,公式也不同,大部分旋转顺序是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))


使用特权

评论回复
mmbs| | 2014-2-26 17:02 | 显示全部楼层

得出初始化四元数计算公式如下图所示:

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

至此初始化四元数完成。

第三步就是使用AHRSUpdate算法了,用完以后再根据公式计算出欧拉角,此公式跟旋转顺序和旋转使用的轴向有关,我们的旋转顺序是Z-X-Y,且绕Z是Yaw,绕X是Pitch,绕Y是Roll,推到过程如下图:


使用特权

评论回复
mmbs| | 2014-2-26 17:02 | 显示全部楼层

首先得出3个方向余旋矩阵:

下图绕Z轴Yaw:

下图绕X轴pitch:

下图绕Y轴Roll:


使用特权

评论回复
mmbs| | 2014-2-26 17:03 | 显示全部楼层

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




使用特权

评论回复
mmbs| | 2014-2-26 17:04 | 显示全部楼层

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


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


使用特权

评论回复
mmbs| | 2014-2-26 17:05 | 显示全部楼层

以下是静止时,刚上电时的数据:


使用特权

评论回复
mmbs| | 2014-2-26 17:05 | 显示全部楼层

以下是上电半个小时以后的数据:


使用特权

评论回复
mmbs| | 2014-2-26 17:05 | 显示全部楼层

可以看出正常的静止状态下,数据的波动范围是不超过1度的,也不会有漂移。具体效果还需要上机架飞起来以后再做调整。


最后总结一下:

我的代码中,0°<yaw<360°, -90°<pitch<+90°,-180°<roll<180°,并且我定义:Yaw北偏西为正,pitch往上为正,roll“右翼”下沉为正。传感器坐标轴:绕y旋转是 roll,绕x旋转为pitch,绕z旋转为yaw。代码里只有姿态更新,72Mhz主频下更新频率在350-400Hz,108Mhz下可以达到400-500Hz,目前我采用定时器定时,代码跑108Mhz,使姿态更新频率控制在400Hz。

注意跑108Mhz时,需要修改延迟函数和串口函数,适当增加一下延迟函数的数值避免I2C通信失败,修改串口函数是为了避免108Mhz下串口乱码问题


使用特权

评论回复
骗子的片子| | 2014-2-26 18:51 | 显示全部楼层
恭喜恭喜

使用特权

评论回复
LED2013| | 2014-2-26 19:39 | 显示全部楼层
本帖最后由 LED2013 于 2014-2-26 19:41 编辑

http://gd32mcu.21ic.com/project/detail/project_id/8





1.jpg

使用特权

评论回复
mmbs| | 2014-2-26 20:34 | 显示全部楼层

使用特权

评论回复
mmbs| | 2014-2-26 20:34 | 显示全部楼层

使用特权

评论回复
mmbs| | 2014-2-26 20:36 | 显示全部楼层

使用特权

评论回复
mmbs| | 2014-2-26 20:36 | 显示全部楼层
等待发奖品了

使用特权

评论回复
Harvard| | 2014-2-27 11:16 | 显示全部楼层
跟一个 支持国产gd32

使用特权

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

本版积分规则