基于動態三角網格和啟發式搜索算法路徑規劃研究

王文霞

摘 要： 在靜態三角網絡的基礎上設計實現了動態三角網絡地圖算法，通過改變三角網格地圖障礙物變化過程中的網格節點代價值，設計相關算法模塊并對新生成的網格地圖實時獲取更新信息，實現動態變化效果。結合搜索算法Anytime Replanning A*（ARA*）和D* Lite，實現在動態環境中的路徑規劃方法Anytime Dynamic A*（AD*），通過對網格地圖中已擴展信息的保存和在有限時間內不斷縮小膨脹因子，找到網格地圖中代價值最小的節點，動態規劃得到最優路徑。在動態環境中，通過模擬動態場景圖，并應用動態規劃方法建立相應的數據搜索結構和方法，對仿真效果進行分析和對比。

關鍵詞： 動態三角網格； AD*算法； 路徑規劃； 模擬仿真

中圖分類號： TN911.1?34； TM417 文獻標識碼： 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

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

1 動態環境地圖網格構建及生成

1.1 動態三角網格設計實現

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

1.1.1 障礙物插入模塊

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

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

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

圖1 尋找被阻斷過程

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

1.1.2 障礙物刪除模塊

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

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

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

（3） 把List V1中相關的頂點信息從CDT中刪除；

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

1.1.3 動態三角網格構建過程

基于動態三角網格地圖的動態環境構建步驟如下：

（1） 獲取三角網格發生變化后的地圖信息。

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

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

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

（5） 更新地圖中三角網格節點信息。

1.2 動態地圖構建實現

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

圖2 動態地圖實現效果

2 啟發式搜索動態路徑規劃方法

2.1 啟發式動態路徑規劃方法（AD*）

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

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

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

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

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

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

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

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

2.2 地圖網格密度計算

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

（1）

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

將agents標記在不同三角形網格中，通過計算當前三角網格的密度信息決定不同節點在OPEN表中的優先權，此時Key的值計算公式（2）如下所示：

（2）

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

3 動態路徑規劃算法設計與實現

3.1 搜索節點數據結構

在動態環境中，以DLCT三角網格[7]作為地圖的劃分方法，將地圖網格中的動態變化信息和移動個體動態搜索路徑方法相關聯，構造如下數據結構，在Map_Node中存儲動態網格中障礙物發生變化后節點的構建網格信息；Agent_Node中記錄移動個體在尋路過程所擴展的三角網格邊。規劃路徑時根據當前Open_Node節點中存放的信息計算最優路徑。

3.2 動態搜索算法實現

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

（3）

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

4 基于動態環境的動態規劃仿真與實現

4.1 地圖規模變化策略

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

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

4.2 群體移動策略

4.2.1 碰撞檢測方法

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

4.2.2 個體規避策略方法實現

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

模擬仿真過程截圖如圖4～圖7所示，在圖4和圖5中，障礙物的移動沒有對個體移動的路徑產生影響，仍按照原規劃路徑前行；在圖6中，當動態地圖網格中障礙物變化導致網格布局發生改變，從而對個體原路徑產生影響時，個體重新規劃路徑；在圖7中，當地圖網格發生動態改變時，若同時存在的若干移動個體發生碰撞，此時應用個體移動的規避策略對路徑進行重新規劃。

4.3 群體動態路徑規劃方法

4.3.1 群體動態路徑方法

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

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

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

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

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

4.3.2 群體規劃路徑實現

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

圖8 群體規劃路徑實現

由仿真效果可知，隨著網格地圖中Agents個體數的不斷增加，發生碰撞的幾率明顯增大，因此在動態環境中，在采用高效的動態路徑搜索算法的同時，需要針對群體中發生碰撞的規避策略來避免碰撞發生。

4.4 仿真實驗結果分析

通過對動態環境中的動態網格效果、動態路徑規劃算法進行驗證，將二者結合實現本文關于動態環境中的路徑規劃方法的研究。在動態網格地圖中分別移動和增添障礙物后，實現了地圖網格發生變化情況的模擬。在動態搜索算法中，通過將D*Lite和ARA*算法結合實現了動態路徑規劃方法的優化AD*算法，通過將二者結合，定義新的數據結構存儲移動個體移動過程中的變量值，對群體路徑規劃和碰撞規避策略的應用計算達到了動態環境路徑規劃的模擬仿真效果。

5 結 論

本文的主要研究內容包括基于動態環境的網格地圖方法和動態路徑規劃方法的研究，在此基礎上實現動態環境中的路徑規劃。在動態路徑規劃的過程中需要對網格構建和搜索算法進行研究，對地圖網格劃分方法在LCT靜態三角網格的基礎上，通過障礙物的移動、添加和刪除實現了動態網格規劃方法（DLCT）；在路徑搜索算法方面，將基于A*算法實現的ARA*算法和動態搜索算法D*Lite相結合，實現了在動態環境中的路徑規劃算法AD*。

參考文獻

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

[2] 張勝.一種基于狀態空間的啟發式搜索算法及其實現[J].現代電子技術，2008，31（16）：79?80.

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

[4] 思磊.距離網格地圖動態更新及基于距離網格地圖路徑規劃的研究[D].西安：西安電子科技大學，2013.

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

[6] 席裕庚，張純剛.一類動態不確定環境下機器人的滾動路徑規劃[J].自動化學報，2002，28（2）：161?175.

[7] 邵峰，黃賢武.嵌入式系統中電子地圖的路徑尋優[J].現代電子技術，2006，29（12）：51?52.

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