在自动驾驶中，我们经常面临线性代数位置表示和坐标系变换的问题。作者之前写过一些

R T exp ( p ∧ ) R = exp ( ( R T p ) ∧ ) ? exp ( p ∧ ) R = R exp ( ( R T p ) ∧ ) \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} RTexp(p∧)R?exp(p∧)R=exp((RTp)∧)=Rexp((RTp)∧)​

一个标量和一个矢量来表示四元数就会如下图所示，这样会便于我们计算四元数之间的累乘变换 q = [ q 0 q 1 q 2 q 3 ] T = [ w x y z ] T = [ s v ] T \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} q​=[q0​​q1​​q2​​q3​​]T=[w​x​y​z​]T=[s​v​]T​ 乘法运算中，向量的计算方法比较容易记忆，相乘后实部为两实部相乘减去两虚部正交，虚部为两个四元数和实部和虚部交叉相乘的和加上虚部的叉积。连续旋转两个角度可以用这两个旋转对应的四元数的乘积来表示，即 q ( t 1 + t 2 ) = q ( t 1 ) q ( t 2 ) \boldsymbol{q}(t_1+t_2) = \boldsymbol{q}(t_1)\boldsymbol{q}(t_2) q(t1​+t2​)=q(t1​)q(t2​) q a ⊗ q b = q ( a ) q ( b ) =     w a w b − x a x b − y a y b − z a z b + ( w a x b + x a w b + y a z b − z a y b ) i + ( w a y b − x a z b + y a w b + z a x b ) j + ( w a z b + x a y b − y a x b + z a w b ) k = [ s a s b − v a T v b T s a v b + s b v a + v a × v b ] \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} qa​⊗qb​=q(a)q(b)==​ wa​wb​−xa​xb​−ya​yb​−za​zb​+(wa​xb​+xa​wb​+ya​zb​−za​yb​)i+(wa​yb​−xa​zb​+ya​wb​+za​xb​)j+(wa​zb​+xa​yb​−ya​xb​+za​wb​)k[sa​sb​−vaT​vbT​​sa​vb​+sb​va​+va​×vb​​]​

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

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

q = [ cos ⁡ θ 2 u sin ⁡ θ 2 ] \boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} \\ \boldsymbol{u}\sin{\frac{\theta}{2}} \end{bmatrix} q=[cos2θ​usin2θ​​