动态环境下融合激光雷达和IMU的激光里程计设计

张 涛，张 晨，魏宏宇

(1.东南大学仪器科学与工程学院，南京 210096；2.东南大学微惯性仪表与先进导航技术教育部重点实验室，南京 210096)

0 引言

近年来，世界各地的各个行业都在推动机器人的发展。 智能移动机器人逐渐走入大众视野，成为人类日常生产、生活中不可或缺的组成部分。在疫情期间，服务机器人用于辅助诊断、快速检测、精准测温、目标识别，极大地提高了诊疗的智能化程度，减少医务人员感染风险，提高管控效率。机器人需要克服许多技术难点才能成功完成上述复杂的操作。其中，同时定位和地图绘制(Simultaneous Localization and Mapping，SLAM)技术是机器人学中最活跃和最具挑战性的研究课题之一。激光雷达SLAM前端根据前后两帧的点云进行配准，在估计变换矩阵时，动态物体会引入一定的误差，最终导致累积误差不断增大。尽管通过引入回环检测节点，可以消除部分估计的累积误差。但是，当没有回环检测和高精度地图时，累积误差依旧无法消除。因此，需要通过去除动态物体对应的点云，使得激光里程计估计变换矩阵时引入的误差尽可能小，以实现更准确的定位和建图。

D. Giveki等提出了将颜色和纹理特征同Choquet模糊积分相结合的方法应用于背景建模，通过该建模背景对现有对象进行分割，以完成动态对象检测。Zou C.等提出了3D全连接条件随机场方法，该网络对激光雷达采集到的连续两个关键帧的点云进行处理，得到每个点作为动态点的概率，从而判断动态点。Yao W.等提出了一种基于物体特征的目标3D分割方法，完成了对移动车辆速度估计的改进，该方法主要应用于一些机载激光雷达在空中获取的少量点云。李波等提出了一个全卷积神经网络，以激光雷达点云数据为输入样本，结合数据增强训练，得到一个可以检测车辆的神经网络，在神经网络学习的过程中需要手动标记学习样本。李响采用傅里叶单像素欠采样成像对数据进行压缩,提取图像特征，并在未获取完整图像信息的情况下，采用深度学习卷积网络对动态物体进行检测和识别。文献[12]的解决方案是先使用前馈神经网络定位目标，计算其边界位置，通过融合方法提取特征，最后用3D卷积网络判断目标，并且能够得到其实际移动速度。

由于实际场景中存在各种形状和大小的物体，因此本文采用常规的聚类算法得到的效果不佳，且由于点云数量过于庞大，存在复杂度高、聚类速度慢等特点。为了识别激光雷达点云中的动静态目标并提取动态点，本文提出了一种融合激光雷达和惯性测量单元(Inertial Measurement Unit，IMU)的动态点云识别算法。该策略通过约束激光雷达扫描范围角与动态中心，将点云快速分割成多个簇类，再借助IMU预估计的位姿变换信息代替特征点匹配，快速建立点云簇类配准关系，从而根据中心点偏移的程度判断出簇类是否为动态物体，完成动态点云的识别与分割任务。最后，再根据簇类的对应关系进行约束，以提高传统特征匹配策略的精度与速度。

1 整体框架

本文的整体方案如图1所示，系统通过搭载的激光雷达扫描得到点云与IMU测量得到的信息作为输入，最终输出系统的位姿信息。整个系统主要包含3个模块：点云预处理模块、特征点提取模块和激光里程计模块。点云预处理模块首先将地面点去除，再通过本文提出的动态点识别去除动态物体与离群散点。特征点提取模块先将由运动造成的畸变通过IMU信息进行畸变补偿，再结合簇类信息与点云的粗糙度，提取各自簇类的边缘特征和平面特征。激光里程计模块通过簇类信息与特征信息进行点云配准，从而完成位姿估计的过程，最终输出位姿。

图1 方案框架

规定本文中载体坐标系(图2)的中心位于激光雷达的坐标系原点，在激光雷达的中心，平面平行于激光雷达的顶平面，轴指向无人地面车(Unmanned Ground Vehicle，UGV)的右侧，轴指向UGV的前方，轴垂直于平面向下。

图2 载体坐标系

2 算法流程

2.1 点云预处理

由于地面点与所有语义对象相连接，因此，首先对点云进行预处理，将地面点进行识别与去除，本文选择采用文献[13]中的方法去除地面点。由于实际场景中存在各种形状和大小的物体，且扫描得到的点云数量庞大，本文根据这些特点提出了一种融合激光雷达和IMU的动态点云识别算法。本文提出的语义分割与点云算法的工作流程描述如下：

第一步：在点云中任意取一点，在该点对应的激光雷达扫描角周围的一定范围内(搜索约束角)，搜索与其欧氏距离相近的点。即满足

(1)

其中，表示两点之间的欧拉距离阈值。通过不断循环第一步，直到范围内所有的点都遍历完毕后，将这些满足条件的点定为一个簇类。

第二步：在上述簇类中，选取上下左右前后6个方向上距离最远的点，不断进行第一步操作，且已遍历过的点不再进行二次遍历，将符合条件的也加入上述簇类，

第三步：不断重复第一步，直到没有点再满足式(1)，则停止迭代。如果最终聚类后得到的簇类中的数字大于给定的数量阈值，则通过式(2)计算出中心点，将该中心点标记为该簇类的核心点,并为该簇分配一个新的标签； 如果小于数量阈值，则将该簇类内的所有点统一标记为离群点。

(2)

图3 聚类结果

(3)

其中，c表示cos函数；s表示sin函数。

(4)

2.2 特征提取

(5)

其中，为激光雷达扫描一周的时长。

为了均匀且快速地提取点云中的特征点，此处对每个簇类分别计算各自的粗糙度，从而提取出边缘特征和平面特征，计算公式如下

(6)

其中，,表示时刻，标签为的点云簇；,为,中的一个点；,为在,中与,不同的其他点。

根据值对每个簇类中的点进行排序，选择较大的几个点为边缘特征点，较小的几个点为平面点。最终得到时刻的边缘特征集合为，平面特征集合为。

2.3 激光里程计

激光里程计的目的是通过最小化点到边缘特征与点到平面特征之间的距离，估计2个连续点云之间的变换关系。为了找到各自的对应关系，本文中使用的激光里程计的基本算法与文献[14]中的一致，但在搜索过程中，加入了簇类约束，因此在进行特征搜索匹配时，仅对同一簇类进行点云配准，通过簇类约束条件排除错误的对应关系。根据2.2节提到的算法，可以得到连续2个时刻的边缘特征与平面特征，分别为,+1与,+1。引入簇类约束后，点到线的距离公式变为

=

(7)

其中，label为对应簇类的标签；∈+1；,∈。

同样，点到面的距离公式变为

=

(8)

其中，∈+1;,,∈。

通过结合IMU的预估计姿态变换矩阵式(3)与式(7)～式(8)，可以得到一个非线性优化函数

(9)

激光里程计最后的目标是将式(9)通过迭代使得尽可能地趋向于0。

(10)

3 实验验证

本实验使用的UGV如图4所示，采用激光雷达采集点云，同时为了便于验证，使用相机采集对应时间的图像信息。激光雷达选择使用Velodyne-VLP16，该设备以10帧/s的速度旋转，每个周期捕获大约30k点。相机选用ZED2，以10帧/s和720p的分辨率获取图像，同时该相机内部自带IMU，可输出400Hz的IMU信息。处理器为英伟达的Jetson AGX Xavier，最高能提供32TOPs的运算能力。在本节中，首先使用配备激光雷达的UGV在多种场景下采集数据，并使用本文提出的算法验证其可行性，最后给出一个直观的分割结果。然后，将该算法应用于UGV和KITTI数据集，并与其他算法进行对比，以验证去除点云中动态物体对应的相关点后对激光雷达SLAM定位精度和效率的影响。整个系统在Ubuntu 18.04 (Linux)中使用ROS执行，并借助ROS中Time_Synchronizer模块对3个传感器输入的信息进行时间戳对齐。实验中涉及的参数如表1所示。

图4 无人地面车

表1 实验参数

3.1 动态点检测试验

实验地点选在走廊。 实验分为以下四种情况。

情况一：当激光雷达静止时，检测到单个动态物体并移除；

情况二：当激光雷达静止时，检测到多个动态物体并移除；

情况三：当激光雷达处于动态状态时，检测并去除走廊拐角处的动态物体；

情况四：当激光雷达处于动态状态时，检测直线走廊并去除动态物体。

如图5所示，从四种不同的情况中选择间隔为1s的两帧进行处理。 在实验环境中，移动的物体主要由人来扮演。通过使用第2节提出的算法进行动态点去除，四种不同情况的结果如图6所示。为了观察方便，图6显示了处理后的点云的俯视图， 其中蓝色六边形代表激光雷达所在位置，聚类得到的动态物体用青色虚线标记。

(a) 情况一

最后，可以在去除图6中的动态点后得到点云的顶视图。从图7中可以看出，在上述四种情况下，本文提出的算法成功去除了环境中的动态物体。在这四种情况中，还可以观察到部分情况下由于人体离激光雷达太近，以至于挡住了身后的整面墙。

(a) 情况一

(a) 情况一

因此，部分墙体在俯视图中产生了断层。由于部分墙只在两个时刻中的一个时刻被检测到，在该算法的处理中，墙的这部分同样被当作动态点被去除。 在后续的点云配准过程中，如果一帧中有一些点在另一帧中没有被检测到，那么在配准过程中强行寻找不对应的点进行配准会引入相当大的误差。 因此，将这部分静态点识别为动态点并去除它们,也有助于提高姿态估计的准确性。 表2显示了处理前后的点云数量，以及在CPU i7-4720上运行的平均处理时间。

表2 分割实验仿真结果

3.2 定位实验

这部分主要使用装载了激光雷达的UGV和KITTI数据集开展实验。通过对比实验结果，可以验证本文提出的动态点去除策略对定位精度的影响。选择开源解决方案LEGO_LOAM与本文的方案进行实验比较。

这部分实验分为以下四种情况：

情况一：UGV静止；

情况二：UGV运动，路径中不存在环路；

情况三：UGV运动，路径中存在环路；

情况四：将两种解决方案应用于 KITTI 数据集。

情况一：由于UGV处于静止状态，实际坐标始终为[0,0,0]。但是，动态物体的存在也会影响UGV本身的位姿估计。实验结果如图8所示，其中蓝线表示使用LEGO_LOAM估计得到的实时位置，绿线表示使用本文方案估计得到的实时位置。从图8可以看出，即使UGV处于静止状态，由于周围物体的移动，在估计位置时也会存在偏差。通过去除动态对象对应的点云，可以减少动态对象引入的误差。具体误差值如表3所示。由表3可知，由动态误差引入的误差的标准偏差降低了约57.7%，最大偏差降低了约49.2%。

图8 情况一的定位结果

表3 情况一的定位结果偏差

情况二：该情况下，由于实时真值难以获取，因此本文仅比较终点的估计位置和实际位置，以观察最后的定位误差。分别在LEGO_LOAM和本文方案下运动UGV，获得估计位置的路径如图9所示。UGV 实际行驶了18.035m，且路径呈直线。通过算法估计得到，LEGO_LOAM的终点位置偏差为0.5494m，本文方案的终点位置偏差为0.4237m，误差降低了约23%。

图9 情况二的路径

情况三：这种情况下，UGV从一个点出发，再次回到起点，从而形成一个闭环。 LEGO_LOAM的最终偏差值为0.1736m，本文方案的最终偏差值为0.1040m。 如图10所示，2个最终位置偏差都不大，这是由于LEGO-LOAM中使用了检测节点，因此可以有效地消除环路中的累积误差，即使行驶的路径更远，也可以得到比情况二更准确的定位结果。

图10 情况三的路径

情况四：为了验证大范围复杂场景下全路径定位精度的提升，将算法应用到KITTI数据集下。最终两种解决方案的结果如图11和表4所示。在整个路径中，最大偏差减少了约31.7%，标准偏差减少了约16.5%。

图11 情况四的路径及位置误差

表4 情况四的定位结果偏差

4 结论

本文针对动态物体造成激光里程计定位产生偏差的问题，提出了一种融合激光雷达和IMU的动态点云识别算法，并且结合该过程改进了传统特征匹配策略，组成了动态环境下融合激光雷达和IMU的激光里程计。算法分析与实验结果表明：

1)动态物体的存在对系统自身位姿估计确实产生了一定的影响，而本文提出的动态点云识别算法,通过约束范围角与动态中心点,将点云快速分割成多个簇类，再借助IMU预估计的变换矩阵，快速建立点云簇类配准关系，从而去除动态点，能够有效地去除环境中的单个和多个动态物体。

2)当动态物体和静态物体在三维空间中重合时，很容易将两者聚类为一个整体，在后续过程中则会将它们作为一个完整的动态物体进行点云去除。然而，在实际情况中，通常只有在动态对象与静态对象碰撞时才会发生，因此可以单独处理。

3)当存在回环的情况下，能够通过增加回环检测节点减小动态物体引入的累积误差，但无法彻底消除，而通过去除动态点可以较大地提高激光里程计定位的准确度。

4)当连续两帧中检测到的物体在另外一帧中没有出现时，本文提出的方案会将其认为是动态物体进行去除，能够提高点云配准的精度。