复杂环境中的无人车多传感器紧耦合SLAM 方法

宋学佳，敖银辉，王文杰

(广东工业大学机电工程学院，广东 广州 510006)

在未知且复杂的环境中，需要实时精准地估计无人驾驶车辆的位姿与速度，并实时感知环境物体，构建环境地图，以作为轨迹规划、车辆反馈控制和障碍物躲避等功能的信息输入，因此，优秀的同时定位与地图构建(SLAM)方法对于无人驾驶车辆(UGV)至关重要。 根据传感器的不同，一般分为基于视觉SLAM 与基于雷达SLAM 方法。 基于视觉的方法通常使用相机传感器，包括单目、双目和深度相机，通过对连续图像信息的处理以确定相机的运动状态。由于相机体积小、成本较低和易于安装等优点，相机被广泛应用于SLAM，并提出了许多方法，例如基于单目相机设计的鲁棒性强且实时性高的ORBSLAM[1]，基于深度相机以解决深度估计问题的RGB-D SLAM[2]，以及基于视觉和惯性测量单元(IMU)信息融合的视觉惯性SLAM 方法[3-5]，进一步提高了视觉SLAM 方法的定位精度与鲁棒性。 尽管视觉SLAM 方法有许多优点，但是其对光照强度、视角范围以及系统初始化的敏感性，影响了视觉SLAM 方法的可靠性。

相反，由于激光雷达主动感知的特性，其受光照强度的影响很小，具备全时段工作的优点，并且随着激光雷达传感器技术的发展，其更加适用于长距离且环境复杂的场景，可以感知环境物体的详细特征，因此，本文将基于激光雷达的SLAM 方法作为重点研究方向。 经过多年的发展，许多优秀的方法被提出。 论文[6-7]提出了一种融合了激光雷达与IMU的测量信息的方法LOAM，获得了低漂移且实时性高的状态估计与建图效果，并作为经典的雷达SLAM 方法被广泛使用，长期在自动驾驶KITTI 数据集解析排名中保持前列。 论文[8]基于LOAM 做改进，获得了更好的效果。 但上述方法仍存在一些问题，例如没有融合类似GNSS 等具有绝对位置测量的传感器数据、在点云特征稀疏或特别稠密的场景时误差大与效率低、回环检测失效以及无法构建赛道桩桶地图等。

针对上述问题，我们提出了一种基于iEKF 理论的多传感器紧耦合SLAM 方法，首先利用IMU 预测车辆的先验状态与消除点云畸变，并使用滑动窗口方法保存点云特征以提高点云匹配精度；然后，在GNSS 测量信息可用时，通过对GNSS 信息作初始化与坐标系变换，分别完成滤波更新得到后验状态。最后在后端环节选择性地引入关键帧进行状态优化与反馈更新，构建全局环境点云地图。 此外，我们还开发了多目标检测与跟踪算法，利用Camera-LiDAR联合检测，实现了多个桩桶的目标检测、跟踪与全局桩桶地图的构建，丰富了本文方法的适用场景。 因此，我们工作的主要贡献可以概括如下:

①基于迭代扩展卡尔曼滤波设计了多传感器紧耦合SLAM 方法，融合了GNSS 测量信息，具有更高的定位精度与冗余度；

②使用点云滑动窗口方法保存点云特征以提高点云匹配精度，使得在开阔场景中仍具有很好的效果；

③提出一种有效的多目标检测与跟踪算法，实现了多个赛道桩桶的目标检测、跟踪与全局桩桶地图的构建。

1 相关研究

由于激光雷达机械结构的旋转扫描机制与传感器自身的运动，会导致激光雷达的测量出现运动畸变，所以只使用单一激光雷达传感器估计车辆状态信息，畸变会导致估计结果出现较大的漂移，特别是在非匀速行驶工况。 因此，激光雷达通常需要融合其他传感器信息，例如IMU、运动传感器以及GNSS等，以获得理想的状态估计结果。 通情况下，多传感器信息融合的方法可以分为松耦合方法和紧耦合方法。

在松耦合方法中，LOAM[6]通过计算点云平滑度来提取边缘和平面特征，提高了匹配效率，并利用IMU 预测帧间运动，以去除点云运动畸变，但匀速运动模型假设与回环检测的缺点，影响了其定位与建图效果。 LEGO-LOAM[8]在LOAM 的基础上进行改进，通过点云分割、地面提取与协助优化，使得其在树木与草地等噪声覆盖的场景中具有较好的效果。 论文[9]利用EKF 融合雷达与IMU 测量，但只适用于二维场景。 Zhen 等[10]先通过高斯粒子滤波器匹配雷达与先验地图得到状态估计，然后再用IMU 协助优化。

虽然松耦合方法在计算上是有效的，但是容易丢失传感器信息[11]，相反紧耦合方法具有更高的估计精度与更强的鲁棒性，成为了重要的研究方向[12]，许多相关方法被提出。 Hemann 等[13]基于误差状态卡尔曼滤波(ESKF)理论紧耦合IMU 与雷达高度图信息。 同样，基于ESKF 设计的紧耦合方法LINS[14]被用于估计机器人状态，但LINS 没有正确推导状态协方差的传递方程，只适用于特定环境。对于联合优化的方法，MC2SLAM[15]为基于雷达连续扫描与局部地图非刚性配准的运动补偿方法，通过构建因子图优化结构紧耦合IMU 测量。 Park等[16]提出了最小化雷达与IMU 约束来优化局部轨迹的方法。 Geneva 等[17]提出了图优化结构的LIPS，以融合IMU 的预积分结果与从雷达点云中提取的平面约束。 Ye 等[18]提出一种紧耦合的方法LIO-Mapping，其通过一种新的旋转约束建图的方法来优化车辆位姿与地图，但其基于图优化的方式来处理所有的传感器测量，无法满足实时性要求。 此外，在GNSS 可用时，融合GNSS 的方法可以获得更好定位效果，郭安等[19]基于KF 理论融合GNSS、IMU、气压计与空速计信息，实现对无人机状态估计的仿真，但由于其三级串联的思路会丢失高频率传感器数据，导致冗余性低。 彭文正等[20]基于iEKF理论融合了IMU、GNSS 与雷达等传感器，实现了二维的车辆状态估计，但对先验地图的依赖较高且没有建图功能。 Zhu 等[21]通过GNSS 约束实现闭环，以融合GNSS 与雷达的测量。

2 系统框架

2.1 系统定义

首先定义具有12 自由度的车辆状态量为X＝[Pvqg]T，分别表示平移P、速度v、三维旋转对应的四元数q以及重力加速度g。 在实验平台的传感器组合中，IMU 的测量频率最高，因此，以IMU的时间戳为基准，将雷达与GNSS 的时间戳与IMU时间戳对齐，使车辆状态量可以被正确地预测与校正更新。 Ligom 的系统框架如图1 所示。

图1 Ligom 的系统框架图

为了方便理解，本文使用的符号与定义如表1所示。

表1 本文使用的符号与定义

2.2 滤波方法

作为EKF 的改进算法，iEKF 同样分为预测和更新两个环节，其中预测环节是结合传感器测量与预测方程，由上一时刻的后验状态递推得到当前时刻的先验状态并完成正确的协方差传递。更新环节是通过计算状态的期望观测与传感器的实际测量的残差，从而更新先验状态，得到后验状态。 预测方程如式(1)所示:

更新方程如式(2)所示:

式中:op为更新环节的迭代次数，α为迭代步长。 本文的收敛条件为状态的更新量足够小或者达到了最大的迭代次数，假如满足收敛条件，结束迭代完成更新环节，否则使，继续进行迭代更新。

2.3 状态预测环节

本文设定IMU 的加速度计测量值为am、陀螺仪角速度测量值为ωm，以及IMU 的随机游走为ab与ωb。 在IMU 的连续测量时间t-1 至t时刻，有时间增量Δt，根据IMU 的积分模型，将车辆t-1 时刻的后验状态递推预测到t时刻先验状态，状态量的预测方程如式(3)所示:

式中:符号⊗为四元数q之间的乘法运算。

式中:

对于时刻tIMU 的测量数据输入ut＝[am ωm]T，会引入其自身的协方差矩阵Qt，如式(8)所示:

式中:Λ和Θ分别是加速度a与角速度ω的高斯白噪声，可以通过IMU 的离线标定得到。 因此，根据式(1)完成车辆先验状态的预测。

2.4 雷达点云校正环节

2.4.1 地面点分割

车辆在行驶过程中，雷达实时感知周围环境，得到地面点云与非地面点云，其中，地面点云属于特征明显的平面特征点云集合，可以通过简单计算将地面点从原始点云中分割出来。 分割地面点主要有两个目的，其一提高了点云特征提取的计算效率，因为去地面后，剩余的非地面点云数量比原始点云减少很多；其二提高了目标检测环节的准确率和效率，避免了地面点云的干扰。

本文采用角度法和深度法相结合，如图2 所示，首先通过标定确定雷达的安装高度，当雷达线束对环境物体扫描时，雷达高度、扫描线束以及地面形成三角形，从而确定地面深度[22]。 假设雷达扫描得到的地面点为P1与P2，角度β1为P1P2与水平方向形成的角度，而当点P3为非地面点时，P2P3与水平方向形成的角度β2明显大于β1，因此通过设置角度阈值并结合深度法，能够准确高效地分割地面点云。

图2 点云分割方法示意图

2.4.2 特征提取与坐标转换

参考论文LeGo-LOAM 的理论，计算局部曲面的平滑度，根据平滑度的大小，将非地面点云划分为边缘特征~eL和平面特征~sL。 本文车辆平台的运行工况是非匀速运动，所以雷达点云的测量信息存在运动畸变，使得测量点不同于真实位置，需要去除当前时刻t的雷达测量畸变。 传统的LIO 算法，其雷达测量的帧间运动信息是通过IMU 预积分得到，相比之下，本文融合了GNSS 测量，得到更准确的车辆状态，根据式(9)求得更加准确的雷达t-1 至t时刻的帧间变换矩阵，从而通过去除点云的运动畸变。

2.4.3 雷达点云信息融合

如2.4.2 所述，本文在地图坐标系M中进行点云配准，所以，当获取到t时刻测量点云时，首先去除当前帧点云运动畸变，然后根据t时刻车辆状态与IMU-Lidar 的外参，将点云投影到地图坐标系M，得到，如式(10)所示:

根据特征点类型的不同，计算观测残差，其残差的定义为:平面点计算点到平面的距离、边缘点计算点到直线的距离，如式(11)所示:

为了便于公式的书写，此处设q＝，根据矩阵运算链式法则，求解点云校正环节的雅克比矩阵Ht如式(12)所示:

式中:

因此，矩阵Ht的最终表达式为:

在校正环节中，通过定义权重值ξ＝1-1.8 ｜rest｜，从而随着t时刻观测残差rest数值的不同，动态调整校正环节中Ht起到的作用。 最后根据式(2)完成状态更新。

2.5 GNSS 校正环节

由车辆的先验状态求得R()，结合已知传感器外参变换角度对应的旋转矩阵，计算初始变换关系。 根据GNSS 坐标系G的z轴与重力坐标系g方向平行的关系，修正三个方向的数值，得到实时的变换关系，并通过式(17)，初始化相关参数:

在t时刻，系统的GNSS 的测量值计算实时的。 本文通过式(18)将车辆的先验状态信息投影到地心系W，并计算观测残差rest:

计算GNSS 校正环节的雅克比矩阵Ht如式(19)所示:

式中:

由于hφq表达式比较复杂，通过数值计算得到。最后根据式(2)完成状态更新。

2.6 多目标检测与跟踪

在赛道工况中，多个桩桶组成的封闭赛道地图信息是未知的，需要车辆在行驶过程中实时感知桩桶信息，并对多帧信息的同一桩桶进行目标跟踪，为建图算法提供桩桶的位置、类别以及ID 信息。 因此设计了Camera-Lidar 联合检测的多目标检测与跟踪算法，框架图如图3 所示，具体步骤为:

图3 多目标检测与跟踪算法框架图

①使用YoloV4-tiny 对图像进行目标检测，得到检测框的类别与位置信息；

②以点云的时间戳为基准，将图像检测框与非地面点云进行时间戳对齐；

③通过雷达与相机离线标定[23]的外参数据，将非地面点云重投影到图像中，使用点云聚类算法对检测框内的点云聚类与坐标解算，得到桩桶的三维坐标与类别信息；

④结合车辆状态与桩桶信息，采用EKF 预测和更新桩桶位置，并使用匈牙利算法完成匹配，赋予每个桩桶独立的ID 信息。

2.7 后端优化与建图

本文后端算法主要有三个作用:①优化前端里程计信息，得到车辆最优后验状态；②构建高精度的环境点云地图，本文基于LEGO-LOAM 的建图算法进行改进，优化了建图的精度与效率。 ③车辆在探索行驶中，构建封闭的赛道桩桶地图。

对于桩桶地图建图算法，本文使用GTSAM 构建两种因子类型，分别为位姿变换因子与桩桶观测因子。 对于位姿变换因子，提取关键帧i和j时刻的车辆位姿，分别为，根据式(21a)求解帧间车辆位姿的期望观测；对于桩桶观测因子，由2.6 节已知桩桶的位置、类别以及ID信息，假设桩桶的ID 为k，根据实时的车辆里程计信息，将其投影到地图系M得到，并结合j时刻的车辆位姿与式(21b)求解桩桶的期望观测。

最后，根据式(24)构建成本函数，通过LM 算法求解此非线性优化问题，优化所有位姿关键帧与桩桶信息。 本算法的优点是多目标跟踪算法，可以在一段时间内，对同一桩桶进行连续跟踪并赋予独立的ID，并根据桩桶的同一ID 去索引不同时刻的位置与类别信息，以准确地构建桩桶观测因子，得到准确的赛道桩桶地图。

3 实验

3.1 实验平台

如图4 所示为一辆大学生无人驾驶方程式车辆平台，其配备了多种传感器，包括元生IMU YIS-510V 的IMU、16 线激光雷达RS-LIDAR-16、华测P3-DU GNSS 接收机与单目相机MV-CE013-50GC，可为车辆提供实时的自身状态与环境的测量信息。 车辆配备了研华MIC-7700 工控机、NVIDIA jetson TX2 开发板以及华海RapidECU-E3 等计算单元硬件，并基于机器人操作系统(ROS)与C++搭建系统，软硬件协同实现传感器数据处理、车辆状态估计与地图构建的功能。

图4 实验平台与传感器位置示意图

3.2 实验工况与结果分析

为了测试验证Ligom 的效果，选择开源算法LEGO-LOAM 与LIO-Mapping 进行对比，并在上述的实验平台中配置对应的算法运行环境，以执行在不同工况中定位与建图效果对比。

本文主要在四个不同平台与环境采集的数据集上进行验证，分别为Steven Campus、公园、KITTI 街道以及GDUT-Tracking 数据集。 其中前三种是开源数据集，而GDUT-Tracking 数据集是上述的自研实验平台，在开阔的环境中，记录车辆在封闭赛道行驶过程的测量数据，用于验证算法的定位与桩桶地图建图的精度。

3.2.1 Steven Campus 数据集

此数据集主要为Stevens 理工学院校园的树木、建筑、道路以及人行道的测量，存在大量的噪声数据，并且整个数据轨迹较长，容易造成误差累积，对算法的测试压力较大。 由于Steven Campus 是LEGO-LOAM 算法中的示例测试集，LEGO-LOAM 运行效果优异，所以在此小节中，以LEGO-LOAM 的结果作为参考，对比分析LIO-Mapping 与Ligom 的定位与建图效果。

如图5 所示，在前半段中，LEGO-LOAM、Ligom与LIO-Mapping 都可以平稳地运行并输出定位与建图结果，并且三个系统的里程计估计基本重合，但是，随着行驶路程的增长，系统的误差在累计，特别是UGV 行驶在框图所在的位置时，地面的高度有较大的变化，此时，Ligom 相比LIO-Mapping，定位轨迹更加接近LEGO-LOAM，因为Ligom 具有有效的回环检测机制，考虑了历史帧的点云信息，以修正车辆状态，具备更强的鲁棒性，在长时间和长距离的运行中漂移较小。 并且如图6 虚线框所示，Ligom 的建图结果更加精细和准确。

图5 Steven Campus 数据集中三种方法的定位轨迹对比

图6 Steven Campus 数据集中三种方法的建图结果

3.2.2 公园数据集

在此测试中，UGV 行驶在公园的行人小道，环境的特征主要包括公园及道路周围的建筑、树木与低矮草丛，并且部分路段仅有少量树木与草丛，此时比较难以获得准确的平面或边缘特征点云，对于点云特征提取与配准算法要求较高。

如图7 的虚线框所示，Ligom 可以将相同树木的点云正确地聚类，得到准确的树木点云特征，而此处LEGO-LOAM 与LIO-Mapping 得到的树木点云出现了不重合的结果，其原因在于这两个方法的回环检测结果不准确，车辆回到起点附近时轨迹出现了较大的漂移，从而导致了建图结果的误差。

3.2.3 KITTI 街道数据集

选择KITTI_2011_09_30_drive_0027 作为本小节的测试数据集，以模拟商用车辆的行驶工况，验证算法在车速较快、路况较复杂的街道场景中的定位与建图效果。

如图8 所示，Ligom 与LEGO-LOAM 的定位轨迹接近，都可以正确估计行驶过程中的路面高度的变化，并实现正确的轨迹回环，而LIO-Mapping 在估计车辆z方向位置中出现了非常大的误差，定位轨迹严重偏离，起始点与终点的z方向高度差达到约1.4 m，无法为车辆提供准确的车辆状态信息。 表2 显示了车辆回到起点时三种方法的相对平移误差。

表2 车辆回到起点时三种方法的相对平移误差单位:m

图8 KITTI 街道数据集中三种方法的定位轨迹对比

由图9 可知，由于算法对于距离的设定，LIOMapping 可以处理更远处的点云信息，但因为远距离物体的测量与其车辆定位信息的误差，LIOMapping 的建图结果中，物体位置出现较大的偏离，特别是起始位置附近的地图由许多模糊结构组成。对比LEGO-LOAM，Ligom 能够更加准确地识别并构建物体的轮廓，例如街道旁的车辆与绿化带。 因此，在复杂街道的场景中，Ligom 的定位与建图算法效果优于其他两种方法，能够为车辆提供高精度的实时定位与建图信息。

图9 KITTI 街道数据集三种方法的建图结果

3.2.4 GDUT-Tracking 数据集

此小节的测试目的是验证融合GNSS 测量对车辆定位精度的提升效果，以及赛道桩桶地图的建图精度。 因此，在图10(a)虚线框所示的开阔场景中，随机摆放两种不同轨迹的闭环桩桶赛道。 在此场景中，获得的点云特征稀疏，对于传统的LIO 算法具有很大的挑战性，并且遮挡物较少，可以接收到准确可用的GNSS 测量，因此可以针对性地验证测试目的。

图10 Tracking 数据集中环境与定位轨迹对比

由于获得的点云特征稀疏，松耦合的LEGOLOAM 算法无法得到有意义的结果，所以只对比LIO-Mapping。 运行Ligom 与LIO-Mapping 得到定位轨迹如图10(b)、图10(c)所示，以GNSS 测量作为参考轨迹，从测试结果可知，Ligom 得到的定位轨迹更接近GNSS 测量，特别是场景2，LIO-Mapping 在x方向中出现了较大的偏移，最大偏差达到了约2.5 m，由此可知，Ligom 融合了GNSS 测量，可以得到更加准确的车辆状态。

在车辆行驶过程中，实时构建桩桶地图如图11所示，Ligom 可以准确地识别不同桩桶的颜色、位置以及ID 信息，然后根据同一ID 将多帧桩桶信息求解最优后验估计，完成回环检测构建桩桶地图。 由于桩桶是随机摆放的，没有先验地图信息作为参考，所以，为了验证桩桶地图的建图精度，如图11 所示，本文选择六个桩桶点，对比地图结果与实际测量的距离误差。 由表3 和表4 可知，桩桶之间的距离误差很小，桩桶建图算法可实现高精度建图。

表3 桩桶A、B、C 的地图结果与实际测量的间距

表4 桩桶D、E、F 的地图结果与实际测量的间距

图11 Tracking 数据集实际环境与桩桶地图结果

4 结论

本文提出了一种多传感器信息紧耦合的无人驾驶车辆SLAM 方法，可以在复杂的环境中，实现无人驾驶车辆状态的实时估计与环境地图的构建。Ligom 基于iEKF 理论设计，适用于融合多传感器信息，特别是融合了GNSS 传感器，可以消除长时间积累的激光雷达惯性里程计漂移，并使用滑动窗口方法保存点云特征，提高了点云匹配精度，在点云特征比较稀疏的开阔环境中仍具有很好的效果。 本文所提出的方法在四个不同平台与环境采集的数据集下进行了充分验证，结果表明，相比LEGO-LOAM 与LIO-Mapping，Ligom 方法可以达到更高的精度与具备更强的鲁棒性。