CN107607107B - 一种基于先验信息的Slam方法和装置 - Google Patents
一种基于先验信息的Slam方法和装置 Download PDFInfo
- Publication number
- CN107607107B CN107607107B CN201710826714.1A CN201710826714A CN107607107B CN 107607107 B CN107607107 B CN 107607107B CN 201710826714 A CN201710826714 A CN 201710826714A CN 107607107 B CN107607107 B CN 107607107B
- Authority
- CN
- China
- Prior art keywords
- map
- features
- observation
- constraints
- prior information
- 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
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000005457 optimization Methods 0.000 claims abstract description 30
- 239000010410 layer Substances 0.000 claims description 58
- 238000000605 extraction Methods 0.000 claims description 10
- 238000009826 distribution Methods 0.000 claims description 7
- 238000005520 cutting process Methods 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 6
- 230000000717 retained effect Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 239000000284 extract Substances 0.000 claims description 3
- 230000006872 improvement Effects 0.000 claims description 3
- 239000011229 interlayer Substances 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 238000000844 transformation Methods 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 abstract description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 240000009164 Petroselinum crispum Species 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 235000011197 perejil Nutrition 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种基于先验信息的Slam方法,所述方法包括:添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;利用观测、先验信息和关键特征扩建地图;在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。先验信息通过可扩展的特征层次引入,因此本发明能够适应不同的环境,不同类型的先验信息;除了结构特征,本发明利用了关键特征,关键特征间额外的约束进一步约束了环境的框架和尺寸,减少了累计误差。
Description
技术领域
本发明涉及机器人定位领域,尤其涉及一种基于先验信息的Slam方法和装置。
背景技术
为了使得移动机器人能够在复杂环境中有效地执行任务,比如搬运,援救,打扫等,机器人需要具有自主定位的能力。GPS在室内等场景中无法使用或者精度不能满足需求;里程计也具有累计误差,因此传统的单一的传感器无法确定位姿。基于已知地图进行定位被认为已经很好地解决,但是先验地图一般不被提供,因此,如何在机器人位姿无法确定的情况下认知环境,构建高精度的地图,对移动机器人的导航具有重要意义,该问题是近几十年来机器人领域的热点,被称为SLAM。基于EKF或者粒子滤波的SLAM能够在线地同步定位和建图,基于地图的SLAM需要根据所有历史观测离线地确定完整路径和地图。对于大尺度和多环的复杂环境,基于滤波的在线SLAM由于其累计误差,得到的地图全局不一致,表现为有重影和不能保持形状,而基于地图的SLAM,由于利用了足够多的信息并且离线计算,构建的地图精度更高,近些年在移动机器人领域和计算机视觉领域变得流行。
Graph SLAM寻求一个路径和地图使得全局的误差最小,然而不准确的数据关联和回环检测,缺乏特征的环境等因素都会在Graph中添加错误的约束使得这个优化问题的解出错。事实上,许多环境中的确存在着人工设计的结构,比如矩形,圆等,将这些先验信息引入到SLAM中,能很大程度提高SLAM的精度和鲁棒性。传统的Graph SLAM中的约束有位姿之间的约束和位姿与特征之间的约束,为了简化地图的优化,流行的做法是将所有约束转化到位姿之间的约束,最后根据优化得到的位姿重建地图。这种做法的主要缺点是没有考虑到特征与特征之间的约束,环境中已知的或者潜在的结构信息都无法得到利用。Trevor等(2010)在square root SAM框架下提出通过“虚拟观测”在特征之间添加了约束,Parsley和Julier(2010)在引入先验信息时考虑了结构元素间的层次,但他们的实现是基于EKFSLAM,Kummerle等人(2011)通过在Graph SLAM中引入先验信息得到了户外场景下的全局一致地图,Puente等人(2012)使用EM使得算法能够适应各种不同的先验信息。
发明内容
基于此,本发明提供了一种基于先验信息的Slam方法。
一种基于先验信息的Slam方法,所述方法包括:
添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
利用观测、先验信息和关键特征扩建地图;
在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
在其中一个实施例中,所述利用观测、先验信息和关键特征扩建地图的步骤包括:
利用观测扩建地图:对每帧雷达观测,与前一帧观测进行Scan Match,Scan Match对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
利用先验信息扩建地图:当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
利用关键特征扩建地图:当雷达帧数是K2的倍数,对地图进行优化和裁剪;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中。
在其中一个实施例中,所述方法还包括:
根据先验信息,对环境中存在的结构特征设计了层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;
提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
在其中一个实施例中,所述方法还包括:
地图中特征之间的约束按以下规则添加:
当i是特征层次中的最后一层时,在该层相邻特征间添加同层约束Di。
在其中一个实施例中,所述方法还包括:
约束的权重主要取决于设定的系数ai,ai的值随层数i递增,即在每一次地图优化中,权重不是全局均匀分布的,是逐层增大的。
在其中一个实施例中,所述方法还包括:
Scan Match基于一种ICP的改进方法进行,Scan Match寻求一个旋转R和位移t以最小化关于两个匹配点集的损失函数;
该过程迭代寻求两帧观测的匹配关系和相对位姿;
对于确定匹配关系,在ICP基于最近距离的准则上加上了方向限制,角度误差不能超过阈值,使得各个点变换的方向具有一致性。
在其中一个实施例中,所述关键特征包括:长度大于设定阈值的线段及其朝向,位于拐角处的线段及其朝向,圆心拟合的直线及其朝向。
一种基于先验信息的Slam装置,所述装置包括:
添加模块,用于添加初始位姿节点,对初始雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
扩建模块,用于利用观测、先验信息和关键特征扩建地图;
优化模块,用于在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
构建模块,用于根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
在其中一个实施例中,所述扩建模块包括:
第一扩建单元,用于利用观测扩建地图,对每帧雷达观测,与前一帧观测进行ScanMatch,Scan Match对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
第二扩建单元,用于利用先验信息扩建地图,当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
第三扩建单元,用于利用关键特征扩建地图,当雷达帧数是K2的倍数,对地图进行优化和裁剪;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中。
在其中一个实施例中,所述装置还包括:
层次设计单元,用于根据先验信息,对环境中存在的结构特征设计层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
提取单元,用于从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;
提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
有益效果:
本发明提供一种基于先验信息的Slam方法,所述方法包括:添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;利用观测、先验信息和关键特征扩建地图;在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。本发明的SLAM方法所构建的地图具有高精度,全局一致性的特征。先验信息通过可扩展的特征层次引入,因此本发明能够适应不同的环境,不同类型的先验信息;除了结构特征,本发明利用了关键特征,关键特征间额外的约束进一步约束了环境的框架和尺寸,减少了累计误差。
附图说明
图1是本发明的一种基于先验信息的Slam方法流程图。
图2是本发明的一种基于先验信息的Slam装置的装置框图。
具体实施方式
为使本领域的普通技术人员更加清楚地理解本发明所要解决的技术问题、技术方案和有益技术效果,以下结合附图和实施例对本发明做进一步的阐述。
请参照图1,一种基于先验信息的Slam方法,所述方法包括:
S100:添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
对于线段和圆弧特征的提取,首先需要根据标定模型(比如多项式)对雷达观测数据进行修正,如果是使用时间较久的雷达还需要对数据降噪。雷达对于平整表面或者具有曲率的表面或者具有拐角的表面的扫描结果是连续的点列,分别表现为线段,曲线段,拐角;如果扫描方向上没有物体会出现撕裂点(列),如果物体表面轮廓错位会出现断点,为了检测出观测中的线段和圆弧特征,需要过滤掉撕裂点和断点,撕裂点可以通过距离识别,断点可以通过自适应阈值检测法识别,相邻扫描点距离差超过阈值认为出现轮廓错位。对过滤后得到的连续点列段基于曲率进行特征分割,即计算连续点列段在各个点处的局部曲率,曲率平均值接近0认为是线段,曲率接近一常数认为是圆弧。不检测圆而检测圆弧的原因是在远距离观测大半径的圆时,观测到的圆弧难以拟合出一致而准确的圆。
S200:利用观测、先验信息和关键特征扩建地图;
需要说明的是,对于相邻两帧观测间的Scan Match,常见的做法是ICP,ICP寻求目标点集{pi}到参考点集{qi}之间的一个旋转R和平移T使得两组数据按照以下准则最优匹配:
每次迭代后,将目标点集按照当前优化结果变换到参考点集坐标系下,根据距离最近的准则重新确定两个点集之间的对应关系,进行下一次迭代直至收敛。ICP一个缺点是寻找到的点对具有不一致的变换方向,为此本发明采取以下规则,对变换后的pi寻求点qi使得同时满足:
|q'-q|£Bw min||qi-(Rpi+T)||
即方向的差距小于阈值的情况下距离最近的点为对应点。由于雷达观测是稀疏采样的,为了使两帧观测能够有足够多的点相互对应,在Scan Match前需要先对观测进行插值;由于观测的视角变换,相邻帧的观测中存在没有匹配点的野值,在每次迭代前都可以检测并移除野值以提高Scan Match精度。
对于观测中的特征和地图中的特征的数据关联,用最大似然估计,寻求一个特征之间的对应关系ct使得观测出现的可能性最大,
ct=argm ax p(zt|c1:t,m,z1:t-1,u1:t)
其中zt是当前观测中的特征,m是地图中的特征,该方法假设过去计算的关联c1:t-1是正确的,这使得算法能够递增地计算对应关系,构建地图SLAM的前端,但是误差可能会逐渐发散,因此在后续的步骤中每次优化都会对应移除不合适的约束和特征,来减少特征提取,数据关联等带来的误差。
之前的步骤主要是传统地图SLAM的前端构建,只利用了观测和控制输入来构建状态量(即位姿节点和特征节点)之间的关系,以下详细推导如何将先验的结构信息融入到现在的状态量估计算法中。假设状态量x,观测z,环境的结构s,用贝叶斯定理为状态推断提供框架:
为了简化求和,假设p(s|z)和p(x|z)的分布是一个尖峰,可以被delta函数取代,从而得到EM的近似:
x*=argm ax{p(x|z)}>>argm ax{p(x|z,s*)p(s*|z)}
>>argm ax{p(x|z,s*)}
s*=argm ax{p(s|z)}>>argm ax{p(s|x*)p(x*|z)}
>>argm ax{p(s|x*}
其中s*和x*是p(s|z)和p(x|z)的尖峰所在处的值。两个式子交替迭代,首先优化地图得到当前的x*,再提取出地图中最可能的结构特征s*,有了新的特征添加到地图里又能再次优化更新x*,不断循环,得到最可能的s*后,假设以状态为条件,观测和结构独立,则:
p(x|z,s*)>>p(x|z)p(x|s*)
通过联合地最小化观测约束和结构约束的误差可以得到对状态的估计:
基于先验信息构建地图:根据上述推导,每当经过K1帧后,对构建的地图进行优化,优化是基于G2O进行,优化之后裁剪掉剩余量低于阈值的约束和孤立的节点,在当前的x*基础上从地图中的特征中提取s*。根据先验信息,事先对环境中的结构特征设计了层次,如附图一所示,此时对当前地图中已有的特征按照该特征层次提取出一层更高的特征,即对已有的线段特征提取朝向特征,对已有的圆弧特征提取圆心特征,对已有的圆心(点)特征提取直线特征,对已有的直线特征提取朝向特征,添加足够准确的特征到地图中,并标记出关键特征。在每层特征和对应高一层特征之间添加约束和权重,如果该层特征在特征层次中已是顶级,则在该层内部相邻特征间添加约束和权重。约束由先验信息给出,圆弧和圆心的约束为半径信息,圆心与直线的约束是已知圆心共线排列,线(段)的朝向之间约束是已知夹角主要有0度和90度,这些约束的权重由拟合高层特征时的协方差和实际约束与先验约束的误差共同决定:假设第i层第k个特征参与拟合第i+1层第l个特征拟合的协方差记为根据先验信息,到的距离应该是Di,这两个特征目前的值分别记为和距离为得到误差为则两个特征之间的约束为Di,约束的权重为ai(xQkl i)-1,其中ai为一系数,只和层数i有关;其中与k,l相关,区别了第i到i+1层间各个约束,而ai区别了不同层次的约束,每一次的优化中最高层的约束权重最大。
利用关键特征扩建地图:在上一步后,如果当前雷达帧数是K2的倍数(K2为一预设常数),同理对地图进行优化和裁剪。对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中,由先验信息决定,权重由实际约束与先验约束的误差决定。
S300:在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
S400:根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
需要说明的是,根据最终得到移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。其中栅格地图按照以下贝叶斯滤波器逐步构建:
l(mi|z1:t,x1:t)=l(mi|zt,xt)+l(mi|z1:t-1,x1:t-1)-l(mi)
其中,
在其中一个实施例中,所述利用观测、先验信息和关键特征扩建地图的步骤包括:
利用观测扩建地图:对每帧雷达观测,与前一帧观测进行Scan Match,Scan Match对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
利用先验信息扩建地图:当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
利用关键特征扩建地图:当雷达帧数是K2的倍数,对地图进行优化和裁剪;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中。
在其中一个实施例中,所述方法还包括:
根据先验信息,对环境中存在的结构特征设计了层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;
提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
在其中一个实施例中,所述方法还包括:
地图中特征之间的约束按以下规则添加:
当i是特征层次中的最后一层时,在该层相邻特征间添加同层约束Di。
在其中一个实施例中,所述方法还包括:
约束的权重主要取决于设定的系数ai,ai的值随层数i递增,即在每一次地图优化中,权重不是全局均匀分布的,是逐层增大的。
在其中一个实施例中,所述方法还包括:
Scan Match基于一种ICP的改进方法进行,Scan Match寻求一个旋转R和位移t以最小化关于两个匹配点集的损失函数;
该过程迭代寻求两帧观测的匹配关系和相对位姿;
对于确定匹配关系,在ICP基于最近距离的准则上加上了方向限制,角度误差不能超过阈值,使得各个点变换的方向具有一致性。
在其中一个实施例中,所述关键特征包括:长度大于设定阈值的线段及其朝向,位于拐角处的线段及其朝向,圆心拟合的直线及其朝向。
本发明提供一种基于先验信息的Slam方法,所述方法包括:添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;利用观测、先验信息和关键特征扩建地图;在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。本发明的SLAM方法所构建的地图具有高精度,全局一致性的特征。先验信息通过可扩展的特征层次引入,因此本发明能够适应不同的环境,不同类型的先验信息;除了结构特征,本发明利用了关键特征,关键特征间额外的约束进一步约束了环境的框架和尺寸,减少了累计误差。
请参照图2,一种基于先验信息的Slam装置,所述装置包括:
添加模块100,用于添加初始位姿节点,对初始雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
扩建模块200,用于利用观测、先验信息和关键特征扩建地图;
优化模块300,用于在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
在其中一个实施例中,所述扩建模块包括:
第一扩建单元,用于利用观测扩建地图,对每帧雷达观测,与前一帧观测进行ScanMatch,Scan Match对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
第二扩建单元,用于利用先验信息扩建地图,当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
第三扩建单元,用于利用关键特征扩建地图,当雷达帧数是K2的倍数,对地图进行优化和裁剪;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中。
在其中一个实施例中,所述装置还包括:
层次设计单元,用于根据先验信息,对环境中存在的结构特征设计层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
提取单元,用于从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;
提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
以上所述仅为本发明的优选实施例,而非对本发明做任何形式上的限制。本领域的技术人员可在上述实施例的基础上施以各种等同的更改和改进,凡在权利要求范围内所做的等同变化或修饰,均应落入本发明的保护范围之内。
Claims (8)
1.一种基于先验信息的Slam方法,其特征在于,所述方法包括:
添加初始位姿节点,对初始的雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
利用观测扩建地图:对每帧雷达观测,与前一帧观测进行Scan Match,Scan Match对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
利用先验信息扩建地图:当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
利用关键特征扩建地图:当雷达帧数是K2的倍数,对地图进行优化和裁剪,其中,K2为一预设常数;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中;
在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
2.根据权利要求1所述的方法,其特征在于,所述方法还包括:
根据先验信息,对环境中存在的结构特征设计了层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
4.根据权利要求1所述的方法,其特征在于:所述方法还包括:
约束的权重主要取决于设定的系数ai,ai的值随层数i递增,即在每一次地图优化中,权重不是全局均匀分布的,是逐层增大的。
5.根据权利要求1所述的方法,其特征在于:所述方法还包括:
Scan Match基于一种ICP的改进方法进行,Scan Match寻求一个旋转R和位移t以最小化关于两个匹配点集的损失函数;
该过程迭代寻求两帧观测的匹配关系和相对位姿;
对于确定匹配关系,在ICP基于最近距离的准则上加上了方向限制,角度误差不能超过阈值,使得各个点变换的方向具有一致性。
6.根据权利要求1所述的方法,其特征在于:所述关键特征包括:长度大于设定阈值的线段及其朝向,位于拐角处的线段及其朝向,圆心拟合的直线及其朝向。
7.一种基于先验信息的Slam装置,其特征在于,所述装置包括:
添加模块,用于添加初始位姿节点,对初始雷达观测提取初级特征,并标记关键特征,向地图中添加特征节点,计算特征节点与初始位姿节点的约束及其权重;
第一扩建单元,用于利用观测扩建地图,对每帧雷达观测,与前一帧观测进行ScanMatch,ScanMatch对这帧雷达观测提取初级特征,和地图中已有的特征进行关联,并将没被观测过的新特征作为节点添加到地图中,添加当前位姿节点和对应特征节点之间的约束和权重;
第二扩建单元,用于利用先验信息扩建地图,当雷达帧数是K1倍数,优化地图,并根据一致性校验移除部分约束和节点;对当前地图中已有的特征提取出更高一层的特征,添加到地图中,标记出关键特征,并根据先验信息添加特征之间的约束和权重;
第三扩建单元,用于利用关键特征扩建地图,当雷达帧数是K2的倍数,对地图进行优化和裁剪,其中,K2为一预设常数;对当前地图中已标记为关键特征的节点间添加约束及权重,约束只建立在同类型关键特征中;
优化模块,用于在最后一帧雷达观测信息被添加到地图后,进行全局优化,得到路径和特征地图;
构建模块,用于根据最终优化得到的移动机器人路径,和每个位姿对应的观测,基于贝叶斯滤波器构建环境的栅格地图。
8.根据权利要求7所述的装置,其特征在于,所述装置还包括:
层次设计单元,用于根据先验信息,对环境中存在的结构特征设计层次,初级特征为线段和圆弧,线段的上一层为朝向特征,圆弧的上一层为圆心,圆心的上一层为直线,直线的上一层为朝向,该结构层次可以扩展,以适应不同环境中的先验信息;
提取单元,用于从雷达观测数据提取初级特征,对雷达观测数据进行预处理后,检测得到的连续点列段局部曲率,根据曲率分布确定线段和圆弧;
从圆弧提取圆心,从圆心提取直线,线到朝向,都是基于最小二乘实现;
提取特征的顺序始终按照该层次从低到高进行,只有当特征提取的方差低于阈值特征才被保留。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710826714.1A CN107607107B (zh) | 2017-09-14 | 2017-09-14 | 一种基于先验信息的Slam方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710826714.1A CN107607107B (zh) | 2017-09-14 | 2017-09-14 | 一种基于先验信息的Slam方法和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107607107A CN107607107A (zh) | 2018-01-19 |
CN107607107B true CN107607107B (zh) | 2020-07-03 |
Family
ID=61063491
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710826714.1A Active CN107607107B (zh) | 2017-09-14 | 2017-09-14 | 一种基于先验信息的Slam方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107607107B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108680185B (zh) * | 2018-04-26 | 2020-09-22 | 广东宝乐机器人股份有限公司 | 移动机器人的陀螺仪数据校正方法、装置和设备 |
CN109146966B (zh) * | 2018-06-26 | 2021-02-23 | 广州视源电子科技股份有限公司 | 视觉slam前端处理方法、***、存储介质及计算机设备 |
CN109345578B (zh) * | 2018-10-15 | 2021-05-11 | 深圳一步智造科技有限公司 | 基于贝叶斯优化的点云配准方法、***及可读存储介质 |
CN112424568A (zh) * | 2018-12-04 | 2021-02-26 | 北京嘀嘀无限科技发展有限公司 | 构建高清地图的***和方法 |
CN109739226A (zh) * | 2018-12-27 | 2019-05-10 | 国网北京市电力公司 | 目标巡检位置的确定方法及装置 |
CN109752019A (zh) * | 2019-02-26 | 2019-05-14 | 西安工程大学 | 基于贝叶斯网络的最佳交通路径规划方法 |
CN110333513B (zh) * | 2019-07-10 | 2023-01-10 | 国网四川省电力公司电力科学研究院 | 一种融合最小二乘法的粒子滤波slam方法 |
CN110515054B (zh) * | 2019-08-23 | 2021-07-23 | 斯坦德机器人(深圳)有限公司 | 滤波方法和装置、电子设备、计算机存储介质 |
DE112019007466T5 (de) * | 2019-09-06 | 2022-03-31 | Robert Bosch Gmbh | Verfahren und Vorrichtung zur Modellierung einer dreidimensionalen Umgebung eines industriellen Roboters, Computerspeichermedium sowie Arbeitsplattform für industriellen Roboter |
CN114088086B (zh) * | 2021-11-23 | 2023-11-24 | 江苏科技大学 | 一种抗测量野值干扰的多目标鲁棒定位方法 |
CN114295138B (zh) * | 2021-12-31 | 2024-01-12 | 深圳市普渡科技有限公司 | 机器人、地图扩建方法、装置和可读存储介质 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107063258A (zh) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | 一种基于语义信息的移动机器人室内导航方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102402225B (zh) * | 2011-11-23 | 2013-09-04 | 中国科学院自动化研究所 | 一种实现移动机器人同时定位与地图构建的方法 |
CN103247225B (zh) * | 2012-02-13 | 2015-04-29 | 联想(北京)有限公司 | 即时定位与地图构建方法和设备 |
CN103278170B (zh) * | 2013-05-16 | 2016-01-06 | 东南大学 | 基于显著场景点检测的移动机器人级联地图创建方法 |
CN104019813B (zh) * | 2014-06-19 | 2017-01-25 | 无锡知谷网络科技有限公司 | 目标即时定位和构建地图的方法与*** |
-
2017
- 2017-09-14 CN CN201710826714.1A patent/CN107607107B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107063258A (zh) * | 2017-03-07 | 2017-08-18 | 重庆邮电大学 | 一种基于语义信息的移动机器人室内导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN107607107A (zh) | 2018-01-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107607107B (zh) | 一种基于先验信息的Slam方法和装置 | |
CN114862949B (zh) | 一种基于点线面特征的结构化场景视觉slam方法 | |
WO2021232470A1 (zh) | 基于多传感器融合的slam制图方法、*** | |
WO2021035669A1 (zh) | 位姿预测方法、地图构建方法、可移动平台及存储介质 | |
Lacroix et al. | Rover self localization in planetary-like environments | |
CN104881029B (zh) | 基于一点ransac和fast算法的移动机器人导航方法 | |
CN112752028B (zh) | 移动平台的位姿确定方法、装置、设备和存储介质 | |
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
CN111862219B (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
CN115880364A (zh) | 基于激光点云和视觉slam的机器人位姿估计方法 | |
CN112802096A (zh) | 实时定位和建图的实现装置和方法 | |
CN111862214A (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
CN110736456A (zh) | 稀疏环境下基于特征提取的二维激光实时定位方法 | |
Stachniss et al. | Analyzing gaussian proposal distributions for mapping with rao-blackwellized particle filters | |
CN112785705A (zh) | 一种位姿获取方法、装置及移动设备 | |
Zhang | LILO: A novel LiDAR–IMU SLAM system with loop optimization | |
Lu et al. | High level landmark-based visual navigation using unsupervised geometric constraints in local bundle adjustment | |
Keivan et al. | Constant-time monocular self-calibration | |
CN117253003A (zh) | 一种融合直接法与点面特征法的室内rgb-d slam方法 | |
CN116429087A (zh) | 一种适应于动态环境的视觉slam方法 | |
CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航*** | |
CN115761693A (zh) | 一种基于全景图像下车位标志点检测与车辆跟踪定位方法 | |
CN112767482B (zh) | 一种多传感器融合的室内外定位方法及*** | |
CN111583331B (zh) | 用于同时定位和地图构建的方法及装置 | |
Kim et al. | Mesh modelling of 3D point cloud from UAV images by point classification and geometric constraints |
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 |