接下来的这段时间，我将在这分享我对姿态解算的一些小心得

姿态解算的目的：得到机体坐标系相对地理坐标系的姿态

那么什么是姿态呢？

姿态就是机体绕机体三个轴旋转的角度所组成的一个矩阵，比如机体坐标系由XYZ组成，假设默认刚开始机体坐标系与地理坐标系重合，机体绕机体坐标系X轴旋转再绕Y轴旋转最后绕Z轴旋转得到此时的机体位置，我们就称分别以XYZ顺序绕XYZ三轴旋转的这种旋转关系就称为姿态，对应的旋转角度称为三轴姿态角，姿态是旋转顺序和旋转角度一起组合的一种描述

欧拉角是姿态最形象的一种表达，以欧拉角来介绍一下姿态，

首先当我们定义了地理坐标系后我们会默认地理坐标系与机体初始坐标系重合；

确定好机体坐标系后需要我们选定机体旋转顺序常用旋转顺序为ZYX的旋转顺序；

确认好旋转顺序后需要我们选定Z轴 Y轴 X轴对应的旋转角度，例如Z轴对应theta_yaw，Y轴对应theta_roll，X轴对应theta_pitch；

接着我们需要理解坐标系，一是最开始我们自己定义的地理坐标系（正交），二是机体上的机体坐标（正交），三是机体旋转产生轴所组成的旋转轴坐标系（非正交），前面两个坐标系很好理解，重点说明一下机体旋转轴产生的非正交坐标系：还是假设我们选定机体旋转顺序为ZYX，先是绕Z轴旋转产生旋转轴坐标Z轴，此时旋转Z轴与机体Z轴重合，接着绕Y轴旋转产生旋转轴坐标系Y轴，此时旋转轴坐标Y轴与机体坐标系Y轴重合，但旋转轴坐标系Z轴与机体坐标系Z轴不重合了，接着绕X轴旋转产生旋转轴坐标X轴，此时旋转轴坐标X轴与机体坐标系X轴重合，但是旋转轴坐标系X轴、Y轴都不与机体坐标系重合了，由此可以理解旋转轴坐标系，并且可以清楚欧拉角其实就是旋转轴产生的角度，假设我们知道旋转轴坐标系的三轴角速度，那么直接对旋转轴坐标系下的三轴角速度进行积分就能得到角度了（这其实也就是陀螺仪得到欧拉角的原理，后续会有更加详细的分析如何使用IMU得到欧拉角）

小结一下：欧拉角是旋转轴坐标系三轴旋转的角度，该角度可以理解为机体坐标系与大地坐标系之间旋转关系的一种表达

那么姿态的表达方式有哪些呢？

表达方式有三种，分别是欧拉角，万向余弦矩阵，四元数。

这里会展开介绍一下欧拉角与万向余弦矩阵，由于四元数这种表达方式极其抽象，但是却很方便应用，因此四元数的方式我们关注在如何具体应用上

欧拉角

欧拉角正是上面我们提到的机体绕机体三个轴旋转的三个角度称为欧拉角。欧拉角可以很形象的表示出机体姿态，但却存在万向节死锁问题，是最形象的表达方式但由于万向节死锁问题所以并不是最理想的表达方式。关于什么是万向节死锁问题我们放在后面进行介绍。

万向余弦矩阵

连续3轴的旋转对应的3个旋转矩阵的有序乘积，称为方向余弦矩阵DCM（direction cos matrix），以下简称D矩阵

万向余弦矩阵需要理解旋转矩阵的运算以及万向余弦矩阵提取重力加速度微分方程

假设存在A坐标系（正交），C坐标系（正交）由A坐标系按照ZYX的旋转顺序得到，已知C坐标系上存在p向量，求p向量在A坐标系上的表达p’

A_T_C = rotz(yaw) * roty(roll) * rotx(pitch)

p’ = A_T_C * p

四元数

由q0 q1 q2 q3组成，一般只需要记住四元数的更新方程和四元数提取重力加速度方向即可

此外针对欧拉角、万向余弦矩阵、四元数都有相互变换的公式

万向节死锁问题

简单描述就是后面的轴旋转无法影响前面轴

导致万向节死锁的原因是第二个旋转轴旋转了90度导致第一和第三个轴重合而产生了万向节死锁

比如机体旋转顺序为ZYX

先绕Z轴旋转10度 再绕Y轴旋转90度 最后绕X轴旋转5度，

转完Y轴90度后想将绕X轴旋转5度由于此时第一次后机体的X轴与原大地坐标系的Z轴重合了，

所以会等效于大地坐标系先绕Z轴旋转15度 再绕Y轴旋转90度

所以就丢失了一个自由度，这就是万向节死锁