定点转动与四元数

描述刚体的转动，我们有旋转矩阵、旋转向量、欧拉角等多种方式。其中旋转矩阵有9个变量，用来描述三自由度的旋转显得太冗余了。旋转向量使用一个向量表示旋转轴，向量的长度用来描述转角，但并不方便计算，往往需要通过罗德里格斯公式转换成旋转矩阵。欧拉角通过先后三次绕不同轴地转动来组合出目标旋转，十分直观，但是存在奇异性。所以我们还是希望能够有一种紧凑地方式来描述刚体地转动，既方便计算，又不存在奇异性。四元数就是为了这一目的而设计的。

四元数是一种扩展的复数，它使用一个标量和一个三轴矢量来描述旋转。其中的标量构成扩展复数的实部，三轴矢量构成其虚部。三轴矢量的各轴之间相互正交，并以虚单位作为单位长度。四元数只需要四个量就可以描述任意的旋转运动，而且具有比较良好的运算特性。缺点就是不直观，我们很难根据一个四元数想象出刚体的实际运动。

本文中，我们从刚体的定点转动出发，引出四元数，然后介绍四元数的特性和运算法则，以及两者之间的关系。

1. 从定点转动到四元数

如右图所示，设参考坐标系$R$，坐标轴为$x_R, y_R, z_R$，坐标轴所对应的单位向量记为$\vec{i}_R, \vec{j}_R, \vec{k}_R$。有一刚体相对$R$系做定点转动，记定点为$O$。建立与刚体固连的动坐标系$b$，坐标轴为$x_b, y_b, z_b$，坐标轴所对应的单位向量记为$\vec{i}_b, \vec{j}_b, \vec{k}_b$。初始时刻$R$系与$b$系重合。

取刚体上一点$A$，记其相对于定点的位置矢量为$\vec{OA} = \vec{r}$。经过时间$t$后运动到$A'$的位置上，此时的位置矢量为$\vec{OA'} = \vec{r'}$。根据欧拉定理，仅考虑0时刻到$t$时刻的角位置时，从$A$到$A'$的转动可以等效为绕瞬轴$\vec{u}$转动$\theta$角。为了计算方便，我们认为$\vec{u}$为单位向量。这样，位置矢量做圆锥运动，$A$和$A'$在同一个圆上，圆心为$O'$，并且$\vec{OO'}$与瞬轴$\vec{u}$同向。

我们在圆$O'$上取一点$B$使得$\angle AO'B = 90°$，根据右图，可以写出如下的几何关系：

$$ \begin{cases} \vec{OO'} & = & (\vec{r} · \vec{u})\vec{u} \\ \vec{O'A} & = & \vec{r} - \vec{OO'} = \vec{r} - (\vec{r} · \vec{u})\vec{u} \\ \vec{O'B} & = & \vec{u} \times \vec{r} \\ \vec{O'A'} & = & \vec{O'A}\cos \theta + \vec{O'B}\sin \theta = \cos \theta \vec{r} - \cos \theta (\vec{r} · \vec{u})\vec{u} + \sin \theta (\vec{u} \times \vec{r}) \end{cases} $$

所以

$$ \vec{r'} = \vec{OO'} + \vec{O'A'} = \cos \theta \vec{r} + (1 - \cos \theta)(\vec{r} · \vec{u})\vec{u} + \sin \theta (\vec{u} \times \vec{r}) $$

根据三重矢积计算公式：

$$ \begin{array}{rcl} \vec{u} \times (\vec{u} \times \vec{r}) & = & \vec{u}(\vec{u} · \vec{r}) - (\vec{u} · \vec{u})\vec{r} \\ & = & (\vec{r} · \vec{u})\vec{u} - \vec{r} \end{array} \Rightarrow (\vec{r} · \vec{u})\vec{u} = \vec{r} + \vec{u} \times (\vec{u} \times \vec{r})$$

所以

$$ \begin{array}{rcl} \vec{r'} & = & \cos \theta \vec{r} + (1 - \cos \theta)\left[\vec{r} + \vec{u} \times (\vec{u} \times \vec{r})\right] + \sin \theta (\vec{u} \times \vec{r}) \\ & = & \vec{r} + \sin \theta (\vec{u} \times \vec{r}) + (1 - \cos \theta)\vec{u} \times (\vec{u} \times \vec{r}) \end{array} $$

在$R$坐标系下，记

$$ \vec{r'} = \begin{bmatrix} r_x' \\ r_y' \\ r_z'\end{bmatrix}, \vec{r} = \begin{bmatrix} r_x \\ r_y \\ r_z\end{bmatrix}, \vec{u} = \begin{bmatrix} l \\ m \\ n\end{bmatrix} $$

根据矢量叉积与斜对称矩阵之间的对应关系，记矩阵$U = \vec{u} \times = \begin{bmatrix} 0 & -n & m \\ n & 0 & -l \\ -m & l & 0 \end{bmatrix}$，进一步矢量$\vec{r'}$可以写作：

$$ \begin{equation}\label{f1.1} \begin{array}{rcl} \vec{r'} & = & \vec{r} + \sin \theta U · \vec{r} + (1 - \cos \theta)U · U · \vec{r} \\ & = & \left(I + 2\sin \cfrac{\theta}{2} \cos \cfrac{\theta}{2} U + 2\sin^2 \cfrac{\theta}{2} U · U \right)\vec{r} \end{array} \end{equation} $$

由于点$A$是刚体上的一点，所以在整个转动过程中，它将与刚体一起转动。位置矢量$\vec{r}$描述的正是点$A$在$b$系下的矢量。而点$A'$则是点$A$在$R$系下的投影。所以上式描述的是$b$系中的矢量到$R$系的变换过程，其坐标变换矩阵$C_b^R$为：

$$ \begin{equation}\label{f1.2} C_b^R = I + 2\sin \cfrac{\theta}{2} \cos \cfrac{\theta}{2} U + 2\sin^2 \cfrac{\theta}{2} U · U \end{equation} $$

这就是著名的罗德里格斯(Rodrigues)公式，写成矩阵的形式有：

$$ C_b^R = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} + 2\cos \cfrac{\theta}{2}\begin{bmatrix} 0 & -n \sin \cfrac{\theta}{2} & m \sin \cfrac{\theta}{2} \\ n\sin \cfrac{\theta}{2} & 0 & -l \sin \cfrac{\theta}{2} \\ -m\sin \cfrac{\theta}{2} & l \sin \cfrac{\theta}{2} & 0 \end{bmatrix} + 2 \begin{bmatrix} -(n^2 + m^2)\sin^2 \cfrac{\theta}{2} & lm\sin^2 \cfrac{\theta}{2} & ln\sin^2 \cfrac{\theta}{2} \\ lm\sin^2 \cfrac{\theta}{2} & -(n^2 + l^2)\sin^2 \cfrac{\theta}{2} & nm\sin^2 \cfrac{\theta}{2} \\ ln\sin^2 \cfrac{\theta}{2} & mn\sin^2 \cfrac{\theta}{2} & -(m^2 + l^2)\sin^2 \cfrac{\theta}{2} \end{bmatrix} $$

令$q_0 = \cos \cfrac{\theta}{2}, q_1 = l \sin \cfrac{\theta}{2}, q_2 = m \sin \cfrac{\theta}{2}, q_3 = n \sin \cfrac{\theta}{2}$，则 $$ \begin{equation}\label{f1.3} C_b^R = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1q_2 - q_0q_3) & 2(q_0q_2 + q_1q_3) \\ 2(q_0q_3 + q_1q_2) & 1 - 2(q_1^2 + q_3^2) & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3 - q_0q_2) & 2(q_0q_1 + q_2q_3) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix} \end{equation} $$

我们可以用$q_0, q_1, q_2, q_3$构造四元数 $\vec{q}$ 来描述刚体的定点转动，即当只关心$b$系相对于$R$系的角位置关系时，可以认为$b$系是由$R$经过一次等效旋转形成，而四元数 $\vec{q}$ 则描述了该等效旋转的所有信息。

$$ \begin{equation}\label{f1.4} \begin{array}{rcl} \vec{q} & = & q_0 + q_1 \vec{i}_R + q_2 \vec{j}_R + q_3 \vec{k}_R = \cos \cfrac{\theta}{2} + (l \vec{i}_R + m \vec{j}_R + n \vec{k}_R) \sin \cfrac{\theta}{2}\\ & = & \cos \cfrac{\theta}{2} + \vec{u}^R \sin \cfrac{\theta}{2} \end{array}\end{equation} $$

其中$\theta$描述了转动的角度，$\vec{u}^R = l \vec{i}_R + m \vec{j}_R + n \vec{k}_R$则描述了$R$系下旋转瞬轴和旋转方向。该四元数确定了从$b$系到$R$系的坐标变换矩阵$C_b^R$。

2. 四元数形式

所谓的四元数就是由四个元构成的数，我们可以记为:

$$ \begin{equation} \label{f2.1} \vec{q}(q_0, q_1, q_2, q_3) = q_0 + q_1 \vec{i} + q_2 \vec{j} + q_3 \vec{k} \end{equation} $$

其中，$q_0, q_1, q_2, q_3$是四个实数，$\vec{i}, \vec{j}, \vec{k}$是三个互相正交的单位向量，并且是虚单位$\sqrt{-1}$，具体体现在如下的乘法规则中：

$$\begin{equation} \label{f2.2} \begin{cases} \vec{i} \otimes \vec{i} = -1, & \vec{j} \otimes \vec{j} = -1, & \vec{k} \otimes \vec{k} = -1 \\ \vec{i} \otimes \vec{j} = \vec{k}, & \vec{j} \otimes \vec{k} = \vec{i}, & \vec{k} \otimes \vec{i} = \vec{j} \\ \vec{j} \otimes \vec{i} = -\vec{k}, & \vec{k} \otimes \vec{j} = -\vec{i}, & \vec{i} \otimes \vec{k} = -\vec{j} \\ \end{cases} \end{equation} $$

上式中，符号$\otimes$表示四元数乘法。相同的单位向量做四元数乘时呈虚单位特性，结果是 -1；不同的单位向量做四元数乘时呈叉乘特性，得到的是第三个轴。四元数有很多种表达方式，式($\ref{f2.1}$)是一种复数式的记法。与复数的定义类似，四元数$\vec{q}$也存在共轭四元数$\vec{q}^* = q_0 - q_1 \vec{i} - q_2 \vec{j} - q_3 \vec{k}$，使得$\vec{q} \otimes \vec{q}^* = \left\| \vec{q} \right\|$。二范数$\left\| \vec{q} \right\|$是四元数$\vec{q}$的模长:

$$ \begin{equation} \label{f2.3} \left\| \vec{q} \right\| = q_0^2 + q_1^2 + q_2^2 + q_3^2 \end{equation} $$

如果模长$\left\| \vec{q} \right\| = 1$，那么我们称$\vec{q}$为规范化四元数，或单位四元数。有时我们也会用一个标量$q_s$和一个向量$\vec{q_v}$的形式把$\vec{q}$写成矢量式:

$$ \begin{equation} \label{f2.4} \vec{q}(q_0, q_1, q_2, q_3) = \begin{bmatrix} q_s \\ \vec{q_v} \end{bmatrix}, \vec{q_v} = \begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix} \end{equation} $$

标量$q_s$被称为$\vec{q}$的实部，向量$\vec{q_v}$则是其虚部。参照刚才定点转动的物理背景，单位四元数还可以表示成三角函数的形式：

$$ \begin{equation} \label{f2.5} \vec{q} = \cos \cfrac{\theta}{2} + \vec{u} \sin \cfrac{\theta}{2} \end{equation} $$

其中$\theta$是一个实数，$\vec{u}$则是由三个虚轴线性组合出的一个单位向量。还可以将之写成指数的形式，其中$\theta$和$\vec{u}$的定义与三角式相同：

$$ \begin{equation} \label{f2.6} \vec{q} = e^{\vec{u}\frac{\theta}{2}} \end{equation} $$

3. 四元数的运算

若$\vec{q} = \begin{bmatrix} q_s & \vec{q_v} \end{bmatrix}^T$与$\vec{p} = \begin{bmatrix} p_s & \vec{p}_v \end{bmatrix}^T$是两个四元素，那么$\vec{q} \pm \vec{p} = \begin{bmatrix} q_s \pm p_s \\ \vec{q}_v \pm \vec{p}_v \end{bmatrix}$。对于四元数的加法，加法交换律和结合律仍然适用。

若$a$为一标量，$\vec{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$为一四元数，则两者相乘为$a\vec{q} = \begin{bmatrix} aq_0 & aq_1 & aq_2 & aq_3 \end{bmatrix}^T$。

若$\vec{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$与$\vec{p} = \begin{bmatrix} p_0 & p_1 & p_2 & p_3 \end{bmatrix}^T$是两个四元素，那么四元素的乘法$\vec{q} \otimes \vec{p}$就是将两个四元数的各项分别相乘后求和，即： $$ \begin{array}{rcl} \vec{q} \otimes \vec{p} &=& (q_0 + q_1 \vec{i} + q_2 \vec{j} + q_3 \vec{k}) \otimes (p_0 + p_1 \vec{i} + p_2 \vec{j} + p_3 \vec{k}) \\ &=& (q_0p_0 - q_1p_1 - q_2p_2 - q_3p_3) + (q_0p_1 + q_1p_0 + q_2p_3 - q_3p_2) \vec{i} + (q_0p_2 + q_2p_0 + q_3p_1 - q_1p_3)\vec{j} + (q_0p_3 + q_3p_0 + q_1p_2 - q_2p_1)\vec{k} \\ &=& r_0 + r_1\vec{i} + r_2\vec{j} + r_3\vec{k} \end{array} $$ 写成矩阵的形式有： $$ \begin{equation}\label{f2.7} \begin{bmatrix} r_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix} = \underset{Q}{\underbrace{\begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & q_0 & -q_3 & q_2 \\ q_2 & q_3 & q_0 & -q_1 \\ q_3 & -q_2 & q_1 & q_0 \end{bmatrix}}}\begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ p_3 \end{bmatrix} = \underset{\bar{P}}{\underbrace{\begin{bmatrix} p_0 & -p_1 & -p_2 & -p_3 \\ p_1 & p_0 & p_3 & -p_2 \\ p_2 & -p_3 & p_0 & p_1 \\ p_3 & p_2 & -p_1 & p_0 \end{bmatrix}}}\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \end{equation}$$ 本文中，我们分别用大写字母 $Q, \bar{Q}$ 表示上式乘法运算中四元数 $\vec{q}$的左乘矩阵和右乘矩阵形式。乘法是不满足交换律的，但满足分配律和结合律，即：

$$ \begin{equation}\begin{array}{rcl} \label{f2.8} \vec{q}_0 \otimes \vec{q}_1 & \neq & \vec{q}_1 \otimes \vec{q}_0 \\ \vec{q}_0 \otimes (\vec{q}_1 + \vec{q}_2) & = & \vec{q}_0 \otimes \vec{q}_1 + \vec{q}_0 \otimes \vec{q}_2 \\ \vec{q}_0 \otimes \vec{q}_1 \otimes \vec{q}_2 & = & (\vec{q}_0 \otimes \vec{q}_1) \otimes \vec{q}_2 = \vec{q}_0 \otimes (\vec{q}_1 \otimes \vec{q}_2) \\ \end{array} \end{equation} $$

设$\vec{p}, \vec{q}$是两个四元数，并且满足关系$\vec{p} \otimes \vec{q} = 1$，那么称$\vec{q}$是$\vec{p}$的逆，记为$\vec{q} = \vec{p}^{-1}$，或者称$\vec{p}$是$\vec{q}$的逆，记为$\vec{p} = \vec{q}^{-1}$。根据共轭四元数和四元数模长的定义，有$\vec{p} \otimes \vec{p}^* = \left\| P \right\|$，所以$\vec{p}^{-1} = \cfrac{\vec{p}^*}{\left\| P \right\|}$。对于一个单位四元素而言，它的逆就是其共轭。根据逆的定义，我们有如下的等式：

$$ \vec{p}\vec{p}^{-1} = \vec{p}^{-1}\vec{p} = 1 $$ $$ (\vec{p} \otimes \vec{q})^{-1} = \vec{q}^{-1} \otimes \vec{p}^{-1} $$

四元数也存在点乘的定义，设$\vec{p}, \vec{q}$是两个四元数，其点乘得到的就是一个标量:

$$ \vec{p} \bullet \vec{q} = p_0 q_0 + p_1 q_1 + p_2 q_2 + p_3 q_3 $$

四元素对自己点乘得到的就是其模长 $\vec{p} \bullet \vec{p} = \| \vec{p} \| $。

4. 四元素的一些有用性质

对于三个四元素$\vec{p}, \vec{q}, \vec{r}$，有 $(\vec{q} \otimes \vec{p}) \bullet (\vec{q} \otimes \vec{r}) = (\vec{q} \bullet \vec{q}) (\vec{p} \bullet \vec{r})$。证明如下：

$$ (\vec{q} \otimes \vec{p}) \bullet (\vec{q} \otimes \vec{r}) = Q\vec{p} \bullet Q\vec{r} = (Q\vec{p})^T(Q\vec{r}) = \vec{p}^T Q^TQ\vec{r} = \vec{p}^T(\vec{q}\bullet\vec{q})I\vec{r} = (\vec{q} \bullet \vec{q}) (\vec{p} \bullet \vec{r}) $$

如果 $\vec{p} = \vec{r}$，根据上式，有推论：

$$ \| \vec{p} \otimes \vec{q} \| = \|\vec{p}\| \|\vec{q}\| $$

此外还有关系:

$$ (\vec{p} \otimes \vec{q}) \bullet \vec{r} = \vec{p} \bullet (\vec{r} \otimes \vec{q}^*) $$

笛卡尔坐标系下的一点 $\boldsymbol{r} = (r_x, r_y, r_z)^T$，也可以写成四元数的形式，$\vec{r} = 0 + r_x \vec{i} + r_y \vec{j} + r_z \vec{k}$，对于 $\vec{r}$我们可以写出其左右乘矩阵：

$$ R = \begin{bmatrix} 0 & -r_x & -r_y & -r_z \\ r_x & 0 & -r_z & r_y \\ r_y & r_z & 0 & -r_x \\ r_z & -r_y & r_x & 0 \end{bmatrix}, \bar{R} = \begin{bmatrix} 0 & -r_x & -r_y & -r_z \\ r_x & 0 & r_z & -r_y \\ r_y & -r_z & 0 & r_x \\ r_z & r_y & -r_x & 0 \end{bmatrix} $$

并且有，$R^T = -R, \bar{R}^T = -\bar{R}$。

4. 四元数之于定点转动

现在让我们回过来看描述定点转动的式($\ref{f1.1}$)，因为$\vec{r'}$是$b$系下位置矢量$\vec{r}$在$R$系下的投影，记$\vec{r^B}$和$\vec{r^b}$分别为位置矢量$\vec{r'}$和$\vec{r}$的零标量四元数。其中右上标表示对应的坐标系。

$$ \vec{r^B} = \begin{bmatrix} 0 \\ r_x^B \\ r_y^B \\ r_z^B \end{bmatrix}, \vec{r^b} = \begin{bmatrix} 0 \\ r_x^b \\ r_y^b \\ r_z^b \end{bmatrix} $$

那么$\vec{r^B}$和$\vec{r^b}$之间的变换关系可以用四元数表示：

$$ \begin{equation}\label{f3.1} \vec{r^B} = \vec{q} \otimes \vec{r^b} \otimes \vec{q}^* \end{equation} $$

上式中$\vec{q}$就是从$b$系到$R$系的旋转四元数，根据式($\ref{f2.7}$)上式可以写成矩阵的形式如下：

$$ \begin{equation}\label{f3.2} \begin{array}{rcl} \vec{q} \otimes \vec{r^b} \otimes \vec{q}^* & = & \underset{\bar{Q}}{\underbrace{\begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & q_0 & -q_3 & q_2 \\ q_2 & q_3 & q_0 & -q_1 \\ q_3 & -q_2 & q_1 & q_0 \end{bmatrix}}} \underset{Q}{\underbrace{\begin{bmatrix} q_0 & q_1 & q_2 & q_3 \\ -q_1 & q_0 & -q_3 & q_2 \\ -q_2 & q_3 & q_0 & -q_1 \\ -q_3 & -q_2 & q_1 & q_0 \end{bmatrix}}} \begin{bmatrix} 0 \\ r_x^b \\ r_y^b \\ r_z^b \end{bmatrix} \\ & = & \underset{\bar{Q}Q}{\underbrace{\begin{bmatrix} × & 0 & 0 & 0 \\ × & 1 - 2(q_2^2 + q_3^2) & 2(q_1q_2 - q_0q_3) & 2(q_0q_2 + q_1q_3) \\ × & 2(q_0q_3 + q_1q_2) & 1 - 2(q_1^2 + q_3^2) & 2(q_2q_3 - q_0q_1) \\ × & 2(q_1q_3 - q_0q_2) & 2(q_0q_1 + q_2q_3) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix}}} \begin{bmatrix} 0 \\ r_x^b \\ r_y^b \\ r_z^b \end{bmatrix} \end{array} \Longrightarrow \begin{bmatrix} 0 \\ \vec{r^B} \end{bmatrix} = \begin{bmatrix} × & \boldsymbol{0} \\ \boldsymbol{×} & C_b^B \end{bmatrix} \begin{bmatrix} 0 \\ \vec{r^b} \end{bmatrix} \end{equation}$$