劉冰雁，葉雄兵，王新波，賈 珺，王 濤

(1. 軍事科學院，北京100091；2. 解放軍32032 部隊，北京100094)

無人地面車輛(UGV)，最初僅作為爆炸物清除設備，隨著城市化發(fā)展，如今已能夠擔當運輸、護送等多類型使命。然而，城市環(huán)境最大的特點是高樓林立，而無人地面車輛應用的最大難點則是規(guī)避路徑的自主規(guī)劃。國內外關于路徑規(guī)劃研究與實現(xiàn)的基本方法依據對周圍環(huán)境信息的掌控情況[1-4]，可大致可歸納為：全局路徑規(guī)劃方法和局部路徑規(guī)劃方法。全局路徑規(guī)劃，是指對全局環(huán)境信息完全已知，不需要進行環(huán)境的實時更新，根據已知信息建立合適的環(huán)境模型，再據此規(guī)劃出合適路徑的方法。典型的全局規(guī)劃法有：可視圖法、柵格法、自由空間法等，其中A*[5,6]、Dijkstra[7]算法最為常用。全局路徑規(guī)劃，由于事前已知環(huán)境信息不再需要大量采集周圍環(huán)境信息，能夠減少計算量，但其規(guī)劃效果卻與環(huán)境粒度劃分密切相關。局部路徑規(guī)劃，是一種在線規(guī)劃法，是指移動體不完全了解周圍環(huán)境，實時采集周圍環(huán)境信息，動態(tài)更新路徑的方法。典型的局部規(guī)劃法有：人工勢場法，遺傳算法[8]、蟻群算法、粒子群算法[9]和模糊邏輯算法等[10]。局部路徑規(guī)劃，是在環(huán)境信息完全未知或部分未知情況下進行，具有實時性強，響應速度快等優(yōu)點。

人工勢場法(Artificial Potential Field, APF)，是局部路徑規(guī)劃法中的一種[11,12]，與其它局部路徑規(guī)劃法算法的特性比較如表1 所示。相比較，人工勢場法具有數(shù)學描述清晰、運算迅速、計算量小、硬件要求低以及規(guī)劃路徑平滑等優(yōu)勢[13]，目前在無人機、無人車、仿生人等的路徑規(guī)劃研究中應用廣泛。例如，文獻[14][15]針對無人機航路規(guī)劃問題，借鑒人工勢場思想，提出了能夠滿足任務執(zhí)行指標并保障飛行安全的路徑規(guī)劃方法。文獻[16][17]針對移動不夠靈活、易入“陷阱”的機器人路徑規(guī)劃問題，發(fā)揮人工勢場法便于控制、路徑平滑的優(yōu)勢，提出了路程短、效率高的路徑規(guī)劃方法。

但是，人工勢場法也存在局部極小值和震蕩等不足，使得人工勢場法不適合直接用于城市環(huán)境規(guī)避路徑自主規(guī)劃問題。為進一步解決無人地面車輛(UGV)在城市自主規(guī)避難的問題，通過構建城市環(huán)境人工勢場模型，剖析經典人工勢場模型運用于城市環(huán)境的優(yōu)勢與不足，改進人工勢場函數(shù)、調整勢場作用區(qū)域，構建以遠距點斥力忽略、障礙點引力減弱的綜合勢場模型，提出了一種基于改進人工勢場的城市障礙規(guī)避算法。

表1 局部路徑規(guī)劃方法比較Tab.1 Comparison of local path planning methods

1 城市規(guī)避人工勢場模型

由Khatib 提出并被稱為人工勢場法的一種虛擬力場法，是運用空間勢場力來研究物體所處的相對運動，并通過不斷變化的位置勢場來控制物體的規(guī)避運動，其勢場變化及路徑規(guī)劃效果如圖1 所示。人工勢場法基本思路是：通過目標位置的引力勢場和障礙物的斥力勢場共同作用，搜索出一條無碰撞的最優(yōu)路徑。人工勢場法相較于其他規(guī)避方法具有數(shù)學描述簡單、運算量小、實用性高以及路徑平滑的比較優(yōu)勢。

圖1 人工勢場規(guī)避路徑示意圖Fig.1 Schematic diagram of artificial potential field avoidance path

城市規(guī)避人工勢場中，目標位置將對無人地面車輛產生引力勢場，城市建筑對無人地面車輛產生斥力勢場，其受力情況如圖2 所示。引力勢場與斥力勢場的合力場將決定無人地面車輛的機動方向和機動速率。

圖2 無人地面車輛在人工勢場中的受力示意圖Fig.2 Diagram of force exerted on unmanned ground vehicle in artificial potential field

于是，無人地面車輛在城市任意位置的綜合勢場可表述為：

式中：x是無人地面車輛當前位置狀態(tài)矢量；U（x）是無人地面車輛所受的綜合勢場；Uatt（x）是目標位置對無人地面車輛的引力勢場；Urep（x）是城市障礙對無人地面車輛產生的斥力勢場。

圖3 城市環(huán)境中的人工引力勢場與斥力勢場Fig.3 Artificial gravitational potential field and repulsion potential field in urban environment

如圖3 所示，目標位置對無人地面車輛產生的引力勢場大小與兩者之間的距離大小相關，兩者間的距離越大勢能則越大，反之則越小。因此，引力勢場與兩者之間的距離成正比，引力勢場可表述為：

式中：katt是引力勢場增益系數(shù)；x是無人地面車輛的當前位置矢量；xg是目標位置矢量；d2（?）是歐氏距離計算函數(shù)。

由該引力勢場對無人地面車輛所產生的引力，為引力勢能的負梯度：

與此同時，城市障礙將對無人地面車輛產生斥力勢場。斥力勢場的大小由無人地面車輛與障礙之間的空間距離確定，兩者之間的距離越小斥力勢場越大，反之越小。由此，斥力勢場可表述為：

式中：krep是斥力勢場增益系數(shù)；xo是城市障礙的位置矢量；dn是無斥力區(qū)域范圍。

由斥力勢場所產生的斥力，為斥力勢能的負梯度：

2 人工勢場優(yōu)劣分析

人工勢場法，能夠很好地將空間規(guī)避行為用數(shù)學表達式進行描述，為規(guī)避路徑規(guī)劃方法提供了具體化的、有效的解決方案，具有顯著優(yōu)點：

①實時規(guī)避性

相較于其他路徑規(guī)劃方法，人工勢場法具有很好的實時規(guī)避性能，能夠將路徑規(guī)劃建模的重心聚焦于無人地面車輛飛行方向以及城市障礙，只需要較少的運算就能達到較好的實時規(guī)避效果[18-20]。

②路徑平滑

無人地面車輛在規(guī)避城市障礙過程中，人工勢場法能夠隨著周圍空間環(huán)境采集無人地面車輛與城市障礙的狀態(tài)信息，并在雙方之間的斥力勢場與引力勢場中體現(xiàn)。人工勢場法在路徑規(guī)劃中，只需根據當前位置結合綜合勢場即可獲得平滑而安全的路徑，不需像別的算法那樣還要進行路徑平滑、避障檢測等操作，應用優(yōu)勢明顯。

然而金無足赤，傳統(tǒng)人工勢場法在大量現(xiàn)實應用中也暴露了自身結構性的缺陷，即存在局部最優(yōu)解。所謂局部最優(yōu)解是指引力勢場與斥力勢場等效反向作用，致使無人地面車輛原地駐留或者來回震蕩，無法繼續(xù)向目標位置行進。局部最優(yōu)解主要有兩方面的體現(xiàn)：

圖4 目標不可達現(xiàn)象示意圖Fig.4 Target unreachable phenomenon diagram

① 目標不可達

無人地面車輛在規(guī)避城市障礙的過程中，如果此刻城市障礙位于目標位置附近，當無人地面車輛離目標位置以及城市障礙都較遠時，受到的引力勢場較大，而斥力勢場較小，斥力勢場小于引力勢場，這時無人地面車輛將在合力勢場作用下向著目標位置行進；然而，無人地面車輛距目標位置和城市障礙越來越近，所受引力勢場越來越小，而斥力勢場反而越來越大，當引力勢場與斥力勢場等大反向時，這將可能致使無人地面車輛在當前位置震蕩或駐留，無法到達目標位置，其情況如圖4 所示。

② 局部極小陷阱

當無人地面車輛臨近城市障礙時，如果目標位置正好在城市障礙另一端，此時無人地面車輛所受到的引力勢場與斥力勢場之間角度為180 °且大小相等，無人地面車輛將停留在此處合力為零的點，即局部極小陷阱，最終無法成功到達目標位置，其情況如圖5 所示。本文將局部極小陷阱分為兩類：

圖5 局部極小陷阱示意圖Fig.5 Local minimum trap schematic

I 局部極小值問題

第一類局部極小陷阱是勢場遇到局部極小值問題，處于該陷阱時任意方向均為極小值，其綜合勢場呈凹狀。局部極小值可根據定理4.1 進行判定。

定理4.1[21]設U（x）為Rn→R的一個二階連續(xù)可微函數(shù)，g∈Rn是U（x）的駐點。若在g處的Hessian 矩陣正定，則g為局部極小值點；反之g則不一定為極值。

II 鞍點問題

第二類局部極小陷阱是鞍點問題，即在某個或某些方向上的綜合勢場均為局部最小值。當無人地面車輛沿著這些局部最小值的方向機動時，可能因為勢場不足而在鞍點附近往復徘徊無法繼續(xù)行進。

設以角度φ∈ ［ 0,2π）為參數(shù)的方向矢量為：

式中：lx與ly是運動方向的單位矢量。

求解矢量lx與矢量ly的一階和二階偏導數(shù)并滿足式(7)的所有空間位置以及對應的角度參數(shù)φ，即可得到第二類陷阱中的鞍點位置及方向。

3 綜合勢場改進模型

人工勢場法通常是將終點作為引力源，障礙作為斥力源，引力勢場與斥力勢場在空間合成綜合勢場，驅使運動體沿著勢場減弱方向在規(guī)避障礙的同時到達終點。

在無人地面車輛規(guī)避機動的運用中，在距城市障礙較遠時應該弱化斥力勢場，以免出現(xiàn)過早軌跡偏離現(xiàn)象。同時，在抵近城市障礙時應弱化引力勢場，以免出現(xiàn)局部震蕩現(xiàn)象。如圖6 所示，本文構建了以遠距點斥力忽略、障礙點引力減弱的綜合勢場模型：

式中：U（x,Δs）為當前位置x無人地面車輛所受的綜合勢場；Uatt（x）為引力勢場；katt為引力勢場系數(shù)；Urep（x）為斥力勢場；krep為斥力勢場系數(shù)。

圖6 無人地面車輛規(guī)避機動人工勢場示意Fig.6 Unmanned ground vehicle evading maneuvering artificial potential field signal

3.1 引力勢場模型

為順利規(guī)避城市障礙并避免局部震蕩現(xiàn)象，對引力勢場函數(shù)進行了改進，在城市障礙附近設置弱化引力勢場的環(huán)形區(qū)域：

式中：η為與城市障礙距離相關的吸引場系數(shù)；xob為城市障礙位置矢量；xgoal為目標位置矢量；n為正整數(shù)；rob為城市障礙的威脅范圍；rsafe為引力衰減區(qū)范圍；為2-范數(shù)。

3.2 斥力勢場模型

為緊跟目標方向避免過早偏離，采用勢場平滑過渡策略對斥力勢場函數(shù)進行改進：

式中：λ為斥力勢場系數(shù)；q為正整數(shù)；d為斥力過渡區(qū)作用范圍。

3.3 連續(xù)可微證明

通常，對人工勢場模型進行負梯度求解可得到勢場力，但要求綜合勢場模型是連續(xù)可微的。然而，本文所構建勢場模型的系數(shù)均是由無人地面車輛相對位置所確定，特別是引力勢場模型和斥力勢場模型系數(shù)都是與相對位置相關的分段函數(shù)。為了檢驗通過所構建的勢場模型可以得到連續(xù)的勢場力，本節(jié)對各分段函數(shù)的連續(xù)可微性給予證明。

對于引力勢場函數(shù)Uatt（x），其連續(xù)可微性主要看式(9)中的分段函數(shù)η：

由此，證明分段函數(shù)η是連續(xù)可微，從而可知引力勢場模型Uatt（x）亦是連續(xù)可微的。同理，可證明斥力勢場模型Urep（x）的連續(xù)可微性。

4 算例分析

通過將改進的人工勢場法與經典人工勢場法以及常用的迪杰斯特拉(Dijkstra)、快速擴展隨機樹(RRT)路徑規(guī)劃算法進行仿真實驗，以對比說明改進算法的應用優(yōu)勢。其中，Dijkstra 算法以起始點為中心向外層層擴展，直到擴展到終點為止，是典型的單源最短路徑算法，主要用于計算一個節(jié)點到其他所有節(jié)點的最短路徑；RRT 算法通過對狀態(tài)空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規(guī)劃問題。針對城市環(huán)境規(guī)避路徑規(guī)劃問題，運用本文所提方法進行仿真，以檢驗本文所提算法的有效性。

4.1 算法對比

為說明改進人工勢場法的比較優(yōu)勢，分別與經典人工勢場法以及常用的最短路徑算法進行了仿真對比。如圖7 所示，設有一屏障規(guī)避問題，即需自主規(guī)劃出從綠色點出發(fā)順利繞過兩屏障到達藍色叉地的最短路徑，在1.6 GHz、1.8 GHz 雙核CPU、8 GRAM 計算硬件上，運用相同的PyCharm 工具，分別運用四種算法進行求解。

運用典型廣度優(yōu)先搜索法（Dijkstra 算法[22-24]）可得到如圖7(a)所示的紅色路徑。在Dijkstra 算法求解過程中，需預先設定搜索區(qū)域（圖中黑框部分）并采取以起始點為中心向外層層擴展的方式（圖中藍色叉點代表已搜索節(jié)點），使得搜索過多無關節(jié)點，平均耗時0.48 s，所得路徑棱角明顯，路徑長度93.3 m。運用增量式、概率完備且不最優(yōu)的路徑規(guī)劃算法（RRT 算法[25-27]）可得到如圖7(b)所示的紅色路徑。RRT 算法采取以初始狀態(tài)作為根節(jié)點、目標節(jié)點作為葉子結點的搜索樹方式（圖中綠色支路為已搜索區(qū)域），平均耗時0.62 s，所得路徑曲折，長度111.4 m。運用經典人工勢場法可得到如圖7(c)所示的紅色路徑。經仿真發(fā)現(xiàn)，接近目標過程中路徑會出現(xiàn)波動，當引力勢場與斥力勢場等大反向且受障礙阻斷時，會產生局部震蕩（圖中紅色加粗部分），出現(xiàn)目標不可達現(xiàn)象。運用改進人工勢場法，平均耗時0.29 s 后順利到達目標位置，所得路徑如圖7(d)所示，路徑長度91.1 m，平滑效果更好?？傮w來說，本文算法比Dijkstra 算法路徑規(guī)劃效率提升了 39.5%，比 RRT 算法路徑規(guī)劃效率提升了53.2%。

圖 7 屏障規(guī)避問題的不同算法求解效果Fig.7 Different algorithms to solve the barrier avoidance problem

因此，改進的人工勢場法能有效彌補經典人工勢場在路徑波動、局部震蕩以及目標不可達方面的不足，與兩種常用路徑規(guī)劃算法相比較耗時短、路程少、路徑平滑，具有較強對比優(yōu)勢和應用優(yōu)勢。

4.2 算例求解

設無人地面車輛質量為50 kg，從起始地穿過城市建筑到達目標地域，期間有高樓林立，需要無人地面車輛自主規(guī)劃出最優(yōu)規(guī)避路徑。如圖8 所示，設計了三個不同復雜度的城市環(huán)境障礙環(huán)境，以驗證本文算法的有效性。

圖8 城市障礙環(huán)境Fig. 8 Urban barrier environment

在運用基于改進人工勢場的城市環(huán)境規(guī)避算法求解過程中，將模型參數(shù)設置為：krefer=0.2，katt=0.4，krep=0.4，δ=0.92，η=0.4，n=1，q= 1，λ=0.4，rsafe= 5m ，d=3m。

如圖9 所示，較為詳細地展現(xiàn)了無人地面車輛在城市環(huán)境-2 中規(guī)避建筑的路徑生成過程。該方法可以根據當前城市環(huán)境實時自主規(guī)劃最佳的規(guī)避路徑，最短路徑為106 m，并順利抵達目標位置。

圖9 城市環(huán)境-2 規(guī)避路徑自主規(guī)劃過程Fig.9 The process of autonomous planning of avoidance path for urban environment-2

如圖10 所示，展示了3 種不同城市環(huán)境復雜度下規(guī)劃得到的最佳規(guī)避路徑，各條路徑均能根據目標位置，結合當前城市環(huán)境，自主規(guī)劃出最合適的路徑，均能順利到達目標位置，滿足城市環(huán)境中障礙自主規(guī)避的需求。

圖10 城市環(huán)境規(guī)避路徑Fig.10 Urban environment avoidance path

此外，為了檢驗算法對城市動態(tài)障礙的規(guī)避性能，在仿真場景中增添了動態(tài)障礙物，即在無人地面車輛行進過程中突遇一黃色障礙車輛的逼近。

如圖11 所示，為最初無人地面車輛在城市環(huán)境中的最優(yōu)機動路徑。但當無人地面車輛行進過程中，突然臨時出現(xiàn)一黃色障礙輛車，且其行進路線與無人地面車輛路線沖突。為了避免兩車相撞，無人地面車輛依據本文算法臨時動態(tài)規(guī)劃了新的路徑。

圖11 無人地面車輛在城市環(huán)境中的最佳機動路徑Fig.11 Optimal mobility path of unmanned ground vehicle in urban environment

根據本文算法，當障礙車輛駛進人工勢場的斥力過渡區(qū)（d=10m）后，如圖12 所示無人地面車輛自動改變了機動方向并采取了動態(tài)規(guī)避行為。

圖12 無人地面車輛改變機動方向規(guī)避障礙車輛Fig. 12 The unmanned ground vehicle changes its maneuvering direction to avoid the obstacle vehicle

當無人地面車輛順利規(guī)避動態(tài)運動的障礙車輛且距離大于無引力區(qū)（rob=5m）后，如圖13 所示無人地面車輛將主要受引力場引導繼續(xù)向目標位置駛去。

圖13 無人地面車輛成功規(guī)避障礙車輛Fig.13 The unmanned ground vehicle successfully evaded the obstacle vehicle

當障礙車輛駛離人工勢場的斥力過渡區(qū)（d=10m），如圖14 所示無人地面車輛僅受引力勢場作用，將實現(xiàn)與最初路徑相一致的行駛方向。

圖14 無人地面車輛繼續(xù)向著目標位置駛去Fig.14 The unmanned ground vehicle continued to move toward the target position

面對動態(tài)運動的障礙車輛，運用本文算法，無人地面車輛動態(tài)規(guī)劃的最優(yōu)機動路徑如圖15 所示。整個規(guī)避過程實現(xiàn)了對障礙車輛的動態(tài)規(guī)劃，并能在規(guī)避過后盡快向目標位置繼續(xù)行進。整個路徑平滑、路程少、應變能力強，將有助于更好地解決城市復雜環(huán)境自主規(guī)避問題。

圖15 無人地面車輛的整個動態(tài)規(guī)避路徑Fig.15 The entire dynamic avoidance path of the unmanned ground vehicle

5 結 論

城市化，是未來社會建設與發(fā)展的重要階段性標志。為進一步解決無人地面車輛(UGV)在城市環(huán)境自主規(guī)避難的問題，提出了一種基于改進人工勢場的城市環(huán)境規(guī)避算法。構建了城市環(huán)境人工勢場模型，給出了引力勢場、斥力勢場以及綜合勢場函數(shù)，將空間規(guī)避路徑規(guī)劃問題轉化為勢場力作用問題。深入剖析了經典人工勢場模型運用于城市環(huán)境的優(yōu)勢與不足，改進了人工勢場函數(shù)、調整勢場作用區(qū)域，構建了以遠距點斥力忽略、障礙點引力減弱的綜合勢場模型，避免了傳統(tǒng)人工勢場法存在過早軌跡偏離以及局部震蕩現(xiàn)象，實現(xiàn)了在城市環(huán)境中的自主規(guī)避。算例分析表明，論文算法具有耗時短、路程少、路徑平滑的對比優(yōu)勢，能夠更有效地應對城市障礙以及動態(tài)車輛規(guī)避問題，為復雜環(huán)境下規(guī)避路徑自主規(guī)劃提供了新思路。同時，對于其他領域中的規(guī)避路徑規(guī)劃問題具有較強借鑒意義。