视觉SLAM的数学基础 第一篇 3D空间的位置表示

视觉SLAM中的数学基础 第一篇 3D空间的位置表示

前言

  转眼间一个学期又将过去,距离我上次写《一起做RGBD SLAM》已经半年之久。《一起做》系列反响很不错,主要由于它为读者提供了一个可以一步步编码、运行的SLAM程序,为读者理解SLAM实现的细节作了详细的介绍。但是我也有很多对它不满意的地方。作为面向实现的介绍,它的代码不够稳定可靠,例如,甚至没有对匹配丢失的情况进行处理,因而只能用于教学。另一方面,对SLAM研究者来说,我只是介绍了编码方面如何调用一些常见的库函数,而没有对这些函数进行深入的,原理上的讲解。这就导致了读者只了解了函数的接口,而没法根据数学原理进行创新。归根到底,研究机器人相关问题,一是要有扎实的数学基础,二是要有强大的动手编程能力,这对大多数刚入门的研究者来说,极具挑战性。我也希望,通过阅读我的博客,你能走进SLAM研究的门槛,有朝一日自己也写出优秀的程序和论文。

  有鉴于此,我准备写一写SLAM相关的数学知识,包括代数、几何、概率、运筹等等。对于重要的算法例如ICP,EKF,细致讨论它的原理,并给出它的实现(原生的代码或在某个库的实现)。由于它们的原理较复杂,我会从最基本的东西开始讲起。但是我毕竟不是在写数学书,我不会像数学书那样写成``定义——定理——推论”的结构。我们不会纠缠于一些定理的严格证明,相反的,我们只在必要的情况下加以说明,告诉读者这些数学公式在SLAM中有何应用,如何应用。

  由于博客编辑器的限制,我们以斜体字$x$表示变量,以粗正体$\mathbf{A}$表示矢量和字母,以黑板粗体$\mathbb{R}$表示空间。希腊字母没有粗体所以保持原样。向量默认为列向量。其余和普通的数学书一致。 

  小萝卜:师兄,这么严肃不是你的风格啊!

  师兄:啊,数学嘛……


 

刚体运动

  本篇我们讨论一个很基础的问题:如何描述机器人的位姿。这也是研究SLAM的第一个问题。注意这里“位姿”的用语包含了位置和姿态。描述位置是很简单的。如果机器人在平面内运动,那么用两个坐标来描述它的位置:$$ \mathbf{x} = \left[ x, y \right]^T.$$ 相应的,如果它在三维空间中,我们就用三个空间坐标来表示:$$ \mathbf{x} = \left[ x, y, z \right].$$

  姿态的表达比点稍为复杂。2D的姿态可以只用一个旋转角$\theta$表达。3D姿态的表达方式则有多种。常见的如欧拉角、四元数、旋转矩阵等。我们将在后文详细介绍。有了位置和姿态,我们就可以描述一个坐标系。进一步,还能描述坐标系间的变换关系。常见的问题如:机器人视野中某个点,对世界坐标系的(或地图的)哪个点?这时,就需要先得到该点针对机器人坐标系坐标值,再根据机器人位姿转换到世界坐标系中。 

齐次坐标系

  在位姿转换中,通常采用射影空间的齐次坐标表示。齐次坐标是什么呢?记$n$维射影空间为$\mathbb{P}^n$,其中一个空间点的坐标为普通的3D坐标加一个齐次分量:$$ \mathbf{x}=\left[ x_1, \ldots, x_n, w \right]^T. $$ 例如,在2维和3维射影空间中的点,分别表示为:

\[\begin{equation}
\mathbf{x}_{2D} = \left[x, y, w \right]^T \quad \mathbf{x}_{3D} = \left[ x, y, z, w \right]^T
\end{equation}\]

  小萝卜:既然一个空间点只有3个坐标,为啥非要用四个数表示呢?

  师兄:嗯,四个数表示点,说明点和坐标肯定不是一一对应的。没错,在齐次坐标中,某个点$\mathbf{x}$的每个分量同乘一个非零常数$k$后,仍然表示的是同一个点。因此,一个点的具体坐标值不是唯一的。如$\left[1,1,1,1\right]^T$和$\left[2,2,2,2\right]^T$是同一个点。但在$w \neq 0$时,我们可以对每个坐标除以最后一项$w$,强制最后一项为1,从而得到一个点唯一的坐标表示:

\[\begin{equation}
\mathbf{x}_{3D} = \left[ x/w, y/, z/w, 1 \right]^T
\end{equation}\]

  这时,忽略掉最后一项,这个点的坐标和欧氏空间就是一样的。那么,为要用齐次坐标呢?原因有以下几条。

  • 齐次坐标下点和直线(高维空间里为超平面)能够使用同样的表达。

  例如,3D空间$\mathbb{R}^3$中,一个平面$\pi$可由一个方程定义:
\[\begin{equation}
\pi : ax + by + cz + d = 0
\end{equation}\]

  则该平面$\pi$可以用$\mathbb{P}^3$中的坐标$\mathbf{\pi} = \left[ a,b,c,d \right]^T$来描述。这样,点位于平面上(2D对应点位于直线上)的事情可以简洁地表示为:
\[\begin{equation}
\label{eq:pointOnPlane}
\mathbf{\pi}^T \mathbf{x} = 0.
\end{equation}\]

  把点和超平面采用同样的表示,这种做法一个非常直接的好处,是射影几何里的“对偶原理”。该原理是说,任何有关“点”与“平面”的命题,都可以交换“点”与“平面”的概念,得到一个对偶的命题。对偶命题和原命题是一样的。通过“对偶原理”,射影几何的数学家就可以偷懒,只需要证一半定理,因为对偶命题和原命题有同样的涵义。例如,我们证明了$\mathbb{P}^2$中某条件下三点共线,那么替换概念后的三线共点则自然成立。

  小萝卜:数学家真是好懒啊!

  • 齐次坐标能囊括无穷远点与无穷远超平面。

  最后一项坐标为零的点称为无穷远点,它们在$\mathbb{P}^n$中真实存在,且能够很方便地参与正常的代数运算。根据式\ref{eq:pointOnPlane},易见所有无穷远点都在一个平面$\mathbf{\pi}_\infty = \left[ 0, 0, 0, 1\right]^T$上,该平面记作无穷远平面(2D对应无穷远直线)。
  $\mathbb{P}^2$中的无穷远直线较容易理解。它就像是地平线,与所有直线相交于位于它之上的无穷远点。而且,在射影变换中(例如照相),很容易在照片中看到地平线并算出它的方程。这说明2D射影变换会把无穷远线变成通常的直线。

  • 齐次坐标可以方便地将平移与旋转放在一个矩阵中。

  师兄:这应该是最明显的好处啦!大家都爱用齐次坐标,包括我。有关坐标系怎么用齐次坐标进行变换,后文会详细解释。现在我们能表达点了,还剩下一个姿态。由于2D与3D差别较大,我们分而述之。


 2D姿态的描述

  2D空间中,物体的位姿可用两个平移量$t_x, t_y$加一个旋转角$\theta$表示,如下图所示。此时,设机器人坐标系$O'-X'-Y'$下某点的坐标为$\left[ x_r, y_r\right]^T$,对应在世界坐标系$O-X-Y$下为$\left[x_w, y_w\right]^T$,那么由直观推得:

\[\begin{equation}
\left\{ \begin{array}{l}
{x_w} = {x_r}\cos \theta - {y_r}\sin \theta + {t_x}\\
{y_w} = {x_r}\sin \theta + {y_r}\cos \theta + {t_y}
\end{array} \right.
\end{equation}\]

  读者可以自行尝试推导一下,在虚线处建立一个中间坐标系即可。若将该式写成矩阵形式,则有:

\[\begin{equation}
\label{eq:2dTransRT}
\mathbf{x}_w = \mathbf{R} \mathbf{x}_r + \mathbf{t}
\end{equation}\]

  其中
$$\mathbf{R} = \left[ {\begin{array}{*{20}{c}}
{\cos \theta }& - \sin \theta \\
{\sin \theta }&{\cos \theta }
\end{array}} \right], \quad \mathbf{t} = \left[ t_x, t_y\right]^T$$
$\mathbf{R}$称为旋转矩阵,$\mathbf{t}$为平移矢量。注意到$\mathbf{R}$是一个正交矩阵,且只有一个自由度。加上平移矢量后,一共有三个自由度。

  此定义下的旋转阵必是正交阵。而正交阵并非全是旋转矩阵。事实上,行列式为$+1$的正交阵才是旋转矩阵,行列式为$-1$的正交阵是镜像后的旋转矩阵。

  式\ref{eq:2dTransRT}中,$\mathbf{x}_w$和$\mathbf{x}_r$还不是线性关系。下面我们用齐次坐标表示它们,即:
$$ \mathbf{x}_w = \left[ x_w, y_w, 1\right]^T, \mathbf{x}_r = \left[ x_r, y_r, 1\right]^T $$
则有:

\[\begin{equation}
{\mathbf{x}_w} = \left[ {\begin{array}{*{20}{c}}
\mathbf{R}_{2 \times 2} & \mathbf{t}_{2 \times 1 }\\
\mathbf{0}^T_{1 \times 2} & I_{1 \times 1}
\end{array}} \right]{\mathbf{x}_r}
\end{equation}\]

  为便于理解,我们在矩阵下方标出了它的维数。可以看使用齐次坐标标满足了线性关系,记作:
\[\begin{equation}
\mathbf{x}_w = \mathbf{T}_{w,r} \mathbf{x}_r
\end{equation}\]
  其中$\mathbf{T}_{w,r}$表示从世界坐标系到机器人坐标系的变换矩阵。我们也可以轻松地写出反向的变换矩阵:

\[\begin{equation}
\mathbf{x}_r = \mathbf{T}_{r,w} \mathbf{x}_w = \mathbf{T}^{-1}_{w,r} \mathbf{x}_w = \left[ {\begin{array}{*{20}{cc}}
{{\mathbf{R}^{ - 1}}}&{{-\mathbf{R}^{ - 1}} \mathbf{t}}\\
{{0^T}} & 1
\end{array}} \right] \mathbf{x}_w
\end{equation}\]

  既然如此,我们就可用$\mathbf{T}$表示机器人的位姿,那么机器人在时刻$t$的位姿就可以记作$\mathbf{T}_t$。当然,从存储上来讲,存储$\mathbf{T}$是不经济的。在2D运动中,它有九个变量,但实际自由度只有三个。所以我们可以只存储位移矢量$\mathbf{t}$与旋转角$\theta$,而在需要计算的时候再构建出$\mathbf{T}$。称2D欧几里得变换,它对矩阵乘法构成群(群是一个集合加一种运算,且运算在该集合上满足封闭性、结合律、有单位元和逆元。),该群记作$SE(2)$。相应的,二维旋转构成二维旋转群(或称特殊正交群)$SO(2)$。有关它们进一步的性质,我们会在以后的李群、李代数中提到。


 3D变换

  3D的旋转可以由旋转矩阵、欧拉角、四元数等若干种方式描述,它们也统称为三维旋转群$SO(3)$。而3D的变换即旋转加上位移,是为$SE(3)$。为了和2D变换统一起见,我们首先介绍旋转矩阵表示法。

旋转矩阵描述

  旋转矩阵是一种$3\times 3$的正交矩阵,它对变换的描述十分类似于2D情形。参照上一节的数学符号,我们有:
\[\begin{equation}
{\mathbf{x}_w} = \left[
{\begin{array}{*{20}{c}}
\mathbf{R}_{3 \times 3} & \mathbf{t}_{3 \times 1 }\\
\mathbf{0}^T_{1 \times 3} & 1_{1 \times 1}
\end{array}} \right]{\mathbf{x}_r}
\end{equation}\]

  这里$\mathbf{R}$为3D的旋转矩阵,同样的,$\mathbf{t}$为3D的平移矢量。

  由于3D旋转都可以归结成按照某个单位向量$\mathbf{n}$进行大小为$\theta$的旋转。所以,已知某个旋转时,可以推导出对应的旋转矩阵。该过程由罗德里格斯公式表明,由于过程比较复杂,我们在此不作赘述,只给出转换的结果:
\[\begin{equation}
\mathbf{R}(\mathbf{n}, \theta) = \left[ {\begin{array}{*{20}{c}}
{n_x^2\left( {1 - c\theta } \right) + c\theta }&{{n_x}{n_y}\left( {1 - c\theta } \right) + {n_z}s\theta }&{{n_x}{n_z}\left( {1 - c\theta } \right) - {n_y}s\theta }\\
{{n_x}{n_y}\left( {1 - c\theta } \right) - {n_z}s\theta }&{n_y^2\left( {1 - c\theta } \right) + c\theta }&{{n_y}{n_z}\left( {1 - c\theta } \right) + {n_x}s\theta }\\
{{n_x}{n_z}\left( {1 - c\theta } \right) + {n_y}s\theta }&{{n_y}{n_z}\left( {1 - c\theta } \right) - {n_x}s\theta }&{n_z^2\left( {1 - c\theta } \right) + c\theta }
\end{array}} \right]
\end{equation}\]

  这里$\mathbf{n} = \left[ n_x, n_y, n_z\right]^T, c\theta=\cos \theta, s\theta=\sin \theta$。公式虽然较为复杂,但实际写成程序后,只需知道旋转方向和角度后即可完成计算。另一件有趣的事是,如果用$$\mathbf{n}^{\wedge} = \left[ {\begin{array}{*{20}{c}}
0&{ - {n_z}}&{{n_y}}\\
{{n_z}}&0&{ - {n_x}}\\
{ - {n_y}}&{{n_x}}&0
\end{array}} \right] $$表示与$\mathbf{n}$对应的一个反对称矩阵,那么有:

\[\begin{equation}
\mathbf{R} (\mathbf{n}, \theta ) = \cos \theta \mathbf{I} + (1-\cos \theta ) \mathbf{n} \mathbf{n}^T + \sin \theta \mathbf{n}^{\wedge} = \exp ( \theta \mathbf{n}^{\wedge} )
\end{equation}\]

  最后那个指数,读者若不理解,可以暂时不管它,这将在之后的李代数中会讲到。根据此式,我们也可以从任意给定的旋转矩阵,求出对应的转轴与转角。关于转角$\theta$,我们对上式两边求矩阵的迹,可得:

\[\begin{equation}
\begin{array}{lll}
tr\left( \mathbf{R} \right) &=& \cos \theta tr\left( \mathbf{I} \right) + \left( {1 - \cos \theta } \right)tr\left( { \mathbf{n} {\mathbf{n}^T}} \right) + \sin \theta tr({\mathbf{n}^ \wedge })\\
&=& 3\cos \theta + (1 - \cos \theta )\\
&=& 1 + 2\cos \theta
\end{array}
\end{equation}\]

  因此:
\[\begin{equation}
\theta = \arccos ( \frac{1}{2} [ tr(\mathbf{R}) - 1 ] )
\end{equation}\]

  关于转轴$\mathbf{n}$,由于旋转轴上的向量在旋转后不发生改变,说明$$\mathbf{R} \mathbf{n} = \mathbf{n}. $$

  因此,只要求此方程的解向量即可。这也说明$\mathbf{n}$是$\mathbf{R}$特征值为$1$的一个特征向量。

  总之,读者应当明白在3D时,旋转和平移仍可用转移矩阵$\mathbf{T}$来描述,其结构也与2D类似。而$\mathbf{T}_{4\times 4}$构成了三维欧氏变换群$SE(3)$。注意到$\mathbf{T}$虽然有16个变量,但真正的自由度只有6个,其中3个旋转,3个位移。

  旋转矩阵描述是一种比较适合数学推导及计算机实现的方式,但它不符合人类的思维方式。当你看到一个$3 \times 3$的矩阵时,很难想象物体实际上发生了怎样的旋转。反之,给定一个旋转方式,人也很难直接写出矩阵的数值。所以,为了便于人类理解,人们还使用了其他方法来表示三维旋转。

欧拉角

  欧拉角是一种广为使用的姿态描述方式,以直观见长。在最常用的欧拉角表达方式中,我们把旋转分解成沿三个轴转动的量:滚转角-俯仰角-偏航角(roll-pitch-yaw)。它的好处是十分的直观,且只有三个参数描述。缺点是会碰到著名的万向锁问题:在俯仰为$\pm 90 ^\circ $时,表达某个姿态的形式不唯一。此外,它也不易于插值和迭代。

  我们并不会详细介绍欧拉角,因为它在SLAM中用处也很有限。我们既不会在数学推导中使用它,也不会在程序中用欧拉角表示机器人的姿态。不过,在您想验证自己算法输出的姿态是否有错时,转换成欧拉角能让你快速辨认结果是否有错。

 四元数

  四元数原理和各种运算将在下一篇博客中提到。


本篇小结

  本篇博客介绍了2D和3D空间中刚体运动的表示方法,以旋转矩阵为主。下一篇我们将介绍四元数表示法,然后演示如何用Eigen3对这些矩阵进行操作。敬请期待。

  如果你觉得我的博客有帮助,可以进行几块钱的小额赞助,帮助我把博客写得更好。

  

posted @ 2016-01-08 16:18 半闲居士 阅读(...) 评论(...) 编辑 收藏