一种基于激光雷达的同步定位建图与自主导航算法研究

SLAM and Autonomous Navigation Algorithm Based on 2D-Lidar

摘要: 同步定位与建图(Simultaneous Localization and Mapping, SLAM)是自动驾驶领域的关键技术之一。基于差异化的应用场景、传感器和算法，主流SLAM实现方法分为视觉SLAM、激光SLAM和多传感器SLAM等，其性能的优化方向主要基于传感器性能，算法策略以及估计方法。室内场景下，视觉SLAM方法因视觉传感器的视野范围有限且对光线明暗，性能受限于环境变化。多线激光雷达因价格昂贵，不适宜广泛应用。多传感器融合的方案，因部分视觉传感器采集环境特征信息丰富而存在系统运行的时延问题且融合设计复杂度高。针对以上问题，本文提出了一种基于单线激光雷达，感知算法与全局路径规划算法融合的SLAM与自主导航方案，设计软件与硬件结合的系统框架，所实现的运动系统在室内场景下高速获取环境信息，能够获取良好的建图性能并完成低时延自主导航。首先，本文系统利用IMU对单线激光雷达采集的回波数据做畸变矫正，对激光雷达数据采用Gmapping算法处理，并对各传感器的坐标系关系做精确计算和转换，而后采用Djikstra算法作为全局路径规划算法来实现运动系统的自主导航。最终通过实验，利用ROS系统，NVIDIA-Jetson-Nano，IMU、2D-Lidar等，在实际环境中实现了算法模型与系统构建，室内场景中建图效果良好，机器车在同步定位与建图过程中自主导航，验证了本文方案及系统的可靠性与有效性。

Abstract: Simultaneous Localization and Mapping (SLAM) is a key technology in autonomous driving. Based on different applications, hardware sensors and algorithms, the SLAM implementation methods are mostly divided into Visual SLAM, Laser SLAM and multi-sensor SLAM. The optimization direction of SLAM performance is mainly based on sensor performance, algorithm and estimation method. In the indoor scene, the performance of the Visual SLAM method is limited by the environment due to the limited field of vision sensor and its sensitivity to light. 3D-Lidar is not suitable for wide application for the high price. The multi-sensor fusion scheme has the problem of the complexity of fusion design and has operation delay due to the rich environmental characteristic information collected by some vision sensors. To solve the problems above, this paper proposes a SLAM scheme based on 2D-Lidar, the fusion of sensing algorithm and global path algorithm, and designs a system framework combining software and hardware. The realized system can obtain environmental information at high speed in indoor scenes and complete low-delay autonomous navigation with good mapping performance. First, the system uses IMU to correct the distortion of the Lidar data which is processed by the Gmapping algorithm. The system realized in this paper performs accurate calculation and conversion of the coordinate system relations of each sensor. After that, the Djikstra algorithm is adopted as the global path planning algorithm. Finally, ROS systems, NVIDIA-Jetson-Nano, IMU, 2D-Lidar, etc. are used to realize the construction of the algorithm model and system in the actual environment. The diagram construction effect in the indoor scene is good, the complete map of the experimental environment is obtained and the motion system positioning is accurate, which verifies the reliability and effectiveness of the scheme and system in this paper.

1. 引言

在智能驾驶中，对环境的感知、自身定位及路径规划是十分重要的，同步定位与建图(Simultaneous Localization and Mapping, SLAM)是自动驾驶领域的关键技术，能够满足导航、避障、路径规划需求。SLAM问题由Smith和Cheeseman提出 [1] ，现已发展出了视觉、激光及多种传感器作用或融合的方案。相较于GPS定位，SLAM能够对动态环境实时感知和决策，区别于GPS的静态地图，SLAM的建图过程具有主动及无源的特性。SLAM技术使系统在运行过程中(如无人驾驶汽车的道路行驶)对各时刻的自身位姿及环境物体进行估计并同步完成自主定位、建图和路径规划，全面实现对环境中的随机障碍物和路径的精确推算，现已广泛应用于移动机器人、自动驾驶、环境感知等领域，相关技术也在不断更新和发展。

SLAM方法的分类主要基于感知环境信息所使用的传感器类别，目前主要有基于视觉传感器的视觉SLAM、基于激光雷达的激光SLAM和多传感器融合SLAM等，其中涉及的内部和外部传感器主要包括单线激光雷达、多线激光雷达、单目相机、立体相机、RGB-D相机、IMU、里程计等。更详细地，SLAM方案中的视觉SLAM方法主要分为基于特征提取的SLAM及直接法SLAM，算法包括ORB-SLAM、LSD-SLAM、MonoSLAM、RGBD-SLAM等 [2] ；激光雷达SLAM算法(包括单线和多线激光雷达)主要有EKF SLAM，Fast SLAM，GMapping，Optimal，RBPF，Karto SLAM，Hector SLAM和Cartographer等 [3] ，应用更为广泛；多传感器融合方法有LVI-SLAM，SuperOdometry等 [4] 。目前，SLAM方案仍在优化中，并呈现出多传感器融合的趋势，以使建图信息更为精确。然而多传感器将视觉信息融合至激光雷达的点云信息虽能达到相比单一传感器更丰富、准确的建图效果，但在多数情况下需要考虑传感器配合产生的信息互扰、时延增加、硬件及调试成本增大等问题，导航实时性和可靠性上相比激光SLAM存在劣势。

针对上述问题，本文对SLAM模型、现有的SLAM技术及相关传感器、机器人路径规划算法、激光SLAM方案等进行了较为全面的研究和概述，创新性地提出一套硬件与软件结合的解决方案，基于单线激光雷达和必要的移动机器人硬件结构，采用Gmapping算法 [5] 和Dijkstra [6] 全局路径规划算法，实现了室内场景下的同步定位与建图效果，获得了完善的激光雷达成像、SLAM栅格地图 [7] 构建，搭载了激光雷达的系统实现实时的自主路径规划，能够对静态或动态的障碍物体识别感知并做出实时移动与避障。

2. 单线激光雷达的信息感知原理

2.1. 激光雷达数据采集过程

激光雷达(Lidar)系统的工作原理是在环境中自转扫描并发射探测信号，将目标物体反射的雷达信号(回波)与发射信号进行比较、计算，可获取目标的相关信息。在SLAM中应用的激光雷达传感器具有精度高、方向性强的优势，但也同时存在成本高、信息类型有限的弊端，类别依据感知范围主要分为单线激光雷达(2D-Lidar)和多线激光雷达(3D-Lidar)两类。

单线激光雷达，即二维激光雷达(2D-Lidar)，可以探测单一平面(激光雷达窗口高度)上的障碍物，适用于飞机移动汽车或机器人的独立定位、导航和绘图。多线激光雷达，即三维激光雷达(3D-Lidar)，能够以高精度收集坐标、角度、距离等点云信息。它可以实时构建地图，并按比例恢复检测目标。目前，三维激光雷达被广泛应用于智能驾驶和户外环境中，其弊端为：成本较高，难以穿透雨水、灰尘等，易受天气影响。相比之下，单线激光雷达点云信息的数据处理和计算过程比图像处理更简单，与多线激光雷达对比又具备成本较低的优势，更适合于室内场景的应用。

基于激光雷达的数据采集过程如图1所示，雷达旋转扫描周围环境，并持续发射信号和接收回波，获取周围环境物体信息。本文利用似然场数学模型，将雷达数据发至下位机处理及计算，最终将处理数据返回上位机进行收集及应用。

单线激光雷达的数据获取与处理步骤为：

(1) 发射激光束：单线激光雷达发射窄而密集的激光束扫描环境，在水平位置旋转扫描。

(2) 接收激光返回：激光束与物体相交时，会被散射或反射，最终返回到激光雷达，激光雷达的接收器会记录返回的激光强度和到达时间。

(3) 计算距离和位置：通过测量激光返回的时间，计算出激光束与物体之间的距离。在雷达持续旋转、扫描、发射与回波接收的过程中，获取物体在激光雷达坐标系上的位置。

(4) 构建点云：通过连续测量激光束与环境的交互，获取一系列距离和位置数据点，由数据点构建点云信息，表示环境中物体的位置和物理形态。

(5) 数据处理：获取到点云数据后，对数据做进一步处理和分析，常规处理操作有滤波(去除噪声和异常点)，分割(将点云分成不同的物体或地面)，配准(多个点云对齐)，特征提取(提取物体的特定特征)等。

(6) 应用和可视化：处理后的点云数据可用于各类应用，如环境建模、障碍物检测、路径规划等，也可使用可视化工具将点云数据呈现为二维或三维场景，以便观测与分析。

Figure 1. Lidar data processing flow

图1. 激光雷达数据采集过程

2.2. 计算原理与畸变预处理

激光雷达数据扫描模型如图2所示，获取探测物体的计算原理如下。

Figure 2. Lidar scanning model

图2. 激光雷达扫描模型

图2中，激光雷达坐标系为 $\left(\theta ,d\right)$ ，设途中被测物体所处位置为 $\left(x,y\right)$ ，则被测物体向激光雷达坐标系转换的转换公式为：

$\{\begin{array}{c}x=d\sin \theta \\ y=d\cos \theta \end{array}$ (1)

观测模型的条件概率表达式为：

$p\left(z_t|x_t,m\right)$ (2)

其中，m表示地图信息，z_t和x_t分别表示在时刻t时的激光雷达观测数据和移动系统的位姿，

激光雷达自转而获取的多次采样的环境观测为：

$z_t=\left\{z_t^1,z_t^2,z_t^3,\cdots \right\}$ (3)

激光雷达每次扫描的数据获取相互独立，观测模型表达式为：

$p\left(z_t|x_t,m\right)=\underset{i}{{\displaystyle \prod }}p\left(z_t^i|x_t,m\right)$ (4)

激光雷达三角测距 [8] 的原理图如图3所示，由两个发射端点位及其与被测物体夹角计算得到距离 ，计算公式如下：

$d=L\frac{\sin \alpha \sin \beta }{\sin \left(\alpha +\beta \right)}$ (5)

Figure 3. Trigonometric ranging principle

图3. 激光雷达三角测距原理

二维激光雷达在水平视角下周旋扫描并获取回波信息，此过程默认系统为同一基准位姿，为校正5.5 Hz扫描频率下激光雷达帧数据的时间差造成的点云畸变，本系统采用IMU角速度积分后对雷达位姿线性插值以去除畸变。IMU旋转角度积分公式如下所示：

$a_t=\frac{w_{t_0}+w_t}{3}dt$ (6)

$a={\displaystyle {\sum }_{i=0}^{n-1}{a}_i}$ (7)

3. 激光雷达SLAM方法

3.1. 模型构建

本文所使用的激光雷达SLAM算法为Gmapping算法，过程模型如图4所示。

Gmapping算法基于RBPF(Rao-Blackwellized Particle Filter)的关键思想，通过外部传感器数据和里程计信息来估计环境地图与机器人轨迹的联合后验概率，建图与定位计算如下：

$p\left(x_{1:t},m|z_{1:t},u_{1:t-1}\right)=p\left(x_{1:t}|z_{1:t},u_{1:t-1}\right)\cdot p\left(m|x_{1:t},z_{1:t-1}\right)$ (8)

其中， $z_{1:t}$ 表示时刻t的雷达观测， $u_{1:t}$ 表示时刻t的里程计数据， $x_{1:t}$ 表示系统位姿，m表示环境地图数据。

Figure 4. GMapping model of laser SLAM

图4. 激光SLAM过程的GMapping算法模型

RBPF是一种基于粒子滤波器的SLAM算法 [9] ，采用粒子滤波器来估计机器人的姿态和地图，将运动系统(如移动机器人)的姿态估计数据与地图的建模数据分开处理，以提高滤波器的效率和精度。GMapping算法使用RBPF作为核心滤波器以优化数据获取准确度。RBPF的权重计算方程如下所示：

$Y_t=\left\{x_{1:t}^1,x_{1:t}^2,x_{1:t}^3,\cdots ,x_{1:t}^m\right\}$ (9)

其中， $Y_t$ 表示采样粒子集，RBPF算法用一定数量的粒子拟合后验分布，每个粒子表示运动系统在真实环境中的一种位姿估计，而 $x_{1:t}^k$ 表示在时刻 $t$ 的系统位姿假设， $k$ 表示粒子集中的第 $k$ 个， $m$ 表示粒子总数。

RBPF粒子滤波器以公式(8)的方式做因式分解，可用两个独立后验概率乘积表示SLAM问题。 $p\left(m|x_{1:t},z_{1:t-1}\right)$ 表示环境地图后验概率， $p\left(x_{1:t}|z_{1:t},u_{1:t-1}\right)$ 表示运动系统的路径轨迹后验概率。

3.2. 数据表示及坐标系

激光雷达SLAM的数据表示方式采用栅格地图 [10] ，因激光雷达和系统在物理世界的相对位置不同，涉及三维空间的坐标系变换问题，方案中的相关坐标系主要有以下几类：

(1) 全局坐标系：通常固定在建图上，表示整个环境的参考框架；

(2) 机器人坐标系：与运动系统本体关联，以运动系统的中心点位为原点；

(3) 激光雷达坐标系：以所使用激光雷达的中心点位为原点，表示激光雷达的安装位置；

(4) 惯性测量单元坐标系：与IMU传感器关联，提供运动系统的加速度和角速度信息。

4. 实验与分析

4.1. 系统方案设计及实验条件

本文所设计方案为基于GMapping算法的二维激光雷达SLAM，软硬件结合系统方案设计如图5所示。

Figure 5. System architecture

图5. 系统架构图

实验所用硬件为嵌入式平台NVIDIA Jetson Nano，2D激光雷达RPLidar A1，IMU和里程计，激光雷达参数如表1所示。

Table 1. RPLidar parameter table

表1. RPLidar参数表

系统整体实物情况如图6所示。

Figure 6. Physical map of the system

图6. 系统实物图

4.2. 实验结果

实验场景下的栅格地图构建如图7和图8所示，设置目标终点及整车配速后的自主导航轨迹如图9所示。系统在导航过程中能够进行全局路径规划，对于路径上的障碍物实现动态避障。

SLAM生成初始栅格地图，构建视窗高度下的周边环境地图，如图7所示。对于远距离物体或障碍面，栅格地图呈现发散状态。

Figure 7. Map construction at initial run time

图7. 初始运行时的栅格地图

实验场景下，在系统运动后，栅格地图构建完备，能够依据导航算法规划静态全局路线，实现自主导航。

Figure 8. Mapping effect in the experimental scene

图8. 实验场景下的栅格地图构建效果

Figure 9. Path planning effect

图9. 路径规划效果

5. 结论

本文概述了SLAM问题，解释了数学模型及数据的采集和表示方式，分析并对比了视觉、雷达、多模融合等的主流SLAM解决方案。针对SLAM问题，本文设计了激光雷达SLAM方案的软硬件结合系统方案，详细给出了构建模型和流程，设计并实现了基于Gmapping和Djikstra算法的自主导航SLAM系统，给出了系统架构图，采用IMU对雷达数据畸变预处理提高了精确度，通过实验验证了EKF和RBPF对于点云精确度和系统响应速度的优化。由实验可知，系统的同步定位与建图、自主导航与避障功能良好，符合预期效果，方案设计与实现具备有效性和可靠性，为相关领域研究提供了解决方案和应用基础。

致谢

感谢我的导师宋春林在研究生期间对我的指导和关照，感谢实验室的师兄师姐们对本人科研工作的指点及帮助，感谢母校同济大学的栽培。感谢在我工作和科研中，对我给予认可的所有组织和机构，使我有信心和动力投身科研和实践、奋发进取。