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  综采工作面三维激光扫描硬件

Figure  1.  Hardware for three dimensional laser scanning in fully mechanized working face

下载: 全尺寸图片 幻灯片

1.1   激光扫描原理

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

图  2  激光雷达线束分布

Figure  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\leqslant i\leqslant N $，N为点云内所有点的个数）个点的坐标为（x_i，y_i，z_i），计算点云P中所有样本的协方差矩阵：

式中：${\boldsymbol{E}}(\cdot) $为期望函数；$ \overline{x}，\overline{y}，\overline{{\textit{z}}} $为点云中所有点对应坐标的算术平均值。

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

式中：V为特征向量组成的变换矩阵；λ₁，λ₂，λ₃为特征值。

3个特征值间接表征了区域的曲率特征，当${\lambda }_{1}$>>${\lambda }_{2}$时，该区域为直线结构；当${\lambda _1} \approx {\lambda _2} $>>$ {\lambda _3} $时，该区域为平面结构；当$ {\lambda _1} \approx {\lambda _2} \approx {\lambda _3} $时，该区域是椭球结构。设L，F，S分别为直线特征、平面特征、椭球特征参数，其计算公式为

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

2.3   位姿估计

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

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

计算2帧中3类特征点之间的距离。对于直线特征点，采用点到线的距离，在P_k中使用最近邻算法搜索与点p_k+1,i最近的2个点，设为${{p}}_{k,{i_1}} $，${p}_{k,{i_2}} $，将这2个点的连线作为目标直线；对于平面特征点，采用点到面的距离，在P_k中使用最近邻算法搜索与点p_k+1,i最近的3个点，设为${p}_{k,{i_3}} $，${p}_{k,{i_4}} $，${p}_{k,{i_5}} $，将这3个点所在平面作为目标平面；对于椭球特征点，采用点${p}_{k+1,i} $到最近点${p}_{k,{i_6}} $的距离。距离计算公式为

式中： $ {d}_{{L}} $为点到直线的距离；$ {d}_{{F}} $为点到平面的距离；$ {d}_{{S}} $为点到点的距离。

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

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

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

式中：${d}_{\delta } $为对距离进行残差处理后的值； d 为距离，$d \in \{{d_L},{d_F},{d_S}\} $； δ 为阈值，本文取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  轨道式巡检机器人

Figure  3.  Orbital inspection robot

下载: 全尺寸图片 幻灯片

3.1   SLAM建图效果

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

图  4  综采工作面实时三维重建效果

Figure  4.  Real time 3D reconstruction effect of fully mechanized working face

下载: 全尺寸图片 幻灯片

3.2   误差分析

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

式中：n为标记点个数；$ {X'_j} $为标记点坐标测量值；$ {X_j} $为标记点坐标实测值。

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

4.   结论

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

表  1  激光点云标记点坐标测量误差分析结果

Table  1.  The error analysis result of marked points coordinate of laser point cloud m

标记点	实测值	测量值1	测量值2	测量值3
1	(12.76，0.51，1.21)	(12.77，0.42，1.27)	(12.65，0.45，1.31)	(12.69，0.49，1.31)
2	(30.30，0.81，1.31)	(30.41，0.78，1.42)	(30.55，0.89，1.50)	(30.23，0.75，1.41)
3	(47.52， 1.51，1.88)	(47.60， 1.32，1.83)	(47.78， 1.41，1.71)	(47.63， 1.41，1.92)
4	(65.77，2.52，1.23)	(65.90，2.48，1.01)	(65.88，2.31，1.24)	(65.57，2.43，1.38)
5	(83.27，2.21，1.84)	(83.42，2.41，1.69)	(83.57，2.61，1.54)	(83.97，2.41，1.74)
6	(101.98，2.75，1.83)	(101.89，2.73，1.79)	(101.92，2.77，1.79)	(101.79，2.63，1.89)
7	(112.03，2.67，1.92)	(112.04，2.69，1.96)	(112.05，2.65，1.94)	(112.06，2.68，1.95)
8	(129.53，2.69，1.90)	(129.41，2.68，1.89)	(129.61，2.67，1.87)	(129.51，2.67，1.84)
绝对误差均值		0.18	0.19	0.14

下载: 导出CSV 

| 显示表格

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