基于最小临近点迹和航迹关联的多源目标融合方法

赵树廉，吴思宇，赵鹏云，田浩东，唐 宇，孟璋劼，唐逸超

（1.清华大学车辆与运载学院，北京 100084；2.中国汽车工程研究院股份有限公司，重庆 401122；3.中汽院智能网联科技有限公司，重庆 401122；4.重庆大学机械与运载工程学院，重庆 400044）

汽车已成为交通运输中的重要组成部分，是民众出行的重要选择。随着汽车持有量持续增加，交通安全、能源消耗越来越成为制约汽车产业发展的主要因素。汽车新能源、智能化、网联化技术的出现，为解决上述问题提供了解决手段。国务院《中国制造2025》、汽车产业“十三五”发展状况图解以及《国家车联网产业标准体系建设指南（智能网联汽车）（2017）》均提出“智能网联汽车”是未来的重要发展方向。2021年上半年，具备L2辅助驾驶功能的乘用车新车累计销量156.8万辆，市场渗透率达到20%左右，其中新能源汽车市场渗透率超过30%。由此可知，智能网联电动车，已成为汽车行业的研究热点。

然而，智能驾驶汽车突破的关键点之一在于构建高精度、高可靠性及实时的环境感知模型。环境感知模型是汽车与外界环境交互的媒介，是汽车决策系统的前提条件。目前，毫米波雷达、视觉相机以及激光雷达是感知系统主要应用的传感器。毫米波雷达能够可靠地检测远距离目标并测速，扩展感知范围，提高跟踪精度；视觉传感器能够提供丰富的环境语义信息，为感知系统对环境的理解提供支持；激光雷达能够稳定可靠地检测远距离目标物并测量其速度。针对单一传感器无法满足感知需要的现状，多传感器目标跟踪融合成为研究的热点。

多传感器目标融合主要包括集中式融合、分布式融合以及混合式融合，数据融合过程主要包括多源传感器目标物信息关联，多目标跟踪及融合3个部分：

（1）数据关联包括点迹与航迹的关联以及航迹与航迹的关联，是按照一定的关联度量标准进行的。所采用的方法主要包括最近邻域法、概率密度数据关联、联合概率数据关联、多假设追踪等。由于这些方法采用的关联指标为马氏距离与欧氏距离，关联区域为不同传感器探测范围的重叠区域，所以存在探测范围较小、易丢失非重叠区域的信息、在多目标情况下匹配不准确、后续跟踪不稳定等问题。此外，多源异构传感器采集的数据在时间和空间上并不完全一致，因此数据融合前需进行空间同步。由于不同传感器的测量精度不同，给后期的数据关联和融合带来较大难度。

（2）多目标跟踪针对传感器会存在数据丢失引起某一目标物下一帧缺少关联值的问题，现有的目标跟踪方法通常采用目标估测值的阈值进行跟踪，通过考察在设定的周期内有无观测值来判断目标物是否丢失或消失。如果周期内均无观测值，则认为目标物消失。但是该方法并未有效解决跟踪不稳定的问题。

（3）目标级数据融合算法对不同类型的传感器系统检测到的目标通过融合算法获得被感知目标物更精确的信息。常用的方法为基于随机统计的方法，主要包括加权平均法、卡尔曼滤波法、贝叶斯估计法、DS证据推理法等。由于不同传感器的测量特性存在差别，所以需要考虑不同传感器的误差进行数据融合，刘卫东等等为得到位置误差更小的、更加精确可靠的、融合性能更优的信息，利用凸组合融合算法和Bar-Shalom-Campo融合算法对目标航迹进行融合估计，并利用这3种方法对设定目标的运动情况进行仿真估计，从而给出仿真结果。

然而，目前大多数传感器数据融合为毫米波雷达与视觉传感器的数据融合，它仅基于毫米波雷达检测到的目标物在视觉传感器生成的图像中设定感知区域，然后通过视觉方法对该区域继续检测。尽管该方法在一定程度上提高了雷达的检测准确率和视觉检测的运算效率，但未加强两种传感器的信息互补，所以融合效果不佳，感知准确度有待提升。ZHANG Bingli等针对这一问题提出了一种基于YOLO v2和分层聚类的融合方法，但在隧道或车辆密集处等复杂场景中仍会出现一定程度的误差。同时，该算法的验证仅局限于照片检测，并未开展实车试验。

因此，针对现有方法存在的探测范围较小，易丢失非重叠信息，并且在多目标情况下匹配不准确，会导致后续跟踪不稳以及融合效果不佳等问题，本文基于欧氏距离与余弦相似度的点迹和航迹的数据关联提出了多源异构的目标融合方法，充分考虑不同传感器的优缺点并进行优势互补，大大减小了计算量并提高了跟踪效果，使用状态更新方程对不同传感器测量值进行融合，并最终通过在实车环境中利用毫米波雷达和视觉传感器进行试验，验证了算法的准确性。

1 目标融合方法

基于最小邻近点迹和航迹的多源异构目标融合方法采用的多源异构数据来源于Mobileye视觉传感器和毫米波雷达感知的目标数据，通过最小临近实现点迹和航迹的正确关联。其中，Mobileye视觉传感器主要用来识别车道线、信号灯、车辆和行人等，并测量目标物的位置和速度；毫米波雷达用于识别障碍物并得到精确的位置和速度测量值。Mobileye视觉传感器的优点为目标物类别识别准确且横向测距精度高，相比于Mobileye视觉传感器，毫米波雷达的纵向测距和测速更准确。因此，该方法利用多源数据的各自优势进行融合互补，开发Mobileye视觉传感器和前向毫米波雷达的目标级数据融合算法，主要技术流程如下：

（1）数据解析：Mobileye视觉传感器和毫米波雷达数据解析。

（2）坐标统一及目标筛选：Mobileye视觉传感器和毫米波雷达坐标统一，并进行目标物的筛选。

（3）目标物初始化：初始化目标物状态信息。

（4）数据关联：使用空间相似度指标（欧式距离、余弦相似度、航迹信息）对两个传感器检测的目标物运动状态信息进行关联。

（5）目标物管理及跟踪：目标物的生命周期管理，确定有效目标物，对于匹配的测量值小于设定的帧数的目标物认为是虚假目标，进行剔除。

（6）数据融合：考虑不同传感器的测量特性进行视觉和雷达系统输出信号融合。

（7）融合结果：在确定目标物状态的基础上，再根据摄像头给出的分类信息来确定目标物种类。

多源异构目标融合方法流程，如图1所示。

图1 多源异构目标融合方法流程

1.1 坐标统一

如图2所示，由于在车体坐标系中，向前、向左、向上，而在激光雷达坐标系中，指向输出电缆的相反方向，向左、向上，不同的传感器坐标系在时间同步和空间坐标不能完全统一，所以会对多传感器数据标定造成困难。

图2 不同传感器间的坐标变换

因此，在数据解析阶段对Mobileye视觉传感器和毫米波雷达的坐标进行统一。坐标统一步骤如下：

（1）在目标点云中取出点集。

（2）从原始点云中取出点集对应的点集。

（3）求出到的变换矩阵。

（4）对原始点云应用变换矩阵得到新的点集。

（5）计算出和点集中对应点的平均距离ˉ。

（6）判断平均距离ˉ是否小于阈值或迭代次数是否大于设定值NUM，若两个条件任满足其一，则输出坐标统一结果，迭代结束；否则返回（2）重新迭代。

其中，到坐标变换矩阵的一般形式为：

1.2 数据关联

数据关联是目标跟踪，尤其是杂波环境下多目标跟踪的重要环节。点迹关联和航迹关联是目前常见的数据关联方法。这两种方法均可对两个传感系统的目标物数据列表中对应同一个目标物的数据进行关联。当仅使用点迹关联方法可对目标物进行快速地关联和匹配，而目标物又距离较远时，会存在较大的误差。如果结合使用航迹关联方法，则可较好地弥补点迹关联的不足。因此，本文提出了点迹关联与航迹关联相结合的关联方法，可同时对距离较远和距离较近的目标物实现准确有效的关联匹配。

本方法包含点迹关联和航迹关联，在进行目标物关联之前，需将Mobileye视觉传感器和毫米波雷达的坐标统一，并对目标物进行筛选和初始化，如图3所示，其具体步骤为：

图3 数据关联算法流程

（1）判断目标物是否为首次出现在传感系统的探测范围内。如果是，则执行（2），否则执行（3）。

（2）根据两个传感系统输出的目标物的点迹位置数据，计算目标物之间的最小临近参数（欧式距离和余弦相似度），并构建数据关联矩阵，再根据关联矩阵和预设阈值对两个传感系统输出的目标物数据进行关联，输出目标物关联结果。

（3）判断目标物航迹信息是否可以使用。如果是则执行（4），否则执行（2）。（4）根据前后帧的目标物的航迹信息，确定关联目标物，输出目标物关联结果。

在（2）中，构建关联矩阵使用的方法为点迹关联，涉及的最小临近参数为欧氏距离与余弦相似度，在构建时需计算两个目标物之间的最小欧式距离和最大余弦相似度，具体计算方法如下：

假设Mobileye视觉传感器检测到的目标为：

毫米波雷达检测到的目标为：

根据点迹最小临近距离和最大余弦相似度，对与进行点迹关联，其关联函数可表述为：

式中：为融合最小临近欧氏距离与最大余弦相似度的点迹关联结果。

为最小临近距离关联函数，其表达式为：

式中：D为与的欧式距离。

为点迹余弦相似度关联函数，其表达式为：

式中：θ为与的夹角。

在（4）中使用的方法为航迹关联方法，使用的航迹信息包括目标物ID和生命周期状态。Mobileye视觉传感器和雷达均对各自探测的目标进行了跟踪，并在系统中赋予对应的ID，在对前后帧数据进行关联时，使用跟踪的ID及目标物生命周期情况进行匹配可增加融合结果跟踪的稳定性，并减小计算量。在目标物刚进入两个传感系统的探测范围中时，可以先使用点迹关联确定关联矩阵，关联完成后，后续的周期中可以使用航迹进行关联，省去计算关联矩阵的计算消耗。

1.3 目标跟踪算法

目标物跟踪步骤主要是用于形成对目标物的生命周期管理，方便对目标物有效性进行评价，及时剔除失效的或虚假的目标，提高检测的可靠性和有效性。目标物跟踪步骤包括：

（1）对目标物数据应用卡尔曼滤波，形成下一时刻的预测状态值和与状态值对应的预测协方差：卡尔曼滤波算法主要是根据当前的相对运动状态，预测下一周期目标物的可行区域，并根据实际目标物信息进行修正，从而实现前后数据的连续跟踪。本文中采用的卡尔曼滤波器的计算过程，如图4所示。

图4 卡尔曼滤波器的计算过程

（2）对当前时刻的各个目标物数据与上个时刻的目标物数据进行关联匹配得到匹配结果，匹配结果包括完全匹配的目标物数据、新目标物数据以及当前时刻不存在目标物数据。

（3）对完全匹配的目标物数据则使用当前时刻的目标物数据对（1）中的预测结果进行修正和更新，得到目标新的状态值，并赋予同一ID。

对当前时刻不存在目标物数据的目标物，即（2）的检测结果为当前时刻不存在目标物数据时，会记录对应目标物连续未出现帧数，当连续未出现帧数大于帧数阈值时，则判定目标物消失；帧数阈值根据目标物的连续存在时长以及目标物位置数据进行调整；目标物已存在的时间越长，帧数阈值越大，目标物已经存在时间越短，帧数阈值越小，如果目标物位于目标区域以内，则增大帧数阈值，如果目标物位于目标区域以外，则减小帧数阈值。在实际情况下由于传感器误差等原因，可能会出现目标物短暂消失或目标物是由误差造成的等情况，所以设置帧数阈值可以避免错误地将目标物移除，目标物已存在的时间越长，则目标物是由误差造成的可能性就越小，因此为其预留更长的帧数阈值；相反，目标物存在的时间越短，则其是由误差造成的可能性就越大，因此为其预留的帧数阈值更小，如果目标物出现在目标区域内，如正前方或与传感系统距离非常近，则代表对当前系统可能有一定的威胁，应当重点关注，帧数阈值应适当调大。

目标物跟踪步骤中，如果一个目标物仅在一个传感系统中有对应的目标物数据，则由对应的传感系统对其进行持续跟踪，当目标物进入两个传感系统共同的探测范围内时，直接进行目标物数据融合。如果某一个传感系统先检测到目标物后，目标物离开第1个传感系统进入第2个传感系统的检测范围，那么会把第1个传感器记录的数据删除掉，不再与第2个传感器感知的数据融合。

本文中，则是在只有1个传感系统检测到目标物时就进行跟踪，当目标进入两个传感器共同的探测范围时，进行融合，进而实现目标物的稳定跟踪，在此过程中，目标物对应的系统ID不变，也不需要进行关联匹配。提高处理速度，同时相比现有技术可以利用和保留更多的数据信息。

目标物的生命周期是对目标物从形成到消亡的整个过程进行描述。不同目标的生命周期值反映了该目标的有效性，从而为后续的传感器信息融合提供依据。为描述目标生命周期，引入表1所示的参数。

表1 目标物周期管理参数

目标跟踪算法流程，如图5所示。

图5 目标跟踪管理流程

1.4 数据融合算法

数据融合的本质是通过融合算法整合来自多个传感器的信息，扩展系统的时空覆盖率，提高系统的精度、可靠性以及信息利用率。融合算法则是使用基于卡尔曼滤波的状态更新方程进行融合。其步骤为根据上一时刻的融合结果预测当前时刻的预测状态值及对应的预测协方差矩阵，然后对两个传感系统的数据进行融合，融合公式如下：

式中：为预测状态值；为预测协方差矩阵；和分别为对应时刻的两个传感系统对同一目标物的观测值；和为对应的噪声矩阵；X和P分别为融合后的状态值和协方差矩阵。

X为目标物的状态变量，本研究中目标物状态变量为目标物与主车的速度和距离，即

其中，噪声矩阵和满足噪声矩阵模型：

在预测过程中，通过调整噪声矩阵中的参数来调节两个传感系统的观测值对融合结果的影响程度。具体的目标物数据包括横向信息、纵向信息、速度信息和类别信息。当融合得到目标物的横向信息或类别信息时，通过调节噪声矩阵增大Mobileye视觉传感器的观测值的影响程度；当融合得到目标物的纵向信息或速度信息时，通过调节噪声矩阵增大毫米波雷达系统的观测值的影响程度。

2 实车试验结果及分析

为验证该方法准确值的有效性，本研究在实车上搭建数据采集平台，对目标车和跟车两种场景进行试验，并分析试验数据。此法是以高精度定位惯导系统为基准，对数据采集的精度进行分析，即根据高精度定位系统计算目标物的位置与运动状态，并与主车传感器输出数据进行对比以明确偏差。

2.1 实车数据获取平台

通过选用6路大陆408 mm波雷达、1个Mobileye EQ2摄像头、6路摄像头、i-Tester数据采集设备以及组合惯导来搭建数据获取平台实车。

（1）毫米波雷达可以提供目标物类型、尺寸、精准的目标物位置与速度信息等，其位置测量精度可以达到分米级，速度测量精度达到0.1 km/h，且位置与速度信息为独立测量，互不干扰。

（2）Mobileye视觉传感器安装于车辆前方，可以弥补毫米波雷达在目标物类别识别、标志牌识别和车道线识别方面的弱势，提供更加丰富的场景信息。

（3）摄像头安装于车辆前方和左、右侧，提供各方向的视场信息，可以弥补Mobileye视觉传感器无法提供视频图像的缺陷，为数据分析人员提供更加直观的视频数据。

（4）i-Tester用来采集自车的CAN总线数据，以及储存毫米波雷达和Mobileye视觉传感器的目标物感知数据。

（5）差分全球定位系统（Differential Global Position System，DGPS）可以提供车辆实时经纬度位置，方便进行车辆定位管理、行驶区域自动区分等操作，DGPS还可以作为多传感器时间同步的基准。

2.2 试验场景

由于毫米波雷达和视觉传感器被广泛应用于自适应巡航控制（Adaptive Cruise Control，ACC）系统、自动紧急制动（Autonomous Emergency Braking，AEB）系统、前方碰撞预警（Forward Collision Warning，FCW）系统中，所以这些系统主要对主车前方目标物进行识别，并根据识别结果触发其功能。因此，本试验设置了两个具有代表性的场地工况与实际道路测试，通过这些案例来验证目标级数据融合算法的精度、多源传感器的横向与纵向位置和速度。其中，两个典型的场地测试场景分别为目标车静止与跟车试验。

2.2.1 目标车静止

主车与目标车均静止，测量不同车间距时的目标物测量精度，其中车间距分别取值7.5 m和125 m。目标车静止如图6所示。

图6 目标车静止

2.2.2 跟车条件

主车与目标车初始间距20 m、100 m，同时起步，主车速度增加至50 km/h以上，目标车稳定至30 km/h，即保证目标车速度低于主车，两车间距可以逐渐减小，主车驾驶员根据车间距变化逐步正常减速跟车，当跟车稳定5 s后，目标车制动停车，主车跟随停车。车间距动态增加，如图7所示。

图7 车间距动态增加

2.2.3 实际道路测试

实际道路测试，是在装有本方案的测试车辆上、在高速公路上采集数据，通过人工标注作为真值与本算法的处理结果来对比验证本方案的准确性，定义入检率、正检率、误检率、漏检率等指标，其计算公式为：

式中：为车辆总数；为检测到的车辆数；为漏检的车辆数；为检测到的正确的车辆数；为检测到的错误的车辆数。

2.3 试验数据分析与结果

本次试验主要目的是验证基于最小邻近点迹和航迹数据关联的多源异构的目标融合方法的纵向位置和纵向速度精度，通过使用该方法在探测时间和空间两个维度上融合Mobileye视觉传感器和毫米波雷达目标数据并跟踪目标。传感器真值使用DGPS数据。数据分析采用均方根误差（Root Mean Squared Error，RMSE）来评价传感器误差大小。融合结果在探测时间和空间范围上为Mobileye视觉传感器和毫米波雷达的并集，对于以下3种情况下的目标物都进行了数据采集与分析：单独毫米波雷达检测到的目标物、单独Mobileye视觉传感器检测到的目标物、Mobileye视觉传感器和毫米波雷达同时检测到的目标物，并与DGPS真值数据进行对比。

2.3.1 静止场景

远距静止场景下纵向测距的计算结果，毫米波雷达的RMSE为0.615 m，Mobileye视觉传感器的RMSE为12.753 m，融合之后的RMSE为0.853 m，Mobileye视觉传感器在远距场景下的识别误差大于毫米波雷达识别误差。而对于纵向速度的检测，毫米波雷达的RMSE为0.051 m/s，Mobileye视觉传感器的RMSE为0.291 m/s，融合之后的RMSE为0.052 m/s。远距静止下各传感器的RMSE见表1。远距静止下的位置与速度的误差对比，如图8所示。

表1 远距静止下各传感器的RMSE

图8 远距静止下的位置与速度的误差对比

近距静止场景下纵向测距的计算结果，毫米波雷达的RMSE为0.144 m，Mobileye视觉传感器的RMSE为0.35 m，融合之后的RMSE为0.134 m，Mobileye视觉传感器在远距场景下的识别误差大于毫米波雷达的识别误差，融合的结果优于两个传感器。而对于纵向速度的检测，毫米波雷达的RMSE为0.046 m/s，Mobileye视觉传感器的RMSE为0.049 m/s，融合之后的RMSE为0.47 m/s。根据误差分析可知，融合结果的速度纵向位置精度更加接近毫米波雷达的探测结果，横向位置的精度更加考虑Mobileye视觉传感器的识别结果，而匹配上的类型均使用Mobileye视觉传感器的识别结果，用毫米波雷达的测距测速性能弥补了Mobileye视觉传感器的短板，根据不同传感器的测量特性来调整误差矩阵，实现上述优化结果。近距静止下各传感器的RMSE，见表2。近距静止场景下各传感器位置与速度对比，如图9所示。

图9 近距静止场景下各传感器位置与速度对比

表2 近距静止下各传感器的RMSE

2.3.2 跟车场景

远距跟车场景下纵向测距的计算结果，毫米波雷达的RMSE为0.96 m，Mobileye视觉传感器的RMSE为2.37 m，融合之后的RMSE为0.87 m，融合后的结果优于两个传感器的识别结果，Mobileye视觉传感器在远距场景下识别误差远大于毫米波雷达。而对于纵向速度的检测，毫米波雷达的RMSE为0.54 m/s，Mobileye视觉传感器的RMSE为0.76 m/s，融合之后的RMSE为0.60 m/s，毫米波雷达在测速方面更有优势。远距跟车场景下各传感器的RMSE，见表3。远距跟车下的位置与速度的误差对比，如图10所示。

图10 远距跟车下的位置与速度的误差对比

表3 远距跟车场景下各传感器的RMSE

近距跟车场景下纵向测距的计算结果，毫米波雷达的RMSE为0.28 m，Mobileye视觉传感器的RMSE为0.21 m，融合之后的RMSE为0.23 m，Mobileye视觉传感器在远距场景下识别误差小于毫米波雷达，融合的结果介于两者之间。而对于纵向速度的检测，毫米波雷达的RMSE为0.14 m/s，Mobileye视觉传感器的RMSE为0.21 m/s，融合之后的RMSE为0.13 m/s，融合的结果优于两个传感器。近距跟车场景下各传感器的RMSE，见表4。近距跟车场景下位置与速度对比，如图11所示。

表4 近距跟车场景下各传感器的RMSE

图11 近距跟车场景下位置与速度对比

2.3.3 实际道路测试

实际道路测试，是在高速公路上采集数据，通过人工标注作为真值与本算法的处理结果来对比验证其准确率。根据式（9），对实际道路测试中传感器的检测数据分析计算出其入检率、正检率、误检率与漏检率并与ZHANG Bingli等的试验结果进行对比，分析结果见表5。由此可知，相比于单一传感器，本文基于最小临近点迹和航迹关联的多源目标融合方法成功融合了毫米波雷达与Mobileye的优势，在降低了漏检率的同时提高了检测准确率。实际道路测试中传感器的检测结果对比，见表5。

表5 实际道路测试中传感器的检测结果对比

2.3.4 试验结论

根据误差分析可知，融合结果的速度纵向位置精度更加接近毫米波雷达探测结果，横向位置的精度更加考虑Mobileye视觉传感器的识别结果，而匹配上的类型均使用Mobileye视觉传感器的识别结果，用毫米波雷达的测距测速性能弥补了Mobileye视觉传感器的短板，根据不同传感器的测量特性来调整误差矩阵，实现上述优化结果。同时，利用状态更新方程融合多传感器匹配目标的数据可以调整噪声矩阵参数，实现多传感器互补的优势。最终的融合结果优于单个传感器。因此，通过毫米波雷达与Mobileye视觉传感器的多源异构的目标融合方法进行环绕视野感知可以获得更准确的环境信息，不仅提高了准确率，还降低了漏检率，从而实现更好的检测效果。

3 结语

本文探讨了多源异构传感器数据融合问题，通过采用点迹和航迹相结合的数据关联算法对多源传感器的数据关联算法进行改进，提高了匹配的精确度，减小了计算量，对不同区域的目标物设置不同的估计预测值的最大阈值，对于感兴趣区域的目标物更加稳定，使用状态更新方程对多传感器匹配的目标物进行数据融合，调整噪声矩阵参数，实现多传感器的优势互补，融合结果优于单一传感器，体现了该融合算法的可行性和有效性。

本文研究的内容为前向毫米波雷达和前向视觉的融合，而对于后向和侧向区域的探测不足，未来考虑加入多个毫米波雷达和多线激光雷达进行环视感知，尤其是激光雷达的测距更精确，并且可以提供目标物三维信息，得到更精确的环境信息。