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

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

0 引言

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

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

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

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

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

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

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

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

(1)

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

(2)

由五次多項(xiàng)式定義得

(3)

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

(4)

2 GWO算法

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

圖1 灰狼種群等級(jí)劃分

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

GWO算法步驟如下：

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

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

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

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

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

3 CS算法

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

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

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

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

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

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

(5)

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

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

(6)

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

4 CS-GWO算法

4.1 算法原理

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

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]的隨機(jī)向量。

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

(12)

(13)

(14)

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

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

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

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

(15)

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

(16)

(17)

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

(18)

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

4.2 算法流程

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

圖2 CS-GWO算法流程

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

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

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

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

(5)計(jì)算種群中每個(gè)個(gè)體的適應(yīng)度f(wàn)itness,更新Xα,Xβ,Xδ，b，A和C。

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

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

5.1 約束條件

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

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

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

(19)

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

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

(20)

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

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

(21)

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

6 仿真分析

6.1 仿真條件

為了驗(yàn)證CS-GWO算法的效果，使用Matlab中的Robotics Toolbox工具箱進(jìn)行機(jī)械臂仿真建模。在仿真中，規(guī)劃出1條起始點(diǎn)為H1、中間點(diǎn)為H2、終點(diǎn)為H3的閉合工作路徑，給定點(diǎn)H1,H2,H3的位姿，根據(jù)逆運(yùn)動(dòng)學(xué)原理解得其對(duì)應(yīng)關(guān)節(jié)角。指定H1,H2,H3的初始速度和加速度均為0，H1到H2、H2到H3、H3到H1的運(yùn)動(dòng)時(shí)間分別為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)；解得對(duì)應(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í)間和能量的增長(zhǎng)速度的比率，本文設(shè)彈性系數(shù)η=0.003。ξ1,ξ2取經(jīng)驗(yàn)值，ξ1=0.5，ξ2=0.5。仿真結(jié)果見(jiàn)表2，其中Std為最優(yōu)目標(biāo)函數(shù)的標(biāo)準(zhǔn)差。

從表2可知，CS-GWO算法尋得的最優(yōu)時(shí)間序列為(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)，全局搜索能力最強(qiáng)。另外，CS-GWO算法的標(biāo)準(zhǔn)差最小，說(shuō)明其穩(wěn)定性最好。

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

算法尋優(yōu)曲線如圖3所示?？梢钥闯?，3種算法的尋優(yōu)曲線最終趨勢(shì)一致，但從收斂速度來(lái)看，GWO算法和CS-GWO算法都能較快收斂到最優(yōu)值，明顯比CS算法的收斂速度快。

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

綜合來(lái)看，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é)果可以看出，機(jī)械臂末端軌跡角位移曲線均較平滑，說(shuō)明仿真過(guò)程中6個(gè)關(guān)節(jié)能夠在規(guī)定的路徑點(diǎn)之間較平滑地進(jìn)行軌跡運(yùn)動(dòng)。從角速度曲線可以看出，6個(gè)關(guān)節(jié)的角速度變化都比較平穩(wěn)，說(shuō)明運(yùn)動(dòng)過(guò)程中角加速度均沒(méi)有發(fā)生突變，且均滿足機(jī)械臂約束條件。光滑、連續(xù)變化的角速度和角加速度軌跡曲線說(shuō)明機(jī)械臂在運(yùn)動(dòng)過(guò)程中所受的沖擊力小，能夠保證電動(dòng)機(jī)平穩(wěn)運(yùn)行，提高了機(jī)器人的工作效率。仿真結(jié)果驗(yàn)證了將CS-GWO算法應(yīng)用于機(jī)器人機(jī)械臂軌跡規(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é)語(yǔ)

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