基于双目视觉的采摘机器人机械臂控制∗

周毛等 张 栋 周 涛 陈 霞

（青岛理工大学信息与控制工程学院 青岛 266520）

1 引言

随着科技的不断进步与发展，采摘机器人将成为代替人力作业，提高农业产值的关键。不同种类的采摘机器人的产生，如西红柿采摘机器人［1］，苹果采摘机器人［2］，葡萄采摘机器人［3］等，都对农业科技的发展有着重要意义。在执行采摘任务的过程中，机械臂的控制是非常重要的，其中机械臂正逆运动学建模与求解和目标定位是采摘机器人机械臂控制的关键问题。同时，采摘机器人通常还面临一些非结构化的和复杂的外部环境，为使采摘机械臂具有从未知环境中获取信息的能力，需要给采摘机械臂配备外部传感器，双目视觉传感器作为采摘机器人的“眼睛”，可以用来感知环境以及实现农业目标果实的识别和获取目标物体的三维坐标信息，为采摘机械臂的控制带来方便。能否准确、快速识别出采摘目标，直接影响着采摘效率。因此，农业目标果实的识别也是采摘机器人的关键技术之一，吸引着广大学者的深入持续研究［4～5］。

本文首先采用经典 D-H［6～8］方法对采摘机械臂进行数学建模，在此基础上进行逆运动学求解以获得满足末端执行器位姿的各关节角，并采用颜色分割［9～11］的方法对目标番茄进行识别，根据识别出的番茄目标三维坐标信息来控制采摘机械臂完成采摘任务。最后通过实验，表明所设计的采摘机器人果实识别与采摘成功率较高。

2 采摘机械臂的数学模型

机械臂数学模型的建立是正逆运动学的重要部分，本系统采用6自由度机械臂，通过D-H（dena⁃vit-hartenberg）法建立采摘机械臂的数学模型。采摘机械臂参考坐标系如图1所示，根据D-H法可以得出采摘机械臂的D-H参数，如表1所示。

图1 采摘机械臂各关节参考坐标系

表1 D-H参数

表1中，θ表示绕z轴的旋转角；d表示在z轴上两条相邻公垂线之间的距离；α表示两个相邻z轴之间的旋转角；a表示相邻两个关节轴线方向的公垂线的长度。相邻坐标系变换矩阵可通过表示四个运动的矩阵相乘得到：

机器人的正运动学方程建立如下，由于关节均为旋转关节的串联机械臂，其中关节和连杆参数dj、αj、和 aj均为常数。

3 采摘机械臂运动学逆解

逆运动学问题在机器人运动学研究中占有重要的地位，直接影响着控制的精确性。根据已知的末端执行器的位置和姿态，求解相应的关节角。

若要求θ1，可将上式两边同时乘以0T1(θ1)-1，得到：

再由式（1）两边矩阵对应元素相等，消去θ2，θ3，θ4，θ5，θ6后，求得 θ1。同理由：

求得 θ2；

求得 θ3；

求得 θ4；

求得 θ5和 θ6。

4 采摘机械臂关节空间运动

图2 采摘机械臂的三维模型

根据采摘机械臂的D-H参数，建立采摘机械臂的模型如图2所示，并对随时间变化的各关节角做仿真，关节运动轨迹是平滑的如图3所示，末端执行器在笛卡儿空间运动轨迹如图4所示。

图3 随时间变化的关节坐标

图4 末端执行器随时间变化的笛卡儿位置

5 目标的识别与定位

对基于视觉的采摘机械臂来说，视觉是非常重要的部分。只有在视觉的引导下，才能获得目标的正确信息，从而控制机械臂完成指定的任务工作。采摘机器人的视觉系统在标定的基础上，识别和定位目标对象。

5.1 双目测距的原理

双目测距原理［12～14］如图5所示，两个摄像机成像平面位于同一平面，光轴平行，基线距离一定，焦距相等，假设主点和已经校准，在左右图像上具有相同的像素坐标。一般情况下，相平面很少与镜头完美重叠，图像中心点也几乎不会和像主点重合，引入cx和cy来模拟图像中心可能的位移。P是空间一点，P世界坐标系为(X，Y，Z)，左右图像上的成像点 pl和 pr，对应的横坐标为xl和xr，假设左边相机坐标系为世界坐标O-XYZ，左边图像坐标系为ol-xlyl，右边图像坐标系为or-xryr，视差d=xl-xr。由透视变化三角几何关系得：

其中 f表示相机的焦距，T表示两相机之间的基线长，焦距 fx实际上透镜的物理距离长度与成像仪每个单元尺寸sx的乘积（sx的单位是像素/毫米，物理焦距的单位是mm，fx的单位是像素）。

图5 双目测距原理图

5.2 目标识别

（a）原始图像；（b）用伪色彩显示的像素分类（C=4）；（c）在xy色度二维空间中的点集中心；（d）类型c=2的所有像素；（e）用半径为15的圆形结构化元素进行形态学开操作后的结果；（f）用半径为4的圆形结构化元素进行形态学闭操作后的结果

基于颜色的图像分割是一种直观有效的方法，首先把每一个彩色像素转换成xy色度坐标，使用K均值算法［15］找出平面上的点集，且每一个点集对应一种可辨识颜色的像素群。K均值算法必须指定要寻找的点集数量，该场景有4种不同颜色的元素：红色番茄、有点黄叶子、橘色番茄、灰暗的背景，像素被类聚到4个色度类型。图像中的像素具有值 c={0，1，2，3}，它表明相应的输入像素被分配到了哪一类。选择属于类型2的像素，它是一个局部图像，类型2的所有像素被显示为白色，对应原始图像中的红色目标。番茄由于镜像反射都出现了空洞，通过形态学操作来提高图像处理结果，把图像像素分为目标和非目标。

5.3 目标定位

为实现采摘机械臂对番茄的自主采摘，关键是获取番茄的三维空间位置，以便为末端执行器动作提供参数。本文通过颜色分类的方法识别出番茄目标，确定番茄的数量及每个番茄对应的像素点二维坐标。假设第i个番茄对应像素点构成集合si，该番茄的质心坐标(xi，yi)可由下式求得：

其中：n为集合si中元素的个数；(xk，yk)为si中的元素。在得到每个番茄质心二维坐标［16］后，由双目得到深度图像中对应像素点的深度值，利用OpenCV中的reprojectimageTo3D函数，可以计算出每个番茄质心的三维坐标值。从而采摘机械臂实现目标定位，控制器控制采摘机械臂将目标番茄摘下。

6 实验

为验证采摘机械臂控制、目标识别定位的准确性与有效性，在室内模拟场景下进行实验。

6.1 采摘机器人硬件介绍

采摘机器人由6自由度机械臂构成，底部由轮子组成。采用莱娜机器视觉公司的HNY-CV-002作为双目视觉传感器，通过提取目标物体的颜色信息进行RGB空间的分割，最终得到目标物体的三维坐标。采摘机器人的主控制器是Raspberry Pi 3B。

6.2 实验过程

实验通过分析番茄目标物体实际坐标位置与末端执行器到达位置之间的误差，从而证明机械臂算法与识别定位的正确性。视觉系统处理图像速度为30帧/s，每一帧图像的分辨率为640×360。本实验用于近距离测量（500mm内），实验得出采摘机器人计算得到的位置与目标番茄实际位置之间的平均误差为（4.8，9.2，5.3）（mm），实验误差在允许误差范围内，采摘机器人可以完成自主采摘任务，文中的方法得到验证。采摘机器人采摘的流程图如图6所示。

图6 采摘机器人采摘流程图

7 结语

首先用D-H方法对采摘机械臂进行建模，给出了6自由度机械臂逆解算法，并采用颜色分割的方法对目标果实进行识别，根据识别出的番茄目标三维坐标信息控制机械臂完成采摘任务。最后，在室内模拟农业现场场景中进行实验，实验结果表明所设计的采摘机器人对果实识别与采摘成功率较高。