视觉SLAM十四讲阅读笔记二-SLAM问题的数学表述与经典框架


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

SLAM问题提出

SLAM问题可以描述为:机器人在某个未知环境中从某个未知位置开始移动,在移动过程中根据传感器数据进行自身定位估计,同时在自身定位的基础上增量式构造地图,从而实现机器人对未知环境的地图构建和在地图中对自身的位置进行定位。对于机器人来说,SLAM主要回答了两个问题:1)我在什么地方?(定位) 2)周围环境是怎么样的?(建图),即机器人一方面要明白自身的状态(即位置),另一方面也要了解外在的环境(即地图)。SLAM问题的本质:对运动主体自身和周围环境空间不确定性的估计。

SLAM问题的数学表述

SLAM要回答这两个问题,机器人需要通过传感器采集数据,然后根据这些数据推断出自身的位姿信息和所处的环境信息。

携带传感器的机器人在某未知环境中运动,由于相机通常是在某些时刻采集数据的,所以我们只关心这些时刻的位置和地图。这样就把一段连续时间的运动变成离散时刻$t=1,…,K$当中发生的事情。在这些时刻,用$x$表示小萝卜自身的位置。于是各时刻的位置就记为$x_1,…,x_K$,它们构成了机器人的轨迹。

地图方面,假设地图由许多个路标(landmark)组成,每个时刻传感器会测量到一部分路标点,设路标点一共有$N$个,用$y_1,…y_N$表示。机器人在某个时间内通过传感器获得一系列连续的传感器数据(路标点的观测数据),即观测值$z$。依据机器人所配备的传感器,观测值$z$可以是激光雷达(laser)数据、图像数据、里程计(odometer)数据和惯性导航单元(IMU)数据等。

运动方程

SLAM需要解决的就是通过观测值$z$评估系统状态$x$。系统状态通常包含两部分,一部分用于表示机器人在环境中的位置,另一部分用于表示环境地图。通常,机器人会携带一个测量自身运动的传感器,比如里程计。因此可以构造一个评估函数,利用当前获得的传感器数据(不一定直接就是位置之差,还可能是加速度、角速度等信息),从前一时刻的系统状态评估当前时刻的系统状态,通用、抽象的数学模型如下所示:

$xk=f(x{k-1},u_k,\omega_k)$

$u_k$是运动传感器的数据(或称为输入),$w_k$为噪声。这里的$f$指代一种计算模型,当输入的运动传感器类型不同时,$f$的具体形式会千差万别。我们通常把它称为运动方程

观测方程

与运动方程相对应是观测方程。观测方程描述的是当机器人在$xk$位置上利用传感器感知环境,看到了某个路标点$y_j$,产生了观测数据$z{k,j}$。此处同样用一个抽象的函数$h$来表述这个关系:

$z{k,j}=h(y_j,x_k,\upsilon{k,j})$

这里$v_k$表示此次观测的噪声。由于观测所用的传感器形式更多,因此这里的观测数据$z$以及观测方程$h$也有许多不同的形式。

参数化过程

对于上述函数$f$、$h$,我们并没有给出具体的形式,没有具体地说明运动和观测是怎么回事,也不知道$x$、$y$、$z$如何表示的。其实根据机器人的真实运动和传感器的种类,存在着若干种参数化(Parameterization)方式。

举例来说,假设机器人在平面运动,其位姿由两个位置和一个转角来描述,即$x_k=[x,y,\theta]^T_k$。同时,运动传感器能够测量到机器人在任意两个时间间隔位置和转角的变化量$u_k=[\Delta x,\Delta y,\Delta \theta]^T_k$,所以运动方程就可以具体化为:

$\left[ \begin{matrix}x\y\\theta\end{matrix} \right]k=\left[ \begin{matrix}x\y\\theta\end{matrix} \right]{k-1}+\left[ \begin{matrix}\Delta x\\Delta y\\Delta \theta\end{matrix} \right]_k+\omega_k$

关于观测方程,机器人携带一个二维激光传感器,在激光传感器观测一个2D路标点时,能够测到两个量:路标点和机器人本体之间的距离$r$和夹角$\phi$,所以观测方程具体化为:

$\left[ \begin{matrix}r\\phi\end{matrix} \right]=\left[ \begin{matrix}\sqrt{(p_x-x)^2+(p_y-y)^2}\ arctan(\frac{p_y-y}{p_x-x})\end{matrix} \right]+\upsilon$

在视觉SLAM中,传感器是相机,那么观测方程就是“对路标点拍摄后,得到图像中的像素”的过程。该过程牵扯到相机模型的描述,在后续的阅读中再详细记录。

最基本的SLAM问题

综上所述,针对不同的传感器,运动和观测方程会有不同的参数化形式。如果保持它们的通用性,取成通用的抽象形式,那么SLAM过程就可以表述为两个基本方程:

$\left{ \begin{array}{ll} xk=f(x{k-1},uk,\omega_k)\ z{k,j}=h(yj,x_k,\upsilon{k,j})\end{array} \right.$

这两个方程描述了最基本的SLAM问题:当知道运动测量的读数$u$,以及传感器的读数$z$时,如果求解定位问题(估计$x$)h和建图问题(估计$y$)。即其中$z$和$u$是已知的,当选定了传感器,运动方程和观测方程也是已知的,因此SLAM求解的目标就是对$x$和$y$进行估计,使得运动方程观测方程等式两边尽可能的成立。这时,就把SLAM问题建模为一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏的状态变量。

疑问:观测方程和运动方程会同时用到吗?视觉SLAM只用到观测方程???

视觉SLAM中:

  • 观测方程。就是“对路标点拍摄后,得到图像中的像素”的过程。具体来说,根据每个时刻的相机位置,计算出各像素对应的空间点的位置,就得到了地图。
  • 运动方程。SLAM系统中视觉里程计(VO)估计了两张图像间相机的运动,只需把相邻时刻的运动“串”起来,就构成了机器人的运动轨迹,从而解决了定位问题。

后续深入理解

本篇文章对SLAM问题的数学建模有了大致的了解,然而仍需深入理解一些问题。

  1. 如何说明机器人的位置$x$是什么,即如何表达位姿。对于三维空间中的机器人来说,其运动要由3个轴上的平移,以及绕着3个轴的旋转来描述,一共有6个自由度。那是否意味着随便用一个$ℝ^6$中的向量就可以描述它呢?其实并没那么简单。对6自由度的位姿(包括了旋转和平移),如何表达的问题涉及到三维空间刚体运动的问题,会在其他文章中详细介绍;
  2. 如何对位姿进行估计和优化。因为在SLAM中位姿是未知的,而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题,一种经典的解决方式是把它们构成一个优化问题,求解最优的$R$和$t$,使得误差最小化。而旋转矩阵自身是带有约束的(正交且行列式为1).它们作为优化变量时,会引入额外的约束,使优化变得困难。通过李群-李代数间的转换关系,我们希望把位姿估计变成无约束的优化问题,以简化求解方式。涉及到李群和李代数会在后续记录;
  3. 视觉SLAM中观测方程如何参数化。即空间中的路标点是如何投影到一张照片上的,这就需要解释相机的成像模型;
  4. 如何求解运动、观测方程。需要用到非线性优化的知识。

SLAM问题的求解思路

从应用的角度来看,SLAM问题涉及很多方面,包括传感器的选择,对$x$的估计方式,地图的表示形式等。
从传感器的角度来讲,SLAM依赖的传感器主要包括激光传感器和视觉传感器两大类。
从X的估计方式来讲,SLAM的求解思路主要包括基于滤波的求解和基于非线性优化的求解。目前普遍认为非线性优化的方法要由于滤波方法。
从地图的表示形式来讲,SLAM得到的地图表示方式主要分为度量地图与拓扑地图,度量地图有二维,三维,稀疏,稠密等多种表现形式。

视觉SLAM经典框架

  1. 传感器获取。在视觉SLAM中主要为相机图像信息的读取和预处理。
  2. 视觉里程计(Visual Odometry,VO)。任务是估算相邻图像间相机的运动,以及局部地图的样子,(恢复场景的空间结构)。又称前端(Front End)。为了定量地估计相机运动,必须在了解相机与空间点的几何关系后进行。视觉里程计估计了两张图像间的相机运动之后,只要把相邻时刻的运动“串”起来,就构成了机器人的运动轨迹,从而解决了定位问题。另一方面,根据每个时刻的相机位置,计算出各像素对应的空间点的位置,就得到了地图。仅仅通过视觉里程计估计轨迹,将不可避免地出现累计漂移(Accumulatin Drift),将导致无法建立一致的地图。为了解决漂移问题,需要用到后端优化回环检测,回环检测负责把“机器人回到原始位置”的事情检测出来,后端优化则根据该信息,校正整个轨迹的形状。
  3. 后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在VO之后,又称为后端(Back End)。笼统地说,它主要是指处理SLAM过程中噪声的问题,即考虑如何从带有噪声的数据中,估计整个系统的状态,以及这个状态估计的不确定性有多大——称为最大后验概率估计(Maximum-a-Posteriori,MAP),这里的状态既包括机器人自身的轨迹,也包括地图。前端为后端提供待优化的数据,以及这些数据的初始值。而后端负责整体的优化过程,往往面对的只有数据,不必关心这些数据到底来自什么传感器。视觉SLAM中,前端和计算机视觉研究领域更为相关,比如图像的特征提取和匹配等,后端则主要是滤波与非线性优化算法。
  4. 回环检测(Loop Closing)。回环检测判断机器人是否曾经到达过先前的位置。如果检测到回环,它会把信息提供给后端处理。又称闭环检测(Loop Closure Detection),主要解决位置估计随时间漂移的问题。为了实现回环检测,需要让机器人具有识别曾到达过的场景的能力,如果通过机器人通过相机获取的图像来完成这一任务,就可以采取判断图像间相似性的方法。视觉回环检测实质上是一种计算图像数据相似性的算法。
  5. 建图(Mapping)。它根据估计的轨迹,建立与任务要求对应的地图。建图并没有一个固定的形式和算法,例如地图可以是一组空间点的集合,也可以是一个漂亮的3D模型。大体上分为度量地图和拓扑地图两种,前者更精确,后者则更强调地图元素之间的关系。拓扑地图是一个图,由节点和边组成,只考虑节点间的连通性,而不考虑节点间到达的过程,放松了地图对精确位置的需要,去掉了地图的细节问题,更为紧凑,但不擅长表达具有复杂结构的地图。

有待研究问题及主要挑战

  1. 拓扑地图不擅长表达具有复杂结构的地图,如何对地图进行分割形成节点和边,又如何使用拓扑地图进行导航和路径规划,有待研究。
  2. 视觉SLAM中,产业化过程中仍需要解决一些关键性难题,比如,如何高效地获得尽可能长而且准确的特征轨迹并将多视频序列之间的复杂回路闭合起来, 如何对于海量图像/视频数据在有限的内存下进行高效的全局优化,如何在动态环境下进行鲁棒的同时定位与地图构建, 如何处理相机快速运动和强旋转, 如何在线动态调整重建的三维几何表面。
  3. 如何对回环序列或多序列进行处理,如何高效率、高精度处理大尺度环境,如何处理动态环境,如何处理快速移动和剧烈旋转的情况?
  4. Covisibility 是一直在用的概念,而Essential Graph是orbslam自己提出的概念,为了减小全局回环的计算量。当你自己实现SLAM时,也会碰到这些困难,并设计一些应对的策略,这些就是你的创新性。事实上,随着SLAM时间的增长,如何控制图的结构和优化的规模,仍是现在SLAM有待解决的一个问题。

研究热点

  1. 在复杂场景中的三维视觉感知问题,主要包含三个方面:一是如何通过视觉和 IMU 融合,进行滑动窗口内的 Bundle Adjustment,来提高运动估计的准确性、稳定性和鲁棒性;二是如何基于深度学习的方法仅用单目相机来构建稠密的 3D 地图,并用于飞控避障、 AR 虚实融合等;三是如何基于单目视觉惯导融合,来实时跟踪动态物体,并恢复物体的绝对物理尺度。
  2. 语义SLAM、激光SLAM
  3. 深度学习与SLAM结合,与新的传感器进行融合
  4. 应用领域:自动驾驶、AR、VR、无人机、无人车、扫地机器人

热门公司和部门

  1. 商汤科技
  2. Momenta
  3. 镭神智能
  4. 图森未来
  5. 纵目科技
  6. 地平线
  7. 旷视科技
  8. 阿里巴巴达摩院AI Labs

参考资料

  1. 《视觉SLAM十四讲》第2讲
  2. 一索哥传奇《SLAM的基本概念》

本文标题:视觉SLAM十四讲阅读笔记二-SLAM问题的数学表述与经典框架

文章作者:阿翔

发布时间:2018年08月05日 - 17:08

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

原始链接:http://ttshun.com/2018/08/05/视觉SLAM十四讲阅读笔记二-SLAM的数学表述/

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

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