融合视觉与雷达数据的改进粒子滤波车辆目标跟踪

2022-10-12李以农张志达

重庆大学学报 2022年9期

张翔，郑玲，李以农,，张志达

(重庆大学 a.机械与运载工程学院;b.机械传动国家重点实验室, 重庆 400044)

随着汽车数量的不断增加，人们出行得到便利的同时，交通事故也引发越来越多的人员伤亡与财产损失[1]。对于智能汽车而言，实现对前方车辆快速准确地检测与预警能够有效降低交通事故概率[2]。国内外在智能车辆研究领域已经提出许多车辆检测的方法，包括基于图像特征的方法[3]、基于模型的方法[4]、基于机器学习的方法[5-6]等。近几年基于深度学习的方法在通用目标检测领域取得了很好的效果[7-10]，也逐渐应用于自动驾驶的环境感知算法中。

在基于单帧图像的车辆检测过程中，由于光照条件与道路交通背景变化复杂，可能会出现目标漏检或误检的情况。此外，智能车辆在进行路径规划时需要考虑障碍目标的运动轨迹，需要提取车辆目标运动的时序特征。为了解决车辆检测方法鲁棒性不足与无法结合时序信息的问题，车辆跟踪方法受到大量研究者的关注。解文华等[11]研究一种引入主动轮廓(C-V)模型的MeanShift车辆目标跟踪算法，通过C-V模型实现对车辆多目标的跟踪管理。但是MeanShift中的窗口尺寸是固定值，难以适应车距变化引起的图像尺寸改变。丁晓娜[12]基于Gaussian模型及卡尔曼滤波实现车辆跟踪，但卡尔曼滤波无法解决交通车辆遮挡情况下的收敛问题，对尺度和运动状态波动性较大的车辆目标跟踪的鲁棒性和实时性不足。近年来基于误差最小平方和的MOSSE(minimum output sum of squarederror)滤波器[13]、基于学习的TLD(tracking learning detection)跟踪器[14]、核相关KCF(kernelized correlation filters)滤波器[15]、基于判别式尺度空间的DSST(discriminative scale space tracking)跟踪器[16]等相关滤波方法被提出并在目标跟踪中引起重视，由于其在频域中点乘运算的方式极大减少了运算量，使得跟踪的实时性得到极大提高。但是相关滤波方法不直接适用于多目标跟踪，难以解决道路车辆跟踪中目标加入、消失、遮挡等问题。鉴于粒子滤波( PF, particle filtering)算法具有高鲁棒性与易拓展性，学者们将图像特征与粒子滤波算法结合进行车辆多目标跟踪[17-18]。为了提高跟踪精度，一些改进粒子滤波算法也被提出。田梦楚等[19]将萤火虫群体的吸引和移动机制引入粒子滤波，利用萤火虫优化算法解决粒子滤波的粒子退化问题，提高了目标跟踪的效率。韩锟等[20]利用果蝇优化算法约束粒子群的区域，提高了跟踪精度。Zhou等[21]基于遗传算法提出改进的粒子滤波重采样策略，改善粒子退化，提高了目标跟踪的精确度。然而，这些跟踪算法在解决复杂背景下的目标跟踪、目标长期遮挡等情况时仍存在问题，无法实现实时稳定的道路车辆跟踪。

毫米波雷达具有环境适应性强、体积小、成本低等优点，因此也得到研究者的广泛采用。Xu等[22]基于联合集成概率数据关联与交互式多模型(JIPDA-IMM)结合方法，使用毫米波雷达实现前方多目标准确跟踪。Huang等[23]基于极坐标系下的卡尔曼滤波实现单目标跟踪，并通过阈值方法进行目标的区分和关联，实现车辆多目标跟踪。Aihara等[24]提出将CNN卷积神经网络模型应用于毫米波雷达，模拟激光雷达点云数据，实现道路可行驶区域分割。此外，利用毫米波雷达与视觉信息融合具有极大提升车载环境感知能力的潜力，信息融合的方法也成为研究热点。王宝锋等[25]首先根据雷达目标投影获取图像中的车辆识别感兴趣区域，然后通过对称性、底部阴影、车辆宽度等特征进行验证，实现高鲁棒性的车辆识别。Wang等[26]利用YOLOv2进行车辆检测，针对雨天道路反光造成的图像干扰，以雷达投影点提取水平线，利用与边界框的交点坐标计算车辆宽度，提高宽度估计的精确。以上基于视觉与雷达信息融合的研究在车辆检测领域表现成熟，但仍无法解决车辆跟踪中尺度变化问题。Chen等[27]在图像感知哈希编码的跟踪算法中引入毫米波雷达探测信息进行尺度更新，但是其仅针对单目标跟踪问题，无法适用于道路环境中的多车辆目标跟踪。

针对视觉图像中车辆目标尺度变化导致跟踪丢失问题，提出一种基于视觉与毫米波雷达信息融合的改进粒子滤波车辆跟踪与尺度修正算法[28-29]。首先采用遗传算法改进粒子滤波的重采样策略，根据实时有效采样粒子数求取动态自适应的遗传交叉概率，并在交叉操作中利用单个子代替换小权重粒子，以提高平均精度。利用高斯函数计算种群适应度、计算遗传变异状态，使跟踪估计窗口更符合真实运动。然后，在改进粒子滤波车辆多目标跟踪算法中，引入毫米波雷达的深度信息对跟踪边界框尺寸进行修正。利用雷达目标投影点与视觉跟踪框的位置关系，设计判别策略实现关联匹配，通过推导目标的图像尺寸与纵向距离呈现反比例关系，融合雷达探测的运动信息实现关联目标的跟踪边界框尺寸修正，实现了智能汽车对道路车辆的连续准确跟踪。

1 雷达与机器视觉的时空同步

德尔福ESR毫米波雷达集成了长距离雷达与中距离雷达的检测功能，获取目标的距离、角度、相对速度等运动信息。长距离雷达的最大探测距离为175 m，探测角度为20°；中距离雷达的最大探测距离为60米且探测角度为90°。选用的车载相机为罗技C920 Pro，其探测视角为78°。雷达安装于车辆前方保险杠中部，相机的安装位置于车辆前挡风玻璃上方，如图1(a)所示。传感器探测范围见图1(b)。

图1 传感器布置与探测范围Fig. 1 Sensor layout and detection range

雷达与相机的空间坐标转换需要标定内外参数。首先利用相机采集不同视角下的标准棋盘格图片不少于20张，然后利用Matlab标定工具箱自动提取每张图片的角点，其中需要输入棋盘尺寸，即可通过张正友标定法的迭代计算可以得到相机内外部参数。再经过测量与迭代计算获取雷达与相机的外部平移与旋转参数后，代入透视变换中的内外参数矩阵，即可完成雷达坐标与图像像素坐标的转换

(1)

其中，(u,v)表示图像像素坐标中的目标位置，(XR,YR,Z)表示雷达坐标系下的目标位置，M1与M2分别表示内外参数矩阵。

由于雷达与相机的工作频率不同，要实现多传感器数据关联，还需要完成时间上的对准。毫米波雷达的采样帧频率为20 Hz，而相机的采样帧频率为30 Hz，按照采样频率低的传感器向下兼容，提取雷达每间隔一帧数据、相机每间隔2帧数据为有效数据。

2 毫米波雷达数据滤波

2.1 雷达数据预处理

德尔福ESR雷达每一帧扫描时返回64个通道的目标数据，并包含每个目标的速度、距离、角度、加速度等运动信息。由于产生的数据量过大，若每次调用数据时全部读取，将十分费时、且占用大量内存空间，因此需要对雷达原始数据进行预处理。具体过程如下：

1)提取运动参数。考虑到毫米波雷达所探测各运动参数的精确度，筛选出每个通道目标的速度、距离、角度三组参数。

2)去除空目标。根据雷达解算协议，雷达未探测到目标时返回的空信号数据为相对距离d=0、相对速度v=81.91、角度α=0。根据这些特征值，可以滤除返回信号中的空目标。

3)基于阈值的有效目标初选。在实际应用中通常需要识别的道路车辆目标为同车道的前方车辆、及左右两侧邻车道的同向车辆，如图2所示。为此需要设置横向距离与相对速度阈值，滤除非目标区域的车辆与逆向车辆。

图2 道路前方车辆分布Fig. 2 Preceding vehicles distribution

此外，由于长距离毫米波雷达的最大探测距离为175 m，而其返回数据的精度不高；且远处目标在相机采集图像中尺寸过小，难以实现准确关联。因此需要设置纵向距离阈值滤除远处目标。综上得到的阈值筛选模型如下

(2)

其中:dx为横向相对距离;vy为纵向相对速度;dy为纵向相对距离；Wth为横向距离阈值，由车道宽度与阈值的乘积表示(根据中国道路技术规范，设置行车道的宽度Wth为3.75 m);系数k通常取1.5；vth是相对速度阈值，为负数，取-20 m/s；dth为纵向距离阈值，取60 m。

2.2 车辆目标关联滤波

除了交通车辆，毫米波雷达还会探测到道路上的其他障碍物目标，比如行人、交通标志、金属护栏等。此外，数据中还存在误检无效目标、漏检有效目标等非连续目标。因此，需要利用车辆目标关联滤波从毫米波雷达数据中获取更精确的车辆目标。

首先使用三阶卡尔曼滤波方法对有效目标运动信息进行预测，选取的观测向量为[dn,vn,an]，即毫米波雷达检测到目标在n时刻的距离、速度、加速度。

(3)

其中，雷达的扫描频率为20 Hz，则每个周期的间隔为0.05 s；而多传感器时间对准后，雷达数据为每间隔一帧提取一次，所以取t=0.1 s。

由于毫米波雷达对加速度的分辨力较低，返回数据中同一个目标的加速度数值存在较大波动，以及车辆目标在道路的运动多为纵向运动，因此对滤波模型作出改进：剔除加速度数据a，由相对距离与角度计算出纵向距离dy与横向距离dx

(4)

最终获得关联滤波模型如下

(5)

考虑到存在有效目标漏检的情况，为检验有效目标的连续性，针对每一帧雷达扫描数据，分别提取其前后各3帧数据进行上述检验，从而滤除无效目标

(6)

其中，j={-3,-2,-1,1,2,3}表示前后各三帧的雷达数据，found_radar(j)表示第j时刻下是否存在目标满足关联滤波阈值要求，值为0或1。若相邻6帧数据中存在至少4帧数据满足式(5)的阈值要求，则判定当前帧的目标为连续有效的车辆目标(如图3所示)。

图3 雷达数据关联滤波效果对比图Fig. 3 Effect of radar data association filtering

3 基于改进粒子滤波的车辆跟踪

3.1 改进粒子滤波算法

标准粒子滤波算法中，重要性采样多次迭代后存在严重的粒子退化现象，即多数新生粒子的观测值与跟踪模板间的相似性越来越低，连续多帧后粒子权重逐渐变成0，其后验概率也失去意义。为了解决粒子退化问题，经典重采样方法采用复制大权重粒子、删除小权重粒子的策略，但是多次滤波后会使得粒子多样性降低，导致样本枯竭，在场景剧变时难以准确跟踪。遗传算法是一种自适应全局优化搜索算法，模拟生物进化过程，将其引入粒子滤波重采样，可以利用优胜劣汰的规则繁殖(复制)更优质的粒子，以产生更好的近似解，解决粒子退化问题同时保持粒子多样性。

遗传算法主要包括基因编码、适应度评价、选择、交叉、变异等运算步骤。对于选择样本参与交叉和变异操作的策略通常有2种，即全部参与、或以一固定概率参与，没有考虑种群适应度。若定义有效粒子数为

(7)

考虑粒子退化程度，基于实时有效采样粒子数Neff，求取动态自适应的交叉概率为[21]

(8)

其中，k是遗传概率系数，Ns是粒子总数，Nth为设定阈值。

动态自适应的交叉概率能够有效解决粒子退化问题，并保持多样性，但是其计算成本会增大，因此不适用于实时的目标跟踪系统。为了在不降低跟踪精度的同时提升计算时效性，对遗传交叉和变异部分进行改进。具体工作如下：

1)基因编码：针对视觉跟踪问题，选用目标在图像中的位置及尺寸为基本状态模型。由于车辆跟踪问题涉及到目标运动与尺度变化，选取基因编码s=[x,y,h,w,vx,vy,sc]，包括目标窗口的位置及尺寸、运动速度、尺度变化因子。

2)个体适应度评价：选取图像HSV颜色直方图特征，设置bins=8进行色彩压缩，可将360×100×100维的直方图数据压缩为8×8×8维，以减少存储量、提高计算效率。再利用巴氏系数度量每个粒子表征窗口的直方图pi与模板直方图q之间的相似度

(9)

巴氏距离ρ越大则表明2个直方图分布越相似。然后利用一维高斯函数处理，使适应度fi符合正态分布

(10)

3)选择：计算前一帧粒子集中每个粒子的适应度(权重)，并据此计算当前有效粒子数，利用动态自适应概率式(8)，计算得到交叉概率Pc，再以轮盘赌策略执行N/2次随机选择粒子对作为父代，逐次参与遗传交叉操作。

4)交叉：利用每对父代粒子交叉产生一个子代

C=α×P1+(1-α)×P2，

(11)

其中：P1与P2为父代粒子的基因编码；C为生成的子代粒子基因编码。为了弥补单个子代可能带来种群多样性的不足，设置比例因子α非固定值，其随机数生成区间为[0.3,0.7]。

完成选择与交叉操作后，利用生成的N/2个子代替换前一帧粒子集中个体适应度排名靠后的相应粒子，记录形成新的粒子集，参与后续运算。此策略可以使重采样结果的平均期望更接近跟踪模板，提高跟踪精度，同时减少每一帧运算量，提高目标跟踪的时效性。

5)变异：以一定概率在种群内随机挑选粒子，针对第i个粒子编码si，对窗口坐标与尺度进行随机变异，以提高种群的多样性。利用正态分布计算粒子变异状态，使概率分布更接近目标运动规律

(12)

其中，r为服从[0,1]均匀分布的随机数，设置变异概率rth=0.1。

所提出的改进粒子滤波算法基于遗传算法改进重采样策略，以正态分布而非均匀分布来计算种群适应度、及实现遗传变异操作，使数值计算更接近真实运动轨迹。且利用每对父代粒子生成单个子代，可以提高平均期望，同时避免大量迭代，能够优化计算效率。改进算法能够保证权重较大的粒子依然有更大概率保留，避免粒子退化导致的滤波发散，同时提高粒子的多样性，避免样本衰竭导致车载相机环境剧变时难以跟踪。

3.2 改进粒子滤波在车辆跟踪中的实现

所提出的改进粒子滤波算法在车辆跟踪中的主要实现步骤如下：

1)首帧初始化：对于每一个目标，根据先验密度函数采样N个粒子。与前文遗传算法的基因编码相同，粒子状态表征模型为s=[x,y,h,w,vx,vy,sc]。窗口位置与尺寸的初始化主要由检测结果给定，在置初始速度vx=vy=0，并且由于车载相机中前方车辆尾部尺寸变化幅度一般不大，限定sc=0.01。此外需要设置粒子集初始化权重wi=1/N。

2)重采样：依据前文提出的基于遗传算法改进重采样方法，针对前一帧粒子集的个体适应度进行自然选择、遗传交叉、变异等操作，得到新的粒子集合。

3)观测权值：与前文个体适应度评价方法相同，利用式(9)度量窗口HSV直方图与模板直方图间相似度，并利用式(10)离散化，再进行归一化处理，得到粒子权值wi。

(13)

式(13)返回Oid为被遮挡粒子的序号。若连续遮挡达到一定帧数，则判定该目标行驶出视野之外，删除对应的直方图模板。

5)状态输出：根据各粒子的权重与状态模型计算加权平均状态，求得的结果可作为目标在新一时刻的估计状态输出。

6)返回步骤2进入下一视频帧的目标跟踪，直至连续跟踪帧数达到设定值。

4 基于信息融合的跟踪结果修正

对于视觉跟踪算法，通常在长时间跟踪后会出现跟踪框尺寸过大或过小的情况，致使跟踪窗口出现目标漂移问题，导致目标丢失。针对此问题，引入毫米波雷达获取目标的深度信息，对跟踪边界框尺寸进行修正。

4.1 雷达与视觉跟踪目标关联匹配

首先需要将滤波后的毫米波雷达数据与视觉跟踪结果关联。根据式(4)，可利用雷达探测到的目标距离与角度信息，由雷达坐标转换得到以车直角坐标系下。同时由式(1)的多传感器空间坐标转换关系，可将雷达目标点投影到图像上，得到其图像坐标[u,v]。同时，基于遗传算法改进的粒子滤波在车辆跟踪中的实现，可得到当前帧的跟踪结果，包括位置与尺寸参数[x,y,w,h]。

如图4所示，绿色圆点表示雷达目标中心点投影，红色矩形框表示视觉跟踪结果。根据雷达投影点与视觉跟踪框的位置关系，可以确定目标关联匹配算法。

取“2.2”项下固定后的肝组织，脱水，石蜡包埋，按4 μm厚度切片。然后进行常规苏木精-伊红（HE）染色，于倒置显微镜下观察，并依据《非酒精性脂肪性肝病诊疗指南》[10]，判定肝细胞脂肪变性程度。

(14)

设计的目标关联匹配判别策略如下：

判据1：若跟踪框内没有雷达投影点，则该视觉跟踪目标没有雷达目标与之关联匹配；判据2：若一个跟踪框内存在且仅有一个雷达投影点，则直接将其关联匹配；判据3：若一个跟踪框内存在多个雷达投影点，则将距离中心点(x+w/2,y+h/2)处最近的雷达目标与之关联。

其中判据2的优先级高于判据3。若两跟踪框存在部分重叠(如图4a)，通常远处车辆目标(跟踪框也较小)投影点可能同时包含于近处目标跟踪框(尺寸较大)中，为避免匹配错误，可以通过处理判据2实现远处目标匹配。其次，为解决同一车道出现多辆车导致图像中远处目标被完全遮挡的情况(如图4b)，可采用判据3解决大多数遮挡目标的匹配。

图4 雷达投影点与视觉跟踪框Fig. 4 Radar projection points and visual tracking bounding boxes

4.2 跟踪边界框修正

根据相机透视变换关系式(1)，相机内外参数矩阵确定时，雷达坐标系与图像像素坐标之间的转换为线性关系。因此对于一个尺度不变的目标，在纵向距离更大的时候，其在图像中尺寸呈现对应反比例的缩小。

W·D=Con，

(15)

其中：W为目标在相机成像中的宽度；D为纵向距离；Con表示常数。

鉴于此，在视觉跟踪中引入毫米波雷达探测的目标深度信息，实现对跟踪边界框的修正。假设前一时刻下，跟踪目标窗口的位置与尺寸由向量[x1,y1,w1,h1]表示，对应的纵向距离为D1；而当前时刻，目标的位置与尺寸表示为[x2,y2,w2,h2]，对应的纵向距离为D2。则可由式(15)推导的反比例关系得

w1·D1=w2·D2。

(16)

在跟踪修正中，主要考虑当前时刻的跟踪框宽度与高度不准确，但是仍要依据跟踪框的中点位置。此外，由于前一时刻的检测结果作为真实值、或跟踪结果已经过修正，也可直接沿用。由此，得到修正后的当前时刻下车辆目标的位置与尺寸[x’,y’,w’,h’]如下

(17)

对每一帧下所有关联匹配的跟踪目标边界框完成位置与尺寸修正后，即可以迭代循环方式进入下一帧，实现车辆多目标的连续跟踪。

5 实验结果分析

为验证所提融合算法的有效性，与标准粒子滤波和改进粒子滤波的跟踪效果进行对比实验。实验数据采集于重庆市日间内环快速路、高速公路及部分城市道路场景，天气为晴天或多云。视频帧数为30FPS，图像分辨率为1280*720，包含了部分树木阴影、天桥、路面损毁等场景，有效涵盖多数车辆高速运行的复杂场景。ESR毫米波雷达数据离线存储为csv格式文件。实验软件平台为MATLAB 2016B，硬件平台为8 G内存、I5处理器的笔记本电脑。

分别采用标准粒子滤波算法、改进粒子滤波算法、及融合雷达运动信息的粒子滤波方法进行道路车辆多目标跟踪实验，在典型工况下采集的视频数据与毫米波雷达数据中选取25段数据，每段长度约为20帧。基于标准粒子滤波算法和改进粒子滤波算法的部分车辆跟踪结果图5和图6所示，在改进粒子滤波基础上引入毫米波雷达信息的跟踪修正实验结果如图7所示。

图5 基于标准粒子滤波算法的车辆跟踪Fig. 5 Vehicle tracking based on standard particle filter algorithm

图6 基于改进粒子滤波算法的车辆跟踪Fig. 6 Vehicle tracking based on improved particle filter algorithm

图7 融合雷达信息的跟踪修正实验Fig. 7 Tracking bounding box correction fusing radar data

从图5(b)、图6(b)和图7(b)中可以看出，连续跟踪10帧后，标准粒子滤波算法的跟踪框与真实值存在一定偏差，而改进粒子滤波算法与融合雷达信息的跟踪框仍能准确表征车辆尾部。由图5(c)与图6(c)可见，由于连续跟踪产生的累积误差，视觉跟踪边界框的位置与尺寸均已经与真实目标偏差较大。相比之下，融合雷达信息的跟踪框修正后尺寸更符合目标实际尺寸，如图7(c)所示，所提出的融合算法能够有效避免仅采用视觉跟踪时窗口尺寸的累积误差导致目标丢失。

对应时刻的毫米波雷达测距信息曲线如图8所示。

图8 雷达测量目标纵向距离Fig. 8 Longitudinal distance of vehicle targets by radar measurements

在25段数据测试后，3种算法的平均准确率与平均精度随帧数的关系如图9(a)与图9(b)所示，统计实验结果数据如表1所示。其中平均准确率(MOTA, multi-object tracking accuracy)表示跟踪算法滤除目标误报与漏报的能力，平均精度(MOTP, multi-object tracking precision)表示跟踪框与真实目标边界框的重叠度[29]。

图9 3种跟踪算法平均准确率与平均精度的视频序列变化曲线Fig. 9 MOTA and MOTP performance of 3 tracking algorithms in video sequences

表1 3种跟踪算法性能比较

分析实验结果可以看出，相比于标准粒子滤波算法，改进粒子滤波算法在跟踪平均准确率与跟踪精度上有显著提升，分别提高22.1%与21.1%。但是跟踪一段时间后精度会降低，跟踪框尺寸累积误差导致难以有效表征跟踪目标。而引入毫米波雷达深度信息后，跟踪平均精度获得进一步提升，达到87.8%，表明修正后的视觉跟踪框位置与尺寸更加接近车辆目标尾部在图像中呈现的真实值。虽然两种改进算法在耗时上有小幅增加，但是能够基本满足传感器时间对准后每秒10帧的实时性需求。