基于自适应卡尔曼滤波的双惯导采煤机定位方法

杨金衡， 宋单阳， 田慕琴， 宋建成， 许春雨

0 引言

采煤机定位是智能综采工作面的关键技术之一，可解决依靠人工根据煤层分布状况对采煤机的综采作业路径和参数进行调整的问题，能够为协调采煤机、刮板输送机及液压支架的联动运行打下基础。目前常用的采煤机定位技术主要有基于红外信号定位[1]、基于超声波信号定位[2]、基于里程计定位[3]、基于无线信号定位[4]、基于惯导定位[5]等。其中基于红外信号、超声波信号、里程计、无线信号定位等技术具有实现简单、可操作性强的优点，但存在红外信号定位实时性弱，超声波信号衰减严重，里程计定位维度低且存在累计误差，无线信号定位易受井下复杂信道传播环境影响而失效等缺点。惯导在井下工作时具有定位维度高、实时性好、自主性高、抗环境干扰能力强、数据全面、安装条件简单等优点，具有非常好的综合定位性能[6]。然而惯导存在定位误差累计、姿态角及位置漂移这2个固有缺陷，无法长时间独立工作。因此，有学者提出引入误差补偿技术和多传感器组合定位技术。

误差补偿技术是对惯导在测量过程中产生的误差进行相应补偿。Fan Qigao等[7]提出了一种基于惯导的采煤机定位误差分析方法，首先建立误差传递模型，再通过扩展卡尔曼滤波(Extended Kalman Filter，EKF)估计惯导失准角和采煤机速度并进行补偿，但补偿效果依赖于误差传递模型的准确度。韩勇强等[8]提出了一种惯导零速修正算法，使用Sage-Husa卡尔曼滤波结合高阶拟合的方法对径向速度残差导致的定位误差进行补偿，但需要在较长时间的停车情况下进行，影响工作效率。Chen Yuming等[9]基于采煤机动力学模型和振动特性，得到初始姿态角并对恶劣环境引起的线性振动进行补偿，有效提高了初始对准的收敛速度和精度，但无法解决惯导定位误差累计的问题。Wang Shibo等[10]利用里程表辅助惯导，结合长壁采煤工作面采煤机封闭路径的运动特性和卡尔曼滤波器，提出了一种惯导定位的最优估计模型，降低了惯导航向角漂移对定位精度的影响，但无法补偿惯导俯仰角和横滚角的漂移误差。

多传感器组合定位技术通过融合惯导和其他传感器输出信息，实现定位误差修正。应葆华等[11]基于惯导与无线传感器网络的耦合模型，每隔一定时间利用无线定位对惯导的定位结果进行校正，能够有效减小平均定位误差，但在井下建立无线传感器网络需要在多点布置无线传感器，成本较高。周开平[12]利用加速度计和陀螺仪组合的惯导解算出采煤机位置信息，结合地图匹配技术进一步降低了惯导在长时间工作中出现的累计误差，但考虑到工作面地形复杂，难以获取精准地理模型，该方法实现的技术难度较高。

本文提出了一种基于自适应卡尔曼滤波的双惯导采煤机定位方法。利用2套惯导系统的位置信息构成双惯导定位模型，克服了单惯导定位误差累计的缺点；但双惯导定位模型可能存在状态突变，导致定位模型准确度降低，因此将双惯导定位模型和自适应卡尔曼滤波算法结合，以减小状态突变对定位精度的影响。

1 定位坐标系建立

2套惯导系统在采煤机上的安装位置如图1所示[13]。在采煤机机身表面的几何中心位置安装惯导1，在同一平面内安装惯导2，惯导2与惯导1之间的距离为一定值r且相互之间呈一定夹角α。定义采煤机行进方向为Xb轴，工作面推进方向为Yb轴，采煤机机身的竖直方向为Zb轴。

图1 双惯导系统安装位置

定位坐标系如图2所示。以采煤机机身上表面的几何中心为坐标原点O，建立采煤机的载体坐标系(O-XbYbZb)，如图2所示。选择东-北-天坐标系(O-XnYnZn)作为参考坐标系，Xn指向北方向，Yn指向东方向，Zn垂直于Xn轴和Yn轴。ψ，θ，φ分别为采煤机机身的横滚角、俯仰角及航向角。

图2 定位坐标系

2 双惯导定位模型

惯导系统在采样周期内输出加速度和姿态角，对加速度积分可得该采样周期内的采煤机速度。由于惯导系统输出的加速度数据是离散形式的，可采用Lagrange插值多项式对一段时间内的离散加速度函数进行插值得到插值函数，再对插值函数积分[14]。

取3次加速度采样数据得到3个插值点(k-2,ak-2)，(k-1,ak-1)，(k,ak)，其中k为当前时刻，ak为当前时刻的加速度。插值函数为

(1)

式中x为采样时刻。

对Lk(x)在(k-1，k)区间上进行积分，可得采样周期内的速度增量。上一时刻的速度加上采样周期内的速度增量即为当前时刻的速度。

(2)

式中：vk为当前时刻的速度；vk-1为上一时刻的速度；T为采样周期。

由于采样周期非常小，可将当前时刻的速度作为1个采样周期内的平均速度，其与采样周期的乘积为采煤机在采样周期内的位移增量ΔS。根据采煤机的实际运动特点(采煤机沿Xb轴运动)，ΔS在参考坐标系上的投影为

(3)

(4)

根据航迹推算原理，采煤机上的2套惯导系统在参考坐标系下的位置为

(5)

根据式(5)，选取X(k)=[N1(k)E1(k)U1(k)N2(k)E2(k)U2(k)]T作为状态量，可得状态方程：

X(k)=A(k)X(k-1)+D(k)+W(k)

(6)

式中：A(k)为状态转移矩阵；D(k)为控制矩阵；W(k)为过程噪声。

(7)

选取2套惯导系统之间的距离r和夹角α作为卡尔曼滤波器的观测量Z(k)，r和α与状态量之间的关系如下：

(8)

由式(8)结合状态量X(k)，可得非线性的观测方程：

Z(k)=f(X(k))+V(k)

(9)

式中：f()为表示状态量和观测量之间非线性关系的函数；V(k)为观测噪声。

用一阶泰勒展开式对式(9)进行线性化处理[15]，得到线性化后的观测方程：

Z(k)=H(k)X(k)+V(k)

(10)

式中H(k)为雅可比矩阵。

(11)

式中d=(N1(k)-N2(k))2+(E1(k)-E2(k))2。

整理可得EKF过程：

(1) 计算一步预测值：

(12)

(2) 计算一步预测均方误差：

(13)

式中:P(k-1)为上一时刻状态估计误差的均方误差矩阵；Q(k)为W(k)的协方差矩阵。

(3) 更新滤波增益：

(14)

式中R(k)为V(k)的协方差矩阵。

(4)k时刻的状态估计更新：

(15)

(5)k时刻的均方误差更新：

[I-K(k)H(k)]T+K(k)R(k)KT(k)

(16)

式中I为单位矩阵。

3 自适应卡尔曼滤波

卡尔曼滤波器在迭代更新时的残差为

(17)

在滤波过程中，当状态量无误差且观测量无较大偏差时，滤波器输出的残差服从标准正态分布。根据卡尔曼滤波的正交性原理[16]，残差的马氏距离的平方服从卡方分布，因此可构造基于残差的卡方检验值：

(18)

式中：ξk服从自由度为m的卡方分布，即ξk～χ2(m)，本文中m为观测量的维数；Crk为残差rk的协方差矩阵。

(19)

当状态突变时，滤波器状态方程的估计值偏离真实状态较远，过程噪声增加。状态突变程度越大，卡方检验值ξk越大。因此，引入三段模糊判别函数调整过程噪声的协方差矩阵Q(k)，减少状态估计不确定性对于滤波结果的影响。三段模糊判别函数通过设置不同的卡方检验临界值，设定状态检测阈值的上界Bmax、下界Bmin，用来调整不同程度的状态突变。Bmax，Bmin分别取置信概率为99%和90%时所对应的卡方临界值，查表可得Bmax=9.21，Bmin=4.61。定义如下三段模糊判别函数，用于实时更新Q(k)，从而达到自适应滤波的目的。

(20)

式中：Qmin为双惯导定位模型未出现状态突变时的Q(k)；Qmax可取Qmin的m倍。

4 方法验证

4.1 仿真分析

仿真总时长为2 000 s，周期为1s，在采煤机运动到第500，1 000，1 500 s时分别对俯仰角、航向角及横滚角进行改变，采煤机运行轨迹如图3所示。

图3 采煤机运行轨迹

2套惯导系统之间的距离r和夹角α分别为1 m和45°。姿态角噪声均设置为由0.2°/h的常值误差、0.5°为基数的高斯白噪声组成的混合噪声，加速度噪声设置为由0.1g(g为重力加速度)的常值误差、0.05g为基数的高斯白噪声组成的混合噪声。

为验证本文方法的有效性，制造双惯导定位模型状态突变的外部环境，在第500，1 000，1 500 s时放大惯导1的过程噪声，增加状态估计不确定性。

双惯导定位模型下引入自适应卡尔曼滤波前后的卡方检验值如图4所示。在引入自适应卡尔曼滤波前，第500，1 000 s附近存在较大的卡方检验值；在引入自适应卡尔曼滤波后，第500，1 000 s的卡方检验值均小于引入自适应卡尔曼滤波前的卡方检验值，且低于检测阈值；在第1 500 s放大过程噪声时，引入自适应卡尔曼滤波前后的卡方检验值并无明显变化且低于检测阈值，表明自适应卡尔曼滤波有效减小了状态突变时的估计误差，双惯导定位模型的准确度得到了一定改善。

(a) 引入自适应卡尔曼滤波前

对比单惯导定位、基于EKF的双惯导定位和基于自适应卡尔曼滤波的双惯导定位方法下采煤机在北、东、天方向上的定位误差，结果如图5所示，具体误差范围见表1。可看出2种双惯导定位方法的定位误差均小于单惯导定位方法；但基于EKF的双惯导定位方法在采煤机存在较大机动和状态突变时，抗干扰能力较弱，各方向上的定位误差会有明显增加；而基于自适应卡尔曼滤波的双惯导定位方法通过动态调整过程噪声的协方差矩阵，降低了过程噪声的协方差矩阵在滤波增益中的比重，可以很好地抑制状态突变带来的影响，使导航精度保持在良好的范围内。

(a) 惯导1位置误差

表1 不同定位方法的误差范围

4.2 实验分析

单惯导定位、基于自适应卡尔曼滤波的双惯导定位方法下定位结果如图6、图7所示。可看出随着运行距离增加，与理想轨迹相比，单惯导的定位误差逐渐增大，而基于自适应卡尔曼滤波的双惯导定位方法的定位误差趋于收敛，无明显的误差累计现象，定位轨迹与理想轨迹重合度较好；基于自适应卡尔曼滤波的双惯导定位方法的定位误差比单惯导的定位误差在各方向上均有所减小。

(a) 水平面定位轨迹

(a) 水平面定位轨迹

5 结语

提出了一种基于自适应卡尔曼滤波的双惯导采煤机定位方法。在采煤机上固定安装2套惯导系统，以惯导系统的位置为状态量，惯导系统之间的距离和夹角为观测量，构建双惯导定位模型；利用自适应卡尔曼滤波算法对双惯导定位模型状态进行评估和调整，即通过卡方检验评估双惯导定位模型是否发生状态突变，并利用三段模糊判别函数动态调整过程噪声的协方差矩阵，从而降低双惯导定位模型状态突变对定位精度的影响。仿真和实验结果表明，自适应卡尔曼滤波相比EKF的抗干扰能力更强，能抑制状态突变带来的影响；基于自适应卡尔曼滤波的双惯导采煤机定位方法的定位误差比单惯导的定位误差在各方向上均有所减小。