多約束條件下六自由度機械臂最優(yōu)時間軌跡規(guī)劃

馬騰宇，胡孝楠，郭新華

(武漢理工大學 機電工程學院，湖北 武漢 430070)

時間最優(yōu)軌跡規(guī)劃是提高空間機械臂效率的關(guān)鍵[1]，空間機械臂的軌跡規(guī)劃和控制算法，為機械臂的平穩(wěn)運行和工作效率的提高提供保障[2]。在空間機械臂的工業(yè)應(yīng)用中，時間最優(yōu)性對于生產(chǎn)效率的最大化是至關(guān)重要的[3]，時間作為軌跡規(guī)劃的重要指標，可以直接反映空間機械臂的效率[4]。此外，機械臂在運動過程中還應(yīng)滿足各關(guān)節(jié)的多重動力學約束。因此，在滿足運動約束的條件下，減少機械臂的工作時間是解決該問題的關(guān)鍵。

針對這一問題，Papadopoulos等[5]提出了一種利用多項式函數(shù)來跟蹤關(guān)節(jié)軌跡的方法，使機械臂的運動軌跡更加平滑。Saravanan等[6]利用三次樣條函數(shù)來描述關(guān)節(jié)軌跡，并用差分進化算法對問題進行了優(yōu)化。Wei等[7]采用遺傳算法求解最優(yōu)軌跡，然而，遺傳算法的迭代過程非常復雜，需要在找到最優(yōu)解后進行編碼和解碼處理。Chen等[8]利用和聲搜索算法尋找最優(yōu)解，但在重要參數(shù)值和聲記憶庫取值概率以及微調(diào)概率的選擇上需要進行改進。粒子群算法也常用于機械臂的軌跡優(yōu)化問題。Jin等[9]利用標準粒子群算法求解冗余度機械臂逆運動學問題。Akbarimajd[10]采用自適應(yīng)粒子群算法對三自由度機械臂的運動時間進行優(yōu)化，并通過在機械系統(tǒng)動力學自動分析的仿真中驗證了算法的有效性。Kim等[11]提出了一種利用標準化步長來初始化粒子群的方法。Wang等[12]使用粒子群算法跟蹤自由漂浮的空間機器人的軌跡。然而，標準粒子群算法存在早熟收斂的缺點，容易收斂到局部極值。

筆者提出了一種時間最優(yōu)的空間機械臂軌跡規(guī)劃方法。首先，利用D-H參數(shù)法建立六自由度機械臂運動學模型，其次，根據(jù)逆運動學的方法將笛卡爾坐標系中的軌跡點轉(zhuǎn)化為關(guān)節(jié)空間中的軌跡點，利用3-5-3樣條函數(shù)表示關(guān)節(jié)角軌跡，然后根據(jù)運動約束生成各關(guān)節(jié)的適應(yīng)度函數(shù)，最后，采用改進的粒子群算法對每個關(guān)節(jié)的運動時間進行優(yōu)化，從而得到最優(yōu)解。

1 六自由度機械臂建模

為了使優(yōu)化過程更加直觀，以一個具有6個旋轉(zhuǎn)自由度的剛性機械臂為研究對象。前3個關(guān)節(jié)確定機械臂末端執(zhí)行器的位置，后三個關(guān)節(jié)確定末端姿態(tài)。采用D-H參數(shù)法建立運動學模型，D-H參數(shù)法包含4個參數(shù)：θ為關(guān)節(jié)變量，d為連桿長度，α為關(guān)節(jié)扭轉(zhuǎn)角，a為關(guān)節(jié)偏移量。整個機械臂系統(tǒng)的關(guān)節(jié)坐標系如圖1所示。

機械臂的D-H參數(shù)如表1所示。六自由度機械臂各關(guān)節(jié)的轉(zhuǎn)動慣量如表2所示。

圖1 六自由度機械臂關(guān)節(jié)坐標系

表1 D-H 參數(shù)表

表2 各關(guān)節(jié)轉(zhuǎn)動慣量

圖2 六自由度機械臂簡易模型

基于表1中的特征參數(shù)，建立機械臂簡化模型，如圖2所示，每個關(guān)節(jié)的初始角度設(shè)置為0度。機械臂的結(jié)構(gòu)圖如圖3所示。文中默認機械臂處于理想狀態(tài)，對末端執(zhí)行器和傳感器的失效以及外界干擾具有免疫性。

圖3 六自由度機械臂結(jié)構(gòu)圖

2 軌跡規(guī)劃問題的描述

一般情況下，根據(jù)預設(shè)的任務(wù)要求，可以得到關(guān)節(jié)空間中的一系列路徑點：θ={θsr|s=1,2,…，m;r=1,2,…，n}，其中θsr為關(guān)節(jié)s第r個路徑點的位置，m為關(guān)節(jié)總數(shù)，n為路徑點的總數(shù)。在路徑點的生成過程中，機械臂的奇點尤為重要，六自由度機械臂在運動過程中遇到腕部奇點、肩部奇點和肘部奇點時，都會發(fā)生卡滯現(xiàn)象，因此在生成路徑時要避免這些奇點的出現(xiàn)。

在得到每個關(guān)節(jié)的路徑點后，需要對關(guān)節(jié)角度進行參數(shù)化處理，筆者采用3-5-3樣條函數(shù)對關(guān)節(jié)角進行描述。為了簡化計算過程，對關(guān)節(jié)的運動時間進行了歸一化處理，并引入一個表示無量綱時間的參數(shù)t[13]：

(1)

式中：j為時間維度，j=1，2，3；τ為軌跡的實際時間，τ∈[τj-1，τj]，τj-1和τj分別為第j段軌跡的開始時間和結(jié)束時間，每段軌跡的方程如下[13]：

(2)

其中θ1(t),θ2(t),θ3(t)為三段多項式軌跡函數(shù)。機械臂在起始點的關(guān)節(jié)角、速度和加速度分別表示為θ0,v0,a0；終點的值分別表示為θf,vf,af。方程中的系數(shù)可由軌跡的連續(xù)性和中間點的位置、速度和加速度的連續(xù)性得到：

(3)

a13=Δθ1-a11-a12

(4)

(5)

(6)

根據(jù)式(3)～式(6)可以得到b1～b3和a23～a25的值：

(7)

(8)

在得到所有未知參數(shù)后，可以得到每個關(guān)節(jié)的運動軌跡。

在機械臂的實際工作過程中，從起點到終點的運動時間應(yīng)盡可能短。在保證最優(yōu)時間的同時，還要考慮各關(guān)節(jié)的運動約束，使運動軌跡更加平滑。綜合考慮最優(yōu)時間和多重約束的影響，適應(yīng)度函數(shù)的定義如下：

(9)

(10)

經(jīng)過上述步驟，將問題轉(zhuǎn)化為多約束條件下求解適應(yīng)度函數(shù)的最小值問題。當機械臂不能滿足約束條件時，可將適應(yīng)度函數(shù)值設(shè)為最大值fitnessmax。

3 軌跡優(yōu)化算法

為了克服標準粒子群算法容易收斂到局部最優(yōu)解的問題，對標準算法進行了改進。改進算法的特點如下：①利用混沌序列增加粒子群的多樣性，增加多樣性參數(shù)來檢測粒子的離散狀態(tài)，使得粒子群在混沌和穩(wěn)定性之間達到平衡。②如果每一個粒子的當前最優(yōu)解經(jīng)過5次迭代后都沒有改變，則增加粒子的慣性權(quán)重，使其能夠及時逃離局部極值。③利用當前適應(yīng)度函數(shù)和迭代次數(shù)的值對慣性權(quán)重ω進行自適應(yīng)調(diào)整，增加全局搜索和局部搜索的協(xié)調(diào)性。改進算法流程如圖4所示。

圖4 算法流程圖

改進算法的具體步驟如下：

步驟1設(shè)置參數(shù)T,ω,c1,c2,D的值，T為迭代的最大代數(shù)，ω為慣性權(quán)重，c1,c2為學習因子，D為維度。

步驟2利用混沌序列對粒子位置和速度進行初始化。粒子的初始值由帳篷映射生成，然后映射到解空間，通過式(9)計算適應(yīng)度函數(shù)，選擇最佳的一部分粒子作為初始粒子群。

步驟3選擇適應(yīng)度函數(shù)最小的粒子作為全局最優(yōu)粒子。

步驟4粒子的值由式(11)和式(12)更新[14]：

νij(k+1)=ωνij(k)+c1r1(pij(k)-xij(k))+

c2r2(gj(k)-xij(k))

(11)

xij(k+1)=xij(k)+νij(k+1)

(12)

式中：i為粒子序數(shù)；k為當前迭代步長；νij為速度值；xij為位置值；pij為個體最優(yōu)極值；gj為全局最優(yōu)極值；r1和r2為0到1之間的隨機數(shù)。慣性權(quán)重ω通過式(13)進行更新：

(13)

學習因子c1和c2分別體現(xiàn)了自我探索和群體學習能力。在優(yōu)化算法的初始階段，粒子應(yīng)該具有較強的自我探索能力和較弱的群體學習能力，以增強粒子的整體搜索能力；在后期的搜索中，粒子應(yīng)具有較弱的自我探索能力和較強的群體學習能力，以保證粒子趨向全局極值。學習因子根據(jù)式(14)進行更新：

(14)

式中：c1max、c2max為學習因子的最大值；c1min、c2min為學習因子的最小值。

如果迭代后粒子的位置和速度超過預設(shè)值，則以區(qū)間的上下限作為粒子的位置和速度值。

步驟5適應(yīng)度函數(shù)的方差按式(15)計算：

(15)

式中：N為粒子總數(shù)。

M為種群多樣性參數(shù)，用來衡量粒子群的分散程度，M是一個常數(shù)，根據(jù)不同的情況分配。如果σ2>M，說明粒子群的多樣性較高，則跳到步驟7；如果σ2

步驟6全局最優(yōu)粒子g=(g1,g2,…,gj)按式(16)映射到解空間[15]：

(16)

其中aj和bj是維度j的邊界值。以zj 0為初始狀態(tài)，利用帳篷映射生成新的混沌粒子z′= [z′1,z′2,…,z′h]，將新的混沌序列映射到解空間，得到新粒子的位置g′=[g′1,g′2,…,g′h]。

計算新的h個粒子的適應(yīng)度函數(shù)值，選擇最佳粒子g′best，用最小適應(yīng)度函數(shù)值的粒子替換原粒子群。

步驟7判斷每個粒子的個體最優(yōu)極值，如果極值在5次迭代中沒有變化，則根據(jù)式(17)重新計算粒子的慣性系數(shù)：

(17)

粒子在迭代開始時不容易收斂到局部極值，隨著迭代次數(shù)來改變ω的值，使其在迭代后期增加慣性權(quán)重，及時逃離局部極值。

如果算法達到迭代次數(shù)的最大值，則迭代結(jié)束，否則跳到步驟4。

4 仿真結(jié)果分析

以六自由度機械臂為研究模型，為了評價改進粒子群優(yōu)化算法的有效性，將其與標準粒子群優(yōu)化算法[16]和遺傳算法[17]進行對比，每種算法迭代100次，實驗參數(shù)設(shè)置如下：

機械臂關(guān)節(jié)速度約束為[-40 40](deg/s)，加速度約束為[-60 60](deg/s2)，標準粒子群算法的參數(shù)設(shè)置為N=20,ωmin=0.4,ωmax=0.9,D=3,T=100。

改進粒子群算法的參數(shù)設(shè)置為N=20,ωmin=0.4,ωmax=0.9,c1=1.49,c2=1.49,D=3,T=100。遺傳算法采用二進制編碼，每一代種群通過輪盤賭選擇法進行更新，交叉概率Pc和變異概率Pm分別設(shè)置為0.6和0.001。

關(guān)節(jié)空間的路徑點如表3所示。

表3 關(guān)節(jié)路徑點

在多約束條件下，仿真以最短時間為目標進行模擬。利用改進的粒子群算法求解，得到6個關(guān)節(jié)的運動時間，并利用每個維度的最大值來形成最優(yōu)時間：fitness=[1.96 2.92 2.45]。以第一關(guān)節(jié)的運動時間為例，將改進的粒子群算法與經(jīng)典的粒子群算法和遺傳算法進行了比較，結(jié)果如表4所示。

從表4可知，粒子群算法的時間比遺傳算法縮短了很多，主要是因為遺傳算法的編碼解碼過

表4 仿真結(jié)果

程比較復雜，導致尋找最優(yōu)解的速度降低。這3種算法的優(yōu)化過程如圖5所示。結(jié)果表明，改進后的粒子群算法比標準算法收斂速度快，最優(yōu)解也優(yōu)于標準粒子群算法和遺傳算法，經(jīng)過30次迭代后，標準粒子群算法陷入局部極值，最終的最優(yōu)解遠低于改進的粒子群算法。

圖5 算法迭代過程

圖6 6個關(guān)節(jié)的角度、速度和加速度比較

6個關(guān)節(jié)的角度、速度和加速度的曲線如圖6所示，橫坐標為機械臂的運動時間，縱坐標為關(guān)節(jié)的角度、速度和加速度的數(shù)值。從圖6可知，關(guān)節(jié)角的運動曲線是非常平滑的，最大速度不超過35 deg/s，最大加速度不超過50 deg/s2，因此機械臂的運動在預設(shè)的約束值之內(nèi)。初始速度和加速度為0，保證了關(guān)節(jié)的初始扭矩為0，從而減小了機械臂的損耗。

5 結(jié)論

筆者提出了一種多約束條件下六自由度機械臂最優(yōu)時間軌跡規(guī)劃方法。針對標準粒子群算法容易早熟收斂以及求解不精確的缺點，提出了一種改進的粒子群算法。通過建模和推導，將軌跡規(guī)劃問題轉(zhuǎn)化為用改進粒子群算法求解全局最優(yōu)解的問題。將該算法與標準粒子群算法和遺傳算法進行了比較，仿真結(jié)果表明，所提出的優(yōu)化算法更容易、更快地找到最優(yōu)解，大大降低了早熟收斂的概率，加快了收斂速度，改進的粒子群算法是一種有效的時間最優(yōu)軌跡規(guī)劃算法。