004旋转矩阵系统理解

学习惯导这么久，以为自己入门了，突然发现自己还是活在梦里。就拿这个旋转矩阵来说，在学惯导之前就已经明白是怎么回事了，然而并没有什么用，在INS的海洋里兜兜转转，又回到了旋转矩阵。入门道阻且长，惟有志存高远，方能钻研不休。
为了自己的健忘脑袋着想，还是再次完整的备忘一下关于旋转矩阵的问题。在本文中，对在学习严恭敏老师的《捷联惯导算法与组合导航原理讲义》中产生的疑问也做一个补充说明。文末附上前述讲义。

一、入门基础

在开始本文之前，首先要把坐标系定义好。我还是要偷一下懒，因为我觉得我不可能写的比这篇好。见判断三维坐标系旋转正方向的简单方法。在本文中，坐标系即右手系，正向旋转如前述判断方法。
如图所示，将坐标系 $ox_1y_1z_1$ 绕 $z_1$ 轴正向旋转至 $ox_2y_2z_2$ ， $P$ 点在 $ox_1y_1z_1$ 中的坐标为 $(x_1,y_1)$ ，在 $ox_2y_2z_2$ 中的坐标为 $(x_2,y_2)$ 。

这里写图片描述

显然，该旋转为正向旋转(不要在意旋转角度的字母)，在二维坐标系中有

[\begin{matrix} x_{2} \\ y_{2} \end{matrix}] = [\begin{matrix} c o s ψ & s i n ψ \\ - s i n ψ & c o s ψ \end{matrix}] [\begin{matrix} x_{1} \\ y_{1} \end{matrix}]

$\left[ \begin{matrix} x_2 \\ y_2 \end{matrix} \right] = \left[ \begin{matrix} cos\psi & sin\psi\\ -sin\psi & cos\psi \end{matrix} \right] \left[ \begin{matrix} x_1 \\ y_1 \end{matrix} \right]$
在三维坐标系中有

[\begin{matrix} x_{2} \\ y_{2} \\ z_{2} \end{matrix}] = [\begin{matrix} c o s ψ & s i n ψ & 0 \\ - s i n ψ & c o s ψ & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{1} \\ y_{1} \\ z_{1} \end{matrix}]

$\left[ \begin{matrix} x_2 \\ y_2 \\ z_2 \end{matrix} \right] = \left[ \begin{matrix} cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} x_1 \\ y_1 \\ z_1 \end{matrix} \right]$
即绕

Z

$Z$ 轴正向旋转的旋转矩阵为:

C_{ψ} = [\begin{matrix} c o s ψ & s i n ψ & 0 \\ - s i n ψ & c o s ψ & 0 \\ 0 & 0 & 1 \end{matrix}]

$C_\psi= \left[ \begin{matrix} cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right]$
同理，绕

X

$X$ 轴正向旋转的旋转矩阵为

C_{θ} = [\begin{matrix} 1 & 0 & 0 \\ 0 & c o s θ & s i n θ \\ 0 & - s i n θ & c o s θ \end{matrix}]

$C_\theta = \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\theta & sin\theta \\ 0 & -sin\theta & cos\theta \\ \end{matrix} \right]$
绕

Y

$Y$ 轴正向旋转的旋转矩阵为

C_{γ} = [\begin{matrix} c o s γ & 0 & - s i n γ \\ 0 & 1 & 0 \\ s i n γ & 0 & c o s γ \end{matrix}]

$C_\gamma= \left[ \begin{matrix} cos\gamma & 0 & -sin\gamma\\ 0 & 1 & 0\\ sin\gamma & 0 & cos\gamma\\ \end{matrix} \right]$
如果对绕

Y

$Y$ 轴旋转的情况不了解，请参考 003备忘补充之惯性导航基本原理（刘保中）—9.1方向余弦与方向余弦矩阵

二、方向余弦阵

这一小节通过严恭敏老师的讲义进行说明，针对20180206版本。

在该讲义的194页中，给出了两种定义欧拉角的方式，对于图B-2而言，通过以下方式旋转(注意：讲义中旋转角字母的定义跟我上面讲的不一样，不要搞混)

O X_{0} Y_{0} Z_{0} \to_{正 向 旋 转 α}^{绕 Z 轴} O X_{1} Y_{1} Z_{1} \to_{正 向 旋 转 β}^{绕 X 轴} O X_{2} Y_{2} Z_{2} \to_{正 向 旋 转 γ}^{绕 Y 轴} O X_{3} Y_{3} Z_{3}

$OX_0Y_0Z_0\xrightarrow[正向旋转\alpha]{绕Z轴}OX_1Y_1Z_1\xrightarrow[正向旋转\beta]{绕X轴}OX_2Y_2Z_2\xrightarrow[正向旋转\gamma]{绕Y轴}OX_3Y_3Z_3$
那么将坐标系从0系旋转到1系需要

[\begin{matrix} X_{1} \\ Y_{1} \\ Z_{1} \end{matrix}] = C_{0}^{1} [\begin{matrix} X_{0} \\ Y_{0} \\ Z_{0} \end{matrix}]

$\left[\begin{matrix} X_1\\ Y_1\\ Z_1 \end{matrix} \right] =C_0^1 \left[ \begin{matrix} X_0\\ Y_0\\ Z_0 \end{matrix} \right]$

C_{0}^{1} = [\begin{matrix} c o s α & s i n α & 0 \\ - s i n α & c o s α & 0 \\ 0 & 0 & 1 \end{matrix}]

$C_0^1= \left[ \begin{matrix} cos\alpha & sin\alpha & 0 \\ -sin\alpha & cos\alpha & 0 \\ 0 & 0 & 1 \end{matrix} \right]$

将坐标系从0系旋转到2系需要

[\begin{matrix} X_{2} \\ Y_{2} \\ Z_{2} \end{matrix}] = C_{0}^{2} [\begin{matrix} X_{0} \\ Y_{0} \\ Z_{0} \end{matrix}] = C_{1}^{2} C_{0}^{1} [\begin{matrix} X_{0} \\ Y_{0} \\ Z_{0} \end{matrix}]

$\left[\begin{matrix}X_2\\Y_2\\Z_2\end{matrix}\right] =C_0^2\left[\begin{matrix}X_0\\Y_0\\Z_0\end{matrix}\right] =C_1^2C_0^1\left[\begin{matrix}X_0\\Y_0\\Z_0\end{matrix}\right]$

C_{1}^{2} = [\begin{matrix} 1 & 0 & 0 \\ 0 & c o s β & s i n β \\ 0 & - s i n β & c o s β \end{matrix}]

$C_1^2=\left[\begin{matrix} 1 & 0 & 0\\ 0 & cos\beta & sin\beta \\ 0 & -sin\beta & cos\beta \end{matrix}\right]$
将坐标系从0系旋转到3系需要

[\begin{matrix} X_{3} \\ Y_{3} \\ Z_{3} \end{matrix}] = C_{0}^{3} [\begin{matrix} X_{0} \\ Y_{0} \\ Z_{0} \end{matrix}] = C_{2}^{3} C_{1}^{2} C_{0}^{1} [\begin{matrix} X_{0} \\ Y_{0} \\ Z_{0} \end{matrix}]

$\left[\begin{matrix}X_3\\Y_3\\Z_3\end{matrix}\right] =C_0^3\left[\begin{matrix}X_0\\Y_0\\Z_0\end{matrix}\right] =C_2^3C_1^2C_0^1\left[\begin{matrix}X_0\\Y_0\\Z_0\end{matrix}\right]$

C_{2}^{3} = [\begin{matrix} c o s γ & 0 & - s i n γ \\ 0 & 1 & 0 \\ s i n γ & 0 & c o s γ \end{matrix}]

$C_2^3=\left[\begin{matrix} cos\gamma & 0 & -sin\gamma \\ 0 & 1 & 0\\ sin\gamma & 0 & cos\gamma \end{matrix}\right]$

那么从3系旋转到0系即为：

C_{3}^{0} = (C_{0}^{3})^{T} = (C_{2}^{3} C_{1}^{2} C_{0}^{1})^{T} = (C_{0}^{1})^{T} (C_{1}^{2})^{T} (C_{2}^{3})^{T} = C_{1}^{0} C_{2}^{1} C_{3}^{2}

$C_3^0=(C_0^3)^T=(C_2^3C_1^2C_0^1)^T=(C_0^1)^T(C_1^2)^T(C_2^3)^T=C_1^0C_2^1C_3^2$

= [\begin{matrix} c o s α & - s i n α & 0 \\ s i n α & c o s α & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 \\ 0 & c o s β & - s i n β \\ 0 & s i n β & c o s β \end{matrix}] [\begin{matrix} c o s γ & 0 & s i n γ \\ 0 & 1 & 0 \\ - s i n γ & 0 & c o s γ \end{matrix}]

$=\left[\begin{matrix}cos\alpha & -sin\alpha & 0 \\sin\alpha & cos\alpha & 0 \\0 & 0 & 1\end{matrix}\right] \left[\begin{matrix} 1 & 0 & 0\\ 0 & cos\beta & -sin\beta \\ 0 & sin\beta & cos\beta \end{matrix}\right] \left[\begin{matrix} cos\gamma & 0 & sin\gamma \\ 0 & 1 & 0\\ -sin\gamma & 0 & cos\gamma \end{matrix}\right]$

资源：

捷联惯导算法与组合导航原理讲义(20180206)密码：mpqp