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

王浩然, 王宏伟, 李正龙, 付翔

王浩然,王宏伟,李正龙,等. 基于捷联惯导与差速里程计的掘进机组合定位方法[J]. 工矿自动化,2022,48(9):148-156. DOI: 10.13272/j.issn.1671-251x.17993
引用本文: 王浩然,王宏伟,李正龙,等. 基于捷联惯导与差速里程计的掘进机组合定位方法[J]. 工矿自动化,2022,48(9):148-156. DOI: 10.13272/j.issn.1671-251x.17993
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. DOI: 10.13272/j.issn.1671-251x.17993
Citation: 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. DOI: 10.13272/j.issn.1671-251x.17993

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

基金项目: 国家重点研发计划项目(2020YFB1314004);山西省揭榜招标项目(20201101008);山西省重点研发计划项目(202102100401015)。
详细信息
    作者简介:

    王浩然(1989—),男,辽宁朝阳人,硕士,主要研究方向为煤矿智能化关键技术、煤矿机电装备智能检测与控制,E-mail:wanghaoran01@tyut.edu.cn

  • 中图分类号: TD632.2

Roadheader combined positioning method based on strapdown inertial navigation and differential odometer

  • 摘要: 掘进机自主精确定位是实现掘进工作面智能化乃至无人化的关键,捷联惯导与里程计组合是一种较理想的掘进机定位方案。针对捷联惯导定位误差随时间累计、单里程计位姿感知能力有限等问题,在捷联惯导定位的基础上引入差速里程计作为辅助定位,提出了一种基于捷联惯导与差速里程计的掘进机组合定位方法。该方法的实现由基于捷联惯导的位姿感知、基于差速里程计的航位推算、基于卡尔曼滤波的数据融合3个部分组成:通过捷联惯导获得导航坐标系下掘进机参考位移与姿态角;由2个分别安装于掘进机左右履带的里程计组成差速里程计,通过差速里程计对掘进机进行航位推算;根据捷联惯导与差速里程计的误差方程设计卡尔曼滤波器,并以捷联惯导与差速里程计得到的掘进机位姿数据之差作为卡尔曼滤波器输入,以卡尔曼滤波器输出值对捷联惯导数据进行校正与补偿。将里程计数据有效性判断融入组合定位方法中,避免了履带打滑对定位精度的影响。在模拟巷道进行了掘进机定位实验,结果表明:该组合定位方法测得的掘进机航向角误差能够控制在0.6°以内,位置误差能够控制在0.19 m以内,具有较高的定位精度。
    Abstract: Autonomous and accurate positioning of roadheader is the key to realize intelligent and unmanned heading face. The combination of strapdown inertial navigation and odometer is an ideal positioning scheme for roadheader. The positioning error of strapdown inertial navigation accumulates with time and the position and posture perception capability of single odometer is limited. In order to solve the above problems, a roadheader combined positioning method based on strapdown inertial navigation and differential odometer is proposed by introducing differential odometer as auxiliary positioning on the basis of strapdown inertial navigation. The realization of the method consists of three parts: position and posture perception based on strapdown inertial navigation, dead reckoning based on differential odometer, and data fusion based on Kalman filtering. The reference displacement and attitude angle of roadheader in navigation coordinate system are obtained by strapdown inertial navigation. The differential odometer is composed of two odometers installed on the left and right tracks of the roadheader, and the dead reckoning of the roadheader is calculated by the differential odometer. Kalman filter is designed according to the error equation of strapdown inertial navigation and differential odometer. The difference between the position and posture data of roadheader obtained by strapdown inertial navigation and differential odometer is used as the input of Kalman filter. The output value of Kalman filter is used to correct and compensate the strapdown inertial navigation data. The validity judgment of odometer data is integrated into the combined positioning method, so that the influence of track slipping on the positioning precision is avoided. The positioning experiment of roadheader is carried out in the simulated roadway. The results show that the heading angle error measured by the combined positioning method can be controlled within 0.6° and the position error can be controlled within 0.19 m. The method has high positioning precision.
  • 煤炭目前仍是我国的主导能源,占能源结构的57%[1-2]。掘进作业环境恶劣,危险系数高,对一线工人的安全与健康产生了较多不利影响,同时采掘不平衡也制约着煤炭生产,因此实现掘进工作面的智能化乃至无人化对煤矿安全高效生产有重要意义。掘进机自主精确定位是实现掘进工作面智能化乃至无人化的关键。

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

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

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

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

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

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

    (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

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

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

    $$ \begin{split} & {\boldsymbol{C}}_{\rm{b}}^{\rm{n}} = \left[ {\begin{array}{*{20}{c}} {{T_{11}}}&{{T_{12}}}&{{T_{13}}} \\ {{T_{21}}}&{{T_{22}}}&{{T_{23}}} \\ {{T_{31}}}&{{T_{32}}}&{{T_{33}}} \end{array}} \right] = \\ & \qquad \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]\\ \end{split} $$ (1)

    式中:$ {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) \otimes {\boldsymbol{Q}}\left( h \right) $$ (2)

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

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

    $$ {\boldsymbol{\varphi }}_{\rm{I}}^{\rm{n}} = \left[ {\begin{array}{*{20}{c}} \gamma \\ \theta \\ \psi \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\arctan \left({ - \dfrac{{{T_{31}}}}{{{T_{33}}}}}\right) } \\ {\arcsin \;{{T_{32}}} } \\ {\arctan \; {\dfrac{{{T_{12}}}}{{{T_{22}}}}}} \end{array}} \right] $$ (3)

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

    $$ {\boldsymbol{\dot V}}_{\rm{I}}^{\rm{n}} = {\boldsymbol{C}}_{\rm{b}}^{\rm{n}}{{\boldsymbol{f}}^{\rm{b}}} - (2{\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} + {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}}) \times {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} + {{\boldsymbol{g}}^{\rm{n}}} $$ (4)

    式中:$ {\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}}} $分别为掘进机所在地理位置的子午圈与卯酉圈曲率半径;gn为导航坐标系下重力加速度。

    式(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}} $,捷联惯导据此初对准,得到初始的姿态转换矩阵。基于速度方程,捷联惯导的位置微分方程为

    $$ \dot {\boldsymbol{P}}_{\rm{I}}^{\rm{n}} = \left[ {\begin{array}{*{20}{c}} {{{\dot L}_{\rm{I}}}} \\ {{{\dot \lambda }_{\rm{I}}}} \\ {{{\dot h}_{\rm{I}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} 0&{\dfrac{1}{{{R_{\rm{M}}} + {h_{\rm{I}}}}}}&0 \\ {\dfrac{{\sec \;{L_{\rm{I}}}}}{{{R_{\rm{N}}} + {h_{\rm{I}}}}}}&0&0 \\ 0&0&1 \\ \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{V_{\rm{E}}}} \\ {{V_{\rm{N}}}} \\ {{V_{\rm{U}}}} \end{array}} \right] $$ (5)

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

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

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

    基于简化模型可得

    $$ \left\{ {\begin{array}{*{20}{l}} \begin{gathered} \begin{array}{*{20}{l}} {{\boldsymbol{V}}_{\rm{D}}^{\rm{b}} = {{[0\;\;\;{v_{\rm{c}}}\;\;\;0]}^{\rm{T}}}} \\ {{\boldsymbol{\omega }}_{\rm{D}}^{\rm{b}} = {{[0\;\;\;0\;\;\;{\omega _{\rm{c}}}]}^{\rm{T}}}} \end{array} \\ {r_{\rm{c}}} = \frac{{{v_{\rm{c}}}}}{{{\omega _{\rm{c}}}}} = \frac{{({v_{\text{r}}} + {v_{\text{l}}})\eta {d_{{\text{ω}}{\rm{b}}}}}}{{2\left( {{v_{\text{r}}} - {v_{\text{l}}}} \right)}} \\ \end{gathered} \end{array}} \right. $$ (6)

    式中:$ {\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}}}$,根据航位推算原理,掘进机线速度与角速度可由履带速度推得:

    $$ \left\{ \begin{gathered} {\boldsymbol{V}}_{\rm{D}}^{\rm{b}} = \frac{1}{2}({\boldsymbol{V}}_{\text{l}}^{\rm{m}} + {\boldsymbol{V}}_{\text{r}}^{\rm{m}}) \\ {\boldsymbol{\omega }}_{\rm{D}}^{\rm{b}} = \frac{1}{{{d_{{\text{ω}}{\rm{b}}}}}}{{\boldsymbol{I}}_{23}}({\boldsymbol{V}}_{\text{l}}^{\rm{m}} - {\boldsymbol{V}}_{\text{r}}^{\rm{m}}) \\ \end{gathered} \right. $$ (7)

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

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

    $$ \left\{ \begin{gathered} {\boldsymbol{V}}_{\rm{D}}^{\rm{n}} = {\boldsymbol{C}}_{\rm{b}}^{\rm{n}}{\boldsymbol{V}}_{\rm{D}}^{\rm{b}} \\ {\boldsymbol{\omega }}_{\rm{D}}^{\rm{n}} = {\boldsymbol{C}}_{\rm{b}}^{\rm{n}}{\boldsymbol{\omega }}_{\rm{D}}^{\rm{b}} \\ \end{gathered} \right. $$ (8)

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

    $$ \dot {\boldsymbol{P}}_{\rm{D}}^{\rm{n}} = \left[ {\begin{array}{*{20}{c}} {{{\dot L}_{\rm{D}}}} \\ {{{\dot \lambda }_{\rm{D}}}} \\ {{{\dot h}_{\rm{D}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} 0&{\dfrac{1}{{{R_{\rm{M}}} + {h_{\rm{D}}}}}}&0 \\ {\dfrac{{\sec \;{L_{\rm{D}}}}}{{{R_{\rm{N}}} + {h_{\rm{D}}}}}}&0&0 \\ 0&0&1 \\ \end{array}} \right]{\boldsymbol{V}}_{\rm{D}}^{\rm{n}} $$ (9)

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

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

    $$ {\boldsymbol{\dot \varphi }}_{\rm{D}}^{\rm{n}} = \left[ {\begin{array}{*{20}{c}} {{{\dot \gamma }_{\rm{D}}}} \\ {{{\dot \theta }_{\rm{D}}}} \\ {{{\dot \psi }_{\rm{D}}}} \end{array}} \right] = {\boldsymbol{\omega }}_{\rm{D}}^{\rm{n}} $$ (10)

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

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

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

    $$ \begin{split} & \Delta {\boldsymbol{\dot V}}_{\rm{I}}^{\rm{n}}{{ = - }}{\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}} \times {{\boldsymbol{f}}^{\rm{n}}} + {\boldsymbol{C}}_{\rm{b}}^{\rm{n}}\left( { {\Delta {{\boldsymbol{K}}_{\rm{A}}}} + {\Delta {{\boldsymbol{\alpha }}_{\rm{A}}}}} \right){{\boldsymbol{f}}^{\rm{b}}} + \\ & \qquad\quad \Delta {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} \times \left( {2{\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} + {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}}} \right) + {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} \times \left( {2{\boldsymbol{\omega }}_{{\rm{ie}}}^{\rm{n}} + {\boldsymbol{\omega }}_{{\rm{en}}}^{\rm{n}}} \right) + {{{{\boldsymbol{\delta}}}}^{\rm{n}}} \end{split} $$ (11)

    式中:$ \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}}}$为加速度计零偏误差。

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

    $$ \begin{split} & {\Delta \dot {\boldsymbol{P}}_{\rm{I}}^{\rm{n}}} = \left[ {\begin{array}{*{20}{c}} {\Delta {{\dot L}_{\rm{I}}}} \\ {\Delta {{\dot \lambda }_{\rm{I}}}} \\ {\Delta {{\dot h}_{\rm{I}}}} \end{array}} \right] =\\ & \left[ {\begin{array}{*{20}{c}} {\dfrac{{\Delta {V_{\rm{N}}}}}{{{R_{\rm{M}}} + {h_{\rm{I}}}}} - \Delta {h_{\rm{I}}}\dfrac{{{V_{\rm{N}}}}}{{{{\left( {{R_{\rm{M}}} + {h_{\rm{I}}}} \right)}^2}}}} \\ {\dfrac{{\Delta {V_{\rm{E}}}}}{{{R_{\rm{N}}} + {h_{\rm{I}}}}}\sec \;{L_{\rm{I}}} + \Delta {L_{\rm{I}}}\dfrac{{{V_{\rm{E}}}}}{{{R_{\rm{N}}} + {h_{\rm{I}}}}}\tan \;{L_{\rm{I}}}\sec \;{L_{\rm{I}}} - \Delta {h_{\rm{I}}}\dfrac{{{V_{\rm{E}}}\sec \;{L_{\rm{I}}}}}{{{{\left( {{R_{\rm{N}}} + {h_{\rm{I}}}} \right)}^2}}}} \\ {\Delta {V_{\rm{U}}}} \end{array}} \right] \end{split} $$ (12)

    式中:${\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]

    $$ \dot {\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}} = {\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}} \times {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} + \Delta {\boldsymbol{\omega }}_{{\rm{in}}}^{\rm{n}} - {\boldsymbol{C}}_{\rm{b}}^{\rm{n}}\left( { {\Delta {{\boldsymbol{K}}_{\rm{G}}}} + {\Delta {{\boldsymbol{\alpha }}_{\rm{G}}}} } \right){\boldsymbol{\omega }}_{{\rm{ib}}}^{\rm{b}} - {{\boldsymbol{\varepsilon }}^{\rm{n}}} $$ (13)

    式中:$ {\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}}} $为陀螺仪零偏误差。

    设里程计工作的坐标系为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],两坐标系的姿态转换矩阵为

    $$ {\boldsymbol{C}}_{\rm{m}}^{\rm{b}} = {\boldsymbol{I}} - \left( {{\boldsymbol{\alpha }} \times } \right) = \left[ {\begin{array}{*{20}{c}} 1&{{\alpha _\psi }}&{ - {\alpha _\gamma }} \\ { - {\alpha _\psi }}&1&{{\alpha _\theta }} \\ {{\alpha _\gamma }}&{ - {\alpha _\theta }}&1 \end{array}} \right] $$ (14)

    式中I为单位矩阵。

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

    $$ {\boldsymbol{\tilde V}}_{\rm{D}}^{\rm{b}}{\text{ = }}{\boldsymbol{C}}_{\rm{m}}^{\rm{b}}\left( {1 + \Delta {\boldsymbol{K}}} \right){\boldsymbol{V}}_{\rm{D}}^{\rm{b}} $$ (15)

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

    $$ \Delta {\boldsymbol{V}}_{\rm{D}}^{\rm{n}} = {\boldsymbol{V}}_{\rm{D}}^{\rm{n}} \times {\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}} + {\boldsymbol{M}}{{\boldsymbol{\gamma }}_{\rm{D}}} $$ (16)

    其中

    $$ \left\{ \begin{array}{l} {\boldsymbol{M}} = \dfrac{1}{2}\left[ {\begin{array}{*{20}{c}} { - {T_{13}}}&{{T_{12}}}&{{T_{11}}} \\ { - {T_{23}}}&{{T_{22}}}&{{T_{21}}} \\ { - {T_{33}}}&{{T_{32}}}&{{T_{31}}} \end{array}} \right] \\ {{\boldsymbol{\gamma }}_{\rm{D}}} = {[ \begin{array}{*{20}{c}} {{v_{\text{l}}}{\alpha _\theta }_{\rm{l}} + {v_{\text{r}}}{\alpha _\theta }_{\rm{r}}}&{{v_{\text{l}}}\Delta {K_{\text{l}}} + {v_{\text{r}}}\Delta {K_{\text{r}}}}&{{v_{\text{l}}}{\alpha _{\psi {\rm{l}}}} + {v_{\text{r}}}{\alpha _{\psi {\rm{r}}}}} \end{array} ]^{\rm{T}}} \\ \end{array} \right.$$ (17)

    式中:ΔKl,ΔKr分别为左右里程计刻度系数误差;$ {\alpha _{\theta {\rm{l}}}} $$ {\alpha _{\theta {\rm{r}}}} $分别为左右里程计的俯仰误差角;$ {\alpha _{\psi {\rm{l}}}} $$ {\alpha _{\psi {\rm{r}}}} $分别为左右里程计的航向误差角。

    由式(17)可知,里程计的横滚误差角并不影响差速里程计的精度。

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

    $$ {\boldsymbol{X}} = {\left[ {\begin{array}{*{20}{c}} {{{({\boldsymbol{\phi }}_{\rm{I}}^{\rm{n}})}^{\rm{T}}}}&{{{\left( {\Delta {\boldsymbol{V}}_{\rm{I}}^{\rm{n}}} \right)}^{\rm{T}}}}&{{{\left( {\Delta {\boldsymbol{P}}_{\rm{I}}^{\rm{n}}} \right)}^{\rm{T}}}}&{{{({{\boldsymbol{\varepsilon }}^{\rm{n}}})}^{\rm{T}}}}&{{{({{\boldsymbol{\delta}} ^{\rm{n}}})}^{\rm{T}}}}&{({{\boldsymbol{\gamma }}_{\rm{D}}})^{\rm{T}}} \end{array}} \right]^{\rm{T}}} $$ (18)

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

    $$ {\boldsymbol{Z}} = \left[ {\begin{array}{*{20}{c}} \begin{gathered} {\boldsymbol{V}}_{\rm{I}}^{\rm{n}} \\ {\boldsymbol{P}}_{\rm{I}}^{\rm{n}} \\ \end{gathered} \\ {{\boldsymbol{\varphi }}_{\rm{I}}^{\rm{n}}} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} \begin{gathered} {\boldsymbol{V}}_{\rm{D}}^{\rm{n}} \\ {\boldsymbol{P}}_{\rm{D}}^{\rm{n}} \\ \end{gathered} \\ {{\boldsymbol{\varphi }}_{\rm{D}}^{\rm{n}}} \end{array}} \right] $$ (19)

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

    $$ \left\{ \begin{gathered} {{\boldsymbol{X}}_k}={\boldsymbol{ A}}{{\boldsymbol{X}}_{k - 1}}+{{\boldsymbol{w}}_{k - 1}} \\ {{\boldsymbol{Z}}_k} ={\boldsymbol{ H}}{{\boldsymbol{X}}_k}+{{\boldsymbol{D}}_k} \\ \end{gathered} \right. $$ (20)

    式中:${{\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^ - = {\boldsymbol{A}}{\hat {\boldsymbol{X}}_{k - 1}} $$ (21)

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

    (2) 计算均方误差的先验估计值:

    $$ {\boldsymbol{S}}_k^ - = {\boldsymbol{A}}{{\boldsymbol{S}}_{k - 1}}{{\boldsymbol{A}}^{\rm{T}}} + {{\boldsymbol{N}}_{k - 1}} $$ (22)

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

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

    $$ {{\boldsymbol{E}}_k} = {\boldsymbol{S}}_k^ - {{\boldsymbol{H}}^{\rm{T}}}{({\boldsymbol{HS}}_k^ - {{\boldsymbol{H}}^{\rm{T}}} + {{\boldsymbol{J}}_k})^{ - 1}} $$ (23)

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

    (4) 计算状态量的后验估计值:

    $$ {\hat {\boldsymbol{X}}_k} = \hat {\boldsymbol{X}}_k^ - + {{\boldsymbol{E}}_k}({{\boldsymbol{Z}}_k} - {\boldsymbol{H}}\hat {\boldsymbol{X}}_k^ - ) $$ (24)

    (5) 更新均方误差:

    $$ {{\boldsymbol{S}}_k} = ({\boldsymbol{I}} - {{\boldsymbol{E}}_k}{\boldsymbol{H}}){\boldsymbol{S}}_k^ - $$ (25)

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

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

    $$ {{\boldsymbol{r}}_k} \sim N(0,{\boldsymbol{HS}}_k^ - {{\boldsymbol{H}}^{\rm{T}}} + {{\boldsymbol{J}}_k}) $$ (26)

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

    $$ \frac{{r_k^2(u)}}{{{m_k}(u,u)}} \sim {\chi ^2}(1) $$ (27)

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

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

    $$\frac{{r_k^2(u)}}{{{m_k}(u,u)}} > \xi $$ (28)

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

    基于捷联惯导与差速里程计的掘进机组合定位实验平台设备主要包括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捷联惯导旁,以减小安装位置带来的误差,左右履带输出轴处各有1个里程计通过齿圈相连。全站仪观测装置部署于掘进机后方10 m处。

    图  4  设备安装
    Figure  4.  Equipment installation

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

    表  2  里程计参数
    Table  2.  Odometer parameters
    参数左里程计右里程计
    允许最高转速/(r·min−16 0006 000
    分辨率/(P·R−11 0001 000
    刻度系数/(m·P−1$ 2.904 \times {10^{ - 4}} $$ 2.904 \times {10^{ - 4}} $
    俯仰误差角/(°)0.460.50
    航向误差角/(°)0.380.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}} = {\boldsymbol{I}} - \left( {{{\boldsymbol{\alpha }}_{{\text{ch}}}} \times } \right) = \left[ {\begin{array}{*{20}{c}} 1&{{\alpha _{\psi {\text{ch}}}}}&{ - {\alpha _{\gamma {\text{ch}}}}} \\ { - {\alpha _{\psi {\text{ch}}}}}&1&{{\alpha _{\theta {\text{ch}}}}} \\ {{\alpha _{\gamma {\text{ch}}}}}&{ - {\alpha _{\theta {\text{ch}}}}}&1 \end{array}} \right] $$ (29)

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

    在相同路况下实验多次,并选取某一组实验数据进行分析。捷联惯导根据全站仪给出的起始点经纬度信息进行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
    高程/m787.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。

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

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

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

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

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

    图  2   掘进机姿态角

    Figure  2.   Attitude angle of roadheader

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

    Figure  3.   Simplified model of roadheader walking section

    图  4   设备安装

    Figure  4.   Equipment installation

    图  5   掘进机航向角

    Figure  5.   Heading angle of roadheader

    图  6   掘进机位移

    Figure  6.   Displacement of roadheader

    图  7   掘进机轨迹

    Figure  7.   Track of roadheader

    表  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

    表  2   里程计参数

    Table  2   Odometer parameters

    参数左里程计右里程计
    允许最高转速/(r·min−16 0006 000
    分辨率/(P·R−11 0001 000
    刻度系数/(m·P−1$ 2.904 \times {10^{ - 4}} $$ 2.904 \times {10^{ - 4}} $
    俯仰误差角/(°)0.460.50
    航向误差角/(°)0.380.72
    下载: 导出CSV

    表  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
    高程/m787.815 0
    当地重力加速度/(m·s−2−9.797
    下载: 导出CSV
  • [1] 谢和平,吴立新,郑德志. 2025年中国能源消费及煤炭需求预测[J]. 煤炭学报,2019,44(7):1949-1960. DOI: 10.13225/j.cnki.jccs.2019.0585

    XIE Heping,WU Lixin,ZHENG Dezhi. Prediction on the energy consumption and coal demand of China in 2025[J]. Journal of China Coal Society,2019,44(7):1949-1960. DOI: 10.13225/j.cnki.jccs.2019.0585

    [2] 黄刚. 我国区域煤炭消费动态变化分析−产业结构调整和新兴能源对煤炭消费的双重影响[J]. 中国煤炭,2017,43(5):22-26. DOI: 10.3969/j.issn.1006-530X.2017.05.004

    HUANG Gang. Analysis on regional coal consumption dynamics:dual influences of industrial structure adjustment and new thriving energy on coal consumption[J]. China Coal,2017,43(5):22-26. DOI: 10.3969/j.issn.1006-530X.2017.05.004

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

    ZHANG Xuhui,LIU Yongwei,MAO Qinghua,et al. Research and progress on intelligent control technology of boom-type roadheader in coal mine[J]. Heavy Machinery,2018(2):22-27. DOI: 10.3969/j.issn.1001-196X.2018.02.006

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

    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. DOI: 10.13199/j.cnki.cst.2020-1334

    [5] 吴淼,沈阳,吉晓冬,等. 悬臂式掘进机行走轨迹及偏差感知方法[J]. 煤炭学报,2021,46(7):2046-2056. DOI: 10.13225/j.cnki.jccs.jj21.0702

    WU Miao,SHEN Yang,JI Xiaodong,et al. Trajectory and deviation perception method of boom-type roadheader[J]. Journal of China Coal Society,2021,46(7):2046-2056. DOI: 10.13225/j.cnki.jccs.jj21.0702

    [6] 沈阳,王鹏江,吉晓冬,等. 二维里程辅助的掘进机自主导航方法研究[J]. 仪器仪表学报,2021,42(11):96-105. DOI: 10.19650/j.cnki.cjsi.J2107815

    SHEN Yang,WANG Pengjiang,JI Xiaodong,et al. Research on autonomous navigation method of roadheader aided by two-dimensional mileage[J]. Chinese Journal of Scientific Instrument,2021,42(11):96-105. DOI: 10.19650/j.cnki.cjsi.J2107815

    [7] 刘豪. 捷联惯导与里程计组合的矿用掘进机自主导航定位系统[D]. 重庆: 重庆大学, 2020.

    LIU Hao. An autonomous navigation system of mining TBM based on the combination of SINS and OD[D]. Chongqing: Chongqing University, 2020.

    [8] 郭楠楠,申亮亮,邵会兵,等. 车载捷联惯导双里程计组合导航方法研究[J]. 导航定位与授时,2020,7(6):86-93. DOI: 10.19306/j.cnki.2095-8110.2020.06.011

    GUO Nannan,SHEN Liangliang,SHAO Huibing,et al. Research on integrated navigation algorithm with SINS and double odometers[J]. Navigation Positioning and Timing,2020,7(6):86-93. DOI: 10.19306/j.cnki.2095-8110.2020.06.011

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

    LIU Pengfei. High-precision vehicle GNSS/INS integrated navigation system aided by odometer[J]. Optics and Precision Engineering,2020,28(4):979-987.

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

    QIN Yongyuan. Inertial navigation[M]. 2nd ed. Beijing: Science Press, 2020: 290-318.

    [11] 史凯,刘马宝. 捷联惯导四元数的四阶龙格库塔姿态算法[J]. 探测与控制学报,2019,41(3):61-65.

    SHI Kai,LIU Mabao. Strapdown inertial navigation quaternion fourth-order Runge-Kutta attitude algorithm[J]. Journal of Detection & Control,2019,41(3):61-65.

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

    YAN Gongmin,LI Sijin,GUO Zhengdong. A new method to obtain high-order error compensation coefficients for equivalent rotation vector[J]. Journal of Chinese Inertial Technology,2020,28(3):347-352,396. DOI: 10.13695/j.cnki.12-1222/o3.2020.03.010

    [13]

    WANG Tianmiao,WU Yao,LIANG Jianhong,et al. Analysis and experimental kinematics of a skid-steering wheeled robot based on a laser scanner sensor[J]. Sensors,2015,15(5):9681-9702. DOI: 10.3390/s150509681

    [14]

    HUANG Yulong,ZHANG Yonggang,WANG Xiaodong. Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS[J]. IEEE Transactions on Instrumentation and Measurement,2017,66(12):3364-3377. DOI: 10.1109/TIM.2017.2737840

    [15] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 2版. 西安: 西北工业大学出版社, 2012.

    QIN Yongyuan, ZHANG Hongyue, WANG Shuhua. Kalman filtering and integrated navigation principle[M]. 2nd ed. Xi'an: Northwestern Polytechnical University Press, 2012.

    [16] 周召发,赵芝谦,张志利,等. 考虑里程计截断误差的SINS/OD组合导航算法[J]. 中国惯性技术学报,2022,30(3):336-344.

    ZHOU Zhaofa,ZHAO Zhiqian,ZHANG Zhili,et al. SINS/OD integrated navigation algorithm considering odometer truncation error[J]. Journal of Chinese Inertial Technology,2022,30(3):336-344.

    [17] 赵洪松,缪玲娟,沈军. 捷联惯导/里程计高精度组合导航算法[J]. 兵工学报,2014,35(4):433-440. DOI: 10.3969/j.issn.1000-1093.2014.04.001

    ZHAO Hongsong,MIAO Lingjuan,SHEN Jun. High accuracy algorithm for SINS/odometer integrated navigation system[J]. Acta Armamentarii,2014,35(4):433-440. DOI: 10.3969/j.issn.1000-1093.2014.04.001

    [18]

    LIU Xuhang,LIU Xiaoxiong,YANG Yue,et al. Robust variational Bayesian method-based SINS/GPS integrated system[J]. Measurement,2022,193:110893. DOI: 10.1016/j.measurement.2022.110893

    [19] 石静,缪玲娟,倪茂林. 一种抗野值自适应滤波算法及在MEMS−SINS/GPS中应用[J]. 宇航学报,2010,31(12):2711-2716. DOI: 10.3873/j.issn.1000-1328.2010.12.014

    SHI Jing,MIAO Lingjuan,NI Maolin. An outlier rejecting and adaptive filter algorithm applied in MEMS-SINS/GPS[J]. Journal of Astronautics,2010,31(12):2711-2716. DOI: 10.3873/j.issn.1000-1328.2010.12.014

    [20] 马智渊,石志勇,王志伟. 捷联惯导/里程计组合导航技术[J]. 火力与指挥控制,2017,42(2):183-186.

    MA Zhiyuan,SHI Zhiyong,WANG Zhiwei. Strapdown inertia/odometer integrated navigation technology[J]. Fire Control & Command Control,2017,42(2):183-186.

  • 期刊类型引用(7)

    1. 许名熠,廖敏辉,黄炳,王帅,何志敏,王元鹏. 基于IMU捷联惯导技术的巷道掘进机测姿系统研究. 煤矿机械. 2025(01): 66-68 . 百度学术
    2. 李烜,杜瑞建,金斗. 动平台下高精度方向指向补偿算法. 光电技术应用. 2024(04): 63-66 . 百度学术
    3. 李福来. 煤矿井下掘进机自动化定姿技术研究. 自动化与仪器仪表. 2024(10): 347-351 . 百度学术
    4. 孙凌飞,刘亚,彭继国,杨木易,马志愿,张坤. 基于惯性技术的掘进机组合定位方法. 煤炭科学技术. 2024(12): 300-310 . 百度学术
    5. 江帆,康静静,皇行涛,朱真才,周公博,彭玉兴. 基于多编码器与惯导融合的无轨胶轮车定位方法. 煤炭科学技术. 2024(S2): 284-293 . 百度学术
    6. 高梓. 悬臂式掘进机组合定位导航方案设计与试验. 山西焦煤科技. 2023(08): 4-7 . 百度学术
    7. 金铃子,王浩然,李正龙. 捷联惯导与双目里程计融合的掘进机组合定位方法. 矿业研究与开发. 2023(12): 171-177 . 百度学术

    其他类型引用(8)

图(7)  /  表(3)
计量
  • 文章访问数:  1045
  • HTML全文浏览量:  60
  • PDF下载量:  44
  • 被引次数: 15
出版历程
  • 收稿日期:  2022-08-03
  • 修回日期:  2022-08-29
  • 网络出版日期:  2022-09-12
  • 刊出日期:  2022-09-25

目录

/

返回文章
返回