基于Kinect的冗余机械臂直线推移操作控制

郭 迪 孙富春 刘华平 黄文炳

(清华大学智能技术与系统国家重点实验室，北京 100084)

在机械臂系统中，机械臂执行的很重要的一类操作是对目标物体位置、姿态的改变.通常所采用的方式是在机械臂末端安装末端执行器(灵巧手)，先完成对目标物的抓取，再把目标物移动到指定位置.然而，当目标物尺寸过大、过重或者表面过于光滑，超过机械臂系统的载荷和工作能力时，抓取操作就不能进行.可见，将机械臂系统的功能仅仅局限在抓取操作明显限制了机械臂系统的应用范围.

推移操作作为一种实用的操作，近年来得到广泛的研究.Mason［1］最先研究了推移操作的力学机理，并指出推移操作是机器人系统中一种很重要的操作过程;随后，Mason 等［2－5］建立了机械臂稳定推移操作的力学、控制和规划理论框架，这为推移操作的发展奠定了深厚的研究基础.此后，机械臂的推移操作广泛应用于平面操作台上，用来完成各种各样的任务.例如:桌面的推移操作被用于目标物的分类［6－9］;对目标物抓取前位姿的预处理，完成人形机器人推移－抓取的操作［10］;对杂乱桌面的清理等［11－12］.

本文研究并采用机械臂的推移操作来解决不易抓取的目标物的位置、姿态改变问题.运用Kinect捕捉实时RGB图像和深度图像，确定处于桌面危险位置的目标物，根据位置信息采用改进RRT算法计算出机械臂到达目标点的轨迹，再利用七自由度机械臂系统完成对目标物的推移操作，最终使目标物到达安全位置.讨论并应用实验验证了基于Kinect的冗余机械臂直线推移操作控制.机械臂的推移操作扩展了机械臂的应用范围，并且为解决平面上目标物体的移动提供了一种更为简单实用的方法.

1 冗余机械臂系统的运动学建模

当机械臂系统的自由度数大于任务空间的维数时，即构成有冗余度的机械臂系统.冗余度可以用来改善机器人的运动学和动力学特性，如增加灵活性、避免关节角极限、回避奇异、优化关节加速度、力矩和能量等.本文采用的机械臂系统可以看作一个刚体系.

实验所采用的机械臂系统的每一个关节都对应相应的刚体坐标系，见图1.已知机械臂的DH参数，可得到末端连杆坐标系An相对于基座坐标系 A0的变换矩阵［13］:

图1 机械臂的DH表示法

用向量X表示机械臂末端执行器的位姿，设其维数为m=6，可得X与关节变量q的关系如下:

式中，由q组成的空间称为关节空间;由X组成的空间称为工作空间.式(2)描述了机械臂末端执行器的位姿与关节变量之间的联系，被称为机械臂的运动学方程.

在本实验和大多数情况下可以得知目标物的位置，因此通过逆运动学求解的方法可得到相应的关节位移，进而完成对机械臂的运动控制.本实验中采取了最大似然逆轨迹优化的算法［14］.

2 基于工作空间的RRT算法

高维数关节空间内的RRT(rapidly－exploring random trees)算法是由LaValle等［15］首先提出的一种解决轨迹规划问题的随机规划算法.RRT适合用于解决含有运动、动力学约束的路径规划问题，被广泛应用于机器人路径规划和轨迹规划.

由于实验中对机械臂末端位姿的要求，需要部分限定末端执行器的运动轨迹.传统基于关节空间的轨迹规划算法会造成末端执行器运动路径不可预知的问题.为此，采用在工作空间中构建RRT树的方法，这样可以消除不完整约束，直接满足收敛条件［14］.

设x1是已经给定轨迹的末端执行器的位姿，x2是需要规划的末端位姿.在X2空间内进行RRT树的构建.首先将X2的初始值x2，init赋予RRT树.再由Extend函数返回最近邻近点x2，near以及其对应的关节变量qnear和时间tnear，同时返回新扩展的点xnew以及其时间tnew.得到最近邻近时间节点上和新的时间节点上的末端位姿x0，xT.然后应用最似梯度法对机械臂进行逆运动学轨迹优化，可得到机械臂系统的关节角信息qnew.最后，对qnew进行障碍检测并确定qnew符合边界条件.重复迭代直到生成树中的点距离目标点充分小［14］.

3 实验研究及结果

3.1 实验系统简介

本实验系统由Kinect、七自由度机械臂和灵巧手组成，如图2～图4所示.体感设备Kinect能够以30帧/s的速率几乎同步地提供分辨率为640×480像素的RGB影像和分辨率为320×240像素的深度影像［16］.系统中所采用的七自由度机械臂是由德国SCHUNK公司开发并生产的PowerCube模块组合而成.机械臂的作业范围可达1870 mm×1650 mm×1870 mm，有效负载10 kg.末端抓取机构采用的是美国Barrett Technology公司生产的BH8.280灵巧手.该灵巧手是一个可编程的多手指抓持器，可以抓取不同形状、大小及姿态的目标物体.

图2 Kinect深度摄像机

图3 七自由度模块化机械臂

图4 Barrett灵巧手

3.2 实验及结果

3.2.1 任务描述

Kinect对其视野范围内的目标物进行检测，当有目标物出现在指定区域时，即表示需要对该目标物进行移动操作，使其到达指定位置.接着，对目标物进行形状尺寸检测，判断是否可以对其进行抓取操作.假设当目标物的尺寸超过机械手抓取的量程时则进行推移操作，本实验中采用了一个形状超过灵巧手抓取范围的盒子作为目标物(见图5).机械臂的推移操作过程如下:首先经过改进RRT算法解算，使得机械臂迅速到达最佳推点位置，确认各条件符合推移要求时执行直线推移操作，最终使得目标物到达指定位置.

图5 目标物

3.2.2 目标检测

本实验的操作任务是通过Kinect检测提取出目标，并使机械臂对处于操作平台边缘的目标物进行位置的移动.由于Kinect可以直接提供深度图像，因此采用基于深度图像的背景相减法可获得较好的快速性和实时性.

首先，提取目标没有出现时的深度图像作为背景图像，进行存储;之后，提取当前帧场景的深度图像作为场景图像.将场景图像与背景图像做差，若某点的深度值之差大于某阈值，则认为该点属于目标物.这样就可得到目标物体的全部像素点，进而得到目标物的形状位置等信息.

3.2.3 推点选择

首先假定推移操作满足准静态假设条件，所以在选取推点时仅考虑静态项.通过考虑MAG指标［17］中的静态项来决定稳定推点的位置.将目标的轮廓平均分割为20个点，然后分别计算每点的静态项值，静态项值越高的点越稳定，由图6可见，4号点和14号点拥有更好的稳态性.所以选取这2点中靠近外侧的点作为推移点以便机械手更容易到达.

图6 各点静态指标

3.2.4 推移操作

机械臂对目标物执行了推移操作，但是机械手与目标物仍为2个独立的个体，这样推移规划与轨迹规划就会产生耦合.事实上，推移操作是靠机械臂的轨迹规划实现的.通过将机械手以合适的姿态与目标物相接触但不固连，利用轨迹规划使得机械臂末端到达目标位置，从而带动目标物达到目标位置完成推移操作.因此，在实验中，当机械臂到达推点位置时，使得机械手/臂的姿态满足推移操作的要求，之后在进行推移操作的轨迹规划时，使得机械臂的末端姿态不变.这样，就可以保证机械臂的每次推动都是有效推动.本实验中为了使灵巧手与目标物体拥有更多的接触点，使操作更平稳，采用灵巧手手指操作全程全张开的姿态(见图4)，这样既可使推移过程为线接触［2］，又可使灵巧手处理各种不同的目标物以适用于新的环境［12］.由于目标物总是与机械臂的末端相接触，因此目标物就可按照机械臂末端运动的轨迹进行移动.

3.2.5 演示实验

当Kinect检测到在操作台边缘有物体出现并处于危险位置时，根据Kinect实时传送过来的深度图像对目标物进行轮廓提取，计算出推点.系统首先会采用基于工作空间的RRT轨迹规划的方法得到机械臂的运动轨迹(见图7)，之后机械臂按照规划路径到达目标位置，执行推移操作，使得物体到达安全区域，最后系统复位，当危险区域中再次出现目标物时，重复上述操作.

图7 改进RRT的运算结果

实验中通过2种推移方法完成对目标物的推移:

1)直线推移.根据目标物的初始位姿和目标位姿，按照直线推移的方式使得灵巧手到达指定位置.这样机械臂系统就会按照事先给定的轨迹移动，其优点是可以实时改变目标物的位置姿态以达到目标位置姿态(见图8(a)).

2)导轨移动.机械臂系统是安装在平行导轨上的，可通过移动导轨来达到推移的目的(见图8(b)).

图8 实验结果

通过对比看出，2种方法都可通过推移操作使得目标物从桌面危险区域到达安全位置.但由于直线推移操作中，机械臂系统的操作是在桌面二自由度平面中进行的，因此可以在推移过程中对目标物的位置姿态进行调整，实验中就实现了将目标物的姿态调整的操作，操作空间为平面二自由度中的任意位置，可使得移动的路径最短、效率最高;而导轨移动过程中，由于机械臂系统只能沿导轨移动，因此将推移的操作空间限定在了一个自由度的轨道上，使得推移只能朝一个方向进行，大大限制了推移操作的工作空间，也使得目标物的位置姿态非常不易控制.综上，直线推移更能满足实际需求.

4 结语

本文研究了基于Kinect的冗余机械臂直线推移操作控制.推移操作作为更接近人本能的一种行为，省去了传统抓取操作的抓取－放置操作，增加了整个操作过程的稳定性与快速性.实验利用Kinect实时检测得到的深度图，对目标物进行检测和推点计算，并应用改进RRT算法对机械臂系统进行轨迹规划，克服了传统关节空间内RRT算法无法确定末端执行器位置的问题，提高了位置控制精度，在实验中获取了良好的效果.最终完成了将处于危险位置的物体通过推移操作移动到安全位置的操作.

机械臂的推移操作扩充了机械臂和灵巧手的应用范围，使得对于一些不易通过抓取完成的操作得以解决，比如目标物过重、过大或表面光滑不易抓取.然而本实验在推移过程中采取的仍为开环控制，将闭环控制加入到直线推移中以提高控制精度，将是未来工作的重点.

References)

［1］Mason M.Mechanics and planning of manipulator pushing operations［J］.International Journal of Robotics Research，1986，5(3):53－71.

［2］Lynch K，Mason M.Stable pushing:mechanics，controllability，and planning［J］.International Journal of Robotics Research，1996，15(6):533－556.

［3］Lynch K.Locally controllable manipulation by stable pushing［J］.IEEE Transactions on Robotics and Automation，1999，15(2):318－327.

［4］Lynch K M.The mechanics of fine manipulation by pushing［C］//IEEE International Conference on Robotics and Automation.Nice，France，1992:2269－2276.

［5］Lynch K M，Mason M T.Controllability of pushing［C］//IEEE International Conference on Robotics and Automation.Nagoya，Japan，1995:112－119.

［6］Fitzpatrick P M，Metta G.Towards manipulation－driven vision［C］//IEEE/RSJ Conference on Intelligent Robots and Systems.Sendai，Japan，2002:43－48.

［7］Katz D，Brock O.A factorization approach to manipulation in unstructured environments［C］//International Symposium on Robotics Research.Lucerne，Switzerland，2009:1－16.

［8］Kenney J，Buckley T，Brock O.Interactive segmenta－tion formanipulation in unstructured environments［C］//IEEE International Conference on Robotics and Automation.Kobe，Japan，2009:1377－1382.

［9］Katz D，Brock O.Extracting planar kinematic models using interactive perception［C］//Lecture Notes in Electrical Engineering.Springer，2008，8:11－23.

［10］Omrĉen D，Böge C，Asfour T，et al.Autonomous acquisition of pushing actions to support object grasping with a humanoid robot［C］//IEEE/RAS International ConferenceonHumanoidRobots. Paris， France，2009:277－283.

［11］Cosgun A，Hermans T，Emeli V，et al.Push planning for object placement on cluttered table surfaces［C］//Proceedings of 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.San Francisco，CA，2011:4627－4632.

［12］Emeli V，Kemp C C，Stilman M.Push planning for object placement in clutter using the PR－2［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.San Francisco，USA，2011:25－30.

［13］孙增圻.机器人智能控制.山西:山西教育出版社，1995.

［14］黄文炳.冗余机械臂运动学轨迹优化［D］.北京:北京航空航天大学数学与系统科学学院，2012.

［15］LaValle S M，Kuffner J J.RRT－connect:an efficient approach to single－query path planning［C］//Proceedings of the 2000 IEEE International Conference on Robotics and Automation.San Francisco，CA，2000:995－1001.

［16］El－Iaithy R A，Huang Jidong，Yeh M.Study on the use of Microsoft Kinect for robotics applications［C］//IEEE/ION Position Location and Navigation Symposium.Myrtle Beach，South Carolina，USA，2012:1280－1288.

［17］Cheraghpour F，Moosavian S A A，Nahvi A.Multiple aspect grasp performance index for cooperative object manipulation tasks［C］//2009 IEEE/ASME International Conference on Advanced Intelligent Mechatronics.Singapore，2009:386－391.