采煤机三维轨迹的惯性基厘米级相对测量方法

江一夫，李四海，谢 波，严恭敏，雷孟宇，

(1. 西北工业大学自动化学院，西安 710029；2. 西安航天精密机电研究所，西安710100；3. 西安科技大学 机械工程学院，西安710054)

采煤机精确自主定位导航技术是实现长壁综采工作面自动化生产的关键技术之一。煤矿安全规程规定，综合机械化采煤时，工作面煤壁、刮板输送机和支架必须保持直线，底板和顶板必须保持平整，即“三直两平两畅通”中的“三直两平”。当前即使国内自动化程度最高的综采工作面，推进2～3 刀后也需要依赖人工调整支架，平直度问题严重制约着综采自动化水平[1]。国外长壁自动化指导委员会（Longwall Automation Steering Committee, LASC）研制的综采工作面自动化系统已经趋于成熟[2]，其对工作面平直度的检测误差小于10 cm，是目前行业内最为主流的采煤机位置测量解决方案。而我国在煤矿井下装备自主导航方面的研究起步较晚，总体来说尚处于落后局面。

惯性导航系统由于其导航信息的连续性、自主性和高可靠性，是目前井下拒止环境中一种不可替代的位置测量方式。鉴于惯性导航定位误差具有随时间发散的特性，其与编码器组合航位推算成为一种简单易用的误差抑制方式[3]。文献[4]对航位推算算法进行了误差分析，认为惯导三维定位精度与惯导初始对准误差及其相对采煤机的安装偏角误差有关。为了消除这些误差的影响，文献[3]基于航位推算轨迹与实际轨迹的相似性，提出了两点法偏差角校准算法。但这种方法需要井下具有全工作面的精确GPS-RTK 信息，且惯导每次断电后初始对准都需要重新校准，不符合实际采煤工况。文献[5]提出了卡尔曼滤波动态零速修正算法来解决该问题，但事实上航位推算本质上就利用了动态零速修正的非完整约束条件，且动态零速修正中航向误差和航向安装角误差在采煤机近似直线运动中几乎不可观测，反而会由于卡尔曼滤波系统模型不准确、振动量测噪声大等问题，难以实现采煤机轨迹的精确刻画。

文献[6]提出了全站仪和超宽带（Ultra Wide-band, UWB）组合的方式，但全站仪和UWB 信号都易受粉尘和非视距传播的影响而出现定位无效的情况，且基站需要跟随工作面的推进而移动，影响采煤作业效率。文献[7]提出了回环路径识别算法，认为采煤机在回环点的跟机移架距离近似为1 m，以此作为量测对位置误差进行修正。但由于机械滑动和实际运行调整，真实推进量会偏离1 m，文献[8][9]因此采用激光雷达辅助来测量采煤机的移架距离，增加自动调直系统的长时间可靠性，但这无疑都增加了系统的硬件成本。

应该注意到的是，上述定位方法及其误差分析都是相对地理坐标系而言的，但实际采煤作业关注的是长壁工作面轨迹的平直度，而非精确的地理坐标。基于此，从航位推算轨迹和真实轨迹的关系出发，提出一种相对测量方法，可以避开惯导系统初始对准误差和安装偏角的影响，工作面三维轨迹的测量精度仅仅取决于导航阶段的航向和姿态保持精度，即将航姿测量误差维持在初始对准误差的能力，其理论定位精度可到厘米级。

1 航位推算误差分析

定义b系(Oxbyb zb)为惯性测量坐标系，xb、yb和zb轴分别与惯性测量组合本体右前上重合；定义m系Oxm ym zm为采煤机坐标系，ym轴沿采煤机编码器表征的虚拟纵轴指向前，xm轴位于采煤机横截面垂直于ym轴指向右，zm轴与xm、ym轴构成右手正交坐标系；定义n系(Oxn yn zn)为导航坐标系，其与东北天地理坐标系重合。令表示采煤机坐标系到惯性测量坐标系之间的转换矩阵，αθ为俯仰安装偏角，αψ为航向安装偏角，表示惯性测量坐标系到导航坐标系的转换矩阵，ΔSi表示编码器第i个采样周期的里程增量，表示第i个采样周期的地理系三维位移增量，则有：

从式(1)可以看出，航位推算实质上已经利用了采煤机的非完整约束，即侧向和法向速度为零的量测信息。式(1)描述了测量采煤机三维地理轨迹的基本原理，惯性测量和编码器输出频率越高，轨迹刻画得越精细。

令φU表示惯性测量航向误差，δαψ表示航向安装偏角误差，δαθ表示俯仰安装偏角误差，同时忽略编码器刻度因子误差，可以由式(1)计算得到每周期位置增量误差模型。

认为惯性测量航向误差φU，安装偏角误差δαψ和δαθ在一段时间内近似为常值，对式(2)积分可以得到位置误差模型。

可见φU+δαψ仅产生水平位置误差，且垂直于采煤机水平位移方向，由于工作面近似直线，因此φU+δαψ产生垂直于工作面的定位误差，且误差大小与位移的大小成正比。δαθ仅产生高度误差，且误差大小与行驶里程相关。具体可用图1 表示。

图中O为采煤机起始位置，航位推算在该点无误差，当采煤机沿工作面行驶到A点时，航位推算显示位置为A′。则图中表示真实位移矢量表示航位推算位移矢量在水平面的投影，在水平面的投影，误差角φU+δαψ产生的水平位置误差与水平位移矢量垂直，为在铅垂面的投影，表示在铅垂面的投影，误差角δαθ产生的高度误差为

实际工作时，采煤机在刮板运输机上往复运动，当采煤机由前进转为后退时，其位移矢量ΔSn和里程ΔS也由正转为负，当采煤机回到原点时产生的定位误差也变为零，因此无法利用原点重合信息对初始对准误差和安装角误差进行标定。文献2 因此提出采用GNSS-RTK 对航向安装角和俯仰安装角进行精确标定的方法，但井下拒止环境没有精确的GNSS-RTK 条件，何况每次惯导上电重新寻北，寻北误差φU都是变化的，即便采用等效零偏漂移为0.003 °/h 的高精度光学陀螺，在北纬40 °地区的理论初始对准航向误差最大可达0.05 °，长度为300 m 的工作面，其水平定位误差可以达到δD=300 ×0.05×π180 =0.2618m ，难以达到厘米级定位精度。

2 采煤机三维轨迹的相对测量方法

2.1 相对定位的基本原理

从图1 中可以看出，航位推算轨迹与真实轨迹具有轨迹相似性，通过一定角度的旋转，两者能够重合。考虑到工作面的平直度测量并不需要地理系坐标，而只需要轨迹点偏离目标工作面的坐标，如图[2]所示，定义df系(Oxdf ydf zdf)为目标工作面坐标系，xdf轴由起始目标工作面的机头指向机尾，ydf轴垂直于xdf轴及其水平面投影构成的平面，指向煤层，zdf轴与xdf、ydf轴构成右手正交坐标系。若采煤机第一刀走过起始目标工作面的首尾两个端点，可以获得两个端点的航位推算坐标。定义rf系(Oxrf yrf zrf)为相对工作面坐标系，xrf轴为两个航位推算坐标的连线，yrf轴垂直于xrf轴及其在水平面投影构成的平面，zrf轴与xrf、yrf轴构成右手正交坐标系。图中J为真实轨迹上的一点，J′为航位推算轨迹上的一点，K为J在Oxdf ydf平面的投影，K′为J′在Oxrf yrf平面的投影，L为J在Oydf zdf平面的投影，L′为J′在Oyrf zrf平面的投影。从图中可以看出，相对工作面坐标系及其航位推算轨迹相当于目标工作面坐标系及其真实轨迹整体旋转了一定角度，航位推算轨迹点在相对工作面坐标系的三维坐标与真实轨迹点在目标工作面坐标系的三维坐标是一致的，实际采煤机轨迹各位置点偏离目标工作面的偏移量能够准确测量。

图1 航位推算位置误差示意图Fig.1 Positioning error diagram of dead-reckoning

图2 采煤工作面平直度的相对测量原理Fig.2 Relative measurement principle of coal mining face flatness and straightness

因此，虽然惯性导航初始对准航向误差较大，其相对采煤机的安装偏角无法井下标校，但是采煤机轨迹目标工作面的定位坐标能够得到测量，测量精度取决于惯性导航系统的航姿保持精度。假设陀螺等效零偏漂移为0.003 °/h，采煤机1 h 走完300 m 工作面，考虑到惯性导航误差的舒拉振荡周期和地球振荡周期，其航向保持误差不大于0.003 °，产生的定位误差δD=300 ×0.003×π180 =0.0157m 。

2.2 相对定位坐标计算方法

采煤机相对定位方法首先在于确定目标工作面的两个端点，其可以根据支架到煤壁的距离测定，也可简单认定采煤机在机头和机尾的位置为目标工作面两个端点。驱动采煤机经过这两个端点，不考虑惯性测量与采煤机直接的安装偏角和寻北误差，此时航位推算得到的采煤机地理系坐标为：

由此测量得到的采煤机轨迹相对真实轨迹旋转了一定角度。这里将航向安装偏角和俯仰安装偏角当作小量处理，因此要求安装时惯性测量坐标系与采煤机坐标系近似一致。如图3 所示，以采煤机经过两个端点时测量坐标的连线建立相对工作面坐标系。

图3 相对工作面坐标系与导航坐标系的关系Fig 3 Relationship between relative face frame and navigation frame

假设经过目标工作面首端点O时的航位推算n系三维坐标为经过尾端点A′时的航位推算n系三维坐标为则有：

由相对工作面的定义可知，xrf轴在n系的单位矢量为：

yrf轴垂直于OA′B′平面，因此yrf轴在n系的单位矢量为：

由此可以计算得到zrf轴在n系的单位矢量为：

导航坐标系到相对工作面坐标系的转换矩阵为：

采煤机在相对工作面坐标系同时也是目标工作面坐标系的坐标为：

至此，可以绘制出反映工作面平直度的采煤机轨迹，计算出各支架偏离初始目标基准线的距离。

轨迹坐标转换到相对工作面坐标系后，目标工作面两个端点被设置在同一个大地高度上，只反映了高度轨迹的形状，不能反映工作面的真实高程变化。为了实现智能自动调高，需要引入目标工作面两端巷道的相对高度差信息hΔ ，该信息具体可由震波CT等地形探测手段获取。此时将OA′改写为：

按照式(6)～(10)重新计算相对工作面坐标系采煤机轨迹，既可以实现工作面平直度测量，又可以反映工作面真实高度变化。

2.3 断电的处理方法

惯导系统由采煤机供电，在实际采煤时会遇到采煤机停机断电的情况，此时惯导系统丢失可用的连续积分信息，一般而言，需要重新进行初始对准，才能继续开展轨迹测量。但5 min 静止对准不符合实际工况和高效开采原则，而运动中对准需要精确的惯性测量坐标系与采煤机坐标系的安装偏角ψα和θα，且不管何种对准方法，都会引入了初始对准误差，使断电前后轨迹测量不连续，相对测量方法失效。

对于采煤机断电的问题，目前LASC 系统额外给惯导系统配备不间断电源UPS，但UPS 电量有限，长时间条件下惯导系统最终还是会陷入断电状态。考虑到采煤机和惯导系统固联安装且采煤机依靠电力驱动，采煤机断电则惯导系统保持静止，采煤机上电则惯导系统上电，因此可以在正常上电工作时实时存储导航信息，在遭遇断电时，由惯导系统内部闪存或者上位机存储该信息，在重新上电后，惯导系统读取该信息进行接续导航。

为了实现接续惯性导航和航位推算，需要实时保存当前时刻方向余弦矩阵和航位推算坐标，其中方向余弦矩阵可以等价转换为航向角、俯仰角和横滚角保存，降低保存数据量。为了实现相对测量转换，需要保存导航坐标系到相对工作面坐标系的转换矩阵，从图3 可以看出导航坐标系经过两次旋转即可转换到相对工作面坐标系，因此也可等价转换为航向旋转角和俯仰旋转角保存。考虑到第一刀尚未走完，采煤机就可能断电，因此还需保存是否经过起始端点标志、起始端点航位推算坐标是否经过末尾端点标志以及末尾端点航位推算坐标这样的话无论采煤机何时断电，都可以在重新上电时实现连续位置测量，而不需要重新对准。

2.4 相对定位方法的误差分析

根据前文分析，采煤机三维轨迹的相对测量方法避开了初始对准误差和安装偏角误差的影响，定位精度主要取决于航姿保持精度。

采煤机工作面近似为直线，航向基本保持不变，按照捷联惯导误差传播方程进行误差分析，惯导系统初始对准航向误差导致的航向保持误差和等效东向陀螺零偏导致的航向保持误差相互对消，同时水平初始对准误差与水平加计零偏造成的俯仰角保持误差相互对消。导航阶段的俯仰保持误差Esφ和航向保持误差φUs可以分别表示为：

其中Eε表示等效东向陀螺零偏，Nε表示等效北向陀螺零偏，Uε表示等效天向陀螺零偏，t表示导航时间，L表示当地地理纬度，ieω表示地球自转角速度，约为 15.0411 °/h，sω为舒拉角频率，约为255.9242 °/h，ωf为傅科角频率，ωf=ωiesinL，在纬度40L= °地区，fω约为9.6682 °/h。

式(12)中，取正余弦项都为最大值 1，εE=εN=0.003 ° h ，可以计算得到俯仰角保持误差最大为0.00134 °。式(13)中，sω远大于ieω，因此忽略最后一项的影响，令 sin（ωiet）为最大值 1，εU=-0.003 ° h ，εN=0.003 ° h ，L= 40°，可以计算航向角的保持误差最大为0.011 °。

注意到式(3)中，采煤机定位误差与采煤机位移大小和里程有关，采煤机采用前进后退的方式，而非走完一个工作面掉头的方式，无论是位移矢量还是里程都是一正一负的方式，因此惯性测量误差导致的定位误差在一正一负的过程中也是相互对消的。最大定位误差取决于工作面长度与航姿保持误差的乘积，假设工作面长度为300 m，则理论最大水平定位误差为300m ×0.011 °×180π=0.058m ，最大高程定位误差为300m ×0.00134 °× 1 80π=0.007 m 。

3 试验验证

采用自研激光陀螺惯导系统在山西双柳煤矿进行了试验验证，由于编码器信息采用无线传输给集控，再由集控下发给采煤机惯导，因此存在不定期延时，且分辨率相当粗糙，许多时候达到0.5 m。惯导系统激光陀螺精度约为 0.003 °/h，加速度计精度约为50 μg。实际采煤作业时，由于安全因素和工作条件等限制，无法下井准确测量实际支架的推移距离，而控制系统给定的推移距离可能偏移实际距离多达数十厘米，无法以之作为位置基准考核惯导系统位置测量精度，因此井下实验采取了跑空刀的验证方式。采煤机从中间进刀在固定工作面进行了几次往复运动，期间由于工作面机电检修缘故，在60架和116架附近进行了两次长达半小时的断电。理论上采煤机的定位轨迹是重合的，其不重合误差即为惯导系统的定位误差。整个试验持续时间6 h，其中上电工作时间约为5 h，绘制出采煤机三维/定位轨迹如图4 所示。

图4 采煤机空刀试验的三维轨迹Fig.4 Shearer 3D path in empty cutting test

为了更直观地分析定位精度，将水平定位轨迹和高程定位轨迹分别画出，如图5 和图6 所示。

图5 采煤机空刀试验的水平定位轨迹Fig.5 Shearer horizontal positioning path in empty cutting test

图5 中最大水平定位误差为3.9 cm，图6 中最大高程定位误差为1.5 cm。从图5 中可以看出，水平轨迹在75 架、90 架以及100 架等轨迹曲率变化较大的地方，呈现误差变大的现象，且曲率变化越大误差也越大。由于惯导系统航向保持误差主要与导航时间有关系，跟轨迹形状无关，经数据处理分析，认为在采煤机经过曲率变化较大轨迹段时，惯性测量坐标系相对编码器所表征的虚拟采煤机坐标系的航向安装角发生了变化，本质上惯组与采煤机是固联安装，因此也可以理解为采煤机与刮板运输机之间存在的航向安装间隙导致航向安装角发生了变化，产生了额外的水平定位误差。这种误差在工程实际中存在一定的随机性，为不可避免的小量误差。而高程定位精度受轨迹曲率影响小，这也间接说明采煤机与刮板运输机之间的俯仰安装间隙相对较小。即使扣除安装间隙的影响，图5 和图6 仍然反映出高程定位精度整体优于水平定位精度，这一点符合式(12)和式(13)中相对测量方法定位误差理论分析结果。主要原因是俯仰角保持误差与航向角保持误差在捷联惯性导航中呈现不同的传播规律。

图6 采煤机空刀试验的高程定位轨迹Fig.6 Shearer height positioning path in empty cutting test

从试验结果还可以看出相对测量方法能适应采煤机断电的情形，其在60架和116架附近并没有因为断电重启产生定位误差放大的情形。整体而言，在高达5 h 的工作时间内，采煤机三维轨迹的相对定位精度达到厘米级，能够达到与国外LASC 系统相当的水平。

4 结 论

基于高精度惯导和编码器，本文提出了一种无源自主的采煤机三维相对定位方法，解决了综采工作面平直度精确测量问题，水平方向最大位置误差为3.9 cm，高程方向最大位置误差为1.5 cm。该方法简单、可靠、环境适应性强，能够满足工程实际中工作面直线度的智能调整，液压支架自动跟机移架、采煤机自适应智能调高等的需求，有利于推进井下综采工作面向安全、高效、绿色、智能化、少人化方向发展。