一种轨迹优化方法和电子设备技术

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

【技术实现步骤摘要】
一种轨迹优化方法和电子设备


[0001]本专利技术涉及高精度地图
，特别是一种轨迹优化方法和电子设备。

技术介绍

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

技术实现思路

[0003]鉴于上述问题，提出了一种克服上述问题或者至少部分地解决上述问题的轨迹优化方法和电子设备。
[0004]本专利技术的一个目的在于提供一种能够提高车体运动轨迹的相对精度的轨迹优化方法。
[0005]本专利技术的一个进一步的目的在于对车体运动轨迹的局部跳变和缓慢漂移进行有效修复。
[0006]本专利技术的另一个目的在于提供一种执行轨迹优化方法的电子设备。
[0007]特别地，根据本专利技术实施例的一方面，提供了一种轨迹优化方法，包括：
[0008]获取车辆的轨迹数据和与所述轨迹对应的激光点云数据，其中，所述轨迹数据包含轨迹中各轨迹点的时间戳和位姿信息，所述激光点云数据为所述轨迹中不同时间由车载激光雷达采集的数据；
[0009]从所述激光点云数据中选取多帧关键帧激光点云，并将与所述关键帧激光点云的时间戳相同的轨迹点的位姿信息作为关键帧位姿；
[0010]遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点，并确定与所述异常轨迹点对应的异常关键帧位姿；
[0011]将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果；
[0012]根据所述相邻帧匹配结果对所有所述关键帧位姿进行第一次位姿图优化，得到一次优化的关键帧位姿，并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹；
[0013]检测所述一次优化的轨迹中定位信号弱的区段，并将落入所述区段中的关键帧位姿作为弱信号关键帧位姿；
[0014]将所述关键帧位姿中间隔第二指定距离的两个关键帧位姿作为一对间隔帧，得到
多对间隔帧；
[0015]对于每对间隔帧，若该对间隔帧的两个关键帧位姿作为端点限定的区间中存在所述弱信号关键帧位姿，则对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进行点云匹配，得到间隔帧匹配结果；
[0016]根据所述相邻帧匹配结果和所述间隔帧匹配结果对所述一次优化的关键帧位姿进行第二次位姿图优化，得到二次优化的关键帧位姿，并根据所述二次优化的关键帧位姿和所述一次优化的轨迹得到最终优化的轨迹。
[0017]可选地，所述遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点的步骤包括：
[0018]遍历所述轨迹中的各轨迹点，分别以各所述轨迹点为中心点，建立所述中心点与其两边临近的指定数量个轨迹点的集合；
[0019]检测所述集合的轨迹平滑性；
[0020]根据轨迹平滑性的检测结果判断所述中心点是否出现异常，所述异常包括跳变和/或断连；
[0021]若是，则确定所述中心点为异常轨迹点。
[0022]可选地，所述确定与所述异常轨迹点对应的异常关键帧位姿的步骤包括：
[0023]将每两个相邻的异常轨迹点之间的距离小于第一指定阈值的异常轨迹点聚合为一异常段以得到至少一个异常段，并将落入所述至少一个异常段中的关键帧位姿确定为异常关键帧位姿。
[0024]可选地，所述将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果的步骤包括：
[0025]将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配；
[0026]若匹配成功，则得到所述异常关键帧位姿和与其相邻的下一帧关键帧位姿之间的匹配后相对位姿。
[0027]可选地，所述关键帧位姿为关键帧位姿矩阵，所述根据所述相邻帧匹配结果对所有所述关键帧位姿进行第一次位姿图优化，得到一次优化的关键帧位姿，并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹的步骤包括：
[0028]以所有所述关键帧位姿矩阵作为待优化变量构成位姿图的顶点；
[0029]以所有所述关键帧位姿矩阵及其逆矩阵构建单边约束条件；
[0030]以匹配成功的异常关键帧位姿的匹配后相对位姿、以及匹配不成功的异常关键帧位姿和非异常的关键帧位姿的初始相对位姿共同构建双边约束条件；
[0031]通过非线性优化算法对所述位姿图进行优化求解得到一次优化的关键帧位姿矩阵，并利用所述一次优化的关键帧位姿矩阵确定的轨迹点替换所述轨迹中对应的轨迹点，生成一次优化的轨迹。
[0032]可选地，在构建所述单边约束条件时，降低由异常关键帧位姿矩阵及其逆矩阵构成的单边约束条件的权重。
[0033]可选地，所述检测所述一次优化的轨迹中定位信号弱的区段的步骤包括：
[0034]获取组合导航系统的信号参数，所述信号参数包括当前接收到的卫星数、卫星系统的位置精度因子和解算位置姿态标准差中的至少之一；
[0035]根据所述信号参数确定所述一次优化的轨迹中定位信号弱的区段。
[0036]可选地，所述对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进行点云匹配，得到间隔帧匹配结果的步骤包括：
[0037]对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进行点云匹配；
[0038]若匹配成功，则得到所述两个关键帧位姿之间的匹配后相对位姿。
[0039]可选地，所述根据所述相邻帧匹配结果和所述间隔帧匹配结果对所述一次优化的关键帧位姿进行第二次位姿图优化，得到二次优化的关键帧位姿，并根据所述二次优化的关键帧位姿和所述一次优化的轨迹得到最终优化的轨迹的步骤包括：
[0040]以所有所述一次优化的关键帧位姿矩阵作为待优化变量构成位姿图的顶点；
[0041]以所有所述一次优化的关键帧位姿矩阵及其逆矩阵构建单边约束条件；
[0042]以匹配成功的异常关键帧位姿的匹配后相对位姿、以及匹配不成功的异常关键帧位姿和非异常的关键帧位姿的初始相对位姿共同构建第一双边约束条件；
[0043]以匹配成功的各所述对间隔帧的所述两个关键帧位姿之间的匹配后相对位姿构建第二双边约束条件；
[0044]通过非线性优化算法对所述位姿图进行优化求解得到二次优化的关键帧位姿矩阵，并利用所述二次优化的关键帧位姿矩阵确定的轨迹点替换所述一次优化的轨迹中对应的轨迹点，生成最终优化的轨迹。
[0045]根据本发本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轨迹优化方法，包括：获取车辆的轨迹数据和与所述轨迹对应的激光点云数据，其中，所述轨迹数据包含轨迹中各轨迹点的时间戳和位姿信息，所述激光点云数据为所述轨迹中不同时间由车载激光雷达采集的数据；从所述激光点云数据中选取多帧关键帧激光点云，并将与所述关键帧激光点云的时间戳相同的轨迹点的位姿信息作为关键帧位姿；遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点，并确定与所述异常轨迹点对应的异常关键帧位姿；将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果；根据所述相邻帧匹配结果对所有所述关键帧位姿进行第一次位姿图优化，得到一次优化的关键帧位姿，并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹；检测所述一次优化的轨迹中定位信号弱的区段，并将落入所述区段中的关键帧位姿作为弱信号关键帧位姿；将所述关键帧位姿中间隔第二指定距离的两个关键帧位姿作为一对间隔帧，得到多对间隔帧；对于每对间隔帧，若该对间隔帧的两个关键帧位姿作为端点限定的区间中存在所述弱信号关键帧位姿，则对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进行点云匹配，得到间隔帧匹配结果；根据所述相邻帧匹配结果和所述间隔帧匹配结果对所述一次优化的关键帧位姿进行第二次位姿图优化，得到二次优化的关键帧位姿，并根据所述二次优化的关键帧位姿和所述一次优化的轨迹得到最终优化的轨迹。2.根据权利要求1所述的轨迹优化方法，其中，所述遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点的步骤包括：遍历所述轨迹中的各轨迹点，分别以各所述轨迹点为中心点，建立所述中心点与其两边临近的指定数量个轨迹点的集合；检测所述集合的轨迹平滑性；根据轨迹平滑性的检测结果判断所述中心点是否出现异常，所述异常包括跳变和/或断连；若是，则确定所述中心点为异常轨迹点。3.根据权利要求1所述的轨迹优化方法，其中，所述确定与所述异常轨迹点对应的异常关键帧位姿的步骤包括：将每两个相邻的异常轨迹点之间的距离小于第一指定阈值的异常轨迹点聚合为一异常段以得到至少一个异常段，并将落入所述至少一个异常段中的关键帧位姿确定为异常关键帧位姿。4.根据权利要求1所述的轨迹优化方法，其中，所述将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果的步骤包括：
将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配；若匹配成功，则得到所述异常关键帧位姿和与其相邻的下一帧关键帧位姿之间的匹配后相对位姿。5.根据权利要求4所述的轨迹优化方法，其中，所述关键帧位姿为关键帧位姿矩阵，所述根...

【专利技术属性】
技术研发人员：罗玮，鲁荣荣，
申请(专利权)人：亿咖通湖北技术有限公司，
类型：发明
国别省市：