基于二维栅格地图的回环检测方法与流程

本发明属于slam领域，特别是一种基于二维栅格地图的回环检测方法。

背景技术：

回环检测技术的目的是构建一个完整的地图，该技术广泛应用于slam的地图构建中。随着传感器的技术进步，激光雷达的激光点数量增加精度提高，对于构建场景地图提供了很大的帮助。如何解决构建地图中的回环问题是slam技术的难题。建图中大量子图带来的误差，通过优化所有扫描和子图的位姿来减小误差，一般通过计算当前扫描在子图中的最大概率来进行优化。

目前大部分回环优化的计算方法，存在的主要问题是当前扫描匹配子图的最大概率计算采用暴力搜索方法，没有算出搜索空间，这样使得回环时计算量大，移动设备往往硬件设施简单，不能做出复杂的计算，所有建图不能保证实时性。

技术实现要素：

本发明的目的在于提供一种基于二维栅格地图的回环检测方法，计算量小、实时性高。

实现本发明目的的技术解决方案为：一种基于二维栅格地图的回环检测方法，包括以下步骤：

扫描提取特征点：激光雷达每帧产生点云信息，将每帧的雷达数据作为一次扫描，选取扫描点计算曲率，以曲率为约束条件选出边缘和平面上的特征点；

子图构建与匹配优化：将得到的扫描坐标转换为子图坐标，利用连续多次扫描点占据栅格的概率形成栅格地图，在扫面点插入到子图前根据定义公式进行优化；

回环优化：创建子图，每个子图相连接来实现大场景的建图；优化所有特征点和子图的位姿，消除子图带来的累积误差。

本发明与现有技术相比，其显著优点为：本发明将每次扫描提取其特征点，再将扫描坐标转换为子图坐标，计算连续多次的扫面点占据栅格的概率形成栅格地图，在扫面点插入到子图前将其对应的特征点代入公式进行优化，在回环中利用特征点算出搜索空间和回环匹配概率，之后将特征点对应的扫描进行回环，这样避免了扫描点数量多和暴力搜索带来的计算量大和实时性差，做不到实时建图等问题。

附图说明

图1为本发明基于二维栅格地图的回环检测方法的主流程图。

图2为扫描提取特征点步骤流程图。

图3为扫描点形成子图步骤流程图。

图4为回环优化步骤流程图。

具体实施方式

如图1所示，本发明的一种基于二维栅格地图的回环检测方法，包括以下步骤：

(10)扫描提取特征点：激光雷达每帧产生点云信息，将每帧的雷达数据作为一次扫描，首先选取扫描点计算曲率，以曲率为约束条件选出边缘和平面上的特征点；

如图2所示，所述扫描提取特征点步骤包括：

(11)曲率计算：假设开始的扫描为空表示为然后输入雷达信息写入扫描点信息每条扫描的边缘5个点不参与特征点选取，因为周边不满足左右各五个点计算曲率的条件；对任意点a，选取左边五个点右边五个点共十个点；每个点的x坐标与点a的x坐标求差，将所有的差求和，得到sx；每个点的y坐标与点a的y坐标求差，将所有的差求和，得到sy；根据公式(1)求得曲率：

c＝sx²+sy²(1)

(12)特征点提取：计算完曲率，就可以根据曲率挑选特征点，为了使得在一周360度上有均匀的约束，将一条激光线平均分为6块，将块内的点按曲率大小排列，设置一个曲率阈值t，来区分边缘点和平面点，设定一个每块的最大点数n；

边缘点选择条件：(a)从曲率最大的点开始，最多选择n个，只有曲率大于t的点才能被选取；(b)若一个点周围五个点中已有点被选为边缘点，跳过这个点，从曲率更小的点中选取；

平面点选择条件：(a)从曲率最小的点开始，最多选择n个，只有曲率小于t的点才能选取；(b)若一个点周围五个点中已有点被评选为平面点，跳过这个点，从曲率更大的点中选取；

将挑选出的特征点信息表示为

(20)子图构建与匹配优化：将得到的扫描坐标转换为子图坐标，利用连续多次扫描点占据栅格的概率形成栅格地图，在扫面点插入到子图前根据定义公式进行优化；

如图3所示，所述扫描点形成子图步骤包括：

(21)扫描点坐标到子图坐标的转换：假设ξ＝(ξx,ξy,ξθ)作为雷达观测位姿包含x、y坐标和雷达的旋转角。子图构造是重复对齐扫描和子图坐标帧的迭代过程，扫描点信息扫描坐标中ξ的位姿在子图坐标系下被表示为tξ，扫描点从扫描坐标到子图坐标转换的公式为：

式(2)中rξ表示雷达观测位姿的旋转角，tξ表示雷达观测位姿的平移坐标，p表示扫描点到子图中的概率。

(22)栅格概率计算：一些连续的扫描构造为一个子图，子图采用概率网格的形式从离散网格点以给定的分辨率r映射到值。每当将扫描插入网格时，都会计算命中的一组网格点和未命中的不相交集。如果每个先前未观察到的网格点在这些集合中，则被分配概率phit或pmiss，phit、pmiss分别为命中概率和未命中概率；如果已经观察到网格点x，将命中和未命中的概率根据公式(3)(4)进行更新：

mnew(x)＝clamp(odds^-1(odds(mold(x))·odds(phit)))(4)

式(3)中，odds表示为发生比，在统计和概率论中，发生比为一个事件发生和不发生的比率。式(4)中m表示更新的网格的相应值，clamp函数表示将随机变化的数值限制在一个给定的区域内，返回的值介于两个数min、max之间，若值小于min返回min，若值大于max则返回max。

(23)扫描匹配优化：在将扫描插入子图之前，使用基于公式(5)的扫描匹配器相对于当前本地子图优化扫描位姿ξ。为了减少计算，扫描匹配器负责找到扫描点对应的特征点在子图的概率最大的位姿，找到概率最大的位姿后将特征点相应的扫描点插入子图中，这就是非线性最小二乘问题：

式(5)中ξ属于扫描位姿，其中tξ根据特征位姿gk将特征点坐标转换成子图坐标，msmooth函数完成了从局部子图中概率值的平滑形式，该函数采用的是双三次插值方法。

(30)回环优化：创建子图，每个子图相连接来实现大场景的建图。为了消除子图带来的累积误差，通过优化所有特征点和子图的位姿，来提高准确度。

如图4所示，所述回环优化步骤包括：

(31)回环优化计算：通过上述方法创建子图，每个子图相连接来实现大场景的建图。为了消除子图带来的累积误差，通过优化所有特征点和子图的位姿，来提高准确度。回环的优化问题与扫描匹配的优化问题类似，都是通过构造非线性最小二乘方式进行的。公式如下：

式(6)中表示子图的位姿，m为子图的数量，是子图的位姿的集合，表示特征点的位姿，s为特征点的数量，是特征点的位姿的集合，这些位姿都是在世界坐标系下的。子图位姿和特征点位姿之间存在约束条件，ξij表示特征点在子图坐标系下的位姿，∑ij是相应的协方差矩阵，可以通过公式(5)获得。ρ是一个损失函数，这个函数用于当扫描匹配器为了优化出现错误出现不正确的约束时，减小离群值的影响。

残差e的计算公式如下：

式(8)中和表示在同一个雷达角度下的子图位姿和特征点位姿；表示特征点位姿矩阵的转置，子图位姿同理；表示特征点位姿逆矩阵的秩。

(32)回环约束计算：式(6)中的回环约束关系ξij通过下面的像素精确匹配公式得到：

式(9)中ξ表示扫描位姿，w是搜索空间，mnearest是该像素对应的网格点的m值，tξ表示子图坐标，gk表示特征点位姿。其中w的计算方法有下列公式得到，将特征点代入下列公式：

式(10)中dmax表示特征点中最远的点到原点的距离，式(11)中r表示分辨率，将得到的角度步长δθ代入式(12)：

式(12)中wx、wy表示雷达水平范围的值，wθ表示扫描角度的值。假设wx＝wy＝7m、wθ＝30°，根据式(12)计算出线性和角度窗口大小，将得到的值代入式(13)：

式(14)中ξ0表示初始搜索框中心的特征点位姿，jx、jy和jθ表示搜索框中的任意水平和角度值。

由上面的式(13)和(14)可以求得搜索空间大小，缩小了搜索的范围，减少了计算时间，将得到的w值代入公式(9)求得约束关系，进而进行回环优化。

本发明将每次扫描提取其特征点，再将扫描坐标转换为子图坐标，计算连续多次的扫面点占据栅格的概率形成栅格地图，在扫面点插入到子图前将其对应的特征点代入公式进行优化，在回环中利用特征点算出搜索空间和回环匹配概率，之后将特征点对应的扫描进行回环，这样避免了扫描点数量多和暴力搜索带来的计算量大和实时性差，做不到实时建图的问题。