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

任文杰 徐啟蕾

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

0 引 言

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

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

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

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

1 凹形障礙物存在時A*算法的局部最優

A*算法為經典的啟發式算法，在基于柵格法[11]建立的柵格地圖中，從起點開始搜索其鄰域點，在三維環境下即搜索其26個鄰域點，如圖1所示，每次計算當前節點的啟發值F。

F=G+H

(1)

式中：G為從起點遍歷到當前節點的實際距離；H為從當前節點到終點的估計距離。

圖1 鄰域點搜索示意圖

每次計算完啟發值，比較之后將啟發值最小的點設置為下一個搜索點，直到搜索至終點，算法即結束。

如圖2所示，障礙物為凹形障礙物，陰影部分為內部凹面，無法從一側經由障礙物內部穿過到達另一側。

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

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

由于A*算法優先搜索的都是F值最小的點，而F等于從起點經過當前節點再到終點的距離。由兩點之間線段最短可知，F的最小值為從起點S到終點T的直線距離。

(2)

對于直線SH上的格點，則有：

F=Fmin

(3)

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

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

LSO+LOP+LPT>LSP+LPT

(4)

即對該凹形障礙物內部從S搜索至O點，然后再搜索至P點得到的路徑非最優路徑，只能得到局部最優解。且對凹形障礙物內部的搜索需要較大的計算量，使算法的運行時間較長，無法保證規劃的實時性。

2 非最優路徑凹陷點處理

為了防止路徑規劃算法在凹形障礙物存在時的局部最優，對非最優路徑凹陷點進行處理來避免對易陷入局部最優時的凹形障礙物內部的搜索。

2.1 障礙物膨化

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

(a) (b)

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

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

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

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

2.2 非最優路徑凹陷點

對于膨化處理的障礙物Oi，設其三維坐標邊界分別為Xi(max)、Xi(min)、Yi(max)、Yi(min)、Zi(max)、Zi(min)。則將邊界范圍內的格點定義為障礙物所占據的格點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分別表示點Pj的三維坐標。

即障礙物Oi所占據的格點為：

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

(6)

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

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

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

(7)

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

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

(8)

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

Uj5=Uj,Vj5=Vj,Rj5≠Rj

(9)

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

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

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

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

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

(a) (b) (c)圖4 非最優路徑凹陷點凹形障礙物

2.3 非最優路徑凹陷點判定處理

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

(a) (b) (c)

(d) (e) (f)

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

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

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

而如圖5(g)所示的凹形障礙物，無法從障礙物的一側經由內部格點直接穿越到達另一側。為了對圖5(g)所示的凹形障礙物下的可達路徑點進行分析，建立三維坐標系XYZ-O，如圖5(h)所示。其中對于非障礙物格點P，存在障礙物點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點為非最優路徑凹陷點，應避免對P點的遍歷。對障礙物內部的格點判定之后，如果為非最優路徑凹陷點，應避免對其的遍歷；如果不是非最優路徑凹陷點，則這些柵格點也應納入最優路徑的考慮。

完整的算法如下：

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

步驟2從i=1開始對于當前障礙物Oi得到其占據的柵格點{Pj|j=1,2,…,k}。

步驟3從j=1開始判斷當前節點Pj是否為障礙物點，若是則轉移至步驟5；否則轉移至下一步。

步驟4判斷當前格點Pj是否為非最優路徑凹陷點，若是則將該格點設置為障礙物點，然后轉移至下一步；否則轉移至下一步。

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

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

完整的算法包括非最優路徑凹陷點的判定和處理兩部分，如圖6所示。

(a) 非最優路徑凹陷點處理

(b) 非最優路徑凹陷點判定圖6 算法流程

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

3 仿真設計與結果

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

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

(a)

(b)

(c)

(d)

(e)圖7 仿真結果

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

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

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

表1 規劃路徑長度與算法運行時間表

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

4 結 語

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