【SLAM十四讲】 第三讲

1.旋转矩阵

2.旋转向量和欧拉角

3.四元数


1.旋转矩阵

1.1点 向量 坐标系

向量 是空间中存在的有方向有长度的一样东西,向量不等于坐标,不同坐标系下向量的坐标表示也不尽相同。只有确定了一个坐标系,我们才能说这个向量的坐标是多少。

{\pmb a} =\left [ {\pmb e_{1}}, {\pmb e_{2}}, {\pmb e_{3}} \right ] \begin{bmatrix} a_{1}\\ a_{2}\\a_{3} \end{bmatrix} =a_{1}{\pmb e_{1}}+a_{2} {\pmb e_{2}}+a_{3}{\pmb e_{3}} \ \eqno(3.1)   

向量的外积运算 

{\pmb a}\times {\pmb b}=\begin{bmatrix}{\pmb i}&{\pmb j}&{\pmb k}\\ a_{1}&a_{2}&a_{3}\\ b_{1}&b_{2}&b_{3}\end{bmatrix}=\begin{bmatrix}a_{2}b_{3}-a_{3}b_{2}\\ a_{3}b_{1}-a_{1}b_{3}\\a_{1}b_{2}-a_{2}b_{1}\end{bmatrix}=\begin{bmatrix}0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0 \end{bmatrix} {\pmb b}\triangleq {\pmb a}^{\Lambda }{\pmb b} \ \eqno(3.2) 

其中,^{\Lambda }代表反对称矩阵,即{\pmb a}=\begin{bmatrix}a_{1}\\ a_{2} \\ a_{3} \end{bmatrix} and \ \ {\pmb a}^{\Lambda }=\begin{bmatrix}0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0 \end{bmatrix} \ \eqno(3.3) 

外积可以表示旋转,利用右手法则,大拇指朝向就是旋转向量的方向,事实也是{\pmb a}\times {\pmb b}的方向。所以{\pmb a}{\pmb b}的旋转就可以由大拇指朝向的向量{\pmb \omega }来表示,也就是说,在这个坐标系下,旋转可以用三个实数来描述(这三个实数就是{\pmb \omega }的坐标)

1.2坐标系间的欧式变换

{\pmb a} =\left [ {\pmb e_{1}}, {\pmb e_{2}}, {\pmb e_{3}} \right ] \begin{bmatrix} a_{1}\\ a_{2}\\a_{3} \end{bmatrix} =\left [ {{\pmb e_{1}}}', {{\pmb e_{2}}}',{{\pmb e_{3}}}' \right ] \begin{bmatrix} {a_{1}}' \\ {a_{2}}' \\{a_{3}}'\end{bmatrix} \ \eqno(3.4) 

同一个向量在不同坐标系下有不同的坐标表示,但这都表示的是一个向量,这个向量是固有存在的。

现在将上式(3.4)左乘\begin{bmatrix} e_{1}^{T}\\ e_{2}^{T}\\ e_{3}^{T} \end{bmatrix},得到

式 (3.5) 中的{\pmb R}为旋转矩阵,旋转矩阵 {\pmb R} 的特殊性质: 

旋转矩阵是行列式为 1 的正交矩阵(旋转矩阵的逆为自身转置,逆矩阵\pmb R^{-1}代表一个相反的旋转)

旋转矩阵的集合称之为特殊正交群

{\bf SO}(n)=\left \{\pmb R\in \mathbb{R}^{n\times n} }\Big| \pmb R \pmb R^{T}={ \emph I} &, det( {\pmb R})=1|\right \} \ \eqno(3.6)

1.3 变换矩阵和齐次变换

之前的变换表达式{{\pmb a}}'={\pmb R\pmb a+\pmb t} \ \eqno(3.7),如果发生多次变换,{{\pmb a}}''={\pmb R_{2}} \left ( {\pmb R_{1} \pmb a+\pmb t_{1}} \right ) +{\pmb t_{2}} \ \eqno(3.8)(2次),这样使得计算过于麻烦,所以引入一个自由度,利用齐次坐标和变换矩阵重写(3.7)

\begin{bmatrix} {{\pmb a}}' \\ 1 \end{bmatrix} =\begin{bmatrix} {\pmb R} & {\pmb t}\\ 0^{T} &1\end{bmatrix} \begin{bmatrix} {\pmb a} \\ 1 \end{bmatrix} \triangleq {\emph T} \begin{bmatrix} {\pmb a} \\ 1 \end{bmatrix} \ \eqno(3.9) 式子中的 {\emph T} 称为变换矩阵,这种矩阵又称为特殊欧式群(Special Euclidean Group): 

{\bf SE}(3)=\left \{ {\pmb T}= \begin{bmatrix} {\pmb R} & {\pmb t}\\ 0^{T} &1\end{bmatrix} \in \mathbb{R}^{4\times 4} }\Big| {\pmb R} \in {\bf SO}(3)&, {\bf t} \in \mathbb{R}^{3} \right \} \ \eqno(3.10),同理,T的逆矩阵也代表一个相反的变换。


2.旋转向量和欧拉角

2.1旋转向量

{\bm R}有九个量,但旋转自由度是3,T有16个量,但欧式变换自由度是6。

于是我们寻找更简洁的替代品,用一个旋转轴和旋转角来描述旋转,旋转轴为{\pmb n},旋转角为\theta,旋转向量为{\pmb n}\theta

从旋转向量到{\bm R}旋转矩阵,由一个罗德里格斯公式表明

{\pmb R}= \cos \theta {\bm I} +(1-\cos \theta ) {\pmb n\pmb n^{T}} +\sin \theta{\pmb n^{\Lambda} } \ \ \eqno(3.11)

由上式可以推出tr \left ( {\pmb R} \right )= 1+2\cos \theta \ \ &, \theta=\arccos(( tr(\pmb R)-1)/2 ) \ \ \eqno(3.12) 

同时,由于转轴{\bm n}在旋转后不发生改变,说明{\pmb R\pmb n}={\pmb R} ,所以{\pmb n}是矩阵{\pmb R}特征值=1时,对应的特征向量

2.2欧拉角

用三个分离的转角来描述旋转。欧拉角描述时。应该指定,旋转的转轴顺序。

yaw偏航角(绕Z轴) pitch俯仰角(绕Y轴) roll翻滚角(绕X轴) ypr对应ZYX。

ZYX的万向锁发生在pitch为\pm 90度时。(好像是第二个旋转轴的\pm 90度旋转时发生)


3.四元数。

3.1定义

欧拉角和旋转向量都具有奇异性,需要不带奇异性的三维向量描述方式。

四元数是一种扩展的复数。既是紧凑的,也没有奇异性,缺点在于计算稍微复杂,表示不够直观。

一个四元数{\bm q}拥有一个实部和三个虚部

{\pmb q}={\bm q_{0}}+{\bm q_{1}i}+{\bm q_{2}j}+{\bm q_{3}k} &. \ \ i,j,k代表三个虚部,虚部满足以下关系式
\begin{Bmatrix} i^{2}=j^{2}=k^{2}=-1 \\ij=k,ji=-k\\jk=i,kj=-i\\ki=j,ik=-j \end{matrix} \ \eqno (3.13 )   

或者是用\pmb q= \begin{bmatrix} s & \pmb v \end{bmatrix}, s=q_{0} \in \mathbb{R}, \pmb v=\begin{bmatrix} q_{1} &q_{2} &q_{3} \end{bmatrix}^{T} \in \mathbb{R}^{3}  此时,s称为实部,\pmb v称为虚部。s=0虚四元数,\pmb v=0实四元数。

四元数是复数的扩展,和复数不同的是,复数中乘以一个i代表逆时针旋转90度,而四元数里还乘以一个i代表旋转180度,而i^{2}=j^{2}=k^{2}=-1,代表需要绕着i (j /k)绕两圈才能回到原来的位置

旋转向量与四元数

假设某个旋转 绕{\pmb n}轴,{\pmb n}=\begin{bmatrix} n_{x},n_{y},n_{z} \end{bmatrix}^{T}转了\theta度,那么这个旋转的四元数形式为

\pmb q =\begin{bmatrix} \cos \frac{\theta}{2}, \ n_{x}\sin \frac{\theta}, \ n_{y}\sin \frac{\theta}, \ n_{z}\sin \frac{\theta}{2}\end{bmatrix}^{T}(3.14) 

由上也可以反算对应的旋转轴和夹角

\left\{\begin{matrix} \theta= a\arccos q_{0}\\ \begin{bmatrix} n_{x},n_{y},n_{z}\end{bmatrix}^{T}=\begin{bmatrix} q_{1},q_{2},q_{3}\end{bmatrix}^{T}/ \sin\frac{\theta}{2} \end{matrix}\right.(3.15) 

直观来看,四元数好像关于\theta是转了一半的感觉,对(3.14)式加上一个2\pi,(从绕着{\pmb n}转了\theta\theta+2\pi,是同一个旋转)

我们会得到一个为-\pmb q的四元数,这说明,同一个旋转可以由互为相反数的一对四元数表示。同理,\theta=0时,得到的是一个没有任何旋转的实四元数,\pmb q =\begin{bmatrix} \pm1, 0,0,0\end{bmatrix}^{T}

3.2运算

现有两个四元数 \pmb q_{a} ={\bm s_{a}}+{\bm x_{a}i}+{\bm y_{a}j}+{\bm z_{a}k} , \ \ \ \pmb q_{b} ={\bm s_{b}}+{\bm x_{b}i}+{\bm y_{b}j}+{\bm z_{b}k}, 或用向量表示\pmb q_{a}= \begin{bmatrix} s_{a} & \pmb v_{a} \end{bmatrix}, \pmb q_{b}= \begin{bmatrix} s_{b} & \pmb v_{b} \end{bmatrix},

1)加减

实部实部相加,虚部虚部相加

2)乘法

乘法是将\pmb q_{a}的每一项与\pmb q_{b}的每一项相乘,最后相加

【SLAM十四讲】 第三讲_第1张图片

3)共轭

实部不变 虚部为相反数

4)

\begin{Vmatrix} \pmb q_{a}\end{Vmatrix}=\sqrt{s_{a}^{2}+{x_{a}^2}+{y_{a}^2}+{z_{a}^2}}

5)

\pmb q^{-1}=\pmb q^{*}/ \begin{Vmatrix}\pmb q \end{Vmatrix}

6)数乘与点乘

3.3四元数表示旋转

类似复数表达旋转,复平面上有一点为\pmb p=[a,b],旋转为\pmb q = [ \cos \theta ,\sin \theta], 变换后的坐标\pmb p^{'}= \pmb p * \pmb q = [a * \cos \theta, b*\sin \theta]^{T},

这里的四元数也可以这样近似理解,\pmb p=[0,x,y,z]=[0, \pmb v], 但是此时表示旋转的四元数\pmb q = [ \cos \frac{\theta}{2} ,\sin \frac{\theta}{2}] (根据3.14式,绕着{\pmb n}轴转了\theta角)

旋转后的点\pmb p^{'}可以表示为这样的乘积

\pmb p^{'}= \pmb q \pmb p \pmb q ^{-1}

3.4四元数到旋转矩阵的变换。

\bm{R} = \begin{matrix} {1 - 2q_2^2 - 2q_3^2}&{2{q_1}{q_2} - 2{q_0}{q_3}}&{2{q_1}{q_3} + 2{q_0}{q_2}}\\ {2{q_1}{q_2} + 2{q_0}{q_3}}&{1 - 2q_1^2 - 2q_3^2}&{2{q_2}{q_3} - 2{q_0}{q_1}}\\ {2{q_1}{q_3} - 2{q_0}{q_2}}&{2{q_2}{q_3} + 2{q_0}{q_1}}&{1 - 2q_1^2 - 2q_2^2} \end {matrix}

(右手系以上公式为准,下图有误,应是其转置)

【SLAM十四讲】 第三讲_第2张图片

 

对于下面那个式子,只要第一个式子得出之后,求出q0,1,2,3其实非常简单。


部分公式懒得打,截取自http://blog.csdn.net/youngpan1101/article/details/71086851 

 

 


作业部分

习题1.

这里我是用

R=R_{x}*R_{y}*R_{z}, R^{T}=R_{z}^{T}*R_{y}^{T}R_{x}^{T}, \\RR^{T}=R_{x}*R_{y}*R_{z}*R_{z}^{T}*R_{y}^{T}R_{x}^{T}=I, with \ \ R_{z}R_{z}^{T}=R_{y}R_{y}^{T}=R_{x}R_{x}^{T}=I

来证明的,R_{x}*R_{y}*R_{z}是分别只绕三个轴旋转而得到的旋转矩阵,

其中

这个推导很简单,其实就是一个轴不动,所以把旋转投影到另外两个轴的平面即可,另外两轴同理。此处略。


习题2

罗德里格斯公式

参考wiki百科 https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

或百度百科

https://baike.baidu.com/item/%E7%BD%97%E5%BE%B7%E9%87%8C%E6%A0%BC%E6%97%8B%E8%BD%AC%E5%85%AC%E5%BC%8F

【SLAM十四讲】 第三讲_第3张图片

其实思路很简单,如果\bf v向量与\pmb k向量平行,绕着旋转轴\pmb k轴转动\theta,等于没有进行旋转。也就是说\bf v向量中与\pmb k轴平行的分量,其实没有发生变化,真正旋转的是与\pmb k轴垂直的\bf v_{\perp }分量。所以最终目标 \pmb v_{rot}=\pmb v_{\perp }_{rot} + \pmb v_{\parallel }

让我们逐步分析

1)得到\pmb v_{\parallel }

\pmb k为单位向量,由投影可知,

\begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix}=\begin{Vmatrix} \pmb v \end{Vmatrix} * \cos \alpha , where \ \alpha \ is \ the \ angle \ between \ \pmb k and \ \pmb v, \\ \cos \alpha =\frac{\pmb v \cdot \pmb k }{\begin{Vmatrix} \pmb v \end{Vmatrix} \begin{Vmatrix} \pmb k \end{Vmatrix} } , \ \ \ \begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix} = \pmb v \cdot \pmb k, \ \ \frac{\pmb v_{\parallel } }{\begin{Vmatrix} \pmb v_{\parallel } \end{Vmatrix} }= \pmb k \\ \pmb v_{\parallel }=(\pmb v \cdot \pmb k )\pmb k

2)得到\pmb v_{\perp }

\pmb v_{\perp }=\pmb v- \pmb v_{\parallel }=\pmb v - (\pmb v \cdot \pmb k )\pmb k

3)引入\pmb w

 (注意,最后的\pmb \omega=\pmb v_{\perp} \pmb h只是为了说明,其大小为\pmb v_{\perp},坐标是依据坐标系的不同而不同的,这个式子是建立在以\pmb k为x轴,\pmb v_{\perp}为y轴的右手系坐标下的,其余坐标系可利用旋转关系得到。)

\pmb \omega = \pmb k \times \pmb v_{\perp}= \pmb k \times \left ( \pmb v- \left ( \pmb v \cdot \pmb k \right )\pmb k \right )= \pmb k \times \pmb v- \pmb k \times\left ( \pmb v \cdot \pmb k \right )\pmb k \right ), where \ \ \pmb k \times\pmb k =0 \\\pmb \omega =\pmb k \times \pmb v,\ \ \omega= \begin{bmatrix} \pmb i & \pmb j&\pmb h \\ 1 &0 &0\\ \pmb v_{parellel} & \pmb v_{\perp}&0 \end{bmatrix}=\pmb v_{\perp} \pmb h

4)求解 \pmb v_{\perp }_{rot},原理见图

\pmb v_{\perp}_{rot}=\pmb v_{\perp}*\cos \theta + \pmb \omega *\sin \theta  带入,\pmb v_{\perp}_{rot}=\cos \theta (\pmb v -(\pmb v\cdot \pmb k) \pmb k) + \sin \theta \pmb k \times \pmb v=\cos \theta \pmb v -\cos \theta (\pmb v\cdot \pmb k) \pmb k+ \sin \theta \pmb k \times \pmb v

5)解得\pmb v_{rot}

\pmb v_{rot}= \pmb v_{\parallel }+\pmb v_{\perp}_{rot}=(\pmb v \cdot \pmb k ) \pmb k + \cos \theta (\pmb v -(\pmb v\cdot \pmb k) \pmb k) + \sin \theta \pmb k \times \pmb v \\ \pmb v_{rot}=\cos \theta \pmb v - (1-\cos \theta)(\pmb v\cdot \pmb k) \pmb k + \sin \theta \pmb k \times \pmb v

6)解得\pmb R, \ \ \pmb v_{rot} = \pmb R \pmb v,

设 \pmb k = \begin {bmatrix} \pmb k _{1} \\ \pmb k _{2}\\ \pmb k _{3} \end{bmatrix}, \ \pmb v = \begin {bmatrix} \pmb v_{1} \\ \pmb v_{2}\\ \pmb v_{3} \end{bmatrix}

\pmb R= \pmb v_{rot} * \pmb v^{-1}= ( \cos \theta \pmb v - (1-\cos \theta)(\pmb v\cdot \pmb k) \pmb k + \sin \theta \pmb k \times \pmb v ) * \pmb v^{-1} \\\pmb R= \cos \theta \pmb I - (1-\cos \theta)\pmb k^{T}\pmb k + \sin \theta \pmb k^{\wedge } \\where (\pmb v\cdot \pmb k) \pmb k = \pmb k (\pmb k^{T}\pmb v) ,\ \ \ \pmb k \times \pmb v =\pmb k^{\wedge } \pmb v

当然这里\pmb v并不一定可逆,但暂时也没有找到其他直观的简易的得到\pmb R的解法

总之,得到了罗德里格斯旋转公式 \pmb R= \cos \theta \pmb I - (1-\cos \theta)\pmb k^{T}\pmb k + \sin \theta \pmb k^{\wedge }


习题3

这个也很简单,利用四元数的乘法公式带入即可,注意只需要关心实部,还有可以认为是单位四元数,则四元数的逆等于共轭,对于更一般的情况只需要共轭除以一个模得到逆即可。

此处略


习题4 

前文已经很细节了


习题5 习题6 习题7

都比较简单 不复述了


*

q1.normalize(); //可以实现归一化

//可以实现从四元数到旋转矩阵 
Tcw.prerotate(q1); 
Tcw.pretranslate(t2);

 


 

你可能感兴趣的:(SLAM十四讲,SLAM领域,CV/统计,理论&算法,SLAM十四讲学习)