基于捷联惯导与差速里程计的掘进机组合定位方法

王浩然，王宏伟，3，4，李正龙，付翔

（1. 太原理工大学 安全与应急管理工程学院， 山西 太原 030024；2. 太原理工大学 山西省煤矿智能装备工程研究中心， 山西 太原 030024；3. 太原理工大学 机械与运载工程学院， 山西 太原 030024；4. 山西焦煤集团有限责任公司 博士后工作站， 山西 太原 030024；5. 太原理工大学 矿业工程学院， 山西 太原 030024）

0 引言

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

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

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

1 掘进机组合定位原理

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

图1 基于捷联惯导与差速里程计的掘进机组合定位原理Fig. 1 Combined positioning principle of roadheader based on strapdown inertial navigation and differential odometer

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

2 掘进机位姿数据测量

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

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

（1） 地心坐标系——oixiyizi。地心坐标系以地心为坐标原点，oixi轴 指向春分点，oizi轴沿地球自转轴指向北极，oiyi轴与其余两轴构成右手坐标系。在捷联惯导系统中，陀螺仪与加速度计的输出均以此为参考系。

（2） 地球坐标系——oexeyeze。地球坐标系以地球中心为坐标原点，oexe与oeye均在地球赤道平面内，其中oexe轴 指向本初子午线与赤道的交点，oeye轴沿地球自转轴指向北极，oeze轴与其余两轴构成右手坐标系。

（3） 导航坐标系——onxnynzn。导航坐标系以掘进机重心为坐标原点，onxn轴 指向地理东向，onyn轴指向地理北向，onzn轴与其余两轴构成右手坐标系。导航坐标系用来表示组合定位结果。导航坐标系相对于地球坐标系的位置可用掘进机的地理位置（纬度L、经度λ、高度h）表示。

（4） 载体坐标系——obxbybzb。载体坐标系以掘进机重心为坐标原点，obxb轴与obyb轴分别指向载体的右方与前方，obzb指向天并与其余两轴构成右手坐标系。

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

图2 掘进机姿态角Fig. 2 Attitude angle of roadheader

2.2 捷联惯导

2.2.1 姿态更新

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

姿态更新主要有方向余弦法、四元数法、欧拉角法，其中四元数法算法简单、易于操作、实用性强[11]。规范化四元数Q=[q0q1q2q3]T（q0-q3为实数）能够描述刚体绕定轴旋转运动的过程。载体坐标系至导航坐标系某一时刻的姿态转换矩阵可用一组四元数表示：

式中：Tij（i,j=1,2,3）为姿态转换矩阵Cnb第i行第j列的元素。

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

式中：Q(k+1)，Q(k)分别为k+1和k时刻的姿态四元数；⊗为四元数乘法；Q(h)为由k至k+1时刻的姿态变化四元数。

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

2.2.2 速度与位置更新

通过将掘进机加速度投影至导航坐标系下，得到掘进机速度微分方程：

式中：为捷联惯导得到的导航坐标系下掘进机速度，VIn=[VEVNVU]T，VE，VN，VU分别为VIn在东向、北向、天向的分量；fb为载体坐标系下加速度计测得的比力；为地球坐标系相对于地心坐标系的旋转角速度；为导航坐标系相对于地球坐标系的旋转角速度，分别为掘进机所在地理位置的子午圈与卯酉圈曲率半径；gn为导航坐标系下重力加速度。

式（4）表明，只有在加速度计输出中去除有害加速度a=(2ωnie+ωnen)×VIn-gn，才能得到掘进机运动的加速度。

掘进机静止时，=0，捷联惯导据此初对准，得到初始的姿态转换矩阵。基于速度方程，捷联惯导的位置微分方程为

式中：为捷联惯导得到的导航坐标系下掘进机地理坐标；LI，λI，hI分别为捷联惯导得到的掘进机纬度、经度、高度。

2.3 差速里程计

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

图3 掘进机行走部简化模型Fig. 3 Simplified model of roadheader walking section

基于简化模型可得

掘进机两侧各有1个里程计对履带测速，左右里程计输出分别为根据航位推算原理，掘进机线速度与角速度可由履带速度推得：

式中I23为初等行变化矩阵。

差速里程计得到的导航坐标系下掘进机速度VDn与角速度ωnD为

由速度VDn可 得掘进机位置的微分方程：

式中：PnD为 差速里程计得到的导航坐标系下掘进机地理坐标；LD，λD，hD分别为差速里程计得到的掘进机纬度、经度、高度；

由ωnD可得掘进机的姿态角微分方程：

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

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

3.1 捷联惯导误差方程

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

式中： ΔVIn为速度误差；为姿态误差；fn为导航坐标系下加速度计测得的比力；ΔKA，ΔαA分别为加速度计的刻度系数误差和安装误差角；δn为加速度计零偏误差。

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

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

3.2 差速里程计误差方程

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

式中I为单位矩阵。

考虑到里程计的刻度系数误差ΔK与安装误差角α，里程计得到的履带实际速度为

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

其中

式中：ΔKl，ΔKr分别为左右里程计刻度系数误差；αθl，αθr分 别为左右里程计的俯仰误差角；αψl，αψr分别为左右里程计的航向误差角。

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

3.3 卡尔曼滤波器设计

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

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

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

式中：Xk为k时刻的状态量；A为状态转移矩阵；wk-1为k-1时 刻系统过程噪声；Zk为k时刻的观测值；H为量测矩阵；Dk为k时刻的量测白噪声。

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

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

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

式中：Sk-1为k-1时 刻的均方误差；Nk-1为k-1时刻的系统噪声协方差矩阵。

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

式中Jk为k时刻的量测噪声协方差矩阵。

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

（5） 更新均方误差：

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

3.4 里程计输出有效性判断

掘进机可能出现打滑状况，此时的里程计输出为无效输出，因此需判断里程计输出的有效性。里程计输出为有效输出时，卡尔曼滤波的新息rk=满足以下正态分布[19]：

记mk=HS-k HT+Jk， 可得自由度为1的χ2分布：

式中：rk(u)为rk的第u个元素；mk(u,u)为mk对角线上的第u个元素。

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

隔离里程计无效输出的方法：对于异常的新息元素rk(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

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

图4 设备安装Fig. 4 Equipment installation

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

表2 里程计参数Table 2 Odometer parameters

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

通过坐标转换矩阵可将CH110捷联惯导的位姿数据转换至b系，以消除安装误差角对定位的影响。

4.2 实验过程及结果分析

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

表3 掘进机初始位姿数据Table 3 Initial position and posture data of roadheader

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

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

图5 掘进机航向角Fig. 5 Heading angle of roadheader

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

图6 掘进机位移Fig. 6 Displacement of roadheader

图7 掘进机轨迹Fig. 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以内，可减小捷联惯导累计误差对掘进机定位的影响，具有较高的定位精度。