0 引言
连续采煤机适用于“三下”压煤、不规则块段及残采区的采煤作业，也可用于巷道掘进。连续采煤机在掘进或采煤过程中大多使用传统的点激光光源对其方向进行指示，采掘司机通过目视断面上的激光光斑控制截割[1]。该方法由于掘进工作面粉尘较大、作业现场光线差、操作人员视线受装备机身遮挡等，难以准确判断截割部位置。同时随着掘进装备的掘进深入，点激光光源需要多次移站跟进，指示精度降低，且测量工作量大，无法满足工作面无人化远程操控的要求，因此，如何实现连续采煤机井下精准定位定向成为目前亟需解决的关键问题。
煤矿井下由于环境特殊且条件恶劣，GPS等地面应用较多的导航定位系统无法在井下完全发挥作用[2]。惯性导航系统作为一种自主式定位定向系统，适用范围广，不依赖外界信息便可获得载体的姿态、速度及位置信息，对煤矿井下采掘装备进行定位定向指示具有较好的效果。
当前较为主流的井下采掘装备定位方法大多依附于惯导系统进行[2]，但惯导系统需通过对测量得到的加速度信息进行二次积分才可获得载体的位置信息，存在随时间累积会产生较大位置漂移误差的缺陷[3]。目前多使用基于惯导的多传感器组合定位方法解决单独使用惯导系统进行定位存在的位置漂移误差问题。文献[4]将惯导和里程计相结合进行井下采掘装备的定位研究，利用卡尔曼滤波结合里程计测得的里程信息对惯导相应测量数据进行校正，但采掘设备在井下易受浮煤影响造成履带打滑及滑移现象，此时里程计的测量误差会远大于惯导自身的漂移误差，仅依靠里程计与惯导进行组合会降低整个定位方法的测量精度。文献[5]采用联邦滤波将惯性测量单元(IMU)分别与立体相机、里程计构成子滤波器，避免了因里程计打滑或滑移失效产生的较大测量误差污染整个定位系统[6]，但井下视觉定位方法目前还处于理论研究阶段，未出现成功应用的工业性案例，故其可实施性与可靠性均属未知。文献[7]采用集中式滤波处理惯性导航系统、里程计及测距雷达所获取的各类传感器的测量信息，在进行卡尔曼滤波状态方程建模时维数较高，整个系统计算量较大且容错性差。
综上可知，目前多采用里程计、视觉测量、测距雷达等作为辅助测量方式，与惯导组合测量降低惯导随时间累积所产生的位置漂移误差，其中辅助测量方式要求可靠性与测量精度均较高。对不同测量方式得到的数据目前多使用集中式卡尔曼滤波进行滤波解算，但集中式卡尔曼滤波的计算量较大且容错性较差。针对上述问题，本文提出了一种由捷联式惯导、测距雷达及里程计组成的连续采煤机定位方法，使用测距雷达与里程计测量连续采煤机的位移量，通过航位推算法推算出机身的二维位置信息，利用二维位置信息对惯导位置解算信息进行反馈修正，减小惯导随时间累积所产生的位置漂移误差[8]。采用联邦式卡尔曼滤波对各传感器采集的位姿信息进行滤波与融合[9-11]，通过降低方程维数减少计算量,在保证精度的同时还具备一定的容错性。该定位方法可为连续采煤机在巷道约束环境下的位姿精确测量、巷道自动成形控制和远程控制奠定基础。
1 连续采煤机定位方法
连续采煤机定位方法结构组成如图1所示。使用测距雷达与里程计同时辅助修正惯导系统，采用联邦式卡尔曼滤波将惯导分别与测距雷达、里程计组成子滤波器。在里程计因履带打滑或滑移产生较大的测量误差时，提高测距雷达与惯导所组成的子滤波器1的置信度；在测距雷达因人员移动或其他障碍物遮挡而导致测量误差较大时，里程计及惯导所组成的子滤波器2采取较高的置信度，防止因个别辅助传感器测量失效产生较大误差而降低定位方法的精度。
1-连续采煤机本体； 2-惯导组件； 3-电控箱； 4-里程计；
5-测距雷达； 6-测距雷达靶标。
图1 连续采煤机定位方法结构组成
Fig.1 Positioning method structure composition of
continuous shearer
1.1 惯导系统
惯导系统根据安装方式可分为平台式与捷联式，根据所采用的陀螺仪种类不同又可分为光纤级、激光级等[12]。本文采用某型号光纤级捷联式惯导，惯导本体及交换机直接固连于连续采煤机机身中部右侧位置，紧邻电控箱，可实时测量机身的姿态、速度及位置信息。该定位方法取测距雷达、里程计与惯导系统所推算得到的机身位置信息差值作为滤波输入来进行滤波处理[13]，各传感器所采集到的位姿信息可通过交换机传送至巷道后方安全位置或地面控制室内的上位机进行显示与控制。
1.2 测距雷达
测距雷达安装于连续采煤机机身尾部右后下方，将规格为1 m×1 m的标靶放置于与测距雷达处于同一水平线的正后方。连续采煤机行进过程中通过测距雷达不断测量与后方标靶的距离，所得的当前测量值与前一次采样频率下的测量值作差便可得到连续采煤机的相对位置增量。利用航位推算法对相对位置增量进行解算便可得到机体坐标系下的位置信息，借助惯导解算中的姿态矩阵实时地将机体坐标系下的位置信息转换为导航坐标系下的信息后再进行滤波解算[14]。
1.3 里程计
里程计安装于履带驱动齿轮处，输出量为连续采煤机短时间内路程增量，通过航位推算法得出所需要的位置信息[15]。里程计由于固连于车体履带转轴处，所测得的信息量依旧是在机体坐标系下的，同样需借助惯导解算中的姿态矩阵实时地将其转换为导航坐标系下的信息量后再进行滤波解算。
2 航位推算法
航位推算法是一种使用航向和里程信息来计算运载体相对于起点的相对位置的定位方法，原理如图2所示。已知起始位置坐标(x0，y0)及起始方位角θ0，通过测量行驶距离Si及方位角变化值Δθi，结合二者值便可推算出下一时刻位置坐标(xk，yk)和方位角θk。
(1)
(2)
θk=θk-1+Δθk-1 k=1,2,…
(3)
式中：θk-1为上一时刻方位角，k为不同时刻；Δθk-1为上一时刻至当前时刻方位角变化。
图2 航位推算法原理
Fig.2 Principle of dead reckoning algorithm
3 滤波器模型构建
3.1 联邦式卡尔曼滤波原理
联邦式卡尔曼滤波器原理如图3所示，通过惯导系统分别与测距雷达和里程计组成子滤波器，各子滤波器独立进行滤波，互不干扰，得出子滤波器局部估计
与主滤波器估计
经主滤波器信息融合便可得到全局最优估计
本文选取的联邦式卡尔曼滤波器结构兼具反馈与重置功能，设βj为子滤波器信息分配系数，βm为主滤波器信息分配系数，N为子滤波器总数，则其关系满足：
(4)
图3 联邦式卡尔曼滤波器原理
Fig.3 Federated Kalman filter principle
主滤波器进行全局估计后将全局最优估计值反馈给各子滤波器作为子滤波器的下一步局部状态估计值，同时将主滤波器的全局滤波方差矩阵Pg进行放大为
后对各子滤波器方差矩阵Pl(l=1,2)进行重置[17]。
为了简化计算，在信息分配上，使用平均分配方式分配子滤波器与主滤波器间的方差矩阵值，即
βj=βm=1/3
(5)
3.2 误差分析
定位方法中的误差主要来源于捷联惯导自身器件误差及算法更新误差、里程计及测距雷达测量误差，通过误差分析可为滤波器建模奠定基础。
3.2.1 惯性导航误差
惯导系统的误差主要考虑随机漂移和噪声，包括陀螺仪的常值漂移εb和高斯白噪声ωb、加速度计零偏
和高斯白噪声ωa。
其中关于捷联式惯导更新算法中的姿态、速度及位置误差推导见文献[16]。
3.2.2 测距雷达测量误差
测距雷达测量误差主要为自身固有误差δρ及初始输入误差δη。经分析这2类误差特性，同时为简化模型及降低计算量，将其设置为固定常值对误差进行补偿，以消除其对实验结果的影响。故在后续建模过程中，将测距雷达这2类误差均设为固定常值。
3.2.3 里程计测量误差
里程计的测量误差主要包括标度因数误差δKD和安装误差角δ α，设δ α=[δαθ δαγ δαψ]，δαθ,δαγ和δαψ分别为安装角误差中的俯仰角、横滚角和航向角误差。其中里程计的横滚角安装误差一般不会对测距雷达的位置输出量造成干扰，故这里不考虑横滚角安装误差。俯仰角安装误差及航向角安装误差对里程计测量误差有影响，将δKD、δαθ、δαψ三者均设为随机常值。
3.3 滤波器模型构建
联邦式卡尔曼滤波作为卡尔曼滤波的变式，其与集中式卡尔曼滤波的区别在于联邦式卡尔曼滤波子滤波器先进行一级滤波，而后进入联邦式卡尔曼滤波主滤波器进行二级滤波，同时将全局滤波最优估计及误差方差矩阵放大后反馈给子滤波器进行下一次滤波[18-19]。
2个子滤波器均采用间接法进行卡尔曼滤波器数学建模，即状态变量由导航参数的误差构成，同时对该连续变量信息进行离散化，以便于卡尔曼滤波进行分步迭代更新[20]，本文为描述简便直接给出离散化后的数学模型。
3.3.1 子滤波器1数学模型
状态方程为
Xk1=Φk1/k1-1Xk1-1+Γk1-1Wk1-1
(6)
式中：Φk1/k1-1为子滤波器1的十五维状态转移矩阵；Γk1-1为子滤波器1的噪声驱动矩阵；Wk1-1为子滤波器1的陀螺与加速度计的白噪声矩阵。
状态向量为
Xk1=
(7)
式中：φE、φN、φU分别为惯导的3个角度误差；VE、VN、VU分别为惯导的3个速度误差；ΔL、Δλ、Δh分别为惯导的3个方位误差；
分别为陀螺仪的3个方向的常值漂移；
分别为加速度计的3个方向的零偏。
量测方程为
Zk1=Hk1Xk1+Vk1
(8)
式中：Zk1为子滤波器1以惯导与测距雷达所输出的位置量作差得到的观测量；Hk1为子滤波器1的量测矩阵；Vk1为子滤波器1的测距雷达的位置量测白噪声矩阵。
3.3.2 子滤波器2数学模型
状态方程为
Xk2=Φk2/k2-1Xk2-1+Γk2-1Wk2-1
(9)
式中：Φk2/k2-1为子滤波器2的二十一维状态转移矩阵；Γk2-1为子滤波器2的噪声驱动矩阵；Wk2-1为子滤波器2的陀螺与加速度计的白噪声矩阵[20]。
状态向量为
Xk2=
(10)
相比于子滤波器1中的状态量，子滤波器2增加了3个初始位置误差
里程计标度因数误差δKD，里程计安装误差角δαθ、δαψ。将里程计标度因数误差、俯仰角及航向角安装误差均取为随机常数。
量测方程为
Zk2=Hk2Xk+Vk2
(11)
式中：Zk2为观测量，子滤波器2以惯导与里程计根据航位推算法推算出的位置量作差作为观测量；Hk2为子滤波器2的量测矩阵；Vk2为子滤波器2的里程计的位置量测白噪声矩阵。
4 仿真
为检验连续采煤机定位方法的定位精度及2类卡尔曼滤波的容错性，本文进行了Matlab 仿真实验。仿真时长取20 min即1 200 s，模拟连续采煤机实际掘进过程中不停机生产时长。在仿真时间为5 min即300 s时加入里程计测量误差扰动，持续时间为30 s，模拟连续采煤机打滑情况。惯导关键精度参数设置：陀螺仪的常值漂移εb设置为0.001 °/h, 加速度计零偏设置为1×10-4g(g为重力加速度)。
4.1 姿态误差
集中式与联邦式卡尔曼滤波航向角误差对比如图4所示。从图4可看出，在前200 s 时，2类滤波波动均较大，其中集中式卡尔曼滤波最大波动可达10′。在200 s后，2类滤波的滤波值逐渐趋于平稳，在0～-2.5′内小幅波动。在300 s时加入里程计测量误差扰动后，与联邦式卡尔曼滤波相比，集中式卡尔曼滤波出现的突变值较大，最大可达-8′，且在30 s内加入误差的过程中持续出现突变值，而联邦式卡尔曼滤波出现突变后，短时间内便恢复至正常滤波值范围。
图4 航向角误差对比
Fig.4 Comparison of heading angle error
集中式与联邦式卡尔曼滤波横滚角误差对比如图5所示。从图5可看出，在前80 s时，2类滤波的滤波值波动均较大，集中式卡尔曼滤波波动范围更大，最高可达14′；在80 s后，2类滤波的滤波值恢复至-1～1′内波动。在300 s时加入里程计测量误差扰动后，2类滤波均出现了瞬时突变，但联邦式卡尔曼滤波突变值较小且持续时间较短，且在较短时间内恢复到了正常值。
图5 横滚角误差对比
Fig.5 Comparison of roll angle error
集中式与联邦式卡尔曼滤波俯仰角误差对比如图6所示。从图6可看出，在滤波初期240 s内，2类滤波的滤波值波动均较大，且与航向角、横滚角相比，波动峰值更大，最大处可达270′。在240 s后，滤波波动值稳定于15′附近。在300 s时加入里程计测量误差扰动后，与前2个姿态角波动相类似，集中式卡尔曼滤波产生的瞬时突变值较大且持续时间长，而联邦式卡尔曼滤波则可以在短时间内恢复至正常波动范围内。
由3个姿态角误差可得出，误差角在滤波稳定后可达到角分级，天向通道中的俯仰角误差在前期滤波过程中波动较大，最大波动值可达270′，稳定后在15′内波动；东向通道中的航向角和北向通道中的横滚角仿真误差值较小，稳定后波动值为2.5′左右。注入里程计测量误差扰动后，与集中式卡尔曼滤波相对比，联邦式卡尔曼滤波出现的瞬时波动值较小且持续时间短，容错能力更强。
图6 俯仰角误差对比
Fig.6 Comparison of pitch angle error
4.2 位置误差
集中式与联邦式卡尔曼滤波经度误差对比如图7所示。从图7可看出，在300 s时加入里程计测量误差扰动后，集中式卡尔曼滤波较联邦式卡尔曼滤波出现的瞬时波动值更大且持续时间更长，可达30 s，与注入里程计扰动时长相符。待稳定后，2类滤波的波动值为-1～1 cm。
图7 经度误差对比
Fig.7 Comparison of longitude error
集中式与联邦式卡尔曼滤波纬度误差对比如图8所示。从图8可看出，在300 s时加入里程计测量误差扰动后，2类滤波均出现了相应波动，其中集中式卡尔曼滤波的波动值较大且持续时间较长，滤波稳定后，2类滤波的波动值为-0.5～0.5 cm。
图8 纬度误差对比
Fig.8 Comparison of latitude error
集中式与联邦式卡尔曼滤波高度误差对比如图9所示。从图9可看出，在300 s时加入里程计测量误差扰动后，集中式卡尔曼滤波出现了较大瞬时波动值，最高达-4.5 cm，且波动持续时间较长，滤波稳定后，2类滤波的波动值为-2～2 cm。
图9 高度误差对比
Fig.9 Comparison of height error
由上述分析可得出，位置误差在滤波稳定后可达厘米级精度，天向通道的高度误差相较于经纬度误差较大，达到10 cm，稳定后在-2～2 cm内波动。经度误差稳定后为-1～1 cm，纬度误差稳定后为-0.5～0.5 cm。与姿态误差最终结果表现相同，联邦式卡尔曼滤波在加入误差扰动后出现的瞬时波动值较小且持续时间短，容错能力强。
5 结论
(1) 使用捷联式惯导、里程计及测距雷达组合测量，可实现煤矿井下连续采煤机较高精度的位姿信息获取。
(2) 通过测距雷达与里程计共同进行位置推算辅助修正捷联惯导系统，可有效抑制惯导随时间积累所产生的位置漂移误差。
(3) 联邦式卡尔曼滤波与集中式卡尔曼滤波相比，2类滤波在精度基本一致的前提下，联邦式卡尔曼滤波在受到较大测量误差干扰后出现的瞬时波动值小且波动时间短，具备一定的容错性。
(4) 采用联邦式卡尔曼滤波的定位方法的定位精度可达±2 cm，定向精度可达15′，可靠性较高，抗干扰能力较强，可为下一步井下现场试验提供理论与实验依据。
[1] 王国法,赵国瑞,任怀伟.智慧煤矿与智能化开采关键核心技术分析[J].煤炭学报,2019,44(1):34-41.
WANG Guofa, ZHAO Guorui, REN Huaiwei. Analysis of key technologies of intelligent coal mine and intelligent mining[J]. Journal of China Coal Society, 2019,44(1):34-41.
[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].煤炭学报，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] 张守祥,李森,宋来亮.基于惯性导航和里程仪的煤矿采掘设备定位[J].工矿自动化,2018,44(5):52-57.
ZHANG Shouxiang, LI Sen,SONG Lailiang. Positioning of coal mining equipments based on inertial navigation and odometer[J].Industry and Mine Automation, 2018,44(5):52-57.
[5] 万文辉,李宇,胡文敏,等.基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J].武汉大学学报(信息科学版)，2018,43(1):101-106.
WAN Wenhui, LI Yu, HU Wenmin,et al.Mobile platform localization by integration stereo camera,IMU and wheel odometer based on federal filter[J]. Journal of Wuhan University (Information Science Edition), 2018,43(1):101-106.
[6] 张旭辉,赵建勋,杨文娟,等.悬臂式掘进机视觉导航与定向掘进控制技术研究[J/OL].煤炭学报:1-11[2020-10-10].https://doi.org/10.13225/j.cnki.jccs.ZN20.0357.
ZHANG Xuhui, ZHAO Jianxun, YANG Wenjuan, et al. Vision-based navigation and directional heading control technologies of boom-type roadheader[J/OL]. Journal of China Coal Society: 1-11[2020-10-10]. https://doi.org/ 10.13225 /j.cnki.jccs.ZN20.0357.
[7] 贺建伟,常映辉,陈宁,等.井下掘进装备组合导航系统研究[J].煤矿机械,2020,41(9):32-34.
HE Jianwei, CHANG Yinghui, CHEN Ning, et al. Research on integrated navigation system of underground driving equipment[J].Coal Mine Machinery, 2020,41(9):32-34.
[8] FORTE M D N, CORREIA W B, NOGUEIRA F G, et al. Reference tracking of a nonholonomic mobile robot using sensor fusion techniques and linear control[J]. IFAC-PapersOnLine, 2018, 51( 4):364-369.
[9] ZHANG Hongmei, ZHANG Huaqing, XU Guangyan,et al. Attitude anti-interference federal filtering algorithm for MEMS-SINS/GPS/magnetometer/SV integrated navigation system[J].Measurement and Control-London-Institute of Measurement and Control, 2020,53(1/2):46-60.
[10] XU Shuqing, ZHOU Haiyin,WANG Jiongqi,et al. SINS/CNS/GNSS integrated navigation based on an improved federated sage-husa adaptive filter[J]. Sensors,2019,19(17):3812.
[11] WANG Qiuying,CUI Xufei, LI Yibing, et al. Performance enhancement of a USV INS/CNS/DVL integration navigation system based on an adaptive information sharing factor federated filter[J]. Sensors,2017,17(2):239.
[12] 秦永元,张洪钺,汪叔华.卡尔曼滤波与组合定位原理[M].3版.西安:西北工业大学出版社,2015.
QIN Yongyuan,ZHANG Hongyue,WANG Shuhua.Kalman filtering and combined positioning principle[M]. 3rd edition. Xi'an: Northwestern Polytechnical University Press, 2015.
[13] 张国良,曾静.组合导航原理与技术[M].西安:西安交通大学出版社,2008.
ZHANG Guoliang, ZENG Jing. The principle and technology of integrated navigation[M]. Xi'an: Xi'an Jiaotong University Press, 2008.
[14] 李久顺.捷联惯导系统误差抑制及补偿方法研究[D].哈尔滨:哈尔滨工程大学,2018.
LI Jiushun. Research on error suppression and compensation methods of strapdown inertial navigation system[D]. Harbin:Harbin Engineering University, 2018.
[15] 白亮,严恭敏,朱启举,等.里程计辅助的捷联惯导系统研究[J].弹箭与制导学报,2013,33(6):16-18.
BAI Liang, YAN Gongmin, ZHU Qiju, et al.SINS/OD integrated navigation system[J].Journal of Projectiles, Rockets, Missiles and Guidance,2013,33(6):16-18.
[16] 段睿,张小红,朱锋.多源信息融合的组合导航自适应联邦滤波算法[J].系统工程与电子技术, 2018,40(2):267-272.
DUAN Rui, ZHANG Xiaohong, ZHU Feng. Adaptive federated filter for multi-sources information fusion in integrated navigation system[J]. Systems Engineering and Electronic Technology,2018,40(2):267-272.
[17] 唐娟.基于联邦强跟踪卡尔曼滤波的组合导航关键技术研究[D].济南：山东大学,2018.
TANG Juan. Research on integrated navigation technology based on federated strong tracking Kalman filter[D]. Jinan:Shandong University, 2018.
[18] 杨宏韬. 基于多信息源数据融合的光电跟踪系统精度提高方法的研究[D].长春：中国科学院长春光学精密机械与物理研究所,2016.
YANG Hongtao. Research on the method of improving accuracy of photoelectric tracking system based on multi-source date fusion[D]. Changchun：Changchun Institute of Optics, Fine Mechanics and Physics, Chinese Academy of Sciences，2016.
[19] 袁克非.组合导航系统多源信息融合关键技术研究[D].哈尔滨：哈尔滨工程大学,2012.
YUAN Kefei.Key technique of multi-source information fusion in integrated navigation system[D]. Harbin：Harbin Engineering University, 2012.
[20] 严恭敏,WANG Jinling,周馨怡.基于实测轨迹的高精度捷联惯导模拟器[J].导航定位学报,2015,3(4):27-31.
YAN Gongmin,WANG Jinling, ZHOU Xinyi. High-precision simulator for strapdown inertial navigation systems based on real dynamics[J].Journal of Navigation and Positioning,2015,3(4):27-31.
