崔春雷,陳詩(shī)豪,沈超航,李 鋒
(廣東交通職業(yè)技術(shù)學(xué)院,廣州 510650)
隨著科技水平的進(jìn)步,移動(dòng)機(jī)器人越來(lái)越多的被應(yīng)用到日常生活和工作的多種場(chǎng)景中,如目標(biāo)的移動(dòng)、探測(cè)和清潔等。 在移動(dòng)機(jī)器人的研究領(lǐng)域中,路徑規(guī)劃算法屬于重點(diǎn)研究方向。移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題可定義為: 在一個(gè)存在障礙物的空間里,給定移動(dòng)機(jī)器人的起點(diǎn)和終點(diǎn),在遵循時(shí)間最短、路徑最優(yōu)以及機(jī)器人運(yùn)動(dòng)學(xué)規(guī)律等一系列約束條件下,找到一條與障礙物無(wú)碰撞的路徑。其中環(huán)境完全已知的規(guī)劃問(wèn)題屬于全局路徑規(guī)劃,環(huán)境部分已知的規(guī)劃問(wèn)題為局部路徑規(guī)劃。常見(jiàn)的路徑規(guī)劃算法包括:A*算法、人工勢(shì)場(chǎng)法、可視圖法、概率路圖算法、模擬退火算法、粒子群算法、蟻群算法、遺傳算法等。以上算法往往需要事先對(duì)狀態(tài)空間內(nèi)的障礙物進(jìn)行環(huán)境建模,導(dǎo)致計(jì)算復(fù)雜高,收斂速度過(guò)于緩慢。
LaValle等人在1998年提出了經(jīng)典的快速搜索隨機(jī)樹(shù) (RRT,rapid-exploring random tree)算法。RRT算法的搜索過(guò)程通過(guò)隨機(jī)采樣的方式把搜索樹(shù)導(dǎo)向空白區(qū),通過(guò)對(duì)空間中的采樣點(diǎn)進(jìn)行碰撞檢測(cè),從而避免了對(duì)空間的建模,該算法具有概率完備、計(jì)算量小、效率高等優(yōu)點(diǎn),能有效地解決高維空間以及復(fù)雜環(huán)境下的路徑規(guī)劃。然而 RRT 算法也存在一些不足,如需要在全圖進(jìn)行采樣與搜索,會(huì)產(chǎn)生較多不相關(guān)的節(jié)點(diǎn),從而增加了算法對(duì)內(nèi)存的需求,并且也增加了相應(yīng)的搜索時(shí)間,導(dǎo)致收斂速率較緩慢。針對(duì)RRT算法存在的缺陷,國(guó)內(nèi)外眾多學(xué)者紛紛進(jìn)行了研究,提出了多種改進(jìn)的RRT算法,如C.Urmson等在文獻(xiàn)[16]發(fā)表了一個(gè)基于啟發(fā)式的偏向算法,它將搜索樹(shù)向目標(biāo)點(diǎn)的位置進(jìn)行了偏移,引導(dǎo)搜索樹(shù)向目標(biāo)區(qū)域生長(zhǎng),使路徑搜索時(shí)間進(jìn)一步優(yōu)化。針對(duì)原始RRT算法中搜索得到的路徑并非最優(yōu)的問(wèn)題,Karaman等在文獻(xiàn)[17]中提出了具有漸進(jìn)最優(yōu)性的RRT*算法,該算法增加了父節(jié)點(diǎn)的重選和剪枝兩個(gè)優(yōu)化過(guò)程,但此算法也存在收斂速度較慢的缺點(diǎn)。為了提高搜索速度,Kuffne等人先后提出了RRT-connect算法和雙向搜索樹(shù)(Bi-RRT)算法。Bi-RRT算法從起點(diǎn)和終點(diǎn)同時(shí)出發(fā),并行生成兩棵RRT樹(shù),直至兩棵樹(shù)相遇,相較于RRT算法,Bi-RRT算法的收斂速度更快,但Bi-RRT算法采用的仍是RRT算法的隨機(jī)節(jié)點(diǎn)擴(kuò)展思想,導(dǎo)致路徑搜索過(guò)程存在無(wú)目標(biāo)導(dǎo)向性等缺點(diǎn)。
為了解決雙向搜索樹(shù)(Bi-RRT)算法在路徑搜索時(shí)無(wú)目標(biāo)導(dǎo)向性所導(dǎo)致的搜索效率過(guò)低的問(wèn)題,本文提出了一種基于高斯采樣的改進(jìn)Bi-RRT算法。該算法在雙向搜索的基礎(chǔ)上,引入啟發(fā)式搜索思想,采樣點(diǎn)以一定概率以高斯分布的方式被約束在起點(diǎn)與終點(diǎn)周邊,從而降低搜索的盲目性,提高了搜索的效率。通過(guò)仿真實(shí)驗(yàn)表明:在多種類(lèi)型的復(fù)雜環(huán)境中,相對(duì)于基本的Bi-RRT算法,該算法搜索過(guò)程中擴(kuò)展節(jié)點(diǎn)更少、收斂速度更快、路徑相對(duì)更優(yōu)。
q
和目標(biāo)點(diǎn)q
為根節(jié)點(diǎn)分別構(gòu)建兩棵隨機(jī)搜索樹(shù)T
和T
,樹(shù)T
以q
為樹(shù)的根節(jié)點(diǎn)進(jìn)行擴(kuò)展,樹(shù)T
以q
為樹(shù)的根節(jié)點(diǎn)進(jìn)行擴(kuò)展,直到兩棵樹(shù)相遇。具體過(guò)程如圖1所示,算法首先在整個(gè)搜索空間中生成隨機(jī)擴(kuò)展節(jié)點(diǎn)g
,然后遍歷隨機(jī)樹(shù)T
和T
,找出兩棵樹(shù)中距離q
最近的節(jié)點(diǎn)并記為q
,接著由q
出發(fā)向q
延伸步長(zhǎng)δ
得到新節(jié)點(diǎn)q
,之后對(duì)新生成的節(jié)點(diǎn)q
進(jìn)行碰撞檢測(cè),若檢測(cè)到q
與障礙物碰撞則舍棄該節(jié)點(diǎn),反之將q
添加到樹(shù)中,此時(shí)q
是的q
父節(jié)點(diǎn);通過(guò)對(duì)上述過(guò)程進(jìn)行迭代,使兩棵搜索樹(shù)不斷向著對(duì)方擴(kuò)展,直到兩棵樹(shù)各自的q
之間的距離小于設(shè)定的閾值,此時(shí)認(rèn)為兩棵樹(shù)相遇,路徑規(guī)劃成功。圖1 Bi-RRT算法節(jié)點(diǎn)擴(kuò)展過(guò)程
相對(duì)于RRT算法,Bi-RRT算法效率更高搜索速度更快,但是Bi-RRT算法采用的仍然是 RRT算法的隨機(jī)擴(kuò)展節(jié)點(diǎn)思想,這也導(dǎo)致Bi-RRT算法和RRT算法一樣存在著構(gòu)型無(wú)目標(biāo)導(dǎo)向性的缺點(diǎn)。
q
不再以均勻分布的形式在搜索空間內(nèi)隨機(jī)出現(xiàn),而是以一定概率以高斯分布的方式出現(xiàn)在目標(biāo)點(diǎn)周邊,這樣搜索過(guò)程不但可以引導(dǎo)搜索樹(shù)盡可能朝著目標(biāo)區(qū)域前進(jìn),同時(shí)也保留了RRT算法的空間搜索能力,算法不僅提高了搜索效率,得到的路徑也相對(duì)更優(yōu)。(1)
其中:μ
,σ
為分量X
的期望值與標(biāo)準(zhǔn)差,μ
,σ
為分量Y
的期望值與標(biāo)準(zhǔn)差,ρ
值決定了X
和Y
的線(xiàn)性相關(guān)程度。這里假定μ
=μ
=0,σ
=σ
=5,來(lái)觀察ρ
取不同值時(shí)二維隨機(jī)變量(X
,Y
)概率密度函數(shù)的圖像。圖2 ρ值與二維高斯分布概率密度函數(shù)圖像關(guān)系
利用二維高斯分布的上述特性,我們引入啟發(fā)式搜索策略,隨機(jī)擴(kuò)展采樣點(diǎn)q
不再以均勻分布的形式出現(xiàn)搜索空間內(nèi),而是被二維高斯分布函數(shù)約束在起點(diǎn)與終點(diǎn)周邊區(qū)域,并引導(dǎo)搜索樹(shù)朝著該區(qū)域方向生長(zhǎng)。采用這種策略,在越接近目標(biāo)點(diǎn)的空間,采樣點(diǎn)q
的出現(xiàn)概率越大,但是又不會(huì)把概率完全鎖死在目標(biāo)點(diǎn)本身,這樣不但可以啟發(fā)隨機(jī)搜索樹(shù)向著目標(biāo)區(qū)域生長(zhǎng),提高搜索的效率,同時(shí)又能以一定的概率繞過(guò)障礙物。q
來(lái)引導(dǎo)隨機(jī)樹(shù)T
的生長(zhǎng),用采樣點(diǎn)q
來(lái)引導(dǎo)隨機(jī)樹(shù)T
的生長(zhǎng),而q
和q
則分別由以起點(diǎn)q
和終點(diǎn)q
為中心的兩個(gè)二維高斯分布函數(shù)來(lái)分別來(lái)約束其生成。圖3 坐標(biāo)系轉(zhuǎn)換示意圖
(2)
其中式(2)中的(x
,y
)為q
在坐標(biāo)系OXY
中的坐標(biāo)。按相同方法,我們可以生成以q
中心點(diǎn)隨機(jī)采樣點(diǎn)q
(x
,y
)。由圖4可以看出隨機(jī)采樣點(diǎn)q
主要分布在起點(diǎn)附近區(qū)域,且越接近點(diǎn),其出現(xiàn)的概率越大,q
則主要分布在終點(diǎn)q
附近區(qū)域,越接近點(diǎn)q
,其出現(xiàn)的概率也越大。圖4 隨機(jī)采樣點(diǎn)的概率分布示意圖
q
和終點(diǎn)q
為根節(jié)點(diǎn)分別構(gòu)建隨機(jī)樹(shù)T
和T
(第1-2行); 然后先以q
為目標(biāo)通過(guò)算法2生成隨機(jī)點(diǎn)q
用來(lái)引導(dǎo)T
的生長(zhǎng),找到隨機(jī)樹(shù)T
中離q
最近的點(diǎn)q
,以q
為起點(diǎn)向q
方向延伸步長(zhǎng)δ
生成葉子節(jié)點(diǎn)q
,判斷q
與q
之間是否存在障礙物,若不存在障礙物則將q
作為T
的子節(jié)點(diǎn),并添加邊(q
,q
)(第4-9行);接著以q
為目標(biāo)通過(guò)算法2生成隨機(jī)點(diǎn)q
用來(lái)引導(dǎo)T
的生長(zhǎng),按照相同的規(guī)則生成葉子節(jié)點(diǎn)q
和邊(q
,q
)(第10~15行);最后如果q
和q
之間的距離小于設(shè)定的閾值s
,且兩者之間沒(méi)有障礙物,則判定兩棵樹(shù)相互連接,路徑規(guī)劃完成(第16~17行)。算法1:改進(jìn)Bi-RRT算法偽代碼
1)V
←{q
};E
←Φ
;T
←(V
,E
);2)V
←{q
};E
←Φ
;T
←(V
,E
);3)fori
=1 toN
do;4)q
←sanple(q
);5)q
←Nearest(T
,q
);6)q
←Steer(q
,q
);7)if ObstacleFree(q
,q
)then8)V
←V
∪{q
};9)E
←E
∪{q
,q
};10)q
←Sample(q
);11)q
←Nearest(T
,q
);12)q
←Steer(q
,q
);13)if ObstacleFree(q
,q
)then;14)V
←V
∪{q
};15)E
←E
∪{q
,q
};16)if (Dis tan ce(q
;q
)q
)) then17)Return(T
,T
)。為了平衡搜索過(guò)程中的隨機(jī)性與目標(biāo)導(dǎo)向性,隨機(jī)點(diǎn)q
的產(chǎn)生由3種機(jī)制共同決定,見(jiàn)公式(3)。設(shè)置兩個(gè)目標(biāo)偏置概率p
和p
,且滿(mǎn)足0<p
<p
<1。(3)
公式(3)中p
為0~1之間的一個(gè)隨機(jī)數(shù);當(dāng)p
≤p
時(shí),q
由2.2中的方法產(chǎn)生,即隨機(jī)點(diǎn)q
滿(mǎn)足二維高斯分布特性;當(dāng)p
≥p
時(shí),隨機(jī)采樣點(diǎn)q
為目標(biāo)點(diǎn)q
或q
本身,這樣可以充分利用目標(biāo)點(diǎn)的信息,使得搜索樹(shù)以一定概率朝著目標(biāo)點(diǎn)方向快速前進(jìn);當(dāng)p
<p
<p
時(shí),q
符合均勻分布,這樣可以讓一部分隨機(jī)點(diǎn)保留采樣時(shí)的隨機(jī)性,確保搜索過(guò)程在概率上的完備性,保障搜索樹(shù)能跳出局部最優(yōu);算法2給出了公式(3)的偽代碼。算法2:隨機(jī)點(diǎn)生成的偽代碼
P
() //生成0~1之間的隨機(jī)數(shù)p
ifp
≤p
q
=q
. //q
可取q
或q
else ifp
>p
q
=q
;//此時(shí)q
按均勻分布elseq
=q
;//此時(shí)q
按二維高斯分布end if return(q
)q
坐標(biāo)為[1,1],終點(diǎn)q
坐標(biāo)為[500,500],步長(zhǎng)δ
=15,兩樹(shù)之間的距離閾值s
=30,目標(biāo)偏置概率p
=0.6,p
=0.9,二維高斯分布的參數(shù)為σ
=σ
=0.
25d
,ρ
=0.
5。L
)、擴(kuò)展節(jié)點(diǎn)數(shù)目(n
)、路徑規(guī)劃時(shí)間(t
) 這3個(gè)參數(shù)作為性能指標(biāo),并求得這3個(gè)指標(biāo)的50次仿真測(cè)試結(jié)果的均值,最后通過(guò)這3個(gè)指標(biāo)對(duì)算法性能進(jìn)行定量分析。圖5 環(huán)境1:簡(jiǎn)單環(huán)境
圖6 環(huán)境2:復(fù)雜環(huán)境
圖7 環(huán)境3:迷宮環(huán)境
圖5為簡(jiǎn)單環(huán)境中兩種算法的表現(xiàn),圖6為充滿(mǎn)障礙物的復(fù)雜環(huán)境中兩種算法的表現(xiàn),圖7為通道狹窄曲折的迷宮環(huán)境下兩種算法的表現(xiàn)。從圖5~7可以明顯看出,基本Bi-RRT算法采樣過(guò)程因?yàn)槿狈δ繕?biāo)導(dǎo)向,采樣點(diǎn)分布過(guò)于隨機(jī),導(dǎo)致搜索樹(shù)往往會(huì)在“無(wú)用區(qū)域”浪費(fèi)過(guò)多資源,產(chǎn)生過(guò)多的無(wú)用節(jié)點(diǎn),而本文改進(jìn)算法引入啟發(fā)式搜索思想,充分利用了目標(biāo)點(diǎn)的位置信息,讓隨機(jī)點(diǎn)不再“盲目”出現(xiàn),而是以高斯分布的形式出現(xiàn)在目標(biāo)點(diǎn)附近的空間,搜索過(guò)程所需的隨機(jī)采樣點(diǎn)的數(shù)量更少,效率也更高。
如表1所示,對(duì)圖5這種簡(jiǎn)單環(huán)境,本文算法的額外的擴(kuò)展節(jié)點(diǎn)更少,路徑也更平滑,路徑長(zhǎng)度也更小。對(duì)圖6這種充滿(mǎn)密集障礙物的復(fù)雜環(huán)境,本文算法的優(yōu)勢(shì)更加明顯,相對(duì)于基本Bi-RRT算法,平均規(guī)劃時(shí)間縮短了43.9%,平均擴(kuò)展節(jié)點(diǎn)數(shù)目減少了41.4%,路徑長(zhǎng)度優(yōu)化了8.1%。對(duì)圖7這種富有挑戰(zhàn)性的充滿(mǎn)狹窄通道的迷宮環(huán)境,相對(duì)于基本Bi-RRT算法,改進(jìn)算法的平均規(guī)劃時(shí)間縮短了30.9%,平均擴(kuò)展節(jié)點(diǎn)數(shù)目減少了27.2%,路徑長(zhǎng)度優(yōu)化了2%。通過(guò)定量分析可知,本文提出的改進(jìn)Bi-RRT算法的路徑搜索時(shí)間更短、擴(kuò)展節(jié)點(diǎn)更少、路徑更優(yōu)。
表1 不同環(huán)境下算法性能對(duì)比
p
,p
對(duì)算法的運(yùn)行效率有著重要影響。由公式(3)可知采樣點(diǎn)由3種機(jī)制共同產(chǎn)生,分別在概率p
=p
下按二維高斯分布出現(xiàn),在概率p
=p
-p
下按均勻分布出現(xiàn),以概率p
=1-p
把起點(diǎn)(或終點(diǎn))作為采樣點(diǎn),顯然p
+p
+p
=1。為了簡(jiǎn)化問(wèn)題,保持p
=0.
1不變,此時(shí)p
=0.
9-p
,這里p
顯然表示的是按高斯分布出現(xiàn)的采樣點(diǎn)占總的采樣點(diǎn)數(shù)的百分比。下面來(lái)分析概率p
的值對(duì)算法性能的影響,這里仍然以圖6中的復(fù)雜環(huán)境作為測(cè)試環(huán)境,在保持其他仿真參數(shù)不變的情況下,測(cè)試p
取不同值時(shí)算法性能的表現(xiàn)。圖8為仿真測(cè)試得到的平均節(jié)點(diǎn)數(shù)目隨概率p
變化的曲線(xiàn),從圖中可以看出,平均節(jié)點(diǎn)數(shù)目隨著概率p
的增加而減少,當(dāng)p
=0.
7附近時(shí)達(dá)到極小值。圖9則為平均規(guī)劃時(shí)間隨概率p
變化的曲線(xiàn),與圖8的規(guī)律一致,這里規(guī)劃時(shí)間也隨著概率p
的增加而減少,當(dāng)p
=0.
7時(shí)規(guī)劃時(shí)間達(dá)到最小值,之后隨著p
的增加規(guī)劃時(shí)間又有所上升。可見(jiàn),概率p
較小時(shí),即按照高斯分布出現(xiàn)的采樣點(diǎn)占比少時(shí),算法效率會(huì)降低;另一方面概率p
如果取到0.
9,即所有隨機(jī)點(diǎn)都按照高斯采樣時(shí),則又因?yàn)槿鄙倭俗杂刹蓸訋?lái)的隨機(jī)性,算法效率也會(huì)降低,只有當(dāng)p
取到合適值時(shí),算法的效率才最高。圖8 平均節(jié)點(diǎn)數(shù)目隨概率變化曲線(xiàn)
圖9 平均規(guī)劃時(shí)間隨概率變化曲線(xiàn)
為了改進(jìn)Bi-RRT算法的效率,本文提出了一種改進(jìn)Bi-RRT算法,該算法分別以機(jī)器人的起點(diǎn)和終點(diǎn)為概率分布的中心,構(gòu)建二維高斯分布概率密度函數(shù),并用該函數(shù)約束隨機(jī)采樣點(diǎn)的生成,同時(shí)也保留一部分均勻分布的采樣點(diǎn),通過(guò)這些具有目標(biāo)啟發(fā)性的采樣點(diǎn)來(lái)引導(dǎo)兩棵搜索樹(shù)快速向目標(biāo)點(diǎn)生長(zhǎng)并相遇。該算法在保證搜索過(guò)程概率完備性的前提下,提高了尋徑的效率和質(zhì)量,相對(duì)于基本的Bi-RRT算法,本文算法在應(yīng)對(duì)復(fù)雜環(huán)境、迷宮環(huán)境等環(huán)境時(shí)的表現(xiàn)更佳,如在復(fù)雜環(huán)境下規(guī)劃時(shí)間縮短了43.9%,擴(kuò)展節(jié)點(diǎn)數(shù)目減少了41.4%,路徑長(zhǎng)度優(yōu)化了8.1%,最后分析了目標(biāo)偏置概率對(duì)算法性能的影響,找到了使算法效果最優(yōu)時(shí)對(duì)應(yīng)的高斯分布采樣點(diǎn)的占比。未來(lái)可以考慮將本文算法結(jié)合RRT*算法,并應(yīng)用到3維以上的高維空間中的路徑規(guī)劃問(wèn)題。