一种基于栅格地图的暴力匹配回环检测方法

本发明涉及一种在封闭环境内部的回环检测算法，利用优化位姿和激光信息来进行基于栅格地图的暴力匹配，得到对累计误差的回环检测优化。

背景技术：

1、同步定位与建图(simultaneous localization and mapping,slam)通过传感器感知周围环境，根据解算出的位置和地图进行自身定位，并同时构建增量式地图，实现自主定位和导航。通过前端里程计与后端优化的共同作用，可以实现位置环境下的定位，但由于都是通过最小二乘优化计算得到的位姿变换，而求解的定位问题是个复杂的非线性系统，因此最终的优化结果可能会陷入局部最小值，即定位错误，例如在某一时刻剧烈运动会造成定位漂移；并且如果选择错误的关键帧位姿的插入到点云地图中，这会导致之后的后端优化时帧与地图匹配会出现较大的误差，甚至会导致定位完全失败。回环检测可以提高系统的定位精度，在出现重复场景的时候及时矫正位姿，对同时定位与建图进行优化。目前大多数回环检测方法是词袋模型，通过自然语言处理和信息检索的一种简单的文档表示方法，在大场景下计算量较大，容易造成误匹配，效率低实时性变差。

技术实现思路

1、本发明要克服现有技术在克服封闭环境下的回环检测中存在的累计误差导致的错误关键帧入栈问题，提出一种基于栅格地图的暴力匹配回环检测方法。

2、本发明为解决现有技术问题所采用的技术方案是：

3、一种基于栅格地图的暴力匹配回环检测方法，包括如下步骤：

4、s1:获取激光传感器数据，由于物体运动导致点云数据存在畸变，矫正激光点云数据；

5、s2:将获取到的初始数据进行二维投影，将投影后二维点云做像素化处理；

6、s3:保证初始地图的准确性，利用广度优先搜索检测初始地图是否为闭合进行判断；

7、s4:更新栅格地图中每一个栅格的极大似然估计，并且构建最大栅格概率地图，同时计算击中点和原点之间的所有栅格坐标；

8、s5:开始前端相邻匹配优化计算位姿，开始暴力匹配，将基于绝对坐标系下的运动估计提取出基于二维平面栅格地图坐标系下的旋转量和位移量。

9、优选地，所述步骤s1具体包括：

10、pk+i,pk+i′代表第k帧内的第i个畸变点和矫正点；由于imu单元测量频率是激光雷达的10倍，所以每一个激光点都可以通过imu信息插值计算得到对应时间戳的当前姿态变换矩阵，根据匀速模型对雷达点云进行插值处理，其位姿变换矩阵为：

11、

12、w，j是第k帧内的所有imu的运动估计帧数和当前点所处的第j帧，tk+j,tk+j+1是当前第k帧的变化矩阵叠加当前imu的运动估计的变化矩阵以及后一帧的变化矩阵。

13、优选地，所述步骤s2具体包括：

14、将雷达开始后静态扫描的初始数帧矫正后的点云构建点云地图，然后投影到二维平面上，将投影后二维点云做像素化处理，那么落入该像素内的点参数值做公式(2)的加权操作，其值作为对应像素估计，并标记为占用像素，避免后续同一个像素出现重复击中得分的现象；

15、

16、其中si代表当前点的权重大小，wi代表当前点的参数值，k代表加权因子，设置越大，则代表越小的点，权重越大。

17、优选地，所述步骤s3具体包括：

18、s3.1:获得s2中的二维平面后，以初始状态为起始点，存入队列；

19、s3.2:从队列中取出第一个像素坐标设置为当前像素，并标记此像素为已搜索状态，然后搜索当前像素的四领域中的未搜索状态像素；

20、s3.3:如果搜索到的像素为黑色占用状态，则跳过此步骤；若搜到的像素为未占用状态则将搜索到的像素坐标推入队列末尾；如果在搜索的过程中，碰到地图的边界则表示地图没有完全闭合，否则重复步骤s3.2；

21、s3.4:如果最终队列为空，但搜索像素的过程中没有碰到地图边界，表示占用像素形成了一个封闭的空间，说明此时的地图是封闭的，停止像素地图的更新并对地图进行保存。

22、优选地，所述步骤s4具体包括:

23、通过s3对s2中构建的初始二维平面像素地图检测其封闭后，接下将s2中构建的初始点云地图投影到栅格地图中，以此构建初始匹配栅格地图；覆盖栅格地图通过栅格击中或击不中来更新极大似然估计，从而达到动态更新点云地图效果；在得到叠加分类后的点云后，将每一帧点云作为先验数据输入到栅格地图中：

24、data＝{x1,t1,x2,t2,…,xn,tn}      (3)

25、data代表当前点云帧信息，xn,tn代表第n个点的坐标和位姿；

26、以生成符合当前帧和历史帧信息数据最大概率的栅格地图：

27、m*＝arg maxmp(m|data)        (4)

28、在初始点云数帧叠加过程中，以公式(5)同步更新栅格地图中每一个栅格的极大似然估计，同时更新公式(3)中先验点云数据，并且构建公式(4)中的最大栅格概率地图，构建完毕后，作为暴力匹配的初始地图；

29、

30、计算击中点和原点之间的所有栅格坐标，如公式(6)：

31、

32、其中，δx和δt代表击中点和原点之间的距离，xi代表击中点的x坐标，那么可以判断δd的正负性来确定栅格坐标y的具体取值，如公式(7):

33、

34、优选地，所述步骤s5具体包括:

35、s5.1:通过帧与帧之间的匹配，联立构建点线和点面的非线性约束方程组，通过lm方法求解此方程组，得到求解后的雷达位姿；并通过帧与地图的匹配，联立构建点线和点面的非线性约束方程组，再次用lm方法求解，得到优化后的运动估计；

36、s5.2:首先将基于绝对坐标系下的运动估计提取出基于二维平面栅格地图坐标系下的旋转量和位移量，即t＝(x,y,θ)；然后创建n个三维的匹配窗口，即ti＝(xi,yi,θi)，通过位姿叠加，获取匹配后点云，并将点云投影到二维栅格上，与6)中构建的初始栅格做得分计算，若点落入占用栅格内，得分增加，若点落入空闲栅格，得分减少，如公式(8):

37、

38、其中m(ti)代表在窗口ti匹配下的栅格得分，f(t)是从所有窗口叠加中最高的得分匹配结果；

39、s5.3:根据公式(8)挑选出f(t)的平移量旋转量，评估最高的得分，若大于所设定的阈值，则认为当前窗口匹配结果符合要求，并且该关键帧的位姿计算符合算法要求，将此时窗口ti＝(xi,yi,θi)叠加到前端匹配优化计算出来的位姿中，允许该关键帧进入到历史点云信息中去。

40、本发明提出了一种基于栅格地图的暴力匹配回环检测方法，通过检测关键帧的位姿计算准确性，来为后端回环检测提供关键帧检测办法；

41、目前很多方法是利用到了计算的位姿在全局地图上的分配情况来确定关键帧，或者实现基于欧式距离的回环检测，所以一旦出现极大运动，出现错误位姿计算结果，会导致整个回环检测存在一定误差，并在之后的匹配中无法去除，导致整个定位失败。

42、本发明用优化位姿和激光信息来进行基于栅格地图的暴力匹配，得到对累计误差的回环检测优化，通过筛选关键帧和叠加窗口位姿来保证定位系统的精确性。

43、而本发明基于位姿匹配后的点云信息，通过点云信息投影栅格得分来评估该位姿是否存在误差，确定关键帧的入栈依据，来保证整个回环检测关键帧的准确性，从而避免优化结果陷入局部最小值，提高前后端优化计算的准确性。

44、本发明的优点是：能保证整个回环检测关键帧的准确性，得到对累计误差的回环检测优化，通过筛选关键帧和叠加窗口位姿来保证定位系统的精确性。