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

说明书

技术领域

本发明涉及即时定位与地图构建方法和设备。更具体地说，涉及通过采用陀螺仪作为附加传感器来加速定位处理的、基于矢量的即时定位与地图构建方法和设备。

背景技术

即时定位与地图构建(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方法。在所述方法中，以法向矢量唯一地表示与其垂直的平面，然后通过全局坐标系下与机器人坐标系下表示相同或相近平面的面匹配(即，法向矢量与特征矢量的匹配)来确定机器人当前的位姿。与采用点匹配的技术相比，由于基于环境矢量的SLAM方法中采用面匹配且在面匹配之前对相同或相近平面进行合并从而进一步减少了需要进行匹配的平面数量，因此大幅度地减小了待处理的数据量，极大地减小了SLAM的计算负荷，并且提高了处理速度。另外，还可以提高包括地图和定位结果在内的输出精度。然而，在这种方法中，由于需要通过在多个特征矢量中寻求匹配来计算旋转矩阵，因此定位速度有待进一步提高。

发明内容

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

根据本发明实施例的一个方面，提供了一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤：

使一设置有陀螺仪的设备以一未知视角获取一帧点云数据；

将所获取的点云数据进行三角形网格化，以形成多个平面，获得与多个平面中的每一个对应的法向矢量，并合并接近的法向矢量以获得法向矢量列表；

获得已知环境地图的环境特征矢量表；

通过读取所述设备上的陀螺仪的相应参数，获得所述设备在该未知视角处的姿态，并根据该姿态获得旋转矩阵；