基于角點PRM的移動機器人路徑規(guī)劃

宋 宇, 王志明

(長春工業(yè)大學 計算機科學與工程學院， 吉林 長春 130012)

0 引 言

隨著自動機器人技術(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é)果。

1 Harris角點檢測算法

文中所采用的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值，若都大于，則此像素所在的位置為角點。

2 改進PRM算法

2.1 采樣點的選擇

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個點中保留不在障礙物中的點作為最終輸出點。

2.2 Cardinal 樣條曲線路徑平滑

樣條曲線是通過在給定點之間內(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樣條組合成的一條光滑曲線。

3 仿 真

3.1 路徑長度與消耗時間對比

使用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ù)表

3.2 Cardinal 樣條曲線路徑平滑

以起點、終點以及A*算法得到的所有路徑點為Cardinal 樣條曲線中的控制點，文中tension系數(shù)設(shè)置為0.8，經(jīng)Cardinal 樣條平滑后得到路徑如圖3所示。

(a) 地圖5

(b) 地圖6圖3 Cardinal 樣條曲線平滑效果圖

4 結(jié) 語

由于傳統(tǒng)PRM算法在產(chǎn)生采樣點時使用的是均勻采樣，導致了窄通道連通率不高的現(xiàn)象出現(xiàn)，文中采用了結(jié)合角點檢測算法采樣的PRM算法，最后采用Cardinal 樣條曲線平滑路徑。仿真結(jié)果顯示，由于改進算法產(chǎn)生的采樣點相對少而有效，故改進算法得到的路徑可行、有效。