基于改進融合蟻群算法的機器人路徑規劃方法研究

趙天亮，張小俊，張明路，于方程

（河北工業大學 機械工程學院，天津 300401）

0 引言

隨著人工智能領域的繁榮，機器人進入了高速發展階段，提升智能化程度成為了機器人目前追求的高級目標，其中路徑規劃是指在環境條件已知或未知的情況下，機器人從起始點目標點規劃出一條能耗低，距離短，時間少的安全無碰撞路徑[1]。

路徑規劃算法包括A*算法、神經網絡算法、遺傳算法、模擬退火算法、蟻群算法、粒子群算法等。蟻群算法具有良好的適應性和魯棒性，但也存在冗余節點多、收斂速度慢、易陷入局部最優等問題。本文針對蟻群算法收斂速度慢，冗余節點多，距離障礙物過近，容易陷入局部最優等問題進行了改進，提出了改進融合蟻群算法。將A*算法的啟發式搜索思想融入到蟻群算法中，增加起始點和目標點對搜索過程的影響；提出新的最優路徑及周邊路徑信息素更新方式，避免算法陷入局部最優，設置安全值避免出現機器人距離障礙物過近的情況。在Matlab 2020a中完成了傳統蟻群算法和改進融合蟻群算法的對比仿真實驗，在實驗室進行機器人實驗，實驗結果表明改進融合蟻群算法具有可行性，有效解決距離障礙物過近的問題，規劃路徑距離更短，規劃效率顯著提升[2]。

1 蟻群算法

蟻群算法是根據蟻群覓食規律而來的仿生智能算法。在蟻群算法中，第k只螞蟻從節點i移動到節點j由多個因素決定，包括路徑上的信息素的濃度τij、信息素重要性因子α、啟發式信息重要性因子β和期望啟發式信息ηij。

根據式(1)選擇下一個節點：

圖1 8鄰域節點搜索圖

節點i和節點j之間的距離dij為歐式幾何距離，歐式幾何的距離如式(3)所示。

第k只螞蟻從節點i 移動到節點j后，留在路徑上的信息素會隨時間揮發。更新當前最優路徑上的信息素濃度，使下一次迭代的螞蟻傾向于走全局最優路徑。更新信息素濃度如式(4)所示。

其中，p(0<ρ<1)表示信息素的揮發水平，1-p的結果表示某-路徑上信息素揮發后的殘留水平，p越大，信息素揮發速度越快。

△rij;(t)是蟻群中所有螞蟻釋放的信息素之和的增量，表示第k只螞蟻在t時刻釋放的信息素增量，螞蟻總數為m，信息素增量計算公式如式(5)所示。

式(5)中，Q為螞蟻在路徑上釋放的信息素強度，L表示第k只螞蟻經過搜索得到的全局最優路徑長度。

2 A*算法

A*算法核心思想是根據給定的估值函數大小判斷搜索路徑是否為全局最優路徑。其估值函數如式(6)所示。

傳統A*算法使用廣泛，其規劃出的路徑存在控制效率低，轉折點多等問題。

3 改進融合算法

3.1 優化期望啟發式函數

其中hjg表示機器人從當前位置j運動至終點g時的代價函數，將式(9)帶入狀態轉移概率式(1)，改進后的狀態轉移概率如威(10)所示。

3.2 改進信息素更新方法

隨著蟻群搜索代數增多，地圖.上殘留的蟻群信息素越來越多，這就導致第N次蟻群搜索時容易出現局部最優的情況[3]。為了避免原始蟻群算法陷入局部最優問題，本文提出一種基于自我調節的信息素更新方式:當第N-1波蟻群完成覓食后，不僅更新當前情況下最優路徑上殘留的信息素濃度，還更新當前情況下最優路徑周圍的可通行路徑上殘留的信息素分布情況，再投放第N波螞蟻。自我調節式的信息素更新方法如式(11)所示。

更新當前情況下最優路徑上的信息素濃度時如式(12)所示。

其中:n表示當前情況下最優路徑_上的信息素濃度改變程度，0<1<1,2越大，變化程度越高。

其中:n表示當前情況下最優路徑_上的信息素濃度改變程度，0<1<1,2越大，變化程度越高。

其中，y表示周圍的可通行路徑信息素濃度變化程度，γ∈N,距離最優路徑越近，y的取值越大，信息素濃度變化程度越高，γγ也會相應減小;相反，距離當前最優路徑越遠，此路徑上信息素濃度變化程度越低。

算法更新后，機器人最優路徑及其周圍可通行路徑上的信息素濃度隨著迭代次數的增加進行不同比例更新，在迭代過程中根據上一代蟻群殘留的信息素濃度進行自我調整，能更快地找到最優解。

3.3 設置選擇概率因子

傳統蟻群算法存在容易陷人局部最優解的缺點，容易找到局部最短的路徑，而忽略全局最優路徑。設選擇概率因子為q。和隨機數因子為q。此時算法選擇下-一個節點策略滿足式(14)所示。

3.4 設置安全值

傳統蟻群算法在規劃機器人路徑時會出現如圖2所示的情況，灰色節點為障礙物節點。在實際應用中，機器人從A點走到B點的過程中會途徑C點，在C點會出現機器人距離障礙物過近的情況。為了提高運動安全性，本文提出采用設置安全值的方法實現避障。

圖2 機器人經過障礙物

改進后的蟻群算法從起始節點出發，將起點周圍所有可移動的節點都放進Openlist表中，根據狀態轉移概率選出最小值的節點x放進Closelist表中，同時將此點作為下一個移動節點。如圖3所示，r為增設的安全值，表示路徑上的點距離障礙物的長度，根據此地圖信息，其此值設置為0.707，當出現r＜0.707時，則通過判斷障礙物位置增加額外距離。將此點從Closelist表中刪除，并從上一節點開始搜索，重新尋找最新節點，重復上述過程，直到找到符合安全值的點。

圖3 設置安全值

設置安全值會增加轉折點數量，為了消除這一影響，利用上述回溯法重新計算其父節點的可移動節點，并且從剩下的節點中根據狀態轉移概率找到一條從起點到當前節點的路徑。

為了保證路徑的平滑，當節點更新時，需要將其父節點同時更新。采用回溯法重新計算其父節點的可移動節點時，從余節點中根據式(14)找到新的路徑。重新布線可減少冗余節點。如圖4所示，在圓心為C半徑為r的圓內存在A點和B點，通過式(3)計算A到B到C的代價和B到C的代價，如果f(A-C)＞f(B-C)，那么選取B作為父節點，如果f(A-C)＞f(B-C)，選取A作為父節點。為進一步優化路徑，參考RRT算法中重新布線的思想，計算總代價值，確定是否重新布線。

圖4 回溯法

綜上所述，改進融合蟻群算法流程如圖5所示。

圖5 改進融合蟻群算法流程圖

4 實驗驗證

4.1 仿真實驗驗證

本文采用MATLAB R2020a進行仿真實驗。首先，建立一個30×30的柵格圖。在地圖中，障礙物設置為黑色區域，機器人無法通過，可以通過的區域設置為白色。

設置蟻群初始參數，如表1所示。傳統蟻群算法和改進融合蟻群算法規劃路徑如圖6、圖7所示。根據環境特點，設置起始點為（15，1），途徑兩個點（25，25），（5，25），最后回到起始點。

表1 改進融合蟻群算法參數

圖6為傳統蟻群算法仿真路徑圖，圖7為改進融合蟻群算法仿真路徑圖；圖8為傳統蟻群算法的收斂曲線圖，圖9為改進融合蟻群算法的收斂曲線圖，其中，上方的曲線代表平均路徑長度，下方的曲線代表最小路徑長度[4]。

從圖6可以看出，使用傳統蟻群算法進行路徑規劃時，路徑轉折點多，路線出現多處距離障礙物過近的情況，在x=7，y=23處，出現局部最優的情況。從圖7中可以看出，相同環境下，相比于傳統蟻群算法，改進融合蟻群算法轉折點明顯減少，路徑更平滑，未出現局部最優情況，全局路徑未出現距離障礙物過近的情況。

圖6 傳統蟻群算法路徑規劃

圖7 改進融合蟻群算法路徑規劃

圖8、圖9為傳統蟻群算法和改進融合蟻群算法在柵格地圖下的收斂曲線，上方曲線代表每次迭代過程搜索出來的平均距離，下方曲線代表每次迭代過程搜索出來的最小距離。圖8為傳統蟻群算法收斂曲線，可知最小路徑迭代到50代附近趨于平穩，搜索出來的最優路徑長度為90，拐點數目為25。圖9為改進融合蟻群算法收斂曲線，可知蟻群最小路徑迭代到36代附近趨于平穩，搜索出來的最優路徑長度為85，拐點數目為15。

圖8 傳統蟻群算法收斂曲線

圖9 改進融合蟻群算法收斂曲線

相同環境中對傳統蟻群算法和改進蟻群算法各進行20次仿真實驗，表2分別列出了傳統蟻群算法和改進融合蟻群算法在平均路徑長度，平均收斂速度，平均拐點數目和平均運行時間方面的對比[5]。

表2 兩種算法仿真結果對比

20仿真實驗結果表明，改進融合蟻群算法具有可行性，且和傳統蟻群算法相比，改進融合蟻群算法搜索的平均路徑長度從93.21減少到88.91，減小22.2%，迭代次數從平均迭代49代減小到平均迭代38代，收斂速度提升51.4%，平均拐點數目從27個較少到15個，減少40%，平均運行時間從12.36s減少到10.25s，減少25.8%。其中，第9次和第16次改進融合蟻群算法出現平均搜索時間過長的情況，因為設置了安全值，使得單只螞蟻搜索時間略有增加，將信息素重要性因子á從1.2調節為1.3即可改善這種情況。

4.2 基于ROS系統的機器人實驗驗證

采用基于ROS系統的機器人作為平臺進行實驗。如圖10所示，此平臺包括深度攝像頭、思嵐A1激光雷達、超聲波傳感器等硬件，其整體架構如圖11所示。

圖11 機器人硬件架構圖

圖10中，通過深度攝像頭和思嵐A1激光雷達聯合掃描建立地圖模型，并將點云圖轉換柵格地圖。傳感器接收到環境信息后，由串口傳遞給計算機。計算機處理信號轉化成位置信息及運動指令，并將運動指令通過串口發送給STM32控制板，控制板發送運動指令給左右電機驅動器，實現運動控制。

圖10 基于ROS系統的機器人

機器人中傳感器型號如表3所示。

表3 傳感器型號

實驗采用的軟件系統中，路徑規劃算法以插件的形式嵌入在“move_base”模塊中。在“move_base”模塊中將系統原有的A*算法替換為上述改進融合蟻群算法，設置實驗參數和仿真實驗相同。此實驗中輸入量包括靜態環境信息，攝像頭信息，激光雷達信息，設定好的坐標變換信息和機器人目標位姿。其中靜態環境信息在實驗前使用SLAM激光雷達掃描生成。系統內置軌跡跟蹤模塊，設定相同的起點和終點，蟻群算法和改進融合蟻群算法路徑規劃過程如圖12和圖13所示，實驗結果如圖14和圖15所示，在機器人上對兩種算法分別進行20次路徑規劃實驗，實驗平均數值如表4所示。

圖12 傳統蟻群算法實驗過程

圖13 改進融合蟻群算法實驗過程

表4 兩種算法實驗結果對比

圖14可以看出，將傳統蟻群算法應用在機器人路徑規劃實驗時，規劃路徑長，轉折點多，并非最佳路徑。從圖15可以看出改進融合蟻群算法規劃出的路徑轉折點減少，曲線更加平滑，路徑長度明顯減小，和障礙物保持合適的距離，機器人行走更加平穩。

圖14 傳統蟻群算法路徑規劃圖

圖15 改進融合蟻群算法路徑規劃圖

20次實驗平均結果表明，在相同環境下，改進融合蟻群算法能完成路徑規劃任務，路徑較為平滑，轉折點少，未出現局部最優及距離障礙物過近的情況，具有可行性。傳統蟻群算法規劃路徑長度為4.02m，尋路時間為18s，改進融合蟻群算法規劃路徑為3.05m，尋路時間為10s。兩者相比，改進融合蟻群算法規劃的路徑更優，路徑長度減少了24%，尋路時間減少了44.4%，將改進融合蟻群算法應用在機器人上，會使機器人路徑規劃性能明顯提升。

5 結語

傳統蟻群算法進行全局路徑規劃時存在冗余節點多，收斂速度慢，距離障礙物過近等問題。本文研究目的為改善傳統蟻群算法上述問題，提高路徑規劃性能。在傳統蟻群算法的基礎上，融合了A*算法的啟發式搜索策略，提出改進融合蟻群算法。首先，設置了選擇概率因子，提高了算法收斂速度；其次，引入自我調節機制對迭代過程中的最優路徑及周圍的信息素進行更新，改善了可能出現的局部最優解的情況；設置了安全值避開了障礙物邊界，避免在實際應用中出現距離障礙物過近的情況。在Matlab中使用柵格法搭建環境模型，實驗表明，在同一環境下，改進融合蟻群算法在路徑長度，收斂速度，拐點數目，運行時間方面均優于傳統蟻群算法。最后將傳統蟻群算法和改進融合蟻群算法應用在機器人中，通過結果對比可以看出，在全局路徑規劃中，改進融合蟻群算法能高效，準確，快速地搜索出安全路徑，轉折點更少，路徑更平滑，搜索時間更少，不會出現局部最優的情況。下一步工作設置更多工況進行仿真實驗，進一步優化改進融合蟻群算法。