融合橢圓約束的快速行進(jìn)樹(shù)路徑規(guī)劃算法

摘 要：

為解決快速行進(jìn)樹(shù)算法（fast marching tree，F(xiàn)MT*）生成路徑拐點(diǎn)多，且由于冗余探索導(dǎo)致路徑規(guī)劃時(shí)間長(zhǎng)的問(wèn)題，提出一種融合橢圓約束的快速行進(jìn)樹(shù)算法（ellipse constraints FMT*，EC-FMT*）。首先引入橢圓約束限制算法探索范圍，并結(jié)合直連策略避免冗余探索，縮短了路徑規(guī)劃時(shí)間；對(duì)于路徑拐點(diǎn)多的問(wèn)題，通過(guò)父節(jié)點(diǎn)重選策略修正路徑，去除不必要的拐點(diǎn)。仿真實(shí)驗(yàn)表明：采樣點(diǎn)數(shù)量為1 000、1 500、2 000個(gè)時(shí)，EC-FMT*與FMT*、RRT*、APF-Dynamic FMT*相比，在平均規(guī)劃時(shí)間上分別降低了81.9%～86.76%、86.15%～89.78%、77.12%～85.76%，并且拐點(diǎn)數(shù)量也有所降低；同時(shí)，EC-FMT*與FMT*、APF-Dynamic FMT*相比，迭代次數(shù)分別減少了84.72%～87.03%、80.89%～85.55%。說(shuō)明EC-FMT*能夠有效減少冗余探索，縮短路徑規(guī)劃時(shí)間，提高路徑質(zhì)量。

關(guān)鍵詞：FMT*算法；橢圓約束；直連策略；父節(jié)點(diǎn)重選

中圖分類(lèi)號(hào)：TP391"" 文獻(xiàn)標(biāo)志碼：A""" 文章編號(hào)：1001-3695（2024）12-009-3595-05

doi： 10.19734/j.issn.1001-3695.2024.05.0162

Fast marching tree path planning algorithm with elliptic constraints

Yuan Lei1， 2， Jia Xiaolin1， 2， Gu Yajun1， 2， Xu Zhengyu1， 2

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

Abstract：

To address the issues of excessive inflection points and long planning times in paths generated by the FMT* algorithm due to redundant exploration， this paper proposed an EC-FMT* algorithm. The EC-FMT* algorithm firstly introduced ellipse constraints to limit the exploration range of the algorithm and incorporated a direct connection strategy to minimize unnece-ssary searches， thereby shortening the path planning time. To tackle the problem of numerous inflection points， the algorithm employed a parent node reselection strategy to refine the path and eliminate unnecessary turns. Simulation experiments show that when the number of sampling points is 1 000， 1 500， and 2 000， respectively， the EC-FMT* algorithm achieves significant reductions in average planning time compared to FMT*， RRT*， and APF-Dynamic FMT*， ranging from 81.9% to 86.76%， 86.15% to 89.78%， and 77.12% to 85.76%. Additionally， the number of inflection points is also reduced. Furthermore， EC-FMT* reduces the number of iterations by 84.72% to 87.03% compared to FMT* and by 80.89% to 85.55% compared to APF-Dynamic FMT*. These results demonstrate that the EC-FMT* algorithm effectively mitigates redundant exploration， shortens path planning time， and enhances path quality.

Key words：FMT* algorithm; elliptical constraint; direct connection strategy; parent node reselection

0 引言

路徑規(guī)劃在機(jī)器人研究領(lǐng)域中是一項(xiàng)基礎(chǔ)且重要的技術(shù)，其核心問(wèn)題是如何在當(dāng)前環(huán)境中尋找一條起點(diǎn)到終點(diǎn)的無(wú)碰撞通行路徑，而規(guī)劃時(shí)間、拐點(diǎn)數(shù)量等則是評(píng)價(jià)路徑質(zhì)量的重要指標(biāo)［1］。

概率路線(xiàn)圖（probabilistic roadmap method，PRM）算法［2］和快速行進(jìn)樹(shù)（fast marching tree， FMT*）算法都是基于采樣的路徑規(guī)劃算法。PRM算法很好地解決了高維空間中構(gòu)造出有效路圖的困難，但需要大量時(shí)間用于碰撞檢測(cè)計(jì)算，因此導(dǎo)致路徑規(guī)劃時(shí)間長(zhǎng)［3，4］，而FMT*算法則可以很好地解決這一問(wèn)題［5］。FMT*算法結(jié)合了快速行進(jìn)方法（fast marching method， FMM）與RRT*（rapidly-exploring random tree star）算法的優(yōu)點(diǎn)，是一種漸進(jìn)最優(yōu)算法。該算法因其出色的性能在不同場(chǎng)景中表現(xiàn)優(yōu)秀，但它仍有一些不足之處，如生成路徑拐點(diǎn)多、存在冗余探索、規(guī)劃時(shí)間長(zhǎng)等問(wèn)題。

為解決上述問(wèn)題，吳旭鵬等人［6］將FMT*算法與人工勢(shì)場(chǎng)法相結(jié)合，約束了采樣點(diǎn)的生成范圍，減少了冗余探索。Starek等人［7］設(shè)計(jì)了一種雙向FMT*（bidirectional FMT*，BFMT*）算法，從起點(diǎn)和終點(diǎn)同時(shí)拓展，以提高算法效率，但增加了計(jì)算資源的消耗。Xu等人［8］提出了IAFMT*（informed anytime fast marching tree）算法，采用增量搜索以及動(dòng)態(tài)最優(yōu)搜索混合的方式提高了搜索效率，但算法復(fù)雜。Wu等人［9］提出了ST-FMT*（secure tunnel FMT*）算法，通過(guò)建立等距路線(xiàn)圖，并對(duì)初始路徑構(gòu)建安全隧道來(lái)減少冗余探索，但不合適的劣質(zhì)路徑解會(huì)導(dǎo)致區(qū)域劃分不合理和探索效率低下的問(wèn)題。張亞莉等人［10］提出了APF-FMT*算法，通過(guò)引入人工勢(shì)場(chǎng)法來(lái)減少FMT*算法的冗余探索問(wèn)題，但在極端情況下可能陷入局部最優(yōu)。Ichter等人［11］從硬件方面進(jìn)行改進(jìn)，利用GPU的并行計(jì)算來(lái)處理FMT*算法的拓展，但該方法對(duì)硬件要求較高。Gao等人［12］在BFMT*的基礎(chǔ)上，以A*算法為指導(dǎo)，提出了啟發(fā)式的HBFMT*（heuristic bidirectional FMT*）算法，但傳統(tǒng)啟發(fā)函數(shù)只利用了部分環(huán)境信息。吳錚等人［13］提出了基于方向選擇的快速行進(jìn)樹(shù)（DS-FMT*）算法，通過(guò)定向拓展來(lái)提高規(guī)劃效率，但探索方向的選擇會(huì)增加計(jì)算資源的消耗。

綜上所述，雖然國(guó)內(nèi)外學(xué)者對(duì)FMT*算法作出了許多改進(jìn)，但仍然存在一些問(wèn)題。因此，針對(duì)FMT*算法冗余探索導(dǎo)致路徑規(guī)劃時(shí)間長(zhǎng)，且路徑拐點(diǎn)多的問(wèn)題，本文提出了一種融合橢圓約束的快速行進(jìn)樹(shù)（EC-FMT*）算法。該算法通過(guò)橢圓約束［14］與直連策略相結(jié)合的方式來(lái)避免冗余探索；同時(shí)，受RRT*算法［15，16］啟發(fā)，采用父節(jié)點(diǎn)重選的方式來(lái)修正路徑，去除多余拐點(diǎn)，并通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了該算法的有效性。

1 FMT*算法

FMT*算法通過(guò)在地圖范圍內(nèi)預(yù)先生成一組采樣點(diǎn)，并對(duì)采樣點(diǎn)執(zhí)行“惰性”動(dòng)態(tài)遞歸，以生長(zhǎng)成樹(shù)，算法同時(shí)執(zhí)行樹(shù)構(gòu)造與圖搜索，即搜索地圖的同時(shí)構(gòu)建路徑樹(shù)。當(dāng)兩個(gè)采樣點(diǎn)距離小于設(shè)定距離時(shí)，兩點(diǎn)互為彼此鄰點(diǎn)，而“惰性”則指的是算法搜索時(shí)會(huì)“懶惰的”忽略障礙物的存在，每當(dāng)樹(shù)中節(jié)點(diǎn)與其局部最優(yōu)的鄰點(diǎn)連線(xiàn)有障礙物阻擋時(shí)，該鄰點(diǎn)會(huì)簡(jiǎn)單跳過(guò)并留待以后連接，而不是尋找其他連接。

由于FMT*算法采樣點(diǎn)的隨機(jī)生成，導(dǎo)致其在規(guī)劃過(guò)程中路徑樹(shù)會(huì)朝各個(gè)方向拓展，如圖1所示。然而部分采樣點(diǎn)對(duì)于路徑來(lái)說(shuō)是冗余的，即無(wú)須將其納入路徑樹(shù)仍能搜索到路徑，這些冗余點(diǎn)的探索造成了計(jì)算資源的浪費(fèi)，是導(dǎo)致路徑規(guī)劃時(shí)間長(zhǎng)的主要原因，且會(huì)增加額外的路徑拐點(diǎn)，降低路徑質(zhì)量。因此本文提出了融合橢圓約束的FMT*（EC-FMT*）算法，以橢圓約束和直連策略限制探索范圍，減少冗余探索；用父節(jié)點(diǎn)重選的方式修剪路徑，去除額外拐點(diǎn)，提高路徑質(zhì)量。

圖1中黑色為地圖邊框，灰色區(qū)域?yàn)檎系K物，綠色線(xiàn)段為路徑樹(shù)，紅色為規(guī)劃路徑（參見(jiàn)電子版）。可以看出，路徑樹(shù)幾乎遍布全圖，有較大的資源消耗，并且規(guī)劃出的路徑有著較多的拐點(diǎn)，并不筆直。

2 EC-FMT*算法

2.1 橢圓約束結(jié)合直連策略

為減少FMT*算法的冗余探索，節(jié)省計(jì)算資源，本文首先引入橢圓約束以限制探索范圍。橢圓以起點(diǎn)（Xs，Ys），終點(diǎn)（Xg，Yg）為焦點(diǎn)，焦點(diǎn)連線(xiàn)的中點(diǎn)（Xo，Yo）為橢圓原點(diǎn)。原點(diǎn)計(jì)算公式如式（1）所示。

Xo=Xs+Xg2Yo=Ys+Yg2（1）

焦距計(jì)算公式如式（2）所示。

d=（Xg－Xs）2+（Yg－Ys）2（2）

以k為橢圓擴(kuò)張參數(shù)，短半軸與長(zhǎng)半軸計(jì)算公式如式（3）所示。

B=kA=d2+k kgt;0（3）

其中：B為短半軸；A為長(zhǎng)半軸。對(duì)于一個(gè)點(diǎn)（X，Y），需要先將其轉(zhuǎn)換到橢圓中心坐標(biāo)系，并旋轉(zhuǎn)到橢圓的標(biāo)準(zhǔn)位置，則橢圓約束方程如式（4）所示。

X′2A2+Y′2B2≤1（4）

其中：X′和Y′為點(diǎn)（X，Y）平移到橢圓中心坐標(biāo)系中，并順時(shí)針旋轉(zhuǎn)θ°后的坐標(biāo)，詳細(xì)計(jì)算公式如式（5）所示。

X′Y′=R（－θ）·X－XoY－Yo（5）

其中：R（-θ）為旋轉(zhuǎn)矩陣，其定義如式（6）所示。

R（－θ）=cosθsinθ－sinθcosθ（6）

將式（5）（6）代入式（4）得到最終橢圓約束方程，如式（7）所示。

（（X－Xo）·cosθ+（Y－Yo）·sinθ）2A2+

（－（X－Xo）·sinθ+（Y－Yo）·cosθ）2B2≤1（7）

式（7）中θ可由分段函數(shù)定義，如式（8）所示。

θ=arctanYg－YsXg－Xs""" Xggt;Xs

arctanYg－YsXg－Xs+πYg≥Ys且Xglt;Xs

arctanYg－YsXg－Xs-πYglt;Ys且Xglt;Xs

π2 Xg=Xs且Yggt;Ys

-π2 Xg=Xs且Yglt;Ys（8）

通過(guò)對(duì)橢圓擴(kuò)張參數(shù)k的賦值來(lái)調(diào)整橢圓約束范圍，若當(dāng)前約束范圍內(nèi)路徑無(wú)解，則增大k值來(lái)增大橢圓約束范圍，直至找到可行路徑；若增大到設(shè)定的最大k值仍未搜索到路徑，則路徑規(guī)劃失敗。橢圓約束如圖2所示。

搜索區(qū)域加入橢圓約束后，雖然得到了限制，但仍會(huì)有部分冗余探索，如圖3中黑色虛線(xiàn)內(nèi)的綠色線(xiàn)段（參見(jiàn)電子版）。

傳統(tǒng)FMT*算法路徑搜索成功的條件是當(dāng)前拓展點(diǎn)Pz等于終點(diǎn)Pgoal，若不等于則一直迭代探索，直到開(kāi)放集合為空。在某些時(shí)候這種探索方式是低效的，因此本文通過(guò)直連策略改進(jìn)這種探索方式，具體措施如下：算法每輪探索時(shí)，都會(huì)檢測(cè)當(dāng)前輪次的拓展點(diǎn)Pz與終點(diǎn)間是否有障礙物阻擋，若沒(méi)有，則直接連接Pz與Pgoal，算法結(jié)束；若有障礙物阻擋，則繼續(xù)探索。引入直連策略后的算法效果如圖4所示。

由圖3與4對(duì)比可知，直連策略不僅減少了算法的冗余探索，而且提高了路徑末端質(zhì)量，使其更加筆直，沒(méi)有多余的拐點(diǎn)轉(zhuǎn)折。

2.2 父節(jié)點(diǎn)重選策略

由圖4可知，雖然經(jīng)過(guò)橢圓約束及直連策略的優(yōu)化后，減少了算法的冗余探索，且路徑末端質(zhì)量有所提高，但前半部分路徑仍存在拐點(diǎn)多的問(wèn)題，因此有必要對(duì)其進(jìn)行優(yōu)化。原算法在選擇父節(jié)點(diǎn)時(shí)，是選擇當(dāng)前采樣點(diǎn)P鄰域內(nèi)路徑代價(jià)最小且無(wú)碰撞的待拓展點(diǎn)Pz為其父節(jié)點(diǎn)，而后將采樣點(diǎn)P納入樹(shù)中成為節(jié)點(diǎn)。

父節(jié)點(diǎn)重選策略則在選定初始父節(jié)點(diǎn)Pz后，向上回溯，尋找Pz的祖先節(jié)點(diǎn)Pfore，并將祖先節(jié)點(diǎn)Pfore的路徑代價(jià)與Pz的路徑代價(jià)對(duì)比，選擇最小且無(wú)碰撞的節(jié)點(diǎn)作為新的父節(jié)點(diǎn)，如圖5所示。若在回溯過(guò)程中P與某個(gè)祖先節(jié)點(diǎn)Pfore的連線(xiàn)與障礙物發(fā)生碰撞，則停止回溯，在已回溯過(guò)的節(jié)點(diǎn)中尋找父節(jié)點(diǎn)。

圖5中紅色線(xiàn)段（參見(jiàn)電子版）所連接的Pfore即為P的新父節(jié)點(diǎn)。Vunvisited表示暫未被訪(fǎng)問(wèn)的節(jié)點(diǎn)集合，Vopen表示路徑樹(shù)中待拓展的節(jié)點(diǎn)集合，Vclosed表示路徑樹(shù)中已拓展的節(jié)點(diǎn)集合。

2.3 EC-FMT*算法

在EC-FMT*算法搜索過(guò)程中，算法會(huì)在橢圓約束范圍內(nèi)進(jìn)行探索，且每探索一個(gè)采樣點(diǎn)都會(huì)檢測(cè)該點(diǎn)與終點(diǎn)之間是否有障礙物，若沒(méi)有障礙物則路徑規(guī)劃成功，若有障礙物則繼續(xù)探索，直到路徑規(guī)劃成功或超過(guò)設(shè)定的最大k值導(dǎo)致路徑規(guī)劃失敗，通過(guò)這種方式來(lái)減少冗余探索，縮短路徑規(guī)劃時(shí)間，并且算法在探索過(guò)程中，會(huì)不斷修正路徑來(lái)減少拐點(diǎn)數(shù)量，提高路徑質(zhì)量。EC-FMT*算法如算法1所示。

算法1：EC-FMT*

輸入：環(huán)境參數(shù)、起點(diǎn)終點(diǎn)信息。

輸出：路徑規(guī)劃結(jié)果。

1 V←{xinit}∪SampleFree（n）;E←

2 Vunvisited←V＼{xinit};Vopen←{xinit}，Vclosed←

3 z←xinit，Max_k←k

4 Nz←Near（V＼{z}， z， r）

5 Save（Nz，z）

6 while z Xgoal do

7 "Vopen， new←

8 "Xnear=Nz∩Vunvisited

9 "Xnear= EllipticalConstraints（k，Xnear） //過(guò)濾掉橢圓約束外的點(diǎn)

10 "for x∈Xnear do

11 ""Nx←Near（V＼{x}，x.r）

12 ""Save（Nx，x）

13 ""Ynear←Nx∩Vopen

14 ""ymin←argminy∈Ynear{c（y）+Cost（y，x）} //動(dòng)態(tài)規(guī)劃方程

15 ""if CollisionFree（ymin，x） then

16 """Update_ParentNode（ymin，x） //父節(jié)點(diǎn)重選

17 """E←E∪{（ymin，x）}

18 """Vopen，new←Vopen，new∪{x}

19 """Vunvisited←Vunvisited＼{x}

20 """c（x）=c（ymin）+Cost（ymin，x）

21 ""end if

22 "end for

23 "Vopen←（Vopen∪Vopen，new）＼{z} //更新集合

24 "Vclosed←Vclosed∪{z}

25 "while Vopen= do

26 ""Max_k←Max_k+5

27 ""if Max_kgt;k×10 then //如果超過(guò)最大范圍

28 """return failure

29 ""end if

30 ""for z∈Vclosed do //擴(kuò)大約束范圍繼續(xù)搜索

31 """step 8～19

32 ""end for

33 "end while

34 "if CollisionFree（z，Xgoal） then //直連策略

35 ""E←E∪{（z，Xgoal）}

36 ""break

37 "end if

38 "z←arg miny∈Vopen{c（y）} //選擇成本最小的節(jié)點(diǎn)

39 end while

40 return Path（z，T=（Vopen∪Vclosed，E））

EC-FMT*算法節(jié)點(diǎn)拓展如圖6所示。

圖6（a）中算法從Vopen中找到成本最低的節(jié)點(diǎn)z，并在Vunvisited中找到橢圓約束范圍內(nèi)z的鄰點(diǎn)；圖6（b）中算法選擇到X的最優(yōu)無(wú)碰撞連接，并將其添加到路徑樹(shù)；圖6（c）中算法探索完z的所有鄰點(diǎn)后，將所有新探索的節(jié)點(diǎn)加入Vopen，并將z加入Vclosed，然后重新在Vopen中選擇成本最小的節(jié)點(diǎn)z進(jìn)入下一輪探索。

3 算法分析

3.1 概率完備性

若路徑規(guī)劃問(wèn)題存在可行解，則當(dāng)采樣點(diǎn)數(shù)量n→∞，解決問(wèn)題的概率P→1［8］，即

limn→∞P［{xgoal∈Vi∩xgoal in T}］=1（9）

EC-FMT*算法從xinit開(kāi)始構(gòu)建路徑樹(shù)，每從Vunvisited中搜索到一個(gè)采樣點(diǎn)，就會(huì)將其納入路徑樹(shù)。當(dāng)xgoal位于Vunvisited，且存在可行路徑時(shí)，該點(diǎn)就會(huì)被探測(cè)到。故當(dāng)采樣點(diǎn)數(shù)量n→∞，且覆蓋整個(gè)地圖空間時(shí)，搜索到可行路徑的概率為1。由于橢圓約束策略的存在，若當(dāng)前約束范圍內(nèi)搜索不到路徑，則會(huì)逐步擴(kuò)大范圍，直至覆蓋整個(gè)地圖空間。所以EC-FMT*算法具備概率完備性。

3.2 漸進(jìn)最優(yōu)性

設(shè)定（xfree，xinit，xgoal）是d維空間的路徑規(guī)劃問(wèn)題，令cn為EC-FMT*算法在采樣點(diǎn)數(shù)量為n、半徑為rn時(shí)生成的路徑長(zhǎng)度，c*為最優(yōu)路徑σ*的長(zhǎng)度，則rn可由式（10）表示。

rn=（1+η）21d1/dμ（xfree）1/dξdlog（n）n1/d（10）

其中：ngt;0；d為空間維數(shù)；ξd表示在d維歐幾里德空間中單位球的體積；μ（Xfree）表示空間中的無(wú)障礙空間的勒貝格測(cè)度（即二維空間面積或者三維空間體積）［6］。當(dāng)采樣點(diǎn)數(shù)量n→∞時(shí)，EC-FMT*算法所規(guī)劃的路徑長(zhǎng)度cn趨于最優(yōu)路徑長(zhǎng)度的概率為1［10］，即

limn→∞P（cn=c*）=1（11）

故EC-FMT*算法具備漸進(jìn)最優(yōu)性。

3.3 算法復(fù)雜度

由于EC-FMT*算法是基于FMT*算法建立的，故需要O（n log n）時(shí)間計(jì)算n個(gè)采樣點(diǎn)的解和O（n log n）時(shí)間內(nèi)來(lái)進(jìn)行O（n）次碰撞檢測(cè)，且調(diào)用代價(jià)函數(shù)的次數(shù)為O（n log n），由大O法則可知，EC-FMT*算法的計(jì)算復(fù)雜度為O（n log n）。由于算法同時(shí)進(jìn)行樹(shù)構(gòu)造與圖搜索，所以當(dāng)采樣點(diǎn)數(shù)量為n時(shí)，EC-FMT*算法計(jì)算一個(gè)可行解會(huì)花費(fèi)O（n log n）的時(shí)間，會(huì)占用O（n log n）的空間，故EC-FMT*算法時(shí)間復(fù)雜度和空間復(fù)雜度為O（n log n）。

4 仿真實(shí)驗(yàn)分析

為驗(yàn)證EC-FMT*算法的優(yōu)越性，本文將其與FMT*［5］、RRT*［16］、APF-Dynamic FMT*［6］算法進(jìn)行仿真對(duì)比，四種算法相應(yīng)參數(shù)保持一致。設(shè)定實(shí)驗(yàn)地圖如圖7所示，地圖為50×30的矩形區(qū)域，其中黑色為地圖邊框，灰色區(qū)域?yàn)檎系K物，藍(lán)色方點(diǎn)為起點(diǎn)，坐標(biāo)（2，2），紅色方點(diǎn)為終點(diǎn)，坐標(biāo)（49，24）（參見(jiàn)電子版）。分別在采樣點(diǎn)個(gè)數(shù)為1 000、1 500、2 000的情況下進(jìn)行實(shí)驗(yàn)，每組實(shí)驗(yàn)重復(fù)進(jìn)行100次，取平均值作為實(shí)驗(yàn)結(jié)果。

從圖7可以看出，F(xiàn)MT*和RRT*算法幾乎都在全圖范圍內(nèi)進(jìn)行了探索，消耗了較多的計(jì)算資源，且規(guī)劃的路徑拐點(diǎn)較多，APF-Dynamic FMT*將人工勢(shì)場(chǎng)法與FMT*算法相融合，使得采樣點(diǎn)的生成變得集中，雖然減少了地圖邊緣的冗余探索，但起點(diǎn)與終點(diǎn)連線(xiàn)之間的冗余探索仍然存在，且路徑質(zhì)量有待提高；而EC-FMT*算法僅在橢圓約束范圍內(nèi)進(jìn)行探索，且由于父節(jié)點(diǎn)重選策略的存在，規(guī)劃出的路徑拐點(diǎn)少，路徑較為筆直，路徑質(zhì)量有所提高，說(shuō)明了EC-FMT*算法的優(yōu)越性。實(shí)驗(yàn)數(shù)據(jù)如表1和圖8所示。

從表1可以看出，四種算法由于都具有漸進(jìn)最優(yōu)性，所以規(guī)劃出來(lái)的路徑長(zhǎng)度差距不大，但FMT*算法拐點(diǎn)個(gè)數(shù)最多；由于RRT*算法也具有修正路徑的效果，拐點(diǎn)數(shù)量有所降低；APF-Dynamic FMT*結(jié)合了人工勢(shì)場(chǎng)法，提高了采樣點(diǎn)的生成質(zhì)量，減少了部分冗余探索，從而提高了路徑質(zhì)量，故拐點(diǎn)個(gè)數(shù)有小幅下降；但EC-FMT*算法的拐點(diǎn)個(gè)數(shù)最少，說(shuō)明EC-FMT*算法父節(jié)點(diǎn)重選策略的有效性，其所規(guī)劃的路徑更為筆直，沒(méi)有多余的轉(zhuǎn)折。

從圖8（a）可知，在不同采樣點(diǎn)個(gè)數(shù)下，EC-FMT*算法的平均規(guī)劃時(shí)間均為最少；且隨著采樣點(diǎn)個(gè)數(shù)的增加，平均規(guī)劃時(shí)間變化不大，較為穩(wěn)定。當(dāng)采樣點(diǎn)數(shù)量為1 000個(gè)時(shí)，EC-FMT*算法平均規(guī)劃時(shí)間相比于FMT*、RRT*、APF-Dynamic FMT*算法分別降低了81.9%、86.15%、84.1%；當(dāng)采樣點(diǎn)為1 500個(gè)時(shí)，平均規(guī)劃時(shí)間分別降低了82.6%、89.06%、77.12%；當(dāng)采樣點(diǎn)個(gè)數(shù)為2 000個(gè)時(shí)，平均規(guī)劃時(shí)間分別降低了86.76%、89.78%、85.76%。

另外，從圖8（b）可知，EC-FMT*算法與FMT*、APF-Dynamic FMT*算法相比，迭代次數(shù)有明顯降低，在采樣點(diǎn)數(shù)量分別為1 000、1 500、2 000個(gè)時(shí)，迭代次數(shù)分別下降了84.73%～87.03%、80.89%～85.55%。

綜上所述，EC-FMT*算法能夠有效提高算法的收斂速度和規(guī)劃效率，減少路徑拐點(diǎn)數(shù)量，提高路徑質(zhì)量，證明了EC-FMT*算法的優(yōu)越性和高效性。

5 結(jié)束語(yǔ)

為解決FMT*算法進(jìn)行路徑規(guī)劃時(shí)存在冗余探索，且路徑拐點(diǎn)多等問(wèn)題，本文提出了一種融合橢圓約束的FMT*算法。首先通過(guò)橢圓約束限制探索范圍，并結(jié)合直連策略減少冗余探索，若約束范圍內(nèi)未搜索到路徑，算法會(huì)逐步擴(kuò)大搜索區(qū)域；同時(shí)，直連策略也提高了路徑末端的質(zhì)量，使其更加筆直。最后，為解決路徑拐點(diǎn)多的問(wèn)題，本文通過(guò)父節(jié)點(diǎn)重選策略修正路徑，減少路徑拐點(diǎn)。仿真實(shí)驗(yàn)表明，EC-FMT*算法由于減少了冗余探索，故算法路徑規(guī)劃時(shí)間明顯減少。在采樣點(diǎn)數(shù)量為1 000、1 500、2 000個(gè)時(shí)，與FMT*、RRT*、APF-Dynamic FMT*算法相比，平均規(guī)劃時(shí)間分別減少了81.9%～86.76%、86.15%～89.78%、77.12%～85.76%，且拐點(diǎn)個(gè)數(shù)也有明顯降低；并且EC-FMT*算法與FMT*、APF-Dynamic FMT*算法相比，迭代次數(shù)分別降低了84.72%～87.03%、80.89%～85.55%，證明了EC-FMT*算法在路徑規(guī)劃效率及規(guī)劃質(zhì)量上更具優(yōu)越性。

本文主要研究靜態(tài)環(huán)境下的路徑規(guī)劃問(wèn)題，在未來(lái)的研究中會(huì)考慮加入動(dòng)態(tài)因素，提高算法的實(shí)用性。

參考文獻(xiàn)：

［1］司徒華杰， 雷海波， 莊春剛. 動(dòng)態(tài)環(huán)境下基于人工勢(shì)場(chǎng)引導(dǎo)的RRT路徑規(guī)劃算法 ［J］. 計(jì)算機(jī)應(yīng)用研究， 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.）

［2］Kavraki L， Svestka P， Overmars M H， et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces ［J］. IEEE Trans on Robotics and Automation， 1994， 12（4）： 566-580.

［3］Ravankar A A， Ravankar A， Emaru T， et al. HPPRM： hybrid potential based probabilistic roadmap algorithm for improved dynamic path planning of mobile robots ［J］. IEEE Access， 2020， 8： 221743-221766.

［4］Esposito J M， Wright J N. Matrix completion as a post-processing technique for probabilistic roadmaps ［J］. The International Journal of Robotics Research， 2019， 38（2-3）： 388-400.

［5］Janson L， Pavone M. Fast marching trees： a fast marching sampling based method for optimal motion planning in many dimensions exten-ded version ［M］// Inaba M， Corke P. Robotics Research. Springer Tracts in Advanced Robotics. Cham： Springer， 2016：667-684.

［6］吳旭鵬， 賈小林， 顧婭軍. 融合人工勢(shì)場(chǎng)法的動(dòng)態(tài)快速行進(jìn)樹(shù)路徑規(guī)劃算法 ［J］. 計(jì)算機(jī)應(yīng)用研究， 2024， 41（9）： 2745-2750. （Wu Xupeng， Jia Xiaolin， Gu Yajun. Dynamic fast marching tree algorithm with integrated artificial potential fields ［J］. Application Research of Computers， 2024， 41（9）： 2745-2750.）

［7］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.

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

［9］Wu Zheng， Chen Yanjie， Liang Jinglin， et al. ST-FMT*： a fast optimal global motion planning for mobile robot ［J］. IEEE Trans on Industrial Electronics， 2022， 69（4）： 3854-3864.

［10］張亞莉， 莫振杰， 田昊鑫， 等. 基于改進(jìn)APF-FMT*的農(nóng)業(yè)機(jī)器人路徑規(guī)劃算法 ［J］. 華南農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2024， 45（3）： 408-415. （Zhang Yali， 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.）

［11］Ichter B， Schmerling E， Pavone M. Group marching tree： sampling-based approximately optimal motion planning on GPUs ［C］// Proc of the 1st IEEE International Conference on Robotic Computing. Pisca-taway， NJ： IEEE Press， 2017： 219-226.

［12］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.

［13］吳錚， 陳彥杰， 何炳蔚， 等. 基于方向選擇的移動(dòng)機(jī)器人路徑規(guī)劃方法 ［J］. 計(jì)算機(jī)集成制造系統(tǒng)， 2021， 27（3）： 672-682. （Wu Zheng， Chen Yanjie， He Bingwei， et al. Direction selection-based algorithm for mobile robot path planning ［J］. Computer Integrated Manufacturing Systems， 2021， 27（3）： 672-682.）

［14］Gammell J D， Srinivasa S S， Barfoot T D. Informed RRT*： optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2014： 2997-3004.

［15］仲健寧， 向國(guó)菲， 佃松宜. 針對(duì)包含狹窄通道復(fù)雜環(huán)境的高效RRT*路徑規(guī)劃算法 ［J］. 計(jì)算機(jī)應(yīng)用研究， 2021， 38（8）： 2308-2314. （Zhong Jianning， Xiang Guofei， Dian Songyi. Efficient RRT* path planning algorithm for complex environments with narrow passages ［J］. Application Research of Computers， 2021， 38（8）： 2308-2314.）

［16］Karaman S， Walter M R， Perez A， et al. Anytime motion planning using the RRT* ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2011： 1478-1483.