SLAM基本概念

    SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建,或并发建图与定位。问题可以描述为:将一个机器人放入未知环境中的未知位置,是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图,所谓完全的地图(a consistent map)是指不受障碍行进到房间可进入的每个角落。

    整个视觉 SLAM 流程包括以下步骤。

1.jpg

    1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果是在机器人中,还可能有码盘、惯性传感器等信息的读取和同步。

    2. 视觉里程计(Visual Odometry,VO)。视觉里程计的任务是估算相邻图像间相机的运动,以及局部地图的样子。VO 又称为前端(Front End)

    3. 后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在 VO 之后,又称为后端(Back End)

    4. 回环检测(Loop Closing)。回环检测判断机器人是否到达过先前的位置。如果检测到回环,它会把信息提供给后端进行处理。

    5. 建图(Mapping)。它根据估计的轨迹,建立与任务要求对应的地图。


SLAM问题的数学表述

    相机通常是在离散的采集数据的,把一段连续时间的运动变成了离散时刻 t = 1, · · · , K 当中发生的事情。在这些时刻,用 x 表示机器人自身的位置,于是各时刻的位置记为 x1, · · · , xK,它们构成了机器人的轨迹。 地图方面,假设地图是由许多个路标(Landmark)组成的,而每个时刻,传感器会测量到一部分路标点,得到它们的观测数据。不妨设路标点一共有 N 个,用 y1, · · · , yN 表示它们。路标顾名思义就是标志物,即场景中比较有辨识度的物体或者区域。在视觉SLAM中,每一个特征点就是一个landmark。

    这里说的位置,其实是位姿,即位置和姿态。在平面运动中,机器人可用一个二维坐标加一个转角将位置形式化。而三维空间中运动的机器人则可以用相对与世界坐标系的变换矩阵描述(或四元数、旋转向量等)。

    “机器人携带着传感器在环境中运动”由运动和观测描述。运动:要考察从k−1时刻到k时刻,机器人的位置 x 是如何变化的。观测:假设机器人在 k 时刻于xk处探测到了某一个路标yj。使用一个通用的、抽象的数学模型来描述运动,即运动方程

xk=f(xk-1,uk,wk)

    其中xk是机器人或者说传感器的位姿,uk是运动传感器(码盘、惯性传感器)的读数或输入,wk是噪声。噪声的存在使得这个模型变成了随机模型。观测方程:在xk位姿看到某个路标点yj,产生观测数据 zk,jvk,j是观测里的噪声:

zk,j=h(yj,xk,vk,j)


举例

    上面两个方程都是抽象的,可以根据真实的机器人进行参数化。机器人在平面中运动,那么位姿可由坐标和转角描述,uk是两个时间间隔位置和转角的变化量,运动方程具体化为:

2.jpg

    假设有一个二维激光传感器,激光传感器观测一个 2D 路标点时,能够测到两个量:路标点与小萝卜本体之间的距离 r 和夹角 ϕ。上式中,路标点为yj=[y1,y2],机器人位姿为xk=[x1,x2],观测数据为zk,j=[rk,jk,j]。

3.jpg

    而在视觉SLAM中,观测方程就是是“对路标点拍摄后,得到图像中的像素”的过程。

    因此SLAM的通用方程:

4.jpg

    最基本的 SLAM 问题:当知道运动测量的读数 u,以及传感器的读数 z 时,如何求解定位问题(估计 x)和建图问题(估计 y)。这时,我们就把 SLAM 问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏着的状态变量。

    按照运动和观测方程是否为线性,噪声是否服从高斯分布进行分类,分为线性/非线性高斯/非高斯系统。其中线性高斯系统(Linear Gaussian,LG 系统)是最简单的,它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter,KF)给出。而在复杂的非线性非高斯系统(Non-Linear Non-Gaussian,NLNG 系统)中,我们会使用以扩展卡尔曼滤波器(Extended Kalman Filter,EKF)非线性优化两大类方法去求解。


前端

    视觉里程计(VO)也称为前端。VO 根据相邻图像的信息估计出粗略的相机运动,给后端提供较好的初始值。VO 的算法主要分为两个大类:特征点法直接法

    VO 的核心问题是如何根据图像来估计相机运动。从图像中选取比较有代表性的点,这些点在相机视角发生少量变化后会保持不变,于是我们能在各个图像中找到相同的点。然后,在这些点的基础上,讨论相机位姿估计问题,以及这些点的定位问题。在经典 SLAM 模型中,我们称这些点为路标(Landmark)。而在视觉 SLAM 中,路标则是指图像特征点(Feature)。一般采用角点作为特征点:

5.jpg

    角点的提取算法有很多,比较早的有Harris 角点FAST 角点GFTT 角点等等。后来发现单纯的角点不能满足需求,于是人们设计了一些更稳定的特征点,比如SIFTSURFORB等。人工设计的特征点的特点如下:

1.可重复性(Repeatability):相同的特征可以在不同的图像中找到。

2.可区别性(Distinctiveness):不同的特征有不同的表达。

3.高效率(Efficiency):同一图像中,特征点的数量应远小于像素的数量。

4.本地性(Locality):特征仅与一小片图像区域相关。

    特征点关键点(Key-point)和描述子(Descriptor)两部分组成。关键点是指该特征点在图像里的位置,有些特征点还具有朝向、大小等信息。描述子通常是一个向量,按照某种人为设计的方式,描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此,只要两个特征点的描述子在向量空间上的距离相近,就可以认为它们是同样的特征点

    ORB 特征亦由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”,是一种改进的 FAST 角点。它的描述子称为BRIEF(Binary Robust Independent Elementary Feature)

    特征匹配是视觉 SLAM 中极为关键的一步,宽泛地说,特征匹配解决了 SLAM中的数据关联问题(data association),即确定当前看到的路标与之前看到的路标之间的对应关系。考虑两个时刻的图像。如果在图像 It 中提取到特征点 x_m_t, m = 1, 2, ..., M,在图像 It+1 中提取到特征点 x_n_t+1, n =1, 2, ..., N,然后将这两组特征点一一匹配起来。

    最简单的特征匹配方法就是暴力匹配(Brute Force Matcher)。即对每一个特征点 xmt 与所有的 xnt+1 测量描述子的距离,然后排序,取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度,实际运用中可以取不同的距离度量范数。对于浮点类型的描述子,使用欧氏距离进行度量即可。而对于二进制的描述子(比如BRIEF 这样的),往往使用汉明距离(Hamming distance)作为度量:两个二进制串之间的汉明距离,指的是其不同位数的个数。

    当特征点数量很大时,暴力匹配法的运算量将变得很大,特别是当想要匹配某个帧和一张地图的时候。此时快速近似最近邻(FLANN)算法更加适合于匹配点数量极多的情况。

    有了匹配好的点后,接下来,根据点对来估计相机的运动。有下列几种情况:

    1. 当相机为单目时,我们只知道 2D 的像素坐标,因而问题是根据两组 2D 点估计运动。该问题用对极几何来解决。

    2. 当相机为双目、RGB-D 时,或者通过某种方法得到了距离信息,那么问题就是根据两组 3D点估计运动。该问题通常用 ICP 来解决。

    3. 如果一组为 3D,一组为 2D,即,我们得到了一些 3D 点和它们在相机的投影位置,也能估计相机的运动。该问题通过 PnP 求解。

    通过特征点估计相机运动的方法虽然是主流,但是有如下缺点:1.关键点的提取和描述子的计算非常耗时;2.使用特征点时,忽略除特征点以外的所有信息;3.有时会运动到没有纹理的地方,无法计算特征点。解决上述缺点的思路:

    A.保留特征点,但只计算关键点,不计算描述子,使用光流法(Optical Flow)来跟踪特征点的运动。

    B.只计算关键点,不计算描述子,使用直接法(Direct Method)来计算特征点在下一时刻图像中的位置。

    C.不使用特征点,而是根据像素灰度的差异,直接计算相机运动。


后端

    前端能给出一个短时间内的轨迹和地图,但由于不可避免的误差累计,这个地图在长时间内是不准确的。所以在VO的基础上,我们希望构建一个尺度、规模更大的优化问题,以考虑长时间内的最优轨迹和地图。

    待续。。

    









下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于