一种基于多模态的回环优化方法

本发明涉及无人驾驶定位与建图。更具体地说，本发明涉及一种基于多模态的回环优化方法。

背景技术：

1、随着人工智能技术的快速发展，无人驾驶技术的研究越来越火热。高精度地图是实现无人驾驶的关键技术之一。高精度地图构建的关键步骤是采集得到数据并通过算法进行处理去除语义得到的初始点云地图，初始点云地图的精度将直接影响到最终通过软件加工生成的高精度地图精度指标。

2、同时定位与建图(simultaneous localization and mapping，简称slam)是目前构建点云地图的主流方法。slam前端是连续帧的点云帧通过点云配准来估计无人车当前的状态，随着时间的累积，仅仅使用激光里程计相对位姿以及imu预积分得到的相对位姿约束无法消除累计误差。所以在slam系统中一般会添加一个模块来增加slam系统约束，消除这个累计误差，通常会使用回环检测模块。

3、回环检测是判断无人车所在的位置是否曾经经过，如果回环检测成功并成功添加回环约束，可以给系统提供很好的绝对观测约束。通过回环检测给非连续帧添加约束可以有效的进行轨迹优化，修正之前由里程计累加得到的累计误差。然而，在实际工程中，激光slam的回环检测部分一般都是结合gps进行判断。但是由于gps自身的局限性(易受遮挡物影响，信号不稳定)，回环检测往往受到影响。而采用scan context技术其回环效率又非常的低，所以，如何避免现有技术中gps、scan context各自单独进行回环的带来的不利影响，提升回环检测的鲁棒性十分重要。

技术实现思路

1、本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

2、为了实现本发明的这些目的和其它优点，提供了一种基于多模态的回环优化方法，基于gps作为先验约束进行回环帧筛选，如不存在信号漂移或信号消失，则使用当前点云帧同历史点云帧进行点云配准，以得到对应的航向角；

3、否则进行模态切换，采用scan context进行补充检测优化点云地图中的回环，进而为后续计算所需的两帧回环帧相对位姿提供角度初值。

4、优选的是，是否存在信号漂移或信号消失的判断流程包括：

5、将gps信息从经纬高数据转换到地图坐标系中以得到当前帧的gps数据；

6、将当前帧的gps数据和上一帧gps数据做差，得到两帧点云数据的差值，将该差值与预先设定的阈值ⅰ进行比较，若差值大于阈值ⅰ则认为发生严重漂移，当前帧的gps数据不可用，否则认为gps数据可用。

7、优选的是，当gps数据可用时，则在预定半径范围内搜索gps数据对应的点云帧，如果能搜索到位姿数据，则使用scan context将当前帧与历史帧进行相似度分数计算；

8、当gps数据不可用时，则采用scan context遍历以前的关键帧，以寻找最优的回环点云帧，并将该回环点云帧与当前帧进行相似度分数计算。

9、优选的是，若相似度分数计算后的结果大于阈值ⅱ，则将当前帧作为对应的航向角、联合所述差值作为初值进行icp匹配；

10、若icp匹配后的误差小于预定的阈值ⅲ，则认为回环成功并构建相应的回环约束加入到图优化窗口，从而对建图结果进行回环优化；

11、否则均认定为回环失败。

12、优选的是，在采用scan context遍历以前关键帧之前，还包括使用scan context算法对点云进行粗定位，所述粗定位的方式为：

13、将点云沿半径增大方向，把点云空间等分成nr个圆环，则每个圆环的宽度dr为：

14、

15、上式中,lmax表示激光雷达的最大扫描距离；

16、将分割得到的每个圆环切割成ns等份，则分割后激光点集合可表示为：

17、

18、上式中，pij表示第i个圆环第j个扇形的分割单元重点的集合；

19、生成scan context矩阵i，矩阵中的每一个值即为对应圆环id以及对应扇形id组成的点云点中的最大值；

20、两帧点云之间的scan context矩阵相似度计算公式为：

21、

22、其中，iq,ic为当前帧和历史帧的scan context矩阵，和为对应矩阵中的第j列向量。

23、本发明至少包括以下有益效果：本发明针对现有回环检测算法鲁棒性不高的问题，将gps信息引入scan context，以结合scan context和gps的优点，对回环检测进行优化，提高了它的鲁棒性。

24、本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

技术特征：

1.一种基于多模态的回环优化方法，其特征在于，基于gps作为先验约束进行回环帧筛选，如不存在信号漂移或信号消失，则使用当前点云帧同历史点云帧进行点云配准，以得到对应的航向角；

2.如权利要求1所述的基于多模态的回环优化方法，其特征在于，是否存在信号漂移或信号消失的判断流程包括：

3.如权利要求2所述的基于多模态的回环优化方法，其特征在于，当gps数据可用时，则在预定半径范围内搜索gps数据对应的点云帧，如果能搜索到位姿数据，则使用scancontext将当前帧与历史帧进行相似度分数计算；

4.如权利要求3所述的基于多模态的回环优化方法，其特征在于，若相似度分数计算后的结果大于阈值ⅱ，则将当前帧作为对应的航向角、联合所述差值作为初值进行icp匹配；

5.如权利要求3所述的基于多模态的回环优化方法，其特征在于，在采用scan context遍历以前关键帧之前，还包括使用scan context算法对点云进行粗定位，所述粗定位的方式为：

技术总结
本发明公开了一种基于多模态的回环优化方法，基于GPS作为先验约束进行回环帧筛选，如不存在信号漂移或信号消失，则使用当前点云帧同历史点云帧进行点云配准，以得到对应的航向角；否则进行模态切换，采用Scan Context进行补充检测优化点云地图中的回环，进而为后续计算所需的两帧回环帧相对位姿提供角度初值。本发明提供一种基于多模态的回环优化方法，其针对现有回环检测算法鲁棒性不高的问题，将GPS信息引入Scan Context，结合Scan Context和GPS的优点，对回环检测进行优化，提高了它的鲁棒性。

技术研发人员：蒋涛,唐祥贵,宁梓豪,张航天,王书豪
受保护的技术使用者：成都信息工程大学
技术研发日：
技术公布日：2024/1/16