郭 迪 孫富春 劉華平 黃文炳

(清華大學(xué)智能技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100084)

在機(jī)械臂系統(tǒng)中，機(jī)械臂執(zhí)行的很重要的一類(lèi)操作是對(duì)目標(biāo)物體位置、姿態(tài)的改變.通常所采用的方式是在機(jī)械臂末端安裝末端執(zhí)行器(靈巧手)，先完成對(duì)目標(biāo)物的抓取，再把目標(biāo)物移動(dòng)到指定位置.然而，當(dāng)目標(biāo)物尺寸過(guò)大、過(guò)重或者表面過(guò)于光滑，超過(guò)機(jī)械臂系統(tǒng)的載荷和工作能力時(shí)，抓取操作就不能進(jìn)行.可見(jiàn)，將機(jī)械臂系統(tǒng)的功能僅僅局限在抓取操作明顯限制了機(jī)械臂系統(tǒng)的應(yīng)用范圍.

推移操作作為一種實(shí)用的操作，近年來(lái)得到廣泛的研究.Mason［1］最先研究了推移操作的力學(xué)機(jī)理，并指出推移操作是機(jī)器人系統(tǒng)中一種很重要的操作過(guò)程;隨后，Mason 等［2－5］建立了機(jī)械臂穩(wěn)定推移操作的力學(xué)、控制和規(guī)劃理論框架，這為推移操作的發(fā)展奠定了深厚的研究基礎(chǔ).此后，機(jī)械臂的推移操作廣泛應(yīng)用于平面操作臺(tái)上，用來(lái)完成各種各樣的任務(wù).例如:桌面的推移操作被用于目標(biāo)物的分類(lèi)［6－9］;對(duì)目標(biāo)物抓取前位姿的預(yù)處理，完成人形機(jī)器人推移－抓取的操作［10］;對(duì)雜亂桌面的清理等［11－12］.

本文研究并采用機(jī)械臂的推移操作來(lái)解決不易抓取的目標(biāo)物的位置、姿態(tài)改變問(wèn)題.運(yùn)用Kinect捕捉實(shí)時(shí)RGB圖像和深度圖像，確定處于桌面危險(xiǎn)位置的目標(biāo)物，根據(jù)位置信息采用改進(jìn)RRT算法計(jì)算出機(jī)械臂到達(dá)目標(biāo)點(diǎn)的軌跡，再利用七自由度機(jī)械臂系統(tǒng)完成對(duì)目標(biāo)物的推移操作，最終使目標(biāo)物到達(dá)安全位置.討論并應(yīng)用實(shí)驗(yàn)驗(yàn)證了基于Kinect的冗余機(jī)械臂直線(xiàn)推移操作控制.機(jī)械臂的推移操作擴(kuò)展了機(jī)械臂的應(yīng)用范圍，并且為解決平面上目標(biāo)物體的移動(dòng)提供了一種更為簡(jiǎn)單實(shí)用的方法.

1 冗余機(jī)械臂系統(tǒng)的運(yùn)動(dòng)學(xué)建模

當(dāng)機(jī)械臂系統(tǒng)的自由度數(shù)大于任務(wù)空間的維數(shù)時(shí)，即構(gòu)成有冗余度的機(jī)械臂系統(tǒng).冗余度可以用來(lái)改善機(jī)器人的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性，如增加靈活性、避免關(guān)節(jié)角極限、回避奇異、優(yōu)化關(guān)節(jié)加速度、力矩和能量等.本文采用的機(jī)械臂系統(tǒng)可以看作一個(gè)剛體系.

實(shí)驗(yàn)所采用的機(jī)械臂系統(tǒng)的每一個(gè)關(guān)節(jié)都對(duì)應(yīng)相應(yīng)的剛體坐標(biāo)系，見(jiàn)圖1.已知機(jī)械臂的DH參數(shù)，可得到末端連桿坐標(biāo)系A(chǔ)n相對(duì)于基座坐標(biāo)系 A0的變換矩陣［13］:

圖1 機(jī)械臂的DH表示法

用向量X表示機(jī)械臂末端執(zhí)行器的位姿，設(shè)其維數(shù)為m=6，可得X與關(guān)節(jié)變量q的關(guān)系如下:

式中，由q組成的空間稱(chēng)為關(guān)節(jié)空間;由X組成的空間稱(chēng)為工作空間.式(2)描述了機(jī)械臂末端執(zhí)行器的位姿與關(guān)節(jié)變量之間的聯(lián)系，被稱(chēng)為機(jī)械臂的運(yùn)動(dòng)學(xué)方程.

在本實(shí)驗(yàn)和大多數(shù)情況下可以得知目標(biāo)物的位置，因此通過(guò)逆運(yùn)動(dòng)學(xué)求解的方法可得到相應(yīng)的關(guān)節(jié)位移，進(jìn)而完成對(duì)機(jī)械臂的運(yùn)動(dòng)控制.本實(shí)驗(yàn)中采取了最大似然逆軌跡優(yōu)化的算法［14］.

2 基于工作空間的RRT算法

高維數(shù)關(guān)節(jié)空間內(nèi)的RRT(rapidly－exploring random trees)算法是由LaValle等［15］首先提出的一種解決軌跡規(guī)劃問(wèn)題的隨機(jī)規(guī)劃算法.RRT適合用于解決含有運(yùn)動(dòng)、動(dòng)力學(xué)約束的路徑規(guī)劃問(wèn)題，被廣泛應(yīng)用于機(jī)器人路徑規(guī)劃和軌跡規(guī)劃.

由于實(shí)驗(yàn)中對(duì)機(jī)械臂末端位姿的要求，需要部分限定末端執(zhí)行器的運(yùn)動(dòng)軌跡.傳統(tǒng)基于關(guān)節(jié)空間的軌跡規(guī)劃算法會(huì)造成末端執(zhí)行器運(yùn)動(dòng)路徑不可預(yù)知的問(wèn)題.為此，采用在工作空間中構(gòu)建RRT樹(shù)的方法，這樣可以消除不完整約束，直接滿(mǎn)足收斂條件［14］.

設(shè)x1是已經(jīng)給定軌跡的末端執(zhí)行器的位姿，x2是需要規(guī)劃的末端位姿.在X2空間內(nèi)進(jìn)行RRT樹(shù)的構(gòu)建.首先將X2的初始值x2，init賦予RRT樹(shù).再由Extend函數(shù)返回最近鄰近點(diǎn)x2，near以及其對(duì)應(yīng)的關(guān)節(jié)變量qnear和時(shí)間tnear，同時(shí)返回新擴(kuò)展的點(diǎn)xnew以及其時(shí)間tnew.得到最近鄰近時(shí)間節(jié)點(diǎn)上和新的時(shí)間節(jié)點(diǎn)上的末端位姿x0，xT.然后應(yīng)用最似梯度法對(duì)機(jī)械臂進(jìn)行逆運(yùn)動(dòng)學(xué)軌跡優(yōu)化，可得到機(jī)械臂系統(tǒng)的關(guān)節(jié)角信息qnew.最后，對(duì)qnew進(jìn)行障礙檢測(cè)并確定qnew符合邊界條件.重復(fù)迭代直到生成樹(shù)中的點(diǎn)距離目標(biāo)點(diǎn)充分?。?4］.

3 實(shí)驗(yàn)研究及結(jié)果

3.1 實(shí)驗(yàn)系統(tǒng)簡(jiǎn)介

本實(shí)驗(yàn)系統(tǒng)由Kinect、七自由度機(jī)械臂和靈巧手組成，如圖2～圖4所示.體感設(shè)備Kinect能夠以30幀/s的速率幾乎同步地提供分辨率為640×480像素的RGB影像和分辨率為320×240像素的深度影像［16］.系統(tǒng)中所采用的七自由度機(jī)械臂是由德國(guó)SCHUNK公司開(kāi)發(fā)并生產(chǎn)的PowerCube模塊組合而成.機(jī)械臂的作業(yè)范圍可達(dá)1870 mm×1650 mm×1870 mm，有效負(fù)載10 kg.末端抓取機(jī)構(gòu)采用的是美國(guó)Barrett Technology公司生產(chǎn)的BH8.280靈巧手.該靈巧手是一個(gè)可編程的多手指抓持器，可以抓取不同形狀、大小及姿態(tài)的目標(biāo)物體.

圖2 Kinect深度攝像機(jī)

圖3 七自由度模塊化機(jī)械臂

圖4 Barrett靈巧手

3.2 實(shí)驗(yàn)及結(jié)果

3.2.1 任務(wù)描述

Kinect對(duì)其視野范圍內(nèi)的目標(biāo)物進(jìn)行檢測(cè)，當(dāng)有目標(biāo)物出現(xiàn)在指定區(qū)域時(shí)，即表示需要對(duì)該目標(biāo)物進(jìn)行移動(dòng)操作，使其到達(dá)指定位置.接著，對(duì)目標(biāo)物進(jìn)行形狀尺寸檢測(cè)，判斷是否可以對(duì)其進(jìn)行抓取操作.假設(shè)當(dāng)目標(biāo)物的尺寸超過(guò)機(jī)械手抓取的量程時(shí)則進(jìn)行推移操作，本實(shí)驗(yàn)中采用了一個(gè)形狀超過(guò)靈巧手抓取范圍的盒子作為目標(biāo)物(見(jiàn)圖5).機(jī)械臂的推移操作過(guò)程如下:首先經(jīng)過(guò)改進(jìn)RRT算法解算，使得機(jī)械臂迅速到達(dá)最佳推點(diǎn)位置，確認(rèn)各條件符合推移要求時(shí)執(zhí)行直線(xiàn)推移操作，最終使得目標(biāo)物到達(dá)指定位置.

圖5 目標(biāo)物

3.2.2 目標(biāo)檢測(cè)

本實(shí)驗(yàn)的操作任務(wù)是通過(guò)Kinect檢測(cè)提取出目標(biāo)，并使機(jī)械臂對(duì)處于操作平臺(tái)邊緣的目標(biāo)物進(jìn)行位置的移動(dòng).由于Kinect可以直接提供深度圖像，因此采用基于深度圖像的背景相減法可獲得較好的快速性和實(shí)時(shí)性.

首先，提取目標(biāo)沒(méi)有出現(xiàn)時(shí)的深度圖像作為背景圖像，進(jìn)行存儲(chǔ);之后，提取當(dāng)前幀場(chǎng)景的深度圖像作為場(chǎng)景圖像.將場(chǎng)景圖像與背景圖像做差，若某點(diǎn)的深度值之差大于某閾值，則認(rèn)為該點(diǎn)屬于目標(biāo)物.這樣就可得到目標(biāo)物體的全部像素點(diǎn)，進(jìn)而得到目標(biāo)物的形狀位置等信息.

3.2.3 推點(diǎn)選擇

首先假定推移操作滿(mǎn)足準(zhǔn)靜態(tài)假設(shè)條件，所以在選取推點(diǎn)時(shí)僅考慮靜態(tài)項(xiàng).通過(guò)考慮MAG指標(biāo)［17］中的靜態(tài)項(xiàng)來(lái)決定穩(wěn)定推點(diǎn)的位置.將目標(biāo)的輪廓平均分割為20個(gè)點(diǎn)，然后分別計(jì)算每點(diǎn)的靜態(tài)項(xiàng)值，靜態(tài)項(xiàng)值越高的點(diǎn)越穩(wěn)定，由圖6可見(jiàn)，4號(hào)點(diǎn)和14號(hào)點(diǎn)擁有更好的穩(wěn)態(tài)性.所以選取這2點(diǎn)中靠近外側(cè)的點(diǎn)作為推移點(diǎn)以便機(jī)械手更容易到達(dá).

圖6 各點(diǎn)靜態(tài)指標(biāo)

3.2.4 推移操作

機(jī)械臂對(duì)目標(biāo)物執(zhí)行了推移操作，但是機(jī)械手與目標(biāo)物仍為2個(gè)獨(dú)立的個(gè)體，這樣推移規(guī)劃與軌跡規(guī)劃就會(huì)產(chǎn)生耦合.事實(shí)上，推移操作是靠機(jī)械臂的軌跡規(guī)劃實(shí)現(xiàn)的.通過(guò)將機(jī)械手以合適的姿態(tài)與目標(biāo)物相接觸但不固連，利用軌跡規(guī)劃使得機(jī)械臂末端到達(dá)目標(biāo)位置，從而帶動(dòng)目標(biāo)物達(dá)到目標(biāo)位置完成推移操作.因此，在實(shí)驗(yàn)中，當(dāng)機(jī)械臂到達(dá)推點(diǎn)位置時(shí)，使得機(jī)械手/臂的姿態(tài)滿(mǎn)足推移操作的要求，之后在進(jìn)行推移操作的軌跡規(guī)劃時(shí)，使得機(jī)械臂的末端姿態(tài)不變.這樣，就可以保證機(jī)械臂的每次推動(dòng)都是有效推動(dòng).本實(shí)驗(yàn)中為了使靈巧手與目標(biāo)物體擁有更多的接觸點(diǎn)，使操作更平穩(wěn)，采用靈巧手手指操作全程全張開(kāi)的姿態(tài)(見(jiàn)圖4)，這樣既可使推移過(guò)程為線(xiàn)接觸［2］，又可使靈巧手處理各種不同的目標(biāo)物以適用于新的環(huán)境［12］.由于目標(biāo)物總是與機(jī)械臂的末端相接觸，因此目標(biāo)物就可按照機(jī)械臂末端運(yùn)動(dòng)的軌跡進(jìn)行移動(dòng).

3.2.5 演示實(shí)驗(yàn)

當(dāng)Kinect檢測(cè)到在操作臺(tái)邊緣有物體出現(xiàn)并處于危險(xiǎn)位置時(shí)，根據(jù)Kinect實(shí)時(shí)傳送過(guò)來(lái)的深度圖像對(duì)目標(biāo)物進(jìn)行輪廓提取，計(jì)算出推點(diǎn).系統(tǒng)首先會(huì)采用基于工作空間的RRT軌跡規(guī)劃的方法得到機(jī)械臂的運(yùn)動(dòng)軌跡(見(jiàn)圖7)，之后機(jī)械臂按照規(guī)劃路徑到達(dá)目標(biāo)位置，執(zhí)行推移操作，使得物體到達(dá)安全區(qū)域，最后系統(tǒng)復(fù)位，當(dāng)危險(xiǎn)區(qū)域中再次出現(xiàn)目標(biāo)物時(shí)，重復(fù)上述操作.

圖7 改進(jìn)RRT的運(yùn)算結(jié)果

實(shí)驗(yàn)中通過(guò)2種推移方法完成對(duì)目標(biāo)物的推移:

1)直線(xiàn)推移.根據(jù)目標(biāo)物的初始位姿和目標(biāo)位姿，按照直線(xiàn)推移的方式使得靈巧手到達(dá)指定位置.這樣機(jī)械臂系統(tǒng)就會(huì)按照事先給定的軌跡移動(dòng)，其優(yōu)點(diǎn)是可以實(shí)時(shí)改變目標(biāo)物的位置姿態(tài)以達(dá)到目標(biāo)位置姿態(tài)(見(jiàn)圖8(a)).

2)導(dǎo)軌移動(dòng).機(jī)械臂系統(tǒng)是安裝在平行導(dǎo)軌上的，可通過(guò)移動(dòng)導(dǎo)軌來(lái)達(dá)到推移的目的(見(jiàn)圖8(b)).

圖8 實(shí)驗(yàn)結(jié)果

通過(guò)對(duì)比看出，2種方法都可通過(guò)推移操作使得目標(biāo)物從桌面危險(xiǎn)區(qū)域到達(dá)安全位置.但由于直線(xiàn)推移操作中，機(jī)械臂系統(tǒng)的操作是在桌面二自由度平面中進(jìn)行的，因此可以在推移過(guò)程中對(duì)目標(biāo)物的位置姿態(tài)進(jìn)行調(diào)整，實(shí)驗(yàn)中就實(shí)現(xiàn)了將目標(biāo)物的姿態(tài)調(diào)整的操作，操作空間為平面二自由度中的任意位置，可使得移動(dòng)的路徑最短、效率最高;而導(dǎo)軌移動(dòng)過(guò)程中，由于機(jī)械臂系統(tǒng)只能沿導(dǎo)軌移動(dòng)，因此將推移的操作空間限定在了一個(gè)自由度的軌道上，使得推移只能朝一個(gè)方向進(jìn)行，大大限制了推移操作的工作空間，也使得目標(biāo)物的位置姿態(tài)非常不易控制.綜上，直線(xiàn)推移更能滿(mǎn)足實(shí)際需求.

4 結(jié)語(yǔ)

本文研究了基于Kinect的冗余機(jī)械臂直線(xiàn)推移操作控制.推移操作作為更接近人本能的一種行為，省去了傳統(tǒng)抓取操作的抓?。胖貌僮?，增加了整個(gè)操作過(guò)程的穩(wěn)定性與快速性.實(shí)驗(yàn)利用Kinect實(shí)時(shí)檢測(cè)得到的深度圖，對(duì)目標(biāo)物進(jìn)行檢測(cè)和推點(diǎn)計(jì)算，并應(yīng)用改進(jìn)RRT算法對(duì)機(jī)械臂系統(tǒng)進(jìn)行軌跡規(guī)劃，克服了傳統(tǒng)關(guān)節(jié)空間內(nèi)RRT算法無(wú)法確定末端執(zhí)行器位置的問(wèn)題，提高了位置控制精度，在實(shí)驗(yàn)中獲取了良好的效果.最終完成了將處于危險(xiǎn)位置的物體通過(guò)推移操作移動(dòng)到安全位置的操作.

機(jī)械臂的推移操作擴(kuò)充了機(jī)械臂和靈巧手的應(yīng)用范圍，使得對(duì)于一些不易通過(guò)抓取完成的操作得以解決，比如目標(biāo)物過(guò)重、過(guò)大或表面光滑不易抓取.然而本實(shí)驗(yàn)在推移過(guò)程中采取的仍為開(kāi)環(huán)控制，將閉環(huán)控制加入到直線(xiàn)推移中以提高控制精度，將是未來(lái)工作的重點(diǎn).

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，F(xiàn)rance，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］孫增圻.機(jī)器人智能控制.山西:山西教育出版社，1995.

［14］黃文炳.冗余機(jī)械臂運(yùn)動(dòng)學(xué)軌跡優(yōu)化［D］.北京:北京航空航天大學(xué)數(shù)學(xué)與系統(tǒng)科學(xué)學(xué)院，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.