基于UWB技术的矿山电铲定位算法研究

郭安斌，苏红俊，闫孝姮

(1.中国神华能源股份有限公司 哈尔乌素露天煤矿， 内蒙古 鄂尔多斯 010300；2.辽宁工程技术大学 电气与控制工程学院， 辽宁 葫芦岛 125105)

0 引言

矿山工作环境恶劣，传统的电缆卷放方法无法保证电铲长时间供电，造成电铲工作效率低下，且供电过程中存在安全隐患。跟随电铲移动的新型电缆卷放车可解决上述问题。要实现电缆卷放车对电铲的自主跟随，电铲的定位与跟踪非常必要，定位是电缆卷放车完成路径规划、自主导航等复杂任务的前提[1]。传统的无线定位技术包括全球定位系统(GPS)、WiFi、射频识别(RFID)等，这些技术由于自身局限性，无法实现对电铲的高精度定位，不适合电铲设备的大规模装备与长期化应用。超宽带(UWB)技术采用纳秒级的非正弦波窄脉冲技术实现定位，具有定位精度高、抗噪声能力强、多径分辨率高与穿透力强的特点，在矿山环境中应用具有很大优势[2-4]。此外，UWB的无线通信空间容量能很好地满足电铲定位需求[5]。文献[6-7]验证了UWB技术适用于矿山环境中大型机械的定位。

目前，常用的UWB定位算法包括到达时间(TOA)、飞行时间(TOF)、到达时间差(TDOA)、到达角(AOA)等，其中TDOA算法与TOF算法在工业应用中使用较多。相较于TOF 算法，TDOA算法中定位标签只需要发射或接收1次UWB信号，缩短了工作时长，降低了功耗，提高了定位容量，且可实现动态定位，更适用于矿山工作环境。因此，本文采用TDOA算法实现电铲定位。

针对基于TDOA算法的运动目标定位问题，张丽[8]提出了一种基于TDOA的扩展卡尔曼滤波(EKF)算法，并将其应用于UWB定位系统中，该算法具有良好的定位能力，实现了高精度的目标运动轨迹跟踪，但当TDOA测量误差较大时，该算法定位精度会降低且定位结果容易发散。当定位系统处于稳定状态时，EKF算法的预测误差协方差趋于最小，但是当目标运动轨迹突变时，EKF算法会失去对目标的跟踪能力。吴涛等[9]提出的强跟踪扩展卡尔曼滤波(STFEKF)算法解决了上述问题，可明显减小定位误差，增强对移动物体的跟踪定位能力。

本文将STFEKF算法应用到矿山环境中，结合滑动平均滤波算法，实现目标电铲的定位与跟踪，使电缆卷放车能够跟随电铲位置移动，提高电缆卷放工作的效率及安全性，延长电铲可工作时间，降低成本及工人劳动强度。

1 基于UWB技术的矿山电铲定位原理

1.1 电铲定位模型

TDOA算法的基本思想：测量不同基站发送到目标点脉冲信号之间的时差，计算距离差，根据基站之间的距离差计算目标点位置[10]。在电铲跟踪定位系统中，2个TDOA数据值可构成一组关于目标电铲位置的双曲线方程组，求解该方程组可得到需要定位的电铲位置。

根据基站的设定位置建立坐标系xoy,基于UWB技术的矿山电铲定位模型如图1所示， 3个UWB基站安装在电缆卷放车上，定位标签安装在要定位的电铲上。

图1 基于UWB技术的矿山电铲定位模型Fig.1 Mine shovel positioning model based on UWB technology

(1)

求解式(1)即可得到电铲位置坐标(x,y)。

1.2 电铲定位流程

在实际的UWB定位系统中，由于基站本身的内部原因，会产生测量噪声，对定位精度和稳定性有一定影响。滑动平均滤波算法实现简单、能实时且快速地处理不平稳数据[11]，减小定位数据误差波动，提高定位精度。因此，本文采用滑动平均滤波算法对距离测量噪声进行滤波。为了提高电铲定位精度，在计算出标签位置坐标后，利用STFEKF算法进一步去噪。

基于UWB 技术的矿山电铲定位流程如图2所示。建立二维定位坐标系，根据TDOA测距算法，测量各基站到目标电铲位置的距离并计算差值；对获取的距离差信息进行滑动平均滤波，以抑制测距过程中产生的噪声，平稳数据；根据滤波修正后的距离差计算出标签位置；用STFEKF算法跟踪目标电铲位置，进一步消除噪声，提高在运动过程中目标电铲的定位精度，得到平滑的运动轨迹。

图2 基于UWB 技术的矿山电铲定位流程Fig.2 Mine shovel positioning process based on UWB technology

2 电铲定位算法实现

2.1 滑动平均滤波算法

在矿山环境中，要求电缆卷放车与电铲的距离保持在25 m左右，当卷放车与电铲之间的距离增加或电铲运动状态发生突变时，电铲的定位误差会增大。滑动平均滤波可有效减小测距误差，使获得的距离差数据波动减小。

设定一个固定长度为N的滑动窗口，在测得的距离差数据(TDOA值)队列上移动滑动窗口，距离差数据随着窗口移动不断更新。每移动一个单位长度，数据队列前端将输入新的距离差数据，并删除队列最后的一位数据，使窗口内始终有N个数据。取每个滑动窗口内N个数据的平均值组成新的队列，完成滤波。滤波后的输出数据为

(2)

式中：n为输出数据的序号；p为输入数据。

滑动平均滤波算法的效果取决于滑动窗口的大小，可根据实际滤波要求合理选择。

2.2 STFEKF算法

EKF算法采用前一时刻的估计值作为参考点做线性化，其线性化模型更可靠。基于TDOA的EKF算法的状态方程是线性的[12]，观测方程是非线性的。EKF算法将非线性函数以泰勒级数展开的方式，保留一阶项，实现对非线性函数的线性化，用Jacoby矩阵代替卡尔曼滤波方程中的状态转移矩阵，以此来解决非线性问题。在电铲定位系统中，观测方程为非线性，因此，EKF算法适用于对目标电铲进行定位跟踪。

在实际的定位过程中，不可避免地存在测量误差，因此有必要通过实时观测系统的输入输出数据，实现对定位数据的滤波优化。由文献[13]可知，EKF应用的非线性系统数学模型为

Xk=Ф(Xk-1)+Wk

(3)

Zk=h(Xk)+Vk

(4)

式中：Xk为电铲在k时刻的状态向量，Xk=[x,y,vx,vy]，vx，vy为k时刻电铲在x轴和y轴的速度；Φ为状态转移矩阵；Wk，Vk为噪声向量，且互不相关；Zk为k时刻的观测值，即距离差信息；h(Xk)为量测函数，h(Xk)=(d21,d31)。

目标电铲的定位精度要求高，且在运动过程中会有运动状态突然改变的情况，因此，采用STFEKF算法来提高定位的精度。当目标电铲运动状态突然发生变化时，输出残差γ(k)就会增大，STFEKF算法通过文献[14]中的次优方法得到时变渐消因子λ(k+1)，通过λ(k+1)来调节预测协方差矩阵Pk+1|k，进而调节增益矩阵Kk+1，得到电铲的最优状态估计值Sk+1。

令残差γ(k)=Zk-h(Sk+1|k)，Sk+1|k为状态预测值，则实际输出残差序列的协方差矩阵为

(5)

式中ρ为遗忘因子，0<ρ≤1，一般取0.95～0.98。

令Hk为k+1时刻h(Sk+1|k)在Sk+1|k处的Jacoby矩阵[15]，即观测矩阵，对h(Sk+1|k)进行Taylor级数展开，得

(6)

式中h1(Sk)，h2(Sk)分别为目标到第2、第3个基站的距离与到基准基站的距离差值。

时变渐消因子的计算公式为[9]

λ(k+1)=tr(N(k+1))/tr(M(k+1))

(7)

(8)

(9)

式中：tr()表示求矩阵的迹;β为弱化因子,β≥1；Pk为估计误差协方差矩阵；Qk，R为分别噪声向量Wk，Vk的协方差矩阵。

STFEKF算法步骤：

(1) 首先由状态方程得到定位预测值，根据上一时刻最优估计值Sk与状态转移矩阵等得到k+1时刻的状态预测值Sk+1|k：

Sk+1|k=ФSk

(10)

(2) 引入时变渐消矩阵Lk+1，计算上述预测值的预测协方差矩阵Pk+1|k：

Pk+1|k=Lk+1ФPkФT

(11)

Lk+1=diag[λ1(k+1),λ2(k+1),…,λ4(k+1)]

(12)

(13)

式中j=1～4。

(3) 计算滤波增益：

(14)

(4) 得到滤波输出值，即输出定位结果：

Sk+1=Sk+1|k+Kk+1(Zk-h(Sk+1|k))

(15)

(5) 更新估计误差协方差矩阵：

Pk+1=(I-Kk+1Hk)Pk+1|k

(16)

式中I为单位矩阵。

电铲工作过程中会由于某些原因致其运动状态发生改变，STFEKF算法可增强对目标电铲跟踪定位的能力，从而保证电缆卷放车依据电铲位置准确移动。

3 仿真分析

为了验证基于UWB技术的矿山电铲定位算法的性能，采用Matlab软件进行仿真分析。电铲的实际移动速度约为3 km/h，假设目标电铲作匀速直线运动，采用3个观测基站，基站位置分别为S1(0,0)，S2(3 m,0)，S3(0,5 m)，以S1为基准基站。设采样周期为1 s，滑动平均滤波数据长度为5，初始航向角为60°，速度vx=0.42 m/s，vy=0.72 m/s；运行20 s后，目标转向，航向角变为0，速度约为0.81 m/s，运动时间为30 s；采用本文算法和传统EKF算法对目标的运动轨迹进行追踪。

当R为0.000 2的纯量矩阵(矩阵对角线元素均为0.000 2，其余元素为0)时，目标跟踪曲线如图3(a)所示。将R增大为0.000 5的纯量矩阵，目标跟踪曲线如图3(b)所示。从图3 可看出，在不同观测噪声的影响下，用本文算法得到的定位轨迹更接近于目标的真实移动轨迹。

(a) R为0.000 2的纯量矩阵

(b) R为0.000 5的纯量矩阵

目标定位误差计算公式为

(17)

定位误差对比如图4所示。从图4可看出，随着电铲运动状态的改变及电铲与电缆卷放车之间距离的增加，本文算法的定位误差小于传统EKF算法。

不同观测噪声状况下2种算法的定位均方差对比见表1，其中E1，E2分别表示R为0.000 2，0.000 5的纯量矩阵时的定位均方差。从表1可看出，在不同的噪声影响下，本文算法定位均方差均不到传统EKF算法的30%，符合电铲定位应用要求。

4 结论

(1) 提出一种基于UWB技术的矿山电铲定位算法，采用TDOA算法实现电铲定位，建立了定位模型。

(a) R为0.000 2的纯量矩阵

(b) R为0.000 5的纯量矩阵

表1 电铲定位均方差对比Table 1 Comparison of mean square deviation of electric shovel positioning

(2) 由于在定位过程中会产生定位误差，且电铲移动轨迹会发生突变，采用滑动平均滤波算法对距离测量噪声进行滤波，并在计算出标签位置坐标后，利用STFEKF算法进一步去噪，提高了电铲定位精度。

(3) 仿真结果表明，在不同观测噪声的影响下，滑动平均滤波+STFEKF的定位方案误差小于传统EKF算法，有效解决了距离增加或电铲运动状态突变时定位误差增大的问题；定位均方差较传统EKF算法降低70%以上，定位轨迹更接近于目标的真实移动轨迹，具有良好的定位跟踪及噪声抑制能力。