国产日韩欧美一区二区三区三州_亚洲少妇熟女av_久久久久亚洲av国产精品_波多野结衣网站一区二区_亚洲欧美色片在线91_国产亚洲精品精品国产优播av_日本一区二区三区波多野结衣 _久久国产av不卡

?

融合改進RRT和Dijkstra算法的機器人動態(tài)路徑規(guī)劃*

2023-03-02 06:43馬新國馬希青
關(guān)鍵詞:障礙物步長動態(tài)

馬新國,馬希青

(河北工程大學(xué)機械與裝備工程學(xué)院,邯鄲 056038)

0 引言

近些年,移動機器人作為智能系統(tǒng)提高了許多行業(yè)的生產(chǎn)力,為人類生活提供了很多便利,因此受到越來越多的關(guān)注。路徑規(guī)劃是研究移動機器人的一個重要內(nèi)容。路徑規(guī)劃旨在通過規(guī)劃算法,在有障礙物的工作環(huán)境內(nèi),規(guī)劃出從起點到目標(biāo)區(qū)域的自身約束條件的無碰撞路徑[1]。該方向迄今已有大量的研究成果。如基于網(wǎng)格劃分的路徑規(guī)劃算法,需要對周圍環(huán)境劃分區(qū)域,擴展遍歷到目標(biāo)點[2-3],該類算法在地圖環(huán)境較大時過于耗費資源;群智能算法雖然適合整體環(huán)境優(yōu)化,但容易陷入局部最優(yōu),且不適合高維環(huán)境下的路徑規(guī)劃。

RRT算法是一種簡單快速的基于采樣的路徑規(guī)劃算法,它在工作空間中隨機采樣,由起始點向采樣點擴展,逐步生成類似于樹的路徑,最終搜索到目標(biāo)點。但RRT算法的缺陷也非常明顯:①搜索過于盲目,無方向性;②節(jié)點利用率低,搜索過程中會產(chǎn)生許多冗余節(jié)點;③得到的路徑只是可行而并非最優(yōu),有非常大的優(yōu)化空間。針對以上RRT算法的缺點和不足,國內(nèi)外學(xué)者提出了許多優(yōu)化改進RRT算法的方法,馬小陸等[5]提出將跳點思想融入到RRT*算法中,縮小搜索節(jié)點數(shù)量,提高搜查效率,并且在ROS中構(gòu)建仿真環(huán)境,驗證了算法高效性。王坤等[6]在RRT-Connect算法的基礎(chǔ)上進行改進,動態(tài)設(shè)置步長并引入目標(biāo)偏置策略,提高了搜索效率。KARAMAN等[7]提出了RRT*算法,對RRT算法產(chǎn)生的路徑進行重新選擇父節(jié)點和重新布線,不斷重復(fù)該操作從而改進和優(yōu)化路徑。GAMMELL等[8]提出了Informed-RRT*算法,在RRT*算法的基礎(chǔ)上,在起點和終點之間形成的橢圓區(qū)域內(nèi)采樣,大大減少了搜索時間。董敏等[9]使用雙向RRT思想,并對改進算法生成的路徑的路徑設(shè)置評價函數(shù)中選出最優(yōu)路徑,并對選出的路徑平滑調(diào)整,通過仿真,驗證算法有效性。劉恩海等[10]針對隨機樹生長的隨機性進行改進,引導(dǎo)向目標(biāo)點生長且能夠根據(jù)周圍環(huán)境做出調(diào)整,算法搜索具有很強的目標(biāo)性和偏向性。李金良等[11]對搜索方向和路徑節(jié)點角度進行了約束,優(yōu)化了路徑平滑度,對改進后規(guī)劃的節(jié)點使用B樣條優(yōu)化。

針對RRT算法在路徑規(guī)劃中無方向性,路徑并非最優(yōu)且不適合機器人移動的問題,首先對RRT算法進行改進,再與Dijkstra算法融合規(guī)劃出優(yōu)化路徑,最后與動態(tài)窗口算法融合。全局路徑規(guī)劃中,設(shè)置目標(biāo)節(jié)點采樣率和動態(tài)步長,提高搜索速度和效率。擴展改進后的RRT算法的每個節(jié)點得到可行區(qū)域,采用Dijkstra算法搜索可行區(qū)域內(nèi)的最優(yōu)路徑,減少路徑長度。局部路徑規(guī)劃中,通過融合RRT和Dijkstra算法得到的路徑節(jié)點引導(dǎo)動態(tài)窗口算法,防止陷入局部最優(yōu)。

1 改進的RRT算法

類似于樹不斷生長,RRT算法不斷地向外擴展分支以找到一條由起始點到目標(biāo)點的唯一路徑。其在尋找分支的過程中是隨機采樣,無需對周圍環(huán)境進行幾何劃分,搜索空間的覆蓋率高,搜索的范圍廣。在時間允許的情況下,只要存在可行路徑就一定能找到。但該特性也決定了RRT算法采樣效率不高,路徑中有大量的冗余節(jié)點,且找到的路徑不一定是最優(yōu)的,有比較大的優(yōu)化空間。針對這些問題,提出以下兩點改進。

1.1 設(shè)置目標(biāo)節(jié)點采樣率

傳統(tǒng)RRT算法的擴展是隨機的,在尋找路徑時具有一定的盲目性。因此對擴展方向進行一定概率的導(dǎo)向。采樣情況可用式(1)表示。

(1)

式中,s為隨機函數(shù)生成的隨機數(shù);sg為目標(biāo)節(jié)點采樣率;Pgoal為目標(biāo)點;Prand為在地圖范圍內(nèi)的隨機采樣點;Pnew為擴展樹的新節(jié)點。

在無障礙物的環(huán)境中,當(dāng)起始點與目標(biāo)點相同,設(shè)置目標(biāo)節(jié)點采樣率為80%時,該RRT算法能夠很快的接近并搜索到目標(biāo)點,而傳統(tǒng)RRT算法導(dǎo)向性一般,耗時且產(chǎn)生了非常多的冗余節(jié)點。兩種算法搜索出的路徑對照如圖1所示。

(a) 無障礙物傳統(tǒng)RRT算法 (b) 無障礙物改進RRT算法圖1 無障礙下改進RRT算法與傳統(tǒng)RRT算法對比

1.2 動態(tài)設(shè)置步長策略

在RRT算法中,步長是該算法尋找路徑的一個重要因素,在無障礙物且步長允許的情況下,算法擁有搜索時間隨步長的增大而減小的特性。

但如果周圍環(huán)境有障礙物,該特性會大打折扣。此時步長選取太大容易陷入障礙物的狹窄范圍,難以搜索到目標(biāo)點和可行路徑;步長選取太小會產(chǎn)生過多的冗余節(jié)點,降低搜索效率。較好的辦法是根據(jù)周圍環(huán)境和障礙物的分布狀況動態(tài)設(shè)置步長,避免上述兩種極端情況,更快地遍歷障礙物較少的空間,減少搜索時間,快速尋找到可行路徑。

本文所采用的步長調(diào)整策略:

(1)給定初始步長step,采樣函數(shù)隨機產(chǎn)生采樣點,判斷該采樣點與最近的樹節(jié)點之間障礙物的數(shù)量。

(2)若未存在障礙物,步長取初始值step來延伸新的樹節(jié)點;否則取式(2)來快速延伸新的樹節(jié)點。

step=step+(1-obs/S)*step

(2)

式中,step為初始步長;S為以距離隨機點最近的樹節(jié)點和目標(biāo)點為對角線的矩形面積;obs為矩形中障礙物的數(shù)量。在搜索過程中,步長是隨障礙物占矩形面積比而變化的,比值越大,代表矩形面積內(nèi)障礙物越多,步長越小。相反則步長取值較大。能夠快速搜索到目標(biāo)點。在無障礙環(huán)境中,固定步長和動態(tài)步長的搜索路徑對照如圖2所示。

(a) 有障礙物傳統(tǒng)RRT算法 (b) 動態(tài)設(shè)置步長后RRT算法圖2 有障礙下改進RRT算法與傳統(tǒng)RRT算法對比

可以清楚地看出在環(huán)境相同的情況下,動態(tài)設(shè)置步長后的RRT算法在搜索路徑中產(chǎn)生的分支和冗余節(jié)點更少。

2 Dijkstra算法

Dijkstra算法經(jīng)常使用在網(wǎng)絡(luò)路由和地理信息系統(tǒng)中,用于解決非負權(quán)重圖的單元最短路徑問題。它是一種典型的基于貪心策略的最短路徑規(guī)劃算法,用于計算一個節(jié)點到其他所有節(jié)點的最短路徑。

Dijkstra算法的核心思想是以起始點為原點,根據(jù)距起始點最短路程長度遞增的次序?qū)β窂竭M行迭代,得到從起始點至目標(biāo)點間的最短路徑。Dijkstra算法的主要特點在于從開始點為中心向外層層擴展,直至延伸到達終點為止。

如圖3所示,是Dijkstra算規(guī)劃產(chǎn)生的路徑。S代表起點,G代表終點,黑色物塊表示障礙物。淺灰色區(qū)域表示搜索路徑時經(jīng)過的區(qū)域。深灰色區(qū)域表示搜索到的最優(yōu)路徑區(qū)域。Dijkstra在搜索最優(yōu)路徑時,需要對地圖環(huán)境進行幾何劃分,從起點開始向周圍所有節(jié)點擴展,直到擴展到目標(biāo)點為止。Dijkstra所尋路徑一定是最優(yōu)的,但在搜索路徑時需要擴展許多無用的節(jié)點。因此非常耗費資源,尤其在地圖環(huán)境較大時,其搜索時間較RRT算法多出許多,搜索效率會變的非常低。

圖3 Dijkstra算法產(chǎn)生路徑

3 融合Dijkstra與RRT路徑規(guī)劃算法

基于上述兩種算法的優(yōu)缺點,本文將Dijstra算法和改進優(yōu)化后的RRT算法進行融合,優(yōu)勢互取,從而能夠?qū)ふ页鲆粭l可行的、距離更優(yōu)的路徑。為方便敘述,下文將該方法稱為RRT-Dijkstra算法。

在使用改進后的RRT算法進行搜索后,形成一條初始可行路徑。由RRT特性可知該路徑并非最短,其周圍區(qū)域存在比初始路徑更短的路徑。將初始路徑的每個節(jié)點周圍進行柵格化,形成路徑的可行區(qū)域。在可行區(qū)域中使用Dijkstra算法搜索,重新規(guī)劃出一條比初始路徑更近的搜索路徑。優(yōu)化步驟如圖4所示。

(a) 改進RRT算法 (b) 向周圍擴展可行區(qū)域 (c) 優(yōu)化后路徑

表1 改進RRT算法與融合Dijkstra算法后路徑對比

從表1中可得,兩種算法融合后,路徑從28.127 1減少到了23.970 6,優(yōu)化了14.778%,路徑拐點從15個減少到3個,優(yōu)化了80%。既利用了改進后RRT算法搜索到可行路徑的快速性,提升了搜索效率;也避免了Dijkstra擴展多余節(jié)點造成的資源浪費,優(yōu)化了搜索路徑。

雖然該路徑長度上得到了優(yōu)化,但是得到的搜索路徑不夠平滑,且移動機器人在拐點處無法迅速改變速度大小和方向,融合算法得到的路徑不符合移動機器人的運動規(guī)律,也不能根據(jù)環(huán)境變化實現(xiàn)動態(tài)避障。因此,將融合后的算法與動態(tài)窗口法相結(jié)合,實現(xiàn)全局最優(yōu)。

4 改進動態(tài)窗口算法

動態(tài)窗口算法是一種局部路徑規(guī)劃算法,該算法在速度空間內(nèi)采樣線速度和角速度,根據(jù)機器人的運動學(xué)模型預(yù)測下一時間間隔的軌跡。對預(yù)測內(nèi)的軌跡進行方位角、與障礙物距離、速度大小等因素進行評分,從而獲得更加安全平滑的局部路徑。傳統(tǒng)的動態(tài)窗口算法評價方式單一,評價權(quán)重固定不變,不能使機器人快速接近目標(biāo)點,所以對動態(tài)窗口算法的評價函數(shù)的權(quán)重做動態(tài)調(diào)整。

4.1 機器人運動模型

機器人運動模型選擇全向移動的,即可以向前和左右移動,也可以轉(zhuǎn)動。機器人相鄰時刻內(nèi)(ms級),運動距離短,可將相鄰兩點之間的運動軌跡看成直線。設(shè)機器人向前移動速度為vx,向左移動速度為vy,角速度為ω,機器人在Δt內(nèi)的運動軌跡如式(3)所示。

(3)

根據(jù)該運動模型在速度空間重采集線速度和角速度,預(yù)測下一個時間段內(nèi)機器人的運動軌跡。

4.2 機器人速度采樣

考慮到機器人本身速度的范圍和周圍環(huán)境對機器人的限制,在速度采樣時,對機器人的角速度和線速度進行限制。如式(4)所示。

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(4)

考慮到機器人的最大力矩,即最大線加速度和最大角加速度會對機器人的線速度和角速度造成約束,因此其速度為:

(5)

為使機器人在碰到障礙物前可以停車,對機器人預(yù)留剎車距離,對機器人的速度加以限制。

(6)

式中,dist(v,ω)為在當(dāng)前運行速度移動機器人的軌跡中與障礙物最短的距離。

4.3 改進評價函數(shù)

在機器人的某一時刻的速度空間確定以后,需要對速度空間內(nèi)的速度產(chǎn)生的路徑軌跡評價,評價函數(shù)為式(7)所示。

G(v,ω)=σ(αhead(v,ω)+βdist(v,ω)+γvel(v,ω))

(7)

式中,head(v,ω)為與目標(biāo)節(jié)點間的方位角大小,其值越大,代表與目標(biāo)節(jié)點方向上越靠近;dist(v,ω)為機器人與周圍障礙物之間的距離,其值越大,代表與周圍障礙物距離越遠;vel(v,ω)為機器人的速度大小,其值越大,代表下一時刻運動距離越遠;α、β、γ為系數(shù),即評價權(quán)重。

式(7)中α與γ和β比例值固定,在機器人周圍環(huán)境復(fù)雜時,無法自適應(yīng)調(diào)整評價權(quán)重以快速到達目標(biāo)點。為了使機器人能夠更加具有目的性,對評價權(quán)重進行調(diào)整。當(dāng)機器人距離目標(biāo)點較遠切周圍障礙物較少時,提高方向和速度評價權(quán)重,當(dāng)靠近障礙物時,提高機器人速度占比,使其快速繞過障礙物,改進后的評價函數(shù)為式(8)所示。

(8)

式中,R為障礙物半徑。對dist(v,ω)進行限定。dist(v,ω)∈(0,2R)。

圖5 傳統(tǒng)動態(tài)窗口法路徑 圖6 改進動態(tài)窗口法路徑

由表2可看出,雖然改進權(quán)重比后的動態(tài)窗口算法所形成的路徑長度減少了0.46%,并無較大變化。但路徑規(guī)劃時間減少了19.02%,搜索效率得到了提升。

表2 傳統(tǒng)動態(tài)窗口法與改進動態(tài)窗口法時間對比

5 全局與局部路徑規(guī)劃算法融合

傳統(tǒng)動態(tài)窗口算法容易陷入局部最優(yōu),導(dǎo)致無法尋找到可行路徑。改進后的RRT和Dijkstra的融合算法規(guī)劃出的的全局路徑不夠平滑,拐點處角度發(fā)生突變,不符合機器人運動學(xué)規(guī)律,且無法動態(tài)避障,容易與周圍障礙物發(fā)生碰撞。針對這兩個問題,將改進RRT和Dijkstra融合后的算法規(guī)劃的路徑分段使用動態(tài)窗口算法。使用全局路徑引導(dǎo)動態(tài)窗口算法進行規(guī)劃,有效地避免了陷入局部最優(yōu),也使路徑更加平滑,適合機器人運動。融合后的算法實現(xiàn)如圖7所示。

圖7 融合算法實現(xiàn)流程圖

6 仿真與分析

為了驗證算法融合后的高效性,在MATLAB搭建仿真平臺,構(gòu)建地圖環(huán)境。黑色區(qū)域表示障礙物,機器人無法通行,白色區(qū)域表示正常環(huán)境,機器人可以通行。設(shè)置起始點(5,3), 目標(biāo)點(17,21)。設(shè)計兩組試驗進行驗證。

實驗一:驗證改進后RRT算法與RRT-Dijkstra融合算法的效率和可行性。如圖8~圖13所示。6種算法各自執(zhí)行50次,將得到的參數(shù)值取平均值,性能對比如表3所示。

圖8 傳統(tǒng)RRT算法路徑 圖9 優(yōu)化改進RRT算法路徑

圖10 Dijkstra算法產(chǎn)生路徑 圖11 RRT_Dijkstra融合算法產(chǎn)生路徑

圖12 傳統(tǒng)動態(tài)窗口算法 圖13 RRT_Dijkstra和動態(tài)窗口融合算法

以上6種算法經(jīng)過仿真得到的各自的規(guī)劃時間、路徑長度、拐點數(shù)和迭代步數(shù)如表3所示。

表3 6種算法對比

如表3所示,優(yōu)化改進后的RRT算法在搜索效率和搜索時間上有很大優(yōu)化,較傳統(tǒng)RRT算法規(guī)劃時間減少了0.614 8 s,拐點數(shù)減少了60.53%,迭代次數(shù)減少了71.91%。但路徑長度不減反增,所以結(jié)合Dijkstra算法優(yōu)化路徑。

Dijkstra算法雖然在規(guī)劃的路徑長度上是最優(yōu)的,但過于耗費系統(tǒng)資源和時間,其規(guī)劃時間是傳統(tǒng)RRT算法的6倍多。

RRT-Dijkstra算法較傳統(tǒng)RRT算法路徑縮短了19.25%,拐點減少了84.21%,迭代次數(shù)減少了71.91%,規(guī)劃時間增加0.207 7 s。增加的規(guī)劃時間在1 s之內(nèi),基本可以忽略。該算法在時間上保持了傳統(tǒng)RRT算法搜索路徑時的快速和高效,路徑長度也得到了較大優(yōu)化。

單獨的動態(tài)窗口算法因為無路徑和節(jié)點指引,沒有達到目標(biāo)點。

將RRT-Dijkstra規(guī)劃的路徑指引改進后的動態(tài)窗口算法,機器人能夠有效地到達目標(biāo)點,且路徑更短,更加平滑和安全,符合機器人的運動規(guī)律。

實驗二:在RRT-Dijkstra算法規(guī)劃的路徑中添加動態(tài)障礙物,動態(tài)障礙物為圖14中的深灰色柵格,以驗證融合算法是否能夠?qū)崿F(xiàn)動態(tài)避障。

圖14 機器人整體運行軌跡 圖15 機器人運行角速度

圖16 機器人運行線速度 圖17 機器人姿態(tài)變化

可以看出,在路徑中加入動態(tài)障礙物后,移動機器人成功從起始點到達目標(biāo)點,且能夠很好的繞過障礙物,實現(xiàn)動態(tài)避障。

7 結(jié)論

在有障礙物和無障礙物的仿真環(huán)境中,將RRT-Dijkstra融合算法與傳統(tǒng)RRT算法分別在規(guī)劃時間、迭代步數(shù)、路徑距離和拐點數(shù)進行對比,驗證了RRT-Dijkstra融合算法的高效性。再結(jié)合改進后的動態(tài)窗口算法,使機器人能夠?qū)崿F(xiàn)動態(tài)避障,在RRT-Dijkstra融合算法規(guī)劃出的全局路徑中加入障礙物,機器人可以很好地避開,驗證了融合動態(tài)窗口算法后的動態(tài)避障的功能。

猜你喜歡
障礙物步長動態(tài)
國內(nèi)動態(tài)
國內(nèi)動態(tài)
國內(nèi)動態(tài)
基于Armijo搜索步長的BFGS與DFP擬牛頓法的比較研究
高低翻越
SelTrac?CBTC系統(tǒng)中非通信障礙物的設(shè)計和處理
動態(tài)
基于逐維改進的自適應(yīng)步長布谷鳥搜索算法
一種新型光伏系統(tǒng)MPPT變步長滯環(huán)比較P&O法
土釘墻在近障礙物的地下車行通道工程中的應(yīng)用