第9卷第3期 智能系统学报 Vol.9 No.3 2014年6月 CAAI Transactions on Intelligent Systems Jun.2014 D0:10.3969/j.issn.1673-4785.201308019 网络出版地址：http://www.enki..net/kcms/doi/10.3969/j.issn.16734785.201308019.html 采用Kinect的多臂协调操作系统 张雪华12，刘华平2，孙富春2，高蒙，韩峥2 (1.石家庄铁道大学电气与电子工程学院，河北石家庄050043：2.清华大学智能技术与系统国家重点实验室，北京100084) 摘要：为完成基于Kinect的多臂协调精细操作的任务，搭建了一套双机械臂操作系统，利用Kinect作为视觉传感 器对场景进行实时检测，并利用基于工作空间的RRT算法对其中一台七自由度机械臂末端进行路径规划完成目标 的自主抓取。根据手眼协调控制技术，利用另外一台六自由度机械臂末端的摄像机采集的图像误差控制机械臂的 运动，并利用粒子滤波算法对目标进行实时跟踪。通过设计一套双臂协作完成物体交接的实验系统，完成了多臂协 同操作的任务，并验证了实验方法的可靠性。 关键词：Kinect:粒子滤波；多臂：路径规划；视觉伺服 中图分类号：TP242文献标志码：A文章编号：1673-4785(2014)03-0307-06 中文引用格式：张雪华，刘华平，孙富春.量子衍生神经网络模型及其在图像恢复中的应用[J].智能系统学报，2014,9(3)：307 312. 英文引用格式：ZHANG Xuehua,LIU Huaping,SUN Fuchun,etal.Quantum-derived nerve network model and its application for image restoration[J].CAAI Transactions on Intelligent Systems,2014,9(3):307-312. Kinect-based dobby coordinate operating system ZHANG Xuehua'2,LIU Huaping?,SUN Fuchun2,GAO Meng',HAN Zheng'.2 (1.College of Electrical and Electronic Engineering,Shijiazhuang Railway University,Shijiazhuang 050043,China;2.Key Laboratory of Intelligent Technology and Systems,Tsinghua University,Beijing 100084,China) Abstract:Using Kinect and mechanical arm,we build a dual-arm accurate operation system in which Kinect is used for real-time scene dection and a work-space based RRT algorithm is proposed to plan the path for the 7-DOF arm's target grabbing.A camera at the end of the other 6-DOF arm is used to control the arm tracking the target. The method used for optical target tracking is particle filter-based algorithm and the difference between the target position in the image and the image center is used for controling the arm.In order to test the system,a dual-arm co- operation object transfer experiment was proceed and the successful experient result proved the reliability and effi- cience of the system. Keywords:Kinect;particle filter;dual path;trajectory planning;visual servoing 随着人类太空探索活动的不断深入，单臂系统NASA Johnson空间中心已经研制出可以替代或帮 的不断成熟为多臂协调操作系统的发展提供了基 助宇航员出舱活动的舱外灵巧机器人Robonaut((机 础，而在复杂空间环境中进行精细操作的需求也使 器人宇航员)。该机器人由2个七自由度的机械 得多臂协调操作系统拥有更大的发展空间。美国 臂、2个12自由度的多指灵巧手、一个4自由度的 立体视觉平台、一个六自由度的尾巴和一个可以让 收稿日期：2013-08-12.网络出版日期：2014-06-14. 基金项目：国家“973”计划资助项目(G2013CB329403)；国家自然科 遥操作者融入机器人工作环境的遥现系统组成。北 学基金资助项目(91120011)：请华大学自主科研计划资助 京航空航天大学机器人所丁希仑等)对冗余度双 项目(2011THZ0). 通信作者：张雪华.E-mail:lovelywaiwai1988@sina.cm. 臂机器人的协调操作进行了初步研究，建立了孔轴
第 ９ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．９ №．３ ２０１４ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１４ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１３０８０１９ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｏｉ ／ １０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３４７８５．２０１３０８０１９．ｈｔｍｌ 采用 Ｋｉｎｅｃｔ 的多臂协调操作系统 张雪华１，２ ，刘华平２ ，孙富春２ ，高蒙１ ，韩峥１，２ （１． 石家庄铁道大学 电气与电子工程学院，河北 石家庄 ０５００４３； ２． 清华大学 智能技术与系统国家重点实验室，北京 １０００８４） 摘 要：为完成基于 Ｋｉｎｅｃｔ 的多臂协调精细操作的任务，搭建了一套双机械臂操作系统，利用 Ｋｉｎｅｃｔ 作为视觉传感 器对场景进行实时检测，并利用基于工作空间的 ＲＲＴ 算法对其中一台七自由度机械臂末端进行路径规划完成目标 的自主抓取。 根据手眼协调控制技术，利用另外一台六自由度机械臂末端的摄像机采集的图像误差控制机械臂的 运动，并利用粒子滤波算法对目标进行实时跟踪。 通过设计一套双臂协作完成物体交接的实验系统，完成了多臂协 同操作的任务，并验证了实验方法的可靠性。 关键词：Ｋｉｎｅｃｔ；粒子滤波；多臂；路径规划；视觉伺服 中图分类号： ＴＰ２４２ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１４）０３⁃０３０７⁃０６ 中文引用格式：张雪华，刘华平，孙富春． 量子衍生神经网络模型及其在图像恢复中的应用［ Ｊ］． 智能系统学报， ２０１４， ９（ ３）： ３０７⁃ ３１２． 英文引用格式：ＺＨＡＮＧ Ｘｕｅｈｕａ，ＬＩＵ Ｈｕａｐｉｎｇ，ＳＵＮ Ｆｕｃｈｕｎ， ｅｔ ａｌ． Ｑｕａｎｔｕｍ⁃ｄｅｒｉｖｅｄ ｎｅｒｖｅ ｎｅｔｗｏｒｋ ｍｏｄｅｌ ａｎｄ ｉｔｓ ａｐｐｌｉｃａｔｉｏｎ ｆｏｒ ｉｍａｇｅ ｒｅｓｔｏｒａｔｉｏｎ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１４， ９（３）： ３０７⁃３１２． Ｋｉｎｅｃｔ⁃ｂａｓｅｄ ｄｏｂｂｙ ｃｏｏｒｄｉｎａｔｅ ｏｐｅｒａｔｉｎｇ ｓｙｓｔｅｍ ＺＨＡＮＧ Ｘｕｅｈｕａ １，２ ，ＬＩＵ Ｈｕａｐｉｎｇ ２ ， ＳＵＮ Ｆｕｃｈｕｎ ２ ， ＧＡＯ Ｍｅｎｇ １ ， ＨＡＮ Ｚｈｅｎｇ １，２ （ １． Ｃｏｌｌｅｇｅ ｏｆ Ｅｌｅｃｔｒｉｃａｌ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｓｈｉｊｉａｚｈｕａｎｇ Ｒａｉｌｗａｙ Ｕｎｉｖｅｒｓｉｔｙ， Ｓｈｉｊｉａｚｈｕａｎｇ ０５００４３， Ｃｈｉｎａ； ２．Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｉｎｔｅｌｌｉｇｅｎｔ Ｔｅｃｈｎｏｌｏｇｙ ａｎｄ Ｓｙｓｔｅｍｓ， Ｔｓｉｎｇｈｕａ Ｕｎｉｖｅｒｓｉｔｙ， Ｂｅｉｊｉｎｇ １０００８４， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｕｓｉｎｇ Ｋｉｎｅｃｔ ａｎｄ ｍｅｃｈａｎｉｃａｌ ａｒｍ， ｗｅ ｂｕｉｌｄ ａ ｄｕａｌ⁃ａｒｍ ａｃｃｕｒａｔｅ ｏｐｅｒａｔｉｏｎ ｓｙｓｔｅｍ ｉｎ ｗｈｉｃｈ Ｋｉｎｅｃｔ ｉｓ ｕｓｅｄ ｆｏｒ ｒｅａｌ⁃ｔｉｍｅ ｓｃｅｎｅ ｄｅｃｔｉｏｎ ａｎｄ ａ ｗｏｒｋ⁃ｓｐａｃｅ ｂａｓｅｄ ＲＲＴ ａｌｇｏｒｉｔｈｍ ｉｓ ｐｒｏｐｏｓｅｄ ｔｏ ｐｌａｎ ｔｈｅ ｐａｔｈ ｆｏｒ ｔｈｅ ７⁃ＤＯＦ ａｒｍ’ｓ ｔａｒｇｅｔ ｇｒａｂｂｉｎｇ． Ａ ｃａｍｅｒａ ａｔ ｔｈｅ ｅｎｄ ｏｆ ｔｈｅ ｏｔｈｅｒ ６⁃ＤＯＦ ａｒｍ ｉｓ ｕｓｅｄ ｔｏ ｃｏｎｔｒｏｌ ｔｈｅ ａｒｍ ｔｒａｃｋｉｎｇ ｔｈｅ ｔａｒｇｅｔ． Ｔｈｅ ｍｅｔｈｏｄ ｕｓｅｄ ｆｏｒ ｏｐｔｉｃａｌ ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ ｉｓ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ⁃ｂａｓｅｄ ａｌｇｏｒｉｔｈｍ ａｎｄ ｔｈｅ ｄｉｆｆｅｒｅｎｃｅ ｂｅｔｗｅｅｎ ｔｈｅ ｔａｒｇｅｔ ｐｏｓｉｔｉｏｎ ｉｎ ｔｈｅ ｉｍａｇｅ ａｎｄ ｔｈｅ ｉｍａｇｅ ｃｅｎｔｅｒ ｉｓ ｕｓｅｄ ｆｏｒ ｃｏｎｔｒｏｌｉｎｇ ｔｈｅ ａｒｍ． Ｉｎ ｏｒｄｅｒ ｔｏ ｔｅｓｔ ｔｈｅ ｓｙｓｔｅｍ， ａ ｄｕａｌ⁃ａｒｍ ｃｏ⁃ ｏｐｅｒａｔｉｏｎ ｏｂｊｅｃｔ ｔｒａｎｓｆｅｒ ｅｘｐｅｒｉｍｅｎｔ ｗａｓ ｐｒｏｃｅｅｄ ａｎｄ ｔｈｅ ｓｕｃｃｅｓｓｆｕｌ ｅｘｐｅｒｉｅｎｔ ｒｅｓｕｌｔ ｐｒｏｖｅｄ ｔｈｅ ｒｅｌｉａｂｉｌｉｔｙ ａｎｄ ｅｆｆｉ⁃ ｃｉｅｎｃｅ ｏｆ ｔｈｅ ｓｙｓｔｅｍ． Ｋｅｙｗｏｒｄｓ：Ｋｉｎｅｃｔ； ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ； ｄｕａｌ ｐａｔｈ； ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ； ｖｉｓｕａｌ ｓｅｒｖｏｉｎｇ 收稿日期：２０１３⁃０８⁃１２． 网络出版日期：２０１４⁃０６⁃１４． 基金项目：国家“９７３”计划资助项目（ Ｇ２０１３ＣＢ３２９４０３） ； 国家自然科 学基金资助项目（ ９１１２００１１） ； 清华大学自主科研计划资助 项目（２０１１ＴＨＺ０）． 通信作者：张雪华．Ｅ⁃ｍａｉｌ：ｌｏｖｅｌｙｗａｉｗａｉ１９８８＠ ｓｉｎａ．ｃｎ． 随着人类太空探索活动的不断深入，单臂系统 的不断成熟为多臂协调操作系统的发展提供了基 础，而在复杂空间环境中进行精细操作的需求也使 得多臂协调操作系统拥有更大的发展空间。 美国 ＮＡＳＡ Ｊｏｈｎｓｏｎ 空间中心已经研制出可以替代或帮 助宇航员出舱活动的舱外灵巧机器人 Ｒｏｂｏｎａｕｔ（机 器人宇航员）。 该机器人由 ２ 个七自由度的机械 臂、２ 个 １２ 自由度的多指灵巧手、一个 ４ 自由度的 立体视觉平台、一个六自由度的尾巴和一个可以让 遥操作者融入机器人工作环境的遥现系统组成。 北 京航空航天大学机器人所丁希仑等［１］ 对冗余度双 臂机器人的协调操作进行了初步研究，建立了孔轴
·308 智能系统学报 第9卷 装配条件下的双臂机器人协调运动学方程，并完成 1.0r 了简单孔轴装配操作。东南大学芦俊等[)对视觉 0.5 引导下的双臂装配机器人最佳装配位置的确定问题 0 进行了研究，并对基于图像定位的轴与轴孔装配中 -0.5 -1.0 的误差进行了分析。 0 0.51.0 00.51.0 本文的双臂协调操作系统中，给出一个基于六 (g)各关节轨迹 (h)末端坐标分量轨迹 自由度机器人的控制子系统，该子系统通过CCD摄 图1系统结构 像机获取图像，传入图像采集卡中，再存入到计算机 Fig.1 The system structure 内存对图像进行处理，获取目标的位置信息，通过设 双臂协作系统的工作流程如下：通过安装在机械 计好的控制算法得出能够控制机器人运动的命令， 臂末端的摄像机获取目标信息，通过图像误差控制六 机器人控制器通过串口接收指令，从而驱动机器人 自由度机械臂抓取目标并送至指定位置，根据Kinect 相应的运行，完成基于图像的视觉伺服。此外还有 获取的环境信息选取目标特征点，七自由度机械臂通 一个七自由度机械臂控制子系统，其中进行图像获 过路径规划控制末端位姿，自主抓取目标，将目标物放 取和目标检测的是微软公司研制的Kinect视觉传 入指定位置，完成双臂交接任务。整个双臂系统主要 感器，Kinect不仅价格低廉，而且能够同时获取深度 分为视觉单元、控制单元和执行单元3个部分。视觉 信息和彩色信息，被研究者们广泛的应用在机器人 单元由Kinect视觉传感器、CCD摄像机、图像采集卡 领域，如机器人的导航与定位[3)、目标的跟踪4]、 一台高性能计算机和局域网组成，摄像机以10帧/s的 障碍物的检测]等。利用基于工作空间的RRT算 速率采集640×480大小的图像，采集图像的过程中计 法对七自由度机械臂末端进行路径规划完成目标的 算机完成对目标的检测，得出目标中心点在图像平面 自主抓取。通过这2套机械臂子系统最终完成双臂 的坐标。 协作实现物体交接的任务。 控制单元是由伺服放大器、，交流伺服电机及配电 1系统结构 组件等构成，其功能是执行接受到的控制指令，完成对 摄像机的操纵，实现对目标的跟踪。 本文的双臂系统是由视觉传感器、机械臂系统 执行单元由Motoman机械臂、模块化机械臂、主控 以及主控计算机组成，其中，视觉传感器包括挂在顶 计算机和运动控制器构成，此单元可以实时地获取机 上的Kinect和安装在六自由度机械臂末端的CCD 器人的位置信息，并根据图像误差生成对机器人各关 摄像机，机械臂系统包括一台模块化七自由度机械 节的控制指令，控制指令可以根据图像实时刷新，通过 臂和一台六自由度机械臂以及灵巧手。整个系统的 主控计算机和运动控制器控制机器人完成指令任务。 构建模型如图1所示。 0.60 0.65 2目标检测与跟踪算法 0.55 0.60 2.1深度信息的背景相减法 0.50 0.55 -0.28 -0.20 本系统采用微软公司研发的Kinect视觉传感器作 -0.32-0.660.64 .30 0.63 0.4 0.30-0.80.6 -0.2 为深度摄像机。Kinect的硬件结构包括一个普通的低 (a)RRT树 (b)末端轨迹 分辨率RGB摄像机、一个红外发射器和一个红外摄像 1.0 0.5*** 头以及一组麦克风阵列，如图2所示。 o v 0 ★2 红外发射器 0.5 1 2 -102一4 (c)各关节轨迹 (d)末端坐标分量轨迹 0.70r 1.0r 0.65 0.5 0.60 . -0.15 02-0448484 0.5 0.4 彩色CMOS摄像头 -0.20 -0.5-0.80.6 0 红外摄像头 (e)RRT树 ()末端轨迹 图2深度摄像机 Fig.2 The depth of the camera
装配条件下的双臂机器人协调运动学方程，并完成 了简单孔轴装配操作。 东南大学芦俊等［２］ 对视觉 引导下的双臂装配机器人最佳装配位置的确定问题 进行了研究，并对基于图像定位的轴与轴孔装配中 的误差进行了分析。 本文的双臂协调操作系统中，给出一个基于六 自由度机器人的控制子系统，该子系统通过 ＣＣＤ 摄 像机获取图像，传入图像采集卡中，再存入到计算机 内存对图像进行处理，获取目标的位置信息，通过设 计好的控制算法得出能够控制机器人运动的命令， 机器人控制器通过串口接收指令，从而驱动机器人 相应的运行，完成基于图像的视觉伺服。 此外还有 一个七自由度机械臂控制子系统，其中进行图像获 取和目标检测的是微软公司研制的 Ｋｉｎｅｃｔ 视觉传 感器，Ｋｉｎｅｃｔ 不仅价格低廉，而且能够同时获取深度 信息和彩色信息，被研究者们广泛的应用在机器人 领域，如机器人的导航与定位［ ３ ］ 、目标的跟踪［ ４ ］ 、 障碍物的检测［ ５ ］等。 利用基于工作空间的 ＲＲＴ 算 法对七自由度机械臂末端进行路径规划完成目标的 自主抓取。 通过这 ２ 套机械臂子系统最终完成双臂 协作实现物体交接的任务。 １ 系统结构 本文的双臂系统是由视觉传感器、机械臂系统 以及主控计算机组成，其中，视觉传感器包括挂在顶 上的 Ｋｉｎｅｃｔ 和安装在六自由度机械臂末端的 ＣＣＤ 摄像机，机械臂系统包括一台模块化七自由度机械 臂和一台六自由度机械臂以及灵巧手。 整个系统的 构建模型如图 １ 所示。 图 １ 系统结构 Ｆｉｇ．１ Ｔｈｅ ｓｙｓｔｅｍ ｓｔｒｕｃｔｕｒｅ 双臂协作系统的工作流程如下：通过安装在机械 臂末端的摄像机获取目标信息，通过图像误差控制六 自由度机械臂抓取目标并送至指定位置，根据 Ｋｉｎｅｃｔ 获取的环境信息选取目标特征点，七自由度机械臂通 过路径规划控制末端位姿，自主抓取目标，将目标物放 入指定位置，完成双臂交接任务。 整个双臂系统主要 分为视觉单元、控制单元和执行单元 ３ 个部分。 视觉 单元由 Ｋｉｎｅｃｔ 视觉传感器、ＣＣＤ 摄像机、图像采集卡、 一台高性能计算机和局域网组成，摄像机以 １０ 帧／ ｓ 的 速率采集 ６４０ × ４８０ 大小的图像，采集图像的过程中计 算机完成对目标的检测，得出目标中心点在图像平面 的坐标。 控制单元是由伺服放大器、交流伺服电机及配电 组件等构成，其功能是执行接受到的控制指令，完成对 摄像机的操纵，实现对目标的跟踪。 执行单元由 Ｍｏｔｏｍａｎ 机械臂、模块化机械臂、主控 计算机和运动控制器构成，此单元可以实时地获取机 器人的位置信息，并根据图像误差生成对机器人各关 节的控制指令，控制指令可以根据图像实时刷新，通过 主控计算机和运动控制器控制机器人完成指令任务。 ２ 目标检测与跟踪算法 ２．１ 深度信息的背景相减法 本系统采用微软公司研发的 Ｋｉｎｅｃｔ 视觉传感器作 为深度摄像机。 Ｋｉｎｅｃｔ 的硬件结构包括一个普通的低 分辨率 ＲＧＢ 摄像机、一个红外发射器和一个红外摄像 头以及一组麦克风阵列，如图 ２ 所示。 图 ２ 深度摄像机 Ｆｉｇ．２ Ｔｈｅ ｄｅｐｔｈ ｏｆ ｔｈｅ ｃａｍｅｒａ ·３０８· 智 能 系 统 学 报 第 ９ 卷
第3期 张雪华，等：采用Kinect的多臂协调操作系统 ·309· 普通RGB摄像机能够以30帧/s的速率传送640× 得到颜色参考模型为q·,k时刻得到某一粒子的颜 480分辨率的RGB图像，而红外发射器和摄像机则是 色直方图为q(x)。本文需要在RGB颜色分布空 用来采集相应的深度信息，通过内部芯片转换并传送 间上选择一个距离D,该距离使得每一次估计均值 320×240分辨率的深度图像[67]。 时的方差最小。通常使用Bhattacharyya相似性系数 文献[14]利用基于深度信息的背景相减法，将 如式(5)，这种度量概率分布之间差别的距离值介 当前帧与背景模型进行差值运算，从而得出环境中 于[0,1]，且忽略了那些不存在的灰度等级。 目标的位置与大小等信息特征，最终能够准确地提 Dg4.(e1=1-立Vmg:](5) 取物品桌上的目标，实现目标检测。由于本系统为 双臂协调操作系统，物体位于六自由度机械臂末端 文中用式(S)表示的Bhattacharyya距离[i统计跟踪 的手爪上，这对灵巧手的精确抓取提出了更高的要 窗口区域内的颜色直方图，并将其与参考直方图比 求，对目标检测的位置信息需更加精确。 较，求解似然估计。可以取 2.2基于颜色的粒子滤波跟踪算法 p(z)exp-AD-q",qu(x)](6) 粒子滤波8]是一种序贯蒙特卡洛滤波方法，其 此时计算的就是p(a4Ix)就是k时刻该粒子权值。 本质是用一系列随机抽取的样本代替状态的后验概 根据上文论述，基于颜色的粒子滤波跟踪算法 分为以下几个过程： 率分布。粒子滤波的基本思想是：首先建立一个概 1)初始化过程 率模型或随机过程，使它的参数等于问题的解，然后 在观测图像中手工标注感兴趣的目标区域，以 通过对模型或过程的观察或抽样试验来计算所求参 该区域中心为原点，产生粒子集{x,},其中n=1, 数的统计特征，最后给出所求解的近似值。粒子滤 2,…,N,每个粒子代表一个目标可能存在的区域。 波包括2个步骤：预测和更新。 2)粒子传播过程 已知系统在k时刻之前的所有观测z1k-=1, 根据目标运动的动态模型，每个粒子都遵循一 2,…,-1（其中4为系统在k时刻的观测值)，那么 个状态转移过程独立地进行自身传播，从而得到下 系统在k时刻的状态x:可由式(1)给出的模型进行 一时刻的新粒子集。即k时刻的粒子集{x}通过 预测911小. 状态转移，得到k+1时刻的粒子集{x1}。 p(x|1k-1)=p(x4lx4-1)p(x-1lak-1)dx-1(1) 3)观测过程 当得到系统在k时刻的观测4后，就可以根据贝叶 对传播后的新粒子进行观测，即计算每个新粒 斯公式得到 子所代表的状态与初始目标状态的相似度。也即计 p()(p() 算每个新粒子区域的颜色直方图与参考直方图的 (2) p(k1之1：k-1) Bhattacharyya距离（每个粒子应该赋予的权值），距 式中：p(4!x)由事先选择的观测模型给出，当前 离越大，说明该粒子越接近参考模型，所赋予的权值 时刻的状态p(a4Ix)由N个权值为w的样本x表 就越大，反之权值就越小。 示，这N个样本就是粒子滤波器中的粒子，权值代 4)估计过程 表该粒子与所跟踪目标的相似程度。样本x4通常 对M≤N个权值较大的粒子计算后验概率，求 是从一个已知的分布g(x1x-1,)中获得，其 出期望值： 权值计算公式为 E()=∑ (7) p(a1)p(x1x） M三1 0=0inq(4x-1】 (3) 式中：0+1是对每个粒子区域按式(6)求得的权值 (概率)。用式(7)计算的期望值作为目标k+1时 权值就是观测概率p(4|x:),因此输出的状态估计 刻状态的最优估计。 可表示为 N x=∑wx 3 机械臂轨迹规划 (4) i=1 3.1七自由度机械臂子系统 通过计算图像的RGB颜色直方图建立目标的颜色 本子系统利用Schunk七自由度机械臂和Bar 模型。假设建立的直方图共有N个灰度级，则颜色 rett四自由度灵巧手，融合Microsoft Kinect和触觉传 直方图表示为q°={q“(n)},n=1,2,…,N,且 ∑g(m)=l。 感器阵列的信息，共同完成目标的稳定抓取。在K- nect的深度信息的引导下，机械臂接近目标物体，当 在初始时刻(k,时刻)手工选取目标位置x:, 到达灵巧手的工作空间后，在触觉信息的配合下，进
普通 ＲＧＢ 摄像机能够以３０ 帧／ ｓ 的速率传送６４０× ４８０ 分辨率的 ＲＧＢ 图像，而红外发射器和摄像机则是 用来采集相应的深度信息，通过内部芯片转换并传送 ３２０×２４０ 分辨率的深度图像［ ６⁃７］ 。 文献［１４］利用基于深度信息的背景相减法，将 当前帧与背景模型进行差值运算，从而得出环境中 目标的位置与大小等信息特征，最终能够准确地提 取物品桌上的目标，实现目标检测。 由于本系统为 双臂协调操作系统，物体位于六自由度机械臂末端 的手爪上，这对灵巧手的精确抓取提出了更高的要 求，对目标检测的位置信息需更加精确。 ２．２ 基于颜色的粒子滤波跟踪算法 粒子滤波［ ８ ］是一种序贯蒙特卡洛滤波方法，其 本质是用一系列随机抽取的样本代替状态的后验概 率分布。 粒子滤波的基本思想是：首先建立一个概 率模型或随机过程，使它的参数等于问题的解，然后 通过对模型或过程的观察或抽样试验来计算所求参 数的统计特征，最后给出所求解的近似值。 粒子滤 波包括 ２ 个步骤：预测和更新。 已知系统在 ｋ 时刻之前的所有观测 ｚ１：ｋ－１ ＝ ｚ１ ， ｚ２ ，…，ｚｋ－１ （其中 ｚｋ 为系统在 ｋ 时刻的观测值），那么 系统在 ｋ 时刻的状态 ｘｋ 可由式（１）给出的模型进行 预测［ ９⁃１１ ］ ： ｐ（ｘｋ ｜ ｚ１：ｋ－１） ＝ ∫ｐ（ｘｋ ｜ ｘｋ－１）ｐ（ｘｋ－１ ｜ ｚ１：ｋ－１）ｄｘｋ－１ （１） 当得到系统在 ｋ 时刻的观测 ｚｋ 后，就可以根据贝叶 斯公式得到 ｐ（ｘｋ ｜ ｚ１：ｋ） ＝ ｐ（ｚｋ ｜ ｘｋ）ｐ（ｘｋ ｜ ｚ１：ｋ－１ ） ｐ（ｚｋ ｜ ｚ１：ｋ－１ ） （２） 式中： ｐ（ｚｋ ｜ ｘｋ） 由事先选择的观测模型给出，当前 时刻的状态 ｐ（ｚｋ ｜ ｘｋ） 由 Ｎ 个权值为 ｗ ｉ ｋ 的样本 ｘ ｉ ｋ 表 示，这 Ｎ 个样本就是粒子滤波器中的粒子，权值代 表该粒子与所跟踪目标的相似程度。 样本 ｘ ｉ ｋ 通常 是从一个已知的分布 ｑ（ｘｋ ｜ ｘ１：ｋ－１ ，ｚ１：ｋ） 中获得，其 权值计算公式为 ｗ ｉ ｋ ＝ ｗ ｉ ｋ－１ ｐ（ｚｋ ｜ ｘ ｉ ｋ ）ｐ（ｘ ｉ ｋ ｜ ｘ ｉ ｋ－１ ） ｑ（ｘｋ ｜ ｘ１：ｋ－１ ，ｚ１：ｋ） （３） 权值就是观测概率 ｐ（ｚｋ ｜ ｘｋ） ，因此输出的状态估计 可表示为 ｘ ＾ ＝ ∑ Ｎ ｉ ＝ １ ｗ ｉ ｋ ｘｋ （４） 通过计算图像的 ＲＧＢ 颜色直方图建立目标的颜色 模型。 假设建立的直方图共有 Ｎ 个灰度级，则颜色 直方图表示为 ｑ ∗ ＝ ｛ｑ ∗ （ｎ）｝，ｎ ＝ １，２，…，Ｎ ， 且 ∑ Ｎ ｎ ＝ １ ｑ ∗ （ｎ） ＝ １。 在初始时刻（ ｋ１ 时刻）手工选取目标位置 ｘ ∗ ｋ１ ， 得到颜色参考模型为 ｑ ∗ ， ｋ 时刻得到某一粒子的颜 色直方图为 ｑｋ（ｘ） 。 本文需要在 ＲＧＢ 颜色分布空 间上选择一个距离 Ｄ ，该距离使得每一次估计均值 时的方差最小。 通常使用 Ｂｈａｔｔａｃｈａｒｙｙａ 相似性系数 如式（５），这种度量概率分布之间差别的距离值介 于 ［０，１］ ，且忽略了那些不存在的灰度等级。 Ｄ［ｑ ∗ ，ｑｋ（ｘ）］ ＝ １ － ∑ Ｎ ｎ ＝ １ ｑ ∗ （ｎ）ｑ [ ｋ（ｎ；ｘ） ] １ ２ （５） 文中用式（５）表示的 Ｂｈａｔｔａｃｈａｒｙｙａ 距离［１４］统计跟踪 窗口区域内的颜色直方图，并将其与参考直方图比 较，求解似然估计。 可以取 ｐ（ｚｋ ｜ ｘｋ） ∝ ｅｘｐ｛ － λＤ ２ ［ｑ ∗ ，ｑｋ（ｘｋ）］｝ （６） 此时计算的就是 ｐ（ｚｋ ｜ ｘｋ） 就是 ｋ 时刻该粒子权值。 根据上文论述，基于颜色的粒子滤波跟踪算法 分为以下几个过程： １）初始化过程 在观测图像中手工标注感兴趣的目标区域，以 该区域中心为原点，产生粒子集 ｘ ｎ ｋ１ { } ，其中 ｎ ＝ １， ２，…，Ｎ ，每个粒子代表一个目标可能存在的区域。 ２）粒子传播过程 根据目标运动的动态模型，每个粒子都遵循一 个状态转移过程独立地进行自身传播，从而得到下 一时刻的新粒子集。 即 ｋ 时刻的粒子集 ｘ ｎ ｋ { } 通过 状态转移，得到 ｋ ＋ １ 时刻的粒子集 ｘ ｎ ｋ＋１ { } 。 ３）观测过程 对传播后的新粒子进行观测，即计算每个新粒 子所代表的状态与初始目标状态的相似度。 也即计 算每个新粒子区域的颜色直方图与参考直方图的 Ｂｈａｔｔａｃｈａｒｙｙａ 距离（每个粒子应该赋予的权值），距 离越大，说明该粒子越接近参考模型，所赋予的权值 就越大，反之权值就越小。 ４）估计过程 对 Ｍ ≤ Ｎ 个权值较大的粒子计算后验概率，求 出期望值： Ｅ（ｘｋ＋１ ） ＝ ∑ Ｍ ｎ ＝ １ ｗ Ｍ ｋ＋１ ｘ Ｍ ｋ＋１ （７） 式中： ｗｋ＋１ 是对每个粒子区域按式（６）求得的权值 （概率）。 用式（７）计算的期望值作为目标 ｋ ＋ １ 时 刻状态的最优估计。 ３ 机械臂轨迹规划 ３．１ 七自由度机械臂子系统 本子系统利用 Ｓｃｈｕｎｋ 七自由度机械臂和 Ｂａｒ⁃ ｒｅｔｔ 四自由度灵巧手，融合 Ｍｉｃｒｏｓｏｆｔ Ｋｉｎｅｃｔ 和触觉传 感器阵列的信息，共同完成目标的稳定抓取。 在 Ｋｉ⁃ ｎｅｃｔ 的深度信息的引导下，机械臂接近目标物体，当 到达灵巧手的工作空间后，在触觉信息的配合下，进 第 ３ 期 张雪华，等：采用 Ｋｉｎｅｃｔ 的多臂协调操作系统 ·３０９·
·310· 智能系统学报 第9卷 行精确稳定抓取。 这也是目标在图像上的期望位置。比例控制器的反馈 七自由度 机器人 控制率为P=k(),为了使运动目标始终保持在图 压力传感器 像的中心区域，需将目标在图像上的当前位置与期望 位置之间的误差传到控制器中，控制机器人运动，改变 机器人在基坐标系下的位置。 机器人 4自由度灵巧手 初始位姿 0 Kienct摄像机 J摄像机 机器人 期望位姿 图3抓取系统的构成 Fig.3 The composition of the scraping system 目标 七自由度机械臂的路径规划算法利用著名的 RRT算法。RRT(Rapidly-exploring random trees)是 一种随机规划算法，是由Steven M.Lavalle和James J.Kuffnert121]首先提出来的一种路径规划算法。 RRT方法被广泛应用在机器人轨迹优化和路径规 划中，不仅计算快速，而且形式简单、概率完全。文 基座 献[14]中详细介绍了基于工作空间RRT算法的工 图4自由度机械臂结构 作原理，通过构建RRT树的方法实现逆运动学轨迹 Fig.4 Six degrees of freedom mechanical arm structure 优化，从而获取相应的关节运动，完成了机械臂自主 由上述可知，期望图像与当前图像的误差为 抓取任务。七自由度机械臂控制系统的轨迹规划由 2部分组成，首先机械臂抓取目标的前提是将机械 f=∫2-。=[u。-。g-o]T(8) 臂零位设置为初始位置，将抓取目标的位置设置为 将图像误差作为控制器的输入，控制器的输出 目标点，从而进行轨迹优化：其次如何利用六自由度 为机器人各关节在基坐标系下的位移增量变化值， 机械臂将目标物准确放到框内在本系统中也是非常 其计算公式为 重要的，采用将抓取点和投掷，点分别作为初始点和 P=P.-P。=[x。-x0yg-yo]T (9) 目标点的方法来完成。通过RRT算法进行数据解 控制策略流程如图5所示。 算，机械臂获取到RRT解算出的关节角度并运行到 指定位置进行目标抓取，实现双臂协调操作。同样 开始 的道理，机械臂将抓取的目标物投掷到指定位置的 框中，以此来完成整改双臂协调操作任务。 目标检测 3.2六自由度机械臂运动控制 当前位置， 如图3所示为六自由度机器人系统的结构图。图 中0、02、03分别为肩、大臂、小臂关节，主要负责机器 前位置 当前位置 当前位置 人的移动：0、0、0。为手腕的3个关节，主要负责手 爪的摇摆、俯仰和偏转：0，、02、03、0，分别为01、02、03、 +40 +40 +40 0,机器人从初始位置到期望位置的旋转角度，方向如 平面运动控制 箭头所示，手爪和摄像机安装在机械臂的末端，因此该 +4B +40 子系统也被称为“眼在手上”视觉伺服控制系统。视觉 控制器采用比例控制器的方法控制机器人关节运动， 深度运动控制 假设目标在机器人基坐标系下的位置为P。= 视觉伺服) [x,y】',目标在图像平面的位置为= 图5控制策略流程 [山】T,图像平面的中心位置为∫=[u]T, Fig.5 The flow chart of control strategy
行精确稳定抓取。 图 ３ 抓取系统的构成 Ｆｉｇ．３ Ｔｈｅ ｃｏｍｐｏｓｉｔｉｏｎ ｏｆ ｔｈｅ ｓｃｒａｐｉｎｇ ｓｙｓｔｅｍ 七自由度机械臂的路径规划算法利用著名的 ＲＲＴ 算法。 ＲＲＴ（Ｒａｐｉｄｌｙ⁃ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅｓ） 是 一种随机规划算法，是由 Ｓｔｅｖｅｎ Ｍ． Ｌａｖａｌｌｅ 和 Ｊａｍｅｓ Ｊ． Ｋｕｆｆｎｅｒ ［１２⁃１３］ 首先提出来的一种路径规划算法。 ＲＲＴ 方法被广泛应用在机器人轨迹优化和路径规 划中，不仅计算快速，而且形式简单、概率完全。 文 献［１４］中详细介绍了基于工作空间 ＲＲＴ 算法的工 作原理，通过构建 ＲＲＴ 树的方法实现逆运动学轨迹 优化，从而获取相应的关节运动，完成了机械臂自主 抓取任务。 七自由度机械臂控制系统的轨迹规划由 ２ 部分组成，首先机械臂抓取目标的前提是将机械 臂零位设置为初始位置，将抓取目标的位置设置为 目标点，从而进行轨迹优化；其次如何利用六自由度 机械臂将目标物准确放到框内在本系统中也是非常 重要的，采用将抓取点和投掷点分别作为初始点和 目标点的方法来完成。 通过 ＲＲＴ 算法进行数据解 算，机械臂获取到 ＲＲＴ 解算出的关节角度并运行到 指定位置进行目标抓取，实现双臂协调操作。 同样 的道理，机械臂将抓取的目标物投掷到指定位置的 框中，以此来完成整改双臂协调操作任务。 ３．２ 六自由度机械臂运动控制 如图 ３ 所示为六自由度机器人系统的结构图。 图 中 Ｏ１、Ｏ２、Ｏ３ 分别为肩、大臂、小臂关节，主要负责机器 人的移动； Ｏ４、Ｏ５、Ｏ６ 为手腕的 ３ 个关节，主要负责手 爪的摇摆、俯仰和偏转； θ１、θ２、θ３、θ４ 分别为 Ｏ１、Ｏ２、Ｏ３、 Ｏ４ 机器人从初始位置到期望位置的旋转角度，方向如 箭头所示，手爪和摄像机安装在机械臂的末端，因此该 子系统也被称为“眼在手上”视觉伺服控制系统。 视觉 控制器采用比例控制器的方法控制机器人关节运动， 假设 目 标 在 机 器 人 基 坐 标 系 下 的 位 置 为 Ｐ０ ＝ ｘ０ ｙ [ ０ ] Ｔ ， 目 标 在 图 像 平 面 的 位 置 为 ｆ０ ＝ ｕ０ ｖ０ [ ] Ｔ ，图像平面的中心位置为 ｆｇ ＝ ｕｇ ｖｇ [ ] Ｔ ， 这也是目标在图像上的期望位置。 比例控制器的反馈 控制率为 Ｐ · ＝ ｋφ（ｆ · ） ，为了使运动目标始终保持在图 像的中心区域，需将目标在图像上的当前位置与期望 位置之间的误差传到控制器中，控制机器人运动，改变 机器人在基坐标系下的位置。 图 ４ 自由度机械臂结构 Ｆｉｇ．４ Ｓｉｘ ｄｅｇｒｅｅｓ ｏｆ ｆｒｅｅｄｏｍ ｍｅｃｈａｎｉｃａｌ ａｒｍ ｓｔｒｕｃｔｕｒｅ 由上述可知，期望图像与当前图像的误差为 ｆ · ＝ ｆｇ － ｆ０ ＝ ［ｕｇ － ｕ０ ｖｇ － ｖ０ ］ Ｔ （８） 将图像误差作为控制器的输入，控制器的输出 为机器人各关节在基坐标系下的位移增量变化值， 其计算公式为 Ｐ · ＝ Ｐｇ － Ｐ０ ＝ ［ｘｇ － ｘ０ ｙｇ － ｙ０ ］ Ｔ （９） 控制策略流程如图 ５ 所示。 图 ５ 控制策略流程 Ｆｉｇ．５ Ｔｈｅ ｆｌｏｗ ｃｈａｒｔ ｏｆ ｃｏｎｔｒｏｌ ｓｔｒａｔｅｇｙ ·３１０· 智 能 系 统 学 报 第 ９ 卷
第3期 张雪华，等：采用Kinect的多臂协调操作系统 ·311- 根据图像误差控制机器人具体步骤如下： 0.60 0.65 1)记录机器人期望位置下图像中目标的中心 0.55 0.60 点在图像上的坐标(xo,o)、矩形框的宽和高(， 0.50 0.55 -0.20 h。)以及期望位置下的关节角： 0.4 -0.30-0.80.6 .25 2)设定限制条件，当目标在图像中心位置的2 0.32-0.660.60.63 (a)RRT树 (b)末端轨迹 个方向上的误差绝对值在设定范围内，并且摄像机 1.0r 与目标的距离适中时，机器人的关节角不变； 0.5**-x o V ★艺 3)当目标超出设定范围时，将图像分为4个区 域，并分别计算当前图像与期望图像间的误差和机 0 1 23 器人当前位置与期望位置间的关节角度差值： -1.0024 (©)各关节轨迹(d末端坐标分量轨迹 4)运动控制过程中，根据期望值=当前 0.70 1.0 值+△0，控制机器人在循环中根据图像误差进行相 0.65 0.5 应的运动。 0.60 0 0. 0.4 4实验 2-0494844 0第.2 0030.80.6 (e)RRT树 ()末端轨迹 为了完成多臂协作物体交接的任务，验证上述 1.0r 研究内容的可行性，本文设计了一套双臂协作的实 0.5 -x 验系统，实验场景如图6所示。 o 交接CCD -0.5 目标物摄像头 -1.0 0 0.51.0 00.51.0 (g)各关节轨迹 ()末端坐标分量轨迹 图7轨迹优化仿真结果 灵巧手 Fig.7 Trajectory optimization simulation results 七自由度 六自由度 摄像头 机械臂 (1 (2) (3) 图6实验场景 Fig.6 The scene graph (4)】 (5 (6) 4.1实验相关说明 实验系统包括双机械臂、Kinect、机械臂末端摄像 机、灵巧手、控制器、交接目标物。实验任务：利用六自 由度机械臂抓取目标，通过视觉伺服控制机械臂将目 7 (8) (9) 标递送到七自由度机械臂附近区域，并通过上方Kinect 视觉传感器对目标进行检测，七自由度通过检测的目 标点位置信息（抓取点的三维坐标）对目标进行自主抓 (10) 12 取，最终实现目标物交接后将其投掷到指定位置。 4.2实验过程 图7为七自由度机械臂2次轨迹优化的解算结 13 (14) 果。其中，图7(a)~(d)为以机械臂末端当前位置为初 始点，以目标物抓取点为目标点进行轨迹优化的仿真 结果，包括RT树、机械臂末端轨迹、机械臂各关节轨 迹和末端坐标分量轨迹；图7(e)~(h)为以目标的抓取 16 (17) 点为初始位置，以投掷点为目标位置进行轨迹优化的 图8实验效果图 结果。双臂协作实验效果如图8所示。 Fig.8 The experimental result
根据图像误差控制机器人具体步骤如下： １）记录机器人期望位置下图像中目标的中心 点在图像上的坐标 （ｘ０ ，ｙ０ ） 、矩形框的宽和高 （ｗ０ ， ｈ０ ） 以及期望位置下的关节角； ２）设定限制条件，当目标在图像中心位置的 ２ 个方向上的误差绝对值在设定范围内，并且摄像机 与目标的距离适中时，机器人的关节角不变； ３）当目标超出设定范围时，将图像分为 ４ 个区 域，并分别计算当前图像与期望图像间的误差和机 器人当前位置与期望位置间的关节角度差值； ４） 运 动 控 制 过 程 中， 根 据 期 望 值 ＝ 当 前 值＋ Δθ ，控制机器人在循环中根据图像误差进行相 应的运动。 ４ 实验 为了完成多臂协作物体交接的任务，验证上述 研究内容的可行性，本文设计了一套双臂协作的实 验系统，实验场景如图 ６ 所示。 图 ６ 实验场景 Ｆｉｇ．６ Ｔｈｅ ｓｃｅｎｅ ｇｒａｐｈ ４．１ 实验相关说明 实验系统包括双机械臂、Ｋｉｎｅｃｔ、机械臂末端摄像 机、灵巧手、控制器、交接目标物。 实验任务：利用六自 由度机械臂抓取目标，通过视觉伺服控制机械臂将目 标递送到七自由度机械臂附近区域，并通过上方 Ｋｉｎｅｃｔ 视觉传感器对目标进行检测，七自由度通过检测的目 标点位置信息（抓取点的三维坐标）对目标进行自主抓 取，最终实现目标物交接后将其投掷到指定位置。 ４．２ 实验过程 图 ７ 为七自由度机械臂 ２ 次轨迹优化的解算结 果。 其中，图 ７（ａ） ～（ｄ）为以机械臂末端当前位置为初 始点，以目标物抓取点为目标点进行轨迹优化的仿真 结果，包括 ＲＲＴ 树、机械臂末端轨迹、机械臂各关节轨 迹和末端坐标分量轨迹；图 ７（ｅ） ～（ｈ）为以目标的抓取 点为初始位置，以投掷点为目标位置进行轨迹优化的 结果。 双臂协作实验效果如图 ８ 所示。 图 ７ 轨迹优化仿真结果 Ｆｉｇ．７ Ｔｒａｊｅｃｔｏｒｙ ｏｐｔｉｍｉｚａｔｉｏｎ ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ 图 ８ 实验效果图 Ｆｉｇ．８ Ｔｈｅ ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔ 第 ３ 期 张雪华，等：采用 Ｋｉｎｅｃｔ 的多臂协调操作系统 ·３１１·
.312 智能系统学报 第9卷 本次实验的目标物为矩形条和圆桶，通过Kinect 器人，2012,34(3)：354-362. 目标检测可以得出目标物轮廓信息，通过轮廓，点的位 XIA Xuan,LIU Huaping,XU Weiming,et al.DSP-based 置计算出抓取点的坐标为(0.132，-0.631,0.598)，投掷 active vision system[J].Robot,2012,34(3):354-362. 点的坐标为(-0.683,0.562,0.257)，这2个点为七自由度 [9]李孟敏改进粒子滤波算法及其在目标跟踪中应用研究 [D].合肥：合肥工业大学，2011：9-11. 机械臂进行目标抓取的点和投掷的点，也是路径规划 LI Mengmin.Improved particle filter algorithm and its appli- 的起始点和终点。从图8中可以看出，本次实验共进 cation research in the target tracking[D].Hefei:Hefei uni- 行了2次连续的物体交接实验。(1)~(8)显示为以矩 versity of technology,2011:9-11. 形条为目标实现第1次双臂协调操作，并完成物体交 [10]张波.基于粒子滤波的图像跟踪算法研究[D].上海：上 接任务，(9)~(17)为以圆桶为目标实现第2次双臂协 海交通大学，2007：17-19. 调操作，两次操作是连续进行的。 ZHANG Bo.Image tracking algorithm based on particle fil- ter is studied D.Shanghai:Shanghai Jiaotong Universi- 5结束语 y,2007:17-19. [11 PeREZ P,HUE C,VERMAAK J,et al.Color-based 本文通过处理由Kinect获取的图像信息来确 probabilistic tracking[C]//Proceedings of the 7th.Euro- 定目标抓取点的位置，并根据基于工作空间的RRT pean Conference on Computer Vision,Copenhagen,Den- 算法实现机械臂路径规划，与此同时另一台机械臂 mark,2002:661-675. 通过视觉伺服实现了目标抓取与递送。文章通过构 [12]蒋平，胡凤轩，陈辉堂，等.机器人手眼协调的图像直接 建一套双臂协作实验系统完成了多臂协作物体交接 反馈方法[J].控制理论与应用，1997,14(6)：431-437. 的任务，并验证了本文研究内容的可行性。 JIANG Ping,HU Fengxuan,CHEN Huitang,et al.The ro- bot hand-eye coordination image direct feedback method[]. 参考文献： Control Theory and Application,1997,14(6):431-437. [1]周军，丁希仑，陆震.冗余度双臂机器人轴孔装配的三维 [13]LU C,MJOLSNESS E HAGER G.Online computation of 动态仿真与实验[J].机器人，2006,28(4)：422-427. exterior orientation with application to hand-eye calibration ZHOU Jun,LIU Huaping,LU Zhen.Redundant dual-arm ro- [C]//DCS RR-1046.New Haven,CT,1994:122-125. bot shaft hole assembly of 3 D dynamic simulation with the [l4]韩峥，刘华平，黄文炳，等.基于Kinect的机械臂目标抓 experiment[J].Robot,2006,28(4):422-427. 取[J]智能系统学报，2013,8(2)：149-155. [2]芦俊，席文明，颜景平.视觉传感器引导下的双臂装配 HAN Zheng,LIU Huaping,HUANG Wenbing,et al. 机器人最佳装配位置的确定[J].工业仪表与自动化装 Based on the mechanical arm to access target capture[J]. 置，2002(2)：3-7. Caai Transactions on Intelligent Systems,2013,8(2): 149-155. JUN Lu,XI Wenming,YAN Jingping.Guided by the visual sensor assembly robot arms optimal assembly position[]. 作者简介： Industrial Instrumentation and Automation Devices,2002 张香华，女，1988年生，硕士研究生 (2):3-7. 主要研究方向为智能控制及机器人。 [3]LAVALLE M,KUFFNER J.Randomized kinodynamic plan- ning[C]//Proceedings of the 1999 IEEE International Con- ference on Robotics Automation Detroit.Michigan,USA, 1999:108-112. [4]LAVALLE M,KUFFNER JJ R.RRT-Connect An effi- cient approach to single-query path planning[C]//Proceed- 刘华平，男，1976年生，副教授，主 ings of the 2000 IEEE International Conference on Robotics 要研究方向为智能控制及机器人、计算 Automation San Francisco,USA,2000:13-24. 机视觉等。 [5]GANGANATH N,LEUNG H.Mobile robot localization u- sing odometry and Kinect sensor C//2010 IEEE Interna- tional Conference on Emerging Signal Processing Applica- tions.NJ,USA,2010:91-94. 孙富春，男，教授，博士生导师」 [6]BENAVIDEZ P,JAMSHIDI M.Mobile robot navigation and EEE高级会员中国人工智能学会理 target tracking system[C]//2011 6th International Confer- 事、智能控制与智能管理专业委员会副 ence on System of Systems Engineering SoSE 2011).NJ. 主任兼秘书长，主要研究方向为智能控 USA,2011:299-304. 制、机器人与飞行器的导航与控制、网 [7]RAKPRAYOON P,RUCHANURUCKS M.AdaCoundoul. 络控制系统、人工认知系统的信息感知 Kinect-based obstacle detection for manipulator[C//2011 和处理等。曾获全国优秀博士论文奖、北京市科学技术进步 IEEE/SICE International Symposium on System Integration, 二等。发表学术论文120余篇，其中被SCI检索52篇。 NJ.USA,2011:68-73. [8]夏轩，刘华平，许伟明.基于DSP的主动视觉系统[J].机
本次实验的目标物为矩形条和圆桶，通过 Ｋｉｎｅｃｔ 目标检测可以得出目标物轮廓信息，通过轮廓点的位 置计算出抓取点的坐标为（０．１３２，－０．６３１，０．５９８），投掷 点的坐标为（⁃０．６８３，０．５６２，０．２５７），这 ２ 个点为七自由度 机械臂进行目标抓取的点和投掷的点，也是路径规划 的起始点和终点。 从图 ８ 中可以看出，本次实验共进 行了 ２ 次连续的物体交接实验。 （１） ～（８）显示为以矩 形条为目标实现第 １ 次双臂协调操作，并完成物体交 接任务，（９） ～（１７）为以圆桶为目标实现第 ２ 次双臂协 调操作，两次操作是连续进行的。 ５ 结束语 本文通过处理由 Ｋｉｎｅｃｔ 获取的图像信息来确 定目标抓取点的位置，并根据基于工作空间的 ＲＲＴ 算法实现机械臂路径规划，与此同时另一台机械臂 通过视觉伺服实现了目标抓取与递送。 文章通过构 建一套双臂协作实验系统完成了多臂协作物体交接 的任务，并验证了本文研究内容的可行性。 参考文献： ［１］周军，丁希仑，陆震． 冗余度双臂机器人轴孔装配的三维 动态仿真与实验［Ｊ］． 机器人， ２００６， ２８（４）： ４２２⁃４２７． ＺＨＯＵ Ｊｕｎ，ＬＩＵ Ｈｕａｐｉｎｇ，ＬＵ Ｚｈｅｎ． Ｒｅｄｕｎｄａｎｔ ｄｕａｌ⁃ａｒｍ ｒｏ⁃ ｂｏｔ ｓｈａｆｔ ｈｏｌｅ ａｓｓｅｍｂｌｙ ｏｆ ３ Ｄ ｄｙｎａｍｉｃ ｓｉｍｕｌａｔｉｏｎ ｗｉｔｈ ｔｈｅ ｅｘｐｅｒｉｍｅｎｔ［Ｊ］． Ｒｏｂｏｔ， ２００６， ２８（４）： ４２２⁃４２７． ［２］芦俊， 席文明， 颜景平． 视觉传感器引导下的双臂装配 机器人最佳装配位置的确定［ Ｊ］． 工业仪表与自动化装 置， ２００２（２）： ３⁃７． ＪＵＮ Ｌｕ， ＸＩ Ｗｅｎｍｉｎｇ， ＹＡＮ Ｊｉｎｇｐｉｎｇ． Ｇｕｉｄｅｄ ｂｙ ｔｈｅ ｖｉｓｕａｌ ｓｅｎｓｏｒ ａｓｓｅｍｂｌｙ ｒｏｂｏｔ ａｒｍｓ ｏｐｔｉｍａｌ ａｓｓｅｍｂｌｙ ｐｏｓｉｔｉｏｎ ［ Ｊ］． Ｉｎｄｕｓｔｒｉａｌ Ｉｎｓｔｒｕｍｅｎｔａｔｉｏｎ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｄｅｖｉｃｅｓ， ２００２ （２）： ３⁃７． ［３］ＬＡＶＡＬＬＥ Ｍ， ＫＵＦＦＮＥＲ Ｊ． Ｒａｎｄｏｍｉｚｅｄ ｋｉｎｏｄｙｎａｍｉｃ ｐｌａｎ⁃ ｎｉｎｇ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ Ｉ９９９ ｌＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎ⁃ ｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ＆ Ａｕｔｏｍａｔｉｏｎ Ｄｅｔｒｏｉｔ． Ｍｉｃｈｉｇａｎ， ＵＳＡ， １９９９： １０８⁃１１２． ［４］ ＬＡＶＡＬＬＥ Ｍ， ＫＵＦＦＮＥＲ Ｊ Ｊ Ｒ． ＲＲＴ⁃Ｃｏｎｎｅｃｔ ： Ａｎ ｅｆｆｉ⁃ ｃｉｅｎｔ ａｐｐｒｏａｃｈ ｔｏ ｓｉｎｇｌｅ⁃ｑｕｅｒｙ ｐａｔｈ ｐｌａｎｎｉｎｇ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄ⁃ ｉｎｇｓ ｏｆ ｔｈｅ ２０００ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ＆ Ａｕｔｏｍａｔｉｏｎ ． Ｓａｎ Ｆｒａｎｃｉｓｃｏ ， ＵＳＡ， ２０００： １３⁃２４． ［５］ＧＡＮＧＡＮＡＴＨ Ｎ， ＬＥＵＮＧ Ｈ． Ｍｏｂｉｌｅ ｒｏｂｏｔ ｌｏｃａｌｉｚａｔｉｏｎ ｕ⁃ ｓｉｎｇ ｏｄｏｍｅｔｒｙ ａｎｄ Ｋｉｎｅｃｔ ｓｅｎｓｏｒ［Ｃ］ ／ ／ ２０１０ ＩＥＥＥ Ｉｎｔｅｒｎａ⁃ ｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｅｍｅｒｇｉｎｇ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｉｎｇ Ａｐｐｌｉｃａ⁃ ｔｉｏｎｓ． ＮＪ， ＵＳＡ， ２０１０： ９１⁃９４． ［６］ＢＥＮＡＶＩＤＥＺ Ｐ， ＪＡＭＳＨＩＤＩ Ｍ． Ｍｏｂｉｌｅ ｒｏｂｏｔ ｎａｖｉｇａｔｉｏｎ ａｎｄ ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ ｓｙｓｔｅｍ［ Ｃ］ ／ ／ ２０１１ ６ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒ⁃ ｅｎｃｅ ｏｎ Ｓｙｓｔｅｍ ｏｆ Ｓｙｓｔｅｍｓ Ｅｎｇｉｎｅｅｒｉｎｇ （ ＳｏＳＥ ２０１１）． ＮＪ， ＵＳＡ， ２０１１： ２９９⁃３０４． ［７］ ＲＡＫＰＲＡＹＯＯＮ Ｐ， ＲＵＣＨＡＮＵＲＵＣＫＳ Ｍ． ＡｄａＣｏｕｎｄｏｕｌ． Ｋｉｎｅｃｔ⁃ｂａｓｅｄ ｏｂｓｔａｃｌｅ ｄｅｔｅｃｔｉｏｎ ｆｏｒ ｍａｎｉｐｕｌａｔｏｒ［Ｃ］ ／ ／ ２０１１ ＮＪ， ＵＳＡ， ２０１１： ６８⁃７３． ［８］夏轩，刘华平，许伟明．基于 ＤＳＰ 的主动视觉系统［ Ｊ］．机 器人， ２０１２， ３４（３）： ３５４⁃３６２． ＸＩＡ Ｘｕａｎ， ＬＩＵ Ｈｕａｐｉｎｇ， ＸＵ Ｗｅｉｍｉｎｇ， ｅｔ ａｌ． ＤＳＰ⁃ｂａｓｅｄ ａｃｔｉｖｅ ｖｉｓｉｏｎ ｓｙｓｔｅｍ［Ｊ］． Ｒｏｂｏｔ， ２０１２， ３４（３）： ３５４⁃３６２． ［９］李孟敏．改进粒子滤波算法及其在目标跟踪中应用研究 ［Ｄ］．合肥：合肥工业大学， ２０１１： ９⁃１１． ＬＩ Ｍｅｎｇｍｉｎ． Ｉｍｐｒｏｖｅｄ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ａｌｇｏｒｉｔｈｍ ａｎｄ ｉｔｓ ａｐｐｌｉ⁃ ｃａｔｉｏｎ ｒｅｓｅａｒｃｈ ｉｎ ｔｈｅ ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ［Ｄ］． Ｈｅｆｅｉ： Ｈｅｆｅｉ ｕｎｉ⁃ ｖｅｒｓｉｔｙ ｏｆ ｔｅｃｈｎｏｌｏｇｙ， ２０１１： ９⁃１１． ［１０］张波． 基于粒子滤波的图像跟踪算法研究［Ｄ］．上海：上 海交通大学， ２００７： １７⁃１９． ＺＨＡＮＧ Ｂｏ． Ｉｍａｇｅ ｔｒａｃｋｉｎｇ ａｌｇｏｒｉｔｈｍ ｂａｓｅｄ ｏｎ ｐａｒｔｉｃｌｅ ｆｉｌ⁃ ｔｅｒ ｉｓ ｓｔｕｄｉｅｄ［Ｄ］． Ｓｈａｎｇｈａｉ： Ｓｈａｎｇｈａｉ Ｊｉａｏｔｏｎｇ Ｕｎｉｖｅｒｓｉ⁃ ｔｙ， ２００７： １７⁃１９． ［ １１］ ＰéＲＥＺ Ｐ， ＨＵＥ Ｃ， ＶＥＲＭＡＡＫ Ｊ， ｅｔ ａｌ． Ｃｏｌｏｒ⁃ｂａｓｅｄ ｐｒｏｂａｂｉｌｉｓｔｉｃ ｔｒａｃｋｉｎｇ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ７ｔｈ． Ｅｕｒｏ⁃ ｐｅａｎ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ， Ｃｏｐｅｎｈａｇｅｎ， Ｄｅｎ⁃ ｍａｒｋ， ２００２： ６６１⁃６７５． ［１２］蒋平，胡凤轩，陈辉堂，等． 机器人手眼协调的图像直接 反馈方法［Ｊ］．控制理论与应用， １９９７， １４（６）： ４３１⁃４３７． ＪＩＡＮＧ Ｐｉｎｇ， ＨＵ Ｆｅｎｇｘｕａｎ， ＣＨＥＮ Ｈｕｉｔａｎｇ， ｅｔ ａｌ． Ｔｈｅ ｒｏ⁃ ｂｏｔ ｈａｎｄ⁃ｅｙｅ ｃｏｏｒｄｉｎａｔｉｏｎ ｉｍａｇｅ ｄｉｒｅｃｔ ｆｅｅｄｂａｃｋ ｍｅｔｈｏｄ［Ｊ］． Ｃｏｎｔｒｏｌ Ｔｈｅｏｒｙ ａｎｄ Ａｐｐｌｉｃａｔｉｏｎ， １９９７， １４（６）： ４３１⁃４３７． ［１３］ＬＵ Ｃ， ＭＪＯＬＳＮＥＳＳ Ｅ ，ＨＡＧＥＲ Ｇ． Ｏｎｌｉｎｅ ｃｏｍｐｕｔａｔｉｏｎ ｏｆ ｅｘｔｅｒｉｏｒ ｏｒｉｅｎｔａｔｉｏｎ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎ ｔｏ ｈａｎｄ⁃ｅｙｅ ｃａｌｉｂｒａｔｉｏｎ ［Ｃ］ ／ ／ ＤＣＳ ＲＲ⁃１０４６． Ｎｅｗ Ｈａｖｅｎ， ＣＴ，１９９４： １２２⁃１２５． ［１４］韩峥，刘华平，黄文炳，等． 基于 Ｋｉｎｅｃｔ 的机械臂目标抓 取［Ｊ］．智能系统学报， ２０１３， ８（２）： １４９⁃１５５． ＨＡＮ Ｚｈｅｎｇ， ＬＩＵ Ｈｕａｐｉｎｇ， ＨＵＡＮＧ Ｗｅｎｂｉｎｇ， ｅｔ ａｌ． Ｂａｓｅｄ ｏｎ ｔｈｅ ｍｅｃｈａｎｉｃａｌ ａｒｍ ｔｏ ａｃｃｅｓｓ ｔａｒｇｅｔ ｃａｐｔｕｒｅ［ Ｊ］． Ｃａａｉ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１３， ８ （ ２）： １４９⁃１５５． 作者简介： 张雪华，女，１９８８ 年生，硕士研究生， 主要研究方向为智能控制及机器人。 刘华平，男，１９７６ 年生，副教授，主 要研究方向为智能控制及机器人、计算 机视觉等。 孙富 春， 男， 教 授， 博 士 生 导 师， ＩＥＥＥ 高级会员中国人工智能学会理 事、智能控制与智能管理专业委员会副 主任兼秘书长，主要研究方向为智能控 制、机器人与飞行器的导航与控制、网 络控制系统、人工认知系统的信息感知 ３１２· ＩＥＥＥ ／ ＳＩＣＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｓｙｓｔｅｍ Ｉｎｔｅｇｒａｔｉｏｎ， 和处理等。 曾获全国优秀博士论文奖、北京市科学技术进步 二等。 发表学术论文 １２０ 余篇，其中被 ＳＣＩ 检索 ５２ 篇。 · 智 能 系 统 学 报 第 ９ 卷