越野環境下基于勢能場模型的智能車概率圖路徑規劃方法

田洪清，王建強，黃荷葉，丁峰

(清華大學 汽車安全與節能國家重點實驗室，北京 100084)

0 引言

智能車能夠在復雜越野環境中完成情報獲取、偵察監視和通信中轉等任務，具有不可替代的作用[1]。越野環境下的智能車路徑規劃是指在有界空間內根據感知系統輸入的越野環境信息，考慮到環境和車輛等多種約束條件基礎上，生成從初始位置到達指定目標安全可行的行駛優化路徑[2]。路徑規劃對智能車在越野環境中完成駕駛行為起到至關重要的作用[3]。

近年來智能車路徑規劃技術發展迅速，建立了完整的理論體系[4]。基于圖搜索的路徑規劃方法采用擴展啟發搜索技術，其經典規劃算法包括Dijkstra算法、A*算法、D*算法、Field D*算法及其多個變種等[5]，其中Hybrid A*算法成功應用于非結構化道路環境下智能車路徑規劃系統[6]。隨機采樣路徑規劃算法，如隨機快速搜索樹(RRT)算法[7]、概率圖(PRM)算法[8]，以及漸進優化隨機快速搜索樹(RRT*)算法、信息優化隨機快速搜索樹(Informed RRT)算法、漸進優化概率圖(PRM*)算法通過在一定范圍[9]，或一定時間內持續采樣的方法優化路徑[10]，并使用環境信息啟發來提升路徑規劃的效率或優化程度[11-12]。人工勢能場(APF)算法將環境信息抽象為引力或斥力場，通過勢能場的引導來規劃從起始點到目標點的無碰撞路徑[13]。幾何曲線法將智能車路徑規劃問題轉化成兩點邊界值問題求解，如B樣條曲線[14]、5次多項式曲線[15]等。

在越野環境路徑規劃方面，文獻[16]通過采集地形信息進行隨機采樣路徑規劃。文獻[17]通過建立優化搜索方向和速度的規劃模型，采用自適應變異遺傳算法，在連續時空內進行多目標最優路徑規劃。文獻[18]提出了改進型A*路徑優化算法，通過分析地形坡度和地表屬性的綜合影響，采用窗口移動法來提高搜索效率。

本文將PRM算法與APF算法結合，通過環境態勢建模、通行代價評估和路徑平滑的方法對傳統PRM算法進行改進，以滿足復雜越野環境下，不同類型車輛安全行駛、高效通行和運動學約束的綜合要求。綜合APF算法和PRM算法，形成一種越野環境下智能車路徑規劃方法，稱為APF-PRM算法，其框架結構如圖1所示。該算法首先建立多層次環境態勢場模型；然后采用隨機采樣方法，生成節點間的連通矩陣和多維度通行代價評估矩陣；通過評估矩陣衡量路徑的可行性和安全性，并根據環境和車輛的約束條件，規劃適應車輛特性的優化路徑。圖1中，vo為起點，vg為終點，T1為環境威脅，P1、P2為越野道路，Z1～Z4為禁行區。

圖1 APF-PRM算法路徑規劃框架結構

APF-PRM算法路徑規劃結合了PRM算法與APF算法的優點，適用于復雜越野環境。在路徑規劃過程中綜合考慮了障礙物、環境威脅、道路條件等多種約束條件，以及車輛防護性能、越野性能、運動學特性，能夠生成安全可行的車輛運動路徑。

1 越野環境建模

本文采用APF算法對車輛所在越野環境進行柵格化環境態勢場建模。環境態勢場即一定環境空間范圍內，根據越野環境中各柵格點的勢能分布，建立多層次勢能場模型，融合為環境態勢場模型，如圖2(a)所示。

圖2 多層次越野環境態勢場模型

1.1 障礙層模型

障礙層模型即將存在建筑、樹林、山地等不可穿越障礙物的越野環境劃分為禁行區和可行區，并考慮到車輛的寬度和轉彎半徑等因素，在障礙物周邊設置限制區。如圖2(b)所示越野環境中有障礙層禁行區Z1～Z4，其周邊為障礙層限制區，障礙層限制區外為安全通行區，障礙層模型為

(1)

式中：UB為障礙層勢能場；Ub為障礙層禁行區勢能場；Ur為障礙層限制區勢能場；Z為禁行區域，Zi∈Z；z為限制區寬度；Uf為障礙層禁行區和限制區之外的安全通行區勢能場；(xi,yi)為越野環境柵格點坐標。

1.2 威脅層模型

威脅層模型是將環境威脅對車輛行駛造成毀傷損失風險程度進行量化處理。環境威脅所產生的風險程度取決于威脅的屬性、數量和狀態等因素，本文基于APF算法[19]用威脅層模型來表征環境威脅對車輛造成的風險程度，威脅層模型為

(2)

(3)

式中：UT為威脅層勢能場；r為環境威脅T與行駛車輛之間的距離；rmin為環境威脅所產生的有效作用距離，在[0，rmin]區間范圍內為威脅有效區，其勢能場為Ue；rmax為環境威脅所能產生的最遠威脅作用距離，在[rmin,rmax]區間范圍為威脅影響區，其勢能場為Ua，Ua值隨r的增大而減小；kw為環境威脅的威脅系數，由環境威脅的種類、數量和狀態等因素決定；在[rmax，∞]區間范圍為安全通行區，其勢能場為Uf.

如圖2(c)所示，越野環境中存在靜態環境威脅Ts和動態環境威脅Td，通過威脅層模型可將環境空間劃分為威脅有效區、威脅影響區和安全通行區3部分。

1.3 道路層模型

道路層模型是將越野環境中結構化道路、土路、草地、沙地、沼澤、山地等不同的地表屬性特征對車輛通行影響進行量化處理，設通行條件最好的結構化道路勢能場為Us，而通行條件最差的泥沼道路勢能場為Uh，越野道路區周邊為越野道路擴展區，道路層模型可用式描述：

(4)

式中：UR為道路層勢能場；kr為越野道路區通行系數；P為越野道路區；p為越野道路擴展區寬度；ke為越野道路擴展區通行系數。參照輪式車輛和履帶式車輛的實驗數據和專家經驗值，將道路通行系數按地表屬性設置等級[20]，如表1所示。

表1 地表屬性與道路通行系數

如圖2(d)所示越野環境中有3片越野道路區，通過道路層模型可將其劃分為黃色沙地區Ps，綠色草地區Pg，藍色泥沼區Pm，考慮車輛的寬度和彎道行駛情況，越野道路區周邊一定范圍設定為越野道路擴展區。各區域的道路層勢能場可由(4)式計算。

1.4 環境態勢場融合

路徑規劃時對各層勢能場模型進行跨層融合，并考慮障礙物對環境威脅的遮擋，障礙層、威脅層與道路層的疊加等耦合作用，形成環境態勢場，如(5)式、(6)式所示：

U=∑UB+∑UT+∑UR,

(5)

(6)

2 車輛路徑規劃

為權衡復雜越野環境下車輛行駛安全性與行車效率的關系，規劃安全可行的行車優化路徑，本文提出APF-PRM算法。算法分為離線采樣學習和在線路徑優化2個階段。

2.1 離線采樣學習

2.1.1 越野環境采樣

首先將越野環境劃分成為均勻分布的柵格化平面空間W，并建立車輛、起始點、目標終點、障礙物、環境威脅和越野道路等所在的位置特征。然后在可行的越野環境內生成采樣節點，并形成越野環境拓撲圖G=(V,E)，V為采樣節點v集合，E為采樣節點之間的路段e集合。

采樣節點v生成于平面空間W內，并選取在障礙層禁行區Z和威脅層環境威脅T的威脅有效區之外，如(7)式所示：

V={v|v∈W,v?B∪T,vi≠vj}.

(7)

2.1.2 節點連接評估模型

節點連接評估即評估各采樣節點之間連接路段的可行性和通行代價，可行性評估用連通矩陣描述，通行代價評估用多維度通行代價評估矩陣描述，并分為擴展代價評估和啟發代價評估，如圖3所示。

圖3 節點連接評估模型

2.1.2.1 連通矩陣

為判定節點間的可行性，利用采樣節點間連通性建立矩陣Av，用節點之間的連通屬性作為連通矩陣Av的元素aij.采樣節點與連通矩陣對應關系如圖4所示，圖4(a)中越野環境包含障礙區Z1和采樣節點v1～v3，其連通矩陣Av如圖4(b)所示。

圖4 越野環境采樣節點連接與對應的連通矩陣

2.1.2.2 通行代價評估矩陣

通行代價評估矩陣由擴展代價評估矩陣和啟發代價評估矩陣組成。擴展代價評估矩陣包括擴展安全代價矩陣Sv、擴展距離代價矩陣Dv和擴展道路代價矩陣Pv；啟發代價評估矩陣包括啟發障礙代價矩陣Bv、啟發距離代價矩陣Hv和啟發道路代價矩陣Mv.

擴展安全代價矩陣Sv由采樣節點集合V中節點間的安全代價sij構成。評估方法是在采樣節點之間的路段上取數量為n的均勻分布采樣子節點vs(1)～vs(n)，用各子節點所在的威脅層勢能場累加和評估路段安全性，如(8)式所示。

(8)

多個節點之間路段的通行安全評估用擴展安全代價矩陣表示，如圖5(a)、圖5(b)分別表征包含環境威脅T1的越野環境及其對應的擴展安全代價矩陣。

圖5 越野環境采樣節點連接與擴展安全代價矩陣

擴展距離代價矩陣Dv由集合V中節點間歐式距離dij構成，其計算方法為

(9)

多個節點之間路段的通行距離評估用擴展距離代價矩陣表示，如圖6(a)、圖6(b)分別表征包含障礙物Z1的越野環境及其對應的擴展距離代價矩陣。

圖6 越野環境采樣節點連接與擴展距離代價矩陣

擴展道路代價矩陣Pv由集合V中節點間的道路代價pij構成，通過在節點之間的路段采樣，用各采樣子節點vr(1)～vr(n)所在道路層勢能場累加和來評估該路徑的道路通過性，如(10)式所示：

(10)

多個節點之間路段的通行道路評估用擴展道路代價矩陣表示，如圖7(a)、圖7(b)分別表征包含越野道路P1～P2的越野環境及其對應的擴展道路代價矩陣。

圖7 越野環境采樣節點連接與擴展道路代價矩陣

啟發障礙代價矩陣Bv由集合V中節點vi與目標終點vg之間的啟發障礙代價big構成。通過在采樣節點與終點間的路段中各子節點vb(1)～vb(n)所在的障礙層和威脅層勢能場累加和來評估，如(11)式所示：

(11)

節點vi與目標終點vg之間路段的通行障礙評估用啟發障礙代價矩陣表示，如圖8(a)、圖8(b)分別表征包含障礙物Z1、環境威脅T1的越野環境及其對應的啟發障礙代價矩陣。

圖8 越野環境采樣節點和終點連接與啟發障礙代價矩陣

啟發距離代價矩陣Hv由集合V中節點vi與目標終點vg之間歐式距離dig構成，其計算方法如(12)式所示：

dig=‖vi-vg‖.

(12)

節點vi與目標終點vg之間路段的通行距離評估用啟發距離代價矩陣表示，如圖9(a)、圖9(b)分別表征包含障礙物Z1、環境威脅T1的越野環境及其對應的啟發距離代價矩陣。

圖9 越野環境采樣節點和終點連接與啟發距離代價矩陣

啟發道路代價矩陣Mv由集合V中節點vi與目標終點vg之間的啟發道路代價pig構成。通過在采樣節點與終點間的路段中各子節點vr(1)～vr(n)所在的道路層勢能場累加和來評估，如(13)式所示：

(13)

節點vi與目標終點vg之間路段的通行道路評估用啟發道路代價矩陣表示，如圖10(a)、圖10(b)分別表征包含越野道路P1、P2的越野環境及其對應的啟發道路代價矩陣。

圖10 越野環境采樣節點和終點連接與啟發道路代價矩陣

2.2 在線路徑優化

在線路徑優化方法采用通行代價評估函數來優化路徑，如(14)式所示：

F(vi)=q(vi)+h(vi),

(14)

式中：F(vi)為由從起始點(即源節點vo)到當前節點vi的通行代價評估函數；q(vi)為從源節點vo到當前節點vi的擴展代價評估函數；h(vi)為當前節點vi到目標點vg的啟發代價評估函數。

2.2.1 節點擴展

節點擴展是在越野環境拓撲圖G中從當前節點vi向周圍節點連接，節點擴展法具有搜索路徑優化程度高的優點，并且能提高路徑規劃速度。節點擴展方法如圖11(a)所示，首先按連通矩陣由源節點vo出發擴展至可行連通節點vi，如vo與vi之間連接路徑aoi可行，生成候選節點集合Q={vi|vi∈V,aoi=1}，在Q集合中搜索通行代價最小的優化節點vmin，再由該節點出發擴展至其周邊的連通節點，并將新擴展節點加入集合Q，如此往復循環，直至擴展至目標終點vg.

圖11 APF-PRM算法節點擴展與啟發方法

節點擴展過程中，對于新擴展節點，使用擴展代價評估函數，通過加權計算，評估由源節點至該節點的擴展代價，如(15)式、(16)式所示：

q(vi)=q(vmin)+qi,

(15)

qi=fd·dij+fs·sij+fr·pij,

(16)

式中：q(vmin)為由源節點vo至節點vmin的擴展代價；qi為節點vmin到vi間的擴展代價；fd、fs和fr分別為距離權重系數、安全權重系數和道路權重系數。

2.2.2 節點啟發

為提高路徑規劃效率，優化節點擴展方向，需要考慮新擴展節點vi與目標終點vg間的啟發代價，并優先由啟發代價最小的方向進行路徑搜索。

節點啟發代價方法如圖11(b)所示，環境中包含有2片障礙物Z1～Z2、1處環境威脅T1和1片越野道路區P1.設候選節點集合Q中包含v1～v5新節點，采用啟發代價評估方法，分別計算啟發障礙代價、啟發距離代價和啟發道路代價，通過加權計算，評估該節點至終點的啟發代價，如(17)式所示：

h(vi)=fb·big+fd·dig+fr·pig，

(17)

式中：h(vi)為新擴展節點vi與目標終點vg間的啟發代價評估函數；fb為障礙權重系數；big、dig和pig分別為啟發代價評估矩陣中節點vi的啟發障礙代價、啟發距離代價、啟發道路代價。

2.2.3 路徑優化

如圖12所示，APF-PRM算法在路徑優化初始化階段生成備選節點集合Qc、候選節點集合Q和優化節點集合C，并將起始點vo加入集合Q中。

圖12 APF-PRM算法流程

在路徑優化過程中，不斷地重復以下步驟，直到目標終點vg進入優化節點集合C中：

1)取集合Q中通行代價最小節點為當前優化節點vmin，如(18)式所示：

(18)

2)利用連通矩陣Av，搜索集合V中與vmin相連通的節點vi，并將其加入集合Qc；用擴展代價評估矩陣Sv、Dv、Pv和啟發代價評估矩陣Bv、Hv、Mv，以及對應的權重系數，計算其擴展代價q(vi)、啟發代價h(vi)和通行代價F(vi)，并記錄其父節點信息vmin.

3)檢查集合Qc中的各個節點vi，如該節點已在集合C中，則將該節點移出集合Qc.如該節點已在集合Q中，則比較集合Qc和Q中同一節點的通行代價F′(vi)和F(vi)：若F′(vi)F(vi)，說明原路徑更優，保留集合Q中的原節點，將節點vi移出集合Qc.

4)將集合Qc與Q合并。將其上一級優化節點vmin加入到集合C中，并記錄其父節點。

5)如當前節點vmin為目標終點vg，則說明已找到優化路徑節點序列，由vmin出發，按集合C中各節點的父節點鏈表信息順序連接，即為優化路徑；否則返回到步驟1，繼續搜索優化路徑。

2.2.4 軌跡優化

優化路徑節點順序連接后，得到車輛運動軌跡由多段折線構成，如圖13中紅色虛線，由于軌跡在節點處存在硬拐點，無法滿足車輛運動學約束要求，本文采用動態曲率平滑法優化車輛運動軌跡。首先設定車輛最小轉彎半徑Rmin和理想轉彎半徑為Ri，在軌跡優化時，首選以Ri為半徑在節點處規劃車輛運動軌跡，見圖中的藍色細實線，vt(1)、vt(4)、vt(5)和vt(6)分別為兩段曲線與直線軌跡之間的切點，如軌跡與障礙或威脅發生干涉(如圖13中障礙Z1與其周邊藍色細實線軌跡干涉)，說明該軌跡不可行，則按(19)式減小轉彎半徑，生成新的車輛運動軌跡，直至消除干涉(如圖13中Z1周邊的黑色粗實線平滑軌跡，vt(2)和vt(3)為新生成曲線與直線軌跡之間的切點)。如減小至最少轉彎半徑Rmin時仍不能避免干涉，則需要對該段路線重新規劃。

圖13 路徑平滑方法

R=Ri-λR(Ri-Rmin)，λR∈(0.1,0.5).

(19)

綜上所述，APF-PRM算法由4部分組成：第1部分為環境建模，即通過獲取的環境感知信息，在柵格化越野環境中生成多層次環境態勢場模型；第2部分為離線采樣學習，通過隨機采樣法生成越野環境拓撲圖，建立節點之間的連通矩陣和多維度通行代價評估矩陣；第3部分為在線路徑優化，首先根據車輛性能和任務要求建立代價權重向量，然后從起始點出發進行節點擴展，擴展過程中通過評估節點擴展代價和啟發代價，搜索候選節點集合中通行代價最優的可行節點，直至到達目標終點；第4部分為軌跡優化，按照車輛的運動學特性，采用動態曲率平滑法優化各節點處的車輛運動軌跡。

3 仿真實驗

為驗證算法的有效性，本文在模擬越野環境中完成了路徑規劃，并通過仿真對比實驗驗證了APF-PRM算法的可行性和綜合性能的優越性。

3.1 實驗參數設置

APF-PRM算法通過權重向量設置來進行個性化路徑規劃，包括擴展權重向量ke=[fsfrfd]和啟發權重向量kh=[fbfrfd]，權重系數由車輛防護、越野性能、任務要求，以及環境障礙、威脅和越野道路的分布決定。本文按專家經驗法以及仿真實驗所得的數據分析，確定權重系數在[1，10]范圍，具體權重系數選擇如表2所示。

表2 APF-PRM算法路徑規劃權重系數

3.2 路徑規劃仿真

為驗證APF-PRM算法性能，在模擬越野環境(1 002 m×1 114 m)下進行路徑規劃，并將規劃結果與PRM、RRT、A*、APF算法進行對比。圖14(a)、圖14(b)為APF-PRM算法按兩組不同的權重向量所規劃的路徑。當車輛的防護、越野能力弱，在非緊急任務條件下，取權重向量ke=[1,1,1]、kh=[1,1,1]，由圖14(a)可知，規劃路徑能夠避開障礙物、環境威脅和越野道路區到達目標終點；當車輛具備較強的防護、越野能力，在非緊急任務條件下，取權重向量ke=[10,10,1]、kh=[10,10,1]，由圖14(b)可知，規劃路徑能夠避開障礙物，穿過環境威脅作用范圍邊緣和越野道路區，取捷徑到達目標終點。

圖14(c)～圖14(f)所示分別為PRM、RRT、A*、APF算法規劃路徑，由圖可知，4種算法都具有避障功能，并具有各自的優點，相比而言APF-PRM算法的優勢在于：1)通過環境態勢建模識別環境威脅和越野道路，能夠避免車輛從威脅影響區和越野區中通過，造成安全風險；2)通過勢能場模型來避開障礙物邊緣，規避碰撞風險；3)通過路徑平滑模塊，解決硬拐點和曲率半徑過小的問題，規劃的路徑符合車輛運動學特性；4)能夠通過車輛特性和任務要求的權重向量設置來進行個性化路徑規劃。

圖14 路徑規劃算法對比

APF-PRM算法與4種算法性能指標對比如表3所示。由表3可知，APF-PRM算法考慮多種越野環境因素影響，并進行環境態勢場建模，因而規劃時間略長，規劃路徑時避開了威脅要素的影響和越野區域，故路徑距離略遠，但車輛行駛的安全性高，生成的路徑符合車輛動力學要求，因此其綜合性能較優。

表3 路徑規劃算法對比

與4種規劃算法對比，APF-PRM算法在基本避障功能基礎上，具備規避環境威脅和越野道路能力，規劃軌跡符合車輛運動學要求，并能夠根據車輛的防護、越野性能，以及任務要求調整車輛路徑規劃策略。因此，APF-PRM算法在特定的越野環境下，其規劃路徑在可行性、安全性、規劃速度、個性化程度方面具有一定綜合優勢。

為驗證APF-PRM算法可行性，在多種場景下進行了路徑規劃仿真實驗。以場景A(2 324 m×686 m)和場景B(2 296 m×1 027 m)兩種越野環境為例，使用不同的權重向量進行路徑規劃，所得到的路徑如圖15和圖16所示。由圖15和圖16可見，該算法能夠根據車輛性能和任務需求，按照權重向量合理規劃路徑。

圖15 場景A環境條件下多權重路徑規劃對比

圖16 場景B環境條件下多權重路徑規劃對比

5 結論

本文基于APF方法對越野環境建模，在車輛行駛量化風險評估的基礎上，提出一種改進的PRM路徑規劃算法，滿足越野環境條件下規劃安全可行路徑的需求。得出以下主要結論：

1)本論文提出一種結合APF模型與PRM方法的路徑規劃框架結構。建立基于APF方法的多層次環境態勢場模型，通過對越野環境中的障礙物、環境威脅和越野道路進行層次化勢能場量化建模，將其融合為環境態勢場，實現了復雜越野環境下車輛行駛量化風險評估。

2)提出基于環境態勢場的可行性和安全性節點連接路徑評估模型。基于PRM方法進行節點采樣，建立采樣節點間可行性評估連通矩陣和多維度通行代價評估矩陣。以可行節點間通行代價為優化指標，按節點擴展與啟發綜合方法規劃路徑；采用動態曲率平滑法，優化車輛運動軌跡，解決復雜越野環境條件下的智能車路徑規劃問題。仿真實驗結果表明APF-PRM算法考慮了越野環境中多種因素的綜合影響，以及車輛的性能特點和任務要求，能在越野環境條件下，規劃安全可行的行車優化路徑。