CN107607107A - 一种基于先验信息的Slam方法和装置 - Google Patents

Abstract

本发明提供一种基于先验信息的Slam方法，所述方法包括：添加初始位姿节点，对初始的雷达观测提取初级特征，并标记关键特征，向地图中添加特征节点，计算特征节点与初始位姿节点的约束及其权重；利用观测、先验信息和关键特征扩建地图；在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。先验信息通过可扩展的特征层次引入，因此本发明能够适应不同的环境，不同类型的先验信息；除了结构特征，本发明利用了关键特征，关键特征间额外的约束进一步约束了环境的框架和尺寸,减少了累计误差。

Description

一种基于先验信息的Slam方法和装置

技术领域

本发明涉及机器人定位领域，尤其涉及一种基于先验信息的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层的某一特征参与拟合了第i+1层中某一特征根据先验信息添加和间的跨层约束

当i是特征层次中的最后一层时，在该层相邻特征间添加同层约束D_i。

在其中一个实施例中，所述方法还包括：

约束的权重主要取决于设定的系数a_i，a_i的值随层数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寻求目标点集{p_i}到参考点集{q_i}之间的一个旋转R和平移T使得两组数据按照以下准则最优匹配：

每次迭代后，将目标点集按照当前优化结果变换到参考点集坐标系下，根据距离最近的准则重新确定两个点集之间的对应关系，进行下一次迭代直至收敛。ICP一个缺点是寻找到的点对具有不一致的变换方向，为此本发明采取以下规则，对变换后的p_i寻求点q_i使得同时满足：

|q'-q|￡B_w min||q_i-(Rp_i+T)||

即方向的差距小于阈值的情况下距离最近的点为对应点。由于雷达观测是稀疏采样的，为了使两帧观测能够有足够多的点相互对应，在Scan Match前需要先对观测进行插值；由于观测的视角变换，相邻帧的观测中存在没有匹配点的野值，在每次迭代前都可以检测并移除野值以提高Scan Match精度。

对于观测中的特征和地图中的特征的数据关联，用最大似然估计，寻求一个特征之间的对应关系c_t使得观测出现的可能性最大，

c_t＝argm ax p(z_t|c_1:t,m,z_1:t-1,u_1:t)

其中z_t是当前观测中的特征，m是地图中的特征，该方法假设过去计算的关联c_1: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个特征拟合的协方差记为根据先验信息，到的距离应该是D_i，这两个特征目前的值分别记为和距离为得到误差为则两个特征之间的约束为D_i，约束的权重为a_i(xQ^kl _i)^-1,其中a_i为一系数，只和层数i有关；其中与k,l相关，区别了第i到i+1层间各个约束，而a_i区别了不同层次的约束，每一次的优化中最高层的约束权重最大。

利用关键特征扩建地图：在上一步后，如果当前雷达帧数是K2的倍数(K2为一预设常数)，同理对地图进行优化和裁剪。对当前地图中已标记为关键特征的节点间添加约束及权重，约束只建立在同类型关键特征中，由先验信息决定，权重由实际约束与先验约束的误差决定。

S300：在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；

S400：根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。

需要说明的是，根据最终得到移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。其中栅格地图按照以下贝叶斯滤波器逐步构建：

l(m_i|z_1:t,x_1:t)＝l(m_i|z_t,x_t)+l(m_i|z_1:t-1,x_1:t-1)-l(m_i)

其中，

在其中一个实施例中，所述利用观测、先验信息和关键特征扩建地图的步骤包括：

利用观测扩建地图：对每帧雷达观测，与前一帧观测进行Scan Match，Scan Match对这帧雷达观测提取初级特征，和地图中已有的特征进行关联，并将没被观测过的新特征作为节点添加到地图中，添加当前位姿节点和对应特征节点之间的约束和权重；

利用先验信息扩建地图：当雷达帧数是K1倍数，优化地图，并根据一致性校验移除部分约束和节点；对当前地图中已有的特征提取出更高一层的特征，添加到地图中，标记出关键特征，并根据先验信息添加特征之间的约束和权重；

利用关键特征扩建地图：当雷达帧数是K2的倍数，对地图进行优化和裁剪；对当前地图中已标记为关键特征的节点间添加约束及权重，约束只建立在同类型关键特征中。

在其中一个实施例中，所述方法还包括：

根据先验信息，对环境中存在的结构特征设计了层次，初级特征为线段和圆弧，线段的上一层为朝向特征，圆弧的上一层为圆心，圆心的上一层为直线，直线的上一层为朝向，该结构层次可以扩展，以适应不同环境中的先验信息；

从雷达观测数据提取初级特征，对雷达观测数据进行预处理后，检测得到的连续点列段局部曲率，根据曲率分布确定线段和圆弧；

从圆弧提取圆心，从圆心提取直线，线到朝向，都是基于最小二乘实现；

提取特征的顺序始终按照该层次从低到高进行，只有当特征提取的方差低于阈值特征才被保留。

在其中一个实施例中，所述方法还包括：

地图中特征之间的约束按以下规则添加：

设第i层的某一特征参与拟合了第i+1层中某一特征根据先验信息添加和间的跨层约束

当i是特征层次中的最后一层时，在该层相邻特征间添加同层约束D_i。

在其中一个实施例中，所述方法还包括：

约束的权重主要取决于设定的系数a_i，a_i的值随层数i递增，即在每一次地图优化中，权重不是全局均匀分布的，是逐层增大的。

在其中一个实施例中，所述方法还包括：

Scan Match基于一种ICP的改进方法进行，Scan Match寻求一个旋转R和位移t以最小化关于两个匹配点集的损失函数；

该过程迭代寻求两帧观测的匹配关系和相对位姿；

对于确定匹配关系，在ICP基于最近距离的准则上加上了方向限制，角度误差不能超过阈值，使得各个点变换的方向具有一致性。

在其中一个实施例中，所述关键特征包括：长度大于设定阈值的线段及其朝向，位于拐角处的线段及其朝向，圆心拟合的直线及其朝向。

本发明提供一种基于先验信息的Slam方法，所述方法包括：添加初始位姿节点，对初始的雷达观测提取初级特征，并标记关键特征，向地图中添加特征节点，计算特征节点与初始位姿节点的约束及其权重；利用观测、先验信息和关键特征扩建地图；在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。本发明的SLAM方法所构建的地图具有高精度，全局一致性的特征。先验信息通过可扩展的特征层次引入，因此本发明能够适应不同的环境，不同类型的先验信息；除了结构特征，本发明利用了关键特征，关键特征间额外的约束进一步约束了环境的框架和尺寸,减少了累计误差。

请参照图2，一种基于先验信息的Slam装置，所述装置包括：

添加模块100，用于添加初始位姿节点，对初始雷达观测提取初级特征，并标记关键特征，向地图中添加特征节点，计算特征节点与初始位姿节点的约束及其权重；

扩建模块200，用于利用观测、先验信息和关键特征扩建地图；

优化模块300，用于在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；

根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。

在其中一个实施例中，所述扩建模块包括：

第一扩建单元，用于利用观测扩建地图，对每帧雷达观测，与前一帧观测进行ScanMatch，Scan Match对这帧雷达观测提取初级特征，和地图中已有的特征进行关联，并将没被观测过的新特征作为节点添加到地图中，添加当前位姿节点和对应特征节点之间的约束和权重；

第二扩建单元，用于利用先验信息扩建地图，当雷达帧数是K1倍数，优化地图，并根据一致性校验移除部分约束和节点；对当前地图中已有的特征提取出更高一层的特征，添加到地图中，标记出关键特征，并根据先验信息添加特征之间的约束和权重；

第三扩建单元，用于利用关键特征扩建地图，当雷达帧数是K2的倍数，对地图进行优化和裁剪；对当前地图中已标记为关键特征的节点间添加约束及权重，约束只建立在同类型关键特征中。

在其中一个实施例中，所述装置还包括：

层次设计单元，用于根据先验信息，对环境中存在的结构特征设计层次，初级特征为线段和圆弧，线段的上一层为朝向特征，圆弧的上一层为圆心，圆心的上一层为直线，直线的上一层为朝向，该结构层次可以扩展，以适应不同环境中的先验信息；

提取单元，用于从雷达观测数据提取初级特征，对雷达观测数据进行预处理后，检测得到的连续点列段局部曲率，根据曲率分布确定线段和圆弧；

从圆弧提取圆心，从圆心提取直线，线到朝向，都是基于最小二乘实现；

提取特征的顺序始终按照该层次从低到高进行，只有当特征提取的方差低于阈值特征才被保留。

以上所述仅为本发明的优选实施例，而非对本发明做任何形式上的限制。本领域的技术人员可在上述实施例的基础上施以各种等同的更改和改进，凡在权利要求范围内所做的等同变化或修饰，均应落入本发明的保护范围之内。

Claims (10)

1.一种基于先验信息的Slam方法，其特征在于，所述方法包括：

添加初始位姿节点，对初始的雷达观测提取初级特征，并标记关键特征，向地图中添加特征节点，计算特征节点与初始位姿节点的约束及其权重；

利用观测、先验信息和关键特征扩建地图；

在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；

根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。

2.根据权利要求1所述的方法，其特征在于，所述利用观测、先验信息和关键特征扩建地图的步骤包括：

利用观测扩建地图：对每帧雷达观测，与前一帧观测进行Scan Match，Scan Match对这帧雷达观测提取初级特征，和地图中已有的特征进行关联，并将没被观测过的新特征作为节点添加到地图中，添加当前位姿节点和对应特征节点之间的约束和权重；

利用先验信息扩建地图：当雷达帧数是K1倍数，优化地图，并根据一致性校验移除部分约束和节点；对当前地图中已有的特征提取出更高一层的特征，添加到地图中，标记出关键特征，并根据先验信息添加特征之间的约束和权重；

利用关键特征扩建地图：当雷达帧数是K2的倍数，对地图进行优化和裁剪；对当前地图中已标记为关键特征的节点间添加约束及权重，约束只建立在同类型关键特征中。

3.根据权利要求1所述的方法，其特征在于，所述方法还包括：

根据先验信息，对环境中存在的结构特征设计了层次，初级特征为线段和圆弧，线段的上一层为朝向特征，圆弧的上一层为圆心，圆心的上一层为直线，直线的上一层为朝向，该结构层次可以扩展，以适应不同环境中的先验信息；

从雷达观测数据提取初级特征，对雷达观测数据进行预处理后，检测得到的连续点列段局部曲率，根据曲率分布确定线段和圆弧；

从圆弧提取圆心，从圆心提取直线，线到朝向，都是基于最小二乘实现；

提取特征的顺序始终按照该层次从低到高进行，只有当特征提取的方差低于阈值特征才被保留。

4.根据权利要求2所述的方法，其特征在于：所述方法还包括：

地图中特征之间的约束按以下规则添加：

设第i层的某一特征参与拟合了第i+1层中某一特征根据先验信息添加和间的跨层约束

当i是特征层次中的最后一层时，在该层相邻特征间添加同层约束D_i。

5.根据权利要求1所述的方法，其特征在于：所述方法还包括：

约束的权重主要取决于设定的系数a_i，a_i的值随层数i递增，即在每一次地图优化中，权重不是全局均匀分布的，是逐层增大的。

6.根据权利要求1所述的方法，其特征在于:所述方法还包括：

Scan Match基于一种ICP的改进方法进行，Scan Match寻求一个旋转R和位移t以最小化关于两个匹配点集的损失函数；

该过程迭代寻求两帧观测的匹配关系和相对位姿；

对于确定匹配关系，在ICP基于最近距离的准则上加上了方向限制，角度误差不能超过阈值，使得各个点变换的方向具有一致性。

7.根据权利要求1所述的方法，其特征在于：所述关键特征包括：长度大于设定阈值的线段及其朝向，位于拐角处的线段及其朝向，圆心拟合的直线及其朝向。

8.一种基于先验信息的Slam装置，其特征在于，所述装置包括：

添加模块，用于添加初始位姿节点，对初始雷达观测提取初级特征，并标记关键特征，向地图中添加特征节点，计算特征节点与初始位姿节点的约束及其权重；

扩建模块，用于利用观测、先验信息和关键特征扩建地图；

优化模块，用于在最后一帧雷达观测信息被添加到地图后，进行全局优化，得到路径和特征地图；

构建模块，用于根据最终优化得到的移动机器人路径，和每个位姿对应的观测，基于贝叶斯滤波器构建环境的栅格地图。

9.根据权利要求8所述的装置，其特征在于，所述扩建模块包括：

第一扩建单元，用于利用观测扩建地图，对每帧雷达观测，与前一帧观测进行ScanMatch，Scan Match对这帧雷达观测提取初级特征，和地图中已有的特征进行关联，并将没被观测过的新特征作为节点添加到地图中，添加当前位姿节点和对应特征节点之间的约束和权重；

第二扩建单元，用于利用先验信息扩建地图，当雷达帧数是K1倍数，优化地图，并根据一致性校验移除部分约束和节点；对当前地图中已有的特征提取出更高一层的特征，添加到地图中，标记出关键特征，并根据先验信息添加特征之间的约束和权重；

第三扩建单元，用于利用关键特征扩建地图，当雷达帧数是K2的倍数，对地图进行优化和裁剪；对当前地图中已标记为关键特征的节点间添加约束及权重，约束只建立在同类型关键特征中。

10.根据权利要求8所述的装置，其特征在于，所述装置还包括：

层次设计单元，用于根据先验信息，对环境中存在的结构特征设计层次，初级特征为线段和圆弧，线段的上一层为朝向特征，圆弧的上一层为圆心，圆心的上一层为直线，直线的上一层为朝向，该结构层次可以扩展，以适应不同环境中的先验信息；

提取单元，用于从雷达观测数据提取初级特征，对雷达观测数据进行预处理后，检测得到的连续点列段局部曲率，根据曲率分布确定线段和圆弧；

从圆弧提取圆心，从圆心提取直线，线到朝向，都是基于最小二乘实现；

提取特征的顺序始终按照该层次从低到高进行，只有当特征提取的方差低于阈值特征才被保留。