基于改進BI-RRT算法的AGV路徑規劃

敖國鑫 李林

摘? 要： 針對雙向快速擴展隨機樹（BI-RRT）算法在路徑規劃中存在收斂速度慢、路徑繞遠、轉折點多等問題，提出一種改進的BI-RRT算法。首先引入可變權重系數實現目標導向的同時又能較快的避開障礙物；然后對生成路徑作基于障礙物判斷策略的剪枝優化；最后用三次B樣條曲線進行平滑處理，并提出“優化域”概念解決路徑碰撞障礙物問題。仿真結果表明，本文改進后的算法收斂速度更快、路徑更加平滑、長度減少。

關鍵詞： 路徑規劃； BI

0 引言

近年來，自動導引車AGV以其高自動化和便捷性在工業生產中得到廣泛應用。優秀的路徑規劃算法對AGV的工作效率有著重要影響。傳統的路徑規劃算法主要有基于搜索和采樣兩大類。基于搜索的一般有蟻群算法[1]、A-star[2]、遺傳算法[3]等，基于采樣的有概率線路圖算法（PRM）[4]、快速拓展隨機樹（RRT）[5]等。其中，RRT算法無需對空間障礙物進行預處理、搜索速度快，在相對復雜的空間有比較廣泛的應用[6]。但是RRT算法也有其不足之處，如繞路現象嚴重，隨機盲目性強、易陷入局部最優等[7]。

針對RRT算法存在的問題，國內外很多學者作了大量研究，提出了不同的方法來改進。Kuffner Jr[8]等人提出雙向生長隨機樹（Bi-RRT）算法，即在起點和終點同時擴展兩棵搜索樹，減少了路徑規劃的時間；阮曉鋼[9]等人提出基于子目標搜索的目標導向算法，減少了RRT算法的冗余搜索；李磊[10]等在BI-RRT算法中引入貪婪算法思想，使得樹盡可能朝目標點生長，大大縮短了路徑長度；孫欽鵬等提出可變步長的方法，優化新節點的選擇方式，實現了在復雜環境下的路徑優化。除了對算法本身的研究，也有不少學者將RRT算法與其他算法融合進行改進，施楊洋[12]等將人工勢場法的目標引力思想引入RRT算法的節點擴展中，減少了樹的隨機性；王雨[13]等對RRT算法規劃出的路徑利用三次B樣條曲線作平滑處理，增加路徑的連續性。

針對BI-RRT算法存在的強隨機性、算法收斂速度慢、路徑拐點多的問題，本文提出相應的改進優化方法。首先，引入可變權重系數，保證目標導向能力的同時優化避障能力；然后，對生成路徑作剪枝優化來剔除冗余點，減少路徑長度。最后，對剪枝后的路徑用3次B樣條曲線作平滑處理，針對3次B樣條曲線平滑后的路徑存在碰撞障礙物的情況，提出“優化域”概念，增加路徑的平滑性和連貫性。通過MATLAB對本文算法進行仿真，驗證算法的有效性。

1 基本BI-RRT算法

RRT算法以起點Kint為根節點定義一顆隨機樹，通過提前給定的約束條件在狀態空間內進行隨機樹的擴展。雙向快速拓展隨機樹（BI-RRT）相較于RRT算法，在節點拓展策略上引入雙樹拓展的環節，在起點和終點分別定義隨機樹X和隨機樹Y，隨機樹X以Kgoal為目標點作隨機生長，隨機樹Y以Kint為目標點作隨機生長，d為每次生長的步長。通過一次采樣得到隨機樹X的隨機點Krand，找到距Krand歐氏距離最小點Knear，朝Krand方向生長步長d得到新節點Knew，然后檢測Knew是否碰撞到障礙物，若是，則將這個采樣點舍去，若不是，則將Knew添加到隨機樹中。隨機樹Y也執行此操作，兩顆搜索樹交替進行擴展，直到檢測到兩棵樹的新節點的歐氏距離小于設定的閥值或者相遇時，便連接兩個點生成隨機樹，路徑也由此生成。

BI-RRT擴展示意圖如圖1所示。BI-RRT算法也存在一些缺點：隨機樹的擴展是基于隨機采樣點的，采樣點的隨機性和偏差性導致兩棵樹難以連接，算法收斂速度較低。算法的無方向性，容易陷入局部極小值問題，生成大量不必要的節點進而發生繞遠現象。生成的路徑轉折較大、連貫性不足。

2 改進Bi-RRT算法

2.1 可變權重系數

原本的RRT算法由于沒有融入目標節點的導向思想，節點隨機生成，導致了隨機樹的無向擴展且有大量的冗余節點，降低了算法收斂速度和路徑質量。針對隨機樹的無方向性，部分研究提出引入固定目標偏置權重系數，核心思想是在目標點和隨機點兩個方向取固定權重系數P1和P2，通過仿真來確定系數之后再做矢量計算來確定隨機樹的拓展方向。該方法改變傳統RRT算法隨機樹生長的無方向性，使得節點生長的同時逐漸向目標節點靠近，但是這一方法在復雜環境（如迷宮環境）下容易陷入局部最優產生大量無效節點。因此，針對這一問題，本文引入可變導向系數目標偏向策略，即在目標點方向和隨機點方向加入互相牽制的權重系數[β]和[1-β]，加入可變權重系數之后，新節點[Knew]的生成方式采用以下公式確定：

[Knew=Knear+β（Kgoal-Knear）Kgoal-Knear+（1-β）（Krand-Knear）Krand-Knear] ⑴

其中，[β]（0<[β]<1）代表權重系數，當[β]>0.5時，[Knew]將偏向于目標點方向；當[β]<0.5時，[Knew]將偏向于隨機方向引入可變權重系數之后，系數的選取影響著新節點[Knew]的確定。在搜索過程中，根據障礙物信息不斷調整[β]值大小，當[Knew]與[Knear]的連線上沒有檢測到障礙物碰撞時，選取 [β ]>0.5，此時新節點將偏向目標點方向；反之，當出現障礙物碰撞時，選取[ β ]<0.5，新節點將偏向隨機點方向，此時的RRT算法與傳統的RRT算法相似，具有較強的避障能力和隨機性。如圖2所示，權重系數[β]的調整實現了算法法目標導向的同時又保持了RRT算法本身的避障能力。

2.2 基于障礙物判斷的剪枝策略

經上述優化之后，BI-RRT算法的收斂速度、路徑質量得到提高，但是生成的路徑依然有較多的冗余節點，冗余節點會導致AGV的運行出現繞遠現象。對此，本文提出基于障礙物判斷的剪枝策略，對路徑進行剪枝處理（見圖3），具體實施策略解釋如下：

⑴ 提取路徑節點。在初始路徑上從起始節點到目標節點按順序依次記為Cn（n=1，2，3…n），C1為起始節點首先加入到路徑節點列表path_tree（）中。

⑵ 障礙物判斷。依次判斷起始節點C1與路徑節點Cn之間的直線路徑上是否存在障礙物。

⑶ 對障礙物判斷結果進行處理。判斷結果分為兩種情況。情形一，如果起始點與節點Cn的直線路徑上檢測到障礙物，則無需對節點Cn后面的節點進行障礙物判斷，剔除C1與Cn-1之間的節點，連接起始點C1和Cn-1生成一條路徑，將Cn-1作為作為新的起始點，進行新的障礙物判斷操作。

⑷ 情形二，如果障礙物判斷一直到目標點都沒有檢測到障礙物，則直接連接起始點與目標點生成最終路徑。

2.3 三次B樣條曲線優化

AGV能夠平穩運行的前提是路線平滑，過多的轉折點不僅會增加AGV工作時間，還會增加能量消耗。當AGV運行到轉彎節點時通常需要停下來、轉彎、起步、加速的動作，這些動作增加了AGV運行時間、降低了運行效率。為了讓AGV能夠在規劃出來的路徑上平穩運行，本文用三次B樣條曲線對改進后的路徑進行擬合。3次B樣條曲線表達式為：

[Pis=j=103Bj，3（s）Ci+j]? ⑵

其中，s為參數，[s∈0，1，Bj，3（s）]為三次B樣條基函數；[Ci+j]為第[i]段中的第[j]個控制點。

3次B樣條的基本函數的矩陣形式為：

[Bj，3s=16s3s2? s? 1G]? ⑶

其中，

[G=13-313-630-30301410]

代入式⑶，可求得三次B樣條曲線的基函數：

[b0=16-k3+3k2-3k+1b1=163k2-6k+4b2=16（-3k3+6k2+3k+1）b3=16k3]

其中，k表示節點，[b0]、[b1]、[b2]、[b3]表示基函數。

但是，在復雜障礙物情況下，若直接采用三次B樣條曲線作路徑平滑處理，容易出現平滑出的路徑穿過障礙物的情況，如圖4所示。

為了避免AGV碰撞障礙物的情況發生，本文提出“優化域”概念，即在轉折點前后相同距離處設置兩個端點，端點離轉折點的距離值根據實際情況調節。如圖5所示，設置M1，M3端點，[M1M2=M2M3]，這兩個端點形成的區域就是三次B樣條曲線的優化區域，在優化域內進行平滑優化，這樣就避免了平滑出的路線相較于原始路線出現太大的變化導致路徑穿過障礙物的情況。

3 算法仿真與驗證

為了驗證改進后的BI-RRT算法有更好的性能和效率，通過仿真將改進算法與其他RRT相關的算法做對比分析，仿真硬件為MacBookAir（M1，2020），計算機配置：8GB統一內存，256GB固態硬盤，M1芯片，8核CPU，macOSMonterey 12.5系統。

3.1 二維仿真分析

由于BI-RRT算法在路徑規劃時存在隨機性，因此建立三種不同類型的地圖（簡單環境，復雜障礙物環境、迷宮環境），在三種類型地圖中分別對RRT，BI-RRT和本文改進后的BI-RRT算法進行實驗，地圖大小為500*500，起點坐標為（10，10），終點坐標為（490，490），每次拓展步長d=20，最大節點數量為3000，每種算法各在三種類型的地圖下運行50次，記錄每次運行的數據。

從圖6～圖8可以看出，在三種不同類型的障礙物環境下，從路徑質量來看，本文改進的BI-RRT算法規劃出來的路徑更符合AGV運行，主要表現為轉彎次數明顯減少，路徑更加平滑。表1～表3是三種障礙物環境下50次仿真實驗記錄的各項數據的平均值。

在地圖1的環境下，障礙物分布比較稀疏。由表1可知，本文改進的BI-RRT算法相比于RRT算法，在平均采樣點數上減少52.4%，平均路徑長度減少24.9%，平均規劃時間減少59.2%；相較于BI-RRT，均采樣點數上減少45.4%，平均路徑長度減少17.4%，平均規劃時間減少26.1%。

在地圖2的地圖環境下，地圖障礙物分布比較密集，通道比較狹窄。由表2可知，本文改進的BI-RRT算法相比于RRT算法，在平均采樣點數上減少43.6%，平均路徑長度減少19.4%，平均規劃時間減少52%；與BI-RRT相比，均采樣點數減少37.9%，平均路徑長度減少14.5%，平均規劃時間減少34.4%。

在地圖3的地圖環境下，障礙物呈迷宮狀，有多個復雜空間和陷阱區域，算法易在復雜區域震蕩而無法收斂。由表3可知，本文改進的BI-RRT算法相比于RRT算法，在平均采樣點數上減少36.7%，平均路徑長度減少30%，平均規劃時間減少58.2%；相較于BI-RRT，均采樣點數上減少30%，平均路徑長度減少25.2%，平均規劃時間減少30%。

4 結束語

本文針對雙向RRT算法的缺點：隨機性強、收斂速度慢、路徑不連續等問題，提出一種基于雙向RRT算法改進的AGV路徑規劃。在BI-RRT算法的基礎上引入可變權重系數，實現目標導向和避障能力的優化，同時提高了算法收斂速度；對生成路徑作剪枝優化剔除冗余點，縮短路徑長度；對優化后的路徑用3次B樣條曲線作平滑處理，增加路徑的平滑性和連貫性，針對路徑碰撞障礙物問題，提出“優化域”概念，實現避障。MATLAB仿真數據表明，本文改進的算法在平均采樣點數、平均路徑長度及平均規劃時間三個維度上都有明顯的降低，規劃出來的路徑連續性更強、轉角更小，證明了本文算法的優越性和實用性。

參考文獻（References）：

[1] 王明輝.基于改進多步長蟻群算法的機器人路徑規劃[J].機床與液壓，2022，50（15）：43-46

[2] 沈克宇，游志宇，劉永鑫，等.基于改進A*算法的移動機器人路徑規劃[J].計算機應用研究，2022，8：1-6

[3] 楊博，劉樹東，魯維佳，等.改進遺傳算法在機器人路徑規劃中的應用[J].現代制造工程，2022（6）：9-16

[4] 李瓊瓊，徐溢琪，布升強，等.基于修正PRM算法的智能車輛路徑規劃研究[J].森林工程，2022（5）：179-186

[5] 陳法法，蔣浩，李振，等.基于改進RRT的智能巡檢機器人狹窄通道路徑規劃[J].組合機床與自動化加工技術，2022（10）：40-45

[6] 張衛波，肖繼亮.改進RRT算法在復雜環境下智能車路徑規劃中的應用[J].中國公路學報，2021，34（3）：225-234

[7] 李揚，張蕾，李鵬飛，等.基于改進RRT結合B樣條的機械臂運動規劃方法[J].計算機集成制造系統，2022，10：1-17

[8] LAVALLE S M，KUFFNER J.Randomized kinodynamicplanning[J].The International Journal of Robotics and Research，1999，15（5）：378－400

[9] 阮曉鋼，周靜，張晶晶，等.基于子目標搜索的機器人目標導向RRT路徑規劃算法[J].控制與決策，2020，35（10）：2543-2548

[10] 李磊，吳翔，包祥威，等.基于改進RRT-Connect算法的機械臂抓取路徑規劃[J].機器人技術與應用，2021（2）：28-32

[11] 孫欽鵬，李猛，王中華.基于改進快速擴展隨機樹算法的移動機器人路徑規劃[J].濟南大學學報（自然科學版），2019，33（5）：431-438

[12] 施楊洋，楊家富，布升強，等.基于RRT改進的智能車輛路徑規劃算法[J].計算技術與自動化，2019，38（4）：81-86

[13] 王雨，劉延俊，賈華，等.基于強化RRT算法的機械臂路徑規劃[J].山東大學學報（工學版），2022：1-9-RRT； 可變權重系數； 剪枝； 三次B樣條