采煤机自主导航定位是实现长壁综采工作面自动化的关键技术之一[1-4]，惯性导航以导航信息的全面性与自主性使其成为一种可行的采煤机定位技术，但由于采煤机的振动等因素的影响，纯惯性导航的位置漂移较大[5]。基于捷联惯性导航与轴编码器组合的采煤机惯性导航定位是将惯性导航元件安装在采煤机机体上测量采煤机的姿态角，轴编码器安装在采煤机牵引部测量采煤机运行速度，通过航位推算算法，根据采煤机姿态和速度进行位置解算从而确定采煤机的三维位置与姿态，此方法能有效抑制惯性导航的位置误差累积[6-8]。郝尚清等[9]建立了采煤机惯性导航安装偏差引起的采煤机定位误差模型，获得了安装偏差与采煤机定位误差之间的关系。张博渊等[10]建立了航位推算误差模型，提出了基于两点法的惯性导航元件安装偏差与初始对准偏差校准算法，并通过试验进行了验证，这些研究获得了惯性导航系统确定性偏差对定位精度的影响，并提出了补偿方法。但惯性导航系统在长时定位的姿态漂移也是影响定位精度的一个重要因素。
零速修正技术是解决惯性导航长时间定位精度差的一种廉价而有效的方法，它利用载体停车时惯性导航的速度输出作为误差的观测量，进而对其他各项误差进行修正。高钟毓等[11]分析比较了曲线拟合和卡尔曼滤波这两种零速修正估计方法，在此基础上提出了平滑估计法并进行了跑车实验，表明平滑估计法对误差估计效果最好。LI X等[12]提出了支持向量机模型回归曲线拟合方法，通过仿真表明比传统曲线拟合方法有更强的抗干扰能力。GREJNER-BRZEZINSKA等[13]将模糊算法与零速修正结合，提高了定位精度。常乐等[14]采用卡尔曼滤波方法，研究了零速修正在GNSS/INS紧组合导航中的应用。一些研究者把载体运动学模型与零速修正技术结合，取得了不错的效果。孙伟等[15]基于端帮采煤机特有运动工作模式，运用零速修正的闭环卡尔曼滤波完成惯导解算姿态及惯性器件偏差信息的修正。LI Y等[16]开发了行人航位推算系统，结合人行走时脚部的运动特性设计零速校正算法，并通过试验进行了验证。零速修正技术要求载体每隔一段时间停车进行各项误差的修正，这大大限制了零速修正技术的应用。据于此，方靖等[17]提出了动态零速修正技术，利用车体坐标系横向和垂直方向上速度为零作为约束条件，构造了虚拟噪声观测量，设计了U-D协方差分解与序贯处理的Kalman滤波方法，进行实时轨迹修正。赵玉等[18]基于车载惯性导航系统，建立了动态零速修正误差模型，实验结果表明此方法能有效抑制位置、速度和姿态误差增长。DISSANAYAKE等[19]把动态零速修正技术与信息滤波算法结合，提高了导航精度。根据综采工作面采煤工艺，采煤机以刮板输送机为轨道进行往复运动截割煤壁，这决定了采煤机只在轨道方向上产生速度，而在其他两个方向的速度为0，这与动态零速修正技术的非完整约束条件是相同的。此外，对于车载惯性导航系统，其只是在高精度定位信号消失时起短时辅助定位，而在环境恶劣的矿井中，惯性导航是采煤机自主导航的核心设备，航位推算算法是主要解算算法，长时定位的姿态漂移对于定位精度的影响就需要最大程度予以消除。因此，本文根据惯性导航姿态误差方程和动态零速修正技术的非完整约束条件，建立卡尔曼滤波模型，并利用移动平台模拟采煤机截割工艺轨迹运行，进行试验验证。
1 基于动态零速修正技术的卡尔曼滤波模型
1.1 动态零速修正技术的非完整约束条件
如图1所示，以采煤机质心Ob为原点，行进方向为Yb轴，横轴向右为Xb轴，垂直于采煤机机身向上为Zb轴建立采煤机坐标系(b系)。理想情况下，采煤机在刮板输送机轨道上运行，可以近似认为不发生侧滑和上下跳跃或发生的程度较小。即理想情况下，采煤机坐标系中，Xb轴和Zb轴速度为0，即
(1)
选择东北天坐标系为导航坐标系(n系)，在导航坐标系中，采煤机姿态用航向角(ψ)、俯仰角(θ)和横滚角(γ)来描述。航向角定义为Yb轴与北方向的夹角，定义域为(0°,360°)。在行进方向上，俯仰角定义为采煤机机身与水平面的夹角，定义域为(-90°,90°)。在采煤机横轴方向上，机身与水平面的夹角定义为横滚角，定义域为(-180°,180°)。
1.2 状态方程的建立
惯性导航姿态误差方程[20]为
(2)
其中，φ=[φE φN φU]T，φE，φN，φU分别为东、北和天向的平台失准角
ωiecos L ωiesin L]T;ωie为地球自转角速率;L为采煤机位置处维度
分别为采煤机东、北向速度，Rm为采煤机位置处地球子午圈曲率半径，Rn为采煤机位置处地球卯酉圈曲率半径，h为采煤机位置距离海平面的高度
分别为
的误差。因
值较小，误差方程可简化为
图1 采煤机坐标系、航向角、俯仰角、横滚角示意
Fig.1 Schematic of the shearer coordinate,heading,pitch,roll
(3)
记
(4)
对式(3)离散化得平台失准角的递推方程为
φ(t)=eA(t-t0)φ(t0)
(5)
取状态量Xk=[φE φN φU]T，设采样周期为Ts，可得卡尔曼滤波状态方程为
Xk/k-1=Fk/k-1Xk-1+Wk-1
(6)
式中，Fk/k-1=eATs;Wk-1为均值，为0，方差为Q的白噪声。
1.3 量测方程的建立
根据动态零速修正非完整约束条件可知，理想情况下，采煤机坐标系Xb轴和Zb轴速度为0。但实际上由于惯性导航存在姿态漂移，其输出采煤机坐标系下Xb轴速度
和Zb轴速度
不为0，输出的速度值即为误差值(δVb)。即
(7)
现以
为量测量，惯性导航输出导航坐标系下的速度Vn转化到采煤机坐标系下速度为
(8)
其中,
为准确的姿态矩阵。同样的，可得到如下
(9)
其中,
为惯性导航输出姿态矩阵。当3个平台失准角较小时，与有如下转换关系[18]
(10)
其中，E为三阶单位矩阵，(φ)为φE，φN，φU组成的反对称矩阵
(11)
定义
(12)
联立方程(8)～(10),(12)，可得状态量和量测量之间的转移矩阵H
(13)
其中，VE,VN和VU分别为采煤机东、北和天向速度，
为惯性导航输出姿态矩阵
(14)
故卡尔曼滤波量测方程为
Zk=HkXk+Vk
(15)
其中，Vk为均值为0，方差为R的白噪声。因此，基于动态零速修正技术的采煤机惯性导航定位流程如图2所示。
图2 基于动态零速修正技术的采煤机惯性导航定位流程
Fig.2 Flow chart of shearer inertial navigation positioning based on DZUPT
2 试验测试
为了验证基于动态零速修正技术的卡尔曼滤波模型的准确性，将采煤机定位装置安装至模拟采煤机的移动平台上进行室外试验。试验装置如图3所示，惯性导航元件安装在移动平台的中心位置，轴编码器安装在移动平台的行走轮，同时在惯性导航元件的中心上方安装GPS-RTK移动站，GPS-RTK基准站固定在试验区域的某一点。试验区域长54 m，宽10 m。试验以GPS-RTK移动站测量的轨迹为载体真实轨迹，GPS-RTK高度方向测量精度小于15 mm，平面测量精度小于8 mm。将GPS-RTK的测量时间和采煤机定位装置的测量时间统一至相同的时间系统下，以便进行轨迹对比。在采煤机运行模拟试验之前，需要校准试验计算确定性偏差角。校准试验是根据文献[10]提出的两点法理论，使移动平台以0.4～0.5 m/s的速度运行，由试验区域起点运行至终点后，调取起点与终点坐标计算安装偏差和初始对准偏差(确定性偏差)。根据安装偏差和初始对准偏差校准算法解算出航向角确定性偏差为2.074 2°，俯仰角确定性偏差为0.326 0°。将计算得到的角度补偿到航位推算算法中，从而消除确定性偏差对于航位推算位置解算的影响。采煤机运行模拟试验是以移动平台模拟采煤机运行，以0.1～0.2 m/s的速度运行4刀，每刀54 m，其中斜切进刀量为1.2 m，验证基于动态零速修正技术的卡尔曼滤波模型的准确性。
图3 采煤机运行模拟试验装置示意
Fig.3 Schematic of shearer operation simulation test device
图4 动态零速修正使用前后姿态角对比
Fig.4 Compare diagram of attitude angle
图5 RTK轨迹与动态零速修正使用前后航位推算轨迹对比
Fig.5 Compare diagram of RTK positioning and dead reckoning positioning before and after DZUPT
图4～6中未使用动态零速修正结果为利用惯性导航原始输出所得的结果，在此基础上，经过第1节所提模型的处理，得到使用动态零速修正后的结果。图4为动态零速修正使用前后姿态角对比。图5为RTK轨迹与动态零速修正使用前后航位推算轨迹对比。图6为动态零速修正使用前后航位推算轨迹在东、北、天3个方向的位置误差对比。通过表1可知，使用动态零速修正对第3刀和第4刀导航轨迹的定位精度提升较为明显。第3刀导航东、北方向最大误差分别由0.639 7，0.856 7 m减小为0.456 4，0.594 2 m。第4刀导航东、北方向最大误差分别由0.644 4，0.910 6 m减小为0.466 5，0.603 0 m。第3刀和第4刀导航东、北方向定位精度提升了30%。采用导航定位中的球概率误差(SEP)[21]作为采煤机定位精度评判标准。此方法是在以真实位置为球心的球内，偏离球心概率为50%的三维点位精度分布度量的，球概率误差表示如下
图6 动态零速修正使用前后航位推算轨迹在东、北、天3个方向的位置误差
Fig.6 Error diagram of dead reckoning positioning before and after DZUPT in east,north and down
表1 动态零速修正使用前后误差
Table 1 Error table before and after DZUPT
SEP=0.51(σE+σN+σU)
(15)
其中，σE，σN，σU分别为东、北、天3个方向的均方根误差。第3刀和第4刀导航球概率误差也分别由0.425 3，0.506 9 m减小为0.331 9，0.460 2 m。由图5(b)的RTK轨迹可知，在东方向-42,-27,-19,-15 m处天方向位置坐标值发生较大波动，这是由于移动平台行走过程中在这些位置产生较大振动，这也导致了第3刀和第4刀导航天方向最大误差有所增加，分别由0.214 2，0.337 9 m变为0.224 3，0.388 1 m。据此，可以发现振动对采煤机惯性导航定位系统的影响是比较大的，可以从此方面入手研究，进一步提高定位精度。
3 结 论
根据惯性导航姿态误差方程，以东、北、天3个方向的平台失准角为状态量构建状态方程，根据动态零速修正技术非完整约束条件，以采煤机坐标系横向和垂直方向速度值为观测量构建量测方程，建立了卡尔曼滤波模型，开展了移动平台模拟采煤机截割工艺运行4刀试验。在校准确定性偏差的基础上，第3刀导航东、北方向最大误差分别由0.639 7，0.856 7 m减小为0.456 4，0.594 2 m。第4刀导航东、北方向最大误差分别由0.644 4，0.910 6 m减小为0.466 5，0.603 0 m。东、北方向定位精度提升了30%。
参考文献(References):
[1] 郝尚清,王世博,谢贵君,等.长壁综采工作面采煤机定位定姿技术研究[J].工矿自动化,2014,40(6):21-25.
HAO Shangqing,WANG Shibo,XIE Guijun,et al.Research of determination technologies of position and attitude of shearer on long-wall fully mechanized coal mining face[J].Industry and Mine Automation,2014,40(6):21-25.
[2] 葛世荣,苏忠水,李昂,等.基于地理信息系统(GIS)的采煤机定位定姿技术研究[J].煤炭学报,2015,40(11):2503-2508.
GE Shirong,SU Zhongshui,LI Ang,et al.Study on the positioning and orientation of a shearer based on geographic information system[J].Journal of China Coal Society,2015,40(11):2503-2508.
[3] KELLY M,HAINSWORTH D,LEVER P,et al.Longwall automation:An ACARP landmark project[A].CMMI Congress[C].AusIMM,2002:159-165.
[4] KELLY M,HAINSWORTH D,REID D,et al.Longwall automation-a new approach[A].3rd International Symposium[C].Aachen,2003.
[5] YANG H,LI W,LUO C,et al.Research on error compensation property of strapdown inertial navigation system using dynamic model of shearer[J].IEEE Access,2016,4:2045-2055.
[6] 李昂.采煤机采区绝对定位定姿技术研究[D].徐州:中国矿业大学,2015.
LI Ang.Research on shearer absolute positioning and attitude technology in mining area[D].Xuzhou:China University of Mining and Technology,2015.
[7] REID D C,HAINSWORTH D W,RALSTON J C,et al.Shearer guidance:A major advance in longwall mining[A].Field and Service Robotics[C].Springer Berlin Heidelberg,2003:469-476.
[8] RALSTON J,REID D,HARGRAVE C,et al.Sensing for advancing mining automation capability:A review of underground automation technology development[J].International Journal of Mining Science and Technology,2014,24(3):305-310.
[9] 郝尚清,李昂,王世博,等.采煤机惯性导航安装偏差对定位误差的影响[J].煤炭学报,2015,40(8):1963-1968.
HAO Shangqing,LI Ang,WANG Shibo,et al.Effects of shearer inertial navigation installation noncoincidence on shearer positioning error[J].Journal of China Coal Society,2015,40(8):1963-1968
[10] 张博渊,王世博,葛世荣.惯性导航初始对准偏差与安装偏差校准方法对采煤机定位精度影响[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.
[11] 高钟毓,王进,董景新,等.惯性测量系统零速修正的几种估计方法[J].中国惯性技术学报,1995(2):24-29.
GAO Zhongyu,WANG Jin,DONG Jingxin,et al.A comparison of ZUPT estimation method for inertial survey systems[J].Journal of Chinese Inertial Technology,1995(2):24-29.
[12] LI X,XIE L,CHEN J,et al.A ZUPT method based on SVM regression curve fitting for SINS[A].Control Conference(CCC),2014,33rd Chinese[C].IEEE,2014:754-757.
[13] GREJNER-BRZEZINSKA D A,YI Y,TOTH C K.Bridging GPS gaps in urban canyons:The benefits of ZUPTs[J].Navigation,2001,48(4):216-226.
[14] 常乐,章红平,高周正,等.零速修正在 GNSS/INS 紧组合导航中的应用研究[J].大地测量与地球动力学,2016,36(10):849-884.
CHANG Le,ZHANG Hongping,GAO Zhouzheng,et al.Research of ZUPT in GNSS/INS tightly coupled navigation[J].Journal of Geodesy and Geodynamics,2016,36(10):849-884.
[15] 孙伟,李松,李瑞豹,等.基于零速修正的采煤机测姿技术研究[J].电子测量与仪器学报,2015,29(12):1829-1835.
SUN Wei,LI Song,LI Ruibao,et al.Research on attitude determination technology of mining machine based on ZUPT[J].Journal of Electronic Measurement and Instrumentation,2015,29(12):1829-1835.
[16] LI Y,LUO X,REN X T,et al.A robust humanoid robot navigation algorithm with ZUPT[A].Mechatronics and Automation(ICMA),2012 International Conference on[C].IEEE,2012:505-510.
[17] 方靖,顾启泰,丁天怀.车载惯性导航的动态零速修正技术[J].中国惯性技术学报,2008,16(3):265-268.
FANG Jing,GU Qitai,DING Tianhuai.Dynamic zero velocity update for vehicle inertial navigation system[J].Journal of Chinese Inertial Technology,2008,16(3):265-268.
[18] 赵玉,赵忠,范毅.零速修正技术在车载惯性导航中的应用研究[J].压电与声光,2012(6):843-847,852.
ZHAO Yu,ZHAO Zhong,FAN Yi.Study on application of zero-velocity update technology to inertial navigation system[J].Piezoelectrics & Acoustooptics,2012(6):843-847,852.
[19] DISSANAYAKE G,SUKKARIEH S,NEBOT E,et al.The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J].IEEE Transactions on Robotics and Automation,2001,17(5):731-747.
[20] HUANG H,CHEN X,ZHANG B,et al.High accuracy navigation information estimation for inertial system using the multi-model EKF fusing adams explicit formula applied to underwater gliders[J].ISA Transactions,2017,66:414-424.
[21] SINGH N.Spherical probable error(SPE) and its estimation[J].Metrika,1970,15(1):149-163.
06