《工业控制计算机｝２０１３年第２６卷第８期
４７
机械臂视觉伺服控制系统的设计
ＤｅｓｉｇｎｏｆＭａｎｉｐｕｌａｔｏｒＳｅｒｖｏＳｙｓｔｅｍＢａｓｅｄ
ｏｎ
ＢｉｎｏｃｕｌａｒＶｉｓｉｏｎ
江杰王鹏月（内蒙古科技大学信．Ｅ－工程学院，内蒙古包头０１４０１０）
摘要
针对传统机械臂无法更好的适应外部坏境的变化，提出了基于双目视觉的机械臂伺服控制系统的设计。首先用Ｄ—Ｈ建模法对机械臂进行运动学分析以及运动学求解，然后利用数字图像处理技术，基于ＳＵＲＦ特征完成目标物体的识别以及双目立体视觉的测量，从而为机械臂的动作提供目标物体在空间中的坐标。最后，对目标测距的精度进行了结果验证，并完成抓取动作。
关键词：双目视觉，机械臂，伺服控制
ＡｂｓｔｍｃＩ
Ｉｎ
ｔｈｅ
ｖｉｅｗ
ｏｆｔｈｅｔｒａｄｉｔｉｏｎａｌｒｏｂｏｔｏｆ
ｒｏｂｏｔ
ａｒｍ
ｓｅｒｖｏ
ａｒｍ
ｉｓｕｎａｂｌｅｔｏａｄａｐｔｔｏｃｈａｎｇｅｓｏｆｔｈｅｅｘｔｅｒｎａｌｅｎｖｉｒｏｎｍｅｎｔ．ｔｈｉｓｓｙｓｔｅｍ
ｂａｓｅｄ
ｏｎ
ｐａｐｅｒｐｕｔｓｆｏｒｗａｒｄｒｏｂｏｔ
ａｒｍ
ｂｙ
Ｄ－Ｈｄｉｇｉｔａｌ
ｄｅｓｉｇｎ
ｃｏｎｔｒｏｌ
ｂｉｎｏｃｕｌａｒｖｉｓｉｏｎ．Ｋｉｎｅｍａｔｉｃｓ
ｂｉｎｏｃｕｌａｒ
ｔｈｅ
ａｎａｌｙｓｉｓ
ｖｉｓｉｏｎ
ｏｆｔｈｅ
ｍｏｄｅｌｉｎｇｍｅｔｈｏｄ，ａｎｄ
ｔｈｅｎ
ｃｏｍｐｌｅｔｅ
ｔｈｅｔａｒｇｅｔ
ｏｎ
ｏｂｊｅｃｔｒｅｃｏｇｎｉｔｉｏｎａｎｄ
ｓｔｅｒｅｏｍｅａｓｕｒｅｍｅｎｔｔｈｅ
ｔａｒｇｅｔ
ｕｓｉｎｇ
ｉｍａｇｅｐｒｏｃｅｓｓｉｎｇ
ａｒｍ．
ｔｅｃｈｎｏｌｏｇｙ，ｂａｓｅｄ
ＳＵＲＦｆｅａｔｕｒｅ．Ｉｎｏｒｄｅｒｔｏ
ｐｒｏｖｉｄｅｄ
ｃｏｏｒｄｉｎａｔｅｓｏｆ
ｏｂｊｅｃｔｆｏｒｒｏｂｏｔ
Ｋｅｙｗｏｒｄｓ：ｂｉｎｏｃｕｌａｒｖｉｓｉｏｎ．ｒｏｂｏｔａｒｍ，ｓｅｒｖｏｃｏｎｔｒｏｌ
视觉伺服控制系统按摄像头的数量可以分为单目视觉伺服控制和双目视觉伺服控制［１－－３］‘，本论文采用的双目视觉技术，通过双目视觉测得目标物体在空间中的位置坐标，从而控制机械臂完成对目标物体的抓取。在对双目立体视觉技术以及机械臂运动学建模等相关理论研究的基础上，搭建了一个简单的机械臂视觉伺服控制实验平台，该实验平台主要由图像采集单元、图像处理计算机、机械臂本体以及机械臂控制器几部分组成。双目立体视觉部分主要研究了视觉系统的标定【４］、立体校正、特征提取㈣以及立体匹配等相关理论，并依据相关理论，完成了算法的设计。
１
图１
机械臂装置实物图
参数如表１所示。
裹１机械臂Ｄ—Ｈ参数裹
其中Ｌ１、Ｌ２、Ｌ３为已知的，可以通过直尺进行测量，在这里分别为７５、１７０、２１５，其中角度的大小对末端执行机构的位置没有影响，该角度只是用来控制机械爪的抓取姿态，而对坐标位置有影响的角度只是ｅｌ，ｅ２和０３。把表１中的参数带入公式中，可得每个关节之间的变换矩阵，其结果如式（１）所示。
Ｆｃｏｓｅ，ｏ—ｓ栅ｆ￡．ｆｃｏｓ８门
机械臂运动控制
朝向ｎＨ力＿Ｘ
Ａ，竹二＝＾≯【．０Ｊ
０
～
０７
１．１机械臂建模
对机械臂本体而言，其是一个复杂的系统，其包括
嗍帆。。
九
嚣；嚣；
㈣警，
００ＤＤ
７
Ｆｃｏｓｅ３
ｏ—ｓｉｍ３Ｌ３ｃｏｓ８３
ｏ
—１
Ａｌ喜ｓ押∞３。１
００
ｃ躐３Ｌ３ｓ确３
００
Ｏ１
了运动学以及动力学等多方产ｉｉ：＋：
面学科，而运动学分析则是对机械臂的空间姿态和各个关节角度之间的关系进行建模分析，其是机械臂运动学中的重要部分。本系统采用４自由度机械臂。其实物图如图１所示，该机械臂的Ｄ—Ｈ模型［６－７］效果如图２所示。
利用Ｄ—Ｈ法，根据图２中建立的各个关节的坐标系，得到的该机械臂的Ｄ—Ｈ
圈２
ｉｐ¨！ｉ
彳
删帆Ｄ
Ｄ
Ｄ
０
Ｄ７
则机械臂的基座和手爪之间的总变换矩阵如式（２）所示：
Ｒ
ｒ三莩
■
Ｌ＝Ａ，＾肖３Ａ４
ｃｌｃ４ｃ∞一ｓｌｓＩ—Ｃｌｓ４ｃ≈一Ｓ１Ｃ４
ｓｌｃ４ｃ口＋ｃｌｓ４
ＣＩｓ∞Ｌ１Ｃ１＋Ｌ２Ｃ１ｃ２＋Ｌ３Ｃｌｃ∞
（２）
Ｃｌｃ４一ｓｔｓ４ｃ∞ｓｔｓ∞Ｌｔｓｌ＋Ｌ２ｓｌｃ２＋Ｌ３Ｓ１ｃ∞
ｓ４ｓ∞
ｃ２３
一ｃ４ｓ∞
一Ｌ２ｓ２一Ｌ３ｓ∞
７
０００
上式中，ｓ。代表ｓｉｎｅ。，ｃ。代表ｃｏｓｅ。，Ｓ∞代表ｓｉｎ（Ｂ２＋ｅ３），ｃ蕾代表ｃｏｓ（ｅ２＋ｅ３）。１．２机械臂逆运动学求解
Ｚ５
机械臂Ｄ—Ｈ模型效果图
在实际应用中，我们往往知道目标物体所在空间中的位置，
机械臂视觉伺服控制系统的设计
而要求控制每个关节转动相应的角度使机械臂末端执行机构到达空间目标位置，这就需要知道每个关节的转动角度，所以逆运
，，，，，，，，，，Ｊ
动学分析往往更加重要，它是机器人运动规划和轨迹控制的基础，有了逆运动学求解，就可以确定每个关节变量的大小，所以，逆运动学求解是对机械臂控制以及系统编程的理论依据。
若机械臂末端执行机构的位姿Ｔ已知，在这里表示为：
Ｉ以ｑ毛以ｌ
ｑ邑Ｂ
８Ｌ—ｑＩｌ亿呸乞Ｂ
（３）
～～
、
’
、＾
，厂“
．！
Ｔｄ＝）【，一ｘｒ
飞
图３双目视觉成像原理图
【＿ｏ
ｏｏ
１ｊ
理世界坐标系中的点Ｐ在左右摄像机上的成像点为Ｐ，和Ｐ，，其在图像坐标系下的坐标为（Ｘ，Ｙ１）和（）（ｒ，ｙｒ），由三角几何关系可得：
前面式（２）我们已经求出机械臂的总变换矩阵，为了对关节角度进行求解，我们从Ａ。’开始，依次用Ａ，’左乘式（２）和式（３）的两个矩阵，化简可得：
Ｘ『＝ｆ拿
‘．Ｃ
（１３）
－ｎ，ｃ，＋ｑｓ，ＯｘＣ，＋ｑｓ，吼ｃ，十邑ｓ，以ｃ，＋ｐｙｓ，－Ｌ，］
一以
＿：ｆ（ｘｏ，－Ｔ）．
‘ｃ
（１４）
一呸一岛—见
ＢＣｌ－ＰｘＳ，
１
０
ｑｃｌ一仉ｓ，ｑｃ广ｑｓＴａｙｃ广氐ｓ，
Ｙｌ＝ｉｔ＝埠
‘．Ｃ
（１５）
Ｌ
００
ｊ
㈤
‘‘
上式中（Ｘｃ，Ｙ。，ｚ。）为点Ｐ在左摄像机坐标系下的坐标，Ｔ为基线距，视差定义为某一点在左右摄像机图像中对应点的位置’差，即ｄ＝ｘ．一ｘ，，由上面的公式，可以得到空间上的点Ｐ在左摄像机坐标系中的坐标为：
ｒ％ｑ一％ｓ４％ＬｃＰ＋Ｌ％］
＝｛警一言”“０驴０匕％Ｊ
ｌ文
乞０
Ｌ
００
７
ｊ
（５）（６）（７）
ｆ。一『．■ｊ，，一卜Ｍ
ｌ，一卜，
根据式（４）中矩阵中对应元素相等，可得：Ｂｃ７＋ｐｙｓ广Ｌ，＝Ｌ２岛＋ｂｃ白－ｐ，＝Ｌ２ｓ２＋Ｌｓ％
ＰｙＣ，－ＰｘＳ，＝Ｄ
ｌ～一丁
ｌ’。一１『－
（１６）
ｒｃ一了
２．２目标特征提取及匹配
在对目标物体进行测量之前，首先要对目标物体进行识别，在这里采用目标模板，提取目标模板的特征，然后跟实时采集的图像进行特征匹配，从而实现对目标物体的识别，在这里采用ＳＵＲＦ特征作为图像的特征点。
在ＳＵＲＦ特征点的提出过程中利用了积分图像、Ｈａｒｒ小波以及Ｈｅｓｓｉａｎ矩阵等运算，从而大大缩短了计算时间，对Ｈａｒｒ小波的利用也增加了特征点的鲁棒性，利用近似Ｈｅｓｓｉａｎ矩阵行列式局部最大值计算得到的特征点是其所在邻域内的像素中
最亮或者最暗的点。
因为关节２和关节３是平行的，所以单独左乘Ａ。。不会产
生有用的结果，下一步要左乘≮。Ａ：。Ａｊｌ，化简并使对应元素相
等，可得：
一￡．，ｃ＿∞一Ｌ２ｃｊ—Ｌ３＋Ｐｘｃ，９却＋ｐ，ｓＴｃ＿∞一ｐ，ｓａ＝Ｏ
（８）
一￡．，％一Ｌ２ｓ３＋ｐｘｃ，％＋Ｂｓ，％＋Ｂ％＝Ｄ仉ｃ，％十ｎ，ｓＴ％＋亿％＝岛
一ｏ，ｓ，＋０ｖｃ，＝ｑ
（９）（１０）（１１）
联立式（５）一式（１１），则可求出对应关节的变量值，在实际求解过程中，有的关节变量值会有多个解，出现这种情况是机械臂末端执行机构到达所需的指定位姿存在不同的关节角度，而根据机械臂实际情况，要对机械臂关节的运动角度以及轨迹进行限定，通过限定条件，则可以得出关节变量有且只有一个唯一的值，求得的关节变量值如下式所示：
给定图像ｆ中的一点ｆ（ｘ，Ｙ），Ｈｅｓｓｉａｎ矩阵Ｈ（ｘ，叮）在ｘ处
尺度为仃的定义如式（１７）所示：
愀∽：∥托盯：一：：：’＝：］【～Ｌ×，仃）～【ｘ，仃，Ｊ
１２‘
（１７）
０，＝ｓｒｃｔａ几（鲁）ｃ一号硼，＜号，
０２＝ａｒｃｓｉｎ（ｓｉｎｅ川Ｄ也＜多）
式中，ｂ（ｘ，叮）为高斯二阶偏导数芸在Ｘ处与图像ｆ的卷
ａＸ。
积，ｂ（ｘ，盯）和ＬＷ（ｘ，盯）类似。
矩阵Ｈ（ｘ，盯）的行列式计算公式如下所示：
／
。４
归陀淄（监孽２鬈Ｌ盛幺）ｃ嗨＜和门２’
。
＼
ｄｅｔ（Ｈ）＝Ｌ。（ｘ，ｏ－）￡－ｗ（ｘ，ｏ－）一～（ｘ，ｏ－）。
（１８）
｝Ｌ・ｓ
对上行列式进行计算，当结果为负数时，其特征值的符号不同，则该点为非极值点，当结果为整数时，其特征值同为正数或同为负数，该点为极值点。
通过模板特征匹配得到的目标匹配效果图如图４所示。同样，根据ＳＵＲＦ特征，进行左右图像目标物体的特征匹配，然后找到对应的匹配点，利用上面介绍的双目测距利用，根据对应匹配点的坐标，计算得到目标物体在空间坐标中的位置。在实际匹配中，存在许多误匹配点，需要根据立体匹配准则进行误匹配剔除，最后根据目标中心位置的特征匹配点在左右图像
０４－－－－－－，＇ａｒｃ胁（鲁）（一号硼。＜≥，
２目标识别与定位２．１双目立体视觉模型
图３为双目视觉成像原理图，假设两台摄像机的像平面平行，光轴严格平行（光轴是从投影中心Ｏ朝像主点ｃ方向引出的一条射线，也称为主光线），距离一定，焦距相同ｆＦｆｒ＝ｆ。
进一步假设两幅成像图像之间的行像素是严格对准的，物
《工业控制计算机＞２０１３年第２６卷第８期
据如表２所示，表中的坐标值是以右摄像机坐标系为参考坐标系，单位为ｍｍ，Ｚ７为Ｚ方向的实测距离。
从表２的测距实验结果中可以看出，当目标物体距离摄像机２０ｃｍ一４０ｃｍ之间时，测量的精度比较高，测量误差控制在１％左右，绝对误差在２～３ｍｍ左右。当距离摄像机较近，测得的误差较大，不能够满足系统的性能要求，这是由于较近时目标物体严重．畸变引起的。当目标物体距离摄像机５０ｃｍ左右时，虽然测量精度在２％左右，但由于此时绝对误差已经较大，仍不能够满足系统的要求，这是因为当目标物体距离摄像机越远，摄像机的非线性
圈４目标特征匹配效果图
特征表现的越突出，从而加大了双目立体视觉的测量误差，另一方面是因为目标物体较远，特征的提取比较困难，而特征提取的不准确性也相应导致了测量精度的下降。在实际控制中，机械臂关节处的舵机采用了位置反馈，机械臂末端夹持器到达的位置与视觉测量值近似相等，误差较小，而系统的误差主要是由视觉部分的测量误差引起的，所以当目标距离摄像机较近或较远时，系统的控制精度较大，此时，提高视觉的测量精度为首要任务。
４结束语
中的坐标进行目标物体空间坐标的计算。常用的立体匹配约束有极线约束、相容性约束、唯一性约束、顺序约束以及视差连续性约束等等。其中心位置特征匹配效果图如图５所示。
本文首先通过Ｄ—Ｈ法对机械臂建模并进行运动学求解，采用双目视觉对目标物体进行空间位置的测量，通过实验平台的验证，基本实现了预期的设计目标，完成了对目标物体的抓取动作，但当目标物体较劲或较远时，视觉的测量精度还有待进一步改进。
图５左右摄像机目标中心特征匹配效果图
根据匹配结果．目标识别及物体空间坐标结果图如图６所示
统，２００６，１２（１）；３２－３５
参考文献
［１］庞云亭，黄强，郑娟，等．仿人机器人的视觉伺服控制系统［Ｊ］．伺服系
［２］Ｄ．Ｂ．Ｚｈａｎｇ，Ｌ．Ｖ．Ｇ００１．Ａ．Ｏｏｓｔｅｒｌｉｎｃｋ。Ｓｔｏｃｈａｓｔｉｃｐｒｅｄｉｃｔｉｖｅ
ｏｆ
ｃｏｎｔｒｏｌ
ｒｏｂｏｔ
ｔｒａｃｋｉｎｇｓｙｓｔ—ｅｍｓ
ｗｉｔｈ
ｄｙｎａｍｉｃ
ｖｉｓｕａｌｆｅｅｄｂａｃｋ。ｉｎ
Ｐｒｏｃ【Ｃ】．ＩＥＥＥ
６１０－６１５
Ｉｎｔ７ｌＣｏｎｆ．ｏｎＲｏｂｏｔｉｃｓａｎｄＡｕｔｏｍａｔ－ｉｏｎ，１９９０：
［３］薛定宇，项龙江，司秉玉，等．视觉伺服分类及其动态过程［Ｊ］．东北大
学学报，２００３。２４（６）：４３—５４７［４］ＦａｕｇｅｒａｓＯ
ａｎｄ
Ｄ，Ｔｏｓｃａｎｉ
Ｇ．Ｔｈｅｃａｌｉｂｒａｔｉｏｎ
ｐｒｏｂｌｅｍ
ｏｎ
ｆｏｒｓｔｅｒｅｏ
［Ｃ】．ＩＥＥＥＣｏｍｐｕｔｅｒＳｏｃｉｅｔ－ｙＣｏｎｆｅｒｅｎｃｅＣｏｍｐｕｔｅｒＶｉｓｉｏｎ
ＰａｔｔｅｒｎＲｅｃｏｇｎｉｔｉｏｎ．ＭｉｎｍｉＢｅａｃｈ．Ｆｌｏｒｉｄａ。１９８６：１５—２０
［５］田村秀行，金喜子，乔双．计算机图像处理［Ｍ］．北京：科学出版社，
２（）０４
［６］ＢｅｎＳｈｎｅｉｄｅｒｍａｎ，ＣａｔｈｅｒｉｎｅＰｌａｉｓａｎｔ．ＤｅｓｉｇｎｉｎｇｔｈｅＵｓｅｒＩｎ－
ｔｅｆｆａｃｅ—Ｓｔ—ａｔｅｇｉｅｓｆｏｒＥｆｆｅｃｔｉｖｅＨｕｍａｎ—ＣｏｍｐｕｔｅｒＩｎｔｅｆａｃｔｉｏｎ
［Ｍ】．Ａｍｅｒｉｃａ：ＰｅａｒｓｏｎＥｄｕｃｔｉｏｎ。２０Ｑ５：５４—５５
图６目标识别及测距效果图
３实验及结果分析
［７］Ｓｈａｈｉｎｐｏｏｒ，Ｍｏｈｓｅｎ．ＡＲｏｂｏｔＥｎｇｉｎｅｅｒｉｎｇＴｅｘｔｂｏｏｋ［Ｍ］，Ｈａｒｐｅｒ
＆Ｒｏｗ．１９８７
双目视觉对目标物体的测量精确直接影响着系统的控制精确度，对目标物体在空间中的不同位置进行测量，得到的实验数
表２空间目标物体双目立体视觉测距结果
［收稿日期：２０１３．４．２６］
区曩擅囊推荐引钡行业变革
全新改版
详情请登陆：ｈｔｔｐ：／／ｗｗｗ．ｇｋｏｎｇ．ｃｏｍ／ｃｏ／ｇｋｏｎｇ／ｆｒｅｓｈ．ｈｔｍ
薹控行业交流交易翠台
机械臂视觉伺服控制系统的设计
作者：作者单位：刊名：英文刊名：年，卷(期)：
江杰， 王鹏月
内蒙古科技大学信息工程学院,内蒙古包头,014010工业控制计算机
Industrial Control Computer2013,26(8)
参考文献(7条)
1. 庞云亭;黄强;郑娟 仿人机器人的视觉伺服控制系统 2006(01)
2. D.B.Zhang;L.V.Gool;A.Oosterlinck Stochastic predictive control of robot tracking syst-ems with dynamic visualfeedback 1990
3. 薛定宇,项龙江,司秉玉,徐心和 视觉伺服分类及其动态过程[期刊论文]-东北大学学报(自然科学版) 2003(6)4. Faugeras O D;Toscani G The calibration problem for stereo 19865. 田村秀行;金喜子;乔双 计算机图像处理 2004
6. Ben Shneiderman;Catherine Plaisant Designing the User Interface-St-ategies for Effective Human-ComputerInteraction 2005
7. Shahinpoor,Mohsen A Robot Engineering Textbook 1987
引用本文格式：江杰. 王鹏月 机械臂视觉伺服控制系统的设计[期刊论文]-工业控制计算机 2013(8)
《工业控制计算机｝２０１３年第２６卷第８期
４７
机械臂视觉伺服控制系统的设计
ＤｅｓｉｇｎｏｆＭａｎｉｐｕｌａｔｏｒＳｅｒｖｏＳｙｓｔｅｍＢａｓｅｄ
ｏｎ
ＢｉｎｏｃｕｌａｒＶｉｓｉｏｎ
江杰王鹏月（内蒙古科技大学信．Ｅ－工程学院，内蒙古包头０１４０１０）
摘要
针对传统机械臂无法更好的适应外部坏境的变化，提出了基于双目视觉的机械臂伺服控制系统的设计。首先用Ｄ—Ｈ建模法对机械臂进行运动学分析以及运动学求解，然后利用数字图像处理技术，基于ＳＵＲＦ特征完成目标物体的识别以及双目立体视觉的测量，从而为机械臂的动作提供目标物体在空间中的坐标。最后，对目标测距的精度进行了结果验证，并完成抓取动作。
关键词：双目视觉，机械臂，伺服控制
ＡｂｓｔｍｃＩ
Ｉｎ
ｔｈｅ
ｖｉｅｗ
ｏｆｔｈｅｔｒａｄｉｔｉｏｎａｌｒｏｂｏｔｏｆ
ｒｏｂｏｔ
ａｒｍ
ｓｅｒｖｏ
ａｒｍ
ｉｓｕｎａｂｌｅｔｏａｄａｐｔｔｏｃｈａｎｇｅｓｏｆｔｈｅｅｘｔｅｒｎａｌｅｎｖｉｒｏｎｍｅｎｔ．ｔｈｉｓｓｙｓｔｅｍ
ｂａｓｅｄ
ｏｎ
ｐａｐｅｒｐｕｔｓｆｏｒｗａｒｄｒｏｂｏｔ
ａｒｍ
ｂｙ
Ｄ－Ｈｄｉｇｉｔａｌ
ｄｅｓｉｇｎ
ｃｏｎｔｒｏｌ
ｂｉｎｏｃｕｌａｒｖｉｓｉｏｎ．Ｋｉｎｅｍａｔｉｃｓ
ｂｉｎｏｃｕｌａｒ
ｔｈｅ
ａｎａｌｙｓｉｓ
ｖｉｓｉｏｎ
ｏｆｔｈｅ
ｍｏｄｅｌｉｎｇｍｅｔｈｏｄ，ａｎｄ
ｔｈｅｎ
ｃｏｍｐｌｅｔｅ
ｔｈｅｔａｒｇｅｔ
ｏｎ
ｏｂｊｅｃｔｒｅｃｏｇｎｉｔｉｏｎａｎｄ
ｓｔｅｒｅｏｍｅａｓｕｒｅｍｅｎｔｔｈｅ
ｔａｒｇｅｔ
ｕｓｉｎｇ
ｉｍａｇｅｐｒｏｃｅｓｓｉｎｇ
ａｒｍ．
ｔｅｃｈｎｏｌｏｇｙ，ｂａｓｅｄ
ＳＵＲＦｆｅａｔｕｒｅ．Ｉｎｏｒｄｅｒｔｏ
ｐｒｏｖｉｄｅｄ
ｃｏｏｒｄｉｎａｔｅｓｏｆ
ｏｂｊｅｃｔｆｏｒｒｏｂｏｔ
Ｋｅｙｗｏｒｄｓ：ｂｉｎｏｃｕｌａｒｖｉｓｉｏｎ．ｒｏｂｏｔａｒｍ，ｓｅｒｖｏｃｏｎｔｒｏｌ
视觉伺服控制系统按摄像头的数量可以分为单目视觉伺服控制和双目视觉伺服控制［１－－３］‘，本论文采用的双目视觉技术，通过双目视觉测得目标物体在空间中的位置坐标，从而控制机械臂完成对目标物体的抓取。在对双目立体视觉技术以及机械臂运动学建模等相关理论研究的基础上，搭建了一个简单的机械臂视觉伺服控制实验平台，该实验平台主要由图像采集单元、图像处理计算机、机械臂本体以及机械臂控制器几部分组成。双目立体视觉部分主要研究了视觉系统的标定【４］、立体校正、特征提取㈣以及立体匹配等相关理论，并依据相关理论，完成了算法的设计。
１
图１
机械臂装置实物图
参数如表１所示。
裹１机械臂Ｄ—Ｈ参数裹
其中Ｌ１、Ｌ２、Ｌ３为已知的，可以通过直尺进行测量，在这里分别为７５、１７０、２１５，其中角度的大小对末端执行机构的位置没有影响，该角度只是用来控制机械爪的抓取姿态，而对坐标位置有影响的角度只是ｅｌ，ｅ２和０３。把表１中的参数带入公式中，可得每个关节之间的变换矩阵，其结果如式（１）所示。
Ｆｃｏｓｅ，ｏ—ｓ栅ｆ￡．ｆｃｏｓ８门
机械臂运动控制
朝向ｎＨ力＿Ｘ
Ａ，竹二＝＾≯【．０Ｊ
０
～
０７
１．１机械臂建模
对机械臂本体而言，其是一个复杂的系统，其包括
嗍帆。。
九
嚣；嚣；
㈣警，
００ＤＤ
７
Ｆｃｏｓｅ３
ｏ—ｓｉｍ３Ｌ３ｃｏｓ８３
ｏ
—１
Ａｌ喜ｓ押∞３。１
００
ｃ躐３Ｌ３ｓ确３
００
Ｏ１
了运动学以及动力学等多方产ｉｉ：＋：
面学科，而运动学分析则是对机械臂的空间姿态和各个关节角度之间的关系进行建模分析，其是机械臂运动学中的重要部分。本系统采用４自由度机械臂。其实物图如图１所示，该机械臂的Ｄ—Ｈ模型［６－７］效果如图２所示。
利用Ｄ—Ｈ法，根据图２中建立的各个关节的坐标系，得到的该机械臂的Ｄ—Ｈ
圈２
ｉｐ¨！ｉ
彳
删帆Ｄ
Ｄ
Ｄ
０
Ｄ７
则机械臂的基座和手爪之间的总变换矩阵如式（２）所示：
Ｒ
ｒ三莩
■
Ｌ＝Ａ，＾肖３Ａ４
ｃｌｃ４ｃ∞一ｓｌｓＩ—Ｃｌｓ４ｃ≈一Ｓ１Ｃ４
ｓｌｃ４ｃ口＋ｃｌｓ４
ＣＩｓ∞Ｌ１Ｃ１＋Ｌ２Ｃ１ｃ２＋Ｌ３Ｃｌｃ∞
（２）
Ｃｌｃ４一ｓｔｓ４ｃ∞ｓｔｓ∞Ｌｔｓｌ＋Ｌ２ｓｌｃ２＋Ｌ３Ｓ１ｃ∞
ｓ４ｓ∞
ｃ２３
一ｃ４ｓ∞
一Ｌ２ｓ２一Ｌ３ｓ∞
７
０００
上式中，ｓ。代表ｓｉｎｅ。，ｃ。代表ｃｏｓｅ。，Ｓ∞代表ｓｉｎ（Ｂ２＋ｅ３），ｃ蕾代表ｃｏｓ（ｅ２＋ｅ３）。１．２机械臂逆运动学求解
Ｚ５
机械臂Ｄ—Ｈ模型效果图
在实际应用中，我们往往知道目标物体所在空间中的位置，
机械臂视觉伺服控制系统的设计
而要求控制每个关节转动相应的角度使机械臂末端执行机构到达空间目标位置，这就需要知道每个关节的转动角度，所以逆运
，，，，，，，，，，Ｊ
动学分析往往更加重要，它是机器人运动规划和轨迹控制的基础，有了逆运动学求解，就可以确定每个关节变量的大小，所以，逆运动学求解是对机械臂控制以及系统编程的理论依据。
若机械臂末端执行机构的位姿Ｔ已知，在这里表示为：
Ｉ以ｑ毛以ｌ
ｑ邑Ｂ
８Ｌ—ｑＩｌ亿呸乞Ｂ
（３）
～～
、
’
、＾
，厂“
．！
Ｔｄ＝）【，一ｘｒ
飞
图３双目视觉成像原理图
【＿ｏ
ｏｏ
１ｊ
理世界坐标系中的点Ｐ在左右摄像机上的成像点为Ｐ，和Ｐ，，其在图像坐标系下的坐标为（Ｘ，Ｙ１）和（）（ｒ，ｙｒ），由三角几何关系可得：
前面式（２）我们已经求出机械臂的总变换矩阵，为了对关节角度进行求解，我们从Ａ。’开始，依次用Ａ，’左乘式（２）和式（３）的两个矩阵，化简可得：
Ｘ『＝ｆ拿
‘．Ｃ
（１３）
－ｎ，ｃ，＋ｑｓ，ＯｘＣ，＋ｑｓ，吼ｃ，十邑ｓ，以ｃ，＋ｐｙｓ，－Ｌ，］
一以
＿：ｆ（ｘｏ，－Ｔ）．
‘ｃ
（１４）
一呸一岛—见
ＢＣｌ－ＰｘＳ，
１
０
ｑｃｌ一仉ｓ，ｑｃ广ｑｓＴａｙｃ广氐ｓ，
Ｙｌ＝ｉｔ＝埠
‘．Ｃ
（１５）
Ｌ
００
ｊ
㈤
‘‘
上式中（Ｘｃ，Ｙ。，ｚ。）为点Ｐ在左摄像机坐标系下的坐标，Ｔ为基线距，视差定义为某一点在左右摄像机图像中对应点的位置’差，即ｄ＝ｘ．一ｘ，，由上面的公式，可以得到空间上的点Ｐ在左摄像机坐标系中的坐标为：
ｒ％ｑ一％ｓ４％ＬｃＰ＋Ｌ％］
＝｛警一言”“０驴０匕％Ｊ
ｌ文
乞０
Ｌ
００
７
ｊ
（５）（６）（７）
ｆ。一『．■ｊ，，一卜Ｍ
ｌ，一卜，
根据式（４）中矩阵中对应元素相等，可得：Ｂｃ７＋ｐｙｓ广Ｌ，＝Ｌ２岛＋ｂｃ白－ｐ，＝Ｌ２ｓ２＋Ｌｓ％
ＰｙＣ，－ＰｘＳ，＝Ｄ
ｌ～一丁
ｌ’。一１『－
（１６）
ｒｃ一了
２．２目标特征提取及匹配
在对目标物体进行测量之前，首先要对目标物体进行识别，在这里采用目标模板，提取目标模板的特征，然后跟实时采集的图像进行特征匹配，从而实现对目标物体的识别，在这里采用ＳＵＲＦ特征作为图像的特征点。
在ＳＵＲＦ特征点的提出过程中利用了积分图像、Ｈａｒｒ小波以及Ｈｅｓｓｉａｎ矩阵等运算，从而大大缩短了计算时间，对Ｈａｒｒ小波的利用也增加了特征点的鲁棒性，利用近似Ｈｅｓｓｉａｎ矩阵行列式局部最大值计算得到的特征点是其所在邻域内的像素中
最亮或者最暗的点。
因为关节２和关节３是平行的，所以单独左乘Ａ。。不会产
生有用的结果，下一步要左乘≮。Ａ：。Ａｊｌ，化简并使对应元素相
等，可得：
一￡．，ｃ＿∞一Ｌ２ｃｊ—Ｌ３＋Ｐｘｃ，９却＋ｐ，ｓＴｃ＿∞一ｐ，ｓａ＝Ｏ
（８）
一￡．，％一Ｌ２ｓ３＋ｐｘｃ，％＋Ｂｓ，％＋Ｂ％＝Ｄ仉ｃ，％十ｎ，ｓＴ％＋亿％＝岛
一ｏ，ｓ，＋０ｖｃ，＝ｑ
（９）（１０）（１１）
联立式（５）一式（１１），则可求出对应关节的变量值，在实际求解过程中，有的关节变量值会有多个解，出现这种情况是机械臂末端执行机构到达所需的指定位姿存在不同的关节角度，而根据机械臂实际情况，要对机械臂关节的运动角度以及轨迹进行限定，通过限定条件，则可以得出关节变量有且只有一个唯一的值，求得的关节变量值如下式所示：
给定图像ｆ中的一点ｆ（ｘ，Ｙ），Ｈｅｓｓｉａｎ矩阵Ｈ（ｘ，叮）在ｘ处
尺度为仃的定义如式（１７）所示：
愀∽：∥托盯：一：：：’＝：］【～Ｌ×，仃）～【ｘ，仃，Ｊ
１２‘
（１７）
０，＝ｓｒｃｔａ几（鲁）ｃ一号硼，＜号，
０２＝ａｒｃｓｉｎ（ｓｉｎｅ川Ｄ也＜多）
式中，ｂ（ｘ，叮）为高斯二阶偏导数芸在Ｘ处与图像ｆ的卷
ａＸ。
积，ｂ（ｘ，盯）和ＬＷ（ｘ，盯）类似。
矩阵Ｈ（ｘ，盯）的行列式计算公式如下所示：
／
。４
归陀淄（监孽２鬈Ｌ盛幺）ｃ嗨＜和门２’
。
＼
ｄｅｔ（Ｈ）＝Ｌ。（ｘ，ｏ－）￡－ｗ（ｘ，ｏ－）一～（ｘ，ｏ－）。
（１８）
｝Ｌ・ｓ
对上行列式进行计算，当结果为负数时，其特征值的符号不同，则该点为非极值点，当结果为整数时，其特征值同为正数或同为负数，该点为极值点。
通过模板特征匹配得到的目标匹配效果图如图４所示。同样，根据ＳＵＲＦ特征，进行左右图像目标物体的特征匹配，然后找到对应的匹配点，利用上面介绍的双目测距利用，根据对应匹配点的坐标，计算得到目标物体在空间坐标中的位置。在实际匹配中，存在许多误匹配点，需要根据立体匹配准则进行误匹配剔除，最后根据目标中心位置的特征匹配点在左右图像
０４－－－－－－，＇ａｒｃ胁（鲁）（一号硼。＜≥，
２目标识别与定位２．１双目立体视觉模型
图３为双目视觉成像原理图，假设两台摄像机的像平面平行，光轴严格平行（光轴是从投影中心Ｏ朝像主点ｃ方向引出的一条射线，也称为主光线），距离一定，焦距相同ｆＦｆｒ＝ｆ。
进一步假设两幅成像图像之间的行像素是严格对准的，物
《工业控制计算机＞２０１３年第２６卷第８期
据如表２所示，表中的坐标值是以右摄像机坐标系为参考坐标系，单位为ｍｍ，Ｚ７为Ｚ方向的实测距离。
从表２的测距实验结果中可以看出，当目标物体距离摄像机２０ｃｍ一４０ｃｍ之间时，测量的精度比较高，测量误差控制在１％左右，绝对误差在２～３ｍｍ左右。当距离摄像机较近，测得的误差较大，不能够满足系统的性能要求，这是由于较近时目标物体严重．畸变引起的。当目标物体距离摄像机５０ｃｍ左右时，虽然测量精度在２％左右，但由于此时绝对误差已经较大，仍不能够满足系统的要求，这是因为当目标物体距离摄像机越远，摄像机的非线性
圈４目标特征匹配效果图
特征表现的越突出，从而加大了双目立体视觉的测量误差，另一方面是因为目标物体较远，特征的提取比较困难，而特征提取的不准确性也相应导致了测量精度的下降。在实际控制中，机械臂关节处的舵机采用了位置反馈，机械臂末端夹持器到达的位置与视觉测量值近似相等，误差较小，而系统的误差主要是由视觉部分的测量误差引起的，所以当目标距离摄像机较近或较远时，系统的控制精度较大，此时，提高视觉的测量精度为首要任务。
４结束语
中的坐标进行目标物体空间坐标的计算。常用的立体匹配约束有极线约束、相容性约束、唯一性约束、顺序约束以及视差连续性约束等等。其中心位置特征匹配效果图如图５所示。
本文首先通过Ｄ—Ｈ法对机械臂建模并进行运动学求解，采用双目视觉对目标物体进行空间位置的测量，通过实验平台的验证，基本实现了预期的设计目标，完成了对目标物体的抓取动作，但当目标物体较劲或较远时，视觉的测量精度还有待进一步改进。
图５左右摄像机目标中心特征匹配效果图
根据匹配结果．目标识别及物体空间坐标结果图如图６所示
统，２００６，１２（１）；３２－３５
参考文献
［１］庞云亭，黄强，郑娟，等．仿人机器人的视觉伺服控制系统［Ｊ］．伺服系
［２］Ｄ．Ｂ．Ｚｈａｎｇ，Ｌ．Ｖ．Ｇ００１．Ａ．Ｏｏｓｔｅｒｌｉｎｃｋ。Ｓｔｏｃｈａｓｔｉｃｐｒｅｄｉｃｔｉｖｅ
ｏｆ
ｃｏｎｔｒｏｌ
ｒｏｂｏｔ
ｔｒａｃｋｉｎｇｓｙｓｔ—ｅｍｓ
ｗｉｔｈ
ｄｙｎａｍｉｃ
ｖｉｓｕａｌｆｅｅｄｂａｃｋ。ｉｎ
Ｐｒｏｃ【Ｃ】．ＩＥＥＥ
６１０－６１５
Ｉｎｔ７ｌＣｏｎｆ．ｏｎＲｏｂｏｔｉｃｓａｎｄＡｕｔｏｍａｔ－ｉｏｎ，１９９０：
［３］薛定宇，项龙江，司秉玉，等．视觉伺服分类及其动态过程［Ｊ］．东北大
学学报，２００３。２４（６）：４３—５４７［４］ＦａｕｇｅｒａｓＯ
ａｎｄ
Ｄ，Ｔｏｓｃａｎｉ
Ｇ．Ｔｈｅｃａｌｉｂｒａｔｉｏｎ
ｐｒｏｂｌｅｍ
ｏｎ
ｆｏｒｓｔｅｒｅｏ
［Ｃ】．ＩＥＥＥＣｏｍｐｕｔｅｒＳｏｃｉｅｔ－ｙＣｏｎｆｅｒｅｎｃｅＣｏｍｐｕｔｅｒＶｉｓｉｏｎ
ＰａｔｔｅｒｎＲｅｃｏｇｎｉｔｉｏｎ．ＭｉｎｍｉＢｅａｃｈ．Ｆｌｏｒｉｄａ。１９８６：１５—２０
［５］田村秀行，金喜子，乔双．计算机图像处理［Ｍ］．北京：科学出版社，
２（）０４
［６］ＢｅｎＳｈｎｅｉｄｅｒｍａｎ，ＣａｔｈｅｒｉｎｅＰｌａｉｓａｎｔ．ＤｅｓｉｇｎｉｎｇｔｈｅＵｓｅｒＩｎ－
ｔｅｆｆａｃｅ—Ｓｔ—ａｔｅｇｉｅｓｆｏｒＥｆｆｅｃｔｉｖｅＨｕｍａｎ—ＣｏｍｐｕｔｅｒＩｎｔｅｆａｃｔｉｏｎ
［Ｍ】．Ａｍｅｒｉｃａ：ＰｅａｒｓｏｎＥｄｕｃｔｉｏｎ。２０Ｑ５：５４—５５
图６目标识别及测距效果图
３实验及结果分析
［７］Ｓｈａｈｉｎｐｏｏｒ，Ｍｏｈｓｅｎ．ＡＲｏｂｏｔＥｎｇｉｎｅｅｒｉｎｇＴｅｘｔｂｏｏｋ［Ｍ］，Ｈａｒｐｅｒ
＆Ｒｏｗ．１９８７
双目视觉对目标物体的测量精确直接影响着系统的控制精确度，对目标物体在空间中的不同位置进行测量，得到的实验数
表２空间目标物体双目立体视觉测距结果
［收稿日期：２０１３．４．２６］
区曩擅囊推荐引钡行业变革
全新改版
详情请登陆：ｈｔｔｐ：／／ｗｗｗ．ｇｋｏｎｇ．ｃｏｍ／ｃｏ／ｇｋｏｎｇ／ｆｒｅｓｈ．ｈｔｍ
薹控行业交流交易翠台
机械臂视觉伺服控制系统的设计
作者：作者单位：刊名：英文刊名：年，卷(期)：
江杰， 王鹏月
内蒙古科技大学信息工程学院,内蒙古包头,014010工业控制计算机
Industrial Control Computer2013,26(8)
参考文献(7条)
1. 庞云亭;黄强;郑娟 仿人机器人的视觉伺服控制系统 2006(01)
2. D.B.Zhang;L.V.Gool;A.Oosterlinck Stochastic predictive control of robot tracking syst-ems with dynamic visualfeedback 1990
3. 薛定宇,项龙江,司秉玉,徐心和 视觉伺服分类及其动态过程[期刊论文]-东北大学学报(自然科学版) 2003(6)4. Faugeras O D;Toscani G The calibration problem for stereo 19865. 田村秀行;金喜子;乔双 计算机图像处理 2004
6. Ben Shneiderman;Catherine Plaisant Designing the User Interface-St-ategies for Effective Human-ComputerInteraction 2005
7. Shahinpoor,Mohsen A Robot Engineering Textbook 1987
引用本文格式：江杰. 王鹏月 机械臂视觉伺服控制系统的设计[期刊论文]-工业控制计算机 2013(8)