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基站安装在电缆卷放车上，定位标签安装在要定位的电铲上。

设3个基站的坐标分别为S1(x1,y1 )，S2(x2,y2 )，S3(x3,y3 )，标签M的坐标为(x,y)，将基站S1作为基准基站；设定位标签到第i(i=2,3)个基站的距离为定位标签到第i个定位基站的距离与到基准基站的距离之差为di1，则TDOA测距方程组为

(1)

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

1.2 电铲定位流程

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

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

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)

M(k+1)=ФPkФ

(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 可看出，在不同观测噪声的影响下，用本文算法得到的定位轨迹更接近于目标的真实移动轨迹。

目标定位误差计算公式为

(17)

式中：(xn,yn)为算法估计坐标；为目标的真实位置。

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

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

4 结论

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

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

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

参考文献（References）：

[1] 张彬,王永雄,邵翔,等.融合异质传感器信息的机器人精准室内定位[J].控制工程,2018,25(7):1335-1343.

ZHANG Bin,WANG Yongxiong,SHAO Xiang,et al.Precise robot indoor localization based on information fusion of heterogeneous sensors[J].Control Engineering of China,2018,25(7):1335-1343.

[2] 卢靖宇,余文涛,赵新,等.基于超宽带的移动机器人室内定位系统设计[J].电子技术应用,2017,43(5):25-28.

LU Jingyu,YU Wentao,ZHAO Xin,et al.Design of indoor positioning system for mobile robot based on ultra-wideband[J].Application of Electronic Technique,2017,43(5):25-28.

[3] 李浩博,王坚,王川阳.超宽带室内动态定位精度影响探究[J].导航定位学报,2018,6(1):45-48.

LI Haobo,WANG Jian,WANG Chuanyang.Discussion on affection factors of UWB indoor kinematic positioning[J].Journal of Navigation and Positioning,2018,6(1):45-48.

[4] 黄梦雨,秦建军,高磊,等.基于UWB的零转径跟随机器人控制系统设计[J].重庆理工大学学报(自然科学),2019,33(9):142-150.

HUANG Mengyu,QIN Jianjun,GAO Lei,et al.Design of zero turning radius following robot control system based on UWB[J].Journal of Chongqing University of Technology(Natural Science),2019,33(9):142-150.

[5] 樊祥宁.超宽带无线通信关键技术研究[D].南京:东南大学,2005.

FAN Xiangning.Researchs on key techniques of ultra wideband (UWB) wireless communication systems[D].Nanjing: Southeast University,2005.

[6] 刘世森.基于UWB的矿井人员精准定位技术[J].煤矿安全,2019,50(6):118-120.

LIU Shisen.Accurate positioning technology for mine workers based on UWB[J].Safety in Coal Mines,2019,50(6):118-120.

[7] 蒲长昊.基于UWB的矿山巨型卡车防碰撞预警系统[D].呼和浩特:内蒙古大学,2019.

PU Changhao.Anti-collision early warning system of mine giant truck based on UWB[D].Hohhot:Inner Mongolian University,2019.

[8] 张丽.超宽带室内精确定位技术研究[D].青岛:中国海洋大学,2012.

ZHANG Li.Research on ultra-wideband precise positioning technology[D].Qingdao:Ocean University of China,2012.

[9] 吴涛,汪立新,林孝焰.改进扩展卡尔曼滤波器对机动目标的跟踪定位[C]//中国通信学会第五届学术年会论文集.南京:电子工业出版社,2008.

WU Tao,WANG Lixin,LIN Xiaoyan.The application of strong tracking extended Kalman filter in passive target tracking[C]//Proceedings of the Fifth Annual Conference of China Institute of Communications.Nanjing:Publishing House of Electronics Industry,2008.

[10] 李劼人,徐莉,徐青山,等.无线定位技术在移动式起重机中的应用[J].中国机械工程,2019,30(6):716-721.

LI Jieren,XU Li,XU Qingshan,et al.Applications of wireless positioning technology in mobile cranes[J].China Mechanical Engineering,2019,30(6):716-721.

[11] 陈小斯,沈重,周群,等.基于TDOA算法的差分UWB室内定位系统研究[J].现代电子技术,2018,41(6):45-49.

CHEN Xiaosi,SHEN Chong,ZHOU Qun,et al.Research on differential UWB indoor positioning system based on TDOA algorithm[J].Modern Electronics Technique,2018,41(6):45-49.

[12] NJAR M,VIDAL J.Kalman tracking for mobile location in NLOS situations[C]//14th IEEE Proceedings on Personal,Indoor and Mobile Radio Communications,Beijing,2003.

[13] 赵霞.扩展Kalman滤波算法在GFSINS角速度解算中的应用[J].弹箭与制导学报,2008,28(4):52-54.

ZHAO Xia.The application of the extended Kalman filer algorithm to the angular velocity calculation of GFSINS[J].Journal of Projectiles,Rockets,Missiles and Guidance,2008,28(4):52-54.

[14] 陈敏泽,周东华.一种基于强跟踪滤波器的自适应故障预报方法[J].上海海运学院学报,2001(3):35-40.

CHEN Minze,ZHOU Donghua.An adaptive fault prediction method based on strong tracking filter[J].Journal of Shanghai Maritime University,2001(3):35-40.

[15] 周恭谦,杨露菁,刘忠.基于TDOA的自适应扩展卡尔曼滤波跟踪算法研究[J].舰船电子工程,2018,38(7):30-33.

ZHOU Gongqian,YANG Lujing,LIU Zhong.Adaptive interation extended Kalman filter tracing algorithm based on TDOA[J].Ship Electronic Engineering,2018,38(7):30-33.

Research on positioning algorithm of electric mine shovel based on UWB technology

GUO Anbin1, SU Hongjun1, YAN Xiaoheng2

(1.Ha'erwusu Open-pit Coal Mine, China Shenhua Energy Company Limited, Erdos 010300, China;2.Faculty of Electrical and Control Engineering, Liaoning Technical University, Huludao 125105, China)

Abstract:In the harsh working environment of mines, the traditional cable reeling method cannot guarantee the power supply of electric shovel for a long time, and there are hidden safety hazards in the process of power supply. A new cable reel car which follows the shovel is proposed to address the above problems. In order to realize the autonomous following of electric shovel by cable reel car and ensure the long time power supply of electric shovel in mining environment, the positioning algorithm of electric mine shovel based on ultra wide-band (UWB) technology is proposed and time difference of arrival (TDOA) algorithm is used to construct the positioning model of electric mine shovel. Based on the TDOA ranging algorithm, the distance from each base station to the target electric shovel position is measured and the difference is calculated. The distance difference information obtained is moving average filtered to suppress the noise generated in the ranging process and achieve smooth data. The tag position is calculated according to the distance difference after filtering correction. The target electric shovel position is tracked with the strong tracking extended Kalman filter (STFEKF) algorithm to further eliminate noise and improve the positioning accuracy of the target electric shovel during movement. The simulation results show that under the influence of different observation noises, the error of the moving filter + STFEKF positioning method is smaller than that of the traditional EKF algorithm. This method effectively solves the problem of positioning error increasing with the distance increasing or the sudden change of shovel movement. The positioning mean square deviation is reduced by more than 70% compared with the traditional EKF algorithm, and the positioning trajectory is closer to the real movement trajectory of the target with good performance of positioning tracking and noise suppression.

Key words:cable reel car; electric shovel positioning; tracking positioning; wireless positioning; UWB; TDOA; strong tracking extended Kalman filter; moving average filter

中图分类号：TD655.3

文献标志码：A