一种机器人视觉识别定位方法及系统

1.本发明涉及机器人自我感知和定位技术领域，具体涉及一种机器人视觉识别定位方法及系统。


背景技术：

2.随着机器人技术的发展，具有移动行走功能、环境感知能力以及自主规划能力智能移动机器人得到了各国研究人员的普遍重视，特别是在20世纪八、九十年代，随着计算机技术、微电子技术、网络技术等的快速发展，机器人技术的发展突飞猛进。智能机器人移动的重要特点在于它的自主性和适应性。自主性是指它可以在一定的环境中，不依赖外部控制，完全自主地执行一定的任务；适应性是指它可以实时识别和测量周围的物体，并根据环境变化，调节自身参数、动作策略以及处理紧急情况。
3.随着智能机器人技术的发展，其在军事、医疗、商业等领域发挥着重要的作用，人们对智能移动机器人的需求和期望也越来越高，越来越迫切，移动机器人研究从而进入了崭新的发展阶段。定位技术是智能移动机器人的研究核心，同时也是其实现完全自动化的关键技术。机器人只有准确知道自身位置，工作空间中障碍物的位置以及障碍物的运动情况等信息，才能安全有效地进行移动，由此可见，自主定位和环境分析是移动机器人最重要的能力之一。


技术实现要素：

4.针对现有技术中的上述不足，本发明提供了一种机器人视觉识别定位方法及系统。
5.为了达到上述发明目的，本发明采用的技术方案为：
6.第一方面，本发明提供了一种机器人视觉识别定位方法，包括以下步骤：
7.s1、构建机器人相机模型，并对相机进行标定；
8.s2、获取相机采集的环境图像，并进行预处理；
9.s3、对预处理后的环境图像中的特征点进行连续跟踪检测；
10.s4、根据检测的特征点对机器人位姿进行估计。
11.进一步地，所述步骤s1具体包括以下分步骤：
12.s1-1、分别建立世界坐标系和图像坐标系；
13.s1-2、以机器人相机的光心作为原点，以机器人相机的光轴作为z轴，以平行于图像平面坐标系的x轴和y轴方向作为x轴和y轴，建立相机坐标系；
14.s1-3、根据世界坐标系、相机坐标系和图像坐标系之间的关系，建立图像坐标系与世界坐标系的转换模型，表示为：
[0015][0016]
式中，(u,v)表示特征点在图像坐标系的二维坐标值，(x,y,z)表示特征点在世界坐标系的三维坐标值，(xc,yc,zc)表示特征点在相机坐标系的三维坐标值，f表示相机焦距，u0,v0表示相机的图像中心在x轴和y轴方向采样时的位置偏移量，dx,dy表示相机的像素在x轴和y轴方向采样时的量化因子。
[0017]
进一步地，所述步骤s2具体包括以下分步骤：
[0018]
s2-1、获取相机采集的环境图像；
[0019]
s2-2、对采集的环境图像进行畸变和切向误差修正，表示为：
[0020][0021]
式中，(u,v)表示特征点在图像坐标系的二维坐标值，表示特征点在图像坐标系修正后的二维坐标值，k1,k2,k3表示畸变系数，p1,p2表示泰勒级数，r表示特征点距离相机光心的距离；
[0022]
s2-3、对修正后的环境图像进行去噪处理；
[0023]
s2-4、对处理后的环境图像进行灰度化处理。
[0024]
进一步地，所述步骤s3具体包括以下分步骤：
[0025]
s3-1、对预处理后的环境图像进行特征检测；
[0026]
s3-2、对连续环境图像中相邻的两幅环境图像中的特征点进行匹配；
[0027]
s3-3、采用拓展卡尔曼滤波方法计算匹配的特征点坐标。
[0028]
进一步地，所述步骤s3-1具体包括以下分步骤：
[0029]
s3-1-1、采用滑动窗口计算每个窗口的加权熵的极大值，得到候选特征点；
[0030]
s3-1-2、分别计算每个候选特征点的特征响应值，并按照图像灰度变化进行分类；
[0031]
s3-1-3、分别对分类的不同候选特征点采用几何阈值进行检测；
[0032]
s3-1-4、将检测得到的特征点采用门限值进行筛选，最终得到检测的特征点。
[0033]
进一步地，所述步骤s3-2具体包括以下分步骤：
[0034]
s3-2-1、采用如下匹配条件对连续环境图像中相邻的两幅环境图像中的特征点进行匹配：
[0035][0036]
xi＝δ
i20
+δ
i02
[0037]
xj＝δ
j20
+δ
j02
[0038]
[0039][0040]
式中，d
ij
表示连续环境图像中相邻的两幅环境图像中的特征点匹配值，δ
ipq
表示特征点i的(p+q)阶规范化中心矩；
[0041]
s3-2-2、判断连续环境图像中相邻的两幅环境图像中的特征点匹配值是否小于或等于匹配阈值；若是，则该两个特征点匹配；否则不匹配。
[0042]
进一步地，所述步骤s3-3具体包括以下分步骤：
[0043]
s3-3-1、构建拓展卡尔曼滤波的非线性差分模型和观测模型，表示为：
[0044][0045][0046]
式中，x(k)表示特征点的真实值，其中各元素为特征点旋转和平移向量，z(k)表示特征点的观测值；
[0047]
s3-3-2、采用拓展卡尔曼滤波方法计算匹配的特征点坐标，表示为：
[0048][0049]
式中，zj(k)表示k时刻环境图像中与k-1时刻环境图像中匹配成功的n个特征点中的第j个特征点在图像坐标系中的坐标。
[0050]
进一步地，所述步骤s4具体包括以下分步骤：
[0051]
s4-1、建立表征机器人位姿的位姿矩阵；
[0052]
s4-2、对检测的所有特征点进行归一化，并结合机器人的位姿矩阵构建定位模型；
[0053]
s4-3、将所有的定位模型展开为向量形式，构建定位线性方程组并进行求解；
[0054]
s4-4、对求解得到的位姿矩阵进行奇异值分解，得到机器人的旋转矩阵和平移向量。
[0055]
进一步地，所述定位模型表示为：
[0056][0057]
式中，(u
1 v
1 1),(u
2 v
2 1)表示两个特征点的归一化坐标，表示位姿矩阵，其中各元素为机器人的旋转和平移向量。
[0058]
第二方面，本发明还提供了一种机器人视觉识别定位系统，包括：
[0059]
标定模块，用于构建机器人相机模型，并对相机进行标定；
[0060]
采集模块，用于获取相机采集的环境图像，并进行预处理；
[0061]
检测模块，用于对预处理后的环境图像中的特征点进行连续跟踪检测；
[0062]
定位模块，用于根据检测的特征点对机器人位姿进行估计。
[0063]
本发明具有以下有益效果：
[0064]
本发明通过构建机器人视觉环境下的坐标系转换模型，并对环境图像中的特征点进行连续跟踪检测，最后基于检测的特征点进行机器人位姿估计，在保证定位精度的同时，能够加快机器人定位算法的计算效率。
附图说明
[0065]
图1为本发明的机器人视觉识别定位方法流程示意图；
[0066]
图2为本发明的机器人视觉识别定位系统结构示意图。
具体实施方式
[0067]
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
[0068]
如图1所示，本发明实施例提供了一种机器人视觉识别定位方法，包括以下步骤s1至s4：
[0069]
s1、构建机器人相机模型，并对相机进行标定；
[0070]
在本发明的一个可选实施例中，步骤s1具体包括以下分步骤：
[0071]
s1-1、分别建立世界坐标系和图像坐标系；
[0072]
具体而言，本发明以机器人所处空间建立世界坐标系，以机器人相机所采集的环境图像建立图像坐标系。
[0073]
s1-2、以机器人相机的光心作为原点，以机器人相机的光轴作为z轴，以平行于图像平面坐标系的x轴和y轴方向作为x轴和y轴，建立相机坐标系；
[0074]
s1-3、根据世界坐标系、相机坐标系和图像坐标系之间的关系，建立图像坐标系与世界坐标系的转换模型，表示为：
[0075][0076]
式中，(u,v)表示特征点在图像坐标系的二维坐标值，(x,y,z)表示特征点在世界坐标系的三维坐标值，(xc,yc,zc)表示特征点在相机坐标系的三维坐标值，f表示相机焦距，u0,v0表示相机的图像中心在x轴和y轴方向采样时的位置偏移量，dx,dy表示相机的像素在x轴和y轴方向采样时的量化因子。
[0077]
具体而言，根据相机成像原理，相机坐标系和图像坐标系之间的关系表示为
[0078][0079]
世界坐标系和相机坐标系之间的关系表示为
[0080][0081]
式中，表示旋转矩阵，t表示平移向量。
[0082]
从而根据上述关系建立图像坐标系与世界坐标系的转换模型。
[0083]
s2、获取相机采集的环境图像，并进行预处理；
[0084]
在本发明的一个可选实施例中，步骤s2具体包括以下分步骤：
[0085]
s2-1、获取相机采集的环境图像；
[0086]
s2-2、对采集的环境图像进行畸变和切向误差修正，表示为：
[0087][0088]
式中，(u,v)表示特征点在图像坐标系的二维坐标值，表示特征点在图像坐标系修正后的二维坐标值，k1,k2,k3表示畸变系数，p1,p2表示泰勒级数，r表示特征点距离相机光心的距离；
[0089]
s2-3、对修正后的环境图像进行去噪处理；
[0090]
具体而言，对修正后的环境图像进行去噪处理可以采用本领域常用的中值滤波、双边滤波、高斯滤波等去噪方法，这里不作赘述。
[0091]
s2-4、对处理后的环境图像进行灰度化处理。
[0092]
具体而言，对处理后的环境图像进行灰度化处理可以采用本领域常用的加权平均法等处理方法，这里不作赘述。
[0093]
s3、对预处理后的环境图像中的特征点进行连续跟踪检测；
[0094]
在本发明的一个可选实施例中，步骤s3具体包括以下分步骤：
[0095]
s3-1、对预处理后的环境图像进行特征检测；
[0096]
具体而言，步骤s3-1具体包括以下分步骤：
[0097]
s3-1-1、采用滑动窗口计算每个窗口的加权熵的极大值，得到候选特征点；
[0098]
具体而言，本发明依次选取环境图像中的一个像素点，采用以该像素点为中心、设定尺寸的窗口来检测其领域内具有最大图像熵的点，让检测窗口中的每个点与该领域内的样本值进行比较，若某一点的领域范围内的图像熵大于其他点，则该点为候选特征点。
[0099]
s3-1-2、分别计算每个候选特征点的特征响应值，并按照图像灰度变化进行分类；
[0100]
具体而言，本发明利用圆形窗口检测该窗口内最小亮度变化，即特征响应值，若特
征响应值大于设定门限值，则该检测点为筛选得到的候选特征点，否则剔除该检测点。
[0101]
本发明根据特征响应值的大小将候选特征点进行分类，来表征图像的灰度变化趋势，分类模型表示为：
[0102][0103]
式中，r
max
,r
min
表示特征响应值的最大值和最小值。
[0104]
s3-1-3、分别对分类的不同候选特征点采用几何阈值进行检测；
[0105]
具体而言，当m＝i时，此时待检测点的灰度变化范围较小，这类点灰度变化最小，本发明使用几何阈值d＝7对其进行处理；当m＝ii时，本发明使用几何阈值d＝5进行处理；当m＝iii时，灰皮变化范围较大的点，本发明使用几何阈值d＝3进行处理。
[0106]
s3-1-4、将检测得到的特征点采用门限值进行筛选，最终得到检测的特征点。
[0107]
具体而言，通过上述分类，将图像中特征点所在区域的灰度变化划分出来，在灰度变化较小的区域，使用较小检测门限，正好更加容易寻找出特征点，防止特征点的漏检；灰度变化较大的区域，使用较大的门限，正好可以去掉一些误特征点。这样的门限设置，既避免了特征点提取过程中的漏检，也避免了特征点的误检。本发明采用的门限值表示为：
[0108][0109]
式中，δ表示门限参数。
[0110]
s3-2、对连续环境图像中相邻的两幅环境图像中的特征点进行匹配；
[0111]
具体而言，步骤s3-2具体包括以下分步骤：
[0112]
s3-2-1、采用如下匹配条件对连续环境图像中相邻的两幅环境图像中的特征点进行匹配：
[0113][0114]
xi＝δ
i20
+δ
i02
[0115]
xj＝δ
j20
+δ
j02
[0116][0117][0118]
式中，d
ij
表示连续环境图像中相邻的两幅环境图像中的特征点匹配值，δ
ipq
表示特征点i的(p+q)阶规范化中心矩；
[0119]
s3-2-2、判断连续环境图像中相邻的两幅环境图像中的特征点匹配值是否小于或
等于匹配阈值；若是，则该两个特征点匹配；否则不匹配。
[0120]
s3-3、采用拓展卡尔曼滤波方法计算匹配的特征点坐标。
[0121]
具体而言，步骤s3-3具体包括以下分步骤：
[0122]
s3-3-1、构建拓展卡尔曼滤波的非线性差分模型和观测模型，表示为：
[0123][0124][0125]
式中，x(k)表示特征点的真实值，其中各元素为特征点旋转和平移向量，即tu(k),tv(k)表示平移向量，表示旋转矩阵，z(k)表示特征点的观测值；
[0126]
s3-3-2、采用拓展卡尔曼滤波方法计算匹配的特征点坐标，表示为：
[0127][0128]
式中，zj(k)表示k时刻环境图像中与k-1时刻环境图像中匹配成功的n个特征点中的第j个特征点在图像坐标系中的坐标。
[0129]
s4、根据检测的特征点对机器人位姿进行估计。
[0130]
在本发明的一个可选实施例中，步骤s4具体包括以下分步骤：
[0131]
s4-1、建立表征机器人位姿的位姿矩阵；
[0132]
具体而言，本发明采用本质矩阵来表征机器人位姿，本质矩阵是一个包含有9个未知数的3x3矩阵，且由对极约束进行定义。本质矩阵共有6个自由度包含3个旋转矩阵和3个平移向量，表示为
[0133][0134]
式中，表示旋转矩阵和平移向量。
[0135]
s4-2、对检测的所有特征点进行归一化，并结合机器人的位姿矩阵构建定位模型；
[0136]
具体而言，将匹配的特征点坐标进行归一化为x1＝[u1,v1,1]
t
，x2＝[u2,v2,1]
t
；根据对极约束，将定位模型表示为：
[0137]
[0138]
式中，(u
1 v
1 1),(u
2 v
2 1)表示两个特征点的归一化坐标，表示位姿矩阵，其中各元素为机器人的旋转和平移向量。
[0139]
s4-3、将所有的定位模型展开为向量形式，构建定位线性方程组并进行求解；
[0140]
s4-4、对求解得到的位姿矩阵进行奇异值分解，得到机器人的旋转矩阵和平移向量。
[0141]
如图2所示，本发明实施例还提供了一种机器人视觉识别定位系统，包括：
[0142]
标定模块，用于构建机器人相机模型，并对相机进行标定；
[0143]
采集模块，用于获取相机采集的环境图像，并进行预处理；
[0144]
检测模块，用于对预处理后的环境图像中的特征点进行连续跟踪检测；
[0145]
定位模块，用于根据检测的特征点对机器人位姿进行估计。
[0146]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0147]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0148]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0149]
本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
[0150]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。