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

DOI：10.12265/j.gnss.2023066基于点云分割的激光里程计算法李一凡1,2，杜世通1,2，李爽1,2，黄璐1,2，杨子寒1,2，陈冲1,2(1.卫星导航系统与装备技术国家重点实验室,石家庄 050081；2.中国电科网络通信研究院,石家庄 050081)摘要：针对激光里程计算法中存在的信息冗余和离散点干扰问题，本文提出了一种基于点云分割的激光里程计算法.该方法根据机械式激光雷达的水平旋转扫描特点，对点云数据进行扫描线分割并赋予标签，提取物体的边缘点作为线特征、表面点作为面特征.相较于传统的特征提取方法，本文算法能够有效提取具有较少标志性的特征点，同时剔除离散点.该算法在降低计算量的同时提升了定位的精确性和鲁棒性，对于机器人导航任务有着很好的应用前景，并通过计算机仿真验证了该算法的性能，并取得了较好的实验效果.关键词：激光雷达；点云；分割；特征提取；特征匹配；里程计中图分类号：P228.4文献标志码：A文章编号：1008-9268（2023）04-0037-070引言同时定位和地图构建(simultaneouslocalizationandmapping，SLAM)是移动机器人执行导航和交互任务的关键技术之一.SLAM 框架通常包括前端里程计、后端和回环检测.前端里程计是 SLAM 系统的基础，也是后端和回环检测的前提.它使用环境感知传感器，如视觉相机或激光雷达，获取周围环境的图像和点云信息，并利用相邻帧间的环境信息变化估计帧间的位姿变化，从而计算出前端里程计.根据传感器输出数据的类型，前端里程计分为两类:基于视觉传感器的视觉里程计(visualodometry，VO)1和基于激光雷达的激光里程计2.视觉传感器有许多缺陷，如受光照影响大，深度信息的尺度不确定性3.相较之下，激光雷达不受环境光影响，具有厘米级的测距离精度，且抗干扰能力强，更具鲁棒性和精确性4.因此，基于激光雷达的 SLAM 方法是自主机器人定位研究的一个热点.激光里程计是通过最小化相邻帧点云之间关于位姿变化的误差函数得到帧间位姿变化.基于激光雷达的 SLAM 方法被分为三个不同的类别：基于点匹配的迭代最近点算法(iterativeclosestpoint，ICP)、基于概率分布的正态分布变换(normaldistributionstransform，NDT)算法和基于特征的匹配的算法5.ICP算法具有实现简单、运算速度快的优点，对于较小的场景和运动较缓慢的情况下表现良好.但 ICP 算法对噪声和异常值较为敏感，且对预测值要求高，容易陷入局部最优解.NDT 是一种基于高斯分布的概率匹配算法，它能够更好地处理激光雷达传感器噪声和数据不完整性的问题，对于复杂场景下的建图和定位表现优秀.但 NDT 算法对于场景的平滑度和连续性有一定要求，同时计算复杂度较高、精度较低.Zhang 等6首先提出了基于特征的激光雷达里程计和建图(LID-ARodometryandmapping，LOAM)算法，特征提取能够将复杂的点云数据简化为一组可用的特征点，从而减少数据量，提高算法效率，同时也为后续的点云匹配并最终得到激光里程计提供基础.LOAM 算法实现了实时的、低漂移的激光里程计，解决了 ICP 算法对噪声敏感、NDT 算法要求场景平滑的问题.但LOAM 算法中采用简单的平滑度判定点云的特征，在噪声、动态物体等复杂环境下容易出现漏检或误检的情况，从而影响 SLAM 的精度和鲁棒性.因此，在复杂环境下的特征提取和匹配仍是一项极大的挑战.为进一步提升激光里程计的鲁棒性和精确性，研究者们提出了许多改进方法来解决特征提取的问题.Horache 等7提出利用多尺度架构和无监督转移学习的 3D 点云配准方法，实现高精度的特征提取和匹配.Serafin 等8提出了一种使用点邻域的主成分分收稿日期：2023-03-30资助项目：国家重点研发计划课题(2021YFB3900803)；新一代北斗室内外高精度无缝导航通用终端研发(226130417A)通信作者：李一凡E-mail：第 48卷第4期全球定位系统Vol.48，No.42023年8月GNSS World of ChinaAugust，2023析(principalcomponentsanalysis，PCA)方法，利用该方法得出数学原理的线和平面特征.Abedini 等9提出引入激光雷达点云的强度数据，利用尺度不变特征变换(scaleinvariantfeaturetransform，SIFT)算法从航空图像中提取不同的特征.另外，Arshad 等10提出利用仿射尺度不变特征变换、线性方程、粒子群优化等方法提取优化激光里程计特征提取算法.引入更多数据和更复杂的算法可以有效提高激光里程计算法中特征提取和匹配的准确性和鲁棒性.然而，这些复杂的算法和需要处理的更多数据会对处理设备的要求产生较大压力，难以在轻量化平台上应用.为了解决上述问题，本文提出了一种基于点云分割的激光里程计算法.该算法通过分析激光雷达点云数据特征，有针对性地对激光雷达点云进行分割并赋予标签，并根据标签与物体扫描情况提出了对应的特征提取和匹配方法，从而提高了特征提取的准确性和里程计的精确性，并且占用较少的计算资源.1激光里程计如图 1 所示，基于特征的激光雷达里程计算法通常包括三个模块：点云分割、特征提取、特征匹配.点云分割将点云聚类并赋予点标签.特征提取从点云中提取直线和平面这两种特征点.随后，每个直线和平面特征点会被分别投影到地图特征点云中进行匹配，并计算点到直线的距离和点到平面的距离与位姿有关，累加这些距离的和即为与位姿相关的误差函数，最小化误差函数以得到激光里程计.点云的特征提取是指对激光雷达获取的点云数据进行处理，从中提取出具有定位特征的点.准确性和鲁棒性对于激光里程计的性能至关重要.本文针对机械式激光雷达的工作原理及所得点云特点，提出了一种新的点云分割算法，该算法具有计算简单、计算量小的优势，并且能够判定并提取的特征点受噪点影响较小，具有更好的定位特性.1.1 点云分割在特征提取前利用点云分割将点云分割、聚类并赋予标签11.点云分割阶段接收激光雷达获取的点云数据，通过分割算法将点云分割成若干个子集，并为每个子集标记一个标签.特征提取阶段基于分割结果提取特征点云，通过对点云分割所得标签判定物体扫描状态，保留符合条件的特征点云，同时剔除噪点.相较于传统的特征提取算法，本文提出的算法可以提高特征提取的准确性和鲁棒性，并且能够处理大规模点云数据.由于激光雷达机器本身的固有特性，其依靠内部多对激光发射器 360水平旋转扫描周围环境，以获取深度信息并形成点云.如图 2 所示，激光雷达的垂直分辨率取决于其扫描线数.举例来说，常用的 16 线激光雷达垂直分辨率仅为 2.但由于激光发射器的高发射频率，其水平分辨率可以高达 0.18.因此，通过对扫描线点云进行水平角度分割，可以更加精确地进行处理点云.+15+13+1011315图 2 激光雷达工作原理示意图由于激光雷达点云数据中每个点的存储顺序与扫描顺序相同，因此只需遍历激光雷达点云中的每一个点，并使用式(1)计算其俯仰角度.然后，根据俯仰角度将点云按照扫描线进行划分.例如针对16 线激光雷达，可以按照图 2 所示的扫描角度将点云划分为 16 份扫描线点云.=arctan(|z|x2+y2)(1)x、y、z式中，分别为点在激光雷达坐标系下的三维坐标.激光雷达点云点云分割点云标签特征提取直线特征点云平面特征点云特征提取特征提取点到直线的距离点到平面的距离最小化误差函数位姿图 1 基于特征的激光里程计流程框图38全 球 定 位 系 统第48卷后续的点云分割需使用点云的水平扫描角度，为降低计算资源的消耗，利用式(2)计算了点云的水平角度=arctan(xy)+90(2)(90,90)(x,y)由于反正弦函数取值范围为，因此当处于第三、四象限时还需增加 180.在激光雷达运转时，同一扫描线上相邻扫描点的角度间隔极小(0.18)，因此在激光雷达探测物体距离较近时，相邻点仅存在两种情况：1)两点为同一物体上的点；2)两点为不同物体上的点.如图 3 所示，通过计算相邻扫描点连线所成向量，与两点中较远距离点坐标向量所成夹角，可以判断相邻点是否属于同一物体12.以相邻点 A、B 为例，可以根据它们的水平扫描角度差和三维坐标进行计算，得到dA=x2A+y2A+z2AdB=x2B+y2B+z2BAB=arctandAdBcos ABdBsin AB(3)x、y、zAB式中：分为点在激光雷达坐标系下的三维坐标；为利用式(2)计算所得 A、B 两点水平角差值.激光雷达BCABACB图 3 激光雷达扫描线俯视示意图为了将点云分割为不同的物体，本文按扫描线遍历点云中的所有点，并计算每个点与其下一点之间的夹角.如果该夹角大于设定的阈值(在本文中为30，即连续两点横向变化超过深度变化的两倍)，则将这两个连续的点视为不属于同一物体.基于相邻点是否属于同一物体，本文将同一物体上的点标记为内点，而不同物体上距离更近的点则被标记为物体边缘点，距离稍远的点则标记为离散点.通过这种方式，本文可以完成点云的分割.1.2 特征提取激光雷达通过发射激光并接收反射的激光测距形成点云，而由于激光会被物体遮挡，因此物体被扫描的区域会随着激光雷达的运动而变化.如图 4 所示，黄色定位特征被激光雷达扫描的情况主要包括两种：a)完全扫描；b)部分扫描.由于激光雷达是按一定顺序旋转扫描，物体被部分扫描时还需要区分左部分、右部分两种情况，可通过遍历扫描线中的每一个点，根据点云分割中赋予的标签与表面点的顺序判定左、右部分扫描的情况.激光雷达激光雷达激光雷达(a)完整扫描(b)部分扫描图 4 物体扫描情况示意图在城市场景中，物体通常呈现出结构化的标准形状，例如墙壁、汽车、房屋等可视为长方体、棱锥等的组合.物体的边缘可视为直线，表面则可视为平面.被扫描区域较小的、或本身较小的物体难以从中提取准确的定位特征，因此仅从横向扫描长度大于一定阈值(防止较小物体左右边沿点相互干扰，设定最邻近搜索的距离为 0.5m)的物体提取其边缘点为直线特征，表面点为平面特征.而城市中含有许多横向较小的物体，例如电线杆、树干等，它们也是不可或缺的定位特征，可视为圆柱体，横截面视为圆.因此对于完整扫描但物体横向距离较小(与较小物体判断相对应)的情况，若其表面点数量大于 5(圆心拟合最少5 个点可以判定拟合误差)，则认为点云中存在足够的点用于拟合，以保证拟合的可靠性.则将所有通过点带入并优化最小二乘式f=i=bi=a(xcxi)2+(ycyi)2R2)2(4)xc、ycxi、yi式中：a、b 分别为在物体扫描点最小、最大点的点云中的序号；是所拟合圆心的坐标，；为点第4期李一凡，等：基于点云分割的激光里程计算法39R的坐标；为所拟合圆心的半径.若优化上式所得圆的半径小于物体两边边缘点之间的距离，即所扫描物体具有圆柱属性且扫描区域大于横截面的三分之一，则认为拟合成功，并将圆心视为直线特征点.除上述符合条件的点作为定位特征点以外，其他点均视为离散点，不作为定位特征点，仅用作建图.特征提取的具体流程如图 5 所示.输入点云物体标签寻找下一物体分扫描线遍历点云物体标签否否否内点数量是否大于5否物体是否有左右两个边沿点提取边沿点为直线特征点所有内点为平面特征点左右边沿点距离是否小于0.5 m圆直径是否小于左右边沿点距离是是是取圆心为直线特征点将该物体所有点拟合成圆图 5 特征提取流程图 1.3 特征匹配在 LOAM 算法中，特征点的匹配分为两个步骤.首先，将当前时刻点云中的特征点投影到历史点云上，形成一组候选匹配点.然后，根据一定的距离阈值，选择与每个候选点最近的点作为最终匹配点.特征点的匹配质量将影响到后续的位姿估计和地图构建质量，因此需要考虑匹配的可靠性和效率.本文也采用了类似的特征点匹配策略，对当前时刻点云中提取的每一个特征点进行候选匹配点的搜索，进而确定最终匹配点.为了提高匹配效率和精度，本文对匹配点的搜索进行了优化，使用了 KD-Tree等数据结构，加速匹配过程.假定激光雷达为均速运动，特征点的投影公式为Pi=PiRk1+tk1(5)PiPiRk1、tk1式中：为投影至地图的位姿；为特征点的三维坐标；分别为上一帧激光雷达的姿态变化矩阵和位移.对于直线特征点，先将其投影到地图的坐标系下，然后利用 KD-tree 在地图直线特征点云中搜索最邻近的 5 个点.接着，采用 PCA 分析这 5 个点所构成的矩阵的最大特征值.如果最大特征值远大于第二大特征值，就可视为最大特征值所对应的向量是该 5 个点所拟合直线的方向.否则应该舍弃该特征点.该直线必过经过该 5 个点的平均坐标点，由此可求得点到直线的距离为di=(PiPi)vi?PiPi?(6)diPivi式中：为点到直线的距离；为 5 个点的平均坐标向量；为最大特征值对应的特征向量；同理，平面特征点也投影到地图坐标系下，然后利用 KD-tree 在地图直线特征点云中搜索最邻近的5 个点.由于 PCA 算法耗时长，且平面特征点较多，因此采用最小二乘法将该 5 个点拟合为一个平面.通过最小化带入 5 个点的坐标，最小化 S 为S=l=5l=0(a0 xl+a1yl+a2zl)2Ax+By+Cz+D=0a0=AC,a1=BC,a2=DC(7)A、B、C、Dxl、yl、zl式中：S 为待优化的量；为平面标准方程的四参数；分别为点的三维坐标值.即可得到标准平面方程，由此可计算点到平面的距离为dj=?A xj+B yj+C zj+D?A2+B2+C2(8)dj xj、yj、zj式中：为点到平面的距离；为地图特征点的三维坐标；将所有特征点中点到直线的距离、点到平面的距离累加即可得到关于位姿的误差函数E(Rk,tk)=i=mi=0di+j=nj=0dj(9)将该误差函数最小化即可得到当前激光雷达姿态、位移的最优估计.2实验验证为了验证本文提出的激光里程计算法的有效性、精确性和鲁棒性，本文进行了实验验证.使用开源的 KITTI 数据，同时在真实场景中模拟导致算法漂移的复杂情况，以对比本文提出的基于 LOAM 算法和点云分割的激光里程计算法的性能.40全 球 定 位 系 统第48卷 2.1 开源数据验证KITTI00 数据集是在典型的城市环境中采集的，该环境的特征丰富且场景简单，因此 LOAM 算法和本文提出的算法都能够鲁棒地运行，里程计结果如图 6 所示.本文提出算法所得轨迹与基准轨迹的平均误差为 3.3m，误差始终保持较小水平，且最终误差为 11.43m，而 LOAM 算法的里程计平均误差为6.8m，轨迹因误差累计导致逐渐偏离基准，最终误差为 62.17m.由此可得，与 LOAM 算法相比，本文提出的基于点云分割的激光里程计算法更具精确性.LOAM算法本文算法基准算法50045040035030025020015010050050300 x/my/m25020015010050050100150200250300350图 6 实验验证硬件平台图 2.2 真实实验验证图 7 为搭建的一个简易组合导航系统硬件平台.该硬件平台包括 16 线激光雷达、低成本微机械陀螺仪(microelectromechanicalsystems，MEMS)、高精度光纤陀螺、高精度双天线 GNSS等传感器以及一台计算机.实验运行环境为 Ubuntu16.04+ROS 环境.平台记录了真实的激光雷达实验数据包，并通过仿真验证了本文算法的特征提取和激光里程计运算结果的优越性.同时，本文使用双天线高精度 GNSS 和高精度光纤陀螺的组合导航结果作为激光里程计的基准，以评估算法的精确性.如图 8 所示，实验验证环境为典型的城市环境，道路两旁包含楼房、广场以及树林等场景，能够良好、全面的检验算法的有效性.如图 9(a)所示，在房屋楼梯处，场景特征非常复杂，且存在大量难以提取的复杂定位特征.本文用绿色框标记出可用于定位的区域位置，由图 9(b)与图 9(c)的对比可知，本文提出的基于点云分割算法所提取的特征具有良好的直线特征，且特征清晰、离散点少，因此本文算法在特征提取方面优于 LOAM算法.此外，本文所提出的特征提取部分计算量更小，经实验验证，在该数据集下的特征提取平均耗时为23.46ms，而 LOAM 算法为 35.43ms，本文算法更具实时性.如图 10 所示，当机器人穿越这个复杂环境时，本文基于点云分割的激光里程计算法在复杂环境激光雷达双天线 GPS单天线 GPS低成本 MEMS光纤陀螺图 7 实验验证硬件平台图图 8 真实实验场景俯视图(a)真实场景(b)LOAM算法特征提取结果(c)文本算法特征提取结果图 9 特征提取对比图第4期李一凡，等：基于点云分割的激光里程计算法41下依旧鲁棒运行，而 LOAM 算法发生了较大的漂移现象.由此可得，较 LOAM 算法，本文提出的基于点云分割的激光里程计算法更具鲁棒性.x/my/m150200100 50050100150250200150100500LOAM算法文本算法基准轨迹失效处图 10 里程计轨迹对比图3结束语本文针对机械式激光雷达点云横向分辨率远高于竖直分辨率的特点，提出了一种根据扫描线分割点云并为其赋予标签的方法.通过标签的赋予，可以判定物体的扫描情况，从而提取出点云中的直线和平面特征点.最后，将这些特征点投影并搜索匹配地图特征点，并分别拟合直线或平面.利用点到直线的距离和点到平面的距离，可以计算位姿误差方程，从而得到对激光里程计的最优估计.本文利用 KITTI 数据集和真实实验数据，通过 ROSBAG 数据录影并回访进行了仿真验证，结果表明本文提出的基于点云分割的激光里程计在特征提取方面比 LOAM 算法更准确，并且具有更高的定位精度和鲁棒性.参考文献 王朋,郝伟龙,倪翠,等.视觉SLAM方法综述J/OL.(2022-12-26)2023-02-21.北京航空航天大学学报,2023:1-9.https:/ technologiesJ.Machine vision&applications,32016,27(7):1005-1020.DOI:10.1007/s00138-016-0784-4ELHOUSNI M,HUANG X M.A survey on 3D LiDARlocalization for autonomous vehiclesC/IEEE IntelligentVehicles Symposium(IV),2020.DOI:10.1109/IV47402.2020.93048124周治国,曹江微,邸顺帆.3D激光雷达SLAM算法综述J.仪器仪表学报,2021,42(9):13-27.5ZHANG J,SINGH S.Low-drift and real-time LidarodometryandmappingJ.Autonomousrobots,2017,41(2):401-416.DOI:10.1007/s10514-016-9548-26HORACHES,DESCHAUDJ-E,GOULETTEF.3Dpointcloudregistrationwithmulti-scalearchitectureandunsupervised transfer learningC/International Conferenceon3DVision(3DV),2021.DOI:10.1109/3DV53792.2021.001427SERAFINJ,OLSONE,GRISETTIG.Fastandrobust3Dfeature extraction from sparse point cloudsC/IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems(IROS),2016.DOI:10.1109/IROS.2016.77596048ABEDINI A,HAHN M,SAMADZADEGAN F.Aninvestigation into the registration of LIDAR intensity dataandaerialimagesusingtheSIFTapproachC/InternationalArchivesofthePhotogrammetry,RemoteSensingandSpatialInformationScienceConference,2008:169-175.9ARSHAD S,KIM G-W.Role of deep learning in loopclosure detection for visual and lidar slam:a surveyJ.Sensors,2021,21(4):1243.DOI:10.3390/s2104124310方国润.三维点云分割研究综述J.计量与测试技术,2021,48(7):52-55.11BOGOSLAVSKII,STACHNISSC.Fastrangeimage-basedsegmentation of sparse 3D laser scans for onlineoperationC/Proceedings of the IEEE/RSJ InternationalConferenceonIntelligentRobotsandSystems,2016.DOI:10.1109/IROS.2016.775905012作者简介李一凡(1996)，男，硕士，助理工程师，研究方向为激光雷达同时定位与建图技术.杜世通(1991)，男，博士，工程师，研究方向多元融合定位技术.李爽(1990)，男，博士，工程师，研究方向为视觉定位技术.42全 球 定 位 系 统第48卷Laser odometry algorithm based on point cloud segmentationLIYifan1,2，DUShitong1,2，LIShuang1,2，HUANGLu1,2，YANGZihan1,2，CHENChong1,2(1.The 54th Research Institute of China Electronics Technology Group Corporation,Shijiazhuang 050081,China;2.State Key Laboratory of Satellite Navigation System and EquipmentTechnology,Shijiazhuang 050081,China)Abstract：Inthispaper,weproposeafeatureextractionandmatchingmethodbasedonpointcloudsegmentationtoaddresstheproblemsofinformationredundancyandinterferenceofdiscretepointsinlaserodometryalgorithms.ByutilizingthehorizontalrotationscanningcharacteristicsofmechanicalLiDAR,thepointclouddataissegmentedandclusteredbyscanlines,andedgepointsareextractedaslinefeaturesandsurface points are extracted as surface features.Compared to traditional feature extraction methods,ouralgorithmcaneffectivelyextractfeaturepointswithlessdistinctivenessandeliminatediscretepoints.Thealgorithmnotonlyreducesthecomputationalcomplexitybutalsoimprovestheaccuracyandrobustnessofpositioning,which has great application prospects in robot navigation tasks.The performance of thealgorithmisvalidatedthroughcomputersimulationandachievedgoodexperimentalresults.Keywords：LiDAR;pointcloud;segmentation;featureextraction;featurematching;odometer(上接第 29 页)Overview of characteristics and development of shadowed space positioningand navigation technologyBAOYachuan1,2，YANGMenghuan1,2，LIJianjia1,2，LIJun1,2(1.The 54th Research Institute of China Electronics Technology Group Corporation,Shijiazhuang 050081,China;2.State Key Laboratory of Satellite Navigation System and EquipmentTechnology,Shijiazhuang 050081,China)Abstract：The long and narrow closed physical structures and complex channel characteristics ofshelteredspaces,suchasundergroundpipecorridorsandbuildinginteriors,poseasignificantchallengetohigh-precision positioning and navigation.This article combs and analyzes the adaptability of typicalscenarios and technical means in sheltered space,and quantifies the challenges faced by high-precisionnavigationandpositioninginshelteredspace.Thefuturedevelopmentofshelteredspacepositioningsystemsis prospected,and a high-precision positioning and navigation system architecture suitable for complexenvironments in sheltered space is designed.The network diagram is integrated.A hybrid positioningnetworkisbuiltbasedonmultimodesystemssuchasultrawideband(UWB),BeiDoupseudo-satellite,andnear-ultrasonic positioning to achieve full scene coverage in sheltered space.Compared with currentlycommonlyusedradiopositioningsystems,ithasachievedsignificantimprovementsinpositioningaccuracy,coverageperformance,andotheraspects.Keywords：shelterspace;scenarioanalysis;meansanalysis;technicalchallenges;systemarchitecture第4期李一凡，等：基于点云分割的激光里程计算法43