姿态解算基础：欧拉角、方向余弦、四元数

什么是姿态解算：

    飞行器的姿态解算过程涉及到两个坐标系，一个是运载体的机体坐标系，该坐标系与运载体固连，当运载体转动的时候，这个坐标系也跟着转动，我们假设运载体的坐标系为b系。另外一个是地理坐标系，即东北天坐标系，X轴指向东，Y轴指向北，Z轴指向天，这个坐标系也称为导航坐标系，我们假设这个坐标系为n系。

    所谓的姿态解算，就是求出飞行器当前的姿态，也就是求出飞行器上固连的机体坐标系b相对于地理坐标系n的变化。由于n系和b 系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可对一个坐标系作平移，使其原点与另一个坐标系的原点重合。因此，两坐标系间的空间角位置关系可理解成刚体的定点转动。而刚体定点转动这个变化可以通过一个变换矩阵来表示，这个矩阵包含了刚体的所有姿态信息。对于飞行器来说，可以根据这个变换矩阵求出偏航角、俯仰角、翻滚角。可以这么理解这个变换矩阵：对于机体坐标系中的一个向量，当它左乘这个变换矩阵之后，就能得出这个向量在地理坐标系中的大小。

    由b系到n系的坐标变换短阵也称为运载体的姿态矩阵，而我们姿态解算的目标就是根据惯性器件的输出实时计算出这个姿态矩阵，从而实时获取飞行器当前的偏航、俯仰角、翻滚角。陀螺和加速度计分别用来测量运载体的角运动信息和线运动信息，根据陀螺仪和加速度计的输出，我们就能实时跟新这个姿态矩阵。在姿态解算中，

    用来表示姿态矩阵的方式有三种，分别是欧拉角、方向余弦、四元数。下面我们一一介绍。

欧拉角表示姿态矩阵：

以下来自秦永元的《惯性导航》。

1、坐标系旋转：

    该式描述了同一矢量在不同坐标系内投影间的变换关系，C称为从坐标系1至坐标系2的变换短阵。上述变换关系分析中，坐标系2是经坐标系1仅绕Z1轴旋转α角后获得的，为便于叙述，称仅绕一根轴的旋转为基本旋转。两坐标系间任何复杂的角位置关系都可以看作有限次基本旋转的复合，变换矩阵等于基本旋转确定的变换短阵的连乘，连乘顺序依基本旋转的先后次序由右向左排列。例如运载体的空间姿态可看作依次绕偏航轴、俯仰轴、翻滚轴作基本旋转后的复合结果。

2、用欧拉角表示旋转矩阵：

    对于飞行器而言，由地理坐标系O-XeYeZe向机体坐标系O-ZbYbZb到的变换过程可以分为三步（假设初始时机体坐标系和地理坐标系重合）：

1）第一步是绕机体坐标系的Z轴逆时针旋转，得到偏航角Yaw，大小为Ψ，旋转矩阵（这里是从机体坐标系变为地理坐标系的反向旋转矩阵，注意与上面的坐标变换区分）为：

    假设旋转前向量为X0, 旋转后坐标向量为X1，则X0 = Rz(Ψ) * X1 。

2）第二步是绕机体坐标系的Y轴逆时针旋转，得到俯仰角Pitch，大小为θ，旋转矩阵为