基于改進(jìn)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃研究

楊軍

(湖南交通工程學(xué)院,湖南 衡陽 421009)

0 引言

移動(dòng)機(jī)器人是集環(huán)境感知、動(dòng)態(tài)決策規(guī)劃、運(yùn)動(dòng)行為控制等復(fù)雜功能于一體的機(jī)械裝備,在民用和軍用領(lǐng)域均具有廣泛應(yīng)用前景。路徑規(guī)劃是開發(fā)多功能移動(dòng)機(jī)器人裝備的核心技術(shù),算法的目標(biāo)是在機(jī)器人工作的復(fù)雜環(huán)境中為其規(guī)劃出一條無碰撞的最短路徑,這是學(xué)術(shù)界的廣泛關(guān)注點(diǎn)[1]。楊恒等[2]對(duì)傳統(tǒng)A*算法存在的路徑規(guī)劃效率低、拐點(diǎn)多等問題提出了聯(lián)合改進(jìn)A*算法和動(dòng)態(tài)窗口法(Dynamic Window Approach,DWA)的路徑規(guī)劃方法,仿真試驗(yàn)指出該方法能夠確保移動(dòng)機(jī)器人實(shí)時(shí)避障,確保規(guī)劃路徑的安全性。楊博等[3]對(duì)傳統(tǒng)遺傳算法所規(guī)劃的路徑不夠平滑、易陷入局部最優(yōu)的問題,設(shè)計(jì)了新的適應(yīng)度函數(shù),同時(shí)采用混合選擇策略改善算法,避免了傳統(tǒng)算法存在的問題,通過與傳統(tǒng)遺傳算法的對(duì)比,所規(guī)劃的路徑在性能上更佳。邱添等[4]針對(duì)概率路線圖法(Probabilistic Roadmap Method,PRM)進(jìn)行路徑規(guī)劃存在的運(yùn)行效率低、狹窄通道采樣困難等問題,提出了柵格概率路徑圖法,仿真結(jié)果表明該方法對(duì)路徑規(guī)劃的效率高,提升了路徑規(guī)劃的質(zhì)量。莫海寧等[5]分析了傳統(tǒng)煙花算法的缺陷,將競(jìng)爭(zhēng)學(xué)習(xí)和煙花算法相結(jié)合對(duì)機(jī)器人路徑實(shí)施規(guī)劃,得到的規(guī)劃路徑具有更高平滑度,能夠更好地適應(yīng)機(jī)器人的作業(yè)環(huán)境?？焖贁U(kuò)展隨機(jī)樹(Rapidly-Exploring Random Trees,RRT)是基于概率采樣的運(yùn)動(dòng)規(guī)劃算法,其具有采樣節(jié)點(diǎn)隨機(jī)、增量式增長、搜索能力強(qiáng)等優(yōu)點(diǎn),是有效解決運(yùn)動(dòng)機(jī)器人路徑規(guī)劃的新方法[6]。實(shí)踐表明,由于RRT算法缺乏路徑導(dǎo)向性,使得實(shí)際路徑規(guī)劃效率不高、轉(zhuǎn)折點(diǎn)多且距離長。在深入研究的基礎(chǔ)上對(duì)RRT算法進(jìn)行改進(jìn),提出了待擴(kuò)展點(diǎn)的選擇策略和擴(kuò)展方向,降低了擴(kuò)展點(diǎn)選擇上的盲目性,同時(shí)提出了路徑冗余點(diǎn)消除算法和路徑平滑處理算法,使得規(guī)劃的路徑距離短、平順性好,路徑規(guī)劃效率大大提升。最后將其應(yīng)用于移動(dòng)機(jī)器人路徑規(guī)劃仿真試驗(yàn)和實(shí)物試驗(yàn)中,驗(yàn)證算法的有效性。

1 RRT算法及改進(jìn)

1.1 RRT算法

RRT算法采用遞增式構(gòu)造,由所搜索的空間隨機(jī)抽取樣本產(chǎn)生。在實(shí)際產(chǎn)生節(jié)點(diǎn)時(shí)傾向于未探測(cè)區(qū)域,不妨設(shè)qstart是路徑規(guī)劃的起始點(diǎn),qrand是路徑規(guī)劃空間的隨機(jī)采樣點(diǎn),qnear是最近鄰點(diǎn),qnew是擴(kuò)展后生成的新節(jié)點(diǎn),L是擴(kuò)展的步長,隨機(jī)樹擴(kuò)展示意如圖1所示[7]。

圖1 隨機(jī)樹擴(kuò)展示意圖

在圖1中,路徑規(guī)劃的起始點(diǎn)qstart為根節(jié)點(diǎn),在擴(kuò)展樹中尋找和隨機(jī)采樣點(diǎn)距離最近的點(diǎn)作為qnear。在隨機(jī)采樣點(diǎn)和最近鄰點(diǎn)的連線上按照拓展步長來拓展新的節(jié)點(diǎn)。連接最近點(diǎn)和新節(jié)點(diǎn),如果連線遇到障礙物,那么該條路徑是不通的,舍棄擴(kuò)展的新節(jié)點(diǎn);如果連線沒有遇到障礙物,那么該條路徑是可行的,將該節(jié)點(diǎn)添加到隨機(jī)樹中,直到尋找到路徑規(guī)劃的目標(biāo)點(diǎn)為止。

很明顯,采樣點(diǎn)的選擇直接影響到算法的效率。尤其是在移動(dòng)機(jī)器人的工作空間比較狹窄時(shí),RRT進(jìn)行路徑規(guī)劃的效率就會(huì)大大降低,同時(shí)由于在路徑規(guī)劃的過程中缺乏導(dǎo)向性,這也使得算法對(duì)最佳路徑的搜索時(shí)間比較長,所規(guī)劃的路徑距離也更長。

1.2 RRT算法改進(jìn)

1.2.1 待擴(kuò)展節(jié)點(diǎn)選擇改進(jìn)

RRT中待擴(kuò)展節(jié)點(diǎn)為隨機(jī)樹中和采樣節(jié)點(diǎn)距離最近的點(diǎn),而采樣節(jié)點(diǎn)是隨機(jī)的,這也使得擴(kuò)展節(jié)點(diǎn)是隨機(jī)的。在極端情況下,當(dāng)擴(kuò)展隨機(jī)樹接近目標(biāo)點(diǎn)時(shí),因擴(kuò)展節(jié)點(diǎn)是隨機(jī)的,從而使得隨機(jī)樹向四周等概率擴(kuò)展,產(chǎn)生大量無效的節(jié)點(diǎn)。基于A*算法的思想,對(duì)待擴(kuò)展節(jié)點(diǎn)的選擇進(jìn)行改進(jìn)[8],定義隨機(jī)樹中任一節(jié)點(diǎn)qi和采樣點(diǎn)qrand的距離為H(i),和目標(biāo)點(diǎn)的距離為G(i),那么

F(i)=H(i)+G(i)。

(1)

將滿足F(i)最小的節(jié)點(diǎn)作為擴(kuò)展節(jié)點(diǎn),記為qnear。很明顯,采用這種方式來選擇待擴(kuò)展節(jié)點(diǎn)能夠在很大程度上降低傳統(tǒng)RRT算法在待擴(kuò)展節(jié)點(diǎn)選擇上的盲目性,使得路徑規(guī)劃的效率大大提升。

1.2.2 擴(kuò)展方向選擇

傳統(tǒng)RRT的擴(kuò)展方向?yàn)殡S機(jī)采樣點(diǎn)qrand所在的位置方向,很明顯擴(kuò)展方向具有很大的隨機(jī)性?；谌斯?shì)力場(chǎng)的思想,對(duì)節(jié)點(diǎn)的擴(kuò)展方向進(jìn)行改進(jìn)[9]。由于路徑規(guī)劃的目標(biāo)是尋找一條從起點(diǎn)到目標(biāo)點(diǎn)的最佳路線,因此當(dāng)待擴(kuò)展的節(jié)點(diǎn)確定之后,優(yōu)先考慮目標(biāo)方向。如果目標(biāo)方向拓展成功,那么直接生成新的節(jié)點(diǎn);如果目標(biāo)方向存在障礙物,那么以隨機(jī)采樣點(diǎn)方向作為拓展方向生成新的節(jié)點(diǎn),同時(shí)對(duì)失敗的待擴(kuò)展節(jié)點(diǎn)進(jìn)行標(biāo)記。再次選擇失敗節(jié)點(diǎn)作為拓展節(jié)點(diǎn)時(shí),直接按照隨機(jī)采樣點(diǎn)方向進(jìn)行拓展。圖2為目標(biāo)方向存在障礙物示意圖。

圖2 目標(biāo)方向存在障礙物示意圖

1.2.3 冗余點(diǎn)消除

由于RRT算法在選擇擴(kuò)展點(diǎn)時(shí)的隨機(jī)性導(dǎo)致所規(guī)劃的路徑中存在諸多的冗余點(diǎn),這些冗余點(diǎn)使得移動(dòng)機(jī)器人在實(shí)際運(yùn)動(dòng)時(shí)的波動(dòng)比較大。如果兩個(gè)不相鄰的路徑之間連接沒有遇到障礙物,那么就可以將兩個(gè)路徑之間的點(diǎn)消除。圖3為路徑冗余點(diǎn)消除示意圖。

圖3 路徑冗余點(diǎn)消除示意圖

路徑冗余點(diǎn)消除就是從起始點(diǎn)開始逐個(gè)進(jìn)行碰撞檢測(cè),按照?qǐng)D3的方式消除路徑上的冗余點(diǎn)。路徑冗余點(diǎn)的消除不僅使得所規(guī)劃的路徑長度大大縮短,同時(shí)也使得所規(guī)劃的路徑更加平順。不妨設(shè)初始路徑點(diǎn)序列為{q1,q2,…,qn},其中q1為起始點(diǎn),qn為目標(biāo)點(diǎn)。構(gòu)建路徑冗余點(diǎn)消除之后的路徑點(diǎn)序列為φ,初始狀態(tài)φ為空集。

1)將起始點(diǎn)q1添加到序列集合φ中,同時(shí)令j=1,i=n;

2)檢測(cè)qj和qi之間是否有障礙物,如果有障礙物,那么繼續(xù)下一步,如果沒有障礙物,那么直接進(jìn)入步驟4;

3)如果i=j+1,那么j=j+1,i=n,轉(zhuǎn)入步驟2,反之,i=i-1,轉(zhuǎn)入步驟2;

4)將點(diǎn)qi添加到序列集合φ中,令j=i,i=n;

5)如果j=n,那么路徑上的所有冗余點(diǎn)被消除,反之,繼續(xù)轉(zhuǎn)入步驟2。

1.2.4 路徑平滑處理

消除規(guī)劃路徑冗余點(diǎn)之后,其軌跡上依舊會(huì)存在比較多的拐點(diǎn),這使得移動(dòng)機(jī)器人在實(shí)際的轉(zhuǎn)向時(shí)存在角度過大的問題。采用B樣條曲線對(duì)路徑進(jìn)行平滑處理,從而使得所規(guī)劃的路徑更加平滑[10]。三次樣條曲線的數(shù)學(xué)表達(dá)式為

(2)

式中Vi+j為基函數(shù)控制點(diǎn),Gj,3(t)為基函數(shù),其數(shù)學(xué)表達(dá)式為

(3)

設(shè)經(jīng)過冗余點(diǎn)消除后的節(jié)點(diǎn)序列集合為{φ1,φ2,…,φm},其中φ1為起始點(diǎn),φm為目標(biāo)點(diǎn)。各點(diǎn)為基函數(shù)的控制點(diǎn),可以得到路徑上點(diǎn)φi和φi+3之間的軌跡,其數(shù)學(xué)表達(dá)式為

(4)

2 仿真分析

為了更好地驗(yàn)證改進(jìn)RRT算法在機(jī)器人路徑規(guī)劃方面的優(yōu)良性能,在MATLAB中構(gòu)建簡單地圖環(huán)境和復(fù)雜地圖環(huán)境,地圖的大小為800×800,移動(dòng)機(jī)器人的初始點(diǎn)坐標(biāo)為(100,700),目標(biāo)點(diǎn)坐標(biāo)為(700,100),具體如圖4所示。

圖4 構(gòu)造的簡單和復(fù)雜地圖環(huán)境

分別采用傳統(tǒng)RRT算法和改進(jìn)RRT算法對(duì)簡單地圖環(huán)境進(jìn)行路徑規(guī)劃,結(jié)果如圖5所示。

圖5 路徑規(guī)劃結(jié)果對(duì)比(簡單地圖環(huán)境)

由圖5可知,相對(duì)于RRT算法,改進(jìn)RRT算法隨機(jī)樹路徑擴(kuò)展分支明顯減少,這使得算法的運(yùn)行效率大大提升。在距離障礙物比較遠(yuǎn)的區(qū)域,改進(jìn)RRT算法能夠更加快速地向目標(biāo)區(qū)域生長,加快了路徑規(guī)劃的速度。另外,改進(jìn)RRT采用B樣條函數(shù)進(jìn)行路徑平滑,很明顯改進(jìn)RRT算法所規(guī)劃的路徑?jīng)]有了比較明顯的拐點(diǎn),而在RRT算法所規(guī)劃的路徑中存在比較多的明顯拐點(diǎn)。

分別采用傳統(tǒng)RRT算法和改進(jìn)RRT算法對(duì)復(fù)雜地圖環(huán)境進(jìn)行路徑規(guī)劃,結(jié)果如圖6所示。

圖6 路徑規(guī)劃結(jié)果對(duì)比(復(fù)雜地圖環(huán)境)

對(duì)比圖5和圖6可知,不論是簡單地圖環(huán)境還是復(fù)雜地圖環(huán)境,改進(jìn)RRT算法隨機(jī)樹路徑擴(kuò)展分支明顯減少,同時(shí)所規(guī)劃的路徑也更加平滑。

從算法規(guī)劃路徑的用時(shí)、擴(kuò)展節(jié)點(diǎn)數(shù)、規(guī)劃路徑長度3個(gè)角度進(jìn)行對(duì)比,對(duì)比結(jié)果見表1。

表1 算法路徑規(guī)劃性能對(duì)比

由表1可知,改進(jìn)RRT不論是從用時(shí)、擴(kuò)展節(jié)點(diǎn)數(shù)還是規(guī)劃路徑長度均明顯優(yōu)于傳統(tǒng)RRT。相對(duì)于簡單地圖,改進(jìn)RRT相對(duì)于RRT在移動(dòng)機(jī)器人路徑規(guī)劃上表現(xiàn)出了更加優(yōu)良的性能。移動(dòng)機(jī)器人實(shí)際作業(yè)環(huán)境比較復(fù)雜,障礙物形狀多種多樣,傳統(tǒng)RRT所規(guī)劃的路徑平順性比較差,移動(dòng)機(jī)器人在實(shí)際的移動(dòng)過程中受慣性的影響,其往往無法在拐點(diǎn)處沿著路徑移動(dòng),很容易和鄰近的障礙物發(fā)生碰撞。改進(jìn)RRT算法所規(guī)劃的路徑更加平順,避免了移動(dòng)機(jī)器人在實(shí)際的移動(dòng)過程中走曲折的路線,降低了在移動(dòng)過程中與周邊障礙物發(fā)生碰撞的風(fēng)險(xiǎn)。

3 試驗(yàn)驗(yàn)證

為了驗(yàn)證改進(jìn)RRT算法在真實(shí)移動(dòng)機(jī)器人上的可行性,選擇四輪獨(dú)立驅(qū)動(dòng)的移動(dòng)機(jī)器人作為試驗(yàn)對(duì)象,其外形如圖7所示。

圖7 試驗(yàn)所用移動(dòng)機(jī)器人

在移動(dòng)機(jī)器人上安裝了激光雷達(dá)、編碼器和攝像頭等,控制系統(tǒng)由試驗(yàn)臺(tái)上的電腦和NPX控制板組成,在電腦上運(yùn)行機(jī)器人操作系統(tǒng),其負(fù)責(zé)機(jī)器人路徑規(guī)劃。NPX控制板通過Wi-Fi與電腦之間進(jìn)行通信,獲取電腦發(fā)送的路徑規(guī)劃信息。移動(dòng)機(jī)器人在運(yùn)動(dòng)的過程中通過激光雷達(dá)、編碼器和攝像頭等設(shè)備來獲取外界的信息。將改進(jìn)RRT算法應(yīng)用于圖7所示的機(jī)器人中,對(duì)不同起始點(diǎn)下的移動(dòng)路徑進(jìn)行規(guī)劃,結(jié)果如圖8所示。

圖8 移動(dòng)機(jī)器人路徑規(guī)劃結(jié)果

由圖8可知,不論是移動(dòng)機(jī)器人最終的目的地在何處,改進(jìn)RRT算法均可以規(guī)劃出最佳路徑。

4 結(jié)論

路徑規(guī)劃是移動(dòng)機(jī)器人開發(fā)的核心技術(shù)之一,針對(duì)傳統(tǒng)RRT算法存在的路徑規(guī)劃效率低、規(guī)劃路徑拐點(diǎn)多的問題,對(duì)RRT進(jìn)行改進(jìn)。通過引入A*算法和人工勢(shì)力場(chǎng)思想來選擇待擴(kuò)展節(jié)點(diǎn)和擴(kuò)展方向,同時(shí)對(duì)所規(guī)劃的路徑進(jìn)行冗余點(diǎn)消除和平滑處理。將改進(jìn)的RRT算法應(yīng)用于簡單地圖環(huán)境和復(fù)雜地圖環(huán)境的仿真中,驗(yàn)證了改進(jìn)RRT算法在路徑規(guī)劃上的有效性,同時(shí)通過實(shí)物試驗(yàn),對(duì)改進(jìn)RRT算法在真實(shí)移動(dòng)機(jī)器人上的可行性進(jìn)行了驗(yàn)證。改進(jìn)RRT算法對(duì)移動(dòng)機(jī)器人的開發(fā)具有一定的參考價(jià)值。