基于激光SLAM 的综采工作面实时三维建图方法

亓玉浩，关士远

（1. 山东能源集团有限公司，山东 济南 250013；2. 北京天玛智控科技股份有限公司，北京 101399）

0 引言

“十三五”以来，国家发展改革委、国家能源局等部门相继印发《能源技术革命创新行动计划（2016-2030 年）》《关于加快煤矿智能化发展的指导意见》，大力推进煤矿智能化建设。对综采工作面进行智能感知，实时构建工作面三维地图，进而实现采煤机自主作业，是煤矿智能化发展的关键一环[1-2]。

煤矿三维地图的构建方法主要包括站点式建图[3]和移动式建图[4]。站点式建图方法先将整个区域划分成多个子区域，然后逐站扫描获取激光点云，同时用全站仪测量各个站点的坐标，最后结合激光点云和站点坐标进行整个区域的点云拼接。该方法所需时间较长，实效性较差，不能满足综采工作面实时建图需求。移动式建图方法主要采用激光雷达进行实时扫描，利用惯导和里程计计算位姿，实现激光点云实时拼接[5-6]，进而实现实时建图。该方法依赖高精度的光纤惯导和里程计进行位姿计算，而在实际工程实践中里程计精度难以满足应用需求，导致获取的工作面三维激光点云不完整，从而难以获取完整、准确的工作面三维地图。

同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）[7-10]是实时移动建图技术研究热点，通过搭载特定的传感器主体，在没有环境先验信息的情况下，在运动过程中建立环境模型，同时估计传感器位姿，如果传感器采用激光雷达，即为激光SLAM。本文提出一种基于激光SLAM 的综采工作面实时三维建图方法。实时获取综采工作面激光点云后，采用惯导的姿态角对激光点云进行去畸变处理，采用主成分分析法[11-13]提取激光点云的几何张量特征，通过计算特征点距离进行点云匹配和位姿估计，最后对姿态进行优化，并增量式构建全工作面三维激光点云。

1 综采工作面实时三维建图方法原理

综采工作面三维激光扫描硬件主要包括2 个16 线激光雷达、惯导、车载工控机，部署在工作面巡检机器人上，如图1 所示。2 个16 线激光雷达旋转轴呈45°安装，负责激光扫描；惯导安装于水平激光雷达底部，测量值用于去畸变处理及为后期优化提供初始值；车载工控机运行激光SLAM 程序，实现实时建图。

图 1 综采工作面三维激光扫描硬件Fig. 1 Hardware for three dimensional laser scanning in fully mechanized working face

1.1 激光扫描原理

16 线激光雷达采用飞行时间测量法，激光发射器发出一束超短激光脉冲，激光投射到物体上并反射，激光接收器接收反射光，通过测量激光束在空中的飞行时间，可以准确计算目标物体与激光雷达的距离。16 线激光雷达有16 个激光发射器和16 个激光接收器，各发射器垂直方向均匀分布，垂直分辨率为2°，水平分辨率为0.2°，如图2 所示，其中红色点为测量原点，黄色点为激光发射器，右侧和下方数据为雷达结构尺寸。

图 2 激光雷达线束分布Fig. 2 Lidar harness distribution

激光雷达工作时，机械旋转部分以10 r/s 的速度高速旋转，各激光发射器和接收器每隔0.2°完成1 次发射和接收，16 线激光雷达每秒完成约30 万次测量。每次测量结果为目标物体上某点的坐标、反射强度、时间戳、激光器ID，指定时间内所有的测量结果称为点云。

激光雷达数据通过以太网UDP（User Datagram Protocol）传输，通常1 次传输1 s 或0.1 s 内的测量数据，用帧表示，则对应的帧率分别为1 Hz 和10 Hz。帧率的选择与移动建图的速度有关，若移动速度慢，可以降低帧率；若移动速度快，则需要提高帧率。由于激光雷达是移动的，帧内第1 个测量点与最后1 个测量点所在的坐标系不一致，会产生畸变。通过提高帧率能有效抑制畸变，但不能消除畸变，本文采用惯导测量值消除畸变。

1.2 激光SLAM 原理

激光雷达获取的环境坐标基于移动的激光雷达坐标系，而要构建的实时地图基于固定的世界坐标系，因此，需要估计出激光雷达坐标系相对于世界坐标系的实时姿态，将测量值变换到世界坐标系，实现实时移动建图。建图和位姿估计可能是互相耦合的，建图依赖位姿估计结果，位姿估计一般根据已建地图进行。激光SLAM 主要解决以下问题：① 通过实时读取的点云数据和已经建立的三维地图估计某时刻的位姿。② 通过该时刻的位姿建立基于世界坐标系的三维地图，即实现定位及建图。

2 综采工作面实时三维建图方法实现

基于激光SLAM 的综采工作面实时三维建图方法主要包括激光点云去畸变、点云特征提取、位姿估计、优化建图等步骤。

2.1 激光点云去畸变

在去畸变处理前，假设已将点云和惯导数据变换到同一坐标系，故惯导坐标系和激光雷达坐标系重合。实际测试采用16 线激光雷达，在运动中执行扫描任务会产生运动畸变。雷达本身的姿态因载体运动而变化，而激光雷达计算周围障碍物距离时是基于激光雷达坐标系的，如果不考虑激光雷达本身的姿态改变，就会产生误差。该类误差可以通过惯导数据进行补偿。根据点云中每个点的时间戳检索对应的惯导数据，获得对应每个点的姿态角。如果没有检索到对应姿态角，则采用四元数法插补，用对应时间戳前后2 个惯导数据进行插补，从而获得对应的姿态角。将所有点云都变换到该次扫描初始时刻的坐标系，一次扫描得到雷达从0°旋转到360°所获得的全部点云，即1 帧点云。变换矩阵可通过惯导初始时刻的姿态角和当前时刻的姿态角求得。

2.2 点云特征提取

综采工作面具有空间狭窄、环境特征重复、振动大等特点，从激光点云中获取的特征点数少，误匹配概率较高，若采用简单几何特征实现三维建图，效果不佳[14-15]。本文采用主成分分析法提取几何张量特征，先求点集的协方差矩阵，再进行特征值分解，得到几何张量特征[16-17]。求得特征后，再融合惯导数据，通过非线性优化求得雷达姿态，最后进行后端优化建图。

在点云中，以每个点为球心、r为半径作球，r与所建地图的空间大小有关，对于综采工作面，一般设为0.1～0.3 m。设点云中第i（1 ≤i≤N，N为点云内所有点的个数）个点的坐标为（xi，yi，zi），计算点云P中所有样本的协方差矩阵：

式中：E(·)为期望函数；为点云中所有点对应坐标的算术平均值。

计算协方差矩阵的所有特征值及特征向量，进行特征值分解，得

式中：V为特征向量组成的变换矩阵；λ1，λ2，λ3为特征值。

3 个特征值间接表征了区域的曲率特征，当λ1> > λ2时，该区域为直线结构；当 λ1≈λ2> > λ3时，该区域为平面结构；当 λ1≈λ2≈λ3时，该区域是椭球结构。设L，F，S分别为直线特征、平面特征、椭球特征参数，其计算公式为

针对3 个特征参数分别设置阈值，当所求特征参数大于相应阈值时，将其归类为对应特征。阈值需要根据具体环境选择，对于综采工作面，直线、平面、椭球特征参数可分别设置为0.65，0.33，0.71。整个点云按照3 个特征参数进行分类，结果以KDTree结构存储。

2.3 位姿估计

设当前时刻k+1 获得的点云为Pk+1，点云内的点为pk+1,i，前一时刻k对应的点云为Pk，点云内的点为pk,i，使用最近邻算法搜索Pk+1与Pk中对应的特征点，进行数据关联。前一时刻的姿态是已知的，通过前后2 帧之间的变换矩阵T，可得到当前时刻的姿态，从而将姿态估计问题转换为变换矩阵求解问题。变换矩阵可表示为

式中：R为旋转矩阵；t为平移矩阵。

计算2 帧中3 类特征点之间的距离。对于直线特征点，采用点到线的距离，在Pk中使用最近邻算法搜 索 与 点pk+1,i最 近 的2 个 点，设 为pk,i1，pk,i2，将 这2 个点的连线作为目标直线；对于平面特征点，采用点到面的距离，在Pk中使用最近邻算法搜索与点pk+1,i最近的3 个点，设为pk,i3，pk,i4，pk,i5，将这3 个点所在平面作为目标平面；对于椭球特征点，采用点pk+1,i到最近点pk,i6的距离。距离计算公式为

式中：dL为点到直线的距离；dF为点到平面的距离；dS为点到点的距离。

构建目标函数[18-19]：

式中：R*，t*分别为旋转矩阵、平移矩阵目标值；ωa，ωb，ωc为3 种距离的对应权重；A，B，C分别为直线特征点、平面特征点、椭球特征点个数。

为了避免偏差过大，即出现所谓的离群点，采用Huber 鲁棒核函数处理残差[20]，得

式中：dδ为对距离进行残差处理后的值；d为距离，d∈{dL,dF,dS}；δ为阈值，本文取0.3 m。

因为R是正交矩阵，无法直接对R求导，求得最优解。将T变换到其对应的切空间，在局部的李代数[21]结构下对R求导，再采用Levenberg-Marquardt算法求解目标函数。

求得当前时刻的姿态后，要判断该帧是否作为关键帧保留，并进行后续优化建图。若当前帧姿态相对于前一帧姿态的平移超过0.5 m，或3 个轴中任意一轴相对于前一帧对应轴的旋转角大于1°，则可判断为关键帧。

2.4 优化建图

得到关键帧后，要与历史关键帧进行联合优化，以进一步减小姿态估计误差。如果将所有的关键帧一起进行优化，随着巡检机器人运行距离增加，优化变量将变得非常多。因此，本文采用滑动窗口法[22]选取邻近的10 个历史关键帧，与当前关键帧一起进行联合优化。构建姿态图[23]，其节点为关键帧的姿态及该帧点云姿态，边为前后帧之间的姿态约束。所有姿态图的节点存储在KDTree 内，便于后续遍历操作。

为保证实时性，采用增量式优化算法，使用GTSAM[11]优化库实现联合优化。联合优化部分为独立进程，优化频率为10 Hz。同时，在历史关键帧中进行匹配，如果历史关键帧与当前关键帧的对应特征点较多，即为回环，将该历史关键帧及其前后各5 帧加入目标函数进行联合优化，即共计21 帧同时进行优化。回环部分为独立进程，优化频率为5 Hz。将获得的所有关键帧点云叠加到一起，即为全局实时三维地图。

3 井下工业性试验

在兖矿能源集团股份有限公司某工作面进行试验，该工作面采高为4.5 m，长度为260 m。将激光雷达安装在轨道式巡检机器人上，现场建图数据及指令通过WiFi 传输，如图3 所示。

图 3 轨道式巡检机器人Fig. 3 Orbital inspection robot

3.1 SLAM 建图效果

远程遥控巡检机器人从刮板输送机的机头运行到机尾，进行实时建图，结果如图4 所示。图4（a）中，红色为综采工作面三维激光点云，青色线为巡检机器人实时轨迹，从图4（b）可清晰看出采煤机轮廓。

图 4 综采工作面实时三维重建效果Fig. 4 Real time 3D reconstruction effect of fully mechanized working face

3.2 误差分析

在支架上选8 个标记点，用全站仪测出这些点相对于地图原点的坐标，作为实测值。由于实时SLAM 所生成的点云稠密程度不够，采用在线录制的3 个数据包生成稠密点云，在点云中识别这些标记点，并计算绝对误差均值：

式中：n为标记点个数；为 标记点坐标测量值；Xj为标记点坐标实测值。

激光点云标记点坐标测量误差分析结果见表1。可看出3 组数据中最大绝对误差为0.19 m，满足综采工作面监控及刮板输送机找直精度需求。

表 1 激光点云标记点坐标测量误差分析结果Table 1 The error analysis result of marked points coordinate of laser point cloud m

4 结论

（1） 提出了基于激光SLAM 的综采工作面实时三维建图方法，给出了激光点云去畸变、点云特征提取、位姿估计、优化建图等关键步骤。通过井下工业性试验进行定性及定量分析，结果表明，该方法能实时、完整、高精度地构建全工作面范围的三维地图，最大绝对误差均值为0.19 m，满足综采工作面监控及刮板输送机找直精度需求。

（2） 该方法也存在一些不足，如没有考虑振动对三维建图设备的影响，因为井下日常生产造成的振动较大，而激光雷达带有旋转部件，随着使用时间增加，激光雷达精度会降低。此外，对于实时产生的激光点云，没有生成三角网等便于渲染的数据格式，未来需要对实时三维点云数据进行网格化处理，进一步改善其显示效果。