﻿ 相似场景下基于局部地图的激光SLAM前端算法改进
 机器人 2022, Vol. 44 Issue (2): 176-185 0

XU Yuwei, YAN Wenxu, WU Wei. Improvement of LiDAR SLAM Front-end Algorithm Based on Local Mapin Similar Scenes[J]. ROBOT, 2022, 44(2): 176-185.

Improvement of LiDAR SLAM Front-end Algorithm Based on Local Mapin Similar Scenes
XU Yuwei , YAN Wenxu , WU Wei
School of Internet of Things Engineering, Jiangnan University, Wuxi 214122, China
Abstract: In the corridor, tunnel and other similar scenes, performances of traditional LiDAR SLAM (simultaneous localization and mapping) algorithms will seriously degrade, and the algorithms might even be completely invalid due to the similarity of observation data.To solve this problem, the motion prediction model is improved firstly with the hdl_graph_slam algorithm based on the assumption of uniform motion to obtain a more accurate initial pose estimation.Then, the concept of local map is introduced to densify the point cloud, and the performances of the front-end odometer are improved in the similar scenes.In the indoor experiment, the average restoration rate of the scene reaches 99.54%, which is 57.25% higher than that before improvement.In the outdoor experiment, the odometer drift is reduced from 111.62 m to 7.65 m by the improved algorithm.The experimental results show that the proposed algorithm can bring a significant performance improvement in both indoor and outdoor similar scenes.
Keywords: similar scene    local map    SLAM (simultaneous localization and mapping)    front-end algorithm

1 引言（Introduction）

2 通用符号定义（Definition of universal symbols）

 $$$_{A}^{C} {{{\mathit{\boldsymbol{T}}}}}={}_{B}^{C} {{{\mathit{\boldsymbol{T}}}}} {} _{A}^{B} {{{\mathit{\boldsymbol{T}}}}}$$$ (3)
3 前端算法改进（Improvements on the front-end algorithm） 3.1 激光SLAM整体框架

SLAM算法框架图如图 1所示，分为点云预处理、前端里程计、地板检测、后端图优化和建图5个部分（可参阅文[8]了解更多算法细节）。其中，前端里程计部分负责利用NDT算法进行点云配准，估计每一帧点云在全局坐标系中的位姿，后端图优化节点则对所有的估计位姿进行批量优化，提高位姿轨迹的全局一致性。由于前端里程计的精度直接决定了整个SLAM算法的定位和建图效果，因此本文主要讨论如何对前端里程计算法进行改进，以克服相似场景下的SLAM性能退化问题。

 图 1 SLAM算法框架 Fig.1 Framework of the SLAM algorithm
3.2 问题分析

(1) 将目标点云$S_{1}$栅格化，并计算每个栅格的正态分布参数；

(2) 利用给定的位姿，将源点云$S_{2}$转换到目标点云坐标系${\rm \{}S_{1} {\rm \}}$，得到$S_{2} '$

(3) 计算$S_{2} '$中每个点落入$S_{1}$中对应栅格的概率，并累加所有的概率，得到评价得分$\sigma$

(4) 基于牛顿迭代法来优化$\sigma$，得到新的估计位姿；

(5) 回到步骤(2)，直至达到收敛判据。

 图 2 走廊原始点云 Fig.2 The raw point cloud of the corridor

 图 3 失效的NDT算法 Fig.3 The failed NDT algorithm

 图 4 改进后的前端算法 Fig.4 The improved front-end algorithm
3.3 运动预测模型的改进

 $$$\begin{cases} \Delta x_{i} =\|_{S_{i}} ^{K}{{\tau}} \|_{2} \\ \Delta \theta_{i} =\arccos \dfrac{{\rm tr} {}_{S_{i}}^{K} {{{\mathit{\boldsymbol{R}}}}}}{2}\\ \end{cases}$$$ (4)

 \begin{align} \Delta x_{i} &\geqslant \Delta x_{{\rm th}} \end{align} (5)
 \begin{align} \Delta \theta_{i} &\geqslant \Delta \theta_{{\rm th}} \end{align} (6)

 \begin{aligned} \boldsymbol{T}_{\text {guess }} &={ }_{S_{i-1}}^{K} \boldsymbol{T}_{S_{i}}^{S_{i-1}} \boldsymbol{T} \\ & \approx{ }_{{S_{i - 1}}}^K{\mathit{\boldsymbol{T}}}_{S_{i-1}}^{S_{i-2}} \boldsymbol{T} \\ &={ }_{{S_{i - 1}}}^K{\mathit{\boldsymbol{T}}}_{S_{i-2}}^K{\boldsymbol{T}}^{-1}{ }_{S_{i-1}}^{K} \boldsymbol{T} \end{aligned} (7)
 图 5 运动预测模型 Fig.5 Motion prediction model

3.4 局部地图的构建

LOAM[6]算法的里程计线程将距离当前帧坐标原点一定范围内的特征点转换至当前帧坐标系下，形成局部特征地图参与最近点迭代匹配，这使得激光里程计可以使用更广更密的几何特征进行位姿估计，从而获得更高的配准精度。该思想在SLAM中亦被称为scan-to-map。受其启发，在前端里程计中引入局部地图概念，将邻近的关键帧均投影至同一个坐标系中，拼接成局部点云地图参与NDT配准。其作用主要有3点：

(1) 邻近关键帧拼接后，则历史关键帧中所包含的几何信息也全部被转换到了局部点云地图中，这使得NDT算法可以获得更广范围的几何特征，丰富了相似场景中的几何信息。

(2) 拼接成局部地图，点云密度显著上升。由于目标点云栅格内的激光点数量增加，根据概率学知识可知，此时计算得到的正态分布参数（均值与方差）更能反映真实的环境特征，有利于NDT算法的精确配准。这一点对于低线束激光雷达的作用尤为显著。

(3) 邻近关键帧拼接后，形如墙壁、隧道壁之类的连续立面会作为整体参与点云配准，相比于原始的帧帧匹配方式，可以提高立面的局部一致性，减少里程计在垂直于立面方向的漂移。

4 实验与分析（Experiments and analyses）

 图 6 UGV照片 Fig.6 Photo of the UGV

(1) 里程计漂移：当行程的起点与终点为同一点时，起点估计位姿和终点估计位姿之间的欧氏距离$d_{{\rm bias}}$表征了里程计漂移性能。规定起点位姿为$(0, 0, 0)$，记终点估计位姿为$(x, y, z)$，则$d_{{\rm bias}}$计算方式为

 $$$d_{{\rm bias}} =\sqrt{x^{2}+y^{2}+z^{2}}$$$ (9)

(2) 场景还原度：考虑到里程计漂移存在互相抵消的情况，本文额外引入场景还原度$\gamma$，其计算方式为

 $$$\gamma =\frac{l_{{\rm m}}} {l_{{\rm t}}} \times 100\%$$$ (10)

(3) 配准耗时：前端里程计进行1次点云配准的平均耗时。

4.1 室内相似场景下SLAM算法性能测试 4.1.1 实验说明

 图 7 走廊局部放大图 Fig.7 A partial enlargement of the corridor
 图 8 室内相似场景下的SLAM结果 Fig.8 The SLAM results in indoor similar scenes

4.1.2 实验结果及分析

3个算法最后建成的点云地图如图 8所示。其中，图 8(a)(c)(e)为无回环检测的建图结果，从中可以测出里程计漂移；图 8(b)(d)(f)为引入回环检测（实验中简记为“$+$Loop”）后的建图结果，全局一致性得到提升，从中可以评价算法的场景还原度。图中走廊上的圆点表示关键帧位置，红色线条表示前端算法输出的里程计信息，半透明红色圆球表示回环检测的范围。需要补充的是，图 8(b)(d)中矩形房间没有对齐的原因在于NDT与GICP算法的里程累积误差较大，在同样的回环检测参数下，无法形成准确的回环，形成了回环重影。相比之下，Local算法的回环一致性显然更好。

 图 9 放大视图对比 Fig.9 Comparison in the zoom-in view

3种算法的量化指标如表 1所示。结合图 8表 1可以看出，NDT算法在相似场景和准相似场景下建图效果极差，走廊1和走廊3的平均场景还原度仅为42.02%。GICP算法相比于NDT算法既减少了里程计漂移，又将走廊1和3的平均场景还原度提升至93.03%，但其耗时几乎是原先的3倍。从细节来看，虽然GICP算法在走廊1的场景还原度达到92.44%，但仍会导致地图产生重叠，影响后续导航环节的展开，而Local算法则几乎没有重叠现象。从数据来看，Local算法的里程计漂移在三者中最低，场景还原度最高，耗时则介于NDT与GICP算法之间。

4.2 室外相似场景下SLAM算法性能测试

 图 10 实际轨迹俯瞰图 Fig.10 Aerial view of the actual trajectory

 图 11 室外相似场景 Fig.11 The outdoor similar scene

 图 15 不同尺寸局部地图实验 Fig.15 Experiments with local maps of different dimensions

5 结论（Conclusion）

 [1] 危双丰, 庞帆, 刘振彬, 等. 基于激光雷达的同时定位与地图构建方法综述[J]. 计算机应用研究, 2020, 37(2): 327-332. Wei S F, Pang F, Liu Z B, et al. Survey of LiDAR-based SLAM algorithm[J]. Application Research of Computers, 2020, 37(2): 327-332. [2] Kohlbrecher S, von Stryk O, Meyer J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics. Piscataway, USA: IEEE, 2011: 155-160. [3] Olson E B. Real-time correlative scan matching[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2009. DOI: 10.1109/ROBOT.2009.5152375. [4] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256. DOI:10.1109/34.121791 [5] Biber P, Strasser W. The normal distributions transform: A new approach to laser scan matching[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2003: 2743-2748. [6] Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robot, 2017, 41: 401-416. DOI:10.1007/s10514-016-9548-2 [7] Shan T X, Englot B. LeGO-LOAM: Lightweight and groundoptimized lidar odometry and mapping on variable terrain[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2018: 4758-4765. [8] Koide K, Miura J, Menegatti E. A portable three-dimensional LIDAR-based system for long-term and wide-area people behavior measurement[J]. International Journal of Advanced Robotic Systems, 2019, 16(2). DOI:10.1177/1729881419841532 [9] 方正, 赵世博, 李昊来. 一种融合稀疏几何特征与深度流的深度视觉SLAM算法[J]. 机器人, 2019, 41(2): 185-196, 241. Fang Z, Zhao S B, Li H L. A depth vision SLAM algorithm combining sparse geometric features with range flow[J]. Robot, 2019, 41(2): 185-196, 241. [10] Ren Z L, Wang L G, Bi L. Robust GICP-based 3D LiDAR SLAM for underground mining environment[J]. Sensors, 2019, 19(13). DOI:10.3390/s19132915 [11] Segal A V, Haehnel D, Thrun S. Generalized-ICP[C]//Robotics: Science and Systems V. Seattle, USA: University of Washington, 2009. DOI: 10.15607/RSS.2009.V.021. [12] 邹谦. 基于图优化SLAM的移动机器人导航方法研究[D]. 哈尔滨: 哈尔滨工业大学, 2016. Zou Q. Research on mobile robot navigation method based on graph optimization SLAM[D]. Harbin: Harbin Institute of Technology, 2016. [13] 陈林. 面向隧道内自主侦察无人机系统设计[D]. 绵阳: 西南科技大学, 2019. Chen L. Design of autonomous investigation UAV system for tunnel[D]. Mianyang: Southwest University of Science and Technology, 2019. [14] Lv J J, Xu J H, Hu K W, et al. Targetless calibration of LiDAR-IMU system based on continuous-time batch estimation[DB/OL]. (2020-07-29)[2020-10-15]. https://arxiv.org/abs/2007.14759. [15] Stanford Artificial Intelligence Laboratory. Robotic operating system[EB/OL].[2020-11-29]. https://www.ros.org.