基于双目视觉的服务机器人仿人机械臂控制

徐昱琳， 杨永焕， 李 昕， 陈万米， 晁衍凯

(上海大学机电工程与自动化学院，上海200072)

基于双目视觉的服务机器人仿人机械臂控制

徐昱琳， 杨永焕， 李 昕， 陈万米， 晁衍凯

(上海大学机电工程与自动化学院，上海200072)

机械臂逆运动学、目标识别与定位是服务机器人手臂控制中的关键技术．为了更好地与复杂多变的非结构化环境进行交互，提出一种应用于服务机器人平台的基于双目视觉的仿人机械臂控制方法．给出一种针对6自由度手臂的逆解算法，并采用基于双目视觉与颜色分割的目标识别方法．然后，根据识别出的目标三维坐标信息控制机械臂完成特定任务．本方法在家庭服务机器人上得到了验证．

仿人机械臂;逆运动学;双目视觉;HSV;服务机器人

Abstract:Inverse kinematics，object localization and manipulation are essential for service robots with manipulators to achieve various human-like tasks．Binocular vision is employed to interact with the environment in which the service robot works．This paper presents an approach for binocular based humanoid manipulation in a service robot system．An inverse kinematic solver is proposed to find all joint angles for a given position of the effectors on the manipulator．The target object is recognized according to segmented colors，and the 3D position computed using the stereo vision system．Having obtained the target position，the manipulator performs a blind grasp．Experimental results show effectiveness of the proposed methods．Key words:humanoid manipulator;inverse kinematics;binocular vision;hue-saturation-value(HSV);service robot

20世纪60年代初，工业革命使工业机器人进入工厂，将人们从危险和有害的工作中解脱出来．伴随着机电一体化和机器人技术的迅猛发展，越来越多不同种类的服务机器人进入人们的生活中，如人形机器人 ASIMO［1］、厨房机器人 HRP-2［2］、助残机器人 ISAC［3］、图书管理机器人 UJI［4］等．先进的机械臂控制对于服务机器人执行各种类人任务是非常重要的，其中机械臂逆运动学和目标定位是服务机器人机械臂控制中的关键技术．

工业机器人工作在结构化的环境中，而服务机器人通常面临一些不确定的、非结构化的和复杂的环境［5］，工作在这种环境下的服务机器人必须实时地与外部环境以及环境中的其他机器人和人进行交互．为了使机械臂具有自主地从未知环境中获取信息的能力，需要给机械臂配备各种外部传感器，如触觉、距离和视觉传感器等，其中视觉传感器具有信号范围大、信息完整等特点，被认为是最重要的传感器．双目视觉传感器由于可以直接获取物体的三维坐标信息，为机械臂的控制带来了很大的方便，因此基于双目视觉的机械臂控制受到国内外研究者的广泛关注．斯坦福大学的 STAIR［6］(Stanford AI robot)是一个具有5自由度(degrees of freedom，DOF)机械臂的人工智能研究平台，通过安装在臂末端的双目摄像头，可以控制机器人拿取一件事先未知具体形状的物体．德国航空航天中心研制的机器人Robutler［7］是一个可移动式平台，其具备固定的立体摄像系统、激光扫描仪和一个安装在平台上的7自由度手臂，该机械臂的视觉伺服系统采用的是手眼(eye-in-hand)系统．

此外，机械臂的逆运动学算法也是机械手控制研究中的重要部分之一．机械手运动学逆解是指通过已知末端执行器的坐标位置或位姿来计算机械手各个关节的角度值［8］．机械臂的逆解问题最终一般都归结为求解非线性方程组的问题．非线性方程组的求解方法有很多，主要包括数值方法和代数方法．常用的数值方法主要有牛顿拉夫森法、优化算法、遗传算法等;常用的代数方法主要有析配消元法、聚筛法、Groebner基法和吴文俊消元法．本工作提出了一种求解6自由度仿人机械臂逆解的算法，采用基于双目视觉与颜色分割的目标识别方法，根据识别出的目标三维坐标信息控制机械臂完成特定任务．本方法在家庭服务机器人上得到了验证．

1 机械臂数学模型的建立

机械臂数学模型的建立是机械臂运动学中的重要部分．本系统采用6自由度仿人机械臂，如图1所示，肩部、肘部与腕部各有2个旋转自由度．本工作采用D-H(denavit-hartenberg)方法建立机械臂的数学模型［9］，机械臂的参考坐标系如图2所示．根据D-H法可以得出本机械臂的D-H参数，如表1所示．

图1 6自由度仿人机械臂Fig．1 6-DOF manipulator of the robot

图2 仿人机械臂各关节参考坐标系Fig．2 Reference frames for the humanoid robot arm

表1 D-H参数Table 1 D-H parameters

表1中，a表示相邻两个关节轴线方向的公垂线的长度(关节偏移);α表示两个相邻z轴之间的角度(关节扭转);d表示在z轴上的两条相邻的公垂线之间的距离;θ表示绕 z轴的旋转角．矩阵Ai(i=1，2，…，6)为后一个关节变换到前一个关节的转换矩阵:

通过从参数表中选取参数带入上述矩阵，便可以写出每两个相邻关节之间的变换．已知各关节转角，通过关节间的位姿矩阵可以求得机器人手臂运动学正解为

式中，p=⌊ px，py，pz」为手臂末端执行器位置矢量;n，o，a为手臂末端执行器的姿态矢量，其中n=⌊nx，ny，nz」为平行于夹持器平面的单位矢量，o=⌊ ox，oy，oz」为垂直于夹持器平面的单位矢量，a=⌊ ax，ay，az」为指向目标物体的接近矢量．

2 手臂运动学逆解

逆运动学问题在机器人运动学、动力学及控制的研究中占有重要的地位，直接影响着控制的准确性与快速性．逆运动学问题就是根据已知的末端执行器的位姿位置和姿态，求解相应的关节变量．本机械臂是模仿人的手臂设计的，其对应的简化模型如图3所示．设机械臂末端位置和姿态矩阵为

本逆解算法由以下4个步骤解得．

步骤(1):关节5在手抓坐标系x6y6z6下的坐标为(0，0，-l5)，其在基础坐标系下的坐标为(x5，y5，z5)，则

图3 手臂模型等效简化图Fig．3 Simplified arm model

3 目标识别与定位

对基于视觉的机械臂控制来说，机器人视觉是一个非常重要的组成部分．机器人只有在视觉的指引之下，才能获得目标的正确信息，从而控制机械臂完成指定的任务．视觉系统必须在标定的基础上，识别和跟踪目标对象的位置和姿态．在图像中，识别目标对象可以基于3个不同的属性［10］，即颜色、纹理和形状．识别出目标物体之后，即可根据双目摄像头计算出目标物体的三维坐标值．颜色特性是应用最广泛、最容易学习与实现的方式，因此，本工作采用基于颜色的物体识别方式．

本系统采用加拿大Point Grey公司的Bumblebee2作为双目视觉传感器，通过提取目标物体的颜色信息进行HSV(hue-saturation-value)阈值分割，得到目标物体的三维坐标．

3．1 双目立体视觉模型

本立体视觉系统基于平行光轴理论［11］(见图4)，其中 b表示基线距离，f为摄像头的焦距，M(xm，ym，zm)为目标物体在世界坐标系的坐标值，I1与I2为左右两个摄像头的成像平面．

根据△MI1I2与△MOF2的相似关系，可以得到目标物体在世界坐标系中的坐标值为

图4 摄像头与成像平面的几何关系Fig．4 Geometric relationship between cameras and image planes

3．2 颜色空间转换

采用彩色图像分割的方法识别目标时，要选择合适的颜色空间，常见的颜色空间有 RGB(redgreen-blue)，HSV，CMY(cyan-magenta-yellow)等．RGB是常见的颜色空间，一般CCD(charge-coupled device)摄像机输出用RGB颜色空间表示，若选择RGB颜色空间，则无须转换．但在实际应用中，由于RGB颜色空间受光源的种类、光照的强度等因素影响，同一颜色属性的物体测出的RGB值分布很分散，3个分量互相关联变化，很难确定识别RGB的阈值范围，容易把并非指定颜色的物体包含进去，或漏掉应该识别的部分物体．

而HSV模型可以在一定程度上避免RGB模型的高分散性和高相关性所带来的阈值划分问题．HSV颜色模型反映了人眼观察彩色的视觉规律，在实际中应用较多．从RGB到HSV的转化过程相当于一个解耦的工作．在HSV空间中，色调H(hue)反映了该颜色最接近何种可见光谱波长，即某一种颜色．色调H用角度来表示，一般规定0°的彩色为红色，120°的为绿色，240°的为蓝色，色调从 0°～ 360°覆盖了所有的彩色．饱和度S(saturation)表示一种颜色相对于其纯色的比例，即纯洁性，可用来区别颜色明暗的程度．完全饱和的颜色是指由单一波长组成的光谱色，当一种颜色掺入其他光成分愈多时，颜色愈不饱和．饱和度在颜色圆上用半径表示，沿径向方向上的不同颜色具有相同的色调和亮度，但饱和度不同．V(value)表示色彩的明亮程度，范围为0～1，一个极端是黑色(没有光)，另一个极端是白色，在这两个极端之间是不同明暗程度的灰色．在圆柱体中常用垂直轴表示，底部的亮度最小，顶部的亮度最大．本研究采用HSV颜色空间进行目标识别，HSV的颜色模型［12］如图5所示，其中 RGB到 HSV的转换公式为

图5 HSV颜色空间Fig．5 Color space of HSV

3．3 目标定位

首先，将每一帧图像的像素点转换成HSV值，通过离线学习并记录目标物体的HSV阈值信息．程序实时运行时，提取阈值信息得到目标区域后，通过双目视觉计算得到目标的三维坐标信息．本系统基于VC++6．0软件平台，图6为程序运行界面，左下角为物体的三维坐标值．

图6 程序调试界面(离线模式)Fig．6 Program debugging interface(off-line mode)

4 实验

本工作提出了一种基于双目视觉的仿人机械臂控制方法，为了验证逆解求解以及颜色滤波方法的正确性，将本方法应用于家庭服务机器人平台，控制服务机器人完成自主抓取任务．

4．1 服务机器人硬件平台介绍

本系统中的家庭服务机器人实验平台如图7所示，由2自由度头部和2个6自由度仿人机械臂组成，底部采用差动轮式结构运动，机器人周围分布着8路光电传感器和8路超声波传感器．机器人主控制器使用一台NISE3110工控机，固定在机器人腹部．机器人双臂电机以及底轮电机通过CAN总线与工控机进行通信．

图7 实验平台Fig．7 Experimental setup

4．2 实 验

本实验设计机器人自主抓取，通过分析目标物体实际坐标位置与手爪到达位置之间的误差值，从而证明逆解算法与颜色滤波算法的正确性．

目标物体为绿色瓶子，保证目标物体在世界坐标系中的位置不变．机器人的位置是变化的，每一次抓取时机器人的位置都是不同的，但需要保证目标物体在机器人手臂的抓取范围内．立体视觉系统处理图像的速度为15帧/s，每一帧图像的分辨率为640×480．实验数据如表2所示，其中3个坐标值的参考坐标系均为机器人右肩关节坐标系，单位为mm．目标物体的实际测量位置是指目标物体在机器人右肩关节坐标系中的坐标值，双目计算值为通过颜色分割算法求出的目标物体坐标，手臂末端夹持器位置值为通过颜色滤波得到的位置值求取手臂逆解后手臂运动到的位置，误差值为实际测量值与夹持器位置之间的差值．

实验得出，机器人计算得到的位置与目标物体实际位置之间的平均误差为(20．25，1．42，0．15)．目标物体高度为15 cm，直径为6 cm，因此，实验误差在允许误差范围内，机器人可以完成自主抓取任务，本工作中的方法得到了验证．机械臂抓取流程如图8所示．

图8 仿人机械臂抓取动作Fig．8 Grasping of humanoid manipulator

表2 实验数据Table 2 Experimental results

5 结束语

本工作基于服务机器人复杂多变的非结构化工作环境，提出了一种基于双目视觉的仿人机械臂控制方法．首先，给出了一种针对6自由度手臂的逆解算法，采用基于双目视觉与颜色分割的目标识别方法;然后，根据识别出的目标三维坐标信息控制机械臂完成特定任务;最后，通过在人形机器人平台上的应用，控制机器人完成自主抓取任务，使本方法得到验证．本研究中的双目视觉系统稳定准确，能够实时得到目标物体的位置与姿态，满足抓取要求，实现了服务机器人非结构化环境下的抓取操作．今后的工作主要在图像处理速度、处理算法上进行改进与提高，使系统实时性更高，提高系统的定位精度．

［1］ SAKAGAMI Y，WATANABE R，AOYAMA C，et al．The intelligent ASIMO:System overview and integration［C］∥Proceeding of the 2002 IEEE/RSJ International Conference on Intecligent Robots and Systems．Saitama:Honda R＆D Co．Ltd．，2002:2478-2483．

［2］ HARA I，ASANO F，ASOH H，et al．Robust speech interface based on audio and video information fusion for humanoid HRP-2 ［J］．Intelligent Robots and Systems，2004:2404-2410．

［3］ KAWAMURA K，PETERS II R A，WILKES D M，et al．ISAC:Foundations in human-humanoid interaction ［J］．IEEE Intelligent System，2000，15(4):38-45．

［4］ PRATS M，SANZ P J，DEL POBIL A P．Model-based tracking and hybrid force/vision control for the UJI librarian robot［C］∥Intelligent Robots and Systems，IEEE/RSJ International Conference on Digital Object Identifier．2005．

［5］ KEMP CC，EDSINGER A，TORRES-JARA E．Challenges for robot manipulation in human environments:Developing robots that perform useful work in everyday settings［J］．IEEE Robotics and Automation Society，2007:20-29．

［6］ ASHUTOSH S，JUSTIN D，JUSTIN K，et al．Robotic grasping of novel objects using vision［J］．International Journal of Robotics Research，2008:157-173．

［7］ HILLENBRAND U，BRUNNER B，BORST C，et al．The robutler:A vision-controlled hand-arm system for manipulating bottles and glasses［C］∥ISR 2004 35th International Symposium on Robotics，Paris-Nord Villepinte Exhibition Centre．2004．

［8］ 范叔炬．机械臂运动学算法设计［D］．杭州:浙江大学，2008:42．

［9］ NIKU S B．Introduction to robotics analysis，systems，applications［M］． Beijing:Publishing House of Electronics Industry，2004:28-85．

［10］ JAN P． Vision based mobile manipulation ［D］．Regensburg:Regensburg University of Applied Sciences，2008．

［11］ LEE H J，LEE M C．Technique for localization and visual servoing of mobile manipulators［J］．IEEE International Symposium on Industrial Electronics，2009．

［12］ SMITH A R．Color gamut transform pairs ［C］∥Computer Graphics(SIGGRAPH)．1978:12-19．

Binocular Vision-Based Humanoid Manipulator Control for Service Robot

XU Yu-lin， YANG Yong-huan， LI Xin， CHEN Wan-mi， CHAO Yan-kai
(School of Mechatronics Engineering and Automation，Shanghai University，Shanghai 200072，China)

TP 24

A

1007-2861(2012)05-0506-07

10．3969/j．issn．1007-2861．2012．05．013

2011-03-12

徐昱琳(1964～)，女，副教授，博士，研究方向为机器人智能控制．E-mail:xuyulin@shu．edu．cn