四轴飞行器Mahony姿态结算

首先我们要搞明白姿态结算在飞控中的位置：

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

转换思路： 

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

什么是方向余弦矩阵？

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

如何求解四元数？

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

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

什么是四元数？

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

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

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

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

下面来说一说误差补偿（Mahony算法）

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

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