导航坐标系与姿态描述方法

我们知道在谈论物体运动的时候需要有坐标系做参考，而导航系统需要不断地确定物体的姿态和位置，将涉及到多个坐标系之间的变换。不同坐标系之间的变换是导航的基础知识，主要涉及到不同坐标系下飞行器的位置和姿态的表达，以及加速度、角速度等物理矢量的描述。相关的数学和物理基础可以参考刚体运动与齐次变换一文。

本文中我们将详细介绍导航系统中常用的几个坐标系。虽然对于我们的四旋翼而言，为了简化建模和计算，很多时候都会忽略地球自转等一些过于宏大的因素。但这里我们仍然介绍与地球相关的一些坐标系和参数。描述姿态的方法有很多种，本文中我们也会予以介绍。

1. 导航坐标系

下面是导航系统中常用的一些坐标系。所谓的导航就是在导航坐标系(n系)下引导物体从一点运动到另一点的过程，为了确定载体的姿态和位置，我们需要将各个传感器的测量值转换到导航坐标系下进行解算。

惯性坐标系(i系, $x_i y_i z_i$轴): 原点在地球中心，$z_i$轴与地球自转轴重合，$x_i, y_i$在赤道平面内与$z_i$构成右手坐标系， $x_i$指向恒星方向。三个坐标轴在惯性空间中固定不动。
地球坐标系(e系, $x_e y_e z_e$轴): 原点在地球中心，$z_e$轴与地球自转轴重合，$x_e, y_e$在赤道平面内与$z_e$构成右手坐标系， $x_e$指向本初子午线。e系与地球固连，相对惯性空间以地球自转角速度$\vec{\omega_{ie}^e}$运动。
地理坐标系(t系，$x_t y_t z_t$轴): 我们采用东北天形式的坐标系，即原点在载体质心，$x_t$轴指东，$y_t$轴指北，$z_t$轴指天与重力加速度$g$的方向相反。有些系统中地理坐标系也会取北东地、北西天。地理坐标系相对于地球坐标系的位置关系就是(经度$\lambda$和纬度$\psi$)。
导航坐标系(n系，$x_n y_n z_n$轴): 导航坐标系是实际进行导航信息解算的参考坐标系。对于一些大型的飞行器、舰船等在地球表面具有长距离运动载体，该坐标系一般是指地理坐标系t系。
机体坐标系(b系，$x_b y_b z_b$轴): 原点设置在运载体的质心，$x_b$轴延载体纵轴朝前，$y_b$轴延载体横轴朝左，$z_b$轴延载体竖轴朝上。 b系相对于n系的方位关系就是载体的姿态。

各种不同的传感器因为其工作原理不一样，所用的坐标系也就不尽相同。比如说加表所获得比力矢量是在机体坐标系(b系)下的，陀螺仪的测量值通常认为是在惯性坐标系(i系)下的， GPS系统的坐标系一般是地理坐标系(t系)。磁罗盘还会涉及地磁坐标系，不过通常都会转换到地球坐标系(e系)或者地理坐标系(t系)下。如果导航系统中还要融合视觉定位，那么还会涉及到关于图像和相机的坐标系，但归根结底还是机体坐标系(b系)。

对于我们的室内四旋翼的导航而言，似乎没有必要了解太多关于地球的信息。通常我们关心的只是四旋翼在房间中的哪个位置，朝向哪里。在这个尺度上，地球的自转以及形状似乎不会对四旋翼有太大的影响。所以为了适应室内环境，我们添加如下两个坐标系:

地图坐标系(m系, $x_m y_m z_m$轴): 以房间中的某一固定点作为原点，构建右手笛卡尔坐标系，与房间固连保持相对静止的状态。
里程计坐标系(o系，$x_o y_o z_o$轴): 原点在载体质心，各坐标轴与m系坐标轴平行且同向。该坐标系随着载体一起平移，但不随之转动。

在忽略地球的各种宏大因素影响的情况下，我们可以认为m系就是一种惯性坐标系，此时进行导航信息解算的参考坐标系一般选择o系。m系与o系之间只有平移运动，没有转动关系，所以这两个坐标系之间的变换关系，我们称之为位置变换。而机体坐标系(b系)与导航坐标系(n系)，或者说是里程计坐标系(o系)之间只有转动关系，称b系到n系之间的变换关系为旋转变换。

更具体的，对于我们的四旋翼而言，导航的过程中需要不断地确定位置变换和旋转变换，进而估计飞行器的位置和姿态。我们将在本系列的第X部分中介绍姿态估计，第XX部分中介绍位置估计。

2. 姿态描述方法

在介绍刚体运动与齐次变换的时候我们了解到，描述两个纯旋转的坐标系之间的坐标变换关系时，通常通过与一个旋转矩阵相乘来完成。那么从b系到n系的旋转变换可以写作：

$$ \begin{equation}\label{f2.1} \vec{r^n} = C_b^n \vec{r^b} \end{equation} $$

其中，$\vec{r^n},\vec{r^b}$表示同一坐标点或者矢量分别在n系和b系下的坐标；$C_b^n$则是从b系到n系的变换矩阵。以后在没有明确说明的情况下，我们用向量符号右上标表示其参考坐标系。旋转矩阵的右下标表示变换的源坐标系，右上标表示目标坐标系。

我们还了解到旋转矩阵是一种正交矩阵，即该矩阵的转置就是它的逆。所以我们可以很容易写出从n系到b系的旋转矩阵：

$$ \begin{equation}\label{f2.2} C_n^b = (C_b^n)^{-1} = (C_b^n)^T \end{equation} $$

$C_b^n$是一个$3 \times 3$的矩阵，有9个量。再加上正交矩阵的约束，也就只有三个自由度，这一点与刚体的三个旋转自由度是一致的。我们用9个量来描述三个旋转自由度显得十分冗余，而且不够直观，很难根据旋转矩阵来想象飞行器的姿态。所以有时我们希望有一种更紧凑直观的方式来描述姿态，欧拉角就是一种常用的方法。

欧拉角是一种非常直观的描述姿态的方法，它把飞行器的姿态分解为先后绕不同轴转动的三个角度。根据转轴的顺序不同，同一个姿态可以有多种不同的欧拉角表示方式，我们经常使用的是一种称为rpy的欧拉角。它是指飞行器先后绕机体坐标系的$z_b$轴、$y_b$轴、$x_b$轴转动。这三个角分别被称为偏航角(yaw)、 俯仰角(pitch)、横滚角(row)。由于每次转动都是相对机体坐标系完成的，所以依次右乘基本旋转矩阵就可以得到$C_b^n$：

$$ \begin{equation}\label{f2.3} \begin{array}{rcl} C_b^n & = & \begin{bmatrix} C_Y & -S_Y & 0 \\ S_Y & C_Y & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} C_P & 0 & S_P \\ 0 & 1 & 0 \\ -S_P & 0 & C_P \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & C_R & -S_R \\ 0 & S_R & C_R \end{bmatrix} = \begin{bmatrix} C_Y & -S_Y & 0 \\ S_Y & C_Y & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} C_P & S_PS_R & S_PC_R \\ 0 & C_R & -S_R \\ -S_P & C_PS_R & C_PC_R \end{bmatrix} \\ & = & \begin{bmatrix} C_YC_P & -S_YC_R + C_YS_PS_R & S_YS_R + C_YS_PC_R \\ C_PS_Y & C_YC_R + S_YS_PS_R & -C_YS_R + S_YS_PC_R \\ -S_P & C_PS_R & C_PC_R \end{bmatrix} \end{array} \end{equation} $$

其中，下标$R, P, Y$分别表示横滚角、俯仰角和偏航角。$C_{\bullet}$和$S_{\bullet}$分别表示余弦函数$\cos (\bullet)$和正弦函数$\sin (\bullet)$。根据式($\ref{f2.3}$)我们也可以很容易通过旋转矩阵求得三个欧拉角：

$$ \begin{equation}\label{f2.4} \begin{cases} R & = & \text{atan2}\left(C_b^n(3, 2), C_b^n(3, 3)\right) \\ P & = & \text{asin}\left(-C_b^n(3, 1)\right) \\ Y & = & \text{atan2}\left(C_b^n(2, 1), C_b^n(1, 1)\right) \end{cases} \end{equation} $$

欧拉角虽然使用起来很直观，但是存在一个奇异性的问题。当俯仰角为$\pm 90°$时会产生自由度退化的问题，从导航系上观察相当于第一次转动与第二次转动绕的是同一个轴，这也就是所谓的万向锁问题。所以欧拉角不适合进行全姿态解算，只适用于水平姿态变化不大的情况。

任何旋转都可以等效为绕着一个转轴的一次转动，因此我们可以使用一个三维的向量表示转轴和转动的方向，向量的模长则表示转过的角度，这个向量通常被称为旋转矢量。我们记$\vec{n}$为转轴的单位向量，$\theta$为转过的角度，那么旋转矢量可以写作$\theta\vec{n}$，通过罗德里格斯公式可以将其转换为旋转矩阵：

$$ \begin{equation}\label{f2.5} C_b^n = \cos \theta \boldsymbol{I} + (1 - \cos \theta) \vec{n} \vec{n}^T + \sin \theta \vec{n_{\times}} \end{equation} $$

其中$\vec{n_{\times}}$是由$\vec{n}$构成的斜对称矩阵。设$\vec{n} = \begin{bmatrix} x & y & z \end{bmatrix}^T，c = \cos \theta, s = \sin \theta$，那么上式可以展开成矩阵的形式：

$$ \begin{equation}\label{f2.6} C_b^n = \begin{bmatrix} x^2(1 - c) + c & xy(1 - c) + zs & xz(1 - c) - ys \\ xy(1 - c) - zs & y^2(1 - c) + c & yz(1 - c) + xs \\ xz(1 - c) + ys & yz(1 - c) - xs & z^2(1 - c) + c \end{bmatrix} \end{equation} $$

根据旋转矩阵的迹，我们可以很容易计算转角$\theta$:

$$ \begin{equation}\label{f2.7} \begin{array}{rl} & \text{tr}(C_b^n) = (x^2 + y^2 + z^2)(1 - c) + 3c = 1 + 2c \\ \Rightarrow & \theta = \arccos \cfrac{\text{tr}(C_b^n) - 1}{2} \end{array} \end{equation} $$

而旋转轴在转动过程中是不会变化的，所以$\vec{n}$是矩阵$C_b^n$的特征值1所对应的特征向量，通过求解如下的线性方程即可求得：

$$ \begin{equation}\label{f2.8} C_b^n \vec{n} = \vec{n} \end{equation} $$

实际上，我们很多时候会使用四元数来描述坐标系之间旋转变换。四元数是一种扩展的复数，它使用一个标量和一个三轴矢量来描述旋转。其中的标量构成扩展复数的实部，三轴矢量构成其虚部。三轴矢量的各轴之间相互正交，并以虚单位作为单位长度。四元数只需要四个量就可以描述任意的旋转运动，而且具有比较良好的运算特性。缺点就是不直观，我们很难根据一个四元数想象出刚体的实际运动。

四元数$\vec{Q}(q_0, q_1, q_2, q_3) = q_0 + q_1 \vec{i} + q_2 \vec{j} + q_3 \vec{k}$所对应的旋转矩阵可以写作：

$$ \begin{equation}\label{f2.9} C_b^n = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1q_2 - q_0q_3) & 2(q_0q_2 + q_1q_3) \\ 2(q_0q_3 + q_1q_2) & 1 - 2(q_1^2 + q_3^2) & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3 - q_0q_2) & 2(q_0q_1 + q_2q_3) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix} \end{equation} $$

在已知旋转矩阵$C_b^n$的情况下，我们可以通过下式计算四元数各项的绝对值：

$$ \begin{equation}\label{f2.10} \begin{cases} |q_0| & = & \cfrac{1}{2}\sqrt{1 + C_b^n(1,1) + C_b^n(2, 2) + C_b^n(3, 3)} \\ |q_1| & = & \cfrac{1}{2}\sqrt{1 + C_b^n(1,1) - C_b^n(2, 2) - C_b^n(3, 3)} \\ |q_2| & = & \cfrac{1}{2}\sqrt{1 - C_b^n(1,1) + C_b^n(2, 2) - C_b^n(3, 3)} \\ |q_3| & = & \cfrac{1}{2}\sqrt{1 - C_b^n(1,1) - C_b^n(2, 2) + C_b^n(3, 3)} \\ \end{cases} \end{equation} $$

$q_1, q_2, q_3$的符号可以通过下式确定，$q_0$的符号可以看心情指定：

$$ \begin{equation}\label{f2.11} \begin{cases} \text{sign}(q_1) & = & \text{sign}(q_0)\text{sign}\left[C_b^n(3, 2) - C_b^n(2, 3) \right] \\ \text{sign}(q_2) & = & \text{sign}(q_0)\text{sign}\left[C_b^n(1, 3) - C_b^n(3, 1) \right] \\ \text{sign}(q_3) & = & \text{sign}(q_0)\text{sign}\left[C_b^n(2, 1) - C_b^n(1, 2) \right] \\ \end{cases} \end{equation} $$

3. 完

本文中，我们介绍了导航系统中常用的坐标系，并针对我们的室内四旋翼额外描述了地图坐标系和里程计坐标系。此外介绍了旋转矩阵、欧拉角、旋转矢量、四元素四种描述姿态的方法。