黃宗建， 方濤

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

0 前言

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

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

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

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

1.1 無跡卡爾曼濾波算法

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

1.2 閉環(huán)框架

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

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

(1)

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

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

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

1.3 冗余機(jī)械手逆運(yùn)動(dòng)學(xué)

(2)

定義聯(lián)合變量的向量為q=[q1,q2,…,q7]，利用D-H法從機(jī)器人的正運(yùn)動(dòng)學(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視為冗余變量。因此，在逆運(yùn)動(dòng)學(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個(gè)相同的變量與位置相關(guān)，因?yàn)榕c該位置相關(guān)的齊次矩陣中沒有冗余信息。

將u1作為輸入，將u2視為測(cè)量輸出，將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法對(duì)機(jī)器人進(jìn)行正運(yùn)動(dòng)學(xué)求解，得到閉環(huán)框架下的測(cè)量方程為模型B，如式(7)所示:

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

(7)

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

2 仿真模擬

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

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

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

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

2.2 劃分輸入序列

為了驗(yàn)證所提出劃分輸入序列的新方法，進(jìn)行了2種方案的對(duì)比。方案一與參考文獻(xiàn)[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°，對(duì)機(jī)器人末端執(zhí)行器位置和旋轉(zhuǎn)的誤差評(píng)估有著類似的影響。另外方案二的計(jì)算時(shí)間短，即計(jì)算效率優(yōu)于方案一，因此新的輸入序列劃分方法具有一定優(yōu)勢(shì)。

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

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

2.3 方法的比較分析

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

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

(8)

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

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

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

2.4 帶噪聲軌跡的仿真

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

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

圖8 新方法對(duì)有噪聲軌跡的估計(jì)誤差

3 實(shí)驗(yàn)結(jié)果

此實(shí)驗(yàn)平臺(tái)用KUKA KMR iiwa機(jī)器人搭建，如圖9所示，該機(jī)械手具有7個(gè)自由度。

圖9 KUKA KMR iiwa機(jī)器人實(shí)驗(yàn)平臺(tái)

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

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

4 結(jié)論

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