基于引力勢能修正的快速行進樹路徑規劃算法

關鍵詞：FMT*算法；人工勢場法；引力勢能修正；路徑規劃

中圖分類號：TP301.6 文獻標志碼：A 文章編號：1001-3695（2025）08-022-2416-05

doi：10.19734/j. issn.1001-3695.2024.11.0478

Fast marching tree algorithm based on gravitational potential energy correction

Xu Zhengyu1，2，Jia Xiaolin ，Gu Yajun1，2，Yuan Lei1，2 （1.Schoolofopueieceamp;hooustUiestfSieeamp;holngSun6haeI ternetofThingsamp;RadioFrequencyIdentficationTechnologyKeyLaboratoryofMianyang，MianyangSichuan61ooChina）

Abstract：Toaddress theisuesofredundant exploration，long planning time，andnumerous turning points inmobilerobot path planning usingtheFMT”algorithm，thispaperproposedaGPE-FMT*algorithm.This algorithmdrewfromtheconceptof artificialpotentialfieldsyintroducingfinitegaviationalpotentialeergyfieldtoostraiteexplorationange，ffectiely reducingredundantsearches.Aditionally，itemployedapath treecorrction strategyasedon gravitational potential energy values to guidethe growth directionofthe pathtreeduring itsexpansion，moving itclosertothetargetandthus reducing turning pointsandshortening planing time.Simulationresults demonstratethat，under equivalentcomputationalresources， GPE-FMT*significantlydecreasespath planningtime，reduces the numberof turning points，and lowers the numberof algorithm iterations，ultimately enhancing path quality.

Key Words：FMT*algorithm；artificial potential field method；gravitational potential energy corrction；path planning

0引言

隨著科技進步，移動機器人在各個領域得到了廣泛應用。然而，復雜和不確定的工作環境使得快速準確地找到初始位置到目標位置的無碰撞路徑，成為當前研究的一大技術挑戰。路徑規劃是移動機器人研究領域中不可或缺的關鍵技術，其主要任務是尋找從初始位置到目標位置的無碰撞可行路徑[1]。通常通過路徑規劃時間、路徑拐點數量和路徑長度等指標來評價路徑規劃算法的效率高低[2]

學者們提出了多種移動機器人路徑規劃算法，其中基于采樣策略的路徑規劃算法因其環境適應性強而廣受歡迎。快速探索隨機樹（rapidlyexploringrandom tree，RRT）算法[3]和概率路線圖算法（probabilistic roadmapmethod，PRM）[4]都是基于采樣的路徑規劃算法。兩種算法都可以通過較少的時間代價來獲取一條可行路徑，但是均被證明無法規劃出最優路徑[5]。為了克服這一問題，Janson等人[于2015年提出了FMT*算法，其借鑒了快速行進方法（fastmarchingmethod，FMM）的思想，是一種漸近最優算法。盡管FMT*算法表現優秀，但它仍然存在一些問題，例如隨著采樣點的增多，算法會出現大量的冗余探索，導致路徑規劃的時間過長，并且由于采樣點是隨機生成的，這也導致最終生成的路徑拐點數較多。

在實際應用中，往往需要算法快速規劃出高質量路徑，因此針對FMT*算法的研究主要集中在減少算法的規劃時間和路徑拐點數上。Starek等人[7]設計了一種雙向FMT*（bidirec-tionalFMT*，BFMT*）算法，該算法采用雙向搜索的思路，縮短了搜索路徑的時間，但同時需要消耗大量計算資源。Reid等人[8]提出的HBFMT*（hierarchical bidirectional fast marchingtree）算法是一種分層雙快速行進樹算法，該算法將配置空間進行分解，并且采用雙向搜索策略，減少了路徑規劃時間，但其性能依賴最初的采樣空間分布。Ichter等人[9利用GPU的性能來對FMT“算法進行拓展，但該方法過于依賴計算機性能。吳旭鵬等人[2]提出了APF-DynamicFMT*算法，該算法通過與人工勢場法融合，在采樣點生成初期對其進行位置調整，減少了生成路徑的拐點數，但隨著采樣點數目的增加，會出現規劃時間過長的問題。

綜上所述，盡管國內外學者對FMT*算法進行了許多改進，但其仍存在一些缺陷。針對FMT*系列算法在采樣點過多的情況下的冗余探索、路徑規劃時間過長、生成路徑拐點過多的問題，本文提出一種基于引力勢能修正的快速行進樹算法（GPE-FMT*）。該算法借鑒人工勢場法[\"]的思想，在地圖上生成一個有限的引力勢能場，將路徑樹的探索范圍限制在該引力勢能場范圍內，以此來避免冗余探索；同時采用基于引力勢能修正的方法，在路徑樹生成過程中對其生長方向進行修正，進一步減少了冗余探索，并且消除了大量拐點。

1問題描述

FMT*算法首先在地圖上隨機生成采樣點，通過采樣點進行路徑規劃，以距離長短為代價，通過“惰性”碰撞檢測的方式生成路徑樹，直到探索到目標點完成路徑規劃。模擬地圖與探索效果如圖1所示（其中矩形物體代表障礙物，藍色點代表起點，綠色點代表終點，紅色點為隨機生成的采樣點，紅色線段為生成的路徑，見電子版）。

圖1FMT*算法效果

如圖1所示， FMT^* 算法在路徑規劃中會根據隨機采樣點擴展路徑樹（圖中黃色線段），在此期間算法會在與生成路徑無關的范圍（圖中紫色圓形范圍外）進行大量探索，而這些探索對于最終路徑的生成并沒有幫助，因此將其定義為冗余探索。隨著采樣點數量的增加，FMT*算法的冗余探索也會隨之增加，這將導致算法路徑規劃時間過長，從而增加實際應用中的時間成本。從圖1中還可以看出，由于FMT*算法中的采樣點是隨機生成的，這導致生成的路徑往往存在較多的拐點。在實際應用中，過多的拐點會增加機器人運行過程中的風險以及能量消耗。因此本文提出了基于引力勢能修正的FMT*算法（GPE-FMT*），該算法通過有限的引力勢能場限制探索范圍，將其控制在與生成路徑相關的區域內，從而減少冗余探索和時間消耗；此外，采用基于引力勢能修正的策略，能有效減少由于隨機采樣點引起的拐點數量，提高路徑的整體質量。

2 GPE-FMT*算法

2.1 有限引力勢能場構建

為解決FMT*算法的冗余探索問題，減少算法路徑規劃的時間消耗，本文受人工勢場法啟發，在地圖中構建出一個圓形的有限引力勢能場。該圓形的有限引力勢能場以起點 （X_i，Y_i） （204號與目標點 （X_g，Y_g） 連線的中點 （X_c，Y_c） 為圓心，圓心計算公式如式（1）所示。

計算出起點 （X_i，Y_i） 與目標點 （X_g，Y_g） 的歐氏距離 d_f ，計算公式如式（2）所示。

圓形的有限引力場半徑 R 設為起點 （X_i，Y_i） 與目標點

（X_g，Y_g） 歐氏距離 d_f 的一半，加上膨脹系數 ，半徑的計算公式如式（3）所示。

當算法在該圓形的有限引力場中不能規劃出路徑時，膨脹系數 會增加，使得有限引力場的范圍增加，增加路徑探索的范圍。當有限引力場范圍覆蓋整個地圖仍不能規劃出可行路徑時，路徑規劃失敗。有限引力勢能場如圖2所示，其中紅色陰影表示目標點產生的引力勢能場（見電子版）。

圖2有限引力勢能場圖

Fig.2Finitegravitational potential energy field diagram

在該有限引力勢能場范圍內的采樣點會受到來自目標點的吸引力，產生一個引力勢能值，而在有限引力場范圍外的采樣點不受吸引力影響，不具備引力勢能值。算法只在具有引力勢能值的采樣點上進行路徑規劃，以此減少算法的冗余探索。其中引力勢能值的計算公式如式（4）所示。

其中： k_att 表示正吸引力常數； p² 表示采樣點到目標點的歐氏距離平方； p₀ 表示采樣點到引力勢能場圓心的歐氏距離。

加入有限的引力勢場限制算法的探索范圍后，算法仍然存在一定的冗余探索，并且生成的路徑拐點較多，如圖3所示。為進一步解決算法的冗余探索以及生成路徑拐點較多的問題，本文提出一種基于引力勢能修正的策略。

Fig.3Explorationdiagramofgravitationalpotential fieldlimitatio

2.2基于引力勢能的修正策略

FMT*算法規劃成功的條件是 V_close 中的當前拓展節點為目標點，當待拓展節點不為目標點時將持續探索。在進行路徑探索時，以待 V_open 中的拓展節點 Z 為圓心，搜索半徑為 r_n 內V_unvisited 中的未訪問節點 X ，如圖4（a）所示。搜索到未訪問節點后，以距離長度為代價，待訪問節點選擇與距離其最近的且無碰撞的待拓展節點作為其父節點，進行路徑樹連接，連接示意圖如圖4（b）所示。

由于采樣點是隨機生成的，這種探索與連接方式存在隨機性，這就導致 FMT^* 算法在引力場范圍內仍存在冗余探索，并且生成的路徑拐點數較多。為此本文提出一種基于引力勢能值的路徑樹修正策略。引力修正示意圖如圖5所示。

圖4FMT*算法節點拓展圖

圖5引力修正示意圖 Fig.5Gravitycorrection diagram

當未訪問節點 X 找到其父節點 Y_m 后，根據式（4）計算未訪問節點在當前位置受到的引力勢能值，并且計算出未訪問節點到其父節點間的歐氏距離 d_ym 。接著對未訪問節點進行引力修正操作。首先計算出未訪問節點的父節點到目標點的方向向量 并歸一化，計算公式如式（5）所示。

其中： X_goal 的目標點為 （X_g，Y_g）;Y_m 表示未訪問節點 X 的父節點。計算出方向向量后，以未訪問節點 X 到其父節點間的歐氏距離相同的距離 d_ym ，在該方向向量上生成一個新的節點X_new 坐標，其計算公式如式（6）所示。

X_new=Y_m+a?d_ym

隨后通過式（4）計算新生成坐標位置所受的引力勢能值，并與原未訪問節點所受引力勢能值進行比較。如果新生成坐標位置處采樣點所受引力勢能值更小，并且無碰撞，則將未訪問節點 X 移動到新坐標位置處，反之則不移動。

2.3 GPE-FMT*算法

在GPE-FMT*算法進行路徑規劃的過程中，算法會在有限的引力勢能中進行路徑探索，并且在搜索過程中使用引力修正策略，以達到減少見余探索和拐點數量的目的。

算法 GPE-FMT*

輸入：起點終點信息。

輸出：規劃出的路徑信息

1 USampleFree（n） ; E? （204號

//初始化起點及隨機采樣點

2 V_open{x_init} ， V_closed? （20號

3 zx_init ，safe_m0，safe-maxm

4 （204號

5 Save （N_z，z）

6 while do//搜索目標點

7 （20 V_open ， （20號

8 X_near=N_z∩V_unvisited （20

9 （204 X_near= Potential（ k，X_near ）//引入有限引力勢能場進行篩選

10 for x∈X_near do

11

12

[13

14 （204號

//搜索代價最小的采樣點

15 x← GravitationCorrection （y_min，x） //采用引力修正策略

16 if CollisionFree （y_min，x） then//碰撞檢測

17 EEU{（ymin，x）}

18 Vopen，new←Vopen，new U{x}

19 VunvisitedVunvisitedx

20 （204號

21 end if

22 end for

23

24

25 while do

26 sa 擴大有限引勢能力場范圍

27 if safe_maxthen//判斷是否達到膨脹限制

28 return Failure

29 end if

30 for z∈V_closed do

31 step 8～20

32 end for

33 end while

34 if CollisionFree（z，Xgoal）then

35 （204號 EE∪{∣（z，X_goal）}

36 break

37 endif

38 z-arg minyeVopen{c（y）}

39 endwhile

40 rerturn ）//規劃成功輸出路

GPE-FMT*算法節點拓展示意圖如圖6所示。

圖6GPE-FMT*算法節點拓展圖

Fig.6GPE-FMT*algorithm node expansion diagram

3算法分析

3.1 概率完備性

對于給定的路徑規劃地圖，若當前地圖中存在可行路徑，則當采樣點數量趨于無窮時，算法找到該可行路徑的概率為1[11]，即

GPE-FMT*算法首先在地圖上隨機生成采樣點，并以起點X_init 為待探索節點，開始路徑規劃。隨后在規劃過程中，從未訪問點集中搜索目標點 X_goal ，并將搜索過的采樣點加入路徑樹中。當給定地圖中存在可行路徑，并且算法搜索到目標點 X_goal 時，輸出路徑。因此，在地圖有可行路徑的情況下，當采樣點數量趨于無窮，并且能夠覆蓋整個地圖時，搜索到可行路徑的概率為1。GPE-FMT*算法采用圓形的有限引力勢能場限制路徑的搜索范圍，若在當前范圍下算法無法搜索到可行路徑，則會擴大引力勢能場的范圍，直到包含整個地圖，故GPE-FMT*算法具備概率完備性。

3.2 漸進最優性

設定 （x_free，x_init，x_goal） 是 d 維空間的路徑規劃問題，令 c^* 為最優路徑長度，隨機采樣點數目為 n 、搜索半徑長度為 r_n 時，GPE-FMT*算法的路徑長度為 c_n ，其中 r_n 可由式（8）計算[2]。

其中： η 為正常數; d 為空間維數 表示無障礙空間中的勒貝格測度（在二維空間中表示面積或在三維空間中表示體積）； ?，ζ_d 表示在 d 維歐幾里德空間中單位球的體積， ngt;0 。當采樣點數量 n 趨于無窮時，GPE-FMT*算法所規劃的路徑長度c_n 趨于最優路徑長度 c^* 的概率為 1^[12] ，即

lim_n∞P（c_n=c^?）=1

故GPE-FMT*算法具備漸進最優性。

3.3 算法復雜度

設定 （x_free，x_init，x_goal） 的路徑規劃問題，當隨機采樣點數量為 n 時， FMT^* 算法規劃出可行路徑需要 O（nlogn） 的時間，并且會占用 的空間[。而GPE-FMT*算法是根據FMT*算法建立的，并且GPE-FMT*算法遵從FMT*算法搜索采樣點的策略。因此當隨機采樣點數量為 n 時，GPE-FMT*算法需要 O（nlogn） 的時間來計算 n 個樣本，進行 O（n） 次碰撞檢測，調用 次代價函數。由大O法則可知，GPE-FMT^* 算法的計算復雜度為 O（nlogn） 。由于GPE-FMT*算法同時進行路徑搜索以及路徑樹的構建，所以GPE-FMT*算法時間復雜度和空間復雜度為 O（nlogn） 。

4仿真實驗分析

為驗證GPE-FMT*算法的性能，本文分別與FMT*、APF-dynamic-FMT*[2]和RRT*算法[13]進行仿真對比實驗。在仿真實驗中，GPE-FMT*算法中的膨脹系數 取值為1.0，膨脹時每次增加1.0，正吸引力常數 k_att 取值為1.0。本文參考文獻[2]中的仿真實驗地圖，將仿真實驗地圖設定為 30×50 的矩形地圖，圖7為采樣點個數設置為1000個時，4種算法路徑規劃效果對比圖，其中藍色點為起始點坐標（2，25），綠色點為目標點坐標（45，5），紅色點為隨機采樣點，黑色矩形為障礙物，黃色線段為探索過程中生成的路徑樹，紅色線段為最終生成的路徑（見電子版）。本文分別設置了5組實驗，每組實驗重復進行100次，取其平均值作為最終結果。

從圖7中可以看出，FMT*與RRT*算法在進行路徑規劃時，冗余探索較多導致規劃時間過長，并且FMT*算法中采樣點是隨機分布的，這也導致了最后生成的路徑拐點較多；而APF-dynamic-FMT*算法限制了隨機采樣點的生成位置，但使得規劃時間過長。

本文提出的GPE-FMT*算法，通過有限引力場限制算法路徑探索的范圍，并在探索過程中使用基于引力勢能的修正策略減少了冗余探索和路徑規劃的時間，并且生成的路徑拐點較少。規劃時間對比如圖8所示。

圖8平均規劃時間對比

Fig.8Comparison of average path planning time

圖9平均拐點數對比

Fig.9Comparison of average turning points

從圖8中可以看出，在5組不同數目的采樣點設置情況下，GPE-FMT*算法消耗的平均時間最少，并且隨著采樣點的增多，算法規劃時間變化穩定。當采樣點數目為2500個時，GPE-FMT*算法相比FMT*APF-dynamic-FMT*和RRT*算法，平均規劃時間分別下降了 51.02% （204號 60.7% 和 74.7% 。平均拐點數對比如圖9所示。

由于RRT*算法具有修正生成路徑的功能， RRT^* 算法生成的拐點數也較少，但相對應地會消耗大量時間。GPE-FMT*算法消耗時間較短，并且生成的路徑拐點數也較少，其中在2500個采樣點的情況下，GPE-FMT*相比于FMT*和APF-dynamic-FMT*算法，平均拐點數分別下降了 51.13% 和49.14% 。

路徑長度與算法迭代次數如表1所示。從表1可以看出，由于四種算法都具有漸近最優性，隨著采樣點的增加，四種算法規劃出的路徑長度都逐漸減少，并且路徑長度差距不大。FMT*與APF-dynamic-FMT*算法在探索過程中，探索的采樣點更多，因此路徑長度稍短，但冗余探索較多，算法的迭代次數較多導致了FMT*與APF-dynamic-FMT*算法規劃時間較長。由于RRT*算法的特性，在路徑規劃時會使用所有生成的采樣點，其迭代次數為采樣點的個數；并且，RRT*算法具有修正生成路徑的功能，所以RRT*算法生成的路徑較短，但相對的算法消耗的時間過長。GPE-FMT*算法只需進行少量采樣點的搜索就可規劃出質量較高的路徑，減少了算法的迭代次數。

表1路徑長度與迭代次數對比

Tab.1 Comparison of path length and iteration times

APF-dynamic-FMT*與GPE-FMT*是在 FMT^* 算法基礎上提出的改進算法，根據文獻[6]可知， FMT^* 算法的時間復雜度與空間復雜度均為 O（nlogn） ；根據文獻[2]中的分析可知，APF-dynamic-FMT*算法的時間復雜度與空間復雜度均為O（nlogn） ；根據文獻[13]中的分析可知， RRT^* 算法的時間復雜度為 O（nlogn） ，空間復雜度為 O（n） ；根據本文的分析可知，GPE-FMT*算法的時間復雜度與空間復雜度均為 O（nlogn） 。

綜上所述，GPE-FMT*算法能夠有效避免冗余探索，減少 算法路徑規劃時間和生成路徑的拐點數目，減少了算法迭代次 數，仿真實驗結果證明了GPE-FMT*算法的優越性和高效性。

5結束語

針對FMT*算法在路徑規劃過程中存在的冗余探索、路徑規劃時間過長、路徑拐點較多的問題，本文提出了一種基于引力勢能修正的FMT*算法。首先，通過建立圓形的有限引力勢能場，限制算法的搜索范圍避免算法的冗余探索并減少路徑規劃時間，若當前引力場無法規劃出路徑，則進行引力場范圍擴張；再通過基于引力勢能的修正策略，修正生成的路徑樹方向，使算法生成的路徑樹方向更靠近目標點，進一步減少冗余探索和路徑規劃時間，并且該策略有效減少了算法生成路徑的拐點數。

本文主要考慮二維環境中的路徑規劃問題，在未來的研究中會考慮三維的場景，以提高算法的實用性。

參考文獻：

[1]Al Beaty A H，Al Azzawi N，Almusawi AR. Survey on path planning ofmobilerobotwithmulti algorithms[J].American Academic Sciences，2022，90（1）：161-174.

[2]吳旭鵬，賈小林，顧婭軍．融合人工勢場法的動態快速行進樹路 徑規劃算法［J]．計算機應用研究，2024，41（9）：2745-2750. （WuXupeng，Jia Xiaolin，Gu Yajun.Dynamic fast marching tree algorithm with integrated artificial potential fields[J].Application Research ofComputers，2024，41（9）：2745-2750.）

[3]LaValle SM，Kuffner JJ.Randomized kinodynamic planning[J]. The International Journal of Robotics Research，2001，20（5）： 378-400.

[4]Kavraki L E，Svestka P，Latombe JC，et al.Probabilistic roadmaps forpath planning in high-dimensional configuration spaces[J]. IEEETransonRoboticsand Automation，1996，12（4）：566- 580.

[5］崔煒，朱發證．機器人導航的路徑規劃算法研究綜述［J]．計算 機工程與應用，2023，59（19）：10-20.（CuiWei，Zhu Fazheng. Review of path planning algorithms forrobot navigation[J].Computer Engineering and Applications，2023，59（19）：10-20.）

[6]JansonL，SchmerlingE，Clark A，et al.Fast marching tree：afast marching sampling-based method for optimal motion planning in many dimensions [J].The International Journal of Robotics Research，2015，34（7）：883-921.

[7]Starek JA，Gomez JV，Schmerling E，et al．An asymptoticallyoptimal sampling-based algorithm for bi-directional motion planning （204號 [C]/′ Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ：IEEE Press ，2O15：2072- 2078.

[8]ReidW，FitchR，GoktoganAH，etal.Sampling-based hierarchical motionplanning for areconfigurablewheel-on-legplanetary analogue exploration rover[J].Journal of Field Robotics，2020，37（5）： 786-811.

[9]Ichter B，Schmerling E，Pavone M. Group marching tree：samplingbased approximatelyoptimal motion planning on GPUs[C]//Proc of the 1st IEEE International Conference on Robotic Computing.Piscataway，NJ：IEEE Press，2017：219-226.

[10]Khatib O.Real-time obstacle avoidance for manipulatorsand mobile robots[C]//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEEPress，1985：500-505.

[11]Xu Jing，Song Kechen，Zhang Defu，et al. Informed anytime fast marching tree for asymptoticallyoptimal motion planning[J].IEEE Trans on Industrial Electronics，2021，68（6）：5068-5077.

[12]張亞莉，莫振杰，田昊鑫，等．基于改進APF-FMT*的農業機器 人路徑規劃算法［J]．華南農業大學學報，2024，45（3）：408- 415.（ZhangYali，Mo Zhenjie，Tian Haoxin，et al.Path planning algorithm of agricultural robot based on improved APF-FMT*[J]. Journal of South China Agricultural University，2024，45（3）： 408-415.）

[13]Karaman S，Frazzoli E.Sampling-based algorithms for optimal motion planning with deterministic μ -calculus specifications [C]//Proc of American Control Conference.Piscataway，NJ：IEEE Press，2012： 735-742.

收稿日期：2024-11-25；修回日期：2025-01-23 基金項目：國家自然科學基金面上項目（61471306）；四川省自然科學基金資助項目（2022NSFSC0548）；市移動物聯射頻識別技術重點實驗室專項資助項目（MYZD23032302）

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