CN111540005B - 基于二维栅格地图的回环检测方法 - Google Patents
基于二维栅格地图的回环检测方法 Download PDFInfo
- Publication number
- CN111540005B CN111540005B CN202010317449.6A CN202010317449A CN111540005B CN 111540005 B CN111540005 B CN 111540005B CN 202010317449 A CN202010317449 A CN 202010317449A CN 111540005 B CN111540005 B CN 111540005B
- Authority
- CN
- China
- Prior art keywords
- points
- point
- scanning
- sub
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Geometry (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于二维栅格地图的回环检测方法,包括:激光雷达每帧产生点云信息,将每帧的雷达数据作为一次扫描,选取扫描点计算曲率,以曲率为约束条件选出边缘和平面上的特征点;将得到的扫描坐标转换为子图坐标,利用连续多次扫描点占据栅格的概率形成栅格地图,在扫面点插入到子图前根据定义公式进行优化;创建子图,每个子图相连接来实现大场景的建图;通过优化所有特征点和子图的位姿,消除子图带来的累积误差,提高准确度。本发明的二维栅格地图的回环检测方法计算量小,实时性高。
Description
技术领域
本发明属于SLAM领域,特别是一种基于二维栅格地图的回环检测方法。
背景技术
回环检测技术的目的是构建一个完整的地图,该技术广泛应用于SLAM的地图构建中。随着传感器的技术进步,激光雷达的激光点数量增加精度提高,对于构建场景地图提供了很大的帮助。如何解决构建地图中的回环问题是SLAM技术的难题。建图中大量子图带来的误差,通过优化所有扫描和子图的位姿来减小误差,一般通过计算当前扫描在子图中的最大概率来进行优化。
目前大部分回环优化的计算方法,存在的主要问题是当前扫描匹配子图的最大概率计算采用暴力搜索方法,没有算出搜索空间,这样使得回环时计算量大,移动设备往往硬件设施简单,不能做出复杂的计算,所有建图不能保证实时性。
发明内容
本发明的目的在于提供一种基于二维栅格地图的回环检测方法,计算量小、实时性高。
实现本发明目的的技术解决方案为:一种基于二维栅格地图的回环检测方法,包括以下步骤:
扫描提取特征点:激光雷达每帧产生点云信息,将每帧的雷达数据作为一次扫描,选取扫描点计算曲率,以曲率为约束条件选出边缘和平面上的特征点;
子图构建与匹配优化:将得到的扫描坐标转换为子图坐标,利用连续多次扫描点占据栅格的概率形成栅格地图,在扫面点插入到子图前根据定义公式进行优化;
回环优化:创建子图,每个子图相连接来实现大场景的建图;优化所有特征点和子图的位姿,消除子图带来的累积误差。
本发明与现有技术相比,其显著优点为:本发明将每次扫描提取其特征点,再将扫描坐标转换为子图坐标,计算连续多次的扫面点占据栅格的概率形成栅格地图,在扫面点插入到子图前将其对应的特征点代入公式进行优化,在回环中利用特征点算出搜索空间和回环匹配概率,之后将特征点对应的扫描进行回环,这样避免了扫描点数量多和暴力搜索带来的计算量大和实时性差,做不到实时建图等问题。
附图说明
图1为本发明基于二维栅格地图的回环检测方法的主流程图。
图2为扫描提取特征点步骤流程图。
图3为扫描点形成子图步骤流程图。
图4为回环优化步骤流程图。
具体实施方式
如图1所示,本发明的一种基于二维栅格地图的回环检测方法,包括以下步骤:
(10)扫描提取特征点:激光雷达每帧产生点云信息,将每帧的雷达数据作为一次扫描,首先选取扫描点计算曲率,以曲率为约束条件选出边缘和平面上的特征点;
如图2所示,所述扫描提取特征点步骤包括:
(11)曲率计算:假设开始的扫描为空表示为然后输入雷达信息写入扫描点信息每条扫描的边缘5个点不参与特征点选取,因为周边不满足左右各五个点计算曲率的条件;对任意点A,选取左边五个点右边五个点共十个点;每个点的x坐标与点A的x坐标求差,将所有的差求和,得到sx;每个点的y坐标与点A的y坐标求差,将所有的差求和,得到sy;根据公式(1)求得曲率:
c=sx2+sy2 (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)的扫描匹配器相对于当前本地子图优化扫描位姿ξ。为了减少计算,扫描匹配器负责找到扫描点对应的特征点在子图的概率最大的位姿,找到概率最大的位姿后将特征点相应的扫描点插入子图中,这就是非线性最小二乘问题:
(30)回环优化:创建子图,每个子图相连接来实现大场景的建图。为了消除子图带来的累积误差,通过优化所有特征点和子图的位姿,来提高准确度。
如图4所示,所述回环优化步骤包括:
(31)回环优化计算:通过上述方法创建子图,每个子图相连接来实现大场景的建图。为了消除子图带来的累积误差,通过优化所有特征点和子图的位姿,来提高准确度。回环的优化问题与扫描匹配的优化问题类似,都是通过构造非线性最小二乘方式进行的。公式如下:
式(6)中表示子图的位姿,m为子图的数量,是子图的位姿的集合,表示特征点的位姿,s为特征点的数量,是特征点的位姿的集合,这些位姿都是在世界坐标系下的。子图位姿和特征点位姿之间存在约束条件,ξij表示特征点在子图坐标系下的位姿,∑ij是相应的协方差矩阵,可以通过公式(5)获得。ρ是一个损失函数,这个函数用于当扫描匹配器为了优化出现错误出现不正确的约束时,减小离群值的影响。
残差E的计算公式如下:
(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)求得约束关系,进而进行回环优化。
本发明将每次扫描提取其特征点,再将扫描坐标转换为子图坐标,计算连续多次的扫面点占据栅格的概率形成栅格地图,在扫面点插入到子图前将其对应的特征点代入公式进行优化,在回环中利用特征点算出搜索空间和回环匹配概率,之后将特征点对应的扫描进行回环,这样避免了扫描点数量多和暴力搜索带来的计算量大和实时性差,做不到实时建图的问题。
Claims (3)
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的点才能选取;若一个点周围五个点中已有点被评选为平面点,跳过这个点,从曲率更大的点中选取;
子图构建与匹配优化:将得到的扫描坐标转换为子图坐标,利用连续多次扫描点占据栅格的概率形成栅格地图,在扫面点插入到子图前根据定义公式进行优化;
回环优化:创建子图,每个子图相连接来实现大场景的建图;优化所有特征点和子图的位姿,消除子图带来的累积误差。
2.根据权利要求1所述的基于二维栅格地图的回环检测方法,其特征在于,所述子图构建与匹配优化的具体步骤包括:
(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)的扫描匹配器相对于当前本地子图优化扫描位姿ξ;扫描匹配器找到扫描点对应的特征点在子图的概率最大的位姿,找到概率最大的位姿后将特征点相应的扫描点插入子图中,即非线性最小二乘问题:
3.根据权利要求2所述的基于二维栅格地图的回环检测方法,其特征在于,所述回环优化步骤包括:
(31)回环优化计算:创建子图,每个子图相连接来实现大场景的建图;通过优化所有特征点和子图的位姿,消除子图带来的累积误差;回环的优化问题是通过构造非线性最小二乘方式进行的,公式如下:
式(6)中表示子图的位姿,m为子图的数量,是子图的位姿的集合,表示特征点的位姿,s为特征点的数量,是特征点的位姿的集合,这些位姿均在世界坐标系下;子图位姿和特征点位姿之间存在约束条件;ξij表示特征点在子图坐标系下的位姿,∑ij为相应的协方差矩阵,通过公式(5)获得;ρ为损失函数;
残差E的计算公式如下:
(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)求得约束关系,进而进行回环优化。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010317449.6A CN111540005B (zh) | 2020-04-21 | 2020-04-21 | 基于二维栅格地图的回环检测方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010317449.6A CN111540005B (zh) | 2020-04-21 | 2020-04-21 | 基于二维栅格地图的回环检测方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111540005A CN111540005A (zh) | 2020-08-14 |
CN111540005B true CN111540005B (zh) | 2022-10-18 |
Family
ID=71977007
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010317449.6A Active CN111540005B (zh) | 2020-04-21 | 2020-04-21 | 基于二维栅格地图的回环检测方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111540005B (zh) |
Families Citing this family (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112180937A (zh) * | 2020-10-14 | 2021-01-05 | 中国安全生产科学研究院 | 一种地铁车厢消毒机器人及其自动导航方法 |
CN112668653A (zh) * | 2020-12-30 | 2021-04-16 | 北京百度网讯科技有限公司 | 基于激光雷达地图的回环检测方法、装置、设备及介质 |
CN113409410B (zh) * | 2021-05-19 | 2024-04-02 | 杭州电子科技大学 | 一种基于3d激光雷达的多特征融合igv定位与建图方法 |
CN113325435B (zh) * | 2021-05-25 | 2024-01-09 | 武汉大学 | 定位方法、装置、设备及可读存储介质 |
CN113375683A (zh) * | 2021-06-10 | 2021-09-10 | 亿嘉和科技股份有限公司 | 机器人环境地图实时更新方法 |
CN113587933B (zh) * | 2021-07-29 | 2024-02-02 | 山东山速机器人科技有限公司 | 一种基于分支定界算法的室内移动机器人定位方法 |
CN113538410B (zh) * | 2021-08-06 | 2022-05-20 | 广东工业大学 | 一种基于3d激光雷达和uwb的室内slam建图方法 |
CN114460933B (zh) * | 2021-12-30 | 2023-11-03 | 南京理工大学 | 一种面向动态环境的移动机器人局部路径规划算法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106595659A (zh) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | 城市复杂环境下多无人机视觉slam的地图融合方法 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110689622A (zh) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | 一种基于点云分割匹配闭环校正的同步定位与构图算法 |
CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
CN110907947A (zh) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | 一种移动机器人slam问题中的实时回环检测方法 |
-
2020
- 2020-04-21 CN CN202010317449.6A patent/CN111540005B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106595659A (zh) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | 城市复杂环境下多无人机视觉slam的地图融合方法 |
CN110261870A (zh) * | 2019-04-15 | 2019-09-20 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110689622A (zh) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | 一种基于点云分割匹配闭环校正的同步定位与构图算法 |
CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
CN110907947A (zh) * | 2019-12-04 | 2020-03-24 | 同济人工智能研究院(苏州)有限公司 | 一种移动机器人slam问题中的实时回环检测方法 |
Also Published As
Publication number | Publication date |
---|---|
CN111540005A (zh) | 2020-08-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111540005B (zh) | 基于二维栅格地图的回环检测方法 | |
CN110631554B (zh) | 机器人位姿的确定方法、装置、机器人和可读存储介质 | |
CN108171780A (zh) | 一种基于激光雷达构建室内真实三维地图的方法 | |
US11367195B2 (en) | Image segmentation method, image segmentation apparatus, image segmentation device | |
CN102800098B (zh) | 多特征多级别的可见光全色与多光谱高精度配准方法 | |
CN107862735B (zh) | 一种基于结构信息的rgbd三维场景重建方法 | |
CN106023298A (zh) | 基于局部泊松曲面重建的点云刚性配准方法 | |
CN108257155B (zh) | 一种基于局部和全局耦合的扩展目标稳定跟踪点提取方法 | |
CN104574401A (zh) | 一种基于平行线匹配的图像配准方法 | |
CN106096497B (zh) | 一种针对多元遥感数据的房屋矢量化方法 | |
CN104197933A (zh) | 一种望远镜视场中高星等滑动恒星的增强和提取方法 | |
CN115564926A (zh) | 基于影像建筑物结构学习的三维面片模型构建方法 | |
CN105004337A (zh) | 基于直线匹配的农用无人机自主导航方法 | |
CN113643365A (zh) | 一种相机位姿估计方法、装置、设备以及可读存储介质 | |
CN114202632A (zh) | 网格线性结构恢复方法、装置、电子设备及存储介质 | |
CN116086484A (zh) | 基于地平面约束和回环检测的激光雷达里程计算方法 | |
CN113409332B (zh) | 一种基于三维点云的建筑物平面分割方法 | |
CN114548606A (zh) | 一种气旋强度预测模型的构建方法及气旋强度预测方法 | |
CN113313765B (zh) | 定位方法、装置、电子设备和存储介质 | |
CN112002007A (zh) | 基于空地影像的模型获取方法及装置、设备、存储介质 | |
CN111192302A (zh) | 一种基于运动平滑性和ransac算法的特征匹配方法 | |
CN106228593A (zh) | 一种图像密集匹配方法 | |
Previtali et al. | Automated detection of repeated structures in building facades | |
CN108090961B (zh) | 一种三维激光点云成像中的快速平差方法 | |
CN112419172A (zh) | 倾斜图像校正去模糊的遥感图像处理方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |