三维空间的旋转

三维空间的旋转

三维空间中两个坐标系之间的旋转关系可以通过旋转矩阵，旋转向量（角轴），欧拉角，四元数表示。

1 旋转矩阵

设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转变成了$(e_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }})$,那么对于同一个向量$\mathbf{a}$(注意该向量并没有随着坐标系的旋转而发生运动)，它在两个坐标系下的坐标为 $(a_1,a_2,a_3{)}^T$ 和$(a_1^{‘},a_2^{‘},a_3^{‘}{)}^T$。根据坐标系的定义，：

$$\left[\begin{array}{c}{e}_1,e_2,e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{c}{e}_1^{{}^{\prime }},e_2^{{}^{\prime }},e_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]（1）$$

为描述两个坐标之间的关系，(1)左右两边同时乘以$(e_1^T,e_2^T,e_3^T{)}^T$，则有：

$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1{}^{\prime }& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{c}{a}_1^{{}^{\prime }}\\ a_2^{{}^{\prime }}\\ a_3^{{}^{\prime }}\end{array}\right]=R{\mathbf{a}}^{{}^{\prime }}(2)$$

令：$R=\left[\begin{array}{ccc}{e}_1^T{e}_1{}^{\prime }& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]$（3）

矩阵$R$描述了坐标系之间的旋转，称为旋转矩阵。
旋转矩阵是行列式为1的正交矩阵，旋转矩阵的集合定义如下：

S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = I } ( 4 ) SO(n) = \{R \in \mathbb{R}^{n\times n} | RR^T = I, det(R) = I \} (4) SO(n)={R∈Rn×n∣RRT=I,det(R)=I}(4)
由于旋转矩阵是正交阵，它的逆(即转置)描述了一个相反的旋转，则有${\mathbf{a}}^{‘}=R^{-1}\mathbf{a}=R^T\mathbf{a}$。显然，$R^T$刻画了一个相反的旋转。

2 旋转向量（角轴，李代数）$w$
坐标系的任意旋转都可以用一个旋转轴和旋转角来刻画。使用一个向量，其方向与旋转轴一致，而长度等于旋转角，这种向量称为旋转向量（角-轴）。
2.1旋转向量和旋转矩阵
假如一个旋转轴为$n$,角度为$\theta$的旋转，它对应的旋转向量为$\theta$$n$，旋转向量$w=n\theta$ 到旋转矩阵的转换过程由罗德里格斯公式表明，转换公式如下：
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta {\left[\begin{array}{c}n\end{array}\right]}_X(5)$$

${\left[\begin{array}{cc}& \end{array}\right]}_X$：表示反对称矩阵

$${\left[\begin{array}{c}W\end{array}\right]}_X=\left[\begin{array}{cccc}0& -W_3& -W_2& \\ W_3& 0& -W_1& \\ -W_2& W_1& 0& \end{array}\right](6)$$

对于转角$\theta$，有：

$$\begin{gathered} tr(R)=cos\theta tr(I)+(1-cos\theta )tr(\mathbf{n}{\mathbf{n}}^T)+sin\theta tr([\mathbf{n}{]}_{\times }) \\ =3cos\theta +(1-cos\theta )=1+2cos\theta （7） \end{gathered}$$

因此 $\theta =arccos\frac{tr(R)-1}{2}$ （8）

计算转轴 $\mathbf{n}$，由于旋转轴上的向量在旋转后不发生改变，有 $R\mathbf{n}=\mathbf{n}$。因此，转轴 $\mathbf{n}$ 是矩阵 $R$ 特征值为1对应的特征向量。求解次方程，再归一化，就得到了转轴。
2.2 旋转向量表示旋转
对于三维空间点$p=\left[\begin{array}{ccc}x& y& z\end{array}\right]$
罗德里格斯旋转公式：$${\mathbf{p}}_{rot}=\mathbf{p}cos\theta +(\mathbf{n}\times \mathbf{p})sin\theta +\mathbf{n}(\mathbf{n}\cdot \mathbf{p})(1-cos\theta )(9)$$
实际中，使用轴角表示法对三维点进行旋转会分两种情况：
$||\mathbf{w}||$ 相比0较大时。这个时候可以通过罗德里格斯旋转公式计算
$||\mathbf{w}||$ 相比0较小时。根据罗德里格斯公式,
$R=cos||\mathbf{w}||I+\frac{[\mathbf{w}{]}_{\times }}{||\mathbf{w}||}sin(||\mathbf{w}||)+\frac{[\mathbf{w}{]}_{\times }^2}{||\mathbf{w}|{|}^2}(1-cos(||\mathbf{w}||))$
当$\mathbf{w}$较小时，$cos(||\mathbf{w}||)\approx 1,sin(||\mathbf{w}||)\approx ||\mathbf{w}||$。因此，$R\approx I+[\mathbf{w}{]}_{\times }$。于是，$R\mathbf{p}=\mathbf{p}+[\mathbf{w}{]}_{\times }\mathbf{p}$
注： 对于李代数 $\mathbf{w}=\theta \mathbf{n}$，其中$\mathbf{n}$为单位转轴，因此 $||\mathbf{n}|{|}^2=1$，于是$||\mathbf{w}|{|}^2={\theta }^2||\mathbf{n}|{|}^2={\theta }^2$。因此，有 $\theta =||\mathbf{w}||$

3 欧拉角
把旋转分解成3次绕不同轴旋转，每次旋转的角度构成欧拉角。旋转时，旋转轴有先后顺序，可以先绕X轴，再绕Y轴，最后绕Z轴，即XYZ轴的旋转，同理可以定义ZYX,ZYX等。欧拉角在摄影测量中应用比较广泛，常见的转角系统有rpy角度系统(ZYX），OPK角度系统(XYZ)。
欧拉角的一个重大缺点是会碰到万向锁问题。
3.1 欧拉角和旋转矩阵
opk角度系统

4 四元数
单位四元数可以表示三维空间的旋转。
四元数：$\mathbf{q}=(c,\mathbf{v})=(q_0,q_1,q_2,q_3)=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$
三个虚部满足以下关系式：
$$\begin{gathered} i^2=j^2=k^2=-1 \\ ij=k,ji=-k, \\ jk=i,kj=-i, \\ ki=j,ik=-j \end{gathered}$$
4.1 四元数和旋转向量
假设旋转绕单位向量 $\mathbf{n}=(n_x,x_y,n_z{)}^T$ 进行了角度为 $\theta$ 的旋转，则该旋转的四元数定义为:

$\mathbf{q}=(cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{)}^T$ (9)

令 $\theta =\theta +2\pi$，则 $\mathbf{q}=(-cos\frac{\theta }{2},-n_x\sin \frac{\theta }{2},-n_{y}sin\frac{\theta }{2},-n_{z}sin\frac{\theta }{2}{)}^T=-\mathbf{q}$。即 $\mathbf{q}$ 和 $-\mathbf{q}$ 表示同一个旋转。

$$\begin{gathered} theta=2arccos{q}_0, \\ (n_x,n_y,n_z{)}^T=\frac{1}{sin\frac{\theta }{2}}(q_1,q_2,q_3{)}^T=\frac{1}{||\mathbf{n}||}(q_1,q_2,q_3{)}^T(10) \end{gathered}$$

4.2 四元数和旋转矩阵

$$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](11)$$

则旋转矩阵到四元数的转换如下:

$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}-R21}{4q_0}$ (12)

$$R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1-2q_1^2-2_3^2& 2q_2{q}_3-2q_0{q}_1\\ 2q_1{q}_3-2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right](13)$$

4.3 用四元数表示旋转
假设一个三维空间点$p=\left[\begin{array}{ccc}x& y& z\end{array}\right]\cap \in {\mathbb{R}}^3$,三维点$p$经过旋转之后变成$p^{{}^{\prime }}$
首先，把三维空间点用一个虚四元数来描述：$p=\left[\begin{array}{cccc}0& x& y& z\end{array}\right]=\left[\begin{array}{cc}0& v\end{array}\right]$
那么旋转后的点$p^{{}^{\prime }}$表示为：$p^{{}^{\prime }}=qp{p}^{-1}$ (14)

参考：
[1]: https://aibluefisher.github.io/2019/04/16/rotation/
[2]:视觉SLAM十四讲 从理论到实践