旋转矩阵与四元数

用旋转矩阵描述旋转

如图所示，坐标系$O{x}_1{y}_1{z}_1$为$O{x}_0{y}_0{z}_0$旋转得到，记其三个单位基向量$x_1$, $y_1$, $z_1$在$O{x}_0{y}_0{z}_0$系下的坐标表示分别为$x_1^0$, $y_1^0$, $z_1^0$，则旋转矩阵为：

$$R_1^0=[x_1^0|y_1^0|z_1^0]$$

根据内积的定义，有

$$R_1^0=\left[\begin{array}{ccc}{x}_1\cdot x_0& y_1\cdot x_0& z_1\cdot x_0\\ x_1\cdot y_0& y_1\cdot y_0& z_1\cdot y_0\\ x_1\cdot z_0& y_1\cdot z_0& z_1\cdot z_0\end{array}\right]$$

绕x, y, z三个轴旋转$\theta$角的旋转矩阵为：

$$R_{x,\theta }=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$$$R_{y,\theta }=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$$$R_{z,\theta }=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

Info

下文的旋转矩阵、向量、四元数等都用右上标表明是在哪个坐标系下描述的。

旋转矩阵的性质

• $R^T=R^{-1}$
• $R\in SO(3)$
• $det(R)=1$
• $R$的各行、各列相互正交，且都是单位向量

用旋转矩阵表示坐标变换

• 点/向量坐标变换：

$$p^0=R_1^0{p}^1$$

• 线性变换的坐标变换：

$$T^1=(R_1^0{)}^{-1}{T}^0{R}_1^0$$

Tips

这个变换在线性代数里面叫做相似变换，其几何意义就是在不同坐标系下对同一矩阵（线性变换）的不同表示。

Important

特殊地，线性变换$T$可以是以一个旋转矩阵来描述的旋转运动，由此就得到了旋转运动在不同坐标系下的变换：

$$R^1=(R_1^0{)}^{-1}{R}^0{R}_1^0$$

旋转的叠加

• 绕随动坐标系的叠加：根据叠加的顺序，依次右乘旋转矩阵

• 绕固定坐标系的叠加：根据叠加的顺序，依次左乘旋转矩阵

证明

记绕固定坐标系的旋转为$R$，则$R_2^0=R_1^0((R_1^0{)}^{-1}R{R}_1^0)=R{R}_1^0$

旋转的转轴-角度表示

可以证明，三维刚体的任意旋转及其叠加，都可以用一个转轴（方向向量）和绕这个轴的旋转角度表示。记转轴$\overrightarrow{k}=[k_x{k}_y{k}_z{]}^T$，角度为$\theta$，则

$$R_{\overrightarrow{k},\theta }=\left[\begin{array}{ccc}{k}_x^2{v}_{\theta }+c_{\theta }& k_x{k}_y{v}_{\theta }-k_z{s}_{\theta }& k_x{k}_z{v}_{\theta }+k_y{s}_{\theta }\\ k_x{k}_y{v}_{\theta }+k_z{s}_{\theta }& k_y^2{v}_{\theta }+c_{\theta }& k_y{k}_z{v}_{\theta }-k_x{s}_{\theta }\\ k_x{k}_z{v}_{\theta }-k_y{s}_{\theta }& k_y{k}_z{v}_{\theta }+k_x{s}_{\theta }& k_z^2{v}_{\theta }+c_{\theta }\end{array}\right]$$

其中$c_{\theta }=\cos \theta$，$s_{\theta }=\sin \theta$，$v_{\theta }=\mathrm{versine}\theta =1-\cos \theta$。

Info

全部的三角函数

反过来，对旋转矩阵

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

则有

$$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right)$$$$k=\frac{1}{2\sin \theta }\left[\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right]$$

转轴和角度不是唯一的，绕一个方向旋转$\theta$等同于绕相反方向旋转$-\theta$：

$$R_{\overrightarrow{k},\theta }=R_{-\overrightarrow{k},-\theta }$$

四元数

• 定义：$\mathcal{i},\mathcal{j},\mathcal{k}$是正交的三个虚数单位，${\mathcal{i}}^2={\mathcal{j}}^2={\mathcal{k}}^2=\mathcal{i}\mathcal{j}\mathcal{k}=-1$，$a,b,c,d\in \mathbb{R}$，则

  $$\mathbf{q}=a+b\mathcal{i}+c\mathcal{j}+d\mathcal{k}$$

  也可以写成向量形式$\mathbf{q}=[abcd]$或$\mathbf{q}=[a\overrightarrow{v}]$

• 加减法：实部和三个虚部分别运算

• 乘法：三个虚数单位的乘法关系为：

  $$\begin{array}{rl}\mathcal{i}\mathcal{j}& =\mathcal{k}\\ \mathcal{j}\mathcal{k}& =\mathcal{i}\\ \mathcal{k}\mathcal{i}& =\mathcal{j}\\ \mathcal{x}\mathcal{y}& =-\mathcal{y}\mathcal{x},\mathcal{x},\mathcal{y}\in \{\mathcal{i},\mathcal{j},\mathcal{k}\}\end{array}$$

  根据分配律和结合律可以写出乘法公式，但是那样太复杂了，比较简单的形式被称为Graßmann积:

  $$[s,\overrightarrow{u}][t,\overrightarrow{v}]=[st-\overrightarrow{u}\cdot \overrightarrow{v},s\overrightarrow{v}+t\overrightarrow{u}+\overrightarrow{u}\times \overrightarrow{v}]$$

• 纯四元数：实部为0的四元数称为纯四元数。性质：

  • $[0,\overrightarrow{u}][0,\overrightarrow{v}]=[-\overrightarrow{u}\cdot \overrightarrow{v},\overrightarrow{u}\times \overrightarrow{v}]$

• 模：$||[abcd]||=\sqrt{a^2+b^2+c^2+d^2}$

• 共轭：${\mathbf{q}}^{\ast }=[s,-\overrightarrow{v}]$，$\mathbf{q}{\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\mathbf{q}=||\mathbf{q}|{|}^2$，${\mathbf{q}}_1^{\ast }{\mathbf{q}}_2^{\ast }=({\mathbf{q}}_2{\mathbf{q}}_1{)}^{\ast }$

• 逆：${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}||$，特殊地对于描述旋转的单位四元数，逆和共轭相等

四元数描述旋转

3D旋转公式：向量$\overrightarrow{v}$绕旋转轴$\overrightarrow{u}$旋转$\theta$角之后的向量${\overrightarrow{v}}^{\prime }$可以使用四元数乘法计算。令$\mathbf{v}=[0,\overrightarrow{v}]$，$\mathbf{q}=[\cos (\theta /2),\sin (\theta /2)\overrightarrow{u}]$，则

$${\mathbf{v}}^{\prime }=\mathbf{q}\mathbf{v}{\mathbf{q}}^{\ast }=\mathbf{q}\mathbf{v}{\mathbf{q}}^{-1}$$

$\mathbf{q}\mathbf{v}{\mathbf{q}}^{\ast }$这个变换对$\mathbf{v}$平行于旋转轴的分量实施的变换是$\mathbf{q}{\mathbf{q}}^{\ast }$（无旋转），而正交于旋转轴的分量实施的变换是${\mathbf{q}}^2$（旋转$\theta /2+\theta /2$）。

四元数和旋转矩阵互相转化

四元数到旋转矩阵

对于一个单位四元数$\mathbf{q}=[a,\overrightarrow{v}]$ ，可以提取它对应的旋转角度和旋转轴：

$$\theta =2\arccos (a)$$$$\overrightarrow{k}=\frac{\overrightarrow{v}}{\sin (\theta /2)}$$

再根据前面旋转的转轴-角度表示一节的公式可以得到：

$$R=\left[\begin{array}{ccc}1-2c^2-2d^2& 2bc-2ad& 2ac+2bd\\ 2bc+2ad& 1-2b^2-2d^2& 2cd-2ab\\ 2bd-2ac& 2ab+2cd& 1-2b^2-2c^2\end{array}\right]$$

旋转矩阵到四元数

算法一

$$\begin{array}{rl}a& =\frac{1}{2}\sqrt{\mathrm{tr}(R)+1}\\ b& =\frac{r_{32}-r_{23}}{4a}\\ c& =\frac{r_{13}-r_{31}}{4a}\\ d& =\frac{r_{21}-r_{12}}{4a}\end{array}$$

但是当$a\simeq 0$时容易出现数值不稳定（除零）

算法二

在算法一基础上分类以避免数值不稳定，核心就是让作为分母的那一项比较大

• 当$\mathrm{tr}(R)>0$

  同算法一

• 当$r_{11}>r_{22}+r_{33}$

  $$\begin{array}{rl}a& =\frac{r_{32}-r_{23}}{4b}\\ b& =\frac{1}{2}\sqrt{1+r_{11}-r_{22}-r_{33}}\\ c& =\frac{r_{12}+r_{21}}{4b}\\ d& =\frac{r_{13}+r_{31}}{4b}\end{array}$$

• 当$r_{22}>r_{11}+r_{33}$

  $$\begin{array}{rl}a& =\frac{r_{13}-r_{31}}{4c}\\ b& =\frac{r_{12}+r_{21}}{4c}\\ c& =\frac{1}{2}\sqrt{1+r_{22}-r_{11}-r_{33}}\\ d& =\frac{r_{23}+r_{32}}{4c}\end{array}$$

• 当$r_{33}>r_{11}+r_{22}$

  $$\begin{array}{rl}a& =\frac{r_{21}-r_{12}}{4d}\\ b& =\frac{r_{13}+r_{31}}{4d}\\ c& =\frac{r_{23}+r_{32}}{4d}\\ d& =\frac{1}{2}\sqrt{1+r_{33}-r_{11}-r_{22}}\end{array}$$

算法三

来自^[1]，首先将旋转矩阵转化成$K_3$矩阵：

$$K_3=\frac{1}{3}\left[\begin{array}{cccc}{r}_{11}-r_{22}-r_{33}& r_{21}+r_{12}& r_{31}+r_{13}& r_{23}-r_{32}\\ r_{21}+r_{12}& r_{22}-r_{11}-r_{33}& r_{32}+r_{23}& r_{31}-r_{13}\\ r_{31}+r_{13}& r_{32}+r_{23}& r_{33}-r_{11}-r_{22}& r_{12}-r_{21}\\ r_{23}-r_{32}& r_{31}-r_{13}& r_{12}-r_{21}& r_{11}+r_{22}+r_{33}\end{array}\right]$$

这个矩阵只有一个特征向量$\overrightarrow{q}=[b,c,d,a{]}^T$，特征值为1.

旋转的叠加（四元数版）

和旋转矩阵其实一样，当绕固定坐标系旋转时（或者理解为旋转刚体，坐标系不动），复合旋转的四元数是每个旋转的依次左乘：

$${\mathbf{v}}^{″}={\mathbf{q}}_2{\mathbf{v}}^{\prime }{\mathbf{q}}_2^{\ast }=({\mathbf{q}}_2{\mathbf{q}}_1)\mathbf{v}({\mathbf{q}}_2{\mathbf{q}}_1{)}^{\ast }$$

而当绕随动坐标系旋转时（或者理解为旋转坐标系，刚体不动），复合旋转的四元数是每个旋转依次右乘。

用四元数实现坐标变换

点/向量

若坐标系B相对于坐标系A的旋转为${\mathbf{q}}_B^A$，平移为${\mathbf{T}}_B^A$（三维向量都用前面所述的方式转化为纯四元数表示），则点$P$在两个坐标系下的坐标关系为：

$${\mathbf{p}}^A={\mathbf{q}}_B^A{\mathbf{p}}^B({\mathbf{q}}_B^A{)}^{\ast }+{\mathbf{T}}_B^A$$

旋转的部分可以这样理解：点不动旋转坐标系，坐标数值上其实相当于坐标系不动反向旋转点，也就是说

$${\mathbf{p}}^B=({\mathbf{q}}_B^A{)}^{\ast }{\mathbf{p}}^A{\mathbf{q}}_B^A\text{(rotation only)}$$

这个结果其实和旋转矩阵很类似。

旋转

对于用四元数表示的一个旋转$r$，在不同坐标系下表示它的四元数关系为：

$${\mathbf{q}}_r^B=(\pm )({\mathbf{q}}_B^A{)}^{\ast }{\mathbf{q}}_r^A{\mathbf{q}}_B^A$$

这和旋转矩阵在形式上一致。

四元数的指数形式与幂运算

类似欧拉公式，对于单位向量$\overrightarrow{u}$，有

$${\mathrm{e}}^{\overrightarrow{u}\theta }=\cos \theta +\overrightarrow{u}\sin \theta$$

由此可以得到四元数的幂运算：

$${\mathbf{q}}^t={\mathrm{e}}^{\overrightarrow{u}(t\theta )}$$

即相当于将其旋转角度乘$t$，旋转轴不变。

四元数的插值

四元数相比其他旋转表示方法的优点之一就是其插值较为容易。

旋转变化量

要计算两个旋转之间的变化量很简单：

$$\mathrm{\Delta }\mathbf{q}={\mathbf{q}}_1{\mathbf{q}}_0^{\ast }$$

如果将四元数看作四维空间的向量，那么${\mathbf{q}}_0\cdot {\mathbf{q}}_1$就是这个四维空间中两个向量夹角的余弦值，这个夹角正是三维空间中该旋转变化量角度的一半。

Lerp

即线性插值（Linear Interpolation），将两个四元数看作四维空间的两个向量，Lerp就是通过线性方式插值两个向量：

$${\mathbf{q}}_t=\mathrm{Lerp}({\mathbf{q}}_0,{\mathbf{q}}_1,t)\triangleq (1-t){\mathbf{q}}_0+t{\mathbf{q}}_1$$

显然，这样插值出的向量终点在弦上，因此并不是单位四元数。

Nlerp

对Lerp的结果归一化为单位四元数就得到了正规化线性插值（Normalized Linear Interpolation）：

$${\mathbf{q}}_t=\mathrm{Nlerp}({\mathbf{q}}_0,{\mathbf{q}}_1,t)\triangleq \frac{(1-t){\mathbf{q}}_0+t{\mathbf{q}}_1}{||(1-t){\mathbf{q}}_0+t{\mathbf{q}}_1||}$$

Nlerp在需要插值的角度较大时，角速度会显著变化，在$t=0.5$附近角速度最大，而在两端角速度较小。

Slerp

球面线性插值（Spherical Linear Interpolation）不是对向量插值而是对角度插值，因此它对角度是均匀的。根据四元数的幂定义可以很容易写出一个版本：

$${\mathbf{q}}_t=\mathrm{Slerp}({\mathbf{q}}_0,{\mathbf{q}}_1,t)=({\mathbf{q}}_1{\mathbf{q}}_0^{\ast }{)}^t{\mathbf{q}}_0$$

可见当$t=0$时为${\mathbf{q}}_0$，当$t=1$时为${\mathbf{q}}_1$。但是这个公式中含有多次四元数乘法和幂运算，实际计算的效率很低。更高效的方法是：

$$\theta =\arccos ({\mathbf{q}}_0\cdot {\mathbf{q}}_1)$$$${\mathbf{q}}_t=\mathrm{Slerp}({\mathbf{q}}_0,{\mathbf{q}}_1,t)=\frac{\sin ((1-t)\theta )}{\sin \theta }{\mathbf{q}}_0+\frac{\sin (t\theta )}{\sin \theta }{\mathbf{q}}_1$$

实际应用中，当夹角很小时$\sin \theta$接近于0，就必须改用Nlerp来插值。

双倍覆盖

由于$\mathbf{q}$和$-\mathbf{q}$表示了相同的旋转，因此四元数空间其实是双倍覆盖了$SO(3)$空间。而插值时对$\mathbf{q}$还是$-\mathbf{q}$进行插值，旋转的变化量是不同的。因此需要判断${\mathbf{q}}_0\cdot {\mathbf{q}}_1$是否为负数，如果是负数说明两个四元数在四维空间中的夹角是钝角，需要将其中一个四元数取反来得到最短的插值路径。

Squad

为了解决Slerp插值只实现$C^0$连续，在轨迹点存在角速度突变的问题，以牺牲固定角速度为条件实现轨迹的$C^1$连续，即角速度连续。Squad指Spherical and quadrangle，球面四边形插值，核心思想是利用Slerp来构建三次贝塞尔曲线。构造贝塞尔曲线有一个著名的递归算法de Casteljau算法，但是它的计算太复杂，对于三次贝塞尔曲线，它是三层一次插值的嵌套：

$$\mathrm{Bezier}({\overrightarrow{v}}_0,{\overrightarrow{v}}_1,{\overrightarrow{v}}_2,{\overrightarrow{v}}_3;t)=L(L(L({\overrightarrow{v}}_0),{\overrightarrow{v}}_1;t),L({\overrightarrow{v}}_1,{\overrightarrow{v}}_2;t);t),L(L({\overrightarrow{v}}_1,{\overrightarrow{v}}_2;t),L({\overrightarrow{v}}_2,{\overrightarrow{v}}_3;t);t);t)$$

其中$L$是Lerp线性插值；对于四元数来说要换成Slerp插值。

而Quad使用一个二次插值和一个一次插值的嵌套来近似它：

$$\mathrm{Bezier}({\overrightarrow{v}}_0,{\overrightarrow{v}}_1,{\overrightarrow{v}}_2,{\overrightarrow{v}}_3;t)=L(L({\overrightarrow{v}}_0,{\overrightarrow{v}}_3;t),L({\overrightarrow{v}}_1,{\overrightarrow{v}}_2;t);2t(1-t))$$

将Lerp换成Slerp就得到Squad插值：

$$Squad({\mathbf{q}}_0,{\mathbf{q}}_1,{\mathbf{q}}_2,{\mathbf{q}}_3;t)=S(S({\mathbf{q}}_0,{\mathbf{q}}_3;t),S({\mathbf{q}}_1,{\mathbf{q}}_2;t);2t(1-t))$$

对于四元数序列${\mathbf{q}}_0,{\mathbf{q}}_1,\dots ,{\mathbf{q}}_n$，对每一对四元数${\mathbf{q}}_i,{\mathbf{q}}_{i+1}$都进行插值$Squad({\mathbf{q}}_i,{\mathbf{s}}_i,{\mathbf{s}}_{i+1},{\mathbf{q}}_{i+1};t)$，其中控制点${\mathbf{s}}_i,{\mathbf{s}}_{i+1}$的选择使得切换点处可导，这里给出结果：

$${\mathbf{s}}_i={\mathbf{q}}_i\exp (-\frac{\log ({\mathbf{q}}_i^{\ast }{\mathbf{q}}_{i-1})+\log ({\mathbf{q}}_i^{\ast }{\mathbf{q}}_{i+1})}{4})$$

Reference

---

1. https://doi.org/10.2514/2.4654 ↩︎