UKF算法在七自由度機械臂逆運動學求解的應用

黃宗建， 方濤

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

0 前言

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

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

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

1 冗余機械手閉環框架算法

1.1 無跡卡爾曼濾波算法

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

1.2 閉環框架

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

圖1 傳統的序列驅動模型

(1)

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

圖2 用于多維序列處理的閉環框架模型

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

1.3 冗余機械手逆運動學

(2)

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

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來獲得旋轉軸的單位向量k和角度θ，可將r11、r22、r33視為冗余變量。因此，在逆運動學閉環框架中，末端執行器的齊次矩陣可分為u1=[r12,r13,r21,r23,r31,r32,x,y,z]T和u2=[r11,r22,r33,x,y,z]T。值得注意的是u1和u2有3個相同的變量與位置相關，因為與該位置相關的齊次矩陣中沒有冗余信息。

將u1作為輸入，將u2視為測量輸出，將q視為狀態，使用Newton-Raphson方法可以構造狀態方程[14]，即閉環框架下的模型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為迭代次數。

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

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

(7)

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

2 仿真模擬

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

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

圖3 七自由度冗余度機器人關節坐標系

2.1 生成仿真數據

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]。方案一的仿真結果如圖4所示，方案二的仿真結果如圖5所示。可以看出：方案一和方案二的所有解決方案都是可行的，方案二的誤差是方案一最大值誤差的5%，均小于2°，對機器人末端執行器位置和旋轉的誤差評估有著類似的影響。另外方案二的計算時間短，即計算效率優于方案一，因此新的輸入序列劃分方法具有一定優勢。

圖4 方案一的仿真計算結果

圖5 方案二的仿真計算結果

2.3 方法的比較分析

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

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

(8)

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

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

圖6 傳統的雅可比偽逆方法計算結果

2.4 帶噪聲軌跡的仿真

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

圖7 傳統方法對有噪聲軌跡的估計誤差

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

3 實驗結果

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

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

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

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

4 結論

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