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

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

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

0 引 言

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

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

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

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

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

(1)

式中：ωMi∈R，是權重常數(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算法時，大部分工作是在無向路徑圖的構建。

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

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

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

PRM是概率完備且不最優(yōu)算法，但在PRM中通常使用精確的最近鄰搜索。這非常耗時，尤其處理復雜環(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ū)域的概率仍然較大，選取一些具有上述性質的函數(shù)來作為哈希函數(shù)，使得相鄰的數(shù)據(jù)映射后處在哈希表的同一個位置，更容易搜索與目標數(shù)據(jù)相似的數(shù)據(jù)集。當哈希函數(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映射變換能夠將原始數(shù)據(jù)集劃分為若干較小的子集，每個子集中元素個數(shù)較小且相鄰。因而把超大集合內查找相鄰元素變成了在一個小集合內查找相鄰元素，計算量因此大大下降。該方法不能保證精確找到與某個數(shù)據(jù)最相似(距離最近)的一個數(shù)據(jù)或多個數(shù)據(jù)，但是會以很高的概率返回非常好的近似值。

LSH方法基于包含多個存儲桶的哈希表。每個存儲桶都和唯一的哈希值關聯(lián)，并且使用哈希函數(shù)h計算移動機器人工作空間M中點的哈希值。通過函數(shù)h的映射，將M中所有點存儲到存儲桶中，一個存儲桶中包含多個點。當搜索鄰近查詢點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ù)、基于質心哈希函數(shù)等等。而基于質心哈希算法[13]易于實現(xiàn)，并且可以擴展以滿足PRM算法的需求。

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

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

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

圖3 基于質心的哈希

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

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

算法4：用質心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é)點，那么應該返回路線圖中所有節(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 仿真結果與分析

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

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

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

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

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

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

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

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

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

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

圖6 改進PRM算法運行結果

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

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

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

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

圖7 改進PRM算法運行結果

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

4 結束語

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