视觉SLAM基础——旋转矩阵、旋转向量、欧拉角与四元数

刚体运动是SLAM的基础，尤其是旋转的几种常用数学表示，即旋转矩阵、欧拉角、旋转向量和四元数，这部分公式比较多，但是很重要。

Heathens777

603人浏览 · 2022-12-09 18:09:44

Heathens777 · 2022-12-09 18:09:44 发布

旋转矩阵

对于物理世界中的某个向量 $v\bm v$ ，其在某个坐标系下可以表示为
$v=[e1e2e3][xyz]=Ep\bm v=\begin{bmatrix} \bm e_1&\bm e_2&\bm e_3 \end{bmatrix} \begin{bmatrix} x\\y\\ z \end{bmatrix}=\bm E\bm p$
我们只考虑笛卡尔坐标系，所以坐标系的基是互相正交的，因此 $EET=I\bm E \bm E^T=\bm I$ 。如果坐标系旋转后
$v=[e1′e2′e3′][x′y′z′]=E′p′\bm v=\begin{bmatrix} \bm e'_1&\bm e'_2&\bm e'_3 \end{bmatrix} \begin{bmatrix} x'\\y'\\ z' \end{bmatrix}=\bm E'\bm p'$
那么有
$p=ETE′p′=[e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′]p′=Rp′\bm p=\bm E^T\bm E' \bm p'=\begin{bmatrix} \bm e_1^T \bm e'_1&\bm e_1^T \bm e'_2&\bm e_1^T \bm e'_3 \\ \bm e_2^T \bm e'_1&\bm e_2^T \bm e'_2&\bm e_2^T \bm e'_3\\ \bm e_3^T \bm e'_1&\bm e_3^T \bm e'_2&\bm e_3^T \bm e'_3 \end{bmatrix}\bm p'=\bm R \bm p'$
由于 $E\bm E$ 和 $E′\bm E'$ 为正交矩阵，所以 $R\bm R$ 也满足正交性，在群论中这种矩阵称之为特殊正交群：
$SO(n)={R∈Rn×n∣RRT=I,det(R)=1}SO(n)=\{\bm R\isin\reals^{n{\times}n}| \bm R \bm R^T=\bm I,det(\bm R)=1\}$
有了旋转之后，我们还需要考虑坐标系原点的平移，我们可以想象一辆车上的相机在世界坐标系下移动，相机相对于世界坐标系的旋转矩阵和平移向量分别记为 $Rwc\bm R_{wc}$ 和 $twc\bm t_{wc}$ ，对于某个环境中的某个路标点在相机坐标系下和世界坐标系下的坐标分别记为 $pc\bm p^c$ 和 $pw\bm p^w$ ，那么有：
$pw=Rwcpc+twc\bm p^w=\bm R_{wc} \bm{p^c}+\bm t_{wc}$ $pc=RwcTpw−RwcTtwc\bm p^c=\bm R_{wc}^T \bm{p^w}-\bm R_{wc}^T\bm t_{wc}$
由上述公式可得： $Rcw=RwcT，tcw=−RwcTtwc\bm R_{cw}=\bm R^T_{wc}，\bm t_{cw}=-\bm R_{wc}^T\bm t_{wc}$ 也就是说世界相对于相机的旋转为反向的旋转，但平移并不是取相反数那么简单，其还与旋转有关系。我们也可以把平移和旋转写到一个大的矩阵里面，用齐次的形式表示，这个矩阵一般称为变换矩阵，即：
$T=[Rt0T1]\bm T=\begin{bmatrix}\bm R& \bm t\\ \bm 0^T&1\end{bmatrix}$
变换矩阵 $T\bm T$ 也有特殊的形式，群论中称之为特殊欧式群：
$SE(3)={T∈R4×4∣R∈SO(3),t∈R3}SE(3)=\{\bm T\isin\R^{4{\times}4}|\bm R\isin SO(3),\bm t\isin\R^3\}$
有了变换矩阵之后，我们同样可以把路标点由相机坐标系变换到世界坐标系下，即 $pw=Twcpc\bm p^w=\bm T_{wc} \bm p^c$ ，注意此时的 $pc\bm p^c$ 和 $pw\bm p^w$ 都为齐次坐标（添加1作为第四个元素）。旋转只有3个自由度，但旋转矩阵却用了9维来表示，因此其表示是冗余的。另外，旋转的正交且行列式为1的约束，使得当我们想用图优化等方式来估计旋转矩阵变的困难。因此还是有必要探究其他表示旋转的方式，但不论是哪种旋转表⽰⽅式，都只有三个⾃由度。那些多于三个参数的表⽰形式，必定有某种约束条件使得⾃由度保持在三个。对于只有三个参数的表⽰形式，则必然存在奇异点。

旋转向量

任意旋转可以用一个旋转轴加旋转角刻画，我们用单位向量 $n\bm n$ 来表示旋转轴的方向， $θ\theta$ 表示旋转的角度，那么旋转向量可以表示为 $a=θn\bm a=\theta\bm n$ 。旋转向量到旋转矩阵的转换可用罗德里格斯公式表示：
$R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge}$
对公式两边求迹（矩阵的迹为对角元素之和）有：
$tr⁡(R)=cos⁡θtr⁡(I)+(1−cos⁡θ)tr⁡(nnT)+sin⁡θtr⁡(n∧)=3cos⁡θ+(1−cos⁡θ)=1+2cos⁡θ\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(\boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \\ &=3 \cos \theta+(1-\cos \theta) \\ &=1+2 \cos \theta \end{aligned}$
可得旋转向量的大小为：
$θ=arccos⁡tr(R)−12\theta=\arccos \frac {tr(\bm R)-1} 2$
由于在旋转向量方向的所有向量都不受旋转的影响，所以有 $Ra=a\bm R \bm a=\bm a$ ，可见旋转向量的方向即是旋转矩阵特征值为1对应的特征向量的方向。旋转向量其实就是旋转矩阵构成的李群对应的李代数，在后续的李群与李代数篇章会详细介绍。旋转向量的奇异性发生在旋转角度超过 $2π2\pi$ 而产生周期性时。

欧拉角

旋转向量和旋转矩阵都不够直观，人们难以理解是怎样旋转的。而欧拉角把旋转分解为了3次绕不同坐标轴的旋转，其分解方法有很多种，以常使用的YPR为例：
先绕Z轴旋转 $θy\theta_y$ ，这个角度称之为偏航角（yaw)，那么此时的旋转矩阵为：
$RZ(θy)=[cos⁡θysin⁡θy0−sin⁡θycos⁡θy0001]\bm R_{Z}(\theta_y)=\left[\begin{array}{ccc} \cos \theta_{y} & \sin \theta_{y} & 0 \\ -\sin \theta_{y} & \cos \theta_{y} & 0 \\ 0 & 0 & 1 \end{array}\right]$
再绕旋转之后的Y轴旋转 $θp\theta_p$ ，这个角度称之为俯仰角（pitch），那么此时的旋转矩阵为：
$RY(θp)=[cos⁡θp0−sin⁡θp010sin⁡θp0cos⁡θp]\bm R_{Y}(\theta_p)=\left[\begin{array}{ccc} \cos \theta_{p} & 0 & -\sin \theta_{p}\\ 0 & 1 & 0\\ \sin \theta_{p}& 0 & \cos \theta_{p} \end{array}\right]$
最后再绕旋转之后的X轴旋转 $θr\theta_r$ ，这个角度称之为滚转角（roll），那么此时的旋转矩阵为：
$RX(θr)=[1000cos⁡θrsin⁡θr0−sin⁡θrcos⁡θr]\bm R_{X}(\theta_r)=\left[\begin{array}{ccc} 1 & 0 & 0\\0& \cos \theta_{r} & \sin \theta_{r} \\ 0 & -\sin \theta_{r} & \cos \theta_{r} \end{array}\right]$
我们就可以用向量 $θ=[θr,θp,θy]T\bm \theta=[\theta_r,\theta_p,\theta_y]^T$ 来表示任意的旋转。并且有：
$R=RXRYRZ=[cos⁡θpcos⁡θrcos⁡θysin⁡θr+sin⁡θysin⁡θpcos⁡θrsin⁡θysin⁡θr−cos⁡θysin⁡θpcos⁡θr−cos⁡θpsin⁡θrcos⁡θycos⁡θr−sin⁡θysin⁡θpsin⁡θrsin⁡θycos⁡θr+cos⁡θysin⁡θpsin⁡θrsin⁡θp−sin⁡θycos⁡θpcos⁡θycos⁡θp]\bm R=\bm R_X\bm R_Y\bm R_Z=\begin{bmatrix} \cos{\theta_p} \cos{\theta_r} & \cos{\theta_y} \sin{\theta_r}+\sin{\theta_y} \sin{\theta_p} \cos{\theta_r} & \sin{\theta_y} \sin{\theta_r}-\cos{\theta_y} \sin{\theta_p} \cos{\theta_r} \\ -\cos{\theta_p} \sin{\theta_r} & \cos{\theta_y} \cos{\theta_r}-\sin{\theta_y} \sin{\theta_p} \sin{\theta_r} & \sin{\theta_y} \cos{\theta_r}+\cos{\theta_y} \sin{\theta_p} \sin{\theta_r} \\ \sin{\theta_p} & -\sin{\theta_y} \cos{\theta_p} & \cos{\theta_y} \cos{\theta_p} \end{bmatrix}$
当 $θp=π2\theta_p=\frac \pi 2$ 时，旋转矩阵可以简化为：
$R=[0cos⁡θysin⁡θr+sin⁡θycos⁡θrsin⁡θysin⁡θr−cos⁡θycos⁡θr0cos⁡θycos⁡θr−sin⁡θysin⁡θrsin⁡θycos⁡θr+cos⁡θysin⁡θr100]=[0sin(θy+θr)−cos⁡(θy+θr)0cos⁡(θy+θr)sin(θy+θr)100]\bm R=\begin{bmatrix} 0 & \cos{\theta_y} \sin{\theta_r}+\sin{\theta_y} \cos{\theta_r} & \sin{\theta_y} \sin{\theta_r}-\cos{\theta_y} \cos{\theta_r} \\ 0& \cos{\theta_y} \cos{\theta_r}-\sin{\theta_y} \sin{\theta_r} & \sin{\theta_y} \cos{\theta_r}+\cos{\theta_y} \sin{\theta_r} \\ 1 & 0 & 0 \end{bmatrix}=\begin{bmatrix} 0 & sin(\theta_y+\theta_r)&-\cos{(\theta_y+\theta_r)} \\ 0& \cos{(\theta_y+\theta_r)}& sin(\theta_y+\theta_r)\\ 1 & 0 & 0 \end{bmatrix}$
从公式中可以看出，此时 $θy\theta_y$ 和 $θr\theta_r$ 是等价的，我们无法从这样一个旋转矩阵中确定出唯一的 $θy\theta_y$ 和 $θr\theta_r$ ，这正是欧拉角的奇异性导致的，也就是著名的万向锁问题。我们考虑足无穷小的旋转，根据麦克劳林公式有 $cos⁡θ≈1\cos \theta\approx1$ 和 $sin⁡θ≈θ\sin\theta\approx\theta$ ，并且忽略极小角度的乘积，那么此时：
$R≈[1θr−θp−θr1θyθp−θy1]≈I+θ∧\bm R\approx\begin{bmatrix}1&\theta_r&-\theta_p\\-\theta_r&1&\theta_y\\\theta_p&-\theta_y&1 \end{bmatrix}\approx\bm I+\bm {\theta}^{\wedge}$
而由罗德里格斯公式同样可以得 $R≈I+a∧\bm R\approx\bm I+\bm a^{\wedge}$ ，由此可见当旋转无穷小时，欧拉角近似为旋转向量。

四元数

对于物理世界中的某个二维向量 $v\bm v$ ，在复平面 $C$ 中的坐标可以表示为 $p=[x,y]T\bm p=[x,y]^T$ ，对应的复数为 $z = x + y i$ ，如果另个一复平面 $C^{'}$ 相对于此复平面旋转了 $θ\theta$ ，那么此复数在新的复平面下为：
$z′=(x+yi)(cos⁡θ+isin⁡θ)=xcos⁡θ−ysin⁡θ+(xsin⁡θ+ycos⁡θ)iz'=(x+yi)(\cos\theta+i\sin\theta)=x\cos\theta-y\sin\theta+(x\sin\theta+y\cos\theta)i$
复数 $z^{'}$ 对应的坐标 $p′=[x′,y′]T\bm p'=[x',y']^T$ 为：
$p′=[cos⁡θ−sin⁡θsin⁡θcos⁡θ][xy]=Rp\bm p'=\begin{bmatrix}\cos\theta&-\sin\theta\\\sin\theta&\cos\theta\end{bmatrix}\begin{bmatrix}x\\y\end{bmatrix}=\bm R\bm p$
由此可见，我们用单位复数 $cosθ+isinθcos\theta+isin\theta$ 表示二维的旋转，类比可用单位四元数表示三维旋转。
四元数定义为 $q=q0+q1i+q2j+q3k=[s,v]T\bm q= q_0+q_1i+q_2j+q_3k=[s,\bm v]^T$ 。其中 $i, j, k$ 为虚部，满足如下关系：
${i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j\begin{cases}i^2=j^2=k^2=-1\\ij=k,ji=-k\\jk=i,kj=-i\\ki=j,ik=-j\end{cases}$
两个四元数 $q1\bm q_1$ 和 $q2\bm q_2$ 的乘积为： $q1q2=[s1s2−v1Tv2,s1v2+s2v1+v1×v2]T\boldsymbol{q}_{1} \boldsymbol{q}_{2}=\left[s_{1} s_{2}-\boldsymbol{v}_{1}^{\mathrm{T}} \boldsymbol{v}_{2}, s_{1} \boldsymbol{v}_{2}+s_{2} \boldsymbol{v}_{1}+\boldsymbol{v}_{1} \times \boldsymbol{v}_{2}\right]^{\mathrm{T}}$
我们定义如下两个矩阵：
$q+=[s−vTvsI+v∧],q⊕=[s−vTvsI−v∧]\boldsymbol{q}^{+}=\left[\begin{array}{cc} s & -\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right], \quad \boldsymbol{q}^{\oplus}=\left[\begin{array}{cc} s & -\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}-\boldsymbol{v}^{\wedge} \end{array}\right]$
那么四元数的乘积也可以写成矩阵乘以向量的形式，即：
$q1q2=q1+q2=q2⊕q1\bm q_1 \bm q_2=\boldsymbol{q}^{+}_1 \bm q_2=\boldsymbol{q}^{\oplus}_2\bm q_1$
四元数的模定义为： $∥q∥=s2+vTv\|\bm q\|=\sqrt{ s^2+\bm v^T \bm v}$ 那么两个四元数乘积的模即为模的乘积，即 $∣∣q1q2∣∣=∥q1∥∥q2∥||\bm q_1 \bm q_2||=\|\bm q_1\|\|\bm q_2\|$ ，所以两个单位四元数的乘积仍为单位四元数。
四元数的共轭四元数为虚部取反，即 $q∗=[s,−v]T\bm q^*=[s,-\bm v]^T$ ，由乘积公式可得 $qq∗=q∗q=[∥q∥,0]T\bm q \bm q^*=\bm q^* \bm q=[\|\bm q\|,\bm 0]^T$
四元数的逆定义为： $q−1=q∗∥q∥\bm q^{-1}=\frac {\bm q^*} {\|\bm q\|}$ 因此 $qq−1=q−1q=[1,0]T\bm q \bm q^{-1}=\bm q^{-1} \bm q=[1,\bm 0]^T$ ，由于单位四元数的模长为1，所以单位四元数的逆与共轭是相同的。另外，四元数乘积的逆等于交换位置后逆的乘积，这个与矩阵的性质类似，即 $(q1q2)−1=q2−1q1−1\left(\boldsymbol{q}_{1} \boldsymbol{q}_{2}\right)^{-1}=\boldsymbol{q}_{2}^{-1} \boldsymbol{q}_{1}^{-1}$
我们把三维空间的某个向量 $v\bm v$ 在某个坐标系下的坐标构造成一个纯虚四元数（即实部为0，虚部对应的向量为空间坐标） $p\bm p$ ，单位四元数 $q\bm q$ 指定了坐标系的旋转，我们令 $p′=qpq−1\bm p'=\bm q \bm p\bm q^{-1}$ ，那么新的四元数 $p′\bm p'$ 的虚部即表示此向量在旋转后新坐标系下的坐标，且此时 $p′\bm p'$ 仍为纯虚四元数。我们用四元数的矩阵乘法表示如下：
$p′=qpq−1=q+p+q−1=q+(q−1)⊕p=[s−vTvsI+v∧][svT−vsI+v∧]p=[100TvvT+s2I+2sv∧+(v∧)2]p\begin{aligned} \boldsymbol{p}^{\prime} &=\boldsymbol{q} \boldsymbol{p} \boldsymbol{q}^{-1}=\boldsymbol{q}^{+} \boldsymbol{p}^{+} \boldsymbol{q}^{-1} =\boldsymbol{q}^{+}\left(\boldsymbol{q}^{-1}\right)^{\oplus}\bm p\\&=\left[\begin{array}{cc} s & -\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right]\left[\begin{array}{cc} s & \boldsymbol{v}^{\mathrm{T}} \\ -\boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right]\bm p\\&=\left[\begin{array}{cc} 1 &\bm 0 \\ \mathbf{0}^{\mathrm{T}} & \boldsymbol{v} \boldsymbol{v}^{\mathrm{T}}+s^{2} \boldsymbol{I}+2 s \boldsymbol{v}^{\wedge}+\left(\boldsymbol{v}^{\wedge}\right)^{2} \end{array}\right] \bm p \end{aligned}$
由此可得旋转矩阵为：
$R=vvT+s2I+2sv∧+(v∧)2\boldsymbol{R}=\boldsymbol{v} \boldsymbol{v}^{\mathrm{T}}+s^{2} \boldsymbol{I}+2 s \boldsymbol{v}^{\wedge}+\left(\boldsymbol{v}^{\wedge}\right)^{2}$
对等式两边取迹可得：
$tr(R)=4s2−1tr(\bm R)=4s^2-1$
结合旋转向量与旋转矩阵转换公式可得，旋转向量大小和四元数实部之间的关系为：
$s=cos⁡θ2\theta=\arccos(2s^2-1)\implies s=\cos \frac \theta 2$
如果令 $p=[0,v]T\bm p=[0,\bm v]^T$ ，那么 $p\bm p$ 在旋转后仍然不变，可见旋转向量的方向为单位四元数的虚部 $v\bm v$ ，即：
$n=v∥v∥=v1−s2\bm n=\frac {\bm v} {\|v\|}=\frac {\bm v}{ \sqrt {1-s^2}}$