同根雙向擴(kuò)展的貪心RRT路徑規(guī)劃算法

杜傳勝，高煥兵，侯宇翔，汪子健

1.山東建筑大學(xué) 信息與電氣工程學(xué)院，濟(jì)南250101

2.山東省智能建筑技術(shù)重點(diǎn)實(shí)驗(yàn)室，濟(jì)南250101

隨著社會(huì)科學(xué)技術(shù)的不斷更新與發(fā)展，機(jī)器人技術(shù)也迎來了其發(fā)展的黃金時(shí)期，智能機(jī)器人逐漸在醫(yī)療服務(wù)、餐飲服務(wù)、病毒消殺、消防救援等行業(yè)得到了廣泛的應(yīng)用。路徑規(guī)劃作為機(jī)器人技術(shù)中的重要研究領(lǐng)域，同時(shí)也是智能機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)之一，也開始迅速發(fā)展[1]。從路徑規(guī)劃角度來看，機(jī)器人的路徑規(guī)劃可以假設(shè)為已知環(huán)境信息，同時(shí)假設(shè)環(huán)境信息為靜態(tài)信息，在這兩個(gè)前提下，常用的路徑規(guī)劃算法有A*算法、人工勢(shì)場(chǎng)法、遺傳算法、蟻群算法、RRT 算法等[2-4]。其中，RRT 算法因具有泛用性好、適合移動(dòng)機(jī)器人和多自由度工業(yè)機(jī)器人等特點(diǎn)而被廣泛使用，但由于采樣的隨機(jī)性，基本RRT 算法在樹的擴(kuò)展過程中存在隨機(jī)性大、冗余節(jié)點(diǎn)多、容易在目標(biāo)點(diǎn)周圍發(fā)生振蕩、規(guī)劃的路徑較長(zhǎng)等問題。

為解決上述RRT算法所存在的問題，國(guó)內(nèi)外的研究學(xué)者對(duì)RTT算法做了許多改進(jìn)[5-8]。例如，Kuffner等[9]提出了一種雙樹同時(shí)擴(kuò)展的RRT-connect 算法，該算法利用起點(diǎn)和終點(diǎn)兩棵樹相向擴(kuò)展的方式加快了算法的擴(kuò)展站速度，減少了搜索時(shí)間，但采樣范圍大的問題依然沒有解決；張瑞等[10]提出了一種雙向動(dòng)態(tài)目標(biāo)偏置RRT*算法，加快了隨機(jī)樹的生長(zhǎng)速度，提高了搜索效率，但仍然存在算法計(jì)算量大且復(fù)雜的問題；辛鵬等[11]將RRT算法和人工勢(shì)場(chǎng)法相結(jié)合，減少了規(guī)劃路徑的長(zhǎng)度和拐點(diǎn)數(shù)，但增加了算法的計(jì)算成本；張丹萌等[12]通過混沌序列來控制采樣范圍，降低了RRT-Connect的無效采樣節(jié)點(diǎn)數(shù)，加快了算法的收斂速度，但搜索路徑仍存在大量冗余節(jié)點(diǎn)；劉奧博等[13]提出改進(jìn)的GBB-RRT*算法，通過目標(biāo)偏置策略，每次迭代兩棵隨機(jī)樹進(jìn)行雙向拓展，提高了運(yùn)行效率，但未解決隨機(jī)樹的拓展規(guī)模的問題；黃壹凡等[14]提出了RRT-connect算法，對(duì)新節(jié)點(diǎn)引入重選父節(jié)點(diǎn)的環(huán)節(jié)，同時(shí)加入動(dòng)態(tài)步長(zhǎng)策略來提高收斂速度，但找到的最終路徑依然存在許多的轉(zhuǎn)折點(diǎn)，影響實(shí)際機(jī)器人穩(wěn)定性。

上述研究均通過不同的方式對(duì)RRT 算法進(jìn)行了改進(jìn)和探索，并取得了一定的進(jìn)展。但采樣點(diǎn)隨機(jī)性大、算法復(fù)雜度高、規(guī)劃路徑冗長(zhǎng)等問題仍未得到有效解決，因此RRT算法在搜索時(shí)間、規(guī)劃路徑長(zhǎng)度、路徑節(jié)點(diǎn)數(shù)和算法收斂速度等方面仍然還有進(jìn)步空間?；谏鲜鰡栴}，本文針對(duì)RRT-Connect算法進(jìn)行改進(jìn)。通過重選隨機(jī)樹根節(jié)點(diǎn)、疊加基于目標(biāo)點(diǎn)的引力場(chǎng)、引入極度貪心思維和剪枝優(yōu)化處理等方式優(yōu)化路徑規(guī)劃過程，從而提高了規(guī)劃路徑的質(zhì)量，獲得了更優(yōu)的規(guī)劃結(jié)果。

1 RRT-Connect算法

1999 年，LAVALLE 提出了一種基于采樣的全局路徑規(guī)劃的RRT算法，但RRT算法的擴(kuò)展性具有隨機(jī)性，導(dǎo)致收斂速度慢。針對(duì)這個(gè)問題，2000年，LAVALLE和KUFFNER 提出了RRT-Connect 算法[15]，該算法分別以起點(diǎn)和終點(diǎn)為根初始化兩棵隨機(jī)樹并交替向?qū)Ψ降母?jié)點(diǎn)擴(kuò)展，同時(shí)第二棵樹在擴(kuò)展的過程中引入貪婪策略[16]，加速算法的收斂速度。

RRT-Connect 隨機(jī)樹生長(zhǎng)示意圖如圖1 所示，具體步驟如下：

圖1 RRT-Connect隨機(jī)樹生長(zhǎng)示意圖Fig.1 RRT-Connect random tree growth diagram

（1）初始化狀態(tài)空間C，標(biāo)記起始點(diǎn)、終點(diǎn)和障礙物坐標(biāo)信息，在起點(diǎn)和終點(diǎn)同時(shí)初始化兩棵隨機(jī)樹Tree1和Tree2，并在狀態(tài)空間C中生成一個(gè)隨機(jī)點(diǎn)xrand。

（2）遍歷Tree1 所有節(jié)點(diǎn)找到距離xrand最近的節(jié)點(diǎn)，記為xnearest1，從xnearest1指向xrand生長(zhǎng)一定步長(zhǎng)Step得到節(jié)點(diǎn)xnew1。判斷xnew1是否滿足狀態(tài)空間中的障礙物約束，如果不滿足，則舍棄該點(diǎn)并重新生成隨機(jī)點(diǎn)；如果滿足，則把新節(jié)點(diǎn)xnew1加入到隨機(jī)樹Tree1中。

（3）遍歷Tree2所有節(jié)點(diǎn)找到距離xnew1距離最近的節(jié)點(diǎn)，記為xnearest2，從xnearest2指向xnearest1生長(zhǎng)一定步長(zhǎng)得到xnew2。若通過碰撞檢測(cè)，則將xnew2加入到Tree2中，同時(shí)根據(jù)貪婪策略從xnew2繼續(xù)往xnew1方向生長(zhǎng)，直到遇到障礙物或者兩棵隨機(jī)樹的最近距離小于設(shè)定閾值后停止；如若遇到障礙物，則交換兩樹的次序并重復(fù)隨機(jī)采樣的過程，直至兩棵樹的距離小于連接閾值，則路徑搜索結(jié)束并返回路徑節(jié)點(diǎn)信息。

RRT-Connect 算法的雙樹擴(kuò)展相較于RRT 算法的單樹擴(kuò)展，在一定程度上縮短了規(guī)劃時(shí)間提高了搜索效率，但是在實(shí)際應(yīng)用中發(fā)現(xiàn)，RRT-Connect算法仍然存在以下缺點(diǎn)：

（1）樹擴(kuò)展的隨機(jī)性大：隨機(jī)點(diǎn)xrand的選取是由均勻函數(shù)產(chǎn)生的，導(dǎo)致新節(jié)點(diǎn)的產(chǎn)生缺少目標(biāo)性。

（2）擴(kuò)展較多的無用節(jié)點(diǎn)：Tree1在生成新節(jié)點(diǎn)xnew1后Tree2會(huì)立即向xnew1的方向進(jìn)行連續(xù)擴(kuò)展，這樣容易在擴(kuò)展的過程中產(chǎn)生許多無用的節(jié)點(diǎn)，從而使得規(guī)劃的路徑不合理。

（3）規(guī)劃的路徑有較多的冗余節(jié)點(diǎn)：由于步長(zhǎng)的限制，使得樹在擴(kuò)展的過程中每次只能擴(kuò)展固定的步長(zhǎng)，這樣最終會(huì)導(dǎo)致規(guī)劃的路徑中有許多冗余的節(jié)點(diǎn)存在。

2 改進(jìn)RRT-Connect算法

基于上述RRT-Connect 算法存在的問題，本文提出了一種同根雙向擴(kuò)展的啟發(fā)式貪心RRT 算法，相較于RRT-Connect算法主要改進(jìn)點(diǎn)為：（1）將原來的兩棵隨機(jī)樹改為一棵同根雙向擴(kuò)展的隨機(jī)樹。（2）在擴(kuò)展新節(jié)點(diǎn)時(shí)添加基于目標(biāo)點(diǎn)的引力場(chǎng)，使新節(jié)點(diǎn)更偏向于目標(biāo)點(diǎn)生長(zhǎng)。（3）新節(jié)點(diǎn)擴(kuò)展完成后引入極度貪心思想，判斷新節(jié)點(diǎn)是否可以直達(dá)目標(biāo)點(diǎn)，以減少迭代次數(shù)，提高搜索效率。（4）對(duì)最終規(guī)劃路徑進(jìn)行剪枝處理，優(yōu)化路徑長(zhǎng)度，減少路徑冗余節(jié)點(diǎn)。

2.1 根節(jié)點(diǎn)的選擇

相較于RRT-Connect 算法同時(shí)將起點(diǎn)和終點(diǎn)初始化為根節(jié)點(diǎn)，本文改進(jìn)算法選擇將根節(jié)點(diǎn)初始化在起點(diǎn)與終點(diǎn)連線的中點(diǎn)位置，若起點(diǎn)坐標(biāo)為(x1,y1)，終點(diǎn)坐標(biāo)為(x2,y2)，則根節(jié)點(diǎn)坐標(biāo)(xinit,yinit)的計(jì)算方式如式（1）與式（2）所示：

若起點(diǎn)和終點(diǎn)的連線的中點(diǎn)位置有障礙物的存在，則需要重新選擇樹的根節(jié)點(diǎn)。為了保證根節(jié)點(diǎn)向終點(diǎn)和起點(diǎn)擴(kuò)展的平衡性，新選取的根節(jié)點(diǎn)需要和起點(diǎn)與終點(diǎn)保持大致相等的距離，因此新節(jié)點(diǎn)的選取位置位于起點(diǎn)和終點(diǎn)連線的中點(diǎn)沿連線垂直方向向上或向下依次平移一個(gè)到多個(gè)單位距離。計(jì)算方法如式（3）和（4）所示：

其中，n代表沿垂直線移動(dòng)了n個(gè)單位距離，若移動(dòng)方向是起點(diǎn)和終點(diǎn)連線的正方向則n為正數(shù)，反之為負(fù)數(shù)。根節(jié)點(diǎn)選擇的示意圖如圖2所示。

圖2 根節(jié)點(diǎn)選擇示意圖Fig.2 Root node selection diagram

如圖2 所示，當(dāng)中點(diǎn)無障礙物時(shí)根節(jié)點(diǎn)為xinit1，有障礙物存在時(shí)根節(jié)點(diǎn)為xinit2。

2.2 疊加基于目標(biāo)點(diǎn)的引力場(chǎng)

關(guān)于RRT-Conect 算法在節(jié)點(diǎn)擴(kuò)展過程中隨機(jī)性大，容易搜索較多無用節(jié)點(diǎn)的問題，本文改進(jìn)算法通過在節(jié)點(diǎn)擴(kuò)展過程中疊加基于目標(biāo)點(diǎn)的引力場(chǎng)，使新節(jié)點(diǎn)偏向目標(biāo)點(diǎn)的方向生長(zhǎng)。疊加引力場(chǎng)的核心思想就是在擴(kuò)展新節(jié)點(diǎn)時(shí)除了隨機(jī)點(diǎn)xrand外，還需將起點(diǎn)或者終點(diǎn)對(duì)于節(jié)點(diǎn)xnearest的引力加入到評(píng)判指標(biāo)中，即為需要擴(kuò)展的節(jié)點(diǎn)增加一個(gè)引力函數(shù)G(n)。此時(shí)的n節(jié)點(diǎn)表示由根節(jié)點(diǎn)向外擴(kuò)展的第n個(gè)新節(jié)點(diǎn)，擴(kuò)展方式如式（5）所示：

式中，F(xiàn)(n)表示新節(jié)點(diǎn)的擴(kuò)展函數(shù)，R(n)表示新節(jié)點(diǎn)的隨機(jī)擴(kuò)展函數(shù)，G(n)表示目標(biāo)點(diǎn)對(duì)于隨機(jī)樹最近節(jié)點(diǎn)xnearest的引力函數(shù)。以向起點(diǎn)方向擴(kuò)展為例，新節(jié)點(diǎn)的隨機(jī)擴(kuò)展函數(shù)的表示方式如式（6）所示：

如式（6）和式（7）所示，擴(kuò)展新節(jié)點(diǎn)時(shí)除了考慮隨機(jī)點(diǎn)xrand外還需考慮xstar對(duì)于xnearest1的引力作用，將兩個(gè)分向量進(jìn)行矢量合成并乘以固定步長(zhǎng)即可得到新節(jié)點(diǎn)。新節(jié)點(diǎn)的擴(kuò)展方法如式（8）所示：

疊加引力場(chǎng)后可使得新節(jié)點(diǎn)逐漸向目標(biāo)點(diǎn)的方向進(jìn)行擴(kuò)展，加速了算法的收斂速度，將少了無用節(jié)點(diǎn)的擴(kuò)展數(shù)量。疊加引力場(chǎng)的新節(jié)點(diǎn)擴(kuò)展示意圖如圖3所示。

圖3 疊加引力場(chǎng)后新節(jié)點(diǎn)擴(kuò)展示意圖Fig.3 Schematic diagram of new node expansion after superimposing gravitational field

2.3 極度貪心思想的引入

節(jié)點(diǎn)擴(kuò)展的過程中加入引力場(chǎng)的影響可以引導(dǎo)新節(jié)點(diǎn)偏向目標(biāo)點(diǎn)的方向進(jìn)行擴(kuò)展，但由于擴(kuò)展步長(zhǎng)的限制，在新節(jié)點(diǎn)xnew與目標(biāo)點(diǎn)之間沒有障礙物時(shí)算法仍需迭代多次才能搜索到可行路徑，為了避免這種情況，同時(shí)為了解決隨機(jī)樹在目標(biāo)點(diǎn)附近來回震蕩的問題，本文改進(jìn)算法在節(jié)點(diǎn)擴(kuò)展中引入極度貪心思想。在隨機(jī)樹擴(kuò)展過程中引入極度貪心思想，即當(dāng)有新節(jié)點(diǎn)被加入隨機(jī)樹時(shí)，立刻將新節(jié)點(diǎn)與目標(biāo)點(diǎn)進(jìn)行連線判斷，若連線沒有跨越障礙物則證明新節(jié)點(diǎn)可直達(dá)目標(biāo)點(diǎn)，此時(shí)則結(jié)束搜索并返回搜索路徑；如果連線之間有障礙物存在，則證明新節(jié)點(diǎn)不可直接到達(dá)目標(biāo)點(diǎn)，此時(shí)則返回主程序繼續(xù)選取隨機(jī)點(diǎn)進(jìn)行采樣擴(kuò)展。引入極度貪心思想的擴(kuò)展示意圖如圖4所示。

圖4 極度貪心RRT算法示意圖Fig.4 Schematic diagram of extreme greedy RRT algorithm

如圖4所示新節(jié)點(diǎn)xnew2通過極度貪心思想與目標(biāo)點(diǎn)xgoal進(jìn)行了直接連接，提前完成了路徑搜索。依據(jù)兩點(diǎn)之間線段最短的原理，如若新節(jié)點(diǎn)與目標(biāo)點(diǎn)進(jìn)行了直連，則新節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的路徑一定為最優(yōu)路徑。新節(jié)點(diǎn)直連判斷的方式雖然在一定程度上增加了算法的計(jì)算量，但其避免了隨機(jī)樹在目標(biāo)點(diǎn)附近來回震蕩，減少了算法的迭代次數(shù)，提高了算法的收斂速度，故算法整體計(jì)算量還是得到了優(yōu)化。

2.4 剪枝優(yōu)化路徑處理

RRT-Connect 算法最終規(guī)劃的路徑均由隨機(jī)點(diǎn)組成，且由于擴(kuò)展步長(zhǎng)的限制，路徑存在許多冗余的的節(jié)點(diǎn)和轉(zhuǎn)折點(diǎn)，因此有必要對(duì)路徑進(jìn)行剪枝優(yōu)化處理。剪枝處理的方式如下：將初步路徑規(guī)劃得到的節(jié)點(diǎn)統(tǒng)一進(jìn)行處理，首先從起點(diǎn)xstar開始依次與后面的節(jié)點(diǎn)相連并判斷是否穿越障礙物，如果沒有穿越障礙物則繼續(xù)與下一節(jié)點(diǎn)進(jìn)行連線判斷；如果與某一節(jié)點(diǎn)xk的連線穿越了障礙物，則將xstar與該節(jié)點(diǎn)的前一節(jié)點(diǎn)xk－1相連并刪除它們之間的所有路徑，同時(shí)將xk－1作為起點(diǎn)重復(fù)上述操作，直至連接到終點(diǎn)。剪枝優(yōu)化前后對(duì)比圖如圖5所示。

圖5 剪枝優(yōu)化處理前后對(duì)比圖Fig.5 Comparison before and after pruning optimization

3 仿真及實(shí)驗(yàn)驗(yàn)證

為了檢驗(yàn)改進(jìn)算法的有效性，本文將利用Matlab平臺(tái)在三種不同的實(shí)驗(yàn)環(huán)境中對(duì)基礎(chǔ)RRT-Connect、文獻(xiàn)[12]改進(jìn)算法（記為ZRRT-Connect）、DRRT-Connect[17]、GB-RRT[18]和本文改進(jìn)算法進(jìn)行仿真對(duì)比實(shí)驗(yàn)。仿真環(huán)境分別為狹窄通道、復(fù)雜多障礙物環(huán)境和實(shí)驗(yàn)室二維仿真環(huán)境，地圖大小均為800×800 像素，起點(diǎn)與終點(diǎn)坐標(biāo)分別為（30，770）和（770，30）。仿真地圖中黑色代表障礙物，白色為可行區(qū)域，紫色圓點(diǎn)代表起點(diǎn)，綠色圓點(diǎn)代表終點(diǎn)，黃色原點(diǎn)為本文改進(jìn)算法的根節(jié)點(diǎn)，實(shí)驗(yàn)過程中為五種算法設(shè)置相同的擴(kuò)展步長(zhǎng)。為保證實(shí)驗(yàn)的公平性，將在同一地圖中對(duì)五種算法分別進(jìn)行30次實(shí)驗(yàn)，最后將所有實(shí)驗(yàn)結(jié)果進(jìn)行匯總并取平均值進(jìn)行分析。

3.1 狹窄通道環(huán)境仿真

在狹窄通道環(huán)境中對(duì)五種算法進(jìn)行仿真對(duì)比，仿真結(jié)果圖6所示。

圖6 狹窄通道環(huán)境仿真結(jié)果Fig.6 Simulation results for narrow aisle environments

由圖6可知，本文改進(jìn)算法相較于其余改進(jìn)算法在狹窄通道環(huán)境中有著更強(qiáng)的目的性，大量減少了無效空間的搜索范圍，且規(guī)劃的路徑更加合理。進(jìn)行30 次仿真實(shí)驗(yàn)后，將五種算法搜索時(shí)間的平均值、路徑長(zhǎng)度的平均值、路徑拐點(diǎn)個(gè)數(shù)的平均值和算法迭代次數(shù)的平均值進(jìn)行統(tǒng)計(jì)，統(tǒng)計(jì)結(jié)果如表1所示。

表1 30次實(shí)驗(yàn)平均值Table 1 Average of 30 experiments

由表1 可知，改進(jìn)算法的平均搜索時(shí)間相較于GBRRT算法下降了57.7%，平均路徑長(zhǎng)度比DRRT-Connect下降了28.9%，平均路徑拐點(diǎn)數(shù)比ZRRT-Connect下降了96.9%，迭代次數(shù)比GB-RRT 下降了89.9%，證明了改進(jìn)算法在狹窄通道環(huán)境中有著更高的規(guī)劃效率。

3.2 復(fù)雜多障礙物環(huán)境仿真

在多障礙物環(huán)境中對(duì)三種算法進(jìn)行仿真，仿真結(jié)果如圖7所示。

圖7 復(fù)雜多障礙物環(huán)境仿真結(jié)果Fig.7 Multi-obstacle environment simulation results

如圖7所示，在復(fù)雜多障礙物環(huán)境中本文改進(jìn)算法所規(guī)劃的路徑明顯更優(yōu)。將五種算法分別進(jìn)行30次實(shí)驗(yàn)，實(shí)驗(yàn)數(shù)據(jù)分析后結(jié)果如表2所示。

表2 30次實(shí)驗(yàn)平均值Table 2 Average of 30 experiments

根據(jù)表2 可知，改進(jìn)算法的平均搜索時(shí)間相較于DRRT-Connect算法下降了44.9%，平均路徑長(zhǎng)度比GB-RRT 下降了10.1%，平均路徑拐點(diǎn)數(shù)比GB-RRT 下降了91.0%，迭代次數(shù)比GB-RRT 下降了41.9%，驗(yàn)證了改進(jìn)算法在復(fù)雜多障礙物環(huán)境中路徑規(guī)劃的有效性。

3.3 實(shí)驗(yàn)室二維環(huán)境仿真

最后在實(shí)驗(yàn)室二維環(huán)境中對(duì)五種算法進(jìn)行仿真對(duì)比，仿真結(jié)果圖8所示。

圖8 實(shí)驗(yàn)室二維環(huán)境仿真結(jié)果Fig.8 Laboratory environment simulation results

由圖8 可知，改進(jìn)算法在實(shí)驗(yàn)室二維仿真環(huán)境中從中間向兩邊進(jìn)行擴(kuò)展的方式明顯好于從起點(diǎn)和終點(diǎn)向?qū)Ψ竭M(jìn)行擴(kuò)展的方式。對(duì)五種算法在該環(huán)境內(nèi)分別進(jìn)行30 次實(shí)驗(yàn)后，將實(shí)驗(yàn)數(shù)據(jù)結(jié)果整理分析后如表3所示。

表3 30次實(shí)驗(yàn)平均值Table 3 Average of 30 experiments

由表3 可知，改進(jìn)算法的平均搜索時(shí)間相較于DRRT-Connect 算法下降了81.8%，平均路徑長(zhǎng)度比DRRT-Connect 下降了25.6%，平均路徑拐點(diǎn)數(shù)比GBRRT 下降了95.7%，迭代次數(shù)比ZRRT-Connect 下降了49.8%，驗(yàn)證了改進(jìn)算法在復(fù)雜多障礙物環(huán)境中路徑規(guī)劃的有效性。證明了改進(jìn)算法在真實(shí)仿真環(huán)境內(nèi)有更強(qiáng)的實(shí)用性。

3.4 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證改進(jìn)RRT 算法在真實(shí)移動(dòng)機(jī)器人中是否可行，將改進(jìn)算法在基于阿克曼模型的移動(dòng)機(jī)器人（圖9所示）中進(jìn)行實(shí)驗(yàn)驗(yàn)證。移動(dòng)機(jī)器人搭配有激光雷達(dá)、IMU、編碼器、廣角攝像頭等傳感器，由工控機(jī)（jetsonnano）上運(yùn)行ROS（機(jī)器人操作系統(tǒng)），進(jìn)行數(shù)據(jù)融合和路徑規(guī)劃任務(wù)。

圖9 路徑規(guī)劃結(jié)果Fig.9 Path planning results

實(shí)驗(yàn)場(chǎng)地為實(shí)驗(yàn)室封閉雜亂的儲(chǔ)物間。通過鍵盤節(jié)點(diǎn)控制機(jī)器人移動(dòng)，利用Gmapping 算法構(gòu)建環(huán)境地圖，并在Rviz可視化界面中進(jìn)行顯示，自主移動(dòng)機(jī)器人如圖9（a）所示，最終導(dǎo)航路徑結(jié)果如圖9（b）所示。

4 結(jié)束語

本文針對(duì)RRT-Connect 算法搜索時(shí)間長(zhǎng)、節(jié)點(diǎn)擴(kuò)展缺乏目標(biāo)性和規(guī)劃路徑冗余節(jié)點(diǎn)多等問題，提出了基于同根雙向擴(kuò)展的貪心RRT算法。首先將兩棵同時(shí)擴(kuò)展的隨機(jī)樹改為一棵同根雙向擴(kuò)展的隨機(jī)樹，降低了計(jì)算量；其次為新節(jié)點(diǎn)的擴(kuò)展疊加了基于目標(biāo)點(diǎn)的引力場(chǎng)，使新節(jié)點(diǎn)的擴(kuò)展更具有導(dǎo)向性；同時(shí)在擴(kuò)展新節(jié)點(diǎn)后加入極度貪心思想，加速了算法的收斂速度；最后對(duì)規(guī)劃的路徑進(jìn)行了剪枝優(yōu)化處理，降低了路徑節(jié)點(diǎn)數(shù)，縮短了路徑長(zhǎng)度。通過三種不同環(huán)境的仿真對(duì)比實(shí)驗(yàn)，證明了本文改進(jìn)算法規(guī)劃的路徑在效率和質(zhì)量上均具有更優(yōu)性。未來的研究中將會(huì)把動(dòng)態(tài)障礙物的影響加入到算法改進(jìn)的考慮中，以提高算法的真實(shí)性和實(shí)用性。