0 引言

煤矿巷道尤其是主巷道对巷道成形精度有较高要求，如GB 50213—2010《煤矿井巷工程质量验收规范》规定“基础掘进断面规格的允许偏差为-50～+150 mm”。掘进机作为巷道成形的主要装备，其定位精度对巷道成形精度有直接影响[1]。考虑到掘进机控制精度不高、工况恶劣等因素，掘进机定位精度应达到厘米级才有可能实现GB 50213—2010要求的巷道成形精度[2]。

掘进机定位属于狭长封闭空间中的刚体定位，且掘进机作业环境一般具有粉尘浓度高、能见度低、通视性差、噪声大等特点[3-5]。传统的掘进机定位以光电导航为主[6-10]，利用激光、可见光、红外光等作为介质关联掘进机和巷道设计基准并进行掘进机位姿测量，具有技术成熟、精度高、成本低等优点，但在煤矿井下应用时，粉尘对可见光的衰减、水雾对红外光的吸收、空间狭窄引起的遮挡等都可能影响定位效果。惯性导航是利用陀螺仪和加速度计测量载体角速率和加速度，经过对时间的积分运算实现姿态和位移检测的方法[11-12]，具有很好的环境适应性和自主检测特性。然而惯性导航技术用于掘进机定位时，由于导航信息经过积分而产生，定位误差随时间增加而增大。

零速修正作为一种误差补偿技术，除了对加速度刻度因子误差、方位对准误差及方位陀螺常值漂移不可观外，对于噪声及时间相关的误差源都可充分修正[13]，可有效减小惯性导航系统长时间工作的累计误差，提高系统精度。因此，本文提出了一种基于零速修正的掘进机惯性导航定位方法，以掘进机停车时的速度误差作为观测量来对惯性导航系统的位置测量值进行修正，可提高掘进机定位精度。

1 掘进机运动特点

根据煤矿巷道掘进工艺，掘进机在不同工况下可分为连续行进、连续截割、短距频繁进退和长时间静置4种运动模式。在大范围调动掘进机时，掘进机以5～10 m/min的速度连续行进，行进过程中需要每隔数米停车整理电缆；掘进机连续行进至指定位置后进行连续截割作业，1个断面截割过程完成后，掘进机前进0.5～1 m，停车开始下一个断面截割，在2个断面截割过程之间，掘进机短距频繁进退以完成断面边界修整；1～2个截割作业循环后，掘进机后撤5～10 m(需30～60 s)并静置数小时，以提供巷道支护空间和时间；巷道支护完成后，掘进机再次前行5～10 m到达截割位置，停车开始断面截割，往复循环。

从掘进机运动特点可看出，掘进机以长时短距离非连续运动为主，为零速修正技术在掘进机惯性导航中的应用提供了必要条件。

2 基于零速修正的掘进机惯性导航定位

惯性导航系统的零速修正实现方法包括二次曲线拟合法、实时卡尔曼滤波法、平滑估计法等[13]，其中二次曲线拟合法相对简单可靠、计算量小。采用二次曲线拟合法进行零速修正的原理：在较短的零速修正时间间隔内，采用二次曲线拟合惯性导航系统的速度误差曲线，速度误差曲线经过积分得到位置误差曲线，然后与惯性导航系统的位置输出参数求和，即可得到修正的位置参数。

典型的二次曲线表达式为

(1)

式中：为速度误差观测值；a0，a1，a2为二次曲线系数；t为停车时间。

通过连续测量m(3≤m≤n，n为总停车次数)次停车时惯性导航系统输出的掘进机速度，可得

(2)

式中：为第i(i=1,2,…,n)次停车时惯性导航系统的速度误差；ti为第i次停车时对应的时间。

即

(3)

式中：

由最小二乘法可知：

(4)

对速度误差在相邻停车时刻区间求积分即可得到相应的位置误差，进而修正惯性导航系统输出的位置参数。

零速修正过程中需要确定掘进机停车时刻。通常利用惯性导航系统中陀螺仪和加速度计的输出参数并根据经验设定阈值以判定掘进机停车状态[14-15]，但可能受路况和掘进机质量及重心分布影响而出现漏判或误判。本文利用掘进机启停信号确定掘进机停车时刻，当掘进机左右任一履带有运动信号时，判断掘进机处于运动状态，否则处于停车状态。

3 试验验证

为验证零速修正对掘进机惯性导航系统定位性能的改善效果，将PHINS Surface惯性导航系统安装在试验小车上，以小车启停信号模拟掘进机启停信号，考虑到掘进机运动特点，选择零速修正时间间隔约为60 s，试验时间约为170 min，按照掘进机行进速度和运动模式进行基于零速修正的惯性导航系统定位试验。

利用陀螺寻北经纬仪角秒级测量精度和全站仪毫米级测量精度完成对基于零速修正的掘进机惯性导航系统的定位精度标定。全站仪测量方位由陀螺寻北仪确定，如图1所示。设陀螺寻北仪位于O1，O1N1为陀螺寻北仪确定的真北方向，则O1E1为陀螺寻北仪东向；全站仪位于o1，当陀螺寻北仪与全站仪互相照准时，O1与o1连线相对O1N1的夹角为α，此时设置全站仪水平角∠Y1o1M=β=α，则全站仪的参考方向及测量基准被设置为真北方向o1Y1，相应地，o1X1为全站仪东向。同步全站仪与基于零速修正的惯性导航系统的测量时间，从而可对比二者的测量数据。

图1 全站仪测量方位
Fig.1 Measured direction of total station

分别利用全站仪和基于零速修正的惯性导航系统测得的小车运动轨迹对比如图2所示。停车时全站仪测量的小车位置作为真实参考值，共采集24个测量点数据进行对比，可看出基于零速修正的惯性导航系统测得的小车运动轨迹基本接近小车真实运动轨迹。

图2 全站仪和惯性导航系统所测运动轨迹对比
Fig.2 Comparison of trajectories measured by total station and inertial navigation system

单次零速修正后，惯性导航系统东向和北向位移偏差如图3所示。

图3 基于零速修正的惯性导航系统东向和北向位移偏差
Fig.3 Displacement deviation of inertial navigation system based on zero-velocity update in east and north direction

通过计算可知,东向和北向位移偏差绝对值的均值分别为0.038，0.049 m，表明基于零速修正的惯性导航系统定位精度可达厘米级；东向和北向位移偏差绝对值的标准差分别为0.026，0.042 m，表明定位误差离散度较小，即定位误差波动范围较小。

基于零速修正的惯性导航系统东向和北向位移累计偏差如图4所示。可看出东向和北向位移累计偏差没有出现明显增大，表明惯性导航系统定位误差随时间累计的趋势得到了有效抑制。

图4 基于零速修正的惯性导航系统东向和 北向位移累计偏差
Fig.4 Accumulated displacement deviation of inertial navigation system based on zero-velocity update in east and north direction

4 结语

针对掘进机长时短距离非连续运动特点，提出利用零速修正对掘进机惯性导航系统位置测量值进行修正，即以多次掘进机停车时惯性导航系统的速度输出参数拟合速度误差的二次曲线，进而得到位置误差曲线来修正惯性导航系统的位置输出参数。试验结果表明，基于零速修正的掘进机惯性导航定位精度可达厘米级，定位误差波动范围小，定位累计误差随时间增加没有明显增大趋势，满足掘进机定位要求。

参考文献 ：

[1] 王焱金,张建广,马昭.综掘装备技术研究现状及发展趋势[J].煤炭科学技术,2015,43(11):87-90.

WANG Yanjin,ZHANG Jianguang,MA Zhao.Research status and development tendency of mine fully-mechanized heading equipment technology[J].Coal Science and Technology,2015,43(11):87-90.

[2] 田原.悬臂式掘进机导航技术现状及其发展方向[J].工矿自动化,2017,43(8):37-43.

TIAN Yuan.Present situation and development direction of navigation technology of boom-type roadheader[J].Industry and Mine Automation,2017,43(8):37-43.

[3] 朱信平,李睿,高娟,等.基于全站仪的掘进机机身位姿参数测量方法[J].煤炭工程,2011,43(6):113-115.

ZHU Xinping,LI Rui,GAO Juan,et al.Position parameter measuring method of mine roadheader body base on total station[J].Coal Engineering,2011,43(6):113-115.

[4] 李军利,邢建华,廉自生,等.悬臂掘进机头位姿的测量系统及其方法:CN101266134[P].2008-09-17.

[5] 吴淼,刘建功,魏景生,等.掘进机机身位姿参数测量系统及其方法:CN101629807[P].2010-01-20.

[6] 田原.基于机器视觉的掘进机空间位姿检测技术研究[J].矿山机械,2013,41(2):27-30.

TIAN Yuan.Research on automatic inspection of spatial attitude and position of roadheader based on machine vision technology[J].Mining & Processing Equipment,2013,41(2):27-30.

[7] 杜雨馨,刘停,童敏明,等.基于机器视觉的悬臂式掘进机机身位姿检测系统[J].煤炭学报,2016,41(11):2897-2906.

DU Yuxin,LIU Ting,TONG Minming,et al.Pose measurement system of boom-type roadheader based on machine vision[J].Journal of China Coal Society,2016,41(11):2897-2906.

[8] 陶云飞,宗凯,张敏骏,等.基于iGPS的掘进机单站多点分时机身位姿测量方法[J].煤炭学报,2015,40(11):2611-2616.

TAO Yunfei,ZONG Kai,ZHANG Minjun,et al.A position and orientation measurement method of single-station, multipoint and time-sharing for roadheader body based on iGPS[J].Journal of China Coal Society,2015,40(11):2611-2616.

[9] 吴淼,贾文浩,华伟,等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[J].煤炭学报,2015,40(11):2596-2602.

WU Miao,JIA Wenhao,HUA Wei,et al.Autonomous measurement of position and attitude of boom-type roadheader based on space intersection measurement[J].Journal of China Coal Society,2015,40(11):2596-2602.

[10] 齐宏亮.掘进机器人位姿测控系统研究[D].阜新:辽宁工程技术大学,2012.

[11] 冯大龙.捷联式惯导系统在无人掘进机中的应用[D].重庆:重庆大学,2007.

[12] 黄东,杨凌辉,罗文,等.基于视觉/惯导的掘进机实时位姿测量方法研究[J].激光技术,2017,41(1):19-23.

HUANG Dong,YANG Linghui,LUO Wen,et al.Study on measurement method of realtime position and attitude of roadheader based on vision/inertial navigation system[J].Laser Technology,2017,41(1):19-23.

[13] 卢志忠,杨先飞.零速校正在惯性导航系统中的仿真研究[J].应用科技,2009,36(6):45-48.

LU Zhizhong,YANG Xianfei.The simulation research on zero-velocity update in inertial navigation system[J].Applied Science and Technology,2009,36(6):45-48.

[14] 冯庆奇.激光陀螺捷联惯性导航系统组合导航及零速修正技术研究[D].长沙:国防科学技术大学,2009.

[15] 李小芳.激光陀螺捷联惯性导航系统零速修正技术研究[D].北京:北京理工大学,2015.