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

（河北工業(yè)大學(xué) 機械工程學(xué)院，天津 300401）

0 引言

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

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

1 蟻群算法

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

根據(jù)式(1)選擇下一個節(jié)點：

圖1 8鄰域節(jié)點搜索圖

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

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

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

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

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

2 A*算法

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

傳統(tǒng)A*算法使用廣泛，其規(guī)劃出的路徑存在控制效率低，轉(zhuǎn)折點多等問題。

3 改進融合算法

3.1 優(yōu)化期望啟發(fā)式函數(shù)

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

3.2 改進信息素更新方法

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

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

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

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

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

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

3.3 設(shè)置選擇概率因子

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

3.4 設(shè)置安全值

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

圖2 機器人經(jīng)過障礙物

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

圖3 設(shè)置安全值

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

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

圖4 回溯法

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

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

4 實驗驗證

4.1 仿真實驗驗證

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

設(shè)置蟻群初始參數(shù)，如表1所示。傳統(tǒng)蟻群算法和改進融合蟻群算法規(guī)劃路徑如圖6、圖7所示。根據(jù)環(huán)境特點，設(shè)置起始點為（15，1），途徑兩個點（25，25），（5，25），最后回到起始點。

表1 改進融合蟻群算法參數(shù)

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

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

圖6 傳統(tǒng)蟻群算法路徑規(guī)劃

圖7 改進融合蟻群算法路徑規(guī)劃

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

圖8 傳統(tǒng)蟻群算法收斂曲線

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

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

表2 兩種算法仿真結(jié)果對比

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

4.2 基于ROS系統(tǒng)的機器人實驗驗證

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

圖11 機器人硬件架構(gòu)圖

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

圖10 基于ROS系統(tǒng)的機器人

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

表3 傳感器型號

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

圖12 傳統(tǒng)蟻群算法實驗過程

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

表4 兩種算法實驗結(jié)果對比

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

圖14 傳統(tǒng)蟻群算法路徑規(guī)劃圖

圖15 改進融合蟻群算法路徑規(guī)劃圖

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

5 結(jié)語

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