移动机器人的综合定位方法与流程

本发明具体涉及一种移动机器人的综合定位方法。

背景技术

随着经济技术的发展和人们生活水平的提高，人工智能和机器人技术也得到了长足的发展。在人工智能和机器人技术的不断提升的背景条件下，传统的人力正朝着无人值守方向发展。目前，基于人工智能的机器人技术已经在市场上不断涌现，机场、银行、商场、酒店、企事业单位、展馆、仓库、码头港口以及社区都已经慢慢实现了少人或无人值班。将机器人赋予人的一种本能，其可移动性是最基本的技能，而这种最基本的技能是实现机器人自主行走与导航的关键。其中，机器人如何进行自主定位是关键，只有在知道自己位置的前提下，才能进行下一步的动作。

现今移动机器人的定位方法主要有：磁定位、惯性定位、卫星定位、传感器数据(激光雷达、超声波等)定位和视觉定位。但是，目前常用的定位方法，激光雷达的定位精度非常高的，但是其成本高昂，难以普及；而其他的定位方法，虽然能够实现机器人的定位，但是其定位精度不高，因此难以实现机器人的精准控制和定位。

技术实现要素：

本发明的目的在于提供一种能够实现机器人的精确定位，而且成本相对低廉的移动机器人的综合定位方法。

本发明提供的这种移动机器人的综合定位方法，包括如下步骤：

s1.当环境良好，特征点丰富时，采用新型视觉定位算法对机器人进行定位；

s2.当视觉定位丢失时，保存当前的视觉定位数据，同时采用码盘对机器人进行定位；

s3.当视觉定位恢复时，继续采用新型视觉定位算法对机器人进行定位。

所述的新型视觉定位算法，具体包括如下步骤：

a.采用深度摄像头实时采集环境的深度数据和彩色数据，同时对深度摄像头进行标定；

b.将摄像头获取的第一帧作为关键帧，并存储第一帧的深度图像和彩色图像；

c.在步骤b得到的第一帧关键帧后，继续插入后续的关键帧数据；

d.对步骤c得到的关键帧序列中的每一个关键帧进行特征检测；

e.根据步骤d的特征检测结果，计算步骤c得到的关键帧序列中两两关键帧之间的旋转矩阵和平移矩阵；

f.根据步骤e得到的结果，对关键帧进行连接，并优化旋转矩阵和平移矩阵；

g.根据步骤f得到的优化后的旋转矩阵和平移矩阵，计算机器人的里程数据，从而完成机器人的实时定位。

步骤c所述的插入后续的关键帧数据，具体为采用如下规则选取插入的关键帧：

r1.若当前帧与前一关键帧之间有足够的视差，即当前帧与上一关键帧之间的匹配点数达到设定的阈值；

r2.距离上一次插入关键帧的时间已经达到设定的阈值t；

当当前帧与上一关键帧之间的差别满足规则r1或r2时，将当前帧作为插入的关键帧。

步骤d所述的对关键帧进行特征检测，具体为采用如下步骤进行特征检测：

a.采用基于构建图像金字塔的方法对图像进行分层，从而保证图像特征检测时的尺度不变性；

b.采用ofast算法对特征点进行检测，同时采用brief描述子对特征进行描述；

c.采用灰度质心法保证特征点的旋转不变性。

步骤e所述的计算两两关键帧之间的旋转矩阵和平移矩阵，具体为采用如下步骤进行计算：

(1)采用如下公式计算世界坐标系下的3d点坐标：

式中，u为图像坐标系的横坐标值，v为图像坐标系的纵坐标值，fx、fy、cx和cy为相机内部参数；inv(fx)为fx的倒数，inv(fy)为fy的倒数，z为深度数据，x、y和z即为世界坐标系下的3d点的坐标；

(2)将前一帧的特征点投影到当前帧的图像平面上，并在投影的位置进行区域匹配；

(3)采用如下算式，对当前帧的姿态进行估算：

式中pi和p'i是匹配的特征点对的3d点，r为旋转矩阵，t为平移矩阵。

(4)根据步骤(3)得到的姿态数据，剔除误匹配对。

步骤(3)所述的对姿态进行估算，具体为采用迭代最近点算法求解，并采用非线性优化方法找到全局最优值。

步骤f所述的对关键帧进行连接并优化旋转矩阵和平移矩阵，具体为根据匹配关系连接关键帧，并采用g2o进行局部优化和闭环优化。

本发明提供的这种移动机器人的综合定位方法，通过新型视觉定位算法和码盘定位算法相融合的方式，实现了机器人的精确定位，而且通过算法实现精确定位，成本相对低廉，而且新型视觉定位算法算法简单可靠，定位精度也较高。

附图说明

图1为本发明方法的方法流程图。

图2为本发明方法的新型视觉定位算法的算法流程图。

具体实施方式

如图1所示为本发明方法的方法流程图：本发明提供的这种移动机器人的综合定位方法，包括如下步骤：

s1.当环境良好，特征点丰富时，采用新型视觉定位算法对机器人进行定位；

s2.当视觉定位丢失时，保存当前的视觉定位数据，同时采用码盘对机器人进行定位；

s3.当视觉定位恢复时，继续采用新型视觉定位算法对机器人进行定位。

其中，所述的新型视觉定位算法的算法流程图如图2所示，其具体包括如下步骤：

a.采用深度摄像头实时采集环境的深度数据和彩色数据，同时对深度摄像头进行标定；

b.将摄像头获取的第一帧作为关键帧，并存储第一帧的深度图像和彩色图像；

c.在步骤b得到的第一帧关键帧后，继续插入后续的关键帧数据；

采用如下规则选取插入的关键帧：

r1.若当前帧与前一关键帧之间有足够的视差，即当前帧与上一关键帧之间的匹配点数达到设定的阈值；

r2.距离上一次插入关键帧的时间已经达到设定的阈值t；

当当前帧与上一关键帧之间的差别满足规则r1或r2时，将当前帧作为插入的关键帧；

d.对步骤c得到的关键帧序列中的每一个关键帧进行特征检测；具体为采用如下步骤进行特征检测：

a.采用基于构建图像金字塔的方法对图像进行分层，从而保证图像特征检测时的尺度不变形；

b.采用ofast算法对特征点进行检测，同时采用brief描述子对特征进行描述；将图像分格子，对每个格子进行ofast特征检测，在每个格子里进行特征检测，然后将特征进行四叉树节点分布，然后对每个节点选取最好的特征，从而保证了特征点分布均匀；

c.采用灰度质心法保证特征点的旋转不变性；其实质是假设角点的灰度与质心之间存在一个偏移，这个向量可以用于表示一个方向，这个向量便是特征点的邻域方向；

e.根据步骤d的特征检测结果，计算步骤c得到的关键帧序列中两两关键帧之间的旋转矩阵和平移矩阵；具体为采用如下步骤进行计算：

(1)采用如下公式计算世界坐标系下的3d点坐标：

式中，u为图像坐标系的横坐标值，v为图像坐标系的纵坐标值，fx、fy、cx和cy为相机内部参数，其中fx，fy为焦距，cx，cy为在像素坐标系与成像平面相差的相对于原点的平移；inv(fx)为fx的倒数，inv(fy)为fy的倒数，z为深度数据，x、y和z即为世界坐标系下的3d点的坐标；

(2)将前一帧的特征点投影到当前帧的图像平面上，并在投影的位置进行区域匹配；

(3)采用如下算式，对当前帧的姿态进行估算：

式中pi和p'i是匹配的特征点对的3d点，r为旋转矩阵，t为平移矩阵；在具体实施时采用迭代最近点算法求解，并采用非线性优化方法(icp)找到全局最优值；

(4)根据步骤(3)得到的姿态数据，剔除误匹配对；

f.根据步骤e得到的结果，对关键帧进行连接，并优化旋转矩阵和平移矩阵；具体为根据匹配关系连接关键帧，并采用g2o进行局部优化和闭环优化；在g2o中，将关键帧位姿和特征点3d点作为顶点，将重投影误差作为边，而这个边是连接关键帧位姿顶点和特征点位姿顶点的2维数据；

g.根据步骤f得到的优化后的旋转矩阵和平移矩阵，计算机器人的里程数据，从而完成机器人的实时定位。