(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210634712.3 (22)申请日 2022.06.06 (71)申请人 亿咖通 （湖北） 技 术有限公司 地址 430051 湖北省武汉市武汉经济技 术 开发区神龙大道18号太子湖文化数字 创意产业园创谷启动区B13 36号 (72)发明人 罗玮　鲁荣荣　 (74)专利代理 机构 北京智汇东方知识产权代理 事务所(普通 合伙) 11391 专利代理师 关艳芬 (51)Int.Cl. G06T 7/254(2017.01) G06T 7/73(2017.01) G06V 10/75(2022.01) (54)发明名称 一种轨迹优化方法和电子设备 (57)摘要 本发明提供了一种轨迹优化方法和电子设 备， 涉及高精度地图技术领域。 该轨迹优化方法 通过多尺度激光点云帧匹配进行位姿修正， 针对 异常轨迹点使用相邻点云帧匹配修正， 针对定位 信号弱的异常区段使用间隔点云帧匹配修正， 并 通过位姿图优化对车体运动轨迹进行优化， 可以 有效地对局部跳变和缓慢漂移的轨迹进行恢复， 提高车体运动轨迹的相对精度， 从而提升高精度 地图的生产质量。 权利要求书3页 说明书9页 附图3页 CN 115170617 A 2022.10.11 CN 115170617 A 1.一种轨 迹优化方法， 包括： 获取车辆的轨迹数据和与所述轨迹对应的激光点云数据， 其中， 所述轨迹数据包含轨 迹中各轨迹点的时间戳和位姿信息， 所述激光点云数据为所述轨迹中不同时间由车载激光 雷达采集的数据； 从所述激光点云数据中选取多帧关键帧激光点云， 并将与所述关键帧激光点云的时间 戳相同的轨 迹点的位姿信息作为关键帧位姿； 遍历检查所述轨迹中的所有轨迹点， 找出异常轨迹点， 并确定与所述异常轨迹点对应 的异常关键帧位姿； 将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点 云进行点云匹配， 得到相邻帧匹配结果； 根据所述相邻帧匹配结果对所有所述关键帧位姿进行第 一次位姿图优化， 得到一 次优 化的关键帧位姿， 并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨 迹； 检测所述一 次优化的轨迹 中定位信号弱的区段， 并将落入所述 区段中的关键帧位姿作 为弱信号关键帧位姿； 将所述关键帧位姿中间隔第 二指定距离的两个关键帧位姿作为一对 间隔帧， 得到多对 间隔帧； 对于每对间隔帧， 若该对 间隔帧的两个关键帧位姿作为端点限定的区间中存在所述弱 信号关键帧位姿， 则对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光点云进 行点 云匹配， 得到间隔帧匹配结果； 根据所述相邻帧匹配结果和所述间隔帧匹配结果对所述一次优化的关键帧位姿进行 第二次位姿图优化， 得到二次优化的关键帧位姿， 并根据所述二次优化的关键帧位姿和所 述一次优化的轨 迹得到最终优化的轨 迹。 2.根据权利要求1所述的轨迹优化方法， 其中， 所述遍历检查所述轨迹中的所有轨迹 点， 找出异常轨 迹点的步骤 包括： 遍历所述轨迹中的各轨迹点， 分别以各所述轨迹点为中心点， 建立所述中心点与其两 边临近的指定数量个轨 迹点的集 合； 检测所述 集合的轨迹平滑性； 根据轨迹平滑性的检测结果判断所述中心点是否出现异常， 所述异常包括跳变和/或 断连； 若是， 则确定所述中心点 为异常轨 迹点。 3.根据权利要求1所述的轨迹优化方法， 其中， 所述确定与所述异常轨迹点对应的异常 关键帧位姿的步骤 包括： 将每两个相邻的异常轨迹点之间的距离小于第一指定阈值的异常轨迹点聚合为一异 常段以得到至少一个异常段， 并将落入所述至少一个异常段中的关键帧位姿确定为异常关 键帧位姿。 4.根据权利要求1所述的轨迹优化方法， 其中， 所述将各异常关键帧位姿对应的关键帧 激光点云分别和与其相邻的下一帧关键帧激光点云进 行点云匹配， 得到相 邻帧匹配结果的 步骤包括：权　利　要　求　书 1/3 页 2 CN 115170617 A 2将各异常关键帧位姿对应的关键帧激光点云分别和与其相邻的下一帧关键帧激光点 云进行点云匹配； 若匹配成功， 则得到所述异常关键帧位姿和与其相邻的下一帧关键帧位姿之间的匹配 后相对位姿。 5.根据权利要求4所述的轨迹优化方法， 其中， 所述关键帧位姿为关键帧位姿矩阵， 所 述根据所述相 邻帧匹配结果对所有所述关键帧位姿进 行第一次位姿图优化， 得到一次优化 的关键帧位姿， 并根据所述一次优化的关键帧位姿和所述轨迹数据得到一次优化的轨迹的 步骤包括： 以所有所述关键帧位姿矩阵作为待优化变量构成位姿图的顶点； 以所有所述关键帧位姿矩阵及其逆矩阵构建单边约束条件； 以匹配成功的异常关键帧位姿的匹配后相对位姿、 以及匹配不成功的异常关键帧位姿 和非异常的关键帧位姿的初始相对位姿共同构建双边约束条件； 通过非线性优化算法对所述位姿图进行优化求解得到一 次优化的关键帧位姿矩阵， 并 利用所述一次优化的关键帧位姿矩阵确定的轨迹点替换所述轨迹中对应的轨迹点， 生成一 次优化的轨 迹。 6.根据权利要求5所述的轨迹优化方法， 其中， 在构建所述单边约束条件时， 降低由异 常关键帧位姿矩阵及其逆矩阵构成的单边约束条件的权 重。 7.根据权利要求1所述的轨迹优化方法， 其中， 所述检测所述一 次优化的轨迹中定位信 号弱的区段的步骤 包括： 获取组合导航系统的信号参数， 所述信号参数包括当前接收到的卫星数、 卫星系统的 位置精度因子和解 算位置姿态标准差中的至少之一； 根据所述信号 参数确定所述 一次优化的轨 迹中定位信号弱的区段。 8.根据权利要求5所述的轨迹优化方法， 其中， 所述对所述对 间隔帧的所述两个关键帧 位姿对应的关键帧激光 点云进行点云匹配， 得到间隔帧匹配结果的步骤 包括： 对所述对间隔帧的所述两个关键帧位姿对应的关键帧激光 点云进行点云匹配； 若匹配成功， 则得到所述两个关键帧位姿之间的匹配后相对位姿。 9.根据权利要求8所述的轨迹优化方法， 其中， 所述根据 所述相邻帧匹配结果和所述间 隔帧匹配结果对所述一次优化的关键帧位姿进行第二次位姿图优化， 得到二次优化的关键 帧位姿， 并根据所述二次优化的关键帧位姿和所述一次优化的轨迹得到最终优化的轨迹的 步骤包括： 以所有所述 一次优化的关键帧位姿矩阵作为待优化变量构成位姿图的顶点； 以所有所述 一次优化的关键帧位姿矩阵及其逆矩阵构建单边约束条件； 以匹配成功的异常关键帧位姿的匹配后相对位姿、 以及匹配不成功的异常关键帧位姿 和非异常的关键帧位姿的初始相对位姿共同构建第一双边约束条件； 以匹配成功的各所述对间隔帧的所述两个关键帧位姿之间的匹配后相对位姿构建第 二双边约束条件； 通过非线性优化算法对所述位姿图进行优化求解得到二 次优化的关键帧位姿矩阵， 并 利用所述二次优化的关键 帧位姿矩阵确定的轨迹点替换所述一次优化的轨迹中对应的轨 迹点， 生成最终优化的轨 迹。权　利　要　求　书 2/3 页 3 CN 115170617 A 3