旋转与姿态的表示
1 旋转矩阵
\[\begin{aligned} {}_1^0 R &= \begin{bmatrix}
\mathbf{x}_0 \cdot \mathbf{x}_1 & \mathbf{x}_0 \cdot \mathbf{y}_1 & \mathbf{x}_0 \cdot \mathbf{z}_1 \\
\mathbf{y}_0 \cdot \mathbf{x}_1 & \mathbf{y}_0 \cdot \mathbf{y}_1 & \mathbf{y}_0 \mathbf{z}_1 \\
\mathbf{z}_0 \cdot \mathbf{x}_1 & \mathbf{z}_0 \cdot \mathbf{y}_1 & \mathbf{z}_0 \mathbf{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^{\top} \\ {}^1\mathbf{y}_0^{\top} \\ {}^1\mathbf{z}_0^{\top}\end{bmatrix}\\
\begin{bmatrix}\mathbf{x}_1 & \mathbf{y}_1 & \mathbf{z}_1\end{bmatrix} &= \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} {}_1^0 R
\end{aligned}\]
这里的 \(\mathbf{x}_0, \mathbf{y}_0, \mathbf{z}_0\) 是坐标系 0 的三个正交单位向量，\(\mathbf{x}_1, \mathbf{y}_1, \mathbf{z}_1\) 是坐标系 1 的三个正交单位向量。6 个单位向量的 原点重合。
旋转矩阵的含义：
- 表示坐标系 1 相对于坐标系 0 的姿态；
- 将坐标系 1 的基在坐标系 0 的基下重新表示；
- 对坐标系 0 的一个旋转操作符，将坐标系 0 旋转成坐标系 1；
- 对坐标系 0 下向量的一个旋转操作符。
对于一个隶属于坐标系 1 的向量 \({}^1 \mathbf{a} = \begin{bmatrix} {}^1x_{a} \\ {}^1y_{a} \\ {}^1z_{a} \end{bmatrix}\) ，可以用 \(_1^0 R \cdot {}^1\mathbf{a}\) 的方式计算该向量在坐标系 0 下的表示方式 \({}^0\mathbf{a}\) ，即
\[{}^{0}A = {}_1^{0}R\cdot {}^{1}A\]
上式可以利用旋转矩阵定义 \(\begin{bmatrix}\mathbf{x}_1 & \mathbf{y}_1 & \mathbf{z}_1\end{bmatrix} = \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} {}_1^0 R\) 证明。
Theorem
\[{}_1^0 R^{\top} = {}_1^0 R^{-1} = {}_0^1 R\]
第一个等号只需要计算 \(RR^{\top} = I\) 即可证明。
第二个等号推导过程如下。考虑三个坐标系，基坐标系和 A, B ，三坐标系原点重合，其中 A 和 B 坐标系的三个轴对应的单位向量在基坐标系下可以分别表示为 \(\begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{A}} & \overset{\rightarrow}{\mathbf{y}_{A}} & \overset{\rightarrow}{\mathbf{z}_{A}}\end{bmatrix}\) 和 \(\begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{B}} & \overset{\rightarrow}{\mathbf{y}_{B}} & \overset{\rightarrow}{\mathbf{z}_{B}}\end{bmatrix}\) . 则有如下式子成立，
\[\begin{aligned}
\begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{A}} & \overset{\rightarrow}{\mathbf{y}_{A}} & \overset{\rightarrow}{\mathbf{z}_{A}}\end{bmatrix} &= \begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{B}} & \overset{\rightarrow}{\mathbf{y}_{B}} & \overset{\rightarrow}{\mathbf{z}_{B}}\end{bmatrix}{}^{B}_{A}R\\
\begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{B}} & \overset{\rightarrow}{\mathbf{y}_{B}} & \overset{\rightarrow}{\mathbf{z}_{B}}\end{bmatrix} &= \begin{bmatrix}\overset{\rightarrow}{\mathbf{x}_{A}} & \overset{\rightarrow}{\mathbf{y}_{A}} & \overset{\rightarrow}{\mathbf{z}_{A}}\end{bmatrix}{}^{A}_{B}R
\end{aligned}\]
稍加计算即得 \({}^{A}_{B}R = {}^{B}_{A}R^{-1}\) .
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 \cdot A \cdot {}_0^1R \]
理解：以 \({}^0\zeta = B \cdot {}^0\alpha = {}_1^0R \cdot A \cdot {}_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\cdot{}^0\alpha = {}_1^0R \gamma\) ：将 \(^1\gamma\) 向量转换回坐标系 0，得到 \(^0\zeta\) 向量。
1.3 右乘连体左乘基
内旋与外旋
内旋（Intrinsic Rotation）指每次旋转都是绕物体自身更新后的坐标系轴转的。
外旋（Extrinsic Rotation）指每次旋转都是绕世界坐标系的固定轴转的。
举个例子，内旋 X-Y-Z 的旋转矩阵是 \(R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)\) ，这也是外旋 Z-Y-X 的旋转矩阵。
“右乘连体” 的理解
将旋转矩阵视为对坐标系的旋转操作符，从左到右依次旋转连体坐标系：
\[\begin{aligned}
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} R_{x}(\phi)&=\begin{bmatrix}\mathbf{x}_1 & \mathbf{y}_1 & \mathbf{z}_1\end{bmatrix}\\
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} R_{x}(\phi)R_{y}(\theta)&=\begin{bmatrix}\mathbf{x}_1 & \mathbf{y}_1 & \mathbf{z}_1\end{bmatrix}R_{y}(\theta) = \begin{bmatrix}\mathbf{x}_2 & \mathbf{y}_2 & \mathbf{z}_2\end{bmatrix}\\
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)&=\begin{bmatrix}\mathbf{x}_2 & \mathbf{y}_2 & \mathbf{z}_2\end{bmatrix}R_{z}(\psi)=\begin{bmatrix}\mathbf{x}_3 & \mathbf{y}_3 & \mathbf{z}_3\end{bmatrix}
\end{aligned}\]
可以发现，坐标系 0 依次经过 X-Y-Z 内旋得到坐标系 3 。
考虑一个在坐标系 0 下表示的初始向量 \(\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}\alpha\) ，经过 \(R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)\) 转换其坐标表示后，它被表示在坐标系 3 下：
\[\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)\alpha = \begin{bmatrix}\mathbf{x}_3 & \mathbf{y}_3 & \mathbf{z}_3\end{bmatrix}\alpha\]
“左乘基” 的理解
将旋转矩阵视为对向量的旋转操作符，从右到左依次在基坐标系下旋转向量：
\[\begin{aligned}
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix} R_{z}(\psi)\mathbf{a} &=\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}\mathbf{b}\\
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{y}(\theta)R_{z}(\psi)\mathbf{a} &= \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_y(\theta)\mathbf{b} = \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}\mathbf{c}\\
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{x}(\phi)R_y(\theta)R_{z}(\psi)\mathbf{a} &= \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{x}(\phi)\mathbf{c} = \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}\mathbf{d}
\end{aligned}\]
可以发现，向量 \(\mathbf{a}\) 沿着基坐标系的 Z-Y-X 轴顺序旋转，先后得到 \(\mathbf{b}, \mathbf{c}, \mathbf{d}\) .
小结
旋转矩阵既可理解为对坐标系的旋转操作符，也可理解为对向量的旋转操作符。
旋转矩阵的右乘是左结合的（先计算左边），因此应当理解为对连体坐标系的旋转操作符；旋转矩阵的左乘是右结合的（先计算右边），因此应当理解为对基坐标系下向量的旋转操作符。
\[\begin{aligned}
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)\alpha &= \begin{bmatrix}\mathbf{x}_3 & \mathbf{y}_3 & \mathbf{z}_3\end{bmatrix}\alpha\\
\begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}R_{x}(\phi)R_y(\theta)R_{z}(\psi)\mathbf{a} &= \begin{bmatrix}\mathbf{x}_0 & \mathbf{y}_0 & \mathbf{z}_0\end{bmatrix}\mathbf{d}
\end{aligned}\]
1.4 旋转矩阵的导数
符号约定
定义一静一动两个坐标系 \(\{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}\]
齐次变换矩阵的逆
考虑两个原点不重合的坐标系 A, B ，用齐次变换矩阵 \({}^{A}_{B}T\) 表示 B 坐标系相对于 A 坐标系的位姿（Pose）：
\[{}^{A}_{B}T = \begin{bmatrix}{}^{A}_{B}R & {}^{A}O_{B}\\ \mathbf{0} & 1\end{bmatrix}\]
我们考虑如何求齐次变换矩阵 \({}^{B}_{A}T\) 表示 A 坐标系相对于 B 坐标系的位姿.
我们已知 \({}^{B}_{A}R = {}^{A}_{B}R^{\top}\) ，还需知道如何用 \({}^{A}O_{B}\) 表示 \({}^{B}O_{A}\) .
这件事情还挺简单的，本质上 \({}^{A}O_{B} = {}^{A}\overset{\longrightarrow}{O_{A}O_{B}}, {}^{B}O_{A} = {}^{B}\overset{\longrightarrow}{O_{B}O_{A}}\) ，显然可得：
\[\begin{aligned}
{}^{B}\overset{\longrightarrow}{O_{B}O_{A}} &= -{}^{B}_{A}R \cdot {}^{A}\overset{\longrightarrow}{O_{A}O_{B}}\\
\implies {}^{B}O_{A} &= -{}^{B}_{A}R \cdot {}^{A}O_{B}
\end{aligned}\]
由此，我们可以推导得到：
\[\begin{aligned}
{}^{B}_{A}T &= {}^{A}_{B}T^{-1}\\
&= \begin{bmatrix}{}^{A}_{B}R & {}^{A}O_{B}\\ \mathbf{0} & 1\end{bmatrix}^{-1}\\
&= \begin{bmatrix}{}^{A}_{B}R^{\top} & -{}^{A}_{B}R^{\top} \cdot {}^{A}O_{B}\\ \mathbf{0} & 1\end{bmatrix}
\end{aligned}\]
点的位姿变换
考虑两个坐标系 A, B 和一个点 P ，已知齐次变换阵 \({}^{A}_{B}T\) 和 \({}^{B}P\) ，求 \({}^{A}P\) .
本质上 \({}^{B}P = {}^{B}\overset{\longrightarrow}{O_{B}P} , {}^{A}P = {}^{A}\overset{\longrightarrow}{O_{A}P}\) ，
从而有
\[\begin{aligned}
{}^{A}\overset{\longrightarrow}{O_{A}P} &= {}^{A}\overset{\longrightarrow}{O_{A}O_{B}} + {}^{B}\overset{\longrightarrow}{O_{B}P}\\
\implies {}^{A}P &= {}^{A}O_{B} + {}^{A}_{B}R \cdot {}^{B}\overset{\longrightarrow}{O_{B}P}\\
\implies {}^{A}P &= {}^{A}O_{B} + {}^{A}_{B}R \cdot {}^{B}P\\
\end{aligned}\]
写成齐次变换形式，则有
\[\begin{bmatrix}{}^{A}P\\ 1\end{bmatrix} = {}^{A}_{B}T\begin{bmatrix}{}^{B}P\\ 1\end{bmatrix}\]