基于連續(xù)路網環(huán)境的實體化主體路徑規(guī)劃算法

劉 鋼, 安志鏢, 張茂軍, 劉 煜, 李 武

(1. 湖南理工學院信息科學與工程學院, 湖南 岳陽 414000; 2. 國防科技大學系統(tǒng)工程學院, 湖南 長沙 410073; 3. 湖南民族職業(yè)學院, 湖南 岳陽 414000)

0 引 言

復雜環(huán)境下的路徑規(guī)劃問題始終是任務規(guī)劃領域研究的熱點。隨著環(huán)境的實時動態(tài)變化,需要同時考慮環(huán)境連續(xù)變化和規(guī)劃主體大小,使得規(guī)劃路徑能夠切實支撐規(guī)劃主體順利通過連續(xù)變化的環(huán)境。

實體化主體路徑規(guī)劃就是指根據主體的實體屬性,規(guī)劃出能夠在真實環(huán)境中正常行駛的最優(yōu)或可行路線。真實環(huán)境信息往往無法直接使用,所以需要對環(huán)境信息進行建模操作。周夢如等[1]提出了基于激光雷達和可見光相機的道路幾何通行性分析方法,利用圖像語義分割技術獲得大致可通行區(qū)域,使用三維點云獲取地面幾何信息,并結合履帶車輛的幾何通行性約束和高斯混合聚類,獲得道路野外環(huán)境模型。Papadakis[2]對于車輛可通行性的研究做了調查,使用激光雷達等技術獲得三維地形數據,并將獲得三維地形數據與車輛模型相結合進行路徑規(guī)劃,是越野行駛、星球探索等環(huán)境下較常用的路徑規(guī)劃環(huán)境建模的方法。Hosseinpoor等[3]提出語義地形分割(semantic terrain segmentation, STS)方法,通過根據不同高度分割地形,并使用深度卷積神經網絡獲得結果。目前,環(huán)境信息建模大多采用二元分類的方式進行,即僅將環(huán)境信息分為可通行性和不可通行性兩種狀態(tài),環(huán)境信息未得到有效利用,為更有效利用環(huán)境信息,提出了連續(xù)環(huán)境建模方式。

針對路徑規(guī)劃的環(huán)境建模,目前主要分為柵格法和可視圖法。Alireza等[4]提出以柵格為單元表示該區(qū)域環(huán)境信息。Yao等[5]針對環(huán)境特征,將柵格圖中的障礙物分為靜態(tài)障礙物、動態(tài)障礙物和準靜態(tài)障礙物。Lozano-Perez等[6]提出以多邊形描述環(huán)境邊界和障礙物輪廓,即可視圖法。針對路徑規(guī)劃的環(huán)境建模方法一般都將路徑規(guī)劃的實體化主體視為一個質點,但在實際應用中,將實體化主體視為一個質點將損失實體化主體的信息,所以針對主體的大小提出實體化主體連續(xù)環(huán)境模型(model continuous environment with subject objective, MCESO)構建方式。

1 MCESO

1.1 建模思路

MCESO的影響因子主要包含地形因子、植被因子、水文因子、人工設施因子和實體化主體因子。地形因子主要表示地形起伏及坡度信息;植被因子、水文因子和人工設施因子是根據實際場景地物分類信息;實體化主體因子主要考慮實體化主體的大小。實體化主體連續(xù)環(huán)境建模需要在坡度信息和地物分類信息融合的連續(xù)環(huán)境模型的基礎上,通過考慮實體化主體大小膨脹操作和清除小連通區(qū)域得到MCESO,本文實體化主體以車輛為例。MCESO生成框架圖如圖1所示。

圖1 MCESO框架圖Fig.1 Frame diagram of MCESO

1.2 連續(xù)環(huán)境建模

為降低連續(xù)環(huán)境建模的數據存儲量,將每個像素值的大小設為1 bit,其對應的數值范圍為[0,255],數據應符合如下公式:

data(x,y)=m,m=0,1,…,255

(1)

式中:data表示野外環(huán)境的數據;m為0～255的正整數。

由于野外環(huán)境信息主要為坡度信息和地物分類信息,為方便使用,將數據做如下定義:

其中obstacle表示障礙:

data(x,y)=255, data(x,y)=obstacle

(2)

首先處理地形因子。地形因子主要是坡度信息,為滿足式(1),坡度信息表達式為

(3)

式中:rad表示弧度制結果;int表示對括號內結果取整數部分;ang表示角度值結果。

以車輛為例,在實際行駛的過程中都存在最大坡度。當坡度大于最大坡度時可能出現(xiàn)側滑情況。為避免車輛發(fā)生側滑現(xiàn)象,將坡度大于最大坡度的位置設為障礙。根據式(2),坡度處理公式為

(4)

式中:slope表示處理后的坡度值;maxslope表示行駛時的最大坡度。

對于植被因子、水文因子和人工設施因子,由于數據是數字正射影像圖(digital orthophoto map, DOM),需要對DOM數據做地物分類操作,然后對分類結果定性分析。根據式(2),認為植被、水體、房屋為障礙obstacle,認為道路street等為易通行。地物分類處理公式為

(5)

式中:classify表示處理后的分類結果;dom為原始的DOM數據。

通過式(4)和式(5)可以生成處理后的坡度信息和地物分類信息。對坡度信息和地物分類信息進行融合操作,即可得到最終的連續(xù)環(huán)境模型。判斷公式為

(6)

式中:accessibility表示連續(xù)環(huán)境模型的柵格值;classify表示地物分類信息的柵格值;slope表示坡度信息的柵格值。

通過式(6)生成的連續(xù)環(huán)境模型充分利用坡度信息和地物分類信息的結果。

1.3 MCESO建模

通過第1.2節(jié)生成的連續(xù)環(huán)境建?？紤]了坡度信息和地物分類信息對于環(huán)境模型的影響,構建連續(xù)環(huán)境模型的主要目的是為了能夠在路徑規(guī)劃時能夠充分利用環(huán)境信息。但車輛路徑規(guī)劃不僅要考慮自然環(huán)境對于路徑規(guī)劃的影響,還需要考慮車輛本身屬性對于路徑規(guī)劃的影響。

對于車輛路徑規(guī)劃,需考慮的車輛屬性應包含車輛大小和車輛轉彎半徑。由于在大范圍全局路徑規(guī)劃時不需考慮車輛的具體姿態(tài),所以提出在連續(xù)環(huán)境模型中加入實體化主體約束,然后在路徑規(guī)劃時通過利用MCESO建模得到考慮主體自身約束的規(guī)劃結果。

實體化主體約束中包含實體化主體大小與轉彎半徑約束,其實際意義是確保實體化主體能夠安全平穩(wěn)的行駛。為將實體化主體約束融合到連續(xù)環(huán)境模型中,提出使用膨脹操作,將實體化主體約束融合到膨脹核中,使用膨脹操作對連續(xù)環(huán)境模型進行操作。膨脹操作示意圖如圖2所示,膨脹操作的公式如下:

圖2 膨脹操作Fig.2 Expansion operation

swell(x-a,y-a)=

(7)

式中:f(x,y)表示原始圖像(x,y)位置的像素值;w(a,b)表示膨脹核(a,b)位置的像素值。

膨脹操作的核心是膨脹核的設計。膨脹核的設計以使用實體化主體大小作為基準。以車輛為例,基于設計的簡單性,僅考慮車輛的8個方向。如圖3所示,A、B、C、D分別表膨脹核的4個頂點;M、N、P、Q分別表示車輛的大小。其中,白色視為車輛,其像素值設為0;綠色為非車輛位置,像素值為1。由于車輛的8個方向可以認為是車輛的4個姿態(tài),圖3(a)為車身上下方向姿態(tài);圖3(b)為車身左右方向姿態(tài);圖3(c)為車身向左傾斜45°方向姿態(tài);圖3(d)為車身向右傾斜45°方向姿態(tài)。

圖3 膨脹核設計Fig.3 Expansion kernel design

使用圖3中的所有膨脹核對連續(xù)環(huán)境模型做膨脹操作,膨脹操作的結果可以認為車輛在膨脹后的任意一點都可通行。對4個膨脹核膨脹后的結果做融合,即認為只要4個膨脹核膨脹后結果中的像素有一個點可通行,該像素點認為可通行。膨脹融合公式為

fusion(i,j)=max(fusion1(i,j),

fusion2(i,j),fusion3(i,j),

fusion4(i,j))

(8)

式中:fusion表示膨脹融合的結果;fusion1, fusion2, fusion3, fusion4分別表示對于4個膨脹核膨脹后的結果。式(8)表示當前像素點的數值為4個膨脹結果像素值的最大值。

對實體化主體連續(xù)環(huán)境建模進行膨脹操作后會出現(xiàn)大量小連通區(qū)域。小連通區(qū)域是指該區(qū)域不與其余區(qū)域連通,并且區(qū)域較小,該區(qū)域進行路徑規(guī)劃計算是不合理的,所以需要對小連通區(qū)域進行清除工作,清除小連通區(qū)域的方法如圖4所示。

圖4 清除小連通區(qū)域方法流程圖Fig.4 Flowchart of clearing small connected domaining method

1.4 建模流程

實體化主體連續(xù)環(huán)境建模的構建步驟如下。

步驟 1根據數字高程模型利用坡度計算公式得到坡度信息數據。

步驟 2根據DOM利用深度學習方法得到地物分類信息數據。

步驟 3將坡度信息數據與地物分類信息數據進行融合,得到連續(xù)環(huán)境模型。

步驟 4將野外環(huán)境模型根據車輛參數信息進行膨脹操作,得到連續(xù)環(huán)境膨脹模型。

步驟 5將可通行性膨脹分析結果進行清除小連通區(qū)域操作,得到MCESO。

MCESO算法流程圖如圖5所示。

圖5 MCESO流程圖Fig.5 Flow chart of MCESO

2 MCESO-RNP-路徑規(guī)劃算法

2.1 MCESO-算法的估價函數優(yōu)化

圖6 MCESO示意圖Fig.6 Diagram of MCESO

(9)

式中:ln-1.n表示上一個節(jié)點到當前節(jié)點的距離;ln,goal表示從當前節(jié)點到終點的距離;slope表示MCESO通過某一位置的代價;α表示對環(huán)境模型的懲罰程度。

2.2 MCESO-RNP-算法的估價函數優(yōu)化

圖7 道路權重圖示意圖Fig.7 Diagram of road weights

(10)

式中:weight表示道路權重圖在某一位置的權重值。

2.3 算法流程

圖8 MCESO-RNP-算法流程圖Fig.8 Flow chart of MCESO-RNP- algorithm

算法流程如下所示。

步驟 1初始化起點列表StartSet,終點列表EndSet和搜索過的列表CloseSet。起點列表表示從起點開始搜索的OpenSet,終點列表表示從終點開始搜索的OpenSet。

步驟 3通過代價表判斷起點或終點是否在障礙中。若在障礙中,則結束算法;若不在,則執(zhí)行步驟4。

步驟 4將起點放入StartSet中,終點放入EndSet中,并從代價表中查出通過當前節(jié)點的代價。

步驟 5分別從StartSet和EndSet取出代價最小的節(jié)點作為當前節(jié)點,并從StartSet和EndSet刪除該節(jié)點。

步驟 6判斷當前節(jié)點是否相遇或StartSet、EndSet中是否為空。若相遇或為空,則執(zhí)行步驟8;若否,則執(zhí)行步驟7。

步驟 7使用八鄰域的方式擴展當前節(jié)點,并將擴展的節(jié)點放入StartSet或EndSet中,并將當前節(jié)點放入CloseSet中,然后執(zhí)行步驟5。

步驟 8從終點回溯到起點,得到最終路徑。

3 仿真試驗與分析

MCESO仿真分別對連續(xù)環(huán)境模型,連續(xù)環(huán)境模型膨脹后結果和MCESO進行對比。

表1 不同算法對比Table 1 Comparison of different algorithms

3.1 MCESO仿真

實驗需要真實環(huán)境下的數字高程模型和DOM,實驗的真實環(huán)境如圖9所示,分別對真實環(huán)境中的數字高程模型和DOM進行處理。

圖9 真實環(huán)境圖Fig.9 Real environment diagram

將坡度圖與地物分類圖進行融合操作得到連續(xù)環(huán)境模型如圖10所示,圖中白色部分表示障礙區(qū)域,黑色表示道路,灰色部分表示該區(qū)域的坡度。構建使用車輛參數為4 m×7.5 m,構建膨脹所需的膨脹核,使用該膨脹核膨脹后的膨脹圖如圖11所示。從圖10和圖11可以看出,連續(xù)環(huán)境模型由于沒有考慮運動實體大小,可通過區(qū)域明顯大于使用膨脹后的結果。MCESO膨脹圖如圖12所示,通過圖11和圖12可以看出,存在小部分的非連通區(qū)域。為避免路徑規(guī)劃時出現(xiàn)非必要的路徑不可通行的情況,所以對膨脹結果圖進行連通性分析操作,即根據的閾值為全部面積的0.001,進行消除小連通區(qū)域操作。

圖10 連續(xù)環(huán)境模型圖Fig.10 Model continuous environment

圖11 連續(xù)環(huán)境建模膨脹后結果Fig.11 Model continuous environment after expansion

圖12 MCESOFig.12 MCESO

3.2 MCESO-RNP-路徑規(guī)劃算法仿真

本算法考慮到環(huán)境信息的地形因素(即坡度)對路徑規(guī)劃結果的影響,而在大范圍內無法直觀感覺出地形因素的影響,所以首先在小范圍內對路徑規(guī)劃進行三維仿真實驗。仿真所用數據圖如圖13所示。圖14為數字環(huán)境圖,圖15為懲罰較小時的結果圖,圖16為懲罰較大時的結果圖。

圖13 仿真數據圖Fig.13 Simulation data diagram

圖14 數字環(huán)境圖Fig.14 Digital environment diagram

圖15 懲罰值為3的結果圖Fig.15 Result diagram with a penalty value of 3

圖16 懲罰值為7的結果圖Fig.16 Result diagram with a penalty value of 7

從圖15與圖16的對比明顯看出,將懲罰值設置較大時可以對坡度值進行有效懲罰。并且懲罰值越大,路徑規(guī)劃的結果越選擇更平緩的路徑。通過圖17結果實驗對比,選擇α=7作為后續(xù)實驗的懲罰值。

圖17 路徑規(guī)劃長度與懲罰值比值圖Fig.17 Ratio of path planning length to penalty value

在進行路徑規(guī)劃時使用三維展示的方式存在無法全局展示、效果不能直接對比等缺點,所以在實際仿真實驗中使用二維的方式展示實驗效果。

圖18 MCE-算法路徑規(guī)劃1Fig.18 MCE- algorithm path planning 1

圖19 MCE-算法路徑規(guī)劃2Fig.19 MCE- algorithm path planning 2

圖20 MCESO-算法路徑規(guī)劃1Fig.20 MCESO-algorithm path planning 1

圖21 MCESO-算法路徑規(guī)劃2Fig.21 MCESO-algorithm path planning 2

圖22 道路權重圖(局部)Fig.22 Road weight map (local)

圖23 小范圍MCESOFig.23 Small scale of MCESO

圖24 MCESO-RNP-算法路徑規(guī)劃1Fig.24 MCESO-RNP-algorithm path planning 1

圖25 MCESO-RNP-算法路徑規(guī)劃2Fig.25 MCESO-RNP-algorithm path planning 2

表2 不同地圖下算法結果對比Table 2 Comparison of algorithm results under different maps

表3 不同算法時間結果對比Table 3 Comparison of time results for different algorithms s

3.3 算法性能分析與比較

對實驗結果進行比較分析,得到如下結論。

(1) 針對文獻[21]中的算法在計算節(jié)點代價時采用實時計算的方式,提出構建基于任務實體的連續(xù)環(huán)境建模的方式將代價融入環(huán)境模型中,提升搜索效率。

4 結束語