付樂樂，陳 宏，鞏偉杰

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

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

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

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

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

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

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

2 改進(jìn)蟻群算法

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

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

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

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

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

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

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

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

2.4 蟻周模型的改進(jìn)

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

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

3 B 樣條曲線分段擬合

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

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

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

4 實(shí)驗(yàn)仿真與分析

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

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

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

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

在低密度即簡(jiǎn)單柵格靜態(tài)環(huán)境下，觀察3 種算法（傳統(tǒng)算法、文獻(xiàn)[8]改進(jìn)算法以及本文改進(jìn)算法）的規(guī)劃效果和收斂曲線情況，其對(duì)比結(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)，文獻(xiàn)[8]和本文算法整體尋優(yōu)效果較好，且本文改進(jìn)算法中迭代次數(shù)更優(yōu)，路徑更短。此實(shí)驗(yàn)下3 種算法的性能指標(biāo)效果對(duì)比情況如表2所示。

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

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

在簡(jiǎn)單柵格環(huán)境下，為避免算法偶然性，對(duì)高密度柵格環(huán)境再次進(jìn)行傳統(tǒng)蟻群算法、文獻(xiàn)[8]改進(jìn)算法、本文算法比較，其運(yùn)動(dòng)軌跡和收斂曲線變化趨勢(shì)如圖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

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

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

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

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

4.2 動(dòng)態(tài)環(huán)境實(shí)驗(yàn)仿真

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

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

5 結(jié)語

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