0.   引言

煤炭目前仍是我国的主导能源，占能源结构的57%^[1-2]。掘进作业环境恶劣，危险系数高，对一线工人的安全与健康产生了较多不利影响，同时采掘不平衡也制约着煤炭生产，因此实现掘进工作面的智能化乃至无人化对煤矿安全高效生产有重要意义。掘进机自主精确定位是实现掘进工作面智能化乃至无人化的关键。

捷联惯导技术是一种不依赖外部信息的自主导航技术，其无源特性符合井下要求^[3]，但利用捷联惯导技术进行掘进机定位，其测量误差会随时间累计，影响定位精度，应考虑与其他测量技术进行组合来减小误差^[4]。里程计具有精度高、自主性强等优点，因此捷联惯导与里程计组合是一种较为理想的掘进机定位方案。吴淼等^[5]将二维里程计、捷联惯导与激光偏距感知系统结合，在巷道坐标下描述了掘进机偏距与偏角，实现了掘进机相对巷道轴向偏距与偏角的精确感知。沈阳等^[6]对文献[5]中二维里程计与捷联惯导的数据融合算法进行了改进，提高了掘进机自主导航系统误差估计的精度。刘豪^[7]将捷联惯导与里程计组合定位用于掘进机，并针对掘进机打滑导致的里程计失效问题，设计了基于BP神经网络的打滑识别与补偿方法，实现了封闭巷道内掘进机的长时导航定位。

上述研究解决了捷联惯导误差随时间累计的问题，但单里程计仅能测得掘进机单条履带速度，不能真实反映掘进机车体中心的实际运行状态^[8]，且存在安装位置带来的误差^[9]。因此，笔者提出了一种基于捷联惯导与差速里程计的掘进机组合定位方法，采用卡尔曼滤波将捷联惯导与差速里程计测量的掘进机位姿数据进行融合，对捷联惯导误差进行补偿校正，以减小捷联惯导累计误差对掘进机定位的影响，提高掘进机定位精度。

1.   掘进机组合定位原理

基于捷联惯导与差速里程计的掘进机组合定位方法采用以捷联惯导为主、差速里程计为辅的定位方式，其原理如图1所示。

图  1  基于捷联惯导与差速里程计的掘进机组合定位原理

Figure  1.  Combined positioning principle of roadheader based on strapdown inertial navigation and differential odometer

下载: 全尺寸图片 幻灯片

该方法的实现由3个部分组成：基于捷联惯导的位姿感知、基于差速里程计的航位推算、基于卡尔曼滤波的数据融合。① 捷联惯导对准后得到初始的姿态转换矩阵，随着掘进机移动，更新姿态转换矩阵，将加速度与角加速度积分，得到载体坐标系下的速度与角速度，进一步得到导航坐标系下掘进机参考姿态角与参考位移。② 2个里程计分别测量掘进机左右履带速度，在已知前一时刻掘进机位姿的前提下，推算出当前时刻掘进机位姿，并通过姿态转换矩阵投影至导航坐标系下。③ 将捷联惯导与差速里程计测量的位姿数据之差作为误差观测值输入卡尔曼滤波器，以卡尔曼滤波器输出值对捷联惯导数据进行校正与补偿。

2.   掘进机位姿数据测量

2.1   坐标系及掘进机姿态角描述

为描述掘进机位姿，本文用到如下坐标系。

（1） 地心坐标系−$ {o_{\rm{i}}}{x_{\rm{i}}}{y_{\rm{i}}}{z_{\rm{i}}} $。地心坐标系以地心为坐标原点，$ {o_{\rm{i}}}{x_{\rm{i}}} $轴指向春分点，$ {o_{\rm{i}}}{z_{\rm{i}}} $轴沿地球自转轴指向北极，$ {o_{\rm{i}}}{y_{\rm{i}}} $轴与其余两轴构成右手坐标系。在捷联惯导系统中，陀螺仪与加速度计的输出均以此为参考系。

（2） 地球坐标系−$ {o_{\rm{e}}}{x_{\rm{e}}}{y_{\rm{e}}}{z_{\rm{e}}} $。地球坐标系以地球中心为坐标原点，$ {o_{\rm{e}}}{x_{\rm{e}}} $与$ {o_{\rm{e}}}{y_{\rm{e}}} $均在地球赤道平面内，其中$ {o_{\rm{e}}}{x_{\rm{e}}} $轴指向本初子午线与赤道的交点，$ {o_{\rm{e}}}{y_{\rm{e}}} $轴沿地球自转轴指向北极，$ {o_{\rm{e}}}{z_{\rm{e}}} $轴与其余两轴构成右手坐标系。

（3） 导航坐标系−$ {o_{\rm{n}}}{x_{\rm{n}}}{y_{\rm{n}}}{z_{\rm{n}}} $。导航坐标系以掘进机重心为坐标原点，$ {o_{\rm{n}}}{x_{\rm{n}}} $轴指向地理东向，$ {o_{\rm{n}}}{y_{\rm{n}}} $轴指向地理北向，$ {o_{\rm{n}}}{z_{\rm{n}}} $轴与其余两轴构成右手坐标系。导航坐标系用来表示组合定位结果。导航坐标系相对于地球坐标系的位置可用掘进机的地理位置（纬度$ L $、经度$ \lambda $、高度$ h $）表示。

（4） 载体坐标系−$ {o_{\rm{b}}}{x_{\rm{b}}}{y_{\rm{b}}}{z_{\rm{b}}} $。载体坐标系以掘进机重心为坐标原点，$ {o_{\rm{b}}}{x_{\rm{b}}} $轴与$ {o_{\rm{b}}}{y_{\rm{b}}} $轴分别指向载体的右方与前方，$ {o_{\rm{b}}}{z_{\rm{b}}} $指向天并与其余两轴构成右手坐标系。

在三维空间中，掘进机姿态的变化为三自由度的刚体定轴旋转，可用一组姿态角（航向角$ \psi $、横滚角$ \gamma $、俯仰角$ \theta $）描述掘进机具体空间指向^[10]，如图2所示。掘进机转动角度由载体坐标系与导航坐标系之间的角位置关系决定。为简化计算模型，本文主要探讨对航向角$ \psi $的感知精度，忽略横滚角$ \gamma $与俯仰角$ \theta $的变化。

图  2  掘进机姿态角

Figure  2.  Attitude angle of roadheader

下载: 全尺寸图片 幻灯片

2.2   捷联惯导

2.2.1   姿态更新

捷联惯导与差速里程计的初始数据为载体坐标系下掘进机的位姿信息，组合定位需要确定导航坐标系下的掘进机状态，因此需将载体坐标系下的数据通过姿态转换矩阵投影至导航坐标系。掘进机在运动过程中，姿态不断变化，姿态信息同样不断更新。

姿态更新主要有方向余弦法、四元数法、欧拉角法，其中四元数法算法简单、易于操作、实用性强^[11]。规范化四元数${\boldsymbol{Q}} =[ {q_0} \;\; \;{q_1} \;\;\;{q_2} \;\;\; {q_3}]^{\rm{T}}$（$ {q_0} $—$ {q_3} $为实数）能够描述刚体绕定轴旋转运动的过程。载体坐标系至导航坐标系某一时刻的姿态转换矩阵$ {\boldsymbol{C}}_{\rm{b}}^{\rm{n}} $可用一组四元数表示：

式中：$ {T_{ij}} $($i,j=1,2,3 $)为姿态转换矩阵$ {\boldsymbol{C}}_{\rm{b}}^{\rm{n}} $第$ i $行第$ j $列的元素。

姿态转换矩阵的更新实质为四元数的更新。文献[10-12]对四元数更新已有详尽描述，本文采用等效旋转矢量法^[12]对四元数更新。

式中：${\boldsymbol{Q}}\left( {{k + 1}} \right)$，${\boldsymbol{Q}}\left( {k} \right)$分别为${k + 1}$和$k$时刻的姿态四元数；$ \otimes $为四元数乘法；$ {\boldsymbol{Q}}\left( h \right) $为由$k$至${k + 1}$时刻的姿态变化四元数。

取得姿态转换矩阵后，捷联惯导可求解得到导航坐标系下掘进机姿态角：

2.2.2   速度与位置更新

通过$ {\boldsymbol{C}}_{\rm{b}}^{\rm{n}} $将掘进机加速度投影至导航坐标系下，得到掘进机速度微分方程：

式中：$ {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} $为捷联惯导得到的导航坐标系下掘进机速度，$ {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} = [{V_{\rm{E}}}\;\; \;{V_{\rm{N}}}\;\;\;{V_{\rm{U}}}]^{\rm{T}} $，$ {V_{\rm{E}}} $，$ {V_{\rm{N}}} $，$ {V_{\rm{U}}} $分别为$ {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} $在东向、北向、天向的分量；$ {{\boldsymbol{f}}^{\rm{b}}} $为载体坐标系下加速度计测得的比力；$ {\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} $为地球坐标系相对于地心坐标系的旋转角速度；$ {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}} $为导航坐标系相对于地球坐标系的旋转角速度，$ {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}} = {\left[ {\begin{array}{*{20}{c}} { - \dfrac{{{V_{\text{N}}}}}{{{R_{\rm{M}}} + h}}}&{\dfrac{{{V_{\text{E}}}}}{{{R_{\rm{N}}} + h}}}&{\dfrac{{{V_{\text{E}}}}}{{{R_{\rm{N}}} + h}}\tan \;L} \end{array}} \right]^{\text{T}}} $，$ {R_{\rm{M}}} $，$ {R_{\rm{N}}} $分别为掘进机所在地理位置的子午圈与卯酉圈曲率半径；gⁿ为导航坐标系下重力加速度。

式（4）表明，只有在加速度计输出中去除有害加速度$ {\boldsymbol{a}} = (2{\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} + {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}}) \times {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} - {{\boldsymbol{g}}^{\rm{n}}} $，才能得到掘进机运动的加速度。

掘进机静止时，$ {\boldsymbol{\dot V}}_{\rm{I}}^{\rm{n}} = {\boldsymbol{0}} $，捷联惯导据此初对准，得到初始的姿态转换矩阵。基于速度方程，捷联惯导的位置微分方程为

式中：$ {\boldsymbol{P}}_{\rm{I}}^{\rm{n}} $为捷联惯导得到的导航坐标系下掘进机地理坐标；L_I，λ_I，h_I分别为捷联惯导得到的掘进机纬度、经度、高度。

2.3   差速里程计

参考文献[13]的方法，对掘进机进行建模，掘进机行走部简化模型如图3所示。掘进运动可看作在$ {x_{\rm{b}}}{o_{\rm{b}}}{y_{\rm{b}}} $平面绕瞬时转动中心$ c $旋转，随着掘进机的行走，$ c $的位置不断变化。

图  3  掘进机行走部简化模型

Figure  3.  Simplified model of roadheader walking section

下载: 全尺寸图片 幻灯片

基于简化模型可得

式中：$ {\boldsymbol{V}}_{\rm{D}}^{\rm{b}} $，$ {\boldsymbol{\omega }}_{\rm{D}}^{\rm{b}} $分别为差速里程计得到的掘进机在载体坐标系下的速度与掘进机中心相对于点$c$的旋转角速度；$ {v_{\rm{c}}} $为掘进机中心线速度；$ {\omega _{\rm{c}}} $为旋转角速度；${r_{\rm{c}}}$为掘进机转弯半径；$ {v_{\text{l}}} $，$ {v_{\text{r}}} $分别为掘进机左右履带的线速度；$ \eta $为常数^[13]，与掘进机总负载、履带与地面的相对摩擦因数、转弯半径等有关；$ {d_{{\text{ω}}{\rm{b}}}} $为履带间距。

掘进机两侧各有1个里程计对履带测速，左右里程计输出分别为${\boldsymbol{V}}_{\text{l}}^{\rm{m}} = {[0\;\;\;{v_{\text{l}}}\;\;\;0]^{\rm{T}}}$，${\boldsymbol{V}}_{\text{r}}^{\rm{m}} = {[0\;\;\;{v_{\text{r}}}\;\;\;0]^{\rm{T}}}$，根据航位推算原理，掘进机线速度与角速度可由履带速度推得：

式中$ {{\boldsymbol{I}}_{23}} $为初等行变化矩阵。

差速里程计得到的导航坐标系下掘进机速度$ {\boldsymbol{V}}_{\rm{D}}^{\rm{n}} $与角速度$ {\boldsymbol{\omega }}_{\rm{D}}^{\rm{n}} $为

由速度${\boldsymbol{V}}_{\rm{D}}^{\rm{n}}$可得掘进机位置的微分方程：

式中：$ {\boldsymbol{P}}_{\rm{D}}^{\rm{n}} $为差速里程计得到的导航坐标系下掘进机地理坐标；L_D，λ_D，h_D分别为差速里程计得到的掘进机纬度、经度、高度；

由$ {\boldsymbol{\omega }}_{\rm{D}}^{\rm{n}} $可得掘进机的姿态角微分方程：

式中：$ {\boldsymbol{\varphi }}_{\rm{D}}^{\rm{n}} $为差速里程计得到的导航坐标系下掘进机姿态角；$ {\gamma _{\rm{D}}} $，$ {\theta _{\rm{D}}} $，$ {\psi _{\rm{D}}} $分别为差速里程计得到的横滚角、俯仰角、航向角。

3.   基于卡尔曼滤波的掘进机位姿数据融合

对于线性系统，卡尔曼滤波可以准确地更新系统的均方误差，对系统状态做出最优估计^[14-15]。根据卡尔曼滤波器原理，将捷联惯导与差速里程计位姿数据的差值作为卡尔曼滤波器的观测值，以卡尔曼滤波器的输出值作为捷联惯导误差的补偿值，对速度进行补偿后得到最终的掘进机定位结果。

3.1   捷联惯导误差方程

捷联惯导速度误差微分方程为

式中：$ \Delta {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} $为速度误差；${\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}}$为姿态误差；$ {{\boldsymbol{f}}^{\rm{n}}} $为导航坐标系下加速度计测得的比力；$ \Delta {{\boldsymbol{K}}_{\rm{A}}} $，$ \Delta {{\boldsymbol{\alpha }}_{\rm{A}}} $分别为加速度计的刻度系数误差和安装误差角；${{\boldsymbol{\delta}} ^{\rm{n}}}$为加速度计零偏误差。

由速度误差可得位置误差微分方程：

式中：${\Delta {\boldsymbol{P}}_{\rm{I}}^{\rm{n}}}$为位置误差；$ \Delta {L_{\rm{I}}} $，$ \Delta {\lambda _{\rm{I}}} $，$ \Delta {h_{\rm{I}}} $分别为纬度、经度、高度误差；$ \Delta {V_{\rm{N}}} $，$ \Delta {V_{\rm{E}}} $，$ \Delta {V_{\rm{U}}} $分别为北向、东向、天向速度误差。

根据四元数的微分方程可得姿态误差微分方程^[10]：

式中：$ {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} $为导航坐标系相对于地心坐标系的旋转角速度，$ {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} = {\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} + {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}} $；$ \Delta {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} $为$ {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} $的误差；$ \Delta {{\boldsymbol{K}}_{\rm{G}}} $，$ \Delta {{\boldsymbol{\alpha }}_{\rm{G}}} $分别为陀螺仪的刻度系数误差和安装误差角；$ {\boldsymbol{\omega }}_{{\rm{ib}}}^{\rm{b}} $为载体坐标系下掘进机相对于地心坐标系的旋转角速度；$ {{\boldsymbol{\varepsilon }}^{\rm{n}}} $为陀螺仪零偏误差。

3.2   差速里程计误差方程

设里程计工作的坐标系为m系，其$ x $，$ y $，$ {\textit{z}} $轴分别指向履带的右方、前方、上方。里程计安装时应使得m系与b系（载体坐标系）重合^[16]，且里程计只能得到履带y轴方向的速度（即履带线速度）。但由于里程计安装误差角${\boldsymbol{\alpha }} = {[{\alpha _\theta }\;\;\;{\alpha _\gamma }\;\;\;{\alpha _\psi }]^{\rm{T}}}$（$ {\alpha _\theta },{\alpha _\gamma },{\alpha _\psi } $分别为俯仰误差角、横滚误差角、航向误差角）的存在，m系与b系会存在旋转角^[17]，两坐标系的姿态转换矩阵为

式中I为单位矩阵。

考虑到里程计的刻度系数误差ΔK与安装误差角$ {\boldsymbol{\alpha }} $，里程计得到的履带实际速度为

单里程计扩展为差速里程计后，在导航坐标系下掘进机速度误差可表示为

其中

式中：ΔK_l，ΔK_r分别为左右里程计刻度系数误差；$ {\alpha _{\theta {\rm{l}}}} $，$ {\alpha _{\theta {\rm{r}}}} $分别为左右里程计的俯仰误差角；$ {\alpha _{\psi {\rm{l}}}} $，$ {\alpha _{\psi {\rm{r}}}} $分别为左右里程计的航向误差角。

由式（17）可知，里程计的横滚误差角并不影响差速里程计的精度。

3.3   卡尔曼滤波器设计

将捷联惯导和差速里程计的误差相关联，选取与掘进机位姿相关的18维向量${\boldsymbol{X}}$作为状态量：

以捷联惯导与差速里程计解算的速度与位姿之差作为卡尔曼滤波器的观测值：

建立卡尔曼滤波状态方程：

式中：${{\boldsymbol{X}}_k}$为$ k $时刻的状态量；$ {\boldsymbol{A}} $为状态转移矩阵；$ {{\boldsymbol{w}}_{k - 1}} $为$k-1 $时刻系统过程噪声；$ {{\boldsymbol{Z}}_k} $为$ k $时刻的观测值；$ {\boldsymbol{H}} $为量测矩阵；$ {{\boldsymbol{D}}_k} $为$ k $时刻的量测白噪声。

将状态方程离散化并通过以下步骤进行滤波^[18]。

（1） 计算状态量的先验估计值：

式中${\hat {\boldsymbol{X}}_{k - 1}}$为$ k - 1 $时刻状态量的后验估计值。

（2） 计算均方误差的先验估计值：

式中：$ {{\boldsymbol{S}}_{k - 1}} $为$ k - 1 $时刻的均方误差；${{\boldsymbol{N}}_{k - 1}}$为$ k - 1 $时刻的系统噪声协方差矩阵。

（3） 更新卡尔曼滤波增益：

式中$ {{\boldsymbol{J}}_k} $为$ k $时刻的量测噪声协方差矩阵。

（4） 计算状态量的后验估计值：

（5） 更新均方误差：

将后验估计值${\hat {\boldsymbol{X}}_k}$作为状态量的最优估计值。捷联惯导速度消去误差最优估计值得到所需速度，积分后便可得到掘进机位移。

3.4   里程计输出有效性判断

掘进机可能出现打滑状况，此时的里程计输出为无效输出，因此需判断里程计输出的有效性。里程计输出为有效输出时，卡尔曼滤波的新息${{\boldsymbol{r}}_k} = {{\boldsymbol{Z}}_k} - {\boldsymbol{H}}\hat {\boldsymbol{X}}_k^ -$满足以下正态分布^[19]：

记${{\boldsymbol{m}}_k} = {\boldsymbol{HS}}_k^ - {{\boldsymbol{H}}^{\rm{T}}} + {{\boldsymbol{J}}_k}$，可得自由度为1的${\;\chi ^2}$分布：

式中：${{{r}}_k}(u)$为$ {{\boldsymbol{r}}_k} $的第u个元素；${m_k}(u,u)$为${{\boldsymbol{m}}_k}$对角线上的第u个元素。

综合掘进机的实际运行环境和对误检率与漏检率的相关要求，选取阈值$ {\xi } $对新息进行检验。当满足下式时，判定新息为异常，里程计输出无效，反之判定新息正常，里程计输出有效。

隔离里程计无效输出的方法：对于异常的新息元素$ {{{r}}_k}(u) $，将其对应的量测噪声的元素设置为无穷大，从而使得卡尔曼滤波增益对应元素为0，在状态估计中便可依靠其他可信的观测量进行状态更新，隔离里程计的无效输出。

4.   实验验证

4.1   实验平台搭建

基于捷联惯导与差速里程计的掘进机组合定位实验平台设备主要包括EBZ160M−2掘锚机、FSON II捷联惯导、TS60全站仪、差速里程计、CH110捷联惯导。实验场地为封闭式模拟巷道，地面为钢制地板。模拟巷道可伸展，全部展开长度达32 m，宽度为4 m，高度为5.5 m。掘进机整机高度为2.5 m，长度为10.4 m，履带间距为1.1 m。全站仪得到的掘进机位置与FSON II捷联惯导得到的掘进机姿态角作为掘进机实际位姿数据，用于验证CH110捷联惯导与差速里程计组合定位的精度。捷联惯导参数见表1。

表  1  捷联惯导参数

Table  1.  Strapdown inertial navigation parameters

参数	CH110捷联惯导	FSON II捷联惯导
零偏稳定性/（°·h−1）	$ \leqslant 3.5 $	$ \leqslant 0.005 $
自寻北误差/（°）	$ \leqslant 0.1 $	$ \leqslant 0.01 $
水平对准精度/（°）	$ 0.1 $	$ 0.01 $
航向精度/（°）	$ \leqslant 0.4 $	$ \leqslant 0.02 $

下载: 导出CSV 

| 显示表格

设备安装如图4所示。以掘进机重心坐标作为整机位置，将CH110捷联惯导安装于掘进机上侧靠近中心的位置，以减小掘进机转动引起的速度误差。棱镜安装于CH110捷联惯导旁，以减小安装位置带来的误差，左右履带输出轴处各有１个里程计通过齿圈相连。全站仪观测装置部署于掘进机后方10 m处。

图  4  设备安装

Figure  4.  Equipment installation

下载: 全尺寸图片 幻灯片

一般情况下里程计安装误差角为小角度，若不满足小角度的条件，只需要首次标定即可使得安装误差角为小角度^[20]。标定后的里程计参数见表2。

表  2  里程计参数

Table  2.  Odometer parameters

参数	左里程计	右里程计
允许最高转速/（r·min−1）	6 000	6 000
分辨率/（P·R−1）	1 000	1 000
刻度系数/（m·P−1）	$ 2.904 \times {10^{ - 4}} $	$ 2.904 \times {10^{ - 4}} $
俯仰误差角/（°）	0.46	0.50
航向误差角/（°）	0.38	0.72

下载: 导出CSV 

| 显示表格

若捷联惯导安装误差角并非小角度，可借助FSON II捷联惯导对CH110捷联惯导进行标定。以CH110捷联惯导几何中心为原点建立坐标系（ch系），其$ x $，$ y $，$ {\textit{z}} $轴分别指向CH110捷联惯导的右方、前方、上方。掘进机静止时CH110捷联惯导相对于FSON II捷联惯导的姿态角差值为安装误差角${{\boldsymbol{\alpha }}_{{\text{ch}}}} = {[{\alpha _{\theta {\text{ch}}}}\;\;\;{\alpha _{\gamma {\text{ch}}}}\;\;\;{\alpha _{\psi {\text{ch}}}}]^{\rm{T}}}$，调整捷联惯导安装角度，使安装误差角为小角度。ch系相对于b系的姿态转换矩阵为

通过坐标转换矩阵$ {\boldsymbol{C}}_{{\rm{ch}}}^{\rm{b}} $可将CH110捷联惯导的位姿数据转换至b系，以消除安装误差角对定位的影响。

4.2   实验过程及结果分析

在相同路况下实验多次，并选取某一组实验数据进行分析。捷联惯导根据全站仪给出的起始点经纬度信息进行15 min的粗对准与精对准。对准后，捷联惯导计算得到初始的姿态转换矩阵。掘进机移动，捷联惯导开始对姿态转换矩阵及速度进行更新。同时，差速里程计获得掘进机左右履带速度，解算得到掘进机速度与航向角。卡尔曼滤波器对两者数据融合，并对捷联惯导误差进行校正与补偿。掘进机初始位姿数据见表3。

表  3  掘进机初始位姿数据

Table  3.  Initial position and posture data of roadheader

参数	值
航向角/（°）	−74.090 9
横滚角/（°）	0.348 1
俯仰角/（°）	−0.560 4
经度/（°）	118.602 0
纬度/（°）	37.745 0
高程/m	787.815 0
当地重力加速度/（m·s−2）	−9.797

下载: 导出CSV 

| 显示表格

操控掘进机从起点位置沿着目标路线行进，并控制掘进机在模拟巷道内完成前进、转向等动作。在掘进机前进一定距离后对其位姿进行调整并继续前进，最终操控掘进机行走至模拟煤壁处。实验持续250 s，掘进机行进东向位移9.51 m，北向位移1.72 m，航向角变化23.78°。

不同定位方法解算的掘进机航向角如图5所示。可看出随着时间增加，CH110捷联惯导得到的航向角与FSON II捷联惯导得到的实际航向角偏离度逐渐增加，而组合定位方法得到的航向角与实际值更吻合；组合定位方法得到的航向角误差最大为0.587 7°，平均为0.140 0°。

图  5  掘进机航向角

Figure  5.  Heading angle of roadheader

下载: 全尺寸图片 幻灯片

将掘进机的经纬度坐标转换为高斯坐标系下坐标并与初始坐标对比，得到掘进机东向位移与北向位移，如图6所示。将掘进机的东向位移与北向位移组合得到掘进机轨迹，如图7所示。

图  6  掘进机位移

Figure  6.  Displacement of roadheader

下载: 全尺寸图片 幻灯片

图  7  掘进机轨迹

Figure  7.  Track of roadheader

下载: 全尺寸图片 幻灯片

从图6、图7可看出，随着时间增加，CH110捷联惯导得到的掘进机位移误差逐渐增大，而组合定位方法得到的位移更接近全站仪得到的实际位移；组合定位方法得到的东向位移误差最大为0.182 1 m，平均为0.145 8 m，北向位移误差最大为0.110 8 m，平均为0.088 5 m。

5.   结论

（1） 综合利用捷联惯导无源性和里程计自主性的优点，提出了一种基于捷联惯导与差速里程计的掘进机组合定位方法。

（2） 根据捷联惯导工作原理，推导了捷联惯导的位姿更新方程与误差方程。根据航位推算原理，简化掘进机行走部模型，通过运动学分析，得到差速里程计的位姿更新方程与误差方程。基于捷联惯导和差速里程计的误差方程，设计了具有18维状态向量的卡尔曼滤波器，以捷联惯导与差速里程计得到的掘进机位姿数据之差作为卡尔曼滤波器的观测量，以卡尔曼滤波器输出值对捷联惯导数据进行校正与补偿。将里程计数据有效性判断融入组合定位方法中，避免了履带打滑对定位精度的影响。

（3） 在模拟巷道中进行了掘进机定位实验，结果表明：组合定位方法测得的掘进机航向角误差能够控制在0.6°以内，位置误差能够控制在0.19 m以内，可减小捷联惯导累计误差对掘进机定位的影响，具有较高的定位精度。