矿山工作环境恶劣,传统的电缆卷放方法无法保证电铲长时间供电,造成电铲工作效率低下,且供电过程中存在安全隐患。跟随电铲移动的新型电缆卷放车可解决上述问题。要实现电缆卷放车对电铲的自主跟随,电铲的定位与跟踪非常必要,定位是电缆卷放车完成路径规划、自主导航等复杂任务的前提[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算法应用到矿山环境中,结合滑动平均滤波算法,实现目标电铲的定位与跟踪,使电缆卷放车能够跟随电铲位置移动,提高电缆卷放工作的效率及安全性,延长电铲可工作时间,降低成本及工人劳动强度。
TDOA算法的基本思想:测量不同基站发送到目标点脉冲信号之间的时差,计算距离差,根据基站之间的距离差计算目标点位置[10]。在电铲跟踪定位系统中,2个TDOA数据值可构成一组关于目标电铲位置的双曲线方程组,求解该方程组可得到需要定位的电铲位置。
根据基站的设定位置建立坐标系xoy,基于UWB技术的矿山电铲定位模型如图1所示, 3个UWB基站安装在电缆卷放车上,定位标签安装在要定位的电铲上。
图1 基于UWB技术的矿山电铲定位模型
Fig.1 Mine shovel positioning model based on UWB technology
设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)。
在实际的UWB定位系统中,由于基站本身的内部原因,会产生测量噪声,对定位精度和稳定性有一定影响。滑动平均滤波算法实现简单、能实时且快速地处理不平稳数据[11],减小定位数据误差波动,提高定位精度。因此,本文采用滑动平均滤波算法对距离测量噪声进行滤波。为了提高电铲定位精度,在计算出标签位置坐标后,利用STFEKF算法进一步去噪。
基于UWB 技术的矿山电铲定位流程如图2所示。建立二维定位坐标系,根据TDOA测距算法,测量各基站到目标电铲位置的距离并计算差值;对获取的距离差信息进行滑动平均滤波,以抑制测距过程中产生的噪声,平稳数据;根据滤波修正后的距离差计算出标签位置;用STFEKF算法跟踪目标电铲位置,进一步消除噪声,提高在运动过程中目标电铲的定位精度,得到平滑的运动轨迹。
图2 基于UWB 技术的矿山电铲定位流程
Fig.2 Mine shovel positioning process based on UWB technology
在矿山环境中,要求电缆卷放车与电铲的距离保持在25 m左右,当卷放车与电铲之间的距离增加或电铲运动状态发生突变时,电铲的定位误差会增大。滑动平均滤波可有效减小测距误差,使获得的距离差数据波动减小。
设定一个固定长度为N的滑动窗口,在测得的距离差数据(TDOA值)队列上移动滑动窗口,距离差数据随着窗口移动不断更新。每移动一个单位长度,数据队列前端将输入新的距离差数据,并删除队列最后的一位数据,使窗口内始终有N个数据。取每个滑动窗口内N个数据的平均值组成新的队列,完成滤波。滤波后的输出数据为
(2)
式中:n为输出数据的序号;p为输入数据。
滑动平均滤波算法的效果取决于滑动窗口的大小,可根据实际滤波要求合理选择。
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算法可增强对目标电铲跟踪定位的能力,从而保证电缆卷放车依据电铲位置准确移动。
为了验证基于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的纯量矩阵
图3 电铲跟踪轨迹对比
Fig.3 Comparison of electric shovel tracking trajectories
目标定位误差计算公式为
(17)
式中:(xn,yn)为算法估计坐标;为目标的真实位置。
定位误差对比如图4所示。从图4可看出,随着电铲运动状态的改变及电铲与电缆卷放车之间距离的增加,本文算法的定位误差小于传统EKF算法。
不同观测噪声状况下2种算法的定位均方差对比见表1,其中E1,E2分别表示R为0.000 2,0.000 5的纯量矩阵时的定位均方差。从表1可看出,在不同的噪声影响下,本文算法定位均方差均不到传统EKF算法的30%,符合电铲定位应用要求。
(1) 提出一种基于UWB技术的矿山电铲定位算法,采用TDOA算法实现电铲定位,建立了定位模型。
(a) R为0.000 2的纯量矩阵
(b) R为0.000 5的纯量矩阵
图4 电铲定位误差对比
Fig.4 Comparison of electric shovel positioning error
表1 电铲定位均方差对比
Table 1 Comparison of mean square deviation of electric shovel positioning
实验次数E1/mE2/m本文算法传统EKF算法本文算法传统EKF算法10.361.440.611.4420.542.010.682.0130.441.680.521.6840.572.420.872.4250.471.750.711.7560.411.580.731.58
(2) 由于在定位过程中会产生定位误差,且电铲移动轨迹会发生突变,采用滑动平均滤波算法对距离测量噪声进行滤波,并在计算出标签位置坐标后,利用STFEKF算法进一步去噪,提高了电铲定位精度。
(3) 仿真结果表明,在不同观测噪声的影响下,滑动平均滤波+STFEKF的定位方案误差小于传统EKF算法,有效解决了距离增加或电铲运动状态突变时定位误差增大的问题;定位均方差较传统EKF算法降低70%以上,定位轨迹更接近于目标的真实移动轨迹,具有良好的定位跟踪及噪声抑制能力。
[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.