移動智能體在三維凹形障礙物下的路徑規(guī)劃

任文杰 徐啟蕾

(青島科技大學(xué)自動化與電子工程學(xué)院 山東 青島 266061)

0 引 言

路徑規(guī)劃是移動智能體研究的主要內(nèi)容之一，保證移動智能體能安全無碰撞地從起點(diǎn)行至終點(diǎn)。為了更好更快地完成移動智能體的任務(wù)，還需要使移動智能體的路徑距離盡可能短，使規(guī)劃時間盡可能短，保證任務(wù)完成的實(shí)時性。

目前對于三維環(huán)境下的路徑規(guī)劃成為當(dāng)前移動智能體研究的熱點(diǎn)。文獻(xiàn)[1]對蟻群算法進(jìn)行改進(jìn)，對路徑節(jié)點(diǎn)的選取方法、信息素的更新方法、啟發(fā)函數(shù)的設(shè)計進(jìn)行了改進(jìn)，從而避免算法陷入局部最優(yōu)解。文獻(xiàn)[2]提出了一種基于LSTM-PPO的三維路徑規(guī)劃算法，將收集到的狀態(tài)空間和動作狀態(tài)引入長短時記憶網(wǎng)絡(luò)，通過額外的獎懲函數(shù)和好奇心驅(qū)動避開大型障礙物，最后利用PPO算法的截斷項(xiàng)機(jī)制使得規(guī)劃策略更新的幅度更加優(yōu)化，可以適用于存在多樣障礙物的未知環(huán)境。文獻(xiàn)[3]使用遺傳算法對無人機(jī)的三維航跡進(jìn)行規(guī)劃，生成了較短也光滑的路徑。文獻(xiàn)[4]在改進(jìn)勢場函數(shù)的基礎(chǔ)上，通過最優(yōu)化方法對引力及斥力函數(shù)中的增益系數(shù)進(jìn)行調(diào)整，從而改變合力的信息，使用人工勢場法規(guī)劃出了可行路徑，可以更好地避免局部極小值。文獻(xiàn)[5]對A*算法進(jìn)行改進(jìn)，綜合飛行高度和航跡長度等權(quán)重因子，設(shè)計了變權(quán)值的路徑估價函數(shù)，最后提出了航線優(yōu)化算法，規(guī)劃出了較好的無人機(jī)航跡路線。

當(dāng)環(huán)境中存在凹形障礙物時，常用的路徑規(guī)劃算法易陷入局部最優(yōu)[6]。對凹形障礙物的處理，是目前移動智能體路徑規(guī)劃研究的重難點(diǎn)。目前在環(huán)境中存在凹形障礙物的規(guī)劃算法多為二維環(huán)境下的。文獻(xiàn)[7]提出了一種突出點(diǎn)法對二維環(huán)境中的凹多邊形障礙物進(jìn)行處理，使其可以用于對凸多邊形進(jìn)行處理的規(guī)劃算法。文獻(xiàn)[8]提出了一種凸點(diǎn)法，解決復(fù)雜環(huán)境下兩點(diǎn)間局部最優(yōu)無碰路徑的規(guī)劃。文獻(xiàn)[9]提出了一種雙蟻群完全交叉算法，在蟻群算法的基礎(chǔ)上增加新型的啟發(fā)因子，解決了存在凹形障礙物的路徑規(guī)劃問題。文獻(xiàn)[10]提出了一種組合導(dǎo)航算法，使移動智能體在運(yùn)行態(tài)和避障態(tài)之間切換從而解決了局部振蕩的問題。

但以上算法都不能普遍適用于三維環(huán)境中存在凹形障礙物的路徑規(guī)劃問題。本文提出一種非最優(yōu)路徑凹陷點(diǎn)處理的方法，對三維環(huán)境中存在凹形障礙物時的地圖進(jìn)行處理，避免存在凹形障礙物時的局部最優(yōu)，保證了移動智能體路徑規(guī)劃的實(shí)時性和可達(dá)性。

1 凹形障礙物存在時A*算法的局部最優(yōu)

A*算法為經(jīng)典的啟發(fā)式算法，在基于柵格法[11]建立的柵格地圖中，從起點(diǎn)開始搜索其鄰域點(diǎn)，在三維環(huán)境下即搜索其26個鄰域點(diǎn)，如圖1所示，每次計算當(dāng)前節(jié)點(diǎn)的啟發(fā)值F。

F=G+H

(1)

式中：G為從起點(diǎn)遍歷到當(dāng)前節(jié)點(diǎn)的實(shí)際距離；H為從當(dāng)前節(jié)點(diǎn)到終點(diǎn)的估計距離。

圖1 鄰域點(diǎn)搜索示意圖

每次計算完啟發(fā)值，比較之后將啟發(fā)值最小的點(diǎn)設(shè)置為下一個搜索點(diǎn)，直到搜索至終點(diǎn)，算法即結(jié)束。

如圖2所示，障礙物為凹形障礙物，陰影部分為內(nèi)部凹面，無法從一側(cè)經(jīng)由障礙物內(nèi)部穿過到達(dá)另一側(cè)。

圖2 凹形障礙物A星算法搜索示意圖

其中起點(diǎn)S、終點(diǎn)T分別位于障礙物兩側(cè)，S、T都是柵格點(diǎn)，且直線ST和障礙物存在交點(diǎn)H，即沿著直線ST無法從起點(diǎn)S到達(dá)終點(diǎn)T。此時使用A*算法來規(guī)劃從起點(diǎn)S到終點(diǎn)T的路徑。

由于A*算法優(yōu)先搜索的都是F值最小的點(diǎn)，而F等于從起點(diǎn)經(jīng)過當(dāng)前節(jié)點(diǎn)再到終點(diǎn)的距離。由兩點(diǎn)之間線段最短可知，F(xiàn)的最小值為從起點(diǎn)S到終點(diǎn)T的直線距離。

(2)

對于直線SH上的格點(diǎn)，則有：

F=Fmin

(3)

即SH上的路徑點(diǎn)的F最小，故算法會優(yōu)先沿著SH的方向搜索，搜索SH上的格點(diǎn)或者與SH距離最近的格點(diǎn)。由于A*算法是在三維柵格地圖下搜索柵格點(diǎn)的，當(dāng)SH上的點(diǎn)非柵格點(diǎn)時，則距離直線SH最近的柵格點(diǎn)優(yōu)先搜索。最后會搜索到距障礙物點(diǎn)H最近的柵格點(diǎn)O，此時沿著ST方向無法繼續(xù)搜索下去，則會搜索至凹形障礙物外一點(diǎn)P。

設(shè)從S點(diǎn)搜索到O點(diǎn)遍歷的最短距離為LSO，從O點(diǎn)搜索到P點(diǎn)遍歷的最短距離為LOP，而直接從S點(diǎn)遍歷至P點(diǎn)的最短距離為LSP，從P點(diǎn)遍歷到T的最短路徑為LPT。則有：

LSO+LOP+LPT>LSP+LPT

(4)

即對該凹形障礙物內(nèi)部從S搜索至O點(diǎn)，然后再搜索至P點(diǎn)得到的路徑非最優(yōu)路徑，只能得到局部最優(yōu)解。且對凹形障礙物內(nèi)部的搜索需要較大的計算量，使算法的運(yùn)行時間較長，無法保證規(guī)劃的實(shí)時性。

2 非最優(yōu)路徑凹陷點(diǎn)處理

為了防止路徑規(guī)劃算法在凹形障礙物存在時的局部最優(yōu)，對非最優(yōu)路徑凹陷點(diǎn)進(jìn)行處理來避免對易陷入局部最優(yōu)時的凹形障礙物內(nèi)部的搜索。

2.1 障礙物膨化

對于空間存在的隨機(jī)形狀的障礙物，在柵格法建立的地圖中其描述較為復(fù)雜。當(dāng)?shù)貓D中存在不規(guī)則形狀的障礙物時將其進(jìn)行填充，使其充滿所在的方格。圖3(a)-圖3(d)分別是柵格地圖中的幾個障礙物，其中陰影部分為障礙物的表面以及障礙物所在的柵格點(diǎn)。

(a) (b)

(c) (d)圖3 凹形障礙物膨化處理前后圖

其次，當(dāng)空間中存在多個障礙物時，存在著障礙物之間比較狹窄，對于移動智能體難以通行的情況，將障礙物進(jìn)行膨化處理之后則兩個比較接近的障礙物就被處理為一個障礙物。

如圖3(c)所示，兩個球形障礙物隔得比較近，中間部分對于移動智能體來說無法通過，故將兩個球形障礙物以及其中間部分處理為一個障礙物，處理完成之后如圖3(d)所示。這樣處理減少了障礙物的個數(shù)，減少算法需要的存儲空間。

設(shè)膨化處理之前的障礙物個數(shù)為m，處理之后的障礙物個數(shù)則為n。即處理完成之后的障礙物為{Oi|i=1,2,…,n}，且m≥n(m,n∈Z+)。

2.2 非最優(yōu)路徑凹陷點(diǎn)

對于膨化處理的障礙物Oi，設(shè)其三維坐標(biāo)邊界分別為Xi(max)、Xi(min)、Yi(max)、Yi(min)、Zi(max)、Zi(min)。則將邊界范圍內(nèi)的格點(diǎn)定義為障礙物所占據(jù)的格點(diǎn)PIB。

PIBi={Pj|Xi(max)≥Xj≥Xi(min),

Yi(max)≥Yj≥Yi(min),Zi(max)≥Zj≥Zi(min)}

(5)

式中：j=1,2,…,k,k∈Z+；Xj、Yj、Zj分別表示點(diǎn)Pj的三維坐標(biāo)。

即障礙物Oi所占據(jù)的格點(diǎn)為：

PIBi={Pj|j=1,2,…,k}(k∈Z+)

(6)

對于PIBi中的非障礙物格點(diǎn)Pj，同時滿足以下條件即定義為非最優(yōu)路徑凹陷點(diǎn)。

(1) 存在障礙物點(diǎn)Pj1、Pj2，使式(7)同時成立。

Rj1=Rj=Rj2,Uj1=Uj=Uj2,Vj1>Vj>Vj2

(7)

(2) 存在障礙物點(diǎn)Pj3、Pj4，使式(8)同時成立。

Rj3=Rj=Rj4,Vj3=Vj=Vj4,Uj3>Vj>Uj4

(8)

(3) 存在障礙物點(diǎn)Pj5，使式(9)同時成立。

Uj5=Uj,Vj5=Vj,Rj5≠Rj

(9)

式中：j、j1、j2、j3、j4和j5是從1到k的整數(shù)，且互不相等。

Rm、Um和Vm(m=j,j1,j2,j3,j4,j5)分別代表點(diǎn)Pm的三維坐標(biāo)。用Ri(max)、Ri(min)、Ui(max)、Ui(min)、Vi(max)和Vi(min)分別表示障礙物Oi的三維坐標(biāo)的邊界。

當(dāng)Rm表示X軸坐標(biāo)時，則Um表示Y軸坐標(biāo)，Vm表示Z軸坐標(biāo)(或者Vm表示Y軸坐標(biāo)，Um表示Z軸坐標(biāo))；當(dāng)Rm表示Y軸坐標(biāo)時，則Um表示X軸坐標(biāo)，Vm表示Z軸坐標(biāo)(或者Vm表示X軸坐標(biāo)，Um表示Z軸坐標(biāo))；當(dāng)Rm表示Z軸坐標(biāo)時，則Um表示X軸坐標(biāo)，Vm表示Y軸坐標(biāo)(或者Vm表示X軸坐標(biāo)，Um表示Y軸坐標(biāo))。

Rm、Um和Vm的三維坐標(biāo)表示與具體的障礙物形狀有關(guān)系。由于膨化處理后的障礙物的最小單位是棱長為格點(diǎn)寬度的正方體，所以以輪廓為類六面體的形狀來對障礙物的形狀進(jìn)行討論。

圖4(a)、圖4(b)、圖4(c)分別是幾種凹形障礙物，陰影部分為其內(nèi)部的凹陷表面，內(nèi)部為非障礙物格點(diǎn)。當(dāng)障礙物凹陷面位于六面體的正面或者后面時，如圖4(a)所示，此時Rm為Y軸坐標(biāo)，Um為X軸坐標(biāo)，Vm為Z軸坐標(biāo)(或者Um為Z軸坐標(biāo)，Um為X軸坐標(biāo))。當(dāng)障礙物凹陷面位于六面體的左面或者右面時，如圖4(b)所示，此時Rm為X軸坐標(biāo)，Um為Y軸坐標(biāo)，Vm為Z軸坐標(biāo)(或者Um為Z軸坐標(biāo)，Vm為Y軸坐標(biāo))。當(dāng)障礙物凹陷面位于六面體的上面或者下面(底面)時，如圖4(c)所示，此時Rm為Z軸坐標(biāo)，Um為X軸坐標(biāo)，Vm為Y軸坐標(biāo)(或者Um為Y軸坐標(biāo)，Vm為X軸坐標(biāo))。

(a) (b) (c)圖4 非最優(yōu)路徑凹陷點(diǎn)凹形障礙物

2.3 非最優(yōu)路徑凹陷點(diǎn)判定處理

圖5(a)-圖5(g)為一些凹形障礙物示意圖。其中陰影部分為其凹陷表面，內(nèi)部為非障礙物格點(diǎn)。

(a) (b) (c)

(d) (e) (f)

(g) (h)圖5 凹形障礙物凹陷點(diǎn)判定

其中圖5(a)、圖5(b)、圖5(c)所示的凹形障礙物，可以由障礙物外一側(cè)的點(diǎn)經(jīng)由障礙物內(nèi)的凹陷格點(diǎn)到達(dá)另一側(cè)，即該格點(diǎn)可以成為最優(yōu)路徑點(diǎn)。在對圖5(a)所示的障礙物進(jìn)行分析，內(nèi)部非障礙物格點(diǎn)顯然是不滿足非最優(yōu)路徑凹陷點(diǎn)的判定條件，即其內(nèi)部的凹陷點(diǎn)不是非最優(yōu)路徑凹陷點(diǎn)，圖5(b)和圖5(c)所示的障礙物也同理。

又如圖5(d)、圖5(e)、圖5(f)所示的凹形障礙物，可以由障礙物的一側(cè)從障礙物的內(nèi)部穿過到達(dá)另一側(cè)。顯然，這些障礙物內(nèi)部的柵格點(diǎn)也可以成為最優(yōu)路徑點(diǎn)。再對圖5(d)所示的障礙物分析，其內(nèi)部格點(diǎn)也不滿足非最優(yōu)路徑凹陷點(diǎn)的判定條件，則內(nèi)部的凹陷點(diǎn)也不是非最優(yōu)路徑凹陷點(diǎn)，圖5(e)和圖5(f)所示的障礙物也同理。

而如圖5(g)所示的凹形障礙物，無法從障礙物的一側(cè)經(jīng)由內(nèi)部格點(diǎn)直接穿越到達(dá)另一側(cè)。為了對圖5(g)所示的凹形障礙物下的可達(dá)路徑點(diǎn)進(jìn)行分析，建立三維坐標(biāo)系XYZ-O，如圖5(h)所示。其中對于非障礙物格點(diǎn)P，存在障礙物點(diǎn)A、B、C、D和E使下式成立：

1)YA=YP=YB,ZA=ZP=ZB,XB>XP>XA

2)XC=XP=XD,YC=YP=YD,ZC>ZP>ZD

3)XE=XP,ZE=ZP,YE>YP

即P點(diǎn)為非最優(yōu)路徑凹陷點(diǎn)，應(yīng)避免對P點(diǎn)的遍歷。對障礙物內(nèi)部的格點(diǎn)判定之后，如果為非最優(yōu)路徑凹陷點(diǎn)，應(yīng)避免對其的遍歷；如果不是非最優(yōu)路徑凹陷點(diǎn)，則這些柵格點(diǎn)也應(yīng)納入最優(yōu)路徑的考慮。

完整的算法如下：

步驟1初始化柵格地圖和膨化處理的凹形障礙物{Oi|i=1,2,…,n}。

步驟2從i=1開始對于當(dāng)前障礙物Oi得到其占據(jù)的柵格點(diǎn){Pj|j=1,2,…,k}。

步驟3從j=1開始判斷當(dāng)前節(jié)點(diǎn)Pj是否為障礙物點(diǎn)，若是則轉(zhuǎn)移至步驟5；否則轉(zhuǎn)移至下一步。

步驟4判斷當(dāng)前格點(diǎn)Pj是否為非最優(yōu)路徑凹陷點(diǎn)，若是則將該格點(diǎn)設(shè)置為障礙物點(diǎn)，然后轉(zhuǎn)移至下一步；否則轉(zhuǎn)移至下一步。

步驟5令j=j+1，并判斷j是否大于k，若是則轉(zhuǎn)移至下一步，否則轉(zhuǎn)移至步驟3。

步驟6令i=i+1，并判斷i是否大于n，若是則轉(zhuǎn)移至步驟2，否則算法結(jié)束。

完整的算法包括非最優(yōu)路徑凹陷點(diǎn)的判定和處理兩部分，如圖6所示。

(a) 非最優(yōu)路徑凹陷點(diǎn)處理

(b) 非最優(yōu)路徑凹陷點(diǎn)判定圖6 算法流程

本文方法只是一種環(huán)境建模的方法，對環(huán)境中存在的凹形障礙物建立地圖時進(jìn)行處理，處理完成之后可以使用其他路徑規(guī)劃算法來規(guī)劃路徑，如Dijkstra算法[12]、深度優(yōu)先算法[13]和廣度優(yōu)先算法[14]等。

3 仿真設(shè)計與結(jié)果

使用柵格法建立三維柵格地圖，設(shè)置格點(diǎn)寬度為1個單位長度。給定一膨化處理之后的凹形障礙物以及起點(diǎn)和終點(diǎn)，分別使用A*算法和蟻群算法規(guī)劃出一條從起點(diǎn)到終點(diǎn)的最優(yōu)路徑。使用MATLAB軟件，在i5-Y470(2.50 GHz)、4 GB內(nèi)存的筆記本電腦上運(yùn)行10次。

為了避免邊界碰撞，設(shè)置地圖范圍為X、Y和Z軸0～21，障礙物與邊界無交點(diǎn)，移動智能體的球心范圍為X、Y和Z軸1～20，即路徑點(diǎn)的范圍。起點(diǎn)坐標(biāo)為(5,2,5)，終點(diǎn)坐標(biāo)為(15,18,15)，仿真結(jié)果如圖7所示。

(a)

(b)

(c)

(d)

(e)圖7 仿真結(jié)果

圖7(a)為直接使用A*算法對凹形障礙物規(guī)劃的結(jié)果，圖7(b)為使用非最優(yōu)路徑凹陷點(diǎn)處理的A*算法對凹形障礙物規(guī)劃的結(jié)果，圖7(c)為A*算法對表面無凹陷的障礙物進(jìn)行規(guī)劃的結(jié)果，圖7(d)為使用蟻群算法對凹形障礙物規(guī)劃的結(jié)果，圖7(e)為使用凹陷點(diǎn)處理的蟻群算法對凹形障礙物規(guī)劃的結(jié)果。陰影部分為凹形障礙物的內(nèi)部凹陷表面、起點(diǎn)S和終點(diǎn)T。

由規(guī)劃得到的路徑可知，使用非最優(yōu)路徑凹陷點(diǎn)處理的A*算法避免了對凹形障礙物內(nèi)部的搜索；而直接使用A*算法規(guī)劃得到一條局部最優(yōu)的路徑，路徑長度明顯較長。

由表1所示的仿真數(shù)據(jù)可以看出，在凹形障礙物存在的環(huán)境直接使用A*算法搜索得到的路徑不是最優(yōu)路徑，規(guī)劃的路徑較長，且對凹陷點(diǎn)的遍歷導(dǎo)致了運(yùn)行時間過長。而使用非最優(yōu)路徑凹陷點(diǎn)處理的A*算法規(guī)劃的路徑明顯變短，避免了對凹陷點(diǎn)的遍歷，算法的計算量變小，保證了實(shí)時性。最后在和圖7(a)、圖7(b)外部輪廓一樣的無凹陷障礙物存在時使用A*算法進(jìn)行路徑規(guī)劃，如圖7(c)所示，得到了和非最優(yōu)路徑凹陷點(diǎn)處理的A*算法規(guī)劃時一樣的路徑，路徑長度也相等，算法的運(yùn)行時間也比較接近，說明非最優(yōu)路徑凹陷點(diǎn)處理有效地避免了對凹陷點(diǎn)的遍歷，能有效地避免算法陷入局部最優(yōu)。

表1 規(guī)劃路徑長度與算法運(yùn)行時間表

選取蟻群為50只，迭代次數(shù)為200，使用蟻群算法得到了如圖7(d)所示的仿真結(jié)果，但是由于多次遍歷導(dǎo)致的算法的計算量增加，且路徑長度比凹陷點(diǎn)處理的A*算法規(guī)劃的路徑要長。通過凹陷點(diǎn)處理以后的蟻群算法仿真結(jié)果如圖7(e)所示，由于避免了對凹陷區(qū)域的遍歷，使算法的計算量相應(yīng)地減少，且相同的蟻群數(shù)量和迭代次數(shù)得到了更短的路徑。

4 結(jié) 語

本文提出一種非最優(yōu)路徑凹陷點(diǎn)處理的方法。通過障礙物膨化處理，減少了所需要的存儲空間，然后給出了非最優(yōu)路徑凹陷點(diǎn)的判定方法，以及對非最優(yōu)路徑凹陷點(diǎn)的處理方法，將使規(guī)劃算法陷入局部最優(yōu)的可達(dá)路徑點(diǎn)轉(zhuǎn)換為障礙物點(diǎn)，避免了規(guī)劃算法對凹形障礙物的凹陷區(qū)域的遍歷，從而避免算法得到局部最優(yōu)解，適用于大范圍凹形障礙物環(huán)境下的移動智能體的路徑規(guī)劃。