符秀輝,劉 然

(沈陽(yáng)化工大學(xué) 信息工程學(xué)院,遼寧 沈陽(yáng) 110142)

快速搜索隨機(jī)樹(shù)(Rapidly-Exploring Random Trees，RRT)算法由美國(guó)愛(ài)荷華州立大學(xué)Steven M.LaValle教授在1998年提出，RRT的思想是快速擴(kuò)張一群像樹(shù)一樣的路徑以探索(填充)空間的大部分區(qū)域，伺機(jī)找到可行的路徑。RRT算法的特點(diǎn)是能夠快速有效地搜索高維空間，通過(guò)狀態(tài)空間的隨機(jī)采樣點(diǎn)，把搜索導(dǎo)向空白區(qū)域，從而尋找到一條從起始點(diǎn)到目標(biāo)點(diǎn)的規(guī)劃路徑，適合解決多自由度機(jī)器人在復(fù)雜環(huán)境下和動(dòng)態(tài)環(huán)境中的路徑規(guī)劃。對(duì)RRT算法的改進(jìn)也有很多，施楊洋等[1]使用雙樹(shù)RRT算法在障礙物周?chē)闪巳舾晒?jié)點(diǎn)，并在這些節(jié)點(diǎn)中引入了斥力，生成多棵局部隨機(jī)樹(shù)，使局部隨機(jī)樹(shù)向遠(yuǎn)離障礙物方向擴(kuò)展，減少了局部隨機(jī)樹(shù)對(duì)障礙物的檢測(cè)次數(shù)和擴(kuò)展過(guò)程中對(duì)障礙物的檢測(cè)時(shí)間，加快了算法的收斂速度，改善了算法的偏差性；劉恩海等[2]使用自適應(yīng)策略改變引力系數(shù)的大小從而改變步長(zhǎng)，引導(dǎo)隨機(jī)樹(shù)朝目標(biāo)點(diǎn)方向高質(zhì)量地生長(zhǎng)，并解決了目標(biāo)振蕩和隨機(jī)樹(shù)在障礙物附近聚集的問(wèn)題，使RRT算法具有一定的可行性和有效性，有效縮短了路徑規(guī)劃時(shí)間；成怡等[3]引入了變概率目標(biāo)偏向思想，通過(guò)設(shè)置算法認(rèn)定目標(biāo)點(diǎn)是局部目標(biāo)點(diǎn)的概率閾值方法，在每次生成隨機(jī)點(diǎn)之前隨機(jī)生成概率值，與概率閾值做比較，如果高于閾值，則將生成的隨機(jī)點(diǎn)作為擴(kuò)展樹(shù)子節(jié)點(diǎn)的生長(zhǎng)目標(biāo)點(diǎn)，反之則采用基本RRT算法中隨機(jī)生成的節(jié)點(diǎn)作為擴(kuò)展樹(shù)子節(jié)點(diǎn)的生長(zhǎng)目標(biāo)點(diǎn)，克服了隨機(jī)擴(kuò)展樹(shù)生長(zhǎng)時(shí)采樣的隨機(jī)性和收斂速度慢的問(wèn)題；徐秉超等[4]采用預(yù)生長(zhǎng)方式選擇步長(zhǎng)，基于Metropolis接受準(zhǔn)則的隨機(jī)點(diǎn)篩選機(jī)制，對(duì)受時(shí)間影響而形成的不同生長(zhǎng)可能性區(qū)域進(jìn)行劃分，在一定程度上解決了隨機(jī)搜索樹(shù)的重復(fù)生長(zhǎng)問(wèn)題，令重要障礙物產(chǎn)生斥力，使機(jī)器人避開(kāi)障礙物，減小了勢(shì)場(chǎng)計(jì)算規(guī)模，提高了路徑規(guī)劃的避障效率；喬慧芬等[5]利用人工勢(shì)場(chǎng)法在初始環(huán)境中預(yù)規(guī)劃一條初始路徑，在新出現(xiàn)的障礙物周?chē)_定合適的規(guī)劃空間，利用RRT算法規(guī)劃局部路徑。最后，合并初始路徑與局部路徑得出最終路徑，減小了路徑長(zhǎng)度，縮短了規(guī)劃時(shí)間。本文針對(duì)RRT算法存在隨機(jī)性強(qiáng)、偏差大、路徑不一定最優(yōu)等缺點(diǎn)，對(duì)RRT算法進(jìn)行改進(jìn)研究，借助人工勢(shì)場(chǎng)法，加入自適應(yīng)策略，不斷改變步長(zhǎng)大小，最終使機(jī)器人加速到達(dá)目標(biāo)點(diǎn)，減小耗時(shí)，提高RRT算法的執(zhí)行效率。

1 RRT算法

RRT算法是以一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過(guò)隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一個(gè)隨機(jī)擴(kuò)展樹(shù)。當(dāng)隨機(jī)樹(shù)中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或機(jī)器人進(jìn)入了目標(biāo)區(qū)域后，則在隨機(jī)樹(shù)中找到一條從初始點(diǎn)到目標(biāo)點(diǎn)的路徑，并在狀態(tài)空間中對(duì)采樣點(diǎn)進(jìn)行碰撞檢測(cè)，使機(jī)器人能夠避開(kāi)障礙物。

在二維空間中，以初始點(diǎn)作為根節(jié)點(diǎn)，生長(zhǎng)隨機(jī)樹(shù)，并建立隨機(jī)節(jié)點(diǎn)列表。如果隨機(jī)節(jié)點(diǎn)在空閑空間上，則通過(guò)歐氏距離公式計(jì)算找到隨機(jī)節(jié)點(diǎn)距離隨機(jī)節(jié)點(diǎn)最近的樹(shù)節(jié)點(diǎn)，再通過(guò)滿(mǎn)足步長(zhǎng)限制的中間節(jié)點(diǎn)隨機(jī)擴(kuò)展一定步長(zhǎng)并生成新節(jié)點(diǎn)，期間與中間點(diǎn)進(jìn)行碰撞檢測(cè)。如果與中間點(diǎn)存在障礙物，則重新尋找新節(jié)點(diǎn)；如果不存在，則將這個(gè)新節(jié)點(diǎn)加入到隨機(jī)樹(shù)中。當(dāng)隨機(jī)樹(shù)中的子節(jié)點(diǎn)包含目標(biāo)點(diǎn)時(shí)，則在隨機(jī)樹(shù)中生成一條由起始點(diǎn)到目標(biāo)點(diǎn)的路徑；如果發(fā)生碰撞，則放棄這次擴(kuò)展，其生長(zhǎng)方式如圖1所示。

圖1 RRT隨機(jī)生長(zhǎng)方式

2 自適應(yīng)RRT算法

針對(duì)RRT算法的隨機(jī)性強(qiáng)、搜索沒(méi)有目的性、路徑不一定最優(yōu)的缺點(diǎn)，提出借助人工勢(shì)場(chǎng)引力函數(shù)思想，改進(jìn)節(jié)點(diǎn)的生成方式，對(duì)隨機(jī)樹(shù)中的每個(gè)節(jié)點(diǎn)xn都增加一個(gè)目標(biāo)引力函數(shù)G(n)，此處的節(jié)點(diǎn)xn表示由起點(diǎn)xstart(或終點(diǎn)xgoal)向外擴(kuò)展的第n個(gè)節(jié)點(diǎn)xnew。其公式可表示為

F(n)=R(n)+G(n)

(1)

式中：F(n)為從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的生長(zhǎng)指導(dǎo)函數(shù)；R(n)為從初始節(jié)點(diǎn)到節(jié)點(diǎn)n的隨機(jī)生長(zhǎng)函數(shù)。

胡兵等[6]和成浩浩等[7]對(duì)RRT算法中的節(jié)點(diǎn)進(jìn)行了改進(jìn)。在改進(jìn)RRT算法過(guò)程中產(chǎn)生的新節(jié)點(diǎn)xnew由隨機(jī)樹(shù)節(jié)點(diǎn)xrand和目標(biāo)節(jié)點(diǎn)xgoal共同決定。如圖3所示，在二維空間下，定義沿隨機(jī)點(diǎn)xrand擴(kuò)展方向的點(diǎn)為x1，步長(zhǎng)為ρ，沿目標(biāo)點(diǎn)xgoal擴(kuò)展方向的點(diǎn)為x2，步長(zhǎng)為kρ，k為系數(shù)，目標(biāo)點(diǎn)方向則由x1、x2共同決定，通過(guò)調(diào)節(jié)系數(shù)k使新節(jié)點(diǎn)不斷向目標(biāo)點(diǎn)偏移，最終到達(dá)目標(biāo)點(diǎn)。

圖2 RRT改進(jìn)節(jié)點(diǎn)生成方式

沿隨機(jī)點(diǎn)擴(kuò)展方向點(diǎn)x1的坐標(biāo)表達(dá)式為

(2)

目標(biāo)點(diǎn)擴(kuò)展方向點(diǎn)x2的坐標(biāo)表達(dá)式為

(3)

式中：‖xrand-xnearest‖為隨機(jī)點(diǎn)到距離隨機(jī)節(jié)點(diǎn)最近的樹(shù)節(jié)點(diǎn)的距離的絕對(duì)值；‖xgoal-xnearest‖為目標(biāo)點(diǎn)到距離隨機(jī)節(jié)點(diǎn)最近的樹(shù)節(jié)點(diǎn)的距離的絕對(duì)值。

因此，根據(jù)式(1)可得新節(jié)點(diǎn)xnew的坐標(biāo)表達(dá)式：

xnew=xnearest+x1+x2

(4)

為了解決機(jī)器人耗時(shí)長(zhǎng)的問(wèn)題，王洪斌等[8]和徐飛[9]在人工勢(shì)場(chǎng)中引入了相對(duì)速度，分別作為優(yōu)化路徑和排除對(duì)機(jī)器人路徑規(guī)劃影響很小的障礙物的指標(biāo)。楊凱等[10]在人工勢(shì)場(chǎng)法中增加了虛擬目標(biāo)并考慮了阻力的影響。本文對(duì)沿目標(biāo)點(diǎn)擴(kuò)展方向的點(diǎn)x2做出改變，使其系數(shù)k與引力的大小有關(guān)。當(dāng)機(jī)器人距離目標(biāo)點(diǎn)遠(yuǎn)時(shí)，k增大；當(dāng)機(jī)器人距離目標(biāo)點(diǎn)近時(shí)，k減小，直至引力為零，從而避免隨機(jī)樹(shù)節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)時(shí)發(fā)生振蕩[11]。其具體過(guò)程如下，定義引力勢(shì)場(chǎng)函數(shù)表達(dá)式為

U(q,v,a)=εa‖q-qgoal‖m+εb‖v-vgoal‖n+εc‖a-agoal‖l

(5)

式中：εa、εb、εc為系數(shù)；m、n、l為正常數(shù)。對(duì)應(yīng)引力函數(shù)表達(dá)式為

F(q,v,a)=F(q)+F(v)+F(a)

(6)

其中：

(7)

式中：Xq、Xv、Xa分別為機(jī)器人相對(duì)目標(biāo)點(diǎn)的位置、速度和加速度的單位向量。

系數(shù)k的表達(dá)式為

k=λ‖F(xiàn)‖

(8)

式中：λ為正常數(shù)；‖F(xiàn)‖為引力的絕對(duì)值，可以進(jìn)行測(cè)量，它與機(jī)器人相對(duì)目標(biāo)點(diǎn)的位置、速度和加速度有關(guān)。

因此，得到新節(jié)點(diǎn)xnew的坐標(biāo)表達(dá)式為

xnew=xnearest+x1+x2

(9)

3 實(shí)驗(yàn)結(jié)果與分析

機(jī)器人小車(chē)如圖3所示，用STM32F103ZET6進(jìn)行控制，外設(shè)采用激光雷達(dá)、里程計(jì)、慣性導(dǎo)航IMU、電機(jī)等，利用激光雷達(dá)Rplider A1進(jìn)行測(cè)距，利用MPU6050測(cè)量偏航角度，進(jìn)而得到小車(chē)的坐標(biāo)，通過(guò)編碼器對(duì)機(jī)器人運(yùn)行時(shí)的車(chē)輪運(yùn)行圈數(shù)計(jì)數(shù)，測(cè)量機(jī)器人運(yùn)行時(shí)的速度，通過(guò)編碼器對(duì)機(jī)器人速度形成閉環(huán)控制，對(duì)單位時(shí)間內(nèi)的速度進(jìn)行積分和微分計(jì)算，即可得到機(jī)器人運(yùn)行的實(shí)際距離和加速度。移動(dòng)小車(chē)實(shí)驗(yàn)場(chǎng)景按照?qǐng)D4所示位置進(jìn)行布置，黑色區(qū)域是障礙物位置，為了使實(shí)驗(yàn)數(shù)據(jù)更具說(shuō)服性，必須使機(jī)器人從障礙物之間的狹窄通道通過(guò)，減少其他因素的影響[11]，為此實(shí)驗(yàn)進(jìn)行50次，將RRT算法和自適應(yīng)RRT算法進(jìn)行實(shí)驗(yàn)對(duì)比，如圖4和圖5所示。實(shí)驗(yàn)中相關(guān)參數(shù)設(shè)置如下：xstart=[2,2]，xgoal=[18,18]，εq=εv=εa=1，m=n=l=2，λ=1，小車(chē)初始速度為0.3 m/s，角速度設(shè)置為0.5 rad/s。

圖3 移動(dòng)小車(chē)

圖4 RRT算法

圖5 自適應(yīng)RRT算法

在加入了自適應(yīng)策略的RRT算法中，不斷調(diào)整系數(shù)k，使機(jī)器人逐漸向目標(biāo)點(diǎn)靠近，只要機(jī)器人不達(dá)到目標(biāo)點(diǎn)，則x≠xgoal或v≠vgoal或a≠agoal總存在，因此式(9)總成立。經(jīng)多次實(shí)驗(yàn)，RRT算法和自適應(yīng)RRT算法耗時(shí)比較如圖6所示。

圖6 兩種方法耗時(shí)比較

實(shí)驗(yàn)結(jié)果表明自適應(yīng)RRT算法可以有效地節(jié)省時(shí)間，更具有目的性且偏差較小，其分析結(jié)果如表1所示。

表1 兩種方法耗時(shí)對(duì)比

由表1可知，自適應(yīng)RRT算法相比于RRT算法節(jié)省了20.45%的時(shí)間。

4 結(jié)束語(yǔ)

本文在RRT算法中借助人工勢(shì)場(chǎng)法，加入了自適應(yīng)策略，不斷改變節(jié)點(diǎn)步長(zhǎng)的大小，使機(jī)器人快速安全到達(dá)目標(biāo)點(diǎn)。由于RRT算法本身的缺陷，會(huì)導(dǎo)致路徑不斷變化，為了減小這種影響，必須使小車(chē)行駛在同一路段，從障礙物中間通過(guò)并躲避障礙物，而不能從地圖邊緣到達(dá)目標(biāo)點(diǎn)。實(shí)驗(yàn)結(jié)果表明，自適應(yīng)RRT算法相比于RRT算法在實(shí)際應(yīng)用中耗時(shí)明顯減少，可以極大地提高機(jī)器人作業(yè)效率，有效地節(jié)省了時(shí)間。目前實(shí)驗(yàn)本身仍然存在的問(wèn)題是算法只適用于比較規(guī)則的障礙物，對(duì)不規(guī)則的障礙物來(lái)說(shuō)，有很多凹凸點(diǎn)，容易使機(jī)器人重復(fù)作業(yè)陷入死區(qū)，因此應(yīng)對(duì)障礙物中凹凸點(diǎn)處進(jìn)行研究，使機(jī)器人適用于更多場(chǎng)合，并對(duì)算法和路徑進(jìn)行優(yōu)化，從而使機(jī)器人小車(chē)快速安全地到達(dá)目的地。