基于毫米波雷达的车辆轨迹修正方法、装置及存储介质与流程

1.本发明涉及计算机领域，特别涉及一种基于毫米波雷达的车辆轨迹修正方法、装置、及存储介质。


背景技术：

2.随着社会经济的发展，人们的交通需求不断上涨，而交通供给在短时间难以改变，因此道路占有率不断提高，道路拥堵事件不断发生。基于雷达的交通信息感知技术作为智能交通技术领域的主要研究内容之一，能够通过各类传感器对道路、水路、空中等不同交通环境下进行实时检测，采集准确的交通数据如交通参数、交通状况、交通环境等信息，为建立智能交通模型，评估道路利用率，改善车辆拥堵情况提供坚实的数据支持。
3.毫米波雷达常用的跟踪方式主要为卡尔曼滤波（如扩展卡尔曼滤波或无迹卡尔曼滤波）与粒子滤波等。然而，这些传统方法对目标的检测误差较大，定位精度不高。针对这一问题，一些学者提出了许多具有创新性的方法，如基于指纹的联合目标检测方法、利用卷积神经网络构建检测模型等。但是，这些方法中多对已成型的车辆轨迹线进行预测与优化阶段缺少对微观信息的处理，如如车辆轨迹点的处理，这会导致出现一些不合理甚至错误轨迹线，对这些不合理轨迹线进行预测会影响目标跟踪和预测结果、不利于车辆导航和交通系统管理。


技术实现要素：

4.本技术实施例提供一种基于毫米波雷达的车辆轨迹修正、装置、及存储介质，解决毫米波雷达数据对车辆轨迹预测精度不高的问题。
5.一方面，本技术提供一种基于毫米波雷达的车辆轨迹修正方法，所述方法包括：s1，通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；所述交通源数据包含观测路段的车辆轨迹点及轨迹信息；所述轨迹信息包含车辆的运动状态信息；s2，根据所述车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；所述车辆轨迹线和车辆编号相对应；s3，基于所述目标轨迹点、所述轨迹信息以及道路规范信息对所述车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；s4，确定所述缺陷轨迹线的缺陷区域，结合所述轨迹信息进行轨迹点预测和修正，获得目标轨迹线。
6.具体的，s2包括：s21，获取观测范围内所述车辆轨迹点和对应的轨迹向量，并根据所述轨迹向量计算空间距离和空间密度比；s22，基于空间距离与空间密度比、约束半径和道路通行能力对所述车辆轨迹点进行空间特征约束，确定轨迹噪点并滤除；s23，基于车辆编号对滤除所述轨迹噪点后的轨迹点集进行聚类，生成各车辆的所
述车辆轨迹线。
7.具体的，s21包括：计算第i轨迹向量的第i均值距离以及第i轨迹向量与第j轨迹向量之间的空间距离；空间距离公式如下：；空间距离公式如下：其中，是根据所有轨迹向量计算的均值向量；是根据所有轨迹向量计算的协方差矩阵；和分别表示第i和j个轨迹向量；表示第i个轨迹向量的第i均值距离；t为向量转秩；基于第i均值距离、第i和第j轨迹向量空间距离以及第i和第j车辆轨迹点对应的车速，计算两个轨迹点之间的空间密度比；空间密度比公式如下：其中，和分别表示第i和第j车辆轨迹点的车速；为所有车辆轨迹点的平均速度；和表示的方向参数。
8.具体的，s22包括：基于毫米波雷达的采样周期确定约束半径，以及根据观测道路确定道路通行能力确定空间密度范围；将约束半径、道路通行能力、第i均值距离、第i和第j轨迹向量的空间距离以及空间密度比代入空间特征约束公式，根据所述特征约束公式确定同一目标车辆的聚类点和噪点；特征约束公式表示如下：其中，和为约束半径阈值，为阈值下限，为阈值上限；和为密度阈值，为阈值下限，为阈值上限；为可聚类的类别。
9.具体的，s23包括：将去噪后的所述目标轨迹点按车辆id编号排序，并基于车辆id生成所述车辆轨迹线、轨迹数据集和轨迹点集。
10.具体的，s3包括：s31，基于所述车辆轨迹线和对应的所述轨迹信息分别构建车辆状态函数；表示如下：下：下：
其中，表示第个车辆轨迹线的车辆状态函数；为车辆相对距离矩阵，为第辆车第车辆轨迹点的相对距离；为车辆相对速度矩阵，为第辆车第车辆轨迹点的相对速度；为车辆相对加速度矩阵，为第辆车第车辆轨迹点的相对加速度；为车辆相对角度矩阵，为第辆车第车辆轨迹点的相对角度；和y分别为车辆轨迹点的横纵坐标矩阵，为第辆车第车辆轨迹点的横坐标，为第车辆轨迹点的纵坐标；为车辆总数，为车辆轨迹线上轨迹点的总数；s32，基于道路规范信息对所述车辆轨迹线上相邻所述车辆轨迹点进行车速约束验证；车速约束条件如下：验证；车速约束条件如下：其中，是道路最高限速；为路面附着系数；为采集周期；是第辆车第车辆轨迹点的相对速度；是第辆车第车辆轨迹点的相对距离；s33，基于道路规范信息对所述车辆轨迹线上相邻所述车辆轨迹点进行位移约束验证；位移约束条件如下：验证；位移约束条件如下：验证；位移约束条件如下：其中，和分别是第辆车第车辆轨迹点的横纵坐标；和分别是横纵坐标最大移动期望值；为车辆最大移动半径；s34，基于车速约束验证和位移约束验证确定所述缺陷轨迹线的缺陷类型；缺陷类型至少包括轨迹线断裂、轨迹线停滞和轨迹线偏移。
11.具体的，s4包括：s41，根据所述缺陷轨迹线的缺陷类型确定缺陷区域、缺陷轨迹起点及缺陷轨迹终点；s42，根据所述缺陷轨迹线的行驶方向确定与所述缺陷轨迹起点相邻的辅助预测点；所述辅助预测点是所述缺陷轨迹起点的前一轨迹点；s43，基于车辆波动系数、所述缺陷轨迹起点和所述辅助预测点的位置、速度、加速度、角度及横纵坐标，计算下一预测轨迹点的车辆状态，并通过相邻两个预测轨迹点迭代计算并修正所述缺陷区域的所有缺陷轨迹点。
12.具体的，预测轨迹点基于如下车辆状态预测公式如下：具体的，预测轨迹点基于如下车辆状态预测公式如下：具体的，预测轨迹点基于如下车辆状态预测公式如下：具体的，预测轨迹点基于如下车辆状态预测公式如下：具体的，预测轨迹点基于如下车辆状态预测公式如下：具体的，预测轨迹点基于如下车辆状态预测公式如下：其中，第辆车的第车辆轨迹点是缺陷轨迹起点，第车辆轨迹点是辅助预测点，第车辆轨迹点是预测轨迹点；为第辆车期望最大加速度；为舒适减速度；和为位置波动系数，和为速度波动系数，和为加速度波动系数。
13.具体的，当时，=1，=0；当时，=0，=1；当时，=1，=0；当时，=0，=1；当时，=1，=0；当时，=0，=1。
14.另一方面，本技术提供一种基于毫米波雷达的车辆轨迹修正装置，所述装置包括：雷达数据获取模块，用于通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；所述交通源数据包含观测路段的车辆轨迹点及轨迹信息；所述轨迹信息包含车辆的运动状态信息；车辆轨迹生成模块，用于根据所述车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；所述车辆轨迹线和车辆编号相对应；缺陷确定模块，用于基于所述目标轨迹点、所述轨迹信息以及道路规范信息对所述车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；缺陷轨迹修正模块，用于确定所述缺陷轨迹线的缺陷区域，结合所述轨迹信息进行轨迹点预测和修正，获得目标轨迹线。
15.又一方面，本技术提供一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述任一方面所述的基于毫米波雷达的车辆轨迹修正方法。
16.又一方面，本技术提供一种计算机程序产品或计算机程序，所述计算机程序产品或计算机程序包括计算机指令，所述计算机指令存储在计算机可读存储介质中；计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得所述计算机设备执行上述任一方面所述的基于毫米波雷达的车辆轨迹修正方法。
17.本技术实施例提供的技术方案带来的有益效果至少包括：本技术利用车辆轨迹点的位置与速度等空间密度特征的相似性，将具有相似性的轨迹点划分为同类，从而筛除低精度点与杂点，实现数据去噪和聚类。为交通场景下实现对相似特征轨迹点的聚类与毫米波雷达数据杂点的筛除提供了新思路；本技术基于车辆轨迹点的轨迹信息和观测道路的规范信息，对车辆轨迹线进行精度约束，筛选出轨迹不合理的缺陷轨迹线，为车辆轨迹线的预测、修正、和优化等提供约束验证条件；本技术基于多目标车辆轨迹线的校验方法确定缺陷区域，并通过车辆轨迹点的轨迹信息计算并修正缺陷区域的预测轨迹线，从而得到车辆高精度的目标轨迹线，有效提高轨迹的利用率，改善轨迹的跟踪结果，提高道路预警和车辆自动/辅助驾驶的精度和安全性。
附图说明
18.图1是本技术实施例提供的基于毫米波雷达的车辆轨迹修正方法的流程图；图2是本技术另一实施例提供的基于毫米波雷达的车辆轨迹修正方法的流程图；图3 是本技术实施例提供的缺陷轨迹线和轨迹修复后的对比示意图；图4是本技术实施例提供的基于毫米波雷达的车辆轨迹修正装置的结构框图。
具体实施方式
19.为使本技术的目的、技术方案和优点更加清楚，下面将结合附图对本技术实施方式作进一步地详细描述。
20.在本文中提及的“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
21.毫米波雷达工作在毫米波段，通常毫米波是指30～300ghz频段(波长为1～10mm)。毫米波的波长介于厘米波和光波之间，因此毫米波兼有微波制导和光电制导的优点。同厘米波导引头相比，毫米波导引头具有体积小、质量轻和空间分辨率高的特点。与红外、激光、电视等光学导引头相比，毫米波导引头穿透雾、烟、灰尘的能力强，具有全天候（大雨天除外）全天时的特点。另外，毫米波导引头的抗干扰、反隐身能力也优于其他微波导引头 。
22.当前常用的雷达传感器主要有超声波雷达、激光雷达与毫米波雷达。发射和接收频域内的电磁波来对目标进行探测，从而获得目标的相对距离、相对速度、相对加速度等信息。本方案中毫米波雷达数据主要为检测到的有关车辆的交通源数据。
23.图1是本技术实施例提供的基于毫米波雷达的车辆轨迹修正方法的流程图，包括如下步骤：s1，通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；交通源数
据包含观测路段的车辆轨迹点及轨迹信息。
24.车辆轨迹点是雷达设备周期性采集到的，雷达设备设置在观测道路的指定位置，实时采集车辆轨迹点。每个车辆轨迹点都具有对应的轨迹信息，轨迹信息包含有车辆（也即车辆轨迹点）的运动状态信息，包括位置、速度、加速度、角度、横纵坐标等。运动状态信息是基于雷达设备位置作为坐标原点建立的笛卡尔坐标系得到。
25.在一种可能的实施方式中，毫米波雷达的参数配置和道路规格参数如下表1和下表2所示：表1 毫米波雷达参数表2 道路规格参数s2，根据车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线。
26.因为雷达设备的数据采集量庞大，同时受限于检测精度、路况复杂度及天气等因素影响，雷达终端设备终端处理的雷达数据中会包含较多的干扰数据。若终端直接根据车辆轨迹点生成车辆轨迹线，其轨迹精度无法满足道路预警和自动/辅助驾驶。
27.就雷达数据本身而言，同一车辆行驶过程的产生的雷达数据应具有相似的空间特征，如速度和位置等特征都具有关联性。所以本技术从车辆轨迹点的空间密度特征角度进行数据分析，对所有车辆轨迹点进行筛选，对车辆轨迹降噪和聚类。聚类的过程就是根据把
具有关联空间密度特征的轨迹点聚合，生成车辆轨迹线。且同一车辆轨迹线具有相同的车辆编号。
28.s3，基于目标轨迹点、轨迹信息以及道路规范信息对车辆轨迹线进行约束检测，提取其中的缺陷轨迹线。
29.此处需要注意，缺陷轨迹线的识别是对车辆轨迹线从检测精度上的进一步约束的表现，例如轨迹线上的轨迹点出现轨迹点偏移、缺失甚至断裂等情况。通过道路规范信息进行约束的目的就是为了检测出其中不合理的缺陷轨迹线。道路规范信息根据观测路段决定，如道路限速范围、位置偏移大小（根据车道宽度确定）、道路通行能力和路面附着系数等等。
30.s4，确定缺陷轨迹线的缺陷区域，结合轨迹信息进行轨迹点预测和修正，获得目标轨迹线。
31.确定缺陷轨迹线和缺陷区域后，就需要根据具体的缺陷类型确定待修正的轨迹点数量。修正的原理基于该缺陷轨迹线的正常车辆轨迹点、轨迹点的空间密度特征、以及对应轨迹点的轨迹信息进行预测，将预测的目标轨迹线替换或补全缺陷区域，得到完整的目标轨迹线。
32.综上所述，本技术利用车辆轨迹点的位置与速度等空间密度特征的相似性，将具有相似性的轨迹点划分为同类，从而筛除低精度点与杂点，实现数据去噪和聚类。为交通场景下实现对相似特征轨迹点的聚类与毫米波雷达数据杂点的筛除提供了新思路；本技术基于车辆轨迹点的轨迹信息和观测道路的规范信息，对车辆轨迹线进行精度约束，筛选出轨迹不合理的缺陷轨迹线，为车辆轨迹线的预测、修正、和优化等提供约束验证条件；本技术基于多目标车辆轨迹线的校验方法确定缺陷区域，并通过车辆轨迹点的轨迹信息计算并修正缺陷区域的预测轨迹线，从而得到车辆高精度的目标轨迹线，有效提高轨迹的利用率，改善轨迹的跟踪结果，提高道路预警和车辆自动/辅助驾驶的精度和安全性。
33.图2是本技术另一实施例提供的基于毫米波雷达的车辆轨迹修正方法的流程图，包括如下步骤：步骤201，通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据。
34.步骤202，获取观测范围内车辆轨迹点和对应的轨迹向量，并根据轨迹向量计算空间距离和空间密度比。
35.空间距离和空间密度比是分别代表车辆轨迹点之间的距离特征和密度特征，对于同类型/同一车辆，其车辆轨迹点具有相似的位置与速度关系，而杂点与低精度点则比较散乱，不具有相近特征。因此提出密度空间聚类模型对所有车辆点进行聚类，筛选出具有相似特征的轨迹点，从而筛除杂点。具体计算过程如该过程具体还包括如下步骤：步骤202a，计算第i轨迹向量的第i均值距离以及第i轨迹向量与第j轨迹向量之间的空间距离。
36.假设毫米波雷达观测到个车辆目标 ，每个目标具有一个向量，即。本方案首先需要计算空间距离；空间距离公式如下：
其中，是根据所有轨迹向量计算的均值向量；是根据所有轨迹向量计算的协方差矩阵；和分别表示第i和j个轨迹向量；表示第i个轨迹向量的第i均值距离，t表示向量转秩。
37.步骤202b，基于第i均值距离、第i和第j轨迹向量空间距离以及第i和第j车辆轨迹点对应的车速，计算两个轨迹点之间的空间密度比。
38.和采用向量间的矩阵运算来确定空间距离，考虑到向量间距离和车速对向量密度的影响，对于选定的和两个轨迹向量，采用积分推算空间密度比，表示如下：其中，和分别表示第i和第j车辆轨迹点的车速；为所有车辆轨迹点的平均速度；和表示的方向参数，和的取值为。
39.步骤203，基于空间距离与空间密度比、约束半径和道路通行能力对车辆轨迹点进行空间特征约束，确定轨迹噪点并滤除。
40.在一种可能的实施方式中，确定向量之间的距离与密度比后，引入点集对轨迹点进行去噪聚类，具体通过约束条件找到具有相似特征的车辆轨迹点，并通过判断是否满足约束条件确定其中的轨迹噪点和滤除。该过程还包括如下步骤：步骤203a，基于毫米波雷达的采样周期确定约束半径，以及根据观测道路确定道路通行能力确定空间密度范围。
41.约束半径基于毫米波雷达的采样周期确定，在成像原理上理解为采样间隔内轨迹点的移动范围，约束半径阈值通过和表示，为下限阈值，为上限阈值。道路通行能力根据车辆的空间密度范围的大小决定，为阈值下限，为阈值上限；为可聚类的类别，也就是划分成同一车辆轨迹线的类型数。
42.步骤203b，将约束半径、道路通行能力、第i均值距离、第i和第j轨迹向量的空间距离以及空间密度比代入空间特征约束公式，根据特征约束公式确定同一目标车辆的聚类点和噪点。
43.空间特征约束公式表示如下：其中，和分别为阈值下限和阈值上限；和分别为阈值下限和阈值上限；为可聚类的类别。
44.对于某一点集，若其空间内的样本轨迹数据占比多，样本密度大，且数据间具有相
似的特征，可认为该点集内的轨迹点为正常轨迹点，保留这些点集中的轨迹点。若空间内的样本数据出现概率低，样本密度小，分布松散，随机，样本间无明显关联性，可认为该点集内的轨迹点为噪点或低精度点，将这些轨迹点筛除，实现初步降噪效果。
45.步骤204，基于车辆编号对滤除轨迹噪点后的轨迹点集进行聚类，生成各车辆的车辆轨迹线。
46.将前述步骤获得的轨迹点集按照车辆编号进行聚类，即可获得各个车辆的车辆轨迹线。也就是一个车辆id编号对应一个车辆轨迹线，车辆轨迹线所包含的车辆轨迹线即为轨迹点集，所有车辆轨迹线构成轨迹数据集。轨迹数据集表示为()；轨迹点点集表示为()。
47.此处需要说明的是，虽然根据id编号车辆，但是并不代表观测路段一定具有这些车辆，因为缺陷轨迹线可能表现出断裂的情况，在聚类过程被划分为两个轨迹线。也就是说，雷达设备在目标追踪时出现跟丢的情况（极少出现），将其误识别成两个车辆的两条轨迹线。
48.步骤205，基于车辆轨迹线和对应的轨迹信息分别构建车辆状态函数。
49.对于每个确定的车辆轨迹线，其轨迹上的每个车辆轨迹点都表示车辆在不同时刻下的运动状态，包括位置、速度、加速度、角度和横纵坐标等。本技术基于轨迹信息构建出对应车辆轨迹线的车辆状态函数，表示如下：应车辆轨迹线的车辆状态函数，表示如下：应车辆轨迹线的车辆状态函数，表示如下：其中，表示第个车辆轨迹线的车辆状态函数；为车辆相对距离矩阵，为第辆车第车辆轨迹点的相对距离；为车辆相对速度矩阵，为第辆车第车辆轨迹点的相对速度；为车辆相对加速度矩阵，为第辆车第车辆轨迹点的相对加速度；为车辆相对角度矩阵，为第辆车第车辆轨迹点的相对角度；和y分别为车辆轨迹点的横纵坐标矩阵，为第辆车第车辆轨迹点的横坐标，为第车辆轨迹点的纵坐标；为车辆总数，为车辆轨迹线上轨迹点的总数。这样可以将轨迹线上所有车辆轨迹点的状态都进行描述。
50.对于某一轨迹，其轨迹线可能出现局部断裂，轨迹线于某一点处停滞，以及轨迹线中的某些轨迹点发生剧烈偏移。为了筛选出不合理的轨迹线，提高轨迹的跟踪精度、稳定性以及准确性，我们需要对一条轨迹线上连续的轨迹点进行约束，从而去除不合理的轨迹点，便于后续修正轨迹。
51.步骤206，基于道路规范信息对车辆轨迹线上相邻车辆轨迹点进行车速约束验证。
52.此步骤中的道路规范信息至少包括道路最高限速和路面附着系数，车速约束条件如下：
其中，是道路最高限速，例如本技术设定为80 km/h；为路面附着系数，取决于道路路面条件，例如本技术设定为0.32；为采集周期；是第辆车第车辆轨迹点的相对速度；是第辆车第车辆轨迹点的相对距离。
53.步骤207，基于道路规范信息对车辆轨迹线上相邻车辆轨迹点进行位移约束验证。
54.对于道路上的车辆而言，轨迹线也有可能发生剧烈偏移，其自身位置应满足以下位移约束条件：位移约束条件：位移约束条件：其中，和分别是第辆车第车辆轨迹点的横纵坐标；和分别是横纵坐标最大移动期望值；为车辆最大移动半径。
55.需要说明的是，步骤206和步骤207可以按照车速约束验证和位移约束验证的顺序执行，或者是按照位移约束验证和车速约束验证的顺序执行，还可以是步骤206和步骤207同时进行，具体先后顺序本技术不作限定。
56.步骤208，基于车速约束验证和位移约束验证确定缺陷轨迹线的缺陷类型。
57.前述说到，轨迹线上的轨迹点可能会出现轨迹点偏移、缺失甚至断裂等情况等。当轨迹线出现停滞现象时，说明目标跟丢，在此时确定轨迹点数量较小的时候直接将其丢弃；当轨迹点数量达到目标设定值时，将其确定为缺陷轨迹线并进行预测修正。如图3中1和2标注所示，标注1表示不满足位移和车速约束的偏离点，标注2表示轨迹线部分断裂缺失的情况，对于局部断裂的轨迹线或轨迹点剧烈偏移的轨迹线，同样采用本方案进行修正。
58.步骤209，根据缺陷轨迹线的缺陷类型确定缺陷区域、缺陷轨迹起点及缺陷轨迹终点。
59.对于轨迹点停滞且可预测的类型，步骤206和步骤207约束检测后确定的缺陷区域即为断点处到设定预测长度（根据实际情况设定修正长度），缺陷轨迹起点即为断点，缺陷轨迹终点根据预测长度决定。
60.对于轨迹点偏移的类型，步骤206和步骤207约束检测后确定的缺陷区域即为偏移线上的轨迹点，缺陷轨迹起点即为偏移线上的起始点，缺陷轨迹终点即为偏移线上的终点（后续轨迹点符合车速和位移约束条件）。
61.对于局部断裂的情况，步骤206和步骤207约束检测后确定的缺陷区域即为两条断点之间的空缺区域，缺陷轨迹起点和终点为两端的断点。
62.对于确定和可修整的缺陷轨迹线，将该轨迹线上所有的车辆轨迹点放入新的缺陷
点集中，即令，基于这些轨迹点进行修正。
63.步骤210，根据缺陷轨迹线的行驶方向确定与缺陷轨迹起点相邻的辅助预测点。
64.辅助预测点是缺陷轨迹起点的前一轨迹点，也就是在前一个时间周期的轨迹点，第辆车的第车辆轨迹点是缺陷轨迹起点，则第车辆轨迹点即为辅助预测点。
65.步骤211，基于车辆波动系数、缺陷轨迹起点和辅助预测点的位置、速度、加速度、角度及横纵坐标，计算下一预测轨迹点的车辆状态，并通过相邻两个预测轨迹点迭代计算并修正缺陷区域的所有缺陷轨迹点。
66.预测轨迹点基于如下车辆状态预测公式如下：预测轨迹点基于如下车辆状态预测公式如下：预测轨迹点基于如下车辆状态预测公式如下：预测轨迹点基于如下车辆状态预测公式如下：预测轨迹点基于如下车辆状态预测公式如下：预测轨迹点基于如下车辆状态预测公式如下：其中，第辆车的第车辆轨迹点是缺陷轨迹起点，第车辆轨迹点是辅助预测点，第车辆轨迹点是预测轨迹点；为第辆车期望最大加速度；为舒适减速度；和为位置波动系数，和为速度波动系数，和为加速度波动系数。
67.该车辆状态预测公式分别从位置、速度、加速度、角度、横坐标及纵坐标等车辆状态信息进行预测和修正。
68.对于位置波动系数，当时，=1，=0；当时，=0，=1；对于速度波动系数，当时，=1，=0；当时，=0，=1；对于加速度波动系数，当时，=1，=0；当时，=0，=1。
69.通过波动系数的设置，可以实时根据相邻车辆轨迹点和预测轨迹点进行迭代式预测，同时根据各个迭代周期内的运动参数动态调节，以确保预测精度，直至预测缺陷区域所有的轨迹点，并将其修正。修正后的轨迹点重新构成目标车辆轨迹线，并将轨迹信息同步到入车辆状态函数中。修正后的轨迹线为，用修正的轨迹代替原轨迹，即令（若轨迹
无需修正，则保留不变），得到修正后的轨迹数据集。
70.如图3所示，修复后的轨迹线（目标车辆轨迹线）筛除了不满足位移约束的偏离点，同时将局部断裂缺失的轨迹线进行修复补全，获得完整的轨迹线。因为轨迹线是位于车道上的，对于终端设备或云端设备来说，可以根据车辆轨迹线和变道情况进行后续的轨迹预测，测算出车辆姿态预测数据，为车辆自动和辅助驾驶提供数据支持。
71.可选的，位于终端设备或显示设备，为了达到更好的为道路预警或为自动驾驶提供可视化信息，在一种可能的实施方式中，采用道路的空间特征与交通信息编辑写入sumo软件中，利用场景搭建工具构建一个虚拟的道路，并标注毫米波雷达的位置以及侦察范围。
72.在sumo软件中，根据轨迹所对应的车辆id号生成相对应的车辆，并输入修正后的轨迹数据集，车辆状态函数，原始轨迹点的点集，修正后预测轨迹点的点集，轨迹点的数据矩阵等信息，从而在软件中成功生成车辆修正后的行驶轨迹线，实现毫米波雷达修正轨迹的可视化。
73.图4是本技术实施例提供的基于毫米波雷达的车辆轨迹修正装置的结构框图，装置包括：雷达数据获取模块410，用于通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；交通源数据包含观测路段的车辆轨迹点及轨迹信息；轨迹信息包含车辆的运动状态信息；车辆轨迹生成模块420，用于根据车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；车辆轨迹线和车辆编号相对应；缺陷确定模块430，用于基于目标轨迹点、轨迹信息以及道路规范信息对车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；缺陷轨迹修正模块440，用于确定缺陷轨迹线的缺陷区域，结合轨迹信息进行轨迹点预测和修正，获得目标轨迹线。
74.本技术还提供一种计算机设备，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述各方面所述的基于毫米波雷达的车辆轨迹修正方法。
75.本技术还提供一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述各方面所述的基于毫米波雷达的车辆轨迹修正方法。
76.本技术还提供一种计算机程序产品或计算机程序，所述计算机程序产品或计算机程序包括计算机指令，所述计算机指令存储在计算机可读存储介质中；计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得所述计算机设备执行上述各方面所述的基于毫米波雷达的车辆轨迹修正方法。
77.以上对本发明的较佳实施例进行了描述；需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案作出许多可能的变动和修饰，或
修改为等同变化的等效实施例，这并不影响本发明的实质内容；因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

技术特征：
1.一种基于毫米波雷达的车辆轨迹修正方法，其特征在于，所述方法包括：s1，通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；所述交通源数据包含观测路段的车辆轨迹点及轨迹信息；所述轨迹信息包含车辆的运动状态信息；s2，根据所述车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；所述车辆轨迹线和车辆编号相对应；s3，基于所述目标轨迹点、所述轨迹信息以及道路规范信息对所述车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；s4，确定所述缺陷轨迹线的缺陷区域，结合所述轨迹信息进行轨迹点预测和修正，获得目标轨迹线。2.根据权利要求1所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s2包括：s21，获取观测范围内所述车辆轨迹点和对应的轨迹向量，并根据所述轨迹向量计算空间距离和空间密度比；s22，基于空间距离与空间密度比、约束半径和道路通行能力对所述车辆轨迹点进行空间特征约束，确定轨迹噪点并滤除；s23，基于车辆编号对滤除所述轨迹噪点后的轨迹点集进行聚类，生成各车辆的所述车辆轨迹线。3.根据权利要求2所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s21包括：计算第i轨迹向量的第i均值距离以及第i轨迹向量与第j轨迹向量之间的空间距离；空间距离公式如下：；空间距离公式如下：其中，是根据所有轨迹向量计算的均值向量；是根据所有轨迹向量计算的协方差矩阵；和分别表示第i和j个轨迹向量；表示第i个轨迹向量的第i均值距离；t为向量转秩；基于第i均值距离、第i和第j轨迹向量空间距离以及第i和第j车辆轨迹点对应的车速，计算两个轨迹点之间的空间密度比；空间密度比公式如下：其中，和分别表示第i和第j车辆轨迹点的车速；为所有车辆轨迹点的平均速度；和表示的方向参数。4.根据权利要求3所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s22包括：基于毫米波雷达的采样周期确定约束半径r，以及根据观测道路确定道路通行能力确定空间密度范围k；
将约束半径、道路通行能力、第i均值距离、第i和第j轨迹向量的空间距离以及空间密度比代入空间特征约束公式，根据所述特征约束公式确定同一目标车辆的聚类点和噪点；特征约束公式表示如下：其中，和为约束半径阈值，为阈值下限，为阈值上限；和为密度阈值，为阈值下限，为阈值上限；为可聚类的类别。5.根据权利要求3所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s23包括：将去噪后的所述目标轨迹点按车辆id编号排序，并基于车辆id生成所述车辆轨迹线、轨迹数据集和轨迹点集。6.根据权利要求5所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s3包括：s31，基于所述车辆轨迹线和对应的所述轨迹信息分别构建车辆状态函数；表示如下：s31，基于所述车辆轨迹线和对应的所述轨迹信息分别构建车辆状态函数；表示如下：s31，基于所述车辆轨迹线和对应的所述轨迹信息分别构建车辆状态函数；表示如下：其中，表示第个车辆轨迹线的车辆状态函数；为车辆相对距离矩阵，为第辆车第车辆轨迹点的相对距离；为车辆相对速度矩阵，为第辆车第车辆轨迹点的相对速度；为车辆相对加速度矩阵，为第辆车第车辆轨迹点的相对加速度；为车辆相对角度矩阵，为第辆车第车辆轨迹点的相对角度；和y分别为车辆轨迹点的横纵坐标矩阵，为第辆车第车辆轨迹点的横坐标，为第车辆轨迹点的纵坐标；为车辆总数，为车辆轨迹线上轨迹点的总数；s32，基于道路规范信息对所述车辆轨迹线上相邻所述车辆轨迹点进行车速约束验证；车速约束条件如下：车速约束条件如下：其中，是道路最高限速；为路面附着系数；为采集周期；是第辆车第车辆轨迹点的相对速度；是第辆车第车辆轨迹点的相对距离；s33，基于道路规范信息对所述车辆轨迹线上相邻所述车辆轨迹点进行位移约束验证；位移约束条件如下：
其中，和分别是第辆车第车辆轨迹点的横纵坐标；和分别是横纵坐标最大移动期望值；为车辆最大移动半径；s34，基于车速约束验证和位移约束验证确定所述缺陷轨迹线的缺陷类型；缺陷类型至少包括轨迹线断裂、轨迹线停滞和轨迹线偏移。7.根据权利要求6所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，s4包括：s41，根据所述缺陷轨迹线的缺陷类型确定缺陷区域、缺陷轨迹起点及缺陷轨迹终点；s42，根据所述缺陷轨迹线的行驶方向确定与所述缺陷轨迹起点相邻的辅助预测点；所述辅助预测点是所述缺陷轨迹起点的前一轨迹点；s43，基于车辆波动系数、所述缺陷轨迹起点和所述辅助预测点的位置、速度、加速度、角度及横纵坐标，计算下一预测轨迹点的车辆状态，并通过相邻两个预测轨迹点迭代计算并修正所述缺陷区域的所有缺陷轨迹点。8.根据权利要求7所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，预测轨迹点基于如下车辆状态预测公式如下：点基于如下车辆状态预测公式如下：点基于如下车辆状态预测公式如下：点基于如下车辆状态预测公式如下：点基于如下车辆状态预测公式如下：点基于如下车辆状态预测公式如下：其中，第辆车的第车辆轨迹点是缺陷轨迹起点，第车辆轨迹点是辅助预测点，第车辆轨迹点是预测轨迹点；为第辆车期望最大加速度；为舒适减速度；和为位置波动系数，和为速度波动系数，和为加速度波动系数。9.根据权利要求8所述的基于毫米波雷达的车辆轨迹修正方法，其特征在于，当
时，=1，=0；当时，=0，=1；当时，=1，=0；当时，=0，=1；当时，=1，=0；当时，=0，=1。10.一种基于毫米波雷达的车辆轨迹修正装置，其特征在于，所述装置包括：雷达数据获取模块，用于通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；所述交通源数据包含观测路段的车辆轨迹点及轨迹信息；所述轨迹信息包含车辆的运动状态信息；车辆轨迹生成模块，用于根据所述车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；所述车辆轨迹线和车辆编号相对应；缺陷确定模块，用于基于所述目标轨迹点、所述轨迹信息以及道路规范信息对所述车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；缺陷轨迹修正模块，用于确定所述缺陷轨迹线的缺陷区域，结合所述轨迹信息进行轨迹点预测和修正，获得目标轨迹线。11.一种计算机设备，其特征在于，所述计算机设备包含处理器和存储器，所述存储器中存储由至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1至9任一所述的基于毫米波雷达的车辆轨迹修正方法。12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如权利要求1至9任一所述的基于毫米波雷达的车辆轨迹修正方法。

技术总结
本申请公开基于毫米波雷达的车辆轨迹修正方法、装置、及存储介质，涉及计算机领域，通过观测路段设置的毫米波雷达设备及雷达参数采集交通源数据；根据车辆轨迹点的空间密度特征进行轨迹降噪和聚类，并根据降噪后的目标轨迹点聚类生成车辆轨迹线；基于目标轨迹点、轨迹信息以及道路规范信息对车辆轨迹线进行约束检测，提取其中的缺陷轨迹线；确定缺陷轨迹线的缺陷区域，结合轨迹信息进行轨迹点预测和修正，获得目标轨迹线。该方案可对交通场景下毫米波雷达数据中具有相似特征的车辆轨迹点进行降噪和聚类，降低干扰，同时能够对车辆轨迹线进行约束诊断，修正其中严重偏离、丢失和断裂的缺陷点进行替换和补全修正，提高轨迹利用率并改善跟踪效果。用率并改善跟踪效果。用率并改善跟踪效果。


技术研发人员：石汉青 周恩德 吴鹏 陈文哲
受保护的技术使用者：健鼎（无锡）电子有限公司
技术研发日：2023.04.21
技术公布日：2023/5/25