CN115856872A - 一种车辆运动轨迹连续跟踪方法 - Google Patents

Abstract

本发明公开了一种车辆运动轨迹连续跟踪方法，主要包括目标运动信息的感知、短轨迹片段的构建、短轨迹片段的关联和中断目标轨迹重构四个阶段：首先利用架设于检测道路上方的毫米波雷达感知检测范围内交通目标的位置和速度信息；根据雷达检测结果将相邻帧目标连接成为可靠的短轨迹片段；然后基于时空特性和运动特性构建模糊相关函数来描述基准轨迹与新轨迹的匹配关系，将短轨迹连接成为长轨迹；最后基于三次Hermite插值法重构满足关联关系的新、旧轨迹的空缺，实现对车辆运动轨迹的连续跟踪。本发明不仅可以提高目标个数估计准确率，还可以解决传统算法因关联错误导致的轨迹断裂、单目标多轨迹等问题。

Description

一种车辆运动轨迹连续跟踪方法

技术领域

本发明涉及智能交通技术领域，特别涉及一种车辆运动轨迹连续跟踪方法。

背景技术

毫米波雷达作为一个可以全天候工作的传感器，具有较强的抗干扰能力以及较高的检测精度，能够很好地适应各种不同的场景，且不受天气、光照等外在因素的影响。毫米波雷达最初用于军事上的低角度跟踪、精确制导、测量和成像应用。得益于智能交通系统的发展，毫米波雷达近年来在智能交通领域得到了广泛的应用，将毫米波雷达安装在路口或者路侧，通过调整安装位置可实现对交通目标的全覆盖检测与跟踪，获取城市道路中车辆行驶轨迹数据。这些轨迹数据蕴含着大量的交通信息，为后续展开交通参数提取、交通异常事件监测、交通状态估计等提供了基础数据支撑。

目前，针对车辆轨迹的采集技术主要包括基于视觉的传感技术、基于激光雷达的传感技术以及基于毫米波雷达的传感技术。基于视觉的传感采集技术最早应用于轨迹跟踪中，一种基于多维特征融合的视频多目标跟踪方法(201910964726)，通过目标历史轨迹分析得到运动方向、预测的位置，结合目标之间的内容特征相似度进行匹配连接，然而由于相机易受到外界因素的影响，会造成图像模糊而导致轨迹跟踪效果较差。随着自动驾驶的出现，雷达技术逐渐成为车辆轨迹跟踪领域先进技术方向。基于激光雷达的传感采集技术主要对运动目标进行检测得到包含运动信息的三维点云数据，实现对物体的三维高精度识别检测。一种基于激光雷达的动态多目标跟踪方法(202110467582)，通过对不同运动特性的物体分别建立不同的运动模型，但激光雷达在浓烟、浓雾等大气条件或恶劣天气下会加剧衰减，传播距离受限。毫米波雷达作为一个可以全天候工作的传感器，具有较强的抗干扰能力以及较高的检测精度，在大气环境影响下衰减较弱、对烟雾的穿透性强，可以实现远距离的感应和检测，解决了激光雷达衰减的问题，在车辆轨迹跟踪领域中有着显著的优势。

发明内容

本发明的目的是针对实际交通场景中多目标跟踪的难点与不足，提出了一种基于毫米波雷达点云的车辆运动短轨迹片段的多阶段目标跟踪方法。首先根据雷达解析结果将相邻帧目标连接成为可靠的短轨迹片段，然后基于时空特性和运动特性构建模糊相关函数来描述基准轨迹与新轨迹的匹配关系，将短轨迹连接成为长轨迹，最后基于三次Hermite插值法连接满足关联关系的轨迹对，实现对交通目标的连续跟踪。本发明能够解决因关联错误导致的轨迹断裂、单目标多轨迹等问题，实现对交通目标运动轨迹的连续跟踪。

本发明至少通过如下技术方案之一实现。

一种车辆运动轨迹连续跟踪方法，包括以下步骤：

S1、利用毫米波雷达获取探测范围内交通目标的原始点云数据以获取雷达检测数据，每一个点代表检测到的交通目标；

S2、基于雷达检测数据提取交通目标位置、速度特征，通过关联连续两帧的测量数据对交通目标轨迹进行生成和终止，将相邻帧目标连接成为短轨迹片段，并对关联结果进行修正，完成初始小轨迹片段的构建；

S3、基于时空特性和运动特性构建模糊相关函数，以描述基准轨迹与新轨迹的匹配关系，计算轨迹之间的相似度得到代价矩阵，将短轨迹段关联问题视为分配问题进行解决；

S4、针对存在间断的已经关联上的新旧轨迹对，连接满足关联关系的中断轨迹，根据轨迹在已知时刻对应的状态值，估算出其在间断时刻对应的数据，填补轨迹片段间的空缺，完成目标轨迹的连续跟踪。

进一步地，根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S1毫米波雷达检测得到的点云数据信息包括交通目标相对于雷达安装位置的横向距离、纵向距离，以及点的横向位置、纵向位置、横向速度、纵向速度和时间戳。

进一步地，步骤S2的短轨迹片段构建，包括：

S201、在进行相邻帧的关联时，选取速度和位置信息进行相似度匹配计算，若超过设定的阈值θ，则两个相邻帧的检测响应d_i(k-1)与d_j(k)属于同一目标的轨迹序列，若不属于同一目标，将第k帧的检测点迹作为新轨迹的起点，继续与下一帧的检测响应进行配对，直至所有检测序列的检测响应关联完毕；

S202、对初步关联的结果进行进一步的修正，剔除同一目标在同一时刻生成的分裂轨迹：

假设有轨迹TR_A和TR_B，两条轨迹采样时间在第i帧到第j帧重叠，建立如式(1)判断准则，若判断准则成立，认为两条轨迹源于同一个目标，则将较短的轨迹视为分裂轨迹剔除，另一轨迹则对应原目标，以此类推，进行多次剔除，直到轨迹采样时间内没有满足以下条件的分裂轨迹：

式中，

分别表示轨迹TR_A、TR_B中第k帧与雷达的距离；

表示轨迹TR_A、TR_B中第k帧时的速度；ξ_R是距离差门限，ξ_v是速度差门限。

进一步地，步骤S3具体包括：

S301、针对交通目标的运动信息，基于运动预测的模糊策略进行关联，假设两条可能关联的短轨迹设TR_i和TR_j，称时间在前的短轨迹为基准轨迹TR_i，该轨迹的起始时刻为

终结时刻为

时间在后的短轨迹为新轨迹TR_j，该轨迹的起始时刻为

终结时刻为

对基准轨迹和新轨迹分别进行前向和逆向预测至

和

的中点时刻k_c，为量化基准轨迹中点时刻k_c预测状态

和新轨迹k_c时刻预测状态

的匹配关系，使用正态型隶属度函数来计算两个轨迹片段的相关程度：

式中，u_m是第m个模糊因素；σ_m是第m个模糊因素的误差方差；τ_m是调整度；ε_T是衰减因子；

式中，u₁、u₂分别表示位置、速度的模糊因素；μ₁(u₁)、μ₂(u₂)分别表示位置、速度的相关性程度；τ₁、τ₂分别表示位置、速度的调整度；

分别表示位置、速度的误差方差；k_c表示

和

的中点时刻；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向位置和横向位置的估计值；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向速度和横向速度的估计值；

基准轨迹TR_i和新轨迹TR_j基于位置和速度的模糊相似度表示为：

f(TR_j|TR_i)＝a₁μ₁(u₁)+a₂μ₂(u₂) (5)

式中，a₁和a₂分别表示位置、速度模糊因素所对应的权重；

S302、基于公式(5)得到关联代价矩阵，基于小轨迹集合T以及模糊关联函数f(·)，使用匈牙利算法在多个关联可能性中选择使得整体最优的关联匹配结果：

式中，T＝{TR₁,TR₂,...,TR_M}是初步关联生成短轨迹的集合；L＝{l₁,l₂,...,l_N}是冲突矩阵的集合；f(·)是用于关联小轨迹的模糊函数；M为短轨迹片段集合中的轨迹总数；N是冲突矩阵的总数；TR_M代表轨迹集合中第M个短轨迹；l_N代表第N个短轨迹的关联结果。

进一步地，步骤S3中，若是由于目标遮挡原因导致中断的，则进行S301步骤计算短轨迹间的模糊相似度；若是由于目标停止导致中断的，则直接由公式(5)计算模糊相似度，无需进行预测。

进一步地，对于步骤S3中已经关联上的新旧轨迹对，采用三次Hermite插值法对成功关联的中断轨迹进行连接重构，基于目标已知状态值实现对其在间断时刻的状态值的估算，寻求最贴近真实目标轨迹的衔接轨迹，使得目标轨迹完整连续，完成目标轨迹的重构。

进一步地，所述步骤S1中，雷达检测数据的获取需要对特定识别范围内的雷达目标进行筛选，将无效目标剔除：

式中，p_x、p_y分别为目标位置属性分量；Y_dist为设定的横向范围；X_max为雷达可探测的最大纵向范围，即有效距离最大值；X_min为雷达可探测的最小纵向范围，即有效距离最小值。

进一步地，所述步骤S2中，相邻帧的关联具体包括三种情况：

情形1：当第k帧车辆数m小于k-1帧车辆数n时，对于第k-1帧没有配对的目标点迹，则认为该目标在k-1帧可能离开了检测区域或者发生了间断，将其存入短轨迹片段集合中；

情形2：在第k帧车辆数m等于k-1帧车辆数n时，此时两帧交通目标最可能都相互关联成功，但也有可能存在第k-1的目标消失，第k帧新增一个新目标的情况；

情形3：在第k帧车辆数m大于k-1帧车辆数n时，此时可能第k帧出现了新的目标，对于第k帧没有配对成功的目标点迹，将其作为新轨迹的起点，参与下一帧的配对。

进一步地，所述步骤S2中，毫米波雷达对目标的速度、位置测量较为精确，样本可以形成相邻帧的连续特征，因此可以选取速度和位置特征来进行关联匹配。

进一步地，所述步骤S3中，待关联轨迹片段的确定包括：

两条待关联的短轨迹不能在各自出现的时间上发生重叠；同一运动轨迹不能同时属于多个目标。时空约束条件是判断两个短轨迹能否进行关联的前提，根据时空约束条件得到冲突矩阵L＝[l(i,j)]_M×M。

式中，

表示轨迹TR_i最后一帧的时间，

表示轨迹TR_j起始帧的时间；

表示两个小轨迹存在时间重叠，l(TR_j,TR_i)＝0表示两个小轨迹不能进行关联，即两个小轨迹不属于同一个目标，l(TR_j,TR_i)＝1表示两个待关联小轨迹来源于同一目标。

本发明与现有技术相比，具有如下优点和有益效果：

一、本发明采用架设于检测道路上的毫米波雷达，充分利用其返回的点云数据，消除了由于部分交通目标反射面积较大而导致的分裂轨迹，大大降低了多余轨迹数量。实现了间断前后的交通目标ID标签的统一，从而获得雷达监测范围内交通目标的连续轨迹。很好的弥补了传统跟踪算法对实际场景中目标跟踪存在的不足，实现了对交通目标运动轨迹的连续跟踪。

二、本发明提出的交通目标轨迹连续跟踪方法不依赖于传统方法所需要的视频图像信息，仅依赖毫米波雷达获取的点云数据即可及时、准确、有效地获得交通目标连续运动轨迹，即使在光照强度较低、恶劣天气等影响下也可以有很好的表现，所需成本低，具有普遍适用性。

附图说明

图1是实施例毫米波雷达点云的车辆运动轨迹连续跟踪方法流程图；

图2是实施例初步关联阈值与轨迹片段数量关系示意图；

图3是实施例场景A跟踪结果图；

图4是实施例场景B跟踪结果图；

图5是实施例场景C跟踪结果图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明涉及一种毫米波雷达点云的车辆运动轨迹连续跟踪方法，该方法充分利用毫米波雷达返回的数据，通过对运动目标进行识别和检测，得到它们在每一帧中的位置、速度等状态信息，然后基于运动特征对目标轨迹进行关联，并对目标轨迹片段进行修正和连接，完成运动目标轨迹的重构，实现交通目标轨迹连续跟踪。

实施例1

如图1所示，一种车辆运动轨迹连续跟踪方法，包括以下步骤：

S1、利用毫米波雷达获取探测范围内交通目标的原始点云数据以获取雷达检测数据，每一个点代表检测到的交通目标；点云数据信息包括交通目标相对于雷达安装位置的横向距离、纵向距离，以及点的横向位置、纵向位置、横向速度、纵向速度和时间戳。

所述步骤S1中，雷达检测数据的获取需要对特定识别范围内的雷达目标进行筛选，将无效目标剔除。

式中，p_x、p_y分别为目标位置属性分量；Y_dist为设定的横向范围；X_max为雷达可探测的最大纵向范围，即有效距离最大值；X_min为雷达可探测的最小纵向范围，即有效距离最小值。

S2、基于雷达检测数据提取交通目标位置、速度特征，通过关联连续两帧的测量数据对交通目标轨迹进行生成和终止，将相邻帧目标连接成为短轨迹片段，并对关联结果进行修正，完成初始小轨迹片段的构建，具体为：

S201、在进行相邻帧的关联时，选取速度和位置信息进行相似度匹配计算，若两个相邻帧的检测响应d_i(k-1)与d_j(k)属于同一目标的轨迹序列(相邻帧数的点迹属于同一目的轨迹序列)，那么它们的连接概率应该足够高，即超过设定的阈值θ；超过设定的阈值就可以认为相邻帧的两个点可以关联了，就认定相邻两帧的点是属于同一个目标的；若不属于同一目标，将第k帧的检测点迹作为新轨迹的起点，继续与下一帧的检测响应进行配对，直至所有检测序列的检测响应关联完毕；

S202、对初步关联的结果进行进一步的修正，剔除同一目标在同一时刻生成的分裂轨迹：

假设有轨迹TR_A和TR_B，两条轨迹采样时间在第i帧到第j帧重叠，建立如式(1)判断准则，若判断准则成立，认为两条轨迹源于同一个目标，则将较短的轨迹视为分裂轨迹剔除，另一轨迹则对应原目标(指产生分裂轨迹的目标，这个步骤的目的是把分裂轨迹剔除掉)，以此类推，进行多次剔除，直到轨迹采样时间内没有满足以下条件的分裂轨迹：

式中，

分别表示轨迹TR_A、TR_B中第k帧与雷达的距离；

表示轨迹TR_A、TR_B中第k帧时的速度；ξ_R是距离差门限，ξ_v是速度差门限。

相邻帧的关联具体包括三种情况：

情形1：当第k帧车辆数m小于k-1帧车辆数n时，对于第k-1帧没有配对的目标点迹，则认为该目标在k-1帧可能离开了检测区域或者发生了间断，将其存入短轨迹片段集合中；

情形2：在第k帧车辆数m等于k-1帧车辆数n时，此时两帧交通目标最可能都相互关联成功，但也有可能存在第k-1的目标消失，第k帧新增一个新目标的情况；

情形3：在第k帧车辆数m大于k-1帧车辆数n时，此时可能第k帧出现了新的目标，对于第k帧没有配对成功的目标点迹，将其作为新轨迹的起点，参与下一帧的配对。

毫米波雷达对目标的速度、位置测量较为精确，样本可以形成相邻帧的连续特征，因此可以选取速度和位置特征来进行关联匹配。

S3、基于时空特性和运动特性构建模糊相关函数，以描述基准轨迹与新轨迹的匹配关系，计算轨迹之间的相似度得到代价矩阵，将短轨迹段关联问题视为分配问题进行解决；具体包括：

S301、针对交通目标的运动信息，基于运动预测的模糊策略进行关联，假设两条可能关联的短轨迹设TR_i和TR_j，称时间在前的短轨迹为基准轨迹TR_i，该轨迹的起始时刻为

终结时刻为

时间在后的短轨迹为新轨迹TR_j，该轨迹的起始时刻为

终结时刻为

对基准轨迹和新轨迹分别进行前向和逆向预测至

和

的中点时刻k_c，为量化基准轨迹中点时刻k_c预测状态

和新轨迹k_c时刻预测状态

的匹配关系，使用正态型隶属度函数来计算两个轨迹片段的相关程度：

式中，u_m是第m个模糊因素；σ_m是第m个模糊因素的误差方差；τ_m是调整度；ε_T是衰减因子；

式中，u₁、u₂分别表示位置、速度的模糊因素；μ₁(u₁)、μ₂(u₂)分别表示位置、速度的相关性程度；τ₁、τ₂分别表示位置、速度的调整度；

分别表示位置、速度的误差方差；k_c表示

和

的中点时刻；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向位置和横向位置的估计值；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向速度和横向速度的估计值。

基准轨迹TR_i和新轨迹TR_j基于位置和速度的模糊相似度表示为：

f(TR_j|TR_i)＝a₁μ₁(u₁)+a₂μ₂(u₂) (5)

式中，a₁和a₂分别表示位置、速度模糊因素所对应的权重。

S302、基于公式(5)可得到关联代价矩阵，基于小轨迹集合T以及模糊关联函数f(·)，使用匈牙利算法在多个关联可能性中选择使得整体最优的关联匹配结果：

式中，T＝{TR₁,TR₂,...,TR_M}是初步关联生成短轨迹的集合；L＝{l₁,l₂,...,l_N}是冲突矩阵的集合；f(·)是用于关联小轨迹的模糊函数；M为短轨迹片段集合中的轨迹总数；

对于已经关联上的新旧轨迹对，采用三次Hermite插值法对成功关联的中断轨迹进行连接重构，基于目标已知状态值实现对其在间断时刻的状态值的估算，寻求最贴近真实目标轨迹的衔接轨迹，使得目标轨迹完整连续，完成目标轨迹的重构。

待关联轨迹片段的确定包括：

两条待关联的短轨迹不能在各自出现的时间上发生重叠；同一运动轨迹不能同时属于多个目标。时空约束条件是判断两个短轨迹能否进行关联的前提，根据时空约束条件得到冲突矩阵L＝[l)i,j)]_M×M。

式中，

表示轨迹TR_i最后一帧的时间，

表示轨迹TR_j起始帧的时间；

表示两个小轨迹存在时间重叠，l(TR_j,TR_i)＝0表示两个小轨迹不能进行关联，即两个小轨迹不属于同一个目标，l(TR_j,TR_i)＝1表示两个待关联小轨迹来源于同一目标。

短轨迹的关联还需要考虑交通目标轨迹中断的原因，若是由于目标遮挡原因导致中断的，则进行S301步骤计算短轨迹间的模糊相似度；若是由于目标停止导致中断的，则直接由公式(5)计算模糊相似度，无需进行预测。

S4、针对存在间断的已经关联上的新旧轨迹对，连接满足关联关系的中断轨迹，根据轨迹在已知时刻对应的状态值，估算出其在间断时刻对应的数据，填补轨迹片段间的空缺，完成目标轨迹的连续跟踪。

本实施例基于毫米波雷达点云的车辆运动轨迹连续跟踪方法的试验场景，具体包括以下步骤：

第一步、本实施例的试验场景为某城市交叉口道路，毫米波雷达安装在一个交叉口路侧的横杆中央，其纵向感知范围为200m，横向范围超过车道整体宽度，测试示意图如图1所示。试验根据车辆密集程度的不同设定了三组实验场景，场景A、B、C分别为交叉口道路上的不同时段的场景，场景A对来向和去向车辆均进行了跟踪，场景B和C仅对一个方向车辆进行了跟踪，每个场景中均包括数量和比例不同的轿车、公交车和电动车。

表1不同测试场景的描述

实际场景中，雷达照射方向的投影与车道方向往往存在夹角，雷达也可能偏离车道中心。假设交通目标P与雷达直角坐标系纵轴的夹角为β，车道坐标系与雷达坐标系的夹角为α，由于后期对雷达数据的处理都是在车道直角坐标系中进行的，对实际安装的雷达所测量的位置和速度信息(p,v)进行坐标转换后，得到(p′,v′)信息为：

式中，p_x,p_y分别为目标的观测值中位置分量；v_x,v_y分别为其对应的速度分量；车道坐标系与雷达坐标系的夹角α为4.5度；p′_x、p′_y分别为进行坐标转换后的目标位置分量；v′_x、v′_y分别为进行坐标转换后的速度分量。

第二步、基于雷达检测数据构建初始小轨迹片段。在进行相邻帧的关联时，选取速度和位置信息进行相似度匹配计算。若两个相邻帧的检测响应d_i(k-1)与d_j(k)属于同一目标轨迹序列，那么那么它们的连接概率应该足够高，即超过设定的阈值θ；若不满足上述条件，将其作为新轨迹的起点，继续与下一帧的检测响应进行配对，直至所有检测序列的检测响应关联完毕。

在进行轨迹关联的过程中，可能会由于一个目标被检测出多个点迹导致同一时刻形成分裂轨迹(比如可能会由于检测目标体型较大(如公交车、卡车)，检测器返回的量测点可能不止一个)，从而造成轨迹中断。因此有必要在轨迹初步关联完成后对关联结果进行修正，剔除同一目标在同一时刻生成的分裂轨迹。假设有轨迹TR_A和TR_B，两条轨迹采样时间存在重叠在第i帧到第j帧重叠，建立如下判断准则。若判断准则成立，则将较短的轨迹视为分裂轨迹剔除，另一轨迹则对应一个目标，以此类推，进行多次剔除，直到轨迹采样时间内没有满足条件的分裂轨迹。

式中，

分别表示轨迹TR_A、TR_B中第k帧与雷达的距离；

表示轨迹TR_A、TR_B中第k帧时的速度；ξ_R是距离差门限，ξ_v是速度差门限。

第三步、基于时空约束信息确定待关联轨迹片段。具体的，时间约束是两条小轨迹能够关联的前提条件，即两条待关联的小轨迹在各自存在的时间内不能有重叠的区域，同一运动轨迹不能同时属于多个目标，根据时间约束条件得到冲突矩阵L＝[l(i,j)]_M×M。

式中，

表示轨迹TR_i最后一帧的时间，

表示轨迹TR_j起始帧的时间；

表示两个小轨迹存在时间重叠，l(TR_j,TR_i)＝0表示两个小轨迹不能进行关联，即两个小轨迹不属于同一个目标，l(TR_j,TR_i)＝1表示两个待关联小轨迹可能来源于同一目标。

针对交通目标的运动信息，基于运动预测的模糊策略进行关联。假设两条可能关联的小轨迹设TR_i和TR_j，称时间在前的小轨迹为基准轨迹TR_j，该轨迹的起始时刻为

终结时刻为

时间在后的小轨迹为新轨迹，该轨迹的起始时刻为

终结时刻为

对基准轨迹和新轨迹分别进行前向和逆向预测至

和

的中点时刻k_c。为量化基准轨迹k_c时刻预测状态

和新轨迹k_c时刻预测状态

的匹配关系，使用正态型隶属度函数来计算两个轨迹片段的相关程度：

式中，u_m是第m个模糊因素；σ_m是第m个模糊因素的误差方差；τ_m是调整度；ε_T是衰减因子。

式中，u₁、u₂分别表示位置、速度的模糊因素；μ₁(u₁)、μ₂(u₂)分别表示位置、速度的相关性程度；τ₁、τ₂分别表示位置、速度的调整度；

分别表示位置、速度的误差方差；k_c表示

和

的中点时刻；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向位置和横向位置的估计值；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向速度和横向速度的估计值。

基准轨迹TR_i和新轨迹TR_j基于位置和速度的模糊相似度表示为：

f(TR_j|TR_i)＝a₁μ₁(u₁)+a₂μ₂(u₂) (7)

式中，a₁和a₂分别表示位置、速度模糊因素所对应的权重。

得到关联代价矩阵后，轨迹段关联问题可视为分配问题进行解决。即基于小轨迹集合T以及模糊关联函数f(·)，使用匈牙利算法在多个关联可能性中选择使得整体最优的关联匹配结果。可以用公式(8)来进行描述。

式中，T＝{TR₁,TR₂,...,TR_M}是初步关联生成小轨迹的集合，L＝{l₁,l₂,...,l_N}是冲突矩阵的集合，f(·)是用于关联小轨迹的模糊函数；M为短轨迹片段集合中的轨迹总数；N是冲突矩阵的总数。第四步、对于已经关联上的新旧轨迹对，采用三次Hermite插值法对成功关联的中断轨迹进行连接重构，基于目标已知状态值实现对其在间断时刻的状态值的估算，寻求最贴近真实目标轨迹的衔接轨迹，使得目标轨迹完整连续，完成目标轨迹的重构。

三个场景得到的轨迹跟踪结果如图3、图4、图5所示，图3的a为横向跟踪结果图，图3的b为纵向跟踪结果图，图4的a为横向跟踪结果图，图4的b为纵向跟踪结果图，图5的a为横向跟踪结果图，图5的b为纵向跟踪结果图，其中各图的a图a表示在y轴方向的跟踪轨迹，b图表示在x方向上的跟踪轨迹。图中的每一条线代表车辆的一条轨迹，三个图中的轨迹均可与目标车辆一一对应，跟踪结果与实际情况相符。本发明提出的基于小轨迹关联的目标跟踪算法有效地去除了同一个目标反射的多个测量值，大大降低了多余轨迹数量，能够对来自同一目标的多条轨迹进行正确的识别并删除，实现了目标标签在轨迹中断前后的统一，交通目标轨迹连续跟踪结果的准确率可达到90％以上，场景中目标估计个数的准确率显著提高。

本发明成果可以实现车辆的连续跟踪，获得精确可靠的高频车辆运动轨迹，丰富了交通环境下车辆轨迹数据集，为后续展开交通参数提取、交通流特性分析、交通事件检测、交通状态估计等提供了数据支撑。

实施例2

一种车辆运动轨迹连续跟踪方法，其特征在于，包括以下步骤：

第一步、原始点云数据的预处理。利用架设于检测道路上方的毫米波雷达获取探测范围内交通目标的原始点云数据。将采集到的数据根据规定的协议进行解析获取交通目标距离、速度运动参数信息。

第二步、基于雷达检测数据提取交通目标位置、速度等特征，通过关联连续两帧的测量数据对交通目标轨迹进行简单地生成和终止，将相邻帧目标连接成为可靠的小轨迹片段。

第三步、将相邻帧的短轨迹关联可以视为求解一对一映射关系的最优线性分配问题。

具体的，令(i,j)为目标关联假设对，其中i表示在第k-1帧检测到的第i个目标，j表示在第k帧中的第j个目标，基于关联事件在两个相邻帧中穷举现有目标和检测目标之间的关系来建立n×m相似矩阵。在进行目标轨迹相邻帧的关联时，对于确定的目标，在短时间内不同帧中速度、位置的变化不显著，此外，毫米波雷达对目标的速度、位置测量较为精确，样本可以形成相邻帧的连续特征。因此可以选取速度和位置特征来进行关联匹配，定义如式(8)所示的相似度函数来计算相邻帧点和点之间的相似度：

S{d_i(k-1)|d_j(k)}＝S_pos(d_i|d_j)·S_v(d_i|d_j) (8)

式中，S_pos(d_j|d_i)反映了第k-1帧第i个目标位置坐标和k帧第j个目标位置坐标相似程度；S_v(d_i|d_j)反映了第k-1帧第i个目标速度和k帧第j个目标速度相似程度；

S{d_i(k-1)|d_j(k)}≥θ (11)

式中，s_x和s_y分别是当前所有轨迹片段在x和y分量上的位置方差；s_vx和s_vy分别是当前所有轨迹片段在x和y分量上的速度方差；θ是初步关联的阈值。

在进行相邻帧初步关联构建短轨迹过程时，初步关联阈值θ的大小可以决定生成轨迹段的长度和数量。当阈值过大时，只要下一帧的数据稍稍偏离上一帧数据就会认为是不可靠的短轨迹，从而会产生较多新生轨迹片段；当阈值过小时，相邻帧关联的条件变得宽松，导致区分度较低，一些被终止的短轨迹能得到继续生长的机会，可能会带来错误关联情况。图2表示三个场景关联阈值的取值对生成轨迹段数量的影响，可以看出：曲线均在θ＝0.9处出现明显拐点；当θ＜0.9时，生成的轨迹片段数量变化波动不大；当θ＞0.9时，生成轨迹段数量急剧增加。因此本文将初步关联阈值设定为0.9，初步关联结果见表2。

本发明实施例的技术方案，通过利用架设于检测道路上方的毫米波雷达获取探测范围内交通目标的点云数据，根据雷达检测数据确定短轨迹初步关联时的阈值，对后续的生成轨迹段的长度和数量起到了关键性作用，提高了车道轨迹连续跟踪的准确度。

实施例3为了检验本发明公开的方法对不同时间段的适用性，在实施例1的基础上将由白天场景换成夜间场景进行实验：

S1、交通目标运动信息的感知。利用架设于检测道路上方的毫米波雷达获取探测范围内交通目标的原始点云数据，每一个点代表检测到的交通目标，其数据字段至少包括雷达安装位置的横向距离、纵向距离，以及点的横向位置、纵向位置、横向速度、纵向速度和时间戳；

S2、小轨迹片段的构建。基于雷达检测数据提取交通目标位置、速度等特征，通过关联连续两帧的测量数据对交通目标轨迹进行简单地生成和终止，将相邻帧目标连接成为可靠的小轨迹片段，并对关联结果进行修正，完成初始小轨迹片段的构建；

S3、小轨迹片段的关联。基于时空特性和运动特性构建模糊相关函数来描述基准轨迹与新轨迹的匹配关系，计算轨迹之间的相似度得到代价矩阵，将小轨迹段关联问题视为分配问题进行解决；

S4、中断目标轨迹重构。针对存在间断的已经关联上的新旧轨迹对，使用三次Hermite插值法连接满足关联关系的中断轨迹，根据轨迹在已知时刻对应的状态值，估算出其在间断时刻对应的数据，填补轨迹片段间的空缺，完成目标轨迹的重构。

实验主要从正确跟踪准确率和轨迹间断率2个方面来对提出的算法进行可行性验证，对轨迹提取算法精度进行分析。实验结果表明：本发明成果在夜间场景、光照强度较低、恶劣天气等影响下也可以有很好的表现，所需成本低，具有普遍适用性。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种车辆运动轨迹连续跟踪方法，其特征在于，包括以下步骤：

S1、利用毫米波雷达获取探测范围内交通目标的原始点云数据以获取雷达检测数据，每一个点代表检测到的交通目标；

S2、基于雷达检测数据提取交通目标位置、速度特征，通过关联连续两帧的测量数据对交通目标轨迹进行生成和终止，将相邻帧目标连接成为短轨迹片段，并对关联结果进行修正，完成初始小轨迹片段的构建；

S3、基于时空特性和运动特性构建模糊相关函数，以描述基准轨迹与新轨迹的匹配关系，计算轨迹之间的相似度得到代价矩阵，将短轨迹段关联问题视为分配问题进行解决；

S4、针对存在间断的已经关联上的新旧轨迹对，连接满足关联关系的中断轨迹，根据轨迹在已知时刻对应的状态值，估算出其在间断时刻对应的数据，填补轨迹片段间的空缺，完成目标轨迹的连续跟踪。

2.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S1毫米波雷达检测得到的点云数据信息包括交通目标相对于雷达安装位置的横向距离、纵向距离，以及点的横向位置、纵向位置、横向速度、纵向速度和时间戳。

3.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，步骤S2的短轨迹片段构建，包括：

S201、在进行相邻帧的关联时，选取速度和位置信息进行相似度匹配计算，若超过设定的阈值θ，则两个相邻帧的检测响应d_i(k-1)与d_j(k)属于同一目标的轨迹序列，若不属于同一目标，将第k帧的检测点迹作为新轨迹的起点，继续与下一帧的检测响应进行配对，直至所有检测序列的检测响应关联完毕；

S202、对初步关联的结果进行进一步的修正，剔除同一目标在同一时刻生成的分裂轨迹：

假设有轨迹TR_A和TR_B，两条轨迹采样时间在第i帧到第j帧重叠，建立如式(1)判断准则，若判断准则成立，认为两条轨迹源于同一个目标，则将较短的轨迹视为分裂轨迹剔除，另一轨迹则对应原目标，以此类推，进行多次剔除，直到轨迹采样时间内没有满足以下条件的分裂轨迹：

式中，

分别表示轨迹TR_A、TR_B中第k帧与雷达的距离；

表示轨迹TR_A、TR_B中第k帧时的速度；ξ_R是距离差门限，ξ_v是速度差门限。

4.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，步骤S3具体包括：

S301、针对交通目标的运动信息，基于运动预测的模糊策略进行关联，假设两条可能关联的短轨迹设TR_i和TR_j，称时间在前的短轨迹为基准轨迹TR_i，该轨迹的起始时刻为

终结时刻为

时间在后的短轨迹为新轨迹TR_j，该轨迹的起始时刻为

终结时刻为

对基准轨迹和新轨迹分别进行前向和逆向预测至

和

的中点时刻k_c，为量化基准轨迹中点时刻k_c预测状态

和新轨迹k_c时刻预测状态

的匹配关系，使用正态型隶属度函数来计算两个轨迹片段的相关程度：

式中，u_m是第m个模糊因素；σ_m是第m个模糊因素的误差方差；τ_m是调整度；ε_T是衰减因子；

式中，u₁、u₂分别表示位置、速度的模糊因素；μ₁(u₁)、μ₂(u₂)分别表示位置、速度的相关

性程度；τ₁、τ₂分别表示位置、速度的调整度；

分别表示位置、速度的误差方差；k_c表示

和

的中点时刻；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向位置和横向位置的估计值；

分别表示基准轨迹和新轨迹预测到k_c时刻的纵向速度和横向速度的估计值；

基准轨迹TR_i和新轨迹TR_j基于位置和速度的模糊相似度表示为：

f(TR_j|TR_i)＝a₁μ₁(u₁)+a₂μ₂(u₂) (5)

式中，a₁和a₂分别表示位置、速度模糊因素所对应的权重；

S302、基于公式(5)得到关联代价矩阵，基于小轨迹集合T以及模糊关联函数f(·)，使用匈牙利算法在多个关联可能性中选择使得整体最优的关联匹配结果：

式中，T＝{TR₁,TR₂,...,TR_M}是初步关联生成短轨迹的集合；L＝{l₁,l₂,...,l_N}是冲突矩阵的集合；f(·)是用于关联小轨迹的模糊函数；M为短轨迹片段集合中的轨迹总数；N是冲突矩阵的总数；TR_M代表轨迹集合中第M个短轨迹；l_N代表第N个短轨迹的关联结果。

5.根据权利要求4所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，步骤S3中，若是由于目标遮挡原因导致中断的，则进行S301步骤计算短轨迹间的模糊相似度；若是由于目标停止导致中断的，则直接由公式(5)计算模糊相似度，无需进行预测。

6.根据权利要求4所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，对于步骤S3中已经关联上的新旧轨迹对，采用三次Hermite插值法对成功关联的中断轨迹进行连接重构，基于目标已知状态值实现对其在间断时刻的状态值的估算，寻求最贴近真实目标轨迹的衔接轨迹，使得目标轨迹完整连续，完成目标轨迹的重构。

7.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S1中，雷达检测数据的获取需要对特定识别范围内的雷达目标进行筛选，将无效目标剔除：

式中，p_x、p_y分别为目标位置属性分量；Y_dist为设定的横向范围；X_max为雷达可探测的最大纵向范围，即有效距离最大值；X_min为雷达可探测的最小纵向范围，即有效距离最小值。

8.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S2中，相邻帧的关联具体包括三种情况：

情形1：当第k帧车辆数m小于k-1帧车辆数n时，对于第k-1帧没有配对的目标点迹，则认为该目标在k-1帧可能离开了检测区域或者发生了间断，将其存入短轨迹片段集合中；

情形2：在第k帧车辆数m等于k-1帧车辆数n时，此时两帧交通目标最可能都相互关联成功，但也有可能存在第k-1的目标消失，第k帧新增一个新目标的情况；

情形3：在第k帧车辆数m大于k-1帧车辆数n时，此时可能第k帧出现了新的目标，对于第k帧没有配对成功的目标点迹，将其作为新轨迹的起点，参与下一帧的配对。

9.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S2中，毫米波雷达对目标的速度、位置测量较为精确，样本可以形成相邻帧的连续特征，因此可以选取速度和位置特征来进行关联匹配。

10.根据权利要求1所述的基于毫米波雷达点云的车辆运动轨迹连续跟踪方法，其特征在于，所述步骤S3中，待关联轨迹片段的确定包括：

两条待关联的短轨迹不能在各自出现的时间上发生重叠；同一运动轨迹不能同时属于多个目标。时空约束条件是判断两个短轨迹能否进行关联的前提，根据时空约束条件得到冲突矩阵L＝[l(i,j)]_M×M。

式中，

表示轨迹TR_i最后一帧的时间，

表示轨迹TR_j起始帧的时间；

表示两个小轨迹存在时间重叠，l(TR_j,TR_i)＝0表示两个小轨迹不能进行关联，即两个小轨迹不属于同一个目标，l(TR_j,TR_i)＝1表示两个待关联小轨迹来源于同一目标。

Images