韓濤， 李靜， 黃友銳， 徐善永， 許家昌

(安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001)

0 引言

惡劣的開采環(huán)境導(dǎo)致煤礦安全事故頻發(fā)。煤礦救援機器人能在復(fù)雜危險的礦井災(zāi)害環(huán)境中靈活運動，從而提升救援效率，減少煤礦災(zāi)難造成的人員傷亡。煤礦救援機器人機械臂軌跡規(guī)劃即在工作空間中尋找與時間相關(guān)的運動路線[1]。機械臂軌跡規(guī)劃算法的性能直接影響機器人機械臂的運動效率，因此其研究具有重要意義。

根據(jù)規(guī)劃空間不同，軌跡規(guī)劃分為關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃[2-3]。關(guān)節(jié)空間軌跡規(guī)劃以各關(guān)節(jié)角度為規(guī)劃對象，求取關(guān)節(jié)角度、角速度、角加速度，而笛卡爾空間軌跡規(guī)劃以末端位姿為規(guī)劃對象，通常以位姿坐標(biāo)形式描述關(guān)鍵點。笛卡爾空間軌跡規(guī)劃方法存在奇異解問題，而關(guān)節(jié)空間軌跡規(guī)劃方法不會出現(xiàn)奇異解問題且計算較簡單，因此，關(guān)節(jié)空間軌跡規(guī)劃應(yīng)用更廣泛。

常用的機械臂軌跡規(guī)劃算法有多項式插值算法和樣條插值算法，其中三次樣條插值、B樣條插值適用于有復(fù)雜連續(xù)任務(wù)要求的運動[4-5]，三次多項式和五次多項式插值算法被廣泛應(yīng)用于點到點運動和簡單連續(xù)運動[6-8]。為了保證角速度和角加速度均在可控范圍內(nèi)，本文選用五次多項式插值算法在機械臂關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃。

軌跡規(guī)劃算法的最終目的是尋找機械臂運動時目標(biāo)函數(shù)的最優(yōu)值。隨著智能仿生算法的發(fā)展，遺傳算法[9-10]、粒子群算法[11-12]、蜂群算法等[13]被應(yīng)用于機械臂軌跡規(guī)劃中，但傳統(tǒng)的智能優(yōu)化算法存在搜索時間長、規(guī)劃軌跡不夠光滑、能量消耗高的問題。因此，研究者提出了基于改進(jìn)智能優(yōu)化算法的軌跡規(guī)劃方法。李國洪等[14]通過聚類方法對遺傳算子進(jìn)行綜合調(diào)控，并通過新的變異操作提高算法后期的尋優(yōu)效率，從而有效縮短了軌跡規(guī)劃時間。鄧偉等[15]利用雙種群遺傳算法全局搜索能力強、進(jìn)化速度快和混沌算法局部搜索能力強的優(yōu)點，提出了一種基于混沌局部搜索的雙種群遺傳最優(yōu)時間軌跡規(guī)劃算法，使機械臂末端執(zhí)行器運行軌跡平滑且時間最優(yōu)。A.Khoukhi等[16]利用神經(jīng)網(wǎng)絡(luò)并行處理、分布存儲的特點及良好的自學(xué)習(xí)能力，提出了一種求解逆運動學(xué)問題的神經(jīng)模糊網(wǎng)絡(luò)，通過對最小時間和相應(yīng)力矩的預(yù)處理建立神經(jīng)模糊控制器，生成最優(yōu)時間軌跡力矩。Y.L.Hsu等[17]提出了一種利用永磁同步電動機對切換機構(gòu)進(jìn)行節(jié)能軌跡規(guī)劃的方法，采用實時編碼遺傳算法確定多項式系數(shù)，最終實現(xiàn)了能量最優(yōu)控制。

針對現(xiàn)有算法存在的軌跡規(guī)劃不合理、算法收斂速度慢等問題，為了同時滿足軌跡規(guī)劃的高精度和高速率要求，本文在上述研究的基礎(chǔ)上，提出了一種基于融合杜鵑搜索的灰狼優(yōu)化(Grey Wolf Optimization with Cuckoo Search，CS-GWO)算法的煤礦救援機器人機械臂軌跡規(guī)劃算法。該算法在灰狼優(yōu)化(Grey Wolf Optimization，GWO)算法的位置更新中融入杜鵑搜索(Cuckoo Search，CS)算法中出現(xiàn)的2次擾動，成功解決了GWO算法容易陷入局部最優(yōu)解的問題，增強了算法的全局搜索能力，使機械臂軌跡規(guī)劃算法能夠更快收斂，且各關(guān)節(jié)角位移、角速度和角加速度曲線均光滑、連續(xù)，提高了軌跡規(guī)劃的精度和速度。

1 關(guān)節(jié)空間軌跡規(guī)劃

設(shè)六維向量θ=[θ1,θ2,θ3,θ4,θ5,θ6]，θ1—θ6分別對應(yīng)6個關(guān)節(jié)角，則五次多項式定義為

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

式中：t為時間；a0—a5為多項式系數(shù)。

(2)

由五次多項式定義得

(3)

由式(2)和式(3)可以求出五次多項式的各項系數(shù)為

(4)

2 GWO算法

GWO算法源于灰狼種群的捕食機制。灰狼種群等級劃分如圖1所示。第1層是種群的領(lǐng)導(dǎo)者，稱為α狼，主要負(fù)責(zé)狩獵、食物分配等決策事宜。第2層為β狼，主要協(xié)助α狼進(jìn)行決策，當(dāng)α狼淘汰或死亡時，會有相應(yīng)的β狼來替補。β狼一方面將α狼的命令下達(dá)給其他灰狼，另一方面將其他灰狼的行動情況反饋給領(lǐng)導(dǎo)者α狼，起著重要的橋梁作用。第3層為δ狼,δ狼聽從α狼和β狼的命令，主要負(fù)責(zé)放哨、偵查、看護(hù)等任務(wù)。最底層為ω狼，主要負(fù)責(zé)種群內(nèi)部關(guān)系的平衡。

圖1 灰狼種群等級劃分

依據(jù)適應(yīng)度大小將狼群分為4個等級，其中最好的前3只灰狼依次為α，β和δ，其余的記為ω。GWO算法的主要思想如下：確定適應(yīng)度最好的3只灰狼α，β和δ，作為最了解獵物信息的灰狼組；在算法迭代時，整個狼群會在灰狼組α，β和δ的帶領(lǐng)下更新位置信息；狼群逐漸包圍攻擊獵物，以獲得最優(yōu)解。

GWO算法步驟如下：

(1)初始化狼群位置，設(shè)置最大迭代次數(shù)nmax，并初始化系數(shù)A,C,b。其中A和C為系數(shù)向量，A決定了更新后的灰狼位置向目標(biāo)物靠近的程度;C主要決定了更新后灰狼位置相對于目標(biāo)物的方位；系數(shù)b的變化決定了A的取值范圍，隨著算法迭代次數(shù)的增加，b由2線性遞減到0。計算狼群中每只灰狼的適應(yīng)度值，確定適應(yīng)度最好的3只灰狼為α，β，δ。

(2)根據(jù)灰狼位置更新規(guī)則計算灰狼更新后的位置。

(3)計算更新后的每只灰狼的適應(yīng)度值。

(4)更新α，β，δ狼的位置Xα，Xβ，Xδ及參數(shù)b，A，C的值。

(5)判斷是否滿足迭代條件，若滿足則返回到步驟(2)繼續(xù)執(zhí)行，否則輸出Xα及其適應(yīng)度值。

3 CS算法

CS算法是一種模仿杜鵑鳥尋窩產(chǎn)卵活動的群集智能優(yōu)化算法。CS算法中，每只杜鵑的寄生巢位置代表待求問題的1個可行解。每只杜鵑每次只會在1個寄生巢中產(chǎn)1個卵。在搜索鳥巢過程中，最優(yōu)秀的寄生巢才會被留到下一代。每個寄生巢的主人都有一定的概率察覺自己的巢中有外來卵而放棄該鳥巢。寄生巢被放棄后，杜鵑會重新隨機選擇1個鳥巢作為新的寄生巢。

為了生存繁衍，杜鵑有2種產(chǎn)卵策略：

(1)萊維飛行。杜鵑將通過萊維飛行所找到的寄生巢與之前的寄生巢對比，選擇較優(yōu)的寄生巢作為下一代的寄生巢。

(2)隨機選擇。每個寄生巢的主人都有一定概率發(fā)現(xiàn)自己的巢被寄生。發(fā)現(xiàn)后，杜鵑將隨機選擇1個新的鳥巢作為自己的寄生巢。

杜鵑搜索鳥巢的過程就是在當(dāng)前寄生巢的位置上按照一定的飛行規(guī)則尋找新的寄生巢的過程。該過程可描述為

xi(l+1)=xi(l)+ψ⊕L(λ)

(5)

式中：xi(l+1)為第i個寄生巢更新后的位置，l為當(dāng)前迭代次數(shù)；xi(l)為第i個寄生巢的當(dāng)前位置；ψ>0為控制步長的變量；⊕表示點乘運算；L(λ)為服從當(dāng)前迭代次數(shù)l的萊維隨機搜索路徑，其概率分布為

L～u1=l-λ, 1<λ<3

(6)

式中：u1為服從正態(tài)分布的變量；λ為指數(shù)變量。

4 CS-GWO算法

4.1 算法原理

灰狼的獵食過程可分為包圍獵物、狩獵和攻擊獵物3個階段。包圍獵物階段的行為可描述為

D=|CXp(n)-X(n)|

(7)

X(n+1)=Xp(n)-AD

(8)

C=2r2

(9)

A=2br1-b

(10)

(11)

式中:D為當(dāng)前灰狼與獵物的距離；Xp(n)為當(dāng)前獵物的位置向量，n為迭代次數(shù)；X(n)和X(n+1)為第n代和n+1代灰狼的位置向量；r1，r2為[0,1]的隨機向量。

狩獵時整個狼群會在α，β，δ的帶領(lǐng)下更新位置信息，該過程可描述為

(12)

(13)

(14)

式中：Dα，Dβ，Dδ為當(dāng)前灰狼與α，β，δ之間的距離；C1，C2，C3分別為Xα，Xβ，Xδ的隨機權(quán)重，其作用是增強或減弱3只最優(yōu)灰狼的影響，以改變灰狼的隨機搜索行為，可通過式(9)計算；A1，A2，A3分別決定了更新后位置為X1(n)，X2(n)，X3(n)的灰狼向3只最優(yōu)灰狼靠近的程度，可通過式(10)計算。

在GWO算法中，灰狼狩獵時對未知獵物的勘探能力主要取決于參數(shù)A和C。當(dāng)|A|>1時，灰狼個體會遠(yuǎn)離獵物位置，對空間進(jìn)行全局搜索；當(dāng)|A|<1時，灰狼向獵物發(fā)起攻擊，對附近的空間進(jìn)行局部搜索。參數(shù)C為獵物提供了一個隨機權(quán)重，當(dāng)|C|>1時，會強化獵物影響，灰狼要靠近獵物更加困難，會對空間進(jìn)行全局搜索；當(dāng)|C|<1時，會弱化獵物影響，使灰狼更加容易接近獵物，會對周圍空間進(jìn)行局部搜索。

GWO算法雖具有較強的搜索能力，但存在以下缺點：① 始終跟隨α,β,δ對狼群的位置進(jìn)行更新，這種優(yōu)秀灰狼組的引導(dǎo)導(dǎo)致算法很難跳出局部最優(yōu)解，容易發(fā)生早熟現(xiàn)象。② 在迭代后期，|A|<1時，提供的主要是局部搜索能力，缺少全局搜索能力，較易陷入局部最優(yōu)。

CS算法中有2種選擇寄生巢策略：萊維飛行和隨機選擇。在GWO算法中結(jié)合萊維飛行方式進(jìn)行搜索，可以擴(kuò)大搜索代理群體的搜索范圍，增加搜索代理個體搜索的隨機性，使算法在運行過程中能跳出當(dāng)前局部最優(yōu)點，尋找更優(yōu)解。采用Mantegna算法模擬萊維飛行模式，算法公式為

(15)

式中：s為萊維飛行的路徑；γ為萊維指數(shù)，取值范圍為[0,2]；u2，ν為服從正態(tài)分布的變量，其服從的正態(tài)分布為

(16)

(17)

式中Γ為Gamma函數(shù)，其定義為

(18)

將CS算法中出現(xiàn)的2次擾動過程融入到GWO算法的位置更新中，分2個部分進(jìn)行：第1部分是先對GWO算法中適應(yīng)度最好的α,β,δ的位置進(jìn)行擾動，然后選擇位置較好的灰狼，將萊維飛行方式應(yīng)用到前3只灰狼的位置更新中，再通過GWO算法計算狼群位置；第2部分是采用預(yù)先設(shè)定的發(fā)現(xiàn)概率對灰狼種群位置進(jìn)行擾動，并保留位置較好的灰狼，將CS算法中的鳥巢位置隨機更新方式應(yīng)用到整個狼群的位置更新中，使得狼群在向獵物逼近的過程中能夠隨機跳出局部搜索區(qū)域，擴(kuò)大了搜索范圍，避免算法陷入局部最優(yōu)解，增強了GWO算法的全局搜索能力。

4.2 算法流程

CS-GWO算法流程如圖2所示，具體實現(xiàn)步驟如下：

圖2 CS-GWO算法流程

(1)初始化種群和算法參數(shù)，設(shè)置最大迭代次數(shù)nmax、參數(shù)b,A和C及狼群被發(fā)現(xiàn)概率，計算狼群適應(yīng)度fitness，記錄α，β，δ的初始位置。

(2)使用萊維飛行機制對α，β，δ的位置進(jìn)行擾動，并保留較好的位置。

(3)根據(jù)式(14)更新每個灰狼個體的位置。

(4)判斷灰狼個體是否被發(fā)現(xiàn)，若被發(fā)現(xiàn)，則對整個灰狼種群的位置進(jìn)行擾動，對比更新前后位置，擇優(yōu)保留位置較好的灰狼；否則，保留上一代灰狼位置。

(5)計算種群中每個個體的適應(yīng)度fitness,更新Xα,Xβ,Xδ，b，A和C。

(6)判斷是否滿足迭代條件，若不滿足則輸出Xα及其適應(yīng)度fitness，若滿足則返回到步驟(2)繼續(xù)執(zhí)行。

5 基于CS-GWO算法的機械臂軌跡規(guī)劃

5.1 約束條件

表1 機器人關(guān)節(jié)角約束條件

5.2 目標(biāo)函數(shù)

在優(yōu)化目標(biāo)的選擇上，機器人工作效率和能量消耗一直是備受關(guān)注的問題。本文綜合考慮時間和能量為目標(biāo)函數(shù)，采用五次多項式插值法進(jìn)行軌跡規(guī)劃。對于1條由m個關(guān)鍵點H1,H2,…,Hm組成的軌跡，建立軌跡的能耗模型：

(19)

式中：E為消耗的能量；tj為點Hj與Hj+1之間的運動時間；τj為關(guān)節(jié)力矩；dqj為關(guān)節(jié)角位移；ω(t)為關(guān)節(jié)角速度。

通過加權(quán)系數(shù)法得到優(yōu)化目標(biāo)函數(shù)F(tf,E)：

(20)

式中：ξ1,ξ2為歸一化權(quán)因子;η為彈性系數(shù)。

最終建立的適應(yīng)度函數(shù)為

(21)

通過CS-GWO算法搜索適應(yīng)度函數(shù)值最大時的tf與E，即得到所需的最優(yōu)解。

6 仿真分析

6.1 仿真條件

為了驗證CS-GWO算法的效果，使用Matlab中的Robotics Toolbox工具箱進(jìn)行機械臂仿真建模。在仿真中，規(guī)劃出1條起始點為H1、中間點為H2、終點為H3的閉合工作路徑，給定點H1,H2,H3的位姿，根據(jù)逆運動學(xué)原理解得其對應(yīng)關(guān)節(jié)角。指定H1,H2,H3的初始速度和加速度均為0，H1到H2、H2到H3、H3到H1的運動時間分別為t1，t2，t3。設(shè)H1，H2，H3的位置為X1=(1.4 m 01.583 m)，X2=(2.3 m -0.6 m 0.75 m)，X3=(2.3 m -0.6 m 0.75 m)；末端姿態(tài)為R1=(0 -3.142 0)，R2=(-2.793 1.484 4.712)，R3=(-2.793 1.484 4.625)；解得對應(yīng)關(guān)節(jié)角：q1=(1.571 -1.571 0 0 1.571 -1.571)，q2=(1.189 -0.470 -0.655 1.483 1.4940.477)，q3=(1.721 -0.601 -0.356 1.8601.046 0.688)。

6.2 結(jié)果分析

設(shè)種群規(guī)模為120,最大迭代次數(shù)為50，分別用CS-GWO、GWO和CS算法進(jìn)行軌跡規(guī)劃。彈性系數(shù)η的值取決于時間和能量的增長速度的比率，本文設(shè)彈性系數(shù)η=0.003。ξ1,ξ2取經(jīng)驗值，ξ1=0.5，ξ2=0.5。仿真結(jié)果見表2，其中Std為最優(yōu)目標(biāo)函數(shù)的標(biāo)準(zhǔn)差。

從表2可知，CS-GWO算法尋得的最優(yōu)時間序列為(1.613 7 s，1.623 9 s，1.498 1 s)，尋得的平均最優(yōu)目標(biāo)函數(shù)值為10.679 2。相較于CS算法和GWO算法，CS-GWO算法得到的平均最優(yōu)目標(biāo)函數(shù)值最小，搜索到的解最優(yōu)，全局搜索能力最強。另外，CS-GWO算法的標(biāo)準(zhǔn)差最小，說明其穩(wěn)定性最好。

表2 軌跡規(guī)劃仿真結(jié)果

算法尋優(yōu)曲線如圖3所示。可以看出，3種算法的尋優(yōu)曲線最終趨勢一致，但從收斂速度來看，GWO算法和CS-GWO算法都能較快收斂到最優(yōu)值，明顯比CS算法的收斂速度快。

圖3 尋優(yōu)曲線對比

綜合來看，CS-GWO算法能夠有效提高CS算法的收斂速度，提高GWO算法的全局搜索能力，其穩(wěn)定性更好，整體性能較優(yōu)。優(yōu)化后關(guān)節(jié)1—關(guān)節(jié)6軌跡規(guī)劃仿真結(jié)果如圖4—圖9所示。從關(guān)節(jié)角位移仿真結(jié)果可以看出，機械臂末端軌跡角位移曲線均較平滑，說明仿真過程中6個關(guān)節(jié)能夠在規(guī)定的路徑點之間較平滑地進(jìn)行軌跡運動。從角速度曲線可以看出，6個關(guān)節(jié)的角速度變化都比較平穩(wěn)，說明運動過程中角加速度均沒有發(fā)生突變，且均滿足機械臂約束條件。光滑、連續(xù)變化的角速度和角加速度軌跡曲線說明機械臂在運動過程中所受的沖擊力小，能夠保證電動機平穩(wěn)運行，提高了機器人的工作效率。仿真結(jié)果驗證了將CS-GWO算法應(yīng)用于機器人機械臂軌跡規(guī)劃的可行性與有效性。

(a)關(guān)節(jié)角位移

(a)關(guān)節(jié)角位移

(a)關(guān)節(jié)角位移

(a)關(guān)節(jié)角位移

(a)關(guān)節(jié)角位移

(a)關(guān)節(jié)角位移

7 結(jié)語

為了使煤礦救援機器人機械臂有效規(guī)劃出一條可行關(guān)節(jié)角軌跡，提出了一種基于CS-GWO算法的煤礦救援機器人機械臂軌跡規(guī)劃算法。以五次多項式插值為基本軌跡規(guī)劃方法，采用CS-GWO算法對得到的軌跡進(jìn)行優(yōu)化，利用CS算法中的萊維飛行模式和鳥巢位置隨機更新方式對基本GWO算法中灰狼位置進(jìn)行擾動，使得狼群能夠跳出局部最優(yōu)解，擴(kuò)大搜索范圍，增強全局搜索能力。仿真結(jié)果表明：與基本GWO算法和CS算法相比，CS-GWO算法能夠以較快的速度收斂到最優(yōu)值，且尋得的平均最優(yōu)目標(biāo)函數(shù)值更??；從各關(guān)節(jié)優(yōu)化后的運動軌跡曲線可以看出，各關(guān)節(jié)角位移、角速度和角加速度曲線均光滑、連續(xù)，角速度和角加速度無突變且滿足約束條件。仿真結(jié)果驗證了CS-GWO算法在搜索能力方面的提升，以及將其應(yīng)用在煤礦救援機器人機械臂軌跡規(guī)劃上的可行性與有效性。