【技术实现步骤摘要】
基于二维栅格地图的回环检测方法
本专利技术属于SLAM领域，特别是一种基于二维栅格地图的回环检测方法。
技术介绍
回环检测技术的目的是构建一个完整的地图，该技术广泛应用于SLAM的地图构建中。随着传感器的技术进步，激光雷达的激光点数量增加精度提高，对于构建场景地图提供了很大的帮助。如何解决构建地图中的回环问题是SLAM技术的难题。建图中大量子图带来的误差，通过优化所有扫描和子图的位姿来减小误差，一般通过计算当前扫描在子图中的最大概率来进行优化。目前大部分回环优化的计算方法，存在的主要问题是当前扫描匹配子图的最大概率计算采用暴力搜索方法，没有算出搜索空间，这样使得回环时计算量大，移动设备往往硬件设施简单，不能做出复杂的计算，所有建图不能保证实时性。
技术实现思路
本专利技术的目的在于提供一种基于二维栅格地图的回环检测方法，计算量小、实时性高。实现本专利技术目的的技术解决方案为：一种基于二维栅格地图的回环检测方法，包括以下步骤：扫描提取特征点：激光雷达每帧产生点云信息，将每帧的雷达数据...
【技术保护点】
1.一种基于二维栅格地图的回环检测方法，其特征在于，包括以下步骤：/n扫描提取特征点：激光雷达每帧产生点云信息，将每帧的雷达数据作为一次扫描，选取扫描点计算曲率，以曲率为约束条件选出边缘和平面上的特征点；/n子图构建与匹配优化：将得到的扫描坐标转换为子图坐标，利用连续多次扫描点占据栅格的概率形成栅格地图，在扫面点插入到子图前根据定义公式进行优化；/n回环优化：创建子图，每个子图相连接来实现大场景的建图；优化所有特征点和子图的位姿，消除子图带来的累积误差。/n
【技术特征摘要】
1.一种基于二维栅格地图的回环检测方法，其特征在于，包括以下步骤：
扫描提取特征点：激光雷达每帧产生点云信息，将每帧的雷达数据作为一次扫描，选取扫描点计算曲率，以曲率为约束条件选出边缘和平面上的特征点；
子图构建与匹配优化：将得到的扫描坐标转换为子图坐标，利用连续多次扫描点占据栅格的概率形成栅格地图，在扫面点插入到子图前根据定义公式进行优化；
回环优化：创建子图，每个子图相连接来实现大场景的建图；优化所有特征点和子图的位姿，消除子图带来的累积误差。
2.根据权利要求1所述的基于二维栅格地图的回环检测方法，其特征在于，扫描提取特征点的具体步骤包括：
(11)曲率计算：假设开始的扫描为空表示为然后输入雷达信息写入扫描点信息每条扫描的边缘5个点不参与特征点选取；对任意点A，选取左边五个点右边五个点共十个点；每个点的x坐标与点A的x坐标求差，将所有的差求和，得到sx；每个点的y坐标与点A的y坐标求差，将所有的差求和，得到sy；根据公式(1)求得曲率：
c＝sx2+sy2(1)
(12)特征点提取：将一条激光线平均分为6块，将块内的点按曲率大小排列，设置一个曲率阈值t，用于区分边缘点和平面点，设定一个每块的最大点数N；
边缘点选择条件：从曲率最大的点开始，最多选择N个，只有曲率大于t的点才能被选取；若一个点周围五个点中已有点被选为边缘点，跳过这个点，从曲率更小的点中选取；
平面点选择条件：从曲率最小的点开始，最多选择N个，只有曲率小于t的点才能选取；若一个点周围五个点中已有点被评选为平面点，跳过这个点，从曲率更大的点中选取；
将挑选出的特征点信息表示为
3.根据权利要求2所述的基于二维栅格地图的回环检测方法，其特征在于，所述子图构建与匹配优化的具体步骤包括：
(21)扫描点坐标到子图坐标的转换：假设ξ＝(ξx,ξy,ξθ)作为雷达观测位姿包含x、y坐标和雷达的旋转角；子图构造是重复对齐扫描和子图坐标帧的迭代过程，扫描点信息扫描坐标中ξ的位姿在子图坐标系下被表示为Tξ，扫描点从扫描坐标到子图坐标转换的公式为；
式(2)中Rξ表示雷达观测位姿的旋转角，tξ表示雷达观测位姿的平移坐标，p表示扫描点到子图中的概率；
(22)栅格概率计算：一些连续的扫描构造为一个子图，子图采用概率网格的形式M:从离散网格点以给定的分辨率r映射到值；每当将扫描插入网格时，均会计算命中的一组网格点和未命中的不相交集；如果每个先前未观察到的网格点在这些集合中，则被分配概率phit或pmiss，phit、pmiss分别为命中概率和未命中概率；如果已经观察到网格点x，将命中和未命中的概率根据公式(3)(4)进行更新：
Mnew(x)＝clamp(odds-1(odds(Mold(x)...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。