基于唯一域方法的機器人逆向運動學求解

李 光 肖 帆 楊加超 章曉峰 馬祺杰

(湖南工業大學機械工程學院， 株洲 412007)

0 引言

機器人逆運動學求解是機器人離線編程、軌跡規劃、控制算法設計等其他課題研究的基礎[1]。逆運動學求解的實質是完成機器人工作空間到關節空間的映射，逆運動學方程組具有高維、非線性的特點，求解復雜且不易求出[2]。很多學者在該領域做了大量研究，提出了許多理論與方法。傳統方法有代數法[3]、幾何法[4]和數值法[5]等。代數法主要以消元的方式，將機器人位置反解中的高維方程組簡化為低維方程組，從而求得所有逆運動學解。該方法需要進行大量的三角代換，簡化過程十分復雜，HUSTY等[6]認為求解非線性代數方程往往需要靠直覺甚至運氣才能獲得。幾何法針對機器人的某些特殊結構進行簡化，再進行求解，一般無法單獨使用，甚至根本無法使用[7]。數值法的求解速度與初始值相關。

隨著計算機技術的快速發展，許多學者將智能優化方法應用于機器人逆運動學問題[8-11]。文獻[8-11]的方法均屬于進化算法的范疇，其迭代過程中不受梯度和初始值的影響，具有通用性，但無法有效求得機器人所有逆運動學解。文獻[12-13]將關節空間劃分為多個子空間，采用多神經網絡的方式，分別求得了平面2R機械手和空間3R機械手的逆運動學多解，但是均未提出一種劃分關節空間的通用方式。

RASTEGAR等[14]提出利用det(J)=0(J為雅可比矩陣)方程，將非冗余機器人關節空間劃分為與逆解數目相同的唯一域，再在每個唯一域中用數值法迭代求解，進而求得所有逆運動學解。其思路可行，因MANOCHA等[15]舉例說明在某種情況下6自由度機器人的逆解問題有16個實數解，并確定解的數目上限為16；WENGER[16-17]進一步完善了利用det(J)=0劃分唯一域的條件。

在det(J)=0劃分唯一域的理論指導下，MOULIANITIS等[18]使用48個多層感知器近似求解了KUKA 7自由度仿人臂前6個關節的逆運動學多解(文中所提的KUKA機器人為該款機器人)；周楓林等[19]用多模塊RBF神經網絡求解了PUMA560機械手前3個關節的逆運動學多解。然而，神經網絡需要大量的樣本進行訓練，無法保證求解的實時性；同時，樣本的數量和分布會影響訓練好的神經網絡的泛化精度，并且位于奇異點附近的樣本，神經網絡無法直接訓練。

本文將方程det(J)=0作為劃分6自由度機器人關節空間的依據，將各個唯一域的邊界作為約束條件，采用協方差自適應矩陣進化策略(Covariance matrix adaptation evolution stategy,CMA-ES)[20]對6自由度機器人進行有約束尋優；使用佳點集方法產生初始搜索點，從而求得所有逆運動學解。在錢江一號6R工業機器人和KUKA仿人機械臂上進行仿真實驗。

1 6R機器人唯一域的確定

文獻[17]將非冗余機器人分為了尖面機器人(Cuspidal robot)和非尖面機器人(Noncuspidal robot)，在非尖面機器人中可以直接采用det(J)=0方程劃分唯一域。圖1為非尖面6自由度機器人劃分唯一域求逆解的方法流程圖，Qn為唯一域中的逆運動學解，其中n≤16。

圖1 非尖面6R機器人逆運動學求解流程圖Fig.1 Flow chart for solving inverse kinematics of noncuspidal 6-DOF robot

由于非尖面6自由度機器人的結構不同，最終劃分唯一域的方法也會有所差異。為了能詳細地闡述本文所提的方法，以工業6R機器人為例進行詳細說明。

1.1 6R工業機器人正向運動學及適應度函數

圖2是錢江一號6R工業機器人[21]以D-H法建立的連桿坐標系，表1是其D-H參數,其中a1=150 mm，a2=550 mm，a3=160 mm，d4=594 mm，機器人零位是關節3以括號內的角度旋轉后所得。

圖2 錢江一號機器人連桿坐標系Fig.2 Linkage coordinate system of Qianjiang No.1 robot

機器人連桿{i}坐標系相對于{i-1}坐標系的變換矩陣為

(1)

表1 錢江一號機器人的關節參數Tab.1 Joint parameters of Qianjiang No.1 robot

其中，sθi、cθi、sαi-1和cαi-1為sinθi、cosθi、sinαi-1和cosαi-1的簡寫形式。已知機器人的關節向量θ=(θ1,θ2,θ3,θ4,θ5,θ6)，根據式(1)和表1中的D-H參數，得到機器人腕部相對于基坐標系的位姿矩陣

(2)

機器人手腕的位置由前3個關節可以完全確定，其姿態最終由后3個關節確定，所以采用臂腕分離的方式建立適應度函數

fitness1=‖pdes-pcur‖

(3)

fitness2=‖ades-acur‖

(4)

式中fitness1——目標位置pdes與實際位置pcur之間的誤差

fitness2——目標接近矢量ades與實際接近矢量acur的誤差

其中接近矢量與θ6無關，如需要求解的機器人不能臂腕分離求解，則適應度函數可參照文獻[7]，用位置誤差和姿態誤差加權和的形式表達。由于姿態是建立在前3個關節基礎上得到的，所以可以先用式(3)求解θ1、θ2、θ3，然后代入式(4)求θ4、θ5。θ6可將求得的前5個關節角作為已知條件，聯立oz、nz求解，即

(5)

其中

A=s23sinθ5-c23cosθ4cosθ5

B=c23sinθ4

式中c23、s23分別表示cos(θ2+θ3)、sin(θ2+θ3)。

1.2 唯一域劃分

通過微分法[22]在Matlab中計算，求得機器人的雅可比矩陣行列式為

det(J)=Pn1Pn2Pn3

(6)

其中

Pn1=a2sinθ5

Pn2=a3cosθ3-d4sinθ3
Pn3=a1+d4c23+a3s23+a2cosθ2

由式(6)可以看出，工業機器人的奇異性只與關節2、3、5有關。

Pn1和Pn2分別只與θ3、θ5有關，可直接求出θ3、θ5的值作為邊界：θ3的邊界分別為-2.940 8 rad、0、3.342 4 rad；θ5的邊界分別為-π、0、π。Pn3不方便求解，可直接作為非線性約束條件。根據1.1節中適應度函數的構造，θ2和θ3所劃分的唯一域只與fitness1有關，令f=Pn2Pn3，可在θ2和θ3組成的平面內進一步研究f=0時唯一域的劃分。

圖3中綠色線為θ2和θ3平面內的奇異軌跡線，在上半區θ3∈[0.200 8,3.342 4] rad，Pn3≤0的部分組成了UD1，Pn3≥0的部分組成了UD3；在下半區θ3∈[-2.940 8,0.200 8] rad，Pn3≤0的部分組成了UD2，Pn3≥0的部分組成了UD4。如果沒有限制θ2，則沿θ2軸的方向上，每個唯一域都會有無數種解，因為根據三角函數知識，θ2與2kπ+θ2(k∈Z)對應的三角函數值相等，所以選擇唯一域時，可以根據圖3適當調整θ2的邊界，使得邊界內包含一個完整的UDi(i=1,2,3,4)即可，如圖4紅色區域，圖中各區域的邊界條件見表2。

圖3 唯一域劃分Fig.3 Uniqueness domains division

圖4 θ2和θ3間的唯一域劃分Fig.4 Uniqueness domains partition between θ2 and θ3

又因為θ5將[-π,π]以0為界劃分成了2個唯一域，與UDi(i=1,2,3,4)可以組合得到8個唯一域，記為UDij(j=1,2)，如圖5所示，圖中“+”表示組合。

2 基于CMA-ES算法的逆運動學求解

2.1 CMA-ES算法原理

CMA-ES算法的本質屬于進化策略類算法。經典進化策略算法主要依賴于突變來尋找最優解，而突變方向需要依據經驗設置，以該方法調整會導致無效的突變，進而浪費計算成本。為克服經典進化策略的局限性，CMA-ES采用多維正態分布N(m，C)產生搜索種群，m代表種群分布的中心；C是協方差矩陣且對稱正定，對其特征分解可得C=BDBT，其中BBT=I，B的列向量由C的特征向量正交基組成，D為對角陣，對角元素是C矩陣特征值的平方根。

表2 各唯一域確定條件Tab.2 Uniqueness domains determination conditions

圖5 8組唯一域組合方式Fig.5 Eight groups of uniqueness domains combinations

CMA-ES算法實現步驟如下[23-25]：

(1)參數設置及初始化。靜態參數:種群大小λ，父代個體數為μ<λ，重組權值ωi(i=1,2，…,μ)，以及自適應調整時所需的常量cσ、dσ、cc、c1、cμ、μeff，最大迭代次數為G。動態參數：初始分布均值m∈RN(N是問題維數)，全局步長σ∈R+，進化路徑pσ=0，pc=0，協方差矩陣C=I，迭代次數g=0。靜態參數的計算公式見文獻[25]。

(2)抽樣。通過對多元正態分布進行抽樣生成搜索種群，抽樣公式為

yk=BDN(0,I)～N(0,C)

(7)

xk=m+σyk～N(m,σ2C)

(8)

式中xk——種群的第k個個體

yk——均值為0的多元正態分布

(3)優選重組，即均值更新，具體計算公式為

(9)

(10)

其中

式中xi:λ——種群排名第i的個體

對于最小化問題，其排名由目標函數從小到大的排序決定。

(4)步長控制。首先更新步長進化路徑pσ，即

(11)

式中μeff——方差有效選擇質量，且1≤μeff≤μ

步長σ的更新公式為

(12)

式中dσ——接近1的阻尼系數

E‖N(0,I)‖——正態分布隨機向量范數的期望

(5)協方差矩陣調整。更新進化路徑

(13)

式中hσ是Heaviside函數，可以在‖pσ‖較大時使pc更新停頓，從而避免協方差矩陣C在線性環境中軸線過快增長。協方差矩陣的調整公式為

(14)

其中

δ(hσ)=(1-hσ)cc(2-cc)

式中c1——C的秩為1的更新學習率

cμ——C的秩為μ的更新學習率

(6)終止判斷。若g

步驟(5)中，協方差矩陣C結合了秩μ、秩1更新率，前者可以充分利用當前代的信息，后者利用了代與代之間的信息，二者的有機結合，避免了算法求解精度上對種群大小的過分依賴，以及進化過程中種群“早熟”的問題；同時引入進化路徑來引導種群的尋優搜索，提高了算法的尋優效率和可靠性。C的更新機制，使得算法在尋優過程中具有確定性。

圖6為CMA-ES算法的進化過程,其中“★”表示種群的均值，“●○”表示種群中所有個體，“●”表示種群中前μ個優秀個體，“▲”表示最優值，虛線為種群分布形狀。可以看出種群的進化過程隨著優秀個體的引導接近最優值。

圖6 CMA-ES進化過程簡示圖Fig.6 Simplified diagram of evolution process of CMA-ES

2.2 初始均值確定

原始的CMA-ES算法初始均值點隨機產生，從第1節唯一域的劃分可以獲知，隨機產生的均值點，可能不在需要求解的唯一域內。同時在每個唯一域內適應度函數都是單峰函數，如果均值點在每個要求解的唯一域內，并且接近峰值，則可以大大地縮小算法的搜索時間。實現初始均值點的產生步驟如下：

(1)采用佳點集[26]方法獲得分布于唯一域邊界條件內的均勻點集，記為Pb。

(2)如果求fitness1，則將Pb中的θ2和θ3代入第1節中的Pn3，根據Pn3大于(或小于)0篩選符合唯一域內的點集，并記為PU；如果是求fitness2，則跳過此步驟。

(3)如果求fitness1，將PU代入適應度函數，選擇其中使得適應度最小的點作為初始均值點；如果求fitness2，則將本步驟中方法的PU改為Pb。

佳點集計算公式[27]為

rk=ek(1≤k≤t)

(15)

Pbi(k)={{r1i},{r2i},…,{rti},i=1,2,…,n}

(16)

式中t——維數

{rki}——rki的小數部分

Pbi(k)中每個維度都在[0,1]內，可以映射至搜索空間，即

(17)

2.3 逆解處理

從表2可看出，θ2和θ3不全部位于[-π,π]之間，可對求得的逆運動學解進行轉換：如θi>π，則θi=2π-θi；如θi<-π，則θi=2π+θi；否則θi不變,i=1,2,…,6。

3 仿真算例

將進行兩個仿真算例，算例1求本文所提的工業機器人的8組逆解；算例2求文獻[18]中仿人臂前6個關節組成的6R機械臂的8組逆解。每個案例中都將用本文所提方法與文獻[5]所提的數值法進行比較。

用于仿真的計算機配置如下：操作系統為64位Windows 7專業版，處理器為IntelCore i3-6100，CPU速度為3.70 GHz，安裝內存為8.00 GB；仿真計算軟件為Matlab 2016a。

3.1 算例1

先在工業機器人唯一域UD31中，利用式(3)與CMA-ES算法分別對奇異點和非奇異點求逆解；然后將求得的非奇異位置作為已知條件，用式(4)與CMA-ES算法求解θ4和θ5；最后求出所有唯一域中的逆運動學解。數值法采用CMA-ES算法的初始值進行迭代，直接求UD31非奇異位置的6個關節角。

3.1.1參數設置

θ1、θ4、θ6的取值范圍均設為[-π,π]。佳點集的點數n設為800，CMA-ES算法中的種群數λ=100，優秀個體數μ=50，初始步長σ=0.1，終止條件為G=100，或者fitness1(fitness2)<10-5mm；數值法的步長α=0.95，終止條件G=100，或者位姿誤差和Eerr<10-5mm。

3.1.2UD31中仿真結果

分別取非奇異位置的關節角Q1=[1,-1.2,2.2,0.5,-1,2]rad和奇異位置關節角Q2=[1,-2.465 226 450 132 348,1.564 034 453 319 104,0.5,-1,2]rad，代入正向運動學式(2)，得到位姿T1、T2，然后在唯一域條件下，分別用CMA-ES算法對T1、T2的位置單獨求解1 000次。

圖7和表3是CMA-ES算法對位置求解的結果，可以看出CMA-ES算法在奇異位置和非奇異位置上的求解誤差和速度總體相差不大，適應度函數幾乎呈線性收斂。

圖7 UD31中CMA-ES單次求解進化過程Fig.7 Single-time evolution of CMA-ES in UD31

圖8和表4為求出的T1前3個關節角作為已知條件求解UD31的θ4和θ5，由于只搜索兩個關節角，所以算法收斂迅速，求解速度比位置求解快將近一

表3 UD31中CMA-ES獨立運行1 000次的fitness1結果Tab.3 fitness1 results for CMA-ES running independently 1 000 times in UD31

圖8 CMA-ES單次求解θ4和θ5進化過程Fig.8 Single solution of θ4 and θ5 evolutionary processes by CMA-ES

fitness2最小值/mm最大值/mm平均值/mm標準差/mm求解速度/(s·次-1)非奇異位置2.0016×10-79.9887×10-65.4552×10-62.4993×10-60.0018

倍。θ6是代數公式求解，其速度為10-4ms/次，可以忽略不計。

圖9是數值法求T1逆解的迭代過程圖，從圖中可以看出，收斂速度很快，并且每次求解都收斂于7.08×10-7，獨立運行1 000次測出的求解速度為7.5 ms/次。

圖9 數值法迭代過程Fig.9 Numerical method iterative process

從數據對比可以看出，在6R工業機器人逆運動學求解中，CMA-ES算法的求解穩定性與精度比數值法稍微遜色，但求解時間優于數值法，CMA-ES算法求一組逆運動學平均求解時間約5.1 ms/次。

3.2 算例2

對文獻[18]中KUKA機器人的前6個關節求逆運動學多解。通過該機器人的雅可比奇異方程，可以直接求出θ2、θ4和θ5(表5)，所以唯一域可以直接由邊界的方式劃分，劃分方法參照第1節。CMA-ES的適應度函數為位置與姿態的加權和[7]，位置誤差的加權系數為0.004，姿態誤差的加權系數為1，θ1、θ3和θ6的取值范圍均為[-π,π]rad。將Q3=[0.5,π/4,0.25,-π/4,π/4,1]rad代入式(2)得到目標位姿T3。

在唯一域UD1中對CMA-ES和數值法進行求解對比，結果見表6。

表5 KUKA機器人的唯一域Tab.5 Uniqueness domains of KUKA robot rad

從表6可以看出，在KUKA機器人逆運動學求解中，CMA-ES算法的穩定性與精度同樣稍遜于數值法，求解速度卻快了將近3倍。表7、8為兩款機器人的8組逆解，各關節角均轉換至[-π,π]。

表6 UD1中CMA-ES獨立運行1 000次的絕對位置誤差Tab.6 Absolute position error for CMA-ES running independently 1 000 times in UD1

表7 工業機器人的8組逆運動學解Tab.7 Eight solutions of inverse kinematics of industry robot rad

表8 KUKA機器人的8組逆運動學解Tab.8 Eight solutions of inverse kinematics of KUKA robot rad

3.3 結果分析

從兩個算例可以看出，在滿足精度要求的前提下，CMA-ES算法的求解速度明顯優于文獻[5]所提的數值法。

KUKA機器人中的逆運動學求解速度比工業機器人求解速度慢，主要因為該機器人無法臂腕分離，從而增加了算法的搜索維度，進而增加了求解過程中的時間消耗。經測算，CMA-ES算法中68%～74%的時間消耗用于適應度函數計算，如果可以加快該算法適應度函數的計算，則其求解速度將會得到顯著提高。同時，CMA-ES算法的原理導致其進化過程中，會向不可行域探索，從而在進化中產生無效搜索，如果在這一方面也進行改進，則求解速度將會有所提高。

4 結論

(1)利用det(J)=0及圖像可視化方法，將錢江一號6R工業機器人的關節空間劃分為8個唯一域，縮小了CMA-ES算法的搜索空間；結合其臂腕分離的特點，分別對手腕位置和接近矢量建立了適應度函數，降低了算法的搜索維度。

(2)利用CMA-ES算法在唯一域中進行有約束尋優；采用佳點集算法均勻分布的特點，優化了算法的初始均值點；應用同樣的逆解唯一域劃分方法和算法，求解了一類KUKA仿人機械臂前6個關節的8組逆解。

(3)在滿足精度要求的前提下，與數值法相比，本文提出的算法平均求解時間更短：在工業機器人中，CMA-ES算法平均求解速度約為5.1 ms/次，數值法平均求解速度約為7.5 ms/次；在KUKA機器人中，CMA-ES算法平均求解速度約為18.9 ms/次，數值法平均求解速度約為54.8 ms/次。CMA-ES算法在兩款機器人中逆解的位置精度均能穩定在10-6mm數量級。