姿态解算算法推导

学习视频：https://www.bilibili.com/video/BV1V7411g7h7?p=6&spm_id_from=pageDriver

一、四元数、欧拉角、方向余弦阵在姿态解算中使用

姿态角是由旋转产生的，一般旋转有4种表示方式:方向余弦矩阵表示、欧拉角表示、轴角表示、四元数表示。其中方向余弦矩阵表示适合变换向量，欧拉角最直观，轴角表示适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量。
所以我们使用四元数来保存飞行器的姿态（也就是飞机在地球坐标系中的俯仰/横滚/航向状态)。姿态控制算法的输入参数必须要是欧拉角。所以在需要控制的时候，会将四元数转化为欧拉角，但是四元数转化成欧拉角又需要借助方向余弦阵来实现。
姿态解算到姿态控制的整个流程。MPU9250输出的3个维度的陀螺仪值、3个维度的加速度值、3个维度的磁力计值，每个值为16位精度。MPU9250输出的AD值通过姿态解算算法得到飞行器当前的姿态（姿态使用四元数表示)，然后将四元数转化为欧拉角，用于姿态控制算法（PID控制）中。

二、Mahony互补滤波算法分析

1．欧拉角描述的方向余弦矩阵

定理:空间中坐标系的任意旋转，等效于依次绕三个坐标轴的定轴旋转

如上图所示紫色的坐标系记为b系，蓝色的坐标系记为n系。b系是由n系任意旋转得到。那么这个旋转等效于绕Xn轴旋转，绕Yn轴旋转，绕Zn轴旋转的叠加。但是这种旋转怎么表示呢？换句话说p点旋转到p’他们的坐标有什么对应关系呢?
下面我们以绕Zn 轴旋转为例，推导一下空间旋转过程中坐标是怎么样转化的:



将上述三个式子写成矩阵形式:

整理一下：

这样我们就用有欧拉角$\alpha$组成的 $C_1^2$描述了这次旋转。那么同理我们也可以推出图12-7的三个旋转阵。

接着我们将这三个旋转阵，组合成一个旋转矩阵。其实也就是将这三个矩阵进行乘法运算（左乘)。运算如下:

那么这个$C_n^b$。矩阵就是方向余弦矩阵（也称旋转矩阵),这个矩阵也就描述了b 系相对于n系的旋转。如果我们知道任意向量在b系中的坐标，那么就可以利用方向余弦矩阵确定它在n系中的坐标,反之亦然。
这个方向余弦矩阵还有一个特点就是这个矩阵的元素都是由欧拉角$\theta 、\gamma 、\psi$的正弦和余弦的组合成的,而我们需要的姿态也就是这三个角,那么我们该怎么求出$\theta 、\gamma 、\psi$呢?那我们只需套用欧拉角的微分方程就可以解出这三个角，但是由于欧拉角微分方程中包含了大量的三角运算,这给实时解算带来了一定的困难而且当俯仰角为90度时方程式会出现神奇的“死锁”。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。
下面我们继续寻找看看有没有其他方法更简便的方法,现在我们将$C_b^n$记做:

由于n系至b系旋转过程中坐标系始终保持直角坐标系，所以$C_n^b$是正交矩阵也就有了:

也就是说矩阵$C_b^n$可以将b系的向量转换到n系表示，矩阵$C_n^b$可以将n系的向量转换到b系表示。
仔细观察上面式子可以得到如下式子:

也就是如果我们能求出，$C_n^b$矩阵中的T32、T31、T12、T22的值就可以求出这个三个欧拉角。

2.四元数描述的方向余弦矩阵

**思考:**既然欧拉角能描述方向余弦矩阵，那么我们是否也能用四元数来描述方向余弦矩阵呢?
答案是肯定的，四元数描述的方向余弦矩阵$C_n^b$如下:

具体的推导过程大家参考:四元数与姿态阵间的关系——《惯性导航》–秦永元P292-297。
既然方向余弦矩阵$C_n^b$的元素，都已经被四元数的四个变量给替换掉了。那么我们求解姿态的问题就转化成了求解这一组四元数的问题了。

一旦求解出qo,q1,q2,q3,也就能反解出欧拉角了。

3.求解四元数

通过前面的分析我知道，一组四元数已经足够表达一个飞行器的完整姿态了，因为通过四元数可以构建出一个方向余弦矩阵了，而方向余弦矩阵正是由于飞行器在空间中发生旋转所引起的,因此我们也就可以说一组四元数就包含了一个完整的旋转过程，也就我们说的飞行姿态。那么我们想要得到飞行姿态，只需要实时更新四元数就行了。
那么我们就可以构建四元数关于时间的微分方程,来研究四元数的变化过程。因为考虑到陀螺仪测量的角速度,所以我们用四元数的三角表示式来建立一个微分方程，如果可以求解出该微分方程，那么也就成功的解出了四元数。
三角表示式:

令四元数对时间t进行微分，可得到微分方程化简后如下:

记为:$dQ/dt=\Phi \cdot Q$；接下来就剩下求解这个微分方程了，但是求解这个微分方程，还需要考虑单片机只能处理离散数据的特点。因此选择一阶龙格库塔法来求解微分方程，一阶龙格库塔法可以通过有限次迭代求解出函数的微分方程的解。
设有微分方程:

根据一阶龙格库塔法可以得到求解y的迭代公式:

套用公式可得四元数微分方程:

写成矩阵形式如下:

观察上面的公式:$[q_0{q}_1{q}_2{q}_3{]}_{t+\Delta t}^T$是当前四元数，$[g_0{q}_1{q}_2{q}_3{]}_t^T$是上个周期的四元数，$\Delta t$是计算周期，最后这个矩阵里的元素是各轴角速度$（W_x,W_y,W_z)$与四元数$（q_0,q_1,q_2,q_3）$的组合。角速度$（W_x,W_y,W_z)$数据是可以通过陀螺仪测出来的，如果陀螺仪的数据是理想状态，那么我们就可以求出准确的四元数。但是事与愿违由于噪声的影响会引入误差，因此我们需要使用加速度计和磁力计对陀螺仪数据进行校准。所以说我们在套用上面公式求解四元数时，一定要先保证陀螺仪数据的可靠性。

4．传感器数据的融合

陀螺仪,测量角速度，具有高动态特性,它是一个间接测量角度的器件。它测量的是角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移
假设陀螺仪固定不动，理想角速度值是0dps(degree per second)，但是有一个偏置0.1dps 加在上面，于是测量出来是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈，也就是说，陀螺仪在短时间内有很大的参考价值。使用陀螺仪获得角度，一定要考虑积分误差的问题
加速度计，测量当前加速度(包含重力加速度g)的方向(也叫重力感应器)，在悬停时，输出为g。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。
加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说无法感知这种水平旋转。

上面解四元数微分方程的公式里，直接用到了角速度数据，所以我们需要尽可能减小陀螺仪的积分误差,我们数据融合的思路是用加速度数据来校准陀螺仪数据的积分误差。那么我们该怎么让陀螺仪数据与加速度数据建立关系呢?
这时候我们就需要用方向余弦阵了:

设在导航坐标系(n系)下有重力加速度向量g,把 $g_n$旋转到机体坐标系(b 系)，得到其机体坐标系的重力加速度向量$v_b$,则两者的转换关系可以用方向余弦矩阵$C_n^b$来实现转换如下:

观察上计算结果我们可以发现$[v_{bx}{v}_{by}{v}_{bz}{]}^T=[T_{31}{T}_{32}{T}_{33}{]}^T$,我们发现转换到b系下的$[v_{bx}{v}_{by}{v}_{bz}{]}^T$恰好是方向余弦矩阵$C_n^b$的第三列，并且第三列是用四元数进行表示的，也就是说四元数和机体的重力加速度是可以相互推导的，并且四元数又是用角速度数据求出来的。那么关系如下:

由图12-11可知四元数和理论重力加速度($v_b$)可以相互推导得出，理论上四元数推出来的$v_b$应该和加速度计测量的实际重力加速度($a_b$)完全重合（$v_b$,$a_b$都是向量)，但是由于四元数是由角速度数据解算出来的，由于角速度数据有误差，导致了$v_b$和$a_b$不再重合，那么我们此时就可以通过向量的叉积（外积)，来度量这个误差的大小。

5.误差补偿

上面已经利用向量的叉积度量了$v_b$和$a_b$的误差error,那么这个error该如何补偿到陀螺仪数据上呢?
我们这里需要借助PID控制思想，用PI(比例-积分)控制器来控制补偿值的大小，PI控制器公式如下:

利用比例项来控制传感器的可信度,其中Kp越大就越相信加速度计，Kp越小就越相信陀螺仪，积分项来消除静态误差。
把PI控制器计算出来的补偿值加在角速度上，既可得到可信度较高的陀螺仪数据，然后再带入到一阶龙格库塔法公式里就可以得到当前的四元数。

由衷的感谢小马哥的课程讲解，让我可以入门姿态解算算法。