基于近似最近鄰搜索的改進PRM算法

薛 陽，孫 越，葉曉康，李 蕊，華 茜

(上海電力大學(xué) 自動化工程學(xué)院，上海200090)

0 引 言

機器人的路徑規(guī)劃是機器人領(lǐng)域的基礎(chǔ)部分也是核心部分[1]。路徑規(guī)劃的方法也很多，經(jīng)典的有：人工勢場法[2,3]、柵格法[4]、蟻群算法[5，6]等；基于隨機采樣的路徑規(guī)劃算法有：快速搜索隨機樹(rapid-exploration random tree，RRT)法[7，8]還有概率路線圖(probabilistic roadmap，PRM)法[9]等。相對于需要知道環(huán)境中障礙物精確位置并精準(zhǔn)建模的路徑規(guī)劃算法，基于隨機采樣技術(shù)的PRM算法可以有效解決高維空間和復(fù)雜約束中的路徑規(guī)劃問題。PRM算法廣泛應(yīng)用于移動機器人、機械臂以及無人機航跡規(guī)劃中。Mohanta JC和Keshari A提出了PRM算法和模糊控制系統(tǒng)結(jié)合使用，使得規(guī)劃的路徑更平滑更短，從而縮短了導(dǎo)航時間[10]。鄒善習(xí)等通過隨機節(jié)點生成函數(shù)在自由空間生成的節(jié)點取代障礙物中節(jié)點，配合改進的節(jié)點增強法，提高節(jié)點利用率，減少PRM算法計算量[11]。

PRM算法的改進一般是通過得到更短的最優(yōu)避障路徑，從而減少了移動機器人在行駛過程的時間。PRM算法基于隨機采樣策略，當(dāng)增加空間中的采樣點或者增加最近鄰點個數(shù)時，無向路徑圖的構(gòu)建將會更完備，最終得到的避障路徑會更優(yōu)。但是以上情況會使得算法運算量呈指數(shù)級增長，從而算法非常耗時。針對PRM算法存在的這一缺陷，文中提出引用處理數(shù)據(jù)的局部敏感哈希算法[12]，加速PRM算法無向路徑圖的構(gòu)建，從而提高了PRM算法的效率。在不降低最優(yōu)路徑的質(zhì)量的前提下，有效提高了PRM算法整體運行速度。

1 傳統(tǒng)PRM算法

PRM算法是一種基于圖搜索的方法。傳統(tǒng)的PRM算法包含兩個階段：離線建圖、在線查詢。第一階段離線建圖：通過隨機采樣點將連續(xù)空間變成離散空間，然后使用局部規(guī)劃器將采集的樣本節(jié)點相互連接并去除與障礙物相交的連線，最終形成了一個無向路徑圖如圖1(a)所示。第二階段在線查詢，使用A*等搜索算法在路徑圖上尋找無碰撞的最優(yōu)路徑如圖1(b)所示。圖中方塊表示障礙物，淺色細線代表可行路徑，深黑色線代表最優(yōu)路徑。

圖1 傳統(tǒng)PRM算法運行結(jié)果

如算法1顯示的一個典型的PRM算法。從一個空的路徑圖G=(V，E)開始，其中V是給定的自由空間Cfree中生成的隨機點集，E是所有可能的兩點之間的路徑。

(1)在自由空間中生成隨機采樣點q，從而確保機器人在采樣點不會與障礙物相碰撞。生成這些采樣點的最簡單方法是使用均勻分布隨機采樣。另一個簡單的方法是使用低差異序列來產(chǎn)生樣本。在一些完全隨機的環(huán)境中，上面兩種方法已經(jīng)足夠。但是移動機器人所處的環(huán)境一般更組織化和結(jié)構(gòu)化。例如，移動機器人在房屋里移動，則墻壁和家具都會成為障礙物，并且在較大的空白區(qū)域機器人都可以自由移動。這類環(huán)境中，要使用不同的方法來增強采樣，一些方法嘗試在障礙物附近節(jié)點采樣，而其它方法嘗試對障礙物盡可能遠的節(jié)點采樣。還有一些方法可以將配置空間劃分為不同的區(qū)域，然后嘗試檢測對每個區(qū)域進行采樣的最佳方法。

(2)根據(jù)一個度量選取k個q點的近鄰點，近鄰點選取的度量指與q點的距離在一定的范圍即N0={n∈N│L(q,q′)

(1)

式中：ωMi∈R，是權(quán)重常數(shù)。

(3)使用局部規(guī)劃器檢測q點和k個近鄰點是否存在路徑，如果有，相互連接形成機器人的移動路徑，并且經(jīng)過碰撞檢測后刪減掉與障礙物相交的路徑。

(4)生成的路徑加入到集合E中。最終所有隨機采樣點相連成一個無向路徑圖。

算法1：傳統(tǒng)PRM偽代碼

Constructs the roadmapG=(V,E)

V←?

E←?

repeat

q←sampled configuration fromCfree

V=V∪{q}

Nq←knearest neighbor configurations ofqchosen fromV

for allq′inNqdo

ifqandq′are not in the same

component of the roadmap then

if local planner finds a path

betweenqandq′then

E←E{(q,q′)}

until there are enough configurations inV

returnG

從算法1可以看出PRM算法在每次向路徑圖中添加新節(jié)點時必定要找到一組最近的相鄰節(jié)點。然后通過局部規(guī)劃器尋找新節(jié)點到每個近鄰點的自由路徑。因此使用PRM算法時，大部分工作是在無向路徑圖的構(gòu)建。

局部規(guī)劃所做的碰撞檢查是構(gòu)建無向路徑圖時最耗時的部分，也是PRM算法最耗時的部分之一。因此，不可能檢查所有路徑是否發(fā)生碰撞，而是選擇一組相鄰節(jié)點調(diào)用一次局部規(guī)劃。

最近鄰搜索是PRM算法的重要組成部分，因為對于每個采樣點，都需要搜索一組最近鄰節(jié)點。當(dāng)路線圖規(guī)模增加時，這會成為一個非常耗時的操作。所以鄰域搜索和碰撞檢測是PRM規(guī)劃的瓶頸之一。當(dāng)工作空間環(huán)境復(fù)雜，尤其是高維空間中，更是成為性能的主要瓶頸。

2 搜索算法優(yōu)化

PRM是概率完備且不最優(yōu)算法，但在PRM中通常使用精確的最近鄰搜索。這非常耗時，尤其處理復(fù)雜環(huán)境下大量數(shù)據(jù)時更加耗時。近似最近鄰查找(approximate nearest neighbor,ANN)可以很好加速查找過程，提高算法效率。而局部敏感哈希(locality-sensitive hashing,LSH)便是屬于近似最近鄰查找的一種。通過使用局部敏感哈希搜索算法來加速PRM算法中無向路徑圖的建立，從而提高PRM算法運行速度。改進PRM算法流程如圖2所示。

圖2 PRM算法流程

2.1 局部敏感哈希

LSH的基本思想：基于原始數(shù)據(jù)空間中相鄰的數(shù)據(jù)經(jīng)過同一個映射變換之后，處于相鄰區(qū)域的概率仍然較大，選取一些具有上述性質(zhì)的函數(shù)來作為哈希函數(shù)，使得相鄰的數(shù)據(jù)映射后處在哈希表的同一個位置，更容易搜索與目標(biāo)數(shù)據(jù)相似的數(shù)據(jù)集。當(dāng)哈希函數(shù)h滿足以下的兩個條件，稱h為局部敏感哈希函數(shù)：

(1)如果L(q1,q2)

(2)如果L(q1,q2)>d2，則P[h(q1)=h(q2)]≤p2；

其中，d1，d2，p1，p2是給定的常數(shù)，L(q1,q2)表示q1，q2兩點之間的相似程度，P[h(q1)=h(q2)]表示q1，q2兩映射的哈希值相同的概率。

通過選取的哈希函數(shù)h映射變換能夠?qū)⒃紨?shù)據(jù)集劃分為若干較小的子集，每個子集中元素個數(shù)較小且相鄰。因而把超大集合內(nèi)查找相鄰元素變成了在一個小集合內(nèi)查找相鄰元素，計算量因此大大下降。該方法不能保證精確找到與某個數(shù)據(jù)最相似(距離最近)的一個數(shù)據(jù)或多個數(shù)據(jù)，但是會以很高的概率返回非常好的近似值。

LSH方法基于包含多個存儲桶的哈希表。每個存儲桶都和唯一的哈希值關(guān)聯(lián)，并且使用哈希函數(shù)h計算移動機器人工作空間M中點的哈希值。通過函數(shù)h的映射，將M中所有點存儲到存儲桶中，一個存儲桶中包含多個點。當(dāng)搜索鄰近查詢點q∈M的一組點，首先計算q的哈希值。從得到哈希值指示的存儲桶中檢索所有點。哈希表可以實現(xiàn)為非常有效地工作，這意味著在實踐中可以快速完成這些點的搜索。但是，不確定所找到點是否包含q的最近鄰居。通過使用多個哈希表，可以增加該方法找到最近點的概率。創(chuàng)建包含多個哈希表的L表，并且為每個哈希表使用一個不同哈希函數(shù)。算法2展示了如何將新點添加到每個表中。

算法2：哈希表中添加點偽代碼

(1)fori=1,2,…,Ldo

(2)h←hash value forqinith hash table

(3)b←bucket identified by a valueh

fromith hash table

(4)Add pointqto the bucketb

使用LSH檢索鄰近點時，首先從每個表中獲取點，然后將它們組合在一起，如算法3所示。

算法3：偽代碼

Returns k nearest nodes to a query pointq

(1)V←?

(2)fori=1,2,…,Ldo

(3)h←hash value forqinith hash table

(4)b←bucket identified by a value h fromith hash table

(5)V′←all nodes from bucketb

(6)V←V∪V′

(7)returnV

LSH算法中，哈希函數(shù)的選擇尤其重要，哈希函數(shù)選擇條件：如果兩個數(shù)據(jù)點彼此相鄰，則經(jīng)過哈希映射變換后這兩個數(shù)據(jù)點很有可能返回相同值。這意味著哈希函數(shù)將保留位置信息，并且彼此鄰近的點可能會存儲在哈希表同一存儲桶中。常見滿足不同距離度量方式的局部敏感哈希函數(shù)有：基于歐氏距離哈希函數(shù)、基于余弦距離哈希函數(shù)、基于Jaccard距離哈希函數(shù)、基于漢明距離哈希函數(shù)、基于質(zhì)心哈希函數(shù)等等。而基于質(zhì)心哈希算法[13]易于實現(xiàn)，并且可以擴展以滿足PRM算法的需求。

2.2 基于質(zhì)心的哈希函數(shù)

在基于質(zhì)心的哈希函數(shù)中，開始時會生成一組質(zhì)心c。質(zhì)心屬于移動機器人工作空間M，通過計算從q到每個質(zhì)心的距離，然后選擇最接近的質(zhì)心，任意點q∈M可以與其中一個質(zhì)心相關(guān)聯(lián)。這實際上將空間M劃分為c個分區(qū)。這種劃分也可以被認為是Voronoi圖，其中每個質(zhì)心對應(yīng)一個Voronoi單元。因此哈希函數(shù)h可以定義：取得一個點，計算它屬于哪個Voronoi單元，返回與該單元關(guān)聯(lián)的哈希值。使用此哈希函數(shù)，存儲桶數(shù)量將與質(zhì)心數(shù)量相同。

在圖3中以二維方式說明了基于質(zhì)心的哈希。在圖3(a)～圖3(c)中，出示了3組不同的質(zhì)心和相應(yīng)Voronoi單元。質(zhì)心用小點標(biāo)記，中間以小圓表示查詢點q，q所屬單元格用灰色標(biāo)記。要為q檢索一組最鄰近的點，從這3個有標(biāo)記的Voronoi單元檢索即可。

圖3 基于質(zhì)心的哈希

基于質(zhì)心的敏感哈希算法要在PRM算法中應(yīng)用進行改進PRM算法，需要解決兩個問題：如何選擇質(zhì)心；如何處理路線圖中只有幾個節(jié)點的情況。

第一個問題是選擇質(zhì)心。最簡單的方法是生成c個隨機點用作質(zhì)心。這是可行的，但會忽略從環(huán)境中得到的一些信息。在此基礎(chǔ)上提出了一種考慮靜態(tài)障礙物的方法。初始化質(zhì)心的方法如算法4所示。假設(shè)路線圖一開始是空的，這意味著不可能使用關(guān)于現(xiàn)有路線圖信息。因此，該方法隨機選擇質(zhì)心，但要求所有質(zhì)心都位于自由組態(tài)空間Cfree。為了在空間中更均勻地分布質(zhì)心，可以使用一些低差分序列來生成質(zhì)心。在改進PRM方法中應(yīng)該注意，如果c大于1，那么L也必須大于1。否則，路線圖將在每個Voronoi單元中單獨構(gòu)建，不會連接在一起。通過增加L，單元之間會有重疊，有助于路線圖連接起來。

算法4：用質(zhì)心c初始化L哈希表偽代碼

(1)fori=1,2,…,Ldo

(2)P←?

(3)fori=1,2,…,cdo

(4)P←a random free configuration

(5)P=P∪{q}

(6)Associate centroidsPwithith hash table

第二個問題是路線圖只有幾個節(jié)點的情況。例如，為一個查詢點q檢索k個最近鄰，而路線圖只有k個節(jié)點，那么應(yīng)該返回路線圖中所有節(jié)點。但是，算法3中所示方法可能只返回所有節(jié)點的一小部分，因為它只返回某些存儲桶中節(jié)點。算法5給出了一種改進方法。如果路線圖的節(jié)點數(shù)少于k個，將立即返回所有節(jié)點，否則將確保始終返回k個最近的節(jié)點。

算法5：偽代碼

(1)if|R|≤kthen

(2)returnR

(3)V←?

(4)fori=1,2,…,Ldo

(5)h←hash value forqinithhash table

(6)b←bucket identified by a valuehfromith hash table

(7)V′←all nodes from bucketb

(8)V←V∪V′

(9)if|V|≤kthen

(10)returnknearest nodes toqfromR

(11)else

(12)returnknearest nodes toqfromV

3 仿真結(jié)果與分析

3.1 一般環(huán)境下仿真

為了驗證改進后PRM算法效果，使用matlab2017b仿真軟件對改進PRM算法進行仿真實驗，仿真實驗中設(shè)置了400×600大小的地圖，單位設(shè)置為厘米(cm)，場景中方塊表示障礙物，淺色細線代表可行路徑，深黑色線代表最優(yōu)路徑。設(shè)置機器人起始點坐標(biāo)為[10,20]，目標(biāo)點為[360,500]，在地圖中選擇隨機點數(shù)分別為100、400、1000,此時最近鄰數(shù)目k設(shè)置為6，生成的無向路徑圖和如圖4(a)～圖4(c)所示。圖4中顯示出，隨著隨機點數(shù)增加無向路徑圖構(gòu)建更為詳細。

圖4 改進PRM算法生成的無向路徑

無向路徑圖構(gòu)建完成后，使用搜索算法進行最優(yōu)路徑查詢，生成一條由起始點至目標(biāo)點的最優(yōu)路徑。圖5顯示的是使用改進PRM算法得出的移動機器人避障的最優(yōu)路徑，圖中粗實線表示搜索出的機器人移動的最優(yōu)路徑。對比圖1(b)和圖5，改進PRM算法得出的最優(yōu)路徑基本與傳統(tǒng)PRM算法得出的路徑相同，這是因為加入LSH搜索算法的改進PRM算法僅加速了構(gòu)建無向路徑圖的速度，所以得到最優(yōu)路徑的質(zhì)量并沒有下降。

圖5 改進PRM算法生成的避障路徑

在仿真實驗中，為了減少因PRM算法隨機性而產(chǎn)生的誤差，對傳統(tǒng)PRM算法和改進PRM算法分別進行了40次實驗取平均值，采樣點的近鄰點個數(shù)k設(shè)置為6，隨機采樣點數(shù)分別設(shè)置為100、400、1000。為了找到合適的參數(shù)c和參數(shù)L的值，在改進PRM算法中給c和L設(shè)定不同的值。實驗結(jié)果見表1。

表1 一般環(huán)境下改進算法與傳統(tǒng)算法實驗數(shù)據(jù)對比

由表1的數(shù)據(jù)可以得出：相對于傳統(tǒng)PRM算法改進后PRM算法在建立無向路徑圖的時間上減少了27.36%～33.27%，且隨著隨機采樣點的數(shù)目增多，改進后的PRM算法效率逐漸增加。當(dāng)采樣點數(shù)目相對不多時質(zhì)心數(shù)目越多，反而算法運行時間增大。隨著采樣點增多，質(zhì)心數(shù)目相對增加時更有利于減少算法時間。哈希表數(shù)目相對增多有利于算法提高效率。

3.2 多障礙物環(huán)境下仿真

同一般環(huán)境相同的仿真環(huán)境下，在地圖中添加8個障礙物。實驗步驟和一般條件下實驗步驟相同，隨機采樣點數(shù)設(shè)置為100，c設(shè)置為5，L設(shè)置為3。當(dāng)近鄰點數(shù)目k設(shè)置為6、10、15時，對應(yīng)的結(jié)果如圖6(a)～圖6(c)所示。通過對比可以得知，隨著k值增大無向路徑圖變得更加完備，而規(guī)劃的路徑也更優(yōu)。

圖6 改進PRM算法運行結(jié)果

實驗結(jié)果顯示，在多障礙物的復(fù)雜地圖環(huán)境下，改進PRM算法相比較傳統(tǒng)PRM算法，在建立無向路徑圖時間上減少了28.61%。隨著k值的增大構(gòu)建無向路徑圖的時間也有所增加。實驗數(shù)據(jù)見表2。

表2 多障礙物環(huán)境下改進算法與傳統(tǒng)算法實驗數(shù)據(jù)對比

3.3 狹窄環(huán)境下仿真

仿真環(huán)境不變，在地圖中添加障礙物構(gòu)建一個狹窄環(huán)境，設(shè)置機器人起始點坐標(biāo)為[10,20]，目標(biāo)點為[360,500]。隨機采樣點數(shù)設(shè)置為200，c設(shè)置為5，L設(shè)置為3，近鄰點數(shù)目k設(shè)置為6。實驗的步驟和一般條件下的實驗步驟相同。使用改進PRM算法最終生成的最優(yōu)路徑如圖7所示。

圖7 改進PRM算法運行結(jié)果

因為PRM算法的隨機性，進行50次仿真實驗。實驗結(jié)果表明，在狹窄環(huán)境下，改進PRM算法時間減少了27.57%～27.71%。50次狹窄環(huán)境下的仿真中，傳統(tǒng)PRM算法出現(xiàn)了23次最優(yōu)路徑無解的狀況，改進PRM算法也出現(xiàn)了23次最優(yōu)路徑無解的結(jié)果。因為在狹窄環(huán)境下，隨機點灑在狹窄通道的幾率會減小，從而構(gòu)建出的無向路徑圖會有缺失，在在線搜索環(huán)節(jié)就會搜尋不到從起始點至目標(biāo)點的最優(yōu)路徑。

4 結(jié)束語

本文基于傳統(tǒng)的PRM算法，使用基于質(zhì)心的局部敏感哈希算法加速無向路徑圖的構(gòu)建，從而減少了PRM算法的運行時間，提高了PRM算法的運行效率，且搜索的最優(yōu)路徑質(zhì)量并沒有下降。并通過移動機器人在一般環(huán)境地圖和多障礙物環(huán)境地圖以及狹窄環(huán)境地圖的仿真實驗，驗證了改進PRM算法確實節(jié)省了算法在建立無向路徑圖環(huán)節(jié)的時間，提高了算法運行速度。