掘进机全站仪与捷联惯导组合定位方法

张旭辉， 刘博兴， 张超， 杨文娟， 赵建勋

(1．西安科技大学 机械工程学院， 陕西 西安 710054；2．陕西省矿山机电装备智能监测重点实验室， 陕西 西安 710054)

0 引言

2050年以前煤炭仍将是我国的主导能源[1-2]。煤矿巷道掘进是煤矿开采的重要部分，但掘进工作面环境恶劣，危险性高，因此实现掘进机智能化，进而实现掘进工作面少人甚至无人化，对于煤矿安全生产具有重要意义。

掘进机定姿定位是实现智能掘进机的关键技术之一，近年来得到了很多研究人员的关注。文献[3-4]提出了基于视觉的掘进机位姿测量方法，但该方法易受井下粉尘等因素的影响。文献[5]利用超宽带技术对掘进机进行定姿定位检测，该方法系统结构简单，但因煤矿井下环境中金属设备较多，对超宽带信号传输造成很大影响。文献[6]提出采用空间交汇技术对悬臂式掘进机进行位姿测量，该方法灵活性强，但开发难度较大。文献[7]采用捷联惯导技术实现掘进机定位，该方法环境适应性强、短时间内精度高[8]，但累计误差随时间推移逐渐增大[9]。文献[10]采用全站仪进行掘进机定位，测量精度较高，但会因粉尘过大或其他原因导致全站仪光路被遮挡而无法进行定位。

本文综合利用全站仪的高精度及捷联惯导的无源性特点，提出一种掘进机全站仪与捷联惯导组合定位方法。该方法采用卡尔曼滤波器融合全站仪测量的掘进机位置信息和捷联惯导测量的位姿信息，实现掘进机组合定位，可在全站仪光路被遮挡的情况下实现掘进机精准定位。

1 总体方案

1.1 组合定位坐标系

掘进机全站仪与捷联惯导组合定位涉及的坐标系如图1所示。

(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)

通过分析，可观测值为ΔVE，ΔVN，ΔVU，φE，φN，φU，εN，εU,因此建立8个状态模型，对式(2)进行离散化处理，之后采用卡尔曼滤波器进行最优化估计，从而获取准确的姿态转换矩阵。

卡尔曼滤波器状态一步预测方程为

(3)

状态估计方程为

(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=[q0q1q2q3]；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]

(61-58tan2L+tan4L+270η2tan2L)

(15)

58η2tan2L)

(16)

式中：(x，y)为掘进机在西安80坐标系下的坐标；s为从赤道到掘进机所在纬度的子午线弧长；η=0.082 094 469cosL。

3 基于卡尔曼滤波的数据融合

采用卡尔曼滤波器实现全站仪和捷联惯导数据融合，以全站仪、捷联惯导测得的位置误差作为融合状态向量和观测值，建立组合定位状态方程和观测方程，最终解算出组合定位下的掘进机位置和姿态角。

组合定位状态方程为

(17)

以捷联惯导解算的位置数据与全站仪测量的位置数据之差为观测值，得观测方程：

Z=ΔpINS-ΔpT

(18)

由此建立组合定位状态方程：

(19)

式中V为观测噪声。

对于全站仪和捷联惯导组合定位数据融合的卡尔曼滤波方法同式(3)—式(7)。

4 组合定位试验

在西安煤矿机械有限公司搭建试验平台，对掘进机全站仪与捷联惯导组合定位方法进行验证，如图3所示。试验采用Xi-1000Q自动全站仪(测量精度为1 mm)和民用航空捷联惯性基准系统。将捷联惯导放置在手推车上，棱镜置于捷联惯导中心位置。将全站仪设置为跟踪模式，连续测量手推车实时位置。人为推动手推车模拟掘进机在设计路线上行走，记录全站仪、捷联惯导测量的位置信息及组合定位信息。假设手推车高度始终为全站仪初始测量值。试验以全站仪测量的位置数据为实际值。

(a) 试验方案

采用经纬仪测得手推车初始位置：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方向

组合定位数据与实际值误差见表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

全站仪和组合定位方法获取的手推车轨迹如图6所示。可看出手推车在运动过程中，组合定位方法获取的轨迹与实际运动轨迹基本重合，验证了组合定位方法具有较高的定位精度。

图5 组合定位误差曲线Fig.5 Combined positioning error curves

图6 手推车轨迹Fig.6 Trolley track

组合定位方法测得的姿态角如图7所示。

(a) 俯仰角

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。下一步将研究该方法用于井下的实际定位精度。