0 引言
2050年以前煤炭仍将是我国的主导能源[1-2]。煤矿巷道掘进是煤矿开采的重要部分，但掘进工作面环境恶劣，危险性高，因此实现掘进机智能化，进而实现掘进工作面少人甚至无人化，对于煤矿安全生产具有重要意义。
掘进机定姿定位是实现智能掘进机的关键技术之一，近年来得到了很多研究人员的关注。文献[3-4]提出了基于视觉的掘进机位姿测量方法，但该方法易受井下粉尘等因素的影响。文献[5]利用超宽带技术对掘进机进行定姿定位检测，该方法系统结构简单，但因煤矿井下环境中金属设备较多，对超宽带信号传输造成很大影响。文献[6]提出采用空间交汇技术对悬臂式掘进机进行位姿测量，该方法灵活性强，但开发难度较大。文献[7]采用捷联惯导技术实现掘进机定位，该方法环境适应性强、短时间内精度高[8]，但累计误差随时间推移逐渐增大[9]。文献[10]采用全站仪进行掘进机定位，测量精度较高，但会因粉尘过大或其他原因导致全站仪光路被遮挡而无法进行定位。
本文综合利用全站仪的高精度及捷联惯导的无源性特点，提出一种掘进机全站仪与捷联惯导组合定位方法。该方法采用卡尔曼滤波器融合全站仪测量的掘进机位置信息和捷联惯导测量的位姿信息，实现掘进机组合定位，可在全站仪光路被遮挡的情况下实现掘进机精准定位。
1 总体方案
1.1 组合定位坐标系
掘进机全站仪与捷联惯导组合定位涉及的坐标系如图1所示。
图1 组合定位坐标系
Fig.1 Combined positioning coordinate system
(1) 东-北-天坐标系(简称T系)OtXtYtZt，表示载体所在位置。T系以载体重心为原点，指向东、北、天(也称北-东-地坐标系，满足“右手定则”)。
(2) 掘进机机体坐标系(简称b系)ObXbYbZb。b系以掘进机机身重心为原点,Xb轴平行于巷道底面指向左，Yb轴指向掘进机前进方向，Zb轴指向掘进机机身竖轴方向。
(3) 导航坐标系(简称n系)OnXnYnZn。n系是导航过程中的基准，一般根据载体导航需要选取。本文采用指北方位系统作为n系，即与T系重合。
(4) 全站仪测量坐标系(简称c系)OcXcYcZc。c系以全站仪测站中心为原点，Xc轴为后视方向，Zc轴竖直向上，Yc轴根据“右手定则”确定。
1.2 组合定位原理
掘进机全站仪与捷联惯导组合定位过程包括基于全站仪的机身位置参数测量、基于捷联惯导的机身位姿参数测量、基于卡尔曼滤波的机身位置参数与位姿参数融合。
组合定位原理如图2所示。将捷联惯导和目标棱镜安装在掘进机机身上，使其随掘进机移动。开始定位前，测绘人员测量掘进机所在位置的经纬度，并根据巷道走向确定西安80坐标系下任意2个坐标点，一个坐标点作为全站仪测站点，另一个作为后视点。根据2个坐标点构建c系，全站仪通过测量掘进机机身上的目标棱镜，解算出掘进机机身在西安80坐标系下的坐标。捷联惯导根据初始经纬度进行初始对准，得到初始转换矩阵。随着掘进机移动，捷联惯导结合掘进机初始经纬度及初始姿态角，可实时测量出掘进机位姿和速度。将捷联惯导测量的经纬度转换为西安80坐标系下的坐标值，利用卡尔曼滤波器将捷联惯导测量的位置、速度及姿态角信息与全站仪测量的掘进机位置信息融合，得出掘进机位姿信息。
图2 掘进机全站仪与捷联惯导组合定位原理
Fig.2 Roadheader positioning principle combining total station and strapdown inertial navigation system
2 基于捷联惯导的掘进机位姿参数测量
2.1 捷联惯导初始对准
捷联惯导在工作前须进行初始对准，包括粗对准和精对准[11]。
初始粗对准的目的是利用捷联惯导三轴加速度计测量的重力加速度和陀螺仪测量的地球自转速度计算出n系到b系的转换矩阵。采用文献[12]中方法实现捷联惯导粗对准。
完成粗对准后，捷联惯导获取粗略的姿态转换矩阵。为了获取更准确的姿态转换矩阵，采用卡尔曼滤波器进行精对准，利用系统误差作为滤波状态，构建卡尔曼滤波方程和观测方程，实现精对准[12-13]。
初始对准时，掘进机的速度为0，因此有
(1)
式中：VE，VN，VU分别为n系下掘进机东、北、天方向速度；fE，fN，fU分别为三轴加速度计测量的东、北、天方向比力；g为重力加速度。
采用卡尔曼滤波器进行精对准时，通常利用外部观测值(如位置、速度、姿态角等)确定失准角[14]。本文以速度作为观测值设计卡尔曼滤波器[15]。捷联惯导在静止状态下的系统误差方程为
(2)
式中：X为状态向量，
分别为东、北、天方向的速度误差，φE，φN，φU分别为东、北、天方向的姿态角误差，
分别为东、北、天方向的三轴加速度计常值零偏，εE，εN，εU分别为东、北、天方向的陀螺漂移；A为状态转移矩阵；Z为观测值；H为观测矩阵。
通过分析，可观测值为ΔVE，ΔVN，ΔVU，φE，φN，φU，εN，εU,因此建立8个状态模型，对式(2)进行离散化处理，之后采用卡尔曼滤波器进行最优化估计，从而获取准确的姿态转换矩阵。
卡尔曼滤波器状态一步预测方程为
(3)
式中：
为第k-1个离散状态向量的一步预测值；φk/k-1为一步转移矩阵；
为第k-1个离散状态向量估计值。
状态估计方程为
(4)
式中：Kk为滤波增益；Zk为离散观测值；Hk为离散观测矩阵。
(5)
式中：Pk为状态估计均方差；Rk为观测噪声方差。
一步预测均方差为
(6)
式中：Γk-1为系统离散噪声驱动矩阵；Qk-1为噪声方差。
状态估计均方差为
Pk=(I-KkHk)Pk/k-1
(7)
式中I为单位矩阵。
2.2 掘进机位姿解算
2.2.1 掘进机姿态解算
采用四元数法进行掘进机姿态解算。掘进机机身姿态角包括俯仰角γ、航向角α和横滚角β。n系到b系的转换矩阵为
(8)
n系到b系的旋转四元数为
(9)
式中：Q为四元数，Q=[q0 q1 q2 q3]；un为旋转的轴和旋转方向；θ为旋转角度。
四元数基体坐标系(n系)到b系的转换矩阵为
(10)
由式(8)、式(10)可得掘进机机身姿态角：
(11)
2.2.2 掘进机位置解算
根据式(12)、式(13)计算掘进机速度。由于掘进机的垂直方向速度远小于水平方向速度，所以在计算掘进机速度时，忽略垂直方向速度，同时忽略掘进机高度。
(12)
(13)
式中：ωie为掘进机跟随地球的旋转角速度；L为掘进机所在位置的纬度；RN为地球卯酉曲率半径。
则掘进机位置更新微分方程为
(14)
式中：RM为子午圈主曲率半径；λ为掘进机所在位置的经度。
2.3 经纬度与西安80坐标系之间转换
进行组合定位数据融合之前，需要将捷联惯导测量的实时经纬度转换为西安80坐标系[16-17]下的坐标，之后与全站仪测量的掘进机机身在西安80坐标系下的位置数据进行融合计算，得出机身在西安80坐标系下的位姿数据。实时经纬度与西安80坐标系的转换公式为[18]
x=
(61-58tan2 L+tan4 L+270η2tan2 L)
(15)
y=
58η2tan2 L)
(16)
式中：(x，y)为掘进机在西安80坐标系下的坐标；s为从赤道到掘进机所在纬度的子午线弧长；η=0.082 094 469cos L。
3 基于卡尔曼滤波的数据融合
采用卡尔曼滤波器实现全站仪和捷联惯导数据融合，以全站仪、捷联惯导测得的位置误差作为融合状态向量和观测值，建立组合定位状态方程和观测方程，最终解算出组合定位下的掘进机位置和姿态角。
组合定位状态方程为
(17)
式中：Y为状态向量，
为姿态角误差，ΔV为速度误差，ΔpINS为捷联惯导解算的位置误差，ΔpT为全站仪计算的位置误差，ε为陀螺漂移，
为三轴加速度计常值零偏；Γ为噪声驱动矩阵；W为系统噪声。
以捷联惯导解算的位置数据与全站仪测量的位置数据之差为观测值，得观测方程：
Z=ΔpINS-ΔpT
(18)
由此建立组合定位状态方程：
(19)
式中V为观测噪声。
对于全站仪和捷联惯导组合定位数据融合的卡尔曼滤波方法同式(3)—式(7)。
4 组合定位试验
在西安煤矿机械有限公司搭建试验平台，对掘进机全站仪与捷联惯导组合定位方法进行验证，如图3所示。试验采用Xi-1000Q自动全站仪(测量精度为1 mm)和民用航空捷联惯性基准系统。将捷联惯导放置在手推车上，棱镜置于捷联惯导中心位置。将全站仪设置为跟踪模式，连续测量手推车实时位置。人为推动手推车模拟掘进机在设计路线上行走，记录全站仪、捷联惯导测量的位置信息及组合定位信息。假设手推车高度始终为全站仪初始测量值。试验以全站仪测量的位置数据为实际值。
(a) 试验方案
(b) 试验装置
图3 试验方案及装置
Fig.3 Experimental scheme and equipment
采用经纬仪测得手推车初始位置：L=34°29′29.846 820 8″，λ=109°21.374 304 7″，h=346.005 5 m。用全站仪测得手推车起点和终点的高斯坐标(x，y)分别为(3 818 663.24 m,592 368.64 m)，(3 818 661.12 m, 592 377.82 m)。全站仪和捷联惯导输出速率均为9 600 bit/s，全站仪采样频率为3 Hz。人为推动手推车从起点到终点运动，每运动5 s暂停1次，记录全站仪、捷联惯导及组合定位方法测量的该位置高斯坐标。试验共测得9个点的位置数据，如图4所示。可看出手推车在运动过程中，x，y方向的组合定位数据与实际值基本相同，而捷联惯导测量数据与实际值偏差随时间延长逐渐增大。
(a) x方向
(b) y方向
图4 位置测量数据
Fig.4 Measured position data
组合定位数据与实际值误差见表1，误差曲线如图5所示。可看出组合定位误差较小：在x方向的定位误差最大值为0.029 1 m，最小值为0.010 0 m，平均值为0.019 93 m；在y方向定位误差最大值为0.029 5 m，最小值为0.011 0 m，平均值为0.018 26 m。
表1 组合定位数据与实际值误差
Table 1 Errors between combined positioning data and the actual ones
方向误差/m0 s5 s10 s15 s20 s25 s30 s35 s40 s平均误差/mx0.024 20.026 80.023 90.010 00.029 10.019 00.024 20.019 50.011 70.019 93y0.027 20.013 80.019 60.011 00.029 50.010 70.018 80.019 90.013 80.018 26
全站仪和组合定位方法获取的手推车轨迹如图6所示。可看出手推车在运动过程中，组合定位方法获取的轨迹与实际运动轨迹基本重合，验证了组合定位方法具有较高的定位精度。
图5 组合定位误差曲线
Fig.5 Combined positioning error curves
图6 手推车轨迹
Fig.6 Trolley track
组合定位方法测得的姿态角如图7所示。
(a) 俯仰角
(b) 横滚角
(c) 航向角
图7 姿态角测量结果
Fig.7 Measuring results of attitude angle
5 结语
掘进机全站仪与捷联惯导组合定位方法基于捷联惯导测量的掘进机位姿参数和全站仪测量的掘进机位置参数，采用卡尔曼滤波方法进行数据融合，实现了掘进机精确定位。试验结果表明该方法定位精度较高：在x方向的定位误差最大值为0.029 1 m，最小值为0.010 0 m，平均值为0.019 93 m；在y方向的定位误差最大值为0.029 5 m，最小值为0.011 0 m，平均值为0.018 26 m。下一步将研究该方法用于井下的实际定位精度。
[1] 谢和平,高明忠,高峰,等.关停矿井转型升级战略构想与关键技术[J].煤炭学报,2017,42(6):1355-1365.
XIE Heping,GAO Mingzhong,GAO Feng,et al.Strategic concept ualization and key technology for the transformation and upgrading of shut-down coal mines[J].Journal of China Coal Society,2017,42(6):1355-1365.
[2] 袁亮,张平松.煤炭精准开采地质保障技术的发展现状及展望[J].煤炭学报,2019,44(8):2277-2284.
YUAN Liang,ZHANG Pingsong.Development status and prospect of geological guarantee technology for precise coal mining[J].Journal of China Coal Society,2019,44(8):2277-2284.
[3] 杜雨馨,刘停,童敏明,等.基于机器视觉的悬臂式掘进机机身位姿检测系统[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.
[4] 张旭辉,刘永伟,杨文娟,等.矿用悬臂式掘进机截割头姿态视觉测量系统[J].工矿自动化,2018,44(8):63-67.
ZHANG Xuhui,LIU Yongwei,YANG Wenjuan,et al.Vision measurement system for cutting head attitude of mine-used boom-type roadheader[J].Industry and Mine Automation,2018,44(8):63-67.
[5] 符世琛,成龙,陈慎金,等.面向掘进机的超宽带位姿协同检测方法[J].煤炭学报,2018,43(10):2918-2925.
FU Shichen,CHENG Long,CHEN Shenjin,et al.Ultra-wideband pose collaborative detection method of roadheader[J].Journal of China Coal Society,2018,43(10):2918-2925.
[6] 吴淼,贾文浩,华伟,等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[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.
[7] 田原.悬臂式掘进机自动定位技术研究与试验[J].矿山机械,2019,47(7):6-10.
TIAN Yuan.Research and test on automatic positioning technology for boomed roadheader[J].Mining & Processing Equipment,2019,47(7):6-10.
[8] 南源桐.基于捷联惯导的综采工作面直线度测量技术研究[D].西安:西安科技大学,2017．
NAN Yuantong.Research on straightness measurement technology of fully mechanized mining face based on strapdown inertial navigation[D].Xi'an:Xi'an University of Science and Technology,2017.
[9] 张旭辉,周颖,杨文娟,等.位姿检测技术在煤矿井下工作面巡检机器人中的应用[J].传感器与微系统,2020,39(5):152-155.
ZHANG Xuhui,ZHOU Ying,YANG Wenjuan,et al.Application of positioning and pose detection technology in working face inspection robot underground coal mine[J].Transducer and Microsystem Technologies,2020,39(5):152-155.
[10] 陶云飞.掘进机位姿激光自动测量方法及系统研究[D].北京:中国矿业大学(北京),2017.
TAO Yunfei.Research on automatic measurement method and system of position and attitude of roadheader based on laser measurement system[D].Beijing:China University of Mining and Technology(Beijing),2017.
[11] 侯迎团,祖肇梓.一种机载惯导系统初始对准方法的研究与仿真[J].计算机仿真,2016,33(7):78-82.
HOU Yingtuan,ZU Zhaozi.Research and simulation of INS's initial alignment method[J].Computer Simulation,2016,33(7):78-82.
[12] 马宏伟,张璞,毛清华,等.基于捷联惯导和里程计的井下机器人定位方法研究[J].工矿自动化,2019,45(4):35-42.
MA Hongwei,ZHANG Pu,MAO Qinghua,et al.Research on positioning method of underground robot based on strapdown inertial navigation and odometer[J].Industry and Mine Automation,2019,45(4):35-42.
[13] 许明成,戴邵武.卡尔曼滤波在惯导初始对准中的应用[J].电子设计工程,2017,25(23):43-46.
XU Mingcheng,DAI Shaowu.Kalman filtering and its application in INS initial alignment[J].Electronic Design Engineering,2017,25(23):43-46.
[14] 柴华,王勇,许大欣,等.地固系下四元数和卡尔曼滤波方法的惯导初始精对准研究[J].武汉大学学报(信息科学版),2012,37(1):68-72.
CHAI Hua,WANG Yong,XU Daxin,et al.Inertial navigation system fine alignment under ECEF frame with quaternion and Kalman filter[J].Geomatics and Information Science of Wuhan University,2012,37(1):68-72.
[15] 杨晓霞,阴玉梅.可观测度的探讨及其在捷联惯导系统可观测性分析中的应用[J].中国惯性技术学报,2012,20(4):405-409.
YANG Xiaoxia,YIN Yumei.Discussions on observability and its applications in SINS[J].Journal of Chinese Inertial Technology,2012,20(4):405-409.
[16] 韩涛.1980西安坐标系统与2000国家大地坐标系转换研究[J].科技资讯,2020,18(2):200-202.
HAN Tao.1980 Xi'an coordinate system and 2000 national geodetic coordinate system conversion research[J].Science and Technology Information,2020,18(2):200-202.
[17] 何林,柳林涛,许超钤,等.常见平面坐标系之间相互转换的方法研究——以1954北京坐标系、1980西安坐标系、2000国家大地坐标系之间的平面坐标相互转换为例[J].测绘通报,2014(9):6-11.
HE Lin,LIU Lintao,XU Chaoqian,et al.Study on common plane coordinate system conversion-plane coordinate conversion between BJ54,XZ80 and CGCS2000[J].Bulletin of Surveying and Mapping,2014(9):6-11.
[18] 钟业勋,童新华,王龙波.从1980西安坐标系到2000国家大地坐标系的坐标变换[J].海洋测绘,2010,30(1):1-3.
ZHONG Yexun,TONG Xinhua,WANG Longbo.Coordinate transformation from Xi'an coordinate system 1980 to national geodetic coordinate system 2000[J].Hydrographic Surveying and Charting,2010,30(1):1-3.