基于卡尔曼滤波的多特征加权最近邻数据关联与跟踪算法

赵丰　王利辉　陈俊吉　张明　徐伟业　李进军　赵超

摘 要：针对传统最近邻数据关联算法正确率较低且容易出现漏关联的问题，提出一种多特征加权的最近邻关联算法。根据智能车环境感知系统获得的障碍物特征数据，定义了一种相似度函数，提出基于生命周期计算有效关联度的方法，从而判定目标是否关联;基于卡尔曼滤波对关联目标进行迭代更新，实现对目标的跟踪;通过实验对比了静止目标、无交互的低速运动目标和有交互的低速运动目标的跟踪轨迹。结果表明，与传统的最近邻数据关联算法相比，所提出的改进算法可以实现对低速运动目标准确连续的关联跟踪，不会出现目标丢失或位置突变的现象，且跟踪目标的交互与遮挡对跟踪效果影响较小，具有较高的有效性与实用性。研究结果可为智能车辆的目標跟踪设计提供参考。

关键词：传感器技术;智能车辆;数据关联;目标跟踪;卡尔曼滤波;最近邻

中图分类号：TN958.98 文献标识码：A

doi：10.7535/hbkd.2020yx03003

Multi-feature weighted nearest neighbor data association and

tracking algorithm based on Kalman filter

ZHAO Feng1， WANG Lihui1， CHEN Junji2， ZHANG Ming2， XU Weiye2， LI Jinjun3， ZHAO Chao3

（1.No. 32381 of the Chinese People′s Liberation Army， Beijing 100072， China; 2.School of Mechanical Engineering， Nanjing University of Science and Technology， Nanjing， Jiangsu 210094， China; 3.No. 32379 of the Chinese People′s Liberation Army， Beijing 100072， China）

Abstract：

Aiming at the problems of low accuracy and missing association in traditional nearest neighbor data association algorithm ， a multi-feature weighted nearest neighbor association algorithm was proposed. A similarity function was defined according to the obstacle data obtained by the intelligent vehicle environment perception system， and a method was proposed to calculate the effective correlation degree based on the life cycle， so as to determine whether the objects are related. Based on Kalman filter， the associated target was updated iteratively to realize the tracking of the target. The tracking trajectory of stationary target， low-speed moving target without interaction and low-speed moving target with interaction were compared through experiments. Experimental results show that compared with the conventional nearest neighbor data association algorithm， the improved algorithm proposed in this paper can realize the accurate and continuous associated tracking of low-speed moving targets， and there will be no the phenomenon of target loss or position mutation. With high effectiveness and practicability， the interaction and occlusion of tracking targets have less effect on the tracking performance. The research results may provide reference for the target tracking design of intelligent vehicles.

Keywords：

sensor technology; intelligent vehicle; data correlation; target tracking; Kalman filter; nearest neighbor

随着科学技术的不断发展，车辆的智能化研究已然成为一个热门研究领域。障碍物的数据关联与跟踪作为智能车辆主动避障策略研究的一个重要环节，可以识别障碍物的不同运动状态，制定不同的避障策略，从而可以对道路变化情况及时作出相应的反应，对提高智能车辆规避障碍的实时性和行驶安全性有着重要和积极的意义。

目前研究者们已经提出多种数据关联与跟踪算法，以不同的特点在不同应用环境发挥作用。王鹏宇等[1]针对智能车辆前向多传感器多目标跟踪问题，提出了一种改进的联合概率数据关联算法，搭建了装备毫米波雷达和摄像头的环境感知系统，实现了目标的稳定跟踪。YANG等[2]对经典的基于卡尔曼滤波的概率数据关联滤波器在非线性非高斯情形下进行了推广，提出概率数据关联-反馈粒子滤波算法，对单个或多个目标进行跟踪。孙进平等[3]提出了一种应用超图匹配的多假设跟踪算法，应用于群目标跟踪问题，大幅提高了群内个体的数据关联正确率。但是前述方法均存在计算量大的问题。蒋富勤等[4]针对多假设跟踪算法计算量大的问题，对算法进行了改进，实现了快速目标关联与跟踪，且提高了检测性能。王宇霞等[5]提出一种对粒子滤波跟踪器进行分裂和合并的自重构算法，可以应对复杂多变的跟踪环境，且通过合并冗余的跟踪器在一定程度上解决了粒子滤波跟踪中计算量大的问题。对于复杂度小的场景，最近邻数据关联算法由于简单有效、计算量小得到了最为广阔的应用。针对传统的最近邻关联算法抗干扰能力弱、关联正确率低的缺点，大量学者进行了算法改进。王晓君等[6]提出一种改进马氏距离计算方法的最近邻关联算法，仿真结果表明在几乎不影响算法运行时间的情况下明显提高了关联正确率。胡朋启等[7]将点拓扑图与最近邻算法结合，提出基于最近邻-点拓扑图的目标关联算法，同时改进了最近邻关联算法正确率低以及点拓扑图运算量大的缺点。ZHANG等[8]针对雷达飞行侦查过程中的多目标跟踪问题，提出一种改进的交互式多模型新最近邻关联算法，能准确识别多目标轨迹，且在目标数据丢失的情况下仍具有良好的跟踪性能。数据关联之后需进行目标跟踪，此时除了前文提到的粒子滤波，在大部分线性高斯情形下，运用最为广泛的算法是卡尔曼滤波[9-11]。为了获得更好的估计精度和跟踪性能，文献＼在标准卡尔曼滤波的基础上考虑了概率卡尔曼滤波，修正卡尔曼滤波和自适应卡尔曼滤波，实现对目标的跟踪。

在对上述系统采集到的原始数据进行数据处理的基础上，本文提出采用基于生命周期的多特征加权最近邻数据有效关联算法，识别不同帧中的观测对象是否为同源对象，再通过卡尔曼滤波器的状态估计过程，实现对目标的跟踪，最后通过实车实验，验证算法的实时性与有效性。

1 智能车环境感知系统

智能车环境感知系统集成了环境感知与动态决策模块，其系统功能由硬件平台和软件系统联合实现。环境感知模块由毫米波雷达感知系统和激光雷达感知系统组成，本文采用速腾聚创（RoboSense）公司的

RS-LiDAR-16型16线激光雷达和德国大陆（Continental）公司的ARS308-2T毫米波雷达进行环境数据采集。动态决策模块由一个车载计算机组成，本文选用研华公司开发的ARK-3530F可扩展模块化无风扇工控机作为车载计算机，进行数据接收与指令发布。

毫米波雷达接受指令发射毫米波，遇到目标后被反射回来，获得目标的相对位置（距离、方位角）和相对速度信息，结合自车状态信息，从而得到目标的绝对速度和位置信息。RS-LiDAR-16激光雷达通过16个激光发射源快速旋转的同时发射高频率激光束，对外界环境进行持续性扫描，返回三维空间点云的距离、角度及物体反射率信息。动态决策模块对原始数据进行数据处理，包括多传感器数据融合，聚类等得到处理后的数据，从而利用处理的数据进行后续目标数据关联与跟踪。

2 目标数据关联

2.1 数据关联算法

所谓数据关联，就是将环境感知系统得到的当前观测对象与之前的观测对象建立联系，从而判断当前观测对象与之前观测对象是否为同源对象。数据关联的正确性会对目标跟踪精度带来直接影响。常用的多目标数据关联算法主要有最近邻数据关联（nearest neighbor data association，NNDA）、概率数据关联（probabilistic data association，PDA）、联合概率数据关联（joint probabilistic data association，JPDA）和多假设跟踪（multiple hypothesis tracking，MHT）等，各数据关联算法的特点如表1所示[16-17]。

在之前的工作中，研究人员采用改进的DBSCAN算法[18]对激光雷达和毫米波雷达融合点云[19]进行了基于密度的聚类[20]，已经大大减少了环境感知系统原始数据中的噪声。此外，智能车辆在实际行驶过程中，关注更多的是自车道及左右相邻车道上距离自车最近的目标，依此来讲，在三车道情况下自车前后方需要重点关注的目标数量最多为6个（自车道后方目标一般在倒车时重点关注）。因此，综合对比各数据关联算法的优缺点，可知采用NNDA算法可以快速、容易地实现智能车辆行驶过程中重点关注目标的数据关联，但传统NNDA算法容易出现误关联和漏关联问题，因此需要对其进行改进。

2.2 基于生命周期的多特征加权最近邻数据有效关联算法

传统的NNDA算法原理如图1所示。以目标预测位置作为跟踪门中心，计算落入跟踪门的各观测对象与跟踪门中心的距离。当跟踪门中只有一个观测对象时，则该对象即为目标的关联对象;当跟踪门中包含了多个观测对象时，则选择距离最小的“最近邻”对象作为目标的关联对象。这种关联原则可以快速找寻到每帧数据中的关联对象，但仅通过距离来判断得到的关联结果，其真实性存在较大疑问。当跟踪门中只有一个观测对象时，该对象可能是目标的真实量测，也有可能是雜波或是新出现的目标;同理，当跟踪门中有多个观测对象时，最近邻对象也未必是目标的真实关联对象。

针对传统NNDA算法的问题，本文设计了基于多特征加权的最近邻数据关联算法，对各车道中的目标进行数据关联。

跟踪门的设计原则主要有两点：1）要使目标的真实量测落入跟踪门的概率尽可能大;2）要尽量减少跟踪门内的无关观测对象。因此，跟踪门的设计关键是要确定其形状与大小。对于道路目标，无论是行人或是车辆等，都可以落入到一个矩形框中跟踪，因此本文将跟踪门形状设定为矩形跟踪门，其宽度设置为车道宽度，即3.75 m，长度取6 m，约一个车长的距离，跟踪门的中心位置由目标运动状态来预测确定。

通过智能车环境感知系统获得的原始数据可以得到目标的位置、速度、长度、宽度和高度、雷达散射截面积（radar cross section，RCS）等，据此，相似度函数定义如式（1）所示。

Si，j（t）=1k1Pd+k2Pv+k3Pwh+k4Pl+k5Pr，（1）

式中：Pd为相邻帧中同车道目标在y方向上的距离;Pv为相邻帧中目标速度的偏差;Pwh为相邻帧中目标的宽和高的乘积（纵切面积）的偏差;Pl为相邻帧中目标长度的偏差;Pr为相邻帧中目标RCS值的差值;kn（n=1，2，3，4，5）为各项偏差的加权系数。

式（1）中各加权系数根据相应信息数据的可靠性来选取。本文相邻两帧的时间间隔很短，可以认为两帧间隔中目标为匀速运动，且感知系统获得的目标位置和速度信息较为可靠，故在计算相似度时可赋予较高的权重。由于自车运动会导致两帧扫描时自车与目标的相对角度变化，从而导致扫描到的目标尺寸产生偏差，尤其是长度尺寸的偏差，来判断相似度的可靠性不高，因此在加权时应赋予较低的权重。RCS值的变化与目标距离和角度有关，但在连续两帧中的变化不太大，可靠性高于尺寸偏差。综上，本文设定各加权系数之间的关系如式（2）所示。

k1+k2+k3+k4+k5=1，

k1≥k2>k5>k3>k4。（2）

若当前帧获得的同车道中落入跟踪门的对象只有1个，且相似度函数值大于相似度阈值时，则认为该对象即为目标在当前帧的真实量测;若当前帧获得的同车道中落入跟踪门的对象有若干个，且相似度函数值大于相似度阈值时，取相似度最高的对象作为目标在当前帧的真实量测;若当前帧没有落入跟踪门的对象，或者所有对象的相似度均小于相似度阈值，则认为目标在当前帧丢失。

通过上述过程，实现了前后连续两帧的数据关联，但由于系统连续两帧时间间隔很短，所关联的数据可能是由环境变化、车辆抖动等带来的虚警目标。为此，本文为目标设定生命周期，符合生命周期长度的目标关联才为有效关联。目标的生命周期即目标存在的时间，由于每帧时间长度一定，因此生命周期可以用目标出现的帧数来表示。基于生命周期的有效关联算法具体步骤如下：

1）每次采样连续n帧数据进行多特征加权的最近邻数据关联;

2）建立一个0，1二值矩阵C=[C1，2 C1，2 … C1，n]，元素值为0时表示当前帧中没有能与被跟踪对象匹配的对象，为1则表示两帧中目标关联成功;

3）有效关联度c定义见式（3）：

c=‖C‖1n。（3）

当c大于有效关联度阈值ct时，则认定关联是有效的，通常ct>0.5。出于实时性考虑，本文取n=3，ct=2/3。

3 目标跟踪算法

卡尔曼滤波（Kalman filtering，KF）是由KALMAN提出的一种状态估计算法[21]，它根据当前测量值与前一采样周期的估计值，采用递归方法进行系统状态估计。其基本思想是依靠状态估计值与当前量测值的误差值的协方差之间的递推关系，不断更新系统的状态估计值和误差协方差，以不断缩小状态估计值与实际量测值之间的差距。

使用KF需要满足2个重要前提：系统是线性的;系统的测量噪声为高斯白噪声。本文利用前述算法获得目标信息，每次更新時间为132 ms，在这极短的时间间隔内，可以认为目标各个特征呈近似线性变化，并假定系统的测量噪声为符合高斯分布的白噪声。因此本文可以采用KF对目标位置、尺寸、速度等进行跟踪。

系统的状态变量包括点的纵坐标yt、长Lt、宽Wt、高Ht及y向速度t，对于前方目标还包括y向加速度t。由于测量过程中没有控制量输入，故Bu（t-1）=0。因此，系统的状态方程为

x（t）=Ax（t-1）+w（t-1），（4）

式中：w（t-1）～N（0，Q）;Q=σ2q·diag（1）。

对于起始位置在车辆前方的目标来说，

完成上述卡尔曼滤波器相关矩阵的设置后，利用KF不断迭代更新，算法主要包含预测与更新2个流程[21]，从而实现目标的跟踪，得到目标相对于本车的真实位置、尺寸及运动状态等信息。

4 试验验证

为了保证目标状态信息的可靠性，本文对所采用的目标数据关联与跟踪算法开展了实车试验。根据前文所制定的传感器配置方案以及选定的车载计算机，在智能小车试验平台上进行硬件安装，得到实车试验平台，如图2所示。将毫米波雷达ARS308-2T安装在智能小车车头端中垂线距地高度0.5 m处，激光雷达安装在车顶中心位置，为了保证激光雷达最低端激光束（-15°）能发射到地面不被车体遮挡，计算得到激光雷达距地高度为0.96 m。

本文目前仅考虑在低速场景下的应用效能，并且试验过程中，若目标与实验车辆均具有速度，则试验过程受自车速度、加速度的影响，无法反映目标的真实轨迹，故使智能小车保持静止状态。为了模拟三车道的目标跟踪，将原本宽度为8.2 m的道路以道路中线为基准划定3条宽度为2.7 m的车道，并在小车前方各车道范围内设有一个障碍目标，包括了行人、自行车以及静止路障，如图3所示。将试验平台置于道路中线，让“毫米波雷达+激光雷达”组成的环境感知系统轴线与道路中心线基本重合，将环境感知系统中横坐标范围在（-4.05， -1.35）、（-1.35，1.35）、（1.35， 4.05）的目标分别划分为左车道、当前车道和右车道中的目标，分别放入数据链表中。让行人和自行车在实验小车前方随机运动，利用本文算法进行连续帧中的数据关联和跟踪。

图4 a）显示了行人和自行车在各自车道范围做远离小车运动时的试验结果，可以看出对各目标跟踪轨迹比较连续，在各自车道内没有出现丢失和位置突变;图4 b）是行人和自行车从远处靠近试验小车的跟踪结果，期间，让自行车做变道动作，使得一段时间内落入目标跟踪门的对象不止1个，可以看出各目标轨迹依然比较连续，未出现目标丢失和位置突变，说明在自行车变道过程中产生的交互与遮挡对跟踪效果产生的影响较小;图4 a）和图4 b）中静止路障位置不变的试验结果可以看出对静止路障能够进行准确识别。试验结果综合表明，在此期间，通过本文的数据关联算法对多种特征加权计算，目标与相似度函数值最大的对象进行关联，能成功完成静态和动态目标在多帧中准确连续的关联跟踪。

5 结 语

本文利用毫米波雷达和激光雷达聚类融合的数据，基于障碍物特征权重，提出了一种多特征加权的最近邻数据关联算法，在传统最近邻关联算法的基础上，有效增加了关联的正确性，并结合卡尔曼滤波的状态估计过程，对目标进行迭代更新，实现了对目标的关联跟踪。试验结果表明，算法可以实现对静止路障以及行人和自行车随机运动的准确连续的关联跟踪，无论是轨迹相交还是轨迹无交集的情况，算法均具有良好的跟踪准确性，符合智能车辆的行驶需求。

本文仅考虑了对低速运动目标的跟踪，未来尚需解决对高速运动目标的准确、连续跟踪难题，以适应更真实的道路状况。

参考文献/References：

[1] 王鹏宇，赵世杰，马天飞，等.基于联合概率数据关联的车用多传感器目标跟踪融合算法[J].吉林大学学报（工学版），2019，49（5）：1420-1427.

WANG Pengyu， ZHAO Shijie， MA Tianfei， et al. Vehicle multi-sensor target tracking and fusion algorithm based on joint probabilistic data association[J]. Journal of Jilin University（Engineering and Technology Edition）， 2019， 49（5）： 1420-1427.

[2] YANG Tao， MEHTA P G. Probabilistic data association-feedback particle filter for multiple target tracking applications[J]. Journal of Dynamic Systems， Measurement and Control， 2018， 140： 1-14.

[3] 孙进平，付福其，付锦斌，等.应用超图匹配的多假设群目标跟踪方法[J].信号处理，2017，33（11）：1498-1504.

SUN Jinping， FU Fuqi， FU Jinbin， et al. Multiple hypothesis group target tracking using hypergraph matching[J]. Journal of Signal Processing， 2017， 33（11）： 1498-1504.

[4] 蒋富勤，朱鲲，祝献，等.一种改进的多假设跟踪算法[J].声学与电子工程，2014（1）：9-12.

[5] 王宇霞，赵清杰，蔡艺明，等.基于自重构粒子滤波算法的目标跟踪[J].计算机学报，2016，39（7）：1294-1306.

WANG Yuxia， ZHAO Qingjie， CAI Yiming， et al. Tracking by auto-reconstructing particle filter trackers[J]. Chinese Journal of Computers， 2016， 39（7）： 1294-1306.

[6] 王晓君，裴福俊，刘红云，等.一种改进马氏距离的最近邻数据关联算法[J].导航定位学报，2015，3（4）：50-56.

WANG Xiaojun， PEI Fujun， LIU Hongyun， et al. Novel nearest neighbor data association algorithm based on improved mahalanobis distance[J]. Journal of Navigation and Positioning， 2015， 3（4）： 50-56.

[7] 胡朋启，李蔚清.异类传感器数据融合的最近邻-点拓扑关联法[J].计算机与数字工程，2019（6）：1347-1350.

HU Pengqi， LI Weiqing. An adjacent-point topological association method based on heterogeneous sensors[J]. Computer & Digital Engineering， 2019（6）： 1347-1350.

[8] ZHANG Kaijie， LIN Haoshen， LIU Gang， et al. Improved interacting multiple model-new nearest neighbor data association algorithm[C]//International Conference on Control Science and Systems Engineering. [S.l.]： IEEE， 2018： 499-504.

[9] 梅立雪，汪兆棟，张浦哲.一种邻帧匹配与卡尔曼滤波相结合的多目标跟踪算法[J].科学技术与工程，2019，19（15）：204-210.

MEI Lixue， WANG Zhaodong， ZHANG Puzhe. A multi-object tracking algorithm based on the combination of neighborhood frame matching and Kalman filtering[J]. Science Technology and Engineering，2019，19（15）： 204-210.

[10]郑彩云.基于卡尔曼滤波的雷达航迹跟踪算法的综述[J].科技视界，2019（11）：33-34.

ZHENG Caiyun. A survey of radar track tracking algorithm based on Kalman filter[J]. Science and Technology Vision， 2019（11）：33-34.

[11]周小钧，高利，赵亚男.一种复杂交通环境下的毫米波雷达目标跟踪方法[J].公路交通科技（应用技术版），2019（10）：332-338.

[12]杨永建，樊晓光，王晟达，等.基于修正卡尔曼滤波的目标跟踪[J].系统工程与电子技术，2014，36（5）：846-851.

YANG Yongjian， FAN Xiaoguang， WANG Shengda， et al. Target tracking based on amendatory Kalman filter[J]. Systems Engineering and Electronics， 2014， 36（5）：846-851.

[13]IZADI-GHODOUSI Z， HOSSEINPOUR M， SAFAEI F， et al. An efficient target tracking in directional sensor networks using adapted unscented Kalman filter[J]. Wireless Personal Communications， 2019（1）： 1925-1954.

[14]FARAHI F， YAZDI H S. Probabilistic Kalman filter for moving object tracking [J]. Signal Processing Image Communication， 2020， 82： 1-10.

[15]郭丽，刘磊，朱宏康.优化自适应卡尔曼滤波跟踪方法研究[J].中国电子科学研究院学报，2019，14（9）：929-934.

GUO Li， LIU Lei， ZHU Hongkang. Optimized adaptive Kalman filter tracking method [J]. Journal of CAEIT， 2019（9），14：929-934.

[16]冯洋. 多目标跟踪的数据关联算法研究[D]. 西安： 西安电子科技大学， 2008.

FENG Yang. The Research on Data Association in Multi-target Tracking[D]. Xi′an： Xidian University， 2008.

[17]KENARI L R， ARVAN M R. Comparison of nearest neighbor and probabilistic data association methods for non-linear target tracking data association[C]//2014 Second RSI/ISM International Conference on Robotics and Mechatronics （ICRoM）. [S.l.]： IEEE， 2014： 47-52.

[18]KHAN K， REHMAN S U， AZIZ K， et al. DBSCAN： Past， present and future[C]//The Fifth International Conference on the Applications of Digital Information and Web Technologies （ICADIWT 2014）. [S.l.]： IEEE， 2014： 232-238.

[19]黄漫国， 樊尚春， 郑德智， 等. 多传感器数据融合技术研究进展[J]. 传感器与微系统， 2010（3）： 5-8.

HUANG Manguo， FAN Shangchun， ZHENG Dezhi， et al. Research progress of multi-sensor data fusion technology[J]. Transducer and Microsystem Technologies， 2010（3）：5-8.

[20]许虎寅， 王治和.一種改进的基于密度的聚类算法[J]. 微电子学与计算机， 2012， 29（2）： 44-47.

XU Huyin， WANG Zhihe. An improved density-based clustering algorithm[J]. Microelectronics and Computer， 2012， 29（2）： 44-47.

[21]BISHOP G， WELCH G. An introduction to the Kalman filter[EB/OL].

http：//cs.unc.edu/～tracker/media/pdf/SIGGRAPH2001_slides_08.pdf，2020-02-15.

收稿日期：2020-03-24;修回日期：2020-05-05;责任编辑：冯 民

基金项目：国家自然科学基金（11802140）

第一作者简介：赵 丰（1979—），男，河南济源人，工程师，主要从事车辆先进技术方面的研究。

通讯作者：张 明副教授。E-mail：13955036975@163.com

赵丰，王利辉，陈俊吉，等.

基于卡尔曼滤波的多特征加权最近邻数据关联与跟踪算法

[J].河北科技大学学报，2020，41（3）：218-224.

ZHAO Feng， WANG Lihui， CHEN Junji， et al.

Multi-feature weighted nearest neighbor data association and tracking algorithm based on Kalman filter

[J].Journal of Hebei University of Science and Technology，2020，41（3）：218-224.