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

1、 基于動態(tài)三角網(wǎng)格和啟發(fā)式搜索算法路徑規(guī)劃研究 王文霞摘 要： 在靜態(tài)三角網(wǎng)絡的基礎上設計實現(xiàn)了動態(tài)三角網(wǎng)絡地圖算法，通過改變?nèi)蔷W(wǎng)格地圖障礙物變化過程中的網(wǎng)格節(jié)點代價值，設計相關算法模塊并對新生成的網(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)場景圖，并應用動態(tài)規(guī)劃方法建立相應的數(shù)據(jù)搜索結(jié)構(gòu)和

2、方法，對仿真效果進行分析和對比。Key： 動態(tài)三角網(wǎng)格； AD*算法； 路徑規(guī)劃； 模擬仿真： TN911.1?34； TM417 ： A ： 1004?373X（2017）11?0103?04Research on path planning based on dynamic triangular meshand heuristic search algorithmWANG Wenxia（Department of Computer Science and Technology， Yuncheng University， Yuncheng 044000， China）Abstract： Th

3、e 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 ma

4、p 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 finit

5、e 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 t

6、he 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)搜索算法，實時更新地圖信息的同時采用更加高效的搜索算法記錄當前群體信息并反饋出動態(tài)環(huán)境中的環(huán)境變化情況，如人群規(guī)模、路徑寬度等相關信息，以便在較短時間內(nèi)避免碰撞和到達目的地。1 動態(tài)環(huán)境地圖網(wǎng)格構(gòu)建及生成1.1 動態(tài)三角網(wǎng)格設

7、計實現(xiàn)在構(gòu)建動態(tài)網(wǎng)格（Dynamic Local Clearance Triangulation，DLCT）2的過程中需要保持原有的LCT三角網(wǎng)格3中對路徑寬度的要求，在動態(tài)化的過程中添加和刪除障礙物時通過多邊形障礙物ID進行操作，在添加算法中把所有障礙物的限制條件遍歷直至找到需要添加障礙物的ID；在之后的刪除算法中會把與找到的障礙物ID相關的限制條件同時刪除。由于在地圖中障礙物之間可以相互覆蓋，因此每個ID可能關聯(lián)相互覆蓋的限制條件。1.1.1 障礙物插入模塊該模塊在原有LCT（S）的存儲單元中插入新的多邊形障礙物，在插入過程中，給障礙物設置新的ID。S設置為現(xiàn)在LCT（S）已存在的限制規(guī)劃

8、，O是將插入的新多邊形障礙物。插入過程如下：（1） 將多邊形障礙物O中的k條線段插入CDT中，對于需要修改的頂點和限制邊界分別存儲在兩個List中。List V中存儲所有已修改邊的鄰接頂點（其中包括因CDT交換的邊）；List C中存儲在插入障礙物過程中受限制的邊界。（2） 對于List C中的限制邊界，通過搜索算法判斷S是否被阻塞。對于每個被阻塞的遍歷找到后，相關聯(lián)頂點加入List V。如圖1所示，通過搜索加入頂點兩邊可能因S引起阻隔。對每次遍歷來說，相關聯(lián)的阻隔頂點v被加入到List V中。圖1 尋找被阻斷過程（3） 在遍歷過程所有被List V中頂點影響的頂點將重新測試其屬性，判斷在當前

9、情況是否需要被限制， Local_Ref函數(shù)定義和遍歷了在變化過程中與V相鄰的頂點。對于每一個所有與相鄰的三角形都要被訪問。設為當前與臨近被訪問的三角形，此時需要調(diào)用函數(shù)檢測是否有遍歷過的節(jié)點需要重新定義；函數(shù)查看的后繼節(jié)點是否被影響，當找到被影響的路徑時，頂點和若不在List V中則系統(tǒng)自動進行添加，然后重新測試所有在V中的頂點。1.1.2 障礙物刪除模塊該模塊將把現(xiàn)有地圖中的多邊形障礙物移除，移除過程中通過在插入中設置的ID找到該障礙物。由于障礙物之間可以發(fā)生覆蓋，故對于每一個障礙物來說可能同時具有多個ID。刪除過程如下：（1） 將已存儲的障礙物O從CDT中刪除，并把所有已修改與邊相關的頂

10、點信息存儲在List V1中；（2） 將與List V1被限制頂點的周圍臨近頂點找到后存入List V2；（3） 把List V1中相關的頂點信息從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） 反復執(zhí)行步驟（3），直至所有的

11、更新信息都處理完畢。（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*

12、中膨脹因子不斷遞減的過程進行一系列路徑搜索。當三角網(wǎng)格地圖環(huán)境發(fā)生改變并影響到邊的代價值時，在D*算法的OPEN表中對應節(jié)點的Key值發(fā)生相應改變。AD*算法具體執(zhí)行過程如下：（1） 程序初始化。將OPEN，CLOSED和INCONS表清空，膨脹因子初始值為（2） 計算最短路徑。計算最短路徑的過程中節(jié)點的擴展順序為從目標點goal到起始點start進行擴展，尋找出從源點到目標點的最短路徑。（3） 個體從start開始沿路徑向goal前進。（4） 在擴展過程中，將已擴展過的節(jié)點存儲在INCONS表中。（5） 判斷前進過程中網(wǎng)格代價值是否發(fā)生變化，如果發(fā)生變化，則以當前節(jié)點為新的起始點，并減小膨脹

13、因子的值。（6） 在下一次擴展過程中，用作為啟發(fā)函數(shù)，在擴展OPEN表中剩余的點之前，把上一輪INCONS表中的點插入OPEN表中，在原來OPEN表的基礎上修改所有與變化節(jié)點相關的rhs，key的值，清空CLOSE表。（7） 以新的start點為起始點，goal為目標點，運行步驟（3）直至到達目標點。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é)點和

14、距離信息來估算到目標點的路徑長度6。此時OPEN中擴展節(jié)點的優(yōu)先權(quán)的計算公式如下：（1）式中：膨脹因子要滿足條件將agents標記在不同三角形網(wǎng)格中，通過計算當前三角網(wǎng)格的密度信息決定不同節(jié)點在OPEN表中的優(yōu)先權(quán)，此時Key的值計算公式（2）如下所示：（2）通過不斷減小在搜索路徑過程中膨脹因子的值找到最優(yōu)路徑。3 動態(tài)路徑規(guī)劃算法設計與實現(xiàn)3.1 搜索節(jié)點數(shù)據(jù)結(jié)構(gòu)在動態(tài)環(huán)境中，以DLCT三角網(wǎng)格7作為地圖的劃分方法，將地圖網(wǎng)格中的動態(tài)變化信息和移動個體動態(tài)搜索路徑方法相關聯(lián)，構(gòu)造如下數(shù)據(jù)結(jié)構(gòu)，在Map_Node中存儲動態(tài)網(wǎng)格中障礙物發(fā)生變化后節(jié)點的構(gòu)建網(wǎng)格信息；Agent_Node中記錄移動

15、個體在尋路過程所擴展的三角網(wǎng)格邊。規(guī)劃路徑時根據(jù)當前Open_Node節(jié)點中存放的信息計算最優(yōu)路徑。3.2 動態(tài)搜索算法實現(xiàn)在動態(tài)搜索算法中，當動態(tài)環(huán)境發(fā)生變化時更新三角網(wǎng)格地圖信息，通過在OPEN表中擴展邊，更新變化的頂點信息與其鄰接節(jié)點，判斷當前移動個體路徑是否受到影響，未受影響的個體按原路徑移動；由于網(wǎng)格變化需要重新尋路時，計算節(jié)點代價值如下所示：（3）通過計算新添加節(jié)點（添加障礙物）或位置變更節(jié)點（移動障礙物）的代價值得到當前三角網(wǎng)格地圖的節(jié)點序列，在OPEN中節(jié)點進行擴展時根據(jù)代價值的排序重新規(guī)劃路徑，通過執(zhí)行RecomputeShortestPath（）得到從當前位置為起始點到目標

16、點的路徑規(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個體從當前位置到目標點移動的規(guī)劃路徑。 4 基于動態(tài)環(huán)境的動態(tài)規(guī)劃仿真與實現(xiàn)4.1 地圖規(guī)模變化策略

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

18、網(wǎng)格中與擴展的三角邊總數(shù)呈正相關的趨勢增長。因此，在復雜多變的環(huán)境中模擬真實場景時，采用高效的地圖網(wǎng)格剖分方法能夠使路徑規(guī)劃效率得到提高。4.2 群體移動策略4.2.1 碰撞檢測方法采用的規(guī)避方法是隨機處理方式和球體處理方法相結(jié)合，在Agents碰撞發(fā)生后進行處理，設置兩個移動個體之間距離的最小值，若小于最小值時則認為碰撞發(fā)生。在碰撞發(fā)生后，若個體所在三角網(wǎng)格密度信息較大，則重新規(guī)劃路徑進行移動直至目標點；若個體所在三角網(wǎng)格在密度范圍內(nèi)則個體延時等待一段時間后，通過避讓的方法繞過障礙物繼續(xù)前行到達目標點。4.2.2 個體規(guī)避策略方法實現(xiàn)在動態(tài)三角網(wǎng)格地圖環(huán)境中，當障礙物發(fā)生移動時，移動個體會根

19、據(jù)當前規(guī)劃路徑是否受到影響決定移動策略，根據(jù)對方向進行判斷，若在移動過程中碰撞發(fā)生，記錄當前位置信息CurPosition，判斷當前三角網(wǎng)格密度值density，在密度滿足閾值條件時應用規(guī)避方法重新規(guī)劃路徑繼續(xù)移動；其他情況下，需改變移動方向，密度滿足要求時進行重新規(guī)劃。模擬仿真過程截圖如圖4圖7所示，在圖4和圖5中，障礙物的移動沒有對個體移動的路徑產(chǎn)生影響，仍按照原規(guī)劃路徑前行；在圖6中，當動態(tài)地圖網(wǎng)格中障礙物變化導致網(wǎng)格布局發(fā)生改變，從而對個體原路徑產(chǎn)生影響時，個體重新規(guī)劃路徑；在圖7中，當?shù)貓D網(wǎng)格發(fā)生動態(tài)改變時，若同時存在的若干移動個體發(fā)生碰撞，此時應用個體移動的規(guī)避策略對路徑進行重新規(guī)

20、劃。4.3 群體動態(tài)路徑規(guī)劃方法4.3.1 群體動態(tài)路徑方法群體路徑規(guī)劃方法是在單獨個體基礎上對每個移動物體進行路徑規(guī)劃的過程。為了模擬大規(guī)模人群仿真技術提出群行為模擬方法，移動個體對不同的Agent的影響有很大差異，主要問題有如下幾個方面：（1） 如在此移動個體之前有另一個體相向而行，則其中一個物體應該改變速度方向避免碰撞發(fā)生。（2） 若兩個移動個體同向時，其中較慢的個體應在人群密度較小的情況下改變速度方向超過前方遮擋個體；而在人群密度較大時，跟在前方物體之后行走。綜合以上問題描述，本文通過碰撞類型分類對優(yōu)先級進行劃分。按照優(yōu)先級原則對仿真環(huán)境中的Agents個體進行優(yōu)先級排序（模擬真實場景

21、中的出場順序或根據(jù)重要程度排序等），根據(jù)優(yōu)先級遞減劃分成不同的組別，對于中的每個Agent個體，保存其start，goal，position，DesSpeed，velDir變量信息。在規(guī)劃群體路徑過程中，根據(jù)Agents的優(yōu)先級大小對速度方向進行修改，若在中Agents和中的移動個體移動方向相同則需要在判斷優(yōu)先級的條件下重新規(guī)劃其余組別的路徑方向。在采用以上方法避免碰撞發(fā)生的條件下，若碰撞發(fā)生，記錄當前碰撞發(fā)生時移動個體的位置信息position，計算當前三角網(wǎng)格的密度density和相鄰節(jié)點構(gòu)造網(wǎng)格密度值，根據(jù)Agents的優(yōu)先級以及已得到的密度信息進行重新排序，若此時的移動個體滿足優(yōu)先級條

22、件，則重新計算地圖網(wǎng)格的代價值并重新規(guī)劃由當前位置position到目標點的路徑；若此時碰撞個體優(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ā)到目標點規(guī)劃出各自的路徑，當有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)本文關于動態(tài)環(huán)境中的路徑規(guī)劃方法的研究。在動態(tài)網(wǎng)格地圖中分別移動和增添障礙物后，實現(xiàn)了地圖網(wǎng)格發(fā)生變化情況的模擬。在動態(tài)