Precise positioning method of tunneling machine for inertial navigation and visual information fusion
-
摘要:
针对煤矿巷道复杂环境下掘进机精确定位难题，提出了一种惯导与视觉融合的掘进机精确定位方法，该方法采用惯导与“视觉+激光标靶”的定位方案。该方案将设计的四特征点大尺寸激光标靶固定于巷道顶板，相机固定于掘进机机身采集激光标靶图像，并运用圆拟合法定位光斑中心和基于四特征点的EPnP算法解算掘进机位置。为了验证“视觉+激光标靶”方法对掘进机位置检测效果，在模拟掘进工作面环境下开展了“视觉+激光标靶”位置检测试验，结果表明：在30 m内沿巷道宽度方向、掘进方向、高度方向最大误差不超过28.549 mm、78.868 mm、44.459 mm，实现了掘进机位置精确检测。针对惯导测量掘进机位姿误差随时间累积和掘进机振动对组合定位系统产生干扰导致位姿检测不准问题，提出改进Sage-Husa自适应滤波的惯导与视觉信息融合方法，该方法通过检测新息方差值修正量测误差来提高定位准确性。在模拟掘进工作面环境下开展了惯导与“视觉+激光标靶”组合定位实验，采用改进前后Sage-Husa自适应滤波算法融合惯导与视觉信息进行对比分析，结果表明：改进后Sage-Husa自适应滤波算法融合得到的定位误差更小，俯仰角、横滚角、航向角最大误差分别为0.029°、0.051°、0.011 3°，在距离激光标靶30 m内巷道宽度位置误差在0.033 m范围内，巷道掘进方向位置误差在0.062 m范围内。所提出的惯导与视觉融合定位方法能够满足巷道掘进定位精度要求。
-
关键词:
- 掘进机定位 /
- 惯导 /
- 机器视觉 /
- EPnP /
- Sage-Husa自适应滤波
Abstract:For the difficulty of pinpointing of tunneling machines in the complex environment of coal mine tunnels,a precise positioning method of roadheader based on inertial navigation and visual fusion is proposed, which uses a positioning scheme of inertial navigation plus "vision+laser target". With the designed four feature point large-sized laser target fixed on the roof of the tunnel and the camera on the tunneling machine to collect laser target images, the scheme uses the circular fitting method to locate the center of the light spot and the EPnP algorithm based on four feature points to calculate the position of the tunneling machine. A “vision+laser target” position detection experiment was conducted in simulated condition of heading face to verify the effectiveness of the method in detecting the position of tunneling machines. The results are as follows: within 30 meters tunnel, the maximum error in the width, position, and height are no more than 28.549, 78.868, and 44.459 mm, which shows accurate detection of the position of the tunneling machine. For the inaccurate pose detection of tunneling machines in inertial navigation measurement, induced by the accumulation of pose errors over time and the machine vibration on the combined positioning system, an improved Sage-Husa adaptive filtering method for inertial navigation and visual information fusion is proposed, which could correct measurement errors by detecting new variance values to improve positioning accuracy. A combined positioning experiment of inertial navigation plus “vision+laser target” was conducted in a simulated heading face environment, using improved Sage-Husa adaptive filtering algorithm to fuse inertial navigation and visual information for comparative analysis, which show that the improved Sage-Husa adaptive filtering algorithm fusion results in smaller positioning errors. The maximum errors of pitch angle, roll angle, and heading angle are 0.029°, 0.051°, and
0.0113 °, respectively. Within 30 meters, the position error of tunnel width is within the range of 0.033 m, and the position error of tunnel excavation direction is within the range of 0.062 m. Overall, the proposed inertial navigation plus visual fusion positioning method can meet the accuracy requirements of tunnel excavation positioning.-
Keywords:
- tunneling machine /
- inertial navigation /
- machine vision /
- EPnP /
- sage-husa filtering
-
0. 引 言
煤炭在未来较长时间内，仍将是我国的主体能源。2020–02–25，国家发展改革委等八部委联合印发了《关于加快煤矿智能化发展的指导意见》，明确了煤矿智能化的发展目标和主要任务，对采掘工作面的智能化提出了更高的要求[1–2]。综采工作面智能化初见成效，而综掘工作面智能化严重滞后，导致“采快掘慢，接续失衡”。掘进装备智能化程度不高已成为制约煤矿高产高效的主要原因，而掘进机精确定位技术是目前影响煤矿掘进装备的智能化水平的核心技术之一[3]。因此，研究对煤矿巷道安全、高效、智能掘进具有极其重要的意义。
煤矿巷道掘进机精确定位技术已成为研究热点，目前国内外学者主要探索了惯导定位技术、iGPS定位技术、全站仪定位技术、机器视觉定位技术等[4]。朱信平等[5]提出了一种基于全站仪的掘进机机身位置测量方法，但全站仪激光易受煤矿环境粉尘和设备影响，导致全站仪光路被遮挡而无法进行定位。张旭辉等[6]提出了以激光光斑为点特征的掘进机机身位姿视觉测量技术，但该方法存在井下设备遮挡光路问题。贾文浩等[7]提出了一种基于iGPS测量原理的掘进机定位方法，通过测量激光接收器在发射站坐标系下的三维坐标对掘进机位置精度检测，但由于粉尘遮挡、动态测量的影响，导致总体定位精度不高，局限性较大。陶云飞等[8]提出了基于惯性导航技术的掘进机位姿测量系统，但单独利用惯导系统测量掘进机位姿存在误差随时间累积问题。通过上述分析，受巷道环境及掘进装备运动复杂性的影响，单一定位技术无法满足掘进装备精确定位检测要求，组合惯导定位有望解决掘进装备精确定位难题[9]。因此，国内外学者开展了组合惯导的定位方法研究，包括惯导与全站仪融合定位技术、惯导与里程计组合定位技术、惯导与超宽带融合定位技术等[10–11]。刘超等[12]提出了基于UWB测距与TSOA原理混合算法的掘进机位姿检测方法，但其精度随基站与机身距离增大而降低。刘宇等[13]借助捷联惯导和里程计构建掘进机位姿曲线测量系统，但掘进机履带存在打滑、掘进机左右移机和里程计累积误差等导致系统测量误差较大。卢新明等[14]构建了基于惯性导航仪、指北仪和具有跟踪功能的全站仪的掘进机定位系统，但该系统的数字全站仪光路易受煤矿井下粉尘、设备遮挡和底板起伏影响导致定位不准。
随着多传感器组合定位系统的发展，有效融合不同传感器测量信息已成为进一步研究方向。目前通常运用卡尔曼滤波(Kalman Filtering, KF)算法实现组合定位系统的数据融合，标准卡尔曼滤波适用于线性系统或可近似于线性的非线性系统，对于非线性问题，卡尔曼滤波很难获得准确的估计结果[15–16]。针对非线性系统，目前使用较为广泛的是扩展卡尔曼滤波(Extended Kalman Filter, EKF)方法。EKF通过对非线性函数进行泰勒级数展开，可以得到线性化的近似结果[17]。但扩展卡尔曼滤波存在高阶偏导数省略问题，且状态噪声与观测噪声的协方差矩阵固定，若协方差估计存在偏差，则容易导致滤波器发散。当系统模型参数不够准确时，需要根据量测输出对部分参数进行重新估计建模。1969年，学者A P Sage和G W Husa提出了一种自适应滤波算法[18–19]，其核心是通过量测输出对系统和量测噪声的特性进行动态的统计估算，并最终解算出所求的状态参数的预测值，减少由模型误差参数带来的影响。但滤波过程中，若量测值存在较大误差，便会导致新息序列产生误差，定位精度降低，无法实现在变化场景下的应用。
综上所述，采用大尺寸光斑成像标靶改进视觉测量装置，解决视觉定位方法在巷道狭长空间中长距离成像效果差的问题，通过光斑特征点坐标解算实现掘进机位置精确检测。提出改进Sage-Husa自适应滤波的惯导与视觉信息融合算法，该方法根据新息方差检测量测故障并调整量测噪声方差系数，从而抑制随机噪声对量测的干扰和惯导定位误差随时间累积问题，对提高煤矿掘进机定位精度具有重要意义。
1. 掘进机定位系统方案
1.1 定位系统坐标系
为准确描述掘进机在巷道空间中的位姿，在掘进机位姿检测前，首先需建立一系列坐标系，如图1所示。
![]()
图 1 “视觉+激光标靶”系统结构及坐标系示意Figure 1. Schematic of the “Vision+Laser Target” system structure and coordinate system1)巷道坐标系(简称h系)$ {O_h}{X_h}{Y_h}{Z_h} $，坐标原点Oh位于巷道初始位置，Xh轴指向巷道右侧，表示巷道宽度方向，Yh轴与巷道中轴线平行并指向巷道掘进方向，Zh轴垂直于巷道地面向上，表示巷道高度方向。
2)视觉坐标系(简称v系)$ {O_v}{X_v}{Y_v}{Z_v} $，在视觉定位系统中，以激光点为基准完成掘进机机身定位。成像标靶某一光斑所在位置Ov为坐标原点，Xv轴平行于标靶，Yv轴与巷道中轴线平行并指向掘进机前进方向，Zv轴竖直向上。
3)机身坐标系(简称b系)$ {O_b}{X_b}{Y_b}{Z_b} $，以掘进机机身重心为原点Ob，Xb轴水平向右与机身侧面垂直，Yb轴沿机身纵轴线指向前进方向、Zb轴垂直于掘进机顶板向上。
1.2 组合定位方案
掘进机惯导与“视觉+激光标靶”组合定位系统包括基于惯性导航系统的机身位姿参数测量、基于“视觉+激光标靶”的机身位置参数测量、基于Sage-Husa自适应滤波的数据融合算法，组合定位原理如图2所示。
将惯导安装在掘进机机身上，通过捷联惯导获取掘进机的姿态角、位置信息；为了避免激光标靶被设备遮挡，工业相机通过云台安装在掘进机机身上，相机通过位姿解算的数据来判断是否受到遮挡，当相机解算不到位姿数据，就把信息反馈给云台，云台控制相机旋转直到找到标靶重新解算出位姿数据。相机解算数据反馈给云台时，通常使用反馈控制系统，该系统通过控制信号将调整指令传递给云台。对激光成像系统初始位置进行标定，并建立v系。激光点投射在成像标靶形成光斑，利用工业相机获取光斑图像，通过对图像进行特征提取获得光斑特征点坐标，并运用EPNP算法对特征点进行解算，得到掘进机位置信息。采用Sage-Husa自适应滤波方法将惯导测量的位姿信息与视觉测量的掘进机位置信息融合，从而得到掘进机的精确定位结果。
2. 基于“视觉+激光标靶”的掘进机机身位置检测方法
2.1 激光标靶设计
为了研究相机与激光源距离对特征点间距的限制条件，即特征点间距与测量范围关系，对工业相机参数进行选型。试验选用MV–EM200 M工业相机，相机参数见表1、2。
项目 参数 最高分辨率/（px×px） 1 600×1 200 像素尺寸/(μm×μm) 4.4×4.4 光学尺寸 1/1.8'' 有效感光面积/(mm×mm) 7×5.3 最大帧率/fps 20 输出颜色 黑白 传输距离/m 100 表 2 镜头参数Table 2. Lens Parameters项目 参数 分辨率/m 5 焦距/mm 8 视场角/DHV 67.1°×56.3°×43.7° 畸变/(°) <1 光圈 F=1:1.4～16 聚焦/m 0.1～0.9 像面尺寸 2/3" 激光成像标靶选用半透磨砂有机玻璃，化学名称为聚甲基丙烯酸甲酯(PMMA)，具有透光性能好、使用寿命长等优点，对光的颜色和强度影响都很小。PMMA的密度小、机械强度高、抗拉伸和抗冲击能力较好，透光率92%。激光源系统由激光点和光斑成像材料组成，激光点由激光指向仪发出，光斑成像材料选择半透磨砂有机玻璃，由于此材料既不会完全遮挡光源，并且能够使一部分光能集中在标靶上，减少光在空气中的散射，同时单面磨砂有利于提升光斑成像质量。
为了研究“视觉+激光标靶”测量系统的有效工作距离，需要根据工业相机与镜头参数进行理论计算。
$$ {D_{\mathrm{w}}} = f \times {\text{CC}}{{\text{D}}_{\mathrm{s}}}/{\text{FOV}} $$ (1) 其中，Dw为工作距离；f为焦距；CCDs为CCD的芯片尺寸；FOV为视场角。代入参数计算后得到Dw为65.96 m，考虑到煤矿井下巷道环境存在复杂干扰因素，为了提高“视觉+激光标靶”系统定位稳定性，故设置系统最大工作距离为60 m。并且有效工作距离是特征点最小间距的限制条件，结合相机拍摄激光源的有效工作距离计算特征点最小间距，根据上式可得：
$$ {\text{FOV}} = f \times {\text{CC}}{{\text{D}}_{\mathrm{s}}}/{D_{\mathrm{w}}} $$ (2) 根据视场角，计算最大工作距离时的有效拍摄范围大小(场景宽)：
$$ {R_{\max }} = 2{D_{\mathrm{w}}} \sin ({\text{FOV}}/2) $$ (3) 式中，$ {R_{\max }} $为最大拍摄范围，根据特征点有效像素得到特征点最小间距：
$$ {\mathrm{p{x_e}}}/{{\mathrm{px}}_{\max }} = {D_{\mathrm{c}}}/{R_{\max }} $$ (4) 式中，$ {{\mathrm{px_e}}} $为有效像素距离；$ {{\mathrm{px}}_{\max }} $为最高分辨率像素；Dc为特征点最小间距。设每60 m对激光源系统进行一次移机，即视野距离=60 m，视场角为33.13°，计算此时场景宽为24 m。经测试，光斑在图像中直径最小6 px，最大12 px，为了避免光斑间相互影响，故设定两个光斑至少相距3倍光斑直径的距离，即36 px。根据以上信息计算得出特征点间距=0.72 m。参考特征点间距要求并且考虑到煤矿井下环境会对图像采集与特征点识别产生干扰，故将成像标靶材料尺寸设置为1 m×1 m，标靶结构如图3所示。选择YHJ800型矿用本安型激光指向仪作为激光发射点，激光指向仪的光束质量好、体积小、重量轻、安装使用简便、性能稳定可靠，广泛用于矿山巷道掘进时的指向定位。
2.2 光斑中心定位方法
激光器激光发射到接收标靶后，所形成的光斑为类圆形，且可以使用图像预处理算法提取出光斑图像中清晰的圆形光斑边缘，因此通过圆拟合的中心定位算法计算光斑中心。圆拟合算法的精度较高，运算速度快，仅需要遍历光斑轮廓的像素点即可计算出中心坐标及光斑尺寸。但其抗干扰性能较差，因此，设计了基于激光标靶的图像采集方案，并采用了多尺度Retinex算法对图像进行增强，其次采用Canny算子提取光斑边缘，边缘较为清晰，提升了圆拟合法性能。
圆拟合法的基本原理是利用最小二乘法来获得与光斑圆最为接近的拟合圆，用圆曲线逼近实际光斑边缘。圆曲线的方程为
$$ {(x - a)^2} + {(y - b)^2} = {r^2} $$ (5) 取残差为
$$ {\varepsilon _i} = {({x_i} - a)^2} + {({y_i} - b)^2} - {r^2} $$ (6) 式中，$ i \in E $，E为图像中圆边缘像素的集合；$ ({x_i},{y_i}) $为圆曲线边界点坐标。残差平方和为
$$ \theta = \sum\limits_{i \in E} {\varepsilon _i^2 = \sum\limits_{i \in E} {{{[{{({x_i} - a)}^2} + {{({y_i} - b)}^2} - {r^2}]}^2}} } $$ (7) 根据最小二乘法原理，存在约束条件：
$$ \frac{{\partial \theta }}{{\partial a}} = \frac{{\partial \theta }}{{\partial b}} = \frac{{\partial \theta }}{{\partial r}} = 0 $$ (8) 即：
$$ \left\{ \begin{gathered} \frac{{\partial \theta }}{{\partial a}} = 2\sum\limits_{i \in E} {{{[{{({x_i} - a)}^2} + {{({y_i} - b)}^2} - {r^2}]}^2} - 2({x_i} - a) = 0} \\ \frac{{\partial \theta }}{{\partial b}} = 2\sum\limits_{i \in E} {{{[{{({x_i} - a)}^2} + {{({y_i} - b)}^2} - {r^2}]}^2} - 2({y_i} - b) = 0} \\ \frac{{\partial \theta }}{{\partial r}} = 2\sum\limits_{i \in E} {{{[{{({x_i} - a)}^2} + {{({y_i} - b)}^2} - {r^2}]}^2} - 2r = 0} \\ \end{gathered} \right. $$ (9) 化简后，求得：
$$ \left\{ \begin{gathered} a = \frac{{(\overline {{x^2}} \overline x + \overline x \overline {{y^2}} - \overline {{x^3}} - \overline x \overline {{y^2}} )({{\overline y }^2} - \overline {{y^2}} ) - (\overline {{x^2}} \overline y + \overline y \overline {{y^2}} - \overline {{x^2}y} - \overline {{y^3}} )(\overline x \overline y - \overline {xy} )}}{{2({{\overline x }^2} - \overline {{x^2}} )({{\overline y }^2} - \overline {{y^2}} ) - 2{{(\overline x \overline y - \overline {xy} )}^2}}} \\ b = \frac{{(\overline {{x^2}} \overline y + \overline y \overline {{y^2}} - \overline {{y^3}} - \overline {{x^2}} \overline y )({{\overline x }^2} - \overline {{x^2}} ) - (\overline {{x^2}} \overline x + \overline x \overline {{y^2}} - \overline {x{y^2}} - \overline {{x^3}} )(\overline x \overline y - \overline {xy} )}}{{2({{\overline x }^2} - \overline {{x^2}} )({{\overline y }^2} - \overline {{y^2}} ) - 2{{(\overline x \overline y - \overline {xy} )}^2}}} \\ r = \sqrt {{a^2} - 2\overline x a + {b^2} - 2\overline y b + \overline {{x^2}} + \overline {{y^2}} } \\ \end{gathered} \right. $$ (10) 由式(10)可以推出，检测算法仅需遍历所有边界坐标一次，即可计算出满足残差平方和最小的光斑中心坐标(a,b)及其半径r。
2.3 基于四特征点EPnP的掘进机位置解算方法
PnP(Perspective-n-Point)算法是基于点特征的位姿估计问题，可以求解3D–2D点对运动[20–21]。PnP算法根据给定3D点坐标、对应图像上的2D点坐标以及内参矩阵，求解相机位置。需要至少3对3D–2D点对，相机的绝对位姿方可被估计，即经典的P3P问题[22]。由于P3P估计算法具有多解性，需要已知第4个3D–2D点确定惟一解[23]。因此，选取四特征点作为研究对象。在PnP问题的研究中大多都假定相机内参数是已知的，即相机是标定的。
EPnP的算法原理如图4所示，复杂度为O(n)，其他算法的复杂度多为O(n3)，所以对于特征点较多的PnP问题，非常高效。EPnP核心思想是将3D点表示为4个特征点的组合，优化也只针对4个特征点，所以速度较快[24–25]。
![]()
图 4 EPnP算法的位置解算原理Figure 4. Schematic diagram of EPnP algorithm for position calculation上标w表示视觉坐标系下坐标，c表示相机坐标系下坐标。3D光斑参考点在视觉坐标系下的坐标为$ P_i^{\mathrm{w}},i = 1, \cdots ,n $，在相机坐标系下的坐标为$ P_i^{\mathrm{c}}, i = 1, \cdots ,n $；4个特征点在视觉坐标系下的坐标为$ c_j^{\mathrm{w}},j = 1, \cdots ,4 $，在相机坐标系下的坐标$ c_j^{\mathrm{c}},j = 1, \cdots ,4 $。用特征坐标线性表示参考点：
$$ P_i^{\mathrm{w}} = \sum\limits_{j = 1}^4 {{\alpha _{ij}}c_j^{\mathrm{w}},\sum\limits_{j = 1}^4 {{\alpha _{ij}}} } = 1 $$ (11) 其中，$ {\alpha _{ij}} $为齐次重心坐标，在相机坐标系中，存在同样的加权和关系：
$$ P_i^{\mathrm{c}} = \sum\limits_{j = 1}^4 {{\alpha _{ij}}c_j^{\mathrm{c}}} $$ (12) 设K为相机的内参矩阵，可以通过标定获得。$ {\left\{ {{u_i}} \right\}_{i = 1, \cdots ,n}} $是参考点$ {\left\{ {{p_i}} \right\}_{i = 1, \cdots ,n}} $的像素坐标，那么：
$$ \left[ {\begin{array}{*{20}{c}} {{u_i}} \\ 1 \end{array}} \right] = {\boldsymbol{K}}p_i^{\mathrm{c}} = {\boldsymbol{K}}\sum\limits_{j = 1}^4 {{\alpha _{ij}}c_j^{\mathrm{c}}} $$ (13) 将$ {\boldsymbol{c}}_j^{\rm{c}} = {\left[ {x_j^{\rm{c}},y_j^{\rm{c}},z_j^{\rm{c}}} \right]^{\rm{T}}} $代入上式，而且把K写成焦距$ {f_u} $，$ {f_v} $和光心$ \left( {{u_c},{u_v}} \right) $的形式：
$$ {\omega _i}\left[ {\begin{array}{*{20}{c}} {{u_i}} \\ {{v_i}} \\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{f_u}}&0&{{u_c}} \\ 0&{{f_v}}&{{v_c}} \\ 0&0&1 \end{array}} \right]\sum\limits_{j = 1}^4 {{\alpha _{ij}}\left[ {\begin{array}{*{20}{c}} {x_j^{\rm{c}}} \\ {y_j^{\rm{c}}} \\ {z_j^{\rm{c}}} \end{array}} \right]} $$ (14) 由上式可得：
$$ \begin{gathered} \sum\limits_{j = 1}^4 {{\alpha _{ij}}{f_u}x_j^{\rm{c}} + {\alpha _{ij}}\left( {{u_c} - {u_i}} \right)z_j^{\rm{c}} = 0} \\ \sum\limits_{j = 1}^4 {{\alpha _{ij}}{f_v}y_j^{\rm{c}} + {\alpha _{ij}}\left( {{v_c} - {v_j}} \right)z_j^{\rm{c}} = 0} \\ \end{gathered} $$ (15) 由n个空间点和图像点之间的对应关系得到方程组为
$$ {\boldsymbol{M}}{\boldsymbol{x}} = 0,{\boldsymbol{x}} = {\left[ {c_1^{{c^{\rm{T}}}},c_2^{{c^{\rm{T}}}},c_3^{{c^{\rm{T}}}},c_4^{{c^{\rm{T}}}}} \right]^{\rm{T}}} $$ (16) x为特征点在相机坐标系下的坐标，且x的解为矩阵M的核空间，即：
$$ {\boldsymbol{x}} = \sum\limits_{i = 1}^N {{\beta _i}{{\boldsymbol{v}}_i}} $$ (17) 式中，$ {{\boldsymbol{v}}_i} $为M的零特征值对应的N特征向量；$ {\beta _i} $为待定系数。
第i个特征点的相机坐标：
$$ c_i^{\rm{c}} = \sum\limits_{k = 1}^N {{\beta _k}{\boldsymbol{v}}_k^{[i]}} $$ (18) 式中，$ {\boldsymbol{v}}_k^{[i]} $为特征向量$ {{\boldsymbol{v}}_k} $的第i个子向量。
特征点在相机坐标系和视觉坐标系的相对位置关系不会发生改变，两个特征之间的相对位置约束可表示为
$$ \left\| {\sum\limits_{k = 1}^N {{\beta _k}v_k^{[i]} - \sum\limits_{k = 1}^N {{\beta _k}v_k^{v[j]}} } } \right\| = \left\| {c_i^{\rm{w}} - c_j^{\rm{w}}} \right\| $$ (19) 由于$ \left\| {c_i^{\rm{w}} - c_j^{\rm{w}}} \right\| $距离已知，可得：
$$ \beta = \frac{{ \displaystyle \sum\limits_{[i,j] \in [1;4]} {\left\| {{v^{[i]}} - {v^{[j]}}} \right\| \left\| {c_i^{\rm{w}} - c_j^{\rm{w}}} \right\|} }}{{ \displaystyle \sum\limits_{[i,j] \in [1;4]} {{{\left\| {{v^{[i]}} - {v^{[j]}}} \right\|}^2}} }} $$ (20) 求解得到4个特征点在相机参考坐标下的坐标，计算3D参考点在相机参考坐标系下的坐标，通过迭代最近点得到相机在视觉坐标系下的位置。由于视觉坐标系xv，yv，zv轴分别平行于巷道坐标系xh，yh，zh轴，故通过标定视觉坐标系原点ov在巷道坐标系下坐标，将相机位置转换到巷道坐标系下。
3. 基于改进Sage-Husa自适应滤波的惯导与视觉融合算法
捷联惯导系统虽然具有良好的自主定位性能，但其定位误差会随时间漂移而逐渐累积，而机器视觉定位方法具有非接触、有效距离长等优点，视觉测量系统能够弥补这一缺点。为了提高掘进机位姿测量的稳定性和准确性，在捷联惯导与视觉定位的研究基础上，结合两者优势，提出一种基于惯导和视觉融合的定位方法，实现两者信息融合，保障煤矿巷道掘进机长时间的精确定位。但融合定位个别数据误差较大，常规的Sage-Husa自适应滤波，随着滤波进行会因观测量误差增大而导致新息中引入大量误差，从而导致滤波结果发散，而改进的Sage-Husa自适应滤波引入新息误差跟踪，比较测量值和预测值之间的差异，来估计系统中的误差，并根据这些误差进行修正。
采用改进Sage-Husa自适应滤波算法对惯导和视觉信息进行融合，改进Sage-Husa自适应滤波原理如图5所示。以惯导与视觉测得的位置之差作为观测值，建立组合定位状态方程和量测方程，通过量测值调整噪声模型，根据新息方差检测量测故障并调整量测噪声方差系数，抑制随机噪声的干扰。
3.1 组合定位状态向量
组合定位的系统状态向量为
$$ {\boldsymbol{X}} = {\left[ {\begin{array}{*{20}{c}} {{\varphi ^{\text{T}}}}&{\delta {{\boldsymbol{v}}^{\text{T}}}}&{\delta {{\boldsymbol{p}}^{\rm{T}}}}&{{{\boldsymbol{\varepsilon }}^{\text{T}}}}&{{{\nabla }^{\text{T}}}} \end{array}} \right]^{\text{T}}} $$ (21) 式中，$ \varphi $为姿态误差角；$ \delta {\boldsymbol{v}} $为速度误差；$ \delta {\boldsymbol{p}} $为位置误差；$ {\boldsymbol{\varepsilon }} $为陀螺仪误差；$ {\nabla } $为加速度计误差。
以惯导与视觉测得的位置之差作为观测值，系统离散数学模型为
$$ \left\{ \begin{gathered} \dot {\boldsymbol{X}} = {\boldsymbol{\phi}} X + {\boldsymbol{GW}} \\ {\boldsymbol{Z}} = \left[ {\begin{array}{*{20}{c}} {{x^{INS}} - {x^V}} \\ {{y^{INS}} - {y^V}} \end{array}} \right] = {\boldsymbol{H}}{\boldsymbol{X}} + {\boldsymbol{V}} \\ \end{gathered} \right. $$ (22) 式中，X为离散状态向量；${\boldsymbol{ \phi}} $为状态一步转移矩阵；G为系统噪声分配矩阵；W为系统噪声向量；Z为离散量测向量；H为量测矩阵；V为量测噪声向量；W和V都是零均值的高斯白噪声向量序列，其统计特性满足以下条件：
$$ \left\{ \begin{gathered} E\left[ {{W_k}} \right] = 0,E\left[ {{W_k}W_j^{\text{T}}} \right] = {Q_k}{\delta _{k,j}} \\ E\left[ {{V_k}} \right] = 0,E\left[ {{V_k}V_j^{\text{T}}} \right] = {R_k}{\delta _{k,j}} \\ E\left[ {{W_k}V_j^{\text{T}}} \right] = 0 \\ \end{gathered} \right. $$ (23) 式中，$ {R_k} $为过程噪声的方差；$ {Q_k} $为测量噪声的方差。
3.2 Sage-Husa自适应卡尔曼滤波
Sage-Husa自适应卡尔曼滤波递推过程如下：
$$ \left\{ \begin{gathered} {{\hat X}_{k/k - 1}} = {\phi _{k/k - 1}}{{\hat X}_{k - 1}} \\ {P_{k/k - 1}} = {\phi _{k/k - 1}}{P_{k/k - 1}}\phi _{k/k - 1}^{\rm{T}} + {Q_{k - 1}} \\ {e_k} = {Z_k} - {H_k}{{\hat X}_{k/k - 1}} \\ {K_k} = {P_{k/k - 1}}H_k^{\rm{T}}{({H_k}{P_{k/k - 1}}H_k^{\rm{T}} + {{\hat R}_k})^{ - 1}} \\ {{\hat X}_k} = {{\hat X}_{k/k - 1}} + {K_k}{e_k} \\ {P_k} = (I - {K_k}{H_k}){P_{k/k - 1}} \\ {{\hat Q}_k} = (1 - {d_k}){{\hat Q}_{k - 1}} + {d_k}({K_k}{e_k}e_k^{\rm{T}}K_k^{\rm{T}} + {P_k} - {\phi _{k/k - 1}}{P_{k - 1}}\phi _{k/k - 1}^{\rm{T}}) \\ {{\hat R}_k} = (1 - {d_k}){{\hat R}_{k - 1}} + {d_k}({e_k}e_k^{\rm{T}} - {H_k}{P_{k/k - 1}}H_k^{\rm{T}}) \\ \end{gathered} \right. $$ (24) $$ {d_k} = \frac{{1 - b}}{{1 - {b^{k + 1}}}} $$ (25) 式中，$ {{\hat{\boldsymbol X}}_{k/k - 1}} $为根据$ {{\hat{\boldsymbol X}}_{k - 1}} $和系统的状态方程对k时刻状态$ {{\boldsymbol{X}}_k} $作最优估计值，即最优一步预测值；$ {{\hat{\boldsymbol X}}_{k - 1}} $为$ k - 1 $时刻的状态最优估计；$ {P_{k - 1}} $为状态估计均方误差阵；$ {e_k} $为新息序列；$ {K_k} $为滤波增益；$ 1 - {d_k} $和$ {d_k} $为自适应系数，其作用是解算时的权重调整，初值$ {d_0} = 1 $，而$ 0 \lt b \lt 1 $称为渐消因子。渐消因子b取得越小，对量测噪声变化的自适应能力就越强；渐消因子b也不能取得过小，否则噪声估计结果会出现剧烈跳变，常取$ b = 0.9\sim 0.99 $。
新息值被用来检验外部观测值的有效性，并决定是否将当前观测值添加到滤波器进行更新迭代。因此新息在一定程度上体现卡尔曼滤波的估计性能。但滤波过程中，若量测值存在较大误差，便会导致新息序列产生误差，定位精度降低，无法实现在变化场景下的应用。
3.3 量测误差跟踪
采用渐消加权方法，新息序列方差阵为
$$ {\hat C_k} = E[{{\boldsymbol{e}}_k}{\boldsymbol{e}}_k^{\text{T}}] = {{\boldsymbol{H}}_k}({{\boldsymbol{\phi}} _{k/k - 1}}{P_{k - 1}}{\boldsymbol{\phi}} _{k/k - 1}^{\text{T}} + {Q_{k - 1}}){\boldsymbol{H}}_k^{\text{T}} + {{\boldsymbol{R}}_k} $$ (26) 当建模不够准确时，上式中参数可能会存在一定误差，导致等式两端不相等，即新息方差阵失配，本文将从量测值和量测方差两方面进行改善。
1）量测故障的检测
对式(20)两端进行迹运算：
$$ {\text{tr(}}{\hat C_k}{\text{)}} \gg {\text{tr(}}{{\boldsymbol{H}}_k}({{\boldsymbol{\phi}} _{k/k - 1}}{P_{k - 1}}{\boldsymbol{\phi}} _{k/k - 1}^{\text{T}} + {Q_{k - 1}}){\boldsymbol{H}}_k^{\text{T}} + {{\boldsymbol{R}}_k}{\text{)}} $$ (27) 当系统参数误差较小，但信息方差阵失配情况却较为严重时，极大可能是量测值出现异常，从而导致新息方差阵增大。此时将该量测值$ {Z_k} $认定为坏值并进行隔离，不进入卡尔曼滤波的量测更新，避免量测故障带来的误差。
2)量测方差自适应滤波
假设系统参数$ {\phi _{k/k - 1}} $，$ {H_k} $，$ {Q_k} $的误差较小，且新息失配情况较为轻微时，则认为是由量测方差阵$ {R_k} $带来的误差，实时对其进行调整：
$$ {{\hat{\boldsymbol C}}_k} - {H_k}({\phi _{k/k - 1}}{P_{k - 1}}\phi _{k/k - 1}^{\text{T}} + {Q_{k - 1}})H_k^{\text{T}} = {\alpha _k}{R_k} $$ (28) 式中，比例系数$ {\alpha _k} $可定义为
$$ {\alpha _k} = \frac{{{\text{tr(}}{{{\hat{\boldsymbol C}}}_k} - {H_k}({\phi _{k/k - 1}}{P_{k - 1}}\phi _{k/k - 1}^{\text{T}} + {Q_{k - 1}})H_k^{\text{T}}{\text{)}}}}{{{\text{tr(}}{{\text{R}}_k}{\text{)}}}} $$ (29) 将修正后的$ {\alpha _k}{R_k} $代替$ {R_k} $用于Kalman滤波量测更新，从而实现量测噪声方差的自适应滤波。
4. 试验验证与结果分析
4.1 “视觉+激光标靶”位置测量试验
1)试验平台
为了验证“视觉+激光标靶”位置检测的准确性，在学校煤炭主体实验室搭建测量系统试验平台，模拟掘进工作面长30 m，如图6所示。“视觉+激光标靶”位置检测系统试验平台主要包括4个激光指向仪、光斑成像标靶、工业相机和上位机等。激光指向仪开启后，对激光位置进行微调，保证产生的4条激光线相互平行且平行于巷道墙壁，激光线水平连线两两平行且平行与地面，激光器水平间距为0.9 m，竖直间距为0.74 m。成像标靶竖直放置于激光器前，调节激光与标靶间距直至成像完整且不含过多杂乱信息为止。相机固定于掘进机机身，拍摄激光指向仪在成像标靶上产生的光斑，并将图像传输到上位机进行特征提取并解算。
2)试验过程
在巷道中移动激光源和成像标靶，以激光标靶和相机的相对运动模拟掘进机在巷道内移动，对视觉位置检测系统进行测试与验证。考虑到在掘进机移动过程中，激光的散射情况会随着距离增大而加剧，且距离越远，图像上的光斑越模糊，对定位精度影响越大，同时由于模拟掘进工作面巷道长为30 m，故分别在相机距离标靶5、10、15、20、25、30 m处各进行了20组定点测试。采集到的图像及处理结果如图7所示。
以全站仪测得的位置作为真实值，根据图像解算掘进机的位置信息，并将测量结果与真实值进行对比。对“视觉+激光标靶”系统测量的位置信息进行评估，分别计算X、Y、Z三个方向的真实值与测量值的误差值如图8所示。机身位置测量数据与真实值误差平均值见表3，误差最大值见表4。
表 3 定位平均误差Table 3. Average positioning error平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 2.706 5.224 3.282 10 5.055 14.258 8.657 15 6.564 25.871 12.213 20 11.848 31.899 13.102 25 12.564 38.431 21.874 30 15.064 44.087 21.346 表 4 定位最大误差Table 4. Maximum positioning error平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 4.925 13.433 7.984 10 6.738 25.896 13.392 15 8.317 49.755 22.563 20 18.103 56.729 29.600 25 26.694 68.563 37.863 30 28.549 78.868 44.459 3)试验结果分析
从以上数据可以看出，机身位置在巷道宽度方向的测量平均误差在5 m处为2.706 mm，30 m处增加到15.064 mm；掘进方向的测量平均误差5 m处为5.224 mm，30 m处增加到44.087 mm；高度方向的测量平均误差在5 m处为3.282 mm，30 m处增加到21.346 mm；沿巷道宽度方向、掘进方向、高度方向最大误差不超过28.549 mm、78.868 mm、44.459 mm。结果表明：测量误差会随测量距离的增大而增大，即距离远近对测量精度具有一定影响。但总体误差在掘进机机身位置测量精度允许范围内且宽度方向的测量结果与真实值较为接近，验证了“视觉+激光标靶”位置测量方法具有较高精度。
4.2 惯导与视觉融合定位试验
为了验证文中提出的捷联惯导与“视觉+激光标靶”组合定位系统的有效性，在学校煤炭主体实验室掘进工作面搭建测量系统试验平台。保持捷联惯导与相机在掘进机上同步运动，激光标靶固定。
1)姿态角测量精度验证。由于测量距离对于姿态角精度影响较小，故仅在巷道最远距离，即“视觉+激光标靶”系统测量距离为30 m时，进行姿态角测量试验。掘进机处于静置状态，以姿态角初始数值作为参考值与组合定位姿态角结果比较，如图9所示。试验结果表明：整体姿态角变化较为稳定，俯仰角、横滚角、航向角最大误差分别为0.029°、0.051°、0.011 3°，测量精度较高，能够反映出掘进机在掘进过程中的姿态角信息。
2)位置测量精度验证。测量距离对于组合定位系统位置数据测量精度的影响较大，故分别在激光源系统距离为10、20、30 m处进行试验。采用改进前后Sage-Husa自适应滤波算法对惯导与视觉进行信息融合，以数字全站仪测量的位置数据作为掘进机实际位置，与组合定位系统测量值进行对比。捷联惯导和“视觉+激光标靶”组合定位曲线如图10a、11a、12a所示，组合定位系统巷道宽度(X)方向位置测量值与实际值间误差如图10b，11b，12b所示，掘进(Y)方向位置测量值与实际值间误差如图10c、11c、12c所示。
![]()
图 10 距离标靶10 m时位置测量结果Figure 10. Position measurement results at a distance of 10 m from the target![]()
图 11 距离标靶20 m时位置测量结果Figure 11. Position measurement results at a distance of 20 m from the target![]()
图 12 距离标靶30 m时位置测量结果Figure 12. Position measurement results at a distance of 30 m from the target3)消融试验结果分析。通过消融试验将Sage-Husa自适应滤波解算轨迹和改进Sage-Husa自适应滤波解算所得轨迹与掘进机实测移动轨迹进行比较分析，所得宽度方向和掘进方向误差与均方根误差见表5。
表 5 Sage-Husa自适应滤波改进前后定位结果Table 5. Positioning results before and after Sage-Husa adaptive filtering improvementSage-Husa自适应 新息误差跟踪 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m √ 0.055 1 0.082 2 0.089 3 0.080 2 0.102 4 0.202 8 0.056 6 0.056 9 0.050 7 0.098 8 0.044 2 0.115 9 √ √ 0.011 2 0.021 5 0.033 2 0.020 5 0.041 3 0.062 3 0.017 6 0.013 0 0.018 2 0.031 1 0.021 1 0.035 8 注：√代表使用该方法。 由图10—图12中可以看出：与Sage-Husa自适应滤波融合的定位轨迹相比，改进后Sage-Husa自适应滤波融合后的掘进机融合定位曲线更接近实际位置曲线，位置误差更小，与真实结果更为接近。捷联惯导与视觉融合的定位方法测得掘进机位置误差与测量距离有关，即随着测量距离增大误差也随之增大。由表中也可以看出，运用改进方法后，在宽度方向和掘进方向上误差都有不同程度的下降，通过均方根误差的对比可知：在距离标靶10、20、30 m时，宽度X方向定位精度分别提升了68.9%、77.15%和64.1%；掘进Y方向定位精度分别提升了68.52%、52.26%和69.11%。且总体在掘进机定位要求范围内，巷道宽度方向上的位置误差在0.033 m范围内，掘进方向上的位置误差在0.062 m范围内。结果表明：组合定位系统抑制了纯惯导位置误差累积，提高了系统的定位精度，且通过改进算法进一步保证了系统的精确性和可靠性。
4.3 改进Sage-Husa自适应滤波算法与SOTA方法的性能对比
目前，多源信息融合技术在算法方面研究重点与应用多为卡尔曼滤波，通过融合多个传感器的信息，提高了系统的状态估计准确性，并具有更好的鲁棒性和可靠性。
为了验证改进的Sage-Husa自适应滤波算法性能，采用距离标靶30 m处的数据与其他SOTA方法进行实验对比。分别使用Sage-Husa自适应滤波、自适应无迹卡尔曼滤波算法、自适应平方根容积卡尔曼滤波算法与改进Sage-Husa自适应滤波算法进行比较，试验结果见表6、图13。
表 6 各算法解算最大误差与均方根误差Table 6. Maximum error and root mean square error of each algorithm融合算法 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE Sage-Husa自适应 0.089 3 0.202 8 0.050 7 0.115 9 自适应UKF 0.051 2 0.101 3 0.031 2 0.062 8 自适应SRCKF 0.034 6 0.073 4 0.021 4 0.042 8 改进Sage-Husa自适应 0.033 2 0.062 3 0.018 2 0.035 8 由表6可知，相比其他算法文中改进的Sage-Husa自适应滤波算法得到的误差更小，且宽度X方向的均方根误差由0.050 7降低到0.018 2，精度提升了64.10%；掘进Y方向的均方根误差由0.115 9降低到0.035 8，精度提升了69.11%。由此可以看出，与其他方法相比，文中提出的改进Sage-Husa自适应滤波算法的定位精度有所提升。
5. 结 论
1)针对煤矿井下粉尘大和物体遮挡影响视觉定位精度问题，提出了大尺寸激光标靶的图像采集方案，选择半透磨砂有机玻璃作为激光点成像标靶材料，并根据相机与镜头参数计算视觉测量的有效工作距离与标靶尺寸，改善了煤矿井下视觉定位系统工作不稳定问题，并在一定程度上提升了定位精度。
2)针对掘进机惯导定位误差随时间累积问题，提出了一种基于Sage-Husa自适应滤波的惯导与视觉信息融合方法，建立组合定位系统的状态方程与量测方程，利用遗忘因子预测量测噪声方差实现自适应滤波，通过检测新息序列方差阵实现量测故障的隔离和量测噪声方差比例系数的调整，从而抑制随机噪声的干扰和纯惯导位置误差累积。
3)在模拟掘进工作面环境下，开展了“视觉+激光标靶”位置测量试验，结果表明：30 m范围内沿巷道宽度方向、掘进方向、高度方向最大误差不超过28.549、78.868、44.459 mm，验证了“视觉+激光标靶”位置检测方法具有较高精度；开展了惯导与“视觉+激光标靶”组合定位实验，采用改进前后Sage-Husa自适应滤波算法融合惯导与视觉信息并进行对比分析，结果表明：改进后Sage-Husa自适应滤波算法融合得到的定位数据误差更小，俯仰角、横滚角、航向角最大误差分别为0.029°、0.051°、0.011 3°，30 m范围内巷道宽度方向上的位置误差在0.033 m范围内、掘进方向上的位置误差在0.062 m范围内，能够满足煤矿井下掘进装备精确定位要求。
4)主要在模拟掘进工作面环境下测试了组合定位系统的准确性，由于设备没有做防爆认证，无法进行真实场景的试验，将继续努力改进和完善，下一步到煤矿井下环境中进行试验验证。
-
![]()
图 1 “视觉+激光标靶”系统结构及坐标系示意
Figure 1. Schematic of the “Vision+Laser Target” system structure and coordinate system
![]()
图 4 EPnP算法的位置解算原理
Figure 4. Schematic diagram of EPnP algorithm for position calculation
![]()
图 10 距离标靶10 m时位置测量结果
Figure 10. Position measurement results at a distance of 10 m from the target
![]()
图 11 距离标靶20 m时位置测量结果
Figure 11. Position measurement results at a distance of 20 m from the target
![]()
图 12 距离标靶30 m时位置测量结果
Figure 12. Position measurement results at a distance of 30 m from the target
项目 参数 最高分辨率/（px×px） 1 600×1 200 像素尺寸/(μm×μm) 4.4×4.4 光学尺寸 1/1.8'' 有效感光面积/(mm×mm) 7×5.3 最大帧率/fps 20 输出颜色 黑白 传输距离/m 100 
下载: 导出CSV
表 2 镜头参数
Table 2 Lens Parameters
项目 参数 分辨率/m 5 焦距/mm 8 视场角/DHV 67.1°×56.3°×43.7° 畸变/(°) <1 光圈 F=1:1.4～16 聚焦/m 0.1～0.9 像面尺寸 2/3"
下载: 导出CSV
表 3 定位平均误差
Table 3 Average positioning error
平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 2.706 5.224 3.282 10 5.055 14.258 8.657 15 6.564 25.871 12.213 20 11.848 31.899 13.102 25 12.564 38.431 21.874 30 15.064 44.087 21.346
下载: 导出CSV
表 4 定位最大误差
Table 4 Maximum positioning error
平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 4.925 13.433 7.984 10 6.738 25.896 13.392 15 8.317 49.755 22.563 20 18.103 56.729 29.600 25 26.694 68.563 37.863 30 28.549 78.868 44.459
下载: 导出CSV
表 5 Sage-Husa自适应滤波改进前后定位结果
Table 5 Positioning results before and after Sage-Husa adaptive filtering improvement
Sage-Husa自适应 新息误差跟踪 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m √ 0.055 1 0.082 2 0.089 3 0.080 2 0.102 4 0.202 8 0.056 6 0.056 9 0.050 7 0.098 8 0.044 2 0.115 9 √ √ 0.011 2 0.021 5 0.033 2 0.020 5 0.041 3 0.062 3 0.017 6 0.013 0 0.018 2 0.031 1 0.021 1 0.035 8 注：√代表使用该方法。
下载: 导出CSV
表 6 各算法解算最大误差与均方根误差
Table 6 Maximum error and root mean square error of each algorithm
融合算法 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE Sage-Husa自适应 0.089 3 0.202 8 0.050 7 0.115 9 自适应UKF 0.051 2 0.101 3 0.031 2 0.062 8 自适应SRCKF 0.034 6 0.073 4 0.021 4 0.042 8 改进Sage-Husa自适应 0.033 2 0.062 3 0.018 2 0.035 8
下载: 导出CSV
-
[1] 王国法,王 虹,任怀伟,等. 智慧煤矿2025情景目标和发展路径[J]. 煤炭学报,2018,43(2):295−305. WANG Guofa,WANG Hong,Ren Huaiwei,et al. 2025 Scenario objectives and development path of smart coal mines[J]. Journal of Coal Industry,2018,43(2):295−305.
[2] 王国法. 煤矿智能化最新技术进展与问题探讨[J]. 煤炭科学技术,2022,50(1):1−27. WANG Guofa. New technological progress of coal mine intelligence and its problems[J]. Coal Science and Technology,2022,50(1):1−27
[3] 杨健健,张 强,王 超,等. 煤矿掘进机的机器人化研究现状与发展[J]. 煤炭学报,2020,45(8):2995−3005. YANG Jianjian,ZHANG Qiang,WANG Chao,et al. Research status and development of robotization of coal mine boring machines[J]. Journal of Coal Industry,2020,45(8):2995−3005.
[4] 马宏伟,王世斌,毛清华,等. 煤矿巷道智能掘进关键共性技术[J]. 煤炭学报,2021,46(1):310−320. MA Hongwei,WANG Shibin,MAO Qinghua,et al. Key common technologies for intelligent excavation of coalmine tunnels[J]. Journal of Coal Industry,2021,46(1):310−320
[5] 朱信平,李 睿,高 娟,等. 基于全站仪的掘进机机身位姿参数测量方法[J]. 煤炭工程,2011(6):113−115. doi: 10.3969/j.issn.1671-0959.2011.06.044 ZHU Xinping,LI Rui,GAO Juan,et al. Measurement method of position and attitude parameters of roadheader body based on total station[J]. Coal Engineering,2011(6):113−115 doi: 10.3969/j.issn.1671-0959.2011.06.044
[6] 张旭辉,沈奇峰,杨文娟,等. 基于三激光点标靶的掘进机机身视觉定位技术研究[J]. 电子测量与仪器报,2022,36(6):178−186. ZHANG Xuhui,SHEN Qifeng,YANG Wenjuan,et al. Research on visual positioning technology of tunneling machine body based on three laser point targets[J]. Journal of Electronic Measurement and Instrumentation,2022,36(6):178−186.
[7] 贾文浩,陶云飞,张敏骏,等. 基于iGPS的煤巷狭长空间中掘进机绝对定位精度研究[J]. 仪器仪表报,2016,37(8):1920−1926. JIA Wenhao,TAO Yunfei,ZHANG Minjun,et al. Research on the absolute positioning accuracy of tunneling machines in narrow and long spaces of coal tunnels based on iGPS[J]. Journal of Instrumentation,2016,37(8):1920−1926.
[8] 陶云飞,杨健健,李嘉赓,等. 基于惯性导航技术的掘进机位姿测量系统研究[J]. 煤炭技术,2017,36(1):235−237. TAO Yunfei,YANG Jianjian,LI Jiageng,et al. Research on the Position and Attitude Measurement System of Tunneling Machine Based on Inertial Navigation Technology[J]. Coal Technology,2017,36(1):235−237.
[9] 陈慎金,成 龙,王鹏江,等. 基于掘进机位姿测量系统的自主标定方法误差分析[J]. 煤炭学报,2018,43(9):285−290. CHEN Shenjin,JACKIE Chan,WANG Pengjiang,et al. Error analysis of autonomous calibration method based on heading machine pose measurement system[J]. Journal of China Coal Society,2018,43(9):285−290.
[10] WANG J,YU B,KANG H,et al. Key technologies and equipment for a fully mechanized top-coal caving operation with a large mining height at ultra-thick coal seams[J]. International Journal of Coal Science & Technology,2015,2(2):97−161.
[11] KHONZI H,FRANCOIS D P. Implementation of an autonomous underground localization system[C]. Robotics and MECHATRONICS Conference. IEEE,2013:1–34.
[12] 刘 超,符世琛,成 龙,等. 基于TSOA定位原理混合算法的掘进机位姿检测方法[J]. 煤炭学报,2019,44(4):1255−1264. LIU Chao,FU Shichen,Jackie CHAN,et al. A hybrid algorithm based on TSOA positioning principle for detecting the pose of tunneling machines[J]. Journal of China Coal Society,2019,44(4):1255−1264.
[13] 刘 宇,陈根林,刘永忠. 煤矿全断面掘进机捷联惯导曲线测量系统[J]. 工矿自动化,2019,45(8):65−69, 73. LIU Yu,CHEN Genlin,LIU Yongzhong. Strap down Inertial Navigation curve measurement system for coal mine full face tunneling machine[J]. Industrial and Mining Automation,2019,45(8):65−69, 73.
[14] 卢新明,闫长青,袁照平. 掘进机精准定位方法与掘进机器人系统[J]. 通信学报,2020,41(2):58−65. doi: 10.11959/j.issn.1000-436x.2020042 LU Xinming,YAN Changqing,YUAN Zhaoping. Precise positioning methods for tunneling machines and tunneling robot systems[J]. Journal of Communications,2020,41(2):58−65. doi: 10.11959/j.issn.1000-436x.2020042
[15] AUGER F,HILAIRET M,GUERRERO J M,et al. Industrial applications of the Kalman filter:A review[J]. IEEE Transactions on Industrial Electronics,2013,60(12):5458−5471. doi: 10.1109/TIE.2012.2236994
[16] LI M,MOURIKIS A I. High-precision,consistent EKF-based visualinertial odometry[J]. The International Journal of Robotics Research,2013,32(6):690−711. doi: 10.1177/0278364913481251
[17] 余 铎,王耀南,毛建旭,等. 基于视觉的移动机器人目标跟踪方法[J]. 仪器仪表学报,2019,40(1):227−235. YU Duo,WANG Yaonan,MAO Jianxu,et al. Visual based object tracking method for mobile robots[J]. Journal of Instrumentation,2019,40(1):227−235.
[18] 孟秀云,王语嫣. 一种SINS/GPS紧组合导航系统的改进自适应扩展卡尔曼滤波算法[J]. 北京理工大学学报,2018,38(6):625–630, 636. MENG Xiuyun,WANG Yuyan. An Improved Adaptive Extended Kalman Filter Algorithm for SINS/GPS Compact Integrated Navigation System [J]. Journal of Beijing University of Technology,2018,38(6):625–60, 636.
[19] NARASIMHAPPA M,MAHINDRAKAR A D,GUIZILINI V C,et al. MEMS based IMU drift minimization:Sage Husa adaptive robust Kalman filtering[J]. IEEE Sensors Journal,2019,20(1):250−260.
[20] 王天宇,董文博,王震宇. 基于单目视觉和固定靶标的位姿测量系统[J]. 红外与激光工程,2017,46(4):153−160. WANG Tianyu,DONG Wenbo,WANG Zhenyu. A pose measurement system based on monocular vision and fixed targets[J]. Infrared and Laser Engineering,2017,46(4):153−160.
[21] 王 平,周雪峰,安爱民,等. 一种鲁棒且线性的PnP问题求解方法[J]. 仪器仪表学报,2020,41(9):271−280. WANG Ping,ZHOU Xuefeng,AN Aimin,et al. A robust and linear method for solving PnP problems[J]. Journal of Instrumentation,2020,41(9):271−280.
[22] ZHENG Y,KUANG Y,SUGIMOTO S,et al. Revisiting the pnp problem:A fast,general and optimal solution[C]//Proceedings of the IEEE International Conference on Computer Vision,2013:2344–2351.
[23] 周道德,高豆豆,董登峰,等. 基于深度学习和PnP模型的激光跟踪仪自动姿态测量[J]. 光学精密程,2022,30(9):1047−1057. doi: 10.37188/OPE.20223009.1047 ZHOU Daode,GAO Doudou,DONG Dengfeng,et al. Laser tracker automatic attitude measurement based on deep learning and PnP model[J]. Optical Precision Engineering,2022,30(9):1047−1057. doi: 10.37188/OPE.20223009.1047
[24] 王中宇,李亚茹,郝仁杰,等. 基于点特征的单目视觉位姿测量算法[J]. 红外与激光工程,2019,48(5):169−176. WANG Zhongyu,LI Yaru,HAO Renjie,et al. A monocular vision pose measure mentalgorithm based on point features[J]. Infrared and Laser Engineering,2019,48(5):169−176.
[25] LEPETIT V,MORENO–NOGUER F,FUA P. EPnP:an accurate O(n) solution to the PnP problem[J]. International Journal of Computer Vision,2008,81(2):155−166.
-
期刊类型引用(2)
1. 马宏伟，苏浩，薛旭升，李超，郭逸风，王星，周文剑，崔闻达，喻祖坤，成佳帅. 煤矿井下移动机器人激光标靶定位方法研究. 煤炭科学技术. 2024(11): 60-73 . 
本站查看
2. 郑瑞东，韩燮，高波. 悬臂式掘进机位姿测量方法研究. 中北大学学报(自然科学版). 2024(06): 823-831 . 百度学术
其他类型引用(0)
计量
- 文章访问数: 132
- HTML全文浏览量: 28
- PDF下载量: 53
- 被引次数: 2
