旋转向量 欧拉角 四元数

旋转向量

表现形式

旋转向量/轴角：用一个旋转轴和一人个旋转角来刻画任意一个旋转，方向和旋转轴一致，长度等于旋转角

所以，对于变换矩阵T，可以使用一个旋转向量和一个平移变量来表示，总共6个变量

旋转向量转化为旋转矩阵

罗德里格斯公式：

​ 长度为$\theta$，轴角为$n$（单位长度的向量），用向量$\theta n$表示旋转。

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

​ 根据 迹 的性质，对该公式两边取迹，求得转角和旋转矩阵的关系。

$$\Rightarrow \theta=arccos\frac{tr(R)-1}{2}$$

转轴

转轴$n$上的旋转向量在旋转后不发生改变，

$$Rn=n \;\Leftarrow\left\{\begin{matrix} Rn=\lambda n\\ \lambda = 1 \end{matrix}\right.$$

所以，可以推导出$n$为旋转矩阵$R$特征值为1对应的特征向量。因此，已知旋转矩阵，即可求得旋转轴。

欧拉角

将三个轴的旋转方向作定义，更加直观的表示旋转的过程。

但是在使用欧拉角时，会出现“万向锁问题”，例如可能导致三个轴的旋转实际只表示了两次旋转等情况。

四元数

定义

$$\begin{align} & q=[s,v]^T,\;s=q_0 \in \mathbb{R},\;v=[q_1, q_2, q_3]^T \in \mathbb{R} . \\ &q = q_0+q_1i+q_2j+q_3k .\\ & s为实部，v为虚部 \end{align}$$

运算

1）自身三个虚部满足的乘法运算关系

2）两四元数加减法

3）两四元数的乘法

$$\begin{align} & q_a\: [s_a, v_a]^T,\;q_a=s_a+x_ai+y_aj+z_ak\\& q_b\:[s_b,v_b]^T,q_b=s_b+x_bi+y_bj+z_bk\\\rightarrow\\ &q_aq_b=[s_as_b-v_a^Tv_b, s_av_b+s_bv_a+v_a\times v_b]^T \end{align}$$

4）模长 $||q||$

5）共轭 $q^*$

$$q^*q=qq^*=[s_a^2+v^Tv, 0]^T.$$

6）逆 $q^{-1}$

$$\begin{align} &q^{-1}=q^*/||q||^2\\ &q^{-1}q=qq^{-1}=1 \end{align}$$

7）数乘

用四元数表示旋转

在二维平面中，旋转数可以写为 $q=cos\theta+i*sin\theta$

引申到三维旋转中，四元数向量表示为 $q\;[cos\theta, v*sin\theta]$

三维空间点的旋转

空间三维点：$p=[x,y,z]\in \mathbb{R^3}$

单位四元数：$q$ ,用于指定旋转

1）将三维空间点用虚四元数表示：

$$p=[0, x,y,z]^T=[0,v]^T$$

2）旋转

$$p'=extend(R)p \Rightarrow p'=qpq^{-1}$$

通过先乘q再作q逆乘法，相当与先用右手坐标系将空间点旋转二分之一的旋转角，再用左手坐标系逆向旋转二分之一旋转角，以此使得最终得到的四元数时纯虚四元数，从而直接表示三维空间坐标。（从几何意义上是将三维向量转化为四维后再回到三维）

四元数到其他旋转表示的转换

定义：（将四元数的乘法写成矩阵乘法）

$$q^+=\begin{bmatrix} s & -v^T\\ v & sI+v\text{^} \end{bmatrix},q^\oplus=\begin{bmatrix} s & -v^T\\ v & sI-v\text{^} \end{bmatrix}$$ $$\Rightarrow q_1q_2=q_1^+q_2=q_2^\oplus q_1$$ $$\Rightarrow p'=qpq^{-1}=q^+p^+q^{-1}=q^+{q^{-1}}^\oplus p$$

即$q^+{q^{-1}}^\oplus=extend(R)$，所以

四元数转换到旋转矩阵

$$q^+{q^{-1}}^\oplus=\begin{bmatrix} s & -v^T\\ v & sI+v\text{^} \end{bmatrix}\begin{bmatrix} s & v^T\\ -v & sI+v\text{^} \end{bmatrix}=\begin{bmatrix} 1 & 0\\ 0^T & vv^T+s^2I+2sv\text{^}+(v\text{^})^2 \end{bmatrix}$$ $$\Rightarrow R=vv^T+s^2I+2sv\text{^}+(v\text{^})^2 \tag{*}$$

四元数转换到旋转向量

对两边取迹，得：

$$tr(R)=4s^2-1$$

其中，关于 $v(v_1,v_2,v_3)$，$v\text{^}$是向量$v$的反对称矩阵形式

$$tr((v\text{^})^2)=2(v_1^2+v_2^2+v_3^2) \tag{1}$$

又由罗德里斯公式中：

$$\Rightarrow \theta=arccos\frac{tr(R)-1}{2}\tag{2}$$

最终得到四元数与旋转向量的转换关系

$$\Rightarrow \theta=2\arccos{s}$$

综上，四元数到旋转向量的转换公式：

$$\left\{\begin{matrix}\theta=2\arccos{q_0}\\ [n_x,n_y,n_z]^T=[q_1,q_2,q_3]^T/\sin{\frac{\theta}{2}}\end{matrix}\right. \tag{*}$$

---

参考资料：

• 视觉SLAM十四讲：从理论到实践（第2版）. 高翔.