基于点云分割的激光里程计算法

李一凡，杜世通，李爽，黄璐，杨子寒，陈冲

(1.卫星导航系统与装备技术国家重点实验室,石家庄 050081；2.中国电科网络通信研究院,石家庄 050081)

0 引言

同时定位和地图构建(simultaneous localization and mapping，SLAM)是移动机器人执行导航和交互任务的关键技术之一.SLAM 框架通常包括前端里程计、后端和回环检测.前端里程计是SLAM 系统的基础，也是后端和回环检测的前提.它使用环境感知传感器，如视觉相机或激光雷达，获取周围环境的图像和点云信息，并利用相邻帧间的环境信息变化估计帧间的位姿变化，从而计算出前端里程计.

根据传感器输出数据的类型，前端里程计分为两类: 基于视觉传感器的视觉里程计(visual odometry，VO)[1]和基于激光雷达的激光里程计[2].视觉传感器有许多缺陷，如受光照影响大，深度信息的尺度不确定性[3].相较之下，激光雷达不受环境光影响，具有厘米级的测距离精度，且抗干扰能力强，更具鲁棒性和精确性[4].因此，基于激光雷达的SLAM 方法是自主机器人定位研究的一个热点.

激光里程计是通过最小化相邻帧点云之间关于位姿变化的误差函数得到帧间位姿变化.基于激光雷达的SLAM 方法被分为三个不同的类别：基于点匹配的迭 代最近 点算法(iterative closest point，ICP)、基于概率分布的正态分布变换(normal distributions transform，NDT)算法和基于特征的匹配的算法[5].ICP算法具有实现简单、运算速度快的优点，对于较小的场景和运动较缓慢的情况下表现良好.但ICP 算法对噪声和异常值较为敏感，且对预测值要求高，容易陷入局部最优解.NDT 是一种基于高斯分布的概率匹配算法，它能够更好地处理激光雷达传感器噪声和数据不完整性的问题，对于复杂场景下的建图和定位表现优秀.但NDT 算法对于场景的平滑度和连续性有一定要求，同时计算复杂度较高、精度较低.Zhang等[6]首先提出了基于特征的激光雷达里程计和建图(LIDAR odometry and mapping，LOAM)算法，特征提取能够将复杂的点云数据简化为一组可用的特征点，从而减少数据量，提高算法效率，同时也为后续的点云匹配并最终得到激光里程计提供基础.LOAM 算法实现了实时的、低漂移的激光里程计，解决了ICP 算法对噪声敏感、NDT 算法要求场景平滑的问题.但LOAM 算法中采用简单的平滑度判定点云的特征，在噪声、动态物体等复杂环境下容易出现漏检或误检的情况，从而影响SLAM 的精度和鲁棒性.因此，在复杂环境下的特征提取和匹配仍是一项极大的挑战.

为进一步提升激光里程计的鲁棒性和精确性，研究者们提出了许多改进方法来解决特征提取的问题.Horache等[7]提出利用多尺度架构和无监督转移学习的3D 点云配准方法，实现高精度的特征提取和匹配.Serafin等[8]提出了一种使用点邻域的主成分分析(principal components analysis，PCA)方法，利用该方法得出数学原理的线和平面特征.Abedini等[9]提出引入激光雷达点云的强度数据，利用尺度不变特征变换(scale invariant feature transform，SIFT)算法从航空图像中提取不同的特征.另外，Arshad等[10]提出利用仿射尺度不变特征变换、线性方程、粒子群优化等方法提取优化激光里程计特征提取算法.引入更多数据和更复杂的算法可以有效提高激光里程计算法中特征提取和匹配的准确性和鲁棒性.然而，这些复杂的算法和需要处理的更多数据会对处理设备的要求产生较大压力，难以在轻量化平台上应用.为了解决上述问题，本文提出了一种基于点云分割的激光里程计算法.该算法通过分析激光雷达点云数据特征，有针对性地对激光雷达点云进行分割并赋予标签，并根据标签与物体扫描情况提出了对应的特征提取和匹配方法，从而提高了特征提取的准确性和里程计的精确性，并且占用较少的计算资源.

1 激光里程计

如图1 所示，基于特征的激光雷达里程计算法通常包括三个模块：点云分割、特征提取、特征匹配.点云分割将点云聚类并赋予点标签.特征提取从点云中提取直线和平面这两种特征点.随后，每个直线和平面特征点会被分别投影到地图特征点云中进行匹配，并计算点到直线的距离和点到平面的距离与位姿有关，累加这些距离的和即为与位姿相关的误差函数，最小化误差函数以得到激光里程计.

图1 基于特征的激光里程计流程框图

点云的特征提取是指对激光雷达获取的点云数据进行处理，从中提取出具有定位特征的点.准确性和鲁棒性对于激光里程计的性能至关重要.本文针对机械式激光雷达的工作原理及所得点云特点，提出了一种新的点云分割算法，该算法具有计算简单、计算量小的优势，并且能够判定并提取的特征点受噪点影响较小，具有更好的定位特性.

1.1 点云分割

在特征提取前利用点云分割将点云分割、聚类并赋予标签[11].点云分割阶段接收激光雷达获取的点云数据，通过分割算法将点云分割成若干个子集，并为每个子集标记一个标签.特征提取阶段基于分割结果提取特征点云，通过对点云分割所得标签判定物体扫描状态，保留符合条件的特征点云，同时剔除噪点.相较于传统的特征提取算法，本文提出的算法可以提高特征提取的准确性和鲁棒性，并且能够处理大规模点云数据.

由于激光雷达机器本身的固有特性，其依靠内部多对激光发射器360°水平旋转扫描周围环境，以获取深度信息并形成点云.如图2 所示，激光雷达的垂直分辨率取决于其扫描线数.举例来说，常用的16 线激光雷达垂直分辨率仅为2°.但由于激光发射器的高发射频率，其水平分辨率可以高达0.18°.因此，通过对扫描线点云进行水平角度分割，可以更加精确地进行处理点云.

图2 激光雷达工作原理示意图

由于激光雷达点云数据中每个点的存储顺序与扫描顺序相同，因此只需遍历激光雷达点云中的每一个点，并使用式(1)计算其俯仰角度 α .然后，根据俯仰角度 α 将 点云按照扫描 线进行划分.例 如针对16 线激光雷达，可以按照图2 所示的扫描角度将点云划分为16 份扫描线点云.

式中，x、y、z分别为点在激光雷达坐标系下的三维坐标.

后续的点云分割需使用点云的水平扫描角度，为降低计算资源的消耗，利用式(2)计算了点云的水平角度

由于反正弦函数取值范围为 (-90,90)，因此当(x,y) 处于第三、四象限时 β 还需增加180°.

在激光雷达运转时，同一扫描线上相邻扫描点的角度间隔极小(0.18°)，因此在激光雷达探测物体距离较近时，相邻点仅存在两种情况：1)两点为同一物体上的点；2)两点为不同物体上的点.如图3 所示，通过计算相邻扫描点连线所成向量，与两点中较远距离点坐标向量所成夹角φ，可以判断相邻点是否属于同一物体[12].以相邻点A、B为例，可以根据它们的水平扫描角度差和三维坐标进行计算，得到

图3 激光雷达扫描线俯视示意图

式中：x、y、z分为点在激光雷达坐标系下的三维坐标；ΔβAB为利用式(2)计算所得A、B两点水平角差值.

为了将点云分割为不同的物体，本文按扫描线遍历点云中的所有点，并计算每个点与其下一点之间的夹角φ.如果该夹角φ大于设定的阈值(在本文中为30°，即连续两点横向变化超过深度变化的两倍)，则将这两个连续的点视为不属于同一物体.基于相邻点是否属于同一物体，本文将同一物体上的点标记为内点，而不同物体上距离更近的点则被标记为物体边缘点，距离稍远的点则标记为离散点.通过这种方式，本文可以完成点云的分割.

1.2 特征提取

激光雷达通过发射激光并接收反射的激光测距形成点云，而由于激光会被物体遮挡，因此物体被扫描的区域会随着激光雷达的运动而变化.如图4 所示，黄色定位特征被激光雷达扫描的情况主要包括两种：a) 完全扫描；b) 部分扫描.由于激光雷达是按一定顺序旋转扫描，物体被部分扫描时还需要区分左部分、右部分两种情况，可通过遍历扫描线中的每一个点，根据点云分割中赋予的标签与表面点的顺序判定左、右部分扫描的情况.

图4 物体扫描情况示意图

在城市场景中，物体通常呈现出结构化的标准形状，例如墙壁、汽车、房屋等可视为长方体、棱锥等的组合.物体的边缘可视为直线，表面则可视为平面.被扫描区域较小的、或本身较小的物体难以从中提取准确的定位特征，因此仅从横向扫描长度大于一定阈值(防止较小物体左右边沿点相互干扰，设定最邻近搜索的距离为0.5 m)的物体提取其边缘点为直线特征，表面点为平面特征.而城市中含有许多横向较小的物体，例如电线杆、树干等，它们也是不可或缺的定位特征，可视为圆柱体，横截面视为圆.因此对于完整扫描但物体横向距离较小(与较小物体判断相对应)的情况，若其表面点数量大于5 (圆心拟合最少5 个点可以判定拟合误差)，则认为点云中存在足够的点用于拟合，以保证拟合的可靠性.则将所有通过点带入并优化最小二乘式

式中：a、b分别为在物体扫描点最小、最大点的点云中的序号；xc、yc是所拟合圆心的坐标，；xi、yi为点的坐标；R为所拟合圆心的半径.

若优化上式所得圆的半径小于物体两边边缘点之间的距离，即所扫描物体具有圆柱属性且扫描区域大于横截面的三分之一，则认为拟合成功，并将圆心视为直线特征点.除上述符合条件的点作为定位特征点以外，其他点均视为离散点，不作为定位特征点，仅用作建图.特征提取的具体流程如图5 所示.

图5 特征提取流程图

1.3 特征匹配

在LOAM 算法中，特征点的匹配分为两个步骤.首先，将当前时刻点云中的特征点投影到历史点云上，形成一组候选匹配点.然后，根据一定的距离阈值，选择与每个候选点最近的点作为最终匹配点.特征点的匹配质量将影响到后续的位姿估计和地图构建质量，因此需要考虑匹配的可靠性和效率.本文也采用了类似的特征点匹配策略，对当前时刻点云中提取的每一个特征点进行候选匹配点的搜索，进而确定最终匹配点.为了提高匹配效率和精度，本文对匹配点的搜索进行了优化，使用了KD-Tree等数据结构，加速匹配过程.假定激光雷达为均速运动，特征点的投影公式为

式中：为投影至地图的位姿；Pi为特征点的三维坐标；Rk-1、tk-1分别为上一帧激光雷达的姿态变化矩阵和位移.

对于直线特征点，先将其投影到地图的坐标系下，然后利用KD-tree 在地图直线特征点云中搜索最邻近的5 个点.接着，采用PCA 分析这5 个点所构成的矩阵的最大特征值.如果最大特征值远大于第二大特征值，就可视为最大特征值所对应的向量是该5 个点所拟合直线的方向.否则应该舍弃该特征点.该直线必过经过该5 个点的平均坐标点，由此可求得点到直线的距离为

式中：di为点到直线的距离；为5 个点的平均坐标向量；vi为最大特征值对应的特征向量；

同理，平面特征点也投影到地图坐标系下，然后利用KD-tree 在地图直线特征点云中搜索最邻近的5 个点.由于PCA 算法耗时长，且平面特征点较多，因此采用最小二乘法将该5 个点拟合为一个平面.通过最小化带入5 个点的坐标，最小化S为

式中：S为待优化的量；A、B、C、D为平面标准方程的四参数；xl、yl、zl分别为点的三维坐标值.

即可得到标准平面方程，由此可计算点到平面的距离为

式中：dj为点到平面的距离；为地图特征点的三维坐标；

将所有特征点中点到直线的距离、点到平面的距离累加即可得到关于位姿的误差函数

将该误差函数最小化即可得到当前激光雷达姿态、位移的最优估计.

2 实验验证

为了验证本文提出的激光里程计算法的有效性、精确性和鲁棒性，本文进行了实验验证.使用开源的KITTI 数据，同时在真实场景中模拟导致算法漂移的复杂情况，以对比本文提出的基于LOAM 算法和点云分割的激光里程计算法的性能.

2.1 开源数据验证

KITTI00 数据集是在典型的城市环境中采集的，该环境的特征丰富且场景简单，因此LOAM 算法和本文提出的算法都能够鲁棒地运行，里程计结果如图6 所示.本文提出算法所得轨迹与基准轨迹的平均误差为3.3 m，误差始终保持较小水平，且最终误差为11.43 m，而LOAM 算法的里程计平均误差为6.8 m，轨迹因误差累计导致逐渐偏离基准，最终误差为62.17 m.由此可得，与LOAM 算法相比，本文提出的基于点云分割的激光里程计算法更具精确性.

图6 实验验证硬件平台图

2.2 真实实验验证

图7 为搭建的一个简易组合导航系统硬件平台.该硬件平台包括16 线激光雷达、低成本微机械陀螺仪(micro electro mechanical systems，MEMS)、高精度光纤陀螺、高精度双天线GNSS等传感器以及一台计算机.实验运行环境为Ubuntu16.04+ROS 环境.平台记录了真实的激光雷达实验数据包，并通过仿真验证了本文算法的特征提取和激光里程计运算结果的优越性.同时，本文使用双天线高精度GNSS 和高精度光纤陀螺的组合导航结果作为激光里程计的基准，以评估算法的精确性.

如图8 所示，实验验证环境为典型的城市环境，道路两旁包含楼房、广场以及树林等场景，能够良好、全面的检验算法的有效性.

图8 真实实验场景俯视图

如图9(a)所示，在房屋楼梯处，场景特征非常复杂，且存在大量难以提取的复杂定位特征.本文用绿色框标记出可用于定位的区域位置，由图9(b)与图9(c)的对比可知，本文提出的基于点云分割算法所提取的特征具有良好的直线特征，且特征清晰、离散点少，因此本文算法在特征提取方面优于LOAM算法.此外，本文所提出的特征提取部分计算量更小，经实验验证，在该数据集下的特征提取平均耗时为23.46 ms，而LOAM 算法为35.43 ms，本文算法更具实时性.如图10 所示，当机器人穿越这个复杂环境时，本文基于点云分割的激光里程计算法在复杂环境下依旧鲁棒运行，而LOAM 算法发生了较大的漂移现象.由此可得，较LOAM 算法，本文提出的基于点云分割的激光里程计算法更具鲁棒性.

图9 特征提取对比图

图10 里程计轨迹对比图

3 结束语

本文针对机械式激光雷达点云横向分辨率远高于竖直分辨率的特点，提出了一种根据扫描线分割点云并为其赋予标签的方法.通过标签的赋予，可以判定物体的扫描情况，从而提取出点云中的直线和平面特征点.最后，将这些特征点投影并搜索匹配地图特征点，并分别拟合直线或平面.利用点到直线的距离和点到平面的距离，可以计算位姿误差方程，从而得到对激光里程计的最优估计.本文利用KITTI 数据集和真实实验数据，通过ROSBAG 数据录影并回访进行了仿真验证，结果表明本文提出的基于点云分割的激光里程计在特征提取方面比LOAM 算法更准确，并且具有更高的定位精度和鲁棒性.