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

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

2.山東省智能建筑技術(shù)重點實驗室，濟南250101

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

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

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

1 RRT-Connect算法

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

RRT-Connect 隨機樹生長示意圖如圖1 所示，具體步驟如下：

圖1 RRT-Connect隨機樹生長示意圖Fig.1 RRT-Connect random tree growth diagram

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

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

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

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

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

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

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

2 改進RRT-Connect算法

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

2.1 根節(jié)點的選擇

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

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

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

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

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

2.2 疊加基于目標(biāo)點的引力場

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

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

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

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

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

2.3 極度貪心思想的引入

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

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

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

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

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

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

3 仿真及實驗驗證

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

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

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

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

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

表1 30次實驗平均值Table 1 Average of 30 experiments

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

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

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

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

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

表2 30次實驗平均值Table 2 Average of 30 experiments

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

3.3 實驗室二維環(huán)境仿真

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

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

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

表3 30次實驗平均值Table 3 Average of 30 experiments

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

3.4 實驗驗證

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

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

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

4 結(jié)束語

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