基于改進蟻群算法的水下機器人路徑規(guī)劃

付樂樂，陳 宏，鞏偉杰

（深圳大學(xué) 機電與控制工程學(xué)院，深圳 518061）

近幾年，機器人路徑規(guī)劃在醫(yī)療、海洋、林業(yè)、航空等行業(yè)發(fā)揮了關(guān)鍵作用。路徑規(guī)劃的首要任務(wù)是在靜態(tài)或者動態(tài)阻礙物的環(huán)境中，根據(jù)相應(yīng)約束條件，尋找一條沒有阻礙的路徑[1]。如今的規(guī)劃算法已相對成熟，常見規(guī)劃算法按不同的分類有全局路徑算法、局部路徑算法、圖搜索、采樣規(guī)劃算法等[2]，通過考察不同性能指標(biāo)，不同算法有著各自的特性，如遺傳算法可能跌入局部最優(yōu)，A*算法中啟發(fā)函數(shù)無法充分確定，較易跌入死循環(huán)，而蟻群算法因為具備穩(wěn)定性、魯棒性等特點在水下機器人路徑規(guī)劃問題解決中得到大量應(yīng)用[3-4]。

文獻[5]中提出一種融合軌跡規(guī)劃算法；文獻[6]提出額外增加信息素方法；文[7]提出多目標(biāo)規(guī)劃策略。本文中不斷嘗試，在傳統(tǒng)蟻群算法上調(diào)整了動態(tài)揮發(fā)因子，信息素更新方式及蟻周模型等，然后在動態(tài)環(huán)境下將優(yōu)化蟻群算法和人工勢場法融合避障，使算法更加高效、更加穩(wěn)定、更加收斂，在Matlab 中進行算法仿真，仿真表明改進的算法無論在靜態(tài)環(huán)境下還是動態(tài)環(huán)境下都具有一定穩(wěn)定性、可行性和高效性。

1 環(huán)境模型建立

建立柵格法地圖容易實現(xiàn)[5]，且描述規(guī)范，故本文用柵格法建模。環(huán)境中黑色是障礙物，白色是可走路徑。環(huán)境中依照上到下、左至右的次序，對每一個柵格標(biāo)碼[6]，柵格的長度為單位長度1，柵格點坐標(biāo)表達式為

式中：b 是單位柵格的邊長；mod 是取余數(shù)運算的值；MM 是地圖的維度；ceil 是沿著正無窮大方向四舍五入后取值；xi，yi為每個柵格坐標(biāo)位置[7]。柵格法只能相似模擬機器人環(huán)境，生活中物體常為不規(guī)則形狀，故采取膨脹法對障礙物處理。網(wǎng)格對應(yīng)環(huán)境模型如圖1所示，行和列都為20 個柵格。從左至右、從上至下對柵格依次標(biāo)記序號1，2，…，N2，每個柵格都有對應(yīng)坐標(biāo)和序號[8]。

圖1 柵格法環(huán)境模型Fig.1 Environment model of grid method

2 改進蟻群算法

2.1 動態(tài)調(diào)整揮發(fā)因子

蟻群算法的揮發(fā)因子與收斂速率有關(guān)，揮發(fā)因子不易過大或過小。在傳統(tǒng)算法中信息素揮發(fā)因子不會隨著環(huán)境變化而改變。本文提出的信息素揮發(fā)因子與迭代次數(shù)相關(guān)，當(dāng)接下來的5 次迭代中若最小距離收斂某一值，將揮發(fā)因子ρ 相應(yīng)減小，繼續(xù)去尋找更短的距離，信息素揮發(fā)因子ρ 公式如下：

2.2 啟發(fā)函數(shù)的改進

傳統(tǒng)蟻群算法中啟發(fā)函數(shù)ηij=1/dij，dij表示螞蟻前進時從節(jié)點i 走至柵格下一可行節(jié)點j 的距離，本文中啟發(fā)函數(shù)不再是當(dāng)前點到下一可行節(jié)點的距離的倒數(shù)，而是對螞蟻從柵格i 行走至目標(biāo)點的方向進行引導(dǎo)，啟發(fā)函數(shù)改進如下：

式中：die為當(dāng)前點到目標(biāo)點距離，將螞蟻向目標(biāo)點方向引導(dǎo)，有利于快速收斂，縮短距離。

2.3 信息素更新規(guī)則的改進

受到最大最小螞蟻系統(tǒng)啟發(fā)，對信息素濃度規(guī)則更新方式進行修改，如式（4）所示：

式中：τmin是所允許的最小信息素濃度；τmax是所允許的最大信息素濃度，超過最大、最小值后將值賦予臨界值，若沒有超過臨界值將不做任何處理，通過反復(fù)仿真實驗，將τmax設(shè)為1，τmin設(shè)為0.2 為最優(yōu)參數(shù)。

2.4 蟻周模型的改進

經(jīng)反復(fù)測試與仿真，可將蟻周模型進一步完善，蟻周模型改進如下：

當(dāng)?shù)趉-1 只螞蟻走ij 中路徑大于第k 只螞蟻走ij 中路徑時，即λ>1，此時Lk路徑上信息素增量增加，隨螞蟻迭代不斷變化為最優(yōu)值，反之Lk路徑上信息素增量將會減小。

3 B 樣條曲線分段擬合

雖然改進后算法可獲得最佳路徑，距離、拐點等均可達到最優(yōu)，但考慮到實際，避障路徑的光滑性格外重要，因此在較完善的修改算法上獲得最優(yōu)路徑后，對此進行B 樣條光滑處理，使曲線能夠在圖中被建立出來[9]。

考慮到曲線效果與計算簡便性，文中使用三次B樣條曲線方法，可求出樣條曲線與控制點間的關(guān)系為

式中：Pi，Pi+1，Pi+2和Pi+3為蟻群算法求出 的控制點，由于具有多個控制點，采取多點樣條曲線分段擬合策略。以Pn-3，Pn-2，Pn-1和Pn四點繪出第n-3 條B 樣條曲線，按此規(guī)律繪出控制點所有的曲線段，然后將繪制的各段曲線平滑連接，形成一條C2 級連續(xù)的光滑曲線。

4 實驗仿真與分析

在仿真軟件Matlab 的R2021a 版本上對改進算法收斂性能及最優(yōu)性能進行測試，處理器型號為Intel（R）Core（TM）i7-10510U CPU@1.80 GHz 2.30 GHz，在仿真前將3 種算法置于同一環(huán)境和設(shè)備下，與傳統(tǒng)蟻群算法和文獻[8]算法路徑規(guī)劃效果進行對比，驗證算法的可靠性，該實驗中初始仿真參數(shù)設(shè)置如表1所示。

表1 實驗參數(shù)設(shè)置Tab.1 Experimental parameter settings

4.1 靜態(tài)環(huán)境實驗仿真

4.1.1 低密度柵格環(huán)境仿真結(jié)果分析

在低密度即簡單柵格靜態(tài)環(huán)境下，觀察3 種算法（傳統(tǒng)算法、文獻[8]改進算法以及本文改進算法）的規(guī)劃效果和收斂曲線情況，其對比結(jié)果如圖2和圖3所示。

圖2 低密度柵格環(huán)境路徑規(guī)劃效果Fig.2 Path planning effect of simple grid environment

圖3 低密度柵格環(huán)境收斂曲線效果Fig.3 Convergence curve effect of simple grid environment

傳統(tǒng)蟻群算法中，初始信息素是相等的，導(dǎo)致易陷入局部最優(yōu)，文獻[8]和本文算法整體尋優(yōu)效果較好，且本文改進算法中迭代次數(shù)更優(yōu)，路徑更短。此實驗下3 種算法的性能指標(biāo)效果對比情況如表2所示。

表2 低密度柵格環(huán)境下算法規(guī)劃效果對比Tab.2 Comparison of planning effects of algorithms in simple grid environment

4.1.2 高密度柵格環(huán)境仿真結(jié)果分析

在簡單柵格環(huán)境下，為避免算法偶然性，對高密度柵格環(huán)境再次進行傳統(tǒng)蟻群算法、文獻[8]改進算法、本文算法比較，其運動軌跡和收斂曲線變化趨勢如圖4和圖5所示。

圖4 高密度柵格環(huán)境路徑規(guī)劃效果Fig.4 Planning effect of mazy grid environment

圖5 高密度柵格環(huán)境收斂曲線效果Fig.5 Convergence curve effect of mazy grid environment

不論在簡單環(huán)境下，還是在復(fù)雜環(huán)境下，對比可發(fā)現(xiàn)本文算法的路徑更優(yōu)，不管在什么環(huán)境下，在獲得最優(yōu)路線后，由于水下機器人在水中的路徑平滑且連續(xù)，故最優(yōu)路徑不可以直接運用到水下機器人上，與實際路線誤差較大，為減小水下機器人沿最優(yōu)路徑游動的耗能，對文中最佳路徑采取三次B 樣條曲線平滑策略，處理后的路徑規(guī)劃結(jié)果如圖6所式。

圖6 B 樣條平滑處理后路徑軌跡Fig.6 Path trajectory after smoothing by cubic B-spline

通過對3 種算法進行對比，可以看出本文算法收斂迭代次數(shù)最小，路徑長度最小，其拐點數(shù)目也相對最少，本文改進算法的效率和收斂速度得到提高，說明了本文算法總體效果要高于傳統(tǒng)蟻群算法和文獻[8]算法的總體效果，并沒有出現(xiàn)傳統(tǒng)蟻群算法那樣有較大的震蕩。在沒有進行B 樣條平滑處理前，實驗在復(fù)雜環(huán)境下的3 種算法路徑運行情況對比如表3所示。

表3 高密度柵格環(huán)境下算法規(guī)劃效果對比Tab.3 Comparison of algorithm planning effect in mazy grid environment

4.2 動態(tài)環(huán)境實驗仿真

在動態(tài)環(huán)境下，某物體勻速朝某方向運動，本文采用了一種全局路徑和局部路徑相融合方法，在采用改進蟻群算法得到整體路徑規(guī)劃后，沿規(guī)劃路徑前進，當(dāng)前進過程中檢測到動態(tài)障礙物時，切換至局部路徑通過人工勢場算法重新規(guī)劃，直到成功躲避動態(tài)障礙物。而以最短距離避開動態(tài)障礙物后，接著繼續(xù)切換成改進的蟻群算法進行全局避障，周而復(fù)始，直到行走到目標(biāo)點，采用混合算法動態(tài)避障過程如圖7所式。

圖7 動態(tài)障礙物路徑規(guī)劃Fig.7 Path planning of dynamic obstacle

5 結(jié)語

本文在傳統(tǒng)蟻群算法上不斷改進，使改進算法應(yīng)用效果更強，并對改進后路徑光滑處理，更好應(yīng)用到實際中。在靜態(tài)環(huán)境以及動態(tài)環(huán)境下進行驗證，通過對比收斂迭代次數(shù)、最小路徑距離、拐點數(shù)目等，結(jié)果表明：本文改進算法更加的優(yōu)越，在收斂速度，最優(yōu)路徑距離方面有明顯提升，且轉(zhuǎn)折點個數(shù)大大減少。在以后工作中將對本文內(nèi)容深入研究進行三維路徑規(guī)劃，規(guī)劃出更切合實際場景的路徑。