[发明专利]一种基于视觉地图的视觉定位方法、装置

说明书

本申请公开了一种基于视觉地图的视觉定位方法，该方法包括，采集当前图像，得到当前帧；提取当前帧的特征点，将当前帧的特征点与地图中的地图点进行匹配，得到匹配成功的第一匹配特征点，以地图匹配约束，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果；其中，地图匹配约束为：与第一匹配特征点匹配的第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差。该方法使得定位结果没有累计误差，从而具有更高的精度。

技术领域

本发明涉及视觉导航领域，特别地，涉及一种基于视觉地图的视觉定位方法。

背景技术

视觉导航是通过摄像装置对周围环境进行图像采集，并基于图像进行计算，完成位置确定和路径识别，通常需要依赖视觉地图来完成，其中，视觉地图可利用图像特征点以及特征点的描述子构建环境地图。以基于地面纹理的地图为例，移动机器人移动经过某纹理点的时候，根据图像配准，解算出当前移动机器人的位姿，即可进行基于地面纹理的定位导航。

现有的基于视觉地图的定位方法，仅仅使用一帧图像的匹配结果进行定位，导致定位精度不足。

发明内容

本发明提供了一种基于视觉地图的视觉定位方法，以提高视觉定位的精度。

本发明提供的一种基于视觉地图的视觉定位方法是这样实现的：

采集当前图像，得到当前帧；

提取当前帧的特征点，

将当前帧的特征点与地图中的地图点进行匹配，得到匹配成功的第一匹配特征点，

基于第一匹配特征点与该第一匹配特征点匹配的第一匹配地图点所构成的地图匹配约束，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果；

其中，

地图匹配约束为：第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差，或者，当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世间坐标系中的空间位置之间的误差。

较佳地，所述基于第一匹配特征点与该第一匹配特征点匹配的第一匹配地图点所构成的约束，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果，进一步包括，

基于地图匹配约束、以及至少帧间匹配约束、惯性约束之一，对当前帧位姿进行优化，得到优化后的位姿，作为定位结果，

其中，

帧间匹配约束为：当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差，或者，与第二匹配特征点匹配的第二匹配地图点反投影到当前帧的像素位置与该第二匹配地图点反投影到上一关键帧的像素位置之间的误差；

惯性约束为：根据当前里程计数据和上一关键帧的里程计数据所得到的帧间里程计相对位姿与根据当前帧位姿和上一关键帧位姿所得到的帧间相对位姿之间的误差。

较佳地，所述第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差，以如下方式获得：

根据第一匹配地图点的空间位置信息，确定第一匹配特征点的空间位置信息；

根据当前帧中第一匹配特征点的空间位置信息和像素位置信息，获得当前帧位姿；

基于当前帧位姿、以及相机内参，根据第一匹配地图点的空间位置信息，计算第一匹配地图点反投影到当前帧上的像素位置，

计算所述第一匹配地图点反投影到当前帧上的像素位置和当前帧中与该第一地图点匹配的第一匹配特征点的像素位置之间的差值；