基于擴展任務(wù)的雙臂機器人位姿優(yōu)化*

郭 鵬，房靈申,趙明揚,朱思俊

(1.中國科學(xué)院沈陽自動化研究所 機器人學(xué)國家重點實驗室，沈陽 110016；2.中國科學(xué)院沈陽自動化研究所 揚州工程技術(shù)研究中心，江蘇 揚州 225127；3.中國科學(xué)院大學(xué)，北京 100049)

0 引言

隨著工業(yè)生產(chǎn)系統(tǒng)對機器人工作站的靈活性和自動化程度的要求不斷提高，雙臂系統(tǒng)作為一種更先進的自動化系統(tǒng)，漸漸成為了學(xué)術(shù)和工業(yè)界的熱點研究問題[1]。

在機器人學(xué)領(lǐng)域，利用冗余機械臂的特性結(jié)合特定的機器人性能指標[15]來提高其在執(zhí)行任務(wù)過程中的靈活性需要用戶自己設(shè)計或修改機器人的逆解算法[2-3],但是一般來說，大多數(shù)工業(yè)機器人廠商并不提供給用戶修改權(quán)限，所以在不修改機器人逆解算法的前提下進行位姿優(yōu)化成為一個必要的研究課題，Maria等在文獻[4]首先提出一種通用的冗余機械臂的位姿優(yōu)化方法并給出了詳細的證明，該方法能夠?qū)⒂脩糇约哼x擇的機器人性能指標作為優(yōu)化目標，實現(xiàn)冗余機械臂的位姿優(yōu)化，在任務(wù)冗余的噴涂[5]和鉆孔操作[6]這兩種案例下已得到驗證，它們選用的優(yōu)化指標分別為單臂動力學(xué)可操作性度和單臂運動學(xué)可操作度。由于雙機器人系統(tǒng)在執(zhí)行一些協(xié)調(diào)操作任務(wù)時的也可認為是冗余機器人，故也可以利用該通用方法對其進行位姿優(yōu)化，文獻[7]即是采用雙臂機器人運動學(xué)可操作度作為優(yōu)化指標一使用雙臂系統(tǒng)的相對雅克比矩陣來構(gòu)造。

本文的主要研究內(nèi)容是將文獻[7]的優(yōu)化目標進行更改，利用基于各向同性指標[8]對雙工業(yè)機械臂系統(tǒng)進行位姿優(yōu)化。為了最大程度地接近實際情況，采用成熟的機器人逆解算法，故利用ABB RobotStudio軟件進行仿真。

1 主從雙臂機器人的運動學(xué)及優(yōu)化問題描述

在 ABB公司開發(fā)的RobotStudio機器人仿真環(huán)境中，使用兩臺ABB IRB140型號的工業(yè)機械臂，它們的DH參數(shù)如表1所示。

表1 ABB機器人DH參數(shù)

1.1 主從雙臂系統(tǒng)的運動學(xué)

雙臂機器人出現(xiàn)冗余特性的一個情形為：用戶在描述一個雙機械臂協(xié)調(diào)操作任務(wù)時，只對兩機械臂末端坐標系的相對位置和姿態(tài)作出規(guī)定，而對單個機械臂的末端的位姿信息不作約束限定。

為了建立雙臂機器人系統(tǒng)的運動學(xué)模型，將單臂關(guān)節(jié)空間擴展為包含兩只機械臂的關(guān)節(jié)空間表達式：

(1)

圖1表示了主從機械臂系統(tǒng)的運動鏈，令W∈SE(3)和T∈SE(3)分別表示主機械臂和從機械臂在某一瞬時的位姿，協(xié)調(diào)操作任務(wù)可以認為是從機械臂相對于主機械臂的位姿變化，所以定義R=W-1T(R∈SE(3))來描述相對運動。當(dāng)主機械臂的運動和從機械臂相對于主機械臂的運動被確定之后，雙臂系統(tǒng)的運動即被完整地描述出來了，如式(2)和式(3)所示：

(2)

(3)

其中，x表示從機械臂相對于主機械臂的相對位姿描述，y表示主機械臂的位姿描述，其中下標r表示相對(relative)，l表示主動(leader)，ρ,θ,φ分別表示三個歐拉角(ZYZ)，f(q)和h(q)表示相對運動和主機械臂的正向運動學(xué)表達式。

圖1 雙臂系統(tǒng)的運動鏈示意圖

1.2 通用位姿優(yōu)化方法的問題描述

如上所述，如果雙臂系統(tǒng)在完成協(xié)調(diào)操作任務(wù)時只需考慮兩機械臂的相對運動x，而對主機械臂和從機械臂的位置不作約束，此時，由于主機械臂的位置和姿態(tài)對協(xié)調(diào)操作任務(wù)沒有影響，所以可以任意設(shè)置，這就出現(xiàn)了冗余的情形。文獻[2]提出的通用優(yōu)化方法本質(zhì)上將主機械臂的位姿(y)作為擴展任務(wù)，通過約束它來實現(xiàn)對兩只手臂的位姿優(yōu)化。

現(xiàn)設(shè)μ(q)為可微的目標優(yōu)化函數(shù)，N為任務(wù)雅克比矩陣的零空間的一個基，所以優(yōu)化問題的數(shù)學(xué)描述為下式：

minμ(q) subject tox-f(q)=0

(4)

通過簡單證明[10]可得(4)式的解為：

(5)

由已知的冗余機械臂逆運動學(xué)算法技術(shù)可知，如果用戶想要優(yōu)化目標函數(shù)μ(q)，就需要修改或重新設(shè)計機器人逆運動學(xué)算法。目前大部分的工業(yè)機器人廠商并不會開放給用戶足夠的權(quán)限來修改其控制器內(nèi)部的算法。所以本文利用這種在不修改和重新設(shè)計逆運動學(xué)算法[2]的前提下來對雙臂機器人系統(tǒng)的位姿進行優(yōu)化。

文獻[2]中提出并證明的基于擴展任務(wù)的位姿優(yōu)化方法可簡述為：設(shè)n為機器人系統(tǒng)的自由度的數(shù)目，k為任務(wù)空間的維度，在一個可使任務(wù)雅克比矩陣Js非奇異的區(qū)域，且存在一個可微的函數(shù)h(·):Rn→Rn-k，令增廣矩陣JA[3,9]：

(6)

是非奇異的。設(shè)σ(q)=0為一個在機械臂運動過程中需要強化的可積分的約束。那么總存在一個可微的函數(shù)p(·):Rk→Rn-k，使逆解算法(7)可以求得對約束σ(q)=0強化的解。

(7)

需要指出的是，上述方法表面上使用的仍然是增廣雅克比矩陣，但是經(jīng)過證明也可以使用任何逆運動學(xué)算法，例如經(jīng)典的轉(zhuǎn)置雅克比矩陣甚至是封閉的解析解[2-4]。

圖2表示通用姿態(tài)優(yōu)化方法的總體框架圖，其中指令x為用戶輸入到機器人逆運動學(xué)算法的任務(wù)指令，而擴展任務(wù)指令y依賴變量x，由y=-p(x)求得。

圖2 冗余系統(tǒng)的求解框圖

優(yōu)化目標函數(shù)μ(q)的選取決定了雙臂系統(tǒng)在執(zhí)行任務(wù)過程中體現(xiàn)的運動性能。機器人運動靈活性指標可以量化地表示機器人靈活性和位形的優(yōu)劣，不同的靈巧性指標針對不同的機器人的性能。經(jīng)典的靈巧性能指標有可操作度和條件數(shù)兩種，條件數(shù)反映了機械臂末端的速度轉(zhuǎn)化能力的“均勻性”，條件數(shù)越小，反映了機械臂末端向操作空間各個方向的速度轉(zhuǎn)化能力分布越均勻[15]。

研究表明，將兩個機械臂的指標相乘得到的新指標可兼顧到兩只機械臂的運動性能[10]。對于工業(yè)機械臂的構(gòu)型來說，前三個關(guān)節(jié)對整體的機械臂系統(tǒng)的運動靈巧性起到?jīng)Q定性的作用，故在確定優(yōu)化目標函數(shù)的時候，只考慮兩只機械臂的前三個關(guān)節(jié)，如式(8)所示，下稱“雙臂機器人各向同性指標(DAIM)”。

(8)

將式(8)代入式(5)中可將目標優(yōu)化函數(shù)轉(zhuǎn)化為可積約束，再使用函數(shù)擬合的方法求出y=-p(x)，最后按照如圖3所示的運動學(xué)結(jié)構(gòu)對機器人進行編程，就可以達到使用現(xiàn)有的工業(yè)機器人的控制器(ABB自帶的逆解算法)來優(yōu)化用戶所要求的性能指標(雙臂機器人各向同性指標)的目的。這種編程方式與普通的工業(yè)機器人編程不同之處是，在確定主機械臂位姿指令的時候調(diào)用了函數(shù)-p(x)。

圖3 雙工業(yè)機械臂系統(tǒng)的控制結(jié)構(gòu)框圖

2 基于蒙特卡洛方法采樣關(guān)節(jié)值

由于無法以解析方法求得方程-p(x)的表達式，故利用方程擬合的方式來求解，具體步驟為：

(1)利用蒙特卡洛方法采樣出流形σ(q)=0附近的關(guān)節(jié)值序列q。本文使用與文獻[2-5]相同的采樣算法，見下述的Hit-and-Run算法及圖4和圖5所示。

(2)將采樣得到的q值代入式(2)，式(3)中求解兩機械臂的相對位姿值x和主機械臂的位姿值y。

(3)將函數(shù)的非線性擬合轉(zhuǎn)化為最小二乘問題，利用優(yōu)化算法如貪心算法來進行求解。

設(shè)預(yù)先選定的函數(shù)模型序列和系數(shù)序列分別為式(9)和式(10)，函數(shù)p(x) 的求解問題最終可以轉(zhuǎn)化為式(11)所示。

(9)

(10)

(11)

首先，在數(shù)學(xué)軟件Matlab編寫采樣程序，采樣足夠多的關(guān)節(jié)空間q值序列，利用改進的Hit-and-Run[5-8,13]算法在流形σ(q)=0鄰域生成一個偽均勻分布D={|σ(q)|<ε}，如圖5所示，該圖表示了采樣算法在生成關(guān)節(jié)點的過程的示意圖。

Hit-and-Run偽代碼如下所示：

假設(shè)已經(jīng)采樣得到k-1

step1：選擇跳轉(zhuǎn)次數(shù)T?1；

step2：設(shè)q(p)為在已經(jīng)生成的(p=1,…，k-1)個q值中任意挑選的一個值，并將其作為起點；

step3：設(shè)s←0,q(k),0←q(p)；

step4：生成一個隨機方向v，且‖v‖=1。

step5：設(shè)現(xiàn)在生成以q(k),s為起點沿方向v生成下一個點q(k),s+1=q(k),s+μv(經(jīng)過多次試驗μ=2較合適)；

step6：設(shè)s←s+1；

step7：若s=T，設(shè)q(k)←q(k),T，否則回到step4；

step8：returnq(k)；

上述的偽代碼可用圖4的流程圖表示。

圖4 Hit-and-Run 算法的流程圖

圖5 Hit-and-Run算法示意圖(T=4)

圖6 σ 值的分布

3 擴展任務(wù)關(guān)系式擬合結(jié)果

根據(jù)上節(jié)敘述的求解步驟，第2步驟為利用采樣得到的關(guān)節(jié)空間求出x,y。之后第3步驟為對擴展任務(wù)y與x的關(guān)系進行擬合，即求式(9)和式(11)的向量，最終確定式y(tǒng)=-p(x)。

挑選合適的模型函數(shù)，可以快速擬合并提高精度，研究表明將改進的傅里葉序列作為模型函數(shù)[14]，可以達到快速擬合的效果，表2和表3列舉了選擇的模型函數(shù)(式(9))。利用綜合優(yōu)化分析軟件1stopt，調(diào)用麥夸特算法(Leverberg -Marquardt)求解，以位移2mm 角度6°為收斂判斷標準，確定式(10)表示的系數(shù)和常數(shù)項c′，最終可求得函數(shù)的形式：

表2 主機械臂位置的模型函數(shù)

表3 主機械臂姿態(tài)的模型函數(shù)

表4 系數(shù)值結(jié)果

4 基于RobotStudio的仿真結(jié)果

RobotStudio是ABB公司開發(fā)的具有強大功能的機器人仿真環(huán)境，它提供了同ABB機器人實體產(chǎn)品相同的逆解算法。在RobotStudio環(huán)境中建立雙工業(yè)機械臂工作站，利用提供的虛擬示教器結(jié)合上述擴展任務(wù)擬合公式進行編程?，F(xiàn)設(shè)定雙臂協(xié)作任務(wù)算例：從機械臂末端沿主臂末端Z軸直線運動(0～100mm)。如式(2)所示，x表示從機械臂(follower)相對于主機械臂(leader)的位置和姿態(tài)，在任務(wù)路徑中共插值5個點即：

對機械臂進行編程。

從機械臂(follower)的程序摘要為：

……

MoveJ x1……Wobj：=Leader；

MoveJ x2……Wobj：=Leader；

MoveJ x3……Wobj：=Leader；

MoveJ x4……Wobj：=Leader；

MoveJ x5……Wobj：=Leader；

……

主機械臂(leader)的程序摘要為：

……

MoveJ func(x1)……；

MoveJ func(x2)……；

MoveJ func(x3)……；

MoveJ func(x4)……；

MoveJ func(x5)……；

……

上述從機械臂的指令中：

(1)MoveJxi：表示將機械臂的末端移至xi變量所表示的位置和姿態(tài)；

(2)變量xi：表示雙臂協(xié)作任務(wù)描述的中間路徑點。如上述，所設(shè)的算例為相對線性運動，現(xiàn)在在路徑中都插值5個中間路徑點(x1-x5)；

(3)Wobj：= Leader：表示機械臂末端的參考坐標系為主機械臂的末端。

(4)func()表示函數(shù)y=-p(x)。

程序運行時的機械臂的運行過程如圖7所示，它們顯示了雙臂系統(tǒng)在利用了擴展任務(wù)的擬合公式(y=-p(x))對機器人編程后的位形隨時間的變化過程。首先設(shè)置兩只機械臂的初始位置(t=0s)為操作性能較劣的位形，隨著時間的進行，兩只機械臂沿著xi路徑點指示的位姿運動，調(diào)用的是ABB機械臂自帶的逆解算法。

圖7 雙臂系統(tǒng)執(zhí)行相對直線任務(wù)時運行的位形隨時間變化

對于工業(yè)機械臂的構(gòu)型，使其在工作中遠離肘關(guān)節(jié)奇點和肩關(guān)節(jié)奇點[11]，具有很重要的現(xiàn)實意義，它是機械臂能夠靈活運動的一個重要因素。觀察圖7僅從外形的視角可以看出，基于擴展任務(wù)關(guān)系對機器人編程可使兩機械臂都遠離兩種奇異位形，這體現(xiàn)出了雙臂各向同性指標的優(yōu)化效果和擬合公式的有效性。

圖7僅是從外形的角度上觀察機械臂的位形。如1.2節(jié)所述機器人性能指標(如可操作度、條件數(shù)、各向同性指標等)的一個重要功能是可以量化地表示機械臂的操作性能。圖7和圖8繪制的曲線圖表示了雙臂的性能指標的值隨時間的變化過程。

圖8繪制的是本文所選用的優(yōu)化項“雙臂各向同性指標(DAIM)”的值隨時間的變化情況，可以看出該值在雙臂系統(tǒng)運行時相對初始位置(t=0s)會變大，這說明基于擴展任務(wù)的優(yōu)化方法可以優(yōu)化DAIM指標的數(shù)值。

為了進一步驗證兩只機械臂確實均處于“速度分布均勻”的位形，現(xiàn)繪制兩只機械臂的經(jīng)典性能指標—條件數(shù)—隨時間的變化情況如圖9所示，可以看出兩只機械臂的條件數(shù)均會出現(xiàn)變小的趨勢，這印證了“雙臂各向同性指標”的優(yōu)化效果同條件數(shù)是類似的。

圖8 雙臂各向同性值隨時間的變化

圖9 兩只機械臂條件數(shù)隨時間的變化

5 結(jié)論

本文對最近提出的一種通用冗余機械臂位姿優(yōu)化方法進行了擴展研究，改變了優(yōu)化指標，將乘積形式的“雙臂各向同性指標”作為優(yōu)化目標，然后對優(yōu)化問題進行了數(shù)學(xué)描述，編寫采樣算法，得到大量的關(guān)節(jié)空間值的樣本，最終擬合出擴展任務(wù)(主機械臂的位姿)和相對位姿的數(shù)學(xué)關(guān)系式，利用該關(guān)系式在仿真環(huán)境RobotStudio對雙臂系統(tǒng)進行編程，觀察雙臂的運行時的位形變化，為了量化顯示位形的優(yōu)化，繪制了性能指標的數(shù)值隨時間的變化情況。

結(jié)果表明：

(1)將各向同性指標作為優(yōu)化項后，基于蒙特卡洛的采樣算法對優(yōu)化函數(shù)采樣能夠在最優(yōu)位姿附近得到大量的有效的關(guān)節(jié)值。

(2)根據(jù)擴展任務(wù)關(guān)系式對機器人編程會使兩只機械臂在運行的過程中都處于遠離軸關(guān)節(jié)奇點和肩關(guān)節(jié)奇點，DAIM的值會有增大趨勢，條件數(shù)的值會有減

小，這顯示了本文使用的基于擴展任務(wù)的位姿優(yōu)化方法結(jié)合“雙臂各向同性指標”的優(yōu)化效果的有效性。

[1] Smith C. Dual arm manipulation-A survey[J]. Robotics and Autonomous Systems, 2012,60(10):1340-1353.

[2] Jamisola R S, R G Roberts. A more compact expression of relative Jacobian based on individual manipulator Jacobians[J]. Robotics and Autonomous Systems, 2015,63: 158-164.

[3] Baillieul. J. Kinematic programming alternatives for redundant manipulators. in Proceedings[C]. IEEE International Conference on Robotics and Automation,Boston: IEEE, 1985: 25-28.

[4] Zanchettin A M, P Rocco. A General User-Oriented Framework for Holonomic Redundancy Resolution in Robotic Manipulators Using Task Augmentation[J]. IEEE Transactions on Robotics, 2012,28(2): 514-521.

[5] Zanchettin A M. Exploiting task redundancy in industrial manipulators during drilling operations[C].IEEE International Conference on Robotics and Automation(ICRA). Shanghai: IEEE, 2011: 128-133.

[6] Zanchettin A. On the Use of Functional Redundancy in Industrial Robotic Manipulators for Optimal Spray Painting[C]. IFAC World Congress 2011. Milano, Italy,2011: 11495-11500.

[7] Zanchettin A M, P Rocco. Dual-arm redundancy resolution based on null-space dynamically-scaled posture optimization[C].IEEE International Conference on Robotics and Automation (ICRA) SaintPaul: IEEE, 2012:311-316.

[8] Jin-Oh Kim,P K Khosla. Dexterity measures for design and control of manipulators[J]. IEEE International Workshop on Intelligent Robots and Systems,1991(2):759-763.

[9] O Egeland.Task-space tracking with redundant manipulator[J].IEEE journal on robotic and automation, 2003,3: 471-475.

[10] Seraji H. Kinematic functions for redundancy resolution using configuration control[P]. US Patent: 5294873,1994-03-15.

[11] 梁新榮. 機器人協(xié)調(diào)控制及其關(guān)節(jié)結(jié)構(gòu)優(yōu)化[J]. 五邑大學(xué)學(xué)報(自然科學(xué)版), 1999,13(1): 42-48.

[12] Siciliano.Robotics: Modelling, Planning and Control[M]. London: Springer, 2009.

[13] M Chen , B Schmeiser.General hit-and-run Monte Carlo sampling for evaluating multidimensional integrals[J]. Operations Research Letters,1996,19(4):161-169.

[14] D Huybrechs,S Olver. Rapid function approximation by modified Fourier series[C]. Proceedings of Highly Oscillatory Problems, Cambridge : Cambridge University Press 2009:51-71.

[15] 謝碧云, 趙京. 機器人運動靈活性問題研究概述[J].機械科學(xué)與技術(shù), 2011,30(8):1386-1393.