一種未知環境下的移動機器人路徑規劃方法

彭 湘，向鳳紅，毛劍琳

(昆明理工大學 信息工程與自動化學院，昆明 650500)

1 引 言

近年來，隨著人工智能領域的逐步擴張[1]，智能機器人在各個行業都得到了較為廣泛的應用.移動機器人的路徑規劃技術是機器人學[2]中一個重要分支[3]，是指機器人在存在障礙物的解空間內運動，搜尋到抵達目標點的最優解[4]或次優解，該最優解或次優解能夠根據人類期望的機器人性能決定，如路徑最優、能耗最優[5]以及路徑最平滑等，也可以是一個或多個約束條件的加權.

移動機器人的路徑規劃方法根據應用環境可區分為全局和局部環境下的路徑規劃，針對不同的環境下的路徑規劃采用的方法也有所差別.全局路徑規劃[6]方法主要有蟻群算法、A*算法[7]、遺傳算法等，局部路徑規劃方法[8]主要有人工勢場法、DWA算法等.在實際應用中，單一算法通常僅能處理某一限定環境下的路徑規劃問題，例如蟻群算法，它是一種通過模擬蟻群覓食規則的群體智能優化算法[9]，在靜態路徑規劃問題中能夠表現出良好的魯棒性、正反饋性和并行性，但蟻群算法在應用到局部環境時，會出現搜索停滯或與動態障礙物碰撞等問題.同理，將人工勢場法等局部路徑規劃算法用于全局環境時，也易出現收斂速度慢、存在局部最優解等弊端.單一算法在處理路徑規劃問題時，由于算法本身的缺陷常常導致規劃的路徑并非最優解，近年來學者們在研究路徑規劃問題時常將不同算法融合[10]，通過算法之間優劣互補，從而得到較優的規劃結果.文獻[11]提出將A*算法與蟻群算法融合，利用A*算法的評價函數改進啟發信息函數，提高了算法的運行效率和路徑的平滑度.但算法僅在全局環境下進行仿真實驗，未考慮局部環境下躲避動態障礙物的問題，因此實際適用范圍較窄.文獻[12]將人工勢場法的力信息轉換為蟻群信息素，改善了算法收斂速度慢和易陷入局部最優解等缺點，但規劃的路徑仍存在較多拐點，路徑平滑程度較低.

針對以上問題，為了兼顧全局與局部環境下的移動機器人路徑規劃，鑒于實際應用中機器人的行走環境往往是未知的，本文提出一種針對可能同時存在任意環境，即移動機器人在未知環境下的路徑規劃方法.通過融合蟻群算法與人工勢場法，將未知環境分為全局和局部環境進行分層規劃，即首先采用蟻群算法進行全局路徑規劃.在移動機器人前進的過程中，若機器人自身攜帶的傳感器檢測到將要與動態障礙物碰撞，此時調用局部避障方法-人工勢場法進行局部避障.避障結束后返回全局規劃，最后，對輸出路徑進行平滑優化.規劃步驟分成環境建模、搜索路徑、優化處理3部分進行.

2 環境建模

柵格法[13]環境建模.假設移動機器人所處環境為柵格規模大小M2(M=1，2，…，n)的二維有限空間C，即C?M2.此工作空間被劃分為自由空間Cfree和障礙空間Cobs.Cfree∪Cobs=C.為了區分不同種類柵格，用不同的值在柵格矩陣中抽象表示.Cfree中柵格的值表示0，Cobs中包含有靜態障礙物與可移動障礙物，用1表示靜態障礙物，2表示可移動障礙物.柵格法是一種經典的建模方法，由于柵格法只能近似地表示實際環境，因此實際應用中需對障礙物進行膨脹處理，即：對實際大小小于單位柵格的障礙物按單位柵格大小處理.令柵格圖的左下角為坐標原點，創建笛卡爾坐標系，橫軸為X，縱軸為Y，對M2個柵格從上至下、由左往右編號，依次為1，2，…，M2，當移動機器人處于某一柵格Pi時，若以Pi為中心柵格的八鄰域柵格內存在自由柵格，移動機器人便能夠將這些自由柵格加入候選可移動柵格中.令任意一個柵格坐標為(xi，yi)，移動機器人的行走半徑Rrobot=1，(xi，yi)與第i個柵格間的關系可用公式(1)表示：

(1)

進行分層路徑規劃前，對工作柵格空間C作如下假設：1)全局環境中障礙物為靜態障礙物，障礙柵格在整個規劃過程中保持靜止，移動機器人在進行全局規劃前已知全局環境障礙物分布情況；2)局部環境中含有動態障礙物，障礙物柵格狀態發生不確定性變化，障礙物運動軌跡未知；3)移動機器人自身攜帶有傳感器，可以實時掃描一定范圍內環境變壞情況.

3 基于蟻群算法的全局路徑規劃

移動機器人的路徑規劃過程是給定機器人某一任務，機器人依靠算法的決策和自身攜帶的傳感器感知環境信息[14]，在給定解空間內完成該任務.蟻群算法(ACO)最早由Marco Dorigo等人1991年提出[15]，是一種經典的求解最優問題的算法，大自然中螞蟻的覓食機制與機器人路徑搜索具有較高的相似度，使得蟻群算法在路徑規劃領域有較好的運用.蟻群算法規則簡單，在給定空間，螞蟻個體夾帶信息素，由單個螞蟻個體組成蟻群，通過群間相互合作、競爭與信息素交換，經過揮發系數和信息素等的反饋作用，從而尋找到食物源.但蟻群算法也存在容易陷入“死鎖”，前期搜尋路徑存在“盲目性”，收斂時間長、易出現局部最優解等弊端[16].

3.1 屏蔽U型陷阱措施

在全局路徑規劃開始前，移動機器人需要識別Cobs內的U型陷阱.U型陷阱是指柵格地圖中以n個自由柵格為中心，若自由柵格左右兩側柵格均為障礙柵格，并且在兩側柵格的任意一端有障礙柵格將兩側柵格相連接，從而將此區域柵格組成一個類似字母U的半封閉空間.本文采用分層路徑規劃方法，由于局部環境中動態障礙物運動狀態未知，因此在局部環境中無法進行U型環境識別，移動機器人在進行路徑規劃前僅需對全局環境下的障礙柵格進行U型陷阱識別.

螞蟻在解空間C內進行搜索，環境中存在U型陷阱.螞蟻搜索時進入U型陷阱，若能夠逃脫，螞蟻需要退出U型陷阱這一過程，才能繼續進行搜索.若無法逃脫，便會導致螞蟻困在U型環境內，陷入“死鎖”，從而停止搜索.考慮到U型陷阱必然導致螞蟻搜索時間變長、搜索不到最優解或直接停止搜索，為此提出一種屏蔽U型陷阱措施.螞蟻在搜索時將U型陷阱視為障礙物，從而避開U型陷阱搜索.識別U型陷阱的方法如下：

(2)

其中：i為任意序號的柵格，x表示大于0的整數；M為柵格地圖橫縱坐標最大值，n為整數，n=1…nmax，nmax=[1，M-1]；1為柵格矩陣中障礙柵格的值.當柵格之間的關系滿足公式(2)所示條件時，即為U型環境.

3.2 全局規劃過程

3.2.1 ACO路徑規劃

傳統蟻群算法(ACO)進行全局路徑規劃步驟如下：

1)初始化：柵格法環境建模，設置起始點S、目標點G，將S加入禁忌表Tabu中；初始化蟻群算法參數，包括螞蟻個數m，啟發因子α，信息素因子β，信息素增強系數Q，局部信息素揮發系數ε和全局信息素揮發系數ρ，螞蟻最大迭代次數Ncmax等；

(3)

其中：ηij為啟發信息，ηij=1/dij；τij為路徑i→j上螞蟻留下的信息素濃度；α和β分別為信息素因子和啟發因子；allowedk為八鄰域內候選可移動柵格集合.

3)更新Tabu；

4)重復步驟1)至步驟3)直到螞蟻抵達目標點；

5)信息素更新：

a)局部更新：螞蟻每更新一次路徑，便對路徑上的局部信息素進行更新，更新方法如公式(4)所示：

τij(t+1)=(1-ε)τij(t)+ετ0

(4)

其中：ε為局部信息素揮發系數，ε∈(0，1]；τ0為初始信息素值，用單位矩陣表示.

b)全局更新：在m只螞蟻結束一次迭代以后，僅對此次迭代搜尋到最優路徑上的信息素進行全局更新：

τij(t+1)=(1-ρ)τij+ρΔτij(t，t+1)

(5)

(6)

其中：ρ為全局信息素揮發系數，ρ∈(0，1]；Δτij(t，t+1)為1個時間間隔Δt內最優路徑中各節點的信息素增量；Lbest為本次迭代搜索到的最優路徑長度.此步驟通過加強Lbest上各節點的信息素濃度，從而產生正反饋.

6)若Nc=Ncmax，全局規劃結束；否則清空禁忌表，Nc=Nc+1，返回步驟(2)，直到Nc=Ncmax.

3.2.2 IACO

針對ACO在路徑規劃問題中存在的全局性較差、收斂時間長和容易陷入局部最優解等問題，對ACO作以下改進：

1)改進啟發函數：由于ACO啟發函數的確定僅考慮了節點i→j之間的歐式距離dij，蟻群缺少目標點的向導作用，因此前期搜索較為盲目，導致收斂速度緩慢，極易陷入局部最優.故本文從算法的全局性出發，設計新的啟發函數：

(7)

其中：A?[0，+∞).

改進啟發函數加入了候選節點j到G的歐式距離djG，使螞蟻在選擇下一節點時受目標點的影響，目標點在整個搜索過程中對螞蟻起持續的向導作用.能夠加快算法的尋優功效，同時還加強了算法的全局性.

2)信息素闕值限制：借鑒最大最小螞蟻系統[17](MMAS)思想，限制信息素的大小范圍，預防收斂過程中因信息素過少而導致收斂速度慢或直接停止搜索，或因信息素積累過多而出現收斂過快，導致陷入局部最優解的情況.

(8)

其中：τmax和τmin為設置的信息素闕值，若信息素超過設定值，將會按照公式(8)重置.

4 局部路徑規劃

4.1 人工勢場算法描述

人工勢場法(APF)[18-19]的思想是，假設空間C內存在虛擬勢場U，U中目標點對移動機器人產生一引力勢場Uatt，障礙物對機器人產生一斥力勢場Urep，且U=Uatt+Urep.移動機器人在Uatt和Urep的共同作用下，從而規劃出一條無碰路徑.

其中：Uatt表示為：

(9)

其中：Katt表示引力勢場位置增益系數；d2(P，G)表示當前位置P到G的歐氏距離.

障礙物的斥力與機器人和障礙物間的距離有關，距離越短，斥力越大.環境中通常存在多個障礙物，假設環境中有n個障礙，第i個障礙產生的斥力勢場Urep表示為：

(10)

其中：Krep為斥力勢場位置增益系數，Krep?C；d(P，Oi)表示P到第i個障礙的歐氏距離；d0表示障礙物斥力場的影響距離，d0?C.對Uatt和Urep求導，可計算機器人在P處的引力和斥力值分別為：

引力：

Fatt(P)=-?Uatt(P)=Krepd(P，G)

(11)

斥力為：

(12)

移動機器人在位置P受到的合力F大小為：

(13)

局部規劃過程中，合力F=0處，移動機器人所受斥力與引力大小相等，可能會因此陷入局部極小值而停止搜索.此時，由于全局路徑規劃過程已經結束，因此當機器人遭遇此種狀況時，可以沿全局規劃完成的路徑行走，此時便可跳出局部極小值點，繼續搜索.

4.2 局部路徑規劃過程

依靠移動機器人所攜帶傳感器檢測到的環境變化情況，本文借鑒預測控制滾動原理[20].令機器人從柵格i到下一個柵格j的運動時間為Δt，通過規劃過程中不斷更新地圖環境信息，通過不斷變化的環境預測障礙物的運動軌跡，并判斷在下個窗口內是否會與動態障礙物相撞.局部路徑規劃流程如圖1所示.

圖1 局部規劃過程

5 拐角優化

由于路徑是在柵格地圖中生成，因此生成的路徑必然會出現拐角，為了避免機器人不必要的能量損失，使用中點替代策略，對生成的路徑做尖峰處理，處理后得到一條平滑路徑.平滑步驟如下：

Step 1.令路徑中任意兩相鄰線段交點坐標為(xold，yold)，夾角為θ；

Step 2.設定角度闕值φ，判斷θ是否大于φ，若否則取兩線段的中點為(xnew1，ynew1)、(xnew2，ynew2)、連接兩點成為新線段，若是，返回Step1，繼續比較下一節點；

Step 3.判斷新線段與鄰邊的夾角θ1、θ2是否大于角度闕值φ，若是，則(xnew1，ynew1)、(xnew2，ynew2)代替(xold，yold)成為新的節點，若否，繼續Step 2；

Step 4.判斷整條路徑是否比較完畢，若是，優化結束，否則，轉到Step 1.

6 結果分析

6.1 分層路徑規劃流程

移動機器人進行分層路徑規劃，規劃步驟如下：

Step 1.初始化參數；

Step 2.識別全局環境及全局環境中的U型陷阱；

Step 3.采用IACO進行全局路徑規劃；

Step 4.若檢測到未知障礙物，調用APF進行局部規劃，避障結束，返回Step 2，否則，繼續進行局部規劃；

Step 5.到達目標點，輸出路徑，否則轉回Step 2；

Step 6.對輸出路徑進行優化處理.

6.2 實驗仿真

為了檢驗所提方法的可行性和有效性，在MATLAB 2014a仿真平臺上進行試驗仿真.實驗設置移動機器人的工作環境為M×M的柵格地圖，S和G的柵格序號分別為1和M2.對實驗參數的設置如表1所示.

表1 參數設定值

為了更好的體現算法的搜索能力和避障效果，使用柵格大小為20×20，并含有多U型陷阱的復雜環境地圖，S=1，G=400，障礙物覆蓋率為46%.移動機器人識別全局環境中的U型陷阱，將識別后的U型陷阱用“+”表示，結果如圖2所示.在搜索時使用屏蔽U型陷阱措施，避開U型陷阱移動.

實驗1首先在全局環境下驗證IACO進行全局規劃的能力.圖3是全局環境下ACO與IACO的規劃結果，可以看到，IACO能夠成功規劃出一條全局最優路徑，并且避開了環境中所有的U型陷阱.對比ACO與IACO，IACO明顯改善了規劃結果，IACO規劃的最優路徑長度相較于ACO長度減少了4.58；從圖4收斂曲線對比可以明顯看出，振幅較大ACO收斂曲線，在設定100次迭代內無法收斂，收斂過程頻繁震蕩，幅度較大，而IACO(點線)在僅11迭代后即收斂，并且，曲線在僅4次震蕩就搜索到最優路徑.并且收斂中不存在搜索到最小值后再產生震蕩的過程，有效地減少了收斂時間，同時，IACO規劃的路徑拐點較少，拐角角度均為鈍角，規劃結果體現了IACO優越的性能.ACO與IACO對比結果如表2所示.

表2 ACO與IACO對比結果

為了進一步檢驗分層規劃這一方法在未知環境下規劃能力，在實驗1的基礎上，實驗2在環境中加入了動態障礙物A、B，初始位置分別為A(6.5，13.5)，B(7.5，9.5).設置動態障礙物的運動機制為在一個時間周期Δt內向前運動一個柵格，A從上往下運動，B從左往右運動.

圖5是未知環境下的避障結果，可以看到，移動機器人能夠成功避開運動的障礙并尋找到一條最佳路徑.在時刻1，動態障礙物A由初始位置(6.5，13.5)運動到(6.5，12.5)，B由位置(7.5，9.5)運動到(8.5，9.5).移動機器人在此過程中執行分層規劃，在時刻1之前，移動機器人按照全局規劃生成的路徑行進，在遭遇動態障礙物A和B時調用APF進行局部規劃，規劃結果如圖5(a)所示，若時刻1局部規劃結束后，環境中不存在動態障礙物，則返回全局規劃.

移動機器人沿著路徑繼續往前運動，在兩個Δt時間間隔后，由于時刻2空間C內仍存在動態障礙物，因此需要再次進行局部路徑規劃，時刻2動態障礙物A向下移動了兩個單位柵格，B向右移動了兩個單位柵格，規劃結果如圖5(b)所示.時刻2結束后，在一定范圍內未檢測到運動的障礙物，移動機器人將當前所處位置(10.5，7.5)作為起始點，返回全局規劃，按照圖5(b)后半地圖所示規劃結果進行運動直到路徑規劃結束.

對比圖5(b)中時刻2平滑前的曲線和平滑后(點線)的曲線，可以看到，平滑前的路徑中有10個拐點，對輸出路徑進行平滑以后，路徑長度較處理前明顯縮短，拐點個數也大大減少.為了更直觀的檢驗平滑效果，分別觀察每一個拐點處的平滑結果，可見平滑后雖然仍舊存在拐點，但拐點角度均較大，幾乎接近180°，在實際應用中不會造成移動機器人在移動過程中過多耗能，使得移動機器人能夠流暢地經過尖峰路徑.

7 結 論

目前，移動機器人單一環境下的路徑規劃技術已經較為成熟，研究熱點主要聚焦于存在環境無法預知的路徑規劃問題.針對并含靜態與動態障礙物的未知環境下的路徑規劃問題，提出將蟻群算法與人工勢場法結合.

1)首先采用改進蟻群算法進行全局路徑規劃，提出一種屏蔽U型陷阱措施，能夠避免在搜索過程因陷入U型陷阱而停滯搜索，無法前進的情況；在啟發信息里加入目標點的引導，加強了蟻群的全局搜尋能力；同時，為了避免螞蟻因信息素值大小而出現的局部最優解和結束搜索的問題，從而引進了最大最小螞蟻系統的思想，將信息素限制在一定范圍內.

2)在規劃過程中，當移動機器人在搜索過程檢測到動態障礙物并預測到將要與它碰撞時，調用人工勢場法進行局部規劃，規劃結束后，將當前點作為起始點，返回全局規劃.

3)最后，將規劃好的路徑輸出并對其做優化處理.

實驗在多U型環境下成功規劃出一條平滑的路徑，驗證了所提方法的良好性能.