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

1万 · 2014-2-26 17:00

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

1万 · 2014-2-26 17:00

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

1万 · 2014-2-26 17:01

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

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

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

1万 · 2014-2-26 17:01

下图，旋转前是[x y z]，旋转后就是[-y x z]：

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

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

1万 · 2014-2-26 17:01

第四部分：算法

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

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

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

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

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

1万 · 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))

1万 · 2014-2-26 17:02

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

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

至此初始化四元数完成。

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

1万 · 2014-2-26 17:02

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

下图绕Z轴Yaw：

下图绕X轴pitch：

下图绕Y轴Roll：

1万 · 2014-2-26 17:03

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

1万 · 2014-2-26 17:04

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

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

1万 · 2014-2-26 17:05

以下是静止时，刚上电时的数据：

1万 · 2014-2-26 17:05

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

1万 · 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下串口乱码问题