煤礦救援機器人機械臂軌跡規劃算法研究

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

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

0 引言

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

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

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

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

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

1 關節空間軌跡規劃

設六維向量θ=[θ1,θ2,θ3,θ4,θ5,θ6]，θ1—θ6分別對應6個關節角，則五次多項式定義為

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

(1)

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

(2)

由五次多項式定義得

(3)

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

(4)

2 GWO算法

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

圖1 灰狼種群等級劃分

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

GWO算法步驟如下：

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

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

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

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

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

3 CS算法

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

為了生存繁衍，杜鵑有2種產卵策略：

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

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

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

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

(5)

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

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

(6)

式中：u1為服從正態分布的變量；λ為指數變量。

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為當前灰狼與獵物的距離；Xp(n)為當前獵物的位置向量，n為迭代次數；X(n)和X(n+1)為第n代和n+1代灰狼的位置向量；r1，r2為[0,1]的隨機向量。

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

(12)

(13)

(14)

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

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

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

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

(15)

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

(16)

(17)

式中Γ為Gamma函數，其定義為

(18)

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

4.2 算法流程

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

圖2 CS-GWO算法流程

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

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

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

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

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

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

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

5.1 約束條件

表1 機器人關節角約束條件

5.2 目標函數

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

(19)

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

通過加權系數法得到優化目標函數F(tf,E)：

(20)

式中：ξ1,ξ2為歸一化權因子;η為彈性系數。

最終建立的適應度函數為

(21)

通過CS-GWO算法搜索適應度函數值最大時的tf與E，即得到所需的最優解。

6 仿真分析

6.1 仿真條件

為了驗證CS-GWO算法的效果，使用Matlab中的Robotics Toolbox工具箱進行機械臂仿真建模。在仿真中，規劃出1條起始點為H1、中間點為H2、終點為H3的閉合工作路徑，給定點H1,H2,H3的位姿，根據逆運動學原理解得其對應關節角。指定H1,H2,H3的初始速度和加速度均為0，H1到H2、H2到H3、H3到H1的運動時間分別為t1，t2，t3。設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)；末端姿態為R1=(0 -3.142 0)，R2=(-2.793 1.484 4.712)，R3=(-2.793 1.484 4.625)；解得對應關節角：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 結果分析

設種群規模為120,最大迭代次數為50，分別用CS-GWO、GWO和CS算法進行軌跡規劃。彈性系數η的值取決于時間和能量的增長速度的比率，本文設彈性系數η=0.003。ξ1,ξ2取經驗值，ξ1=0.5，ξ2=0.5。仿真結果見表2，其中Std為最優目標函數的標準差。

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

表2 軌跡規劃仿真結果

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

圖3 尋優曲線對比

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

(a)關節角位移

(a)關節角位移

(a)關節角位移

(a)關節角位移

(a)關節角位移

(a)關節角位移

7 結語

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