基于惯导和激光雷达的采煤机定位方法

郑江涛，李四海，刘士明，付强文，陶渊博，李 津

（1.西北工业大学 自动化学院，西安 710072；2.中国兵器工业导航与控制技术研究所，北京100089；3.电子信息控制重点实验室，成都 610036）

综采工作面包括采煤机，液压支架以及刮板输送机。图1为综采工作面俯视图，其中采煤机以刮板输送机为轨道前进或倒退实现割煤作业，液压支架与刮板输送机相连并排列于一侧，液压支架除了为采煤机提供安全作业空间外，还会推移刮板输送机靠向煤壁(沿图1的推移方向)，确保采煤机割煤的连续性。

无人智能化开采要求采煤机自动调高以及液压支架自动跟机，采煤机自主定位是实现二者自动调节的基础。最初采煤机的定位依赖于支架传感器或轨道形态等外部参数，定位可靠性和精度都难以保证。惯性导航系统可以自主地进行连续三维定位和三维定向，被引入到采煤机定位工作中。Yang 等人通过试验验证基于捷联惯导的采煤机定位技术是一种可行的方案，并针对振动引入的影响进行了建模分析[1]；之后Yang等人[2]又提出以车辆运动学约束及零速修正技术辅助惯导实现采煤机定位，并沿30 m×8 m 的矩形轨道运行了180 s，最大误差为1.6 m，该方法往往需要采煤机频繁停车，影响开采效率；Fan 等人[3]采用惯性导航和无线传感网络技术组合实现采煤机自主定位，试验表明沿20 m 轨道运行200 s，位置误差可维持在0.2 m内，然而该方法依赖于安装在液压支架上的无线节点的位置精度，液压支架的间歇移动会对该方法的定位精度产生影响；利用零速校正技术、里程计辅助惯性导航系统是当前一种流行的定位方法，当采煤机静止时以零速修正技术辅助惯导实现采煤机定位，当采煤机运动时利用基于惯导和里程计的航位推算算法实现采煤机定位[4,5]；航位推算系统的定位误差主要与初始对准误差、惯导与车体的安装误差、里程计的刻度系数误差以及惯导的姿态精度有关[6]；张博渊等针对采煤机的直线运动特性，提出了在两个绝对位置已知的点之间，沿直线快速跑车，进而标定出惯导与车体之间的安装偏角，该方法可有效改善航位推算中初始对准误差以及安装误差的影响[7]；为了进一步改善航位推算的定位精度，Wang 等人引入了车辆运动学约束，并沿54 m 的轨道往复运行了4 刀，历时约2700 s，最大定位误差约0.6 m[8]。

图1 综采工作面俯视图Fig.1 The top view of fully mechanized working face

随着开采技术向智能化迈进，迫切需求寻找更多可适用于矿下环境的传感器，提取更丰富的信息以满足矿下设备对定位系统高精度高稳定性的要求。激光雷达采用回波技术能有效避免粉尘干扰，成功应用于未知环境探索、三维重建等领域，逐渐被矿下环境研究者们注意。Mohsen Azizi 等人利用激光雷达等传感器搭建的导航定位系统，可辅助矿山运输车在隧道等环境中自主导航[9]；Ralston Jonathon C 等人将激光雷达引入到综采工作面，成功实现对综采工作面蠕动的控制[4]；由于激光雷达不是专门为矿下极端环境设计，缺乏预防灰尘进入或凝结在激光设备表面的措施，是激光雷达在矿井应用中面临的最大挑战[10]；学者们针对除尘技术也展开了研究，揭膜式除尘法[11]以及气幕除尘法[12]均可有效提高激光设备在矿下的工作效率。随着除尘技术的进一步发展，激光雷达在矿下的应用势必会更加广泛。

相比传统的采煤机定位方法，惯导与激光雷达的组合系统不需要频繁停车，不受液压支架移架的影响，且对惯导姿态有一定估计能力。本文旨在研究可行的惯导/激光雷达组合方法，首先介绍了惯导/激光雷达的组合定位原理，并推导了组合导航系统误差模型，然后通过仿真分析了主要误差源影响，最后搭建了试验平台进一步验证了算法可行性。

1 惯导/激光雷达组合方法

1.1 激光雷达测距原理

图2为激光雷达测距原理图，激光雷达以设定的角度分辨率和角速度在某一区域内进行扫描，当光线碰到障碍物时返回，此时根据光速和时间差可计算出物体表面与激光雷达原点的相对距离。定义激光雷达坐标系为l，原点Ol位于激光发射点处，Xl轴指向0 °出光方向，Yl轴指向90 °出光方向，Xl Ol Yl构成激光雷达扫描平面，Zl与Xl、Yl轴满足右手定则。障碍物i在l系下的坐标值可用式(1)表示：

式中ρi表示障碍物到激光雷达原点的距离，θi为激光雷达原点指向障碍物构成的向量与激光雷达坐标系Xl轴夹角，ρi和θi由激光雷达输出。

图2 激光雷达工作原理Fig.2 Working principle of lidar

液压支架的立柱是起支撑作用的核心部件，外形是倾斜的圆柱体，如图2(a)所示；激光雷达的扫描平面与倾斜圆柱体外表面所成交线为椭圆，通过数据拟合可以得到椭圆中心在l系下坐标值，如图2(b)所示。本文选择拟合所得的液压支架立柱中心作为特征点，从激光雷达的一次输出数据中可拟合出多个特征点。

考虑到激光雷达相对车体为侧向安装，为了方便后续的研究，定义s系来描述特征点在激光雷达扫描平面内的位置坐标。s系与l系原点相同，Xs、sY以及Zs轴分别与lY-、Xl及Zl轴对应的方向相同。在后文的分析中激光雷达探测的特征点均表示在s系下。

1.2 组合定位原理

组合定位系统俯视效果如图3所示，可视范围指激光雷达的可用扫描范围，可视范围内的特征点会参与组合定位，可视范围大小受激光雷达自身结构和工作环境的影响，鉴于采煤机所经过的液压支架会发生移架操作，对应的特征点位置发生变化不能参与组合定位，同时考虑到激光雷达测距精度的影响，本文选择采煤机侧前方距激光雷达D=10 m 内的特征点构成的范围为本文的可视范围，随着采煤机的前进，单个液压支架会经历“进入可视范围到退出可视范围”的过程，处在可视范围内特征点记为“有效特征点”，有效特征点位置、惯导解算位置以及激光雷达测距值满足式(2)所示的关系：

式中，Mi/k表示k时刻i号特征点的位置，以经纬高表示；Pk表示k时刻惯导解算的采煤机位置，以经纬高表示；表示k时刻i号特征点与激光雷达相对位置矢量在s系下的分量；b系为惯导坐标系；表示b系与s系的姿态矩阵，表示导航解算的姿态矩阵，C1由子午圈曲率半径RM、卯酉圈曲率半径RN、纬度L和高度h表示。

就单个液压支架而言，会在可视范围内维持一段时间，此时的液压支架一直处于静止状态，对应的特征点也处于静止状态，因此特征点位置及位置误差微分方程满足式(3)关系。

式中δMi表示i号特征点位置误差。

在特征点参与滤波解算前给定特征点初始位置是算法推进的基础，本文算法不需要事先人工标定特征点位置。当首次探测到特征点时，可通过式(4)为其赋值(赋值过程发生在特征点参与滤波之前)。

式中

图3 系统组成俯视图Fig.3 Top view of system composition

1.3 组合导航系统模型

惯导与激光雷达组合导航系统状态量选取：惯导系统速度误差δVn，姿态误差φn，位置误差δ P，陀螺零偏εb，加计零偏∇b及特征点位置误差δ Mi：

滤波器状态方程可表述为：

式中W为系统噪声，F为状态转移矩阵，具体形式如下：

Fins为常规15 维惯导状态转移矩阵；015×n、0n×15和0n×n分别表示15 行n列、n行15 列以及n行n列的全零矩阵(n为参与滤波特征点的总维度)。

导航解算的特征点与采煤机相对位置可表示为

式中：

式(7)与式(2)作差得观测方程：

式中ν表示量测噪声；Hi为特征i对应的量测矩阵。

2 误差源的影响仿真分析

为了研究惯导/激光雷达组合系统的定位效果，设计了如图4所示的运动轨迹。采煤机按箭头指示1-2-3的顺序运行，纵向前进位移为50 m，横向推进位移为1 m，运行速度约0.1 m/s。仿真中北向对应采煤机前进方向(纵向)，东向对应于垂直采煤机前进的方向(横轴)，后文中以东向、北向的方式表述采煤机位置。

图4 仿真轨迹图Fig.4 Simulation trajectory diagram

系统状态方程和观测方程直接关系着状态量的估计效果，从式(6)和式(8)可看到惯性器件性能、b与s系的姿态标定精度以及激光雷达的测距精度均会影响到导航解算，因此从上述误差源着手进行仿真分析。

2.1 惯性器件性能对定位效果影响

为了研究惯性器件性能影响，选取了如表1所示的不同等级惯性器件用以仿真分析。

表1 不同等级惯性器件参数Tab.1 Different levels of inertial device parameters

初始航向误差依次设置为0.05 °、0.5 °、0.5 °、1 °、1 °，激光雷达测距误差：系统误差为0 m、随机误差为0.006 m，所得组合导航位姿误差效果如图5所示。

图5 不同等级惯性器件条件下位姿误差Fig.5 Pose errors of different levels of inertial device

从图5可看出，IMU2 与IMU3、IMU4 与IMU5的位姿误差曲线相似，说明陀螺性能是影响位姿估计效果的重要因素。随着陀螺性能的提升，东向误差呈锯齿形波动且幅度逐渐减小，北向发散趋势逐渐减缓，高度方向变化不明显，俯仰和横滚误差稳定在初值附近，IMU1～IMU3 对应的航向误差在初值附近波动，IMU4、IMU5对应的航向误差从初值开始缓慢的发散，说明航向精度受陀螺性能制约。为了提高导航定位精度，东向的大波动误差是首要考虑和研究的，类比相关文献[7]，采煤机的东向误差是北向的前进位移量经航向误差分解所致，因此随着采煤机前进与倒退，东向误差重复“增大-减小”的变化，而航向误差与惯性器件性能相关，因此选择合适惯性器件对定位结果有重要意义。

2.2 b 与s 系姿态标定误差对定位效果影响

为了研究b与s系在 X、Y、Z 三轴存在标定角误差δα时对组合导航定位效果影响，设置了表2所示的姿态标定误差。

表2 姿态标定误差Tab.2 Attitude calibration errors

惯性器件性能按表1IMU1 设计，激光雷达测距误差：系统误差为0 m、随机误差为0.006 m，仿真所得组合导航位姿误差如图6所示。

图6 不同姿态标定误差条件下位姿误差Fig.6 Pose errors of different attitude calibration errors

从图6可看到，姿态标定误差主要对组合导航位置估计产生影响，如图6(a)δ1α和δ2α对应的曲线所示，X 方向标定角误差，主要在高度方向上产生锯齿形波动；δ5α和δ6α对应的曲线表明，Z 方向的标定角误差，主要在东向引起锯齿形波动，同时对北向位置估计效果有一定影响；δ3α和δ4α所示曲线表明，Y 方向标定角误差对位置估计几乎不产生影响。从图中还可看到随着标定角误差的增大，位置误差变化斜率和峰值均增大，因此提升姿态标定精度有助于改善定位效果。

2.3 激光雷达测距误差对定位效果影响

根据文献[15]的分析可知，激光雷达测距误差分为系统误差和随机误差，而系统误差模型参数包括刻度系数及常值两部分，结合德国西克LMS5 系列的用户手册，设计了表3所示的测距误差。

表3 激光雷达测距误差Tab.3 Ranging errors of lidar

本节仿真中，惯性器件性能按表1IMU1 设计，姿态标定误差设置为0，位姿误差效果如图7所示。

图7 不同测距误差条件下位姿误差Fig.7 Pose errors of different ranging errors

从图7可看出，激光雷达测距误差主要对组合导航位置估计产生影响。图7(a)中R1 和R2 曲线表明激光雷达的随机测距误差不会对定位效果产生影响；R3和R4 曲线说明刻度系数误差只在北向产生锯齿波动，波动幅度与刻度系数大小有关；R5 和R6 表明系统误差中的常值分量在东向产生线性累积的误差，且常值分量越大，线性发散的越快，同时会在北向产生锯齿波动，波动幅度与常值大小相关。因此激光雷达系统误差，尤其是其中的常值分量，是制约定位精度的重要因素。

2.4 集成主要误差源的系统整体仿真

通过单独对各个误差源的仿真，得到了各误差源对定位效果的影响。为了研究同时存在多误差源时的定位效果，设计了集成主要误差源的系统整体仿真。为了与后续章节的试验对比，获得更全面的试验结论，在本节中传感器的参数根据试验中所使用的传感器进行设定，具体参数如表4所示。

惯导初始的俯仰和横滚角利用加速度计粗对准获得，航向由外部注入。激光雷达借助ICP 算法[14]可实现里程计的功能，借助文献[7]所示的标定方法可获得惯导与激光雷达之间的安装偏角。为了验证该标定算法的效果，将“初始对准+标定”后的结果与仅增加初始对准误差以及仅增加安装误差（标定后的残余误差）的结果进行了对比，位姿误差如图8所示，此时激光雷达的系统误差设置为0。

表4 传感器参数Tab.4 Specifications of the sensors.

图8 标定前后位姿误差Fig.8 Pose errors before and after calibration

从图8(a)可看出，标定后的东向和北向误差明显优于单独增加初始对准误差或安装误差时的结果，高度误差与仅增加安装误差时的精度相当。仿真表明采用文献[7]提出的标定方法可有效改善初始对准误差以及惯导与激光雷达安装误差对定位结果的影响。

在初始对准和安装偏角标定工作完成后，按表4设置激光雷达测距误差，研究同时存在初始对准误差、安装偏角标定误差以及激光雷达测距误差时组合系统的定位效果。考虑到激光雷达测距误差对定位结果的影响通常与特征点的选择有关，该仿真从特征点数目以及特征点与激光雷达位置关系两个角度展开。

首先研究不同特征点数目对定位结果的影响，选择位于采煤机侧前方且离激光雷达最近的N 个特征点参与组合导航解算，依次设置N 为1～6，并标记为N1～N6，位姿误差如图9所示。

图9 不同特征点数目条件下的位姿误差Fig.9 Pose errors of different numbers of feature points

从图9(a)看出，在不同特征点条件下，0～510 s 内的东向累积误差均小于510～1021 s 内的累积，说明激光雷达测距误差的常值分量在东向产生的误差与其他误差源在东向产生的合误差，在0～510 s 内存在抵消，而在510～1021 s 内产生累加。在0～510 s 东向误差大小随着特征点数目增多而出现增大趋势，考虑到安装偏角为确定性偏差，而从图9(b)看出随着特征点数目的改变，航向误差在0～510 s 内的变化较小，说明随着特征点数目增多，测距误差的常值分量在东向产生的误差减小。北向误差主要由激光雷达测距误差引起，随着特征点数目增加，北向误差不断减小，说明增加点特征数目可以减小因测距误差引起的北向误差。

然后研究特征点与激光雷达距离不同时对定位效果的影响。设置特征点数目为3，选择位于采煤机侧前方且离激光雷达距离为d 的特征点参与组合导航解算，d 表示3 个特征点距离激光雷达最近的距离，依次设置d 为0 m、1.5 m、3 m、4.5 m、6 m 以及7.5 m，并标记为d0～d5，位姿误差如图10所示。

从图10(a)看出，增大特征点与激光雷达的距离，东向和北向误差均得到改善，说明选择离激光雷达较远的特征点参与组合导航解算，有助于减小激光雷达测距误差对定位结果的影响。从图10(b)可看出特征点与激光雷达距离的不同，对姿态误差的影响较小。

图10 激光雷达与特征点距离不同时的位姿误差Fig.10 Pose errors of different distances between lidar and feature points

3 试验验证

为了进一步验证本文算法在试验条件下的可行性，搭建了如图11所示的试验验证平台，惯导与激光雷达安装于移动平台上，惯导与激光雷达的参数如表4所示，移动平台沿预先布置的“轨道模拟线”运行，移动速度约0.1～0.2 m/s，在移动平台一侧放置了一系列“立柱模拟装置”，模拟液压支架的立柱。在移动平台上还放置了RTK 移动站，配合千寻位置公司提供的网络差分服务，可获得精度为厘米级的定位结果，网络差分结果不参与算法的使用，仅为试验提供考核依据。

图11 试验平台示意图Fig.11 Schematic diagram of test platform

考虑到在实际开采过程中采煤机的运行轨道往往因液压支架推移不精确以及工作面的“上蹿下跳”而发生弯曲，因此在试验过程中针对轨迹不发生弯曲和发生弯曲两种情况进行验证，轨迹分别对应图12(a)和(b)。参考当前的综采工作面安全质量标准，轨道的最大弯曲误差在0.1 m 内。

图12 移动平台运行轨迹图Fig.12 Trajectory diagram of mobile platform

在每次试验开始前均先对安装偏角进行标定，并设置特征点数目为3，特征点与激光雷达的最小距离为6 m。两种不同轨迹对应的位置误差如图13所示。

从图13(a)可看到，北向和高度方向误差曲线呈齿形波动，东向误差除了齿形波动外还存在快速的发散现象，根据第2 节的分析可知，东向误差发散是受航向的影响。从图13(b)可看到，位置误差表现出的现象与图13(a)相似，说明即使轨道存在小的弯曲，本文提出的组合方法仍可实现定位功能，误差源及其影响与平直轨道的情况相同。不论运行轨道是否存在弯曲，随着移动平台在50 m 轨道运行约50 min，北向和高度方向误差均可维持在0.3 m 内，而东向误差可维持在0.5 m 内，整体而言本文提出的组合方法可满足当前的定位需求。

图13 位置误差曲线Fig.13 Position errors curve

4 结 论

本文研究了基于惯导/激光雷达的采煤机组合方法，并搭建了试验平台，试验表明在50 m 轨道上运行约50 min，东向、北向和高度方向位置误差均维持在分米级，满足当前的定位需求。通过对仿真与试验的分析可以得到以下结论：

(1) 惯导的姿态误差、惯导与激光雷达之间的安装误差以及激光雷达的系统测距误差是影响定位精度的主要因素；

(2) 惯导姿态误差的初值以及安装误差对定位精度的影响可通过预标定处理得到改善；

(3) 激光雷达的系统测距误差对定位精度的影响可通过调整特征点的选择得到改善；

(4) 航向误差的发散是制约定位精度提升的重要因素。增加诸如车辆运动学约束、闭环修正模型等辅助技术，进一步提高航向估计能力，是必要的工作，也是本文后续的研究方向。