CN117687029B - 一种基于毫米波雷达的车辆运动轨迹跟踪方法及系统 - Google Patents

Abstract

本发明涉及车辆轨迹预测领域，更具体的说，它涉及一种基于毫米波雷达的车辆运动轨迹跟踪方法及系统。一种基于毫米波雷达的车辆运动轨迹跟踪系统，包括：三维点云数据获取模块、目标车辆信息集建立模块、目标车辆关联对构建模块、待构建轨迹目标车辆集构建模块、目标车辆轨迹构建模块和目标车辆轨迹重构模块。本发明通过车辆检测模型对道路上的车辆进行三维点云数据获取，并且基于三维点云数据检测出目标车辆，再基于所有目标车辆在相邻帧之间的关系建立目标车辆关联对，之后基于目标车辆关联对建立目标车辆的轨迹信息，实现对车辆运行轨迹的跟踪。

Description

一种基于毫米波雷达的车辆运动轨迹跟踪方法及系统

技术领域

本发明涉及车辆轨迹预测领域，更具体的说，它涉及一种基于毫米波雷达的车辆运动轨迹跟踪方法及系统。

背景技术

轨迹数据的采集对于实现道路交通的实时监测和精细化管理，以及提升道路交通的智能化水平，具有重要的现实意义。轨迹数据描述了道路参与者在移动过程中的连续变化和规律特性，反映了车辆在时空上的运行变化规律。这些数据为多个领域提供了重要的数据支撑，包括路口信号配时优化与交通组织改善、交通异常事件感知与预警、交通状态精准辨别，以及道路参与者行为机制分析。通过采集轨迹数据，我们能够获得道路参与者在实际交通环境中的行为信息，从而更好地理解道路交通的运行状况和问题。这种数据采集方法可以为交通管理部门提供准确的交通状态信息，帮助他们做出决策和制定策略，以优化路口信号配时、改善交通组织，并及时感知和预警交通异常事件。此外，通过对轨迹数据进行分析，我们还能够研究道路参与者的行为机制，从而进一步改善交通规划和设计，提高道路交通的效率和安全性。因此，轨迹数据的采集是推动道路交通智能化发展的重要一环。

发明内容

本发明通过车辆检测模型对道路上的车辆进行三维点云数据获取，并且基于三维点云数据检测出目标车辆，再基于所有目标车辆在相邻帧之间的关系建立目标车辆关联对，之后基于目标车辆关联对建立目标车辆的轨迹信息，实现对车辆运行轨迹的跟踪。

一种基于毫米波雷达的车辆运动轨迹跟踪方法，包括：

通过毫米波雷达获取当前帧的三维点云数据，每在采样时间点获取三维点云数据时视为获取了一帧三维点云数据；

将当前帧的三维点云数据送入训练好的车辆检测模型，输出当前帧所有目标车辆的目标车辆三维坐标，基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集F_t，目标信息集F_t中存储的数据为{f₁（t），f₂（t），f₃（t），…，f_n（t），…，f_N（t）}，其中f_n（t）为当前帧中第n个目标车辆对应的目标车辆信息，且f_n（t）={x_n（t），y_n（t），z_n（t），v_n（t）}，目标车辆信息f_n（t）中，x_n（t）为当前帧中第n个目标车辆对应的X轴坐标值，y_n（t）为当前帧中第n个目标车辆对应的Y轴坐标值，z_n（t）为当前帧中第n个目标车辆对应的Z轴坐标值，v_n（t）为当前帧中第n个目标车辆对应的瞬时速度，t为当前帧目标车辆信息集F_t对应的帧数编号，初始为1，每进行一次三维点云数据获取操作，对t值执行增一操作，n为目标车辆信息集F_t中目标车辆对应的车辆编号，且n=1，2，3，……，N，N为当前帧中所有目标车辆的总数量；

基于当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1构建目标车辆关联对G_n（t）；

基于当前帧对应的所有目标车辆关联对G_n（t）并与上一帧对应的目标信息集F_t-1中的目标车辆信息f_n（t-1）输出待构建轨迹目标车辆集S（t-1）；

基于待构建轨迹目标车辆集S（t-1）和t-1帧和t-1帧之前的所有目标车辆关联对输出目标车辆轨迹集A_r，并根据目标车辆轨迹集A_r构建目标车辆轨迹L_r；

根据所有目标车辆轨迹集A_r和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹L_r进行重构。

优选地，基于当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1构建目标车辆关联对G_n（t），具体包括如下步骤：

逐个选择当前帧对应的目标信息集F_t中的目标车辆信息f_n（t），针对选择的目标车辆信息f_n（t），基于目标车辆信息f_n（t）中的X轴坐标值x_n（t）和Y轴坐标值y_n（t）确定目标车辆所处的道路区域μ_i，i=1，2，3，……，I，I为所有道路区域的总个数，再通过道路区域μ_i对选择的目标车辆信息f_n（t）进行标记；

获取当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1，逐个选择目标信息集F_t中的目标车辆信息f_n（t），针对每个选择的目标车辆信息f_n（t），输出目标车辆信息f_n（t）对应的道路区域μ_i，从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_n（t-1）中逐个选择目标车辆信息f_j（t-1），j∈{1，2，3，……，N}，计算车辆欧式距离dis_n（t-1，t）和车辆位移向量δ_n（t-1，t），其中车辆欧式距离dis_n（t-1，t）为选择的目标车辆信息f_n（t）与从目标信息集F_t-1中选择的目标车辆信息f_j（t-1）之间的欧式距离，车辆位移向量δ_n（t-1，t）=[x_n（t）-x_j（t-1），y_n（t）-y_j（t-1），z_n（t）-z_j（t-1），v_n（t）-v_j（t-1）]，判断是否满足关联条件，关联条件为“dis_n（t-1，t）＜ε&&δ_n（t-1，t）·Vec_i≥0”，其中ε为欧式距离阈值，Vec_i为目标车辆信息f_n（t）对应的道路区域μ_i对应的位移单位向量，由操作人员提前设定，表征道路区域μ_i上车辆的行驶方向，若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_n（t-1）满足关联条件，则基于选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）建立目标车辆关联对G_n（t），G_n（t）=（f_n（t），f_j（t-1）），将f_n（t）记为目标车辆关联对G_n（t）的前项，f_n（t-1）记为目标车辆关联对G_n（t）的后项；若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）不满足关联条件，则从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）中选择下一个目标车辆信息f_j（t-1）；直至目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）都被选择完毕，则从目标信息集F_t中选择下一个目标车辆信息f_n（t）；直至目标信息集F_t中的所有目标车辆信息f_n（t）都被选择完毕，输出所有目标车辆关联对G_n（t）。

优选地，基于当前帧对应的所有目标车辆关联对G_n（t）并与上一帧对应的目标信息集F_t-1中的目标车辆信息f_n（t-1）输出待构建轨迹目标车辆集S（t-1），具体包括如下步骤：

遍历当前帧对应的所有目标车辆关联对G_n（t）并与目标信息集F_t-1中的目标车辆信息f_n（t-1）进行匹配，输出目标信息集F_t-1中未匹配成功的目标车辆信息f_n（t-1），并将目标信息集F_t-1中未匹配成功的所有目标车辆信息f_n（t-1）组成待构建轨迹目标车辆集S（t-1）。

优选地，基于待构建轨迹目标车辆集S（t-1）和t-1帧和t-1帧之前的所有目标车辆关联对输出目标车辆轨迹集A_r，并根据目标车辆轨迹集A_r构建目标车辆轨迹L_r，具体包括如下步骤：

遍历待构建轨迹目标车辆集S（t-1），再将t-1帧和t-1帧之前的所有目标车辆关联对组成目标车辆关联对集合，逐个选择待构建轨迹目标车辆集S（t-1）中的目标车辆信息f_n（t-1），针对选择的目标车辆信息f_n（t-1），从目标车辆关联对集合中选择满足前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1），将选择的目标车辆信息f_n（t-1）与前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项存入目标车辆轨迹集A_r，r初始为1，再将前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项作为下一个选择的目标车辆关联对G_n（t-2）的前项，进行目标车辆关联对G_n（t-2）的查找，以此类推，直至无法选择到目标车辆关联对G_n（t-k）时，k∈{1，2，3，…，t-1}，输出目标车辆轨迹集A_r，再基于目标车辆轨迹集A_r中的所有目标车辆信息f_n（t-k）构建目标车辆轨迹L_r，构建目标车辆轨迹L_r的方式可以为以目标车辆轨迹集A_r中所有目标车辆信息f_n（t-k）对应的X轴坐标值、Y轴坐标值和Z轴坐标值建立坐标点，再连接所有坐标点，最后平滑处理得到对应的目标车辆轨迹L_r，且每构建一个目标车辆轨迹L_r，对r值进行增一操作，并从待构建轨迹目标车辆集S（t-1）中选择下一个目标车辆信息f_n（t-1）；直至待构建轨迹目标车辆集S（t-1）中所有的目标车辆信息f_n（t-1）都被选择完毕，输出所有车辆轨迹集A_r和对应的目标车辆轨迹L_r。

优选地，根据所有目标车辆轨迹集A_r和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹L_r进行重构，具体包括如下步骤：

将车辆轨迹集A_r中所有目标车辆信息对应的起始帧记为ST_r，将车辆轨迹集A_p中所有目标车辆信息对应的终帧记为END_r，遍历所有车辆轨迹集A_r，针对选择的车辆轨迹集A_r，将选择的车辆轨迹集合记为A_p，针对每个选择的车辆轨迹集合记为A_p执行如下操作：

将选择的车辆轨迹集A_p分别送入训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型，分别输出前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben，且前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben中分别存储着目标车辆信息和目标车辆预测信息，前向车辆预测轨迹序列A_for中的目标车辆信息为END_p之前帧数对应预测的目标车辆信息，后向车辆预测轨迹序列A_ben中的目标车辆信息为ST_p之后帧数对应预测的目标车辆信息；

将所有车辆轨迹集A_r中终帧END_r在ST_p之前的所有车辆轨迹集A_r组成前向待连接车辆轨迹集合，从前向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号记为{h，h+1，h+2，…，m，...，h+a}，其中a为选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_for），，其中U_r（m）为选择的车辆轨迹集A_r中帧数编号为m对应的目标车辆信息，U_for（m）为前向车辆预测轨迹序列A_for中帧数编号为m对应的目标车辆信息，dis（U_r（m），U_for（m））为目标车辆信息U_r（m）与目标车辆信息U_for（m）之间的欧式距离，判断“ζ（A_r，A_for）＜ε”是否成立，若是“ζ（A_r，A_for）＜ε”成立，将车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r通过前向车辆预测轨迹序列A_for进行拼接，即在车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r添加前向车辆预测轨迹序列A_for中不与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从前向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是“ζ（A_r，A_for）＜ε”不成立，则从前向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至前向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕；

将所有车辆轨迹集A_r中起始帧ST_r在END_p之前的所有车辆轨迹集A_r组成后向待连接车辆轨迹集合，从后向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号记为{e，e+1，e+2，…，c，...，e+b}，其中b为选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_bed），，其中U_r（c）为选择的车辆轨迹集A_r中帧数编号为c对应的目标车辆信息，U_bed（c）为后向车辆预测轨迹序列A_bed中帧数编号为c对应的目标车辆信息，dis（U_r（m），U_bed（m））为目标车辆信息U_r（m）与目标车辆信息U_bed（m）之间的欧式距离，判断“ζ（A_r，A_bed）＜ε”是否成立，若是“ζ（A_r，A_bed）＜ε”成立，将车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r通过后向车辆预测轨迹序列A_bed进行拼接，即在车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r添加后向车辆预测轨迹序列A_bed中不与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从后向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是“ζ（A_r，A_bed）＜ε”不成立，则从后向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至后向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕。

优选地，车辆检测模型基于Voxelnet模型建立。

优选地，前向车辆轨迹预测模型和后向车辆轨迹预测模型均基于LSTM模型建立。

一种基于毫米波雷达的车辆运动轨迹跟踪系统，包括：

三维点云数据获取模块，用于通过毫米波雷达获取当前帧的三维点云数据；

目标车辆信息集建立模块，用于将当前帧的三维点云数据送入训练好的车辆检测模型，输出当前帧所有目标车辆的目标车辆三维坐标，并基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集；

目标车辆关联对构建模块，用于基于当前帧对应的目标信息集和上一帧对应的目标信息集构建目标车辆关联对；

待构建轨迹目标车辆集构建模块，用于基于当前帧对应的所有目标车辆关联对并与上一帧对应的目标信息集中的目标车辆信息构建待构建轨迹目标车辆集；

目标车辆轨迹构建模块，用于基于待构建轨迹目标车辆集和所有目标车辆关联对输出目标车辆轨迹集，并根据目标车辆轨迹集构建目标车辆轨迹；

目标车辆轨迹重构模块，用于根据所有目标车辆轨迹集和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹进行重构。

本发明具有以下优点：

1、本发明通过车辆检测模型对道路上的车辆进行三维点云数据获取，并且基于三维点云数据检测出目标车辆，再基于所有目标车辆在相邻帧之间的关系建立目标车辆关联对，之后基于目标车辆关联对建立目标车辆的轨迹信息，实现对车辆运行轨迹的跟踪。

2、本发明通过对目标车辆的所有轨迹进行预测，基于预测的车辆轨迹来判断不同车辆轨迹是否对应同一目标车辆，并选择可能为同一目标车辆的车辆轨迹进行连接，同时通过预测的目标车辆信息对缺失的轨迹进行补足，保证目标车辆轨迹的连续性。

附图说明

图1为本发明实施例采用的基于毫米波雷达的车辆运动轨迹跟踪系统的结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

实施例1

一种基于毫米波雷达的车辆运动轨迹跟踪方法，包括：

通过毫米波雷达获取当前帧的三维点云数据，毫米波雷达一般设置在道路上方，呈适应角度，能够方便毫米波雷达进行检测，且此处所说的道路是泛指，可以是国道、高速路和城市路等，在提前设置好的采样时间点对道路上车辆进行毫米波雷达探测，并生成对应的RAD（Range-Angle-Distance）雷达数据，再根据毫米波雷达位置对RAD雷达数据进行解算，得到对应的三维点云数据，需要说明的是，在解算期间，还包括数据清洗这一步骤，例如去除不在道路范围内的虚假目标等，每在采样时间点获取三维点云数据时视为获取了一帧三维点云数据；

将当前帧的三维点云数据送入训练好的车辆检测模型，车辆检测模型基于Voxelnet模型建立，输出当前帧所有目标车辆的目标车辆三维坐标，目标车辆三维坐标所在的坐标系一般选择世界坐标系，也可以以毫米波雷达为原点自行定义一个坐标系，基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集F_t，目标信息集F_t中存储的数据为{f₁（t），f₂（t），f₃（t），…，f_n（t），…，f_N（t）}，其中f_n（t）为当前帧中第n个目标车辆对应的目标车辆信息，且f_n（t）={x_n（t），y_n（t），z_n（t），v_n（t）}，目标车辆信息f_n（t）中，x_n（t）为当前帧中第n个目标车辆对应的X轴坐标值，y_n（t）为当前帧中第n个目标车辆对应的Y轴坐标值，z_n（t）为当前帧中第n个目标车辆对应的Z轴坐标值，v_n（t）为当前帧中第n个目标车辆对应的瞬时速度，t为当前帧目标车辆信息集F_t对应的帧数编号，初始为1，每进行一次三维点云数据获取操作，对t值执行增一操作，n为目标车辆信息集F_t中目标车辆对应的车辆编号，且n=1，2，3，……，N，N为当前帧中所有目标车辆的总数量，可以预期的是，不同帧内的目标车辆的总数量不一定相同，可能存在目标车辆在当前帧驶入毫米波雷达的探测范围，或者在当前帧驶出毫米波雷达的探测范围；

逐个选择当前帧对应的目标信息集F_t中的目标车辆信息f_n（t），针对选择的目标车辆信息f_n（t），基于目标车辆信息f_n（t）中的X轴坐标值x_n（t）和Y轴坐标值y_n（t）确定目标车辆所处的道路区域μ_i，一种实现方式是在以X轴坐标值x_n（t）和Y轴坐标值y_n（t）对应的XOY二维平面上，判断目标车辆信息f_n（t）中的X轴坐标值x_n（t）和Y轴坐标值y_n（t）对应的点落在哪个道路区域μ_i内，即将目标车辆信息f_n（t）中的X轴坐标值x_n（t）和Y轴坐标值y_n（t）与道路区域μ_i的边界进行大小判断，i=1，2，3，……，I，I为所有道路区域的总个数，再通过道路区域μ_i对选择的目标车辆信息f_n（t）进行标记，需要说明的是，道路区域的划分方式可以由操作人员进行设定，例如道路上是双向车道，则可以直接划分为两个道路区域，若是道路多弯，则可以基于道路弯道变化率进一步划分，在此不过多说明；

获取当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1，逐个选择目标信息集F_t中的目标车辆信息f_n（t），针对每个选择的目标车辆信息f_n（t），输出目标车辆信息f_n（t）对应的道路区域μ_i，从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_n（t-1）中逐个选择目标车辆信息f_j（t-1），j∈{1，2，3，……，N}，这里需要补充，由于是从目标信息集F_t-1中逐个选择目标车辆信息f_j（t-1），所以选择的目标车辆信息f_n（t）中n值与目标车辆信息f_j（t-1）中的j不一定相同，例如从目标信息集F_t中选择了目标车辆信息f₃（t），从目标信息集F_t-1中可以选择目标车辆信息f₂（t-1）、目标车辆信息f₄（t-1）和目标车辆信息f₅（t-1）等，计算车辆欧式距离dis_n（t-1，t）和车辆位移向量δ_n（t-1，t），其中车辆欧式距离dis_n（t-1，t）为选择的目标车辆信息f_n（t）与从目标信息集F_t-1中选择的目标车辆信息f_j（t-1）之间的欧式距离，车辆位移向量δ_n（t-1，t）=[x_n（t）-x_j（t-1），y_n（t）-y_j（t-1），z_n（t）-z_j（t-1），v_n（t）-v_j（t-1）]，判断是否满足关联条件，关联条件为“dis_n（t-1，t）＜ε&&δ_n（t-1，t）·Vec_i≥0”，其中ε为欧式距离阈值，由于在实际车辆运行的过程中，相邻帧之间的时间较短，也就意味着在相邻帧之间目标车辆的姿态变化不会太大，操作人员根据经验或者其他模拟优化的手段确定欧式距离阈值ε，Vec_i为目标车辆信息f_n（t）对应的道路区域μ_i对应的位移单位向量，由操作人员提前设定，表征道路区域μ_i上车辆的行驶方向，若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_n（t-1）满足关联条件，说明选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）之间姿态接近，并且符合行驶方向，则基于选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）建立目标车辆关联对G_n（t），G_n（t）=（f_n（t），f_j（t-1）），将f_n（t）记为目标车辆关联对G_n（t）的前项，f_n（t-1）记为目标车辆关联对G_n（t）的后项；若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）不满足关联条件，则从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）中选择下一个目标车辆信息f_j（t-1）；直至目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）都被选择完毕，则从目标信息集F_t中选择下一个目标车辆信息f_n（t）；直至目标信息集F_t中的所有目标车辆信息f_n（t）都被选择完毕，输出所有目标车辆关联对G_n（t）；

遍历当前帧对应的所有目标车辆关联对G_n（t）并与目标信息集F_t-1中的目标车辆信息f_n（t-1）进行匹配，即将所有目标车辆关联对中的后者组成一个集合，再将这个集合与目标信息集F_t-进行并集计算，并集计算输出的即为匹配成功，除开并集计算输出的便是匹配失败，输出目标信息集F_t-1中未匹配成功的目标车辆信息f_n（t-1），并将目标信息集F_t-1中未匹配成功的所有目标车辆信息f_n（t-1）组成待构建轨迹目标车辆集S（t-1）；

遍历待构建轨迹目标车辆集S（t-1），再将t-1帧和t-1帧之前的所有目标车辆关联对组成目标车辆关联对集合，逐个选择待构建轨迹目标车辆集S（t-1）中的目标车辆信息f_n（t-1），针对选择的目标车辆信息f_n（t-1），从目标车辆关联对集合中选择满足前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1），将选择的目标车辆信息f_n（t-1）与前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项存入目标车辆轨迹集A_r，r初始为1，再将前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项作为下一个选择的目标车辆关联对G_n（t-2）的前项，进行目标车辆关联对G_n（t-2）的查找，例如选择的目标车辆信息为f₃（9），匹配的目标车辆关联对为（f₃（9），f₇（8）），则选择f₇（8）继续匹配目标车辆关联对，若是匹配到的目标车辆关联对为（f₇（8），f₁₀（7）），则目前的目标车辆轨迹集A_r便会存储{f₃（9），f₇（8），f₁₀（7）}，以此类推，直至无法选择到目标车辆关联对G_n（t-k）时，k∈{1，2，3，…，t-1}，输出目标车辆轨迹集A_r，再基于目标车辆轨迹集A_r中的所有目标车辆信息f_n（t-k）构建目标车辆轨迹L_r，构建目标车辆轨迹L_r的方式可以为以目标车辆轨迹集A_r中所有目标车辆信息f_n（t-k）对应的X轴坐标值、Y轴坐标值和Z轴坐标值建立坐标点，再连接所有坐标点，最后平滑处理得到对应的目标车辆轨迹L_r，且每构建一个目标车辆轨迹L_r，对r值进行增一操作，并从待构建轨迹目标车辆集S（t-1）中选择下一个目标车辆信息f_n（t-1）；直至待构建轨迹目标车辆集S（t-1）中所有的目标车辆信息f_n（t-1）都被选择完毕，输出所有车辆轨迹集A_r和对应的目标车辆轨迹L_r；

本申请通过车辆检测模型对道路上的车辆进行三维点云数据获取，并且基于三维点云数据检测出目标车辆，再基于所有目标车辆在相邻帧之间的关系建立目标车辆关联对，之后基于目标车辆关联对建立目标车辆的轨迹信息，实现对车辆运行轨迹的跟踪。

将车辆轨迹集A_r中所有目标车辆信息对应的起始帧记为ST_r，将车辆轨迹集A_p中所有目标车辆信息对应的终帧记为END_r，遍历所有车辆轨迹集A_r，针对选择的车辆轨迹集A_r，将选择的车辆轨迹集合记为A_p，针对每个选择的车辆轨迹集合记为A_p执行如下操作：

将选择的车辆轨迹集A_p分别送入训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型，前向车辆轨迹预测模型和后向车辆轨迹预测模型均基于LSTM模型建立，分别输出前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben，且前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben中分别存储着目标车辆信息和目标车辆预测信息，前向车辆预测轨迹序列A_for中的目标车辆信息为END_p之前帧数对应预测的目标车辆信息，后向车辆预测轨迹序列A_ben中的目标车辆信息为ST_p之后帧数对应预测的目标车辆信息；

将所有车辆轨迹集A_r中终帧END_r在ST_p之前的所有车辆轨迹集A_r组成前向待连接车辆轨迹集合，从前向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号记为{h，h+1，h+2，…，m，...，h+a}，其中a为选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_for），，其中U_r（m）为选择的车辆轨迹集A_r中帧数编号为m对应的目标车辆信息，U_for（m）为前向车辆预测轨迹序列A_for中帧数编号为m对应的目标车辆信息，dis（U_r（m），U_for（m））为目标车辆信息U_r（m）与目标车辆信息U_for（m）之间的欧式距离，判断“ζ（A_r，A_for）＜ε”是否成立，若是“ζ（A_r，A_for）＜ε”成立，将车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r通过前向车辆预测轨迹序列A_for进行拼接，即在车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r添加前向车辆预测轨迹序列A_for中不与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从前向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是“ζ（A_r，A_for）＜ε”不成立，则从前向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至前向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕；

将所有车辆轨迹集A_r中起始帧ST_r在END_p之前的所有车辆轨迹集A_r组成后向待连接车辆轨迹集合，从后向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号记为{e，e+1，e+2，…，c，...，e+b}，其中b为选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_bed），，其中U_r（c）为选择的车辆轨迹集A_r中帧数编号为c对应的目标车辆信息，U_bed（c）为后向车辆预测轨迹序列A_bed中帧数编号为c对应的目标车辆信息，dis（U_r（m），U_bed（m））为目标车辆信息U_r（m）与目标车辆信息U_bed（m）之间的欧式距离，判断“ζ（A_r，A_bed）＜ε”是否成立，若是“ζ（A_r，A_bed）＜ε”成立，将车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r通过后向车辆预测轨迹序列A_bed进行拼接，即在车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r添加后向车辆预测轨迹序列A_bed中不与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从后向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是“ζ（A_r，A_bed）＜ε”不成立，则从后向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至后向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕。

由于在车辆轨迹跟踪时，车辆在红绿灯或者遮挡等原因的影响下会停止运动或者降低速度，此时通过毫米波雷达是无法检测到目标车辆，而当目标车辆重新运动后，会将目标车辆视为新的目标车辆，导致目标车辆的轨迹跟踪无法保持连续性，对车辆轨迹的使用造成影响，本申请通过对目标车辆的所有轨迹进行预测，基于预测的车辆轨迹来判断不同车辆轨迹是否对应同一目标车辆，并选择可能为同一目标车辆的车辆轨迹进行连接，同时通过预测的目标车辆信息对缺失的轨迹进行补足，保证目标车辆轨迹的连续性。

实施例2

一种基于毫米波雷达的车辆运动轨迹跟踪系统，参见图1，包括：

三维点云数据获取模块，用于通过毫米波雷达获取当前帧的三维点云数据；

目标车辆信息集建立模块，用于将当前帧的三维点云数据送入训练好的车辆检测模型，输出当前帧所有目标车辆的目标车辆三维坐标，并基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集；

目标车辆关联对构建模块，用于基于当前帧对应的目标信息集和上一帧对应的目标信息集构建目标车辆关联对；

待构建轨迹目标车辆集构建模块，用于基于当前帧对应的所有目标车辆关联对并与上一帧对应的目标信息集中的目标车辆信息构建待构建轨迹目标车辆集；

目标车辆轨迹构建模块，用于基于待构建轨迹目标车辆集和所有目标车辆关联对输出目标车辆轨迹集，并根据目标车辆轨迹集构建目标车辆轨迹；

目标车辆轨迹重构模块，用于根据所有目标车辆轨迹集和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹进行重构。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。本说明书中未作详细描述的部分属于本领域专业技术人员公知的现有技术。

Claims (5)

1.一种基于毫米波雷达的车辆运动轨迹跟踪方法，其特征在于，包括：

通过毫米波雷达获取当前帧的三维点云数据，每在采样时间点获取三维点云数据时视为获取了一帧三维点云数据；

将当前帧的三维点云数据送入训练好的车辆检测模型，输出当前帧所有目标车辆的目标车辆三维坐标，基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集F_t，目标信息集F_t中存储的数据为{f₁（t），f₂（t），f₃（t），…，f_n（t），…，f_N（t）}，其中f_n（t）为当前帧中第n个目标车辆对应的目标车辆信息，且f_n（t）={x_n（t），y_n（t），z_n（t），v_n（t）}，目标车辆信息f_n（t）中，x_n（t）为当前帧中第n个目标车辆对应的X轴坐标值，y_n（t）为当前帧中第n个目标车辆对应的Y轴坐标值，z_n（t）为当前帧中第n个目标车辆对应的Z轴坐标值，v_n（t）为当前帧中第n个目标车辆对应的瞬时速度，t为当前帧目标车辆信息集F_t对应的帧数编号，初始为1，每进行一次三维点云数据获取操作，对t值执行增一操作，n为目标车辆信息集F_t中目标车辆对应的车辆编号，且n=1，2，3，……，N，N为当前帧中所有目标车辆的总数量；

基于当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1构建目标车辆关联对G_n（t）；

基于当前帧对应的所有目标车辆关联对G_n（t）并与上一帧对应的目标信息集F_t-1中的目标车辆信息f_n（t-1）输出待构建轨迹目标车辆集S（t-1）；

基于待构建轨迹目标车辆集S（t-1）、t-1帧和t-1帧之前的所有目标车辆关联对输出目标车辆轨迹集A_r，并根据目标车辆轨迹集A_r构建目标车辆轨迹L_r；

根据所有目标车辆轨迹集A_r和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹L_r进行重构；

基于当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1构建目标车辆关联对G_n（t），具体包括如下步骤：

逐个选择当前帧对应的目标信息集F_t中的目标车辆信息f_n（t），针对选择的目标车辆信息f_n（t），基于目标车辆信息f_n（t）中的X轴坐标值x_n（t）和Y轴坐标值y_n（t）确定目标车辆所处的道路区域μ_i，i=1，2，3，……，I，I为所有道路区域的总个数，再通过道路区域μ_i对选择的目标车辆信息f_n（t）进行标记；

获取当前帧对应的目标信息集F_t和上一帧对应的目标信息集F_t-1，逐个选择目标信息集F_t中的目标车辆信息f_n（t），针对每个选择的目标车辆信息f_n（t），输出目标车辆信息f_n（t）对应的道路区域μ_i，从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_n（t-1）中逐个选择目标车辆信息f_j（t-1），j∈{1，2，3，……，N}，计算车辆欧式距离dis_n（t-1，t）和车辆位移向量δ_n（t-1，t），其中车辆欧式距离dis_n（t-1，t）为选择的目标车辆信息f_n（t）与从目标信息集F_t-1中选择的目标车辆信息f_j（t-1）之间的欧式距离，车辆位移向量δ_n（t-1，t）=[x_n（t）-x_j（t-1），y_n（t）-y_j（t-1），z_n（t）-z_j（t-1），v_n（t）-v_j（t-1）]，判断是否满足关联条件，关联条件为dis_n（t-1，t）＜ε&&δ_n（t-1，t）·Vec_i≥0，其中ε为欧式距离阈值，Vec_i为目标车辆信息f_n（t）对应的道路区域μ_i对应的位移单位向量，由操作人员提前设定，表征道路区域μ_i上车辆的行驶方向，若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_n（t-1）满足关联条件，则基于选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）建立目标车辆关联对G_n（t），G_n（t）=（f_n（t），f_j（t-1）），将f_n（t）记为目标车辆关联对G_n（t）的前项，f_n（t-1）记为目标车辆关联对G_n（t）的后项；若是选择的目标车辆信息f_n（t）与选择的目标车辆信息f_j（t-1）不满足关联条件，则从目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）中选择下一个目标车辆信息f_j（t-1）；直至目标信息集F_t-1内道路区域μ_i对应的所有目标车辆信息f_j（t-1）都被选择完毕，则从目标信息集F_t中选择下一个目标车辆信息f_n（t）；直至目标信息集F_t中的所有目标车辆信息f_n（t）都被选择完毕，输出所有目标车辆关联对G_n（t）；

基于当前帧对应的所有目标车辆关联对G_n（t）并与上一帧对应的目标信息集F_t-1中的目标车辆信息f_n（t-1）输出待构建轨迹目标车辆集S（t-1），具体包括如下步骤：

遍历当前帧对应的所有目标车辆关联对G_n（t）并与目标信息集F_t-1中的目标车辆信息f_n（t-1）进行匹配，输出目标信息集F_t-1中未匹配成功的目标车辆信息f_n（t-1），并将目标信息集F_t-1中未匹配成功的所有目标车辆信息f_n（t-1）组成待构建轨迹目标车辆集S（t-1）；

基于待构建轨迹目标车辆集S（t-1）和t-1帧和t-1帧之前的所有目标车辆关联对输出目标车辆轨迹集A_r，并根据目标车辆轨迹集A_r构建目标车辆轨迹L_r，具体包括如下步骤：

遍历待构建轨迹目标车辆集S（t-1），再将t-1帧和t-1帧之前的所有目标车辆关联对组成目标车辆关联对集合，逐个选择待构建轨迹目标车辆集S（t-1）中的目标车辆信息f_n（t-1），针对选择的目标车辆信息f_n（t-1），从目标车辆关联对集合中选择满足前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1），将选择的目标车辆信息f_n（t-1）与前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项存入目标车辆轨迹集A_r，r初始为1，再将前项为选择的目标车辆信息f_n（t-1）的目标车辆关联对G_n（t-1）中的后项作为下一个选择的目标车辆关联对G_n（t-2）的前项，进行目标车辆关联对G_n（t-2）的查找，以此类推，直至无法选择到目标车辆关联对G_n（t-k）时，k∈{1，2，3，…，t-1}，输出目标车辆轨迹集A_r，再基于目标车辆轨迹集A_r中的所有目标车辆信息f_n（t-k）构建目标车辆轨迹L_r，构建目标车辆轨迹L_r的方式为以目标车辆轨迹集A_r中所有目标车辆信息f_n（t-k）对应的X轴坐标值、Y轴坐标值和Z轴坐标值建立坐标点，再连接所有坐标点，最后平滑处理得到对应的目标车辆轨迹L_r，且每构建一个目标车辆轨迹L_r，对r值进行增一操作，并从待构建轨迹目标车辆集S（t-1）中选择下一个目标车辆信息f_n（t-1）；直至待构建轨迹目标车辆集S（t-1）中所有的目标车辆信息f_n（t-1）都被选择完毕，输出所有车辆轨迹集A_r和对应的目标车辆轨迹L_r。

2.根据权利要求1所述的一种基于毫米波雷达的车辆运动轨迹跟踪方法，其特征在于，根据所有目标车辆轨迹集A_r和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹L_r进行重构，具体包括如下步骤：

将车辆轨迹集A_r中所有目标车辆信息对应的起始帧记为ST_r，将车辆轨迹集A_p中所有目标车辆信息对应的终帧记为END_r，遍历所有车辆轨迹集A_r，针对选择的车辆轨迹集A_r，将选择的车辆轨迹集合记为A_p，针对每个选择的车辆轨迹集合记为A_p执行如下操作：

将选择的车辆轨迹集A_p分别送入训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型，分别输出前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben，且前向车辆预测轨迹序列A_for和后向车辆预测轨迹序列A_ben中分别存储着目标车辆信息和目标车辆预测信息，前向车辆预测轨迹序列A_for中的目标车辆信息为END_p之前帧数对应预测的目标车辆信息，后向车辆预测轨迹序列A_ben中的目标车辆信息为ST_p之后帧数对应预测的目标车辆信息；

将所有车辆轨迹集A_r中终帧END_r在ST_p之前的所有车辆轨迹集A_r组成前向待连接车辆轨迹集合，从前向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号记为{h，h+1，h+2，…，m，...，h+a}，其中a为选择的车辆轨迹集A_r与前向车辆预测轨迹序列A_for中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_for），，其中U_r（m）为选择的车辆轨迹集A_r中帧数编号为m对应的目标车辆信息，U_for（m）为前向车辆预测轨迹序列A_for中帧数编号为m对应的目标车辆信息，dis（U_r（m），U_for（m））为目标车辆信息U_r（m）与目标车辆信息U_for（m）之间的欧式距离，判断ζ（A_r，A_for）＜ε是否成立，若是ζ（A_r，A_for）＜ε成立，将车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r通过前向车辆预测轨迹序列A_for进行拼接，即在车辆轨迹集合A_p与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r添加前向车辆预测轨迹序列A_for中不与从前向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从前向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是ζ（A_r，A_for）＜ε不成立，则从前向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至前向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕；

将所有车辆轨迹集A_r中起始帧ST_r在END_p之前的所有车辆轨迹集A_r组成后向待连接车辆轨迹集合，从后向待连接车辆轨迹集合中逐个选择车辆轨迹集A_r，并将选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号记为{e，e+1，e+2，…，c，...，e+b}，其中b为选择的车辆轨迹集A_r与后向车辆预测轨迹序列A_bed中重复的帧数编号的总个数，计算轨迹欧氏距离平均值ζ（A_r，A_bed），，其中U_r（c）为选择的车辆轨迹集A_r中帧数编号为c对应的目标车辆信息，U_bed（c）为后向车辆预测轨迹序列A_bed中帧数编号为c对应的目标车辆信息，dis（U_r（m），U_bed（m））为目标车辆信息U_r（m）与目标车辆信息U_bed（m）之间的欧式距离，判断ζ（A_r，A_bed）＜ε是否成立，若是ζ（A_r，A_bed）＜ε成立，将车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r通过后向车辆预测轨迹序列A_bed进行拼接，即在车辆轨迹集合A_p与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r添加后向车辆预测轨迹序列A_bed中不与从后向待连接车辆轨迹集合选择的车辆轨迹集A_r重复的帧数编号对应的目标车辆信息，对车辆轨迹集合A_p进行更新，并删除从后向待连接车辆轨迹集合选择的车辆轨迹集A_r，基于更新后的辆轨迹集合A_p，重新构建对应的目标车辆轨迹L_p；若是ζ（A_r，A_bed）＜ε不成立，则从后向待连接车辆轨迹集合中选择下一个车辆轨迹集A_r；直至后向待连接车辆轨迹集合中所有车辆轨迹集A_r被选择完毕。

3.根据权利要求2所述的一种基于毫米波雷达的车辆运动轨迹跟踪方法，其特征在于，车辆检测模型基于Voxelnet模型建立。

4.根据权利要求3所述的一种基于毫米波雷达的车辆运动轨迹跟踪方法，其特征在于，前向车辆轨迹预测模型和后向车辆轨迹预测模型均基于LSTM模型建立。

5.一种基于毫米波雷达的车辆运动轨迹跟踪系统，其特征在于，所述系统应用上述权利要求1-4任一项所述的一种基于毫米波雷达的车辆运动轨迹跟踪方法，包括：

三维点云数据获取模块，用于通过毫米波雷达获取当前帧的三维点云数据；

目标车辆信息集建立模块，用于将当前帧的三维点云数据送入训练好的车辆检测模型，输出当前帧所有目标车辆的目标车辆三维坐标，并基于当前帧所有目标车辆的目标车辆三维坐标建立目标车辆信息集；

目标车辆关联对构建模块，用于基于当前帧对应的目标信息集和上一帧对应的目标信息集构建目标车辆关联对；

待构建轨迹目标车辆集构建模块，用于基于当前帧对应的所有目标车辆关联对并与上一帧对应的目标信息集中的目标车辆信息构建待构建轨迹目标车辆集；

目标车辆轨迹构建模块，用于基于待构建轨迹目标车辆集和所有目标车辆关联对输出目标车辆轨迹集，并根据目标车辆轨迹集构建目标车辆轨迹；

目标车辆轨迹重构模块，用于根据所有目标车辆轨迹集和训练好的前向车辆轨迹预测模型和训练好的后向车辆轨迹预测模型对目标车辆轨迹进行重构。