低成本激光和视觉相结合的同步定位与建图研究

作者尹磊，欧勇盛，江国来，彭建盛

文章目录

低成本激光和视觉相结合的同步定位与建图研究
- 1. 引言
- 2. 主要工作
- 3. 基于图优化的同步定位与地图构建框架
- - 3.1 视觉重投影误差
  - 3.2 激光的误差
- 4. 后端优化与闭环检测
- - 4.1 误差函数
  - 4.2 稀疏姿态调整
  - 4.3 闭环检测
- 5. 实验结果
- - 5.1 实验平台和环境
  - 5.2 小范围场景下进行定点定位精度的对比实验
  - 5.3 闭环实验
  - 5.3 结果分析
- 6. 总结与展望

1. 引言

激光雷达和视觉传感是目前两种主要的服务机器人定位与导航技术，但现有的低成本激光雷达定位精度较低且无法实现大范围闭环检测，而单独采用视觉手段构建的特征地图又不适用于导航应用。

2. 主要工作

本文以配备低成本激光雷达与视觉传感器的室内机器人为研究对象，提出了一种激光和视觉相结合的定位与导航建图方法：通过融合激光点云数据与图像特征点数据，采用基于稀疏姿态调整的优化方法，对机器人位姿进行优化。同时，采用基于视觉特征的词袋模型进行闭环检测，并进一步优化基于激光点云的栅格地图。
真实场景下的实验结果表明，相比于单一的激光或视觉定位建图方法，基于多传感器数据融合的方法定位精度更高，并有效地解决了闭环检测问题。

3. 基于图优化的同步定位与地图构建框架

激光 SLAM 主要通过相邻帧激光的匹配来计算相邻帧位姿变换，但由于激光数据并非完全无噪声的数据，尤其对于低成本激光雷达，打出去的激光点较为稀疏，从而导致计算得到的相邻帧位姿变换存在误差，因此往往需要加入滤波或优化的方法来使定位更加精准。
        视觉 SLAM 是通过特征点提取与匹配来反算位姿，但若其中一帧出现问题就会导致误差累积并不断地增大。
        图优化是 SLAM 后端优化的热门方法，通过构造节点和约束边清晰地展示了一系列位姿和观测量的关系，再通过非线性优化的手段求出最优变量从而得到精准的位姿估计。

        X 表示关键帧位姿，O 表示观测量，包含了特征点所对应的三维空间点坐标及二维激光数据。

3.1 视觉重投影误差

3.2 激光的误差

激光 SLAM 往往需要通过扫描匹配来实现相邻帧的位姿变换估计，而该估计值(R, t)无法保证前一帧所有激光数据经过该位姿变换与后一帧激光数据完全重合。激光的误差定义如下：

通过非线性优化来使误差函数最小化，求得的位姿再返回到前端作为下一帧的参考帧位姿。

4. 后端优化与闭环检测

视觉 SLAM 构建的地图由特征点构成，特征地图最大的问题在于无法用于导航，仅适合用于定位。低成本激光雷达构建的栅格地图更适合用于导航，但存在着激光稀疏、噪声大的问题。文章提出视觉激光融合的方法，在提高定位精度的同时确保地图更加精准，还解决了激光 SLAM 构建的栅格地图难以回环的问题。而二维栅格地图的构建主要依赖于一系列位姿和激光测量数据，根据栅格被占用的概率来增量式的建图。
同一时刻观测到的视觉信息和激光数据并非完全相互独立，文章提出视觉激光联合优化来充分利用数据之间的约束。加入视觉信息的激光视觉联合优化的 SLAM 整体框架如下：

4.1 误差函数

见上节。

将视觉相邻帧误差函数公式以及激光误差函数公式改写成李代数形式并合并，则相邻帧的整体误差函数为：

单独的视觉 SLAM 有其自有的计算相邻帧位姿变换算法，但由于图像数据的特点，计算得到的(R, t)往往没有通过激光扫描匹配得到的位姿变换精准。因此，采用激光扫描匹配得到的位姿变换来估计误差函数中的位姿变换初始值。

4.2 稀疏姿态调整

将机器人位姿作为唯一变量并对其进行优化，视觉观测量和激光点云数据作为位姿之间的约束。
误差函数：

x 为位姿的集合。

加入增量后，目标函数为：

通过加入拉格朗日乘子对误差函数进行改进：

采用 L-M 法来求解上述最小二乘问题。

4.3 闭环检测

闭环检测是 SLAM 中的一个核心问题，通过对曾经走过的地点进行识别，能有效地减小累积误差，提高定位精度。基于激光雷达的 SLAM 算法往往因为数据单一而无法有效地进行闭环检测，而视觉图像丰富的纹理特征则刚好可以弥补激光雷达的这一缺陷。
        文章采用视觉 SLAM 中最常用的词袋模型(Bag-of-Words，BoW)，通过视觉特征来构建关键帧所对应的字典，在检测到回环后通过回环帧与当前帧的匹配来计算当前位姿，并将这一约束加入到后端中来进行回环帧与当前帧之间的全局优化，提高定位精度的同时，也防止了激光所构建的栅格地图常出现的无法闭合的情况。
        关键帧选取机制：
        (1)距离上一次全局重定位已经过去了 15 帧；
         (2)距离上一次插入关键帧已经过去了 15 帧；
        (3)关键帧必须已经跟踪到了至少 50 个三维特征点。
         (1)、(2)是其独特性的基础，因为短时间视野内的特征不会发生明显变化；(3)保证了其鲁棒性，过少的地图点会导致计算误差的不均匀。
         相对而言，激光 SLAM 关键帧的选取要更为简单且稳定，但采用激光作为关键帧选取参量容易出现关键帧包含的图片特征点不够、图像不连续等问题。文章采用基于视觉的关键帧选取机制，既保证相邻关键帧不会过于接近，又保证有足够的信息进行关键帧之间的匹配。

5. 实验结果

5.1 实验平台和环境

本文实验在 Turtlebot2 上进行，配备笔记本电脑以及激光雷达和深度摄像头，电脑配置为 Intel Core i5 处理器、8 G 内存，运行 ubuntu14.04＋ ROS Indigo 系统。二维激光雷达采用单线激光雷达 RPLIDAR A2，人为设置采样频率为 5～15 Hz，测量半径为 8 m。深度相机采用奥比中光公司出品的 Astra 深度相机。其中，该相机深度测距的有效范围为 0.6～8 m，精度达 3 mm；深度相机视角可以达到水平 58°和垂直 45.5°。

5.2 小范围场景下进行定点定位精度的对比实验

5.3 闭环实验

5.3 结果分析

本文提出的方法可以有效地检测回环，并优化两点之间的所有位姿，位姿优化之后再根据每一帧位姿所携带的激光数据重新构建地图，成功消除了无法闭合的地图部分。但由于加入了视觉信息，建图过程中对机器人速度限制变大，同时对动态障碍物的抗干扰能力变小，只能在无人时进行建图，且需要避免摄像头面向大面积白墙等无特征物体。

6. 总结与展望

文章研究了 SLAM 中多传感器融合的问题，针对低成本激光雷达噪声大、视觉构建的稀疏特征地图不适用于导航的问题，提出了激光视觉相结合定位建图的方案。通过采用基于稀疏姿态调整的方法，将激光数据和图像信息进行联合优化，有效地提高了定位和建图的精度，并通过视觉词袋模型实现闭环检测，解决了激光闭环检测难实现的问题。

但由于加入了视觉信息，系统鲁棒性变差，且室内环境光照变化往往较小，同时存在大量的动态障碍物，如摄像头捕捉到移动的行人时会导致视觉约束的错误，从而影响整体的定位建图效果。同时，当遇到白墙等物体时无法提取足够的特征，视觉约束失效从而导致建图失败。

下一步工作将利用激光雷达和视觉各自的优点，提高整体的鲁棒性，保证建图的效果。