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

权利要求书

1.一种轨迹优化方法，包括：

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的轨迹优化方法，其中，所述遍历检查所述轨迹中的所有轨迹点，找出异常轨迹点的步骤包括：

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

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

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

若是，则确定所述中心点为异常轨迹点。

3.根据权利要求1所述的轨迹优化方法，其中，所述确定与所述异常轨迹点对应的异常关键帧位姿的步骤包括：

将每两个相邻的异常轨迹点之间的距离小于第一指定阈值的异常轨迹点聚合为一异常段以得到至少一个异常段，并将落入所述至少一个异常段中的关键帧位姿确定为异常关键帧位姿。

4.根据权利要求1所述的轨迹优化方法，其中，所述将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点云进行点云匹配，得到相邻帧匹配结果的步骤包括：

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

若匹配成功，则得到所述异常关键帧位姿和与其相邻的下一帧关键帧位姿之间的匹配后相对位姿。

5.根据权利要求4所述的轨迹优化方法，其中，所述关键帧位姿为关键帧位姿矩阵，所述根据所述相邻帧匹配结果对所有所述关键帧位姿进行第一次位姿图优化，得到一次优化的关键帧位姿，并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹的步骤包括：

以所有所述关键帧位姿矩阵作为待优化变量构成位姿图的顶点；

以所有所述关键帧位姿矩阵及其逆矩阵构建单边约束条件；

以匹配成功的异常关键帧位姿的匹配后相对位姿、以及匹配不成功的异常关键帧位姿和非异常的关键帧位姿的初始相对位姿共同构建双边约束条件；

通过非线性优化算法对所述位姿图进行优化求解得到一次优化的关键帧位姿矩阵，并利用所述一次优化的关键帧位姿矩阵确定的轨迹点替换所述轨迹中对应的轨迹点，生成一次优化的轨迹。

6.根据权利要求5所述的轨迹优化方法，其中，在构建所述单边约束条件时，降低由异常关键帧位姿矩阵及其逆矩阵构成的单边约束条件的权重。