移動(dòng)機(jī)器人的柵格概率路徑圖法路徑規(guī)劃

邱添，張志安，王海龍

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

0 前言

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

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

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

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

1 GPRM算法原理

1.1 經(jīng)典PRM算法

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

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

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

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

圖2 不同采樣數(shù)量的路徑圖

1.2 GPRM算法原理

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

1.2.1 柵格劃分方法

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

(1)

(2)

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

1.2.2 采樣策略

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

圖6 可行解與最優(yōu)解

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

圖3 障礙物內(nèi)采樣示意

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

圖4 狹窄通道內(nèi)采樣

為了驗(yàn)證該采樣方法在狹窄通道內(nèi)的效率，本文作者用不同算法在該地圖內(nèi)分別進(jìn)行50次仿真，表1記錄了采樣數(shù)目相同時(shí)，不同方法在通道內(nèi)的采樣點(diǎn)數(shù)目，其中，GPRM算法柵格縮放系數(shù)為8。由于4種方法采樣時(shí)間均小于10 ms，對(duì)算法運(yùn)行實(shí)時(shí)性的影響忽略不計(jì)，故未記錄采樣耗時(shí)?？梢悦黠@看出:GPRM算法的采樣方法在狹窄通道中的采樣效率相比其他算法有優(yōu)勢。

表1 狹窄通道內(nèi)采樣統(tǒng)計(jì)

1.2.3 連接策略

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

圖5 不同柵格關(guān)系示意

1.2.4 路徑優(yōu)化與平滑

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

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

優(yōu)化結(jié)果如圖7所示:優(yōu)化前，路徑總長度為943.9，節(jié)點(diǎn)數(shù)量為10；優(yōu)化后，路徑總長度為907.3，節(jié)點(diǎn)數(shù)量為7，路徑長度和節(jié)點(diǎn)數(shù)量分別下降了3.8%和30%。

圖7 路徑優(yōu)化示意

優(yōu)化后，路徑的質(zhì)量有所提升，但其仍然是由數(shù)條線段相連組成的，不符合實(shí)際應(yīng)用場景中移動(dòng)機(jī)器人的運(yùn)動(dòng)約束，因此需要對(duì)路徑進(jìn)行平滑，去掉路徑之間的折角，產(chǎn)生一條光滑連續(xù)的路徑。

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

圖8 路徑平滑示意

圖9 路徑平滑效果展示

2 算法及結(jié)果分析原理

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

2.1 柵格縮放系數(shù)分析與性能對(duì)比

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

圖10 測試地圖

首先對(duì)簡單地圖進(jìn)行測試，選取不同的采樣點(diǎn)數(shù)量和柵格縮放系數(shù)k，每次測試運(yùn)行100次程序，結(jié)果如表2所示。

表2 簡單地圖測試結(jié)果

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

在簡單地圖上的仿真很難看出柵格縮放系數(shù)k的選取與算法成功率的關(guān)系，下面將會(huì)選取不同的采樣點(diǎn)數(shù)目和系數(shù)k，在大小為1 000×1 000的復(fù)雜地圖上進(jìn)行仿真實(shí)驗(yàn)和對(duì)比，每種系數(shù)運(yùn)行100次，結(jié)果如表3所示，圖11為GPRM算法在復(fù)雜地圖上的運(yùn)行結(jié)果。分析可知：在采樣數(shù)不變的情況下，增加?xùn)鸥窨s放系數(shù)k能夠有效減少程序運(yùn)行時(shí)間，但是k的增加可能導(dǎo)致生成路徑成功率的下降。根據(jù)對(duì)失敗時(shí)采樣點(diǎn)分布信息的觀察發(fā)現(xiàn)：在采樣數(shù)量固定時(shí)，增加?xùn)鸥窨s放系數(shù)，會(huì)導(dǎo)致平均分在每個(gè)柵格內(nèi)的點(diǎn)數(shù)減少，當(dāng)分配到每個(gè)柵格內(nèi)的采樣點(diǎn)數(shù)小于1.5時(shí)，算法在復(fù)雜地圖上的成功率會(huì)明顯下降。

表3 復(fù)雜地圖測試結(jié)果

圖11 復(fù)雜地圖運(yùn)行結(jié)果

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

2.2 算法性能測試

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

表4 綜合測評(píng)結(jié)果

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

3 結(jié)語

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