03-导航算法中姿态的表示方法.md

导航算法中姿态的表示方法

[TOC]

1、姿态的基本概念

载体的姿态指的是载体坐标系（b系）和导航坐标系（n系）之间的方位关系，而两坐标系直接的方位关系可以等效成力学中的刚体定点转动问题。在刚体定点转动理论中，描述动坐标系相对参考坐标系方位关系的传统方法有欧拉角法、方向余弦法、四元数法。由于捷联惯导系统采用通过姿态矩阵计算建立的数学平台，在捷联惯导系统的姿态、速度和位置的更新算法中，姿态算法对整个系统的精度影响最大，它是算法研究和设计的核心。

1. 反对称阵

$$ (\boldsymbol{V} \times)=\left[\begin{array}{ccc} 0 & -V_{z} & V_{y} \\ V_{z} & 0 & -V_{x} \\ -V_{y} & V_{x} & 0 \end{array}\right] $$

反对称阵要点：

1. 对角线上都为 0，上下两部分对称相反，即： $(V \times)=-(V\times)^{T}$ ，所以称之为反对称阵。

2. 一个向量叉乘另一个向量，等于点乘另一个向量的反对称阵，化叉乘为点乘。

3. 反对称阵的幂方通式： $$ (\boldsymbol{V} \times)^{i}=\left{\begin{array}{ll} (-1)^{(i-1) / 2} v^{i-1}(\boldsymbol{V} \times) & i=1,3,5, \cdots \ (-1)^{(i-2) / 2} v^{i-2}(\boldsymbol{V} \times)^{2} & i=2,4,6, \cdots \end{array}\right. $$

2. 欧拉角

$$ \boldsymbol{A}=\left[\begin{array}{lll}\theta & \gamma & \psi\end{array}\right]^{\mathrm{T}} $$

欧拉角是一种用绕坐标轴转换的角度描述两个坐标系之间相对姿态的经典方法，物理含义直观，易于理解，尤其是用于描述载体坐标系相对于当地导航坐标系的运动。

欧拉角表示转动：三维空间中，一个直角坐标系相对于另一个直角坐标系的方位关系，可通过三个依次转动来描述，每次旋转所绕的坐标轴与前后旋转轴正交。欧拉证明任意两个正交坐标系之间的相对朝向关系可以通过一组不少于 3 的角度来描述，这三次旋转的转角称为一组欧拉角（欧拉角组）。

欧拉角表示姿态：想描述载体的姿态，可以指定一个参考坐标系，然后用一组欧拉角来描述与载体与固联的坐标系相对于参考坐标系的转动。在地球附近的导航应用中，参考坐标系一般默认选择当地地理坐标系， 而动坐标系为与运载体固联的坐标系。

• 航向角：载体纵轴正方向在当地水平面上的投影与当地地理北向的夹角，常取北偏东为正，一般用 ψ 表示，角度范围为 0 ∼ 360◦ 或 −180◦ ∼ +180◦。
• 俯仰角：载体纵轴正方向与其水平投影线之间的夹角，当载体“抬头”时定义为正，一般用 θ 表示，取值范围为 −90◦ ∼ 90◦。
• 横滚角：载体立轴正方向与载体纵轴所在铅垂面之间的夹角，当载体向右倾斜（如飞机右机翼下压）时为正，用 $\phi$ 表示，取值范围为 −180◦ ∼ +180◦。

欧拉角要点：

1. 两个正交坐标系之间的相对朝向关系可以通过一组欧拉角组来描述，给定一组欧拉角须同时指定对应的转轴顺序。
2. 导航姿态角，包括航向角、俯仰角和横滚角，与载体的物理轴向紧密关联，而与选择的载体数学坐标系无关。
3. 当俯仰角为 $\pm 90^{\circ}$ 时会出现“万向锁”现象，欧拉角变换存在奇异值，此时无法区分横滚角和航向角。因此，欧拉角法不能用于全姿态导航应用。
4. 欧拉角组不能直接相加来表示两次转动的合成。

3. 旋转矩阵/方向余弦阵

$$ \boldsymbol{C}=\left[\begin{array}{lll}c_{x x} & c_{x y} & c_{x z} \ c_{y x} & c_{y y} & c_{y z} \ c_{z x} & c_{z y} & c_{z z}\end{array}\right] $$

方向余弦矩阵（Direction Cosine Matrix，DCM）用向量的方向余弦来表示的姿态矩阵，常用于描述两个坐标系的相对姿态。相比于欧拉角，方向余弦矩阵显得抽象许多，但它处理向量的投影变换非常方便。

我们知道，空间中的矢量的三个投影值就是坐标，同一个矢量在 $i$ 系和 $b$ 系有两套坐标，它们的关系可以表示为： $$ \boldsymbol{V}=V_{x}^{i} \boldsymbol{i}{i}+V{y}^{i} \boldsymbol{j}{i}+V{z}^{i} \boldsymbol{k}{i}=V{x}^{b} \boldsymbol{i}{b}+V{y}^{b} \boldsymbol{j}{b}+V{z}^{b} \boldsymbol{k}{b} $$ 即 $\left[\begin{array}{lll}\boldsymbol{i}{i} & \boldsymbol{j}{i} & \boldsymbol{k}{i}\end{array}\right]\left[\begin{array}{c}V_{x}^{i} \ V_{y}^{i} \ V_{z}^{i}\end{array}\right]=\left[\begin{array}{lll}\boldsymbol{i}{b} & \boldsymbol{j}{b} & \boldsymbol{k}{b}\end{array}\right]\left[\begin{array}{c}V{x}^{b} \ V_{y}^{b} \ V_{z}^{b}\end{array}\right]=\left[\begin{array}{lll}\boldsymbol{i}{i} & \boldsymbol{j}{i} & \boldsymbol{k}{i}\end{array}\right] \boldsymbol{P}\left[\begin{array}{c}V{x}^{b} \ V_{y}^{b} \ V_{z}^{b}\end{array}\right]$

有： $$ \left[\begin{array}{c}V_{x}^{i} \ V_{y}^{i} \ V_{z}^{i}\end{array}\right]=\boldsymbol{P}\left[\begin{array}{c}V_{x}^{b} \ V_{y}^{b} \ V_{z}^{b}\end{array}\right] $$ 记为 $\boldsymbol{V}^{i}=\boldsymbol{P} \boldsymbol{V}^{b}=\boldsymbol{C}_{b}^{i} \boldsymbol{V}^{b}$，$C_b^i$ 称为坐标系变换矩阵 (i->b) 或 坐标变换矩阵 (b->i)，也称之为方向余弦阵、过渡矩阵。

方向余弦阵要点：

1. 由于都是单位阵，方向余弦阵中每一个元素都表示两套坐标系坐标轴夹角的余弦值，以此得名。
2. 区分清坐标系变换矩阵和坐标变换矩阵。
3. 方向余弦阵虽然有 9 个参数，但其中包含了 6 个约束条件，即行向量模为 1 且两两正交，只有 3 个参数是独立的。
4. 方向余弦矩阵是正交矩阵，$\left(\mathbf{C}{\alpha}^{\beta}\right)^{-1}=\left(\mathbf{C}{\alpha}^{\beta}\right)^{\mathrm{T}}$ 。多次连续转动可以用相应的多个方向余弦矩阵连乘来表示：$\mathbf{C}{a}^{d}=\mathbf{C}{b}^{d} \mathbf{C}_{a}^{b}$ 。
5. 当载体运动时，两个坐标系之间的相对姿态随时间变化，对应的方向余弦矩阵 $\mathbf{C}{b}^{R}(t)$ 则是 一个时变矩阵，其微分方程为 $\dot{\mathrm{C}}{b}^{R}=\mathbf{C}{b}^{R}\left(\boldsymbol{\omega}{R b}^{b} \times\right)$ 。
6. 可用毕卡迭代法求解方向余弦矩阵的微分方程，得到一个以无限重积分表示的级数，也被称为毕卡级数，上述级数是收玫的，但只有在 “定轴转动” 这一特殊情形下才能得到如下形式的闭合解：$\mathbf{C}{b}^{R}(t)=\mathbf{C}(0) \exp \left(\int{0}^{t} \boldsymbol{\Omega}(\tau) d \tau\right)$ 。
7. 姿态更新公式：$\mathbf{C}{b(k)}^{i}=\mathbf{C}{b(k-1)}^{i} \mathbf{C}{b(k)}^{b(k-1)}$，$\mathbf{C}{b(k)}^{b(k-1)}=\mathbf{I}+\frac{\sin \Delta \theta_{k}}{\Delta \theta_{k}}\left[\Delta \boldsymbol{\theta}{k} \times\right]+\frac{1-\cos \Delta \theta{k}}{\Delta \theta_{k}^{2}}\left[\Delta \boldsymbol{\theta}_{k} \times\right]^{2}$
8. 对于普通惯性导航应用来说，$b$ 系定轴转动这一假设一般都不成立的，当该条件不满足时，仍然将陀螺输出的角增量套用到式中进行姿态更新的递推计算，则会带来姿态更新求解的不可交换性误差，可用等效旋转矢量来解决这个问题。

4. 四元数

$$ \boldsymbol{Q}=q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k}=q_{0}+\boldsymbol{q}_{v} $$

我们知道复数可以看成是复平面上的向量，即一个二维的向量，将复数的概念再拓展可以得到四元数，用以描述三维空间中的向量。由于只有四个元素，四元数比坐标变换矩阵计算快，也避免了欧拉角中的“万向锁”问题，但理解起来更困难。

四元数要点：

1. 四元数主要有以下表示方式：

   • $\boldsymbol{q}=q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k}$ 、
   • 向量坐标形式 ：$\boldsymbol{q}=\left[q_{0}, q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}}$
   • 实部+虚部：$\boldsymbol{q}=q_{s}+\boldsymbol{q}_{v}$
   • 三角函数形式： $\boldsymbol{q}=\cos \frac{\theta}{2}+\boldsymbol{u} \sin \frac{\theta}{2}$ (与等效旋转矢量有关，做了归一化，模值为一)

2. 四元数虚数单位之间的乘法有如下特点：单位与单位自己相乘时，表现与复数一样，而两两之间的乘法则与三维向量的外积 (叉乘) 一样。 $$ \left{\begin{array}{l}\boldsymbol{i} \circ \boldsymbol{i}=\boldsymbol{j} \circ \boldsymbol{j}=\boldsymbol{k} \circ \boldsymbol{k}=-1 \ \boldsymbol{i} \circ \boldsymbol{j}=\boldsymbol{k}, \quad \boldsymbol{j} \circ \boldsymbol{k}=\boldsymbol{i}, \quad \boldsymbol{k} \circ \boldsymbol{i}=\boldsymbol{j}, \quad \boldsymbol{j} \circ \boldsymbol{i}=-k, \quad k \circ \boldsymbol{j}=-i, \quad \boldsymbol{i} \circ \boldsymbol{k}=-\boldsymbol{j}\end{array}\right. $$

3. 用四元数的旋转变换算子可实现三维向量的坐标变换：$\boldsymbol{v}^{R}=\boldsymbol{q}{b}^{R} \circ \boldsymbol{v}^{b} \circ \boldsymbol{q}{b}^{R^{*}}$ 。

4. 单位四元数三角表示法：$\boldsymbol{Q}=q_{0}+\boldsymbol{q}{v}=\cos \frac{\phi}{2}+\boldsymbol{u} \sin \frac{\phi}{2}$ 具有很清晰的物理意义。常在四元数的右边加上角标，写成 $\boldsymbol{Q}{b}^{i}$，中 $\boldsymbol{u}$ 表示动坐标系 ( $b$ 系) 相对于参考坐标系 ( $i$ 系) 旋转的单位转轴，$\phi$ 表示旋转角度大小， $\phi \boldsymbol{u}$ 表示等效旋转矢量。使用角标后，共轭四元数可记为 $\boldsymbol{Q}{i}^{b}=\left(\boldsymbol{Q}{b}^{i}\right)^{*}$，这与矩阵转置的表示方法类似，比如 $\boldsymbol{C}{i}^{b}=\left(\boldsymbol{C}{b}^{i}\right)^{\mathrm{T}}$ 。

5. 四元数的微分方程式：$\dot{\boldsymbol{q}}{b}^{R}=\frac{1}{2} \boldsymbol{q}{b}^{R} \circ\left[\begin{array}{c}0 \ \boldsymbol{\omega}{R b}^{b}\end{array}\right]$，或写作矩阵形式：$\dot{\boldsymbol{q}}{b}^{R}=\frac{1}{2} \mathbf{W} \boldsymbol{q}_{b}^{R}$ 。由此建立了四元数与旋转角速度之间的关系。

6. 求解四元数的微分方程，当 $b$ 系满足 “定轴转动” 条件时，可得解析解：$\boldsymbol{q}{b}^{R}(t)=$ $\left[\mathbf{I} \cos \frac{\Delta \theta}{2}+\frac{\Theta}{\Delta \theta} \sin \frac{\Delta \theta}{2}\right] \boldsymbol{q}{b}^{R}(0)$ 。

7. 还有八元数，用于描述螺旋运动，有线运动、角运动。

5. 等效旋转矢量

欧拉在研究刚体运动时最早提出了等效旋转矢量的概念，他指出刚体的定点有限旋转都可以用绕经过该固定点的一个轴的一次转动来等效实现。并建立了刚体上单位矢量在转动前后的变换公式。现代捷联惯性导航姿态更新算法研究的关键在于如何使用陀螺的测量值构造出对应的等效旋转矢量，以尽量减小或消除姿态更新算法中的不可交换性误差。后续使用等效旋转矢量来计算姿态的变化，如用方向余弦矩阵或四元数来表示，将变得非常简单。

用几个角增量的采样值，根据各种子样算法，算一个等效旋转矢量，再用算出的等效旋转矢量更新方向余弦阵、四元数。

空间矢量 $r$ 绕转轴矢量 $u$ 转动 $\phi$ ，转动得到的矢量 $r^{'}$ 。四个量直接一定有函数关系 $r^{'}=f(r,u,\phi)$ ，利用一些几何关系，可以推导得到： $$ \begin{aligned} \boldsymbol{r}^{\prime} & =(\boldsymbol{u} \cdot \boldsymbol{r}) \boldsymbol{u}+(\boldsymbol{u} \times \boldsymbol{r}) \times \boldsymbol{u} \cos \phi+\boldsymbol{u} \times \boldsymbol{r} \sin \phi \ & =\left[\boldsymbol{I}+(\boldsymbol{u} \times)^{2}\right] \boldsymbol{r}-(\boldsymbol{u} \times)^{2} \boldsymbol{r} \cos \phi+\boldsymbol{u} \times \boldsymbol{r} \sin \phi \ & =\left[\boldsymbol{I}+\sin \phi(\boldsymbol{u} \times)+(1-\cos \phi)(\boldsymbol{u} \times)^{2}\right] \boldsymbol{r}=\boldsymbol{D} \boldsymbol{r}\end{aligned} $$ 可记 $\boldsymbol{D}=\boldsymbol{I}+\sin \phi(\boldsymbol{u} \times)+(1-\cos \phi)(\boldsymbol{u} \times)^{2}$ ，此公式称为罗德里格旋转公式；$r'$ 转到 $r$ 中间乘的矩阵称为罗德里格旋转矩阵，和转轴$u$ 及转角 $\phi$ 有关。

将罗德里格旋转公式用于直角坐标系的单位坐标轴，可将参考坐标系转到动坐标系： $$ \left.\begin{array}{c}\boldsymbol{i}{b}=\boldsymbol{D} \boldsymbol{i}{i} \ \boldsymbol{j}{b}=\boldsymbol{D} \boldsymbol{j}{i} \ \boldsymbol{k}{b}=\boldsymbol{D} \boldsymbol{k}{i}\end{array}\right} $$

$$ \left{\begin{array}{c} 基旋转变换关系：\left[\begin{array}{lll}\boldsymbol{i}{b} & \boldsymbol{j}{b} & \boldsymbol{k}{b}\end{array}\right]=\boldsymbol{D}\left[\begin{array}{lll}\boldsymbol{i}{i} & \boldsymbol{j}{i} & \boldsymbol{k}{i}\end{array}\right] \ 基投影变换关系：{\left[\begin{array}{lll}\boldsymbol{i}{b} & \boldsymbol{j}{b} & \boldsymbol{k}{b}\end{array}\right]=\left[\begin{array}{lll}\boldsymbol{i}{i} & \boldsymbol{j}{i} & \boldsymbol{k}{i}\end{array}\right] \boldsymbol{P}} \ \end{array}\right. $$

$$ 在 i 系投影：\left.\begin{array}{l}{\left[\begin{array}{lll}\boldsymbol{i}{b}^{i} & \boldsymbol{j}{b}^{i} & \boldsymbol{k}{b}^{i}\end{array}\right]=\boldsymbol{D}\left[\begin{array}{lll}\boldsymbol{i}{i}^{i} & \boldsymbol{j}{i}^{i} & \boldsymbol{k}{i}^{i}\end{array}\right]=\boldsymbol{D} \boldsymbol{I}} \ {\left[\begin{array}{lll}\boldsymbol{i}{b}^{i} & \boldsymbol{j}{b}^{i} & \boldsymbol{k}{b}^{i}\end{array}\right]=\left[\begin{array}{lll}\boldsymbol{i}{i}^{i} & \boldsymbol{j}{i}^{i} & \boldsymbol{k}{i}^{i}\end{array}\right] \boldsymbol{P}=\boldsymbol{I P}}\end{array}\right} \quad \boldsymbol{D}=\boldsymbol{P}=\boldsymbol{C}_{b}^{i} $$

$P=D$ 表明，罗德里格旋转矩阵 $D$ 正好是从参考坐标系（$i$ 系）到动坐标系（$b$ 系）的过渡矩阵，它也是从 $b$ 系到 $i$ 系的坐标变换矩阵。因此可以将罗德里格旋转公式重新写为方向阵的形式： $$ \boldsymbol{C}{b}^{i}=\boldsymbol{I}+\sin \phi(\boldsymbol{u} \times)+(1-\cos \phi)(\boldsymbol{u} \times)^{2} $$ 进一步，若记 $\boldsymbol{\phi}=\phi \boldsymbol{u}$ 和 $\phi=|\boldsymbol{\phi}|$，则有 $\boldsymbol{u}=\boldsymbol{\phi} / {\phi}$ ，得： $$ \boldsymbol{C}{b}^{i}=\boldsymbol{I}+\frac{\sin \phi}{\phi}(\boldsymbol{\phi} \times)+\frac{1-\cos \phi}{\phi^{2}}(\boldsymbol{\phi} \times)^{2} $$ 这里 $\boldsymbol{\phi}$ 称为等效旋转矢量（RV，简称旋转矢量）

等效旋转矢量要点：

1. 在几种姿态表示方法中，等效旋转矢量是唯一可以对旋转过程线性插值的方法。
2. 刚体的有限转动是不可交换的。刚体的定点有限旋转都可以用绕经过该固定点的一个轴的一次转动来实现。
3. 其矢量方向表示旋转的方向，模的大小表示旋转角度大小。
4. 当载体的角速度方向随时间变化而不是做 “定轴转动” 时，直接将传感器角增量测量值代入方向余弦矩阵或四元数微分方程的求解计算式中，会带来 “不可交换性误差”。
5. Bortz 方程： $\dot{\phi}=\boldsymbol{\omega}+\frac{1}{2}(\phi \times \omega)+\frac{1}{\phi^{2}}\left[1-\frac{\phi \sin \phi}{2(1-\cos \phi)}\right] \boldsymbol{\phi} \times(\boldsymbol{\phi} \times \boldsymbol{\omega})$ 。是一种常用的等效旋转矢量微分方程，利用等效旋转矢量进行转动不可交换误差补偿。
6. 等效旋转矢量的单子样+前一周期求解式：$\phi_{k}=\Delta \theta_{k}+\frac{1}{12} \Delta \theta_{k-1} \times \Delta \theta_{k}$ 。

2、基于 Eigen 的姿态表示

• 反对称阵：使用skewsymmetric()函数，计算向量的反对称阵。

• 欧拉角：用 Vector3d 表示，Eigen 库并不直接支持欧拉角计算，需要确定好顺序。

• 方向余弦阵：用 Matrix3d 类型表示，默认

• 四元数：用 Quaterniond 类型表示，有三种方式构造四元数：

  Eigen::Quaterniond q1(w, x, y, z);			// 直接构造，虚部在前实部在后
  Eigen::Quaterniond q2(Vector4d(x, y, z, w));// 四维向量构造，实部在前虚部在后
  Eigen::Quaterniond q2(Matrix3d(R));			// 旋转矩阵构造

  注意四元数的顺序：直接构造的时候虚部在前，通过四维向量构造时虚部在后，存储时虚部在后。

• 角轴：AngleAxisd，绕单位旋转轴旋转角度，在 MSF 中作为四元数和等效旋转矢量间的过渡。

• 等效旋转矢量：直接用 Vector3d 向量表示，其大小等于该刚体旋转的角速度，方向该刚体的旋转轴线一致，所以用单位旋转轴乘以弧度制的旋转角得到等效旋转矢量。