四轴飞行器Mahony姿态结算

首先我们用一个图解搞明白姿态结算在飞控中的位置：

简而言之就是将采集的数据通过算法转换为可以供姿态控制使用的欧拉角，即较为精准的数据

转换思路：

在计算的时候我们一般将欧拉角（当然也可以叫做方向角）转换成四元数来计算：飞行的姿态可以用欧拉角描述的方向余弦矩阵来表示，因为欧拉角存在万向节“死锁”的问题，所以不适合全姿态飞行器，飞行的姿态也可以用四元数描述的方向余弦矩阵来表示。两者的方向余弦矩阵数据一一对应。所以我们将四元数作为中间量进行运算，最后的结果转换为欧拉角，这样就可以解决“死锁”问题了。

什么是方向余弦矩阵？

你可以看到我们这里的公式是按z，x，y顺序旋转的，前提是你知道一些向量的知识，由于是绕固定的坐标轴，所以，计算旋转矩阵的时候的时候，是先乘以y的向量，再乘以x的向量，最后乘以z的向量，是反过来的；

简单补充：而如果你是绕着欧拉角旋转的话，每一次旋转以上一次旋转结束的坐标轴为参考轴，那么按z，x，y顺序旋转的话，矩阵计算的相乘顺序和旋转顺序一致（右乘），聪明的你可能已经发现，以固定角按z，x，y顺序旋转得到的姿态和以欧拉角按y，x，z旋转的顺序一致。简单的证明方法就是他们的计算公式一致；

（旋转矩阵也是上面提到的方向余弦矩阵，由于可以使得两个不同坐标系下的坐标相互转换，所以也可以这么叫）

如何用四元数来表示姿态矩阵？

编辑

什么是四元数？

在姿态融合解算的时候常常使用“四元数”来表示姿态，它由三个实数及一个虚数组成，因而被称之为四元数。使用四元数表示姿态并不直观，但因为使用欧拉角(即前面说的偏航角、横滚角及俯仰角)表示姿态的时候会有“万向节死锁”问题，且运算比较复杂，所以一般在数据处理的时候会使用四元数，处理完毕后再把四元数转换成欧拉角。

如何求解四元数？

利用一阶龙格塔公式就可以结合陀螺仪测出的角速度（Mahony算法：结合加速度计的测量值进行算法补偿，减少误差）数据来结算出四元数的值，进而求出对应欧拉角的三角函数值，进而结算出欧拉角。

这里要注意一点欧拉角描述的坐标旋转矩阵可能会因为x，y，z轴不同的旋转顺序导致得到的坐标矩阵不同。这就导致了一个问题，不同的绕定轴旋转顺序就对应不同的反解欧拉角公式（方向余弦矩阵不同），例如绕z，y，x与绕z，x，y得到的欧拉角描述的旋转矩阵是不一样的；

问：在姿态解算算法中如何确定绕定轴旋转顺序从而确定四元数反解欧拉角的公式呢？这个问题呢，可以理解为不同旋转顺序对四元数反解欧拉角的公式是否一一对应，或者说四元数表示的姿态矩阵是否唯一；

显然四元数的表达式就是我们上面所说的一种，即是惟一的，所有的旋转矩阵都和他对应，所以如果旋转的顺序不同，反解公式也会有所不同；在这里你需要知道一件事就是；比如同样的两次旋转，绕定轴XYZ方向的旋转角度都一致，但是如果旋转次序不同，那么旋转的结果也是不同的。这里用旋转矩阵的求法：如矩阵ABC与ACB的结果显然不一样，对应的姿态也不同；所以这里如果你改变了旋转顺序，那么反解公式必然和之前不同，需要重新确立，否则，姿态必然是会改变的。

如果按照我们的z，x，y顺序旋转，将两个姿态矩阵每个元素对应，可以得到如下关于欧拉角的式子：

为什么要进行数据融合（误差是如何产生的）？

可以发现，使用陀螺仪检测角度时，在静止状态下存在缺陷，且受时间影响。而加速度传感器检测角度时，在运动状态下存在缺陷，且不受时间影响，刚好互补。假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重，当物体处于运动状时，增大陀螺仪数据的权重，从而获得更准确的姿态数据。

同理，检测偏航角，当载体在静止状态时，可增大磁场检测器数据的权重，当载体在运动状态时，增大陀螺仪和GPS检测数据的权重。这些采用多种传感器数据来检测姿态的处理算法被称为姿态融合。

下面来说一说误差补偿（Mahony算法如何实现）

如何将陀螺仪数据与角速度数据建立联系？

在导航坐标系（设为n系）下有重力加速度向量g，那么将g旋转到机体坐标系（设为b系）中就得到了机体坐标系下的重力加速度v，（这里是不是有点似曾相识，因为之前我们求的方向余弦矩阵正是用来描述b系相对于n系的旋转，这里求重力加速度，就只需要用b系相对于n系的旋转的方向余弦矩阵乘以n系下的重力加速度g 即可）

什么是导航坐标系，什么是机体坐标系？

为什么将g旋转到机体坐标系（设为b系）中?

无论机体怎么旋转重力向量在世界坐标系中都是[0，0，1]，但是我们有加速度计读到的值是基于机体坐标系的。现在就明白了，刚才为什么要用四元数将[0，0，1]乘一个旋转矩阵，我们之后关于加速度计和陀螺仪的计算都是基于机体坐标系的。

相乘后我们发现b系下的加速度v恰好是b系相对于n系的旋转的方向余弦矩阵的第三列：所以四元数描述的姿态可以与机体的重力加速度之间相互推导。这样正推就可以得到理论的加速度。

如果没有误差的话那么这里用四元数推导的重力加速度应该和加速度计测量的一样，但是测量值往往难免存在误差。

所以计算误差的方法是将理论加速度g的单位向量与测量加速度v的单位向量进行叉乘，结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。如果这里描述的有些不清楚，可以回过头去看看之前的示意图；（这里的单位向量也就解释了代码中为什么将加速度向量归一了）

为什么使用叉乘求误差？

这个进行叉积得到的误差结果近似等于当期时刻相对于上一时刻旋转的角度，这个角度即是旋转角度和预测误差角度的叠加，总称为向量误差，角度越大，误差越大。这里是不是很神奇，我们用加速度计的值间接得出了误差变化的角度相关值。

如何找到一个另一个角速度量纲的值来修正陀螺仪的角速度值呢？

这里明明只有陀螺仪可以测量角速度，这时候前面提到的向量叉积得到的误差向量就帮上大忙了，这个误差向量不就是反映出了角度的变化量吗。算法巧妙的将加速度相关量转化为角度相关量，因而可以用这个角度值乘一个系数来修正陀螺仪的角速度（PI补偿），因为在偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系，也就说明陀螺仪积分误差和向量叉积存在正比关系。所以我们只要确定合适的系数，就可以对陀螺仪数据进行补偿

补偿计算公式及代码：

    //PI（PID）算法补偿，积分处理
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;
    //将计算到的补偿值加到陀螺仪数据上
    gx = gx + Kp*ex + exInt;
    gy = gy + Kp*ey + eyInt;
    gz = gz + Kp*ez + ezInt;