旋转与姿态的表示
1 旋转矩阵
\[\begin{aligned} _1^0 R &= \begin{bmatrix}
x_0 \cdot x_1 & x_0 \cdot y_1 & x_0 \cdot z_1 \\
y_0 \cdot x_1 & y_0 \cdot y_1 & y_0 \cdot z_1 \\
z_0 \cdot x_1 & z_o \cdot y_1 & z_0 \cdot z_1
\end{bmatrix}\\
&= \begin{bmatrix}^0\mathbf{x}_1 & ^0\mathbf{y}_1 & ^0\mathbf{z}_1\end{bmatrix}\\
&= \begin{bmatrix}^1\mathbf{x}_0 \\ ^1\mathbf{y}_0 \\ ^1\mathbf{z}_0\end{bmatrix}
\end{aligned}\]
这里的 \(x_0, y_0, z_0\) 是坐标系 0 的三个单位向量，\(x_1, y_1, z_1\) 是坐标系 1 的三个单位向量。要求，坐标系 0 和坐标系 1 的原点是重合的。
矩阵既表示 三维坐标系 \(o_1x_1y_1z_1\) 相对于参考系 \(o_0x_0y_0z_0\) 的姿态角 ，也描述了 从三维坐标系 1 到三维坐标系 0 的旋转坐标变换 ，此外，还可以看作 在固定坐标系0中的一个旋转操作符 。
对于一个隶属于坐标系 1 的向量 \(^1 A = \begin{bmatrix} ^1x_{a} \\ ^1y_{a} \\ ^1z_{a} \end{bmatrix}\) ，可以用 \(_1^0 R \cdot (^1A)\) 的方式计算该向量在坐标系 0 下的表示方式 \(^0A\) 。
理解： \(_1^0 R\) 将坐标系 1 的基在坐标系 0 下重新表示了一遍。
性质： \((_1^0 R)^{\top} = (_1^0 R)^{-1} = (_0^1 R)\) 。
1.1 基本旋转矩阵
\[\begin{align}
R_{z, \psi} &= \begin{bmatrix}
\cos\psi & -\sin \psi & 0 \\
\sin \psi & \cos \psi & 0 \\
0 & 0 & 1
\end{bmatrix}\tag{1-1-1}\\
R_{x, \phi} &= \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos \phi & -\sin\phi \\
0 & \sin\phi & \cos\phi
\end{bmatrix}\tag{1-1-2}\\
R_{y, \theta} &= \begin{bmatrix}
\cos\theta & 0 & \sin\theta \\
0 & 1 & 0 \\
-\sin\theta & 0 & \cos\theta
\end{bmatrix}\tag{1-1-3}
\end{align}\]
1.2 相似变换
如果 A 描述了一个线性变换在坐标系 1 中的表示，B 描述了同一个线性变换在坐标系 0 中的表示，则有：
\[ B = (_1^0R) A (_0^1R) \]
理解：以 \((^0\zeta) = B \cdot (^0\alpha) = (_1^0R) A (_0^1R)\cdot (^1\alpha)\) 为例
- \(\beta = (_0^1R)\cdot (^1\alpha)\)：将 \(^0\alpha\) 向量转换到坐标系 1 内，得到 \(^1\beta\) 向量。
- \(\gamma = A \beta\) ：对 \(^1\beta\) 作线性变换 A，得到 \(^1\gamma\) 向量。
- \((^0\zeta) = B(^0\alpha) = (_1^0R) \gamma\) ：将 \(^1\gamma\) 向量转换回坐标系 0，得到 \(^0\zeta\) 向量。
1.3 旋转矩阵的导数
符号约定
定义一静一动两个坐标系 \(\{A\}\) 和 \(\{B\}\)，设它们的原点重合，
坐标系 \(\{B\}\) 正在旋转，旋转角速度在坐标系 \(\{B\}\) 下表示为 \(^B\omega\)，
在坐标系 \(\{A\}\) 下表示为 \(^A\omega\)，满足 \(^A\omega = (^AR_B)^B\omega\) .
定义反对称矩阵的符号为 \([ \cdot ]^{\wedge}\)，即若 \(\omega = \begin{bmatrix}\omega_1 & \omega_2 & \omega_3\end{bmatrix}^{\top}\)，则
\[[\omega]^{\wedge} = \begin{bmatrix}0 & -\omega_3 & \omega_2\\
\omega_3 & 0 & -\omega_1\\ -\omega_2 & \omega_1 & 0\end{bmatrix}\]
结论
\[\begin{aligned}
\dot{R}_B^A &= [^A\omega]^{\wedge} R_B^A\\
\dot{R}_B^A &= R_B^A [^B\omega]^{\wedge} \\
\dot{R}_A^B &= -R_A^B[^A\omega]^{\wedge} \\
\dot{R}_A^B &= -[^B\omega]^{\wedge} R_A^B \\
\end{aligned}\]
推导过程
参见这篇 arxiv 上的 tutorial，写得很详细。
2 任意旋转的最小参数化
三维空间的旋转一共拥有三个自由度，但在实际工程中，我们一般选四个量作为任意旋转的最小参数化表示：
- 欧拉角表示：欧拉角顺序和三个欧拉角的值
- 等轴角表示：旋转轴和旋转角度
- 四元数表示：四元数
2.1 欧拉角
欧拉角表示法将一个任意旋转拆分为三次基本旋转，通过指定三次旋转的旋转轴和旋转角度来表示任意一个旋转。
注意，每次旋转都绕着机体坐标系的某根轴。
欧拉角 -> 旋转矩阵
一般来说，我们规定绕 \(X\) 轴旋转的角度为 \(\phi\)，绕 \(Y\) 轴旋转的角度为 \(\theta\)，绕 \(Z\) 轴旋转的角度为 \(\psi\) .
我们给出一些结论：
\[\begin{align}
R_{XYZ} &= \begin{bmatrix}
c\theta c\psi & -c\theta s\psi & s\theta \\
s\phi s\theta c\psi + c\phi s\psi & -s\phi s\theta s\psi + c\phi c\psi & -s\phi c\theta \\
-c\phi s\theta c\psi + s\phi s\psi & c\phi s\psi s\theta + s\phi c\phi & c\phi c\theta
\end{bmatrix} \tag{2-1-1}\\
R_{ZYX} &= \begin{bmatrix}
c\psi c\theta & -s\psi c\phi + c\psi s\theta s\phi & s\psi s\phi + c\psi s\theta c\phi\\
s\psi c\theta & c\psi c\phi + s\psi s\theta s\phi & -c\psi s\phi + s\psi s\theta c\phi\\
-s\theta & c\theta s\phi & c\theta c\phi
\end{bmatrix} \tag{2-1-2}\\
\end{align}\]
欧拉角奇异点（万向节死锁）
我们以 \(ZY'X''\) 欧拉角为例，当绕 \(Y'\) 轴转动的 Pitch 角度 \(\theta\) 是 \(\pi/2\) 的奇数倍时，\(X''\) 轴和 \(Z\) 轴重合了，此时 \(\phi\) 和 \(\psi\) 有无穷多种可能，无法确定。这就是欧拉角的奇异点，该现象又称万向节死锁。
欧拉角的导数
我们利用旋转矩阵导数的结论，推导欧拉角的导数与角速度的关系，以 ZYX 欧拉角为例：
\[\begin{aligned}
\dot{R}_{ZYX} &= \dot{R}_Z R_Y R_X + R_Z \dot{R}_Y R_X + R_Z R_Y \dot{R}_X\\
&= [\omega]^{\wedge}R_{ZYX}\\
\Rightarrow [\omega]^{\wedge} &= \dot{R}_{ZYX} R_{ZYX}^{\top}\\
&= (\dot{R}_Z R_Y R_X + R_Z \dot{R}_Y R_X + R_Z R_Y \dot{R}_X)R_X^{\top}R_Y^{\top}R_Z^{\top}\\
&= \dot{R}_Z R_Z^{\top} + R_Z \dot{R}_Y R_Y^{\top} R_Z^{\top} + R_Z R_Y \dot{R}_X R_X^{\top} R_Y^{\top} R_Z^{\top}
\end{aligned}\]
其中
\[\begin{aligned}
\dot{R}_Z &= \dot\psi\begin{bmatrix}
-s\psi & -c\psi & 0\\
c\psi & -s\psi & 0\\
0 & 0 & 0\\
\end{bmatrix}\\
\dot{R}_Y &= \dot\theta\begin{bmatrix}
-s\theta & 0 & c\theta\\
0 & 0 & 0\\
-c\theta & 0 & -s\theta\\
\end{bmatrix}\\
\dot{R}_X &= \dot\phi\begin{bmatrix}
0 & 0 & 0\\
0 & -s\phi & -c\phi\\
0 & c\phi & -s\phi\\
\end{bmatrix}
\end{aligned}\]
所以
\[\begin{aligned}
\left[\omega\right]^{\wedge} &= \begin{bmatrix}0 & -1 & 0\\
1 & 0 & 0\\
0 & 0 & 0\end{bmatrix}\dot\psi + \begin{bmatrix}
0 & 0 & c\psi \\
0 & 0 & s\psi \\
-c\psi & -s\psi & 0
\end{bmatrix} \dot\theta + \begin{bmatrix}0 & s\theta & s\psi c\theta\\
-s\theta & 0 & -c\psi c\theta\\
-s\psi c\theta & c\psi c\theta & 0\end{bmatrix} \dot\phi\\
&= \begin{bmatrix}0 & -\dot\psi + \dot\phi s\theta & \dot\theta c\psi + \dot \phi s\psi c\theta\\
\dot\psi - \dot\phi s\theta & 0 & \dot\theta s\psi -\dot\phi c\psi c\theta\\
-\dot\theta c\psi - \dot \phi s\psi c\theta & -\dot\theta s\psi + \dot\phi c\psi c\theta & 0\end{bmatrix}\\
\Rightarrow \begin{bmatrix}\omega_x\\ \omega_y \\ \omega_z\end{bmatrix} &=
\begin{bmatrix} \dot\phi c\psi c\theta - \dot\theta s\psi\\
\dot \phi s\psi c\theta + \dot\theta c\psi\\
\dot\psi - \dot\phi s\theta\end{bmatrix} = \begin{bmatrix}c\psi c\theta & -s\psi & 0\\
s\psi c\theta & c\psi & 0\\
-s\theta & 0 & 1\end{bmatrix}
\begin{bmatrix}\dot\phi \\ \dot\theta \\ \dot\psi\end{bmatrix}
\end{aligned}\]
所以
\[\begin{bmatrix}\dot\phi \\ \dot\theta \\ \dot\psi\end{bmatrix} =
\begin{bmatrix}\frac{c\psi}{c\theta} & \frac{s\psi}{c\theta} & 0\\
-s\psi & c\psi & 0\\
\frac{s\theta c\psi}{c\theta} & \frac{s\theta s\psi}{c\theta} & 1\end{bmatrix}
\begin{bmatrix}\omega_x \\ \omega_y \\ \omega_z\end{bmatrix}\tag{2-1-3}\]
上式推导中的 \(\omega\) 在世界坐标系下，我们在机体坐标系下也推导一遍：
\[\begin{aligned}
\left[^B\omega\right]^{\wedge} &= R_{ZYX}^{\top} \dot{R}_{ZYX}\\
&= R_X^{\top} R_Y^{\top} R_Z^{\top} (\dot{R}_Z R_Y R_X + R_Z \dot{R}_Y R_X + R_Z R_Y \dot{R}_X)\\
&= R_X^{\top} R_Y^{\top} R_Z^{\top} \dot{R}_Z R_Y R_X + R_X^{\top}R_Y^{\top} \dot{R}_Y R_X + R_X^{\top} \dot{R}_X \\
&= \begin{bmatrix}0 & -c\theta c\phi & c\theta s\phi\\
c\phi c\theta & 0 & s\theta\\
-s\phi c\theta & -s\theta & 0\end{bmatrix}\dot\psi + \begin{bmatrix}0 & s\phi & c\phi\\
-s\phi & 0 & 0\\
-c\phi & 0 & 0\end{bmatrix}\dot\theta + \begin{bmatrix}0 & 0 & 0\\
0 & 0 & -1\\
0 & 1 & 0\end{bmatrix}\dot\phi\\
\Rightarrow \begin{bmatrix}^B\omega_x\\ ^B\omega_y \\ ^B\omega_z\end{bmatrix}&= \begin{bmatrix}\dot\phi - \dot\psi s\theta\\
\dot\theta c\phi + \dot\psi s\phi c\theta\\
-\dot\theta s\phi + \dot\psi c\phi c\theta
\end{bmatrix} = \begin{bmatrix}1 & 0 & -s\theta\\
0 & c\phi & s\phi c\theta\\
0 & -s\phi & c\phi c\theta
\end{bmatrix}
\begin{bmatrix}\dot\phi \\ \dot\theta\\ \dot\psi\end{bmatrix}
\end{aligned}\]
所以
\[
\begin{aligned}
\begin{bmatrix}\dot\phi \\ \dot\theta\\ \dot\psi\end{bmatrix}
&= \begin{bmatrix}1 & s\phi \tan\theta & c\phi \tan\theta\\
0 & c\phi & -s\phi\\
0 & \frac{s\phi}{c\theta} & \frac{c\phi}{c\theta}\end{bmatrix}
\begin{bmatrix}^B\omega_x\\ ^B\omega_y \\ ^B\omega_z\end{bmatrix}
\end{aligned}\tag{2-1-4}
\]
2.2 等轴角表示
等轴角表示 -> 旋转矩阵
\[ R_{k, \theta} = \begin{bmatrix}
k_x^2v_{\theta}+c_{\theta} & k_xk_yv_{\theta}-k_zs_{\theta} & k_xk_zv_{\theta}+k_ys_{\theta}\\ k_xk_yv_{\theta}+k_zs_{\theta} & k_y^2v_{\theta}+c_{\theta} & k_yk_zv_{\theta}-k_xs_{\theta}\\ k_xk_zv_{\theta}-k_ys_{\theta} & k_yk_zv_{\theta}+k_xs_{\theta} & k_z^2v_{\theta}+c_{\theta}
\end{bmatrix} \tag{2-2-1}\]
其中，\(v_{\theta} = vers(\theta) = 1 - c_{\theta}\) ，\(k\)是单位向量\(\begin{bmatrix}k_x & k_y & k_z\end{bmatrix}^{\top}\) .
旋转矩阵 -> 等轴角表示
若已知一个等轴角表示的旋转矩阵：
\[R_{\mathbf{k}, \theta} = \begin{bmatrix}
r_{11} & r_{12} & r_{13}\\
r_{21} & r_{22} & r_{23}\\
r_{31} & r_{32} & r_{33}
\end{bmatrix}\]
则有
\[\begin{align}
\theta &= \arccos \frac{r_{11} + r_{22} + r_{33} - 1}{2} \tag{2-2-2}\\
\mathbf{k} &= \frac{1}{2\sin\theta}\begin{bmatrix}r_{32} - r_{23}\\ r_{13} - r_{31}\\ r_{21} - r_{12}\end{bmatrix}\tag{2-2-3}
\end{align}\]
2.3 四元数
四元数的详细内容参见 quaternion. Yan-Bin Jia, 2024.
四元数 \(v = w+xi+yj+zk\) 其中 \(i,j,k\) 可以视为三个垂直的坐标轴，满足叉乘关系。
等轴角表示转四元数
四元数和等轴角表示的旋转矩阵有如下转换关系：
\[w = \cos(\theta/2), x = \sin(\theta/2)k_x, y = \sin(\theta/2)k_y, z = \sin(\theta/2)k_z\]
四元数转旋转矩阵
\[R = \begin{bmatrix}
1-2y^2-2z^2 & 2xy-2wz & 2wy+2xz \\
2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx \\
2xz-2wy & 2wx+2yz & 1-2x^2-2y^2
\end{bmatrix}\]
四元数乘法
设两个四元数分别为 \(p = p_0 + \mathbf{p}\) 和 \(q = q_0 + \mathbf{q}\) ，则其乘法定义为
\[p\otimes q = p_0q_0 - \mathbf{p} \cdot \mathbf{q} + p_0 \mathbf{q} + q_0 \mathbf{p} + \mathbf{p} \times \mathbf{q}\]
该定义其实就是将两个四元数相乘展开化简的结果。其中 \(i,j,k\) 按照叉乘的规定计算，即 \(ij = k, jk = i, ki = j, ji = -k, kj = -i, ik = -j\) 。
四元数导数
对于一个含时间参数的单位四元数 \(\hat{q}(t) = (q_0, \mathbf{q})\) （描述了刚体相对于世界坐标系的姿态变化），以及定义在自身/世界坐标系下的角速度对应的四元数 \(^B\omega(t) = \left(0, \boldsymbol{\omega}(t)\right),\ ^W\omega(t) = \left(0, \ ^W\boldsymbol{\omega}(t)\right)\) ，则有
\[\dot{\hat{q}} = \frac{1}{2} \hat{q} \otimes \ (^B\omega)= \frac{1}{2} (^W\omega) \otimes \hat{q}\]
证明详见上文参考资料的 14-16 页。
我们更加深入一点，有
\[\begin{aligned}
\dot{\hat{q}} &= \frac{1}{2} \hat{q} \otimes \ ^B\omega \\
&= \frac{1}{2}\left(-\mathbf{q}\cdot \boldsymbol{\omega} + q_0 \boldsymbol{\omega} + \mathbf{q} \times \boldsymbol{\omega} \right)
\end{aligned}\]
所以
\[\dot{q_0} = -\frac{1}{2}\mathbf{q}^{\top}\boldsymbol{\omega},\ \ \dot{\mathbf{q}} = \frac{1}{2} \left(q_0 \mathbf{I}_{3\times3} + \mathbf{q}^{\wedge} \right) \boldsymbol{\omega}\]
给出一种有别于参考资料的证明方式：
我们考虑固定坐标系中的一个固定向量 \(r\) ，其在机体坐标系下的表示为 \(r_b\) ，则有
\[\begin{bmatrix}0\\ r\end{bmatrix} = q\otimes \begin{bmatrix}0\\r_b\end{bmatrix}\otimes q^* \tag{2-3-1}\]
对等式两边同时求导，有
\[\begin{bmatrix}0\\ ^{\mathcal{W}}\boldsymbol{\omega}_{\mathcal{B}}\times r\end{bmatrix} = \dot{q}\otimes \begin{bmatrix}0\\r_b\end{bmatrix}\otimes q^* + q\otimes \begin{bmatrix}0\\r_b\end{bmatrix}\otimes \dot{(q^*)}\]
注意到 \(\dot{(q^*)} = (\dot{q})^*\) 。将（2-3-1）式代入，有
\[\begin{aligned}
\begin{bmatrix}0\\ ^{\mathcal{W}}\boldsymbol{\omega}_{\mathcal{B}}\times r\end{bmatrix} &= \dot{q} \otimes q^* \otimes \begin{bmatrix}0\\r\end{bmatrix} + \begin{bmatrix}0\\r\end{bmatrix} \otimes q \otimes (\dot{q})^*\\
&= \dot{q} \otimes q^* \otimes \begin{bmatrix}0\\r\end{bmatrix} + \begin{bmatrix}0\\r\end{bmatrix} \otimes (\dot{q} \otimes q^*)^*\\
\end{aligned}\]
设
\[\dot{q} \otimes q^* = \begin{bmatrix}v_0\\ \boldsymbol{v}\end{bmatrix} \tag{2-3-2}\]
则
\[\begin{aligned}
\begin{bmatrix}0\\ ^{\mathcal{W}}\boldsymbol{\omega}_{\mathcal{B}}\times r\end{bmatrix} &= \dot{q} \otimes q^* \otimes \begin{bmatrix}0\\r\end{bmatrix} + \begin{bmatrix}0\\r\end{bmatrix} \otimes (\dot{q} \otimes q^*)^*\\
&= \begin{bmatrix}v_0\\ \boldsymbol{v}\end{bmatrix}\otimes \begin{bmatrix}0\\r\end{bmatrix} + \begin{bmatrix}0\\r\end{bmatrix} \otimes \begin{bmatrix}v_0\\ -\boldsymbol{v}\end{bmatrix}\\
&= \begin{bmatrix}-\boldsymbol{v} \cdot r + r\cdot \boldsymbol{v}\\ v_0r + \boldsymbol{v}\times r + v_0r - r\times \boldsymbol{v}\end{bmatrix}\\
&= \begin{bmatrix}0\\ 2(v_0r + \boldsymbol{v}\times r)\end{bmatrix}
\end{aligned}\]
所以 \(v_0 = 0, \boldsymbol{v} = \dfrac{1}{2}(^{\mathcal{W}}\boldsymbol{\omega}_{\mathcal{B}})\) 。代回到（2-3-2）式，得到
\[\dot{q} = \frac{1}{2}\begin{bmatrix}0\\ ^\mathcal{W}\boldsymbol{\omega}_\mathcal{B}\end{bmatrix}\otimes q\]
3 齐次变换
齐次变换利用 SE(3) 中的矩阵来描述刚性运动：平移和旋转。
\[
T = \begin{bmatrix} \mathbf{R}_{3\times 3} & \mathbf{t}_{3\times 1}\\ \mathbf{0}_{1\times 3} & 1 \end{bmatrix}
\]
其中，\(\mathbf{R} \in \mathrm{SO}(3)\) 是一个旋转矩阵，\(\mathbf{t} \in \mathbb{R}^3\) 是一个平移向量。
齐次变换矩阵可直接用于描述一个刚体相对于另一个刚体的位置与姿态。
齐次变换矩阵之所以要写成 4*4 的形式，是为了实现一个合理的乘法运算，继承旋转矩阵的链式性质。
\[\begin{aligned}
{}^{C}T_{A} &= {}^{C}T_{B} {}^{B}T_{A}\\
&= \begin{bmatrix}{}^{C}\mathbf{R}_{B} & {}^{C}\mathbf{t}_B\\ \mathbf{0} & 1\end{bmatrix}
\begin{bmatrix}{}^{B}\mathbf{R}_{A} & {}^{B}\mathbf{t}_A\\ \mathbf{0} & 1\end{bmatrix}\\
&= \begin{bmatrix}{}^{C}\mathbf{R}_{B}{}^{B}\mathbf{R}_{A} & {}^{C}\mathbf{R}_{B}{}^{B}\mathbf{t}_{A}+{}^{C}\mathbf{t}_{B}\\ \mathbf{0} & 1\end{bmatrix}
\end{aligned}\]