基于DBSCAN算法的改進RRT Connect路徑規(guī)劃研究

摘"要：針對雙向快速擴展隨機樹（RRTConnect）算法收斂速度慢、路徑搜索效率低、路徑曲折的問題，提出了一種基于DBSCAN算法的改進RRTConnect算法。在RRTConnect算法基礎上加入節(jié)點引力場引導新節(jié)點產生方向，使收斂速度變快；同時通過引入橢圓采樣方法，縮小采樣范圍，提高路徑規(guī)劃效率；最后在改進RRTConnect的基礎上引入DBSCAN聚類算法，使得到的規(guī)劃路徑更加平滑可靠，增強算法的魯棒性。為了驗證改進后的算法優(yōu)化效果，分別在不同環(huán)境中與RRT算法、RRTConnect算法進行仿真比較。仿真實驗表明，改進后的RRTConnect算法路徑規(guī)劃效果均要優(yōu)于其他兩種算法，不僅加快了路徑規(guī)劃速度，而且得到的路徑接近最優(yōu)解，具有普遍適用性、魯棒性高等特點。

關鍵詞：RRTConnect算法；DBSCAN算法；橢圓采樣；引力場；路徑規(guī)劃

中圖分類號：TP39""""""文獻標識碼：A

Research"on"Improved"RRTConnect"Path"Planning"

Based"on"DBSCAN"Algorithm

LI"Liuyang，"WANG"Keqing，"JIAO"Sitao，"ZHOU"Qi"

（School"of"Automation，"Nanjing"University"of"Information"Science"and"Technology，Nanjing，Jiangsu"210044，China）

Abstract：Aiming"at"the"problems"of"slow"convergence"speed，"low"path"search"efficiency"and"tortuous"path"of"RRTConnect"algorithm，"an"improved"RRTConnect"algorithm"based"on"DBSCAN"algorithm"is"proposed."On"the"basis"of"the"RRTConnect"algorithm，"the"node"gravitational"field"is"added"to"guide"the"direction"of"the"new"node，"which"makes"the"convergence"speed"faster."At"the"same"time，"by"introducing"the"elliptical"sampling"method，"the"sampling"range"is"reduced"to"improve"the"efficiency"of"path"planning."Finally，"based"on"the"improved"RRTConnect，"the"DBSCAN"clustering"algorithm"is"introduced"to"make"the"obtained"planning"path"smoother"and"more"reliable，nbsp;and"enhance"the"robustness"of"the"algorithm."In"order"to"verify"the"optimization"effect"of"the"improved"algorithm，"it"is"simulated"and"compared"with"the"RRT"algorithm"and"the"RRTConnect"algorithm"in"different"environments."The"simulation"results"show"that"the"improved"RRTConnect"algorithm"has"better"path"planning"effect"than"the"other"two"algorithms."It"not"only"accelerates"the"path"planning"speed，"but"also"obtains"the"path"close"to"the"optimal"solution，"which"has"the"characteristics"of"universal"applicability"and"high"robustness.

Key"words：RRTConnect"algorithm;"DBSCAN"algorithm;"ellipse"sampling;"gravitational"field;"path"planning

路徑規(guī)劃是機器人技術和自動化領域中的一個核心問題，它涉及在給定環(huán)境中，避免與障礙物碰撞的前提下，找到最優(yōu)路徑的任務[1]。路徑規(guī)劃在眾多實際應用中發(fā)揮著重要作用，包括自動駕駛[2]、機器人導航[3]、無人機航行[4]和運輸[5]等領域。隨著技術的不斷發(fā)展和應用的廣泛需求，路徑規(guī)劃算法的研究變得越來越關鍵。常見的路徑規(guī)劃算法有：A*算法[6]、蟻群算法[7]、人工勢場算法[8]、概率圖算法（PRM）[9]、D*算法[10]、快速探索隨機樹（RRT）。

其中，最常用的算法是基于采樣的方法。RRT算法憑借其搜索路徑效率高、適用于高維空間的優(yōu)點，在路徑規(guī)劃算法中得到快速發(fā)展和應用。傳統(tǒng)的RRT算法采用隨機采樣的方法進行搜索，具有較強的搜索能力，但RRT的隨機性也導致其搜索效率有提升的空間，同時規(guī)劃路徑的質量方面也不能達到最優(yōu)，特別是在復雜環(huán)境和高維空間中。為了提高RRT算法的搜索效率和路徑質量問題，很多學者對其進行了改進。Kuffner等[11]在2000年提出RRTConnect算法，通過起點和目標點同時生成隨機擴展樹來提高算法的搜索效率，但是規(guī)劃的路徑比較曲折，在應用于空間機械臂避障路徑規(guī)劃時，存在盲采樣點大、無效點多等問題[12]。Karaman等[13]在2011年提出RRT*算法，該算法隨著樣本數(shù)量的增加而不斷優(yōu)化隨機樹，由于其漸進優(yōu)化的特性，RRT*算法理論上可以得到最優(yōu)解，但是收斂速度過慢。趙超力等[14]將引力場思想引入RRTConnect算法，引導隨機樹生長，并且在路徑起點和終點之間設置第三節(jié)點，由兩棵隨機樹變?yōu)樗目秒S機樹，大大加快搜索效率，但存在獲取路徑曲折問題。游達章等[15]提出一種基于橢球子集采樣的RRTConnect算法，縮小采樣空間，同時引入目標偏執(zhí)采樣策略，提高路徑規(guī)劃效率，最后基于三角不等式的路徑修剪策略來優(yōu)化路徑質量。Zhu等[16]提出將Dijkstra算法與RRTConnect算法相結合，去除冗余路徑節(jié)點，縮短路徑長度，引用人工勢場思想，減少路徑盲目搜索，最后利用B樣條曲線對規(guī)劃路徑進行平滑處理，獲得連續(xù)軌跡。

通過對上面的文獻總結得知，RRTConnect算法優(yōu)點還是比較突出的，相較于RRT算法和RRT*算法，它采用兩棵隨機擴展樹來搜索路徑，使搜索效率大大提升，但是RRTConnect算法的缺點也很明顯，比如擴展路徑曲折、隨機盲目擴展等問題。為了解決以上問題，以RRTConnect算法為基礎，在路徑起點以及目標點上疊加引力場引導隨機樹擴展方向，使收斂速度變快，實現(xiàn)路徑平滑的效果；同時引入橢圓采樣策略，將采樣空間限制在一個橢圓空間內，以減少路徑搜索的盲目性，提高路徑規(guī)劃效率；最后針對路徑曲折問題，在改進RRTConnect算法的基礎上采用DBSCAN聚類算法，對以往的仿真數(shù)據(jù)進行聚類分析，就可以直接、有效地通過已有的歷史數(shù)據(jù)提取出重要且有意義的路徑點，使得到的路徑更接近最優(yōu)解，算法普適性和魯棒性更高。通過仿真實驗，驗證改進算法的有效性和可行性。

1"傳統(tǒng)算法

1.1"RRT算法

RRT算法作為一種隨機性算法，在搜索空間中隨機生成節(jié)點，并將新節(jié)點連接到樹中已有的節(jié)點，從而構建一棵隨機生長的樹，直到找到目標節(jié)點為止。RRT算法憑借其快速、簡單且易于實現(xiàn)的特點在路徑規(guī)劃問題中得到廣泛的研究和應用。

RRT算法通過在起始點構建一棵只包含根節(jié)點的樹，定義起始節(jié)點為n"init，見圖1，在可搜索空間中隨機采樣一個點n"rand，再根據(jù)定義的搜索步長λ在樹中搜索最近的節(jié)點n"near，如果n"rand和n"near之間的直線上有障礙物，則舍棄該采樣點并重新生成一個新的采樣點；如果沒有障礙物，則根據(jù)搜索步長λ逐步連接n"rand和n"near。重復上面的步驟直到到達目標點n"goal或設定好的迭代次數(shù)。最后通過回溯隨機樹在上述過程中生成的節(jié)點，獲取從起始點到終點的路徑。

1.2"RRTConnect算法

RRTConnect算法在RRT算法的基礎上將起始點和目標點構建為兩棵隨機樹，分別向彼此的方向擴展，同時引入節(jié)點貪婪擴展策略，在每次迭代生成節(jié)點時，盡量將另一棵樹的最近節(jié)點作為該樹的擴展方向，使兩棵樹快速相交。另外，如果在擴展過程中沒有遇到障礙物，算法繼續(xù)在該方向上擴展節(jié)點；如遇到障礙物，該算法使用交換函數(shù)，讓另一個隨機樹擴展，這樣避免算法陷入局部最優(yōu)困境。相較于RRT算法，搜索路徑效率得到了極大提升。RRTConnect算法擴展過程如圖2所示。

2"改進算法

RRTConnect算法相較于RRT算法在搜索效率以及路徑規(guī)劃質量方面雖然有所提升，但是本身的盲目搜索特性還有路徑曲折問題依然存在，所以我們就根據(jù)RRTConnect算法所存在的問題，提出針對性的算法優(yōu)化。針對盲目搜索問題，引入節(jié)點引力場概念，給起始點和目標點各疊加一個引力場，引導隨機樹向目標節(jié)點生長；同時引入橢圓采樣策略，將隨機采樣空間限制在一個橢圓空間內，進一步降低路徑盲目搜索性；針對規(guī)劃路徑曲折問題，采用DBSCAN聚類算法去除路徑冗余點，優(yōu)化路徑質量，提高算法適用性和魯棒性。

2.1"節(jié)點引力場

將目標節(jié)點引力思想[17，18]引入RRTConnect算法中，分別對雙向隨機樹中的每個節(jié)點n都疊加一個目標引力函數(shù)G（n）"，這個節(jié)點是由起始節(jié)點（或目標接點）向外擴展的第n個節(jié)點，表達式為：

F（n）=R（n）+G（n）""""（1）

方程（1）表示節(jié)點"n"到目標點的生長指導函數(shù)"F（n）"是由隨機生長函數(shù)"R（n）"和目標引力函數(shù)"G（n）"組成的。生長指導函數(shù)F（n）"是指導節(jié)點n向目標點生長的總力量。隨機生長函數(shù)R（n）表示從初始節(jié)點到節(jié)點n的隨機生長力量，它模擬了隨機的生長過程。目標引力函數(shù)G（n）則代表了目標點對節(jié)點n的引力，它使節(jié)點"n"被引導向目標點。

新節(jié)點的隨機生長函數(shù)R（n）的計算公式為：

R（n）=λ*nrand-nnearnrand-nnear（2）

其中λ為步長。目標引力函數(shù)G（n）為：

G（n）=λ*γ*ngoal-nnear2（3）

其中γ為引力系數(shù)，ngoal-nnear2為目標點到隨機生長樹上最近點的歐氏距離的平方，這樣離目標點越遠，引力越大。

將式（2）和式（3）代入式（1）得：

F（n）=λ*nrand-nnearnrand-nnear+

λ*γ*ngoal-nnear2（4）

根據(jù)式（4）可得到疊加引力場后的新節(jié)點產生方式為：

nnew=nnear+λ*nrand-nnearnrand-nnear+

γ*ngoal-nnear2（5）

在疊加引力場之后，每個節(jié)點在雙向隨機樹中都使用相同的生長指導函數(shù)F（n），該函數(shù)綜合考慮了引力分量和自由空間的因素。引力分量使節(jié)點朝向目標方向生長，而自由空間的概念確保節(jié)點在生長過程中避免碰撞或穿越障礙物。這種一致性的生長指導函數(shù)確保了樹的協(xié)調和一致性，使得雙向隨機樹能夠高效地搜索路徑并找到目標點。引力場下影響節(jié)點擴展示意圖如圖3所示。

2.2"橢圓采樣

在規(guī)劃路徑時，由于起始點和目標點都是已知的，兩點連線是成本最低的路徑。將隨機點限制在最小路徑周圍生成的橢圓區(qū)域內，而不是在自由空間中任意搜索，這樣既能提高收斂速度，又能克服一定的隨機性。

為了將采樣空間限制在橢圓子集中，引用以起始點和目標點為焦點的橢圓。Cmin是起始點和目標點之間的歐氏距離，橢球的短軸為C2best-C2min，橢球的長軸為Cbest，見圖4。

該采樣過程需要在子集Xellipse～U（Xellipse）內均勻地分布樣本。通過將樣本均勻分布在半徑為n的球上，然后將它們轉移到橢圓子集XBall～U（Xball）來實現(xiàn)。

xellipse=Lxball+xcenter""（6）

xcenter=xf1+xf22"""（7）

xcenter為橢球的中心，xf1、xf2為橢圓子集的焦點，Xball是單位r球內的樣本。

Xball=x∈Xx2≤１}""（8）

使用超橢球內迭代采樣方法，初始路徑長度與后續(xù)迭代生成的路徑長度相比較，后者較短。隨著迭代次數(shù)的增加，超橢球的體積逐漸縮小，導致路徑長度不斷減小。最終，算法能夠找到最優(yōu)路徑。橢圓采樣在高維空間中展現(xiàn)出更顯著的優(yōu)勢，是因為它具有更快的收斂速度和更高的搜索效率。

2.3"DBSCAN聚類算法

經過RRTConnect算法的多次規(guī)劃，雖然可以避開障礙物并到達目的地，但每個規(guī)劃路徑過程都是相互獨立的，雖然有些路徑能夠安全到達目標點，但它們遠不是最優(yōu)路徑，如果將規(guī)劃結果直接應用到真實的場景中，可能會付出高昂的代價。引進的DBSCAN算法不需要事先確定簇數(shù)，對野值不敏感，操作簡單，計算速度快。DBSCAN算法是一種典型的基于密度分布的聚類算法。該算法將聚類定義為密度連接的最大點集，可以將密度足夠大的區(qū)域劃分為聚類，也可以在噪聲空間數(shù)據(jù)庫中找到任意形狀的聚類。該算法需要預先定義半徑區(qū)域和最小點。

半徑區(qū)域：DBSCAN算法用來判斷數(shù)據(jù)點的相似度、接近度以及是否屬于同一類別的標準距離。

最小點：標準用于選擇聚類中心。如果附近點的數(shù)量小于N，則該點被標記為噪聲點。如果附近點的數(shù)量大于或等于N，則將該點和附近點組合以形成聚類。然后標記點，采用遞歸算法不斷擴展聚類。

DBSCAN算法根據(jù)每個數(shù)據(jù)點的空間分布，將點集劃分為核心點、邊界點和離群點。核心點均在半徑范圍內，其他數(shù)據(jù)點包含的點數(shù)超過最小值;邊界點在半徑內，其他數(shù)據(jù)點包含少于最小數(shù)量的點;離群點既不是核心點，也不是邊界點。

使用最近鄰的思想，Minkowski（閔可夫斯基）距離用于測量最近距離，其表示如下：

D（x，y）=p（|x1-y1|）p+（|x2-y2|）p+…+xm-ymp=p∑mi=1xi-yip（9）

本文采用曼哈頓距離來度量樣本距離，其表示如下：

D（x，y）=x1-y1+x2-y2+…+

xm-ym=∑mi=1xi-yi（10）

每個聚類基于密度進行分離，用每個聚類的樣本均值表示聚類的質心。

改進后的RRTConnect算法偽代碼如下;

其中，center為橢圓的中心點；semi_major_axis為橢圓的半長軸；semi_minor_axis為橢圓的半短軸；theta_min和theta_max為橢圓上采樣點的角度范圍；random"value"between"0"and"1為在區(qū)間"[0，"1]"內生成一個隨機值；sqrt為平方根函數(shù)；empty"list為空列表；append為將元素添加到列表的末尾；eps為領域半徑，用于確定點的鄰域；minpts為鄰域中所需的最小點數(shù)。

3"仿真結果與分析

相關仿真實驗在MATLAB"R2020a軟件中完成，實驗環(huán)境配置如下：11th"Gen"Intel（R）"Core（TM）"i711800H@2.3GHz，RAM16GB。

為了驗證改進后的算法的適用性和有效性，進行仿真實驗，與RRT算法、RRTConnect算法規(guī)劃的路徑結果相比較。并且應用于三種仿真環(huán)境，比較三種算法的生成節(jié)點數(shù)、路徑長度以及規(guī)劃時間三個參數(shù)。為了各算法實驗參數(shù)的一致性，三種算法相關參數(shù)設置和步長設置一致。

3.1"少障礙物環(huán)境

本環(huán)境地圖采用15×15，起點位置為[3，3]，終點位置為[13，13]。三種算法在該環(huán)境下路徑搜索及規(guī)劃路徑（紅色路線為最終規(guī)劃路徑，灰色路線為搜索路徑）見圖5。

搜索及規(guī)劃路徑

在該環(huán)境下，分別用三種算法進行50次仿真實驗，對路徑生成過程中的節(jié)點數(shù)、路徑長度以及規(guī)劃時間三個參數(shù)做記錄取平均值，生成表2。

由圖5和表2可得，本文改進的算法相較于RRT算法和RRTConnect算法，得到的規(guī)劃路徑更加平滑，搜索路徑節(jié)點更少。在生成節(jié)點數(shù)方面，改進算法只有RRT算法的17.8%，RRTConnect算法的75%；改進算法在路徑長度方面比RRT算法縮短27.3%，比RRTConnect算法縮短9.8%；規(guī)劃時間也相應減短59.6%、13.1%。在三個參數(shù)方面以及生成路徑平滑度均有提升效果。

3.2"適度障礙物環(huán)境

本環(huán)境地圖采用20×20，起點位置為[2，2]，終點位置為[19，19]。三種算法在該環(huán)境下路徑搜索及規(guī)劃路徑（紅色路線為最終規(guī)劃路徑，灰色路線為搜索路徑）見圖6。

路徑搜索及規(guī)劃路徑

在該環(huán)境下，分別用三種算法進行50次仿真實驗，對路徑生成過程中的節(jié)點數(shù)、路徑長度以及規(guī)劃時間三個參數(shù)做記錄取平均值，生成表3。

由圖6和表3可得，在生成節(jié)點數(shù)方面，改進算法只有RRT算法的15.1%，RRTConnect算法的73.5%；改進算法在路徑長度方面比RRT算法縮短31.5%，比RRTConnect算法縮短11.1%；規(guī)劃時間也相應減短63.2%、15.1%。同樣在三個參數(shù)方面以及生成路徑平滑度均有提升效果。

3.3"多障礙物環(huán)境

本環(huán)境地圖采用25×25，起點位置為[2，2]，終點位置為[24，24]。三種算法在該環(huán)境下路徑搜索及規(guī)劃路徑（紅色路線為最終規(guī)劃路徑，灰色路線為搜索路徑）見圖7。

路徑搜索及規(guī)劃路徑

在該環(huán)境下，分別用三種算法進行50次仿真實驗，對路徑生成過程中的節(jié)點數(shù)、路徑長度以及規(guī)劃時間三個參數(shù)做記錄取平均值，生成表4。

由圖7和表4可得，在生成節(jié)點數(shù)方面，改進算法只有RRT算法的14.5%，RRTConnect算法的68.1%；改進算法在路徑長度方面比RRT算法縮短28.6%，比RRTConnect算法縮短13.6%；規(guī)劃時間也相應減短69.7%、21.2%。在三個參數(shù)方面以及生成路徑平滑度均有提升效果。

從以上仿真數(shù)據(jù)可以看出，改進后的算法相較于RRT算法、RRTConnect算法數(shù)值方面均有提升，尤其是在環(huán)境復雜度較高的時候，能夠逐步提升規(guī)劃時間，且最終規(guī)劃路徑均取得不錯的效果。

4"結"論

通過對RRTConnect算法的改進，解決了擴展路徑曲折以及隨機盲目擴展等問題。通過在路徑起始點和目標點上引入引力場，以及橢圓采樣策略，引導隨機樹在橢圓區(qū)域向目標點方向擴展，減少無用擴展，加快路徑規(guī)劃效率。通過DBSCAN聚類算法對歷史采樣點數(shù)據(jù)進行聚類分析，去除冗余點，增強算法魯棒性，使得到的路徑接近最優(yōu)解。通過在三種環(huán)境下與RRT算法、RRTConnect算法的比較，驗證了改進算法的有效性和適用性。

參考文獻

［１］"湯云峰．改進智能算法在移動機器人路徑規(guī)劃中的研究［D］．南京："南京郵電大學，2021.

［2］"李學鋆．基于UTMD的汽車自動駕駛的路徑規(guī)劃尋優(yōu)算法［J］．"汽車安全與節(jié)能學報，2018，9（4）：449-455.

［3］"陳敏，李笑，武交峰．基于改進RRT算法的差動機器人路徑規(guī)劃［J］．"計算機應用與軟件，2019，36（9）"："276-280.

［4］"尹高揚，周紹磊，吳青坡．基于改進RRT算法的無人機航跡規(guī)劃［J］．"電子學報，2017，45（7）：1764-1769．

［5］"LIU"C"L，"PAI"T"W，"CHANG"C"T"，et"al."Pathplanning"algorithms"for"public"transportation"systems［C］//2001"IEEE"Intelligent"Transportation"Systems."Proceedings"（Cat."No.01TH8585），"Oakland，"CA，"2001：1061-1066.

［6］"DING"H，"LI"Y，"CHAI"Y，"et"al."Path"planning"for"2DOF"manipulator"based"on"Bezier"curve"and"A*"algorithm[C]//2018"Chinese"Automation."Chinese"Automation"Congress"（CAC）."IEEE，2018：670-674.

［7］"LIU"T，"SONG"C，"JIANG"J."Robotic"path"planning"based"on"improved"ant"colony"algorithm[C]//International"Symposium"on"Neural"Networks."Springer，"Cham，"2019："351-358.

［8］"BOUNINI"F，"GINGRAS"D，"POLLART"H，"et"al."Modified"artificial"potential"field"method"for"online"path"planning"applications［C］//2017"IEEE"Intelligent"Vehicles"Symposium"（IV），"2017：180-185.

［9］"KAVRAKI"L"E，"SVESTKA"P，"LATOMBE"J"C，"et"al."Probabilistic"roadmapsnbsp;for"path"planning"in"highdimensional"configuration"spaces[J]."IEEE"Transactions"on"Robotics"and"Automation"1996，12（4）：566-580.

［10］KOENIG"S，"LIKHACHEV"M."D*"lite［J］."Journal"of"Artificial"Intelligence"Research，2002，17：1-5.

［11］KUFFNER"J"J，"LAVALLE，"S"M.RRTconnect："An"efficient"approach"to"singlequery"path"planning［C］//IEEE"International"Conference"on"Robotics"and"Automation."IEEE，"2000，"2："995-1001.

［12］CAO"Y，"ZHANG"Y"B，"ZHOU"Y."Research"on"obstacle"avoidance"path"planning"of"spatial"manipulator"based"on"improved"RRTconnect[J]."Machine"Tool"amp;"Hydraulics，"2020，48"（12）：177-183.

［13］KARAMAN"S，"FRAZZOLI"E."Samplingbased"algorithms"for"optimal"motion"planning[J]."The"International"Journal"of

Robotics"Research，"2011，"30（7）：846-894.

［14］趙超力，馬行，張春濤，等.基于引力場引導的RRTConnect路徑規(guī)劃算法[J].電子測量技術，2021，44（22）：44-49.

［15］游達章，楊智杰，張業(yè)鵬.基于改進RRTConnect算法的機械臂運動規(guī)劃[J].電子測量技術，2023，46（8）：112-119.

［16］ZHU"Y，"TANG"Y，"ZHANG"Y，"et"al."Path"planning"of"manipulator"based"on"improved"RRTConnect"algorithm［C］//2021"2nd"International"Conference"on"Big"Data"amp;"Artificial"Intelligence"amp;"Software"Engineering"（ICBASE），"Zhuhai，"China，"2021：44-47.

［17］GAMMELL"J"D，"SRINIVASA"S"S，"BARFOOT"T"D."Informed"RRT*：optimal"samplingbased"path"planning"focused"via"direct"sampling"of"an"admissible"ellipsoidal"heuristic[C]//Proc."IEEE/RSJ"Int."Conf."Intell."Robots"Syst.，"2014："2997-3004.

［18］LUCHI"D，"LOUREIROS"R"A，"Miguel"Varejo"F."Sampling"approaches"for"applying"DBSCAN"to"large"datasets[J]."Pattern"Recogn"Lett，2019，117：90-96.