歐 新，周 璐，張 鷙，吳月家，趙云斌

貴州電網(wǎng)有限責(zé)任公司 貴陽供電局，貴陽 550002

隨著機(jī)器人技術(shù)的發(fā)展和工業(yè)自動化的推進(jìn)，串聯(lián)機(jī)械臂被應(yīng)用于越來越多的場景中，時間最優(yōu)[1-4]軌跡規(guī)劃能夠有效縮短機(jī)械臂運(yùn)動時間，提高工作效率，具有十分重要的研究意義。目前主流的時間最優(yōu)軌跡規(guī)劃方法主要分為兩個階段，首先利用軌跡插值算法對機(jī)械臂運(yùn)動路徑進(jìn)行擬合；其次通過優(yōu)化算法對機(jī)械臂在擬合路徑上的運(yùn)動時間進(jìn)行優(yōu)化。

較為常用的機(jī)械臂軌跡插值算法有樣條插值和多項式插值兩種，其中三次樣條插值、B樣條插值等樣條插值算法通過部分已知路徑點擬合完整路徑，適用于要求復(fù)雜的連續(xù)運(yùn)動任務(wù)[5-6]；三次多項式插值、五次多項式插值等多項式插值算法由機(jī)械臂首末狀態(tài)決定運(yùn)動路徑，適用于點到點運(yùn)動[7]。在實際應(yīng)用場景中點到點運(yùn)動或簡單的連續(xù)運(yùn)動更為常見，并且簡單的連續(xù)運(yùn)動可以離散為幾個點到點運(yùn)動，所以機(jī)械臂點到點運(yùn)動軌跡規(guī)劃的研究更具廣泛適應(yīng)性[8]。為了保證關(guān)節(jié)首末角位移、角速度及角加速度均可控，文中選擇五次多項式插值算法對機(jī)械臂的運(yùn)動路徑進(jìn)行擬合。

機(jī)械臂運(yùn)行時間一般通過群體智能優(yōu)化算法進(jìn)行優(yōu)化，如遺傳算法、蟻群算法、粒子群算法等[9-12]，其中以遺傳算法在機(jī)械臂時間最優(yōu)軌跡規(guī)劃問題中的研究最為深入，許多學(xué)者針對機(jī)械臂運(yùn)動特性提出了很多針對性的改進(jìn)。文獻(xiàn)[13]針對超冗余度機(jī)械臂提出了一種時間最優(yōu)軌跡規(guī)劃方法，并用標(biāo)準(zhǔn)多種群遺傳算法優(yōu)化了機(jī)械臂的運(yùn)行時間。文獻(xiàn)[14]提出一種自適應(yīng)精英遺傳算法，在五次多項式插值軌跡規(guī)劃算法的基礎(chǔ)上，采用基于遺傳算法框架的精英群和自適應(yīng)調(diào)節(jié)機(jī)制提高優(yōu)化性能。文獻(xiàn)[15]中通過自適應(yīng)調(diào)整交叉和變異的概率，達(dá)到更快的收斂速度。文獻(xiàn)[16]提出一種基于混沌局部搜索的雙種群遺傳最優(yōu)時間軌跡規(guī)劃算法，利用雙種群遺傳算法全局搜索能力強(qiáng)、進(jìn)化速度快和混沌算法局部搜索能力強(qiáng)的優(yōu)點，規(guī)劃時間最優(yōu)的運(yùn)動軌跡。文獻(xiàn)[17]在三次樣條插值軌跡規(guī)劃算法的基礎(chǔ)上采用聚類方法改進(jìn)遺傳算法，保留優(yōu)良個體的同時兼顧種群多樣性，提高搜索效率。

上述文獻(xiàn)中分別通過多種群、自適應(yīng)交叉和變異、貪婪選擇策略等方面對遺傳算法進(jìn)行改進(jìn)，然而仍然存在算法收斂一致性差，收斂速度有待提高等問題。為此文中提出一種基于改進(jìn)多種群遺傳算法的軌跡規(guī)劃方法，在五次多項式插值算法擬合的機(jī)械臂運(yùn)動路徑上，通過改進(jìn)的多種群遺傳算法對機(jī)械臂運(yùn)動時間進(jìn)行優(yōu)化，改進(jìn)之處包括：設(shè)計含有懲罰項的適應(yīng)度函數(shù)，降低不滿足運(yùn)動學(xué)約束條件的個體被選擇的概率；引入貪婪選擇策略，按照比例保留父代種群優(yōu)秀個體，替換子代種群較差個體；自適應(yīng)調(diào)整交叉和變異概率，加快算法收斂速度。以ER-4iA機(jī)械臂為實驗對象，通過仿真實驗驗證所提算法的可行性，并將所提算法與蟻群算法、粒子群算法、標(biāo)準(zhǔn)遺傳算法和標(biāo)準(zhǔn)多種群遺傳算法進(jìn)行對比實驗，結(jié)果表明所提軌跡規(guī)劃方法具有更好的收斂一致性和更快的收斂速度。

1 串聯(lián)機(jī)械臂運(yùn)動學(xué)模型

文中提出的機(jī)械臂時間最優(yōu)軌跡規(guī)劃方法適用于串聯(lián)機(jī)械臂結(jié)構(gòu)，串聯(lián)機(jī)械臂由一組剛性連桿通過轉(zhuǎn)動關(guān)節(jié)串聯(lián)而成，是目前最常見的機(jī)械臂結(jié)構(gòu)。1955年Denavit和Hartenberg提出的D-H方法是建立串聯(lián)機(jī)械臂運(yùn)動學(xué)模型的標(biāo)準(zhǔn)方法[18]，其原理是在各連桿的末端關(guān)節(jié)處建立坐標(biāo)系，相鄰坐標(biāo)系通過4個參數(shù)計算變換矩陣，從而建立機(jī)械臂末端和基坐標(biāo)之間的位姿變換關(guān)系。然而標(biāo)準(zhǔn)D-H方法無法表示連桿關(guān)于y軸的運(yùn)動，且在樹形機(jī)械臂結(jié)構(gòu)的建模上存在歧義，這些問題在1986年Khalil和Kleinfinger提出的改進(jìn)D-H方法中得以改善[19]。改進(jìn)D-H方法在連桿首端關(guān)節(jié)處建立坐標(biāo)系，且相鄰坐標(biāo)系之間的參數(shù)變換順序和標(biāo)準(zhǔn)D-H方法完全相反，是目前最常用的串聯(lián)機(jī)械臂運(yùn)動學(xué)建模方法。

以圖1所示的六自由度串聯(lián)機(jī)械臂為例，通過改進(jìn)D-H方法建立機(jī)械臂各連桿坐標(biāo)系的步驟如下：

圖1 六自由度串聯(lián)機(jī)械臂結(jié)構(gòu)Fig.1 Structure of tandem manipulator with six degrees of freedom

（1）確定Z軸：Zi軸沿關(guān)節(jié)軸i的方向。

（2）確定原點：原點Oi為Zi軸和Zi+1軸的交點或其公垂線與Zi軸的交點。

（3）確定X軸：Xi軸沿Zi軸和Zi+1軸的公垂線由關(guān)節(jié)軸i指向關(guān)節(jié)軸i+1，若Zi軸與Zi+1軸相交則Xi軸垂直于它們所在的平面。

（4）確定Y軸：Yi軸由右手定則確定。

以地面坐標(biāo)系為基坐標(biāo)系，相鄰兩個關(guān)節(jié)坐標(biāo)系之間的變換矩陣可以通過鏈?zhǔn)椒▌t計算：

其中，αi-1是Zi-1到Zi繞Xi-1正方向轉(zhuǎn)動的角度，ai-1是轉(zhuǎn)動后的Zi-1到Zi沿Xi-1正方向的距離，θi是移動后的Xi-1到Xi繞Zi正方向轉(zhuǎn)動的角度，也是實際控制機(jī)械臂運(yùn)動的關(guān)節(jié)角度，di是轉(zhuǎn)動后的Xi-1到Xi沿Zi方向上的距離，坐標(biāo)系(Xi-1,Yi-1,Zi-1)通過上述四個變換之后能夠與坐標(biāo)系(Xi,Yi,Zi)完全重合。機(jī)械臂末端坐標(biāo)系與基坐標(biāo)系的變換矩陣為：

根據(jù)上述機(jī)械臂運(yùn)動學(xué)模型，已知各關(guān)節(jié)角度即可計算機(jī)械臂末端位姿，通過控制機(jī)械臂各關(guān)節(jié)角θ關(guān)于時間的角位移曲線Qi(t)就能夠控制機(jī)械臂末端的運(yùn)動軌跡，從而將機(jī)械臂的空間軌跡規(guī)劃問題轉(zhuǎn)換為各關(guān)節(jié)角運(yùn)動曲線規(guī)劃問題。

2 優(yōu)化問題描述

串聯(lián)機(jī)械臂在工業(yè)生產(chǎn)和物流運(yùn)輸?shù)阮I(lǐng)域擁有廣泛的應(yīng)用，時間最優(yōu)軌跡規(guī)劃能夠有效縮短機(jī)械臂的運(yùn)行周期，提高工作效率。文中以機(jī)械臂點到點軌跡的運(yùn)行時間為主要優(yōu)化目標(biāo)，假設(shè)機(jī)械臂從初始關(guān)節(jié)狀態(tài)Q0運(yùn)動到終止關(guān)節(jié)狀態(tài)Qf的時間為tf，則目標(biāo)函數(shù)可以表示為：

此外機(jī)械臂在運(yùn)動過程中應(yīng)嚴(yán)格滿足其固有的運(yùn)動學(xué)約束條件，因此優(yōu)化問題可以描述為：

式中，V、A、J分別表示機(jī)械臂各關(guān)節(jié)運(yùn)動過程中的角速度、角加速度和角加加速度，Vs、As、Js分別為它們的額定最大值。

對于五次多項式插值算法擬合的運(yùn)動軌跡，關(guān)節(jié)角θ關(guān)于時間的角位移曲線Q(t)、角速度曲線V(t)、角加速度曲線A(t)及角加加速度曲線J(t)可以表示為：

通過求解五次多項式中的6個系數(shù)a0～a5即可得到各關(guān)節(jié)運(yùn)動曲線表達(dá)式。

3 機(jī)械臂時間最優(yōu)軌跡規(guī)劃方法

3.1 多種群遺傳算法

多種群遺傳算法是遺傳算法的一種改進(jìn)，多個種群同時沿不同方向進(jìn)化，種群之間通過移民算子進(jìn)行交互，最優(yōu)解從各種群最優(yōu)個體構(gòu)成的精英種群中產(chǎn)生，算法流程圖如圖2所示。

圖2 多種群遺傳算法流程圖Fig.2 Flow chart of multi-population genetic algorithm

其中將自身種群的最優(yōu)個體作為移民算子，在每次選擇父代個體之前遷移到相鄰種群中，替代目標(biāo)種群中的最劣個體，這有三點好處：

（1）為目標(biāo)種群帶去新的優(yōu)秀的基因，豐富了目標(biāo)種群的多樣性。

（2）目標(biāo)種群陷入局部最優(yōu)時，移民算子的加入有利于打破僵局。

（3）種群之間協(xié)同進(jìn)化，加速種群收斂。

3.2 基于改進(jìn)多種群遺傳算法的時間最優(yōu)軌跡規(guī)劃

針對機(jī)械臂點到點軌跡規(guī)劃任務(wù)，以五次多項式軌跡插值的時間tf為優(yōu)化目標(biāo)，在多種群遺傳算法的基礎(chǔ)上進(jìn)行相應(yīng)的調(diào)整和改進(jìn)，提出一種基于改進(jìn)多種群遺傳算法的時間最優(yōu)軌跡優(yōu)化方法，具體改進(jìn)點如下：

帶有懲罰項的適應(yīng)度函數(shù)。根據(jù)式（4）描述的優(yōu)化問題，設(shè)計如下個體適應(yīng)度函數(shù)：

式中，x是個體基因的表現(xiàn)形式，即運(yùn)行時間tf,kt是時間調(diào)和系數(shù)，Ei是各關(guān)節(jié)根據(jù)機(jī)械臂運(yùn)動約束條件設(shè)計的懲罰項，其計算公式為：

貪婪選擇策略：在父代種群選擇階段，按照一定比例保留當(dāng)前種群的部分優(yōu)秀個體，經(jīng)過交叉和變異操作之后通過重插入的方法，用保留個體逐個替換子代種群中的較差個體，既保證了優(yōu)秀個體不被破壞，也能加快算法收斂。

自適應(yīng)交叉和變異：在多種群遺傳算法中，為了實現(xiàn)不同種群進(jìn)化方向的差異性，各種群的交叉和變異概率Pm和Pc在特定范圍[PmL,PmU]和[PcL,PcU]內(nèi)隨機(jī)產(chǎn)生，為了達(dá)到更快的收斂速度，將交叉和變異概率設(shè)置為隨迭代次數(shù)自適應(yīng)降低，迭代早期通過較大的交叉和變異概率保證種群多樣性，迭代后期通過較小的交叉和變異概率保留最優(yōu)解并加快收斂速度，自適應(yīng)交叉和變異概率表達(dá)式為：

其中，gen為當(dāng)前算法迭代次數(shù)。

3.3 算法整體流程

綜上所述，文中設(shè)計的基于改進(jìn)多種群遺傳算法的機(jī)械臂時間最優(yōu)軌跡規(guī)劃方法主要步驟如下：

（1）設(shè)置主要參數(shù)：基因編碼長度chromlen，種群大小popsize，種群數(shù)量popnum，式（7）～（9）中的參數(shù)。

（2）初始化種群。

（3）進(jìn)行貪婪選擇、自適應(yīng)交叉和變異操作。

（4）計算適應(yīng)度值：對個體解碼獲取實數(shù)x，根據(jù)式（5）和式（6）計算五次多項式系數(shù)，得到各關(guān)節(jié)角速度、角加速度、角加加速度函數(shù)，根據(jù)式（7）和式（8）計算個體適應(yīng)度函數(shù)值。

（5）獲取移民算子和精英種群。

（6）判斷是否滿足收斂條件：若滿足則輸出最優(yōu)個體，不滿足則進(jìn)行移民操作并重復(fù)步驟（3），收斂條件為精英種群最優(yōu)個體保持一定的代數(shù)。

4 仿真實驗與結(jié)果分析

4.1 仿真實驗平臺

文中使用發(fā)那科的ER-4iA機(jī)械臂作為實驗驗證平臺，通過MATLAB中的Robotics Toolbox工具箱進(jìn)行機(jī)械臂仿真建模。根據(jù)改進(jìn)D-H方法計算ER-4iA機(jī)械臂的D-H參數(shù)如表1所示，用SerialLink函數(shù)建立如圖3所示的ER-4iA機(jī)械臂仿真模型。

圖3 ER-4iA機(jī)械臂仿真模型Fig.3 Simulation model of manipulator ER-4iA

表1 ER-4iA機(jī)械臂D-H參數(shù)Table 1 D-H parameters of manipulator ER-4iA

為了在實驗平臺上驗證文中時間最優(yōu)軌跡規(guī)劃方法的可靠性，基于控制變量的原則，設(shè)置機(jī)械臂各關(guān)節(jié)始末角度、速度和加速度為定值：

對此只要提供機(jī)械臂運(yùn)行時間tf就可以求解五次多項式的6個系數(shù)a0～a5，從而根據(jù)式（5）得到各關(guān)節(jié)角速度函數(shù)、角加速度函數(shù)和角加加速度函數(shù)。

4.2 算法仿真實驗

設(shè)置基因編碼長度chromlen=10,種群大小popsize=40,種群數(shù)量popnum=10,適應(yīng)度函數(shù)中的時間調(diào)和系數(shù)kt=0.1，懲罰系數(shù)kV=kA=kJ=1，隨機(jī)交叉概率范圍[PmL,PmU]=[0.7,0.9]，隨機(jī)變異概率[PcL,PcU]=[0.001,0.05]，機(jī)械臂關(guān)節(jié)運(yùn)動學(xué)約束條件如表2所示。

表2 機(jī)械臂關(guān)節(jié)約束條件Table 2 Constraints of mechanical arm joint

按照上述參數(shù)設(shè)置進(jìn)行多組實驗，最優(yōu)運(yùn)行時間均為3.914 9 s，此時各關(guān)節(jié)角位移、角速度、角加速度及角加加速度隨時間的變化曲線如圖4所示。

圖4 各關(guān)節(jié)角運(yùn)動曲線Fig.4 Angular motion curves of each joint

仿真實驗結(jié)果表明：各關(guān)節(jié)角運(yùn)動軌跡平滑，始末點狀態(tài)均符合參數(shù)設(shè)定，運(yùn)動過程中角速度、角加速度及角加加速度均無突變，并始終滿足機(jī)械臂關(guān)節(jié)約束條件，驗證了文中設(shè)計的機(jī)械臂時間最優(yōu)軌跡規(guī)劃方法的有效性。

4.3 優(yōu)化算法對比實驗

為了驗證文中改進(jìn)的多種群遺傳算法具有更好的收斂一致性和更快的收斂速度，在優(yōu)化目標(biāo)函數(shù)和收斂條件不變的情況下，分別用蟻群算法（ACO）、粒子群算法（PSO）、標(biāo)準(zhǔn)遺傳算法（SGA）、標(biāo)準(zhǔn)多種群遺傳算法（MPGA）和文中改進(jìn)的多種群遺傳算法（IMPGA）對4.1節(jié)中的運(yùn)動軌跡進(jìn)行10組重復(fù)實驗，對比5種優(yōu)化算法在收斂一致性和收斂速度上的表現(xiàn)，實驗結(jié)果如表3所示。

表3 5種優(yōu)化算法的實驗結(jié)果Table 3 Experimental results of five optimization algorithms

5種優(yōu)化算法的進(jìn)化過程如圖5所示，對比實驗結(jié)果表明：蟻群算法和粒子群算法收斂速度較慢，多次重復(fù)實驗收斂結(jié)果基本一致；標(biāo)準(zhǔn)遺傳算法收斂速度較快但多次重復(fù)實驗結(jié)果很難保持一致，算法穩(wěn)定性較差；標(biāo)準(zhǔn)多種群算法收斂速度較快且在多次重復(fù)實驗中均能搜索到全局最優(yōu)解；文中改進(jìn)的多種群遺傳算法在標(biāo)準(zhǔn)多種群算法穩(wěn)定搜索最優(yōu)解的基礎(chǔ)上，進(jìn)一步加快了收斂速度。由此驗證了文中改進(jìn)的多種群遺傳算法相較于其他同類算法具有更好的收斂一致性和更快的收斂速度。

圖5 5種優(yōu)化算法的進(jìn)化過程Fig.5 Evolution of five optimization algorithms

5 結(jié)束語

針對串聯(lián)機(jī)械臂時間最優(yōu)軌跡規(guī)劃問題，在5次多項式軌跡插值的基礎(chǔ)上，利用改進(jìn)的多種群遺傳算法對機(jī)械臂運(yùn)動軌跡進(jìn)行優(yōu)化。以發(fā)那科ER-4iA機(jī)械臂為實驗驗證平臺，以機(jī)械臂點到點軌跡運(yùn)行時間為優(yōu)化目標(biāo)，分別用蟻群算法、粒子群算法、標(biāo)準(zhǔn)遺傳算法、標(biāo)準(zhǔn)多種群遺傳算法和文中提出的改進(jìn)多種群遺傳算法進(jìn)行優(yōu)化。實驗結(jié)果表明文中算法在多組重復(fù)實驗中均能收斂至全局最優(yōu)解，且相較于同類算法具有更快的收斂速度；以最優(yōu)運(yùn)行時間擬合機(jī)械臂運(yùn)動軌跡，各關(guān)節(jié)角速度、角加速度及角加加速度曲線均平滑且滿足機(jī)械臂運(yùn)動學(xué)約束條件，驗證了文中改進(jìn)多種群遺傳算法的機(jī)械臂時間最優(yōu)軌跡規(guī)劃方法的可行性。

文中以機(jī)械臂點到點運(yùn)動的時間作為優(yōu)化目標(biāo)進(jìn)行仿真和對比實驗，然而機(jī)械臂實際工作任務(wù)可能由多段點到點運(yùn)動構(gòu)成，文中改進(jìn)的多種群遺傳算法在以多段運(yùn)動時間為目標(biāo)的多目標(biāo)優(yōu)化任務(wù)中是否仍然具備優(yōu)勢仍有待驗證。此外，在某些特殊任務(wù)中，除了對機(jī)械臂關(guān)節(jié)運(yùn)動有所約束，往往還對末梢速度、關(guān)節(jié)力矩等指標(biāo)有所要求，如何調(diào)整算法適應(yīng)更為復(fù)雜的優(yōu)化目標(biāo)也是未來潛在的研究方向之一。