“惯导+里程计”的采煤机定位方法研究

2021-10-26石金龙马宏伟毛清华张羽飞华洪涛

煤炭工程 2021年10期

石金龙，马宏伟，毛清华，张羽飞，华洪涛

(1.西安科技大学电气与控制工程学院，陕西西安 710054；2.陕西省矿山机电装备智能监测重点实验室，陕西西安 710054；3.西安科技大学机械工程学院，陕西西安 710054)

采煤机精准定位是实现煤矿综采工作面智能化开采的关键[1]。由于井下环境复杂，一般传感器容易受外部影响，定位效果不理想[2，3]，基于捷联惯导的采煤机定位系统，受到国内外专家学者的广泛关注。杨海[4]等提出一种基于捷联惯导的采煤机定位方法，利用陀螺仪与加速度计采集的三轴角速度和三轴加速度信息，解算得到采煤机的位姿信息。虽然惯导系统可以在短期内检测采煤机位姿，但在长期运行过程中，受到传感器漂移与累积误差的影响，定位精度不足。李楚[5]提出一种惯导与视觉相机组合定位的方法，通过采集采煤机姿态信息与位置信息，视觉摄像机采集信标修正惯导的累计误差，提高采煤机定位精度，但采煤机运行过程中灰尘过多，影响视觉相机定位精度；夏婷[6]提出了一种惯导与超声波组合的采煤机定位方法，运用容积卡尔曼滤波算法对惯导和无线传感器的位置信息进行滤波，输出可信的位置信息，但超声定位系统需要信标安装数量过多，节点出现异常后定位精度不足。李昂[7]等应用惯导与轴编码器组合定位方法，构建采煤机定位方程，验证了方法的可行性，虽然惯导与里程计两者受外界因素干扰较小，也不向外辐射能量，自主定位能力强等特点，但采用的惯导精度较低，组合后定位精度较差。澳大利亚联邦科学与工业研究组织(CSIRO)的LASC系统[8]，使用高精度光纤陀螺仪与里程计，结合找直算法，分析采煤机空间位置信息，但国外惯导领域长期对我国处于封锁状态，技术不对外开放，使用价格高昂。

综上所述，针对当前国内采煤机定位精度不足，稳定性较差的问题，本文采用高精度国产光纤捷联惯导与里程计组合，测量采煤机位姿，建立惯导与里程计组合定位模型，获得采煤机运行轨迹。通过仿真实验与井下综采工作面实验，验证了“惯导+里程计”组合惯导在煤矿井下的定位精度与可靠性。

1 “惯导+里程计”采煤机位姿测量原理

综采工作面中的“三机”有采煤机、刮板输送机和液压支架，采煤机骑行在刮板输送机上，刮板输送机沿工作面截割方向放置，置于液压支架前，液压支架通过液压缸推动刮板输送机移动，向前推进。捷联惯导系统安装在采煤机内部，里程计与采煤机齿轨轮轴连接。通过惯导解算采煤机位姿信息，卡尔曼滤波融合惯导与里程计单位时间内位置信息，利用闭环反馈的方式校正惯导与里程计的误差参数，输出采煤机的位姿，工作原理如图1所示。

图1 “惯导+里程计”测量位姿原理图

2 “惯导+里程计”组合定位模型

对惯导的位姿信息进行解算是是惯性导航系统的核心部分，是影响整个惯导系统精度的主要因素之一。采煤机的位姿解算更新算法主要有欧拉角法、四元数法、方向余弦矩阵法、Rodrigues法，其中四元数法计算量小，精度高，应用范围较广，本文将采用四元数法解算惯导位姿。

捷联惯导固定在采煤机上，设采煤机坐标系为载体坐标系(b系)，采煤机前进方向为xb轴，机身平面上与移动方向垂直的为yb轴，与机身平面垂直的方向为zb轴。选取地理坐标系(g系)为导航坐标系(n系)，以采煤机重心为中心，规定xn，yn，zn分别指向东、北、天方向。其中，采煤机的航向角为φ，俯仰角为θ，横滚角为γ。

采煤机的姿态四元数微分方程为：

(1)

(2)

根据采煤机姿态转换矩阵：

(3)

得到姿态角为：

θ=sin-1(T32)

(4)

γ=-tan(T31/T33)

(5)

φ=tan-1(T12/T22)

(6)

采煤机速度更新的微分方程为：

(7)

采煤机位置更新微分方程为：

(8)

通过上述方法，即可得到惯导解算后的姿态、速度、位置方程，接下来分析里程计输出模型。

里程计通常采用脉冲方式输出采样时间间隔内的里程增量，里程计与采煤机齿轨轮连接，使里程计的速度输出方向时刻指向采煤机运行方向。假设采煤机在运行过程中正常行驶，与齿轨齿合，里程计的输出值为采煤机的运行方向的速度量。设采煤机上里程计的坐标系为m系，是与采煤机固连的右-前-上坐标系，即ox轴沿采煤机纵轴向前，oy轴沿采煤机横轴向右，oz轴沿垂直地面向上，里程计在采样时间间隔内输出的脉冲数N在m系投影矢量为：

(9)

(10)

进一步整理后，计算得到里程计的位置增量为：

(11)

式中，αφ、αθ为采煤机坐标系相对于里程计坐标系的航向、俯仰安装角；i为输出轴到齿轨轮传动比；N为里程计旋转一周产生的脉冲数目；n(t)为单位时间t内产生的脉冲数目，行走轮的分度圆半径为R。

只考虑标度因数误差δKD、安装误差角δαφ、δαθ，则里程计实际输出的采煤机坐标系下分量可简化为：

(12)

式中，wd为随即噪声干扰。

本文建立位置观测模式下的系统模型，与传统速度观测模式下的组合算法相比矩阵维数更少，计算的精度更高[9]。组合系统中包含的误差主要为惯导的姿态、速度、位置误差，陀螺仪与角速度计零漂误差，里程计刻度因数误差和俯仰角航向角安装误差，建立的组合定位系统与这些误差之间存在着互相关联，构成了一个闭环反馈系统，对误差进行优化修正，提高定位精度。

建立Kalman滤波器系统方程为：

(14)

式中，F(t)为系统矩阵；w(t)为系统噪声；G(t)为系统噪声转移矩阵，二者一般为白噪声。

其中该组合导航状态变量为18维：

X=[(δvn)T，(φn)T，(δp)T，(εb)T，

(b)T，δKD，δαθ，δαφ]T

(13)

式中，vn为速度误差；φn为姿态误差；δp为位置误差；εb为陀螺仪零漂；b为加速度计零漂；δKD为里程计标度因数误差；δαφ、δαθ为俯仰、航向安装误差角。

其中里程计刻度因数误差、安装误差角均可视为随即常数。因此，对应的系统矩阵为：

(15)

式中，F1为惯导系统误差方程对应的转移矩阵，计算方法如文献[10]所示。

惯导位置更新需要计算每个周期导航坐标系下的位置增量，位置增量的计算公式为：

(16)

由里程计输出的航位推算位置更新算法公式为：

(17)

利用单位时间内惯导和航位推算位置增量之差作为量测，将航位推算误差扩充到状态中。由于惯导位置解算使用微分计算，随着时间的增加误差累积增大，因此采用导航坐标系下惯导位置增量和航位推算位置增量的每秒位置之差作为量测值：

(18)

式中，K为1s时间内位置更新次数。

在采煤机运行过程中惯导解算的真实位置和里程计航位推算的位置相同，根据公式进一步整理，即可得到观测矩阵H(t)。

(19)

(20)

F(k|k-1)=F(k，k-1)P(k-1)×

FT(k，k-1)+Q(k-1)

(21)

量测更新方程为：

K(k)=P(k|k-1)HT(k)[H(k)×

P(k|k-1)HT(k)+R(k)]-1

(22)

(23)

P(k)=[I-K(k)H(k)]P(k|k-1)

(24)

对于确定的滤波方程，选取合适的初始值X0和均方误差初值P0分别进行滤波回路与增益回路的时间更新与量测更新，利用递推算法，随着滤波次数增加，提取的被估计误差值的信息浓度增加，逐渐接近真实值，提高系统误差补偿精度。

3 实验验证

3.1 仿真实验

通过模拟惯导与里程计数据，应用闭环卡尔曼滤波组合导航算法，对运行轨迹进行仿真，加入了爬坡转向等情况，其中选取的误差设定如下：光纤陀螺仪漂移为0.01°/h，加速度计零偏为5×10-5g，井下采煤机运行速度较慢，打滑情况较少，因此设里程计刻度系数误差为0.01%；初始位置误差为0，惯导水平姿态角初始误差为0.5′，航向角误差为0.5′。仿真轨迹为：首先以0.1m/s2加速度运行10s，上行15s，保持不变10s，下行15s，保持匀速运行150s，向左运行15s，向右运行15s，保持匀速运行100s，最后以-0.1m/s2加速度运行10s，对比纯惯导系统运行轨迹、理论轨迹和测量轨迹对比，仿真轨迹如图2所示。

图2 三维轨迹仿真

纯惯导测量轨迹随着运行时间推移，东向最大误差13.5m，北向最大误差5.02m，天向最大误差2.46m，轨迹发散较为严重。组合定位轨迹中，东向最大误差2.23m，北向最大误差0.04m，天向最大误差0.19m，抑制了定位误差发散。因此采煤机采用高精度光纤陀螺仪与里程计组合定位系统，应用闭环卡尔曼滤波修正方法有效的提高了定位精度与稳定性。

3.2 井下实验

为了测试在井下复杂环境下的定位精度与稳定性，将此组合定位系统安装到某煤矿综采工作面的采煤机上进行井下实验，惯导安装于弱电箱内，惯导系统由高精度国产光纤惯导、任务机、UPS电源组成。

采煤机低速在工作面113.43m处到139.37m处反复空刀运行，单刀运行约25min，共用时40min，保持液压支架不动，验证采煤机定位精度。其中，井下每个液压支架宽度为1.5m，采煤机型号为MG650/1630-WD。建立采煤机坐标系，x轴指向采煤机牵引方向，y轴指向工作面推进方向，z轴指向高度方向。惯导对准后，陀螺仪精度为0.01°/h，加速度计精度为5×10-5g，采煤机往复运行过程中，俯仰角、横滚角、航向角初始值分别为3.69°、6.27°、181.6°。采煤机在连续两刀往复行走，观测值与实验值的角度对比如图3所示，俯仰角最大误差为0.07°，横滚角最大误差为0.184°，航向角最大误差为0.22°，角度误差如图4所示。

图3 角度对比

图4 角度差

观测轨迹与实验轨迹的位置对比如图5所示，x方向最大误差为2.046cm，y方向最大误差为2.8cm，高度方向最大误差为1.4cm，误差曲线如图6所示。实验过程中存在一定误差，主要原因在于采煤机往复行走过程中一是受浮煤阻力及振动等影响，产生位姿变化；二是刮板输送机中部槽与液压缸之间销耳间隙，导致位姿变化。综合分析，“惯导+里程计”测量系统位姿检测精度与稳定性较好，在复杂环境下定位误差小于5cm。