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

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

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

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

大多數(shù)機器人校準算法更關(guān)注定位精度，對姿態(tài)精度的研究相對較少。采用直接法進行位置和姿態(tài)的同時校準存在非同質(zhì)性和坐標系建系策略的問題，導致校準結(jié)果和算法收斂性較差[8]。文獻[9]通過在末端安裝三個靶球并同時測量其點位數(shù)據(jù)，最小化三靶球定位精度的平方和來實現(xiàn)姿態(tài)精度的提升。為了提高雙臂機器人兩臂各自校準的位姿精度并同時考慮校準算法魯棒性，本文提出了最小化三個靶球?qū)?yīng)的最大定位誤差的方法?；诖藘?yōu)化目標，本文使用二次序列規(guī)劃(sequential quadratic programming,SQP)方法對極小極大問題進行求解。

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

1 魯棒的校準問題建模

1.1 先進機器人測量系統(tǒng)與校準流程

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

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

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

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

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

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

Δp=|y-p|

(1)

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

(2)

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

Δd=|dm-do|

(3)

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

(4)

1.3 協(xié)作定位誤差的線性近似

(5)

可以得到

Δpj-Δpi

(6)

圖3 雙臂機器人左右臂末端間協(xié)作定位誤差的線性近似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的協(xié)作定位誤差-幾何參數(shù)的雅可比矩陣為

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

1.4 機械臂的幾何參數(shù)校準問題描述

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

(10)

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

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

(11)

對式(11)兩邊求導得到

(12)

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

對于左右兩臂而言，對于單個靶球?qū)?yīng)的最小二乘誤差可以表示為

(13)

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

(14)

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

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

(15)

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

(16)

即優(yōu)化問題的不等式約束。單臂的幾何參數(shù)校準優(yōu)化問題可總結(jié)為

(17)

1.5 雙臂機械臂間的坐標轉(zhuǎn)換關(guān)系校準

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

(18)

(19)

(20)

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

(21)

(22)

式中，下標L和R分別為對應(yīng)左右臂的參數(shù)，則左右臂末端的協(xié)作定位名義計算值為

(23)

測量而得的距離為

dm=||yR-yL||

(24)

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

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

(25)

在坐標轉(zhuǎn)換關(guān)系辨識過程中，定義指標函數(shù)為

(26)

(27)

此問題與幾何參數(shù)校準問題類似，也是極小極大優(yōu)化搜索問題。

2 校準問題的求解

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

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

(28)

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

(29)

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

(30)

其中,

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

(31)

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

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

(32)

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

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

(33)

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

(34)

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

(35)

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

(36)

式中，p為二次規(guī)劃子問題的迭代次數(shù)，定義KKT算子為

(37)

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

zp+1=zp-αpTp

(38)

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

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

(39)

其中,

(40)

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

2.2 目標函數(shù)及約束的梯度

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

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

(41)

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

(42)

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

(43)

約束c(x)的梯度為

式中，t=[ta,tb,tc]T為描述在工具坐標系下的三個靶球?qū)?yīng)的名義坐標值。

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

(45)

(46)

2.3 凸性及收斂性分析

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

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

(47)

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

二次規(guī)劃子問題的目標函數(shù)

(48)

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

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

(49)

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

(50)

式中，γp為比例因子，用于調(diào)節(jié)收斂速度。

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

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

算法：極小極大搜索算法

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

輸出:辨識的最優(yōu)參數(shù)x*

whilex未收斂do

Δx0=0,λ0=0

whileΔx未收斂do

主二元子梯度算法

endwhile

更新狀態(tài)、約束、梯度和海塞矩陣：

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 試驗結(jié)果分析

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

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

3.1 單臂絕對精度的試驗結(jié)果分析

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

圖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

從試驗結(jié)果中可以看出，兩種方法都能明顯提升機器人的絕對定位和定向精度。從絕對定位與定向誤差的角度來分析，本文提出的方法(方法1)的提升效果明顯優(yōu)于傳統(tǒng)方法(方法2)，尤其對于絕對定向精度指標而言。左右臂相關(guān)絕對精度的統(tǒng)計指標詳細列在表2中。從表2可以看出，對于左臂而言，方法1和方法2的提升效果幾乎一致，表明方法2的優(yōu)化靶球測量點位正好是方法1中三個靶球誤差最大的那一點。使用方法1可以使校準的參數(shù)辨識過程有較強的魯棒性，經(jīng)過參數(shù)補償后，左右臂的絕對定位精度分別提升了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

我們將辨識后的結(jié)構(gòu)參數(shù)寫回機器人控制器，并測試了不同于校準測量點的驗證點絕對位姿精度。發(fā)現(xiàn)即使不參與校準的機器人位型，也能夠很好地實現(xiàn)精度提升，相應(yīng)的位姿精度提升效果如圖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 雙臂協(xié)作精度的試驗結(jié)果分析

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

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

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

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

4 結(jié) 論

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

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

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

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