基于UWB和KF的无人车目标跟踪方法*

赵宏强, 邓文斌, 辛 涛, 吴 钪

(1.中南大学 机电工程学院,湖南 长沙 410083； 2.山河智能装备股份有限公司,湖南 长沙 410100； 3.中国人民解放军32181部队,河北 石家庄 050003)

0 引 言

目标跟踪是无人车领域中的一个研究热点,许多传感器被应用于解决这个问题。相机具有采集信息丰富、成本低的优点,目前较为主流的应用方法有检测—跟踪框架,思路是采集颜色信息、深度信息作为感知输入,在每帧图像中检测出行人,然后对不同图像帧中的行人进行数据关联,使用算法模型预测行人状态,进而实现对行人目标的跟踪[1,2]。但相机工作受到光照亮度限制,且容易因为目标走出摄像范围而跟踪失败。此外,激光雷达也被用于对目标进行检测与跟踪。李帅印等人对点云数据进行目标聚类,利用改进的匈牙利算法建立跟踪器,通过卡尔曼滤波(Kalman filtering,KF)算法对目标进行估计[3,4]。但激光雷达是通过点云信息进行检测,计算复杂耗时长、检测准确率低,当遇到多个相似目标时,难识别期望跟踪目标。文献[5]使用扩展卡尔曼滤波(extended Kalman filtering,EKF)算法对行人位置进行估计与预测,定位效果比最小二乘估计(least square estimation,LSE)定位更好。文献[6]将无迹卡尔曼滤波(unscented Kalman filtering,UKF)应用于目标跟踪,利用无迹变换计算目标跟踪问题的后验密度,实验结果表明,在系统非线性化高时UKF的精度要优于EKF。Arasaratnam I等人提出了容积卡尔曼滤波(cubature Kalman filtering,CKF),仿真实验表明其在高维度下精度比UKF更高[7]。在滤波中,不准确的噪声统计特性也会严重影响滤波精度和稳定性。文献[8]提出了一种自适应UKF,其在精度与稳定性上比常规滤波方法更好。

本文提出的无人车目标跟踪方法使用超宽带(ultra-wideband,UWB)传感器采集距离信息作为观测量,通过KF定位算法对噪声统计特性进行在线估计并计算动态目标后验概率密度,实现对期望目标的定位跟踪。并以8×8全地形无人车作为实验平台[9],通过多种场景下的实车试验,证明了该方法在目标跟踪问题中具有良好的定位精度和稳定性。

1 UWB技术

1.1 概 念

UWB技术[10]因具有高时间分辨力、穿透力强、信号功率谱密度低、功耗小等特点,近年来被广泛应用于雷达探测、无线通信和高精度室内定位等领域[11]。

本文使用的Humatics公司旗下P440 UWB传感器,通过脉冲信号双向飞行时间(two-way time-of-flight,TW-ToF)进行点对点测距,如图1所示。

图1 TW-ToF测距原理示意

S=c·((Ta2-Ta1)-(Tb2-Tb1))/2

(1)

式中S为2个P440之间的待测距离；c为光速。在大多数环境下P440具有较高的测距精度,实测能达到厘米(cm)级精度；在完全空旷的视距环境(line of sight,LoS)中,其最大测量距离可达410 m,误差在2 cm以内。

1.2 应用与观测模型

本文将所用UWB传感器区分为锚点(anchor)和标签(tag)两种。标签由期望跟踪目标携带,锚点固定在无人车上提供距离信息作为感知输入。

如图2所示，观测量zk=[r1,r2,…,rm]T,ri(i=1,2,…,m)代表Tag与Anchor间的距离,m代表Anchor数量,在本文中m=3。设Anchor的坐标位置(xj,yj),j=1,2,3；基于三边定位法建立观测模型

zk=h(xk)=[r1,r2,r3]T

(2)

(3)

图2 观测模型

2 KF定位算法

2.1 平方根CKF

xk=f(xk-1,uk)+wk-1，zk=h(xk)+vk

(4)

式中xk和uk为k时刻的系统状态向量(n维)和控制输入量；f(xk-1)和wk-1～N(0,Qk-1)代表系统状态模型函数和系统噪声；zk为k时刻的观测量；h(xk)和vk～N(0,Rk)为系统观测模型函数与观测噪声。SRCKF通过Cubature规则选取并计算2n个权值相同的容积点集合,利用时间更新和量测更新对系统状态值进行估计。

2.2 平方根噪声估值器

Sage-Husa噪声估值器被广泛应用于各种滤波算法,但其是在常规卡尔曼框架所推导[12]。本文引入一种新型噪声估值方法[13],推导出平方根噪声估值器,用其估计系统状态噪声的统计特性

(5)

(6)

dk=(1-b)/(1-bk)

(7)

2.3 滤波发散的抑制方法

针对滤波中可能会出现的发散,使用协方差匹配判据判断其是否出现

(8)

式中 trace(A)为求取矩阵A的迹；S为可调系数(S≥1)。

在滤波过程中,若满足上述判据,则认为滤波收敛；否则认为滤波出现发散趋势,计算衰减因子λk,对Sk|k-1进行修正

(9)

(10)

式中ρ为衰减系数,取值范围0<ρ≤1,常取ρ=0.95,ρ越大越突出残差εk的影响。

2.4 自适应平方根CKF

SRCKF算法:

时间更新

Tria( )为QR分解表达式

测量更新

传播容积点:Zi,k|k-1=h(Xi,k|k-1)

计算自相关协方差、互相关协方差:

Szz,k|k-1=Tria([Zk|k-1SR,k-1])

更新协方差平方根:

Sk|k=Tria([χk|k-1-KkZk|k-1KkSR,k-1])

3 无人车目标跟踪方法设计与实现

3.1 建立系统模型

通过对无人车与目标进行运动学分析建立系统状态模型,如图3。

图3 运动模型

取系统初始化时的车体坐标系作为参考坐标系,状态变量xk=[xk,yk,vx,k,vy,k]T,(xk,yk)表示k时刻目标坐标位置；vx,k与vy,k代表k时刻目标在运动速度。

假设目标在k-1到k时刻内做匀速运动,时间间隔为Δt。无人车对目标进行定位跟踪时,往往两者都在运动。因此,在建立系统状态模型时,将无人车状态[v,ΔX,ΔY,Δθ]T作为系统控制向量uk

(11)

式中wk-1为系统噪声,其协方差矩阵为Q2×2。v为无人车的速度,ΔX为无人车在Δt时间内沿X轴的移动距离,ΔY为无人车在Δt时间内沿Y轴的移动距离,Δθ为无人车在Δt时间内的偏转角度。

3.2 无人车动态目标跟踪方法实现步骤

4 实验与分析

为验证本文所设计目标跟踪方法的性能,在多种场景下进行了实车试验。

4.1 前期工作

本文实验平台为某型8×8差分轮越野无人平台,图4为P440模块布局示意,以车体中心建立车体坐标系,3只传感器的坐标位置分别是(1.4,0.43),(0,-0.43),(-1.4,0.43)m。

为了保证数据的有效性并得到足够数据,对P440的性能参数进行设定,设置其信号积分增益系数为7,最大测距距离小于207 m,最大测距频率为46.9 Hz。

图4 传感器布局

4.2 实验场景

本文设计了3种实验场景,使用LSE定位算法、EKF定位算法、UKF定位算法、CKF定位算法作为对比方法与本文所提方法跟踪轨迹进行对比分析。

实验一：在室外有明显金属物遮挡的情况下,车辆静止时系统对动态目标位置进行跟踪,如图5(a)；

实验二：在室外无明显金属遮挡的情况下,车辆静止时系统对动态目标位置进行跟踪,如图5(b)；

实验三：如图5(c),用粗粉笔在试验场地画出长8 m,宽5.6 m的方框代表目标运动轨迹。控制车辆跟随目标运动,将得到的目标轨迹与方框进行对比。

设R=diag{0.1,0.1,0.1}；设初始状态时Q=diag{0.05,0.05,0.05},本文方法在迭代中会不断对其进行更新。

图5 实验场景

4.3 结果与分析

1)实验一

由图6(a)可知,大型金属遮挡物对定位跟踪结果的影响很大,所以，通过LSE方法所得轨迹曲线多处出现大的跳变；EKF、CKF和UKF在一定程度减小了定位误差,平滑了曲线,但效果不够明显。图6(b)中曲线明显比图6(a)中光滑,且基本消除了大的跳变。本文方法通过容积变换有效减小了模型误差,因此,提升了定位精度和稳定性。

图6 实验一结果

2)实验二

场景二中无明显遮挡,所以轨迹曲线整体上较图6平滑。对比方法中LSE跳变最大；CKF和UKF跟踪效果较LSE略好；EKF的定位效果最优。本文方法使用噪声估值器自适应估计系统噪声,能有效地改善定位效果。所以,图7(b)中轨迹曲线明显比图7(a)中曲线更光滑,且基本没有位置跳变。

图7 实验二结果

3)实验三

设置该场景有两个目的：1)分析无人车运动时本文方法的跟踪效果；2)分析本文提出目标跟踪方法的定位精度。实验中,UKF跟踪结果出现发散,所以,图8(a)中没有UKF轨迹曲线。

在第三种场景中,系统噪声特性更加复杂,常值噪声与实际情况相差较大,所以本文方法的定位效果更好。图8(b)中曲线比图8(a)更加光滑且波动小。考虑到在实际运动中,目标运动轨迹很难与真实轨迹完全一致,可以认为本文方法的跟踪轨迹与实际中的目标运动轨迹基本相符,其定位误差约为15 cm。

图8 实验三结果

5 结 论

本文设计了一种UWB应用方案，将3只UWB传感器固定于无人车,1只作为目标,利用传感器之间的距离信息进行目标定；设计了一种KF定位算法,利用平方根噪声估值器对系统噪声进行估计,有效地提高了定位精度和稳定性；对所提无人车目标跟踪方法进行了实车试验。结果表明：所提方法具有很好的定位精度和稳定性,在动态定位中有15 cm的精度,能满足无人车应用的要求。本文只使用了单一传感器,仍存在一定局限性,后续工作考虑加入视觉跟踪等方法进行多传感器融合,提升目标跟踪的可靠性和定位精度。