Shearer positioning method based on non-holonomic constraints
-
摘要: 目前基于惯性导航系统和里程计组合的采煤机定位方法直接用里程计输出对惯性导航系统推算得到的采煤机前进速度进行修正，但抑制惯性导航系统误差发散的能力非常有限。采煤机在运动过程中满足非完整性约束的特点，即在采煤机不发生跳跃和侧滑的情况下，牵引齿轮和履带连接处的侧向和垂向速度为零。基于该特点，在惯性导航系统和里程计组合定位的基础上，提出了一种基于非完整性约束的采煤机定位方法。安装于采煤机机身中部的惯性测量单元输出经机械编排获得采煤机姿态、速度和位置信息；安装于采煤机牵引齿轮上的里程计输出用于计算采煤机瞬时速度。使用惯性导航系统的机械编排结果和误差传播模型建立卡尔曼滤波状态方程，在采煤机牵引齿轮和履带的连接处引入非完整性约束，利用惯性导航系统投影在连接处的速度与里程计输出的速度之差作为观测向量，建立卡尔曼滤波观测方程；将卡尔曼滤波算法处理后的结果作为误差反馈，对惯性导航系统的输出进行修正，得到采煤机姿态、速度和位置的最优估计。实验结果表明，相较于传统惯性导航系统和里程计组合的定位方法，加入非完整性约束后定位误差没有随时间发散，对实际轨迹具有良好的追踪性能；采煤机在前向、侧向、垂向上的定位误差分别降低了66%，62%，67%。Abstract: At present, the shearer positioning method is based on the combination of the inertial navigation system and odometer. The method directly uses the output of the odometer to correct the shearer forward speed calculated by the inertial navigation system. However, the capability of suppressing the error divergence of the inertial navigation system is very limited. The shearer in the process of movement meets the characteristics of the non-holonomic constraints. When the shearer does not jump and sideslip, the lateral velocity and vertical velocity at the connection between the traction gear and the crawler are zero. Based on this characteristic, a new shearer positioning method based on non-holonomic constraints is proposed on the basis of the combination of the inertial navigation system and odometer. The output of the inertial measurement unit arranged in the middle of the shearer's body is mechanically arranged, so as to obtain the attitude, speed and position information of the shearer. The output of the odometer installed on the traction gear of the shearer is used to calculate the instantaneous velocity of the shearer. The Kalman filtering state equation is established by using a mechanical arrangement result of the inertial navigation system and an error propagation model. The non-integrity constraint is introduced at the joint of a traction gear and a crawler of the shearer. The Kalman filtering observation equation is established by using the difference between the velocity projected at the joint by the inertial navigation system and the velocity output by the mileometer as an observation vector. The output of the inertial navigation system is modified by using the results of the Kalman filtering algorithm as error feedback. Then the optimal estimation of the attitude, speed and position of the shearer is obtained. The experimental results show that compared with the traditional combined positioning method of inertial navigation system and odometer, the positioning error does not diverge with time after the non-holonomic constraint is added. The positioning method has good tracking performance on the actual trajectory. The positioning errors of the shearer in the forward, lateral and vertical directions are reduced by 66%, 62% and 67% respectively.
-
0. 引言
采煤机准确定位是综采工作面实现自动化开采的关键技术之一，不仅可为记忆截割、工作面校直提供采煤机准确位置，还可为工作面“三机”联动、自动化采煤提供参考依据。目前采煤机定位方法大多依靠基于惯性测量单元（由三轴陀螺仪和三轴加速度计构成）的惯性导航系统（以下称惯导系统）[1]，但由于传感器误差、环境因素等影响，定位误差会随着时间而累计，所以需要引入辅助信息抑制惯导系统的误差累计。文献[2]结合惯导系统和综采工作面精准地理模型，利用地图匹配技术约束定位过程中惯导系统的误差，但建立综采工作面精准地理模型的难度过大，该方法的可行性有待确定。文献[3]采用将里程计和惯导系统相结合的方法，通过采集里程计的速度或位移信息，校正惯导系统的定位结果，但里程计在定位过程中只对惯导系统的前进速度进行修正，无法对其侧向和垂向速度进行修正。文献[4]在超声波定位系统的基础上，运用卡尔曼滤波算法对惯导系统和超声波传感器的组合定位结果进行过滤，但井下非视距环境会降低超声波传感器测量精度，进而影响超声波定位系统精度。文献[5-6]构建了惯导系统与超宽带组合定位方案，能够有效抑制惯导系统的误差发散，但需要增加大量额外的超宽带锚节点，增加了成本和复杂度。文献[7]采用联邦滤波将惯导系统分别与视觉相机、里程计构成子滤波器，解决了传统卡尔曼滤波中单个传感器测量误差降低整个定位系统定位精度的问题，但工作面粉尘大、光线弱、振动强，影响视觉相机定位精度。文献[8-9]以2套惯导系统之间的距离为约束条件建立卡尔曼滤波模型，能够有效抑制单套惯导系统的误差发散，但2套惯导系统的安装条件严苛，影响模型定位精度。
采煤机在运动过程中满足非完整性约束的特点，即在采煤机不发生跳跃和侧滑的情况下，牵引齿轮和履带连接处的侧向和垂向速度为零。结合该特点，本文基于惯导系统和里程计组合定位，引入非完整性约束作为辅助信息，提出了一种基于非完整性约束的采煤机定位方法，可抑制惯导系统的误差发散，提高定位精度。
1. 采煤机定位方法
1.1 方法原理
基于非完整性约束的采煤机定位方法原理如图1所示。安装于采煤机机身中部的惯性测量单元输出经机械编排获得采煤机姿态、速度和位置信息；安装于采煤机牵引齿轮上的里程计输出用于计算采煤机瞬时速度。将惯导系统的机械编排结果、里程计输出及非完整性约束通过卡尔曼滤波算法进行数据融合，将卡尔曼滤波结果作为误差反馈，修正惯导系统的机械编排结果，从而得到采煤机姿态、速度和位置信息的最优估计。
![]()
图 1 基于非完整性约束的采煤机定位方法原理Figure 1. Principle of shearer positioning method based on non-holonomic constraints1.2 坐标系定义
本文求解位置信息过程中用到的坐标系包括参考坐标系（n系）、采煤机机体坐标系（m系）和惯性测量单元坐标系（b系），如图2所示。
参考坐标系
${o}_{{{\rm{n}}}}{x}_{\rm{n}}{y}_{\rm{n}}{z}_{\rm{n}}$ ：使用当地水平坐标系作为参考坐标系，坐标系原点${{o}}_{\rm{n}}$ 位于惯性测量单元的测量中心，$ {x}_{\mathrm{n}} $ 轴指向正北，$ {y}_{\mathrm{n}} $ 轴指向正东，$ {z}_{\mathrm{n}} $ 轴垂直于$ {x}_{\mathrm{n}} $ $o_{\mathrm{n}} $ $ {y}_{\mathrm{n}} $ 平面，向下为正。采煤机机体坐标系
$ {o}_{{{\rm{b}}}}{x}_{\rm{b}}{y}_{\rm{b}}{z}_{\rm{b}} $ ：坐标系原点$o_{\rm{b}} $ 为采煤机牵引齿轮与底板的接触点，$ {x}_{\mathrm{m}} $ 轴沿采煤机前进方向，$ {y}_{\mathrm{m}} $ 轴指向采煤机靠近液压支架一侧，$ {z}_{\mathrm{m}} $ 轴垂直向下，与$ {x}_{\mathrm{m}} $ ，$ {y}_{\mathrm{m}} $ 轴共同构成右手坐标系。惯性测量单元坐标系
${o}_{{{\rm{m}}}}{x}_{\rm{m}}{y}_{\rm{m}}{z}_{\rm{m}} $ ：坐标系原点$o_{\rm{m}} $ 位于惯性测量单元的中心，$ {x}_{\mathrm{b}} $ ，$ {y}_{\mathrm{b}} $ ，$ {z}_{\mathrm{b}} $ 轴构成右手坐标系，并分别平行于惯性测量单元的3个敏感轴。1.3 卡尔曼滤波状态方程
为提高卡尔曼滤波效果，惯导系统一般采用间接卡尔曼滤波进行组合定位解算，即采用惯导系统的误差作为状态向量，可解决连续时间条件下定位系统非线性变化的问题。选择惯导系统的部分误差作为定位系统的状态向量：
$$ \boldsymbol{X}={\left[\boldsymbol{\phi }\quad{\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}} \quad{\boldsymbol{\delta }}_{{{\boldsymbol{r}}}^{\mathrm{n}}} \quad{\boldsymbol{b}}_{\mathrm{g}} \quad{\boldsymbol{b}}_{\mathrm{a}} \quad {\boldsymbol{s}}_{\mathrm{g}} \quad{\boldsymbol{s}}_{\mathrm{a}}\right]}^{\mathrm{T}} $$ (1) 式中：
$ \boldsymbol{\phi } $ 为三维姿态误差向量；$ {\boldsymbol{\delta }}_{{{{\boldsymbol{v}}}}^{\mathrm{n}}} $ 为三维速度${\boldsymbol{v}}^{\rm{n}} $ 误差向量；$ {\boldsymbol{\delta }}_{{{\boldsymbol{r}}}^{\mathrm{n}}} $ 为三维位置rn误差向量；$ {\boldsymbol{b}}_{\mathrm{g}} $ 为三轴陀螺仪的零偏向量；$ {\boldsymbol{b}}_{\mathrm{a}} $ 为三轴加速度计的零偏向量；${\boldsymbol{s}}_{\rm{g}} $ 为三轴陀螺仪的比例因子误差向量；${\boldsymbol{s}}_{\rm{a}} $ 为三轴加速度计的比例因子误差向量。对惯导系统的误差扰动进行分析，可得连续时间状态下姿态、速度和位置的误差微分方程：
$$ \dot{\boldsymbol{\phi }}=-{\boldsymbol{\omega }}_{\mathrm{i}\mathrm{n}}^{n}\times \boldsymbol{\phi }+{\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}}-{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{n}}{\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{b}}^{\mathrm{n}}} $$ (2) $$ \begin{split} {\dot{\boldsymbol{\delta }}_{{v}^{\mathrm{n}}}}=&{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{n}}{\boldsymbol{\delta }}_{{{\boldsymbol{f}}}^{\mathrm{b}}}+{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{n}}{\boldsymbol{f}}^{\mathrm{b}}\times \boldsymbol{\phi }-\left(2{\boldsymbol{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}+{\boldsymbol{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}\right)\times {\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}}+ \\&{\boldsymbol{v}}^{\mathrm{n}}\times \left(2{\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}}+{\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}}\right)+{\boldsymbol{\delta }}_{{{\boldsymbol{g}}}_{\mathrm{l}}^{\mathrm{n}}} \end{split} $$ (3) $$ {\dot{\boldsymbol{\delta }}_{{{{{\boldsymbol{r}}}}}^{\mathrm{n}}}}=-{\boldsymbol{\omega }}_{{\rm{en}}}^{{\rm{n}}}\times {\boldsymbol{\delta }}_{{{\boldsymbol{r}}}^{\mathrm{n}}}+{\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}} $$ (4) 式中：
$ {\boldsymbol{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}} $ 为参考坐标系自转角速度向量在参考坐标系下的投影；$ \times $ 为叉乘运算符号；$ {\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{n}}^{\mathrm{n}}} $ 为$ {\boldsymbol{\omega }}_{\mathrm{i}\mathrm{n}}^{\mathrm{n}} $ 的测量误差向量；$ {\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{n}} $ 为由惯性测量单元坐标系到参考坐标系的姿态矩阵；$ {\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{b}}^{\mathrm{n}}} $ 为参考坐标系下三轴陀螺仪角速度测量误差向量；$ {\boldsymbol{\delta }}_{{{\boldsymbol{f}}}^{\mathrm{b}}} $ 为三轴加速度计测量误差向量；$ {\boldsymbol{f}}^{\mathrm{b}} $ 为三轴加速度计的输出向量；$ {\boldsymbol{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}} $ 为地球自转角速度向量在参考坐标系下的投影；$ {\boldsymbol{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}} $ 为参考坐标系相对于地球的转动角速度向量在参考坐标系下的投影；$ {\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{e}}^{\mathrm{n}}} $ 为$ {\boldsymbol{\omega }}_{\mathrm{i}\mathrm{e}}^{\mathrm{n}} $ 的测量误差向量；$ {\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}} $ 为$ {\boldsymbol{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}} $ 的测量误差向量；$ {\boldsymbol{\delta }}_{{{\boldsymbol{g}}}_{\mathrm{l}}^{\mathrm{n}}} $ 为重力加速度${\boldsymbol{g }}_{\mathrm{l}}^{\mathrm{n}}$ 的测量误差向量[10]。三轴陀螺仪和三轴加速度计的零偏和比例因子误差向量的变化可用一阶高斯−马尔科夫过程表示，其微分方程为
$$ \left\{ \begin{array}{c} \begin{array}{c}{\dot{\boldsymbol{b}}_{\mathrm{g}}}\left(t\right)=-\dfrac{1}{{T}_{\boldsymbol{b}_{\mathrm{g}}}}{\boldsymbol{b}}_{\mathrm{g}}\left(t\right)+{\boldsymbol{w}}_{\boldsymbol{b}_{\mathrm{g}}}\left(t\right)\\ {\dot{\boldsymbol{b}}_{\mathrm{a}}}\left(t\right)=-\dfrac{1}{{T}_{\boldsymbol{b}_{\mathrm{a}}}}{\boldsymbol{b}}_{\mathrm{a}}\left(t\right)+{\boldsymbol{w}}_{\boldsymbol{b}_{\mathrm{a}}}\left(t\right)\end{array}\\ \begin{array}{c}{\dot{\boldsymbol{s}}_{\mathrm{g}}}\left(t\right)=-\dfrac{1}{{T}_{{{\boldsymbol{s}}}_{\mathrm{g}}}}{\boldsymbol{s}}_{\mathrm{g}}\left(t\right)+{\boldsymbol{w}}_{{{\boldsymbol{s}}}_{\mathrm{g}}}\left(t\right)\\ {\dot{\boldsymbol{s}}_{\mathrm{a}}}\left(t\right)=-\dfrac{1}{{T}_{{{\boldsymbol{s}}}_{\mathrm{a}}}}{\boldsymbol{s}}_{\mathrm{a}}\left(t\right)+{\boldsymbol{w}}_{{{\boldsymbol{s}}}_{\mathrm{a}}}\left(t\right)\end{array}\end{array}\right. $$ (5) 式中：t为时间；
$ {T}_{{{\boldsymbol{b}}}_{\mathrm{g}}} $ ，$ {T}_{{{\boldsymbol{b}}}_{\mathrm{a}}} $ ，$ {T}_{{{\boldsymbol{s}}}_{\mathrm{g}}} $ ，$ {T}_{{{\boldsymbol{s}}}_{\mathrm{a}}} $ 为一阶高斯−马尔科夫过程的相关时间；$ {{\boldsymbol{w}}}_{{\boldsymbol{b}}_\mathrm{g}} $ ，${{\boldsymbol{w}}}_{{\boldsymbol{b}}_\mathrm{a}}$ ，${{\boldsymbol{w}}}_{{\boldsymbol{s}}_\mathrm{g}}$ ，${{\boldsymbol{w}}}_{{\boldsymbol{s}}_\mathrm{a}}$ 为一阶高斯−马尔科夫过程的驱动白噪声。为方便计算，在连续时间下的状态方程需要先离散化[11]。由式（2）—式（5）可得卡尔曼滤波器状态方程在离散化之后的简化形式：
$$ {\boldsymbol{X}}_{k+1,k}={\boldsymbol{F}}_{k+1,k}{\boldsymbol{X}}_{k}+{\boldsymbol{G}}_{k+1,k}{\boldsymbol{w}}_{k} $$ (6) 式中：
$ \boldsymbol{F} $ 为惯导系统误差转移矩阵；$\boldsymbol{G}$ 为噪声驱动矩阵；${\boldsymbol{w }}$ 为噪声矩阵；$ k $ 为时刻标记。建立状态方程，可将惯导系统误差作为状态信息融入卡尔曼滤波器中，为卡尔曼滤波器提供预测值。
1.4 卡尔曼滤波观测方程
采煤机截割过程中，惯性测量单元必然要受安装角和采煤机振动的影响，因此非完整性约束在惯性测量单元安装处并不完全成立，但对于采煤机牵引齿轮和履带的连接处，牵引齿轮紧紧地卡在履带上，非完整性约束是成立的。因此将惯导系统经过机械编排得到的速度投影到牵引齿轮和履带的连接处，利用非完整性约束与里程计输出的速度对采煤机构成三维速度约束。
当采煤机满足非完整性约束时，牵引齿轮和履带的连接处在采煤机机体坐标系下的速度为
$$ {\boldsymbol{v}}_{\mathrm{m}}={\left[{v}_{{t}}\quad 0\quad 0\right]}^{\mathrm{T}} $$ (7) 式中
$ {v}_{{t}} $ 为采煤机真实前进速度。由惯导系统推算得到的速度投影到牵引齿轮和履带的连接处时，需要考虑惯性测量单元的安装角及牵引齿轮和履带的连接处到惯性测量单元中心的杆臂，投影到牵引齿轮和履带的连接处的速度为
$$\begin{split} {\hat{\boldsymbol{v}}}_{\mathrm{m}}=&{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\hat{\boldsymbol{C}}}_{\mathrm{n}}^{\mathrm{b}}{\hat{\boldsymbol{v}}}^{\mathrm{n}}+{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}\left({\hat{\boldsymbol{\omega }}}_{\mathrm{n}\mathrm{b}}^{\mathrm{n}}\times \right){\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}}\approx \\ &{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}}\left[\boldsymbol{I}+\left(\boldsymbol{\phi }\times \right)\right]\left({\boldsymbol{v}}^{\mathrm{n}}+{\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}}\right)+{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}\left({\boldsymbol{\omega }}_{\mathrm{n}\mathrm{b}}^{\mathrm{n}}\times \right){\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}} +\\& {\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}\left({\boldsymbol{\delta }}_{{{\boldsymbol{\omega }}}_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}}\times \right){\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}}\approx {\boldsymbol{v}}_{\mathrm{m}}+{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}}{\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}}- \\& {\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}}\left({\boldsymbol{v}}^{\mathrm{n}}\times \right)\boldsymbol{\phi }-{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}\left({\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}}\times \right){\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}} \end{split} $$ (8) 式中：
$ {\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}} $ 为由惯性测量单元坐标系到采煤机机体坐标系的姿态矩阵[12]，由惯性测量单元的安装角确定；$ {\hat{\boldsymbol{C}}}_{\mathrm{n}}^{\mathrm{b}} $ 为带有对准误差的由参考坐标系到惯性测量单元坐标系的姿态矩阵，由惯导系统推算得到的姿态角确定；${\hat{\boldsymbol{v}}}^{{\rm{n}}}$ 为惯导系统推算得到的速度向量；$ {\hat{\boldsymbol{\omega }}}_{\mathrm{n}\mathrm{b}}^{\mathrm{n}} $ 为惯导系统推算得到的惯性测量单元坐标系相对于参考坐标系的转动角速度向量在惯性测量单元坐标系下的投影；$ {\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}} $ 为牵引齿轮和履带的连接处到惯性测量单元中心的位置向量在惯性测量单元坐标系下的投影；$ {\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}} $ 为由参考坐标系到惯性测量单元坐标系的姿态矩阵；${\boldsymbol{\delta }}_{{{\boldsymbol{\omega }}}_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}}$ 为惯性测量单元坐标系下三轴陀螺仪角速度测量误差向量。里程计输出的速度为
$$ {\widetilde {\boldsymbol{v}}}_{\mathrm{m}}={\boldsymbol{v}}_{\mathrm{m}}+\boldsymbol{V} $$ (9) 式中
$ \boldsymbol{V} $ 为观测噪声。由式（7）—式（9）确定卡尔曼滤波的观测向量：
$$ \begin{split} \boldsymbol{Z}=&{\widetilde {\boldsymbol{v}}}_{\mathrm{m}}-{\hat{\boldsymbol{v}}}_{\mathrm{m}}=-{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}}{\boldsymbol{\delta }}_{{{\boldsymbol{v}}}^{\mathrm{n}}}+{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}{\boldsymbol{C}}_{\mathrm{n}}^{\mathrm{b}}\left({\boldsymbol{v}}^{\mathrm{n}}\times \right)\boldsymbol{\phi }+\\ &{\boldsymbol{C}}_{\mathrm{b}}^{\mathrm{m}}\left({\boldsymbol{l}}_{\mathrm{I}\mathrm{M}\mathrm{U}}^{\mathrm{b}}\times \right){\boldsymbol{\delta }}_{{{\boldsymbol{\omega}} }_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}}+{\boldsymbol{V}} \end{split}$$ (10) 通过观测向量建立观测方程，可将非完整性约束与里程计输出的速度作为观测信息融入卡尔曼滤波器中，抑制惯导系统的定位误差累计。
1.5 卡尔曼滤波算法
卡尔曼滤波算法是一种能够融合预测信息和观测信息，从而获得系统下一时刻状态最优估计的算法。卡尔曼滤波算法分为预测和更新2个部分，预测算法为
$$ {\boldsymbol{X}}_{k+1,k}={\boldsymbol{F}}_{k+1,k}{\boldsymbol{X}}_{k} $$ (11) $$ {\boldsymbol{P}}_{k+1,k}={\boldsymbol{F}}_{k+1,k}{\boldsymbol{P}}_{k}{{\boldsymbol{F}}^{\mathrm{T}}_{k+1,k}}+{\boldsymbol{Q}}_{k} $$ (12) 更新算法为
$$ {\boldsymbol{K}}_{k+1}={\boldsymbol{P}}_{k+1,k}{{\boldsymbol{H}}^{\mathrm{T}}_{k}}{\left({\boldsymbol{H}}_{k}{\boldsymbol{P}}_{k+1,k}{{\boldsymbol{H}}^{\mathrm{T}}_{k}}+{\boldsymbol{R}}_{k}\right)}^{-1} $$ (13) $$ {\boldsymbol{X}}_{k+1}={\boldsymbol{X}}_{k+1,k}+{\boldsymbol{K}}_{k+1}\left({\boldsymbol{Z}}_{k+1}-{\boldsymbol{H}}_{k+1}{\boldsymbol{X}}_{k+1,k}\right) $$ (14) $$ {\boldsymbol{P}}_{k+1}=\left(\boldsymbol{I}-{\boldsymbol{K}}_{k}{\boldsymbol{H}}_{k}\right){\boldsymbol{P}}_{k+1,k}{\left(\boldsymbol{I}-{\boldsymbol{K}}_{k}{\boldsymbol{H}}_{k}\right)}^{\mathrm{T}}+{\boldsymbol{K}}_{k}{\boldsymbol{R}}_{k}{{\boldsymbol{K}}^{\mathrm{T}}_{k}} $$ (15) 式中：
$ \boldsymbol{P} $ 为预测误差的协方差矩阵；$ \boldsymbol{Q} $ 为状态方程中系统噪声的协方差矩阵；$ \boldsymbol{K} $ 为滤波增益矩阵；H为观测矩阵；$ \boldsymbol{R} $ 为观测噪声的协方差矩阵；I为单位矩阵。通过卡尔曼滤波算法可计算得到采煤机姿态、速度和位置信息的最优估计。
2. 实验验证
2.1 实验方案
为验证本文方法的可行性，与传统惯导系统和里程计组合定位方法进行对比。实验设备采用由ICM42605陀螺仪及ADXL355加速度计集成的惯性测量单元（参数见表1，其中g为重力加速度），并在采煤机的牵引齿轮上安装E6H−CWZ6C编码器作为里程计的观测数据来源，编码器分辨率为3 600 P/r。在整个实验过程中实时采集惯性测量单元和编码器的数据，数据采集频率为100 Hz，并在实验后对数据进行处理。
表 1 惯性测量单元性能参数Table 1. Performance parameters of inertial measurement unit参数 陀螺仪 加速度计 动态范围 $ \pm 500 $ °/s $ \pm 2g $ 零偏 $\pm 0.5 $ °/s $ \pm 0.025g $ 随机误差 $0.24 $ °/$\sqrt{\mathrm{h} } $ 9 μm·$ \sqrt{\mathrm{h}}/\mathrm{s} $ 比例因子误差 $\pm 0.82{\text{%}}$ $\pm 1.46{\text{%}}$ 实验过程模拟采煤机在工作面中斜切进刀及割三角煤的过程，进刀距离为0.3 m，全程总长30 m。为便于对实验结果进行观察，同时建立工作面坐标系oxyz，其以采煤机起始位置为原点o，采煤机前进方向为x轴，工作面推进方向为y轴，z轴符合右手定则，将定位结果从参考坐标系转换到工作面坐标系。在采煤机行驶过程中，人为不定时地在采煤机移动轨迹上做一系列标记点，测量得到标记点在工作面坐标系中的坐标，记录标记时刻并对标记点进行编号，从而得到采煤机实测移动轨迹标记点。将标记点作为定位参考点，与本文方法解算所得轨迹对应点坐标进行比较分析。
2.2 实验结果
采煤机在工作面坐标系下的行驶轨迹如图3所示。可看出传统惯导系统和里程计组合定位方法（传统方法）由于只有里程计对采煤机前进速度起到一定的约束作用，误差发散很快；本文方法加入非完整性约束后，由于在采煤机侧向和垂向速度上增加了运动约束信息，定位误差没有随时间发散，对实际轨迹具有良好的追踪性能。
2种方法在x，y，z轴3个方向上的定位误差如图4所示。可看出在加入非完整性约束后，x，y，z轴方向上的误差均大幅下降，分别降低了66%，62%，67%。
各方向上的定位误差最大值见表2。可看出加入非完整性约束的本文方法可大大提高采煤机在x，y，z轴方向上的定位精度，但在x轴方向上的定位误差仍远大于y，z轴方向上的定位误差，这是由于虽然有里程计可以辅助惯导系统对采煤机前进速度进行约束，但里程计本身存在刻度因数等误差，这些误差会通过滤波器的不断迭代而累计，进而叠加在采煤机前向定位误差上。
表 2 采煤机定位误差最大值Table 2. The maximum positioning error of shearerm 方向 定位误差最大值 传统方法 本文方法 x 0.88 0.30 y 0.08 0.03 z 0.12 0.04 3. 结论
（1） 在传统惯导系统和里程计组合定位方法的基础上，根据采煤机工作过程中的运动信息，通过结合非完整性约束与里程计输出的速度，对采煤机构成三维方向的速度约束，抑制惯导系统的定位误差累计。
（2） 实验结果表明，相较于传统惯导系统和里程计组合定位方法，加入非完整性约束后，采煤机在前向、侧向、垂向的定位误差分别降低了66%，62%，67%，有效提高了采煤机定位精度。
（3） 采煤机在前向的定位误差大于侧向和垂向的定位误差，考虑是里程计的刻度因数等误差累计造成，今后可通过采用精度更高的里程计或增加新的传感器来修正里程计的输出，从而降低里程计误差累计带来的影响。
-
![]()
图 1 基于非完整性约束的采煤机定位方法原理
Figure 1. Principle of shearer positioning method based on non-holonomic constraints
表 1 惯性测量单元性能参数
Table 1 Performance parameters of inertial measurement unit
参数 陀螺仪 加速度计 动态范围 $ \pm 500 $ °/s $ \pm 2g $ 零偏 $\pm 0.5 $ °/s $ \pm 0.025g $ 随机误差 $0.24 $ °/$\sqrt{\mathrm{h} } $ 9 μm·$ \sqrt{\mathrm{h}}/\mathrm{s} $ 比例因子误差 $\pm 0.82{\text{%}}$ $\pm 1.46{\text{%}}$ 
下载: 导出CSV
表 2 采煤机定位误差最大值
Table 2 The maximum positioning error of shearer
m 方向 定位误差最大值 传统方法 本文方法 x 0.88 0.30 y 0.08 0.03 z 0.12 0.04
下载: 导出CSV
-
[1] 王世博,葛世荣,王世佳,等. 长壁综采工作面无人自主开采发展路径与挑战[J]. 煤炭科学技术,2022,50(2):231-243. DOI: 10.13199/j.cnki.cst.2020-1150 WANG Shibo,GE Shirong,WANG Shijia,et al. Development and chanllege of unmanned autonomous longwall fully-mechanized coal mining face[J]. Coal Science and Technology,2022,50(2):231-243. DOI: 10.13199/j.cnki.cst.2020-1150
[2] 周开平. 薄煤层综采工作面采煤机组合定位方法研究[J]. 工矿自动化,2019,45(6):52-57,68. DOI: 10.13272/j.issn.1671-251x.2019010096 ZHOU Kaiping. Research on combined positioning method of shearer on fully mechanized mining face of thin coal seam[J]. Industry and Mine Automation,2019,45(6):52-57,68. DOI: 10.13272/j.issn.1671-251x.2019010096
[3] 张守祥,李森,宋来亮. 基于惯性导航和里程仪的煤矿采掘设备定位[J]. 工矿自动化,2018,44(5):52-57. DOI: 10.13272/j.issn.1671-251x.2018010042 ZHANG Shouxiang,LI Sen,SONG Lailiang. Positioning of coal mining equipments based on inertial navigation and odometer[J]. Industry and Mine Automation,2018,44(5):52-57. DOI: 10.13272/j.issn.1671-251x.2018010042
[4] 夏婷. 综采工作面刮板输送机直线度检测方法研究[D]. 徐州: 中国矿业大学, 2019. XIA Ting. Study on the measurement method of scraper conveyor straightness in fully mechanized mining face[D]. Xuzhou: China University of Mining and Technology, 2019.
[5] 许晓伟,赖际舟,吕品,等. 基于采煤机工作面端头量测的改进因子图高精度自主定位方法研究[J]. 控制与决策,2022,37(8):2170-2176. XU Xiaowei,LAI Jizhou,LYU Pin,et al. High-precision autonomous positioning method based on improved factor graph of measurements at both ends of shearer working face[J]. Control and Decision,2022,37(8):2170-2176.
[6] YANG Hai,LUO Tao,LI Wei,et al. A stable SINS/UWB integrated positioning method of shearer based on the multi-model intelligent switching algorithm[J]. IEEE Access,2019,7:29128-29138. DOI: 10.1109/ACCESS.2019.2898212
[7] 万文辉,李宇,胡文敏,等. 基于联邦滤波进行立体相机/IMU/里程计运动平台组合导航定位[J]. 武汉大学学报(信息科学版),2018,43(1):101-106. WAN Wenhui,LI Yu,HU Wenmin,et al. Mobile platform localization by integration of stereo cameras,IMU and wheel qdometer based on federated filter[J]. Geomatics and Information Science of Wuhan University,2018,43(1):101-106.
[8] 鲁程,王世博,葛世荣,等. 多惯导冗余的采煤机定位原理及其合理性分析[J]. 煤炭学报,2019,44(增刊2):746-753. DOI: 10.13225/j.cnki.jccs.2019.0491 LU Cheng,WANG Shibo,GE Shirong,et al. Redundant multi-INS positioning algorithm of shearer and analysis of its rationality[J]. Journal of China Coal Society,2019,44(S2):746-753. DOI: 10.13225/j.cnki.jccs.2019.0491
[9] WANG Shijia. Research on shearer positioning with double-INS[J]. Sensor Review,2019,39(4):577-584. DOI: 10.1108/SR-12-2018-0318
[10] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 3版. 西安: 西北工业大学出版社, 2015. QIN Yongyuan, ZHANG Hongyue, WANG Shuhua. Kalman filtering and integrated navigation principle[M]. 3rd ed. Xi'an: Northwestern Polytechnical University Press, 2015.
[11] 郑江涛,李四海,刘士明,等. 基于惯导和激光雷达的采煤机定位方法[J]. 中国惯性技术学报,2020,28(5):595-602. DOI: 10.13695/j.cnki.12-1222/o3.2020.05.005 ZHENG Jiangtao,LI Sihai,LIU Shiming,et al. Positioning method of a shearer based on inertial navigation and lidar[J]. Journal of Chinese Inertial Technology,2020,28(5):595-602. DOI: 10.13695/j.cnki.12-1222/o3.2020.05.005
[12] 沈阳,王鹏江,吉晓冬,等. 二维里程辅助的掘进机自主导航方法研究[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
-
期刊类型引用(2)
1. 赵昕. 采煤机截割过程中滚筒作用力及转矩变化分析. 机械管理开发. 2024(04): 45-46+49 . 
百度学术
2. 赵亮，邵斌，原帅. 基于模糊算法的采煤机双滚筒同步调高控制方法. 工业仪表与自动化装置. 2023(05): 93-97 . 百度学术
其他类型引用(2)
计量
- 文章访问数: 302
- HTML全文浏览量: 60
- PDF下载量: 29
- 被引次数: 4
