[发明专利]一种基于先验信息的Slam方法和装置

权利要求书

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

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

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

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

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

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

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

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

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

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

从圆弧提取圆心，从圆心提取直线，线到朝向，都是基于最小二乘实现；提取特征的顺序始终按照该层次从低到高进行，只有当特征提取的方差低于阈值特征才被保留。

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

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

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

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

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

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

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

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

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

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

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