基于動態(tài)三角網(wǎng)格和啟發(fā)式搜索算法路徑規(guī)劃研究

王文霞

摘 要： 在靜態(tài)三角網(wǎng)絡(luò)的基礎(chǔ)上設(shè)計實現(xiàn)了動態(tài)三角網(wǎng)絡(luò)地圖算法，通過改變?nèi)蔷W(wǎng)格地圖障礙物變化過程中的網(wǎng)格節(jié)點代價值，設(shè)計相關(guān)算法模塊并對新生成的網(wǎng)格地圖實時獲取更新信息，實現(xiàn)動態(tài)變化效果。結(jié)合搜索算法Anytime Replanning A*（ARA*）和D* Lite，實現(xiàn)在動態(tài)環(huán)境中的路徑規(guī)劃方法Anytime Dynamic A*（AD*），通過對網(wǎng)格地圖中已擴展信息的保存和在有限時間內(nèi)不斷縮小膨脹因子，找到網(wǎng)格地圖中代價值最小的節(jié)點，動態(tài)規(guī)劃得到最優(yōu)路徑。在動態(tài)環(huán)境中，通過模擬動態(tài)場景圖，并應(yīng)用動態(tài)規(guī)劃方法建立相應(yīng)的數(shù)據(jù)搜索結(jié)構(gòu)和方法，對仿真效果進行分析和對比。

關(guān)鍵詞： 動態(tài)三角網(wǎng)格； AD*算法； 路徑規(guī)劃； 模擬仿真

中圖分類號： TN911.1?34； TM417 文獻標(biāo)識碼： A 文章編號： 1004?373X（2017）11?0103?04

Research on path planning based on dynamic triangular mesh

and heuristic search algorithm

WANG Wenxia

（Department of Computer Science and Technology， Yuncheng University， Yuncheng 044000， China）

Abstract： The dynamic triangular network map algorithm was designed and implemented on the basis of static triangular mesh to change the grid node cost value in the obstacle changing process of the triangular mesh map. The related algorithm was designed to acquire the updata information of the generated grid map in real time to realize the dynamic change effect. The search algorithm Anytime Replanning A*（ARA*） and D* Lite are combined to realize the path planning method A*（AD*） in dynamic environmet. The extended information in grid map is saved， and the expansion factor is reduced constantly in the finite time to find the node with minimum cost value in the grid map， and obtain the optimul path of dynamic planning. The dynamic scene graph is simulated in dynamic environment. The dynamic planning method is used to establish the corresponding data search structure and method to analyze and contrast the simulation results.

Keywords： dynamic triangular mesh； AD* algorithm； path planning； analog simulation

隨著社會經(jīng)濟的發(fā)展，找到合適、優(yōu)化而且適用于各種額外條件如動態(tài)環(huán)境、大規(guī)模人群、任意寬度路徑等一系列需求成為路徑規(guī)劃算法的研究方向[1]。本文研究在動態(tài)地圖網(wǎng)格中通過動態(tài)搜索算法，實時更新地圖信息的同時采用更加高效的搜索算法記錄當(dāng)前群體信息并反饋出動態(tài)環(huán)境中的環(huán)境變化情況，如人群規(guī)模、路徑寬度等相關(guān)信息，以便在較短時間內(nèi)避免碰撞和到達目的地。

1 動態(tài)環(huán)境地圖網(wǎng)格構(gòu)建及生成

1.1 動態(tài)三角網(wǎng)格設(shè)計實現(xiàn)

在構(gòu)建動態(tài)網(wǎng)格（Dynamic Local Clearance Triangulation，DLCT）[2]的過程中需要保持原有的LCT三角網(wǎng)格[3]中對路徑寬度的要求，在動態(tài)化的過程中添加和刪除障礙物時通過多邊形障礙物ID進行操作，在添加算法中把所有障礙物的限制條件遍歷直至找到需要添加障礙物的ID；在之后的刪除算法中會把與找到的障礙物ID相關(guān)的限制條件同時刪除。由于在地圖中障礙物之間可以相互覆蓋，因此每個ID可能關(guān)聯(lián)相互覆蓋的限制條件。

1.1.1 障礙物插入模塊

該模塊在原有LCT（S）的存儲單元中插入新的多邊形障礙物，在插入過程中，給障礙物設(shè)置新的ID。S設(shè)置為現(xiàn)在LCT（S）已存在的限制規(guī)劃，O是將插入的新多邊形障礙物。插入過程如下：

（1） 將多邊形障礙物O中的k條線段插入CDT中，對于需要修改的頂點和限制邊界分別存儲在兩個List中。List V中存儲所有已修改邊的鄰接頂點（其中包括因CDT交換的邊）；List C中存儲在插入障礙物過程中受限制的邊界。

（2） 對于List C中的限制邊界，通過搜索算法判斷S是否被阻塞。對于每個被阻塞的遍歷找到后，相關(guān)聯(lián)頂點加入List V。如圖1所示，通過搜索加入頂點兩邊可能因S引起阻隔。對每次遍歷來說，相關(guān)聯(lián)的阻隔頂點v被加入到List V中。

圖1 尋找被阻斷過程

（3） 在遍歷過程所有被List V中頂點影響的頂點將重新測試其屬性，判斷在當(dāng)前情況是否需要被限制， Local_Ref函數(shù)定義和遍歷了在變化過程中與V相鄰的頂點。對于每一個所有與相鄰的三角形都要被訪問。設(shè)為當(dāng)前與臨近被訪問的三角形，此時需要調(diào)用函數(shù)檢測是否有遍歷過的節(jié)點需要重新定義；函數(shù)查看的后繼節(jié)點是否被影響，當(dāng)找到被影響的路徑時，頂點和若不在List V中則系統(tǒng)自動進行添加，然后重新測試所有在V中的頂點。

1.1.2 障礙物刪除模塊

該模塊將把現(xiàn)有地圖中的多邊形障礙物移除，移除過程中通過在插入中設(shè)置的ID找到該障礙物。由于障礙物之間可以發(fā)生覆蓋，故對于每一個障礙物來說可能同時具有多個ID。刪除過程如下：

（1） 將已存儲的障礙物O從CDT中刪除，并把所有已修改與邊相關(guān)的頂點信息存儲在List V1中；

（2） 將與List V1被限制頂點的周圍臨近頂點找到后存入List V2；

（3） 把List V1中相關(guān)的頂點信息從CDT中刪除；

（4） 對于所有在執(zhí)行過程中需要限制的頂點，通過調(diào)用Local_Ref函數(shù)再次遍歷與其臨近的頂點信息，并對發(fā)生變化的進行改變。

1.1.3 動態(tài)三角網(wǎng)格構(gòu)建過程

基于動態(tài)三角網(wǎng)格地圖的動態(tài)環(huán)境構(gòu)建步驟如下：

（1） 獲取三角網(wǎng)格發(fā)生變化后的地圖信息。

（2） 獲取地圖中障礙物變化信息。

（3） 地圖網(wǎng)格中障礙物節(jié)點信息發(fā)生改變時，若有新節(jié)點加入，則添加后更新相鄰節(jié)點；若有節(jié)點刪除，則刪除后更新相鄰節(jié)點信息。

（4） 反復(fù)執(zhí)行步驟（3），直至所有的更新信息都處理完畢。

（5） 更新地圖中三角網(wǎng)格節(jié)點信息。

1.2 動態(tài)地圖構(gòu)建實現(xiàn)

動態(tài)三角網(wǎng)格地圖構(gòu)建過程如圖2所示，其中第一幅圖展示了在原始地圖中通過添加障礙物實現(xiàn)了地圖中存在若干個障礙物的情況，然后通過改變地圖網(wǎng)格中障礙物的個數(shù)、重新劃分三角網(wǎng)格以及移動障礙物和刪除算法等方法，實現(xiàn)了仿真過程中對真實地圖情況的模擬和地圖網(wǎng)格的構(gòu)建過程。

圖2 動態(tài)地圖實現(xiàn)效果

2 啟發(fā)式搜索動態(tài)路徑規(guī)劃方法

2.1 啟發(fā)式動態(tài)路徑規(guī)劃方法（AD*）

Anytime Dynamic A*（AD*）算法是在原有的動態(tài)環(huán)境下將D* Lite和ARA*結(jié)合，解決了在動態(tài)環(huán)境下高效率的路徑規(guī)劃問題[4]。在AD*算法中利用ARA*中膨脹因子不斷遞減的過程進行一系列路徑搜索。當(dāng)三角網(wǎng)格地圖環(huán)境發(fā)生改變并影響到邊的代價值時，在D*算法的OPEN表中對應(yīng)節(jié)點的Key值發(fā)生相應(yīng)改變。AD*算法具體執(zhí)行過程如下：

（1） 程序初始化。將OPEN，CLOSED和INCONS表清空，膨脹因子初始值為

（2） 計算最短路徑。計算最短路徑的過程中節(jié)點的擴展順序為從目標(biāo)點goal到起始點start進行擴展，尋找出從源點到目標(biāo)點的最短路徑。

（3） 個體從start開始沿路徑向goal前進。

（4） 在擴展過程中，將已擴展過的節(jié)點存儲在INCONS表中。

（5） 判斷前進過程中網(wǎng)格代價值是否發(fā)生變化，如果發(fā)生變化，則以當(dāng)前節(jié)點為新的起始點，并減小膨脹因子的值。

（6） 在下一次擴展過程中，用作為啟發(fā)函數(shù)，在擴展OPEN表中剩余的點之前，把上一輪INCONS表中的點插入OPEN表中，在原來OPEN表的基礎(chǔ)上修改所有與變化節(jié)點相關(guān)的rhs，key的值，清空CLOSE表。

（7） 以新的start點為起始點，goal為目標(biāo)點，運行步驟（3）直至到達目標(biāo)點。

2.2 地圖網(wǎng)格密度計算

基于三角網(wǎng)格密度的改進方法[5]，模擬人群仿真的過程在不同時間內(nèi)每個三角網(wǎng)格參數(shù)值因agents數(shù)量的不同而發(fā)生變化，因此把密度信息添加到Key值的計算中，可以估算每一個OPEN表中節(jié)點的優(yōu)先權(quán)從而得到最優(yōu)路徑。density（n）表示第個節(jié)點所在三角網(wǎng)格的密度值；是通過節(jié)點和距離信息來估算到目標(biāo)點的路徑長度[6]。此時OPEN中擴展節(jié)點的優(yōu)先權(quán)的計算公式如下：

（1）

式中：膨脹因子要滿足條件

將agents標(biāo)記在不同三角形網(wǎng)格中，通過計算當(dāng)前三角網(wǎng)格的密度信息決定不同節(jié)點在OPEN表中的優(yōu)先權(quán)，此時Key的值計算公式（2）如下所示：

（2）

通過不斷減小在搜索路徑過程中膨脹因子的值找到最優(yōu)路徑。

3 動態(tài)路徑規(guī)劃算法設(shè)計與實現(xiàn)

3.1 搜索節(jié)點數(shù)據(jù)結(jié)構(gòu)

在動態(tài)環(huán)境中，以DLCT三角網(wǎng)格[7]作為地圖的劃分方法，將地圖網(wǎng)格中的動態(tài)變化信息和移動個體動態(tài)搜索路徑方法相關(guān)聯(lián)，構(gòu)造如下數(shù)據(jù)結(jié)構(gòu)，在Map_Node中存儲動態(tài)網(wǎng)格中障礙物發(fā)生變化后節(jié)點的構(gòu)建網(wǎng)格信息；Agent_Node中記錄移動個體在尋路過程所擴展的三角網(wǎng)格邊。規(guī)劃路徑時根據(jù)當(dāng)前Open_Node節(jié)點中存放的信息計算最優(yōu)路徑。

3.2 動態(tài)搜索算法實現(xiàn)

在動態(tài)搜索算法中，當(dāng)動態(tài)環(huán)境發(fā)生變化時更新三角網(wǎng)格地圖信息，通過在OPEN表中擴展邊，更新變化的頂點信息與其鄰接節(jié)點，判斷當(dāng)前移動個體路徑是否受到影響，未受影響的個體按原路徑移動；由于網(wǎng)格變化需要重新尋路時，計算節(jié)點代價值如下所示：

（3）

通過計算新添加節(jié)點（添加障礙物）或位置變更節(jié)點（移動障礙物）的代價值得到當(dāng)前三角網(wǎng)格地圖的節(jié)點序列，在OPEN中節(jié)點進行擴展時根據(jù)代價值的排序重新規(guī)劃路徑，通過執(zhí)行RecomputeShortestPath（）得到從當(dāng)前位置為起始點到目標(biāo)點的路徑規(guī)劃方法[8]。例如，個體Agent移動到節(jié)點所連接的邊edge時，移動障礙物模塊至三角網(wǎng)格edge邊移動方向的前方，此時動態(tài)規(guī)劃域與個體移動域有交集，在此區(qū)域里由于頂點變化三角網(wǎng)格地圖重新劃分，若劃分后節(jié)點不存在，通過不斷執(zhí)行Pred（s）找到前驅(qū)節(jié)點至此節(jié)點在更新后的網(wǎng)格地圖中存在，同理，后繼節(jié)點通過執(zhí)行Succ（s）不斷向后查找直至找到的后繼節(jié)點與新生成的網(wǎng)格地圖中匹配的節(jié)點（未找到則重新規(guī)劃此區(qū)域的路徑），在前驅(qū)節(jié)點和后繼節(jié)點之間的區(qū)域D中根據(jù)式（3）計算得到網(wǎng)格地圖變化后Agent個體從當(dāng)前位置到目標(biāo)點移動的規(guī)劃路徑。

4 基于動態(tài)環(huán)境的動態(tài)規(guī)劃仿真與實現(xiàn)

4.1 地圖規(guī)模變化策略

為了驗證動態(tài)搜索算法在不同規(guī)模三角網(wǎng)格地圖上的適用性和模擬仿真場景中復(fù)雜逼真程度，同時證明DLCT網(wǎng)格劃分方法對不同地圖的劃分結(jié)果的差異。本實驗應(yīng)用動態(tài)搜索算法AD*實現(xiàn)在不同規(guī)模地圖上個體規(guī)劃路徑過程。如圖3所示，圖中地圖網(wǎng)格的復(fù)雜程度由簡到繁分別采用5×5，10×10，20×20，40×40的網(wǎng)格拓撲結(jié)構(gòu)，通過不斷增大迷宮場景圖的復(fù)雜程度，在地圖中選擇起始點和目標(biāo)點后通過啟發(fā)式動態(tài)路徑搜索算法，AD*在不斷變化的三角網(wǎng)格中進行路徑規(guī)劃，最終找到當(dāng)前狀態(tài)下的最優(yōu)路徑。

通過采用不同規(guī)模地圖對單一移動個體規(guī)劃路徑，不同的搜索算法尋路過程與地圖的復(fù)雜程度正相關(guān)，隨著地圖復(fù)雜程度的增加，搜索路徑的過程在地圖網(wǎng)格中與擴展的三角邊總數(shù)呈正相關(guān)的趨勢增長。因此，在復(fù)雜多變的環(huán)境中模擬真實場景時，采用高效的地圖網(wǎng)格剖分方法能夠使路徑規(guī)劃效率得到提高。

4.2 群體移動策略

4.2.1 碰撞檢測方法

采用的規(guī)避方法是隨機處理方式和球體處理方法相結(jié)合，在Agents碰撞發(fā)生后進行處理，設(shè)置兩個移動個體之間距離的最小值，若小于最小值時則認為碰撞發(fā)生。在碰撞發(fā)生后，若個體所在三角網(wǎng)格密度信息較大，則重新規(guī)劃路徑進行移動直至目標(biāo)點；若個體所在三角網(wǎng)格在密度范圍內(nèi)則個體延時等待一段時間后，通過避讓的方法繞過障礙物繼續(xù)前行到達目標(biāo)點。

4.2.2 個體規(guī)避策略方法實現(xiàn)

在動態(tài)三角網(wǎng)格地圖環(huán)境中，當(dāng)障礙物發(fā)生移動時，移動個體會根據(jù)當(dāng)前規(guī)劃路徑是否受到影響決定移動策略，根據(jù)對方向進行判斷，若在移動過程中碰撞發(fā)生，記錄當(dāng)前位置信息CurPosition，判斷當(dāng)前三角網(wǎng)格密度值density，在密度滿足閾值條件時應(yīng)用規(guī)避方法重新規(guī)劃路徑繼續(xù)移動；其他情況下，需改變移動方向，密度滿足要求時進行重新規(guī)劃。

模擬仿真過程截圖如圖4～圖7所示，在圖4和圖5中，障礙物的移動沒有對個體移動的路徑產(chǎn)生影響，仍按照原規(guī)劃路徑前行；在圖6中，當(dāng)動態(tài)地圖網(wǎng)格中障礙物變化導(dǎo)致網(wǎng)格布局發(fā)生改變，從而對個體原路徑產(chǎn)生影響時，個體重新規(guī)劃路徑；在圖7中，當(dāng)?shù)貓D網(wǎng)格發(fā)生動態(tài)改變時，若同時存在的若干移動個體發(fā)生碰撞，此時應(yīng)用個體移動的規(guī)避策略對路徑進行重新規(guī)劃。

4.3 群體動態(tài)路徑規(guī)劃方法

4.3.1 群體動態(tài)路徑方法

群體路徑規(guī)劃方法是在單獨個體基礎(chǔ)上對每個移動物體進行路徑規(guī)劃的過程。為了模擬大規(guī)模人群仿真技術(shù)提出群行為模擬方法，移動個體對不同的Agent的影響有很大差異，主要問題有如下幾個方面：

（1） 如在此移動個體之前有另一個體相向而行，則其中一個物體應(yīng)該改變速度方向避免碰撞發(fā)生。

（2） 若兩個移動個體同向時，其中較慢的個體應(yīng)在人群密度較小的情況下改變速度方向超過前方遮擋個體；而在人群密度較大時，跟在前方物體之后行走。

綜合以上問題描述，本文通過碰撞類型分類對優(yōu)先級進行劃分。按照優(yōu)先級原則對仿真環(huán)境中的Agents個體進行優(yōu)先級排序（模擬真實場景中的出場順序或根據(jù)重要程度排序等），根據(jù)優(yōu)先級遞減劃分成不同的組別，對于中的每個Agent個體，保存其start，goal，position，DesSpeed，velDir變量信息。

在規(guī)劃群體路徑過程中，根據(jù)Agents的優(yōu)先級大小對速度方向進行修改，若在中Agents和中的移動個體移動方向相同則需要在判斷優(yōu)先級的條件下重新規(guī)劃其余組別的路徑方向。在采用以上方法避免碰撞發(fā)生的條件下，若碰撞發(fā)生，記錄當(dāng)前碰撞發(fā)生時移動個體的位置信息position，計算當(dāng)前三角網(wǎng)格的密度density和相鄰節(jié)點構(gòu)造網(wǎng)格密度值，根據(jù)Agents的優(yōu)先級以及已得到的密度信息進行重新排序，若此時的移動個體滿足優(yōu)先級條件，則重新計算地圖網(wǎng)格的代價值并重新規(guī)劃由當(dāng)前位置position到目標(biāo)點的路徑；若此時碰撞個體優(yōu)先級較低，則處于等待狀態(tài)至下一次重新規(guī)劃。

4.3.2 群體規(guī)劃路徑實現(xiàn)

為了驗證本文的群體動態(tài)路徑規(guī)劃方法，對在地圖網(wǎng)格中可能發(fā)生碰撞的Agents個體采用碰撞規(guī)避策略對每個Agents個體進行路徑規(guī)劃，在仿真的過程中不斷增加Agents規(guī)模，分別對Agents=50，Agents=100，Agents=150，Agents=200在不同時刻的尋路情況仿真模擬，實現(xiàn)結(jié)果截圖如圖8所示，圖中不同顏色的小方格分別代表不同的Agents移動個體，每個移動個體從初始點出發(fā)到目標(biāo)點規(guī)劃出各自的路徑，當(dāng)有Agents個體之間發(fā)生相互碰撞時（此時原規(guī)劃路徑移動方向被阻擋），則Agent個體根據(jù)本文中采用的碰撞規(guī)避原則，采用AD*算法繞過對方后重新規(guī)劃路徑。

圖8 群體規(guī)劃路徑實現(xiàn)

由仿真效果可知，隨著網(wǎng)格地圖中Agents個體數(shù)的不斷增加，發(fā)生碰撞的幾率明顯增大，因此在動態(tài)環(huán)境中，在采用高效的動態(tài)路徑搜索算法的同時，需要針對群體中發(fā)生碰撞的規(guī)避策略來避免碰撞發(fā)生。

4.4 仿真實驗結(jié)果分析

通過對動態(tài)環(huán)境中的動態(tài)網(wǎng)格效果、動態(tài)路徑規(guī)劃算法進行驗證，將二者結(jié)合實現(xiàn)本文關(guān)于動態(tài)環(huán)境中的路徑規(guī)劃方法的研究。在動態(tài)網(wǎng)格地圖中分別移動和增添障礙物后，實現(xiàn)了地圖網(wǎng)格發(fā)生變化情況的模擬。在動態(tài)搜索算法中，通過將D*Lite和ARA*算法結(jié)合實現(xiàn)了動態(tài)路徑規(guī)劃方法的優(yōu)化AD*算法，通過將二者結(jié)合，定義新的數(shù)據(jù)結(jié)構(gòu)存儲移動個體移動過程中的變量值，對群體路徑規(guī)劃和碰撞規(guī)避策略的應(yīng)用計算達到了動態(tài)環(huán)境路徑規(guī)劃的模擬仿真效果。

5 結(jié) 論

本文的主要研究內(nèi)容包括基于動態(tài)環(huán)境的網(wǎng)格地圖方法和動態(tài)路徑規(guī)劃方法的研究，在此基礎(chǔ)上實現(xiàn)動態(tài)環(huán)境中的路徑規(guī)劃。在動態(tài)路徑規(guī)劃的過程中需要對網(wǎng)格構(gòu)建和搜索算法進行研究，對地圖網(wǎng)格劃分方法在LCT靜態(tài)三角網(wǎng)格的基礎(chǔ)上，通過障礙物的移動、添加和刪除實現(xiàn)了動態(tài)網(wǎng)格規(guī)劃方法（DLCT）；在路徑搜索算法方面，將基于A*算法實現(xiàn)的ARA*算法和動態(tài)搜索算法D*Lite相結(jié)合，實現(xiàn)了在動態(tài)環(huán)境中的路徑規(guī)劃算法AD*。

參考文獻

[1] 孫俊，戴國駿，張懷相.裁剪優(yōu)化的Anytime算法[J].杭州電子科技大學(xué)學(xué)報，2010，30（2）：41?44.

[2] 張勝.一種基于狀態(tài)空間的啟發(fā)式搜索算法及其實現(xiàn)[J].現(xiàn)代電子技術(shù)，2008，31（16）：79?80.

[3] 孫蘭會，成鋒，陸愈實.基于GIS的路徑規(guī)劃算法研究與實現(xiàn)[J].現(xiàn)代電子技術(shù)，2016，39（5）：101?104.

[4] 思磊.距離網(wǎng)格地圖動態(tài)更新及基于距離網(wǎng)格地圖路徑規(guī)劃的研究[D].西安：西安電子科技大學(xué)，2013.

[5] 朱大奇，顏明重.移動機器人路徑規(guī)劃技術(shù)綜述[J].控制與決策，2010，25（7）：961?967.

[6] 席裕庚，張純剛.一類動態(tài)不確定環(huán)境下機器人的滾動路徑規(guī)劃[J].自動化學(xué)報，2002，28（2）：161?175.

[7] 邵峰，黃賢武.嵌入式系統(tǒng)中電子地圖的路徑尋優(yōu)[J].現(xiàn)代電子技術(shù)，2006，29（12）：51?52.

[8] 張娜，鄭駿.基于三角網(wǎng)格請求集的動態(tài)位置管理算法[J].計算機工程，2007，33（22）：145?147.