基于蝙蝠算法-人工勢場的機器人路徑規(guī)劃研究

（上海理工大學(xué)，上海 200093）

0 引言

近年來智能機器人領(lǐng)域發(fā)展飛速，移動機器人在各個領(lǐng)域中都得到了廣泛的應(yīng)用和發(fā)展，如智能倉儲、貨物碼垛、無人機偵查、未知環(huán)境探索、深海探索、機械加工等，例如，焊接機器人利用粒子群算法實現(xiàn)焊接過程中避障的路徑規(guī)劃[1]；礦山救援機器人利用改進遺傳算法實現(xiàn)在礦山救援任務(wù)中的路徑規(guī)劃問題[2]。在考慮到環(huán)境的復(fù)雜，障礙物交錯，機器人自主尋找出一條安全、可靠的最短路徑是機器人研究中的關(guān)鍵技術(shù)。工程中，最短路徑可以節(jié)約機器人運動過程的時間成本，提高工作效率；路徑減少銳角轉(zhuǎn)向，可減少機器人運動過程中不必要的損耗，減少后期維護成本。國內(nèi)外學(xué)者對機器人路徑規(guī)劃問題做了大量的工作并取得不少成果并運用到了各個領(lǐng)域中。A*算法、傳統(tǒng)人工勢場、強化學(xué)習(xí)方法、深度學(xué)習(xí)方法[3～5]等是較常用的路徑規(guī)劃算法，這些算法或多或少存在著難以滿足實際要求的問題。如A*算法復(fù)雜度較大，內(nèi)存占用量大，不適用于追求靈活運動的小型系統(tǒng)的機器人的路徑規(guī)劃中。基本人工勢場法在迭代過程中容易陷入局部最優(yōu)解，產(chǎn)生路徑震蕩[4]。近年來，機器人路徑規(guī)劃研究逐漸向仿生智能算法轉(zhuǎn)移，對于機器人路徑規(guī)劃問題上，仿生智能算法更具有先天優(yōu)勢。如蟻群算法、人工魚群算法、遺傳算法、粒子群算法[6,7]等。粒子群算法容易陷入局部最優(yōu)解，蟻群算法搜索迭代時間長，容易停滯等缺點。因此研究對環(huán)境適應(yīng)性強、算法步驟簡單、求解速度快和路徑最短成為研究者們追求的目標(biāo)。

1 基本算法

1.1 蝙蝠算法

蝙蝠的生物學(xué)機理較簡單，集群生活，通過聲波回聲定位和搜索獵物，搜索獵物時發(fā)射低頻脈沖。劍橋?qū)W者Yang于2010年，根據(jù)模擬蝙蝠回聲定位的仿生學(xué)機理提出了蝙蝠算法[3]。蝙蝠全局搜索獵物時，發(fā)射低頻脈沖，聲波響度大，能量損耗小，傳播距離長，能快速鎖定獵物大致方位。當(dāng)聲波傳遞到獵物后，蝙蝠接收回傳聲波信號，結(jié)束全局搜索。此時開始局部搜索，并發(fā)射高頻脈沖。高頻脈沖的特點是，聲波響度小，聲波間距小，傳播距離短，能量損耗大。聲波間距小，對于獵物的形狀大小能精確檢測。蝙蝠利用隨機飛行策略將獵物距離告知其他蝙蝠，同時更新位置和速度。目前蝙蝠算法已經(jīng)在電力系統(tǒng)廣域協(xié)調(diào)控制[6]、AGV物料配送調(diào)度[8]、車輛調(diào)度路徑[10]等領(lǐng)域得到了實際的應(yīng)用。

基本的蝙蝠算法中，蝙蝠群體中單一個體被視為單一的粒子，粒子無質(zhì)量和體積，僅代表空間中的每一個目標(biāo)函數(shù)的一個可行解，通過比較目標(biāo)函數(shù)值的大小，即可判斷蝙蝠位置是否處于最優(yōu)位置。蝙蝠飛行過程中不斷更新自身位置和速度，因此在機器人路徑規(guī)劃的求解過程中可以利用蝙蝠飛行、捕捉獵物的一系列行為進行類比。蝙蝠算法的仿生思路為：蝙蝠開始搜索獵物時發(fā)出聲波響度大，脈沖頻率低的聲波，為了能在一個較大空間中尋找全局最優(yōu)；在發(fā)現(xiàn)獵物后，降低聲波響度，提高脈沖頻率，為了能在局部空間精確尋找定位獵物。

在此，由于蝙蝠發(fā)出的聲波是間斷的，脈沖頻率是指單位時間內(nèi)聲波的頻數(shù)，而不指聲波震動的頻率，聲波頻率取決于蝙蝠種類。

蝙蝠算法規(guī)則為：蝙蝠在d維空間中尋找獵物，即適應(yīng)度函數(shù)的最優(yōu)解。假設(shè)在t時刻，第i只蝙蝠位置為，其速度為，當(dāng)前最優(yōu)位置為x*，更新位置和速度公式如下：

其中，fi為第i只蝙蝠發(fā)出的搜索聲波頻率，fmax為蝙蝠搜索聲波的最大頻率，fmin為蝙蝠搜索聲波的最小頻率，因此fi∈[fmin，fmax]。

當(dāng)蝙蝠發(fā)現(xiàn)獵物，通過改變聲波響度和脈沖速率使全局搜索策略轉(zhuǎn)變?yōu)榫植克阉鞑呗浴p小聲波響度，增大脈沖速率，進行局部搜索，遵循以下公式：

這里需要再次強調(diào)的是，蝙蝠的搜索聲波頻率f和蝙蝠脈沖速率r是有非常大的區(qū)別。f指的是蝙蝠搜索時發(fā)出的一小段聲波的頻率，取值取決于需要解決的實際問題；r指的是蝙蝠每秒發(fā)出聲波的數(shù)量，局部搜索時，r將逐漸增大，直至找到最優(yōu)解，取值等于最大值。

1.2 人工勢場法

人工勢場法是通過設(shè)定終點坐標(biāo)為引力勢場，設(shè)定障礙物為斥力勢場，尋找躲避障礙物無碰撞的最優(yōu)路徑。隨著機器人運動過程中逐漸靠近障礙物，機器人和障礙物的距離越近，斥力越大，反之越小；機器人躲避障礙物后逐漸靠近終點，機器人和終點的距離越近，引力越小，反之越大。基本人工勢場法算法定義如下：

引力勢場函數(shù)Ugra：

其中機器人當(dāng)前位置為X=（x，y），終點位置為Xg=（xg，yg），k為大于零的任意引力場常數(shù)。

斥力勢場函數(shù)Urep：

其中m為大于零的任意斥力場常數(shù)，ρ為機器人到障礙物的距離，ρ0為障礙物的最大影響半徑。在引力勢場和斥力勢場的共同作用下，機器人受斥力和引力，朝合力最大的方向運動最終到達(dá)終點。

1.3 三次樣條插值

三次樣條插值用于平滑機器人路徑，從起點到終點上給定區(qū)間[a，b]上分割成n個插值點滿足：

若滿足以下條件，則在[a，b]上的一個函數(shù)S（x）就稱為三次樣條插值函數(shù)。

1）每個區(qū)間（xi，xi+1）（i=1，2，3，…，n）中，都有三次多項式函數(shù)：

2）S（x）、S'（x）和S''（x）在[a，b]上連續(xù)，且滿足

插值條件：

連續(xù)性條件：

其中滿足插值條件的方程個數(shù)共有n+1個，滿足連續(xù)性條件的方程個數(shù)共有3n-3個，因此共可建立4n-2方程，加上邊界條件后共有4n個方程，因此可求解出分段函數(shù)S（x）。

2 基于人工勢場的蝙蝠算法（BA-APF）

和其他路徑規(guī)劃的群集智能算法相似，蝙蝠算法在尋優(yōu)的過程中，存在種群多樣性和算法深度挖掘能力之間存在矛盾，兩者難以兼顧。對于路徑規(guī)劃算法的改進目標(biāo)都是針對保證種群多樣性和算法的快速收斂性，同時避免局部最優(yōu)。本文對蝙蝠路徑節(jié)點進行勢場化處理，引入擾動系數(shù)g改進ri脈沖速率，提高算法的挖掘深度能力。對相鄰兩個路徑節(jié)點[15]加入引力勢場。建立基于人工勢場的蝙蝠算法（Bat Algorithm-Artificial Potential Field，BA-APF）模型。對算法迭代Tmax次，最后求得最優(yōu)解。

2.1 動態(tài)擾動系數(shù)

當(dāng)蝙蝠發(fā)現(xiàn)獵物，即將接近全局最優(yōu)解時，基本蝙蝠算法將在最優(yōu)個體附近開始局部搜索策略。此時生成均勻分布的隨機數(shù)P作為判斷閾值[16]，當(dāng)P>r時，使用局部策略，否則重復(fù)全局搜索。因此，基本蝙蝠算法中蝙蝠是否進入局部搜索，完全取決于r值的取值。若r值的大小在算法前期一直保持一個較小的值，則可以增大算法進入局部搜索策略概率，較有效防止算法局部最優(yōu)，同時滿足蝙蝠算法要求。

其中，t是當(dāng)前迭代次數(shù)，Tmax是最大迭代次數(shù)。g值隨著迭代次數(shù)的增加而自適應(yīng)的增大，在算法前期擾動影響較小，在算法中后期仍然保持在一個相對較小的范圍中，例如，時，，引入擾動系數(shù)后的是基本蝙蝠算法中的0.7倍。改進蝙蝠脈沖速率r 后，BA-APF 算法保證擾動系數(shù)g 滿足1≥g>0，每次迭代過程都能保證系數(shù)的取值相比基本蝙蝠算法所獲得的取值更小，并能滿足基本蝙蝠算法脈沖速率的取值條件：保持增大到設(shè)定值。

2.2 基于人工勢場的改進

在機器人路徑規(guī)劃過程中，基本人工勢場僅僅對障礙物和終點設(shè)置斥力場和引力場[17]。然而，當(dāng)機器人所處的某點的引力勢場和斥力勢場大小相等方向相反時，基本人工勢場法容易發(fā)生鎖死，無法求解；當(dāng)陷入障礙物斥力場范圍疊加時，機器人路徑容易發(fā)生震蕩。

為同時保證BA-APF算法過程中路徑盡可能平滑，在BA-APF算法中引入人工勢場，設(shè)置相鄰兩個路徑節(jié)點勢場。

具體方法如下：

1）按式(6)對目標(biāo)點設(shè)置引力勢場。

2）按式(7)對障礙物中心設(shè)置斥力勢場，影響半徑為障礙物半徑。

3）路徑勢場化，方法如下：

圖1 路徑節(jié)點勢場化示意

路徑節(jié)點的設(shè)置方法如下：

3 適應(yīng)度函數(shù)的構(gòu)建

機器人路徑規(guī)劃需要避免與障礙物碰撞、尋求路徑最短和避免出現(xiàn)急轉(zhuǎn)。為保證路徑有效、最優(yōu)以及機器人運動穩(wěn)定性，BA-APF算法重復(fù)迭代t次，每一只蝙蝠代表一條機器人路徑均要求通過適應(yīng)度對比計算來判斷路徑優(yōu)劣。BA-APF算法需要求滿足上述條件，路徑規(guī)劃的適應(yīng)度函數(shù)構(gòu)造如下：

其中，式(18)中，L為機器人路徑總長度；（xm，ym）為機器人路徑節(jié)點坐標(biāo)；式(19)中，η和σ是初值為0的避障標(biāo)志變量和轉(zhuǎn)向標(biāo)志變量；μ為增大系數(shù)，僅用于增大標(biāo)志變量對適應(yīng)度值的影響，故取值大于1即可，為使增大效果更為直觀取值100。當(dāng)η和σ為0時，不對適應(yīng)度值產(chǎn)生影響；當(dāng)η和σ不為0時，增大適應(yīng)度值。

為了保證機器人路徑盡量避免產(chǎn)生銳角路徑，減小失控可能，利用式(20)三角形三邊關(guān)系對路徑節(jié)點的坐標(biāo)進行約束。

推導(dǎo)如下：

轉(zhuǎn)向標(biāo)志變量σ計算方法如下：

通過式(21)判斷相鄰的路徑節(jié)點連線的夾角是否小于90°，若小于90°有σ=σ+1；否則σ=σ。

對σ進行迭代，若路徑規(guī)劃過程中σ值恒為初值0，則機器人路徑不存在小于等于90的度轉(zhuǎn)向角，反之機器人轉(zhuǎn)向角過小，機器人運動穩(wěn)定性和可控性降低。

計算機器人當(dāng)前路徑節(jié)點與障礙物的距離dk來確定機器人路徑是否穿過障礙物（如圖1所示），若穿過障礙物則發(fā)生碰撞，路徑無效，避障標(biāo)志變量η>0。

η計算流程如下：

步驟1：按式(22)計算機器人路徑節(jié)點與所有障礙物的距離。式中，dk為機器人運動路徑節(jié)點距離障礙物的距離；（xobsk，yobsk）為障礙物輪廓圓圓心。

步驟2：按式(23)產(chǎn)生一個大于0集合θk。式中robs為障礙物輪廓圓半徑。

步驟3：若集合θk中元素平均值mean（θk）>0，η迭代加1，依據(jù)式(24)；否則η保留上一代的值。若迭代過程中η的值始終等于初值0，說明規(guī)劃的路徑均躲避了障礙物，反之路徑不滿足約束條件。

4 算法步驟

步驟1：初始化蝙蝠種群數(shù)量npop、蝙蝠聲波響度A、速度vx，vy、聲波頻率最大值fmax最小值fmin、蝙蝠個體個數(shù)d和人工勢場斥力系數(shù)m、引力系數(shù)kx、算法迭代次數(shù)t、最大迭代次數(shù)Tmax、障礙區(qū)個數(shù)nobs以及障礙位置坐標(biāo)

步驟2：初始化蝙蝠坐標(biāo)位置：

其中xi，yi分別為第i只蝙蝠的橫坐標(biāo)和縱坐標(biāo)，每只蝙蝠包含d個路徑節(jié)點，對蝙蝠路徑節(jié)點x1和路徑節(jié)點xd分別賦值起點xs和終點xT。初始化人工勢場法的斥力系數(shù)kc和引力系數(shù)kx，障礙物吸引半徑dx和排斥半徑dc，對障礙物中心設(shè)置斥力勢場和引力勢場。

步驟3：將蝙蝠坐標(biāo)代入式(22)、式(23)求出只蝙蝠的dk和θk，再利用式(21)、式(24)求出標(biāo)志變量η和σ，最后代入式(18)路徑L長度以及適應(yīng)度函數(shù)的值，并提取最佳蝙蝠的位置坐標(biāo)。

步驟4：將步驟3中的最佳蝙蝠的位置坐標(biāo)（路徑節(jié)點）勢場化，加入引力場式(16)、式(17)。利用三次樣條插值法求解出S（x）分段函數(shù)，將離散的路徑節(jié)點平滑連接，獲得連續(xù)平滑的路徑。

步驟5：將蝙蝠坐標(biāo)代入式(1)～式(3)進行位置更新，若閾值P>r，則進入局部搜索策略，利用式(4)、式(14)、式(15)更新聲波響度和脈沖速率，更新所有蝙蝠位置坐標(biāo)和速度；否則直接重復(fù)步驟3）。

步驟6：t=Tmax是否成立，是進入步驟7）；否則重復(fù)步驟3）。

步驟7：輸出路徑結(jié)果。

步驟8：結(jié)束。

5 實驗與結(jié)果分析

為了驗證本文算法在機器人路徑規(guī)劃問題的可行性和有效性，在路徑最短、路徑無銳角路線、無碰撞的前提下，在相同仿真實驗環(huán)境、迭代次數(shù)、種群規(guī)模條件下，對比BA-APF、粒子群算法、基本蝙蝠算法。

5.1 實驗參數(shù)設(shè)置與軟件

仿真實驗采用操作系統(tǒng)為Windows10，編程環(huán)境為MATLAB R2016a。路徑規(guī)劃環(huán)境尺寸為2000mm×2000mm、種群規(guī)模npop=100、最大迭代次數(shù)t=100、路徑節(jié)點（維度）個數(shù)d=20，三種算法上訴取值均保證一致；其中基本蝙蝠算法與勢場化蝙蝠算法中的蝙蝠聲波響度A0=0.25、r0=0.5、速度vx=0.5，vy=0.5、聲波波長根據(jù)環(huán)境和障礙物大小確定，最后利用波長取值范圍，可計算得出聲波頻率的取值范圍（fmax，fmin）=（7000，600）。在PSO算法中，個體經(jīng)驗C1和群體經(jīng)驗C2對結(jié)果有重要的影響，參考同類型實驗后，本文選用C1=C2=1.5。

5.2 路徑規(guī)劃結(jié)果分析

仿真實驗分別在單位面積下障礙物密度為N1個/m2和N2個/m2（N1<5，N2>5）的環(huán)境下進行。障礙物數(shù)量、位置和半徑均隨機產(chǎn)生。機器人從S點出發(fā)到達(dá)T點，其中黑色圓為障礙物的輪廓圓，障礙圓心設(shè)置斥力勢場，終點設(shè)置引力場，所獲路徑結(jié)果如圖3所示。

圖2 算法路徑規(guī)劃對比

表1 障礙物環(huán)境在N1個/m2環(huán)境下四種算法路徑長度對比

通過圖2、表1實驗數(shù)據(jù)可得出結(jié)論。障礙物密度在N1個/m2的環(huán)境的路徑規(guī)劃中，勢場化蝙蝠算法（BAAPF）無論在最優(yōu)解還是平均解中，所獲機器人路徑的長度、平滑程度或轉(zhuǎn)向角度均優(yōu)于另外兩種算法。機器人路徑均滿足約束條件，故路徑有效。BA-APF路徑規(guī)劃中，存在人工勢場的調(diào)整和干預(yù)，在躲避第二個障礙物時，路徑明顯優(yōu)于基本蝙蝠算法和粒子群算法，障礙物中心設(shè)置了斥力勢場，由于路徑節(jié)點勢場化的調(diào)整，路徑并沒有因此刻意躲避障礙物造成過度轉(zhuǎn)向。勢場化蝙蝠算法的路徑更為平穩(wěn)，波動也較小。

障礙物密度在N2個/m2的環(huán)境的路徑規(guī)劃中，障礙物數(shù)量、位置和半徑隨機產(chǎn)生。路徑結(jié)果如圖3、圖4所示。

圖3 十位數(shù)量級障礙環(huán)境下三種算法路徑規(guī)劃對比

圖4 三種算法迭代曲線對比圖

表2 十位數(shù)量級障礙物環(huán)境下四種算法路徑長度對比

通過圖3、和表1可知，障礙物密度在N2個/m2的環(huán)境的路徑規(guī)劃中，算法在路徑長度、收斂速度方面均優(yōu)于對比算法。

如圖4所示，對比其他算法BA-APF收斂速度最快，算法第40代時收斂，并求出最優(yōu)解。得益于算法中加入動態(tài)擾動系數(shù)g，擾動系數(shù)影響了蝙蝠脈沖速率，增大蝙蝠算法進入局部搜索策略次數(shù)，相比較BA算法的盲目搜索重復(fù)全局搜索策略，改進后的BA-APF算法提升了早期的收斂速度，一定程度的減小了算法迭代次數(shù)。

最后驗證BA-APF算法在Maklink圖論環(huán)境下基于Dijkstra算法下使用BA-APF算法二次規(guī)劃路徑。（Maklink圖論廣泛運用于機器人、飛行航線、船舶航道等問題的路徑規(guī)劃研究和工程中。Dijkstra算法基本思想是按照路徑長度增加順序疊加尋找最短路徑，是Makink圖論的底層計算算法）。

圖5 Maklink圖論實現(xiàn)

如圖5所示，BA-APF算法在Dijkstra算法規(guī)劃出的黃色軌跡的基礎(chǔ)上二次規(guī)劃出紅色路徑。路徑成功躲避障礙物，并以最短路徑抵達(dá)目標(biāo)。由此可以看出，在Maklink圖論中，基于Dijkstra算法下使用BA-APF算法二次規(guī)劃路徑，BA-APF算法也能有效的規(guī)劃出合理的運動路徑。BA-APF算法仍具有一定局限性，BA-APF算法適用于已知環(huán)境的路徑規(guī)劃，并規(guī)劃出適合的路徑。但在未知環(huán)境中，BA-APF算法需要提前輸入未知環(huán)境地圖的相關(guān)參數(shù)，或利用SLAM算法完成對實時場景的地圖構(gòu)建后，再利用BA-APF算法完成路徑規(guī)劃并反饋至機器人端完成對未知環(huán)境的路徑規(guī)劃。

6 結(jié)語

本文利用勢場化蝙蝠算法求解機器人路徑規(guī)劃問題的最優(yōu)解，規(guī)劃最優(yōu)路徑。利用動態(tài)擾動系數(shù)g對蝙蝠算法的脈沖速率r進行改進，增加算法進入局部搜縮策略次數(shù)，提升算法的挖掘能力，并一定程度減小算法盲目隨機搜索，保證了算法較快速收斂。加入人工勢場思想策略，對蝙蝠算法中位置路徑節(jié)點進行勢場化處理，路徑節(jié)點之間轉(zhuǎn)角進一步減小，同時對障礙物添加斥力勢場的基礎(chǔ)上加入應(yīng)力勢場影響機器人路徑。最后，利用插值法平滑曲線，多次迭代求得算法最優(yōu)解。建立了在多種環(huán)境下機器人路徑規(guī)劃的可行性方案。實驗結(jié)果表明，基于勢場化蝙蝠算法的路徑規(guī)劃求解性能優(yōu)于對比算法。