三维空间刚体旋转描述_怎么评价空间中刚体的旋转

三维空间中通常可以用旋转矩阵、旋转向量、欧拉角和四元数来描述旋转

---

旋转矩阵

先回顾下向量的内积和外积

$$\begin{cases}a\cdot b=a^Tb=\sum^3_{i=1}{a_ib_i}=|a||b|cos(a,b) \\ a\times b=\left[\begin{matrix}i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \\ \end{matrix}\right]=\left[\begin{matrix}a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1\end{matrix}\right]=\left[\begin{matrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{matrix}\right]b=\hat{a}b \end{cases}(1)$$
其中 $\hat{a}=\left[\begin{matrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{matrix}\right]$是取向量 $a$的反对角矩阵

外积只对三维向量存在定义，并且可以表示向量的旋转：
假设两个不平行向量$a$， $b$，可以用一个与$a,b$所在平面垂直的向量描述 $a$到$b$的旋转（图1）
　　　　　　　　　　　　　　　　
　　　　　　　　图1. $w$与$a\times b$方向一致，相当于旋转轴，模值等于角度

对空间中一个向量$a$，假设有两组单位正交基$O_1=(e_1,e_2,e_3)$和$O_2=(e'_1,e'_2,e'_3)$，向量在两个坐标系下坐标为$[a_1,a_2,a_3]^T$和$[a'_1,a'_2,a'_3]^T$，那么有

$$[e_1,e_2,e_3][a_1,a_2,a_3]^T=[e'_1,e'_2,e'_3][a'_1,a'_2,a'_3]^T(1)$$
(1)式两边同左乘 $[e^T_1,e^T_2,e^T_3]^T$，有
$$\left[ \begin{array} \\ a_1 \\ a_2 \\ a_3 \end{array}\right]=\left[ \begin{array} \\ e^T_1e'_1&e^T_1e'_2&e^T_1e'_3 \\ e^T_2e'_1&e^T_2e'_2&e^T_2e'_3 \\ e^T_3e'_1&e^T_3e'_2&e^T_3e'_3 \end{array}\right]\left[ \begin{array} \\ a'_1 \\ a'_2 \\ a'_3 \end{array}\right]=Ra'(2)$$

(2)式的$R$称为旋转矩阵，用来描述相机的旋转

旋转矩阵是行列式为1的正交矩阵，反之，行列式为1的正交矩阵也是一个旋转矩阵

$$SO(n)=\{R\in R^{n\times n}|RR^T=I,det(R)=1\}$$
其中$SO(n)$是特殊正交群，特别的$SO(3)$就是三维空间的旋转

旋转矩阵的劣势

• 旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的
• 旋转矩阵有自身约束：必须是正交矩阵，且行列式为1，优化算法比较难应用

---

旋转向量

旋转向量用一个三维向量来描述旋转：其方向与旋转轴一致，长度等于旋转角

回顾向量外积，图1的$w$就是$a$到$b$的旋转向量

旋转向量与旋转矩阵转换
假设旋转轴为$n$，角度为$\theta$，使用罗德里格斯公式可以转换到旋转矩阵

$$R=cos\theta I+(1-cos\theta)nn^T+sin\theta \hat{n}(3)$$
同样可以得到由旋转矩阵到旋转向量的公式
$$\begin{cases} \theta=arccos(\frac{tr(R)-1}{2}) \\ n=R特征值为1的特征向量\end{cases}(4)$$

此外，旋转矩阵实际就是$SO(3)$的李群，旋转向量就是对应的李代数$so(3)$，两者通过指数映射相联系（与罗德里格斯等价）

$$R=exp(\hat{\phi})(5)$$
其中 $\hat{\phi}$就是对旋转向量 $\phi$取反对称矩阵

---

欧拉角

欧拉角是一种最直观的旋转描述方式，也是一个3维向量，分别代表绕某个轴的旋转角度

• 相同的角度，旋转次序的不同，旋转结果不一样。一般常见的是rpy角（旋转顺序是ZYX）
• 使用欧拉角一个最大的缺点是万向锁问题：俯仰角为$\pm90$度时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度
• 因此欧拉角不适于插值和迭代，往往只用于人机交互中

---

四元数

旋转矩阵用9个量描述3自由度的旋转，具有冗余性
欧拉角和旋转向量用3个量描述3自由度的旋转，是紧凑的，但具有奇异性
四元数用4个量描述3自由度的旋转，紧凑又没有奇异性

一个四元数$q$拥有1个实部和3个虚部

$$q=q_0+q_1i+q_2j+q_3k(6)$$
满足

$$\begin{cases} i^2+j^2+k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j\end{cases}(7)$$

旋转向量与四元数的转换
对于一个旋转向量：绕单位向量$n=[n+x,n_y,n_z]^T$做了$\theta$度的旋转，那么其四元数为

$$q=[cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_zsin\frac{\theta}{2}]^T(8)$$
同样，也可以由四元数求得旋转向量
$$\begin{cases} \theta=2arccosq_0 \\ [n_x,n_y,n_z]^T=\frac{[q_1,q_2,q_3]^T}{sin\frac{\theta}{2}} \end{cases}(9)$$

旋转矩阵与四元数的转换
设四元数$q=q_0+q_1i+q_2j+q_3k(6)$，对应的旋转矩阵为

$$R=\left[ \begin{array} \\ 1-2q^2_2-2q^2_3&2q_1q_2+2q_0q_3&2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3&1-2q^2_1-2q^2_3&2q_2q_3+2q_0q_1 \\ 2q_1q_3+2q_0q_2&2q_2q_3-2q_0q_1&1-2q^2_1-2q^2_2\end{array}\right](10)$$
反之也可以由 $R$推得四元数$q$
$$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{R_{23}-R_{32}}{4q_0},q_2=\frac{R_{31}-R_{13}}{4q_0},q_3=\frac{R_{12}-R_{21}}{4q_0}(11)$$

用四元数表示旋转
对于一个空间三维点$p=[x,y,z]$，指定一个绕$n$做$\theta$角的旋转，旋转后的点为$p'$
1）首先将三维点用一个虚四元数来描述

$$p=[0,x,y,z]=[0,v]$$
2）用四元数 $q$来表达旋转
$$q=[cos\frac{\theta}{2},nsin\frac{\theta}{2}]$$
3）旋转后的点为
$$p'=qpq^{-1}$$