基于特征点密度峰值的视觉伺服目标选择方法

朱宏辉，王嘉豪，朱 轶

(武汉理工大学 物流工程学院，湖北 武汉 430063)

0 引 言

机器人视觉伺服是机器人研究领域的重要方向之一，通过利用图像信息作为反馈，构成机器人或者机械臂的闭环控制系统[1]。利用无标定视觉伺服技术能够提高机器人环境适应能力，实现移动机器人在室内环境的实际应用[2-4]。在室内环境下，机器人能够正确识别和选择视觉伺服目标是实现机器人视觉伺服的关键。

快速进行目标识别是实现移动机器人视觉伺服的前提，采用特征点匹配的方式能够提高目标识别的速度。目前，常用的特征点匹配算法有：SIFT算法、SURF算法和ORB算法[5-7]，其中ORB算法因运算速度快、抗噪能力强，被广泛用于各种实时特征检测和目标识别场景中。通过特征匹配方式识别目标物体后，利用图像信息作为反馈控制机器人抓取目标物体，从而完成机器人视觉伺服。王博等[8]利用动态拟牛顿算法实现机器人无标定视觉伺服，避免了标定技术对工业机器人视觉伺服系统的影响，提高了机器人视觉伺服系统的鲁棒性。梁喜凤等[9]通过自适应无迹卡尔曼滤波算法估计图像雅可比矩阵，成功实现了七自由度采摘机械手的视觉伺服。但是这些方法都只能实现对单一目标物体的视觉伺服，而移动机器人应用的实际场景中往往会出现多个同样目标物体。为此，张璐等[10]通过无标定视觉伺服方法完成采摘机器人的采摘作业，并以果蔬成熟特性作为采摘目标选择依据，成功实现采摘机器人的选择性采摘作业，但是却不适合用于室内移动机器人视觉伺服。

视觉伺服过程中，机器人视野中可能存在多个相同目标物体，机器人容易因无法选择目标而导致视觉伺服失败。为此，提出一种基于特征点密度峰值的视觉伺服目标选择方法，通过该方法机器人能够选择唯一视觉伺服目标，提高机器人视觉伺服的成功率。通过ORB算法对所有目标物体进行识别，然后采用特征点密度峰值聚类算法[11]将图像中不同目标物体特征点进行分离，最后通过位置优先决策方法在多个相同目标物体中选择一个最佳的抓取目标。最后，通过实验验证该方法的正确性和可行性。

1 特征点匹配与分离

1.1 特征提取与匹配

机器人视觉伺服首先需要识别目标物体，采用ORB算法通过特征点提取与匹配能够实现对目标物体的识别与定位。ORB算法建立在FAST特征点检测算法和BRIEF特征点描述算法理论基础上，并针对原算法进行了一些改进及优化。保留了算法的旋转、尺度不变性，同时极大提高了特征提取与匹配的速度。

ORB算法首先通过FAST算法检测图像尺度空间中的特征点，且ORB算法通过灰度质心法为FAST特征点定义了主方向，以增加FAST特征点的旋转不变性。

为计算特征点方向，ORB定义领域矩，领域矩的定义式见式(1)

(1)

其中，(x,y)∈[-r,r]表示相对于FAST特征点的位置，r表示邻域半径；I(x,y)表示该位置的灰度值，邻域质心计算方法如下

(2)

其中，m00为零阶矩，m01和m10为一阶矩。

ORB定义质心与特征点连线方向即为FAST特征点的方向，角度计算方法如式(3)

θ=atan2(m01,m10)

(3)

ORB使用BRIEF算法计算特征点的描述子，BRIEF描述子是一种二进制特征描述子。BRIEF在特征点邻域内随机选取n对像素点，然后利用一个τ测试比较两者的灰度值，τ测试定义如式(4)。最后，将所有测试结果形成一个由0和1组成的字符串，也就是特征点的描述子，BRIEF描述子表示方法见式(5)

(4)

(5)

然而，BRIEF描述子同样不具备旋转不变性，ORB利用特征点检测得到的主方向作为描述子的主方向，以增强BRIEF对于旋转变化的不变性。利用BRIEF中所有点对定义一个2×n的矩阵，如下

(6)

由特征点主方向θ可以得到旋转矩阵Rθ，由此能够利用Rθ计算得到改进后的BRIEF特征描述子，计算过程如下

(7)

gn(p,θ)=fn(p)|(xi,yi)∈Mθ

(8)

最后筛选出相关性较低的256个像素点对计算ORB特征描述符。然后，利用Hamming距离等相似性度量方法进行特征点匹配。特征匹配中出现的误匹配点会降低目标定位精度，同时会导致机器人视觉伺服控制精度降低甚至失败，为了提高匹配精度，本文采用PROSAC算法剔除误匹配点[12]，图1为剔除误匹配点后的特征点匹配结果。

图1 剔除误匹配点后特征匹配结果

1.2 特征点分离

移动机器人视觉伺服过程中出现多个相同目标物体的时候，通过特征匹配方法将识别图像中所有的目标物体，而机器人视觉伺服需要选择唯一目标物体作为抓取目标。为此，通过引入特征点密度峰值聚类算法将匹配特征点进行分离，以实现对多个相同目标物体的分离，进而选择最佳视觉伺服目标。

匹配特征点分离流程如下：

(1)目标物体中心点估计

第一步需要寻找期望图像上目标物体的中心点，使用期望图像目标物体的几何中心点作为模板中心位置，记为C。期望图像中ORB特征点集合为T，当前图像上的特征点集合为S，(t,s)表示一个特征匹配对，满足t∈T且s∈S。期望图像特征点到中心位置的向量如式(9)所示

ri(ti)=p(C)-p(ti)

(9)

其中，p(C)表示期望图像上目标物体的中心点位置，p(ti)是期望图像上第i个特征点的图像坐标。

(10)

式中：θi=θs-θt，θs是当前图像上特征点的方向，θt是在期望图像上对应特征点的方向。

(2)特征点分离

(11)

ρq1≥ρq2≥…≥ρqN

(12)

(13)

图2 估计中心点分类

由不同中心点集中的估计中心点寻找与之对应的图像特征点，得到图像中每一个目标物体的图像特征点集。如此图像中不同目标物体上的特征点被分离，特征点分离的结果如图3所示。

图3 分离匹配特征点

2 视觉伺服目标选择

2.1 位置优先目标决策方法

通过特征点密度峰值聚类算法分离图像中各个目标物体之后，需在多个目标物体之中选择一个最佳视觉伺服目标。为此，本文提出一种基于位置优先的目标决策方法，利用机器人手眼关系计算各目标物体与机械臂末端的空间位置关系，以各目标物体位置作为视觉伺服目标的选择依据，为机器人视觉伺服选择最佳抓取目标。

摄像机模型反映图像坐标系与笛卡尔空间坐标系之间的关系，利用摄像机模型能够根据图像信息计算目标物体的空间位置，从而实现对目标物体的空间定位。摄像机模型本质上是像素坐标系、图像坐标系、摄像机坐标系和世界坐标系之间的转换关系，P(Xc,Yc,Zc)表示空间中的一点，其在图像坐标系的成像点为p(x,y)，f表示摄像机焦距，摄像机坐标系和像素坐标系的转换关系如式(14)所示

(14)

通过深度相机获取的RGB-D图像的深度信息和摄像机模型能够计算出目标物体在摄像机坐标系中的位置，然后通过机器人手眼关系将目标物体在摄像机坐标系下的位置信息转化为目标物体在机械臂末端坐标系下的位置信息，从而便于根据目标物体位置信息选择最佳目标物体。

手眼关系计算原理如图4所示，将标定板固定在机械臂末端上，控制机械臂运动到不同位置，通过机器人全局摄像机获取多组图像。其中，摄像机和标定板之间的外参矩阵Ci由摄像机标定实验获取。根据机器人机械臂机械结构，使用D-H参数法构建机械臂连杆坐标系，通过机械臂运动学分析[13,14]得到机械臂基坐标系与机械臂末端坐标系的位姿转换矩阵Ai。

图4 手眼关系计算原理

根据摄像机坐标系与机械臂基坐标系相对位姿不变条件可得到式(15)

A1·X·C1=A2·X·C2

(15)

由于标定板与机械臂末端固定在一起，所以式中X为恒定值，经过变换之后可以得到如下关系

(16)

将式(16)按照矩阵形式展开，利用等式左右元素相等，可以得到如下方程

RA·RX=RX·RC(RA-I)·tX=RX·tC-tA

(17)

(18)

仅通过就近原则选择最佳目标物体无法保证目标选择的唯一性，特殊情况下场景中可能存在多个目标物体同时被选择作为抓取对象。当根据距离最近方法同时选择多个视觉伺服目标时，则表示这些目标物体都处于以机械臂末端为球心的空间球面与桌面相交形成的圆上，此时需要根据辅助决策方法选择唯一的视觉伺服目标。图5为机器人结构俯视图，图中全局摄像头位于机器人顶端，机器人前方安装有置物台用于临时放置抓取目标物体。图中A、B、C表示根据距离就近原则同时选中的目标物体，3个目标物体与机器人机械臂末端的距离相同。通过机械臂正运动学分析与实验数据分析知，机械臂末端沿YH轴方向的运动由两个关节控制，沿XH方向的运动由3个关节控制，故相同距离下，物体距离轴YH越近，则机械臂的控制效率越高，所以物体A、C相比物体B更适合作为最佳视觉伺服目标。抓取目标之后，机器人需将物体放置于安装在机器人上的置物台上，所以选择距离置物台更近的物体A作为最佳视觉伺服目标。通过上述辅助决策方法和距离最近原则可选择唯一的视觉伺服目标，解决多目标视觉伺服中的目标选择问题。

图5 机器人结构俯视图

2.2 移动机器人视觉伺服

由于目标物体可能处于机器人机械臂抓取范围之外，所以移动机器人的视觉伺服过程分为两步。初始的时候，全局摄像机视角范围大，能够拍摄到当前视角下所有的目标物体。但是由于距离目标物体较远，获取的图像特征信息受环境影响较大，无法进行精确的视觉伺服控制，因此采用基于位置的视觉伺服方法，以保证不丢失目标。首先，通过全局摄像机获取图像，使用视觉伺服目标选择方法选定抓取目标。然后，根据目标物体与机械臂末端的位置信息计算得到机器人旋转和移动控制量，从而控制机器人靠近目标物体，并控制机械臂运动到期望位姿，使得目标物体处于局部摄像机视场之中。

当机器人靠近目标物体后，使用机械臂末端的局部摄像机获取图像，能够获得精确的特征点信息。视觉伺服中图像雅可比矩阵反映了图像特征变化与机械臂末端运动之间的映射关系，通过图像雅可比矩阵能将当前图像与目标图像的特征偏差转化为机械臂的控制量，进而实现视觉伺服。采用基于图像雅可比矩阵在线估计的无标定视觉伺服方法对机器人机械臂进行运动控制，能够有效避免摄像机标定和机器人模型的误差，控制机械臂精确抓取目标物体。无标定视觉伺服方法通过粒子滤波算法对当前时刻的图像雅可比矩阵进行在线估计，视觉伺服控制器根据当前图像与期望图像的特征偏差图像雅可比矩阵估计值计算得到机械臂各关节控制量。然后，机械臂关节控制器控制机械臂运动，使得图像特征误差不断减小，直到趋近于零，从而控制机械臂运动到期望位姿，实现对目标物体的准确抓取，这一阶段视觉伺服控制方法结构如图6所示。

图6 无标定视觉伺服方法结构

完整的视觉伺服目标选择流程如图7所示，通过ORB特征提取与匹配、误匹配点剔除、特征点分离和位置优先目标决策等方法，可从图像中多个相同目标物体中选择唯一的视觉伺服目标。

图7 视觉伺服目标选择流程

3 多目标抓取决策实验

本文搭建移动机器人作为实验平台，如图8所示。机器人视觉系统包括固定在头部的全局摄像机和固定在机械臂末端的局部摄像机，全局摄像机为奥比中光AstraPro深度摄像头，局部摄像机是普通USB摄像头。机器人基座为两轮差速驱动的移动平台，机器人右臂为使用六个数字舵机和一个机械手爪构建的六自由度机械臂，机器人上通过嵌入式计算机运行程序，实现移动机器人视觉伺服。实验在Windows7平台下编写程序，程序开发环境为Visual Studio2013，使用计算机视觉库OpenCV3.4.1实现目标识别和目标选择，并借助视觉伺服库VISP完成机器人视觉伺服控制，最终根据视觉信息完成对静态目标物体的正确抓取。

图8 机器人实验平台

3.1 目标识别实验

机器人在不同视点下使用全局摄像机获取5组图像，分别使用SIFT、SURF和ORB算法对获取图像进行特征点提取与匹配实验。实验中，ORB算法特征提取与匹配平均耗时为423 ms，SIFT算法平均耗时约为ORB算法的4.34倍，SURF算法平均耗时约为ORB算法的4倍，实验数据见表1所示。实验结果表明使用ORB算法特征提取与匹配耗时远远小于SIFT算法和SURF算法，在保证能够快速、准确识别目标物体的同时，更能够确保视觉伺服中目标识别的实时性，更适合用于移动机器人视觉伺服。

表1 目标识别算法耗时

3.2 多目标视觉伺服抓取实验

目标物体与机械臂末端相对位置和目标物体姿态都可能影响视觉伺服目标选择的决策结果，算法以目标物体与机械臂末端相对位置作为主要决策依据，为验证目标物体位姿对视觉伺服目标选择结果正确性的影响进行如下实验。

实验中，保证其它物体位置不变的前提下，改变同一位置下的单个目标物体的位姿状态，使用视觉伺服目标选择方法进行多次实验，并通过视觉伺服方法控制机器人抓取目标物体。图9(a)、图9(b)中方框选中目标分别为改变右边目标物体位姿前后通过视觉伺服目标选择方法选定的目标物体，分别进行3组视觉伺服实验。获取图像中右边目标物体相比左边的目标物体距离机器人右机械臂更近，多次实验中均选择右边的目标物体作为最佳视觉伺服目标，结果与实际相符合，验证目标物体位姿变化不会影响视觉伺服目标选择方法的正确性。

图9 不同位姿下目标选择结果

统计实验中目标物体与机械臂末端距离和抓取目标物体所耗时间，利用机器人视觉伺服耗时变化可以表示机器人视觉伺服受影响大小，见表2。由实验结果和表2中数据可知，改变视觉伺服目标前后机器人视觉伺服过程耗时变化较小，即验证视觉伺服目标位姿对机器人视觉伺服影响较小。

表2 机器人抓取不同位姿下的目标物体耗时

移动机器人在实际场景中抓取目标物体的时候，无法确定机器人与目标物体的初始相对位置，为此改变机器人初始位置进行多次视觉伺服实验。图10为实验中通过视觉伺服目标选择方法选定的抓取目标，由实验结果可知，机器人与目标物体初始相对位置对视觉伺服目标选择结果的正确性无影响，在任意初始位置选择的视觉伺服目标均为位置最优。表3为实验中通过算法计算得到的目标物体与机械臂的距离及统计获取的机器人视觉伺服耗时，由实验数据可知选定的视觉伺服目标为多个相同目标物体中距离机械臂末端最近的，且距离机械臂越远，视觉伺服耗时越长。

图10 视觉伺服目标选择结果

对比表2和表3中数据可知，目标物体与机械臂末端距离变化相比目标物体摆放位姿对移动机器人视觉伺服耗时影响更大，目标物体位姿对视觉伺服几乎无影响。实验结果表明目标物体位姿对位置优先目标决策方法的结果正确性无影响，验证了视觉伺服目标选择方法的正确性。

表3 视觉伺服实验数据

当实际应用场景中出现多个相同目标物体时，无法通过视觉伺服方法控制机械臂正确抓取目标物体。采用基于特征点密度峰值的视觉伺服目标选择方法选定目标物体后，移动机器人视觉伺服成功率在90%以上。图11为机器人视觉伺服过程，机器人先靠近目标物体，然后通过无标定视觉伺服方法控制机械臂准确抓取目标。

图11 移动机器人视觉伺服过程

4 结束语

视觉伺服方法在移动机器人实际应用中，多个相同目标同时出现在机器人视野中会导致机器人视觉伺服失败。针对移动机器人视觉伺服中出现的目标选择问题，本文提出了一种基于特征点密度峰值的视觉伺服目标选择方法，该方法可以在多个相同目标物体中选择一个最佳视觉伺服目标。为保证机器人视觉伺服实时性要求，通过ORB特征点匹配方法识别图像中的目标物体，并通过PROSAC算法剔除误匹配点，从而提高视觉伺服精度。将特征点密度峰值聚类算法引入视觉伺服，成功将图像中不同目标物体特征点分离，然后通过基于位置优先的目标决策方法选择最佳视觉伺服目标。通过视觉伺服目标选择方法有效解决了移动机器人视觉伺服中的目标选择问题，提高了机器人视觉伺服的成功率。最后，通过实验验证了视觉伺服目标选择方法的正确性，实验结果表明机器人在任意室内场景下任意位置，本文提出的视觉伺服目标选择方法均能在当前视场中选择唯一视觉伺服目标，并且不受目标物体位姿影响。本文研究内容对于机器人视觉伺服在实际中的应用具有一定的价值，能够提高移动机器人视觉伺服对室内场景的适用性。