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

什么是姿态解算：

    飞行器的姿态解算过程涉及到两个坐标系，一个是运载体的机体坐标系，该坐标系与运载体固连，当运载体转动的时候，这个坐标系也跟着转动，我们假设运载体的坐标系为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，大小为θ，旋转矩阵为（这里是从机体坐标系变为地理坐标系的反向旋转矩阵）：

    假设旋转前向量为X1, 旋转后坐标向量为X2，则X1 = Ry(θ) * X2。

3）第三步是绕机体坐标系的X轴逆时针旋转，得到翻滚角Roll，大小为Φ，旋转矩阵为（这里是从机体坐标系变为地理坐标系的反向旋转矩阵）：

    假设旋转前向量为X2, 旋转后坐标向量为X3，则X2 = Rx(Φ) * X3

4）假设旋转前向量为X0，经过三次旋转后的向量变为X3，则由X3变换到X0的步骤为：

X0=Rz(Ψ) * Ry(θ) * Rx(Φ) * X3

那么此时的从机体坐标系到地理坐标系的旋转矩阵为：

R   =   Rz(Ψ)  *  Ry(θ)  *  Rx(Φ)

这个姿态矩阵就是用三个欧拉角（偏航、俯仰、翻滚）来表示的，这个第一种表示方式。

方向余弦表示姿态矩阵：

     以下来自Starlino_DCM_Tutorial.pdf这篇文章，原文：http://www.starlino.com/dcm_tutorial.html，本文参考：https://www.cnblogs.com/andychenforever/p/6298073.html。

     我们将本体坐标系定义为O-xyz，另外一个全局坐标系定义为O-XYZ，两坐标系的具有相同的原点O，如图所示。本体坐标系的x、y、z轴对应的单位向量分别为i，j，k；全局坐标系X、Y、Z轴对应的单位向量分别为I，J，K。

这就是用方向余弦表示姿态矩阵，这是第二种表示方式。

四元数表示姿态矩阵：

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

四元数介绍：

四元数表示姿态矩阵：

    一个坐标系到另一个坐标系的旋转，除了可以分解为绕坐标系中的Z轴、Y轴、X轴旋转三次得到外，旋转的一个神奇之处就在于，三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。这样以来，可以用一个三维向量(θ*x, θ*y, θ*z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式。

    Axis-Angle的表示方法还可以推导出另一种很常用的三维旋转表示方法，叫四元数(Quaternion)，具体的证明参考秦永元的《惯性导航》。同上，假设(x,y,z)是axis方向的单位向量，θ是绕axis转过的角度，那么四元数可以表示为[cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2)]。注意，这里可以推导出，用于表示旋转的四元数向量也必须是单位向量。四元数的神奇之处在于，对于三维坐标的旋转，可以通过四元数乘法直接操作，与上述旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。捷联惯导中的姿态更新实质上是如何计算四元数Q。 

其中R代表地理坐标系，b代表机体坐标系，u是R系中一个旋转向量（单位向量），机体绕着这个旋转向量旋转，θ表示转过的角度。

这就是用四元数表示姿态矩阵，这是第三种表示方式。

欧拉角、方向余弦、四元数之间的关系：

这里的四元数q0=a，q1=b，q2=c，q3=d，三者关系为：

通过比较上述等式的各个元素，四元数可以直接用欧拉角或余弦表示，同样欧拉角也可用方向余弦或四元数表示。 

1）用方向余弦表示四元数：

2）用欧拉角直接表示四元数：

3）用方向余弦表示欧拉角：

    上面这些就是姿态解算用到的四元数及四元数求解欧拉角的方法了。基本思路是四元数—>方向余弦—>欧拉角。这样就又有问题了四元数的q0-q3数据来源是什么呢？又是如何填充q0-q3呢？请看下一节。

参考：《惯性导航》秦永元
            https://www.cnblogs.com/andychenforever/p/6298073.html
            http://www.starlino.com/dcm_tutorial.html
            https://legacy.gitbook.com/book/nephen/direction-cosine-matrix-imu-theory/details