高级检索

基于视觉与惯导的掘进机组合定位方法

万继成, 张旭辉, 杨文娟, 张超, 董征, 杜昱阳, 雷孟宇, 陈鑫, 杨骏豪

万继成,张旭辉,杨文娟,等. 基于视觉与惯导的掘进机组合定位方法[J]. 煤炭科学技术,2025,53(6):456−467. DOI: 10.12438/cst.2024-0335
引用本文: 万继成,张旭辉,杨文娟,等. 基于视觉与惯导的掘进机组合定位方法[J]. 煤炭科学技术,2025,53(6):456−467. DOI: 10.12438/cst.2024-0335
WAN Jicheng,ZHANG Xuhui,YANG Wenjuan,et al. Combined localization method of roadheader based on vision and inertial navigation[J]. Coal Science and Technology,2025,53(6):456−467. DOI: 10.12438/cst.2024-0335
Citation: WAN Jicheng,ZHANG Xuhui,YANG Wenjuan,et al. Combined localization method of roadheader based on vision and inertial navigation[J]. Coal Science and Technology,2025,53(6):456−467. DOI: 10.12438/cst.2024-0335

基于视觉与惯导的掘进机组合定位方法

基金项目: 

国家自然科学基金资助项目(52104166);陕西省重点研发计划资助项目(2023-YBGY-063 );陕煤联合基金资助项目(2021JLM-03)

详细信息
    作者简介:

    万继成: (1997—),男,陕西凤翔人,博士研究生。E-mail:20105016013@stu.xust.edu.cn

    通讯作者:

    张旭辉: (1972—),男,陕西凤翔人,教授,博士生导师,博士。E-mail:zhangxh@xust.edu.cn

  • 中图分类号: TD421.5

Combined localization method of roadheader based on vision and inertial navigation

  • 摘要:

    针对煤矿井下掘进机单一位姿测量方法导致的掘进机位姿精度低、测量稳定性差的问题,提出了一种基于视觉和光纤惯导的掘进机组合定位方法。以固定在巷道顶板上的三激光指向仪作为特征目标,构建了基于三激光标靶的掘进机机身位姿视觉测量系统,采用机载防爆相机采集激光标靶,通过图像处理得到激光指向仪光斑中心位置,结合掘进机机身位姿视觉测量模型及空间坐标转化得到掘进机位姿数据。同时利用安装在掘进机机身上的光纤惯导实时获得掘进机的位置和姿态信息,采用拉格朗日三点插值法将视觉位姿数据和惯导位姿数据在时间上进行配准处理后,运用卡尔曼滤波融合算法对视觉测量数据和光纤惯导数据进行融合。采用两者融合后的位姿数据对惯导累积误差进行补偿校正,对测量系统传感器的输出有效性进行判断,消除了惯导长时间积分运算导致的累积误差及视觉标靶短时遮挡、数据丢失对融合结果的影响,从而获得掘进机实时精确位姿数据。在实验室搭建实验平台,对视觉定位方法及组合定位方法进行评估,在60 m测量距离内,视觉定位最大位置误差在35 mm以内,最大姿态角误差在0.5°以内,组合定位位置误差在30 mm以内,定位结果更接近真实位移。同时在煤矿井下工业现场对掘进机视觉与惯导融合定位进行了试验,结果表明:视觉与惯导融合定位方法能够克服井下复杂工况影响,弥补了单一测量方法的不足,且掘进机机身定位误差在30 mm以内,姿态角误差在0.5°以内,满足巷道施工精度要求。

    Abstract:

    Aiming at the problem of low pose accuracy and poor measurement stability of roadheader caused by single position measurement method of roadheader in coal mine, a combined positioning method of roadheader based on machine vision and optical fiber inertial navigation is proposed. Taking the three-laser pointer fixed on the roof of the roadway as the characteristic target, a visual measurement system of the body pose of the roadheader based on the three-laser target is constructed. The airborne explosion-proof camera is used to collect the laser target, and the spot center position of the laser pointer is obtained by image processing. Combined with the visual measurement model of the body pose of the roadheader and the spatial coordinate transformation, the pose data of the roadheader are obtained. At the same time, the position and attitude information of the roadheader are obtained in real time by using the fiber optic inertial navigation installed on the roadheader fuselage. The Lagrange three-point interpolation method is used to register the visual pose data and the inertial pose data in time. After processing, the Kalman filter fusion algorithm is used to fuse the visual measurement data and the fiber optic inertial navigation data. The pose data after the fusion of the two is used to compensate and correct the cumulative error of the inertial navigation, and the output validity of the sensor of the measurement system is judged, which eliminates the cumulative error caused by the long-term integration operation of the inertial navigation and the short-term occlusion of the visual target and the influence of data loss on the fusion result, so as to obtain the real-time accurate pose data of the roadheader. The visual measurement platform of roadheader pose is built in the laboratory to evaluate the visual positioning method. Within the measurement distance of 60 m, the maximum position error is within 35 mm, and the maximum attitude angle error is within 0.5°. At the same time, the fusion positioning of vision and inertial navigation of roadheader was tested in the industrial field of underground coal mine. The results show that the fusion positioning method of vision and inertial navigation can overcome the influence of complex working conditions in underground mine and make up for the deficiency of single measurement method. The positioning error of roadheader body is within 30 mm, and the attitude angle error is within 0.5°, which meets the requirements of roadway construction accuracy.

  • 煤矿井下巷道掘进作为煤炭开采的重要环节,相比于综采工作面工艺复杂、智能化水平较低,易造成“采掘失衡”,严重影响煤矿开采效率[1-3]。因此提高巷道掘进装备的智能截割、定向掘进及协同控制等智能化水平,是解决该问题的重要途径,关键在于解决掘进装备的位姿测量及精确定位。

    针对复杂工况下掘进机的位姿测量,国内外研究学者做了大量研究,主要包括惯性导航方法、超宽带定位方法、全站仪定位方法、iGPS定位方法、视觉定位方法等方面。惯性定位方法[4-5]具有无源性、自主测量性强、环境适应性高的特点,能够在较短时间为掘进机提供较高精度的位姿数据,但受掘进机运动特性(慢速往复运动、作业时间长、剧烈振动)导致积分运算误差增大,位姿发散严重。超宽带定位方法[6-8]利用站点之间往返的超短脉冲信号实现测距定位,在有效范围内能够提供较高精度位姿信息,但需要多个基站配合,井下空间受限、设备多,信号易受遮挡衰减,测量稳定性需进一步提升。全站仪定位方法[9-10]多用于盾构机的作业定位具有较高定位精度,但煤矿井下巷道环境复杂、高粉尘水雾、空间狭小,全站仪发射的光路很容易被遮挡,无法实时获得掘进机的位置信息。如要获取掘进机的位姿信息必须在机身上安装多个棱镜通过几何关系得到掘进机的位姿信息,无法实现自主测量。iGPS定位方法[11-12]具有较高的实时定位能力,但需要合适的基站部署和精确的信号测量,以确保定位精度。但受粉尘、遮挡等因素,影响信号传播,导致总体定位精度不高,局限性较大。视觉定位方法[13-15]具有系统简单、成本低、非接触无累积误差等特点,多用于掘进设备的局部与全局定位,通常采用人为构造的几何特征标靶作为视觉识别目标,构建基于单目或多目位姿视觉解算模型,实现掘进装备的高精度位姿测量。但易受到巷道顶底板起伏、非结构化受限环境干扰等出现标靶遮挡造成视觉失效。

    基于上述分析,单一的测量方式在复杂、恶劣的井下巷道环境下面临累积误差、性能退化、设备干扰、遮挡失效等限制,很难提供实时、稳定的掘进机位姿信息,因此融合多传感器信息的掘进机组合定位方法成为热点。LI等[16]采用紧耦合的方式将惯导和UWB系统组合,试验验证了该方法能够减小系统非视距误差,提高掘进机定位精度。张旭辉等[17]、马宏伟等[18]采用惯导与全站仪组合定位的方法对掘进机位姿进行长距离测量,依靠全站仪精确的位置输出对惯导补偿在一定程度上提高了掘进机的定位精度及自主性。HAN等[19]、王浩然等[20]、刘送永等[21]在惯导定位的基础上结合里程计输出的速度位置信息,采用滤波算法对两者数据进行融合处理实现掘进机的位姿测量,但里程计大都安装在掘进机履带上,受巷道底板环境影响履带容易出现打滑,导致里程计输出非掘进机实际运动状态。毛清华等[22]、张超等[23]、黄东等[24]利用惯导与视觉测量技术融合对掘进机实时定位,区别在于一般采用单/多目视觉、不同的视觉位姿测量模型或基于视觉环境信息(颜色、纹理等)的视觉测量系统位姿输出,以适应井下低照度、粉尘、多干扰等复杂环境。上述煤矿掘进装备组合定位大都采用“惯导+”的组合方式,借助惯导短时定位精度高、自主性强的特点,但受限于长时定位发散的缺点,综合其他传感器优势有效改善了惯性累积误差,提升了系统整体定位性能。

    因此,结合团队前期研究基础,提出一种基于视觉与光纤惯导的掘进机组合定位方法,提高复杂环境下掘进机位姿测量可靠性。以三激光指向仪为特征的视觉测量系统、光纤惯导构建掘进机组合定位方案并阐述组合定位原理。介绍了视觉定位与惯导定位方法,通过卡尔曼滤波算法将惯导数据与视觉测量系统位姿数据进行融合实现掘进机的精确定位定向,为实现掘进机智能控制提供数据基础。

    掘进机组合定位系统组成如图1所示,主要由悬臂式掘进机、防爆相机、三激光指向仪标靶、光纤惯导和防爆计算机组成。光纤惯导和防爆计算机安装在导航控制箱内;三激光指向仪标靶通过安装架固定连接构成三角形安装在掘进机后方巷道顶板上朝向巷道掘进方向;防爆相机与掘进机机身刚性连接朝后拍摄三激光指向仪标靶;光纤惯导安装在防爆电控箱内;防爆计算机用来图像处理及数据解算。

    图  1  掘进机组合定位系统组成
    Figure  1.  Composition of roadheader combined localization system

    掘进机组合定位原理如图2所示,采用单目视觉测量方法,利用机载防爆相机采集三激光标靶图像,通过图像预处理、形态学变换、特征点提取得到相机坐标系下3个激光光斑的中心坐标,建立基于三点特征的掘进机位姿测量模型,结合矿用全站仪外参标定结果,得到了掘进机机身位姿信息。

    图  2  掘进机组合定位原理
    Figure  2.  Principle of combined localization of roadheader

    光纤惯导通过自身的数学解算平台实时获取陀螺仪和加速度计采集的地球自转角速度分量、重力加速度分量,以惯性坐标系为参考坐标系,在惯性坐标系中测量出载体的角运动和线运动历程,以及重力加速度的方向变化情况,初始标定对准后精确估算出初始姿态矩阵,通过积分运算得到掘进机位置和姿态。融合前对数据进行有效值判断及异常值剔除后,采用卡尔曼滤波算法对这两者数据进行融合,实时输出掘进机的精准位姿信息,并利用融合后的位姿信息对惯导进行校正,实现掘进机的精确定位定向。

    为了实现掘进机位姿测量,笔者建立如图3所示坐标系。

    图  3  掘进机组合定位系统坐标系定义
    Figure  3.  Coordinate system definition of roadheader combined localization system

    1)导航坐标系${o_{\mathrm{g}}}{x_{\mathrm{g}}}{y_{\mathrm{g}}}{z_{\mathrm{g}}}$。以掘进机重心为导航坐标系坐标原点${o_{\mathrm{g}}}$,${x_{\mathrm{g}}}$轴指向地理的东向,${y_{\mathrm{g}}}$轴指向地理北向,${z_{\mathrm{g}}}$轴与$ {x}_{{\mathrm{g}}}、{y}_{{\mathrm{g}}} $轴构成右手坐标系。导航坐标系用来表示惯导定位结果。

    2)载体坐标系${o_{\mathrm{ri}}}{x_{\mathrm{ri}}}{y_{\mathrm{ri}}}{z_{\mathrm{ri}}}$。以掘进机重心为坐标原点${o_{\mathrm{ri}}}$,${x_{\mathrm{ri}}}$轴指向掘进机的左方,${y_{\mathrm{ri}}}$轴指向掘进机的前方,${z_{\mathrm{ri}}}$指向天与$ {x}_{{\mathrm{ri}}}、{y}_{{\mathrm{ri}}} $两轴构成右手坐标系。

    3)相机坐标系${o}{x}{y}{z}$。相机坐标系以相机光心为坐标原点${o}$,${x}$轴指向相机右方,${y}$轴指向相机下方,${z}$轴指向相机前方。

    4)巷道坐标系${o_{\mathrm{la}}}{x_{\mathrm{la}}}{y_{\mathrm{la}}}{z_{\mathrm{la}}}$。以巷道中线起点为坐标原点${o_{\mathrm{la}}}$,${x_{\mathrm{la}}}$轴指向巷道掘进方向的右方,${y_{\mathrm{la}}}$轴与巷道中线重合指向巷道掘进方向,${z_{\mathrm{la}}}$轴与$ {x}_{{\mathrm{la}}}、{y}_{{\mathrm{la}}} $垂直指向巷道顶板方向。为了更好的表征掘进机的运动状态,本文将掘进机的位姿信息均转化到了巷道坐标系。

    5)地理坐标系${o_{\mathrm{ge}}}{x_{\mathrm{ge}}}{y_{\mathrm{ge}}}{z_{\mathrm{ge}}}$。地理坐标系以地球中心为坐标原点${o_{\mathrm{ge}}}$。${o_{\mathrm{ge}}}{x_{\mathrm{ge}}}$轴指向本初子午线与赤道的交点,${o_{\mathrm{ge}}}{y_{\mathrm{ge}}}$轴沿地球自转轴指向北极,${o_{\mathrm{ge}}}{z_{\mathrm{ge}}}$轴与其余两轴构成右手坐标系。

    传统的光斑中心提取算法有灰度质心法[25]、Hough变换法[26]、高斯曲面拟合算法[27]等,这些算法在特定环境下能够有效的提取光斑中心,但煤矿井下掘进工作面存在粉尘、水雾、非平稳噪声,伴有矿灯、巷道照明灯、地测用激光指向仪等干扰光源,环境背景复杂,难以稳定、高精度的提取激光光斑中心。

    考虑井下复杂干扰因素影响及激光指向仪的发光颜色,选用彩色防爆相机进行图像采集,对采集到的彩色图像经过图像处理后利用高斯曲面拟合确定激光光斑中心。根据激光指向仪光斑的HSV颜色空间即色度H、饱和度S、明度V作为颜色空间分量,通过设置参数范围可有效地滤除矿灯、照明灯等其他颜色杂光,区分出激光光斑的像素信息。闭运算操作将图像上光斑的区域边界向光斑中心填充,增大边界区域使光斑边界连续并标记出光斑位置。利用高斯曲面拟合方法得到图像上各区域的光斑中心,再通过求取各光斑之间的距离及比例,可以去除剩余与激光光斑特征不一致的杂光。激光光斑中心定位流程如图4所示,具体的光斑中心定位步骤:① 相机采集彩色光斑图像通过畸变矫正、高斯滤波等预处理后,遍历图像所有像素信息通过设置图像HSV参数,H的范围为[0,180],S的范围为[0,255],V的范围为[0,255],可以得到满足色度H在[60,120],饱和度S在[150,255],明度V在[30,180]的所有光斑$\sum {h\left( {i,j} \right)} $。② 对所有光斑$\sum {h\left( {i,j} \right)} $进行轮廓提取,去除大于激光光斑轮廓的杂光轮廓,剩余包括激光光斑在内的区域$\sum {g\left( {i,j} \right)} $。③ 对所有剩余光斑$\sum {g\left( {i,j} \right)} $进行闭运算使光斑区域连续平滑。④ 再利用高斯曲面拟合方法得到图像区域所有光斑的中心位置。⑤ 求取各光斑之间的距离D及比例S,保存满足给定的距离范围$\left( {{d_{\min }} \sim {d_{\max }}} \right)$及比例范围$\left( {{s_{\min }} \sim {s_{\max }}} \right)$光斑坐标,即激光指向仪光斑中心坐标${O_{p_1}}\left( {i,j} \right)$、${O_{p_2}}\left( {i,j} \right)$、${O_{p_3}}\left( {i,j} \right)$。在杂光背景下提取的激光光斑效果图如图5所示。

    图  4  激光光斑中心定位流程
    Figure  4.  Laser spot center positioning process
    图  5  激光光斑提取效果图
    Figure  5.  Laser spot extraction effect diagram

    图6所示建立悬臂式掘进机位姿解算模型,已知空间3个特征点$ {P}_{1}、{P}_{2}、{P}_{3} $构成空间三角形,在相机像平面的投影分别为$ {p}_{1}、{p}_{2}、{p}_{3} $,O为相机光轴中心。

    图  6  掘进机视觉位姿解算模型
    Figure  6.  Roadheader visual pose calculation model

    图中三角形存在对应关系:$\Delta O{P_1}{P_2} 与 \Delta O{p_1}{p_2}$、$\Delta O{P_1}{P_3} 与 \Delta O{p_1}{p_3}$、$\Delta O{P_2}{P_3} 与 \Delta O{p_2}{p_3}$,利用余弦定理有:

    $$ \left\{\begin{array}{l} l_{{{{OP}}}_{1}}^{2}+l_{{{{OP}}}_{2}}^{2}-2l_{{{{OP}}}_{1}}l_{{{{OP}}}_{2}}\cos\;\alpha =l_{{{{P}}}_{1}{{{P}}}_{2}}^{2}\\ l_{{{{OP}}}_{1}}^{2}+l_{{{{OP}}}_{3}}^{2}-2l_{{{{OP}}}_{1}}l_{{{{OP}}}_{3}}\cos\;\beta =l_{{{{P}}}_{1}{{{P}}}_{3}}^{2}\\ l_{{{{OP}}}_{2}}^{2}+l_{{{{OP}}}_{3}}^{2}-2l_{{{{OP}}}_{2}}l_{{{{OP}}}_{3}}\cos\;\gamma =l_{{{{P}}}_{2}{{{P}}}_{3}}^{2}\end{array} \right. $$ (1)

    为了简化计算在式(1)两边除以$O{P_3}^2$,并假设:

    $$ \left\{ \begin{gathered} x = \frac{{{l_{{OP_1}}}}}{{l_{{{OP_3}}}}} \\ y = \frac{{{l_{{OP_2}}}}}{{{l_{{OP_3}}}}} \\ \end{gathered} \right. $$ (2)

    将式(2)代入式(1),得:

    $$ \left\{\begin{array}{l}{x}^{2}+{y}^{2}-2xy{\cos}\;\alpha =l_{{{{P}}}_{1}{{{P}}}_{2}}^{2}/l_{{{{OP}}}_{3}}^{2}\\ {x}_{1}{}^{2}+{y}^{2}-2y{\cos}\;\beta =l_{{{{P}}}_{1}{{{P}}}_{3}}^{2}/l_{{{{OP}}}_{3}}^{2}\\ {x}^{2}+1-2x{\cos}\;\gamma =l_{{{{P}}}_{2}{{{P}}}_{3}}^{2}/l_{{{{OP}}}_{3}}^{2}\end{array} \right. $$ (3)

    对式(3),令

    $$ \left\{ \begin{gathered} {{a = l_{{{{P}}_1}{{{P}}_2}}^2} \mathord{\left/ {\vphantom {{a = l_{{{{P}}_1}{{{P}}_2}}^2} {l_{{{OP}}_3}^2}}} \right. } {l_{{{OP}}_3}^2}} \\ ab = {{l_{{{{P}}_1}{{{P}}_3}}^2} \mathord{\left/ {\vphantom {{l_{{{{P}}_1}{{{P}}_3}}^2} {l_{{{OP}}_3}^2}}} \right. } {l_{{{OP}}_3}^2}} \\ ac = {{l_{{{{P}}_2}{{{P}}_3}}^2} \mathord{\left/ {\vphantom {{l_{{{{P}}_2}{{{P}}_3}}^2} {l_{{{OP}}_3}^2}}} \right. } {l_{{{OP}}_3}^2}} \\ \end{gathered} \right. $$ (4)

    将式(4)带入式(3)中,得:

    $$ \left\{\begin{array}{l}{x}^{2}+{y}^{2}-2xy\mathrm{cos}\;\alpha =a\\ {x}_{1}{}^{2}+{y}^{2}-2y\mathrm{cos}\;\beta =ab\\ {x}^{2}+1-2x\mathrm{cos}\;\gamma =ac\end{array} \right. $$ (5)

    对式(5)进行化简,得:

    $$ \left \{\begin{array}{l}\left(1-b\right){y}^{2}-b{x}^{2}-y\mathrm{cos}\;\beta +2bxy\mathrm{cos}\;\alpha +1=0\\ \left(1-c\right){x}^{2}-c{y}^{2}-x\mathrm{cos}\;\gamma +2cxy\mathrm{cos}\;\alpha +1=0\end{array}\right. $$ (6)

    式中:$ \mathrm{cos}\;\alpha 、\mathrm{cos}\;\beta 、\mathrm{cos}\;\lambda $可以通过激光点的图像坐标计算得到,结合矿用全站仪标定获得的$ {P}_{1}、 {P}_{2}、{P}_{3} $空间点在巷道坐标系下的坐标$ {{\mathbf{P}}_{{\mathrm{w}}i}} = [ {x_{{\mathrm{w}}i}}, {y_{{\mathrm{w}}i}}, {z_{{\mathrm{w}}i}} ]^{\text{T}} \left( {i = 1,2,3} \right) $计算得到bcxy作为未知数随着相机的位姿改变而发生变化。

    因此对式(6)采用吴消元法求解,该方程最多可以得到4个解$ ({x}_{g},{y}_{g}) $,$ g = 1,2,3,4 $,即可以得到$ {P}_{1} $、${P}_{2} $、${P}_{3} $在相机坐标系下4组坐标$ {{\mathbf{P}}_{{\mathrm{c}}i}} = {\left[ {x_i^m,y_i^m,z_i^m} \right]^{\text{T}}} \left( {i = 1,2,3;m = 1,2,3,4} \right) $。

    根据相机透视投影模型,相机坐标系与巷道坐标系间的旋转矩阵R和平移向量t可以表示为

    $$ {{\boldsymbol{P}}_{{\mathrm{ci}}}} = {\boldsymbol{R}}{{\boldsymbol{P}}_{{\mathrm{w}}i}} + {\boldsymbol{t}} $$ (7)

    根据旋转矩阵R和平移向量t,得到防爆相机在巷道坐标系下的位置和航向角、俯仰角、横滚角信息。但其中只有一组姿态解是最优解,寻求最优解将其转化为最小化重投影误差的问题,建立最小化重投影误差函数为

    $$ \mathop \sum \limits_{i = 1}^n \mathop \sum \limits_{j = 1}^m {\left\| {{p_i} - \frac{1}{s}{\boldsymbol{K}}\left( {{{\boldsymbol{R}}_j}{P_i} + {{\boldsymbol{t}}_i}} \right)} \right\|^2} $$ (8)

    式中:${p_i}$为第i个激光点在图像坐标系的图像坐标;s为投影像素点坐标转化为齐次坐标的系数;K为相机的内参矩阵;${{\boldsymbol{R}}_j},{{\boldsymbol{t}}_j}$分别为式(7)得到的第j个旋转矩阵和平移矩阵。最后再结合系统初始标定的掘进机机身坐标系与相机坐标系之间的转换关系,得到掘进机机身在巷道坐标系下的位姿信息。

    为了便于计算本文采用四元数法[28]进行掘进机姿态角计算。四元数可定义为

    $$ {\boldsymbol{Q}} = {q_0} + {q_1}{\boldsymbol{i}} + {q_2}{\boldsymbol{j}} + {q_3}{\boldsymbol{k}} $$ (9)

    式中:${q_0},{q_1},{q_2},{q_3}$为实数;i, j, k为相互正交的单位向量。机身坐标系到地理坐标系的姿态转化矩阵${\boldsymbol{C}}_{\mathrm{o}}^n$用四元数可表示为

    $$ {\boldsymbol{C}}_{\mathrm{o}}^n = \left[ {\begin{array}{*{20}{c}} {q_0^2 + q_1^2 - q_2^2 - q_3^2} & {2\left( {{q_1}{q_2} - {q_0}{q_3}} \right)} & {2\left( {{q_1}{q_3} + {q_0}{q_2}} \right)} \\ {2\left( {{q_1}{q_2} - {q_0}{q_3}} \right)} & {q_0^2 - q_1^2 + q_2^2 - q_3^2} & {2\left( {{q_2}{q_3} + {q_0}{q_1}} \right)} \\ {2\left( {{q_1}{q_3} + {q_0}{q_2}} \right)} & {2\left( {{q_2}{q_3} - {q_0}{q_1}} \right)} & {q_0^2 - q_1^2 - q_2^2 + q_3^2} \end{array}} \right] $$ (10)

    姿态更新周期为${T_k} = {t_{k + 1}} - {t_k}$ 时,姿态更新可表示为

    $$ {\boldsymbol{Q}}_{\mathrm{o}}^b\left( {{{{t}}_{k + 1}}} \right) = \frac{1}{2}{\boldsymbol{Q}}_{\mathrm{o}}^b\left( {{{{t}}_k}} \right) \otimes {\boldsymbol{Q}}_{\mathrm{o}}^b\left( {{{{T}}_k}} \right) $$ (11)

    得到姿态转化矩阵后即可求解掘进机在地理坐标系下的姿态角:

    $$ \begin{array}{*{20}{l}} {\alpha = \arcsin \left[ {2\left( {{q_2}{q_3} - {q_0}{q_1}} \right)} \right]} \\ {\beta = \arctan \left[ { - \dfrac{{2\left( {{q_2}{q_3} - {q_0}{q_1}} \right)}}{{q_0^2 - q_1^2 - q_2^2 + q_3^2}}} \right]} \\ {\gamma = \arctan \left[ { - \dfrac{{2\left( {{q_1}{q_3} + {q_0}{q_2}} \right)}}{{q_0^2 - q_1^2 + q_2^2 - q_3^2}}} \right]} \end{array} $$ (12)

    将掘进机加速度投影至导航坐标系下,可得到掘进机速度更新方程:

    $$ {\dot V^n} = C_{\mathrm{o}}^n{f^{\mathrm{o}}} - \left( {2\omega _{{\mathrm{ie}}}^n + \omega _{{\mathrm{en}}}^n} \right) \times {V^n} + {g^n} $$ (13)

    其中

    $$ \begin{array}{*{20}{c}} {\omega _{{\mathrm{ie}}}^n = {{\left[ {\begin{array}{*{20}{c}} 0&{{\omega _{{\mathrm{ie}}}}\cos\; L}&{{\omega _{{\mathrm{ie}}}}\sin \;L} \end{array}} \right]}^{\text{T}}}} \\ {{\mathbf{\omega }}_{{\mathrm{en}}}^n = {{\left[ { - \dfrac{{{V_{\text{N}}}}}{{{R_{\mathrm{M}}} + h}}\quad \dfrac{{{V_{\text{E}}}}}{{{R_{\mathrm{N}}} + h}}\quad \dfrac{{{V_{\text{E}}}}}{{{R_{\mathrm{N}}} + h}}\tan\; L} \right]}^{\text{T}}}} \end{array} $$ (14)

    式中:${f^{\mathrm{o}}}$为加速度计测量值,m/s2;$ \omega _{{\mathrm{ie}}}^n $为地球坐标系相对于地心坐标系的旋转角速度,rad/s;$\omega _{{\mathrm{en}}}^n$为导航坐标系相对于地球坐标系的旋转角速度,rad/s;${g^n}$为导航坐标系下重力加速度,m/s2;${{\boldsymbol{V}}^{n}} = \left[ {{V_{\mathrm{E}}},{V_{\mathrm{N}}},{V_{\mathrm{Z}}}} \right]$, ${V_{\mathrm{E}}},{V_{\mathrm{N}}},{V_{\mathrm{Z}}}$为东向、北向、天向的速度,m/s;$L$为地理纬度,(°);h为高度,m;再去除有害加速度$ \left( {2\omega _{{\mathrm{ie}}}^n + \omega _{{\mathrm{en}}}^n} \right) \times {{\boldsymbol{V}}^n} + {g^n} $才能获得掘进机的加速度。

    根据速度方程,获得惯导的位置微分方程后再积分得到掘进机的位置:

    $$ \left\{ {\begin{array}{*{20}{l}} {{L_{{{s}} + 1}} = {L_{{s}}} + \displaystyle\int_{{t_{{s}}}}^{{t_{{{s}} + 1}}} {\dfrac{{{V_{\mathrm{N}}}}}{{{R_{\mathrm{M}}} + h}}{\mathrm{d}}t} } \\ {{\lambda _{{{s}} + 1}} = {\lambda _{{s}}} + \displaystyle\int_{{t_{{s}}}}^{{t_{{{s}} + 1}}} {\dfrac{{{V_{\mathrm{E}}}}}{{\left( {{R_{\mathrm{N}}} + h} \right)\cos\; L}}{\mathrm{d}}t} } \\ {{h_{{{s}} + 1}} = {h_{{s}}} + \displaystyle\int_{{t_{{s}}}}^{{t_{{{s}} + 1}}} {{V_{\mathrm{Z}}}{\mathrm{d}}t} } \end{array}} \right. $$ (15)

    式中:${L_{{{s}} + 1}}$为${t_{{{s}} + 1}}$时刻掘进机在地理坐标系下的经度,(°);${\lambda _{{{s}} + 1}} $为${t_{{{s}} + 1}} $时刻掘进机在地理坐标系下的纬度,(°);${h_{{{s}} + 1}} $为${t_{{{s}} + 1}} $时刻掘进机在地理坐标系下的高度,m;${L_{{s}}}$为掘进机在${t_{{s}}}$时刻掘进机在地理坐标系下的经度,(°);${\lambda _{{s}}} $为${t_{{s}}} $时刻掘进机在地理坐标系下的纬度,(°);${h_{{s}}} $为${t_{{s}}} $时刻掘进机在地理坐标系下的高度,m;${R_{\mathrm{M}}}、{R_{\mathrm{N}}}$分别为地球卯酉圈和子午圈的曲率半径,m。

    通过惯导解算得到的掘进机位姿均是在导航坐标系下的位姿,掘进机位姿表示通常是在巷道坐标系下,掘进机在导航坐标系的位姿表示为${{\boldsymbol{M}}_{\mathrm{n}}}$,假设导航坐标系与巷道坐标系的转换矩阵为${{\boldsymbol{M}}_{{\mathrm{nh}}}}$,即掘进机在巷道坐标系的位姿可表示为

    $$ {{\boldsymbol{M}}_{\mathrm{h}}} = {{\boldsymbol{M}}_{{\mathrm{nh}}}} {{\boldsymbol{M}}_{\mathrm{n}}} $$ (16)

    采用卡尔曼滤波算法对光纤惯导和视觉测量位姿数据进行融合,在融合前对各传感器数据进行有效性判断,并在时间实现配准,运用卡尔曼滤波融合算法融合视觉与惯导数据,最终得到掘进机的位姿信息,同时采用融合后的位姿数据对惯导进行补偿校正。

    视觉测量与惯导测量两种测量方式的采样频率及计算速度各不相同,在数据融合时要考虑两种测量结果在时间上同步。通常采用插值方法进行时间配准,为使插值误差尽可能小,根据插值原则插值节点应尽可能位于插值区间中部,由高采样率传感器测得的数据向低采样率传感器测得数据进行时间配准。本文采用拉格朗日三点插值[29]实现惯导位姿数据向视觉位姿数据进行时间配准。根据插值原理,假设$ {t_{n - 1}},{t_n},{t_{n + 1}} $时刻传感器测量数据分别为${x_{n - 1}}, {x_n},{x_{n + 1}}$由于掘进机的移动速度很小,因此可以将$ {t_{n - 1}},{t_n},{t_{n + 1}} $看作是等间距的,若在时间$t = {t_n} + \Delta t$处插值,运用拉格朗日三点插值可以计算出t时刻的测量值为

    $$ \begin{split} x_t= & \frac{\left(t-t_n\right)\left(t-t_{n-1}\right)}{\left(t_{n-1}-t_n\right)\left(t_{n-1}-t_{n+1}\right)} x_{n-1} +\frac{\left(t-t_{n-1}\right)\left(t-t_{n+1}\right)}{\left(t_n-t_{n-1}\right)\left(t_n-t_{n+1}\right)} x_n +\\& \frac{\left(t-t_{n-1}\right)\left(t-t_n\right)}{\left(t_{n+1}-t_{n-1}\right)\left(t_{n+1}-t_n\right)} x_{n+1} \end{split} $$ (17)

    惯导的采样周期为0.05 s,视觉系统的采样周期为0.1 s,所有位姿数据都带有时间戳,如图7所示,假设利用拉格朗日三点插值方法将惯导数据向视觉系统的550 ms点处进行时间配准,惯导在该点附近的数据为550、600、650 ms点,提取时间$ t = 550{\text{ ms}} $,$ t_{n-1}=550 \mathrm{\;ms} $,$t_{n}=600 \mathrm{\;ms} $,$ t_{n+1}=650 \mathrm{\;ms} $,根据式(17)完成两者数据的时间配准。

    图  7  数据时间配准示意
    Figure  7.  Schematic diagram of data time registration

    视觉测量给出的掘进机位姿误差多由激光光斑提取误差及位姿测量误差引起,可以在图像处理算法、视觉硬件等方面进一步优化,减少视觉测量系统误差。虽然上述方法在一定程度上可以改善误差对测量系统的影响,但仍不能完全消除误差。因此,建立光斑中心定位与特征点三维坐标之间的误差传递函数,根据视觉测量误差理论,对光斑中心坐标求导,按泰勒级数展开并忽略高次项,得到特征点图像坐标与三维坐标测量误差为

    $$ \delta {X_{{\mathrm{w}}i}} = \frac{{\partial {X_{{\mathrm{w}}i}}}}{{\partial {u_i}}}\delta {u_i} + \frac{{\partial {X_{{\mathrm{w}}i}}}}{{\partial {v_i}}}\delta {v_i} $$ (18)
    $$ \delta {Y_{{\mathrm{w}}i}} = \frac{{\partial {Y_{{\mathrm{w}}i}}}}{{\partial {u_i}}}\delta {u_i} + \frac{{\partial {Y_{{\mathrm{w}}i}}}}{{\partial {v_i}}}\delta {v_i} $$ (19)

    式中:$ \delta u_{1} $,$ \delta v_{i} $为光斑图像坐标提取误差;i为特征点个数,i=1,2,3。由式(18)、式(19)得到特征点图像与三维坐标之间的误差传递关系为

    $$ \delta {\boldsymbol{W}} = {{\boldsymbol{E}}_{{\mathrm{w}}P}}\delta {\boldsymbol{P}} $$ (20)

    式中:$ {{\boldsymbol{E}}_{{\mathrm{w}}P}} = {\left[ \begin{gathered} \frac{{\partial {X_{{\mathrm{w}}1}}}}{{\partial {u_1}}}\quad \frac{{\partial {X_{{\mathrm{w}}1}}}}{{\partial {v_1}}}\quad \cdots \quad \frac{{\partial {X_{{\mathrm{w}}1}}}}{{\partial {u_3}}}\quad \frac{{\partial {X_{{\mathrm{w}}1}}}}{{\partial {v_3}}} \\[-2pt] \quad \vdots \quad \quad \;\; \vdots \quad \quad \vdots \quad \quad \;\; \vdots \quad \quad \;\; \vdots \\[-2pt] \frac{{\partial {Y_{{\mathrm{w}}3}}}}{{\partial {u_1}}}\quad \frac{{\partial {Y_{{\mathrm{w}}3}}}}{{\partial {v_1}}}\quad \; \cdots \quad \frac{{\partial {Y_{{\mathrm{w}}3}}}}{{\partial {u_3}}}\quad \frac{{\partial {Y_{{\mathrm{w}}3}}}}{{\partial {v_3}}} \\ \end{gathered} \right)_{6 \times 6}} $为光斑图像坐标系与世界坐标系之间的误差传递模型;$\delta {\boldsymbol{P}} = {\left( {\delta {u_1},\delta {v_1},\delta {u_2},\delta {v_2},\delta {u_3},\delta {v_3}} \right)^{\text{T}}}$为特征点图像坐标误差,px;$\delta {\boldsymbol{W}} = {\left( {\delta {X_{{\mathrm{w}}1}},\delta {Y_{{\mathrm{w}}1}},\delta {X_{{\mathrm{w}}2}},\delta {Y_{{\mathrm{w}}2}},\delta {X_{{\mathrm{w}}3}},\delta {Y_{{\mathrm{w}}3}}} \right)^{\text{T}}}$为世界坐标测量误差,m。

    根据视觉位姿解算模型,位姿参数与光斑的三维坐标有关,可以表示为

    $$ \begin{split} f( {t_x},{t_y},{t_z},{\theta _x}, & {\theta _y},{\theta _z},{X_{{\mathrm{w}}1}},{Y_{{\mathrm{w}}1}},{Z_{{\mathrm{w}}1}},{X_{{\mathrm{w}}2}},{Y_{{\mathrm{w}}2}},{Z_{{\mathrm{w}}2}},\\&{X_{{\mathrm{w}}3}}, {Y_{{\mathrm{w}}3}},{Z_{{\mathrm{w}}3}} ) = 0 \end{split} $$ (21)

    对式(21)进行泰勒展开,利用最小二乘法得到三维坐标与位姿解算的误差传递模型:

    $$ \delta {\boldsymbol{q}} = {\left( {{\boldsymbol{J}}_{\mathrm{f}}^{\text{T}}{{\boldsymbol{J}}_{\mathrm{f}}}} \right)^{ - 1}}{\boldsymbol{J}}_{\mathrm{f}}^{\text{T}}\delta {\boldsymbol{W}} $$ (22)

    式中:J为式(21)的雅可比矩阵,令$\delta {{\boldsymbol{E}}_P} = {\left( {{\boldsymbol{J}}_{\mathrm{f}}^{\text{T}}{{\boldsymbol{J}}_{\mathrm{f}}}} \right)^{ - 1}}{\boldsymbol{J}}_{\mathrm{f}}^{\text{T}}$,结合式(20)得到视觉测量系统误差为

    $$ \delta {\boldsymbol{E}} = {{\boldsymbol{E}}_{\mathrm{P}}}{{\boldsymbol{E}}_{{\mathrm{wP}}}}\delta {\boldsymbol{W}} $$ (23)

    受惯导本身测量原理及外界因素干扰,导致系统出现测量误差或偏差,致使系统性能下降。通常由外界因素如振动、电磁干扰等影响,可采取物理减震、隔离、屏蔽等措施;由原理性导致的可采用滤波技术、定时校准、校正等措施,进一步降低误差,但仍不能完全消除。因此,根据文献[30]建立惯导的误差方程为:

    $$ \dot \phi = \phi \times \omega _{{\mathrm{in}}}^n + \delta \omega _{{\mathrm{in}}}^n - {\varepsilon ^n} $$ (24)
    $$ \begin{split} \delta {\dot V^n} = & - \phi {f^n} - \left( {2\omega _{{\mathrm{ie}}}^n + \omega _{{\mathrm{en}}}^n} \right) \delta {V^n} - \\& \left( {2\delta \omega _{{\mathrm{ie}}}^n + \delta \omega _{{\mathrm{en}}}^n} \right) {V^n} + {\nabla ^n} \end{split} $$ (25)
    $$ \left\{ \begin{gathered} \delta \dot L = \frac{{\delta {V_{\mathrm{N}}}}}{{{R_{\mathrm{M}}} + h}} - \delta h\frac{{{V_{\mathrm{N}}}}}{{{{\left( {{R_{\mathrm{M}}} + h} \right)}^2}}} \\ \delta \dot \lambda = \frac{{\delta {V_{\mathrm{E}}}}}{{{R_{\mathrm{N}}} + h}}\sec \;L + \delta L\frac{{{V_{\mathrm{E}}}}}{{\left( {{R_{\mathrm{N}}} + h} \right)}}\tan\; L\sec\; L - \\\qquad \delta h\frac{{{V_{\mathrm{E}}}\sec\; L}}{{{{\left( {{R_{\mathrm{N}}} + h} \right)}^2}}} \\ \delta \dot h = \delta {V_{\mathrm{Z}}} \\ \end{gathered} \right. $$ (26)

    式中:$ \phi $为姿态误差,(°);$ \omega _{{\mathrm{in}}}^n $为导航坐标系相对于地心坐标系的旋转角速度,rad/s;$ \delta \omega _{{\mathrm{in}}}^n $为$ \omega _{{\mathrm{in}}}^n $的误差,rad/s;$ {\varepsilon ^n} $为陀螺仪零偏误差,rad/s;$ \delta {V^n} $为速度误差,m/s;$ {f^n} $为导航坐标系下加速度计测得的比力,m/s2;$ {{\boldsymbol{V}}^n} $为惯导得到的导航坐标系下掘进机速度,m/s;$ {\nabla ^n} $为加速度计的零偏误差,m/s2;$ \delta L $为纬度误差,(°);$ \delta \lambda $为经度误差,(°);$ \delta {{h}} $高度误差,m;$ \delta {V_{\mathrm{N}}} $,$ \delta {V_{\mathrm{E}}} $,$ {V_{\mathrm{Z}}} $别为北向、东向、天向速度误差,m/s。

    采用卡尔曼滤波器对惯导数据和视觉数据进行融合,系统的状态向量表示为

    $$ \begin{split} {\boldsymbol{X}} = & [ {\phi _{\mathrm{N}}},{\phi _{\mathrm{E}}},{\phi _{\mathrm{Z}}},\delta {v_{\mathrm{N}}},\delta {v_{\mathrm{E}}},\delta {v_{\mathrm{Z}}},\delta L,\delta \lambda ,\delta h,\\& {\varepsilon _{\mathrm{N}}},{\varepsilon _{\mathrm{E}}},{\varepsilon _{\mathrm{Z}}},{\nabla _{\mathrm{N}}},{\nabla _{\mathrm{E}}},{\nabla _{\mathrm{Z}}},\delta {\boldsymbol{E}} ]^{\text{T}} \end{split} $$ (27)

    式中:$ {\phi _{\mathrm{N}}},{\phi _{\mathrm{E}}},{\phi _{\mathrm{Z}}} $为姿态角误差,(°);$ \delta {v_{\mathrm{N}}},\delta {v_{\mathrm{E}}},\delta {v_{\mathrm{Z}}} $为速度误差,m/s;$ {\varepsilon _{\mathrm{N}}},{\varepsilon _{\mathrm{E}}},{\varepsilon _{\mathrm{Z}}} $为3个轴向的陀螺仪漂移,rad/s;$ {\nabla _{\mathrm{N}}},{\nabla _{\mathrm{E}}},{\nabla _{\mathrm{Z}}} $为3个轴向的加速度漂移,m/s2;$\delta {\boldsymbol{E}}$为视觉测量系统误差,m。

    以视觉测量和惯导解算的掘进机位置与姿态数据之差作为系统的观测量为

    $$ {\boldsymbol{Z}} = \left[ \begin{gathered} {\phi _{{\mathrm{INS}}}} - {\phi _{\mathrm{C}}} \\ {{\boldsymbol{P}}_{{\mathrm{INS}}}} - {{\boldsymbol{P}}_{\mathrm{C}}} \\ \end{gathered} \right] $$ (28)

    系统卡尔曼滤波状态方程和观测方程为

    $$ \left\{ \begin{gathered} {{\boldsymbol{X}}_k} = {\boldsymbol{A}}{{\boldsymbol{X}}_{k - 1}} + {\boldsymbol{\varGamma }}{{\boldsymbol{W}}_{k - 1}} \\ {{\boldsymbol{Z}}_k} = {\boldsymbol{H}}{{\boldsymbol{X}}_k} + {{\boldsymbol{V}}_k} \\ \end{gathered} \right. $$ (29)

    式中:${{\boldsymbol{X}}_k}$为$k$时刻的状态量;${\boldsymbol{A}}$为状态转移矩阵;${{\boldsymbol{X}}_{k - 1}}$为$k - 1$时刻的状态量;${\boldsymbol{\varGamma }}$为噪声驱动矩阵;${{\boldsymbol{W}}_k}$为$k - 1$时刻的输入噪声;${\boldsymbol{H}}$为量测矩阵;${{\boldsymbol{Z}}_k}$为$k$时刻的观测值;${{\boldsymbol{V}}_k}$为$k$时刻的观测噪声。

    已知卡尔曼滤波状态方程和观测方程后,就可以对下一时刻状态进行预测

    $$ \begin{gathered} {{{\boldsymbol{\hat X}}}^ - }_k = {\boldsymbol{A}}{{{\boldsymbol{\hat X}}}^{}}_{k - 1} \\ {\boldsymbol{P}}_k^ - = {\boldsymbol{A}}{{\boldsymbol{P}}_{k - 1}}{{\boldsymbol{A}}^T} + {\boldsymbol{D}} \\ \end{gathered} $$ (30)

    式中:$ {{\boldsymbol{\hat X}}^ - }_k $为$k$时刻的先验估计值(预测值);$ {{\boldsymbol{\hat X}}^ - }_{k - 1} $为$k - 1$时刻的后验估计值(最优估计);$ {\boldsymbol{P}}_k^ - $为先验估计的协方差矩阵;$ {{\boldsymbol{P}}_{k - 1}} $为$k - 1$时刻的后验估计协方差矩阵;${\boldsymbol{D}}$为噪声协方差矩阵。

    对卡尔曼滤波器状态进行更新

    $$ \begin{gathered} {{\boldsymbol{{ K}}}_k} = {\boldsymbol{P}}_k^ - {{\boldsymbol{H}}^{\rm T}}{\left( {{\boldsymbol{HP}}_k^ - {{\boldsymbol{H}}^{\text{T}}} + {{\boldsymbol{R}}_{\mathrm{e}}}} \right)^{ - 1}} \\ {{{\boldsymbol{\hat X}}}_k} = {\boldsymbol{\hat X}}_k^ - + {{\boldsymbol{K}}_k}\left( {{{\boldsymbol{Z}}_k} - {\boldsymbol{H\hat X}}_k^ - } \right) \\ {{\boldsymbol{P}}_k} = \left( {{\boldsymbol{I}} - {{\boldsymbol{K}}_k}{\boldsymbol{H}}} \right){\boldsymbol{P}}_k^ - \\ \end{gathered} $$ (31)

    式中:$ {{\boldsymbol{K}}_k} $为卡尔曼滤波增益;$ {\boldsymbol{P}}_k^ - $为$k$时刻预测误差协方差矩阵;$ {{\boldsymbol{R}}_{\mathrm{e}}} $为观测噪声协方差矩阵;$ {{\boldsymbol{\hat X}}_k} $为$k$时刻的后验估计值;${\boldsymbol{I}}$为单位矩阵。

    为解决视觉测量系统因视觉标靶短时遮挡造成的数据丢失及各传感器出现的异常值(以下均成称为无效数据)对系统精度、稳定性的影响,在两者数据融合前对系统中每个传感器i分别进行预测估计$ {}^i{{\boldsymbol{\hat X}}^ - }_k = {\boldsymbol{A}} {}^i{{\boldsymbol{\hat X}}^{}}_{k - 1} $,可得传感器i的预测观测值

    $$ {}^i{{\boldsymbol{\hat Z}}_k} = {\boldsymbol{H}} {}^i{{\boldsymbol{\hat X}}_k} $$ (32)

    式中:${}^i{{\boldsymbol{\hat Z}}_k}$为$k$时刻传感器i的预测观测值。给定阈值${\varepsilon _1}$,${\varepsilon _2}$$\left( {{\varepsilon _1} < {\varepsilon _2}} \right)$,若新的观测${}^i{{\boldsymbol{Z}}_k}$满足:

    $$ {\varepsilon _1} < \left| {{}^i{{\boldsymbol{Z}}_k} - {}^i{{{\boldsymbol{\hat Z}}}_k}} \right| < {\varepsilon _2} $$ (33)

    则认为观测值${}^i{{\boldsymbol{Z}}_k}$数据有效。不在该范围内则认为观测值${}^i{{\boldsymbol{Z}}_k}$数据无效,在判断确定无效数据的传感器后,系统只对有效数据的传感器计算局部估计及其观测量进行状态更新,即将无效传感器对应的卡尔曼滤波增益对应元素设置为0,以此剔除系统中的无效数据,降低该传感器对融合结果的影响,确保系统的稳定性和鲁棒性。

    为了验证笔者提出的掘进机组合定位方法的有效性及组合定位系统功能,搭建悬臂式掘进机位姿测量实验平台如图8所示,试验平台包括移动履带机器人、三激光指向仪标靶、防爆相机、光纤惯导、防爆计算机、数字全站仪。用烟雾制造机模拟井下粉尘环境,以第三方测量仪器数字全站仪测量的掘进机机身位姿为真实值,构建全站仪统一全局坐标系(X轴指向右、Y轴指向上、Z轴指向履带机器人前进方向)。试验验证设置如下:① 基于三激光指向仪标靶的掘进机位姿视觉测量精度评价试验,由于组合定位系统位置基准由视觉测量系统提供,精确的视觉定位数据是组合定位系统数据有效融合的保证。② 视觉定位、惯导定位、组合定位系统定位结果对比试验,进一步验证本文提出的视觉短时失效时有效值判断及处理方法的有效性。

    图  8  掘进机机身位姿视觉测量平台
    Figure  8.  Vision measurement platform for body pose of roadheader

    试验过程中首先对三激光指向仪标靶完成测量,将视觉测量系统得到的位姿转化到全站仪坐标系下,以全站仪直接测量的履带机器人位姿为真实值,对比视觉测量系统得到的测量值。在楼道移动履带机器人模拟掘进机在巷道内运动,利用防爆相机采集三激光指向仪标靶图像,规定履带机器人距离三激光标靶初始距离为10,随后每次移动10距离,测量距离分别为20、30、40、50、60 m。并在每个位置采集不少于30张激光标靶图像,在计算机中完成图像处理、激光光斑中心定位,计算得到掘进机机身位姿,同时对比全站仪测量的履带机器人真实位姿,对掘进机视觉测量系统精度进行验证。

    履带机器人在楼道移动过程中机身位姿测量值(视觉测量结果)和真实值(全站仪测量结果)见表1。结果表明:本文算法能够在60 m测试范围内实现机身位姿测量。不同距离下测量误差结果如图9所示,结果表明在60 m测量范围内本文所述的悬臂式掘进机位置视觉测量方法计算得到的掘进机机身XYZ 3个轴方向上的误差均在35 mm以内,掘进机机身姿态角误差均在0.5°以内。

    表  1  不同距离时视觉位姿测量结果
    Table  1.  Visual pose measurement results at different distances
    项目 距离/m x/mm y/mm z/mm 俯仰角/
    (°)
    横滚角/
    (°)
    航向角/
    (°)
    测量值 10 −457 567 9997 0.56 2.91 4.93
    20 −471 544 20022 1.80 2.80 4.84
    30 −564 561 29983 −1.19 2.38 5.03
    40 −516 563 40054 0.07 2.96 4.17
    50 −535 541 50071 0.81 3.06 5.09
    60 −488 546 59975 −2.09 3.01 4.16
    真实值 10 −452 556 10005 0.69 2.76 4.76
    20 −480 559 20009 1.66 2.59 4.62
    30 −551 542 30005 −1.36 2.61 4.70
    40 −537 540 40027 0.26 2.58 4.54
    50 −559 512 50041 0.54 2.61 4.68
    60 −516 514 60009 −1.74 2.54 4.58
    下载: 导出CSV 
    | 显示表格
    图  9  不同距离下视觉位姿测量误差
    Figure  9.  Absolute error of visual pose measurement at different distances

    在位姿测量试验开始前,惯导根据全站仪给出的起始点坐标进行对准,由于试验场地受限无法长距离运行,因此将惯导静置20 min后开始试验,模拟掘进机长时间作业运动,视觉系统实时采集履带机器人不同位姿时的标靶图像,通过位姿解算模型计算获得履带机器人实时位姿信息。通过卡尔曼滤波器对视觉与惯导数据融合,利用融合后的数据对惯导进行校正与补偿。

    试验中操作履带机器人从规定的起点沿着楼道模拟掘进机向前行进,并伴有转向动作,通过全站仪跟踪测量(X轴指向右、Y轴指向上、Z轴指向履带机器人前进方向)向前行进约50 m,期间采用惯导、视觉定位、组合定位方法分别对履带机器人进行定位测量,并在履带机器人移动过程中某一时刻短时遮挡标靶,模拟实际掘进机运行过程中视觉标靶遮挡的情况。机器人运动轨迹及不同方法定位结果如图10所示,可以看出,随着履带机器人的前进,惯导定位由于积分累计原因定位偏差逐渐增大,视觉系统定位结果与全站仪测量趋势一致,组合定位结果更接近全站仪得到的实际位移,位置最大偏差在30 mm以内,并且视觉标靶短时遮挡时组合定位结果出现波动但偏差仍在允许范围内。

    图  10  机器人运动轨迹及不同方法定位结果
    Figure  10.  Robot motion trajectory and positioning results of different methods

    基于本方法研发的掘进机组合定位系统在山西某矿胶带运输巷掘进工作面进行了工业性试验。试验设备包括EBZ260型悬臂式掘进机、防爆相机、光纤惯导系统、计算机、三激光指向仪标靶,系统硬件及安装如图11所示。

    图  11  系统硬件及安装
    Figure  11.  System hardware and installation

    井下应用时,为保证组合定位系统的功能,根据巷道条件及各设备的安装位置,将三激光指向仪标靶通过固定架安装在巷道顶板上并指向巷道掘进方向,根据井下防爆要求将光纤惯导固定在特制减震架上与计算机共同安装在导航防爆控制箱内。防爆相机安装在掘进机机尾部靠右侧保证激光指向仪在相机视角范围内,并将实时采集的标靶图像通过网线传输至导航控制箱计算机完成图像处理及位姿解算。截取掘进机作业过程中部分位姿数据如图12所示,分析截取的2700个位姿数据,可以看出,组合定位系统输出稳定,能够实时测量掘进的位姿信息。

    图  12  掘进机作业过程中部分位姿数据
    Figure  12.  Partial pose data of roadheader during operation

    在井下近2个月测试运行期间系统稳定运行,机身定位误差20 m范围内在30 mm以内,姿态角误差均在0.5°以内,实现了掘进机位姿实时测量,满足巷道掘进施工精度要求[31],并能为掘进机智能控制提供准确可靠的位姿数据。

    1)为解决煤矿复杂巷道环境下掘进装备单一位姿测量方法存在适应性、稳定性差的问题,提出一种基于视觉与光纤惯导融合的悬臂式掘进机组合定位方法。

    2)构建基于三激光指向仪标靶的悬臂式掘进机位姿视觉测量模型,推导了视觉系统与光纤惯导的误差方程,设计基于卡尔曼滤波的视觉与光纤惯导数据融合算法,利用融合后的位姿数据对惯导进行补偿校正,消除了惯导长时间积分运算导致的累积误差。

    3)提出基于预测传感器观测向量的有效值判断及异常值剔除方法,在数据融合前对传感器输出进行有效性判断,解决了因视觉标靶短时遮挡导致数据丢失对融合结果的影响,提高了定位系统的稳定性。

    4)在某矿井下进行了组合定位现场应用,结果表明组合定位方法得到的掘进机机身位置误差总体在30 mm以内,态角误差均在0.5°以内,且能持续稳定输出,满足巷掘进施工及掘进机智能截割数据精度要求。

  • 图  1   掘进机组合定位系统组成

    Figure  1.   Composition of roadheader combined localization system

    图  2   掘进机组合定位原理

    Figure  2.   Principle of combined localization of roadheader

    图  3   掘进机组合定位系统坐标系定义

    Figure  3.   Coordinate system definition of roadheader combined localization system

    图  4   激光光斑中心定位流程

    Figure  4.   Laser spot center positioning process

    图  5   激光光斑提取效果图

    Figure  5.   Laser spot extraction effect diagram

    图  6   掘进机视觉位姿解算模型

    Figure  6.   Roadheader visual pose calculation model

    图  7   数据时间配准示意

    Figure  7.   Schematic diagram of data time registration

    图  8   掘进机机身位姿视觉测量平台

    Figure  8.   Vision measurement platform for body pose of roadheader

    图  9   不同距离下视觉位姿测量误差

    Figure  9.   Absolute error of visual pose measurement at different distances

    图  10   机器人运动轨迹及不同方法定位结果

    Figure  10.   Robot motion trajectory and positioning results of different methods

    图  11   系统硬件及安装

    Figure  11.   System hardware and installation

    图  12   掘进机作业过程中部分位姿数据

    Figure  12.   Partial pose data of roadheader during operation

    表  1   不同距离时视觉位姿测量结果

    Table  1   Visual pose measurement results at different distances

    项目 距离/m x/mm y/mm z/mm 俯仰角/
    (°)
    横滚角/
    (°)
    航向角/
    (°)
    测量值 10 −457 567 9997 0.56 2.91 4.93
    20 −471 544 20022 1.80 2.80 4.84
    30 −564 561 29983 −1.19 2.38 5.03
    40 −516 563 40054 0.07 2.96 4.17
    50 −535 541 50071 0.81 3.06 5.09
    60 −488 546 59975 −2.09 3.01 4.16
    真实值 10 −452 556 10005 0.69 2.76 4.76
    20 −480 559 20009 1.66 2.59 4.62
    30 −551 542 30005 −1.36 2.61 4.70
    40 −537 540 40027 0.26 2.58 4.54
    50 −559 512 50041 0.54 2.61 4.68
    60 −516 514 60009 −1.74 2.54 4.58
    下载: 导出CSV
  • [1] 田伟琴,田原,贾曲,等. 悬臂式掘进机导航技术研究现状及发展趋势[J]. 煤炭科学技术,2022,50(3):267−274.

    TIAN Weiqin,TIAN Yuan,JIA Qu,et al. Research status and development trend of cantilever roadheader navigation technology[J]. Coal Science and Technology,2022,50(3):267−274.

    [2] 王虹,王步康,张小峰,等. 煤矿智能快掘关键技术与工程实践[J]. 煤炭学报,2021,46(7):2068−2083.

    WANG Hong,WANG Bukang,ZHANG Xiaofeng,et al. Key technology and engineering practice of intelligent rapid heading in coal mine[J]. Journal of China Coal Society,2021,46(7):2068−2083.

    [3] 王虹,李发泉,张小峰. 软弱围岩掘锚一体化快速掘进关键技术与工程实践[J]. 煤炭科学技术,2024,52(1):280−287.

    WANG Hong,LI Faquan,ZHANG Xiaofeng. Key technology research and engineering practice of speedy drivage with driving and bolting integration in soft surrounding rock[J]. Coal Science and Technology,2024,52(1):280−287.

    [4]

    SHEN Y,WANG P J,ZHENG W X,et al. Error compensation of strapdown inertial navigation system for the boom-type roadheader under complex vibration[J]. Axioms,2021,10(3):224. doi: 10.3390/axioms10030224

    [5] 陶云飞,杨健健,李嘉赓,等. 基于惯性导航技术的掘进机位姿测量系统研究[J]. 煤炭技术,2017,36(1):235−237.

    TAO Yunfei,YANG Jianjian,LI Jiageng,et al. Research on position and orientation measurement system of heading machine based on inertial navigation technology[J]. Coal Technology,2017,36(1):235−237.

    [6] 符世琛,成龙,陈慎金,等. 面向掘进机的超宽带位姿协同检测方法[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.

    [7]

    FU S C,LI Y M,ZONG K,et al. Ultra-wideband pose detection method based on TDOA positioning model for boom-type roadheader[J]. AEU - International Journal of Electronics and Communications,2019,99:70−80. doi: 10.1016/j.aeue.2018.11.023

    [8]

    FU S C,LI Y M,ZHANG M J,et al. Ultra-wideband pose detection system for boom-type roadheader based on Caffery transform and Taylor series expansion[J]. Measurement Science and Technology,2018,29(1):015101. doi: 10.1088/1361-6501/aa91c0

    [9] 马宏伟,杨金科,毛清华,等. 煤矿护盾式掘进机器人系统精确定位研究[J]. 工矿自动化,2022,48(3):63−70.

    MA Hongwei,YANG Jinke,MAO Qinghua,et al. Research on precise positioning of shield roadheader robot system in coal mine[J]. Industry and Mine Automation,2022,48(3):63−70.

    [10]

    LUO Y B,CHEN J X,XI W Z,et al. Analysis of tunnel displacement accuracy with total station[J]. Measurement,2016,83:29−37. doi: 10.1016/j.measurement.2016.01.025

    [11] 贾文浩,陶云飞,张敏骏,等. 基于iGPS的煤巷狭长空间中掘进机绝对定位精度研究[J]. 仪器仪表学报,2016,37(8):1920−1926.

    JIA Wenhao,TAO Yunfei,ZHANG Minjun,et al. Research on absolute positioning accuracy of roadheader based on indoor global positioning system in narrow and long coal tunnel[J]. Chinese Journal of Scientific Instrument,2016,37(8):1920−1926.

    [12] 陶云飞,李瑞,李嘉赓,等. iGPS的单站多点分时测量系统对掘进机偏向位移精度研究[J]. 煤炭技术,2017,36(2):246−247.

    TAO Yunfei,LI Rui,LI Jiageng,et al. Research on positioning accuracy of roadheader based on singlestation,multipoint and time-shared of iGPS measurement system[J]. Coal Technology,2017,36(2):246−247.

    [13] 黄喆,燕庆德,邵震宇,等. 基于双相机标靶的直线顶管掘进机导向方法[J]. 激光与光电子学进展,2022,59(4):458−465.

    HUANG Zhe,YAN Qingde,SHAO Zhenyu,et al. Guiding method of linear pipe jacking machine based on dual camera target[J]. Laser & Optoelectronics Progress,2022,59(4):458−465.

    [14] 杨文娟,张旭辉,张超,等. 基于三激光束标靶的煤矿井下长距离视觉定位方法[J]. 煤炭学报,2022,47(2):986−1001.

    YANG Wenjuan,ZHANG Xuhui,ZHANG Chao,et al. Long distance vision localization method based on triple laser beams target in coal mine[J]. Journal of China Coal Society,2022,47(2):986−1001.

    [15] 张旭辉,沈奇峰,杨文娟,等. 基于三激光点标靶的掘进机机身视觉定位技术研究[J]. 电子测量与仪器学报,2022,36(6):178−186.

    ZHANG Xuhui,SHEN Qifeng,YANG Wenjuan,et al. Research on visual positioning technology of roadheader body based on three laser point target[J]. Journal of Electronic Measurement and Instrumentation,2022,36(6):178−186.

    [16]

    LI M G,ZHU H,YOU S Z,et al. UWB-based localization system aided with inertial sensor for underground coal mine applications[J]. IEEE Sensors Journal,2020,20(12):6652−6669. doi: 10.1109/JSEN.2020.2976097

    [17] 张旭辉,刘博兴,张超,等. 掘进机全站仪与捷联惯导组合定位方法[J]. 工矿自动化,2020,46(9):1−7.

    ZHANG Xuhui,LIU Boxing,ZHANG Chao,et al. Roadheader positioning method combining total station and strapdown inertial navigation system[J]. Industry and Mine Automation,2020,46(9):1−7.

    [18] 马宏伟,毛金根,毛清华,等. 基于惯导/全站仪组合的掘进机自主定位定向方法[J]. 煤炭科学技术,2022,50(8):189−195.

    MA Hongwei,MAO Jingen,MAO Qinghua,et al. Automatic positioning and orientation method of roadheader based on combination of ins and digital total station[J]. Coal Science and Technology,2022,50(8):189−195.

    [19]

    HAN S L,REN X Y,LU J Z,et al. An orientation navigation approach based on INS and odometer integration for underground unmanned excavating machine[J]. IEEE Transactions on Vehicular Technology,2020,69(10):10772−10786. doi: 10.1109/TVT.2020.3010979

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

    WANG Haoran,WANG Hongwei,LI Zhenglong,et al. Roadheader combined positioning method based on strapdown inertial navigation and differential odometer[J]. Journal of Mine Automation,2022,48(9):148−156.

    [21] 刘送永,崔玉明,孟德远,等. 巷道掘进机多传感融合定位系统及试验研究[J]. 振动 测试与诊断,2023,43(3):476-484,618.

    LIU Songyong,CUI Yuming,MENG Deyuan,et al. Multi-sensor fusion positioning system and experimental study of roadheader[J]. Journal of Vibration,Measurement & Diagnosis,2023,43(3):476-484,618.

    [22] 毛清华,周庆,安炎基,等. 惯导与视觉信息融合的掘进机精确定位方法[J]. 煤炭科学技术,2024,52(5):236−248.

    MAO Qinghua,ZHOU Qing,AN Yanji,et al. Precise positioning method of tunneling machine for inertial navigation and visual information fusion[J]. Coal Science and Technology,2024,52(5):236−248.

    [23] 张超,张旭辉,杜昱阳,等. 基于双目视觉的悬臂式掘进机位姿测量技术研究[J]. 煤炭科学技术,2021,49(11):225−235.

    ZHANG Chao,ZHANG Xuhui,DU Yuyang,et al. Measuring technique of cantilever roadheader position based on binocular stereo vision[J]. Coal Science and Technology,2021,49(11):225−235.

    [24] 黄东,杨凌辉,罗文,等. 基于视觉/惯导的掘进机实时位姿测量方法研究[J]. 激光技术,2017,41(1):19−23.

    HUANG Dong,YANG Linghui,LUO Wen,et al. Study on measurement method of realtime position and attitude of roadheader based on vision/inertial navigation system[J]. Laser Technology,2017,41(1):19−23.

    [25] 钱锋,杨名宇,张晓沛. 基于序列图像提高光斑质心定位精度[J]. 光学 精密工程,2016,24(11):2880−2888. doi: 10.3788/OPE.20162411.2880

    QIAN Feng,YANG Mingyu,ZHANG Xiaopei. Improvement of localization accuracy of spot centroid based on sequential images[J]. Optics and Precision Engineering,2016,24(11):2880−2888. doi: 10.3788/OPE.20162411.2880

    [26] 刘爱胤,熊根良,姚健康,等. 基于霍夫圆检测的标志物多图像特征定位方法[J]. 计算机仿真,2021,38(3):341−345. doi: 10.3969/j.issn.1006-9348.2021.03.069

    LIU Aiyin,XIONG Genliang,YAO Jiankang,et al. Marker localization method on multiple image features based on Hough circle detection[J]. Computer Simulation,2021,38(3):341−345. doi: 10.3969/j.issn.1006-9348.2021.03.069

    [27] 段振云,王宁,赵文珍,等. 基于高斯积分曲面拟合的亚像素边缘定位算法[J]. 仪器仪表学报,2017,38(1):219−225. doi: 10.3969/j.issn.0254-3087.2017.01.029

    DUAN Zhenyun,WANG Ning,ZHAO Wenzhen,et al. Sub-pixel edge location algorithm based on Gauss integral curved surface fitting[J]. Chinese Journal of Scientific Instrument,2017,38(1):219−225. doi: 10.3969/j.issn.0254-3087.2017.01.029

    [28] 王琪,汪立新,周小刚,等. 混合式惯导系统姿态四元数连续自标定模型[J]. 北京航空航天大学学报,2019,45(7):1424−1434.

    WANG Qi,WANG Lixin,ZHOU Xiaogang,et al. Attitude quaternion continuous self-calibration model of hybrid inertial navigation system[J]. Journal of Beijing University of Aeronautics and Astronautics,2019,45(7):1424−1434.

    [29] 郭建刚,陈鹏,郑伟. 多表冗余惯导数据融合算法及在自对准中的应用[J]. 北京航空航天大学学报,2020,46(12):2211−2216.

    GUO Jiangang,CHEN Peng,ZHENG Wei. Data fusion algorithm of multi-sensor redundant inertial navigation and its application in self-alignment[J]. Journal of Beijing University of Aeronautics and Astronautics,2020,46(12):2211−2216.

    [30] 乔文超,王红雨,王鸿东. 基于BP神经网络的无人机IMU多传感器冗余的补偿算法[J]. 电子测量与仪器学报,2020,34(12):19−28.

    QIAO Wenchao,WANG Hongyu,WANG Hongdong. Compensation algorithm for UAV IMU multi-sensor redundancy based on BP neural network[J]. Journal of Electronic Measurement and Instrumentation,2020,34(12):19−28.

    [31] 中华人民共和国住房和城乡建设部. 煤矿井巷工程质量验收规范:GB 50213—2010[S]. 北京:中国计划出版社,2010.
图(12)  /  表(1)
计量
  • 文章访问数:  53
  • HTML全文浏览量:  9
  • PDF下载量:  19
  • 被引次数: 0
出版历程
  • 收稿日期:  2024-03-18
  • 网络出版日期:  2025-06-11
  • 刊出日期:  2025-06-24

目录

/

返回文章
返回