UKF算法在七自由度機械臂逆運動學(xué)求解的應(yīng)用

黃宗建， 方濤

(1.河南工業(yè)職業(yè)技術(shù)學(xué)院機電工程學(xué)院， 河南南陽 473000；2.長春大學(xué)機械與車輛工程學(xué)院， 吉林長春 130000)

0 前言

六自由度機器人的逆運動學(xué)已經(jīng)相當成熟，然而其較差的避障能力和較弱的操作靈活性限制了其廣泛的應(yīng)用[1]。七自由度機器人是一種眾所周知的冗余機器人，它有七個關(guān)節(jié)來驅(qū)動末端執(zhí)行器，冗余自由度大大提高了系統(tǒng)的避障能力和操作靈活性，然而求解七自由度機器人的逆運動學(xué)是非常困難的，因為它們有無窮多個解。

現(xiàn)有文獻對七自由度冗余度機器人的逆運動學(xué)進行了大量的研究，目前研究逆運動學(xué)方法可分為3種類型:幾何方法、數(shù)值方法、智能算法。幾何方法是利用機器人連桿之間的幾何關(guān)系來獲得逆運動學(xué)的閉合/解析解[2-4]。對于七自由度機器人，無限的幾何關(guān)系使解決方案不可能實現(xiàn)，除非指定預(yù)定義的“冗余φ”或附加的關(guān)節(jié)約束。通常，有指定的“冗余φ”，逆運動學(xué)的原始映射為(θ1,θ2,…,θ7)=f(x,y,z,α,β,γ)，變成(θ1,θ2,…,θ7)=f(x,y,z,α,β,γ,φ)，然后應(yīng)用優(yōu)化算法獲得“冗余φ”和聯(lián)合變量的最優(yōu)解，幾何方法效率高、可靠性高、精度高，但只能應(yīng)用于具有特殊結(jié)構(gòu)的機器人。數(shù)值方法采用數(shù)值迭代算法求解逆運動學(xué)。常用的數(shù)值方法有偽逆-雅可比法[5]、梯度投影法[6]、加權(quán)范數(shù)法[7-8]等。這些方法簡單，通用性強，不需要特殊的結(jié)構(gòu)。然而這些方法需要高性能的計算機硬件，它們有累積誤差，對初始值敏感，并且不能保證找到可行的解決方案。智能算法有許多不同的形式[9-11]，如神經(jīng)網(wǎng)絡(luò)算法、支持向量機、粒子群優(yōu)化，這些算法通常用于離線求解逆運動學(xué)問題。由于計算量大，不利于實時在線實現(xiàn)，且在逆運動學(xué)中沒有明顯的意義，也不能保證收斂和準確。

在前人研究的基礎(chǔ)上，本文作者采用一種新的數(shù)值序列處理方法求解七自由度機器人的運動學(xué)逆解，該方法可以看作是傳統(tǒng)雅可比偽逆方法在運動學(xué)逆問題上的改進，具有數(shù)值方法簡單、通用性強、計算效率和可靠性高、不需要特殊結(jié)構(gòu)等優(yōu)點。為了驗證該方法的有效性，對所提方法進行了仿真，并與傳統(tǒng)的雅可比偽逆方法進行了比較，分析了兩者的差異。最后在真實的機器人系統(tǒng)中測試了所提方法的有效性。

1 冗余機械手閉環(huán)框架算法

1.1 無跡卡爾曼濾波算法

無跡卡爾曼濾波(Unscented Kalman Filter, UKF)算法由于較高的實時精度，在非線性系統(tǒng)估計方面具有廣泛的應(yīng)用。UKF的核心是無跡變換(Unscented Transform，UT)，直接作用于非線性動力學(xué)模型過程中，不需要線性化，即不需要計算其雅可比矩陣，可以處理不可導(dǎo)非線性函數(shù)[12]。與擴展卡爾曼濾波算法相比，UKF算法具有更好的實時性和更高的精度。因此，UKF算法在系統(tǒng)參數(shù)估計領(lǐng)域得到了廣泛的應(yīng)用。利用UKF系統(tǒng)狀態(tài)估計方法構(gòu)建自適應(yīng)最優(yōu)控制器，能夠克服外部復(fù)雜性和非線性等因素引起的機器人系統(tǒng)動態(tài)參數(shù)的不確定性，提高了系統(tǒng)的魯棒性和機器人定位的準確性。JOUKHADAR 和 KASS HANNA[13]提出了小腦模型關(guān)節(jié)控制器(Cerebellar Model Articulation Controller，CMAC)算法來估計機器人末端執(zhí)行器的姿態(tài)，并使用UKF算法來比較機器人的實際運動參數(shù)和名義運動參數(shù)。對串聯(lián)機器人的運動參數(shù)誤差進行了自標定，其優(yōu)點是可以在不停車的情況下對機器人的姿態(tài)進行準確的修正，從而提高了動態(tài)條件下機器人修正的效率和實用性?？梢奤KF算法已經(jīng)成為世界上在線系統(tǒng)建模方法的一個研究熱點，并取得了豐碩的研究成果。

1.2 閉環(huán)框架

傳統(tǒng)的序列驅(qū)動模型總是向前移動，沒有信息作為反饋來修正其輸出，可能會由于估計誤差的累積而逐漸偏離真值。這類典型的實例如圖1所示。

圖1 傳統(tǒng)的序列驅(qū)動模型

(1)

式(1)中：K為時變增益。為構(gòu)建狀態(tài)空間形式的閉環(huán)框架，需建立2個模型，如圖2所示。在閉環(huán)模型中，可以使用UKF等濾波算法估計在線狀態(tài)。

圖2 用于多維序列處理的閉環(huán)框架模型

在閉環(huán)框架下，估計的狀態(tài)不會偏離它們的實際值，因為測量可以反饋實時的狀態(tài)估計。此外，當多維向量u(k)中包含冗余信息時，可能會增加計算的復(fù)雜度，降低模型的精度，但閉環(huán)框架可以充分利用冗余信息，并降低計算的復(fù)雜度。可以通過機械手末端執(zhí)行器的運動軌跡作為輸入序列，采用UKF算法對閉環(huán)模型的狀態(tài)進行估計。相比之下，傳統(tǒng)的雅可比矩陣的偽逆計算方法得到的結(jié)果是發(fā)散的，而閉環(huán)模型產(chǎn)生的估計誤差可以收斂到零。

1.3 冗余機械手逆運動學(xué)

(2)

定義聯(lián)合變量的向量為q=[q1,q2,…,q7]，利用D-H法從機器人的正運動學(xué)得到：

rij=fijqi=1,2,3j=1,2,3,4

(3)

(4)

其中：sθ=sinθ;cθ=cosθ；versθ=1-cθ。式(4)的逆解為

(5)

式(5)表示需要r12、r13、r21、r23、r31、r32來獲得旋轉(zhuǎn)軸的單位向量k和角度θ，可將r11、r22、r33視為冗余變量。因此，在逆運動學(xué)閉環(huán)框架中，末端執(zhí)行器的齊次矩陣可分為u1=[r12,r13,r21,r23,r31,r32,x,y,z]T和u2=[r11,r22,r33,x,y,z]T。值得注意的是u1和u2有3個相同的變量與位置相關(guān)，因為與該位置相關(guān)的齊次矩陣中沒有冗余信息。

將u1作為輸入，將u2視為測量輸出，將q視為狀態(tài)，使用Newton-Raphson方法可以構(gòu)造狀態(tài)方程[14]，即閉環(huán)框架下的模型A，如式(6)所示：

q(n+1)=q(n)+J+q(n)·[u1(n)-f1q(n)]

(6)

其中：f1(·)=[f12,f13,f21,f23,f31,f32,f14,f24,f34]，J=[?f1/?q]為雅可比矩陣，J+為J的偽逆矩陣；n為迭代次數(shù)。

采用D-H法對機器人進行正運動學(xué)求解，得到閉環(huán)框架下的測量方程為模型B，如式(7)所示:

u2(k)=f2[q(k)]

(7)

其中f2(·)=[f11,f22,f33,f14,f24,f34]。結(jié)合式(6)和式(7)，即可建立具有閉環(huán)框架的七自由度冗余機器人逆運動學(xué)的狀態(tài)空間模型。

2 仿真模擬

分別采用UKF算法和傳統(tǒng)的Jacobi偽逆方法對七自由度冗余機器人逆運動學(xué)進行了仿真，通過機器人仿真工具箱與MATLAB相結(jié)合，在Windows 7操作系統(tǒng)上進行模擬仿真，CPU為Intel(R) Core(TM) i5-3470, CPU為3.20 GHz，內(nèi)存為8 GB。仿真中使用的七自由度冗余機器人的D-H參數(shù)如表1所示，七自由度冗余機器人的關(guān)節(jié)坐標系如圖3所示。

表1 七自由度冗余機器手的D-H參數(shù)

圖3 七自由度冗余度機器人關(guān)節(jié)坐標系

2.1 生成仿真數(shù)據(jù)

2.2 劃分輸入序列

為了驗證所提出劃分輸入序列的新方法，進行了2種方案的對比。方案一與參考文獻[15]相同，將u分成了u1=[r11,r12,r21,r23,r33]和u2=[r14,r24,r34,r32]；方案二如前所述，將u分為u1=[r12,r13,r21,r23,r31,r32,x,y,z]和u2=[r11,r22,r33,x,y,z]。方案一的仿真結(jié)果如圖4所示，方案二的仿真結(jié)果如圖5所示?？梢钥闯觯悍桨敢缓头桨付乃薪鉀Q方案都是可行的，方案二的誤差是方案一最大值誤差的5%，均小于2°，對機器人末端執(zhí)行器位置和旋轉(zhuǎn)的誤差評估有著類似的影響。另外方案二的計算時間短，即計算效率優(yōu)于方案一，因此新的輸入序列劃分方法具有一定優(yōu)勢。

圖4 方案一的仿真計算結(jié)果

圖5 方案二的仿真計算結(jié)果

2.3 方法的比較分析

在新的閉環(huán)框架中，采用UKF算法作為遞推算法進行實時狀態(tài)的估計。傳統(tǒng)的雅可比偽逆方法通常采用基于梯度下降法的迭代算法形式，如式(8)所示：

q(i+1)=q(i)+J+q(i)·{tr2diff[fkineq(i)]t}

(8)

其中，i表示迭代次數(shù)，fkineq(·)為正運動學(xué)方程，tr2diff(·)將變換差分轉(zhuǎn)換為微分；t為末端執(zhí)行器的目標齊次矩陣。

傳統(tǒng)的雅可比偽逆方法的結(jié)果如圖6所示，在0.2°以內(nèi)的誤差范圍內(nèi)，傳統(tǒng)方法計算的結(jié)果，要小于圖5中方案二的誤差。但軌跡上的3個點(即關(guān)節(jié)1、關(guān)節(jié)4和關(guān)節(jié)7)用傳統(tǒng)方法無法求解。因此與傳統(tǒng)方法相比，新方法具有更高可靠性。

圖6 傳統(tǒng)的雅可比偽逆方法計算結(jié)果

2.4 帶噪聲軌跡的仿真

在仿真中，將七自由度冗余機器人末端執(zhí)行器的軌跡輸入序列加入高斯噪聲，高斯噪聲的均值為0，方差為0.001。然后分別采用傳統(tǒng)方法和提出的方法對包含高斯噪聲的輸入序列進行聯(lián)合變量的估計。估計誤差如圖7和圖8所示，軌跡上的7個點(關(guān)節(jié)1—關(guān)節(jié)7)采用傳統(tǒng)方法無法求解，由于高斯噪聲的影響，傳統(tǒng)方法的估計誤差是發(fā)散的，如圖7所示；而新方法的誤差即使變大，仍然是收斂的，并且較大的誤差低于最大誤差。

圖7 傳統(tǒng)方法對有噪聲軌跡的估計誤差

圖8 新方法對有噪聲軌跡的估計誤差

3 實驗結(jié)果

此實驗平臺用KUKA KMR iiwa機器人搭建，如圖9所示，該機械手具有7個自由度。

圖9 KUKA KMR iiwa機器人實驗平臺

為了驗證該方法的有效性，在真實的機器人上運行了該方法。圖10所示為基于UKF狀態(tài)估計的閉環(huán)框架計算得到的各關(guān)節(jié)位置誤差，以及正運動學(xué)解計算得到的各關(guān)節(jié)角度誤差。通過所提方法得到的機械手終端位置估計誤差小于0.07 mm，角度誤差小于0.008 rad，該誤差可以滿足機器人的各種工作條件，即所提方法可以用來求解機器人的逆運動學(xué)。

圖10 新方法在實驗過程中的位置和旋轉(zhuǎn)角度誤差

4 結(jié)論

在六自由度機器人逆運動學(xué)求解的基礎(chǔ)上，提出采用一種閉環(huán)框架求解七自由度冗余度機器人的運動學(xué)逆解。將無跡卡爾曼濾波算法作為遞推算法對機械臂實時狀態(tài)進行估計，提出了在冗余機器人逆運動學(xué)條件下劃分輸入序列的新方法。分析對比了新方法與傳統(tǒng)雅可比偽逆計算方法的位置誤差精度和計算效率，仿真結(jié)果表明新方法計算效率高、可靠性高。當在末端執(zhí)行器的軌跡中加入噪聲時，傳統(tǒng)的方法會迅速發(fā)散，而新方法的收斂性較好，證明所提方法在七自由度機器人上的應(yīng)用是可行的，最后通過實驗表明該方法具有較高的通用性。