符号与说明

b：机体坐标系；e/i：地球固联坐标系；
NED：North-East-Down坐标系，三个单词依次为x轴、y轴、z轴；
ENU：East-North-Up坐标系，三个单词依次为x轴、y轴、z轴；
由于mark down中无法输入左上标和左下标，所以用上标和下标代替左上标和左下标。
记 $\overset{b}{b_1}=e_1=$$\begin{pmatrix}
1 \
0 \
0
\end{pmatrix}$,$b_2、b_3$依次类推，其中 $\overset{b}{b_1}$表示在 $b$ 参考系下，沿着第一个轴的单位向量。
一般，假定绕 $z$ 轴所转角度为 $\psi$ （偏航角）,绕 $y$ 轴所转角度为 $\theta$ （俯仰角）,绕 $x$ 轴所转角度为 $\phi$ （滚转角）
飞行器从固联坐标系到机体坐标系，采用的是 $e\to k\to n\to b$ 的顺序，从机体坐标系到固联坐标系，采用的是 $b\to n\to k\to e$ 的顺序；
旋转矩阵分为向量绕轴旋转和坐标系旋转，二者是逆的关系，前者是坐标系不变，向量转动 $\alpha$ ，后者是坐标轴转动 $\alpha$ 。注意，此处的旋转均满足右手定则，即旋转的时候，角速度方向是大拇指的方向即旋转轴的方向。
- 在二维平面内向量的转动（绕z轴的转动），旋转矩阵为: $\left(\begin{matrix} \cos \alpha & -\sin\alpha \ \sin \alpha & \cos\alpha \end{matrix}\right ) \label{1.1}$
- 在在二维平面内坐标轴的转动（绕z轴的转动），旋转矩阵为 $\left(\begin{matrix} \cos \alpha & \sin\alpha \-\sin \alpha & \cos\alpha \end{matrix}\right ) \tag{1.1}$

欧拉角

以 $ Z-Y-X$ 欧拉角为例，它表示的是坐标系从 $e$ 系出发，先绕 $Z$ 轴 $\psi$ ,到达 $k$ 系；再绕 $k$ 系中的 $Y$ 轴 $\theta$ ，到达 $n$ 系；最后再绕 $n$ 系中的 $X$ 轴 $\phi$ ，到达 $b$ 系，中间绕的 $Y$ 、$X$ 轴均为每次旋转后的新的坐标系下的轴，而不是原始的 $e$ 系中的坐标轴。（这种旋转方法称之为内旋，还有一种旋转方法是固定每次旋转的转轴，比如设置为固联坐标系下的坐标轴，此为外旋，下面介绍的是内旋）

$e\to k\to n\to b$

我们仿照二维坐标系中向量绕轴旋转，可以得到三次旋转的旋转矩阵如下:

$\tag{1} R_z(\psi)=\left(\begin{matrix} \cos \psi &\sin \psi &0\\ -\sin \psi &cos \psi &0\\ 0 & 0& 1 \end{matrix}\right)\\ R_y(\theta)=\left(\begin{matrix} \cos \theta &0 &-\sin \theta\\ 0 & 1& 0 \\ \sin \theta&0 &\cos \theta \end{matrix}\right) \\ R_x(\phi)=\left(\begin{matrix} 1 & 0& 0 \\ 0 &\cos \phi&\sin \phi\\ 0&-\sin \phi &cos \phi\\ \end{matrix}\right) \\$

我们可以得到各个坐标系向 $b$ 系变换的公式,注意顺序是每次变换要左乘！

$\tag{2} \begin{align} R_e^b&=R_x(\phi)R_y(\theta)R_z(\psi)\\ &=\left(\begin{matrix} \cos\psi\cos\theta& \cos\theta\sin\psi&-\sin\theta\\ \cos\psi\sin\phi\sin\theta- \cos\phi\sin\psi&\cos\phi\cos\psi+\sin\phi\sin\psi\sin\theta&\cos\theta\sin\phi\\ \sin\phi\sin\psi+ \cos\phi\cos\psi\sin\theta&\cos\phi\sin\psi\sin\theta- \cos\psi\sin\phi&\cos\phi\cos\theta \end{matrix}\right)\\ R_k^b&=R_x(\phi)R_y(\theta)\\ &=\left(\begin{matrix} \cos\theta & 0& -\sin\theta \\ \sin \theta\sin \phi &\cos \phi&\cos\theta \sin\phi\\ \sin \theta\cos \phi & -\sin \phi &\cos\theta \cos \phi\\ \end{matrix}\right)\\ R_n^b&=R_x(\phi)\\ &=\left(\begin{matrix} 1 & 0& 0 \\ 0 &\cos \phi&\sin \phi\\ 0 &-\sin \phi &cos \phi\\ \end{matrix}\right)\\ \end{align}$

由于 $ Z-Y-X$ 欧拉角每次变换旋转的轴不固定，所以欧拉角的变化率 $\neq$ 机体角速度。为了推导出来每次的角速度，我们从定义出发:

坐标系从 $e$ 系出发，先绕 $Z$ 轴 $\psi$ ,到达 $k$ 系；再绕 $k$ 系中的 $Y$ 轴 $\theta$ ，到达 $n$ 系；最后再绕 $n$ 系中的 $X$ 轴 $\phi$ ，到达 $b$ 系

那么每次变换的时候有一个轴在旋转前后的坐标系中是相同的。即 $ \mathbf e_3=\mathbf k_3,\quad \mathbf k_2=\mathbf n_2,\quad \mathbf n_1=\mathbf b_1$

$\tag{3} \overset{b}{\overrightarrow {\omega}}=\dot{\psi}\overset{b}{\mathbf k_3}+\dot{\theta}\overset{b}{\mathbf n_2}+\dot{\phi}\overset{b}{\mathbf b_1}\\ or\\ \overset{b}{\overrightarrow {\omega}}=\dot{\psi}\overset{b}{\mathbf e_3}+\dot{\theta}\overset{b}{\mathbf k_2}+\dot{\phi}\overset{b}{\mathbf n_1}$

我们采用第一种形式，将其写为矩阵，即:

$\tag{4} \begin {pmatrix} \omega_{x_b}\\ \omega_{y_b}\\ \omega_{z_b} \end {pmatrix} = \left(\begin{matrix} 1 & 0& -\sin\theta \\ 0 &\cos \phi&\cos\theta \sin\phi\\ 0&-\sin \phi &\cos\theta cos \phi\\ \end{matrix}\right) \begin{pmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end {pmatrix}$

上述矩阵中的 $3\times 3$矩阵即为欧拉矩阵。欧拉矩阵的推导方法就是把(3)式中的 $\mathbf k_3,\quad \mathbf n_2,\quad \mathbf b_1$ 当成一个简单的向量（注意是在自己的坐标系中），然后带入(2)式中，得到其在 $b$ 系下的表示， $\overset{b}{\mathbf k_3},\quad\overset{b}{\mathbf n_2},\quad\overset{b}{\mathbf b_1}$ 即可。

当 $\theta=90\degree$ 的时候，可以发现 $ \mathbf n_3=\mathbf k_1$，此时发生了“万向锁”，即绕三个轴旋转的效果可以用两个轴的旋转实现。即便绕着Z轴旋转了某一个角度，但是最后的结果仍然是相当于没有绕着Z轴旋转，损失了一个自由度。旋转的表示方法不再唯一。

旋转矩阵

旋转矩阵是一个正交矩阵，因此其逆矩阵等于其转置，且其行列式等于 $1$.

从 $e$ 系到 $b$ 系的旋转矩阵为 $R_e^b$ ，从 $b$ 系到 $e$ 系的旋转矩阵为 $R_b^e$ ，二者互为逆矩阵。

既然 $R_e^b=R_x(\phi)R_y(\theta)R_z(\psi)$ ,那么:

$\tag{5} \begin{align} R_b^e&=(R_x(\phi)R_y(\theta)R_z(\psi))^{-1}\\ &=R_z(\psi)^{-1}R_y(\theta)^{-1}(R_x(\phi)^{-1}\\ &=R_z(\psi)^{T}R_y(\theta)^{T}(R_x(\phi)^{T}\\ &=\left(\begin{matrix}\cos\theta\cos\phi&\sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi&\cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi\\\cos\theta\sin\phi&\sin\psi\sin\theta\sin\phi+\cos\psi\cos\phi&\cos\psi\sin\theta\sin\phi-\sin\psi\cos\phi\\-\sin\theta&\sin\psi\cos\theta&\cos\psi\cos\theta\end{matrix}\right) \end{align}$

由此我们可以得到 $\psi$ 、 $\theta$ 、 $\phi$ 的表达式

$\begin{cases} \psi=\arctan(\dfrac{r_{21}}{r_{11}})\\ \theta=\arcsin(r_{31})\\ \phi=\arctan(\dfrac{r_{32}}{r_{33}})\\ \end{cases}$

取 $\cos\theta=0$ ,即 $\theta=\pm \frac{\pi}{2}$的时候，

$\tag{6} R_e^b= \left( \begin{matrix} 0&\sin (\psi\mp\phi)&\cos (\psi\mp\phi)\\ 0&\cos (\psi\mp\phi)&\sin (\psi\mp\phi)\\ \mp1& 0&0 \end{matrix} \right)$

此时，给定旋转矩阵，$\psi$ 和 $\phi$ 的取值有无数种组合，奇异性仍然存在。

接下来介绍叉乘的斜对称阵（反对称阵）表示

对于一个 $3\times 3$ 的矩阵 $\mathbf a=(\mathbf a_x,\mathbf a_y,\mathbf a_z)$ 而言，其斜对称阵定义为：

$\tag{7} [\mathbf a]_\text{x}\overset{def}{=} \left( \begin{matrix} 0&-\mathbf a_z &\mathbf a_y\\ \mathbf a_z&0&-\mathbf a_x\\ -\mathbf a_y&\mathbf a_x&0 \end{matrix} \right)$

斜对称阵可以用来简化叉乘，将叉乘变成点乘

$\mathbf a \times\mathbf b=[\mathbf a]_\text{x}\cdot \mathbf b$

刚体力学复习

从转动轴上取一点，这一点到刚体上面某一点的连线所构成的矢量是位矢，位矢对于时间的导数是线速度，线速度也等于角速度和位矢的外积，即:

$\frac{\mathrm d \mathrm {\overset{e}{r}}} {\mathrm d t}={\overset{e}{\omega}}\times {\overset{e}{r}}$

四元数

所谓四元数，指的是实部为 $q_0$ ，虚部为 $q_v=[q_1,q_2,q_3]^T$ 的向量。即:

$\left[ \begin{matrix} q_w\\ \vec{q_v} \end{matrix} \right] = \left[ \begin{matrix} q_w\\ q_1\\ q_2\\ q_3 \end{matrix} \right] = q_w+q_1\vec{i}+q_2\vec{j}+q_3\vec{k}$

其中向量 $i 、j、k $的运算规则如下:

$ij=k\\ jk=i\\ ki=j\\ ji=-1\\ kj=-1\\ ik=-1\\ i^2=j^2=k^2=ijk=-1$

四元数加法和乘法

加法即对应元素相加，乘法可以展开后相乘，也可以利用下面的公式

$q_a\otimes q_b= \left[ \begin{matrix} q_{aw}\\\vec{q_{av}} \end{matrix} \right] \otimes \left[ \begin{matrix} q_{bw}\\\vec{q_{bv}} \end{matrix} \right] = \left[ \begin{matrix} q_{aw}q_{bw}-\vec{q_{av}}^T\vec{q_{bv}}\\ \vec{q_{av}}\times \vec{q_{bv}}+q_{aw}\vec{q_{bv}}+q_{bw}\vec{q_{av}} \end{matrix} \right]$

显然，四元数乘法不满足交换律。

但是，四元数乘法满足交换律和结合率:

$\begin{gathered} \mathbf{(p\otimes q)\otimes r=p\otimes(q\otimes r)} \\ \mathbf{p}\otimes(\mathbf{q}+\mathbf{r})=\mathbf{p}\otimes\mathbf{q}+\mathbf{p}\otimes\mathbf{r} \\ (\mathbf{p+q})\otimes\mathbf{r}=\mathbf{p}\otimes\mathbf{r}+\mathbf{q}\otimes\mathbf{r} \end{gathered}$

共轭、模和逆

共轭

共轭即对虚部取反，实部不变：

$\mathbf{q}^*=\begin{bmatrix}q_w\\-\mathbf{q}_v\end{bmatrix}$

四元数相乘的共轭可以拆开，和矩阵乘法之后的转置有一点像:

$(\mathbf{p}\otimes\mathbf{q})^*=\mathbf{q}^*\otimes\mathbf{p}^*$

四元数与自己的共轭相乘得到纯虚数

$\mathbf{q}\otimes\mathbf{q}^*=\mathbf{q}^*\otimes\mathbf{q}=\begin{bmatrix}q_w^2+q_x^2+q_y^2+q_z^2\\0\end{bmatrix}$

模

四元数的模定义和相关性质如下：

$\|\mathbf{q}\|=\sqrt{\mathbf{q}\otimes\mathbf{q}^*}=\sqrt{\mathbf{q}^*\otimes\mathbf{q}}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}\\\|\mathbf{p}\otimes\mathbf{q}\|=\|\mathbf{q}\otimes\mathbf{p}\|=\|\mathbf{p}\|\|\mathbf{q}\|$

逆

四元数的逆等于其共轭四元数除以模的平方:

$\mathbf{q}^{-1}=\frac{\mathbf{q}^*}{\|\mathbf{q}\|^2}\\\mathbf{q}\otimes\mathbf{q}^{-1}=\mathbf{q}^{-1}\otimes\mathbf{q}=\mathbf{q}_1=\begin{bmatrix}1\\\mathbf{0}\end{bmatrix}$

四元数和它的逆做乘法的时候满足交换律，且结果等于幺元

交换子和幺元

交换子定义如下:

$[\mathbf{p},\mathbf{q}]\triangleq\mathbf{p}\otimes\mathbf{q}-\mathbf{q}\otimes\mathbf{p}=\begin{bmatrix}0\\2\mathbf{p}_v\times\mathbf{q}_v\end{bmatrix}$

幺元定义如下:

$\mathbf{q}_1=\begin{bmatrix}1\\\mathbf{0}\end{bmatrix}\\\mathbf{q}_1\otimes\mathbf{q}=\mathbf{q}\otimes\mathbf{q}_1=\mathbf{q}$

四元数与旋转

在三位空间中，原来有一个点 $P=[x_0,y_0,z_0]^T$ ，假定此时点 $P$ 绕着某一个轴 $\mathbf{v}$ 旋转，旋转后得到点 $P’=[x_0,y_0,z_0]^T$。我们将 $\mathbf{v}$ 方向的单位矢量也用 $\mathbf{v}$ 表示，即： $||\mathbf{v}||=1$ 。