0 引言
采煤机定位是智能综采工作面的关键技术之一，可解决依靠人工根据煤层分布状况对采煤机的综采作业路径和参数进行调整的问题，能够为协调采煤机、刮板输送机及液压支架的联动运行打下基础。目前常用的采煤机定位技术主要有基于红外信号定位[1]、基于超声波信号定位[2]、基于里程计定位[3]、基于无线信号定位[4]、基于惯导定位[5]等。其中基于红外信号、超声波信号、里程计、无线信号定位等技术具有实现简单、可操作性强的优点，但存在红外信号定位实时性弱，超声波信号衰减严重，里程计定位维度低且存在累计误差，无线信号定位易受井下复杂信道传播环境影响而失效等缺点。惯导在井下工作时具有定位维度高、实时性好、自主性高、抗环境干扰能力强、数据全面、安装条件简单等优点，具有非常好的综合定位性能[6]。然而惯导存在定位误差累计、姿态角及位置漂移这2个固有缺陷，无法长时间独立工作。因此，有学者提出引入误差补偿技术和多传感器组合定位技术。
误差补偿技术是对惯导在测量过程中产生的误差进行相应补偿。Fan Qigao等[7]提出了一种基于惯导的采煤机定位误差分析方法，首先建立误差传递模型，再通过扩展卡尔曼滤波(Extended Kalman Filter，EKF)估计惯导失准角和采煤机速度并进行补偿，但补偿效果依赖于误差传递模型的准确度。韩勇强等[8]提出了一种惯导零速修正算法，使用Sage-Husa卡尔曼滤波结合高阶拟合的方法对径向速度残差导致的定位误差进行补偿，但需要在较长时间的停车情况下进行，影响工作效率。Chen Yuming等[9]基于采煤机动力学模型和振动特性，得到初始姿态角并对恶劣环境引起的线性振动进行补偿，有效提高了初始对准的收敛速度和精度，但无法解决惯导定位误差累计的问题。Wang Shibo等[10]利用里程表辅助惯导，结合长壁采煤工作面采煤机封闭路径的运动特性和卡尔曼滤波器，提出了一种惯导定位的最优估计模型，降低了惯导航向角漂移对定位精度的影响，但无法补偿惯导俯仰角和横滚角的漂移误差。
多传感器组合定位技术通过融合惯导和其他传感器输出信息，实现定位误差修正。应葆华等[11]基于惯导与无线传感器网络的耦合模型，每隔一定时间利用无线定位对惯导的定位结果进行校正，能够有效减小平均定位误差，但在井下建立无线传感器网络需要在多点布置无线传感器，成本较高。周开平[12]利用加速度计和陀螺仪组合的惯导解算出采煤机位置信息，结合地图匹配技术进一步降低了惯导在长时间工作中出现的累计误差，但考虑到工作面地形复杂，难以获取精准地理模型，该方法实现的技术难度较高。
本文提出了一种基于自适应卡尔曼滤波的双惯导采煤机定位方法。利用2套惯导系统的位置信息构成双惯导定位模型，克服了单惯导定位误差累计的缺点；但双惯导定位模型可能存在状态突变，导致定位模型准确度降低，因此将双惯导定位模型和自适应卡尔曼滤波算法结合，以减小状态突变对定位精度的影响。
1 定位坐标系建立
2套惯导系统在采煤机上的安装位置如图1所示[13]。在采煤机机身表面的几何中心位置安装惯导1，在同一平面内安装惯导2，惯导2与惯导1之间的距离为一定值r且相互之间呈一定夹角α。定义采煤机行进方向为Xb轴，工作面推进方向为Yb轴，采煤机机身的竖直方向为Zb轴。
图1 双惯导系统安装位置
Fig.1 Installation position of dual inertial navigation systems
定位坐标系如图2所示。以采煤机机身上表面的几何中心为坐标原点O，建立采煤机的载体坐标系(O-XbYbZb)，如图2所示。选择东-北-天坐标系(O-XnYnZn)作为参考坐标系，Xn指向北方向，Yn指向东方向，Zn垂直于Xn轴和Yn轴。ψ，θ，φ分别为采煤机机身的横滚角、俯仰角及航向角。
图2 定位坐标系
Fig.2 Positioning coordinate system
2 双惯导定位模型
惯导系统在采样周期内输出加速度和姿态角，对加速度积分可得该采样周期内的采煤机速度。由于惯导系统输出的加速度数据是离散形式的，可采用Lagrange插值多项式对一段时间内的离散加速度函数进行插值得到插值函数，再对插值函数积分[14]。
取3次加速度采样数据得到3个插值点(k-2,ak-2)，(k-1,ak-1)，(k,ak)，其中k为当前时刻，ak为当前时刻的加速度。插值函数为
(1)
式中x为采样时刻。
对Lk(x)在(k-1，k)区间上进行积分，可得采样周期内的速度增量。上一时刻的速度加上采样周期内的速度增量即为当前时刻的速度。
vk=vk-1+
Lk(x)dx
(2)
式中：vk为当前时刻的速度；vk-1为上一时刻的速度；T为采样周期。
由于采样周期非常小，可将当前时刻的速度作为1个采样周期内的平均速度，其与采样周期的乘积为采煤机在采样周期内的位移增量ΔS。根据采煤机的实际运动特点(采煤机沿Xb轴运动)，ΔS在参考坐标系上的投影为
(3)
式中
为采煤机载体坐标系到参考坐标系的坐标转换矩阵。
(4)
根据航迹推算原理，采煤机上的2套惯导系统在参考坐标系下的位置为
(5)
式中：Pi(k)为当前时刻第i(i=1，2)套惯导系统在参考坐标系下的位置；Ni(k)为北向的坐标；Ei(k)为东向的坐标；Ui(k)为天向的坐标；
为当前时刻第i套惯导系统的坐标转换矩阵；ΔSi为第i套惯导系统在采样周期内的位移增量。
根据式(5)，选取X(k)=[N1(k) E1(k) U1(k) N2(k) E2(k) U2(k)]T作为状态量，可得状态方程：
X(k)=A(k)X(k-1)+D(k)+W(k)
(6)
式中：A(k)为状态转移矩阵；D(k)为控制矩阵；W(k)为过程噪声。
(7)
选取2套惯导系统之间的距离r和夹角α作为卡尔曼滤波器的观测量Z(k)，r和α与状态量之间的关系如下：
(8)
由式(8)结合状态量X(k)，可得非线性的观测方程：
Z(k)=f(X(k))+V(k)
(9)
式中：f()为表示状态量和观测量之间非线性关系的函数；V(k)为观测噪声。
用一阶泰勒展开式对式(9)进行线性化处理[15]，得到线性化后的观测方程：
Z(k)=H(k)X(k)+V(k)
(10)
式中H(k)为雅可比矩阵。
(11)
式中d=(N1(k)-N2(k))2+(E1(k)-E2(k))2。
整理可得EKF过程：
(1) 计算一步预测值：
(12)
(2) 计算一步预测均方误差：
(13)
式中:P(k-1)为上一时刻状态估计误差的均方误差矩阵；Q(k)为W(k)的协方差矩阵。
(3) 更新滤波增益：
(14)
式中R(k)为V(k)的协方差矩阵。
(4) k时刻的状态估计更新：
(15)
(5) k时刻的均方误差更新：
[I-K(k)H(k)]T+K(k)R(k)KT(k)
(16)
式中I为单位矩阵。
3 自适应卡尔曼滤波
卡尔曼滤波器在迭代更新时的残差为
(17)
在滤波过程中，当状态量无误差且观测量无较大偏差时，滤波器输出的残差服从标准正态分布。根据卡尔曼滤波的正交性原理[16]，残差的马氏距离的平方服从卡方分布，因此可构造基于残差的卡方检验值：
(18)
式中：ξk服从自由度为m的卡方分布，即ξk～χ2(m)，本文中m为观测量的维数；Crk为残差rk的协方差矩阵。
(19)
由式(6)的状态方程可知，2套惯导系统在采煤机行进中同一时间段内输出的姿态角、加速度存在较大差异时，会导致双惯导定位模型出现状态突变，状态更新准确度降低，此时ξk不再服从卡方分布。因此，通过计算基于残差的卡方检验值，可评估双惯导定位模型的状态。评估方法：通过设置卡方检验的显著性水平β，查表得到该显著性水平下的卡方检验的临界值χ
(m)，当ξk>χ(m)，就可判断双惯导定位模型出现了状态突变。
当状态突变时，滤波器状态方程的估计值偏离真实状态较远，过程噪声增加。状态突变程度越大，卡方检验值ξk越大。因此，引入三段模糊判别函数调整过程噪声的协方差矩阵Q(k)，减少状态估计不确定性对于滤波结果的影响。三段模糊判别函数通过设置不同的卡方检验临界值，设定状态检测阈值的上界Bmax、下界Bmin，用来调整不同程度的状态突变。Bmax，Bmin分别取置信概率为99%和90%时所对应的卡方临界值，查表可得Bmax=9.21，Bmin=4.61。定义如下三段模糊判别函数，用于实时更新Q(k)，从而达到自适应滤波的目的。
(20)
式中：Qmin为双惯导定位模型未出现状态突变时的Q(k)；Qmax可取Qmin的m倍。
4 方法验证
4.1 仿真分析
仿真总时长为2 000 s，周期为1s，在采煤机运动到第500，1 000，1 500 s时分别对俯仰角、航向角及横滚角进行改变，采煤机运行轨迹如图3所示。
图3 采煤机运行轨迹
Fig.3 Shearer running track
2套惯导系统之间的距离r和夹角α分别为1 m和45°。姿态角噪声均设置为由0.2°/h的常值误差、0.5°为基数的高斯白噪声组成的混合噪声，加速度噪声设置为由0.1g(g为重力加速度)的常值误差、0.05g为基数的高斯白噪声组成的混合噪声。
为验证本文方法的有效性，制造双惯导定位模型状态突变的外部环境，在第500，1 000，1 500 s时放大惯导1的过程噪声，增加状态估计不确定性。
双惯导定位模型下引入自适应卡尔曼滤波前后的卡方检验值如图4所示。在引入自适应卡尔曼滤波前，第500，1 000 s附近存在较大的卡方检验值；在引入自适应卡尔曼滤波后，第500，1 000 s的卡方检验值均小于引入自适应卡尔曼滤波前的卡方检验值，且低于检测阈值；在第1 500 s放大过程噪声时，引入自适应卡尔曼滤波前后的卡方检验值并无明显变化且低于检测阈值，表明自适应卡尔曼滤波有效减小了状态突变时的估计误差，双惯导定位模型的准确度得到了一定改善。
(a) 引入自适应卡尔曼滤波前
(b) 引入自适应卡尔曼滤波后
图4 引入自适应卡尔曼滤波前后卡方检验值
Fig.4 The chi-square test values before and after introduction of adaptive Kalman filter
对比单惯导定位、基于EKF的双惯导定位和基于自适应卡尔曼滤波的双惯导定位方法下采煤机在北、东、天方向上的定位误差，结果如图5所示，具体误差范围见表1。可看出2种双惯导定位方法的定位误差均小于单惯导定位方法；但基于EKF的双惯导定位方法在采煤机存在较大机动和状态突变时，抗干扰能力较弱，各方向上的定位误差会有明显增加；而基于自适应卡尔曼滤波的双惯导定位方法通过动态调整过程噪声的协方差矩阵，降低了过程噪声的协方差矩阵在滤波增益中的比重，可以很好地抑制状态突变带来的影响，使导航精度保持在良好的范围内。
(a) 惯导1位置误差
(b) 惯导2位置误差
图5 不同定位方法下位置误差
Fig.5 Position error of different positioning methods
表1 不同定位方法的误差范围
Table 1 Error range of different positioning methods m
方法惯导1惯导2北向东向天向北向东向天向单惯导定位-26.08^14.18-1.25^9.75-9.94^0.51-11.09^11.62-0.27^7.94-0.14^4.81基于EKF的双惯导定位-14.17^7.28-0.42^2.46-2.89^0.35-6.04^2.87-0.21^5.00-2.66^1.81基于自适应卡尔曼滤波的双惯导定位-7.54^3.22-0.40^1.96-2.62^0.14-3.59^1.30-0.20^2.14-1.88^1.37
4.2 实验分析
实验平台由WT931惯导模块、代表采煤机的小车及含有自适应卡尔曼滤波程序的计算终端组成。2套惯导模块之间的距离为0.1 m，夹角为45°。小车运行轨迹根据采煤机在工作面中的实际运行轨迹按比例缩小得到，模拟采煤机进刀的运动状态。小车运行轨迹总长25 m，在8 m处设有弯曲段，斜切进刀段长度为8 m，航向角由0°转为15°，惯导模块采集数据的频率为12.5 Hz，滤波周期为0.1 s。实验前，根据2套惯导系统的安装方式，可设初始状态量为X(0)=[0 0 0 0.1 0.1 0]T，初始观测量为
实验时，小车按照设定的运行路线(即理想轨迹)运行。通过固定在小车上的2套惯导模块实时同步采集小车的加速度和姿态角数据，并通过ESP8266 WiFi无线模块上传至计算终端。
单惯导定位、基于自适应卡尔曼滤波的双惯导定位方法下定位结果如图6、图7所示。可看出随着运行距离增加，与理想轨迹相比，单惯导的定位误差逐渐增大，而基于自适应卡尔曼滤波的双惯导定位方法的定位误差趋于收敛，无明显的误差累计现象，定位轨迹与理想轨迹重合度较好；基于自适应卡尔曼滤波的双惯导定位方法的定位误差比单惯导的定位误差在各方向上均有所减小。
(a) 水平面定位轨迹
(b) 竖直平面定位轨迹
(c) 定位误差
图6 惯导1定位结果
Fig.6 Positioning results of inertial navigation system 1
(a) 水平面定位轨迹
(b) 竖直平面定位轨迹
(c) 定位误差
图7 惯导2定位结果
Fig.7 Positioning results of inertial navigation system 2
5 结语
提出了一种基于自适应卡尔曼滤波的双惯导采煤机定位方法。在采煤机上固定安装2套惯导系统，以惯导系统的位置为状态量，惯导系统之间的距离和夹角为观测量，构建双惯导定位模型；利用自适应卡尔曼滤波算法对双惯导定位模型状态进行评估和调整，即通过卡方检验评估双惯导定位模型是否发生状态突变，并利用三段模糊判别函数动态调整过程噪声的协方差矩阵，从而降低双惯导定位模型状态突变对定位精度的影响。仿真和实验结果表明，自适应卡尔曼滤波相比EKF的抗干扰能力更强，能抑制状态突变带来的影响；基于自适应卡尔曼滤波的双惯导采煤机定位方法的定位误差比单惯导的定位误差在各方向上均有所减小。
[1] 许春雨,宋渊,宋建成,等.基于单片机的采煤机红外线位置检测装置开发[J].煤炭学报,2011,36(增刊1):167-171.
XU Chunyu,SONG Yuan,SONG Jiancheng,et al.Development of the device to detect the position of coal mining machine by infrared ray based on MCU[J].Journal of China Coal Society,2011,36(S1):167-171.
[2] 张连昆,谢耀社,周德华,等.基于超声波技术的采煤机位置监测系统[J].煤炭科学技术,2010,38(5):104-106.
ZHANG Liankun,XIE Yaoshe,ZHOU Dehua,et al.Position monitoring and measuring system of coal shearer based on ultrasonic wave technology[J].Coal Science and Technology,2010,38(5):104-106.
[3] 张博渊,王世博,葛世荣.惯性导航初始对准偏差与安装偏差校准方法对采煤机定位精度影响[J].煤炭学报,2017,42(3):789-795.
ZHANG Boyuan,WANG Shibo,GE Shirong.Effects of initial alignment error and installation noncoincidence on the shearer positioning accuracy and calibration method[J].Journal of China Coal Society,2017,42(3):789-795.
[4] 樊启高.综采工作面“三机”控制中设备定位及任务协调研究[D].徐州:中国矿业大学,2013.
FAN Qigao.Study on equipments positioning and task coordination for three machines controlling on the mechanized mining face[D].Xuzhou:China University of Mining and Technology,2013.
[5] 张丹,郝尚清,宋胜伟.运动状态检测的采煤机捷联惯导系统误差组合算法[J].黑龙江科技大学学报,2019,29(3):299-303.
ZHANG Dan,HAO Shangqing,SONG Shengwei.Error combinatorial algorithm for strap-down inertial navigation system of shearer based on motion state detection[J].Journal of Heilongjiang University of Science and Technology,2019,29(3):299-303.
[6] 樊启高,李威,王禹桥,等.一种采用捷联惯导的采煤机动态定位方法[J].煤炭学报,2011,36(10):1758-1761.
FAN Qigao,LI Wei,WANG Yuqiao,et al.A shearer dynamic positioning method using strap down inertial navigation[J].Journal of China Coal Society,2011,36(10):1758-1761.
[7] FAN Qigao,LI Wei,LUO Chengming.Error analysis and reduction for shearer positioning using the strapdown inertial navigation system[J].International Journal of Computer Science Issues,2012,9(5):49-54.
[8] 韩勇强,王新健,孙寿才,等.一种基于径向速度残差曲线拟合的车载SINS零速修正算法[J].中国惯性技术学报,2020,28(3):281-286.
HAN Yongqiang,WANG Xinjian,SUN Shoucai,et al.A zero velocity update algorithm for vehicle SINS based on radial velocity residual curve-fitting[J].Journal of Chinese Inertial Technology,2020,28(3):281-286.
[9] CHEN Yuming,LI Wei,YANG Hai,et al.Research on the compensation strategy of the initial alignment of the SINS based on the dynamic model of the shearer[J].IEEE Access,2019,7:36736-36747.
[10] WANG Shibo,ZHANG Boyuan,WANG Shijia,et al.Dynamic precise positioning method of shearer based on closing path optimal estimation model[J].IEEE Transactions on Automation Science and Engineering,2019,16(3):1468-1475.
[11] 应葆华,李威,罗成名,等.一种采煤机组合定位系统及实验研究[J].传感技术学报,2015,28(2):260-264.
YING Baohua,LI Wei,LUO Chengming,et al.Experimental study on combinative positioning system for shearer[J].Chinese Journal of Sensors and Actuators,2015,28(2):260-264.
[12] 周开平.薄煤层综采工作面采煤机组合定位方法研究[J].工矿自动化,2019,45(6):52-57.
ZHOU Kaiping.Research on combined positioning method of shearer on fully mechanized mining face of thin coal seam[J].Industry and Mine Automation,2019,45(6):52-57.
[13] 鲁程,王世博,葛世荣,等.多惯导冗余的采煤机定位原理及其合理性分析[J].煤炭学报,2019,44(增刊2):746-753.
LU Cheng,WANG Shibo,GE Shirong,et al.Redundant multi-INS positioning algorithm of shearer and analysis of its rationality[J].Journal of China Coal Society,2019,44(S2):746-753.
[14] 郭梁,宋建成,宁振兵,等.基于捷联惯性导航的矿用单轨吊机车定位算法[J].工矿自动化,2021,47(1):49-54.
GUO Liang,SONG Jiancheng,NING Zhenbing,et al.Positioning algorithm of mine-used monorail crane locomotive based on strapdown inertial navigation[J].Industry and Mine Automation,2021,47(1):49-54.
[15] 赵靖.基于卡尔曼滤波算法的采煤机惯导定位方法[J].工矿自动化,2014,40(10):29-32.
ZHAO Jing.Inertial navigation positioning method of shearer based on Kalman filtering algorithm[J].Industry and Mine Automation,2014,40(10):29-32.
[16] 何飞,双丰,许玉云,等.双渐消Kalman滤波补偿六维力传感器矩形板噪声模型误差[J].机械科学与技术,2019,38(4):594-600.
HE Fei,SHUANG Feng,XU Yuyun,et al.Model error compensation of double-fading Kalman filtering to rectangular board of six-axis force sensors[J].Mechanical Science and Technology for Aerospace Engineering,2019,38(4):594-600.
