馮 垚，周志峰，沈亦純，王立端

（1. 上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海 201620； 2. 上海衛(wèi)星工程研究所，上海 200240；3. 上海司南衛(wèi)星導(dǎo)航技術(shù)股份有限公司，上海 201801）

隨著機(jī)器人技術(shù)的快速發(fā)展，機(jī)器人運(yùn)動過程中的避障問題受到了學(xué)者們的廣泛關(guān)注。高效的路徑規(guī)劃可以提高機(jī)器人的執(zhí)行效率。因此，研究如何降低避障路徑規(guī)劃的時間成本和長度成本具有重要意義。

近年來，國內(nèi)外學(xué)者針對機(jī)器人的路徑規(guī)劃算法做了大量研究。Fan等[1]提出的人工勢場法引入了力場原理，即：將障礙物附近區(qū)域定義為斥力場，將目標(biāo)點(diǎn)附近區(qū)域定義為引力場。基于該算法規(guī)劃的路徑雖能實現(xiàn)有效避障，但當(dāng)引力和斥力的合力為0 N 時，易陷入局部最小值，導(dǎo)致無法規(guī)劃出一條無碰撞路徑。Dolgov等[2]提出的A*算法是一種圖形遍歷算法，該算法在簡單環(huán)境中易找到最優(yōu)路徑，但隨著環(huán)境復(fù)雜程度的提高，每一個節(jié)點(diǎn)處的運(yùn)算量增大，導(dǎo)致路徑規(guī)劃效率大幅下降[3-4]。相較于上述算法，LaValle等[5]提出的快速搜索隨機(jī)樹（rapid‐ly-exploring random tree, RRT）算法具有在空間內(nèi)隨機(jī)快速采樣的特點(diǎn)，且無需建模，具備空間搜索的完整性[6-7]。但是，由于搜索方向的隨機(jī)性，RRT算法的收斂速度慢，且在復(fù)雜環(huán)境中會產(chǎn)生大量無效節(jié)點(diǎn)，導(dǎo)致所規(guī)劃路徑的平滑度較差[8-10]。

針對RRT算法在路徑規(guī)劃效率上的不足，許多學(xué)者對其進(jìn)行了改進(jìn)優(yōu)化。Yuan 等[11]提出的Bias-RRT 算法是在RRT 算法的基礎(chǔ)上引入目標(biāo)偏置策略，利用偏置概率使隨機(jī)樹朝目標(biāo)點(diǎn)方向拓展。在障礙物較少的環(huán)境中，Bias-RRT算法能快速規(guī)劃出無碰撞路徑，但在復(fù)雜環(huán)境中過大的偏置概率會降低規(guī)劃速度[12-13]。Karaman 等[14]提出的RRT*算法在RRT算法的基礎(chǔ)上增加了重選父節(jié)點(diǎn)以及重新布線兩大策略，以增加時間成本的方式來規(guī)劃一條接近最優(yōu)的路徑。Luo等[15]基于RRT*算法提出了一種In‐formed-RRT*算法，通過將采樣區(qū)域限制在橢圓內(nèi)的方式對規(guī)劃路徑進(jìn)行優(yōu)化。相比于RRT*算法，Informed-RRT*算法獲取漸進(jìn)最優(yōu)路徑的速度有所提升[16]。Kuffner 等[17]提 出 了RRT-Connect 算 法，該算法是在RRT算法的基礎(chǔ)上以目標(biāo)點(diǎn)為根節(jié)點(diǎn)增加了1棵隨機(jī)樹，2棵隨機(jī)樹同時朝對方進(jìn)行生長。相較于RRT 算法，RRT-Connect算法在路徑規(guī)劃速度上有所提升，但其規(guī)劃的路徑仍不夠平滑[18-21]。

綜上所述，已有研究雖通過改進(jìn)使RRT算法在路徑規(guī)劃效率上得到了一定程度的提升，但仍存在環(huán)境適應(yīng)性差、收斂速度慢和規(guī)劃路徑質(zhì)量差等問題。為解決上述問題，筆者提出了一種新的改進(jìn)RRT算法。首先，在傳統(tǒng)RRT算法中引入地圖復(fù)雜程度評估策略，以計算得到最適合相應(yīng)地圖的步長和偏置概率；然后，利用采樣區(qū)域動態(tài)更新策略和采樣點(diǎn)優(yōu)化策略來提高采樣點(diǎn)的有效性及質(zhì)量，以實現(xiàn)在保留傳統(tǒng)RRT算法隨機(jī)性的同時獲取漸進(jìn)最優(yōu)采樣點(diǎn)，從而確保隨機(jī)樹朝目標(biāo)點(diǎn)附近正向生長；最后，基于節(jié)點(diǎn)重連策略，在重新連接初始路徑節(jié)點(diǎn)后進(jìn)行碰撞檢測，以刪除冗余節(jié)點(diǎn)，使得避障路徑更加優(yōu)化。

1 RRT算法

圖1 RRT算法原理Fig.1 Principle of RRT algorithm

令M表示地圖，T表示隨機(jī)樹，k表示迭代次數(shù)，則RRT算法的偽代碼如下：

2 改進(jìn)RRT算法

2.1 地圖復(fù)雜程度評估策略

傳統(tǒng)RRT算法不能充分利用地圖中的障礙物信息來自適應(yīng)調(diào)整步長，導(dǎo)致即使面向障礙物較少的簡單地圖，也難以快速規(guī)劃出一條漸進(jìn)最優(yōu)的路徑。Bias-RRT算法采用目標(biāo)偏置策略，通過設(shè)定偏置概率P來選取目標(biāo)點(diǎn)作為采樣點(diǎn)，在簡單地圖中可以有效提高路徑規(guī)劃的收斂效率[11]。但偏置概率P的取值采用主觀定義，當(dāng)面向不同環(huán)境時，Bias-RRT算法的適用性不足，尤其是對于障礙物較多的復(fù)雜地圖，過大的偏置概率P會增加路徑規(guī)劃的時間成本，有時甚至可能會導(dǎo)致路徑規(guī)劃失敗[12-13]。

此外，傳統(tǒng)RRT算法中隨機(jī)樹生長的步長為定值，而同一步長無法適應(yīng)不同地圖。當(dāng)在障礙物較少的簡單地圖中進(jìn)行路徑規(guī)劃時，若步長過短，則會增加節(jié)點(diǎn)數(shù)量，導(dǎo)致規(guī)劃時間延長。當(dāng)在障礙物較多的復(fù)雜地圖中進(jìn)行路徑規(guī)劃時，若步長過長，則會增大與障礙物碰撞的概率，導(dǎo)致難以得到一條無碰撞的路徑。

針對上述問題，本文提出了一種地圖復(fù)雜程度評估策略，即基于地圖中的障礙物信息計算地圖復(fù)雜程度系數(shù)C1，從而得到最合適的偏置概率P和步長S。假設(shè)地圖的復(fù)雜程度主要與地圖中障礙物的總面積及分布情況有關(guān)，且兩者對應(yīng)的權(quán)重相等（均為0.5），則地圖的復(fù)雜程度系數(shù)C1可表示為：

式中：Aobstacle表示障礙物面積；Amap表示地圖面積；Dobstacle表示障礙物分布情況，將地圖劃分為100個均等格子，障礙物所占格子數(shù)與100之比即為障礙物的分布情況。

地圖復(fù)雜程度系數(shù)C1越趨近于1，說明地圖的復(fù)雜程度越高，則偏置概率P和步長S的取值應(yīng)越小。合適的步長S應(yīng)考慮偏置概率P以及起點(diǎn)到目標(biāo)點(diǎn)的距離。本文將步長S設(shè)為偏置概率P與起點(diǎn)到目標(biāo)點(diǎn)的距離之積。為確定偏置概率P的最佳取值，在傳統(tǒng)RRT 算法中引入偏置概率P和步長S，并將偏置概率P分別取為1-C1、(1-C1)2、(1-C1)3和(1-C1)4，在同一地圖中分別進(jìn)行50 次路徑規(guī)劃仿真實驗，整理相關(guān)數(shù)據(jù)并對比，結(jié)果如表1所示。由表1 可知，當(dāng)偏置概率P過大時，會導(dǎo)致路徑規(guī)劃失敗；當(dāng)偏置概率P過小時，偏置效果變差，導(dǎo)致路徑規(guī)劃的時間成本增加；當(dāng)偏置概率P=(1-C1)3時，路徑規(guī)劃的時間成本和長度成本均最低。綜上，偏置概率P和步長S的計算式可表示為：

表1 引入不同偏置概率時RRT算法的路徑規(guī)劃結(jié)果對比Table 1 Comparison of path planning results of RRT algo‐rithm with different bias probabilities

2.2 采樣區(qū)域動態(tài)更新策略

傳統(tǒng)RRT算法規(guī)劃的無碰撞路徑通常不具有方向性，甚至可能會出現(xiàn)逆向生長，導(dǎo)致路徑規(guī)劃的時間成本和長度成本過高。出現(xiàn)上述問題的主要原因在于傳統(tǒng)RRT算法會在無效區(qū)域內(nèi)進(jìn)行采樣，使得隨機(jī)樹生長出過多的無效樹枝。為了避免該情況發(fā)生，本文提出了采樣區(qū)域動態(tài)更新策略，其核心思想是隨著隨機(jī)樹的生長，不斷向目標(biāo)點(diǎn)所在區(qū)域縮小采樣范圍，以逐漸逼近目標(biāo)點(diǎn)。

隨機(jī)樹節(jié)點(diǎn)的正向生長是指朝當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的區(qū)域生長，可不斷縮小當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離，即有效生長；逆向生長是指朝當(dāng)前節(jié)點(diǎn)與起點(diǎn)之間的區(qū)域生長，會不斷擴(kuò)大當(dāng)前節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離，即無效生長。如圖2所示，在二維地圖中，根據(jù)隨機(jī)樹上最新增加的節(jié)點(diǎn)Qnew，將搜索空間分為2塊區(qū)域，其中目標(biāo)點(diǎn)所在區(qū)域定義為有效區(qū)域，即區(qū)域Ⅱ，另一塊區(qū)域定義為無效區(qū)域，即區(qū)域I。在下一次隨機(jī)采樣時，僅在區(qū)域Ⅱ內(nèi)進(jìn)行采樣，隨著隨機(jī)樹節(jié)點(diǎn)的增加，有效區(qū)域不斷向目標(biāo)點(diǎn)收縮，這樣可以保證隨機(jī)樹正向生長。但如圖2(a)所示，當(dāng)節(jié)點(diǎn)Qnew生長到障礙物附近且正向生長基本被障礙物阻擋時，由于僅可在有效區(qū)域內(nèi)采樣，RRT算法易陷入局部無解，使得隨機(jī)樹無法繼續(xù)生長。為解決該問題，在動態(tài)更新采樣區(qū)域的基礎(chǔ)上引入節(jié)點(diǎn)拒絕策略。當(dāng)隨機(jī)樹在當(dāng)前節(jié)點(diǎn)經(jīng)過50次迭代后仍然無法繼續(xù)生長時，即認(rèn)為該節(jié)點(diǎn)無效，將其從隨機(jī)樹中刪除，如圖2(b)所示的節(jié)點(diǎn)Qvoid即為無效節(jié)點(diǎn)。此時，隨機(jī)樹的有效采樣區(qū)域為基于無效節(jié)點(diǎn)的父節(jié)點(diǎn)Qparent分割得到的區(qū)域Ⅱ，隨機(jī)樹在更新后的有效采樣區(qū)域內(nèi)重新開始生長，如圖2(c)所示。

圖2 采樣區(qū)域動態(tài)更新示意Fig.2 Schematic diagram of dynamic update of sam‐pling area

2.3 采樣點(diǎn)優(yōu)化策略

傳統(tǒng)RRT算法在地圖中隨機(jī)生成的采樣點(diǎn)不具有導(dǎo)向性，導(dǎo)致隨機(jī)樹的樹枝生長得雜亂無章。大量低質(zhì)量采樣點(diǎn)的產(chǎn)生，不僅會增加路徑規(guī)劃的時間成本，而且會導(dǎo)致節(jié)點(diǎn)冗余，使得路徑規(guī)劃的長度成本增加。

“我家種了四畝葡萄，在家正要吃早飯，聽說這邊要舉行爭霸賽，我飯都沒吃，去園子里摘了幾串葡萄就過來了?！币晃恍諒埖拇蠼愀嬖V記者，威縣葡萄的種植面積非常大，幾乎家家戶戶都有種，小到一兩畝，大到幾十畝不等。種植的葡萄種類主要以巨峰為主，紅寶石、維多利亞等品種并存?！斑^來參賽不是說一定要當(dāng)葡萄王，就是想證明一下自家的葡萄種的不比他們的差，不信你嘗嘗我家葡萄多甜。這葡萄就像自己孩子似的，誰不想讓人夸夸自己孩子呢，你說是不是？”張大姐一邊聊，一邊邀請記者品嘗。

為此，本文設(shè)計了一種采樣點(diǎn)優(yōu)化策略。如圖3所示，在每一次采樣時，同時隨機(jī)產(chǎn)生3個待定采樣點(diǎn)Qrand1、Qrand2、Qrand3并構(gòu)成集合，計算集合中每個待定采樣點(diǎn)到目標(biāo)點(diǎn)的距離，選取距離目標(biāo)點(diǎn)最近的待定采樣點(diǎn)作為最終采樣點(diǎn)。該優(yōu)化方法的核心思想在于：從待定采樣點(diǎn)集合中選取距離目標(biāo)點(diǎn)最近的采樣點(diǎn)來確定節(jié)點(diǎn)生長的方向，以保證隨機(jī)樹朝目標(biāo)點(diǎn)附近生長。通過提高采樣點(diǎn)的質(zhì)量，既避免了隨機(jī)樹生長的長度成本過高，又保留了RRT算法搜索方向的完整性。

圖3 采樣點(diǎn)優(yōu)化過程示意Fig.3 Schematic diagram of sampling point optimiza‐tion process

2.4 節(jié)點(diǎn)重連策略

基于上述策略改進(jìn)的RRT算法雖能快速規(guī)劃得到初始路徑，但該初始路徑存在節(jié)點(diǎn)冗余現(xiàn)象，導(dǎo)致路徑的彎折次數(shù)較多。為解決該問題，本文提出了節(jié)點(diǎn)重連策略，即通過對初始路徑上的節(jié)點(diǎn)進(jìn)行重新連接來減少轉(zhuǎn)折點(diǎn)，從而優(yōu)化路徑。

節(jié)點(diǎn)重連策略的原理如圖4所示。圖中：虛線所示路徑為未引入節(jié)點(diǎn)重連策略的改進(jìn)RRT算法規(guī)劃的初始路徑，該路徑由初始節(jié)點(diǎn)集合{Q1,Q2,Q3,Q4,Q5,Q6}中的節(jié)點(diǎn)依次連接而成，其中Q1和Q6分別代表起點(diǎn)和目標(biāo)點(diǎn)。引入節(jié)點(diǎn)重連策略后，將目標(biāo)點(diǎn)Q6作為隨機(jī)樹的根節(jié)點(diǎn)，基于Q6依次連接初始節(jié)點(diǎn)集合中的各個節(jié)點(diǎn)并進(jìn)行碰撞檢測。通過檢測發(fā)現(xiàn)，線段Q6Q1和線段Q6Q2均會與障礙物發(fā)生碰撞，但Q6直接連接Q3時沒有發(fā)生碰撞，故將Q3加入優(yōu)化節(jié)點(diǎn)集合。然后，以Q3作為碰撞檢測的起點(diǎn)，依次連接初始節(jié)點(diǎn)集合中的各個節(jié)點(diǎn)并進(jìn)行新一輪的碰撞檢測。結(jié)果顯示，Q3直接連接Q1時沒有發(fā)生碰撞，故將Q1加入優(yōu)化節(jié)點(diǎn)集合。當(dāng)起點(diǎn)被加入到優(yōu)化節(jié)點(diǎn)集合中時，表明節(jié)點(diǎn)重連結(jié)束。將優(yōu)化節(jié)點(diǎn)集合中的節(jié)點(diǎn)按順序依次連接，獲得優(yōu)化路徑，如圖4中實線所示路徑。相較于初始路徑而言，節(jié)點(diǎn)重連后路徑的節(jié)點(diǎn)數(shù)量明顯減少，有效降低了路徑規(guī)劃的長度成本。

3 仿真實驗與結(jié)果分析

為驗證本文改進(jìn)RRT算法在避障路徑規(guī)劃中的可行性及其規(guī)劃效率的提升效果，分別利用傳統(tǒng)RRT 算法、Bias-RRT 算法、RRT-Connect 算法和改進(jìn)RRT算法進(jìn)行路徑規(guī)劃仿真并對比。本文仿真環(huán)境為Intel(R) Core(TM) i5-7200U CPU @ 2.50 GHz，其內(nèi)存為4 GB。各RRT 算法均采用Python3.6 和MATLAB 2017a軟件來實現(xiàn)。

3.1 二維空間仿真

為驗證本文改進(jìn)RRT 算法在二維空間中規(guī)劃避障路徑的可行性，分別利用傳統(tǒng)RRT 算法、Bias-RRT算法、RRT-Connect算法和改進(jìn)RRT算法在3種復(fù)雜程度不同的二維地圖中進(jìn)行50次路徑規(guī)劃仿真實驗。其中：二維地圖的大小均為18 cm×18 cm；路徑的起點(diǎn)為(2，2) cm，終點(diǎn)為(17，17) cm。在本文中，傳統(tǒng)RRT 算法、Bias-RRT 算法、RRTConnect 算法的固定步長均取1.5 cm；Bias-RRT 算法的偏置概率取20%。

4 種RRT 算法在3 種二維地圖中的路徑規(guī)劃效果分別如圖5 至圖7 所示。從圖5(a)、圖6(a)、圖7(a)中可以看出，傳統(tǒng)RRT算法生長的隨機(jī)樹沒有方向性，產(chǎn)生了大量無效節(jié)點(diǎn)，且規(guī)劃的路徑較為曲折。從圖5(b)、圖6(b)、圖7(b)中可以看出，偏置概率取20%的Bias-RRT算法在簡單環(huán)境中可以使隨機(jī)樹的生長具有方向性，但在復(fù)雜環(huán)境中無法實現(xiàn)。從圖5(c)、圖6(c)、圖7(c)中可以看出，RRT-Connect算法中2棵隨機(jī)樹的雙向生長雖提高了路徑規(guī)劃效率，但仍存在不少無效節(jié)點(diǎn)且路徑質(zhì)量較差。從圖5(d)、圖6(d)、圖7(d)中可以看出，本文的改進(jìn)RRT算法基于對地圖復(fù)雜程度的自動評估得到的步長相比于其他3種RRT算法的固定步長具有明顯優(yōu)勢，尤其在復(fù)雜程度不高的地圖1 和地圖2中，合適的偏置概率和步長大幅提高了路徑規(guī)劃效率；在復(fù)雜程度較高的地圖3中，當(dāng)隨機(jī)樹生長到障礙物附近無法繼續(xù)正向生長時，節(jié)點(diǎn)拒絕策略有助于隨機(jī)樹擺脫局部無解，有效地提高了路徑規(guī)劃效率。綜上所述，相較于其他3種RRT算法，本文的改進(jìn)RRT算法生長的隨機(jī)樹的樹枝大幅減少，采樣點(diǎn)數(shù)量減少且均向目標(biāo)點(diǎn)收縮，經(jīng)過節(jié)點(diǎn)重連后獲得的無碰撞路徑的質(zhì)量明顯優(yōu)于其他RRT算法。

圖5 二維地圖1中4種RRT算法的路徑規(guī)劃效果對比Fig.5 Comparison of path planning effect of four RRT algorithms in 2D map 1

圖6 二維地圖2中4種RRT算法的路徑規(guī)劃效果對比Fig.6 Comparison of path planning effect of four RRT algorithms in 2D map 2

圖7 二維地圖3中4種RRT算法的路徑規(guī)劃效果對比Fig.7 Comparison of path planning effect of four RRT algorithms in 2D map 3

對4 種RRT 算法在3 種復(fù)雜程度不同的地圖中的路徑規(guī)劃數(shù)據(jù)（耗時、節(jié)點(diǎn)數(shù)、路徑長度）進(jìn)行均值處理并對比，結(jié)果分別如表2至表4所示。

表2 二維地圖1中4種RRT算法的路徑規(guī)劃數(shù)據(jù)對比Table 2 Comparison of path planning data of four RRT al‐gorithms in 2D map 1

表3 二維地圖2中4種RRT算法的路徑規(guī)劃數(shù)據(jù)對比Table 3 Comparison of path planning data of four RRT al‐gorithms in 2D map 2

表4 二維地圖3中4種RRT算法的路徑規(guī)劃數(shù)據(jù)對比Table 4 Comparison of path planning data of four RRT al‐gorithms in 2D map 3

由表2可得，相較于傳統(tǒng)RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進(jìn)RRT算法在復(fù)雜程度較低的地圖1中的路徑規(guī)劃耗時分別下降了96.24%，90.08%，81.93%，路徑長度分別縮短了18.89%，13.74%，16.12%。

由表3可得，相較于傳統(tǒng)RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進(jìn)RRT算法在復(fù)雜程度一般的地圖2中的路徑規(guī)劃耗時分別下降了93.28%，88.52%，62.29%，路徑長度分別縮短了26.64%，21.66%，13.44%。

由表4可得，相較于傳統(tǒng)RRT算法、Bias-RRT算法和RRT-Connect算法，本文的改進(jìn)RRT算法在復(fù)雜程度較高的地圖3中的路徑規(guī)劃耗時分別下降了93.11%，92.49%，73.41%，路徑長度分別縮短了24.25%，23.05%，21.16%。

綜上所述，相較于傳統(tǒng)RRT算法、Bias-RRT算法和RRT-Connect算法，在復(fù)雜程度不同的二維地圖中，本文的改進(jìn)RRT算法的避障路徑規(guī)劃速度更快，且路徑的節(jié)點(diǎn)更少，長度更短。

3.2 三維空間仿真

為驗證本文的改進(jìn)RRT算法在更加復(fù)雜的三維空間中規(guī)劃路徑的可行性，分別利用傳統(tǒng)RRT 算法、Bias-RRT 算法、RRT-Connect 算法和改進(jìn)RRT算法在三維地圖中進(jìn)行50次路徑規(guī)劃仿真實驗。其中，三維地圖的大小為100 cm×100 cm ×100 cm，起點(diǎn)為(0，0，0) cm，終點(diǎn)為(100，100，100) cm。在本文中，傳統(tǒng)RRT 算法、Bias-RRT 算法、RRTConnect 算法的固定步長取5 cm，其中Bias-RRT 算法的偏置概率取20%。

4 種RRT 算法在三維地圖中的路徑規(guī)劃效果如圖8所示。通過對比可知，在三維地圖中，傳統(tǒng)RRT算法因搜索空間擴(kuò)大且隨機(jī)樹的生長不具有方向性，生長出了大量的無效樹枝，其規(guī)劃的路徑質(zhì)量較差。偏置概率取20%的Bias-RRT算法的隨機(jī)樹生長具有一定的導(dǎo)向性，但仍存在較多冗余節(jié)點(diǎn)。相較于Bias-RRT算法，RRT-Connect算法中2棵隨機(jī)樹的雙向生長更具有導(dǎo)向性，但也存在冗余節(jié)點(diǎn)，最終的避障路徑質(zhì)量較差。與上述3種RRT算法相比，本文的改進(jìn)RRT算法具有更適合三維地圖的步長以及其隨機(jī)樹生長的導(dǎo)向性更優(yōu)，使得無效節(jié)點(diǎn)的數(shù)量大大減少，經(jīng)節(jié)點(diǎn)重連后得到的避障路徑也更加平滑。

圖8 三維地圖中4種RRT算法的路徑規(guī)劃效果對比Fig.8 Comparison of path planning effect of four RRT algorithms in 3D map

將4種RRT算法在三維地圖中的路徑規(guī)劃數(shù)據(jù)（耗時、節(jié)點(diǎn)數(shù)、路徑長度）進(jìn)行均值處理并對比，結(jié)果如表5 所示。由表5 可知，相較于傳統(tǒng)RRT 算法、Bias-RRT算法和RRT-Connect算法，本文的改進(jìn)RRT算法在三維地圖中的路徑規(guī)劃耗時分別下降了99.74%，90.77%，88.47%，路徑長度分別縮短了26.71%，14.43%，13.19%。

表5 三維地圖中4種RRT算法的路徑規(guī)劃數(shù)據(jù)對比Table 5 Comparison of path planning data of four RRT al‐gorithms in 3D map

3.3 機(jī)械臂避障路徑規(guī)劃仿真

為驗證本文提出的改進(jìn)RRT 算法在機(jī)械臂避障路徑規(guī)劃中的適用性，在MATLAB 2017a 軟件中開展仿真實驗，本文以六自由度機(jī)械臂為研究對象。首先，對機(jī)械臂的工作空間進(jìn)行分析，若起點(diǎn)和目標(biāo)點(diǎn)均在工作空間內(nèi)，則可直接開展機(jī)械臂末端的避障路徑規(guī)劃。機(jī)械臂末端避障路徑的規(guī)劃過程為：先利用改進(jìn)RRT 算法規(guī)劃初始避障路徑，并將初始避障路徑分解為若干路徑點(diǎn)；然后，將路徑點(diǎn)的坐標(biāo)依次代入機(jī)械臂的逆運(yùn)動學(xué)方程，以求解得到8組關(guān)節(jié)轉(zhuǎn)角。若某路徑點(diǎn)對應(yīng)的8組關(guān)節(jié)轉(zhuǎn)角中有滿足機(jī)械臂各連桿與障礙物無碰撞且各連桿之間無碰撞的關(guān)節(jié)轉(zhuǎn)角，則認(rèn)為該路徑點(diǎn)有效。但若8組關(guān)節(jié)轉(zhuǎn)角均不能滿足上述要求，則將該路徑點(diǎn)的前一個路徑點(diǎn)作為新的起點(diǎn)，重新規(guī)劃路徑并進(jìn)行連桿碰撞檢測，直到獲得一條無碰撞路徑。

基于改進(jìn)RRT算法的機(jī)械臂末端避障路徑規(guī)劃結(jié)果（規(guī)劃50次）如圖9所示。其中：起點(diǎn)為(30，40，50) cm，終點(diǎn)為(-40，30，30) cm，圓球表示障礙物，實線為機(jī)械臂末端的避障路徑。與此同時，分別利用傳統(tǒng)RRT 算法、Bias-RRT 算法、RRTConnect 算法對機(jī)械臂末端的避障路徑規(guī)劃50 次（起點(diǎn)、終點(diǎn)以及障礙物位置等均相同），并對4種RRT 算法的避障路徑規(guī)劃數(shù)據(jù)進(jìn)行均值處理和對比，結(jié)果如表6所示。結(jié)果表明，利用本文的改進(jìn)RRT算法對機(jī)械臂末端的避障路徑進(jìn)行規(guī)劃時，所得路徑的質(zhì)量更高，時間成本更低，驗證了其適用性。

圖9 基于改進(jìn)RRT算法的機(jī)械臂避障路徑規(guī)劃結(jié)果Fig. 9 Obstacle avoidance path planning result of robotic arm based on improved RRT algorithm

表6 基于4種RRT算法的機(jī)械臂避障路徑規(guī)劃數(shù)據(jù)對比Table 6 Comparison of obstacle avoidance path planning data of robotic arm based on four RRT algorithms

4 結(jié) 論

針對傳統(tǒng)RRT算法在避障路徑規(guī)劃時存在效率低的問題，本文提出了一種新的改進(jìn)RRT算法，該算法引入了以下4個策略：1）地圖復(fù)雜程度評估策略，即根據(jù)地圖中的障礙物信息，計算適合相應(yīng)地圖的步長和偏置概率；2）采樣區(qū)域動態(tài)更新策略，令RRT算法僅在有效區(qū)域內(nèi)采樣，以確保隨機(jī)樹正向生長；3）采樣點(diǎn)優(yōu)化策略，從待定采樣點(diǎn)集合中選取距離目標(biāo)點(diǎn)最近的采樣點(diǎn)作為最終采樣點(diǎn)，提高了隨機(jī)樹朝目標(biāo)點(diǎn)附近生長的概率；4）節(jié)點(diǎn)重連策略，由于初始規(guī)劃路徑的彎折次數(shù)較多，通過對初始路徑節(jié)點(diǎn)進(jìn)行優(yōu)選和重新連接，可去除冗余節(jié)點(diǎn)，實現(xiàn)了路徑優(yōu)化。最后，通過在二維、三維地圖中以及對機(jī)械臂開展避障路徑規(guī)劃仿真實驗，驗證了改進(jìn)RRT 算法對環(huán)境的適應(yīng)性比傳統(tǒng)RRT 算法、Bias-RRT 算法、RRT-Connect 算法強(qiáng)，其規(guī)劃無碰撞路徑的耗時更短，質(zhì)量更高且更適用于機(jī)械臂。