[发明专利]一种轨迹优化方法和电子设备

说明书

本发明提供了一种轨迹优化方法和电子设备，涉及高精度地图技术领域。该轨迹优化方法通过多尺度激光点云帧匹配进行位姿修正，针对异常轨迹点使用相邻点云帧匹配修正，针对定位信号弱的异常区段使用间隔点云帧匹配修正，并通过位姿图优化对车体运动轨迹进行优化，可以有效地对局部跳变和缓慢漂移的轨迹进行恢复，提高车体运动轨迹的相对精度，从而提升高精度地图的生产质量。

技术领域

本发明涉及高精度地图技术领域，特别是一种轨迹优化方法和电子设备。

背景技术

高精度地图(High Definition Map，简称为HD Map)对于自动驾驶至关重要。高精度地图的生产依赖于精准的车体运动轨迹。高精度组合导航系统一般在GNSS(GlobalNavigation Satellite System，全球导航卫星系统)信号良好的开阔场景可以稳定输出符合精度要求的车体位姿，但对于高楼林立的城市峡谷以及高架下等场景，由于建筑物遮挡、多路径效应等问题，GNSS信号失锁乃至丢失，依赖于GNSS系统提供绝对位姿修正的高精度组合导航系统解算出的车体位姿容易出现一些异常跳点和缓慢漂移，轨迹相对精度和绝对精度下降。

发明内容

鉴于上述问题，提出了一种克服上述问题或者至少部分地解决上述问题的轨迹优化方法和电子设备。

本发明的一个目的在于提供一种能够提高车体运动轨迹的相对精度的轨迹优化方法。

本发明的一个进一步的目的在于对车体运动轨迹的局部跳变和缓慢漂移进行有效修复。

本发明的另一个目的在于提供一种执行轨迹优化方法的电子设备。

特别地，根据本发明实施例的一方面，提供了一种轨迹优化方法，包括：

获取车辆的轨迹数据和与所述轨迹对应的激光点云数据，其中，所述轨迹数据包含轨迹中各轨迹点的时间戳和位姿信息，所述激光点云数据为所述轨迹中不同时间由车载激光雷达采集的数据；

从所述激光点云数据中选取多帧关键帧激光点云，并将与所述关键帧激光点云的时间戳相同的轨迹点的位姿信息作为关键帧位姿；

遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点，并确定与所述异常轨迹点对应的异常关键帧位姿；

将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果；

根据所述相邻帧匹配结果对所有所述关键帧位姿进行第一次位姿图优化，得到一次优化的关键帧位姿，并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹；

检测所述一次优化的轨迹中定位信号弱的区段，并将落入所述区段中的关键帧位姿作为弱信号关键帧位姿；

将所述关键帧位姿中间隔第二指定距离的两个关键帧位姿作为一对间隔帧，得到多对间隔帧；

对于每对间隔帧，若该对间隔帧的两个关键帧位姿作为端点限定的区间中存在所述弱信号关键帧位姿，则对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进行点云匹配，得到间隔帧匹配结果；

根据所述相邻帧匹配结果和所述间隔帧匹配结果对所述一次优化的关键帧位姿进行第二次位姿图优化，得到二次优化的关键帧位姿，并根据所述二次优化的关键帧位姿和所述一次优化的轨迹得到最终优化的轨迹。

可选地，所述遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点的步骤包括：

遍历所述轨迹中的各轨迹点，分别以各所述轨迹点为中心点，建立所述中心点与其两边临近的指定数量个轨迹点的集合；

检测所述集合的轨迹平滑性；

根据轨迹平滑性的检测结果判断所述中心点是否出现异常，所述异常包括跳变和/或断连；