CN109612468A - 一种顶标地图构建及机器人定位方法 - Google Patents

Abstract

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

Description

一种顶标地图构建及机器人定位方法

技术领域

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

背景技术

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

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

发明内容

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

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

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；

S43、基于公式X＝-R^-1*t求得相机中心在路标地图中的坐标，相机的旋转角为θ＝actan(R_(1,0)/R_(0,0))，即获取相机中心在路标地图中的位姿。

进一步的，在步骤S4之后还包括：

S7、将机器人当前在路标地图中的位姿P(x,y,θ)作为扩展卡尔曼滤波模型的输入，对机器人的当前位姿进行优化。

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

S51、基于已知顶标在图像中的坐标x_i及路标地图中的坐标x_w，计算相机坐标系在地图坐标系中的位姿rMatU，tMatU；

S52、基于公式(1)及公式(2)计算顶标坐标系到地图坐标系的转换矩阵R′、及平移向量t′，公式(1)及公式(2)表示如下：

R′＝rMatU^-1*rMat (1)

t′＝rMatU^-1*(tMat-tMatU) (2)

其中，rMat为相机坐标系到地图坐标系的旋转矩阵，tMat为相机坐标系到地图坐标系的平移向量。

进一步的，在步骤S6之后还包括：

S8、将更新后的路标地图作为扩展卡尔曼滤波模型的输入，对路标地图进行优化。

本发明提供的顶标地图构建及定位方法具有如下有益效果：

1.基于当前顶标坐标系到地图坐标系的转换矩阵及平移向量来计算为未知顶标在路标地图中的位置，即构建路标地图，可以减弱误差的累积；

2.基于扩展卡尔曼滤波对机器人当前在路标地图中的位姿进行优化，能够减弱相机与屋顶不平行等误差造成定位偏差，从而也提高了地图的构建精度；

3.基于扩展卡尔曼滤波对更新后的路标地图进行优化，能够减弱由于误差积累而造成的顶标地图构建的偏差。

附图说明

图1为本发明实施例提供的顶标地图构建及机器人定位方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明需要将顶标贴于机器人需要运动区域的屋顶，相机平行于屋顶进行设置，相机在使用前需要进行标定，得到相机的内参矩阵M_cam。

图1为本发明实施例提供的顶标地图构建及机器人定位方法流程图，该方法具体包括如下步骤：

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

只有在接收到切换指令后，才会进入建图模式，构建路标地图，顶标即为路标地图中的路标，建立路标地图的过程就是不断的将未知顶标纳入路标地图，当所有未知顶标都纳入路标地图，则路标地图构建完毕，已知顶标是指纳入路标地图中的顶标，未知顶标是指未纳入路标地图中的顶标，在将当前帧中的未知顶标纳入路标地图后，自动切回定位模式。

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

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

检测路标地图中是否存在与图像顶标ID一致的路标ID，若存在，则说明该顶标为已知顶标，即已经纳入路标地图中，读取已知顶标在路标地图中的坐标X_w，若不存在，则说明该顶标为未知顶标；

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

在本发明实施例中，位姿的计算方法具体如下：

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

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

S43、基于公式X＝-R^-1*t求得相机中心在路标地图中的坐标，相机的旋转角为θ＝actan(R_(1,0)/R_(0,0))，R_(1,0)为旋转矩阵R的第二行第一个元素，R_(0,0)为旋转矩阵R的第二行第一个元素，即获取相机中心在路标地图中的位姿P(x,y,θ)。

在步骤S4之后包括：

S5、将机器人当前在路标地图中的位姿P(x,y,θ)作为扩展卡尔曼滤波模型的输入，对机器人的当前位姿进行优化，能够减弱相机与屋顶不平行等误差造成定位结果的偏差。

S6、基于已知顶标在图像中的坐标x_i及路标地图中的坐标x_w，计算相机坐标系在地图坐标系中的位姿rMatU，tMatU；

S7、计算顶标坐标系到地图坐标系的转换矩阵R′、及平移向量t′，计算公式具体如下：

R′＝rMatU^-1*rMat

t′＝rMatU^-1*(tMat-tMatU)

其中，rMat为相机坐标系到地图坐标系的旋转矩阵，即步骤S42中的转矩阵R，tMat为相机坐标系到地图坐标系的平移向量，即步骤S42中的平移向量t。

S8、基于未知顶标在顶标坐标系中的坐标x_map，来计算未知顶标在路标地图中的坐标x_w，即将未知顶标纳入路标地图中，计算公式具体如下：

x_map＝R′*x_w+t′

其中，x_map为未知顶标在顶标坐标系中的坐标，R′为顶标坐标系到地图坐标系的转换矩阵、t′为顶标坐标系到地图坐标系的平移向量。

S9、将更新后的路标地图作为扩展卡尔曼滤波模型的输入，对路标地图进行优化，能够减弱由于误差积累而造成的顶标地图构建结果的偏差。

本发明提供的顶标地图构建及机器人定位方法具有如下有益效果：

1.基于当前顶标坐标系到地图坐标系的转换矩阵及平移向量来计算为未知顶标在路标地图中的位置，即构建路标地图，可以减弱误差的累积；

2.基于扩展卡尔曼滤波对机器人当前在路标地图中的位姿进行优化，能够减弱相机与屋顶不平行等误差造成定位偏差，从而也提高了地图的构建精度；

3.基于扩展卡尔曼滤波对更新后的路标地图进行优化，能够减弱由于误差积累而造成的顶标地图构建的偏差。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种顶标地图构建及机器人定位方法，其特征在于，所述方法包括如下步骤：

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

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

S3、提取当前图像中的已知顶标和未知顶标；

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

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

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

2.如权利要求1所述顶标地图构建及机器人定位方法，其特征在于，所述步骤S4具体包括如下步骤：

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

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

S43、基于公式X＝-R^-1*t求得相机中心在路标地图中的坐标，相机的旋转角为θ＝actan(R_(1,0)/R_(0,0))，即获取相机中心在路标地图中的位姿。

3.如权利要求1或2所述顶标地图构建及机器人定位方法，其特征在于，在步骤S4之后还包括：

S7、将机器人当前在路标地图中的位姿P(x,y,θ)作为扩展卡尔曼滤波模型的输入，对机器人的当前位姿进行优化。

4.如权利要求1所述顶标地图构建及机器人定位方法，其特征在于，所述步骤S5具体包括如下步骤：

S51、基于已知顶标在图像中的坐标x_i及路标地图中的坐标x_w，计算相机坐标系在地图坐标系中的位姿rMatU，tMatU；

S52、基于公式(1)及公式(2)计算顶标坐标系到地图坐标系的转换矩阵R′、及平移向量t′，公式(1)及公式(2)表示如下：

R′＝rMatU^-1*rMat (1)

t′＝rMatU^-1*(tMat-tMatU) (2)

其中，rMat为相机坐标系到地图坐标系的旋转矩阵，tMat为相机坐标系到地图坐标系的平移向量。

5.如权利要求1或4所述顶标地图构建及机器人定位方法，其特征在于，在步骤S6之后还包括：

S8、将更新后的路标地图作为扩展卡尔曼滤波模型的输入，对路标地图进行优化。