錢燮暉,何秀鳳,郭俊文,王 礫

(1.河海大學(xué)地球科學(xué)與工程學(xué)院,江蘇 南京 211100; 2.北方信息控制研究院集團有限公司,江蘇 南京 211100)

車輛在野外環(huán)境的路徑規(guī)劃是部隊作戰(zhàn)和演習(xí)的關(guān)鍵問題,一直受到廣泛關(guān)注。車輛路徑規(guī)劃指的是從起始位置(起始點)到終止位置(目標點)的空間內(nèi),根據(jù)研究人員對其所設(shè)定的某種具有可選擇最優(yōu)路徑規(guī)劃法則,來輸出一條能夠精確到達目標的最優(yōu)或次優(yōu)的安全無碰撞路徑[1]。由于受野外環(huán)境復(fù)雜且信息量大、障礙物多等因素的影響,路徑規(guī)劃問題已被確定為非確定性多項式困難問題[2]。

目前有很多智能算法應(yīng)用于路徑規(guī)劃問題中,各有其優(yōu)缺點。遺傳算法具有較強的魯棒性,能自適應(yīng)地調(diào)整搜索方向,但在求解過程中常常伴隨著大量的冗余迭代,降低了搜索效率;蟻群算法具有良好地正反饋和自適應(yīng)能力,適合全局搜索,但其收斂性對初始化參數(shù)較為敏感,需要較長的搜索時間,易出現(xiàn)停滯現(xiàn)象;模擬退火算法的計算過程簡單,魯棒性強,適用于并行處理,但其收斂速度慢,收斂時間長,算法性能受初始值和有關(guān)參數(shù)的影響很大[3];A*算法適合全局最優(yōu)路徑規(guī)劃,有較強的實時性,但隨著搜索空間和存儲數(shù)據(jù)量的增大,導(dǎo)致計算量增大和搜索時間變長,其實時性無法獲得保證[4]。

考慮到車輛在野外的路徑規(guī)劃對實時性有較高的要求,選擇實時性更高的A*算法進行路徑規(guī)劃。許多研究學(xué)者針對A*算法在相應(yīng)場景下做了很多研究,如文獻[5]中通過改進障礙搜索方式和啟發(fā)函數(shù),在犧牲了路徑長度的情況下,提高了算法的性能和路徑的安全性,但這是建立在室內(nèi)仿真環(huán)境下,沒有考慮復(fù)雜的野外環(huán)境以及遭遇未知障礙物的情況;文獻[6]中通過采用優(yōu)先子節(jié)點的生成策略來避免生成穿過障礙物柵格節(jié)點的路徑,但僅僅通過一個簡單的仿真實驗并不足以驗證該算法在實際環(huán)境下的可靠性;文獻[7]中提出了一種雙向時效A*算法,并采用多近鄰柵格距離計算方案,在提高算法搜索效率的同時平滑路徑,但不適合大尺寸復(fù)雜地圖?？偟膩碚f,雖然A*算法有所改進,但仍有些問題沒有得到很好的解決:沒有考慮在復(fù)雜的野外環(huán)境下算法的可靠性;僅僅考慮優(yōu)化搜索效率,卻忽略了算法對車輛機動性的提高。

為了考慮在實際的野外環(huán)境,給車輛規(guī)劃出一條快速到達目的地的最優(yōu)路徑,研究借鑒文獻[6]中提出的優(yōu)先子節(jié)點生成策略,結(jié)合真實的野外背景,計算得到了問題的解決方案。通過改進傳統(tǒng)A*算法的搜索策略,提升車輛在野外環(huán)境下的機動性,同時提出了一種加入預(yù)處理的二次A*算法使車輛能夠?qū)崟r避障。首先,對目標區(qū)域進行預(yù)處理,得到不同地物的環(huán)境信息并加以區(qū)分;然后,為驗證改正后算法的可靠性將其與A*算法進行實驗對比。當車輛沿著規(guī)劃好的全局最優(yōu)路徑行進中遇到未知障礙物時,實時更新環(huán)境信息,確定局部規(guī)劃區(qū)域。再次,應(yīng)用A*算法進行局部路徑規(guī)劃,將全局路徑規(guī)劃和局部路徑規(guī)劃相結(jié)合,從而規(guī)劃出一條安全無碰撞的最優(yōu)路徑。

1 基于二次A*算法的路徑規(guī)劃

1.1 A*算法原理

A*算法綜合分析BFS算法和Dijkstra算法各自的特點,取長補短,通過選擇合適的估價函數(shù)來引導(dǎo)搜索方向,減小搜索范圍,在保證搜索效率的情況下找到最優(yōu)路徑,是一種典型的啟發(fā)式搜索算法[8-10]。由于A*算法在搜索過程中都是選擇代價最低的節(jié)點,因此最終得到的路徑也是代價最低的。在A*算法中最關(guān)鍵的就是估價函數(shù)[11],其公式為

f(n)=g(n)+h(n),

(1)

其中:f(n)表示從起始節(jié)點經(jīng)由任意節(jié)點n到目標點的估價函數(shù);g(n)表示從起始節(jié)點到任意節(jié)點n的實際代價;h(n)表示從節(jié)點n到目標點的啟發(fā)式評估代價。啟發(fā)式函數(shù)h(n)表示任意節(jié)點n到目標節(jié)點的最小代價評估值。

通過實驗發(fā)現(xiàn)A*算法在進行復(fù)雜環(huán)境路徑規(guī)劃時存在一個明顯的缺點:無法在動態(tài)環(huán)境下進行路徑規(guī)劃。從A*算法的原理進行分析:估價函數(shù)值f(n)由g(n)和h(n)2個部分組成,而g(n)和h(n)的取值都和環(huán)境信息有著密切的關(guān)系。因此只有在全局環(huán)境信息已知的情況下,A*算法才能規(guī)劃得到最優(yōu)路徑[12]。如果已知環(huán)境中突然出現(xiàn)未知障礙物或不可通行區(qū)域,A*算法就不能完成預(yù)設(shè)定的路徑。

運動過程中突然出現(xiàn)障礙物情形如圖1所示,其中黑色方格表示障礙物,其他白色方格為可通行區(qū)域,紅色三角形表示起點P,紅色圓圈表示目的地Q,虛線為規(guī)劃的路徑。當按照規(guī)劃好的路徑移動,運動到M點時突然遇到障礙物(灰色方格所示),此時就不能繼續(xù)前進,此次路徑規(guī)劃失敗。

圖1 運動過程中突然出現(xiàn)障礙物Fig.1 Sudden obstructions during motion

1.2 改進A*算法

考慮到車輛在野地中的行駛速度會比在道路上慢得多,因此將道路設(shè)置為最高優(yōu)先級,即道路凌駕于障礙區(qū)之上,在居民區(qū)等不可通行區(qū)域中假如存在道路,那么車輛依然可以在這些障礙區(qū)內(nèi)的道路中行進。由于傳統(tǒng)A*算法在生成8鄰域的子節(jié)點時,沒有優(yōu)先順序。研究借鑒文獻[6]中的優(yōu)先級子節(jié)點生成策略提出一種改進搜索策略。

節(jié)點O的子節(jié)點生成策略如圖2所示。圖2中把與當前節(jié)點O的8個方向上的節(jié)點(A、B、C、D、P、Q、M、N)分成2組,將水平和垂直方向上的節(jié)點(A、B、C、D)設(shè)為優(yōu)先級,將對角線方向上的節(jié)點(P、Q、M、N)設(shè)為次優(yōu)先級。在生成子節(jié)點的過程中,優(yōu)先生成節(jié)點A、B、C、D,至于剩下的次優(yōu)先級節(jié)點根據(jù)如下規(guī)則生成:

(1) 當相鄰優(yōu)先節(jié)點A、B或B、C或C、D或D、A同是在道路上的節(jié)點,則優(yōu)先生成此方向與其對角線方向的子節(jié)點,比如節(jié)點A、B都在道路上,則優(yōu)先生成子節(jié)點P,并且路徑搜索方向偏向于OA、OB和OP;

圖2 節(jié)點O的子節(jié)點生成策略Fig.2 Node O’s child node generation policy

(2) 當A、B、C、D同時都是道路或野地上的節(jié)點,則生成的子節(jié)點沒有優(yōu)先順序;

(3) 當優(yōu)先節(jié)點中包含有野地上的節(jié)點,則不生成與其相鄰的子節(jié)點,比如節(jié)點A在野地上,則不生成子節(jié)點P、N,也就不會生成路徑OA、OP、ON。

此方法優(yōu)先選取在道路上的節(jié)點,從而在搜索路徑時優(yōu)先生成道路的路徑。為了區(qū)分道路速度和野地速度,通過MapX工具分別計算出所規(guī)劃路徑在道路和野地的長度,計算方法是:首先判斷間隔足夠小的2個特征點是否同時在道路或野地上,然后通過迭代將每2個點間的距離分別相加得到道路和野地的長度。同時給道路行駛速度和野地行駛速度取一個適當?shù)谋壤?這里設(shè)定車輛在道路的速度為57 km/h,在野地的速度為26 km/h。

1.3 基于二次A*算法的路徑規(guī)劃原理

由上所述,A*算法的估價函數(shù)與環(huán)境信息有著密切的關(guān)系。在環(huán)境信息未知的情況下,A*算法規(guī)劃路徑存在很大的不足,且無法動態(tài)規(guī)劃路徑。研究提出了一種預(yù)處理的算法,在進行路徑規(guī)劃前先將所選區(qū)域進行預(yù)處理,這樣就可以充分掌握該地區(qū)的環(huán)境信息,進一步我們可以將該區(qū)域內(nèi)的可通行區(qū)域和障礙物進行區(qū)分。同時,加入預(yù)處理算法能夠有效彌補A*算法只能靜態(tài)規(guī)劃路徑的缺陷。在復(fù)雜的野外環(huán)境下,當車輛由于遭遇極端天氣等不可抗力因素導(dǎo)致原先可通行區(qū)域變得不可通行,這時就需要實時更新局部環(huán)境信息從而實現(xiàn)動態(tài)路徑規(guī)劃。

二次A*算法實質(zhì)上就是已規(guī)劃好的路徑在運動過程中突然出現(xiàn)未知障礙物時,再次采用A*算法進行障礙區(qū)的局部規(guī)劃而不需要重新運用A*算法進行全局規(guī)劃,局部規(guī)劃區(qū)域明顯小于全局規(guī)劃區(qū)域,因而局部規(guī)劃的時間比全局規(guī)劃少,提高了車輛運動的實時性。

基于二次A*算法的路徑規(guī)劃步驟如下:

(1) 地圖預(yù)處理,明確全局環(huán)境信息后選取起始點和終點進行全局最優(yōu)路徑規(guī)劃;

(2) 運動過程中突然出現(xiàn)未知障礙物,那么已規(guī)劃好的路徑就無法到達目的地,此時更新局部環(huán)境信息,確定局部規(guī)劃區(qū)域,運用A*算法進行局部路徑規(guī)劃;

(3) 結(jié)合全局規(guī)劃和局部規(guī)劃得到最優(yōu)路徑。路徑規(guī)劃流程如圖3所示。

圖3 基于二次A*算法的路徑規(guī)劃流程Fig.3 Path planning flowchart based on the quatratic A* algorithm

1.4 確定局部規(guī)劃區(qū)域

車輛運動過程中突然遇到障礙物時需要進行局部路徑規(guī)劃,當運動的下一步有障礙物時,記下當前點的位置,并檢測前進路徑中障礙物所占有的格子數(shù),記為n,設(shè)s=n+5,當s為奇數(shù),則s=n+6。局部規(guī)劃區(qū)域如圖4所示。①當下一步障礙物所占格子(圖4中黑色格子)在當前格子(圖4(a)中灰色格子)的正上方(或正下方),則局部規(guī)劃區(qū)域為(s+1)×s,如圖4(a)中第2行到第8行的方框區(qū)域(圖4(a)中線條突出區(qū)域);②當下一步障礙物所占格子在當前格子的左側(cè)(或右側(cè)),則局部規(guī)劃區(qū)域為s×(s+1),如圖4(b)中第2行到第7行的方框區(qū)域(圖4(b)中線條突出區(qū)域);③當下一步障礙物所占格子在當前格子的斜45°方向,則局部規(guī)劃區(qū)域為s×(s+1),如圖4(c)中第2行到第9行的方框區(qū)域(圖4(c)中線條突出區(qū)域)。

圖4 局部規(guī)劃區(qū)域Fig.4 Local planning area

1.5 預(yù)處理算法分析

(1) 地圖環(huán)境構(gòu)建 真實地圖柵格化如圖5所示。圖5(a)表示的是真實地圖,其中綠色的部分和白色的部分分別表示障礙物區(qū)域和可通行區(qū)域。圖5(b)表示對真實地圖環(huán)境加以柵格化,黑色表示障礙區(qū),白色表示可通行區(qū)域。在將真實地圖柵格化的過程中,把障礙區(qū)適當?shù)財U大以達到車輛安全無碰撞通行的目的。

圖5 真實地圖柵格化Fig.5 Real map rasterization

(2) 地圖要素的提取與表示 地圖要素可以分為障礙要素和可通行要素。障礙要素包括居民區(qū)、湖泊、河流等,可通行要素一般是指野外環(huán)境下可以通行的道路。各個地圖要素的類型和處理方式如表1所列。

由于不可通行的障礙都是有特征的(線狀要素或面狀要素)而且相對于可通行區(qū)域要小的多,為了更方便路徑規(guī)劃,將不可通行區(qū)域提取出來,把表示障礙的區(qū)域或者曲線用紅色繪制出來,表示不可通行區(qū)域;同時將可通行的道路用綠色繪制出來,表示可通行的道路。

障礙區(qū)域的提取與繪制見圖6。圖6(a)表示河流、湖泊以及居民區(qū)等障礙區(qū)域,白色條紋區(qū)域表示居民區(qū),淺藍色線條和區(qū)域分別表示河流和湖泊。經(jīng)過地圖要素的提取,將這些障礙區(qū)域用紅色標出,對比圖6(a)和圖6(b),可以看出,在繪制障礙區(qū)域時,將表示線狀要素的障礙區(qū)(河流)以及面狀要素的障礙區(qū)域(居民區(qū)和湖泊)的邊界適當擴大,來達到使車輛安全通行的目的。

道路及等距線繪制見圖7。圖7(a)中的黑色線條表示可通行的道路。經(jīng)過地圖要素的提取,將可通行的道路用綠色標出(見圖7(b))。圍繞在綠色道路周圍的藍色線條則是道路等距線,在進行路徑規(guī)劃時用來分析離道路的距離,從而達到快速接近道路的目的。

表1 地圖要素及其處理方式

圖6 障礙區(qū)域的提取與繪制Fig.6 Extraction and plotting of obstacle areas

圖7 道路及等距線繪制Fig.7 Road and equidistant line plotting

2 實驗驗證

為了驗證提出方法的可行性,選擇沈陽某地區(qū)的dem地圖進行實驗。實驗環(huán)境:Windows 10,Visual Studio 2010,MapX 5.0,以C++為實驗平臺。由于dem地圖的區(qū)域過大,因此實驗時選取其中具有代表性的一塊區(qū)域進行分析。

研究將所選區(qū)域柵格化,具備居民區(qū)、湖泊、河流、道路等地圖要素,可以滿足實驗的要求,如圖8所示。將所選區(qū)域進行地圖預(yù)處理,明確全局環(huán)境信息,結(jié)果如圖9所示,圖9中的柵格表示采樣密度為8 000的實際大小。

圖8 地圖柵格化Fig.8 Map rasterization

圖9 地圖預(yù)處理結(jié)果Fig.9 Map preprocessing results

為了提升實驗的可靠性,根據(jù)不同的地形條件選取4組不同的起點和終點進行對比實驗,如圖10所示。圖10中黃線表示傳統(tǒng)A*算法規(guī)劃的路徑,藍線表示改進A*算法規(guī)劃的路徑,圖10(a)～(d)對應(yīng)的地形條件越來越復(fù)雜,相應(yīng)的實驗結(jié)果見表2和表3。當遇到未知障礙物時,二次A*算法動態(tài)規(guī)劃的路徑結(jié)果如圖11所示,圖11(a)是沒有障礙物時的路徑規(guī)劃;圖11(b)為突然遇到障礙物,紅色網(wǎng)格區(qū)域為未知障礙區(qū)域;圖11(c)為局部規(guī)劃后,結(jié)合全局規(guī)劃和局部規(guī)劃得到的運動路徑。

實際規(guī)劃路徑長度對比見表2,相應(yīng)的搜索時間與行駛時間對比見表3。從實驗結(jié)果可知:①預(yù)處理使全局環(huán)境信息更加明了,便于之后的路徑規(guī)劃。②改進搜索策略的算法雖然在實際規(guī)劃的路徑長度和搜索時間有所犧牲,但減少了行駛時間,提高了車輛的機動性。從實驗結(jié)果來看,隨著地形條件的不斷復(fù)雜,行駛時間分別減少了26.6%、29.5%、32.4%和35.2%,說明改進后的算法適用于復(fù)雜的環(huán)境。③局部規(guī)劃的區(qū)域比全局區(qū)域小,減少了總體規(guī)劃時間,提高了車輛運動的實時性。

3 結(jié)論

針對車輛無法在環(huán)境信息未知的復(fù)雜野外環(huán)境下快速到達目的地的問題,提出了一種加入預(yù)處理算法的基于二次A*算法的車輛路徑規(guī)劃方法。通過地圖預(yù)處理明確全局環(huán)境信息,提取障礙物信息和可通行區(qū)域,通過改進搜索策略的A*算法規(guī)劃出一條快速到達目的地的路徑,當出現(xiàn)未知障礙物時確定局部規(guī)劃區(qū)域,結(jié)合全局規(guī)劃和局部規(guī)劃,得到最優(yōu)路徑。實驗結(jié)果表明:改進算法規(guī)劃的路徑雖然犧牲了部分搜索時間和路徑長度,但對于車輛的機動性有了極大的提高,有利于車輛在作戰(zhàn)或演習(xí)中快速到達指定位置,同時地形越復(fù)雜,算法的優(yōu)化程度越明顯,適用于復(fù)雜環(huán)境的路徑規(guī)劃。并且在遇到障礙物時,由于局部規(guī)劃區(qū)域比全局區(qū)域小,因而減少了總體的規(guī)劃時間,提高了車輛的實時性。

圖10 2種算法對比Fig.10 Comparison of two kinds of algorithms

圖11 動態(tài)路徑規(guī)劃Fig.11 Dynamic path planning

表2 實際規(guī)劃路徑長度對比

表3 搜索時間與行駛時間對比