基于改進(jìn)PSO算法的林地作業(yè)車軌跡優(yōu)化

莊徐 ，鄭哲文，李科軍，張恭宇

(1.中南林業(yè)科技大學(xué) 材料科學(xué)與工程學(xué)院，長沙 410004；2.湖南交通工程學(xué)院 交通運(yùn)輸工程學(xué)院,湖南 衡陽 421001)

0 引言

隨著我國林木采伐強(qiáng)度增大，傳統(tǒng)采伐方式對森林構(gòu)成了極大的破壞，制約著社會的發(fā)展[1]。而林木產(chǎn)業(yè)是我國國民經(jīng)濟(jì)的重要組成部分，科學(xué)的采伐與轉(zhuǎn)運(yùn)能夠更加有效利用森林資源，保護(hù)生態(tài)環(huán)境[2]。為了保證木材安全轉(zhuǎn)運(yùn)實(shí)現(xiàn)機(jī)械化、智能化，林地作業(yè)車結(jié)合了工程機(jī)械與工業(yè)機(jī)器人的特點(diǎn)，能夠像工業(yè)機(jī)器人一樣實(shí)現(xiàn)自主控制，對木材進(jìn)行夾取、轉(zhuǎn)運(yùn)，有效地保障了工作效率且減少了林區(qū)工作安全事故的發(fā)生。林地作業(yè)車臂架末端的執(zhí)行器作為臂架系統(tǒng)的重要組成部分，其運(yùn)動軌跡關(guān)系著工作精度以及穩(wěn)定性，因此對林地作業(yè)車臂架進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃，對提升工作效率有著重要意義。

在林區(qū)等復(fù)雜環(huán)境下，為了使林地作業(yè)車臂架末端的執(zhí)行器能夠快速、準(zhǔn)確、平穩(wěn)地到達(dá)目標(biāo)位置，提升自動化程度，對林地作業(yè)車進(jìn)行最優(yōu)時(shí)間的軌跡規(guī)劃顯得尤其重要。目前，對軌跡規(guī)劃進(jìn)行優(yōu)化主要以實(shí)現(xiàn)時(shí)間最優(yōu)、能量最優(yōu)以及沖擊最優(yōu)開展[3-4]。陳晗等[5]采用復(fù)合形法對5-7-5分段多項(xiàng)式插值軌跡進(jìn)行優(yōu)化，達(dá)到了優(yōu)化時(shí)間的目的，但分段多項(xiàng)式階次較高，運(yùn)行時(shí)間較長。李虹等[6]分別運(yùn)用粒子群算法與差分進(jìn)化算法實(shí)現(xiàn)了對4-3-3-3-4分段多項(xiàng)式插值軌跡的時(shí)間優(yōu)化，提高了各關(guān)節(jié)的工作效率，但分段次數(shù)多，計(jì)算量大。Qiao等[7]采用改進(jìn)遺傳算法對五次多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，實(shí)現(xiàn)了關(guān)節(jié)軌跡全局最優(yōu)，但其插值采用五次多項(xiàng)式，相較于分段多項(xiàng)式插值，該方法用時(shí)較多。李國洪等[8]通過聚類方法對遺傳算子進(jìn)行改進(jìn)，以新的變異操作提升了尋優(yōu)效率，縮短了關(guān)節(jié)軌跡運(yùn)行時(shí)間，但全局采用7次樣條插值，階次較高，計(jì)算量大。以上研究雖取得一定成果，但普遍存在計(jì)算量大、耗時(shí)較多等問題。

針對上述問題，本研究以林地作業(yè)車為研究對象，通過改進(jìn)PSO算法對臂架末端進(jìn)行以時(shí)間最短為目標(biāo)的軌跡優(yōu)化。首先通過標(biāo)準(zhǔn)型D-H(Denavit-Hartenberg)法構(gòu)建林地作業(yè)車運(yùn)動學(xué)模型，通過在工作空間中選取4個(gè)途徑位置作為軌跡插值點(diǎn)，采用3-5-3分段多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，最后在Matlab軟件中通過改進(jìn)PSO算法與標(biāo)準(zhǔn)PSO算法對軌跡運(yùn)行時(shí)間進(jìn)行優(yōu)化，結(jié)果表明在相同運(yùn)動學(xué)約束下，改進(jìn)PSO算法能夠更快搜索到全局最優(yōu)解并驗(yàn)證了該方法的可行性。

1 林地作業(yè)車運(yùn)動學(xué)分析

本研究的林地作業(yè)車如圖1所示，其臂架系統(tǒng)可視為一款5自由度機(jī)械臂，結(jié)構(gòu)緊湊，小巧靈活，能夠在林區(qū)復(fù)雜工作環(huán)境中連續(xù)作業(yè)，保障生產(chǎn)效率。根據(jù)標(biāo)準(zhǔn)型D-H表達(dá)法建立連桿坐標(biāo)系[9]，見表1，其中θ1—θ5為關(guān)節(jié)回轉(zhuǎn)角度，d1—d5為連桿之間的偏距，a1—a5為各連桿的長度，α1—α5為各連桿的扭轉(zhuǎn)角度。

圖1 林地作業(yè)車Fig.1 Forest work vehicles

表1 林地作業(yè)車D-H參數(shù)

結(jié)合表1，可以通過齊次變換矩陣求得臂架的正運(yùn)動學(xué)方程，其中第i個(gè)連桿相對于第i-1個(gè)連桿的齊次變換矩陣為[10]

(1)

將表1中的各參數(shù)代入公式(1)中可計(jì)算出臂架末端相對于基坐標(biāo)系的位姿矩陣

(2)

公式(2)中，前3列為臂架末端相對于基坐標(biāo)系的姿態(tài)，最后1列為臂架末端相對于基坐標(biāo)系的位置。

為了確保臂架末端能夠到達(dá)路徑中的各點(diǎn)，需要計(jì)算出各關(guān)節(jié)在軌跡中的位置，基于Matlab軟件Robotics Toolbox對各關(guān)節(jié)進(jìn)行逆運(yùn)動學(xué)求解。根據(jù)公式(2)可以確定臂架末端經(jīng)過4個(gè)路徑點(diǎn)時(shí)在笛卡爾空間(Cartesian space)中的位置，見表2。

表2 臂架末端運(yùn)動軌跡路徑坐標(biāo)Tab.2 Trajectory path coordinates at the end of the boom

根據(jù)逆運(yùn)動學(xué)求解方法[11]，可以求得各關(guān)節(jié)在路徑點(diǎn)中的轉(zhuǎn)角大小，結(jié)合Robotics Toolbox求得各關(guān)節(jié)路徑點(diǎn)的解見表3。

表3 路徑點(diǎn)各關(guān)節(jié)逆解Tab.3 Inverse kinematics of each joint at the path point

2 基于改進(jìn)PSO算法的3-5-3多項(xiàng)式插值函數(shù)

2.1 分段多項(xiàng)式軌跡規(guī)劃方法

根據(jù)3-5-3分段多項(xiàng)式軌跡(時(shí)間最優(yōu)軌跡規(guī)劃，就是三次多項(xiàng)式、五次多項(xiàng)式以及B樣條插值等進(jìn)行軌跡規(guī)劃)連續(xù)、平滑無突變且計(jì)算量小的特點(diǎn)，本研究采用3-5-3分段多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，通過計(jì)算各區(qū)間運(yùn)動所需的時(shí)間獲取整段軌跡的運(yùn)行時(shí)間。3-5-3多項(xiàng)式的通式如下[12]

(3)

式中：Li1(t)、Li2(t)、Li3(t)為關(guān)節(jié)i(i=1,2,3,4,5)在3段多項(xiàng)式插值中的軌跡；aijm為關(guān)節(jié)i在第j段軌跡中的第m+1個(gè)未知數(shù)；t1、t2、t3表示各關(guān)節(jié)分別在3段區(qū)間中的運(yùn)行時(shí)間。

為了保持臂架末端執(zhí)行器軌跡的連續(xù)性與平順性，各關(guān)節(jié)在初始點(diǎn)與終止點(diǎn)的速度與加速度為0,經(jīng)過各路徑點(diǎn)時(shí)的速度與加速度連續(xù)，即

(4)

根據(jù)以上約束，推導(dǎo)出未知系數(shù)與各插值點(diǎn)的關(guān)系式為

(5)

(6)

(7)

公式(5)中的t1、t2、t3表示關(guān)節(jié)在3段多項(xiàng)式中的插值時(shí)間；公式(6)中b為關(guān)節(jié)對應(yīng)的角位移矩陣；公式(7)為關(guān)節(jié)系數(shù)矩陣。

2.2 改進(jìn)PSO算法

本研究采用改進(jìn)粒子群算法計(jì)算各關(guān)節(jié)最短運(yùn)動時(shí)間，通過將關(guān)節(jié)完成運(yùn)動所用的時(shí)間t1、t2、t3組成1個(gè)三維粒子群，隨機(jī)生成N個(gè)粒子，其適應(yīng)度函數(shù)(fitness，f(t))為[13]

f(t)=min(t1+t2+t3) 。

(8)

式中，ti分別為關(guān)節(jié)在第i段軌跡中的運(yùn)動時(shí)間。

初始化種群后，根據(jù)函數(shù)適應(yīng)度計(jì)算每個(gè)粒子的適應(yīng)度值，將當(dāng)代粒子中適應(yīng)度最小的粒子作為局部最優(yōu)粒子pid，經(jīng)迭代完成后，將所有粒子中適應(yīng)度最小的粒子作為全局最優(yōu)粒子pgd。粒子群算法的速度與位置更新公式如下[14]

(9)

(10)

標(biāo)準(zhǔn)PSO算法的慣性權(quán)重系數(shù)(ω)以及學(xué)習(xí)因子c1、c2為固定值，在計(jì)算過程中容易早熟收斂至局部最優(yōu)且迭代收斂速度慢[16]。針對此問題，本研究采用一種慣性權(quán)重線性遞減且動態(tài)調(diào)整學(xué)習(xí)因子的自適應(yīng)方式，保證了粒子的前期搜索能力，加快了收斂速度，計(jì)算方式為

(11)

式中：k為當(dāng)前迭代次數(shù)；n為全局迭代次數(shù)；迭代前期c2逐漸增大使粒子從當(dāng)前位置逐漸向全局最優(yōu)位置搜索；后期c1逐漸增大使粒子個(gè)體搜尋能力加強(qiáng)，抑制了粒子在最優(yōu)解附近進(jìn)行重復(fù)搜索，避免陷入局部最優(yōu)[17]。

通過對各關(guān)節(jié)進(jìn)行速度與加速度的限制，保證末端執(zhí)行器平穩(wěn)運(yùn)行，以時(shí)間t為尋優(yōu)量，對關(guān)節(jié)軌跡運(yùn)行時(shí)間進(jìn)行優(yōu)化，步驟如下。

(1)初始化種群，對每個(gè)關(guān)節(jié)3段軌跡的插值時(shí)間分別隨機(jī)生成M個(gè)粒子，初始化粒子的位置與速度。

(2)將M組插值時(shí)間帶入公式(5)中，進(jìn)而計(jì)算出多項(xiàng)式中的關(guān)節(jié)系數(shù)矩陣a。并將矩陣a中的各項(xiàng)系數(shù)帶入公式(3)中，根據(jù)時(shí)間計(jì)算出各關(guān)節(jié)的位置，并求導(dǎo)計(jì)算出各關(guān)節(jié)的速度與加速度。

(3)計(jì)算粒子個(gè)體適應(yīng)度，將所計(jì)算出的各關(guān)節(jié)速度與加速度與設(shè)定的最大速度與加速度進(jìn)行對比分析，若在約束范圍內(nèi)，則通過公式(8)作為該粒子的適應(yīng)度值，若存在大于約束值的現(xiàn)象，則將一個(gè)極大值賦值予此粒子，便于后期迭代過程中淘汰該粒子。

(4)經(jīng)n次迭代后，選擇每個(gè)粒子所經(jīng)過的最優(yōu)位置賦予pid，選取pid中適應(yīng)度最小的粒子為pgd。將pid、pgd代入公式(9)與公式(10)中生成新的種群。

(5)判斷是否達(dá)到最大迭代值或找到全局最優(yōu)解，滿足條件則輸出最優(yōu)時(shí)間值，迭代結(jié)束，否則返回至步驟2。

3 仿真結(jié)果分析

根據(jù)公式(2)可以得到臂架末端相對于基坐標(biāo)系的齊次變換矩陣。通過Matlab軟件中Robotics Toolbox對林地作業(yè)車進(jìn)行建模，如圖2所示，根據(jù)工作特點(diǎn)，在不考慮回轉(zhuǎn)的情況下，設(shè)定插值時(shí)間為10 s，t1=3 s,t2=4 s,t3=3 s，分別為對林地作業(yè)車4個(gè)關(guān)節(jié)采用3-5-3分段多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，各關(guān)節(jié)軌跡曲線如圖3所示。

分析圖3可知，林地作業(yè)車臂架各關(guān)節(jié)角度位置、速度、加速度變化連續(xù)，但仍存在一定優(yōu)化空間。

圖2 林地作業(yè)車三維模型Fig.2 3D model of forest work vehicles

圖3 3-5-3分段多項(xiàng)式插值軌跡曲線Fig.3 3-5-3 segmented polynomial interpolated trajectory curve

根據(jù)表4中各關(guān)節(jié)的約束條件，分別應(yīng)用標(biāo)準(zhǔn)PSO算法和改進(jìn)PSO算法，按照步驟1—步驟5對各關(guān)節(jié)運(yùn)行時(shí)間進(jìn)行優(yōu)化，設(shè)置總迭代次數(shù)N=100，隨機(jī)散布M=50個(gè)粒子，標(biāo)準(zhǔn)PSO算法的慣性權(quán)重系數(shù)范圍為[0.4,0.9],學(xué)習(xí)因子c1、c2取值為2。改進(jìn)PSO算法的慣性權(quán)重系數(shù)與學(xué)習(xí)因子根據(jù)公式(11)變化，分析其動態(tài)變化對算法收斂速度與適應(yīng)度的影響，迭代曲線如圖4所示。

表4 各關(guān)節(jié)約束條件Tab.4 Constraints of individual joint

圖4 各關(guān)節(jié)迭代曲線Fig.4 Iterative curves for each joint

由各關(guān)節(jié)的迭代曲線可以看出，采用改進(jìn)PSO算法的適應(yīng)度曲線收斂速度比采用標(biāo)準(zhǔn)PSO算法的適應(yīng)度曲線收斂速度快，且收斂后的最優(yōu)適應(yīng)度更低，驗(yàn)證了改進(jìn)PSO算法求解優(yōu)于標(biāo)準(zhǔn)PSO算法，因此基于改進(jìn)PSO算法的3-5-3多項(xiàng)式插值具有更好的收斂速度與尋優(yōu)效率。

為了保證臂架各關(guān)節(jié)能夠同時(shí)將末端工作裝置運(yùn)達(dá)指定工作位置，因此選用各關(guān)節(jié)的最大運(yùn)行時(shí)間作為插值時(shí)間，優(yōu)化后各關(guān)節(jié)的位置、速度、加速曲線如圖5和圖6所示。

圖5 標(biāo)準(zhǔn)PSO算法優(yōu)化各關(guān)節(jié)軌跡曲線Fig.5 The standard PSO algorithm optimized the trajectory curve of each joint

圖6 改進(jìn)PSO算法優(yōu)化各關(guān)節(jié)軌跡曲線Fig.6 The improved PSO algorithm optimized the trajectory curve of each joint

分析圖5和圖6可以看出，在速度與加速度約束條件下，采用改進(jìn)PSO算法相對于標(biāo)準(zhǔn)PSO算法，其關(guān)節(jié)速度與加速度更大，且最大速度與加速度均在約束范圍內(nèi)，符合預(yù)期設(shè)定。所用時(shí)間縮短了1.3 s，縮短約13%，有效提升了各關(guān)節(jié)的工作效率。

4 結(jié)論

本研究以林地作業(yè)車為研究對象，通過標(biāo)準(zhǔn)型D-H法建立運(yùn)動學(xué)模型，采用3-5-3分段多項(xiàng)式插值對工作空間中4個(gè)途經(jīng)點(diǎn)進(jìn)行軌跡規(guī)劃，在各關(guān)節(jié)運(yùn)動學(xué)約束下采用改進(jìn)PSO算法對軌跡運(yùn)行時(shí)間進(jìn)行優(yōu)化，并對比分析與標(biāo)準(zhǔn)PSO算法的優(yōu)化性能。研究結(jié)果表明，改進(jìn)PSO算法的收斂速度更快，且收斂后的適應(yīng)度更小，各關(guān)節(jié)的速度與加速度曲線由9.64 s縮短至8.34 s，縮短約13%，提高了林地作業(yè)車的工作效率。