基于改進(jìn)BI-RRT算法的AGV路徑規(guī)劃

敖國鑫 李林

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

關(guān)鍵詞： 路徑規(guī)劃； BI

0 引言

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

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

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

1 基本BI-RRT算法

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

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

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

2.1 可變權(quán)重系數(shù)

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

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

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

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

經(jīng)上述優(yōu)化之后，BI-RRT算法的收斂速度、路徑質(zhì)量得到提高，但是生成的路徑依然有較多的冗余節(jié)點(diǎn)，冗余節(jié)點(diǎn)會(huì)導(dǎo)致AGV的運(yùn)行出現(xiàn)繞遠(yuǎn)現(xiàn)象。對(duì)此，本文提出基于障礙物判斷的剪枝策略，對(duì)路徑進(jìn)行剪枝處理（見圖3），具體實(shí)施策略解釋如下：

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

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

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

⑷ 情形二，如果障礙物判斷一直到目標(biāo)點(diǎn)都沒有檢測(cè)到障礙物，則直接連接起始點(diǎn)與目標(biāo)點(diǎn)生成最終路徑。

2.3 三次B樣條曲線優(yōu)化

AGV能夠平穩(wěn)運(yùn)行的前提是路線平滑，過多的轉(zhuǎn)折點(diǎn)不僅會(huì)增加AGV工作時(shí)間，還會(huì)增加能量消耗。當(dāng)AGV運(yùn)行到轉(zhuǎn)彎節(jié)點(diǎn)時(shí)通常需要停下來、轉(zhuǎn)彎、起步、加速的動(dòng)作，這些動(dòng)作增加了AGV運(yùn)行時(shí)間、降低了運(yùn)行效率。為了讓AGV能夠在規(guī)劃出來的路徑上平穩(wěn)運(yùn)行，本文用三次B樣條曲線對(duì)改進(jìn)后的路徑進(jìn)行擬合。3次B樣條曲線表達(dá)式為：

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

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

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

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

其中，

[G=13-313-630-30301410]

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

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

其中，k表示節(jié)點(diǎn)，[b0]、[b1]、[b2]、[b3]表示基函數(shù)。

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

為了避免AGV碰撞障礙物的情況發(fā)生，本文提出“優(yōu)化域”概念，即在轉(zhuǎn)折點(diǎn)前后相同距離處設(shè)置兩個(gè)端點(diǎn)，端點(diǎn)離轉(zhuǎn)折點(diǎn)的距離值根據(jù)實(shí)際情況調(diào)節(jié)。如圖5所示，設(shè)置M1，M3端點(diǎn)，[M1M2=M2M3]，這兩個(gè)端點(diǎn)形成的區(qū)域就是三次B樣條曲線的優(yōu)化區(qū)域，在優(yōu)化域內(nèi)進(jìn)行平滑優(yōu)化，這樣就避免了平滑出的路線相較于原始路線出現(xiàn)太大的變化導(dǎo)致路徑穿過障礙物的情況。

3 算法仿真與驗(yàn)證

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

3.1 二維仿真分析

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

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

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

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

在地圖3的地圖環(huán)境下，障礙物呈迷宮狀，有多個(gè)復(fù)雜空間和陷阱區(qū)域，算法易在復(fù)雜區(qū)域震蕩而無法收斂。由表3可知，本文改進(jìn)的BI-RRT算法相比于RRT算法，在平均采樣點(diǎn)數(shù)上減少36.7%，平均路徑長度減少30%，平均規(guī)劃時(shí)間減少58.2%；相較于BI-RRT，均采樣點(diǎn)數(shù)上減少30%，平均路徑長度減少25.2%，平均規(guī)劃時(shí)間減少30%。

4 結(jié)束語

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

參考文獻(xiàn)（References）：

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

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

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

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

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

[6] 張衛(wèi)波，肖繼亮.改進(jìn)RRT算法在復(fù)雜環(huán)境下智能車路徑規(guī)劃中的應(yīng)用[J].中國公路學(xué)報(bào)，2021，34（3）：225-234

[7] 李揚(yáng)，張蕾，李鵬飛，等.基于改進(jìn)RRT結(jié)合B樣條的機(jī)械臂運(yùn)動(dòng)規(guī)劃方法[J].計(jì)算機(jī)集成制造系統(tǒng)，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] 阮曉鋼，周靜，張晶晶，等.基于子目標(biāo)搜索的機(jī)器人目標(biāo)導(dǎo)向RRT路徑規(guī)劃算法[J].控制與決策，2020，35（10）：2543-2548

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

[11] 孫欽鵬，李猛，王中華.基于改進(jìn)快速擴(kuò)展隨機(jī)樹算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].濟(jì)南大學(xué)學(xué)報(bào)（自然科學(xué)版），2019，33（5）：431-438

[12] 施楊洋，楊家富，布升強(qiáng)，等.基于RRT改進(jìn)的智能車輛路徑規(guī)劃算法[J].計(jì)算技術(shù)與自動(dòng)化，2019，38（4）：81-86

[13] 王雨，劉延俊，賈華，等.基于強(qiáng)化RRT算法的機(jī)械臂路徑規(guī)劃[J].山東大學(xué)學(xué)報(bào)（工學(xué)版），2022：1-9-RRT； 可變權(quán)重系數(shù)； 剪枝； 三次B樣條