长期以来，煤炭一直是我国的主导能源。现行的煤炭开采方针为：采掘并重，掘进先行。煤矿巷道的快速掘进是保证矿井高产稳产的关键技术措施。然而，煤矿巷道掘进存在地质条件复杂、工艺装备落后、作业人员偏多、操作环境恶劣、采掘不平衡等问题，严重影响着煤矿的安全、高效、智能生产。长期的实践表明，要从根本上改变综掘工作面的落后状态，必须研发高效率、高可靠性、高智能的新型掘进设备[1,2,3,4,5]。

掘进机的智能化是实现综掘面智能化与无人化的前提。掘进机的实时位姿感知作为掘进机智能化亟待解决的技术难题之一，受到了国内外学者的广泛关注。捷联惯导系统(strapdown inertial navigation system, SINS)是一种不需要与外界交互，也不向外界辐射能量的自主式导航系统[6],SINS通过陀螺仪测得载体的角速度，通过加速度计测得载体的线加速度，对角速度与加速度积分便可得到载体的姿态与速度。然而，惯性导航技术一般应用于高速、短时、长距离飞行器导航定位中，而悬臂式掘进机是以低速、长时、短距的工作方式进行作业[7],而且由于其工作方式的特殊性，测量误差会随时间累计并最终影响整个系统的定位精度。为充分发挥捷联惯导自主性与无源性的优点，多位学者研究了适用于掘进机的惯导系统误差补偿方法。田原[8]提出了一种基于零速修正的掘进机惯性导航定位方法，该方法利用二次曲线拟合惯导的误差，从而修正惯导的测量误差。为减小掘进机工作过程产生的复杂震动对惯导系统的影响， SHEN Y等[9]分析了掘进机角震动与线振动源特性，建立了基于切削载荷的掘进机动力学模型，进而提出了一种基于掘进机振动特性的误差补偿方法，减小了惯导系统的圆锥与划桨误差。多传感器融合的定位方法可以充分发挥各种传感器的优点，弥补单个传感器的不足。为解决惯导误差随时间累计与单里程计位姿感知能力有限的问题，王浩然等[10]在捷联惯导系统基础上，引入差速里程计作为辅助定位系统，有效提高了惯导系统的位姿感知能力。同步定位与建图(simultaneous localization and mapping, SLAM)和机器视觉技术的发展为掘进机的定位导航提供了新思路。YANG J J等[11]提出了基于自耦合和Hector SLAM的巷道环境建模及掘进机定位方法，实现了巷道的建模与掘进机定位。WANG L X等[12]基于单目视觉测量原理，采用最小二乘法使捷联惯导与单目相机数据对齐，并通过扩展卡尔曼滤波融合两者的信息，实现了掘进机位姿的高精度感知。ZHANG W T等[13]采用基于RGB-D相机的SLAM方法，不仅实现了对掘进机位姿的感知，而且还能够实现巷道地图的构建。

基于机器视觉定位的方案因具有非接触测量和信息丰富直观的优势而被广泛应用于移动机器人的定位。捷联惯导适应性强，且具有自主性与无源性的优点，同样适用于巷道中掘进机的定位。因此，提出了一种捷联惯导与双目里程计融合的掘进机组合定位方法，设计了具有15维状态量的卡尔曼滤波器，对捷联惯导数据与双目里程计数据进行融合，最终得到掘进机的位姿信息。

1、掘进机组合定位导航原理

捷联惯导与双目里程计融合的掘进机定位方法是在捷联惯导定位系统的基础上，引入双目里程计的位姿信息，以修正捷联惯导的误差。组合定位系统的原理如图1所示，该组合定位系统由3部分组成：基于捷联惯导的位姿感知系统、基于双目里程计的位姿感知系统、基于卡尔曼滤波的数据融合模块。

捷联惯导随掘进机移动，陀螺仪与加速度计分别输出载体的角速度与线加速度，通过捷联更新算法得到位姿。双目里程计与掘进机固连并利用捷联惯导的数据初始化位姿。随着掘进机运动，双目相机拍摄连续图像，并对图像进行特征点提取与匹配，通过运动估计的方法更新掘进机的位姿。

捷联惯导与双目里程计均可输出掘进机的位姿信息，因此为了不改变捷联惯导的系统特性，根据捷联惯导的误差方程设计卡尔曼滤波器，以捷联惯导与双目里程计位姿数据之差作为滤波器观测量，采用输出校正的方法对捷联惯导进行校正。

组合定位系统用到的坐标系如下：

导航坐标系(n系)—Onxnynzn:原点位于载体中心；x轴指向地理东向；y轴指向地理北向；z轴指向天向。

载体坐标系(b系)—Obxbybzb:原点位于载体中心；x轴指向载体右向；y轴指向载体前向；z轴指向载体上方。

相机坐标系(c系)—Ocxcyczc:原点位于双目相机左相机的光心；x轴指向右；y轴指向下；z轴指向前。

地心坐标系(i系)—Oixiyizi:原点位于地心；x轴指向春分点；y轴指向北极；z轴与其余两轴构成右手坐标系。

地球坐标系(e系)—Oexeyeze:原点位于地心；其中x轴指向本初子午线与赤道的交点；y轴指向北极；z轴与其余两轴构成右手坐标系。

图1 组合定位原理   

2、基于双目里程计的位姿感知

单目相机存在尺度不确定的问题，因此单目相机在位姿估计时会存在严重耦合，而双目相机测量精度高，测量范围大，且具有较高的稳定性，因此，本方法采用双目相机对掘进机进行位姿估计。

图2为双目相机模型，分别以左相机光心Ol与右相机光心Or建立左相机Olxlylzl坐标系与右相机坐标系Orxryrzr,两坐标系x轴均指向右，z轴指向前方，两坐标系均为右手系；以像素平面的原点建立相像素坐标系，u轴指向右，v轴指向下；pl与pr分别为点p的像在左右像素平面中的位置；xl与xr分别为该特征点在左右相机坐标系中的横坐标；相机坐标系Ocxcyczc与左相机坐标系重合。

图2 双目里程计位姿感知原理   

在位姿感知初始化阶段，首先对左目相机获取的图像进行特征点提取，然后对左右目相机在同一时刻获取的图像进行特征点匹配。双目相机经标定之后可获得其基线b与焦距f,根据相似三角形原理，匹配点的深度zc可用下式求解：

在获取尺度信息之后，根据特征点在左目相机的像素坐标(u,v)便可求得点p在相机坐标系下的位置(xc,yc,zc)。

式中，K为相机的内参矩阵。

由于相机与掘进机固连，因此需得到导航坐标系与相机坐标系的转换关系才能够对掘进机的位姿进行求解。在某一时刻，特征点坐标在导航坐标系与相机坐标系下的关系可用下式表示[14]。

其中，Τk=[ Rktk0 1 ]

为k时刻相机的外参矩阵。根据外参矩阵，可求得相机相导航坐标系下的姿态：

式中，Rk,ij为矩阵Rk的第i行j列的元素；φnc表示相机坐标系相对于导航坐标系的姿态；θc、γc、ψc分别为双目里程计得到的俯仰角、横滚角与航向角(角标c表示该数据为双目里程计得到的位姿数据)。

掘进机在导航坐标系下的位置可表示如下：

Pnc=-R-1kt (5)

相机的外参矩阵不同于内参矩阵，内参矩阵为标定后获得，并且不会在使用中变化，外参矩阵会随着相机的运动而变化，因此位姿更新的实质便是外参矩阵Tk的更新。

在双目相机获得最新一帧的图像后，通过特征点匹配法跟踪前一帧图像中的特征点，得到特征点之间的对应关系。对于匹配好的点集P={p1,…,pn}与P′={p′1,…,p′n},需构建如下最小二乘问题，求使得误差平方和J达到极小的Rk+1,k与tk+1,k[15]。

Jmin=minR,t

得到Rk+1,k与tk+1,k之后，外参矩阵通过式(7)进行更新。根据式(4)与式(5)便可获得掘进机在当前时刻的位姿。

Tk+1=Tk+1,kTk (7)

3、基于捷联惯导的掘进机位姿感知

3.1 姿态更新

掘进机在运动过程中，姿态不断变化，组合定位要确定的是导航坐标系下的掘进机位姿，因此需将载体坐标系下的位姿信息通过姿态转换矩阵投影至导航坐标系。

两坐标系的空间角位置关系可理解成刚体的定点转动，这种转动关系可用规范化四元数Q=q0+q1i+q2j+q3k(q0,q1,q2,q3为实数，i,j,k为虚数单位)描述。

第i行第j列的元素。姿态矩阵的更新可转换为四元数的更新，根据四元数的微分方程，采用等效旋转矢量法[16]对四元数更新：

Q(k+1)=Q(k)⨂Q(h) (9)

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

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

式中，φnΙ为捷联惯导得到的掘进机姿态(角标I表示该数据由捷联惯导系统得到的位姿数据)。

3.2 速度与位置更新

导航坐标系下，捷联惯导的速度微分方程[17]:

⋅

为捷联惯导得到的导航坐标系下的掘进机速度，V

在东向、北向、天向的分量；fb为比力；ω

为地球坐标系相对于地心坐标系的旋转角速度；ω

为导航坐标系相对于地球坐标系相的旋转角速度；gn当地重力加速度。捷联惯导的位置微分方程为：

为捷联惯导得到的掘进机的位置；LI、λI、hI分别为捷联惯导得到的掘进机纬度、经度、高度；RM与RN分别为掘进机所在地理位置的子午圈与卯酉圈曲率半径。

4、基于卡尔曼滤波的位姿信息融合

4.1 捷联惯导误差方程

捷联惯导速度误差方程为[18]:

为载体坐标系下捷联惯导相对于地心坐标系的旋转角速度；εn为陀螺仪零偏误差。

4.2 卡尔曼滤波器设计

根据捷联惯导的误差方程，选取与掘进机位姿相关的15维向量x作为状态量：

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

建立离散化状态方程与观测方程：

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

卡尔曼滤波器通过以下步骤进行更新估计值[19]:

(1) 更新状态先验估计值

(2) 更新均方误差先验估计值S-k:

S-k=ASk-1AT+Nk-1 (20)

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

(3) 更新卡尔曼滤波增益Ek:

Ek=S-kHT(HS-kHT+Jk)-1 (21)

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

(4) 更新状态量后验估计值

(5) 更新均方误差Sk:

Sk=(I-EkH)S-k (23)

将后验估计值为状态量的最优估计值。捷联惯导系统根据估计值对误差进行修正，进而得到掘进机的位姿信息。

5、定位试验

捷联惯导与双目里程计融合的掘进机组合定位试验平台主要包括EBZ160M-2掘进机、FSON Ⅱ九轴捷联惯导、全站仪(TS60)、CH110捷联惯导、Realsense D455相机。试验场地为封闭式模拟巷道，掘进机整机高度为2.5 m, 长度为10.4 m, 履带间距为1.1 m。

CH110是一种低精度的捷联惯导。FSON Ⅱ为高精度九轴捷联惯导，可以进行精确对准，并将对准后的姿态作为组合定位系统的初始姿态。在定位试验中，由全站仪得到的掘进机位置数据与FSON Ⅱ的姿态数据为掘进机的真实位姿数据，用于验证组合定位方法的精度。Realsense D455相机左右各有一相机拍摄周围环境图像，用于双目里程计的位姿解算。

试验平台如图3所示。在相同路况下试验多次，并随机选取一组试验数据进行分析。捷联惯导根据全站仪提供的起始点经纬度信息进行对准。对准后，捷联惯导计算得到初始的位姿。掘进机移动，捷联惯导开始更新姿态转换矩阵、速度以及位置。同时，双目里程计根据初始的位姿信息更新掘进机位姿。卡尔曼滤波器对两者数据融合，并对捷联惯导误差进行校正。

图3 试验平台   

图4 位姿感知曲线   

在组合定位试验中，分别用CH110与组合定位方法对掘进机的位姿进行感知。将掘进机的经纬度坐标转换为高斯坐标系下的坐标，并与初始位置对比，得到掘进机的北向位移、东向位移与天向位移。不同定位方法得到的掘进机北向、东向与天向位移分别如图4(a)、图4(b)、图4(c)所示；横滚角、航向角与俯仰角分别如图4(d)、图4(e)、图4(f)所示。从图4中可以看出，由于掘进机震动和外部因素的影响，CH110感知的数据波动较大，而由捷联惯导与双目里程计融合的组合定位方法所解算的位姿曲线更加平滑，组合定位方法得到的位姿数据与实际值更加吻合，偏离度更小，说明组合定位方法能够有效减小噪声的影响。对位姿感知误差进行分析，结果见表1。由表1可知，东向位移的平均误差与标准差的绝对值均比其他两轴的大，这是因为掘进机在东向移动的距离较长。同样的，掘进机在行进过程中，由于捷联惯导短期定位的特性，这种效应在惯导位姿感知上体现的更加明显，掘进机在75 s和180 s时刻的转弯动作使得捷联惯导的误差有了显著的增加，而组合定位方法能够有效地限制这种误差。

通过分析对比组合定位位姿数据、捷联惯导位姿数据与真实位姿数据可知，捷联惯导与双目里程计融合的掘进机组合定位方法能够有效减小捷联惯导累计误差的影响，实现掘进机的高精度定位。

表1 位姿误差

6、结论

(1) 为解决捷联惯导用于掘进机定位存在累计误差的问题，提出了一种捷联惯导与双目里程计融合的掘进机组合定位方法，在捷联惯导系统的基础上，引入双目里程计作为辅助系统对掘进机进行位姿感知。

(2) 分析了双目里程计的位姿感知原理，推导了双目里程计的姿态与位置更新方程，根据捷联惯导的工作原理，推导了捷联惯导的姿态、速度与位置更新方程。基于捷联惯导的误差方程，设计了具有15维状态量的卡尔曼滤波器，以捷联惯导与双目里程计的位置与姿态之差作为滤波器观测值，并以输出校正的方式对捷联惯导的误差进行校正。

(3) 为验证提出的组合定位方法的精度，于悬臂式掘进机上搭建试验平台，并设计了组合定位试验，组合定位在北向、东向与天向的位移最大误差分别为3.39 cm、31.15 cm与1.65 cm, 横滚角、航向角与俯仰角的最大误差分别为0.1356°、0.8290°与1.2200°。试验结果表明，本文提出的组合定位方法能够减小捷联惯导累计误差的影响，实现掘进机的高精度定位。

参考文献:

[5]谢和平,吴立新,郑德志.2025年中国能源消费及煤炭需求预测[J].煤炭学报,2019,44(7):1949-1960.

[6]张旭辉,刘永伟,毛清华,等.煤矿悬臂式掘进机智能控制技术研究及进展[J].重型机械,2018(2):22-27.

[7]田伟琴,田原,贾曲,等.悬臂式掘进机导航技术研究现状及发展趋势[J].煤炭科学技术,2022,50(3):267-274.

[8]田原.基于零速修正的掘进机惯性导航定位方法[J].工矿自动化,2019,45(8):70-73.

[10]王浩然,王宏伟,李正龙,等.基于捷联惯导与差速里程计的掘进机组合定位方法[J].工矿自动化,2022,48(9):148-156.

[14]张超.悬臂式掘进机双目视觉与捷联惯导组合定位技术研究[D].西安:西安科技大学,2020:19-21.

[16]严恭敏,李思锦,郭正东.一种求解等效旋转矢量高阶误差补偿系数的新方法[J].中国惯性技术学报,2020,28(3):347-352+396.

[17]秦永元.惯性导航(2版)[M].北京:科学出版社,2020:290-298.

[18]刘鹏飞.里程计辅助的高精度车载GNSS/INS组合导航系统[J].光学精密工程,2020,28(4):979-987.

[19]严恭敏,翁浚.捷联惯导算法与组合导航原理(2版)[M].西安:西北工业大学出版社,2020:139-140.

基金资助:山西省科技计划揭榜招标项目(20201101005,20201101008);

文章来源:金铃子,王浩然,李正龙.捷联惯导与双目里程计融合的掘进机组合定位方法[J].矿业研究与开发,2023,43(12):171-177.