宋 宇, 王志明

(長(zhǎng)春工業(yè)大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院， 吉林 長(zhǎng)春 130012)

0 引 言

隨著自動(dòng)機(jī)器人技術(shù)的發(fā)展，近年來(lái)路徑規(guī)劃算法得到越來(lái)越多的國(guó)內(nèi)外學(xué)者關(guān)注，路徑規(guī)劃的相關(guān)算法層出不窮，從蟻群算法、A*算法[1]、D*算法、RRT算法、PRM算法、人工勢(shì)場(chǎng)法到模糊邏輯算法、神經(jīng)網(wǎng)絡(luò)算、強(qiáng)化學(xué)習(xí)算法，其中PRM算法由于相對(duì)于A*算法用時(shí)較少得到廣泛應(yīng)用，但由于傳統(tǒng)PRM算法存在窄通道連通率低的缺陷。基于高斯采樣[2]的PRM算法增加了窄通道連通率，但由于高斯采樣得到的點(diǎn)大多數(shù)集中在障礙物邊緣線上，而障礙物邊界點(diǎn)大多并不是產(chǎn)生最終路徑所經(jīng)過(guò)的點(diǎn)，導(dǎo)致后續(xù)點(diǎn)與點(diǎn)之間的連通性檢測(cè)花費(fèi)時(shí)間成倍增加?；跇驕y(cè)試采樣[3]的PRM算法增加了路徑連通率，但由于橋測(cè)試采樣產(chǎn)生的是由兩個(gè)在障礙物中的點(diǎn)的中點(diǎn)位置得到的，導(dǎo)致基于橋測(cè)試采樣的PRM算法只適用于通道寬度變化不大的情形。文獻(xiàn)[5]提出了利用人工勢(shì)場(chǎng)力將落在障礙物中的點(diǎn)移動(dòng)到自由空間的方法，提高了采樣點(diǎn)的利用率。由于真正對(duì)最終路徑起決定性作用的點(diǎn)是障礙物的角點(diǎn)，故文中提出了一種結(jié)合角點(diǎn)檢測(cè)的PRM算法。仿真結(jié)果表明，改進(jìn)算法在窄通道場(chǎng)景與普通場(chǎng)景下，消耗時(shí)間與路徑長(zhǎng)度都得到了較好的結(jié)果。

1 Harris角點(diǎn)檢測(cè)算法

文中所采用的Harris角點(diǎn)檢測(cè)算法步驟如下:

1)計(jì)算圖像的水平、豎直灰度值變化量矩陣Ix，Iy。文中采用了高斯函數(shù)求微分，即用Fx矩陣與Fy矩陣分別與原圖像的像素矩陣作卷積得到Ix與Iy。

(1)

(2)

(3)

3)計(jì)算圖像中所有像素的R值

R(x,y)=detM(x,y)-

k*(traceM(x,y))2。

(4)

4)對(duì)于上述得到的R矩陣，分別判斷每個(gè)像素的R值是否同時(shí)大于這個(gè)像素的8個(gè)鄰居像素的R值，若都大于，則此像素所在的位置為角點(diǎn)。

2 改進(jìn)PRM算法

2.1 采樣點(diǎn)的選擇

1)首先采用Harris角點(diǎn)檢測(cè)算法檢測(cè)到圖像中所有角點(diǎn)，然后從檢測(cè)到的所有角點(diǎn)中排除距離過(guò)近的角點(diǎn)。即為了排除1個(gè)實(shí)際角點(diǎn)附近產(chǎn)生了許多輸出角點(diǎn)的情況，文中在使用Harris角點(diǎn)檢測(cè)算法的過(guò)程中，對(duì)新選擇的角點(diǎn)加了距離限制條件，若新產(chǎn)生的角點(diǎn)與之前產(chǎn)生的所有角點(diǎn)的最小距離小于給定閾值k(文中取k=10)，則此新角點(diǎn)不計(jì)入最終角點(diǎn)內(nèi)。

2)為了增加角點(diǎn)之間的連通性，在Harris產(chǎn)生出n個(gè)角點(diǎn)后，以每個(gè)角點(diǎn)為中心，上下左右各產(chǎn)生距離每個(gè)角點(diǎn)d(文中取d=10)的4個(gè)臨近點(diǎn)，即若第一步產(chǎn)生的角點(diǎn)數(shù)量為n，此時(shí)額外產(chǎn)生4n個(gè)點(diǎn)，然后從這4n個(gè)點(diǎn)中保留不在障礙物中的點(diǎn)作為最終輸出點(diǎn)。

2.2 Cardinal 樣條曲線路徑平滑

樣條曲線是通過(guò)在給定點(diǎn)之間內(nèi)插點(diǎn)的方式實(shí)現(xiàn)的，文中采用的是Cardinal 樣條曲線。單段Cardinal 樣條曲線是由4個(gè)給定點(diǎn)確定的，如圖1所示。

圖1 Cardinal 樣條曲線

圖中:P0，P1，P2，P3為給定的4個(gè)點(diǎn)，u為一個(gè)從0增加到1的參數(shù)，u從0取到1對(duì)應(yīng)得到P1到P2之間的所有內(nèi)插點(diǎn)，點(diǎn)P1與P2之間的內(nèi)插點(diǎn)是由下式得到的：

(5)

(6)

其中，s=1-t/2，t為tension系數(shù)。由于每段Cardinal樣條曲線在分段點(diǎn)處一階導(dǎo)數(shù)連續(xù)，故給定n個(gè)控制點(diǎn)，可得到一條由n-3段Cardinal樣條組合成的一條光滑曲線。

3 仿 真

3.1 路徑長(zhǎng)度與消耗時(shí)間對(duì)比

使用Matlab 2014a在cpu為i3-2120，3 G內(nèi)存，32位Windows7的電腦上進(jìn)行了仿真，給定500×500像素的地圖下改進(jìn)算法與傳統(tǒng)PRM算法、高斯采樣PRM、橋測(cè)試采樣PRM的路徑圖，起點(diǎn)坐標(biāo)為[10,10]，終點(diǎn)坐標(biāo)為[490,490]，由于文中改進(jìn)算法加入了角點(diǎn)檢測(cè)過(guò)程，故不需要額外設(shè)置參數(shù)，傳統(tǒng)PRM算法、高斯采樣PRM、橋測(cè)試采樣PRM的采樣點(diǎn)數(shù)設(shè)置為固定值100，橋測(cè)試采樣中的其他參數(shù)經(jīng)反復(fù)測(cè)試設(shè)置為較優(yōu)參數(shù)。不同地圖算法路徑對(duì)比如圖2所示。

(a) 地圖1

(b) 地圖2

(c) 地圖3

(d) 地圖4圖2 不同地圖算法路徑對(duì)比

從圖2可以看出，由于改進(jìn)算法采用的是角點(diǎn)，而橋測(cè)試算法得到的點(diǎn)大多為通道的中點(diǎn)，高斯采樣得到的多數(shù)點(diǎn)為障礙物邊界，故改進(jìn)算法得到的路徑長(zhǎng)度最短。

各算法在路徑長(zhǎng)度和運(yùn)行時(shí)間方面的結(jié)果對(duì)比見(jiàn)表1。

由表1可以看出，當(dāng)給定地圖總的角點(diǎn)數(shù)量不多時(shí)(如地圖4)，改進(jìn)算法產(chǎn)生的最終點(diǎn)個(gè)數(shù)較少，導(dǎo)致后續(xù)用于點(diǎn)與點(diǎn)之間的連通性檢測(cè)的時(shí)間(相比于其他3種算法給定100個(gè)點(diǎn))有所減少。同時(shí)由于改進(jìn)算法產(chǎn)生的4n個(gè)點(diǎn)中，每4個(gè)點(diǎn)相對(duì)距離較近，用于這些點(diǎn)之間連通性檢測(cè)的時(shí)間也相對(duì)較少，故改進(jìn)算法雖然加入了角點(diǎn)檢測(cè)的過(guò)程，但算法效率反而較高。

算法在每種地圖下100次實(shí)驗(yàn)路徑連通性對(duì)比見(jiàn)表2。

表2 100次實(shí)驗(yàn)算法連通次數(shù)表

3.2 Cardinal 樣條曲線路徑平滑

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

(a) 地圖5

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

4 結(jié) 語(yǔ)

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