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

说明书

本发明提供一种基于先验信息的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对这帧雷达观测提取初级特征，和地图中已有的特征进行关联，并将没被观测过的新特征作为节点添加到地图中，添加当前位姿节点和对应特征节点之间的约束和权重；