煤矿井下顾及特征点动态提取的激光SLAM算法研究.pdf

241mineof feature points in underground coal mineJ.Safety in Coal Mines,2023,54(8):241-246.移动扫码阅读DONG Zhihua,YAOWanqiang,LIN Xiaohu,et al.LiDAR SLAM algorithm considering dynamic extraction全0:241-246董志华，姚顽强，蔺小虎，等.煤矿井下顾及特征点动态提取的激光SLAM算法研究 JI.煤矿安Safety in Coal Mines2023年8 月Aug.2023No.8煤矿发全Vol.54第8 期第5 4卷D0I:10.13347/ki.mkaq.2023.08.033煤矿井下顾及特征点动态提取的激光SLAM算法研究董志华，姚顽强，蔺小虎，郑俊良，马柏林，高康洲（西安科技大学测绘科学与技术学院，陕西西安7 10 0 5 4）摘要：针对煤矿井下无GNSS信号、主流激光SLAM算法易出现特征约束不足而导致退化问题，提出了一种面向煤矿井下环境的激光雷达、IMU紧耦合SLAM算法。首先，设计一种动态提取特征点方法，通过检测煤矿井下环境是否发生退化，动态调整特征点提取数量，构建丰富且良好的特征信息约束矩阵，提高位姿估计准确性；然后，利用因子图优化实现煤矿井下稳健精确的SLAM；最后，通过煤矿井下实测数据进行了广泛的试验分析。结果表明：提出的激光SLAM算法表现较好，位姿估计误差在平面方向较LI0_SAM降低了5 0.93%，在高程方向降低了42.13%，可为煤矿机器人智能感知、安全巡检提供了技术参考。关键词：煤矿机器人；激光SLAM；动态阅值；特征提取；智能感知中图分类号：TD175文献标志码：A文章编号：10 0 3-496 X(2023)08-0241-06LiDAR SLAM algorithm considering dynamic extraction of feature points in underground coalDONG Zhihua,YAO Wanqiang,LIN Xiaohu,ZHENG Junliang,MA Bolin,GAO Kangzhou(College of Geomatics,Xian University of Science and Technology,Xian 710054,China)Abstract:Aiming at the problem that there is no GNSS signal in the coal mine,the state-of-the-art LiDAR SLAM algorithm isprone to degenerate due to insufficient feature constraints,a tight coupling SLAM algorithm of LiDAR and IMU for the coal mineenvironment is proposed.First,we design a dynamic feature point extraction method,by detecting whether there is degradation inthe underground environment of coal mine to dynamically adjust the number of feature points extracted,build a rich and goodfeature information constraint matrix,improve the accuracy of pose estimation;then,the factor diagram optimization is used torealize the robust and accurate SLAM in the coal mine.Finally,a wide range of experimental analysis is carried out through themeasured data in the coal mine.The results show that the proposed laser SLAM algorithm performs well,the pose estimation erroris reduced by 50.93%in the horizontal direction and 42.13%in the vertical direction compared with LI0_SAM,it can providetechnical reference for intelligent perception and safety inspection of coal mine robots.Key words:coal mine robots;LiDAR SLAM;dynamic threshold;feature extraction;intelligent perception煤矿智能化是实现煤矿产业转型和解决煤矿安全开采问题的关键 1-2 。在煤矿智能化趋势下，煤矿机器人系统设计、机器人自主导航定位技术成为重要的研究课题 3 。实时定位与建图 4(Simultaneous Lo-calizationandMaping，SLA M）是实现煤矿井下机器人自主导航定位的基础，而机器人利用各种传感器，在三维空间中进行6 DOF状态估计，是实现高性能SLAM的关键5。国内外学者对基于激光雷达的SLAM收稿日期：2 0 2 2-10-2 8责任编辑：李力欣基金项目：国家自然科学基金资助项目（42 2 0 148 4）作者简介：董志华（1997），男，安徽毫州人，硕士研究生，研究方向为激光雷达与IMU融合SLAM算法。E-mail:242Safety in Coal Mines2023年8 月Aug.2023第5 4卷No.8Vol.54煤砺发全第8 期算法研究已趋于成熟 6-9。由于激光雷达具有测距精度高、不易受光照和视角变化的影响，具有全天候条件下运行的能力，被广泛应用于无人驾驶领域,且更适合复杂多变、光照条件差的煤矿环境中的SLAM。H U BEr 等 使用高精度激光雷达扫描仪建立高精度煤矿三维地质模型，但此方法需要点云的后处理，不能满足实时定位与建图需求，且成本较高；REN等 12)研究了基于广义ICP(GICP)的轻量级闭环检测和优化算法，并提出了基于GICP三维点云配准的SLAM优化方法，但其精度仍有待提高。作为目前最先进的算法之一,LIO_SAM13通过引人激光雷达关键帧的滑动窗口来限制计算复杂度，利用因子图进行联合IMU和激光雷达约束优化，以获得更高的鲁棒性，它是一种基于因子图优化的融合框架，适用于多传感器融合。杨林等 14 提出了一种适用于煤矿井下的激光雷达、IMU融合SLAM算法，在因子图优化框架中添加了回环检测和地面约束，优化了相邻关键顿之间的相对位姿，减少累积漂移，提高了定位精度。尽管激光雷达性能优越，但结合煤矿井下的特殊环境存在空间非结构化、环境复杂、空间狭窄 15 等实际情况，这会给激光SLAM带来退化、漂移等新的挑战。为此，针对煤矿井下实时定位与建图需求，提出一种特征点动态提取的激光SLAM算法，该算法根据煤矿巷道实际环境，在线评估退化程度，并自适应调整特征点提取数量，在退化环境中充分利用特征点信息，提高点云配准的精度；在因子图优化中采用了自适应关键顿选取方法，根据前后2 帧的位姿差选取关键顿；并通过试验验证了该算法在煤矿井下应用的有效性。1总体框架算法框架如图1。动态提取特征点闯值调整退化检测激光雷达畸变校正特征提取特征匹配里程计因子IMUIMU预积分IMU预积分因子因子图构建全局轨迹闭环因子及优化全局地图局部地图图1算法框架Fig.1Algorithmframework提出的基于激光雷达与IMU紧耦合SLAM算法，结合了煤矿井下环境特点，在紧耦合SLAM基础上，增加特征点动态提取算法，在缺少特征约束的煤矿井下增加边缘特征点的提取数量，达到增加特征约束、提高位姿估计与建图精度。同时在因子图优化中使用自适应关键顿选取方案，通过设置2 顿点云间位姿变换阈值，自适应选取关键帧，提高算法的实时性。1)IMU预积分。根据一定采样间隔利用IMU运动方程对角速度、线加速度进行积分，获取激光点云对应时刻位姿。2)畸变校正与特征点提取。利用IMU预积分得到相对运动位姿纠正激光点云的运动畸变，并利用深度图像提取边缘点、平面点。3)特征匹配与位姿估计。利用Scan-map匹配方式进行匹配结算，组成激光里程计因子，并将其与IMU预积分因子加入局部地图匹配进行优化，实时纠正激光里程计解算的姿态。4)因子图构建及优化。实际上是对位姿估计优化，利用激光里程计因子、IMU预积分因子、闭环因子更新所有关键顿因子。5)闭环检测。执行Scan-map优化，调用ICP匹配方法，得到优化后位姿，延迟更新当前顿位姿，生成闭环因子，传递给因子图更新。6)全局地图构建。接收来自因子图优化位姿图对应的三维点云，对点云进行拼接，得到全局一致轨迹与地图。2前端里程准确估计激光点云帧间的相对位姿是构建局部243Safety in Coal Mines2023年8 月Aug.2023煤矿发全No.8第8 期Vol.54第5 4卷地图的必要前提。首先使用IMU预积分位移信息消除点云位移畸变，使用IMU输出的姿态信息消除点云旋转畸变。对纠正后的点云进行特征点提取，采用基于边缘特征和平面特征的匹配方式，利用IMU预积分提供的位姿预测值为匹配提供初值，采用Scan-map匹配进行位姿解算。2.1特征点动态提取考虑到煤矿井下局部环境特征信息不足，固定阅值提取边缘点特征会使得1顿点云丢失许多特征信息，而自适应的动态值特征点提取方法可以在特征地物丰富时获得更多良好边缘特征点，同时在发生退化时利用较多的特征信息可以提供更为鲁棒的局部位姿估计。1)点云投影。将单扫描获取的激光点云集p=(p1,P2,pn投影到深度图像(c，r)当中,其中每一个深度点在深度图像中都有对应的像素位置，C、r分别为图像的列数和行数，图像深度值为点云到雷达坐标系原点的欧氏距离。2)曲率计算。计算点云的曲率时，使用与该点位于同一扫描线上左右5 个点的深度差来计算，以减少计算开销。特征点位于平面时，平面上的点距离远点的距离是近似相同的，则曲率差的平方和会是1个很小的值，相反，特征点位于平面突起的位置，则突起位置的点与周围点的深度差值较大，则深度差的平方和是较大的值。C;=Z.R-R.(1)ljes,jti式中：ci为第i点的曲率;R；为第i点的深度；S为第i点左右两侧各取5 个点组成的点集；R为该点集中点的深度。3)特征点动态提取。计算每个点的曲率后，为保证提取的特征点分布均匀，需要控制特征点的分布。VLP-16激光雷达每条扫描线可以覆盖水平360的范围，将其点云均匀分成6 份，即每份为6 0 的覆盖范围，则每份最多包含3 0 0 个点。遍历点云并按照曲率值大小进行排序，设置边缘点曲率阈值Teonter和平面点曲率阅值Tsuf。提取曲率小于Taur的点作为候选平面特征点，考虑到环境中的平面特征点数量较多，这里不对平面特征点数量进行限制，对所有曲率小于阈值的候选点进行体素滤波，作为最后提取的平面特征点。提取曲率大于Tomnter的点作为候选边缘特征点N,参与特征匹配的边缘特征点最大提取数量为M。根据ZHANG16退化判断理论在SLAM算法基础上额外增加了退化在线判断，退化程度的大小可通过退化因子入来判断（入越小代表退化越严重），行进中退化因子变化如图2。250斤一退化因子分布一经验阅值20015010050050100150200250300350400450时间/s图2 行行进中退化因子变化Fig.2Variation of the degeneration factor in progress因此，根据经验设置发生退化阈值入h=100。若入入h，将从候选边缘特征点中选取5 0%曲率较大的作为边缘特征点（即，M=0.5N,0.5为经验值）。否则，将使用8 0%曲率较大的候选边缘特征点数量作为最大提取数量阅值。通过对每一条扫描线每一份点云进行上述处理，即可得到均匀分布的特征点。将边缘特征点集记作F。；平面特征点集记作F，平面特征点没有进行数量限制，其数量往往多于边缘点。2.2基于特征匹配的位姿解算特征点提取后，需要进行特征关联，构造位姿优化的残差函数，采用基于主成分分析（PrincipalComponentAnalysis,PCA)算法进行线、面特征拟合。基于特征的匹配算法是将点到线、点到面的距离作为优化的残差函数，通过最小化点到线、点到面距离的平方和求解最优位姿变换。设求解的平移旋转参数为x，由点到线、点到面的距离构造优化目标残差函数可以表示为：f(pE,x)=de(2)f(ps,x)=ds(3)式中：Pe为边缘点集；de为边缘点集对应的点到直线的距离；ps为平面点集ds为平面点集对应的点到直线的距离，可进一步表示为：f(p,x)=d(4)式中:p为所有的特征点;d为所有的特征点对应的点到线、点到面的距离。式（4)为最终优化的目标残差函数。L-M位姿估计如图3。244SafetyinCoalMinesAug.20232023年8 月No.8Vol.54第8 期第5 4卷煤矿发全侧向变化量边缘特征点匹配前向变化量航向角前端位姿变换垂向变化量平面特征点匹配横滚角俯仰角图3 L-M位姿估计Fig.3L-M pose estimation通过上述构造点到线、点到面的距离残差函数，对其进行优化，解算出位姿变换。求解坐标转换的雅可比公式，使用L-M2步位姿估计法（图3)进行求解。先利用平面点解算出垂直方向变化量、横滚角、俯仰角，再利用边缘特征点解算出侧向变化量、前向变化量、航向角，最终联合2 步优化获取相邻顿的位姿变换3后端优化及地图构建算法侧重于SLAM系统的定位方法，因此对全局位姿进行优化，直接对局部地图进行拼接完成地图构建，保证后端运算效率。由于LiDAR数据量过于庞大且余，仅选取具有代表性的关键帧作为优化估计状态量；不同于固定距离、固定时间选取关键顿，把位姿变化(平移与旋转变化)超过一定阈值的点云顿选取为关键帧，小于阈值的被忽略。定义X为t时刻的所有关键帧，其对应的状态量为Xi=1x:ltex;其对应的所有观测量为 Z=(P,lenl(a)ex.。状态估计问题可描述为：在给定观测信息Z和先验信息p（X。）的条件下，预测X。的后验概率问题，即：p(Xi/Z)p(X o)p(Z/X h)=p(Xo)Ip(P,lc)/Xa)(5)(ij)eF因子图优化模型如图4。该模型包含3 种因子：激光里程计因子、IMU预积分因子以及闭环因子。其中,对到时刻的IMU数据进行预积分，得到预积分因子，添加到因子图中。随着载体的运动，激光雷达关键顿对应的状态节点不断被加入因子图中，与此对应的闭环因子也被加人因子图中，完成因子图的构建。将激光里程计因子、IMU预积分因子及闭环因子作为边更新状态节点。每当因子图中插人新的节点，就对整个因子图优化计算1次，更新当前时刻的位姿估计。采用g2o优化库，对最终建立的位姿图进行优IMU观测值雷达顿关键顿状态节点里程计因子IMU预积分闭环因子IMU预积分因子位姿递推匹配图4因子图优化模型Fig.4Factor graph optimization model化。当优化了整个LiDAR运动的位姿图后，将位姿节点对应的三维点云进行拼接，进而得到全局一致的轨迹和地图。4试验分析为验证算法的有效性，自主搭建适用于煤矿井下的移动机器人数据采集平台，该平台集成了VelodyneVLP-16、El l i p s e N2 IM U 传感器，并对Li-DAR、I M U 传感器进行时空同步标定。试验巷道场景整体为小型回环，总长2 8 0 m，巷道宽度3 m。但由于巷道环境复杂，机器人小车仅能贴着巷道一侧行进，且巷道中完全无GNSS信号，因此使用测量仪器观测出7 个检查点作为评价轨迹精度的参考4.1特征点动态提取试验分别使用动态特征点提取算法与原算法对巷道内的数据进行了处理，在同一场景、同一时刻下比较两者提取的特征点数量变化情况，特征点提取算法对比如图5（紫红色表示1顿点云，绿色代表提取的边缘特征点）。结果显示：特征点动态提取方法（图5（b）)在同一场景下中提取到了更多良好边缘特征点，墙壁与地面交线与墙壁标识牌等曲率较小的边缘特征信息均被提取出来，相比于固定阈值提取方法，局部激光点云边缘特征点提取数量与质量均有提高。为局部地图位姿估计提供良好特征信息4.2位姿估计精度与地图定性分析因巷道下无GNSS信号，无法得到准确行进轨迹，故在观测条件良好巷道转弯处内设置7 个检查点，用于对位姿估计的精度验证。平面误差统计结果见表1，高程误差统计结果见表2。245.SafetyinCoalMines2023年8 月Aug.2023煤砺发全No.8第8 期Vol.54第5 4卷(a)LIO-SAM(b)Our图5 特征点提取算法对比Fig.5Comparison of feature point extraction algorithms表1平面误差统计结果Table1Statistical results of horizontal error算法最小值/m最大值/m平均值/mRMSE/mLeGO-LOAMN/AN/AN/AN/ALIO-SAM0.0410.5300.3820.432Our0.0520.4600.2360.212表2 高高程误差统计结果Table2Statistical results of vertical error算法最小值/m最大值/m平均值/mRMSE/mLeGO-LOAMN/AN/AN/AN/ALIO-SAM0.0134.0120.7871.417Our0.0122.6830.4740.820试验结果表明：所提出的算法平面中误差为0.212m，高程中误差为0.8 2 0 m。两算法在垂直方向上的累计误差较大，提出的算法最大偏差为2.6 8 3m,LIO-SAM最大偏差为4.0 12 m。该算法相较于LIO-SAM算法在平面和高程方向上的累积偏差均有不同程度减小。验证了其位姿估计精度的提高。由于LeGO-LOAM算法的失败，因此只对提出的算法与LIO-SAM算法进行轨迹的精度评定。由于场景的行进路径宽度均不足1m，因此机器人小车两侧的扫描范围被极大地限制，提取到的特征信息也相应减少，这就导致了两算法在平面、垂直方向累积误差较大。根据3 种不同的激光SLAM算法分别构建煤矿井下的点云地图，试验煤矿井下巷道为“口字型”闭环结构。地图对比如图6。(a)LeGO-LOAM(b)LIO-SAM(c)Our图6地图对比Fig.6Mapping comparisonLeGO_LOAM由于其算法特点，较依赖地面点云的提取，一旦地面点云提取失败，就会造成位姿估计的失败，由图6(a)可观察到：LeGO-LOAM算法在巷道试验中出现明显断裂现象，地图构建失败。LIO_SAM算法是在LeGO_LOAM基础上增加了IMU预积分因子作为约束信息，并通过因子图优化模型优化位姿估计，进而获得还原度较高的点云地图，相较于LeGO_LOAM算法有很大进步。由图6（b）可观察到：LIO_SAM算法构建出了较为完整的全局地图，但放大可见地图点云噪声较多，点云地图质量较低。由图6(c)可观察到：提出的激光SLAM算法点云地图构建整体结果良好，且放大后的点云噪声较少，墙体点云较薄。由此可见，该算法相较于LeGO_LOAM与LIO_SAM具有较高的鲁棒性。5结语1)通过在线评估煤矿井下环境是否发生退化，动246.SafetyinCoalMinesAug.20232023年8 月No.8Vol.54煤防发全第5 4卷第8 期态调整特征点提取数量，在保证实时性的同时获得数量更多良好特征点构成特征约束矩阵，为局部位姿精确估计提供基础。2)顾及特征点动态提取的激光SLAM算法能够在煤矿井下环境充分利用已有特征约束信息，提高局部位姿估计精度，有效降低全局误差累积，提高全局位姿估计与建图精度。3)相较于目前主流的激光SLAM算法，提出的算法在位姿估计精度及建图效果均有提升，且能更好应对部分退化场景，可为煤矿井下机器人自主定位、智能感知提供参考。参考文献(References):1 贺海涛.综采工作面智能化开采系统关键技术 J.煤炭科学技术,2 0 2 1,49(S1):8-15.HE Haitao.Key technology of intelligent mining systemin fully-mechanized mining faceJ.Coal Science andTechnology,2021,49(S1):8-15.2王国法.煤矿智能化最新技术进展与问题探讨 J.煤炭科学技术,2 0 2 2,5 0(1)：1-2 7.WANG Guofa.New technological progress of coal mineintelligence and its problemsJ.Coal Science and Tech-nology,2022,50(1):1-27.3王国法，赵国瑞，任怀伟.智慧煤矿与智能化开采关键核心技术分析 J.煤炭学报，2 0 19,44（1）：3 4-41.WANG Guofa,ZHAO Guorui,REN Huaiwei.Analysison key technologies of intelligent coal mine and intelli-gent miningJJ.Journal of China Coal Society,2019,44(1):34-41.4王璐，蔡自兴.未知环境中移动机器人并发建图与定位（CML）的研究进展J.机器人，2 0 0 4（4）：3 8 0-3 8 4.WANG Lu,CAI Zixing.Progress of CML for mobilerobot in unknown environments J.Robot,2004(4):380-384.5马宏伟，王岩，杨林.煤矿井下移动机器人深度视觉自主导航研究 J.煤炭学报，2 0 2 0，45（6）：2 193-2 2 0 6.MA Hongwei,WANG Yan,YANG Lin.Research ondepth vision based mobile robot autonomous navigationin underground coal mineJ.Journal of China Coal So-ciety,2020,45(6):2193-2206.6RUSU R,BRADSKI G,THIBAUX R,et al.Fast 3drecognition and pose using the viewpoint feature his-togram C J/2010 IEEE/RSJ International Conference onIntelligent Robots and Systems.IEEE,2010:2155-2162.7FERNANDES D,AFONSO T,GIRO P,et al.Real-Time3D Object Detection and SLAM Fusion in a Low-CostLiDAR Test Vehicle SetupJ.Sensors,2021,21(24):8381.8ZHAO S,FANG Z,LI H,et al.A robust laser-inertialodometry and mapping method for large-scale highwayenvironments C J/2019 IEEE/RSJ International Confer-ence on Intelligent Robots and Systems(IROS).IEEE,2019:1285-1292.9SHAN T,ENGLOT B,MEYERS D,et al.Lio-sam:Tightly-coupled lidar inertial odometry via smoothingand mapping CJ/2020 IEEE/RSJ International Confer-ence on Intelligent Robots and Systems(IROS).IEEE,2020:5135-5142.10HU Yuhui,WANG Xu,HU Jiaming,et al.An overviewon unmanned vehicle technology in off-road environ-mentJ.Transactions of Beijing Institute of Technology,2021,41(11):1137-1144.11HUBER DF,VANDAPEL N.Automatic three-dimen-sional underground mine mapping J.InternationalJournal of Robotics Research,2006,25(1):7-17.12REN Z,WANG L,BI L.Robust GICP-based 3D LiDARSLAM for underground mining environmentJ.Sensors,2019,19(13):2915.13SHAN T,ENGLOT B,MEYERS D,et al.Lio-sam:Tightly-coupled lidar inertial odometry via smoothingand mappingC J/2020 IEEE/RSJ International Confer-ence on Intelligent Robots and Systems(IROS).IEEE,2020:5135-5142.14杨林，马宏伟，王岩.煤矿井下移动机器人基于激光惯性的融合SLAM算法 JJ.煤炭学报，2 0 2 2,47(9)：3 5 2 3-3534.YANG Lin,MA Hongwei,WANG Yan.LiDAR-inertialSLAM for mobile robot in underground coal mine J.Journal of China Coal Society,2022,47(9):3523-3534.15BARFOOT T D,MCMANUS C,ANDERSON S,et al.Into darkness:Visual navigation based on a lidar-inten-sity-image pipeline M.Springer,2016:487-504.16ZHANG J,KAESS M,SINGH S.On degeneracy of opti-mization-based state estimation problems CJ/2016 IEEEInternational Conference on Robotics and Automation(I-CRA).IEEE,2016:809-816.