基于非完整性约束的采煤机定位方法

宋单阳， 杨金衡， 陶心雅， 卢春贵， 田慕琴， 宋建成

（1. 太原理工大学 矿用智能电器技术国家地方联合工程实验室，山西 太原 030024；2. 太原科技大学 电子信息工程学院，山西 太原 030024）

0 引言

采煤机准确定位是综采工作面实现自动化开采的关键技术之一，不仅可为记忆截割、工作面校直提供采煤机准确位置，还可为工作面“三机”联动、自动化采煤提供参考依据。目前采煤机定位方法大多依靠基于惯性测量单元（由三轴陀螺仪和三轴加速度计构成）的惯性导航系统（以下称惯导系统）[1]，但由于传感器误差、环境因素等影响，定位误差会随着时间而累计，所以需要引入辅助信息抑制惯导系统的误差累计。文献[2]结合惯导系统和综采工作面精准地理模型，利用地图匹配技术约束定位过程中惯导系统的误差，但建立综采工作面精准地理模型的难度过大，该方法的可行性有待确定。文献[3]采用将里程计和惯导系统相结合的方法，通过采集里程计的速度或位移信息，校正惯导系统的定位结果，但里程计在定位过程中只对惯导系统的前进速度进行修正，无法对其侧向和垂向速度进行修正。文献[4]在超声波定位系统的基础上，运用卡尔曼滤波算法对惯导系统和超声波传感器的组合定位结果进行过滤，但井下非视距环境会降低超声波传感器测量精度，进而影响超声波定位系统精度。文献[5-6]构建了惯导系统与超宽带组合定位方案，能够有效抑制惯导系统的误差发散，但需要增加大量额外的超宽带锚节点，增加了成本和复杂度。文献[7]采用联邦滤波将惯导系统分别与视觉相机、里程计构成子滤波器，解决了传统卡尔曼滤波中单个传感器测量误差降低整个定位系统定位精度的问题，但工作面粉尘大、光线弱、振动强，影响视觉相机定位精度。文献[8-9]以2套惯导系统之间的距离为约束条件建立卡尔曼滤波模型，能够有效抑制单套惯导系统的误差发散，但2套惯导系统的安装条件严苛，影响模型定位精度。

采煤机在运动过程中满足非完整性约束的特点，即在采煤机不发生跳跃和侧滑的情况下，牵引齿轮和履带连接处的侧向和垂向速度为零。结合该特点，本文基于惯导系统和里程计组合定位，引入非完整性约束作为辅助信息，提出了一种基于非完整性约束的采煤机定位方法，可抑制惯导系统的误差发散，提高定位精度。

1 采煤机定位方法

1.1 方法原理

基于非完整性约束的采煤机定位方法原理如图1所示。安装于采煤机机身中部的惯性测量单元输出经机械编排获得采煤机姿态、速度和位置信息；安装于采煤机牵引齿轮上的里程计输出用于计算采煤机瞬时速度。将惯导系统的机械编排结果、里程计输出及非完整性约束通过卡尔曼滤波算法进行数据融合，将卡尔曼滤波结果作为误差反馈，修正惯导系统的机械编排结果，从而得到采煤机姿态、速度和位置信息的最优估计。

图1 基于非完整性约束的采煤机定位方法原理Fig. 1 Principle of shearer positioning method based on non-holonomic constraints

1.2 坐标系定义

本文求解位置信息过程中用到的坐标系包括参考坐标系（n系）、采煤机机体坐标系（m系）和惯性测量单元坐标系（b系），如图2所示。

图2 坐标系Fig. 2 Coordinate systems

参考坐标系onxnynzn：使用当地水平坐标系作为参考坐标系，坐标系原点on位于惯性测量单元的测量中心，xn轴 指向正北，yn轴指向正东，zn轴 垂直于xnonyn平面，向下为正。

采煤机机体坐标系obxbybzb： 坐标系原点ob为采煤机牵引齿轮与底板的接触点，xm轴沿采煤机前进方向，ym轴指向采煤机靠近液压支架一侧，zm轴垂直向下，与xm，ym轴共同构成右手坐标系。

惯性测量单元坐标系omxmymzm：坐标系原点om位于惯性测量单元的中心，xb，yb，zb轴构成右手坐标系，并分别平行于惯性测量单元的3个敏感轴。

1.3 卡尔曼滤波状态方程

为提高卡尔曼滤波效果，惯导系统一般采用间接卡尔曼滤波进行组合定位解算，即采用惯导系统的误差作为状态向量，可解决连续时间条件下定位系统非线性变化的问题。选择惯导系统的部分误差作为定位系统的状态向量：

式中：φ为三维姿态误差向量； δvn为 三维速度vn误差向量； δrn为 三维位置rn误差向量；bg为三轴陀螺仪的零偏向量；ba为三轴加速度计的零偏向量；sg为三轴陀螺仪的比例因子误差向量；sa为三轴加速度计的比例因子误差向量。

对惯导系统的误差扰动进行分析，可得连续时间状态下姿态、速度和位置的误差微分方程：

三轴陀螺仪和三轴加速度计的零偏和比例因子误差向量的变化可用一阶高斯-马尔科夫过程表示，其微分方程为

式中：t为时间；Tbg，Tba，Tsg，Tsa为一阶高斯-马尔科夫过程的相关时间；wbg，wba，wsg，wsa为一阶高斯-马尔科夫过程的驱动白噪声。

为方便计算，在连续时间下的状态方程需要先离散化[11]。由式（2）-式（5）可得卡尔曼滤波器状态方程在离散化之后的简化形式：

式中：F为惯导系统误差转移矩阵；G为噪声驱动矩阵；w为噪声矩阵；k为时刻标记。

建立状态方程，可将惯导系统误差作为状态信息融入卡尔曼滤波器中，为卡尔曼滤波器提供预测值。

1.4 卡尔曼滤波观测方程

采煤机截割过程中，惯性测量单元必然要受安装角和采煤机振动的影响，因此非完整性约束在惯性测量单元安装处并不完全成立，但对于采煤机牵引齿轮和履带的连接处，牵引齿轮紧紧地卡在履带上，非完整性约束是成立的。因此将惯导系统经过机械编排得到的速度投影到牵引齿轮和履带的连接处，利用非完整性约束与里程计输出的速度对采煤机构成三维速度约束。

当采煤机满足非完整性约束时，牵引齿轮和履带的连接处在采煤机机体坐标系下的速度为

式中vt为采煤机真实前进速度。

由惯导系统推算得到的速度投影到牵引齿轮和履带的连接处时，需要考虑惯性测量单元的安装角及牵引齿轮和履带的连接处到惯性测量单元中心的杆臂，投影到牵引齿轮和履带的连接处的速度为

式中：为由惯性测量单元坐标系到采煤机机体坐标系的姿态矩阵[12]，由惯性测量单元的安装角确定；为带有对准误差的由参考坐标系到惯性测量单元坐标系的姿态矩阵，由惯导系统推算得到的姿态角确定；为惯导系统推算得到的速度向量；为惯导系统推算得到的惯性测量单元坐标系相对于参考坐标系的转动角速度向量在惯性测量单元坐标系下的投影；为牵引齿轮和履带的连接处到惯性测量单元中心的位置向量在惯性测量单元坐标系下的投影；为由参考坐标系到惯性测量单元坐标系的姿态矩阵；为惯性测量单元坐标系下三轴陀螺仪角速度测量误差向量。

里程计输出的速度为

式中V为观测噪声。

由式（7）-式（9）确定卡尔曼滤波的观测向量：

通过观测向量建立观测方程，可将非完整性约束与里程计输出的速度作为观测信息融入卡尔曼滤波器中，抑制惯导系统的定位误差累计。

1.5 卡尔曼滤波算法

卡尔曼滤波算法是一种能够融合预测信息和观测信息，从而获得系统下一时刻状态最优估计的算法。卡尔曼滤波算法分为预测和更新2个部分，预测算法为

更新算法为

式中：P为预测误差的协方差矩阵；Q为状态方程中系统噪声的协方差矩阵；K为滤波增益矩阵；H为观测矩阵；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

实验过程模拟采煤机在工作面中斜切进刀及割三角煤的过程，进刀距离为0.3 m，全程总长30 m。为便于对实验结果进行观察，同时建立工作面坐标系oxyz，其以采煤机起始位置为原点o，采煤机前进方向为x轴，工作面推进方向为y轴，z轴符合右手定则，将定位结果从参考坐标系转换到工作面坐标系。在采煤机行驶过程中，人为不定时地在采煤机移动轨迹上做一系列标记点，测量得到标记点在工作面坐标系中的坐标，记录标记时刻并对标记点进行编号，从而得到采煤机实测移动轨迹标记点。将标记点作为定位参考点，与本文方法解算所得轨迹对应点坐标进行比较分析。

2.2 实验结果

采煤机在工作面坐标系下的行驶轨迹如图3所示。可看出传统惯导系统和里程计组合定位方法（传统方法）由于只有里程计对采煤机前进速度起到一定的约束作用，误差发散很快；本文方法加入非完整性约束后，由于在采煤机侧向和垂向速度上增加了运动约束信息，定位误差没有随时间发散，对实际轨迹具有良好的追踪性能。

图3 采煤机行驶轨迹Fig. 3 Shearer running trajectory

2种方法在x，y，z轴3个方向上的定位误差如图4所示。可看出在加入非完整性约束后，x，y，z轴方向上的误差均大幅下降，分别降低了66%，62%，67%。

图4 采煤机定位误差Fig. 4 Shearer positioning error

各方向上的定位误差最大值见表2。可看出加入非完整性约束的本文方法可大大提高采煤机在x，y，z轴方向上的定位精度，但在x轴方向上的定位误差仍远大于y，z轴方向上的定位误差，这是由于虽然有里程计可以辅助惯导系统对采煤机前进速度进行约束，但里程计本身存在刻度因数等误差，这些误差会通过滤波器的不断迭代而累计，进而叠加在采煤机前向定位误差上。

表2 采煤机定位误差最大值Table 2 The maximum positioning error of shearer m

3 结论

（1） 在传统惯导系统和里程计组合定位方法的基础上，根据采煤机工作过程中的运动信息，通过结合非完整性约束与里程计输出的速度，对采煤机构成三维方向的速度约束，抑制惯导系统的定位误差累计。

（2） 实验结果表明，相较于传统惯导系统和里程计组合定位方法，加入非完整性约束后，采煤机在前向、侧向、垂向的定位误差分别降低了66%，62%，67%，有效提高了采煤机定位精度。

（3） 采煤机在前向的定位误差大于侧向和垂向的定位误差，考虑是里程计的刻度因数等误差累计造成，今后可通过采用精度更高的里程计或增加新的传感器来修正里程计的输出，从而降低里程计误差累计带来的影响。