基于距離變換的PRM路徑規(guī)劃算法

李　敏，周遠(yuǎn)遠(yuǎn)，黃　魯

(1. 中國科學(xué)技術(shù)大學(xué) 電子科學(xué)與技術(shù)系，安徽 合肥 230027；

0　引言

隨著機(jī)器人的快速發(fā)展，作為機(jī)器人關(guān)鍵技術(shù)之一的運(yùn)動規(guī)劃得到廣泛的關(guān)注和研究，以概率路圖法 (Probabilistic RoadMap, PRM) 和快速隨機(jī)生成樹 (Rapidly exploring Random Trees, RRT)為代表的基于采樣的路徑規(guī)劃技術(shù)在實(shí)踐中表現(xiàn)出較好的性能，同時(shí)這些算法也擁有理論上的概率完備性[1]。

在構(gòu)型空間中直接計(jì)算路徑被證明是非常困難的，尤其是在高維空間中[2]。PRM算法在自由空間中獲取采樣點(diǎn)，并通過采樣點(diǎn)之間的互聯(lián)來構(gòu)建無向圖，用于近似表示自由空間，這種方法將連續(xù)空間中的規(guī)劃問題轉(zhuǎn)化到拓?fù)淇臻g中解決，大大減小了計(jì)算可行路徑的復(fù)雜性，因此自PRM算法提出以來，受到了廣泛關(guān)注并且在實(shí)際應(yīng)用中取得較好的效果[3]。

盡管PRM算法在路徑規(guī)劃領(lǐng)域尤其是高維路徑規(guī)劃上取得了巨大的成功，但是當(dāng)工作空間中存在窄通道(narrow passage)時(shí)，由于采用均勻采樣，最終獲得的采樣點(diǎn)大部分會處于開闊的自由空間中，而在窄通道內(nèi)的采樣點(diǎn)相對較少(甚至沒有)，這樣就會造成構(gòu)建的概率路圖在窄通道處失去連接性，進(jìn)而會造成路徑規(guī)劃失敗[4]。

為解決PRM算法在窄通道區(qū)域的困難，許多文獻(xiàn)提出了多種改進(jìn)PRM算法。文獻(xiàn)[4]采用增大窄通道區(qū)域的方法來增加窄通道內(nèi)的采樣點(diǎn)數(shù)目；文獻(xiàn)[5]提出的OBPRM(Obstacle-based PRM)方法通過增加在障礙物邊界處的采樣點(diǎn)來改善PRM性能；文獻(xiàn)[6-7]采取高斯采樣策略增大在障礙物邊界處的采樣點(diǎn)密度；文獻(xiàn)[8-9]采用橋測法，通過RBB(Randomized Bridge Builder)采樣策略提高在窄通道區(qū)域的采樣點(diǎn)密度；文獻(xiàn)[10]結(jié)合PRM和胞元分解(Cell Decomposition)提出了一種非均勻采樣策略用于解決窄通道問題；文獻(xiàn)[11]通過引入人工勢場，對落在威脅體內(nèi)的點(diǎn)施加勢場力，使之移動到自由空間內(nèi)，增加窄通道內(nèi)的節(jié)點(diǎn)數(shù)量；文獻(xiàn)[12]提出了一種基于隨機(jī)游走策略(Random Walk Strategy, RWS)的改進(jìn)PRM算法；文獻(xiàn)[13]在RBB采樣策略的基礎(chǔ)上提出了一種RSB(Randomized Star Builder)采樣策略。

不同于以上方法，本文采用一種新的思路來解決窄通道問題。首先通過距離變換從原始地圖中得到距離變換地圖；然后利用距離變換地圖來計(jì)算工作空間中障礙物稠密度，進(jìn)而自適應(yīng)計(jì)算不同環(huán)境下需要的采樣點(diǎn)數(shù)目；最后用距離變換地圖識別自由空間中的窄通道區(qū)域、開闊區(qū)域和邊緣角落區(qū)域，在不同區(qū)域內(nèi)采用不同密度的采樣方法，使得采樣點(diǎn)合理分布,構(gòu)建能夠通過窄通道的無向圖。

1　基本PRM算法

基本的PRM算法包含兩個階段：學(xué)習(xí)階段和查詢階段[2]。在學(xué)習(xí)階段，PRM算法在自由空間中進(jìn)行采樣獲取采樣點(diǎn)，然后互聯(lián)這些采樣點(diǎn)構(gòu)成代表自由空間的概率路圖；在查詢階段，首先將起始構(gòu)型和目標(biāo)構(gòu)型連接到概率路圖中合適的節(jié)點(diǎn)，然后利用基于圖的路徑規(guī)劃算法(D，A*，D*等)完成從起始構(gòu)型到目標(biāo)構(gòu)型的路徑搜索。對于一個環(huán)境已知的工作空間，學(xué)習(xí)階段只需要離線執(zhí)行一次即可，之后查詢階段便可以高效地在線執(zhí)行。

1.1　學(xué)習(xí)階段

學(xué)習(xí)階段是PRM算法的關(guān)鍵步驟，算法流程如算法1所示[2]：

算法1：PRM(Learning Phase)

1.V←φ;E←φ;

2.fori=0,…,ndo

3.xrand← SampleFreei;

4.U ← Near(G(V, E), xrand, r);

5.V ← V ∪ {xrand};

7.ifxrandand u are not in the same connected

component of G(V, E)then

8. E←E∪{(xrand, u), (u, xrand)};

9.returnG(V, E);

其主要內(nèi)容包括：

(1) 初始化。G(V,E)為一個無向圖，初始狀態(tài)為空。G(V,E)的頂點(diǎn)對應(yīng)自由構(gòu)型，連接頂點(diǎn)的邊對應(yīng)兩個自由構(gòu)型之間的無碰撞路徑。

(2) 采樣(SampFree)：從自由空間Cfree中采樣得到自由構(gòu)型xi，加入到頂點(diǎn)集V中進(jìn)行頂點(diǎn)擴(kuò)展。不同的采樣策略將直接影響最后采樣點(diǎn)的密度分布，原始的PRM[1]采用均勻隨機(jī)采樣。

(3) 鄰接點(diǎn)計(jì)算(Near)：在空間C中定義一個距離度量ρ:C×C→。存在于頂點(diǎn)集V中的頂點(diǎn)ni，如果按照距離度量ρ為小距離(比設(shè)定的距離r小)，則將ni選擇為構(gòu)型xi鄰域的一部分。假設(shè)構(gòu)型xi的鄰接點(diǎn)集合為Nc={n1,n2,…,nk|ni∈V,ρ(ni,xi) ≤r}，利用局部規(guī)劃器嘗試連接xi與其鄰接點(diǎn)ni，對G(V,E)進(jìn)行邊擴(kuò)展，完成圖中節(jié)點(diǎn)之間的互聯(lián)。

(4) 局部規(guī)劃(Local Planner)：給定自由空間中兩個自由構(gòu)型x1,x2∈Cfree，局部規(guī)劃器生成一條(x1,x2)之間的無碰撞路徑，局部規(guī)劃器需要在單次計(jì)算速度和路徑質(zhì)量之間進(jìn)行折中。最簡單的局部規(guī)劃器就是生成(x1,x2)之間的直線段，這種局部規(guī)劃器生成的路徑簡單且單次計(jì)算量代價(jià)小，應(yīng)用廣泛。同時(shí)為了節(jié)省空間，學(xué)習(xí)階段不保存路徑，因此局部規(guī)劃器還會在查詢階段被使用。

(5)碰撞檢測(Collision Checker)：給定兩個構(gòu)型x1,x2∈C，若連接x1和x2的直線全部位于自由空間Cfree中，即[x1,x2]∈Cfree，則x1,x2∈C為一組無碰撞的構(gòu)型對。

1.2　查詢階段

借助在學(xué)習(xí)階段構(gòu)建的概率路圖G(V,E)就能完成很高效的路徑規(guī)劃。具體方法如下。

嘗試將起始構(gòu)型xI和目標(biāo)構(gòu)型xG通過可行路徑連接到G(V,E)中，若失敗則無可行解，若成功則在新的無向圖中利用圖搜索算法得到由起始構(gòu)型xI到目標(biāo)構(gòu)型xG的可行解。

查詢階段的關(guān)鍵在于如何將起始構(gòu)型xI和目標(biāo)構(gòu)型xG連接到G(V,E)中，即尋找起始構(gòu)型xI和目標(biāo)構(gòu)型xG到G(V,E)的可行路徑。一個最簡單的策略是借助與學(xué)習(xí)階段一樣的局部規(guī)劃器嘗試遍歷連接xI與G(V,E)，同時(shí)為了限制圖的規(guī)模，減小查詢復(fù)雜度，文獻(xiàn)[1]還對最大路徑長度進(jìn)行了限制，即超過最大長度的邊將被拋棄。

2　合理的采樣點(diǎn)分布

解決窄通道問題最直接的方法是改變采樣策略，使采樣點(diǎn)分布滿足合理的分布特征[6，13-14]。

自由空間被分為三類：窄通道區(qū)域、開闊區(qū)域和邊緣角落區(qū)域，不同區(qū)域的示意如圖1所示。在窄通道區(qū)域，采樣點(diǎn)少是造成概率路圖連通性差的主要原因；在開闊區(qū)域，過多的采樣點(diǎn)會影響路徑查詢的效率。因此合理的采樣點(diǎn)分布應(yīng)該如下：窄通道區(qū)域密集分布，開闊區(qū)域稀疏分布，邊緣和角落區(qū)域中等密度分布。

3　基于距離變換的PRM路徑規(guī)劃

使用距離變換將原始地圖中每個點(diǎn)的值變換為該點(diǎn)到距其最近障礙物的距離，這樣得到的地圖稱為距離變換地圖。距離變換地圖和原始地圖的對比如圖1所示。經(jīng)過分析，發(fā)現(xiàn)距離變換地圖可以用來計(jì)算工作空間中的障礙物稠密度，以及識別自由空間中點(diǎn)所處的區(qū)域。基于距離變換地圖的以上兩種作用，本文提出一種新的基于距離變換的PRM路徑規(guī)劃算法來解決窄通道問題。距離變換有許多實(shí)現(xiàn)算法，本文采用文獻(xiàn)[15]的算法得到距離變換地圖，該算法被廣泛采用。

圖1　原始地圖與距離變換地圖對比

3.1　距離變換地圖的作用

3.1.1計(jì)算障礙物稠密度

距離變換地圖中每個點(diǎn)的數(shù)值代表該點(diǎn)到距其最近障礙物的距離(本文采用歐氏距離)，由此推測距離變換地圖的平均距離值可以在一定程度上反映整個工作空間中障礙物的稠密程度。

以移動機(jī)器人所處的二維工作空間為例，工作空間尺寸為W×L，在自由空間Cfree中，num(Cfree) 表示自由空間中的總點(diǎn)數(shù)，d(xi) 為自由空間中某點(diǎn)xi對應(yīng)的距離值，定義Dm為自由空間中的平均距離值，Dref為參考平均距離值(工作空間中除邊界障礙物外，內(nèi)部不存在障礙物時(shí)的平均距離值)。Dm計(jì)算公式如下：

參考平均距離值Dref的推導(dǎo)如下：

如圖2所示，尺寸為W×L的矩形，將其劃分為A和B兩部分，A區(qū)域的點(diǎn)的距離值為其到上下邊界的最小值；B區(qū)域可以進(jìn)一步劃分為8個等價(jià)的等腰直角三角形區(qū)域，每個Bi區(qū)域內(nèi)點(diǎn)的距離值為其到外圍邊界的距離。

圖2　無內(nèi)部障礙物地圖

在同一尺寸的地圖中，障礙物越稠密，Dm越小。為了設(shè)計(jì)出一種適應(yīng)不同尺寸工作空間的比較合理的度量方法，首先利用參考平均距離值Dref對Dm進(jìn)行歸一化處理，然后用1減去歸一化處理后的結(jié)果，得到障礙物稠密度OD計(jì)算公式為：OD=1-Dm/Dref。

利用上式計(jì)算得到的障礙物稠密度度量OD范圍為(0,1)，OD值越大表示工作空間中障礙物越稠密。

3.1.2識別不同區(qū)域

通過距離值辨別是否屬于開闊區(qū)域，定義d(xi)>NarrawThreshold為開闊區(qū)域，否則為非開闊區(qū)(窄通道區(qū)域或者邊緣角落區(qū))，其中NarrawThreshold為定義的窄通道的寬度。

窄通道和邊緣角落區(qū)域的區(qū)別：在窄通道區(qū)域的點(diǎn)，連續(xù)沿著距離值最大上升方向若干步數(shù)后，會出現(xiàn)局部極值點(diǎn)，且一直不進(jìn)入開闊區(qū)域；而邊緣角落區(qū)域采取這種操作時(shí)距離值則會一直增大，最終進(jìn)入開闊區(qū)域。所以窄通道和邊緣角落區(qū)域的區(qū)分方法為：以當(dāng)前點(diǎn)為起點(diǎn)，連續(xù)向最大鄰接點(diǎn)方向擴(kuò)展NarrawThreshold-d(xi) 步，若中途出現(xiàn)局部極值點(diǎn)，則為窄通道區(qū)域，否則為邊緣角落區(qū)域。

3.2　算法說明

基于距離變換的改進(jìn)PRM路徑規(guī)劃算法包含三個步驟：

(1)計(jì)算距離變換地圖。根據(jù)輸入的二值地圖，進(jìn)行距離變換得到距離變換地圖。

(2)構(gòu)建PRM無向圖。這一步是整個算法的核心。基于距離變換的PRM首先根據(jù)障礙物稠密度來計(jì)算所需要的采樣點(diǎn)數(shù)目，然后在采樣階段，根據(jù)距離變換地圖識別不同區(qū)域，在不同區(qū)域采取不同采樣策略，使得采樣點(diǎn)在窄通道內(nèi)密集分布。

(3)路徑查詢。輸入路徑查詢請求，基于步驟(2)中構(gòu)建的PRM無向圖，利用D算法進(jìn)行路徑規(guī)劃，返回得到的最優(yōu)路徑。

距離變換地圖使用3.1節(jié)中提及的方法計(jì)算，路徑查詢步驟與其他PRM算法一樣：首先將起始點(diǎn)和終點(diǎn)連接到PRM無向圖中合適的節(jié)點(diǎn)上，然后利用通用的D、A*等路徑規(guī)劃算法即可得到路徑，本文的路徑查詢使用D算法。

PRM無向圖的構(gòu)建是整個PRM算法的核心部分，其構(gòu)建流程如下：

算法2：DT-Based PRM

1.INIT G(V,E);

2.Init_Node = Sampling();

3.SamplingNodes.add(Init_Node)；

4.MaxNumber = getSamplingNumbers(DTMap, RobotSize);

5.while(numbers_vertex(G) < MaxNumber){

6.foreachminSamplingNodes{

7.newPts = SamplingNode(m, DTMap);

8.SamplingNodesBuffer.add(newPts);

9.Range = Range(m, DTMap);

10.neighbors = getNeightbors(G, m, Range);

11.V.add(m);

12.foreachkinneightbors {

13.if(CollisionFree(k, m))

14.E.add((k, m));

15.SamplingNodes = SamplingNodeBuffer;

16.SamplingNodeBuffer.clear()

17.

第1行， 初始化一個空的PRM無向圖G(V,E)，V代表圖中的節(jié)點(diǎn)集合，E代表圖中的邊集合。

第2～3行， 隨機(jī)獲得一個位于自由空間中的初始起點(diǎn)Init_Node作為采樣起始點(diǎn)，并放入存儲采樣點(diǎn)的隊(duì)列SamplingNodes中。

第5～17行，重復(fù)進(jìn)行采樣和局部規(guī)劃操作，直到圖中節(jié)點(diǎn)V的數(shù)目達(dá)到MaxNumber個。

第6～14行，逐個以隊(duì)列SamplingNodes中的采樣點(diǎn)為中心進(jìn)行采樣操作。其中：第7行，以當(dāng)前采樣點(diǎn)為起點(diǎn)，首先識別當(dāng)前點(diǎn)所在的區(qū)域，然后根據(jù)不同的區(qū)域采取不同的采樣策略獲取新的采樣點(diǎn)，d(xi)為xi處的距離值，MDW=M/2為安全距離。不同區(qū)域的采樣策略為，在開闊區(qū)域，進(jìn)行稀疏采樣：分別向最大鄰域和最小鄰域進(jìn)行d(xi)-MDW步采樣，得到兩個方向的采樣點(diǎn)；在窄通道區(qū)域，進(jìn)行密集采樣：以當(dāng)前點(diǎn)為中心、2·MDW～4·d(xi)為半徑的環(huán)狀區(qū)域內(nèi)均勻采樣8個點(diǎn)；在邊緣和角落區(qū)域進(jìn)行適量采樣：以當(dāng)前點(diǎn)為中心，半徑為3·MWD～6·d(xi)的環(huán)狀區(qū)域均勻采樣4個點(diǎn)。第8行，將新的采樣點(diǎn)放入另外一個隊(duì)列SamplingNodesBuffer中。第9～10行，首先根據(jù)當(dāng)前采樣點(diǎn)所處區(qū)域得到需要的范圍Range，然后獲取G(V,E)中當(dāng)前采樣點(diǎn)的鄰接點(diǎn)neighbors。顯然如果與PRM無向圖中所有的節(jié)點(diǎn)進(jìn)行碰撞檢測,計(jì)算量將非常大，考慮到算法中不同區(qū)域采樣的不同采樣策略，為了減小碰撞檢測和局部規(guī)劃的次數(shù)，在不同區(qū)域采用不同半徑范圍內(nèi)的鄰接點(diǎn)來完成節(jié)點(diǎn)間的內(nèi)部互聯(lián)，該范圍與不同區(qū)域的采樣策略有關(guān)。第11行，將當(dāng)前采樣點(diǎn)加入G(V,E)頂點(diǎn)集V中。第12～14行，對neighbors中所有節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)進(jìn)行碰撞檢測和局部規(guī)劃，若存在安全路徑，則添加邊進(jìn)入G(V,E)。第15～16行，交換SamplingNodesBuffer和SamplingNodes，準(zhǔn)備開始下一輪迭代。

4　實(shí)驗(yàn)結(jié)果

為了驗(yàn)證算法的有效性，在不同環(huán)境下分別進(jìn)行了障礙物稠密計(jì)算、區(qū)域識別以及PRM構(gòu)建和路徑查詢仿真。

首先，設(shè)計(jì)了四種仿真環(huán)境(地圖尺寸為500×500)用于測試，如圖3所示。

圖3　仿真環(huán)境

4.1　障礙物稠密度計(jì)算

四種環(huán)境的障礙物稠密度計(jì)算結(jié)果如表1所示，結(jié)果表明OD值能度量不同環(huán)境下障礙物稠密程度。

表1　障礙物稠密度結(jié)果

4.2　區(qū)域識別

區(qū)域識別的結(jié)果如圖4所示，圖中白色區(qū)域?yàn)殚_闊區(qū)域、斜線陰影區(qū)域?yàn)檫吘壗锹鋮^(qū)域、豎線網(wǎng)格區(qū)域?yàn)檎ǖ绤^(qū)域、黑色區(qū)域?yàn)檎系K物區(qū)域。從圖中可以看出，利用距離變換地圖的距離值特征可有效地識別圖中不同的區(qū)域。

圖4　區(qū)域識別結(jié)果

4.3　采樣點(diǎn)分布和概率路圖

圖5為采樣點(diǎn)的分布示意圖，圖6為最終生成的PRM無向圖，可以看出最終的采樣點(diǎn)符合期望的分布，能夠在窄通道區(qū)域內(nèi)成功建立PRM無向圖，表明本文基于距離變換的PRM路徑規(guī)劃算法能解決窄通道問題。

圖5　采樣點(diǎn)分布結(jié)果

圖6　PRM圖

表2列出了算法中的關(guān)鍵操作的耗時(shí)，包括：距離變換耗時(shí)、區(qū)域識別耗時(shí)、平均PRM構(gòu)建耗時(shí)(10次)、路徑查詢平均耗時(shí)(50次)。仿真使用的機(jī)器主要參數(shù)如下：Intel(R) Core(TM) i5-4590 CPU @ 3.30 GHz，2 GB內(nèi)存， 操作系統(tǒng)為Ubuntu 16.04。

表2　關(guān)鍵操作耗時(shí)　(ms)

相比于其他用于解決窄通道的PRM算法，本文提出的算法主要多了距離變換和區(qū)域識別的耗時(shí)，從表2中可以看出計(jì)算距離變換地圖和區(qū)域識別的耗時(shí)相對PRM構(gòu)建耗時(shí)幾乎可以忽略。

5　結(jié)論

本文提出基于距離變換的PRM路徑規(guī)劃算法，分析了障礙物稠密度對距離均值的影響和不同區(qū)域內(nèi)的距離值的分布特征，設(shè)計(jì)了一種計(jì)算工作空間中障礙物稠密度的方法，借助障礙物稠密度，算法能自適應(yīng)地計(jì)算不同環(huán)境下所需采樣點(diǎn)數(shù)目，然后利用距離分布特征來簡單地識別不同區(qū)域，在不同區(qū)域采用不同的采樣策略。實(shí)驗(yàn)結(jié)果表明，相對于其他算法，算法增加的距離計(jì)算時(shí)間代價(jià)很小，但能夠根據(jù)不同環(huán)境自適應(yīng)計(jì)算總采樣點(diǎn)數(shù)目，成功解決了窄通道問題。

[1] KARAMAN S, FRAZZOLI E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846-894.

[2] KAVRAKI L E, SVESTKA P, LATOMBE J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J]. IEEE Transactions on Robotics and Automation, 1996, 12(4): 566-580.

[3] ELBANHAWI M, SIMIC M. SAMPLING-based robot motion planning: a review[J]. IEEE Access, 2014 (2): 56-77.

[4] HSU D, KAVRAKI L E, LATOMBE J C, et al. On finding narrow passages with probabilistic roadmap planners[C].Robotics: The Algorithmic Perspective: 1998 Workshop on the Algorithmic Foundations of Robotics, 1998: 141-154.

[5] AMATO N M, BAYAZIT O B, DALE L K. OBPRM: an obstacle-based PRM for 3D workspaces[C]. Proceedings of Third Workshop on Algorithm Foundations of Robotics (WAFR), Houston TX, 1998: 155-168.

[6] BOOR V, OVERMARS M H, VAN DER STAPPEN A F. The Gaussian sampling strategy for probabilistic roadmap planners[C].1999 IEEE International Conference on Robotics and Automation. IEEE, 1999: 1018-1023.

[7] LIN Y T. The Gaussian PRM sampling for dynamic configuration spaces[C].9th International Conference on Control, Automation, Robotics and Vision, 2006. ICARCV'06. IEEE, 2006: 1-5.

[8] HSU D, JIANG T, REIF J, et al. The bridge test for sampling narrow passages with probabilistic roadmap planners[C].2003. Proceedings. ICRA'03. IEEE International Conference on Robotics and Automation. IEEE, 2003: 4420-4426.

[9] SUN Z, HSU D, JIANG T, et al. Narrow passage sampling for probabilistic roadmap planning[J]. IEEE Transactions on Robotics, 2005, 21(6): 1105-1115.

[10] VAN DEN BERG J P, OVERMARS M H. Using workspace information as a guide to non-uniform sampling in probabilistic roadmap planners[J]. The International Journal of Robotics Research, 2005, 24(12): 1055-1071.

[11] 劉洋, 章衛(wèi)國, 李廣文. 基于改進(jìn) PRM 算法的路徑規(guī)劃研究倡[J]. 計(jì)算機(jī)應(yīng)用研究, 2012, 29(1): 104-106, 139.

[12] BERA T, BHAT M S, GHOSE D. An efficient random walk strategy for sampling based robot motion planners[C].13th FIRA Robot World Congress, 2010: 234-241.

[13] ZHONG J, SU J. Robot path planning in narrow passages based on probabilistic roadmaps[J]. International Journal of Robotics and Automation, 2013, 28(3).

[14] SICILIANO B, KHATIB O. Springer handbook of robotics[M]. Springer, 2016.

[15] FELZENSZWALB P, HUTTENLOCHER D. Distance transforms of sampled functions[R]. Cornell University, 2004.