0. 简介

在自动驾驶中，我们常常会面对这线性代数位姿表示和坐标系变换问题。之前作者陆陆续续写了一些

1. 线性代数位姿

1.1 矩阵伴随

$\begin{aligned} \boldsymbol{R^\mathrm{T}}\exp{(\boldsymbol{p}^\wedge)}\boldsymbol{R} &= \exp{((\boldsymbol{R^\mathrm{T}p})^\wedge)} \\ \Rightarrow\exp{(\boldsymbol{p}^\wedge)}\boldsymbol{R} &= \boldsymbol{R}\exp{((\boldsymbol{R^\mathrm{T}p})^\wedge)} \end{aligned}$

1.2 四元数实部和虚部

一个标量和一个矢量来表示四元数就会如下图所示，这样会便于我们计算四元数之间的累乘变换
$\begin{aligned} \boldsymbol{q} &= \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T \\ &= \begin{bmatrix} w & x & y & z \end{bmatrix}^T \\ &= \begin{bmatrix} s & \boldsymbol{v} \end{bmatrix}^T \end{aligned}$
乘法运算中，向量的计算方法比较容易记忆，相乘后实部为两实部相乘减去两虚部正交，虚部为两个四元数和实部和虚部交叉相乘的和加上虚部的叉积。连续旋转两个角度可以用这两个旋转对应的四元数的乘积来表示，即 $\boldsymbol{q}(t_1+t_2) = \boldsymbol{q}(t_1)\boldsymbol{q}(t_2)$
$\begin{aligned} \boldsymbol{q}_a \otimes \boldsymbol{q}_b = \boldsymbol{q}(a)\boldsymbol{q}(b) =& \quad\, \ w_aw_b - x_ax_b - y_ay_b - z_az_b \\ &+ (w_ax_b + x_aw_b + y_az_b - z_ay_b)i \\ &+ (w_ay_b - x_az_b + y_aw_b + z_ax_b)j \\ &+ (w_az_b + x_ay_b - y_ax_b + z_aw_b)k \\ =&\begin{bmatrix} s_as_b - \boldsymbol{v}_a^T\boldsymbol{v}_b^T & s_a\boldsymbol{v}_b + s_b\boldsymbol{v}_a + \boldsymbol{v}_a\times \boldsymbol{v}_b \end{bmatrix} \end{aligned}$

1.3 四元数与时间

我们说四元数是除了旋转矩阵以外的另一种对旋转表达方式，并且它不具备奇异性，可以表达任意三维旋转，因此有必要学习一下它对时间的求导方式。

首先来看四元数和角轴的转换关系，假设某个旋转运动的旋转轴为单位向量 $\boldsymbol{u}$ ，绕该轴旋转的角度为 $\theta$ 那这个旋转的对应的四元数可表示为：

$\boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} \\ \boldsymbol{u}\sin{\frac{\theta}{2}} \end{bmatrix}$

对时间求导：当旋转一段微小时间时，旋转经过角度可以视为趋于 $0$ ，因此上述四元数为：

$\begin{aligned} \Delta \boldsymbol{q} &= \begin{bmatrix} \cos{\frac{\theta}{2}} \\ \boldsymbol{u}\sin{\frac{\theta}{2}} \end{bmatrix} \\ &\approx \begin{bmatrix} 1 \\ \boldsymbol{u}\frac{\delta \theta}{2} \end{bmatrix} \\ &= \begin{bmatrix} 1 \\ \frac{1}{2} \delta\boldsymbol{\theta} \end{bmatrix} \end{aligned}$

其中， $\delta\boldsymbol{\theta}$ 的方向为旋转轴，模长为（这段时间内非常微小的）旋转角度。

下面对时间求导：

$\begin{aligned} \dot{\boldsymbol{q}} &= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}(t+\Delta{t})- \boldsymbol{q}(t)}{\Delta{t}} \\ &= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}(t)\otimes\boldsymbol{q}(\Delta{t})- \boldsymbol{q}(t)}{\Delta{t}} \\ &= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}\otimes\Delta\boldsymbol{q}- \boldsymbol{q}}{\Delta{t}} \\ &= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}\otimes( \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\theta} \end{bmatrix} - \begin{bmatrix} 1 \\ \boldsymbol{0} \end{bmatrix})}{\Delta{t}} \\ &= \boldsymbol{q}\otimes\lim_{\Delta t \rightarrow 0}\frac{( \begin{bmatrix} 0 \\ \frac{1}{2}\delta\boldsymbol{\theta} \end{bmatrix} )}{\Delta{t}} \\ &= \boldsymbol{q}\otimes\begin{bmatrix} 0 \\ \frac{1}{2}\boldsymbol{\omega} \end{bmatrix} \end{aligned}$

上式中， $\boldsymbol{\omega} = \lim_{\Delta t \rightarrow 0}\frac{\delta\boldsymbol{\theta}}{\Delta{t}}$ 是求导时间的角速度。

1.4 旋转矩阵和旋转向量

从旋转向量到旋转矩阵基本就是通过罗德里格斯公式表明：

$\boldsymbol{R} = \boldsymbol{\mathrm{\cos{\theta}}I + \mathrm{(1-\cos{\theta})}nn^\mathrm{T} + \mathrm{\sin{\theta}}n^\wedge}$

从旋转矩阵到旋转向量的过程可以利用矩阵迹（trace）的性质进行推导：

$\begin{aligned} \mathrm{tr}(\boldsymbol{R}) &= \boldsymbol{\mathrm{\cos{\theta}\ tr}(I) + \mathrm{(1-\cos{\theta})\ tr}(nn)^\mathrm{T} + \mathrm{\sin{\theta}}\ n^\wedge} \\ &= 3\cos{\theta} + (1 - \cos{\theta}) \\ &= 1 + 2\cos{\theta} \\ \Rightarrow \theta &= \arccos{\frac{\mathrm{tr}(\boldsymbol{R}) - 1}{2}} \end{aligned}$

1.5 李群李代数扰动

李群（ $S O (3)$ ）对应的李代数 $(\boldsymbol{\phi}_1, \boldsymbol{\phi}_2)$ 的指数映射时。并且两个李代数其中有一个为小量时，二次项以上的小量都可以忽略。此时， BCH 公式有以下线性形式：

$\ln{(\exp(\boldsymbol{\phi}_1^\wedge)\exp{(\boldsymbol{\phi}_2^\wedge))}}^\vee \approx\begin{cases} \boldsymbol{J}_l(\boldsymbol{\phi}_2)^{-1}\boldsymbol{\phi}_1 + \boldsymbol{\phi}_2\qquad \mathrm{当\phi_1 为小量} \\ \boldsymbol{J}_r(\boldsymbol{\phi}_1)^{-1}\boldsymbol{\phi}_2 + \boldsymbol{\phi}_1\qquad \mathrm{当\phi_2 为小量} \end{cases}$
当我们对一个旋转 $\boldsymbol{R}$ 左乘或者右乘一个扰动 $\Delta\boldsymbol{R}$ 时，BCH 近似过程可以表示为：

$\begin{aligned} \exp{(\Delta \boldsymbol{\phi}^\wedge)}\exp{(\boldsymbol{\phi}^\wedge))} &= \exp{((\boldsymbol{\phi}+J_l^{\mathrm{-1}}(\phi)\Delta\phi})^\wedge) \\ \exp{(\Delta \boldsymbol{\phi}^\wedge)}\exp{(\boldsymbol{\phi}^\wedge))} &= \exp{((\boldsymbol{\phi}+J_r^{\mathrm{-1}}(\phi)\Delta\phi})^\wedge) \end{aligned}$

而反过来，当我们对一个李代数 $\boldsymbol{\phi}$ 加上一个微小扰动 $\Delta \boldsymbol{\phi}$ 时，通过 BCH 可以近似为：

$\begin{aligned} \exp{(\boldsymbol{(\phi + \Delta\phi)}^\wedge)} &= \exp{\boldsymbol{(J_l\Delta\phi)^\wedge}}\exp{(\boldsymbol{\phi}^\wedge)} \\ &= \exp{(\boldsymbol{\phi}^\wedge)}\exp{\boldsymbol{(J_r\Delta\phi)^\wedge}} \end{aligned}$