基于距離變換的PRM路徑規劃算法

李　敏，周遠遠，黃　魯

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

0　引言

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

在構型空間中直接計算路徑被證明是非常困難的，尤其是在高維空間中[2]。PRM算法在自由空間中獲取采樣點，并通過采樣點之間的互聯來構建無向圖，用于近似表示自由空間，這種方法將連續空間中的規劃問題轉化到拓撲空間中解決，大大減小了計算可行路徑的復雜性，因此自PRM算法提出以來，受到了廣泛關注并且在實際應用中取得較好的效果[3]。

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

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

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

1　基本PRM算法

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

1.1　學習階段

學習階段是PRM算法的關鍵步驟，算法流程如算法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);

其主要內容包括：

(1) 初始化。G(V,E)為一個無向圖，初始狀態為空。G(V,E)的頂點對應自由構型，連接頂點的邊對應兩個自由構型之間的無碰撞路徑。

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

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

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

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

1.2　查詢階段

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

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

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

2　合理的采樣點分布

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

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

3　基于距離變換的PRM路徑規劃

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

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

3.1　距離變換地圖的作用

3.1.1計算障礙物稠密度

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

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

參考平均距離值Dref的推導如下：

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

圖2　無內部障礙物地圖

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

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

3.1.2識別不同區域

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

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

3.2　算法說明

基于距離變換的改進PRM路徑規劃算法包含三個步驟：

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

(2)構建PRM無向圖。這一步是整個算法的核心?；诰嚯x變換的PRM首先根據障礙物稠密度來計算所需要的采樣點數目，然后在采樣階段，根據距離變換地圖識別不同區域，在不同區域采取不同采樣策略，使得采樣點在窄通道內密集分布。

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

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

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

算法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代表圖中的節點集合，E代表圖中的邊集合。

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

第5～17行，重復進行采樣和局部規劃操作，直到圖中節點V的數目達到MaxNumber個。

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

4　實驗結果

為了驗證算法的有效性，在不同環境下分別進行了障礙物稠密計算、區域識別以及PRM構建和路徑查詢仿真。

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

圖3　仿真環境

4.1　障礙物稠密度計算

四種環境的障礙物稠密度計算結果如表1所示，結果表明OD值能度量不同環境下障礙物稠密程度。

表1　障礙物稠密度結果

4.2　區域識別

區域識別的結果如圖4所示，圖中白色區域為開闊區域、斜線陰影區域為邊緣角落區域、豎線網格區域為窄通道區域、黑色區域為障礙物區域。從圖中可以看出，利用距離變換地圖的距離值特征可有效地識別圖中不同的區域。

圖4　區域識別結果

4.3　采樣點分布和概率路圖

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

圖5　采樣點分布結果

圖6　PRM圖

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

表2　關鍵操作耗時　(ms)

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

5　結論

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

[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] 劉洋, 章衛國, 李廣文. 基于改進 PRM 算法的路徑規劃研究倡[J]. 計算機應用研究, 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.