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

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

0 引 言

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

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

1 傳統(tǒng)PRM算法

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

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

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

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

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

(1)

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

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

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

算法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é)點(diǎn)時(shí)必定要找到一組最近的相鄰節(jié)點(diǎn)。然后通過局部規(guī)劃器尋找新節(jié)點(diǎn)到每個(gè)近鄰點(diǎn)的自由路徑。因此使用PRM算法時(shí)，大部分工作是在無向路徑圖的構(gòu)建。

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

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

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

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

圖2 PRM算法流程

2.1 局部敏感哈希

LSH的基本思想：基于原始數(shù)據(jù)空間中相鄰的數(shù)據(jù)經(jīng)過同一個(gè)映射變換之后，處于相鄰區(qū)域的概率仍然較大，選取一些具有上述性質(zhì)的函數(shù)來作為哈希函數(shù)，使得相鄰的數(shù)據(jù)映射后處在哈希表的同一個(gè)位置，更容易搜索與目標(biāo)數(shù)據(jù)相似的數(shù)據(jù)集。當(dāng)哈希函數(shù)h滿足以下的兩個(gè)條件，稱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兩點(diǎn)之間的相似程度，P[h(q1)=h(q2)]表示q1，q2兩映射的哈希值相同的概率。

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

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

算法2：哈希表中添加點(diǎn)偽代碼

(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檢索鄰近點(diǎn)時(shí)，首先從每個(gè)表中獲取點(diǎn)，然后將它們組合在一起，如算法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ù)選擇條件：如果兩個(gè)數(shù)據(jù)點(diǎn)彼此相鄰，則經(jīng)過哈希映射變換后這兩個(gè)數(shù)據(jù)點(diǎn)很有可能返回相同值。這意味著哈希函數(shù)將保留位置信息，并且彼此鄰近的點(diǎn)可能會存儲在哈希表同一存儲桶中。常見滿足不同距離度量方式的局部敏感哈希函數(shù)有：基于歐氏距離哈希函數(shù)、基于余弦距離哈希函數(shù)、基于Jaccard距離哈希函數(shù)、基于漢明距離哈希函數(shù)、基于質(zhì)心哈希函數(shù)等等。而基于質(zhì)心哈希算法[13]易于實(shí)現(xiàn)，并且可以擴(kuò)展以滿足PRM算法的需求。

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

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

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

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

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

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

第二個(gè)問題是路線圖只有幾個(gè)節(jié)點(diǎn)的情況。例如，為一個(gè)查詢點(diǎn)q檢索k個(gè)最近鄰，而路線圖只有k個(gè)節(jié)點(diǎn)，那么應(yīng)該返回路線圖中所有節(jié)點(diǎn)。但是，算法3中所示方法可能只返回所有節(jié)點(diǎn)的一小部分，因?yàn)樗环祷啬承┐鎯ν爸泄?jié)點(diǎn)。算法5給出了一種改進(jìn)方法。如果路線圖的節(jié)點(diǎn)數(shù)少于k個(gè)，將立即返回所有節(jié)點(diǎn)，否則將確保始終返回k個(gè)最近的節(jié)點(diǎn)。

算法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)境下仿真

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

圖4 改進(jìn)PRM算法生成的無向路徑

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

圖5 改進(jìn)PRM算法生成的避障路徑

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

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

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

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

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

圖6 改進(jìn)PRM算法運(yùn)行結(jié)果

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

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

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

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

圖7 改進(jìn)PRM算法運(yùn)行結(jié)果

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

4 結(jié)束語

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