融合人工勢場法的動態快速行進樹路徑規劃算法

摘 要：針對快速行進樹算法（FMT*）由于隨機采樣導致的冗余探索問題以及不適用于動態環境的問題，提出一種融合人工勢場法的動態快速行進樹路徑規劃算法（APF-Dynamic FMT*），該算法設計了一種基于人工勢場法的采樣點引導函數，該函數根據環境信息動態的調整采樣點生成范圍，減少冗余探索。同時，該算法設計了一種路徑樹動態調整機制，當現有路徑受到環境改變的影響時，能在未受影響的剩余路徑樹的基礎上重新規劃出新的優秀路徑，適用于解決動態環境下的路徑規劃問題。仿真實驗結果表明，APF-Dynamic FMT*算法在消耗相同計算資源的同時，顯著提高了路徑規劃的成功率與路徑質量，且當現有路徑受動態環境影響后，能夠高效地重新規劃出可通行的優秀路徑。

關鍵詞：FMT*算法；人工勢場法；動態路徑規劃

中圖分類號：TP391 文獻標志碼：A 文章編號：1001-3695（2024）09-025-2745-06

doi：10.19734/j.issn.1001-3695.2024.01.0004

Dynamic fast marching tree algorithm with integrated artificial potential fields

Wu Xupeng1，2，Jia Xiaolin1，2，Gu Yajun1，2

（1.RFID & IOT Laboratory，School of Computer Science & Technology，Southwest University of Science & Technology，Mianyang Sichuan 621010，China;2.Mobile Internet of Things & Radio Frequency Identification Technology Key Laboratory of Mianyang，Mianyang Sichuan 621010，China）

Abstract：Addressing the issues of redundant exploration and inapplicability in dynamic environments inherent in fast mar-ching tree algorithm（FMT*），this paper proposed the APF-Dynamic FMT* algorithm，which integrated artificial potential field method.This algorithm designed a sampling point guidance function based on artificial potential field method，which could dynamically adjust the sampling point generation range according to the environment information to reduce redundant exploration.Additionally，this algorithm designed a dynamic path tree adjustment mechanism，when the existing path was affected by the environment change，it could re-plan a new excellent path on the basis of the remaining path tree that is not affected，which was suitable for solving the path planning problem in dynamic environment.The results verify that the APF-Dynamic FMT* algorithm can significantly improve the success rate and path quality of path planning while consuming the same computational resources，and when the existing path is affected by the dynamic environment，it can efficiently re-plan the passable excellent path.

Key words：FMT* algorithm;artificial potential field method;dynamic path planning

0 引言

路徑規劃在機器人導航、自動駕駛汽車以及眾多自動化系統中扮演著至關重要的角色。它涉及在環境中尋找一條從起始點到目標點的有效路徑，同時考慮優化路徑的長度、安全性和可行性［1，2］。

概率路線圖快速探索隨機樹（rapidly exploring random trees，RRT）與快速行進樹算法（fast marching tree，FMT*）都是基于采樣的路徑規劃算法［3，4］。RRT算法雖然路徑規劃效率較高，但已被證明無法規劃出最優的路徑［5］。在高維空間中，傳統路徑規劃算法往往因計算復雜度高而效率不佳，為了克服這一限制，Janson等人［6］于2013年提出了FMT*算法。FMT*算法是一種基于采樣的漸近最優算法。該算法通過有效地利用采樣點來構建一棵覆蓋搜索空間的樹，從而高效地找到從起點到終點的路徑。FMT*算法在多種應用場景下展現了出色的性能，但在特定條件下，如復雜地形或動態環境中，它仍然面臨一些挑戰［7～9］。

針對FMT*算法的研究主要集中在如何提高其在復雜環境中的適應性和效率上。為了提高FMT*算法的路徑規劃效率，Starek等人［10］提出了一種雙向拓展的FMT*算法（bidirectional fast marching Tree，BFMT*），縮短了搜索時間，但增加了計算資源消耗；Gao等人［11］將BFMT*與A*算法結合，形成了一種雙向搜索的啟發式路徑規劃算法（heuristic bidirectional fast marching tree，HBFMT*），提高了探索效率，但傳統啟發函數使用的樣本信息不夠充分，有時會影響計算效率。Wu等人［12］提出APF-IRRT*算法（artificial potential field-informed RRT*），通過引入人工勢場方法來提高移動機器人路徑規劃的效率和質量，但在動態環境中，一旦環境變化影響了現有路徑，需要從頭開始重新規劃路徑，效率較低。

針對FMT*算法在采樣點數量較少時路徑規劃成功率較低、路徑不夠優秀且無法應對動態環境的問題，提出一種融合人工勢場法的動態快速行進樹路徑規劃算法（dynamic fast marching tree algorithm with integrated artificial potential field，APF-Dynamic FMT*）。APF-Dynamic FMT*與APF-IRRT*算法在利用人工勢場法改善路徑規劃效率與質量方面有著共同的理念。然而，APF-Dynamic FMT*算法在此基礎上進一步針對動態環境的路徑規劃問題進行優化，展現出針對動態環境適應性的創新和進步。該算法結合了人工勢場法的環境適應性和FMT*算法的路徑規劃策略，顯著提高了在采樣點數量較少時的路徑規劃成功率與路徑質量，同時采用路徑樹動態調整機制，在動態環境中能高效地規劃出新路徑。通過仿真實驗驗證了APF-Dynamic FMT*算法的有效性以及在動態環境下的可用性。

1 問題描述

本文主要考慮二維路徑規劃問題，FMT*算法的核心理念是在一組預先生成的采樣點上實施動態遞歸探索，通過設定的連接半徑和鄰域篩選策略來構建出一棵高效的路徑樹。在FMT*算法中，采樣點是隨機生成并均勻分布于整個搜索空間的，如圖1所示，其中矩形代表障礙物，藍色點是起點，綠色點是目標點，紅色小點是采樣點（見電子版），這種均勻分布的采樣方法會導致較多的冗余探索，影響探索效率。

在FMT*算法的路徑規劃過程中，如圖2所示，路徑樹會均勻地向各個方向擴展。然而，由于某些采樣點距離較遠，它們無法有效地被整合到路徑樹中，這導致了計算資源的不必要浪費。

由于FMT*算法在規劃開始之前對整個搜索空間進行一次性采樣，并在這些采樣點上構建一棵樹。在動態環境中，障礙物的位置可能隨時間改變，這使得預先采樣的點可能變得不再有效或安全，因而需要重新采樣和構建路徑。并且FMT*算法沒有應對環境變化的響應機制，在動態環境下，一旦環境發生變化，需要從起點開始重新規劃，包括重新采樣和構建路徑樹。這一過程消耗了大量的計算資源與時間，特別是在環境頻繁發生變化的情況下，這會大大降低算法的實用性。

綜上所述，FMT*算法進行路徑規劃時，采樣點隨機生成導致的冗余探索，計算資源浪費等問題是導致FMT*算法在采樣點數量較少時路徑規劃成功率低、路徑不夠優秀的主要原因，且由于FMT*算法通常針對靜態環境設計，難以適應環境的實時變化［13］。在已有的環境基礎上，如果向環境中動態添加障礙物以阻擋現有路徑，由于FMT*算法無法對實時的環境變化作出應對，不適用于動態環境［14］。

2 APF-Dynamic FMT*算法

針對FMT算法在動態環境下的局限性，本文提出了APF-Dynamic FMT算法。該算法設計了一種基于人工勢場法的采樣點引導函數SampleAPF（n），不同于FMT*的隨機采樣策略，SampleAPF（n）能夠根據環境的障礙物占比信息動態調整采樣點的生成范圍。同時，APF-Dynamic FMT算法設計了一種路徑樹動態調整機制以實時監測環境變化（如障礙物的新增），若新的障礙物出現阻擋了現有路徑，能夠快速規劃出新的優秀路徑。

2.1 基于人工勢場法的采樣點生成函數

由于FMT*算法采用隨機生成采樣點的策略，導致大量冗余探索和計算資源的浪費。FMT*算法所生成的采樣點中，如圖3所示，圖中綠色橢圓虛線所圈住的范圍內的采樣點與FMT*算法路徑規劃的成功率、路徑質量高度相關，但位于綠色橢圓虛線圈外范圍的采樣點，對路徑規劃的影響程度低且數量占比較大，將其定義為冗余采樣點。

設定一組實驗，設定采樣點數量為100，在障礙物占比分別為0%、10%、20%、30%、40%、50%的地圖環境下，進行路徑規劃，統計冗余采樣點的占比數據。如表1所示，隨著環境障礙物占比的增加，冗余采樣點的占比由74%降至54%，雖有小幅降低，但仍有大量冗余采樣點，這導致了大量的冗余探索。

為解決由于采樣點隨機生成所導致的冗余探索問題，APF-Dynamic FMT*算法設計了一種基于人工勢場法的采樣點引導函數SampleAPF（n）以調整采樣點的生成范圍，該函數可以根據環境信息動態地調整采樣點生成范圍，適用于不同障礙物比例的情況，該函數返回一個具有q∈n（n為采樣點數量）個點的集合，具體實現細節如算法1所示，主要使用以下函數：

a）random（0，J（q））生成一個取值為（0，J（q））的隨機值，J（q）表示對采樣點q的限制勢力。

b）U（q）表示采樣點q的合力勢絕對值，用于判斷采樣點q是否滿足生成條件。

算法1 SampleAPF（n）

輸入：采樣點數量n。

輸出：一個有n個采樣點的集合Sample（n）。

1 N=n，m=0，Sample（n）←{}

2 for m≤N do

3 Generate a random sample point q

4 θ=random（0，J（q））

4 if U（q）≤θ then

5 Sample（n）←Sample（n）∪{q}

6 m+=1

7 end if

8 end for

9 return Sample（n）

SampleAPF（n）返回一個有n個采樣點的集合，首先根據目標位置和障礙物位置，構建一個勢能場，如圖4所示，勢能場將地圖范圍內的起點與目標視為引力極，障礙物視為斥力極。抽象定義引力極產生的引力勢為采樣點位置與引力極位置的距離相關函數；斥力極產生的斥力勢為采樣點位置與斥力極位置的距離相關函數。

假設地圖中新生成一個采樣點q，SampleAPF（n）將根據合力勢絕對值U（q）判斷該采樣點是否滿足生成條件，相關公式如式（1）所示。

4 仿真實驗分析

4.1 采樣點分布對比

設定實驗環境為30×50的矩形地圖如圖7所示，圖中灰色線條為地圖邊界，灰色矩形為障礙物，藍色點為起點，綠色點為目標點，紅色小圓點為采樣點。相比于FMT*算法，在相同數量采樣點的情況下，APF-Dynamic FMT*算法所生成的采樣點大部分位于以起點與目標點連線為長軸的橢圓內，且越靠近起點與目標點的連線范圍內采樣點越密集，而FMT*算法所生成的采樣點則隨機分布于整個地圖范圍。

由于FMT*算法在空間中隨機生成采樣點，導致大部分與目標方向無關的冗余采樣點需要被探索，這占用了大量的計算資源。而APF-Dynamic FMT*算法生成的采樣點絕大部分分布在具有探索價值的范圍內，較少生成與目標方向無關的采樣點。如圖8所示，在相同數量采樣點的情況下，FMT*算法中有大量與目標點方向無關的采樣點被探索，且有部分采樣點一直未被探索，這導致部分計算資源浪費，影響了路徑規劃效率。而APF-Dynamic FMT*算法中大部分被探索的采樣點都具有一定的探索價值，并且APF-Dynamic FMT*算法進行路徑規劃的過程中未被探索到的采樣點數量遠少于FMT*算法，因此采用APF-Dynamic FMT*算法具有更高的采樣點利用率。

4.2 路徑規劃效果對比

以下是APF-Dynamic FMT*與FMT*算法在仿真實驗中的路徑規劃效果對比分析。如圖9～11所示，兩種算法在采樣點數量同為100、200、300的情況下均能成功規劃一條可行路徑，但是FMT*所規劃出的路徑長度過長，且路徑不平滑，冗余拐點較多。相比于FMT*算法，APF-Dynamic FMT*算法所規劃出的路徑長度明顯更短且路徑更加平滑，更加趨向于最優路徑。

由于APF-Dynamic FMT*算法與FMT*算法基于采樣點探索的特性，采樣點分布的有效性影響了路徑規劃的成功率。如圖12所示，當出現面積較大障礙物擋住采樣點之間的聯通路線時，由于采樣點分布不合理，可能會導致FMT*算法路徑規劃失敗，所以采樣點的分布會對路徑規劃的成功率有較大影響。

在環境一致的情況下，分別采用APF-Dynamic FMT*與FMT*、RRT、APF-IRRT*算法的路徑規劃成功率隨采樣點數量的變化曲線如圖13所示，可以得知在較少采樣點的情況下采用APF-Dynamic FMT*算法進行路徑規劃的成功率明顯高于其余算法，且APF-Dynamic FMT*算法的路徑規劃成功率能夠更快地收斂于95%以上的高成功率并且保持穩定，采用APF-Dynamic FMT*算法進行路徑規劃有著更高的成功率，解決了FMT*算法在較少采樣點數量時路徑規劃成功率低的問題。

由于APF-Dynamic FMT*、FMT*、RRT與APF-IRRT*算法基于采樣點探索的特性，在相同空間內，采樣點數量越多，所生成的路徑相對越短且越平滑，但相對而言所消耗的計算資源就越大，所以采樣點的數量設定并不是越多越好，而是需要隨著采樣點數量的增加更快地收斂于最優路徑。APF-Dynamic FMT*與FMT*算法和RRT算法路徑規劃所得的路徑長度隨采樣點數量的變化曲線，如圖14所示，隨著采樣點數量的遞增，FMT*算法規劃所得路徑長度起伏較大，受采樣點數量影響較大，在采樣點數量達到400以上才逐漸收斂于最短路徑長度且保持相對穩定；由于采樣點數量不足，RRT算法規劃所得路徑長度起伏較大，且未收斂于最優路徑；APF-IRRT*算法相較于RRT算法有不錯的提升，但收斂于最優路徑的速度較為緩慢；APF-Dynamic FMT*算法規劃所得路徑長度整體相對穩定且更快地收斂于最短路徑長度，受采樣點數量的影響較小，具有不錯的穩定性，相比于FMT*算法規劃所得路徑更為優秀，解決了FMT*算法在較少采樣點數量時規劃所得路徑不夠優秀的問題。

4.3 APF-Dynamic FMT*算法在動態環境下路徑規劃效果

APF-Dynamic FMT*算法具有路徑樹動態調整機制，環境發生變化時，算法會判斷當前路徑是否被障礙物所阻擋。若有障礙物阻礙了當前已規劃出的路徑時，路徑樹動態調整機制會實時調整當前的路徑樹，剔除掉被障礙物覆蓋的采樣點以及被障礙物阻擋的路徑樹，并在現存路徑樹的基礎上進行路徑規劃。如圖15所示，在采樣點數量設定為300的情況下，初次進行路徑規劃成功后，在地圖中連續三次添加圓形障礙物障礙物（如圖中灰色圓形）阻礙當前路徑后，APF-Dynamic FMT*算法均能在未重新采樣的前提下規劃出一條可通行路徑，證明了該算法在動態環境下的可用性。

為驗證APF-Dynamic FMT*算法在動態環境下重新規劃出新路徑的高效性，與文獻［15］提出的一種動態RRT路徑規劃算法（replanning algorithm for rapidly-exploring random trees，replanning-RRT）進行對比實驗。replanning-RRT算法能刪除RRT算法在動態環境中失效部分并保留其他部分，且在已生成的擴張樹的基礎上繼續生長，直到規劃出新的路徑。設定實驗環境為動態二維地圖環境，采樣點數量為500，在首次路徑規劃成功后，向地圖中逐次添加障礙物以阻擋現有路徑，記錄重新規劃出新路徑的平均時間。實驗結果如圖16所示，隨著添加障礙物次數的增加，replanning-RRT算法的平均路徑規劃時間增幅較大，且沒有達到穩定的趨勢，而APF-Dynamic FMT*算法的平均路徑規劃時間的增幅較小，且逐漸趨向穩定，證明了其在動態環境中的高效性。

5 結束語

為了解決FMT*算法在采樣點數量較少時路徑規劃效率低以及不適用于動態路徑規劃等問題，本文提出一種融合人工勢場法的動態快速行進樹路徑規劃算法（APF-Dynamic FMT*）。APF-Dynamic FMT*設計了一種基于人工勢場法的采樣點引導函數SampleAPF（n），該函數可以根據環境信息動態的調整采樣點生成范圍，適用于不同障礙物比例的情況，這減少了對空間的冗余探索，提高了采樣點的利用率。同時，APF-Dynamic FMT*算法設計了一種路徑樹動態調整機制，監測環境是否發生改變，當環境發生改變時，算法會判斷當前路徑是否被障礙物所阻擋。若有障礙物阻礙了當前已規劃出的路徑時，算法會實時調整當前的路徑樹，剔除掉被障礙物覆蓋的采樣點以及被障礙物阻擋的路徑樹，并在剩余路徑樹的基礎上進行路徑規劃。仿真實驗結果表明，APF-Dynamic FMT*算法在消耗相同計算資源的同時，顯著提高了路徑規劃的成功率與路徑質量。在動態環境中，能夠高效地再次規劃出可通行的優秀路徑。

參考文獻：

［1］劉澳霄，周永錄，劉宏杰.基于改進人工勢場法的醫療配送機器人路徑規劃［J］.計算機應用研究，2024，41（3）：842-847.（Liu Aoxiao，Zhou Yonglu，Liu Hongjie.Path planning of medical delivery robot based on improved artificial potential field method［J］.Application Research of Computers，2024，41（3）：842-847.

［2］張艷菊，吳俊，程錦倩，等.多搬運任務下考慮碰撞避免的AGV路徑規劃［J］.計算機應用研究，2024，41（5）：1462-1469.（Zhang Yanju，Wu Jun，Cheng Jinqian，et al.AGV path planning considering collision avoidance of multiple handling tasks［J］.Application Research of Computers，2024，41（5）：1462-1469.

［3］Chen Jiagui，Zhao Yun，Xu Xing.Improved RRT-connect based path planning algorithm for mobile robots［J］.IEEE Access，2021，9：145988-145999.

［4］司徒華杰，雷海波，莊春剛.動態環境下基于人工勢場引導的RRT路徑規劃算法［J］.計算機應用研究，2021，38（3）：714-717，724.（Situ Huajie，Lei Haibo，Zhuang Chungang.Artificial potential field based RRT algorithm for path planning in dynamic environment［J］.Application Research of Computers，2021，38（3）：714-717，724.）.

［5］Fareh R，Baziyad M，Rabie T，et al.Enhancing path quality of real-time path planning algorithms for mobile robots：a sequential linear paths approach［J］.IEEE Access，2020，8：167090-167104.

［6］Janson L，Pavone M.Fast marching trees：a fast marching sampling-based method for optimal motion planning in many dimensions-exten-ded version［EB/OL］.（2013-06-15）.https：//arxiv.org/abs/1306.3532.

［7］Fan Jiaming，Chen Xia，Liang Xiao.UAV trajectory planning based on bi-directional APF-RRT* algorithm with goal-biased［J］.Expert Systems with Applications，2023，213：119137.

［8］Li Qiongqiong，Xu Yiqi，Bu Shenqiang，et al.Smart vehicle path planning based on modified PRM algorithm［J］.Sensors，2022，22（17）：6581.

［9］Xu Jing，Song Kechen，Zhang Defu，et al.Informed anytime fast ma-rching tree for asymptotically optimal motion planning［J］.IEEE Trans on Industrial Electronics，2020，68（6）：5068-5077.

［10］Starek J A，Gomez J V，Schmerling E，et al.An asymptotically-optimal sampling-based algorithm for bi-directional motion planning［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2015：2072-2078.

［11］Gao Wenxiang，Tang Qing，Yao Jin，et al.Heuristic bidirectional fast marching tree for optimal motion planning［C］//Proc of the 3rd Asia-Pacific Conference on Intelligent Robot Systems.Piscataway，NJ：IEEE Press，2018：71-77.

［12］Wu Daohua，Wei Lisheng，Wang Guanling，et al.APF-IRRT*：an improved informed rapidly-exploring random trees-star algorithm by introducing artificial potential field method for mobile robot path planning［J］.Applied Sciences，2022，12（21）：10905-10905.

［13］Janson L，Schmerling E，Clark A，et al.Fast marching tree：a fast marching sampling-based method for optimal motion planning in many dimensions［J］.The International journal of robotics research，2015，34（7）：883-921.

［14］Wang Kuan，Xu Jing，Song Kechen，et al.Informed anytime bi-directional fast marching tree for optimal motion planning in complex cluttered environments［J］.Expert Systems with Applications，2023，215：119263.

［15］Ferguson D，Kalra N，Stentz A.Replanning with RRTs［C］//Proc of IEEE International Conference on Robotics and Automation.Pisc-ataway，NJ：IEEE Press，2006：1243-1248.

收稿日期：2024-01-06

修回日期：2024-03-08

基金項目：國家自然科學基金面上項目（61471306）；四川省自然科學基金資助項目（2022NSFSC0548）；四川省重點研發計劃資助項目（2020YFS0360）；四川省教育廳教改資助項目（JG2021-1414）

作者簡介：吳旭鵬（1999—），男，四川眉山人，碩士，CCF會員，主要研究方向為移動物聯網技術、移動機器人導航技術；賈小林（1975—），男（通信作者），四川南江人，教授，博導，博士，CCF高級會員，主要研究方向為射頻識別（RFID）技術、物聯網（IoT）技術、計算機應用系統（my_jiaxl@163.com）；顧婭軍（1979—），女，四川綿陽人，講師，碩士，主要研究方向為計算機應用技術、物聯網（IoT）技術.