[发明专利]一种顶标地图构建及机器人定位方法

说明书

本发明适用于自动控制技术领域，提供了一种顶标地图的构建及机器人定位方法，包括：S1、判断当前是否进入定位模式，若判断结果为是，进入定位模式，执行步骤S2、步骤S3、及步骤S4，若判断结果为否，进入建图模式，执行步骤S2至步骤S6；S2、提取当前图像中的顶标，获取顶标ID，计算顶标在图像中的位置；S3、提取当前图像中的已知顶标和未知顶标；S4、基于至少3个已知顶标来计算机器人当前在路标地图中的位姿；S5、计算从顶标坐标系到地图坐标系的转换矩阵R′、及平移向量t′；S6、基于未知顶标在顶标坐标系中的坐标x_map，计算未知顶标在路标地图中的坐标x_w，更新路标地图。基于当前顶标坐标系到地图坐标系的转换矩阵及平移向量来计算为未知顶标在路标地图中的位置，即构建路标地图，可以减弱误差的累积。

技术领域

本发明属于自动控制技术领域，提供了一种顶标地图构建及机器人定位方法。

背景技术

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述所有功能就必须要准确的知道自身所在的位置，即实时定位。而实现机器人实时定位的先决条件是建立地图，这是实现机器人导航和其它智能行为的关键。移动机器人建图常用的传感器有摄像机和激光雷达等。其中激光雷达硬件成本较高，不利于移动机器人的大范围普及推广。而利用摄像机进行定位硬件成本低、定位精度高，在室内定位中，基于视觉的定位方法被广泛的应用。

基于视觉的室内定位首先要构建精准的室内地图，用于绝对坐标系下的摄像机姿态的计算以及机器人移动路径的规划。可以通过人工路标构建精确的几何地图，也可以利用环境地标来构建地图。其中基于环境地标的方法虽然普适性较好，不需要人工布设额外的标志，但是计算复杂，实用性较差。人工路标往往在颜色、形状等视觉特征上具有明显的唯一性，通过计算机视觉方法能够很容易的将人工路标识别出来。其中有一种在屋顶布设人工路标的方法，屋顶环境单一易于提取，且摄像机的视野不容易受到干扰，在室内定位中被广泛应用。但是现有的基于顶标的地图构建方法是根据已知路标依次计算未知路标在地图中的位置，计算过程中的误差会累积向后传播，在顶标数目较多时地图构建结果不准确。

发明内容

本发明实施例提供一种顶标地图构建及机器人定位方法，旨在提高地图构建的精准性。

为了实现上述目的，本发明提供了顶标地图构建及机器人定位方法，该方法包括如下步骤：

S1、判断当前是否进入定位模式，若判断结果为是，进入定位模式，执行步骤S2、步骤S3、及步骤S4，若判断结果为否，进入建图模式，执行步骤S2至步骤S6；

S2、提取当前图像中的顶标，获取顶标ID，计算顶标在图像中的位置x_i；

S3、将提取到的顶标ID与路标地图中的路标ID进行对比，得到已知顶标和未知顶标；

S4、基于至少3个已知顶标来计算机器人当前在路标地图中的位姿P(x,y,θ)；

S5、计算顶标坐标系到地图坐标系的转换矩阵R′、及平移向量t′；

S6、基于未知顶标在顶标坐标系中的坐标x_map，计算未知顶标在路标地图中的坐标x_w，更新路标地图。

进一步的，所述步骤S4具体包括如下步骤：

S41、基于顶标在图像中的坐标x_i及相机的内参矩阵M_cam计算

S42、基于仿射变换方程计算出旋转矩阵及平移向量旋转矩阵及平移向量去除摄影深度因子得到相机坐标系到地图坐标系的旋转矩阵R、及平移向量t；