双目视觉伺服的4-DOF机械手臂运动控制

张新良，冷正明，赵运基，付陈琳

(河南理工大学电气工程与自动化学院，河南焦作 454000)

0 引言

多自由度机械臂和工业机器人越来越多地应用于生产制造、机械加工、零件分拣及货物搬运等领域，这类机器人通过精确的示教作业，能完成复杂的点到点的动作，极大提高了生产加工效率[1]。然而传统的示教方法在生产环境和被控对象变化时，容易引起目标位置和姿态偏差，进而导致产品加工不合格或机器人任务执行失败[2]。为了保证机器人对生产环境的变化具有适应性，有必要引入目标识别和定位技术实时获取目标位置和姿态的变化，自动调整执行机构位姿，减少生产准备时间。基于机器视觉的目标识别和定位是其中一种重要的方法，其基于采集的含有目标物体的场景图像，结合摄像机的标定参数，利用空间几何映射关系，计算得到目标物体几何尺寸、形貌和位置等参数，具有非接触、动态响应快、效率高等特点，在场景建模[3]、目标识别[4]以及生产线上工件尺寸的在线测量[5]等有广泛的应用。相比于单目视觉技术，双目视觉不存在盲区，拥有更丰富的深度信息，可以解决大范围空间尺寸三坐标的现场测量问题。因此，基于双目视觉技术的机械手臂目标定位系统，在摆放、装配、采摘等作业中具有更广泛的应用价值[6-7]。本文以矩形目标物体为例，结合四自由度(4-DOF)的机械手臂执行机构，研究了基于双目立体视觉技术的目标物体识别、空间位姿测量以及运动定位控制等关键技术，结合机械手臂的动力学模型分析，给出了控制系统的实现方法。

1 4-DOF机械手臂控制系统

4-DOF机械手臂控制系统整体架构如图1所示。整个系统由视觉系统和机械手臂控制系统2部分组成，视觉系统以PC机为控制核心，采集和处理目标图像。机械手臂控制系统以DSP为核心，控制机械手臂抓取目标物体放置于目标位置。本文设计的机械手臂控制任务是抓取矩形类物体放置于矩形区域中。

图1 机器视觉辅助定位4-DOF机械手臂实验平台

控制系统的任务执行采用基于Eye-to-Hand双目视觉的位置控制方案，利用双目摄像机采集目标图像信息，在PC机上进行图像处理，完成目标的识别与空间位姿测量。然后将目标的位姿信息发送至DSP控制器，转化为机械手臂各自由度上的控制指令，驱动电机转动相应角度，使得末端夹持器以特定的姿态到达目标物体的位置，控制夹持器实现对目标物体的抓取与搬运，同时在机械手臂上安装传感器形成位置闭环，保证机械手臂的控制精度。

2 矩形目标识别与空间位姿测量

根据控制任务的要求，对于任意放置的矩形目标和放置区域的位置，4-DOF机械手臂需要按照矩形的长边方向抓取目标物体，按与放置区域长边一致的方向放置目标物体。因此，基于双目视觉的目标识别和位姿测量过程即是提取目标(目标物体和目标区域)的轮廓特征参数和矢量参数，利用图像匹配算法识别对象，结合双目视觉相机的标定参数，确定目标相对于机械手臂参考系的空间坐标。

2.1 矩形目标轮廓提取与匹配

为了保证检测出单像素边缘的图像轮廓，本文通过掏空二值化图像内部像素点，利用链码跟踪的方法提取和存储轮廓边缘信息[8]。首先对目标图像进行灰度化、滤波和图像阈值分割，得到二值图像。遍历图像中的每一个像素点，如果当前像素点灰度值为255，且周围相邻8个像素点灰度值也为255，则该点灰度值置0；如果当前像素点灰度值为0，该点灰度值仍置0；除上述2种情况的其他像素点灰度值设置为255。通过链码的方向进行轮廓跟踪，利用轮廓起始点坐标和相应的方向链码值连接轮廓边界，不同的轮廓通过结束标志区分，从而得到由链码表示的轮廓序列，保存图像中目标的轮廓信息。如图2所示的2个矩形轮廓，可以看出，采用的轮廓提取方法能够准确地提取目标的轮廓，较好地保留了目标的特征信息。

(a)目标物体和目标区域轮廓

(b)原始图像的轮廓信息

进一步，通过比较2个矩形轮廓的周长以及轮廓面积，便可识别矩形目标物体和目标区域，即周长/面积小的矩形轮廓为待抓取的目标物体，大矩形轮廓为待放置区域。

2.2 矩形目标矢量参数提取

矩形目标物体和目标区域的矢量信息决定了机械手臂末端夹持器抓取和放置动作的方向。本文利用目标的质心坐标和方向来计算目标的空间三维坐标和方向，得到图像中目标的矢量参数。

图像中第k个目标的质心坐标定义为

(1)

式中：m00为图像中目标的零阶图像矩；m10和m01为一阶图像矩。

(i+j)阶图像矩定义为

(2)

式中：i、j为非负整数；f(r,c)为图像中(r,c)处的灰度值，即当位于目标上时值为1，否则为0。

图像中第k个目标的方向角度θ定义为穿过该目标的长轴与x轴的夹角，即

(3)

式中：C11(k)、C20(k)和C02(k)分别为第k个目标的图像中心矩，定义为

(4)

分别将目标物体和目标位置轮廓坐标值带入式(1)和式(2)，即可求得其质心坐标和方向，如图3所示。

图3 矩形目标的质心和方向矢量参数

2.3 矩形目标空间位姿测量

采用立体匹配方法求取双目视觉相机采集的图像对中目标的视差，结合标定的双目视觉相机内外参数和三角测距原理，计算得到目标的空间三维坐标，实现目标的定位。立体匹配方法的原理是首先利用质心作为匹配点得到目标视差的估计值，对该值加减一定的视差余量来设定搜索范围。然后利用区域匹配，在右目图像中搜索与左目图像中目标质心相似度最高的匹配点从而得到视差[9]。

灰度图局部窗口之间灰度值的相似度使用去均值的归一化互相关(ZNCC)系数r表示，即,

(5)

则视差搜索范围表示为

r(x,y)=max({r(x,y,p)|(pc-dd

(6)

式中：pc为质心匹配时得到的目标视差值；ud、dd为对应视差值的上偏移量和下偏移量。

结合双目成像系统中的三角测距原理，以左目相机的光心作为坐标系的原点，计算得到目标在空间中的三维坐标，即

(7)

式中：Tx为双目视觉相机的基线长度，mm；(xl,yl)和(cx,cy)分别为左目相机图像坐标系的形心坐标和校正后的主点坐标;f为左目相机焦距，mm；xr为右目相机在图像坐标系的横坐标。

3 4-DOF机械手臂运动控制

基于目标的三维位置信息，对机械手臂进行运动学分析获得机械手臂各关节的变量，将期望位置坐标转换为各自由度上的旋转或伸缩位移，控制机械手臂的末端夹持器进行抓取和放置操作，完成控制任务。

4-DOF机械手臂如图4(a)所示，各自由度上的运动分别为：腰部机械臂的水平旋转运动；大臂沿水平导轨的直线伸缩运动；小臂垂直腰部平面的旋转运动；小臂末端的机械夹持器拾取。机械臂由5个关节组成，其中关节3和关节4共用一个步进电机，关节1、关节3、关节4和关节5为旋转关节，关节2为移动关节。选取机械手臂的基座中心作为空间坐标系的原点，对机械手臂进行坐标系建模，如图4(b)所示。其中X0-Z0表示机械臂基座的坐标系(基坐标系)，即关节1的坐标系，依次建立关节2～4的坐标系，X5-Z5表示机械臂末端夹持器的坐标系，也是夹持器的实际位姿。

(a)4-DOF机械手臂结构

(b)4-DOF机械手臂关节坐标系图4 4-DOF机械手臂结构及关节坐标系

利用D-H法对机械臂进行分析建模[10]，根据机械手臂的结构参数，建立其运动学模型如下：

x=[18.7+23.4cos(88.723 99-α)+λ]sinβ

(8)

y=8.7+23.4sin(88.723 99-α)

(9)

z=[18.7+23.4cos(88.723 99-α)+λ]cosβ

(10)

式中：(x,y,z)为机械臂夹持器的空间坐标；α、β和γ分别为小臂旋转角度、腰部旋转角度和大臂伸缩距离。

通过该正运动学模型和机械手臂各个关节的角度，计算可得机械手臂末端夹持器在机械手臂坐标系中的位置。反之，通过该运动学模型的逆模型可求解给定期望位置条件下各个关节的旋转角度和移动位移，完成4-DOF机械手臂运动路径的规划。

由式(8)～式(10)，4-DOF机械手臂逆运动学模型描述为：

(11)

(12)

(13)

4 实验验证

本文采用USB3.0通信协议双目视觉相机采集彩色未压缩的双目720P图像，速率31帧/s。双目视觉相机的基线距离为6 cm，分辨率为1 280×480，工作距离为48 cm，镜头焦距为4.3 mm。分别使用张正友平面标定法[11]和Brown算法[12]标定双目视觉相机的内参数矩阵、外参数矩阵和畸变参数向量，标定值如表1所示。

表1 双目视觉相机标定参数

4-DOF机械手臂DSP控制系统实验平台如文献[13]所述，关节的旋转或伸缩动作通过各自由度上的步进电机驱动，旋转角度和伸缩距离与电机脉冲个数nx、ny、nd的关系为：

α=0.002 25nx+30

(14)

β=0.002 25ny

(15)

λ=0.003 175nd

(16)

末端夹持器初始坐标为(0,30.85,28.7)cm，即各自由度上的初始位移α=30°，β=0°，γ=0 cm。图5为机械手臂和双目视觉相机的工作空间中任意放置的矩形目标物体和目标区域，4-DOF机械手臂末端夹持器按照矩形长边方向抓取目标物体并按与目标区域长边对齐的方式放置的工作过程。其中的22组目标位置测试实验数据如表2所示。

图5 机械手臂目标抓取与放置过程

编号目标物体坐标和方向视觉定位坐标和方向夹持器坐标和方向X/mmY/mmθ/(°)X/mmY/mmθ/(°)X/mmY/mmθ/(°)150-604548.7-59.94547.7-60.644.82-100-30120-100.4-29.2121.8-101.2-30.5122.33-50-30110-50.2-29.9111.0-48.9-30.9122.440-30800.1-30.480.51.9-29.881.3550-306050.3-30.459.750.3-27.561.66100-302099.6-32.421.8101.5-49.922.57-1000150-99.3-0.1149.6-99.22.2151.48-500120-48.4-0.9121.6-48.51.0122.1900900.00.090.00.20.592.5105006050.3-0.859.951.22.059.51110003098.2-2.429.797.21.631.512-10030160-100.131.1160.0-101.4-28.062.113-503080-49.429.979.4-49.930.881.214030600.828.759.949.628.659.51550304050.327.941.450.131.440.716100302099.827.421.899.832.221.517-10090110-100.192.7110.6-100.493.580.618-509070-48.790.269.0-48.391.971.619090500.888.650.51.391.550.52050903049.587.831.051.491.330.8211009020101.888.620.0101.390.619.9220120400.8119.741.51.1121.041.5

目标物体大小为52.0 mm×14.0 mm，目标区域大小为114.0 mm×53.0 mm，实验过程目标物体高度已知，故数据中的坐标只包含X、Y轴坐标。每次实验记录3类数据，即矩形目标物体实际的坐标和方向、视觉系统的定位坐标和方向及机械手臂末端夹持器到达目标物体时的实际坐标和方向。从表2可以看出，视觉系统的定位误差在3 mm以内，方向误差小于2°；机械手臂夹持器的抓取误差小于4 mm，方向误差最大值小于3°，具有较高的定位精度和控制精度，达到了机械臂控制系统的设计要求。

5 结论

本文针对生产线上工作的工业机器人生产柔性差，无法适应工作环境变化的情况，以4-DOF机械手臂为研究对象，设计了机器视觉辅助定位的运动控制系统，利用视觉系统定位矩形目标，实现目标的抓取与放置操作。设计的机械臂控制系统工作稳定，具有较高的定位和控制精度，对其他的工业机器人目标定位与抓取作业研究有一定借鉴意义。