基于改進(jìn)人工勢場的無人地面車輛路徑規(guī)避算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

由該引力勢場對無人地面車輛所產(chǎn)生的引力，為引力勢能的負(fù)梯度：

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

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

由斥力勢場所產(chǎn)生的斥力，為斥力勢能的負(fù)梯度：

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

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

①實(shí)時(shí)規(guī)避性

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

②路徑平滑

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

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

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

① 目標(biāo)不可達(dá)

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

② 局部極小陷阱

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

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

I 局部極小值問題

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

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

II 鞍點(diǎn)問題

第二類局部極小陷阱是鞍點(diǎn)問題，即在某個(gè)或某些方向上的綜合勢場均為局部最小值。當(dāng)無人地面車輛沿著這些局部最小值的方向機(jī)動時(shí)，可能因?yàn)閯輬霾蛔愣诎包c(diǎn)附近往復(fù)徘徊無法繼續(xù)行進(jìn)。

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

式中：lx與ly是運(yùn)動方向的單位矢量。

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

3 綜合勢場改進(jìn)模型

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

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

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

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

3.1 引力勢場模型

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

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

3.2 斥力勢場模型

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

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

3.3 連續(xù)可微證明

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

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

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

4 算例分析

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

4.1 算法對比

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

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

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

4.2 算例求解

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

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

在運(yùn)用基于改進(jìn)人工勢場的城市環(huán)境規(guī)避算法求解過程中，將模型參數(shù)設(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 所示，較為詳細(xì)地展現(xiàn)了無人地面車輛在城市環(huán)境-2 中規(guī)避建筑的路徑生成過程。該方法可以根據(jù)當(dāng)前城市環(huán)境實(shí)時(shí)自主規(guī)劃最佳的規(guī)避路徑，最短路徑為106 m，并順利抵達(dá)目標(biāo)位置。

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

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

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

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

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

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

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

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

當(dāng)無人地面車輛順利規(guī)避動態(tài)運(yùn)動的障礙車輛且距離大于無引力區(qū)（rob=5m）后，如圖13 所示無人地面車輛將主要受引力場引導(dǎo)繼續(xù)向目標(biāo)位置駛?cè)ァ?/p>

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

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

圖14 無人地面車輛繼續(xù)向著目標(biāo)位置駛?cè)ig.14 The unmanned ground vehicle continued to move toward the target position

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

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

5 結(jié) 論

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