矿山环境三维激光雷达SLAM算法建图与定位

张清宇,崔丽珍,杜秀铎,马宝良

(内蒙古科技大学信息工程学院,内蒙古 包头 014010)

近年来,我国煤矿智能化发展迅速,矿山环境下的精确定位导航技术研发更是重中之重。由于煤炭工业的工作环境相对顽劣,安全问题一直制约着煤矿的生产建设。同步定位与地图构建是无人驾驶领域解决恶劣环境建图定位问题的关键技术,三维激光雷达SLAM算法正被广泛应用于煤矿环境的高精度制图工作。

针对煤矿复杂环境,文献[1]设计了一种煤矿巡检机器人,以满足实际矿山探测工作的需求。文献[2]利用地面激光雷达计算复杂地形土方量。文献[3]对煤矿井下巷道的难定位问题提出了一种改进的巷道环境建模算法。文献[4]针对煤矿环境地图构建过程中出现的点云运动畸变提出了点云双边滤波算法。文献[5]研究了煤矿井下无GPS的机器人自主定位问题,实现了三维激光雷达SLAM算法的仿真试验。

对于三维激光雷达SLAM算法的应用与发展,文献[6]提出的LOAM改变了特征提取的方式,通过点云匹配对雷达位姿进行优化。文献[7]提出了迭代最近点ICP算法。文献[8]提出了通过惯性测量单元(inertial measurement unit,IMU)估计运动状态消除激光雷达运动畸变的方法。文献[9]提出的BALM算法,在雷达建图过程中引入光速法平差以降低建图过程中的累积误差。文献[10]提出了激光-惯性里程计相结合的方法LIO。文献[11]提出的LOL方法,将 LOAM和分割场景识别算法相结合。文献[12]提出的LIRO方法,将UWB、雷达和惯导紧耦合。文献[13—14]提出了LeGO-LOAM和LIO-SAM算法,前者在算法的后端添加地面优化且特征提取方式更轻量级,提高了特征点的提取效率;后者将LiDAR里程计因子、IMU预积分因子、GPS因子和回环因子通过因子图优化的方式融合,实现多传感器融合定位。针对激光雷达耗时长、受动态障碍物影响大的问题,文献[15]在闭环检测部分添加了SC-Context。文献[16]提出了FAST-LIO算法,紧耦合迭代卡尔曼滤波和雷达惯性里程计,提高了稳健性。

本文为了解决矿山复杂环境下建图定位的难点,首先对各传感器进行内外参数标定,然后通过巡检机器人采集到的数据,对比多种三维激光雷达SLAM算法,以期验证在矿山环境中多传感器融合算法LIO-SAM具有更好的建图效果与定位精度,并构建在LIO-SAM特征提取部分利用点的强度和环境值消除不必要的特征点的IALIO算法,以期在定位精度上有进一步的提高。

1 传感器参数标定

单一的激光雷达、里程计、惯性测量单元很难单独满足高效、实时、准确、稳健等性能,因此多传感器融合的方式应用于复杂环境下的建图定位是目前SLAM算法主要的发展趋势。

对于LiDAR融合IMU的系统,为解决机器人运动产生的激光雷达运动畸变,需要标定LiDAR与IMU的外参。本文采用LiDAR＿align的方法进行激光雷达与IMU的外参标定,利用NLopt库处理定义冲突问题并改写里程计接口为IMU接口,录制包含大量旋转量和平移量的LiDAR数据及IMU数据,多次标定迭代后取收敛结果最小的参数。

IMU内参未标定时会造成建图的漂移现象甚至无法建图,严重影响定位精度。本文采用的IMU的观测模型中存在两种误差:一种是变化缓慢的零偏b;另一类是变化比较剧烈的测量白噪声n。其中,零偏是由传感器的温度等物理因素产生的传感器内部误差的综合参数;测量白噪声是由于模/数转换器引起的外部噪声,角速度测量值ωo和真实值ω之间的关系为

ωo(t)=ω(t)+b(t)+n(t)

(1)

本文通过内参标定工具imu＿utils进行IMU内参的标定,即标定随机游走噪声和白噪声。通过ceres优化库分析IMU的Allan方差,将IMU保持静止录制2 h的数据包,确定最终的高斯白噪声和随机游走噪声。

2 特征匹配及相对位姿估计

不同SLAM算法的特征提取方式是有差别的,特征点的获取决定了机器人相对位姿数量的多少,直接影响建图效果与定位精度。

LOAM通过计算当前帧点云pk中每个点i的平滑度c,将c最小的点作为平面点,c最大的点作为边缘点,将一帧扫描到的点云分为4段,每段只提取最多4个平面点和2个边缘点,以保证特征点的分布均匀,其中平滑度c的计算公式为

(2)

LeGO-LOAM是在LOAM基础上改进的激光雷达建图方法,与LOAM相比,在轻量级和地面优化部分有了显著提升,可以在平坦路面有效处理点线特征和点面特征,但是由于提取的线面特征较多,会使得累计误差较大。LeGO-LOAM的主要特点包括:①对地面进行分割,减小了特征搜索范围;②提取特征之前进行聚类分析,提高了特征质量;③以帧为单位进行优化,使得全局地图可以多次调整;④增加了回环修正。LeGO-LOAM的地面优化主要是在LiDAR Odometry的特征提取部分,通过使用采集到的地面面特征优化高度和水平角、采集到的线特征优化水平位移和航向角,构建点与线之间、面与面之间的对应关系,具体框架如图1所示。

图1 LiDAR Odometry框架

本身促进两者联动的活动较少。社工机构是让社会工作和志愿服务的联动的主要组织一，在这个载体上可以开展很多活动。但目前，我国社会工作和志愿服务的联动模式和机制都处于探索过程中，还没有完全探索出本土化的联动模式和机制，因此，除了政府购买的社会服务合作项目之外，社工机构很少有能力有主动性去开展合作项目和活动，还是自上而下的项目比较多，有社工机构主导的自下而上的合作项目相对较少，还处于初始阶段。

本文提出LIO-SAM在特征提取部分仅计算每个点的曲率,通过曲率的大小提取角点和平面点,规定当前激光点为pi,利用pi前后的5个点计算当前的曲率K。前后5个点与pi的距离差值为M,曲率K为距离差值的平方,具体计算公式为

M=M[i-5]+M[i-4]+M[i-3]+M[i-2]+

M[i-1]+M[i]·10+M[i+1]+M[i+2]+

M[i+3]+M[i+4]+M[i+5]

(3)

K=M·M=M2

(4)

这种方法没有区分“平面”与“近似平面”,将没有计算分类的点全部加入“近似平面”中,会丢失一部分符合曲率要求的特征点。本文采用的IALIO算法增加了曲率的阈值,利用点的强度和环境删除不必要的点,一定程度上弥补了丢失点的问题,在定位精度上也有一定提高。

3 多传感器融合算法LIO-SAM

LIO-SAM是通过激光雷达、IMU和GPS多种传感器融合,利用因子图优化的方式在因子图的顶端设置了激光雷达惯性里程计,收集不同来源的多种相对测量和绝对测量值作为因子,全部合并到系统中。将激光雷达里程计因子、IMU预积分因子、GPS因子和回环因子全部优化,在矿山复杂环境下能够针对性地应用。

3.1 IMU预积分因子

IMU预积分因子部分的具体流程如图2所示。

图2 IMU预积分因子

其中,Xi表示每个关键帧,线条表示IMU预积分因子,它由两个相邻关键帧之间的IMU测量积分得到。IMU的角速度和加速度的测量值公式为

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

式中,ΔVij、ΔPij和ΔRij分别为时间i至j之间的相对速度、位置和旋转矩阵;Vi和Vj分别为i、j时刻的速度;Pi和Pj分别为i、j时刻的位置;Ri和Rj分别为i、j时刻的旋转矩阵;g为重力加速度;Δtij为i、j时刻的时间间隔。

3.2 激光里程计因子

图3 激光里程计因子

构建得到的局部地图Mi公式为

(13)

3.3 回环检测因子

在后端优化部分,LIO-SAM引入了回环检测,添加了回环因子,回环检测因子如图4所示。

图4 回环检测因子

当因子图中加入一个新的状态Xi+1时,首先搜索整个因子图,找到与之相近的先验状态,X3是返回的候选之一,然后尝试匹配Fi+1到子关键帧{F3-m,…F3,…,F3+m}。本文对LeGO-LOAM算法的回环检测部分添加了Scan-Context描述子,定位精度更优。

4 试验与分析

4.1 试验设备及试验方案

为验证矿山环境三维激光点云SLAM算法建图定位的有效性,采用了集成Velodyne VLP-16 LiDAR、CH110-usb IMU、北斗星通C201 GNSS系统和NVIDAI AGX工控机的巡检机器人,如图5所示。

图5 试验设备

利用巡检机器人,本文选择采集3组场景(街区道路、矿山斜坡、采矿区)进行了数据获取,矿山数据采集地点为哈尔乌素露天煤矿。数据采集前,为实现激光雷达与IMU的时间同步,将雷达数据点云格式由XYZI更改为XYZIRT,并用本文方法对各传感器参数进行标定。在试验区中,街区道路旁有大量树木与房屋,并且有移动车辆和行人的干扰。矿山斜坡具有轻微的坡度,地形较为平坦。采矿区地形复杂多变,有严重的拐角和地面颠簸。3组场景的数据集长度均大于1 km,采集时间均在500 s以上。本文采用轨迹评估工具EVO绘制误差曲线,各组试验轨迹误差对比的真值均为GNSS信息。

4.2 街区道路

街区道路的建图效果与轨迹对比如图6所示,图6 (c)为LIO-SAM算法与GNSS信息的轨迹对比曲线。

图6 街区道路试验

由图6可以看出街区道路试验在拐角处轨迹漂移的现象比较严重,利用轨迹评估工具EVO,得到街区道路下多种SLAM算法的定位误差,见表1。

表1 街区道路SLAM轨迹APE统计结果 m

由表1可知,ALOAM的RMSE为6.267 415 m,采用Eigen库和Ceres优化库直接计算旋转矩阵与雅可比矩阵的方式,能够在街区道路取得更优的定位结果。由于受树木、行人车辆的影响,LOAM单激光与LeGO-LOAM地面优化的方式均没有多传感器融合算法LIO-SAM定位精度高,而在LIO-SAM特征提取部分增加强度特征的IALIO算法,可以提取更多的特征点,定位误差较LIO-SAM降低了20.17%。

4.3 矿山斜坡

矿山斜坡的建图效果与轨迹对比如图7所示,图7 (c)为LIO-SAM算法与GNSS信息的轨迹对比。

图7 矿山斜坡试验

可以看出,在矿山斜坡环境拐弯时会因运动畸变产生轻微的失真漂移现象,受建筑物的影响较小,地形较为平坦,建图效果平滑。利用EVO工具对矿山斜坡环境下多种SLAM算法的定位误差结果见表2。

表2 矿山斜坡SLAM轨迹APE统计结果 m

由表2可知,在矿山斜坡地形环境,LIO-SAM的RMSE为0.811 041 m,相较于ALOAM和LeGO-LOAM分别提高了2.70%和45.28%。因此在矿山斜坡地形环境下,多传感器融合算法LIO-SAM可以有效地解决表面倾斜所造成的运动畸变问题,使建图效果更平滑清晰。

4.4 采矿区

采矿区的建图效果与轨迹对比如图8所示,图8(c)为LIO-SAM算法与GNSS信息的轨迹对比曲线。

图8 采矿区试验

由图8可以看出,采矿区环境巡检机器人在朝一个方向行进时误差累积较大,且在拐角处轨迹偏移量较大,轨迹误差见表3。

表3 采矿区SLAM轨迹APE统计结果 m

由表3可知,在采矿区地形颠簸及周围环境特征不明显的问题下,多传感器融合算法LIO-SAM更能适应复杂环境,具有更好的定位结果,它的RMSE为22.055 274 m,相较于ALOAM和SC-LeGO误差分别降低了21.53%和55.18%。LIO-SAM以局部地图优化的方式可以有效减少误差的累积,而对LIO-SAM特征提取部分添加强度特征的算法IALIO在采矿区有更好的定位精度,它的RMSE为21.855 380 m,相较于LIO-SAM误差降低了0.91%。

5 结 语

本文针对矿山地形未知环境中建图定位需求, 首先配置了激光雷达、IMU、GPS等多传感融合的巡检机器人,并对激光雷达和IMU进行内外参数标定,解决了建图漂移失真问题。考虑矿山环境的复杂性,巡检机器人分别采集 3组场景(街区道路、矿山斜坡、采矿区)的数据集,以GNSS信息作为轨迹真值,对比了多种三维激光雷达SLAM算法的建图效果并分析了轨迹误差与定位精度。试验表明,在采矿区复杂环境,LIO-SAM通过多传感器融合,不仅以局部地图优化的方式减少了累积误差,更在后端优化部分添加了回环因子,具有更优的定位精度。针对LIO-SAM特征提取部分计算曲率提取特征点的不足之处,引入了强度特征,验证了IALIO算法定位精度在采矿区更优于LIO-SAM。

本文验证了矿山环境下三维激光雷达SLAM算法的可行性,在解决矿山环境地形多变造成的累积误差问题上,LOAM和LeGO-LOAM算法的稳健性和定位精度都较差,使用多传感器融合算法LIO-SAM才能得到精确的定位精度。LIO-SAM的特征提取部分还有所欠缺,改变特征点的提取方式对提高定位精度具有可行之处。