基于矿区巷道巡检机器人的LOAM-SLAM地图重建改进算法的研究

秦学斌 王 炳 景宁波 薛宇强 朱信龙 张俊乐

(1.西安科技大学电气与控制工程学院,陕西 西安 710054;2.西安科技大学工程训练中心,陕西 西安 710054;3.陕西陕煤陕北矿业有限公司,陕西 榆林 719000;4.陕西省计量科学研究院,陕西 西安 710100)

1 问题的提出

中国煤炭使用历史已超千年,规模化、机械化煤炭挖掘有百多年历史,前期不科学的开采工作严重破坏部分矿区环境,包括地下水数量和质量下降、地面沉降、采矿废物储存不当、土地占用和其他影响,给当地居民生活带来不便,且该行业一直被广泛认知为高危、艰苦行业,相关技术受关注度较低,发展缓慢,矿业工作人员安全存在风险[1]。针对采煤产生的环境问题以及安全问题,“十三五”期间,全国矿山产业结构不断优化,淘汰退出煤矿5 464处,涉及产能9.4亿t,整顿关闭金属非金属矿山和尾矿库1.9万余座[2]。

近年来,在关停不合格矿区矿井以增加行业安全性、投入使用“少人/无人化”智能矿用机械以提高开采科学性和效率性的多重影响下,我国矿业智能化建设取得积极进展,这是矿业工业发展的必然选择,且符合“中国制造2025”的国家战略[3]。2019年国家煤监局提出的《煤矿机器人重点研发目录》[4]明确了矿区机器人的分类及相应标准,智能化采掘工作面从2015年仅有3个增加到275个,截至2021年1月增至494个、同比增加80%,掘进、巡检、运输等19种机器人在矿区现场已有示范应用[5]。2020年12月,国家电投集团内蒙古白音华煤电有限公司矿用卡车无人驾驶项目实现阶段性目标,并稳定参与到剥离生产作业中,矿区已完成12台无人驾驶矿用卡车编组运行,每台核载约为50 t物料,行驶速度为12 km/h,实现了单车中间运输路段无人运行;2021年3月,陕煤集团黄陵矿业公司部署煤矿巡检机器人群,该机器人群可以实现远程操控、智能巡检,减轻了约180位巡检工人的工作量[6]。

根据《煤矿机器人重点研发目录》,需要环境感知、路径规划技术支撑的煤矿机器人可以分为5类:掘进类(掘进机器人)、采煤类(采煤机机器人)、运输类(搬运机器人、巷道清理机器人、井下无人驾驶运输车、露天矿卡车无人驾驶系统)、安控类(工作面巡检机器人、皮带机巡检机器人、巷道巡检机器人)、救援类(井下抢险作业机器人、矿井救援机器人、灾后搜救水陆两栖机器人)[7]。本研究根据现有行业指导方案及国内外研究进展,以巷道巡检机器人SLAM(simultaneous localization and mapping)建图算法为切入点,结合矿区井下巷道复杂环境,同时考虑矿区成本,实现巡检机器人巡检地图路线精度的提升。传统井下巷道巡检机器人SLAM建图算法存在以下弊端:①对传感器数量及配置要求较高,需要多传感器联合调配,从而进行数据采集,增加矿区成本;②建图与定位过程所需数据量较高,使得算法的时间复杂度较高,大大降低了运行效率;③难以适应矿区井下复杂路况,在井下采集数据时,算法自身无法处理采集的数据,从而降低地图路线精度,导致巡检机器人无法完成井下巡检任务,从而存在井下安全隐患。

对上述弊端进行分析之后,在 LOAM(Lidar Odometry and Mapping in Real-time)建图算法的基础上,提出了一种基于矿区巷道巡检机器人的LOAMSLAM地图重建改进算法。该算法选用单轴单线激光雷达进行数据采集,降低矿区成本。算法层面利用Harris 3D角点检测,选取角点作为点云帧数据中的关键点进行匹配,保证计算量的同时,去除部分冗余数据。通过EKF(Extended Kalman Filter,扩展卡尔曼滤波)消除由于激光雷达抖动所产生的点云运动畸变,提高建图定位精度,同时对机器人进行运动估计,保证巡检机器人巡检路线的准确性和安全性。

2 LOAM算法

由于点云中的点随着激光雷达运动会产生运动畸变,也就是点云中的点会相对实际环境中的物品表面上的点存在位置上的误差。运动畸变会造成点云在匹配时发生错误,从而不能正确获得两帧点云的相对位置关系也就无法获得正确的里程计信息[8]。LOAM算法将SLAM问题分成了2部分,使用2个算法并行运行来处理这2部分问题,以达到实时构建低漂移的地图(即使用1个三维空间中运动的两轴单线激光雷达来构建实时激光里程计并建图)。其中一个算法以10 Hz的频率运行,得到1个较为粗糙的里程计信息。该算法使用基于特征点的scan-to-scan match(点集与点集配准)方法,可以快速计算得到里程计信息。另一个算法以1 Hz的频率运行,输出1个更为精确的里程计信息。

如图1为LOAM算法主要流程图:①Point Cloud Registration,使用IMU数据进行点云运动畸变去除;②特征点提取,LOAM中提取的特征点有2种——Edge Point和Planar Point,2种特征使用曲率来进行区分,曲率最大的为Edge Point,曲率最小的为Planar Point;③Lidar Odometry,激光帧的时间同步、点云匹配与误差函数;④Lidar Mapping,机器人位姿变换;⑤Transform Integration,融合了Lidar Mapping得到的位姿变换和Lidar Odometry得到的位姿变换。最终发布1个频率与Lidar Odometry发布频率一致的位姿变换。

图1 LOAM算法主要流程图Fig.1 The flow-process diagram of LOAM algorithm

3 基于矿区巷道巡检机器人的 LOAMSLAM地图重建改进算法

3.1 算法描述

该算法继承LOAM算法对于点云数据的配准scan-to-map match(点集与子图配准)方式,在此基础上运用Harris-3D角点检测算法,在选定区域内检测点云数据帧的关键点,以此为中心点进行点云数据的配准,保证计算量的同时,去除了部分冗余数据。由于该算法选用单轴单线激光雷达,故不能通过原算法中利用三维空间中运动的两轴单线激光雷达将定位和建图分别进行的方式,从而消除运动畸变。故考虑运用EKF(扩展卡尔曼滤波)消除由于激光雷达抖动所产生的运动畸变,同时通过状态方程和预测方程,对机器人进行运动估计,提高定位精度。

3.2 特征点提取

利用单轴单线激光雷达采集点云数据后,传统SLAM建图算法中相互关联的特征点是通过特征值和特征向量来获得,这种方法计算量大,且容易造成局部差异。本算法利用Harris 3D角点检测,选取角点作为点云帧数据中的关键点进行点云数据帧的匹配,保证计算量的同时,可去除部分冗余数据,而且具备稳定性质的特征。

Harris 3D确定角点的方法是方块体内点云的数量变化。在点云中存在1点p,在p上建立1个局部坐标系:z方向是法线方向,x、y方向和z垂直。在p上建立1个小正方体:①如果小正方体沿z方向移动,那小正方体里的点云数量不变;②如果小正方体位于边缘上,则沿边缘移动,点云数量几乎不变,沿垂直边缘方向移动,点云数量改变;③如果小正方体位于角点上,则有2个方向都会大幅改变点云量。

由二维图像的矩阵M扩展至三维点云,矩阵M元素为法向量(包含法线和方向2个信息)x、y、z构成协方差矩阵,属于对称矩阵,该矩阵的特征向量有1个方向是法线方向,另外2个方向和法线垂直。其中,半径r可以用来控制角点的规模,r小,则对应的角点越尖锐(对噪声更敏感),r大,则可能在平缓的区域也检测出角点。

计算点云的Harris响应值:

通过提取方块内的点云数据,计算Harris 3D算法中的矩阵M的迹和行列式,最后由计算出来的结果,配合公式,输出关键角点的坐标(x,y,z)。

3.3 点云配准

SLAM中点云匹配的问题分为scan-to-scan(点集与点集)、scan-to-map(点集与子图)、map-to-map(子图与子图)。而三者各有特点,scan-to-scan匹配:优点是计算量小速度快,缺点是误差累计大,长距离误差累计后地图精度下降。map-to-map的匹配:优点是精度高,误差累计小;缺点就是计算量大,实时性压力大;scan-to-map的匹配居中。由于该算法利用Harris 3D选取角点作为点云帧数据中的关键点进行点云数据帧的匹配,去除了部分冗余数据,故选用scan-tomap点云匹配方式。

如图2(a)所示,通过提取角点作为关键点选取某几帧点云数据组成子图,之后点云数据帧如图2(b)所示,与子图相互匹配,配合上述完成的Harris 3D点云检测,最终完成点云配准。

图2 点云配准过程图Fig.2 Point cloud registration process diagram

图3为通过Harris 3D角点所配准的最终效果图,图中白色点表示所检测出来的Harris 3D角点,所选场景为井下模拟矿道。

图3 点云配准图Fig.3 The point cloud registration map

3.4 EKF扩展卡尔曼滤波

3.4.1 消除运动畸变

矿区井下路况复杂,利用激光雷达采集点云数据时,点云中的点会随着激光雷达的抖动产生运动畸变,从而无法获得正确的里程计信息,导致既定路线精度较低,容易造成井下安全问题。故通过选用EKF扩展卡尔曼滤波消除运动畸变,同时对巡检机器人进行运动估计。

扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,属于一种高效率的递归滤波器(自回归滤波器)。其基本思想是利用泰勒级数展开将非线性系统线性化,然后采用卡尔曼滤波框架对信号进行滤波[9]。

图4为扩展卡尔曼滤波器的算法结构和步骤:卡尔曼滤波利用点云目标的动态信息,去掉其噪声,得到1个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或者平滑)[10]。

图4 扩展卡尔曼滤波器的算法结构图Fig.4 Algorithm structure diagram of Extended Kalman Filter

3.4.2 运动估计

任何含有不确定信息的动态系统中使用卡尔曼滤波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,其总是能指出真实发生的情况。

在连续变化的系统中使用卡尔曼滤波是非常理想的,具有占用内存小的优点(除了前一个状态量外,不需要保留其他历史数据),并且速度很快,很适合应用于实时问题和嵌入式系统。

扩展卡尔曼滤波EKF的状态转移方程和观测方程为

利用泰勒展开式对(4)式在上一次的估计值(θk-2)处展开得:

再利用泰勒展开式对式(6)在本轮的状态预测值θ′k处展开得:

其中,F k-1和H k分别表示函数f(θ)和h(θ)在θk-2和θ′k处的雅可比矩阵。这里对泰勒展开式只保留到一阶导数,二阶导数以上的都舍去,噪声假设均为加性高斯噪声。

整体结构如图5所示。

图5 EKF运动估计结构图Fig.5 The motion estimation structure of EKF

通过传感器采集数据后,进行位姿估计阶段,需限定点云数量的范围,之后通过点云配准,以及传感器采集数据,进行点云位姿的原始估计,最终利用EKF滤波器进行位姿的预测与更新。

3.5 LOAM-SLAM算法

根据以上描述,算法整体框架如图6所示。

图6 LOAM-SLAM算法主要流程图Fig.6 The flow-process diagram of LOAM-SLAM algorithm

基于矿区巷道巡检机器人的LOAM-SLAM地图重建改进算法的整体流程图,此过程描述数据采集、滤波处理、运动估计以及建图与定位。算法步骤如下:

Step1:利用单轴单线激光雷达采集所需点云数据,本研究选用Velodyne型号激光雷达,场景为井下模拟矿道。

Step2:通过 Harris 3D角点检测算法,对于获取的点云数据进行关键点的检测,用于之后的点云配准。Harris 3D确定角点的方法是方块体内点云的数量变化。若小正方体位于角点上,则在不同方向都会大幅改变点云数量。

Step3:EKF滤波器对于点云数据进行运动畸变的消除,消除运动畸变之后,通过检测出的点云关键点,进行配准,以及通过状态方程和预测方程对于巡检机器人进行运动估计,预测和更新巡检机器人的位姿。

Step4:通过EKF对于点云数据的处理以及运动估计,得到机器人的位姿变换。

Step5:融合了Lidar Mapping得到的位姿变换和经EKF处理后的点云。最终发布1个频率与里程计发布频率一致的位姿变换。

4 算法改进前后实验结果对比

实验环境:操作系统为Ubuntu 18.04;ROS环境为ros-melodic;cuda-10.2,cudnn-7.6。计算机硬件:Intel(R)Core(TM)i5-9300H CPU,8GB RAM,512G固态。编译语言:C++。传感器为单轴单线激光雷达。数据采集环境为矿区井下模拟巷道。本次实验主要验证:①改进前后算法中里程计信息对于旋转,2帧数据推算时,角度是否变化较大;②改进前后算法对于建图效果的对比。

4.1 里程计信息对于旋转角度变化实验

对于点云旋转,实际上里程计的前后2帧推算时,角度不会变化很大,大多数情况下都是欧拉角中偏航角(yaw)发生变化,同时由于角度变化过小时,点云帧数据不会有太大差异,故该实验对点云施加5°旋转,按照步长为5°增加,从而对比改进前后算法精度。

实验1:2帧点云帧之间yaw角相差5°,改进前后算法点云帧变化如图7所示。

图7 yaw角相差5°时点云帧数据图Fig.7 Point cloud frame data diagram when yaw angle difference is 5 degrees

实验2:2帧点云帧之间yaw角相差10°,改进前后算法点云帧变化如图8所示。

图8 yaw角相差10°时点云帧数据图Fig.8 Point cloud frame data diagram when yaw angle difference is 10 degrees

上述实验均为单轴单线激光雷达测试结果,可以看出改进后算法在偏航角变化时,精度更好。

4.2 井下巷道建图实验

实验场景为矿区井下模拟巷道,效果对比如图9所示。

图9 算法改进前后效果对比图Fig.9 Comparison of effects before and after algorithm improvement

上述实验均为单轴单线激光雷达测试结果,可以看出改进前算法会出现地图叠影的现象,而且巷道中的岔路口也没有体现出来。改进后,叠影现象消失,且可以完整呈现出井下巷道的情况。

5 结 论

提出了一种基于矿区巷道巡检机器人的LOAMSLAM地图重建改进算法,以LOAM算法为基础,在此基础上改用单轴单线激光雷达进行数据采集。同时进行Harris 3D算法、EKF扩展卡尔曼滤波的融合,解决了点云数据存在大量冗余数据、运动畸变和运动估计等问题,完成了单轴单线激光雷达对于井下巷道地图的建立。后续将其进行多传感器数据融合,提升建图定位精度,适用于更多场景。