機器人多指手的多目標優(yōu)化抓取規(guī)劃

郭 語 孫志峻

(南京航空航天大學機械結構力學及控制國家重點實驗室，南京210016)

抓取規(guī)劃是機器人多指手進行穩(wěn)定抓取任務的基礎，也是靈巧操作的重要前提.一個完整的多指抓取規(guī)劃包括接觸點位置布局、接觸模型和多指手位形規(guī)劃等，涉及到靜力平衡、接觸約束和運動約束等問題.

當物體接觸點布局完成后，首先需要確定合適的接觸力，這對多指手穩(wěn)定抓取非常重要.接觸力的解空間可分為相互正交的操作力空間和內(nèi)力空間，前者起平衡外力作用，后者自成平衡力系，且對接觸力具有重要的調(diào)整作用，因此如何合理地確定內(nèi)力是多指手能否穩(wěn)定抓取物體的重要條件.現(xiàn)有的研究大多通過規(guī)劃最小或最大內(nèi)力，實現(xiàn)對抓取力的優(yōu)化調(diào)整［1-4］.但是這些研究存在以下問題:①最小內(nèi)力容易造成抓取力接近摩擦錐約束邊界，增大了接觸滑移可能性.②最大內(nèi)力可減少相對滑動的可能性，但當摩擦系數(shù)較小時，接觸力的微小偏差可能使得內(nèi)力越大，反而穩(wěn)定性越差.因此，過大或過小的抓取內(nèi)力都是不合適的.一些學者在人手兩指精度抓取物體實驗中發(fā)現(xiàn)，手指施加的抓取內(nèi)力往往比約束物體所需最小內(nèi)力多出一個安全余量，為保證對物體的穩(wěn)定抓取，該余量約為最小內(nèi)力的50%［5］，該結論為機器人多指手抓取內(nèi)力的確定提供了另一種思路.

多指抓取規(guī)劃還應考慮多指手的自身運動學約束.研究者在確定預抓取位形時一般按運動靈巧性指標或最小關節(jié)力矩指標進行規(guī)劃［6-8］，但往往僅考慮某一方面，然而一個好的多指抓取規(guī)劃需要綜合考慮多個抓取性能指標.

基于以上分析，本文借鑒人手抓取經(jīng)驗，提出接觸安全裕度的概念并用于內(nèi)力規(guī)劃，以獲取多指手穩(wěn)定、安全地抓取物體的接觸力.通過將物體相對手掌的位姿參數(shù)作為變量，確定了接觸力與多指手關節(jié)力矩之間的映射關系.綜合考慮轉角位置度和相對承載能力作為優(yōu)化指標，最終得到衡量多指手抓取性能的多目標優(yōu)化模型.利用多目標粒子群優(yōu)化算法獲得了目標函數(shù)的非劣解集，從而實現(xiàn)了完整的多指手抓取規(guī)劃.

1 多指抓取的規(guī)劃模型

1.1 靜力平衡方程

圖1 多指抓取物體模型

設有n 個手指抓取一物體，見圖1.假設手指i的關節(jié)數(shù)為mi; θi，τi∈Rmi分別表示手指i 的關節(jié)向量和關節(jié)力矩向量.并設指尖與物體之間的接觸模型為有摩擦點接觸.p 為手掌參考坐標系，si為手指i 的基坐標系，o 為與物體固連的坐標系，原點與物體質(zhì)心重合.在手指i 與物體的接觸點處，建立與物體固連的接觸坐標系ci，原點與接觸點重合，z 軸指向接觸點處表面內(nèi)法矢方向.cfi為手指i的指尖坐標系，若忽略手指指尖輪廓，則cfi的坐標系原點與接觸點重合.fci={fcix，fciy，fciz}T∈R3為物體在接觸點i 處所受的指尖接觸力，其中fcix和fciy為接觸力的切向分量，fciz為法向分量.

作用在物體上的任何力系總可以合成為一個作用于某直線的集中力和繞該直線的力矩，這種組合稱之為力螺旋，可表示為Fe={fe，τe}T.指尖對物體的作用可以看作是手指施加于接觸點的力與物體參考點合力螺旋之間的一種映射［9］.若物手系統(tǒng)是靜止的或運動速度較慢，則接觸力形成的合力螺旋與外力螺旋滿足靜力平衡方程:

式中，G=［G1，G2，…，Gm］∈R6×3m為抓取矩陣;fc∈R3m為接觸力向量.若接觸點相對于物體坐標系的位置和姿態(tài)由goci=(poci，Roci)∈SE(3)描述，則單接觸點的抓取映射矩陣Gi由下式確定:

1.2 摩擦約束條件

為了保證物體與指尖在接觸點處不出現(xiàn)滑動，必須使接觸力處于接觸點摩擦錐內(nèi)部，即切向分量小于或等于法向分量fciz與摩擦系數(shù)μ 的乘積，同時，接觸力的法向分量具有單向性特點，因此滿足有摩擦點接觸模型的摩擦錐約束條件為

1.3 接觸力規(guī)劃

設接觸布局和物體所受外力螺旋已知，由于多指抓取映射矩陣G 一般為非滿秩矩陣，則接觸力為

式中，G+為G 的M-P 廣義逆;fcp=－G+Fe為平衡物體所受外力螺旋的操作力分量;fch=(I－G+G)λ為內(nèi)力.對于確定的外力螺旋，操作力分量可直接求出，而由于內(nèi)力具有不確定性，因此如何確定內(nèi)力對抓取性能具有關鍵作用.一般規(guī)劃方法多以能量最小作為優(yōu)化目標，而某些情況下這種優(yōu)化得到的接觸力正好在摩擦錐邊界上，一旦有外擾作用或控制誤差，規(guī)劃的抓取將失效［10］.為避免這種情況的出現(xiàn)，發(fā)現(xiàn)通過調(diào)整摩擦錐約束條件可以保證接觸力處于摩擦錐內(nèi)部.定義ρi∈［0，1］為i 接觸點處的接觸安全裕度，其意義可由下式表示:

式中，ρi表示接觸力偏離約束邊界的相對程度，當ρi=0，表明接觸點i 處的接觸力正處于摩擦錐的邊界上，此時接觸點處于滑移的臨界狀態(tài)，稍有外擾，接觸穩(wěn)定性即被破壞;當ρi=1，表明接觸點i 處沒有滑移趨勢，此時該接觸點最穩(wěn)定.通過引入接觸安全裕度概念，摩擦錐約束條件修正為

則滿足式(1)和式(6)的最小內(nèi)力規(guī)劃模型可概括為

式中，Q 為權重矩陣(一般為對角矩陣)，和各手指的指尖抓取能力有關.優(yōu)化后的內(nèi)力fch*就是具有接觸安全裕度ρ 的最優(yōu)內(nèi)力，相應的接觸力為

1.4 關節(jié)空間性能規(guī)劃

當被抓取物體的接觸點布局及抓取力分配確定后，由于接觸點的約束，各手指與物體形成若干運動閉鏈.當物體在手掌坐標系中的位姿變化時，各手指的抓取位形也相應變化，因而分配到各手指關節(jié)上的驅動力矩也將重新分配.因此，可通過物體位姿參數(shù)建立接觸力到手指關節(jié)空間的映射.

設第i 個接觸坐標系相對于手指基坐標系的位姿為gsici=(psici，Rsici)，fsici為接觸力矢量在手指基坐標系中的表示，則有

考慮到剛體變換性質(zhì)，有

即

式中

設Ji為手指i 的雅可比矩陣.聯(lián)立式(9)～(12)可得多指手抓取物體時關節(jié)空間力矩與接觸力的映射關系:

式(12)、(13)描述了多指手關節(jié)力矩與物體位姿以及手指位形之間的關系，因此通過對物體位姿的規(guī)劃可以優(yōu)化多指手關節(jié)空間中的抓取性能.

對于機器人多指手精度抓取而言，首先希望其用最小的關節(jié)力矩實現(xiàn)穩(wěn)定抓取物體，即具有最大的輸出潛力，以實現(xiàn)多指手抓取性能的最大化.其次，指尖抓取的突出優(yōu)點是可對物體進行靈巧操作，因此操作靈活性也是衡量其抓取性能的一個重要指標.本文主要考慮多指預抓取階段，因此將多指手的轉角位置度和相對承載能力作為優(yōu)化性能指標，分別建立其目標函數(shù)［7-8］，即

式中，θijmax和θijmin分別為手指i 的關節(jié)j 的最大和最小轉角;θijm=(θijmin+θijmax)/2 為手指i 的關節(jié)j的轉動中間位置;τij為手指i 的關節(jié)j 的輸出力矩，τijmax為其最大輸出力矩.

2 多目標粒子群優(yōu)化算法

2.1 多目標優(yōu)化模型

式(14)表示的多指抓取規(guī)劃模型由于性能指標的多樣性，其本質(zhì)上是一個多目標優(yōu)化問題.各目標之間通常相互制約，所以多目標優(yōu)化命題一般不存在唯一的全局最優(yōu)解.實際上多目標優(yōu)化命題往往是如何尋求非劣解集及其在非劣解前沿的分布過程，最后決策者在其中選取一個最滿意解.則多指抓取的多目標優(yōu)化問題模型可歸納為

式中，x = { xpo，ypo，zpo，α，β，γ}T為決策矢量;Lh(·)為多指手的正運動學; xmin，xmax表示物體位姿參數(shù)的變化范圍.

2.2 多目標粒子群優(yōu)化算法

目前有多種不同算法用于求解多目標優(yōu)化問題，基于種群搜索的粒子群優(yōu)化算法由于其易實現(xiàn)、計算高效、不要求目標函數(shù)可微，以及基于種群的特點，使其非常適用于多目標優(yōu)化領域.許多研究者成功地將基于粒子群的多目標優(yōu)化算法應用于各種領域［11-13］.本文采用基于擁擠因子和擾動算子的多目標粒子群優(yōu)化算法［14］來求解多指抓取規(guī)劃問題，該算法被證明相對其他多目標優(yōu)化算法，能夠更好地逼近非劣解前沿.算法流程如下:

①初始化粒子群.確定種群規(guī)模N，隨機產(chǎn)生每個粒子的位置xi，初始化粒子速度vi=0，i =1，2，…，N.

②計算初始種群適應度，篩選粒子群中的非劣解放入領導集，將領導集放入ε-支配文檔.

③計算領導集中的每個粒子的擁擠因子.

④初始化迭代代數(shù)g =0，確定最大迭代次數(shù)gmax.

⑤進入迭代循環(huán).

a)從領導集中隨機選取一個粒子作為當前全局最優(yōu)gbest，更新每個粒子的速度，即

式中，w 為慣性權重，取(0.1，0.5)之間的隨機數(shù);c1，c2為學習因子，取(1.5，2.0)之間的隨機數(shù);r1，r2為(0.0，1.0)之間的隨機數(shù).

b)更新每個粒子的位置

c)更新pibest.

d)計算領導集中粒子的擁擠因子，并按降序排列，更新領導集，更新ε-支配文檔，g=g+1.

e)若g≤gmax，返回⑤，否則輸出ε-支配文檔.

⑥結束程序.

3 算例分析

以圖2所示的三指手抓取圓球模型為例，每個手指結構相同，各有3 個自由度，其運動學參數(shù)如表1所示，手指在手掌中的布局如圖3所示，手掌坐標系原點位于三指基座圍成的三角形中心處，物體坐標系原點位于球心，圓球半徑r =30 mm，指尖與物體之間的摩擦系數(shù)μ=0.3.為方便分析，假設物體所受外力螺旋為純力矢量，且大小不變，為1 N，方向如圖2所示，則外力螺旋可表示為

圖2 三指手抓取圓球

圖3 手指在手掌中的布局

表1 手指D-H 參數(shù)

3.1 接觸力規(guī)劃結果與分析

三指指尖接觸點在圓球大圓上均勻分布，則抓取矩陣G 為

取接觸安全裕度ρ1=ρ2=ρ3=0.3，式(7)中的權重矩陣為單位陣.以外力螺旋的方向角δ1，δ2為變量，在Matlab 中編制由式(7)和(8)表示的優(yōu)化模型，輸出為規(guī)劃后的抓取接觸力隨外力螺旋方向變化的分布，如圖4所示.由于抓取的對稱性，僅給出δ2從－90°到90°的變化結果.可看出，若δ2固定，當δ1=0 時，3 個接觸力的模都最小，因此在進行具體抓取時，應盡量選擇使外力螺旋方向在3 個接觸點決定的接觸面上.

圖4 ρi =0.3 時各指尖接觸力大小分布

圖5給出了各指尖的接觸安全裕度隨外力螺旋方向變化的情況.可看出，3 個接觸點的接觸安全裕度都大于等于0.3，接觸力處于摩擦錐內(nèi)部.進一步計算可知接觸力與摩擦錐邊界的夾角至少為4.84°，說明內(nèi)力的規(guī)劃可以允許一定擾動的發(fā)生.若不設置接觸安全裕度，則在一些情況下規(guī)劃后的接觸力將處于摩擦錐邊界，一旦有外部擾動或控制誤差，抓取將變得不穩(wěn)定甚至失效.

3.2 關節(jié)空間內(nèi)的多目標優(yōu)化

設物體所受外力螺旋Fe為純力矢量，方向為δ1=30°，δ2=60°，大小為5 N.利用內(nèi)力規(guī)劃模型獲得的各接觸力如表2所示.

圖5 ρi =0.3 時各指尖接觸安全裕度分布

表2 接觸力規(guī)劃結果 N

假定手指各關節(jié)驅動能力相同，且τmax=0.8 N·m.優(yōu)化變量為物體位姿參數(shù)，優(yōu)化目標是使多指手的相對承載能力和轉角位置度最大.多目標粒子群優(yōu)化算法的參數(shù)設置為:粒子群規(guī)模N=100，最大迭代次數(shù)gmax=200，ε=0.05.圖6為最終得到的非劣解前沿曲線.可看出利用該多目標粒子群優(yōu)化算法可有效地求得非劣解前沿，且分布較均勻.

圖6 非劣解前沿

此外，相對承載能力與轉角位置度互相制約，因此在實際應用時，只能根據(jù)抓取任務的要求選取不同的性能指標.通過將非劣解集保存為專家知識庫，可方便地獲取不同性能指標組合下的最優(yōu)抓取規(guī)劃.表3為若干優(yōu)化結果，可看出，相比初始位姿，優(yōu)化后的相對承載能力和轉角位置度都有明顯改善.

表3 位姿優(yōu)化結果對比

4 結語

本文借鑒人手抓取經(jīng)驗，提出了接觸安全裕度的概念，并建立了非線性優(yōu)化模型以求取最優(yōu)安全內(nèi)力以及接觸力.以物體位姿參數(shù)作為變量，確定了接觸力與多指手關節(jié)力矩的關系，并將多指手的抓取規(guī)劃問題歸結為由多指手關節(jié)空間內(nèi)的相對承載能力和轉角位置度為優(yōu)化目標的多目標優(yōu)化問題.以三指手抓取系統(tǒng)為例進行了數(shù)值求解，探討了外力螺旋的方向變化對接觸力和接觸安全裕度的影響.首次嘗試利用多目標粒子群優(yōu)化算法，對多指抓取進行規(guī)劃，所求得的非劣解前沿可作為預抓取位形規(guī)劃依據(jù).

本文所提出的規(guī)劃方法不受被抓物體輪廓和多指手結構的限制，對于其他多指抓取系統(tǒng)的規(guī)劃具有一定的參考意義.

References)

［1］Kerr J，Roth B.Analysis of multifingered hands［J］.The International Journal of Robotics Research，1986，4(3):3-17.

［2］李家煒，胡海鷹，王濱，等.多指抓取內(nèi)力的簡單算法［J］.自然科學進展，2005，15(6):733-738.

Li Jiawei，Hu Haiying，Wang Bin，et al.A simple algorithm for internal force of multi-fingered robot hands grasping ［J］.Progress in Natural Science，2005，15(6):733-738.(in Chinese)

［3］劉慶運，岑豫皖，謝能剛，等.基于抓取穩(wěn)定性的手指接觸力規(guī)劃算法［J］.機械工程學報，2010，46(7): 57-62.

Liu Qingyun，Cen Yuwan，Xie Nenggang，et al.Optimization algorithm for contact forces based on grasping stability ［J］.Journal of Mechanical Engineering，2010，46(7): 57-62.(in Chinese)

［4］王濱，李家煒，劉宏.機器人多指手的優(yōu)化抓取力計算［J］.吉林大學學報:工學版，2008，38(1):178-182.

Wang Bin，Li Jiawei，Liu Hong.Optimal grasping force computation for multi-fingered robot hand ［J］.Journal of Jilin University: Engineering and Technology Edition，2008，38(1):178-182.(in Chinese)

［5］Johansson R S，Riso R，Hager C，et al.Somatosensory control of precision grip during unpredictable pulling loads-Ⅰ.changes in load force amplitude［J］.Experimental Brain Research，1992，89(1): 181-191.

［6］Bicchi A.On the closure properties of robotic grasping［J］.The International Journal of Robotics Research，1995，14(4): 319-334.

［7］李國順，張永德.基于抓取接近方向的機器人多指手的優(yōu)化抓取［J］.電機與控制學報，2003，7(1):62-66.

Li Guoshun，Zhang Yongde.Optimal grasp of multifingered robot hands based on approach direction［J］.Electric Machines and Control，2003，7(1):62-66.(in Chinese)

［8］李繼婷.機器人靈巧手的抓持規(guī)劃［D］.北京: 北京航空航天大學，2004.

［9］理查德·摩雷，李澤湘，夏恩卡·薩恩特里.機器人操作的數(shù)學導論［M］.徐衛(wèi)良，等譯.北京: 機械工業(yè)出版社，1998.

［10］王國慶，張啟先，李大寨，等.基于抓持穩(wěn)定度的多指靈巧手抓持控制［J］.航空學報，1997，18(3):294-298.

Wang Guoqing，Zhang Qixian，Li Dazhai，et al.Grasping control of the dexterous hand based on the degree of stability of grasping［J］.Acta Aeronautica et Astronautica Sinica，1997，18(3): 294-298.(in Chinese)

［11］Masehian E，Sedighizadeh D.Multi-objective robot motion planning using a particle swarm optimization model［J］.Journal of Zhejiang University—Science C: Computers ＆Electronics，2010，11(8):607-619.

［12］Marinaki M，Marinakis Y，Stavroulakis G E.Fuzzy control optimized by a multi-objective particle swarm optimization algorithm for vibration suppression of smart structures［J］.Structural and Multidisciplinary Optimization，2011，43(1): 29-42.

［13］孫光永，李光耀，陳濤，等.多目標粒子群優(yōu)化算法在薄板沖壓成形中的應用［J］.機械工程學報，2009，45(5): 153-159.

Sun Guangyong，Li Guangyao，Chen Tao，et al.Application of multi-objective particle swarm optimization in sheet metal forming［J］.Journal of Mechanical Engineering，2009，45(5):153-159.(in Chinese)

［14］Reyes-Sierra M，Coello Coello C A.Improving PSObased multi-objective optimization using crowding，mutation and ε-dominance ［C］//The Third International Conference on Evolutionary Multi-Criterion Optimization.Guanajuato，Mexico，2005: 505-519.