[发明专利]即时定位与地图构建方法和设备

说明书

技术领域

本发明涉及即时定位与地图构建方法和设备。更具体地说，涉及能够减小计算量并提高精度的即时定位与地图构建方法和设备。

背景技术

即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)是目前在机器人定位方面的热门研究课题。所谓SLAM就是将移动机器人定位与环境地图创建融为一体，即机器人在运动过程中根据自身位姿估计和传感器对环境的感知构建增量式环境地图，同时利用该地图实现自身的定位。

在SLAM中，存在两种坐标系：机器人坐标系和全局坐标系。机器人的位姿是指它在全局坐标系下的绝对坐标，例如三维环境中记为X＝(x，y，z，θ)，其中x，y，z表示在全局坐标系下的坐标，θ表示在该点的姿态(或，视角)。在初始状态下，机器人坐标系与全局坐标系重合。移动机器人在场景中运动，不需要任何先验知识，利用双目立体视觉来感知周围环境信息，并提取稳定的图像特征点来表征3D空间实际物理点，以此作为自然路标，来构建环境的几何地图，同时通过与当前时刻之前所创建的环境地图(自然路标库)中的路标进行匹配，估计机器人当前位姿并更新自然路标库，从而实现移动机器人的SLAM过程。

在现有的2D或3D SLAM应用中，迭代最近点法(Iterative Closest Point，ICP)是经常使用的算法。该算法通过迭代优化矩阵，在每次迭代过程中，对目标点集上的每个点，在参考点集中寻找最近点，并利用这样的对应点，计算相应的旋转矩阵和平移向量，将其用于目标点集上，得到新的目标点集并进入下次迭代过程，最终得到优秀的转换矩阵，实现两点集的精确配准。然而，该算法的缺点在于：1)深度数据的噪声频繁地导致最近点的不匹配；2)尤其对于大尺度平面的偏移来说，地图校正能力很差；3)定位结果不精确。

另外，基于尺度不变特征变换(Scale-invariant feature transform，SIFT)的SLAM也是已知的。SIFT是一种基于特征的配准方法。SIFT特征匹配算法是DavidG.Lowe在2004年总结了现有的基于不变量技术的特征检测方法的基础上，提出的一种基于尺度空间的、对图像缩放、旋转甚至仿射变换保持不变性的特征匹配算法。该算法匹配能力较强，能提取稳定的特征，可以处理两幅图像之间发生平移、旋转、仿射变换、视角变换、光照变换情况下的匹配问题，甚至在某种程度上对任意角度拍摄的图像也具备较为稳定的特征匹配能力，从而可以实现差异较大的两幅图像之间的特征的匹配。但是，这种方法的缺点在于对于环境亮度和随机出现的路标具有很强的依赖性。

可见，在使用通过范围检测设备(例如，单视场主动红外相机或立体视觉相机)产生的深度数据的2D/3D SLAM中，大多通过点匹配来进行特征匹配。但是由于点云数据(Point Cloud Data，PCD)是海量的，从而使得处理非常耗时。

发明内容

鉴于以上情形，本发明提出了一种使用环境特征矢量的新的SLAM的算法和工作流程。这些特征矢量表示在待开发的地区或空间中可能频繁出现的平面。

根据本发明实施例的一个方面，提供了一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤：以一未知视角获取一帧点云数据；将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；合并接近的法向矢量以形成法向矢量列表；获得已知环境地图的环境特征矢量表；将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；将未能匹配的法向矢量表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；以及利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

此外，在根据本发明实施例的方法中，如果所述特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。