移動機器人的柵格概率路徑圖法路徑規劃

邱添，張志安，王海龍

(南京理工大學機械工程學院，江蘇南京 210094)

0 前言

近年來，移動機器人被廣泛應用于工業、娛樂、軍事等領域，作為機器人研究領域核心問題的路徑規劃也成為研究熱點。路徑規劃算法根據其搜索路徑的方式不同，大體可以分為三類：基于圖論的路徑規劃算法，基于采樣的路徑規劃算法和智能路徑規劃算法。基于圖論的經典算法包括Dijkstra算法[1]和A*算法[2-3]等。智能路徑算法的代表包括人工勢場法[4]、遺傳算法[5]、蟻群算法[6]等。基于采樣的路徑規劃算法主要為快速搜索隨機樹算法[7](Rapidly-Exploring Random Trees，RRT)和概率路徑圖算法(Probability Roadmap Method, PRM)。

概率路徑圖算法在地圖中進行有限數量的采樣，隨后在這些點間建立連接，再使用基于圖論的方法規劃路徑，極大地減少了運算量。但是PRM算法的缺陷在于：(1)算法很難在狹小的通道中進行采樣，若是增加采樣數量，算法計算量會呈指數形式增長；(2)PRM算法生成的路徑常帶有大量折角，不符合移動機器人在實際運動時的約束。

針對這些問題，BOOR等[8]提出了一種基于高斯分布的采樣方法，增強了在障礙物附近的采樣概率，提升了采樣效率。但是該算法對采樣點是“挑剔”的：只有當一個采樣點本身無碰撞且周圍有障礙物時，該采樣點才會被添加到地圖中，該方法在障礙物附近采樣時的耗時為隨機采樣的4倍，在復雜地圖上難以滿足實時性要求。HSU等[9]提出一種橋型測試，增加將點放入狹窄通道中的概率以提升算法可靠性。孫鳳池等[10]提出了智能概率圖算法(SPRM)，在障礙物周圍建立局部柵格地圖，并且進行路徑平滑，提升了路徑質量，但是依靠障礙物建立柵格也需要額外的復雜度，因此算法實時性較差。RAVANKAR等[11]提出了HPPRM算法，該算法將人工勢場法與PRM算法融合，提升了在狹窄通道的通過率，但是需要額外的時間計算勢場力。宋宇、王志明[12]將角點檢測加入了PRM算法，直接針對障礙物采樣，提升了窄道通過率，但是角點檢測需要對整個地圖作卷積，這需要額外的時間復雜度，并且角點檢測對圓形等沒有角點的障礙物效果不明顯。CAO等[13]改進了采樣器，選擇分布在障礙物內部的采樣點，按距離均勻采樣，從而增加狹窄通道內的采樣點數量，但是該方法在簡單地圖上的優化效果較好，在復雜地圖運算量較大。

本文作者提出一種柵格概率路徑圖算法(GridProbability Roadmap Method, GPRM)，將整個地圖劃分為不同區域，根據區域的不同使用不同的采樣策略，提升采樣的效率,減少安全區域大量無效的采樣，增加狹窄通道內的采樣，從而提升算法的實時性和可行性;在采樣后，優化連接方式，減少了路徑長度；提升路徑平滑度，使路徑符合移動機器人實際的運動約束，提升路徑的安全程度。

1 GPRM算法原理

1.1 經典PRM算法

經典PRM算法分為兩個階段：學習階段和查詢階段。在學習階段，建立空的無向圖G=(V,E)，通過采樣器在給定的構型空間C上隨機且均勻地采樣n個點，采樣點落在自由空間Cfree保留，將點加入V，落在障礙空間Cobs則重新采樣，直至采樣點數量達到n。采樣結束后，連接器將遍歷節點集V，建立點與點之間的連接。如果點與點之間沒有障礙物，則認為兩個點相通，計算并保存兩點之間的距離，將這條邊加入集合E。圖1展示了學習階段生成的采樣圖和連線圖。

查詢階段使用圖搜索算法搜索出一條最短路徑，通常使用Dijkstra算法或A*算法，最后得到一條從起點到終點的路徑。

其中，查詢階段花費時間較少，影響算法整體效率的部分主要在學習階段。由于要遍歷所有點，進行碰撞檢測和距離計算，最耗時的部分在學習階段連接器建立連接的過程，時間復雜度為O(n2)。因此，采樣點的數量直接關系到算法的運行速度。

大多數情況下，相對少的采樣點足以覆蓋大多數可行空間，可以保持算法的實時性，但是隨著環境變復雜，或者采樣點分布不合理，算法可能出現無解的情況。如圖2(a)所示，圖中的采樣點數量為50，由于采樣點分布不合理，無法形成從起點到終點的連通路徑。這種情況常常發生在狹窄的通道處，通過增加采樣點可以解決辦法，但是算法運行時間會呈指數形式上升。圖2(b)中采樣點數量為100，形成了連通終點到起點的連通路徑。

圖2 不同采樣數量的路徑圖

1.2 GPRM算法原理

經典PRM算法采用隨機采樣的方式，在狹窄通道處采樣非常困難，而大片無障礙的連通區域往往會堆積較多數量的采樣點，這些采樣點對優化路徑的意義較小，并且會增加計算量。因此，GPRM算法主要進行了3點改進：(1)用柵格劃分構型空間，根據柵格內障礙物的面積，劃分柵格類型，并基于柵格的類型采取不同采樣策略，增加在障礙物附近尤其是狹小通道附近的采樣，減少連通區域的采樣；(2)在查詢階段建立點與點的連接時，不再遍歷所有節點，每個節點只和附近柵格的節點進行連接，減少運算量；(3)建立路徑后，使用算法平滑、優化路徑。

1.2.1 柵格劃分方法

在學習階段前，增加預處理階段，假設構型空間C大小為ncol×nrow,設定柵格縮放系數k(k>1)，柵格的邊長l通過公式(1)得到，柵格的數量n通過公式(2)得到，其中ceil()為向上取整運算。

(1)

(2)

劃分柵格后，通過對柵格區域的數值求和計算每個柵格中障礙物占的比例。如果柵格完全處于自由空間Cfree中，將柵格劃分為安全柵格；如果柵格完全處于障礙物Cobs中，則將柵格劃分為威脅柵格；否則，將柵格劃分為阻礙柵格，若柵格中的障礙物占比大于一半，記為威脅阻礙柵格，否則記為安全阻礙柵格。

1.2.2 采樣策略

在學習階段，采樣將基于柵格類型進行。將構型空間C采樣的總數mtotal平均分給每個柵格，每個柵格的采樣數量mgrid=mtotal/n，當mgrid不為整數時，且障礙物小于柵格面積的一半時mgrid向上取整，否則向下取整。本文作者將一個柵格上、下、左、右4個柵格稱為它的相通柵格，將一個柵格上、下、左、右、左上、左下、右上、右下的8個柵格稱為它的相鄰柵格，如圖6所示。對于安全柵格，如果它的相通柵格均為安全柵格，則只在柵格的中心采樣一次便開始下一個柵格的采樣，否則在柵格內隨機采樣。這樣可以避免在自由空間Cfree內的頻繁采樣，減少計算量。對于威脅柵格，將分配給該柵格的采樣點隨機分給相鄰的阻礙柵格，如果它的相鄰柵格均為阻礙柵格，則不采樣。對于阻礙柵格，進行隨機采樣，得到采樣點Ps(xs,ys)，Ps落在自由空間Cfree中則保留；落在障礙物Cobs中，若該柵格為安全阻礙柵格則重新采樣，若是威脅阻礙柵格則利用障礙物內采樣法進行重采樣，保留重采樣點Pd。

圖6 可行解與最優解

阻礙柵格內的障礙物內采樣方法步驟如下：首先在柵格內提取障礙物邊緣點，得到障礙物邊界的點集B，然后遍歷點集B，得到離采樣點最近的邊緣點Pb(xb,yb),接著計算出Ps和Pb之間的歐氏距離d，沿著PsPb延長一個隨機距離dr(0

圖3 障礙物內采樣示意

這種采樣方法有利于在狹小的通道內進行采樣，能充分利用采樣失敗時獲得的數據，提升采樣效率，從而有效提升算法成功率。圖4展示了障礙物內采樣的效果，此刻算法的采樣點數為150，柵格縮放系數為8，在通道內生成了15個采樣點。

圖4 狹窄通道內采樣

為了驗證該采樣方法在狹窄通道內的效率，本文作者用不同算法在該地圖內分別進行50次仿真，表1記錄了采樣數目相同時，不同方法在通道內的采樣點數目，其中，GPRM算法柵格縮放系數為8。由于4種方法采樣時間均小于10 ms，對算法運行實時性的影響忽略不計，故未記錄采樣耗時。可以明顯看出:GPRM算法的采樣方法在狹窄通道中的采樣效率相比其他算法有優勢。

表1 狹窄通道內采樣統計

1.2.3 連接策略

采樣結束后，得到節點集E，此時，需要將能相通的節點連接起來。經典PRM算法遍歷整個節點集E，逐個測試每個節點是否相通，時間復雜度為O(n2)，當節點數量增加時，算法運行時間呈指數形式增長，難以滿足實時性的要求。在GPRM算法中，節點不需要與所有其他節點進行連接測試，只需要對節點所在柵格的相通柵格以及其相通柵格內的節點進行連接測試，其示意見圖5(c)。并且，連接范圍可以根據需求靈活調整。時間復雜度下降為O(mn)，其中m為起始點所在柵格的碰撞檢測柵格(圖5(c))內采樣點的數量，且m

圖5 不同柵格關系示意

1.2.4 路徑優化與平滑

Dijkstra算法可以得到最優路徑，但是在GPRM算法中，并非所有可連接的點都被連接，因此只能得到可行解而非最優解，可能出現如圖6所示的情況，點P1為起點，點P3為終點，容易看出最短路徑為P1P3而不是P1P2加上P2P3，但點P3不在點P1的檢索范圍內，因此兩點沒有連通。對于這種情況可以加大連接過程時的檢索范圍，但這樣會增加算法復雜度，算法實時性受到影響，因此需要優化路徑。

路徑優化過程如下：(1)獲得Dijkstra算法獲得的路徑點集P{Pi,i=1,2,…,n}，P1和Pn分別為起點和終點，建立優化路徑點集N{P1}用來存放優化后的路徑，初始只包括起點。(2)將P1作為測試點，逐個測試P中的點，若測試點與點Pm無法直接相連，則認為點Pm-1為轉折點，點P2,…,Pm-2為冗余節點，將Pm-1放入優化路徑點集N中，并將Pm-1作為新的測試點，逐個向后測試，直到測試到終點。(3)逐個連接優化路徑點集N中的點，生成完整的路徑。

優化結果如圖7所示:優化前，路徑總長度為943.9，節點數量為10；優化后，路徑總長度為907.3，節點數量為7，路徑長度和節點數量分別下降了3.8%和30%。

圖7 路徑優化示意

優化后，路徑的質量有所提升，但其仍然是由數條線段相連組成的，不符合實際應用場景中移動機器人的運動約束，因此需要對路徑進行平滑，去掉路徑之間的折角，產生一條光滑連續的路徑。

如圖8所示，點P1、P2、P3、P4為一系列路徑點，移動機器人最小轉向半徑為rmin，∠P1P2P3=α1, ∠P2P3P4=α2, 對于路徑的第一部分P1P2P3,取l=min(lP1P2,lP2P3)/2，在P1P2和P2P3上找到與點P2距離為l的點Pl1和Pl2，那么可以得到半徑r1=l/tan(α1/2)，圓心O1通過求線段P1P2和線段P2P3過Pl1Pl2的垂線的交點得到。得到圓弧后，檢測圓弧上的每個點，如果圓弧在障礙物上，則將l減半，重復上述步驟，若半徑r1

圖8 路徑平滑示意

圖9 路徑平滑效果展示

2 算法及結果分析原理

文中算法仿真部分將分為兩個部分：第一部分將分析柵格縮放系數的選取對GPRM算法性能的影響；第二部分將在不同類型的地圖上比較GPRM算法與經典PRM算法的各項性能。仿真設備CPU為Intel(R)Core(TM)i5-9300H 2.4GHz，RAM為8 GB，仿真軟件為MATLAB 2018a。

2.1 柵格縮放系數分析與性能對比

GPRM算法的核心思想是用柵格劃分整個地圖，采樣和連接都針對柵格進行。柵格的大小與數量都和柵格縮放系數k有直接的關系，因此，柵格縮放系數的選取非常重要。文中分別在大小為500×500的簡單地圖和大小為1 000×1 000復雜地圖上選取不同的柵格縮放系數k和采樣點數，分析其對性能的影響。兩張測試地圖如圖10所示。

圖10 測試地圖

首先對簡單地圖進行測試，選取不同的采樣點數量和柵格縮放系數k，每次測試運行100次程序，結果如表2所示。

表2 簡單地圖測試結果

對表1結果進行分析可知：采樣點數量不變時，柵格縮放系數k越大，運行時間越短。當柵格縮放系數k≥5時，GPRM算法運行時間和通過率都優于經典PRM算法，尤其是采樣點數量為100、柵格縮放系數為10時，運行時間的提升達到了84.5%。但當k取3時，GPRM算法的運行時間反而超過了經典算法。在分析算法不同部分的耗時后發現：算法的耗時主要集中在學習階段的連接部分。經典PRM算法的連接部分需要遍歷所有點，而GPRM算法只需遍歷附近柵格內的點即可，因此能節省很多時間。但是當柵格縮放系數k較小時，GPRM算法也近乎遍歷所有點進行連接測試，除此以外還需進行提取附近柵格采樣點的操作，因此算法性能反而不如經典算法。

在簡單地圖上的仿真很難看出柵格縮放系數k的選取與算法成功率的關系，下面將會選取不同的采樣點數目和系數k，在大小為1 000×1 000的復雜地圖上進行仿真實驗和對比，每種系數運行100次，結果如表3所示，圖11為GPRM算法在復雜地圖上的運行結果。分析可知：在采樣數不變的情況下，增加柵格縮放系數k能夠有效減少程序運行時間，但是k的增加可能導致生成路徑成功率的下降。根據對失敗時采樣點分布信息的觀察發現：在采樣數量固定時，增加柵格縮放系數，會導致平均分在每個柵格內的點數減少，當分配到每個柵格內的采樣點數小于1.5時，算法在復雜地圖上的成功率會明顯下降。

表3 復雜地圖測試結果

圖11 復雜地圖運行結果

通過以上的仿真實驗，可以得到柵格縮放系數k的取值原則：(1)如果沒有特殊要求，k的取值必須大于3，否則算法效率會低于經典算法。(2)在采樣點數不變的情況下，k的值越大，算法的運行速度越快。(3)總采樣點數不變的情況下，每個柵格中的采樣點數隨著k的增大而減小，當柵格中采樣點數小于3時，算法成功率會下降，因此，在復雜地圖上，不能讓k無限增大，盡量保持每個柵格中的采樣點數大于等于2。在實際應用中，可以選擇較多的采樣點數和較大的柵格縮放系數k，同時兼顧算法的實時性和有效性。

2.2 算法性能測試

由于移動機器人的路徑規劃并沒有標準的數據集供算法進行測試，本文作者將選取來自文獻[1-11]中的32幅地圖，以及8幅作者根據周圍環境建立的地圖作為數據集。經過統計得到地圖上障礙物的平均占比為36.2%。為了統一數據，將所有地圖的尺寸縮放為500×500，在每張地圖上運行10次，40張地圖共400次。除了GPRM算法外，選取了經典PRM算法、高斯采樣法[8]和橋測試采樣法[9]作為對照，由于高斯法和橋測試法的采樣需要較多的采樣點才能完成，因此這兩種方法采樣數均為100，而經典PRM和GPRM有采樣數為50和100的測試。仿真結果如表4所示。

表4 綜合測評結果

根據表4可知：GPRM算法相對于經典PRM算法，能節省60%以上的運行時間，地圖通過率能提升20%以上；同時，算法性能也優于高斯采樣法和橋測試采樣法，極大地提升了PRM算法的可靠性和實時性，并且GPRM算法生成的路徑平滑，符合移動機器人的運動約束。

3 結語

針對經典PRM算法實時性差，并且難以通過狹窄通道的問題，提出了柵格概率路徑圖法。利用柵格劃分地圖，根據柵格內障礙物面積將柵格化分為不同威脅等級，并根據柵格的威脅程度進行采樣，提升了在狹窄通道內采樣的概率。并且簡化了建立采樣點間連接的步驟，極大地提升了算法的效率；規劃路徑后，優化、平滑生成的路徑，使之符合移動機器人的約束。還分析了柵格縮放系數k的選取對算法性能和結果的影響，k的取值使每個柵格內的采樣點數量為2時可以兼顧實時性與可靠性。仿真結果表明，GPRM算法有較快的運行速度和較高的魯棒性，并且生成的路徑平滑，符合移動機器人的移動約束，有較高的推廣應用價值。