基于雙向同時無碰撞檢測目標偏置RRT算法的路徑規劃方法

陳海洋， 王露楠

(西安工程大學電子信息學院，西安，710048)

隨著人工智能、物聯網、5G等創新技術的發展，傳統的倉儲技術與生產線已經不能滿足現在的環境需求，為了實現物流倉儲的智能化和生產線的柔性化，各大企業紛紛嘗試將智能機器人(automated guided vehicle，AGV)投入到物流倉儲和生產車間中。為了使AGV能在復雜的環境中更加安全地運行，應針對不同環境對AGV進行路徑規劃，路徑規劃算法的目標是為移動機器人在障礙物地圖中規劃出一條高效可行的路徑，該路徑對移動機器人導航效果有著至關重要的作用。如何改進現有算法使其更適用于物流倉儲和柔性化生產線是當前生產制造業向智能化轉變面臨的巨大挑戰[1]。

目前，AGV路徑規劃算法的改進多在A*算法[2]、Dijkstra算法[3]、蟻群算法[4]、粒子群算法[5]、遺傳算法[6]、人工勢場法[7]等，也有學者對這些智能算法進行改進，比如樊志凱等[8]提出基于多目標的遺傳模擬退火算法，雖然在簡單環境下可以快速得到較優路徑，但是在復雜環境下，收斂速度慢，規劃路徑需要較長時間。為了解決復雜環境下機器人的路徑規劃問題，概率路圖(probabilistic road map, PRM)[9]和快速搜索隨機樹(rapidly-exploring random trees, RRT)[10]等基于采樣的路徑規劃算法被提出。PRM將連續空間分割成若干個離散空間，然后再利用其他算法尋找路徑，由于分割離散空間過程的隨機性，使得PRM搜索路徑的效率不高。針對PRM路徑規劃的不足，Lavalle等[10]提出了同樣基于采樣的RRT算法，該算法雖然具有概率完備性，但是由于只能通過一棵樹擴展路徑，尋路時間過長。

針對RRT算法的不足，Kuffner等[11]于2000年提出了RRT-Connect算法，該算法通過在起點和目標點建立兩棵樹擴展路徑，并引入貪婪策略減少尋路時間。Karaman等[12]于2010年提出RRT*算法，該算法通過重選父節點減小時間代價，但隨著采樣點的增加，會使RRT*算法計算量暴增。Jordan等[13]受RRT-Connect的啟發，于2013年提出雙向RRT*(bidirectional rapidly-exploring random trees, B-RRT*)，在RRT*的基礎上，建立兩棵樹進行擴展，搜索速度加快，但在復雜環境下節點的計算量較大。同年，Janson等[14]提出利用堆棧技術選取合適的節點作為生長節點，同時采用碰撞檢測策略避免與障礙物的碰撞，但該方法不適用于動態環境。在此基礎上，趙燕江等[15]于2017年在RRT算法中引入直線段路徑，采用直線、曲線結合的路徑形式，使得生成的路徑較為平滑。Jonathan等[16]于2020年提出使用采樣和啟發式方法交替逼近和搜索問題域(batch informed trees, BIT*)，有效地解決了連續路徑規劃問題。針對機器人在復雜環境(如凹形、迷宮等)的特殊情況，鄒啟杰等[17]提出了基于強化學習的快速探索隨機樹(reinforce-ment learning rapidly-exploring random trees, RL-RRT)，許萬等[18]提出了基于簡化地圖的區域采樣RRT*算法(regional sampling based on simplified map rapidly-exploring random trees, SMRS-RRT*)，張衛波等[19]提出基于RRT的——同心圓RRT算法，李秀智等[20]提出了一種將RRT與前沿法協同實施的復合式候選目標點檢測策略，使移動機器人沿著該優化路徑快速、平穩、安全地到達目標點。當障礙物越多、環境越復雜時，上述研究將大部分時間都花費在尋找終點上，尋找終點所用的時間越長，尋路的計算代價就越高，算法效率也大幅降低。針對現有算法不能同時滿足尋路時間少及尋路代價低的情況，本文考慮到B-RRT*的快速性與BIT*的尋優性，提出了一種雙向同時無碰撞檢測目標偏置快速擴展隨機樹算法(two-way simultane-ous non-collision goal based RRT*, TNCG-RRT*)，首先，將B-RRT*中的雙向搜索策略與BIT*算法中的啟發式搜索相融合，在此基礎上對RRT算法進行改進：引入神經網絡的批量抓取數量決定一次采樣的節點數目，增加或減少每次采樣的節點數目影響路徑的搜索速度；其次，在雙向搜索策略的基礎上，使正向樹與反向樹的擴展同時進行，加快隨機樹的搜索速度；并通過改變目標偏向策略中的擴展頂點隊列和不斷更新采樣區域明確擴展方向，縮小隨機樹的生長范圍；然后，采用3次B樣條曲線對生成的路徑進行優化，使其更為平滑；最后，通過仿真實驗驗證了在一定的復雜環境中，TNCG-RRT*算法進行路徑規劃的有效性。雖然其適用性尚未得到驗證，但利用TNCG-RRT*算法進行路徑規劃能夠在一定程度上有效降低機器人的尋路代價與成本，實現控制其按最優路徑行駛的目的。

1 基礎算法

利用B-RRT*的快速性與BIT*的尋優性作為基礎算法，提出一種雙向同時無碰撞檢測目標偏置快速擴展隨機樹算法——TNCG-RRT*，在提高路徑規劃效率的前提下解決采樣點隨機性大、路徑不平滑等問題。

1.1 B-RRT*算法基本原理

B-RRT*算法在原始RRT*算法的基礎上，額外增加了基于RRT-Connect算法中的connect()函數，通過起點和終點各生成一個樹的方式，加速隨機樹的搜索速度，既具有概率完備性，又具備漸進最優性。圖1為算法B-RRT*算法流程圖，只要迭代的次數足夠多，得到的最終路徑就可以是較為接近最短的可行路徑。

圖1 B-RRT*算法流程圖

1.2 BIT*算法基本原理

BIT*在B-RRT*的基礎上加入潛在解代價排序的隊列，該算法只根據潛在的解決方案質量對整個搜索進行排序，按潛在解質量的順序即搜索隊列QV搜索目標點，改善當前的路徑方案。算法BIT*流程圖見圖2。

圖2 BIT*算法流程圖

搜索隊列QV根據函數f(x)=g(x)+h(x)確定，它可以提供比當前最佳解決方案成本更好的解決方案，其中，g(x)是通過當前樹從開始到x的成本，h(x)是一個啟發式函數，代表從狀態x到目標的允許估計值，它引導樹探索到目標上，我們使用x和目標之間的距離來估計它。在函數f(x)確定的隊列中獲得最低的估計成本，將該函數確定的成本較低邊、頂點加入至搜索隊列中，以獲得較優路徑，并將此時的成本代價記為c(x)，繼續搜索，直至搜索隊列中沒有可以改善當前路徑的解即f(x)

2 TNCG-RRT*

針對B-RRT*和BIT*仍然存在搜索時間長、采樣點隨機性大和路徑不平滑等問題，本文提出了雙向同時無碰撞檢測目標偏置快速擴展隨機樹算法——TNCG-RRT*，通過引入神經網絡中的批量抓取數量[21]和雙向同時搜索加快路徑搜索速度，利用目標偏向策略和不斷更新橢圓狀態子集縮小隨機樹的搜索范圍，最后用B樣條曲線對路徑進行優化。TNCG-RRT*流程圖見圖3。

圖3 TNCG-RRT*算法流程圖

2.1 采樣

2.1.1 引入神經網絡中批量抓取數量

當機器人在簡單環境中進行路徑規劃時，全環境下多數采樣點確定的方向能夠更好地代表目標點所在的方向，但當機器人在有大量障礙物的復雜環境中進行路徑規劃時，全環境下的采樣點會影響確定目標點所在的方向和速度。此時引入神經網絡中的批量抓取數量，其值決定一次采樣的節點數目，繼而影響路徑的搜索速度。

圖4為在同種環境中，經過200次迭代后，批量抓取數量取不同值時的實驗結果，表1為同種環境下批量抓取數量不同取值時經過200次迭代后的運行代價的對比。由表1可以看出，當批量抓取數量值過小時，采樣點的穩定性較差，難以收斂導致代價變大；當批量抓取數量值過大時，進行一次采樣的節點數目過多導致所需內存容量變大，代價升高。圖5為當批量抓取數量值恒為1時，經過200次迭代后，在4種不同復雜度場景中的實驗結果，表2為批量抓取數量恒定，但場景復雜度不同時的運行代價。當場景簡單時，如圖5(a)所示，搜索代價明顯較小，但當場景逐漸復雜時，如圖5(b)～(d)所示，搜索代價有明顯的提升。因此，針對場景復雜程度選擇一個適當的批量抓取數量值可以減少完成代價，有效提升尋路效率。

圖4 同種環境下批量抓取數量取不同值時的實驗結果

表1 同種環境批量抓取數量取不同值時的運行結果對比

表2 批量抓取數量恒定,場景不同時的運行結果對比

圖5 批量抓取數量恒定，場景不同時的實驗結果

在確定批量抓取數量的值時，可以一次性讓路徑搜索利用全部樣本點(即傳統的梯度下降法)，也可以讓路徑搜索一次只采用一個樣本點(即隨機梯度下降法，也稱在線梯度下降法)，本文提出的算法采取折中方案，即每次采取一部分樣本點讓其完成本輪迭代(即batch梯度下降法)。

2.1.2 目標偏向策略

機器人在使用B-RRT*和BIT*算法進行路徑規劃時，隨機樹在環境中進行自由采樣時，采樣范圍大且隨機性強，具有一定的盲目性，對此，本文根據目標采樣概率確定采樣點如式(1)所示:

(1)

在初始化參數時設定目標采樣概率Paim=0.5，按照均勻概率分布隨機獲取一個概率值P(0

圖6 目標偏向策略

2.1.3 雙向同時搜索

B-RRT*和BIT*算法在迭代時，先擴展隨機樹Ta，對Ta進行一系列采樣后，在下次迭代時才會對隨機樹Tb進行擴展，在復雜環境中進行路徑規劃時搜索時間過長。針對上述問題，本文在每次迭代時，將隨機樹Ta、Tb同時進行擴展，雙向同時搜索簡圖如圖7所示，改進后的算法在起點和終點同時搜索將大大縮短節點搜索時間，提升路徑搜索效率。

圖7 雙向同時搜索

2.2 路徑優化

當機器人在有大量障礙物的復雜環境中進行路徑規劃時，會生成較多路徑節點，其中一大部分為冗余樣本點。隨著冗余樣本點的增多，使得路徑搜索速度逐漸變慢，因此，為了提高路徑搜索效率，使搜索出的路徑更符合機器人運動軌跡，需要對無效路徑進行剪枝并使最終路徑趨于平滑。

2.2.1 橢圓狀態子集

雖然在2.1.1中提出引用批量抓取數量來決定每次采樣的節點數，但是當批量抓取數量值偏大時，會導致該次采樣在整個狀態空間內進行，無效樣本點可能增多，針對該問題，本文利用不斷更新采樣空間的方法進行采樣。

圖8 橢圓采樣空間

2.2.2 基于B樣條曲線的路徑平滑

圖9為簡化路徑與優化路徑的對比圖，圖 9(a)中紅色實線為初始可行路徑，圖9(b)中黃色實線為經過3次B樣條曲線擬合生成的優化路徑，經過平滑處理過的路徑明顯更符合現實中機器人的行動軌跡。

圖9 簡化路徑與優化路徑對比圖

3 仿真試驗及對比分析

為了驗證TNCG-RRT*在路徑規劃中具有更好的性能，本文采用Python3.7作為仿真平臺，將本文算法與基本RRT、RRT-Connect、RRT*、FMT*、B-RRT和BIT*算法在二維仿真環境下進行對比驗證。仿真中，設置步長為1，搜索半徑為5，最大迭代次數為1 000次，重復實驗30次，二維仿真環境如圖10所示，起點坐標為(2,2)，終點坐標為(43,43)，黑色區域為障礙物。7種算法在簡單地圖的仿真結果見圖11，可以看出，RRT、RRT-Connect、RRT*、FMT*和B-RRT算法由于采樣隨機性過大，樹的生長較為發散，生成的路徑都比較曲折，雖然RRT-Connect、FMT*、B-RRT*和BIT*都加入了回溯過程，但這4種算法仍然不能生成較為平滑的路徑，本文提出的TNCG-RRT*算法采用更新橢圓采樣空間進行路徑優化，剪除冗余枝，使隨機樹擴散具有一定的方向性，從而達到路徑漸進最優的效果。這7種算法30次實驗結果平均值如表3所示，在相同場景下，RRT、RRT-Connect、RRT*、FMT*和B-RRT*由于迭代次數過多，需要大量時間才能找到路徑，且尋路代價較高，BIT*雖然也可以在較短時間內找到可行路徑，但相比之下，TNCG-RRT*尋路代價更小，所用時間更短，生成的最終路徑也更為平滑。

圖10 二維仿真環境

圖11 7種算法在簡單地圖中的仿真結果

表3 7種算法實驗結果平均值

上述實驗中，RRT、RRT-Connect、RRT*、FMT*和B-RRT需要經過較長時間才能找到相對較優路徑，BIT*與TNCG-RRT*尋路時間短效果較好，因此，在其他環境下對這2種算法再次進行30次重復試驗進行對比。

圖12為設定的規則障礙物環境下2種算法的結果對比，該環境的起點坐標為(2,2)，終點坐標為(90,95)，黑色區域為障礙物，綠色實線為搜索路徑，紅色實線為最終路徑，黃色虛線包圍的區域為橢圓狀態子集，紫色虛線包圍的區域為某次更新后的橢圓狀態子集。圖13～14為BIT*算法和TNCG-RRT*算法在規則障礙物環境中生成最終路徑的代價和尋路時間。從圖12可以看出，BIT*算法的隨機樹向碰撞概率小的區域擴展，但生成的樹枝仍然較為發散，而TNCG-RRT*通過不斷更新橢圓采樣空間和明確隨機樹生長方向，減少對不必要區域的搜索，經過3次B樣條曲線的平滑處理，規劃出的路徑質量更高。從圖13～14中可以看出，在30次仿真實驗中，大部分情況下TNCG-RRT*尋路時間較BIT*短，且由于目標采樣概率的存在以及采樣區域的不斷縮小，TNCG-RRT*在路徑代價上具有明顯優勢。

圖12 2種算法規則障礙物環境規劃結果對比

圖13 2種算法30次實驗的路徑代價對比

圖14 2種算法30次實驗的尋路時間對比

其30次實驗結果平均值如表4所示：與BIT*算法相比，TNCG-RRT*在剪枝數上增加80%，在路徑代價上減少9%，在尋路時間上減少4.5%。在兩種不同環境下，TNCG-RRT*與其他算法相比，擴展節點與搜索時間明顯減少，尋路代價也明顯降低，說明了減少不必要區域搜索的必要性與對路徑進行光滑處理的有效性。

表4 2種算法30次實驗結果平均值

4 結語

本文針對現有基于采樣的算法在復雜環境中進行路徑規劃時存在對不必要區域搜索過多和時間代價過高等問題，提出了雙向同時無碰撞檢測目標偏置快速擴展隨機樹算法——TNCG-RRT*，利用B-RRT*的快速性與BIT*的尋優性，將B-RRT*中的雙向搜索策略與BIT*算法中的啟發式搜索相融合作為基礎算法，引入神經網絡的批量抓取數量加快路徑搜索速度，通過雙向同時搜索、擴展頂點策略和不斷更新橢圓狀態子集明確隨機樹的擴展方向，縮小隨機樹的生長范圍，最后采用3次B樣條曲線對生成的路徑進行平滑處理。通過在2種仿真環境下與其他算法進行對比實驗，實驗結果表明TNCGB-RRT*在尋路時間、尋路代價等方面均優于其他算法，證明了算法的有效性。雖然該算法的適用性尚未得到驗證，但利用TNCG-RRT*算法進行路徑規劃能夠有效降低機器人的尋路代價與成本，可廣泛應用于無人車、無人機、無人船等自動駕駛交通工具中，以實現控制其按最優路徑行駛的目的。