请添加图片描述

旋转矩阵表示形式

由欧拉旋转定理可得：刚体在三位空间里的一般运动可以分解为刚体上某一点的平移，以及绕过该点旋转轴的转动；
只考虑旋转：
三维空间中的点p在坐标系 $(e_{1}, e_{2}, e_{3}) (e_1, e_2, e_3)$ 中的坐标为 $(a_1, a_2, a_3)$ ,当坐标系 $(e_1, e_2, e_3)$ 旋转到坐标系 $(e_1^{\prime}, e_2^{\prime}, e_3^{\prime})$ ,点p的坐标为 $(a_1^{\prime}, a_2^{\prime}, a_3^{\prime})$ ，如式1所示：
$\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]\tag1$

等式两边同乘 $\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]^T$ ，由于 $\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}$ 为正交向量有：
$\left[\begin{array}{c} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{T} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{T} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq \boldsymbol{R} \boldsymbol{a}^{\prime}\tag2$

则称 $R$ 为旋转矩阵，其包含以下性质：

R是正交矩阵: $R^{-1}=R, \ R^TR = I$
R的行列是为1： $|R|=1$
注：上式两个条件是充分必要条件，只要满足上式两个条件的矩阵都可以作为一个旋转矩阵，即：
$S O(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}\tag3$

将所有满足式3的矩阵称为 Special Orthognal Group （特殊正交群），下面只考虑三维空间中的特殊正交群，坐标系1到坐标系2的之间的旋转关系可由式4来表示：
$a_1 = R_{12}a_2\\ a_2 = R_{21}a1\tag4$
其中 $R_{21}=R_{12}^{-1}=R_{12}^{T}$ ，若一个旋转矩阵取转置或取逆，则表示一个相反方向的旋转；
旋转+平移：
若同时考虑平移，则两个坐标系下p点的坐标关系如式5所示：
$a^{\prime} = Ra+t\tag5$
具体表示形式为：
$a_{2}=R_{21} \cdot a_{1}+t_{21} \\ a_{1}=R_{12} \cdot a_{2}+t_{12}$
注： $t_{12}\neq -t_{21}$

其次坐标表示：
由于刚体在三维空间中可能发生多次旋转与平移，如式6所示：
$\boldsymbol{b}=\boldsymbol{R}_{1} a+t_{1}, \quad c=\boldsymbol{R}_{2} b+t_{2} . \quad \Longrightarrow \quad c=\boldsymbol{R}_{2}\left(\boldsymbol{R}_{1} a+t_{1}\right)+t_{2} .\tag6$
为了方便表达这种运动，引入了其次坐标表示形式如式7所示：
$\left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \triangleq \boldsymbol{T}\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right]$
其中 $T = \left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{T} & 1 \end{array}\right]$ 为变换矩阵(同时包含旋转与平移),变换矩阵的逆 $\boldsymbol{T}^{-1}$ 为：
$\boldsymbol{T}^{-1}=\left[\begin{array}{cc} \boldsymbol{R}^{T} & -\boldsymbol{R}^{T} t \\ \mathbf{0}^{T} & 1 \end{array}\right]$

将变换矩阵的集合称为特殊欧式群SE(3) (Special Euclidean Group)：
$\mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\}\tag7$

其中 $\left[\begin{array}{l} \boldsymbol{a}^{\prime} \\ 1\end{array}\right]$ 为齐次坐标，齐次坐标乘以任意非零常熟仍然表示三维空间中的同一个坐标：
$\tilde{a}=\left[\begin{array}{l} a \\ 1 \end{array}\right]=k\left[\begin{array}{l} a \\ 1 \end{array}\right]\tag8$

则多次的旋转与平移可以如式9所示：
$\tilde{b}=\boldsymbol{T}_{1} \tilde{\boldsymbol{a}}, \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \tilde{\boldsymbol{b}} \quad \Rightarrow \tilde{\boldsymbol{c}}=\boldsymbol{T}_{2} \boldsymbol{T}_{\mathbf{1}} \tilde{\boldsymbol{a}}\tag9$

旋转向量的表达形式

用9个元素的旋转矩阵仅仅表示3个自由度的信息，表达形式过于复杂，不节省内存空间。从直观上看，旋转可以表示为绕某个轴 $n n$ 旋转一定的角度 $\theta$ ，为此引入了**旋转向量（Rotation Vector）**或角轴/轴角（Angle Axis）
旋转矩阵与旋转向量是等价的，可以互相转换：
旋转向量到旋转矩阵的变换：罗德里格斯公式(Rodrigus’s Formula):
$\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}+\sin \theta \boldsymbol{n}^{\wedge}\tag{10}$
其中 $\cos \theta \boldsymbol{I}$ 为3x3矩阵， $(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{T}$ 也为3x3的矩阵， $\boldsymbol{n}^{\wedge}$ 表示旋转向量 $n$ 的叉积。
旋转矩阵到旋转向量：
$\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)\\ Rn = n\tag{11}$
$n$ 为 $R$ 矩阵对应特征值为1的特征向量；

欧拉角(Euler Angles)

旋转向量表示的旋转方式不够直观，无法从旋转向量中直接看出旋转轴的位置，欧拉角将旋转分解为三个方向上的旋转（如Z-Y-X）方向，轴可以为定轴或者动轴，轴的顺序也可以改变，如最常用绕动轴的Z-Y-X旋转方式:

绕物体的Z轴转动，得到偏航角yaw；
绕旋转之后的Y轴旋转，得到俯仰角pitch；
绕旋转之后的X轴旋转，得到滚转角roll；

万向锁（Gimbal Lock）：欧拉角在某些旋转情况下，旋转自由度会减1，存在奇异性；由于万向锁的存在，欧拉角不适合做插值或迭代，应用不广泛；

四元数

四元素基础

四元数是一种既节省空间，又不存在奇异性的刚体旋转表示方式；
在复数平面坐标系下，可以用单位复数来表达旋转操作：
$\begin{matrix} z = x + i y = ρ e^{i θ} & (12) \end{matrix} z=x+i y=\rho e^{i \theta}\tag{12}$
当 $z$ 乘以一个纯虚数 $i$ 表示原来的向量旋转90度：
$zi=(x+i y)i=\rho e^{i (\theta+\frac{\pi}{2})}\tag{13}$
在三维情况下，四元数可以作为复数的扩充：
$\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k\tag{14}$
四元数包含1个实部与3个虚部，虚部之间满足以下的关系（自己与自己运算像复数运算，自己与别人运算像叉乘）：
$\left\{\begin{array}{l} i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j \end{array}\right.\tag{15}$
单位四元数可以表达旋转：
$\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k, \quad \boldsymbol{q}=[s, \boldsymbol{v}], \quad s=q_{0} \in \mathbb{R}, \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3}\tag{16}$

基本运算如下所示：
加减法：
$\begin{aligned} \boldsymbol{q}_{a} \pm \boldsymbol{q}_{b}=& {\left[s_{a} \pm s_{b}, \boldsymbol{v}_{a} \pm \boldsymbol{v}_{b}\right] }\end{aligned}$
乘法：
$\begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) j \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{b} x_{a}+z_{a} s_{b}\right) k \end{aligned}$
$\begin{aligned}\boldsymbol{q}_{a} \boldsymbol{q}_{b}=\left[s_{a} s_{b}-\boldsymbol{v}_{a}^{T} \boldsymbol{v}_{b}, s_{a} \boldsymbol{v}_{b}+s_{b} \boldsymbol{v}_{a}+\boldsymbol{v}_{a} \times \boldsymbol{v}_{b}\right] \end{aligned}$
共轭：
$\boldsymbol{q}_{a}^{*}=s_{a}-x_{a} i-y_{a} j-z_{a} k=\left[s_{a},-\boldsymbol{v}_{a}\right]$
取模长：
$\left\|\boldsymbol{q}_{a}\right\|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}}$
取逆：
$\boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2}$
数乘：
$k \boldsymbol{q}=[k s, k \boldsymbol{v}]$
点乘：
$\boldsymbol{q}_a \cdot \boldsymbol{q}_{b}=s_{a} s_{b}+x_{a} x_{b} i+y_{a} y_{b} j+z_{a} z_{b} k$

旋转角转换为四元数：
$\boldsymbol{q}=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T}\tag{17}$

四元数转为旋转向量：
$\left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{T}=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right.\tag{18}$