宋 宇, 王志明
(長春工業(yè)大學 計算機科學與工程學院, 吉林 長春 130012)
隨著自動機器人技術(shù)的發(fā)展,近年來路徑規(guī)劃算法得到越來越多的國內(nèi)外學者關(guān)注,路徑規(guī)劃的相關(guān)算法層出不窮,從蟻群算法、A*算法[1]、D*算法、RRT算法、PRM算法、人工勢場法到模糊邏輯算法、神經(jīng)網(wǎng)絡算、強化學習算法,其中PRM算法由于相對于A*算法用時較少得到廣泛應用,但由于傳統(tǒng)PRM算法存在窄通道連通率低的缺陷。基于高斯采樣[2]的PRM算法增加了窄通道連通率,但由于高斯采樣得到的點大多數(shù)集中在障礙物邊緣線上,而障礙物邊界點大多并不是產(chǎn)生最終路徑所經(jīng)過的點,導致后續(xù)點與點之間的連通性檢測花費時間成倍增加。基于橋測試采樣[3]的PRM算法增加了路徑連通率,但由于橋測試采樣產(chǎn)生的是由兩個在障礙物中的點的中點位置得到的,導致基于橋測試采樣的PRM算法只適用于通道寬度變化不大的情形。文獻[5]提出了利用人工勢場力將落在障礙物中的點移動到自由空間的方法,提高了采樣點的利用率。由于真正對最終路徑起決定性作用的點是障礙物的角點,故文中提出了一種結(jié)合角點檢測的PRM算法。仿真結(jié)果表明,改進算法在窄通道場景與普通場景下,消耗時間與路徑長度都得到了較好的結(jié)果。
文中所采用的Harris角點檢測算法步驟如下:
1)計算圖像的水平、豎直灰度值變化量矩陣Ix,Iy。文中采用了高斯函數(shù)求微分,即用Fx矩陣與Fy矩陣分別與原圖像的像素矩陣作卷積得到Ix與Iy。
(1)
(2)
(3)
3)計算圖像中所有像素的R值
R(x,y)=detM(x,y)-
k*(traceM(x,y))2。
(4)
4)對于上述得到的R矩陣,分別判斷每個像素的R值是否同時大于這個像素的8個鄰居像素的R值,若都大于,則此像素所在的位置為角點。
1)首先采用Harris角點檢測算法檢測到圖像中所有角點,然后從檢測到的所有角點中排除距離過近的角點。即為了排除1個實際角點附近產(chǎn)生了許多輸出角點的情況,文中在使用Harris角點檢測算法的過程中,對新選擇的角點加了距離限制條件,若新產(chǎn)生的角點與之前產(chǎn)生的所有角點的最小距離小于給定閾值k(文中取k=10),則此新角點不計入最終角點內(nèi)。
2)為了增加角點之間的連通性,在Harris產(chǎn)生出n個角點后,以每個角點為中心,上下左右各產(chǎn)生距離每個角點d(文中取d=10)的4個臨近點,即若第一步產(chǎn)生的角點數(shù)量為n,此時額外產(chǎn)生4n個點,然后從這4n個點中保留不在障礙物中的點作為最終輸出點。
樣條曲線是通過在給定點之間內(nèi)插點的方式實現(xiàn)的,文中采用的是Cardinal 樣條曲線。單段Cardinal 樣條曲線是由4個給定點確定的,如圖1所示。
圖1 Cardinal 樣條曲線
圖中:P0,P1,P2,P3為給定的4個點,u為一個從0增加到1的參數(shù),u從0取到1對應得到P1到P2之間的所有內(nèi)插點,點P1與P2之間的內(nèi)插點是由下式得到的:
(5)
(6)
其中,s=1-t/2,t為tension系數(shù)。由于每段Cardinal樣條曲線在分段點處一階導數(shù)連續(xù),故給定n個控制點,可得到一條由n-3段Cardinal樣條組合成的一條光滑曲線。
使用Matlab 2014a在cpu為i3-2120,3 G內(nèi)存,32位Windows7的電腦上進行了仿真,給定500×500像素的地圖下改進算法與傳統(tǒng)PRM算法、高斯采樣PRM、橋測試采樣PRM的路徑圖,起點坐標為[10,10],終點坐標為[490,490],由于文中改進算法加入了角點檢測過程,故不需要額外設(shè)置參數(shù),傳統(tǒng)PRM算法、高斯采樣PRM、橋測試采樣PRM的采樣點數(shù)設(shè)置為固定值100,橋測試采樣中的其他參數(shù)經(jīng)反復測試設(shè)置為較優(yōu)參數(shù)。不同地圖算法路徑對比如圖2所示。
(a) 地圖1
(b) 地圖2
(c) 地圖3
(d) 地圖4圖2 不同地圖算法路徑對比
從圖2可以看出,由于改進算法采用的是角點,而橋測試算法得到的點大多為通道的中點,高斯采樣得到的多數(shù)點為障礙物邊界,故改進算法得到的路徑長度最短。
各算法在路徑長度和運行時間方面的結(jié)果對比見表1。
由表1可以看出,當給定地圖總的角點數(shù)量不多時(如地圖4),改進算法產(chǎn)生的最終點個數(shù)較少,導致后續(xù)用于點與點之間的連通性檢測的時間(相比于其他3種算法給定100個點)有所減少。同時由于改進算法產(chǎn)生的4n個點中,每4個點相對距離較近,用于這些點之間連通性檢測的時間也相對較少,故改進算法雖然加入了角點檢測的過程,但算法效率反而較高。
算法在每種地圖下100次實驗路徑連通性對比見表2。
表2 100次實驗算法連通次數(shù)表
以起點、終點以及A*算法得到的所有路徑點為Cardinal 樣條曲線中的控制點,文中tension系數(shù)設(shè)置為0.8,經(jīng)Cardinal 樣條平滑后得到路徑如圖3所示。
(a) 地圖5
(b) 地圖6圖3 Cardinal 樣條曲線平滑效果圖
由于傳統(tǒng)PRM算法在產(chǎn)生采樣點時使用的是均勻采樣,導致了窄通道連通率不高的現(xiàn)象出現(xiàn),文中采用了結(jié)合角點檢測算法采樣的PRM算法,最后采用Cardinal 樣條曲線平滑路徑。仿真結(jié)果顯示,由于改進算法產(chǎn)生的采樣點相對少而有效,故改進算法得到的路徑可行、有效。