视觉SLAM十四讲阅读笔记一-三维空间刚体运动


这篇文章是视觉SLAM十四讲第3讲阅读过程中总结和记录的学习内容。

三维空间中的刚体(相机、机器人)不光有位置,还有姿态,即位姿,位置表示相机在空间中哪个地方,姿态表示相机的指向。SLAM中一个很基本的问题就是计算机器人在三维空间中的位姿。而机器人在三维空间中位姿的计算往往与三维空间中的刚体运动有关。本篇是学习《视觉SLAM十四讲》第3讲三维空间刚体运动有关记录,参考了童博士的笔记内容。介绍在三维空间中刚体运动的位姿表示方法。

位姿=位置+姿态

SLAM中的位姿有6个自由度,即位置和姿态,其变换过程包含了旋转(Rotation)平移(Translation)。直观地理解就是,对于在平面中运动的机器人,其位姿的变换由两个位置和一个转角来描述,转角描述旋转过程,两个位置描述平移过程。机器人任意两个时间间隔位置和转角的变化都可以通过运动传感器检测得到。这里涉及到SLAM中一个很重要的问题,即位姿的表达优化

位置表示

准确地说应该是机器人运动过程中位置变化的表示。

在一个三维空间中,建立三维坐标系之后,就可以用一个三维坐标来表示机器人的位置。
对于机器人的位置变换,可以用一个三维的平移向量来表示,比如机器人从初始位置为$(x_0,y_0,z_0)$经过一个平移向量$t=(a,b,c)$,可以得到平移后的位置$(x_0+a,y_0+b,z_0+c)$。相对来说,机器人的位置表达比较简单,只需要一个平移向量即可。

姿态表示

准确地说应该是机器人运动过程中姿态变化的表示。

在一个三维空间中,通常用一个三维向量来表示机器人的姿态。更直观的讲,机器人的姿态可以想象成机器人自带一个坐标系,这个坐标系的原点就是机器人,z轴表示机器人面向的方向,这样的一个坐标系可以通过表示机器人姿态的三维向量来构造。

机器人的姿态表达相对复杂,表达方法通常有旋转矩阵、旋转向量、欧拉角、四元数。

旋转矩阵$SO(3)$

在SLAM问题中,旋转矩阵是表示姿态变换最常用的方式。
假设空间中的某一点$P$,机器人旋转前的坐标系的单位正交基为$(e1,e2,e3)$,$P$的坐标为$(x_1,y_1,z_1)$。。机器人旋转后的坐标系的单位正交基为$(e′_1,e′_2,e′_3)$,$P$的坐标为$(x′_1,y′_1,z′_1)$。

根据坐标的定义有:

$\left[ \begin{matrix}e_1&e_2&e_3\end{matrix} \right] \left[ \begin{matrix}a_1\a_2\a_3\\end{matrix} \right]=[e′_1 e′_2 e′_3]\left [\begin{matrix}a′_1\a′_2\a′_3\\end{matrix} \right]$

在等式的左右同时左乘$\left[ \begin{matrix}a^T_1\a^T_2\a^T_3\\end{matrix} \right]​$,可得

$\left[ \begin{matrix}a1\a2\a3\\end{matrix} \right]=\left[ \begin{matrix}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{matrix} \right]\left [\begin{matrix}a′_1\a′_2\a′_3\\end{matrix} \right]=Ra′​$

矩阵$R$即为旋转矩阵。旋转矩阵是一个行列式为1的正交矩阵。同样所有行列式为1的正交矩阵都是旋转矩阵,因此可以把旋转矩阵做如下定义:

$SO(n)={Rϵℝ^{n×n}∣RR^T=I,det(R)=1}$

$SO(n)$称为特殊正交群,这个集合由$n$维空间的旋转矩阵组成,特别的,$SO(3)$就是三维空间下的旋转矩阵。

值得注意的是,旋转矩阵本质上是表示两个坐标系之间的旋转,而坐标系恰恰能够表示机器人姿态,因此旋转矩阵可以描述机器人(相机)的旋转,即姿态变换。

变换矩阵$SE(3)$

在旋转矩阵的基础之上,加上平移向量,就可以完整的刻画三维空间中的刚体运动,即:$a′=Ra+t$。该式用一个旋转矩阵$R$和一个平移向量$t$完整地描述了一个欧式空间的坐标变换关系。

但是这种形式下会存在一个问题,假设我们进行了两次变换$R_1$,$t_1$和$R_2$,$t_2$。相应的三维空间中经历了从a点到b点到c点的变换。则满足公式:

$b=R{1}a+t_1,c=R{2}b+t_2$

从a到c的变换为:
$c=R2(R{1}a+t_1)+t_2$

这样的形式在变换多次之后会过于复杂。聪明的数学家引入了齐次坐标和变换矩阵的概念,使得三维空间中的刚体运动有如下的变换形式:

$\left[ \begin{matrix}a′\1\end{matrix} \right]=\left[ \begin{matrix}R&t\0^T&1\end{matrix} \right]\left [\begin{matrix}a\1\\end{matrix} \right]=T\left [\begin{matrix}a\1\\end{matrix} \right]$

矩阵$T$即称为变换矩阵

此时从a到c的变换可表示为:
$c=T_2T_1a$

此处的a和c为相应齐次坐标。

变换矩阵的左上角为旋转矩阵,右侧为平移向量,左下角为0向量,右下角为1。这种矩阵称为特殊欧式群(Special Euclidean Group)

$SE(3)={T=\left[ \begin{matrix}R&t\0^T&1\end{matrix} \right]ϵℝ^{4×4}∣RϵSO(3),tϵℝ^3}$

齐次坐标

透视变换是非线性变换,可以通过引入齐次坐标以线性表示透视变换。齐次坐标是在普通坐标上增加一维(值为1)后的坐标表示。在是三维向量的末尾添加1,将其变成了四维向量,多了一个自由度,允许把变换写成线性的形式。对于四维向量,就可以把平移和旋转写在一个矩阵里面,使得整个关系变成线性关系。

  1. 将普通坐标转换为齐次坐标:增加一维坐标,值为1。

  2. 齐次坐标转换为普通坐标:除以最后一维坐标。

  3. 齐次坐标是缩放不变的(Invariant to scaling)

旋转向量

用旋转矩阵来表示旋转有两个缺点:

  1. $SO(3)$的旋转矩阵有九个量,但一次旋转只有三个自由度。因此这种表达方式是冗余的。
  2. 旋转矩阵自身两个约束,即必须是正交矩阵和行列式为1,这些约束会使得求解变得更困难。

从直观上讲,任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量称为旋转向量(或轴角,Axis-Angle),这样只需要一个三维向量就可以描述旋转。旋转向量跟之后要介绍的李代数是相对应的。旋转向量到旋转矩阵的变换可由罗德里格斯公式求得。
假设旋转轴为$n$,角度为$θ$,则旋转向量为$θn$对应的旋转矩阵$R$为:

$R=cosθI+(1−cosθ)nn^T+sinθ[n]^\wedge$

反之,对于转角$\theta$有:

$tr(R)=cos\theta tr(I)+(1-cos\theta)tr(nn^T)+sin\theta tr(n^\wedge)\=3cos\theta+(1-cos\theta)\=1+2cos\theta$

因此:

$θ=arccos(\frac{tr(R)−1}2)$

由于旋转轴上的向量在旋转后不发生改变,说明:$Rn=n$

因此转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程,再归一化,就得到了旋转轴。

欧拉角

欧拉角是一种最为直观的姿态变换描述方式。在欧拉角的表示方式中,将旋转分解成沿三个坐标轴旋转的量:滚转角-俯仰角-偏航角(roll-pitch-yaw),此时可以使用$[r,p,y]^T$这样一个三维向量描述任意旋转。

欧拉角的一个重大缺点就是万向锁问题:在俯仰角为±90度时,第一次旋转与第三次旋转将使用同一个轴,这被称为奇异性问题。由于欧拉角不适于插值和迭代,所以在SLAM问题中通常不使用欧拉角来表示旋转,所以不再展开记录。

四元数

旋转矩阵具有冗余性,欧拉角和旋转向量不冗余但是具有奇异性。其实,我们找不到不带奇异性的三维向量描述方式。
四元数只有四个自由度,即是紧凑的而且没有奇异性。它是一种扩展的复数的表达方式。缺点是不够直观,运算稍复杂。

四元数q拥有一个实部和三个虚部。如下:

$q=q_0+q_1i+q_2j+q_3k$

其中:

$\left{ \begin{array}{ll} i^2=j^2=k^2=−1\ ij=k,ji=−k \jk=i,kj=−i,ki=j,ik=−j\end{array} \right.​$

我们可以用单位四元数表示三维空间中的任意一个旋转。

假设某个旋转绕单位向量$n=[n_x,n_y,n_z]^T$进行了角度$θ$的旋转,那么对应的四元数为:

$q=[cos\fracθ2,n_xsin\fracθ2,n_ysin\fracθ2,n_zsin\fracθ2]$

反之也可以从单位四元数中计算出对应旋转轴和夹角:

$\left{ \begin{array}{ll} θ=2arccosq_0\ [n_x,n_y,n_z]^T=\frac{[q_1,q_2,q_3]^T}{sin\fracθ2}\end{array} \right.$

对$θ$加上$2π$可以得到一个相同的旋转,但此时对应的四元数变成了$−q$。因此任意的旋转都可以由两个互为相反数的四元数表示,即两个互为相反数的四元数可以表示同一个旋转。

四元数的运算

见《视觉SLAM十四讲》P55,不再做记录。

四元数表示旋转

假设空间中有一点$p=[x,y,z]$,,其绕旋转轴$n$,进行了角度为$θ$的旋转得到了点$p′$,使用旋转矩阵表述有:

$p′=Rp$。

使用四元数描述旋转该如何表达?首先,把三维空间点用一个虚四元数表示:

$p=[0,x,y,z]=[0,v]$

相当于把四元数的3个虚部与空间中的3个轴相对应。然后,由前述公式可知,用四元数$q$表示该旋转:

$q=[cos\fracθ2,n_xsin\fracθ2,n_ysin\fracθ2,n_zsin\fracθ2]$

则可以验证:
$p′=qpq^{−1}$

最终结果实部为0,故为纯虚四元数,其虚部的3个分量表示旋转后的3D点的坐标。

四元数到旋转矩阵的转换

设四元数$q=q_0+q_1i+q_2j+q_3k$,对应的旋转矩阵$R$为:

$R=\left[ \begin{array}{ccc}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]$

反之,假设旋转矩阵为$R={m_{ij}},i,j\epsilon[1,2,3]$,其对应的四元数$q$为:

$q0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m{23}-m{32}}{4q_0},q_2=\frac{m{31}-m{13}}{4q_0},q_3=\frac{m{12}-m_{21}}{4q_0}​$

注意:实际编程中,当$q_0$的值接近0时,其它3个分量会非常大,导致解不稳定,此时可以再考虑使用其他方式进行转换。

旋转的四种表示方法比较

表示方法 形式 是否奇异性 分量数目 紧凑or冗余程度 直观性
旋转矩阵 $R$ 9 冗余 不直观
旋转向量 $\theta{n}$ 3 较紧凑 不直观
欧拉角 $[r,p,y]^T$ 3 较紧凑 直观
四元数 $q=q_0+q_1i+q_2j+q_3k$ 4 较紧凑 较不直观

旋转的四种表示方法间互相转换

1.矩阵转换成其他方式

旋转矩阵 $R={m_{ij}},i,j\epsilon[1,2,3]$
旋转向量 $θ=arccos(\frac{tr(R)−1}2)$;求解方程$Rn=n$,归一化处理得到旋转轴n
欧拉 -
四元数 $q0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m{23}-m{32}}{4q_0},q_2=\frac{m{31}-m{13}}{4q_0},q_3=\frac{m{12}-m_{21}}{4q_0}$

2.向量转换成其他方式

旋转向量 $θn$
旋转矩阵 $R=cosθI+(1−cosθ)nn^T+sinθ[n]^\wedge$
欧拉角 -
四元数 $q=[cos\fracθ2,n_xsin\fracθ2,n_ysin\fracθ2,n_zsin\fracθ2]$

3.数转换成其他方式

四元数 $q=q_0+q_1i+q_2j+q_3k$
旋转矩阵 $R=\left[ \begin{array}{ccc}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]$
旋转向量 $\left{ \begin{array}{ll} θ=2arccosq_0\ [n_x,n_y,n_z]^T=\frac{[q_1,q_2,q_3]^T}{sin\fracθ2}\end{array} \right.$
欧拉角 -

参考资料

  1. 《视觉SLAM十四讲》第二讲、第三讲
  2. 一索哥传奇-《三维空间中的刚体运动》

本文标题:视觉SLAM十四讲阅读笔记一-三维空间刚体运动

文章作者:阿翔

发布时间:2018年08月04日 - 15:08

最后更新:2019年05月28日 - 21:05

原始链接:http://ttshun.com/2018/08/04/视觉SLAM十四讲阅读笔记一/

许可协议: 署名-非商业性使用-禁止演绎 4.0 国际 转载请保留原文链接及作者。

点击给我一些鼓励叭!
-------------本文结束感谢您的阅读-------------
0%