【转载】从头开始探讨做四轴飞控之姿态程序解读【六】

china

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

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

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

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

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

第四部分：算法

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

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

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

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

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

其中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))

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

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

至此初始化四元数完成。

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

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

下图绕Z轴Yaw：

下图绕X轴pitch：

下图绕Y轴Roll：

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

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

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

谦谦君子102

分析的不错，不过我还有很多地方不明白，希望以后多跟楼主学习