利用增廣雅克比矩陣的冗余機械臂逆運動學求解算法

王 騰， 張承瑞*， 陳齊志， 薛祥儒

(1.山東大學機械工程學院， 濟南 250061； 2.山東大學， 高效潔凈機械制造教育部重點實驗室， 濟南 250061)

當機械臂自身所具有的自由度多于執(zhí)行給定任務(wù)所需的自由度時，機械臂存在運動學冗余。相比于非冗余機械臂，運動學冗余使得機械臂具備了更高的靈活性。

一般來說，運動學逆解分為封閉解[1-2]和數(shù)值解兩種形式。封閉解具有很高的計算效率，這使其在實時控制中受到青睞。文獻[3]在獲得的與臂型角相關(guān)的解析表達式的基礎(chǔ)上，通過優(yōu)化最小勢能指標確定臂型角的大小，然后對各關(guān)節(jié)位置進行了求解。文獻[4]通過限制臂型角的范圍，使得逆解結(jié)果不會導(dǎo)致機械臂發(fā)生自身碰撞。文獻[5]對機械臂的構(gòu)型進行了分析，并根據(jù)幾何特性獲得了多關(guān)節(jié)偏置機械臂的封閉解。然而封閉形式的運動學逆解只適用于特定結(jié)構(gòu)的機械臂，通用性較差。數(shù)值計算方法不尋求一種解析關(guān)系[6-7]，因此適用于大多數(shù)的機械臂。為了滿足實時控制需求，在實際應(yīng)用中需要考慮數(shù)值逆解算法的求解效率。

梯度投影法(gradient projection method，GPM)[8]利用機械臂在雅可比矩陣零空間的“自運動”解決冗余問題。文獻[9]在此基礎(chǔ)上選用阻尼最小二乘形式的偽逆，使機械臂具備了避開奇異位姿的能力。文獻[10]將阻尼最小二乘偽逆求解方法與牛頓-拉夫遜迭代方法相結(jié)合，提高了軌跡跟蹤精度。文獻[11]利用由碰撞檢測反饋的避障向量構(gòu)造目標函數(shù)，實現(xiàn)了機械臂在復(fù)雜場景下的避障。然而，工作空間中的封閉路徑通過偽逆方法映射到關(guān)節(jié)空間后卻不是封閉的，即執(zhí)行循環(huán)任務(wù)時，機械臂并不能回到初始位姿，這使得機械臂的運動不可預(yù)測[12]。運動的不可重復(fù)性限制了冗余機械臂的應(yīng)用范圍。文獻[13]將機械臂的子任務(wù)選擇為優(yōu)化保證可重復(fù)運動的目標函數(shù)，使機械臂的運動具備可重復(fù)性。但是將運動學冗余用于獲得非冗余機械臂的基本特性，并沒有發(fā)揮出冗余機械臂應(yīng)有的優(yōu)勢。雖然通過加權(quán)可以使目標函數(shù)結(jié)合多種優(yōu)化準則，但由于優(yōu)化能力有限，任務(wù)的執(zhí)行效果會受到影響。

增廣雅克比矩陣方法通過添加作為額外約束的優(yōu)化任務(wù)，對原始任務(wù)進行增廣以解決冗余[14]。該方法能夠保證機械臂運動的可重復(fù)性，但隨著機械臂關(guān)節(jié)數(shù)目的增多，約束條件的計算難度增大。為解決增廣雅可比矩陣方法中約束計算復(fù)雜的問題，提出了一種冗余機械臂逆運動學求解算法。該算法采用分解求解策略，實現(xiàn)對復(fù)雜的約束條件的計算，求解效率高，可實際應(yīng)用于七自由度冗余機械臂的實時控制?，F(xiàn)以避關(guān)節(jié)極限準則作為約束條件增廣雅可比矩陣，采用提出的分解計算策略對約束條件進行分解，計算獲得增廣部分的數(shù)值結(jié)果，在笛卡爾空間進行軌跡規(guī)劃后，利用增廣雅可比矩陣計算出關(guān)節(jié)速度，對關(guān)節(jié)速度積分得到關(guān)節(jié)位置，完成逆運動學的求解。

1 運動學冗余求解方法

對于具有n個關(guān)節(jié)的機械臂，定義qi(i=1,2,…,n)表示第i個關(guān)節(jié)的位置，則機械臂的位姿可由向量q=[q1,q2,…,qn]T唯一確定。維度為m的向量x表示機械臂末端相對于基坐標的位置與姿態(tài)。當n>m時，對于給定的任務(wù)來說，機械臂是運動學冗余的，冗余自由度r=n-m。關(guān)節(jié)空間與笛卡爾空間之間的關(guān)系由正運動學公式表示為

x=f(q)

(1)

式(1)中：f為與關(guān)節(jié)位置和機械臂連桿參數(shù)有關(guān)的非線性向量函數(shù),f:Rn→Rm。

對時間求導(dǎo)，得到速度層面的運動學關(guān)系為

(2)

1.1 梯度投影法

微分運動學的最小范數(shù)解為

(3)

式(3)中：J?=JT(JJT)-1為雅克比矩陣J的Moore-Penrose偽逆。在式(3)的基礎(chǔ)上增加一個齊次項，則微分運動學的通解可以表示為

(4)

式(4)中：In為n×n的單位陣；z為n維的任意向量；In-J?J為J的零空間的投影算子。式(4)使得冗余機械臂具備在跟蹤目標軌跡的過程中，利用“自運動”執(zhí)行與優(yōu)化準則相關(guān)的子任務(wù)的能力。

令H(q)為選定的目標函數(shù)，要最小化H(q)，那么z可以表示為

(5)

式(5)中：α為標量正實數(shù)，其數(shù)值的大小影響優(yōu)化的收斂速率。

1.2 增廣雅可比矩陣方法

引入跟蹤軌跡之外的任務(wù)擴展任務(wù)空間，則機械臂在運動過程中會受到額外任務(wù)的約束。將機械臂的額外任務(wù)選擇為在運動中滿足給定的準則，那么機械臂的運動也可以看作是在運動學公式的約束下優(yōu)化相關(guān)的目標函數(shù)H(q)。優(yōu)化問題可以表示為

(6)

式(6)中：F(q)=f(q)-x由運動學公式獲得，表示運動學約束函數(shù)。

當目標函數(shù)H(q)取極值時，H(q)的梯度在雅克比矩陣零空間上的投影為零[14]。假定q0為使目標函數(shù)局部最優(yōu)的非奇異位姿，那么存在的關(guān)系為

(7)

(8)

式(8)中：Nr為由In-J?J中線性獨立的行所組成的r×n的矩陣。

將式(8)作為運動學公式的額外約束，那么機械臂運動過程中所要遵循的條件可表示為

(9)

對式(9)兩側(cè)求導(dǎo)，能夠獲得增廣后的微分運動學公式為

(10)

(11)

對于工作空間中任意給定的點，求解式(11)能獲得唯一相對應(yīng)的位姿，這是機械臂運動具備可重復(fù)性的表現(xiàn)。

2 冗余機械臂逆運動學求解算法

利用增廣雅可比矩陣方法求解逆運動學問題，關(guān)鍵是雅可比矩陣增廣部分的計算。本文算法從增廣部分的形式和計算方法兩方面入手，解決機械臂關(guān)節(jié)數(shù)目較多時，增廣部分計算復(fù)雜的問題。

2.1 增廣部分推導(dǎo)

式(6)所表示的優(yōu)化問題可使用拉格朗日乘子法求解[15]。通過構(gòu)建拉格朗日函數(shù)，可以將帶約束的優(yōu)化問題轉(zhuǎn)換為無約束的形式。定義拉格朗日函數(shù)為

L(q,λ)=H(q)+λTF(q)

(12)

式(12)中：λ為m維的拉格朗日乘子向量。

拉格朗日函數(shù)取極值時滿足：

(13)

(14)

根據(jù)式(14)，文獻[15]推導(dǎo)出額外約束公式，可表示為

(15)

定義Ji(i=1,2,…,n)為J的第i列。式(15)中，投影算子Z為(n-m)×n的矩陣，它由J的零空間的n-m個線性獨立的行向量組成，其表達式為

(16)

式(15)形式下的額外約束的導(dǎo)數(shù)矩陣Ca為

(17)

對比式(15)與式(16)中投影算子的形式可以看出，由于涉及雅可比矩陣的偽逆J?，使得Nr計算復(fù)雜。由式(16)可以看出，Z的計算主要與雅可比矩陣的列有關(guān)，利用矢量積方法[16]得出雅可比矩陣后，很容易便能確定所需的列向量。更重要的是，矩陣的逆運算僅涉及形式簡單的單個矩陣，與J?相比，計算量大幅減小。為構(gòu)造增廣雅可比矩陣，需要將額外約束相對于關(guān)節(jié)位置向量的偏導(dǎo)數(shù)作為矩陣的增廣部分。然而，對于實際使用的七自由度冗余機械臂來說，計算Z已經(jīng)足夠復(fù)雜，對矩陣的增廣部分的計算就變得更加困難。

2.2 增廣部分分解計算

為計算Ca的數(shù)值結(jié)果，對式(17)做進一步分解。當n-m=1時，Ca可以表示為

(18)

為了進一步減少計算量，應(yīng)盡可能地避免對目標函數(shù)進行兩次求導(dǎo)運算。實際上，一個簡單且有效的目標函數(shù)能夠提供顯式的導(dǎo)數(shù)表達式，這樣二階導(dǎo)數(shù)值可以僅通過對顯式的導(dǎo)數(shù)表達式進行一次求導(dǎo)而獲得。避關(guān)節(jié)極限準則[17]可以選作目標函數(shù)，其定義為

(19)

式(19)中：qi,min和qi,max分別為第i個關(guān)節(jié)的位置qi的上限和下限。

2.3 算法流程

求解冗余逆運動學問題的算法流程如下。

Step 1利用矢量積方法計算雅可比矩陣J。

Step 4利用Ca對J增廣，構(gòu)造增廣雅可比矩陣Je。

Step 5計算Je的逆矩陣。

3 仿真及實驗驗證

為驗證本文方法的有效性，首先通過仿真與梯度投影法進行對比，然后將其應(yīng)用于如圖1所示的7自由度(degree of freedom, DOF)冗余機械臂的實時控制。機械臂的MDH(modified Denavit-Hartenberg)參數(shù)如表1所示。選用式(19)作為兩種方法中被優(yōu)化的目標函數(shù)，以避免運動過程中機械臂關(guān)節(jié)位置超出限位。機械臂各關(guān)節(jié)的運動范圍如表2所示。

圖1 7自由度冗余機械臂Fig.1 7-DOF redundant manipulator

表1 機械臂MDH參數(shù)

表2 機械臂關(guān)節(jié)運動范圍

3.1 仿真對比

為評估本文算法的性能表現(xiàn)，使機械臂末端姿態(tài)保持不變，從A點開始，按逆時針方向，沿如圖2所示的空間四邊形封閉路徑運動。機械臂的初始位姿q=[-10°,-20°,0°,65°,10°,-50°,35°]T。四邊形路徑的頂點分別為A(507.02,-84.91,234.01)、B(507.02,75.09,234.01)、C(487.02,75.09,394.01)、D(487.02,-84.91,394.01)。該路徑分為AB、BC、CD、DA四段，每段分別使用S曲線[18]進行加減速規(guī)劃，規(guī)劃后機械臂末端的位置、速度變化曲線分別如圖3、圖4所示。

圖2 笛卡爾空間封閉路徑Fig.2 Closed path in Cartesian space

圖3 機械臂末端規(guī)劃位置Fig.3 Planned position of the end-effector of the manipulator

圖4 機械臂末端規(guī)劃速度Fig.4 Planned velocity of the end-effector of the manipulator

整個運動過程中，機械臂各關(guān)節(jié)的位置變化曲線如圖5所示?？梢钥闯?，曲線光滑連續(xù)，關(guān)節(jié)位置都在允許的范圍內(nèi)。在相同的起始位姿下，利用梯度投影法跟蹤同樣的空間封閉四邊形軌跡。如圖6所示，關(guān)節(jié)位置同樣沒有超出限位。兩種方法都利用運動學冗余對目標函數(shù)實現(xiàn)了很好的優(yōu)化。

圖5 本文方法獲得的關(guān)節(jié)位置Fig.5 Joint position obtained by the proposed method

圖6 梯度投影法獲得的關(guān)節(jié)位置Fig.6 Joint position obtained by the gradient projection method

利用兩種方法跟蹤封閉軌跡后，各個關(guān)節(jié)始末位置的對比如表3所示。在機械臂末端回到起始位置時，本文方法使機械臂的位姿與初始位姿相同，這使得運動具有可重復(fù)性，而使用梯度投影法(gradient projection method，GPM)時，關(guān)節(jié)的最終位置相對于初始位置發(fā)生了偏移。

表3 機械臂關(guān)節(jié)始末位置對比

逆運動學問題求解的準確性影響到機械臂跟蹤目標軌跡時的精度，將兩種方法下的機械臂末端軌跡與期望軌跡進行對比，得到如圖7所示的位置誤差。本文方法對軌跡實現(xiàn)了準確的跟蹤，梯度投影法Z軸方向的誤差相對較大，X軸和Y軸方向的精度較高，但誤差仍略大于本文方法。本文方法相對于梯度投影法，在軌跡跟蹤過程逆運動學的精確求解上具有很好的表現(xiàn)。

圖7 機械臂末端軌跡誤差Fig.7 Trajectory error of the end-effector of the manipulator

3.2 軌跡跟蹤實驗

本文算法由C++實現(xiàn)，控制程序運行在配置為Intel(R) Core(TM) i5-3437U 1.90 GHz CPU、4 GB內(nèi)存的PC上，由實時拓展套件KRTS(kithara realtime suite)提供PC的實時操作系統(tǒng)。實驗系統(tǒng)的通信采用自主研發(fā)的EtherMAC(ethernet for manufacture automation control)[19]總線，通過網(wǎng)關(guān)與支持RS-485通信的機械臂連接，通信周期為8 ms。利用本文算法進行10 000次逆解運算，平均計算時間為176 μs，求解速度能夠滿足實時控制的要求。

如圖8所示，使機械臂執(zhí)行仿真時所采用的末端軌跡。循環(huán)運動3次，機械臂的關(guān)節(jié)位置、速度分別如圖9、圖10所示?？梢钥闯?，針對七自由度冗余機械臂，本文方法可保證逆運動學求解的準確性，且關(guān)節(jié)位置、速度曲線平滑。同時，關(guān)節(jié)位置變化具有周期性，表明本文方法可保證機械臂運動的可重復(fù)性。

圖8 機械臂跟蹤規(guī)劃軌跡Fig.8 Manipulator tracking the planned trajectory

圖9 機械臂實際關(guān)節(jié)位置Fig.9 Actual joint position of the manipulator

圖10 機械臂實際關(guān)節(jié)速度Fig.10 Actual joint velocity of the manipulator

4 結(jié)論

提出了一種利用增廣雅可比矩陣的冗余機械臂逆運動學求解算法。將優(yōu)化給定的目標函數(shù)作為約束任務(wù)對機械臂的任務(wù)空間進行增廣，在此基礎(chǔ)上構(gòu)造了增廣雅克比矩陣。針對機械臂關(guān)節(jié)數(shù)目較多時，增廣雅可比矩陣難以計算的問題，通過對其中的增廣部分采用分解后計算的方式實現(xiàn)了增廣雅可比矩陣的有效求解。仿真結(jié)果表明，相比于梯度投影法，本文方法具更高的求解精度，且能夠保證機械臂追蹤封閉軌跡時運動的可重復(fù)性。算法平均求解時間為176 μs，計算效率滿足實時控制的要求。通過七自由度冗余機械臂軌跡跟蹤實驗，驗證了本文方法的可行性和穩(wěn)定性。