煤礦救援機(jī)器人機(jī)械臂軌跡規(guī)劃算法研究

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

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

0 引言

惡劣的開采環(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ī)劃方法存在奇異解問題，而關(guān)節(jié)空間軌跡規(guī)劃方法不會(huì)出現(xià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ī)劃軌跡不夠光滑、能量消耗高的問題。因此，研究者提出了基于改進(jìn)智能優(yōu)化算法的軌跡規(guī)劃方法。李國(guó)洪等[14]通過聚類方法對(duì)遺傳算子進(jìn)行綜合調(diào)控，并通過新的變異操作提高算法后期的尋優(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é)問題的神經(jīng)模糊網(wǎng)絡(luò)，通過對(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ī)劃不合理、算法收斂速度慢等問題，為了同時(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)解的問題，增強(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)的β狼來替補(bǔ)。β狼一方面將α狼的命令下達(dá)給其他灰狼，另一方面將其他灰狼的行動(dòng)情況反饋給領(lǐng)導(dǎo)者α狼，起著重要的橋梁作用。第3層為δ狼,δ狼聽從α狼和β狼的命令，主要負(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算法是一種模仿杜鵑鳥尋窩產(chǎn)卵活動(dòng)的群集智能優(yōu)化算法。CS算法中，每只杜鵑的寄生巢位置代表待求問題的1個(gè)可行解。每只杜鵑每次只會(huì)在1個(gè)寄生巢中產(chǎn)1個(gè)卵。在搜索鳥巢過程中，最優(yōu)秀的寄生巢才會(huì)被留到下一代。每個(gè)寄生巢的主人都有一定的概率察覺自己的巢中有外來卵而放棄該鳥巢。寄生巢被放棄后，杜鵑會(huì)重新隨機(jī)選擇1個(gè)鳥巢作為新的寄生巢。

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

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

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

杜鵑搜索鳥巢的過程就是在當(dāng)前寄生巢的位置上按照一定的飛行規(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 算法原理

灰狼的獵食過程可分為包圍獵物、狩獵和攻擊獵物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)下更新位置信息，該過程可描述為

(12)

(13)

(14)

式中：Dα，Dβ，Dδ為當(dāng)前灰狼與α，β，δ之間的距離；C1，C2，C3分別為Xα，Xβ，Xδ的隨機(jī)權(quán)重，其作用是增強(qiáng)或減弱3只最優(yōu)灰狼的影響，以改變灰狼的隨機(jī)搜索行為，可通過式(9)計(jì)算；A1，A2，A3分別決定了更新后位置為X1(n)，X2(n)，X3(n)的灰狼向3只最優(yōu)灰狼靠近的程度，可通過式(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)行過程中能跳出當(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)過程融入到GWO算法的位置更新中，分2個(gè)部分進(jìn)行：第1部分是先對(duì)GWO算法中適應(yīng)度最好的α,β,δ的位置進(jìn)行擾動(dòng)，然后選擇位置較好的灰狼，將萊維飛行方式應(yīng)用到前3只灰狼的位置更新中，再通過GWO算法計(jì)算狼群位置；第2部分是采用預(yù)先設(shè)定的發(fā)現(xiàn)概率對(duì)灰狼種群位置進(jìn)行擾動(dòng)，并保留位置較好的灰狼，將CS算法中的鳥巢位置隨機(jī)更新方式應(yīng)用到整個(gè)狼群的位置更新中，使得狼群在向獵物逼近的過程中能夠隨機(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)注的問題。本文綜合考慮時(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é)角速度。

通過加權(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ù)值最大時(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é)果見表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)差最小，說明其穩(wěn)定性最好。

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

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

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

綜合來看，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ī)械臂末端軌跡角位移曲線均較平滑，說明仿真過程中6個(gè)關(guān)節(jié)能夠在規(guī)定的路徑點(diǎn)之間較平滑地進(jìn)行軌跡運(yùn)動(dòng)。從角速度曲線可以看出，6個(gè)關(guān)節(jié)的角速度變化都比較平穩(wěn)，說明運(yùn)動(dòng)過程中角加速度均沒有發(fā)生突變，且均滿足機(jī)械臂約束條件。光滑、連續(xù)變化的角速度和角加速度軌跡曲線說明機(jī)械臂在運(yùn)動(dòng)過程中所受的沖擊力小，能夠保證電動(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算法中的萊維飛行模式和鳥巢位置隨機(jī)更新方式對(duì)基本GWO算法中灰狼位置進(jìn)行擾動(dòng)，使得狼群能夠跳出局部最優(yōu)解，擴(kuò)大搜索范圍，增強(qiáng)全局搜索能力。仿真結(jié)果表明：與基本GWO算法和CS算法相比，CS-GWO算法能夠以較快的速度收斂到最優(yōu)值，且尋得的平均最優(yōu)目標(biāo)函數(shù)值更??；從各關(guān)節(jié)優(yōu)化后的運(yùn)動(dòng)軌跡曲線可以看出，各關(guān)節(jié)角位移、角速度和角加速度曲線均光滑、連續(xù)，角速度和角加速度無(wú)突變且滿足約束條件。仿真結(jié)果驗(yàn)證了CS-GWO算法在搜索能力方面的提升，以及將其應(yīng)用在煤礦救援機(jī)器人機(jī)械臂軌跡規(guī)劃上的可行性與有效性。