基于極小極大算法的雙臂機器人精度提升方法

祖洪飛， 陳章位， 毛晨濤， 陳廣初， 李 杰

(1.浙江理工大學 機械與自動控制學院,杭州 310023；2.浙江大學 流體動力與機電系統國家重點實驗室,杭州 310014；3.杭州億恒科技有限公司,杭州 311112；4.佛山市南海區質量技術監督檢測所, 廣東 佛山 528200)

在機器人生產過程中往往會產生幾何參數誤差，對這些幾何參數進行運動學校準是提高機器人精度的重要手段[1]。已有大量研究表明，利用校準后的幾何參數，機器人末端的定位精度可以得到極大提升。然而目前對于雙臂機器人的校準問題研究較少。而且，在協同抓取、機械加工等操作中，除定位精度外，協作精度也是一項重要的指標。但之前關于雙臂機器人運動學校準的研究一般主要關注定位精度，而忽視了協作精度。因此，亟需尋找一種兼顧雙臂各自的絕對位姿精度和雙臂間協作位姿精度的校準算法，從而提升雙臂機器人的綜合性能。

雙臂機器人的校準可分解為兩臂各自幾何參數校準及其坐標轉換的校準。對于兩臂各自幾何參數的校準，其主要原理是：利用激光跟蹤儀等測量設備獲取機器人在不同位型下的末端點位數據，利用機器人前向運動學模型，搜索最優的幾何參數以得到基于測量數據的機器人末端位置的最優擬合[2-4]。其中最常見的方法是通過對機器人末端絕對定位二乘誤差最小化來辨識幾何參數誤差，其本質是一個非線性優化問題，求解算法有高斯牛頓法、萊文貝格-馬夸特(L-M)法、卡爾曼濾波法等。高斯牛頓法通常是最小化殘差平方和，其收斂速度較快，在機器人校準中得到廣泛使用[5]。L-M法通過改善高斯-牛頓法中矩陣的正定性，增大了算法對初值的敏感性[6]?？柭鼮V波法被用來減小測量誤差和機器人重復性誤差對辨識過程的影響[7]。

大多數機器人校準算法更關注定位精度，對姿態精度的研究相對較少。采用直接法進行位置和姿態的同時校準存在非同質性和坐標系建系策略的問題，導致校準結果和算法收斂性較差[8]。文獻[9]通過在末端安裝三個靶球并同時測量其點位數據，最小化三靶球定位精度的平方和來實現姿態精度的提升。為了提高雙臂機器人兩臂各自校準的位姿精度并同時考慮校準算法魯棒性，本文提出了最小化三個靶球對應的最大定位誤差的方法。基于此優化目標，本文使用二次序列規劃(sequential quadratic programming,SQP)方法對極小極大問題進行求解。

以前關于雙臂機器人運動學校準的研究注重其絕對位姿誤差[10-11]，而忽視了協作位姿精度。協作位姿精度與機器人雙臂間坐標轉換關系的校準結果相關。本文以兩臂間點位的距離來衡量協作定位精度，通過魯棒的極小極大算法來提升雙臂間的協作位姿精度。

1 魯棒的校準問題建模

1.1 先進機器人測量系統與校準流程

圖1是機器人測量系統，由雙臂機器人、機器人控制器、靶球組件和激光跟蹤儀組成。在圖1(b)中，在機械手末端法蘭盤上安裝了三個可以原路反射激光的靶球，使用激光跟蹤儀測量三個球心對應的實到位置。對于左右兩臂分別隨機選擇k個機器人位型，在每個位型下同時記錄各個關節軸的角度值θ和三個靶球的點位數據。機器人的幾何參數校準即是在一定的性能指標下，對所測量點位數據的最優擬合。

圖1 億恒雙臂機器人的測量試驗平臺搭建Fig.1 Construction of a measurement experiment platform for a dual-arm robot

雙臂機器人的校準流程圖如圖2所示。首先，根據絕對定位精度的指標優化單臂的幾何參數；然后，通過以協作定位精度作為目標函數來優化左右臂之間坐標轉換的參數。單臂的幾何參數校準和左右臂坐標轉換參數校準都是通過最小化三個靶球對應的最差定位精度來實現的。通過空間三個點可以約束機器人末端工具坐標系六個自由度方向的運動，盡可能減少這三個點的定位誤差，既可以保證工具坐標系的位姿精度，又增加了校準過程的魯棒性，此外避免了位姿向量定義的參考坐標系依賴與單位歸一化等問題。

圖2 雙臂機器人的校準過程流程圖Fig.2 Flow chart of the calibration process of a dual-arm robot

1.2 雙臂機器人性能指標定義

針對雙臂機器人特性，我們在標準GB 12642 (工業機器人 性能規范及其試驗方法)的基礎上提出復合的性能指標，重新定義了定向精度和協作精度。在標準ISO 9283中，對絕對定位精度的定義是

Δp=|y-p|

(1)

式中：y為單臂的實到測量位置；p為單臂末端的名義位置。如果按照標準ISO 9283將姿態精度定義為歐拉角的偏差，會在奇異位型的附近產生很大的計算偏差。因此，將絕對定向精度定義為[12]

(2)

協作定位精度描述了雙臂機器人左右臂末端之間的相對位置距離準確度，可以將其描述為

Δd=|dm-do|

(3)

式中，位型為i的左臂末端和位型為j的右臂末端之間的距離測量值為dm=||yi-yj||,對應的名義值記為do=||pi-pj||。類似地，為了避免由于歐拉角奇異產生的問題，將協作定向精度描述為

(4)

1.3 協作定位誤差的線性近似

(5)

可以得到

Δpj-Δpi

(6)

圖3 雙臂機器人左右臂末端間協作定位誤差的線性近似Fig.3 Linear approximation of cooperative positioning error between the left and right arm ends of a dual-arm robot

(7)

(8)

對式(8)兩邊求微分，可以得到雙臂機器人左臂位型為i和右臂位型為j的協作定位誤差-幾何參數的雅可比矩陣為

式中，JL和JR分別為相應位型下的左右臂各自的絕對定位誤差-幾何參數的雅可比矩陣。

1.4 機械臂的幾何參數校準問題描述

機械臂末端中心的位姿可用下面的非線性映射描述

(10)

式中，矩陣T包含末端位置p、旋轉矩陣R，映射f()描述了其與關節角度θ、幾何參數x的關系。將靶球安裝位置的工具坐標記為ta,tb和tc，則靶球中心的名義坐標為

[pa,pb,pc]=[Rta+p,Rtb+p,Rtc+p]

(11)

對式(11)兩邊求導得到

(12)

式中：下標s=a,b，c為靶球序號;Js為機器人末端絕對定位誤差-幾何參數雅可比矩陣。

對于左右兩臂而言，對于單個靶球對應的最小二乘誤差可以表示為

(13)

式中，向量ys為第s個靶球點位的測量值。將所有待辨識的參數定義為x=[μ;ta;tb;tc]，設計目標函數為

(14)

通過最小化三個靶球中絕對定位精度最差的二乘誤差，在保證絕對定位精度的同時，對于末端姿態精度也有很大的提升。

注意到三個靶球在各個位型下具有固定的相對位置，因此，靶球間距離的測量與其名義值的偏差應小于由跟蹤儀測量誤差引入的不確定度。同時，為使約束條件在微分空間連續，本優化問題采用以下等價的約束條件

(15)

式中：tl,l=1,2,3為兩個靶球間的距離偏差值，距離測量值為k個位型下的平均值；ε為由于測量誤差引入的不確定度。將該非線性不等式約束重寫為

(16)

即優化問題的不等式約束。單臂的幾何參數校準優化問題可總結為

(17)

1.5 雙臂機械臂間的坐標轉換關系校準

完成左右兩臂各自的幾何參數校準之后，第二步是對左臂和右臂之間的坐標轉換關系進行辨識。從左臂的基坐標系到右臂基坐標系的坐標變換矩陣可表示為

(18)

(19)

(20)

式中：xb，yb和zb為三維坐標量；Ψ，θ和φ為旋轉變換的角度值，分別對應ZYX歐拉角繞X軸，Y軸和Z軸方向的轉動角，這六個參數組成了左右臂間坐標轉換的待辨識參數tB=[xb,yb,zb,Ψ,θ,φ]T。機器人左臂和右臂的前向運動學矩陣分別表示為

(21)

(22)

式中，下標L和R分別為對應左右臂的參數，則左右臂末端的協作定位名義計算值為

(23)

測量而得的距離為

dm=||yR-yL||

(24)

式中，向量yR和yL分別為在測量坐標系{M}下右臂和左臂末端的測量坐標值。根據測量和名義的距離偏差，協作定位誤差可表示為

us(xL,xR,tB)=|dm-do|

(25)

在坐標轉換關系辨識過程中，定義指標函數為

(26)

(27)

此問題與幾何參數校準問題類似，也是極小極大優化搜索問題。

2 校準問題的求解

2.1 極小極大搜索算法的求解

單臂的幾何參數校準問題可以等價于下述帶有不等式約束的單目標優化問題[15]

(28)

式中，x為在校準問題中待辨識的幾何參數

(29)

式(28)中描述的優化問題近似為序列二次規劃子問題。SQP方法的核心思想是將非線性的非凸優化問題線性化后生成二次規劃子問題，并通過迭代逐步逼近原問題的目標解。因為Fs(x)是可微的，對于任何搜索方向Δx，其方向導數為

(30)

其中,

I(x)={s∶Fs(x)=ψ(x)}

(31)

那么，近似的序列二次規劃子問題可以表示為

subject to∶C(x)+G(x)Δx≤0

(32)

式中:Δx=xκ+1-xκ為迭代的步長，即每次迭代過程二次規劃問題的解，上標κ為迭代次數；海塞矩陣Hk為目標函數二階微分矩陣的近似，在迭代過程中通過BFGS方法[16]不斷更新，約束條件中G(x)是不等式約束C(x)的梯度。該二次規劃子問題的解Δx必須滿足Karush-Kuhn-Tucker(KKT)條件[17]，KKT條件中目標函數的拉格朗日形式可表示為

λT[C(x)+G(x)Δx]

(33)

式中，λ≧0為拉格朗日算子，則可推導出二次規劃子問題式(32)的KKT條件為

(34)

式中，運算符()+定義為

(35)

為了搜索滿足KKT條件的迭代步長及算子(Δx,λ)的最優解，采用下式所示的主二元子梯度算法迭代尋優

(36)

式中，p為二次規劃子問題的迭代次數，定義KKT算子為

(37)

令z=(Δx,λ)T，則搜索最優解z*過程中的更新方程是

zp+1=zp-αpTp

(38)

式中:Tp∈T(zp);αp為比例因子。

對于左右臂坐標轉換參數校準問題，將式(28)中的參數替換為tB，有

(39)

其中,

(40)

其余步驟與單臂的幾何參數校準方法類似。

2.2 目標函數及約束的梯度

對于單臂的幾何參數校準，目標函數和約束的梯度可以通過解析和數值計算兩種方法求得，采用解析式的計算精度更高，從而使優化算法的收斂速度更快。近似的序列二次規劃問題式(32)的目標函數梯度可表示為

P(x)=HkΔx+?Fs(x)|s∈I(x)

(41)

式中：海塞矩陣Hk可以通過BFGS法迭代更新；Fs(x)的梯度為

(42)

式中，Js為描述對應第s組絕對定位誤差-幾何參數誤差的雅可比矩陣。此外，不等式約束式(29)的梯度可表示為

(43)

約束c(x)的梯度為

式中，t=[ta,tb,tc]T為描述在工具坐標系下的三個靶球對應的名義坐標值。

對于左右臂間的坐標轉換參數校準，Us(x)的梯度為

(45)

(46)

2.3 凸性及收斂性分析

SQP將非凸的原非線性優化問題線性近似為迭代的二次規劃子問題，可以證明，二次規劃子問題是凸優化問題。根據凸優化理論，若想證明目標函數及約束條件是凸的，需滿足

f(αx+βy)≤αf(x)+βf(y)

(47)

式中，α+β=1,α≥0,β≥0。

二次規劃子問題的目標函數

(48)

式中，海塞矩陣Hk是正定的，因此

Q(αx+βy)-αQ(x)-βQ(y)=
-αβ(x-y)THk(x-y)≤0

(49)

目標函數的凸性由此得證。類似的，容易證明約束條件的凸性。對于凸優化問題，全局收斂到最優解的條件是[18]

(50)

式中，γp為比例因子，用于調節收斂速度。

2.4 極小極大搜索算法偽代碼

作為總結，下面列出了極小極大算法的偽代碼。首先初始化目標函數、約束條件和它們的梯度，然后采用主二元子梯度算法求解二次規劃子問題的全局收斂解。更新待辨識的參數和相應的目標函數、約束條件、梯度、海塞矩陣等信息。在更新后的待辨識參數處重新生成新的二次規劃子問題，不斷迭代直至原校準問題收斂。

算法：極小極大搜索算法

輸入：參數名義值(0)x

輸出:辨識的最優參數x*

whilex未收斂do

Δx0=0,λ0=0

whileΔx未收斂do

主二元子梯度算法

endwhile

更新狀態、約束、梯度和海塞矩陣：

xk+1=xk+Δxk,sk=Δxk,

qk=?Fs(xk+1)|s∈I+GT(xk+1)λk+1-[?Fs(xk)|s∈I+GT(xk)λk]

endwhile

3 試驗結果分析

為驗證本文所提出算法的有效性，選取雙臂機器人yumi作為研究對象進行了試驗驗證。yumi機器人左右臂由兩個七軸機械臂IRB14000組成，其MD-H參數列在表1中。雙臂機器人的左右臂由于擁有冗余自由度，可以靈巧地到達工作空間任意點，能夠完成雙臂協同類人操作，廣泛應用于醫療、化工等領域。本節對雙臂機器人左右臂各選定100個位型點，測量機械臂末端的位姿信息，通過本文提出的方法對雙臂機器人的幾何參數與坐標轉換進行辨識，與傳統方法對比校準后的絕對精度和協作精度指標。

表1 機器人IRB14000的MD-H參數Tab.1 MD-H parameters of robot IRB14000

3.1 單臂絕對精度的試驗結果分析

為驗證本文提出算法對雙臂機器人左右臂絕對精度的提升效果，以右臂為例，對不同算法的精度改善效果進行了試驗驗證，試驗對比結果如圖4和圖5所示。其中：方法1是本文提出的單臂幾何參數校準方法；方法2是單臂使用單靶球考慮絕對定位精度的幾何參數校準方法。注意到三個靶球一共對應三個絕對定位精度，在圖4和圖5中只呈現誤差最大的一項。

圖4 右機械臂校準前的絕對精度指標Fig.4 Absolute accuracy index before calibration of the right manipulator

圖5 右機械臂使用不同校準方法后絕對精度提升效果對比Fig.5 Comparison of absolute accuracy improvement effect of the right manipulator after using different calibration methods

從試驗結果中可以看出，兩種方法都能明顯提升機器人的絕對定位和定向精度。從絕對定位與定向誤差的角度來分析，本文提出的方法(方法1)的提升效果明顯優于傳統方法(方法2)，尤其對于絕對定向精度指標而言。左右臂相關絕對精度的統計指標詳細列在表2中。從表2可以看出，對于左臂而言，方法1和方法2的提升效果幾乎一致，表明方法2的優化靶球測量點位正好是方法1中三個靶球誤差最大的那一點。使用方法1可以使校準的參數辨識過程有較強的魯棒性，經過參數補償后，左右臂的絕對定位精度分別提升了83.08%和88.29%，絕對定向精度分別提升了88.52%和94.96%。

表2 使用不同校準方法前后的左右臂絕對位姿精度指標Tab.2 Absolute position and posture accuracy indexes of the left and right arms before and after using different calibration methods

我們將辨識后的結構參數寫回機器人控制器，并測試了不同于校準測量點的驗證點絕對位姿精度。發現即使不參與校準的機器人位型，也能夠很好地實現精度提升，相應的位姿精度提升效果如圖6所示。從圖6中可以看出，驗證位型的絕對定位精度從校準前的6.85 mm提升了90.51%到0.65 mm，絕對定向精度從校準前的0.139 rad提升了96.40%到0.005 rad。

圖6 右機械臂校準前后驗證點的絕對精度提升效果對比Fig.6 Comparison of absolute accuracy improvement effect of the verification points of the right manipulator

3.2 雙臂協作精度的試驗結果分析

為驗證本文提出算法對雙臂機器人左右臂間的協作精度的提升效果，將機器人幾何參數與坐標轉換參數進行辨識并補償后，對不同算法的精度改善效果進行了試驗驗證，試驗對比結果如圖7和圖8所示。其中：方法1是本文提出的雙臂機器人校準方法；方法2是單臂使用單靶球考慮絕對定位精度的幾何參數校準方法，結合本文提出的左右臂間坐標轉換參數校準方法。

圖7 雙臂機器人校準前的協作精度指標Fig.7 Collaboration accuracy index before calibration of the dual-arm robot

圖8 雙臂機器人使用不同校準方法后協作精度提升效果對比Fig.8 Comparison on the effect of collaboration accuracy improvement after using two-arm robots with different calibration methods

從試驗結果中可以看出，兩種方法都能明顯提升機器人的協作定位和定向精度。從協作定位與定向誤差分布的角度來分析，本文提出的方法(方法1)的提升效果明顯優于傳統方法(方法2)。對于協作定位精度，方法1分別有13.15%和83.52%的點位位型對應的協作定位誤差分布在區間[0,0.1]mm和[0,0.5]mm中。而對于方法2，只有9.36%和81.61%的點位位型對應的協作定位誤差分布在區間[0,0.1]mm和[0,0.5]mm中。對于協作定向精度，方法1提升效果更為明顯，有60.37%和99.83%的點位位型對應的協作定向誤差分布在區間[0,0.005]rad和[0,0.01]rad中，而方法2只有16.47%和72.70%的點位位型對應的協作定向誤差分布在區間[0,0.005]rad和[0,0.01]rad中。

4 結 論

本文針對雙臂機器人絕對精度和協作精度的指標要求，基于激光跟蹤儀測量的雙臂末端三靶球點位數據，使用極小極大的搜索算法，提出了魯棒的雙臂機器人運動學幾何參數校準方法。該方法有以下三方面的優勢：

(1)使用極小極大搜索算法最小化單臂末端最大的絕對定位誤差，增加了校準辨識過程的魯棒性。

(2)考慮姿態信息而不直接計算姿態誤差，避免了姿態歐拉角表示的奇異性對校準過程的影響，同時也避免了位置與姿態單位的歸一化問題。

(3)該方法相對于傳統方法能夠明顯提升雙臂機器人左右臂間的協作定位與定向精度。在雙臂機器人yumi上的試驗結果表明，本文提出的魯棒校準方法不僅能夠顯著提高絕對定位和定向精度，而且對于協作定位和協作定向精度也有顯著的改進，相比傳統校準算法具有明顯的精度優勢。