一种基于雷达检测的车辆多路段轨迹匹配方法和系统

本发明属于智慧交通领域，具体涉及一种基于雷达检测的车辆多路段轨迹匹配方法和系统。

背景技术：

1、车辆跟踪和导航时，需要准确掌握车辆的实时位置及完整轨迹。一般通过轨迹匹配的方式来跟踪车辆。多路段轨迹匹配是指一个毫米波雷达仅覆盖一个以相邻路口为端点的路段场景下，要从不同雷达下的轨迹检测记录找到来自于同一个交通单元的轨迹记录的问题。

2、现有技术中，存在多种基于雷达检测的路段轨迹匹配方式。例如，申请号为cn202111026263.6的中国专利公开了一种路口多雷达检测轨迹匹配融合方法，通过将雷达检测的实际轨迹与预设的参考轨迹进行匹配，对同一雷达内部和不同雷达之间检测的同一辆车多条轨迹片断进行实时匹配融合，从而输出同一辆车经过该路口的完整行驶轨迹。但是上述轨迹匹配时，需要车道级的雷达数据，对数据质量要求较高，匹配效果严重依赖于数据。申请号为cn202310094737.3的中国专利公开了一种基于雷达数据的高速公路车辆轨迹拼接方法，通过实时获取多个雷达的车辆轨迹数据并进行预处理和轨道划分，对各雷达视域范围内的道路车辆的位置进行坐标系统一后，遍历相邻两雷达处理后的车辆轨迹数据，根据匹配条件确定两轨迹的统一轨迹点，并进行轨迹拼接。但上述方法适用于高速公路上进行车道划分的车辆，普适性不强。现有的方法中普遍存在严重依赖于数据的问题，当雷达架设稀疏且覆盖范围存在不重合时，则无法获得完整的车辆轨迹，匹配效果差。

技术实现思路

1、鉴于现有技术中的上述缺陷或不足，本发明旨在提供一种基于雷达检测的车辆多路段轨迹匹配方法和系统，通过设置入路口轨迹池和出路口轨迹池，并设定相应的阈值进行轨迹匹配，当雷达覆盖范围小且部分无法重合时，仍然可以获得较准确的轨迹匹配，得到完整的车辆路线，解决雷达的覆盖范围仅包括路段且重合区域较小的情况下，车辆在不同雷达间的id保持问题。

2、为了实现上述目的，本发明实施例采用如下技术方案：

3、第一方面，本发明实施例提供了一种基于雷达检测的车辆多路段轨迹匹配方法，所述方法包括如下步骤：

4、步骤s1，获取雷达所检测路口的轨迹数据，当预定时长后未再读取到该雷达检测的原始的轨迹数据，则判定该雷达对该车辆所探测的轨迹数据已经全部到达；

5、步骤s2，建立全局轨迹池sall；对已经全部到达的轨迹数据进行预处理，得到轨迹id，且设置与轨迹id对应的匹配标识字段；并将轨迹数据、对应的轨迹id以及匹配标识字段全部放入sall中，其中sall中包括所有路口跟踪且已到达的轨迹；

6、步骤s3，在每个路口设置两个独立的轨迹池分别为入路口轨迹池sin和出路口轨迹池sout，将sall中的轨迹按预定逻辑分配到对应路口的sin或sout中；

7、步骤s4，依次遍历每个路口，将同一路口的sin和sout中的轨迹两两配对，计算对应的轨迹距离，并将同一路口的所有计算的轨迹距离按升序排列，取出轨迹距离小于阈值σ2的轨迹对；

8、步骤s5，识别所取出的轨迹对中的轨迹数据，若该轨迹数据未曾匹配过，则判定为该轨迹对匹配成功；

9、步骤s6，匹配成功的轨迹对中，将从sout中取出的轨迹设置start_matched＝true匹配标识，将从sin中取出的轨迹设置end_matched＝true匹配标识；记录匹配成功的结果作为输出，并将匹配成功的轨迹对分别从路口的sin和sout中清理出去。

10、作为本发明的一个优选实施例，步骤s3中所述预定逻辑包括如下步骤：

11、步骤s31，若轨迹的匹配标识为start_matched＝true，则跳过当前轨迹；否则执行下面的步骤：以路口中心为路口坐标值，计算每个路口距离当前轨迹id对应的轨迹数据起始点的距离并进行排序，若起始点与最近的路口的距离小于阈值σ1，则将该轨迹分配给距离起始点最近的路口的sout，否则跳过；

12、步骤s32，若轨迹的匹配标识end_matched＝true，则跳过当前轨迹；否则执行下面的步骤：计算每个路口距离当前轨迹id对应的轨迹数据终止点的距离并进行排序，若终止点与最近的路口的距离小于阈值σ1，则将该轨迹分配给距离终止点最近的路口的sin，否则跳过；

13、步骤s33，若一条轨迹没有被分配到任意一个路口的sin或sout，则认为车辆在某个路段中央驶出路网，不对车辆进行操作，等待至步骤s6清理该轨迹。

14、作为本发明的一个优选实施例，步骤s4中所述计算对应的轨迹距离时，同时考虑了轨迹的时间和空间信息，具体过程如下：

15、步骤s41，给定两条轨迹其中为第j条轨迹的第i个轨迹点，分别为第j条轨迹的第i个轨迹点的经度、纬度及时间戳；

16、步骤s42，判断两条轨迹t1和t2的轨迹id中的雷达id是否相同，若相同，则令t1与t2间的轨迹距离为正无穷，结束计算；若不同，则执行步骤s43；

17、步骤s43，将t1在t2的时间戳上进行外推，其中时间大于小于的片段使用线性插值进行填充，小于的片段用开头两个轨迹点进行线性插值来外延，大于的片段用结尾两个轨迹点进行线性插值来外延，得到外延轨迹t1′；

18、步骤s44，将大于的外延片段旋转45°、90°、-45°和-90°得到t′1a，t′1b，t′1c，t′1d四条模拟转弯的外延轨迹；

19、步骤s45，计算t′1，t′1a，t′1b，t′1c，t′1d分别与t2的大圆距离，取其中最小的值作为t1和t2间的轨迹距离。

20、作为本发明的一个优选实施例，在获取雷达检测数据时，每隔τinputs读取所监测时间段内产生的轨迹输入，τinput为数据读取的时间间隔。

21、作为本发明的一个优选实施例，步骤s1中所述原始的轨迹数据所含有的字段包括：雷达id、车辆id、时间戳、经度、纬度和速度。

22、作为本发明的一个优选实施例，所述轨迹id，包括雷达id、车辆id和起始时间戳。

23、作为本发明的一个优选实施例，步骤s5还包括：在预设时间点，遍历全局轨迹池sall，如果存在轨迹长时间未匹配上的轨迹则将其从全局轨迹池sall中清理出去。

24、第二方面，本发明实施例还提供了一种基于雷达检测的车辆多路段轨迹匹配系统，所述系统包括：数据获取模块，全局轨迹池建立模块、独立轨迹池设置模块、轨迹分配模块、轨迹距离计算模块、匹配识别模块、后处理模块；其中，

25、所述数据获取模块用于获取雷达所检测路口的轨迹数据，当预定时长后未再读取到该雷达检测的原始的轨迹数据，则判定该雷达对该车辆所探测的轨迹数据已经全部到达；

26、所述全局轨迹池建立模块用于建立全局轨迹池sall；对已经全部到达的轨迹数据进行预处理，得到轨迹id，且设置与轨迹id对应的匹配标识字段；并将轨迹数据、对应的轨迹id以及匹配标识字段全部放入sall中，其中sall中包括所有路口跟踪且已到达的轨迹；

27、所述独立轨迹池设置模块用于在每个路口设置两个独立的轨迹池分别为入路口轨迹池sin和出路口轨迹池sout；

28、所述轨迹分配模块用于将sall中的轨迹按预定逻辑分配到对应路口的sin或sout中；

29、所述轨迹距离计算模块用于依次遍历每个路口，将同一路口的sin和sout中的轨迹两两配对，计算对应的轨迹距离，并将同一路口的所有计算的轨迹距离按升序排列，取出轨迹距离小于阈值σ2的轨迹对；

30、所述匹配识别模块用于识别所取出的轨迹对中的轨迹数据，若该轨迹数据未曾匹配过，则判定为该轨迹对匹配成功；

31、所述后处理模块用于将匹配成功的轨迹对中从sout中取出的轨迹设置start_matched＝true匹配标识，将从sin中取出的轨迹设置end_matched＝true匹配标识，记录匹配成功的结果作为输出，并将匹配成功的轨迹对分别从路口的sin和sout中清理出去。

32、作为本发明的一个优选实施例，所述轨迹距离计算模块计算对应的轨迹距离时，同时考虑轨迹的时间和空间信息。

33、本发明实施例所提供的技术方案具有如下有益效果：

34、本发明实施例所提供的基于雷达检测的车辆多路段轨迹匹配方法和系统，通过在路口设置入口池和出口池，计算入口池和出口池中轨迹对的距离，对不同雷达间检测的同一车辆进行实时匹配，从而实现车辆从一条路段行驶到另一条路段，在不同雷达下的id保持不变，获得车辆完整的行驶轨迹。本发明实现简单、数据质量要求宽松、实施灵活，只需要标定每个路口的中心经纬度即可，不需要标定车道范围、车道允许的行驶方向、路段的上下游邻接关系等其他信息；不要求不同雷达覆盖范围有重叠，对于数据质量的要求并不苛刻；在实施的时候既可以对包含数百个路口的路网也可以对数个相邻或不相邻的路口进行部署，在雷达覆盖范围交叉的但没有路口的地方还可以手动添加路口完成部署，具有很高的灵活性，适用范围广，实用能力强。

35、当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。