一種復(fù)雜場景下的路徑規(guī)劃問題解決方法

郭書杰，田 華，王 偉

(大連東軟信息學(xué)院 智能與電子工程學(xué)院，遼寧 大連 116023)

0 引 言

路徑規(guī)劃是在有障礙物的環(huán)境中，按照某種評價標(biāo)準(zhǔn)尋找一條從起始點到目標(biāo)點的無碰撞路徑[1]。路徑規(guī)劃在移動機器人、電子游戲、汽車導(dǎo)航等多個領(lǐng)域均有重要的應(yīng)用價值，是一個具有較高使用價值的研究方向。常用的路徑規(guī)劃算法有兩種：基于采樣的路徑規(guī)劃算法和基于搜索的路徑規(guī)劃算法。Dijstra[2]和A*算法[3]是典型的基于搜索的路徑規(guī)劃算法。這類算法是完備的和最優(yōu)的。由于要使用較小的步長遍歷規(guī)劃空間，其規(guī)劃效率相對較低。典型的基于采樣的路徑規(guī)劃算法有PRM[4]、 RRT[5]和EST[6]等。 這類算法使用隨機采樣的方式探索規(guī)劃空間，所以具有比基于搜索的路徑規(guī)劃更高的效率?；诓蓸拥囊?guī)劃方式有一定的隨機性，僅僅是概率完備的，而且不是最優(yōu)的。為了進一步提高算法的搜索效率，JJK Jr和Steven M. LaValle提出了一種改進算法RRT-connect[7]。RRT-connect能夠提高RRT算法的效率，不過它仍然不是最優(yōu)的，甚至不是漸近最優(yōu)的。為了提高規(guī)劃路徑的質(zhì)量，出現(xiàn)了一些改進算法，如RRT*、PRM*[8]、LBT-RRT[9]。這類改進算法能夠達到漸近最優(yōu)。還有一個改進方向是將基于搜索的相關(guān)技術(shù)引入到基于采樣的規(guī)劃中來，結(jié)合基于采樣的規(guī)劃算法和基于搜索的規(guī)劃算法的優(yōu)勢，兼顧了效率和路徑質(zhì)量，如HRRT[10]、Anytime RRT[11]、Bi-RRT*[12]、C-FOREST[13]、Informed RRT*[14]等。這類算法在一定程度上提高了算法的收斂速度，但由于它們均是基于RRT的改進算法，所以仍然難以保證規(guī)劃出來的路徑質(zhì)量的最優(yōu)性。為了進一步提高路徑質(zhì)量，出現(xiàn)了BIT*、AIT*和ABIT*[15-16]等算法，通過引入節(jié)點排序和邊排序，在限定的子集中執(zhí)行排序搜索，從而達到提高路徑質(zhì)量的目的。還有其他A*算法的改進算法[17-18]和基于智能優(yōu)化算法的路徑規(guī)劃算法[19]。

實驗發(fā)現(xiàn)，盡管這些改進算法在一定程度上提高了規(guī)劃效率和路徑質(zhì)量，但在限制較多的空間中，特別是在窄通道場景和Z字形場景中，它們的規(guī)劃效率會急劇降低，難以在有限的時間內(nèi)規(guī)劃出一條路徑來。為了解決復(fù)雜場景下的路徑規(guī)劃問題，提出了一種相對高效的關(guān)鍵點路徑規(guī)劃算法(key point path planning，KPP)，該算法在提高路徑優(yōu)化效率的同時，也優(yōu)化了規(guī)劃出來的路徑的質(zhì)量，較好地解決了特殊環(huán)境下的路徑規(guī)劃問題。實驗結(jié)果表明，KPP不僅能夠高效地完成窄通道場景和Z字形場景中的路徑規(guī)劃，在其他常見場景的應(yīng)用中也有不俗的表現(xiàn)。

1 路徑規(guī)劃問題

路徑規(guī)劃就是在給定空間中，規(guī)劃出一條從起始點到目標(biāo)點的無碰撞路徑，使得該路徑的代價盡可能小。記空間X?Rn為P，Xobs表示P上被障礙物占據(jù)的空間；Xfree=XXobs表示P上可以通行的空間；Dstart表示起始點；Dgoal表示目標(biāo)點；σ：[0,1]X表示連接Dstart和Dgoal之間的路徑；∑表示Dstart和Dgoal之間所有的路徑組成的集合；S：∑R表示一條路徑的代價函數(shù)；則路徑規(guī)劃問題可以描述為：

Dgoal, ?t∈[0,1],σ(1)∈Xfree}

2 關(guān)鍵點路徑規(guī)劃算法

關(guān)鍵點路徑規(guī)劃算法基于“兩點之間直線最短”的原理，首先找出阻礙起始點與目標(biāo)點直線連通的障礙物KObses；然后找出繞過KObses的最近路線必須經(jīng)過的點作為關(guān)鍵點；接著以這些關(guān)鍵點為引導(dǎo)，將路徑規(guī)劃從隨機搜索轉(zhuǎn)換為啟發(fā)式搜索，從而提高規(guī)劃效率和路徑質(zhì)量；最后通過路徑壓縮來減少路徑長短，進一步提高規(guī)劃出的路徑的質(zhì)量。

圖1 KPP處理過程

關(guān)鍵點路徑規(guī)劃算法的處理過程如圖1所示。第一步：找出起始點到目標(biāo)點之間的關(guān)鍵點(見圖1(a))。這些關(guān)鍵點是通過對起始點和目標(biāo)點連線上的障礙物的某些頂點外推得到的。第二步：選用某種路徑規(guī)劃算法規(guī)劃出各個關(guān)鍵點之間的子路徑；再以關(guān)鍵點為連接點，將這些子路徑連接在一起，形成起始點到目標(biāo)點間的初始路徑。第三步：對第二步中得到的初始路徑進行壓縮，刪除其中的冗余路徑段，得到最終路徑(見圖1(c))。

2.1 獲取關(guān)鍵點

關(guān)鍵點是指繞過某一障礙物的最短路徑上，與該障礙物的距離小于某一設(shè)定閾值的點。假設(shè)障礙物Obsi的外邊界為boundryi，從起始點到目標(biāo)點的路徑中，一條繞過Obsi的最短路徑為pathi，則稱集合{pointi|pointi∈Xfree∧pointi∈pathi∧distance(pointi,boundryi<ε}中的點為關(guān)鍵點。這些關(guān)鍵點是距離障礙物較近的可通行節(jié)點，所以經(jīng)過關(guān)鍵點的路徑是繞過某障礙物的近似最優(yōu)路線。關(guān)鍵點的獲取要經(jīng)過兩步完成，第一步是獲取所有潛在關(guān)鍵點，第二步是從潛在關(guān)鍵點中選出相對較好的點作為最終的關(guān)鍵點。

2.1.1 獲取潛在關(guān)鍵點

為了便于討論，該文以實際障礙物的正外接矩形來代表障礙物本身，文中的障礙物均指實際障礙物的正外接矩形。在尋找可能的關(guān)鍵點時，首先找到所有與起始點和目標(biāo)點的連線相交的障礙物。然后對這些障礙物的四個頂點分別向遠離中心點的方向外推，如對左上點進行左推和上推，對右下點進行右推和下推。通過這種外推得到的點就是可能的關(guān)鍵點。左上點的外推過程如下：首先完成對左上點A的左推，即以步長step向左尋找障礙物的左側(cè)邊界，若在地圖邊界內(nèi)找到了該障礙物的左側(cè)邊界則把當(dāng)前點拉回到障礙物內(nèi)，并以步長step向上尋找障礙物的上側(cè)邊界；若在地圖邊界內(nèi)找到了該障礙物的上側(cè)邊界，則對該點進行一步左推，并返回左推后的點，即A的外推點。具體過程如圖2所示。障礙物的其他頂點的外推過程與此類似，不再贅述。需要說明的是，為了便于后續(xù)操作，起始點和目標(biāo)點也分別被視為可能的關(guān)鍵點。

圖2 左上點的外推過程

2.1.2 關(guān)鍵點選擇

在一次路徑規(guī)劃中，可能的關(guān)鍵點數(shù)量較多，其中有一些點能夠在路徑規(guī)劃過程中起到正向的啟發(fā)作用，也就是說經(jīng)過這些關(guān)鍵點的路徑代價會比較小，另一些則不能。關(guān)鍵點選擇的目的就是從可能的關(guān)鍵點中，選出那些具有正向啟發(fā)作用的點作為最終的關(guān)鍵點。

在進行關(guān)鍵點選擇時，從起始點開始，在可能的關(guān)鍵點組成的集合SP中查找距離最近的無碰撞連接點p，若找到則將p加入到關(guān)鍵點集合SKP中，并將p點從SP中刪除，然后從p開始在SP中繼續(xù)查找距離最近的無碰撞連接點。重復(fù)上述過程，直至找不到無碰撞連接點或找到的點是目標(biāo)點。如果目標(biāo)點不在SKP中，則從目標(biāo)點開始，在SP中查找距離最近的無碰撞連接點p，若找到則將p加入到關(guān)鍵點集合SKP中，并將p點從SP中刪除，然后從p開始在SP中查找距離最近的無碰撞連接點。重復(fù)上述過程，直至找不到無碰撞連接點或找到的點已經(jīng)存在于SKP中。簡單來說，關(guān)鍵點選擇的過程，就是分別從起始點和目標(biāo)點開始，在SP中查找最近的可以無碰撞連接的點的過程。

2.2 實施路徑規(guī)劃

通過關(guān)鍵點的選擇，得到了一個起始點到目標(biāo)點之間的關(guān)鍵點數(shù)組。數(shù)組中相鄰兩個關(guān)鍵點之間的路徑稱為子路徑。接下來根據(jù)需要選用某種路徑規(guī)劃算法完成子路徑的規(guī)劃。最后以關(guān)鍵點為連接點，將這些子路徑連接在一起，形成初步路徑。由于難以找到適合于任何情況的最優(yōu)關(guān)鍵點選擇策略，所以選出的關(guān)鍵點并不都是對路徑規(guī)劃具有正向的啟發(fā)作用，如圖1(b)中的關(guān)鍵點B；同時，因為所選擇的路徑規(guī)劃算法不一定是最優(yōu)算法，甚至可能不是漸近最優(yōu)的算法，所以初步規(guī)劃出來的路徑通常不是最短路徑。為了解決這一問題，需要對初始路徑做進一步優(yōu)化，也就是路徑壓縮。

路徑規(guī)劃的具體過程如下：假設(shè)經(jīng)過關(guān)鍵點的生成與選擇后，得到了關(guān)鍵點數(shù)組arrayKeypoint。對于arrayKeypoint中的每一個點arrayKeypoint[i]，使用選定的路徑規(guī)劃算法，規(guī)劃以arrayKeypoint[i]和arrayKeypoint[i+1]為起始點和目標(biāo)點的路徑，得到子路徑subPath_i，并將subPath_i加入到初始路徑Path中。最后對Path進行壓縮，以提高路徑的質(zhì)量。路徑壓縮是為了減少路徑長度，提高路徑質(zhì)量，其實質(zhì)就是刪除一條路徑中的冗余路徑點。冗余路徑點的定義如下：

冗余路徑點：對于一條路徑Path上的一段子路徑sPath，令sPath的起止點分別是PointStart和PointEnd，如果由PointStart和PointEnd兩個點構(gòu)成的子路徑也是無碰撞的可行路徑，則稱sPath上除起止點之外的其他路徑點為冗余路徑點。

圖3 冗余路徑點

如圖3所示，因為由PointStart和PointEnd兩點構(gòu)成的路徑是無碰撞的可行路徑，所以以PointStart和PointEnd為起止點的子路徑sPath上的其他點均為冗余路徑點，子路徑sPath就可以被壓縮成compressed path，由此來降低路徑長度。路徑壓縮從路徑的目標(biāo)點開始，逐一查找距離當(dāng)前點curr_point最遠的無障礙連接點farthest_collision_free_point，并刪除curr_point與farthest_collision_free_point之間的冗余路徑點。路徑壓縮算法首先將目標(biāo)點賦值給當(dāng)前點curr_point，并將curr_point加入到壓縮后的路徑中；然后查找路徑中距離當(dāng)前點最遠的無碰撞點farthest_collision_free_point，并將該點加入到壓縮后的路徑中；最后以farthest_collision_free_point為當(dāng)前點，繼續(xù)尋找。循環(huán)執(zhí)行上述過程，直到找到的最遠無碰撞點為起始點。

2.3 算法的總流程

關(guān)鍵點路徑規(guī)劃算法首先獲取可能的關(guān)鍵點，潛在關(guān)鍵點的獲取是通過對起始點與目標(biāo)點連線上的障礙物的頂點進行外推實現(xiàn)的。然后進行關(guān)鍵點選擇，依據(jù)步長最小且可連通為原則，在可能的關(guān)鍵點中選出路徑規(guī)劃所需的關(guān)鍵點。接下來進行初步的路徑規(guī)劃，選用某種路徑規(guī)劃算法在兩個相鄰的關(guān)鍵點之間規(guī)劃子路徑，并將這些子路徑合并成總路徑。最后進行路徑壓縮，采用刪除總路徑中的冗余路徑點的方式，對原始路徑進行優(yōu)化，得到最終路徑。

2.4 算法分析

關(guān)鍵點路徑規(guī)劃算法的優(yōu)點是效率較高，規(guī)劃的路徑質(zhì)量也較好。與其他路徑規(guī)劃算法相比，關(guān)鍵點路徑規(guī)劃算法多了兩個步驟，一個是關(guān)鍵點的獲取，另一個是路徑壓縮。通過關(guān)鍵點獲取，算法找到了繞過從起始點到目標(biāo)點之間的障礙物的關(guān)鍵點，并使用這些關(guān)鍵點來引導(dǎo)路徑規(guī)劃算法完成規(guī)劃。這些關(guān)鍵點將一條未知路徑分割成多條子路徑。通過分割將規(guī)劃空間切分成更小的子空間，從而減小了問題規(guī)模，提高了規(guī)劃效率。另一方面，由于大多數(shù)關(guān)鍵點之間是可以無障礙連通的，所以這些子路徑的規(guī)劃就變得非常簡單，只要將兩個關(guān)鍵點連接起來就行了，路徑規(guī)劃的效率得到了較大的提高。同時，關(guān)鍵點的引入將隨機搜索轉(zhuǎn)換為啟發(fā)式搜索，能夠使算法朝著目標(biāo)點的方向進行搜索，所以算法規(guī)劃出了更短的路徑。盡管關(guān)鍵點的引入極大地提高了算法的效率和路徑的質(zhì)量，然而，由于地圖環(huán)境復(fù)雜多樣，起始點和目標(biāo)點的位置又不確定，導(dǎo)致難以找到一個最優(yōu)的關(guān)鍵點選擇策略，障礙物的一個頂點在某種情況下是最優(yōu)路徑經(jīng)過的點，在另外一種情況下又不是了。所以照固定的關(guān)鍵點選擇策略選出的關(guān)鍵點中，會出現(xiàn)少量壞點，這些壞點降低了路徑的質(zhì)量。另外用于完成關(guān)鍵點間路徑規(guī)劃的算法不一定是最優(yōu)算法，規(guī)劃出來的子路徑也可能會出現(xiàn)較多的冗余路徑點。對此，路徑壓縮很好地解決了這兩個問題。路徑壓縮能夠以較高的效率找出路徑中的冗余部分，并將其刪除，從而提高最終路徑的質(zhì)量。所以通過關(guān)鍵點和路徑壓縮兩個操作，可以提高路徑規(guī)劃的效率和路徑的質(zhì)量，特別是在狹窄通道環(huán)境和“Z”字形通道環(huán)境下。

關(guān)鍵點路徑規(guī)劃算法的缺點是需要進行頻繁的碰撞檢測。在進行關(guān)鍵點獲取和路徑壓縮時，都需要多次進行碰撞檢測。當(dāng)?shù)貓D上障礙物數(shù)量過多時，會在一定程度上影響規(guī)劃效率。

3 實 驗

為了驗證關(guān)鍵點路徑規(guī)劃算法的性能，進行了多組對比實驗。實驗中選用RRT-connect完成關(guān)鍵點之間的路徑規(guī)劃。

3.1 與RRT-connect算法的對比實驗

3.1.1 實驗過程

實驗?zāi)M了四種場景(scenario)，如圖4所示。分別是離散障礙物場景、窄通道場景、Z字形場景和迷宮場景。實驗中選用規(guī)劃效率和規(guī)劃出來的路徑質(zhì)量作為評價算法優(yōu)劣的標(biāo)準(zhǔn)。每個場景選擇五組典型的起始點和目標(biāo)點進行路徑規(guī)劃，圖4中的S[i]表示第i組的起始點，G[i]表示第i組的目標(biāo)點。每組運行50次，記錄每次運行的時間和路徑長度。通過比較平均運行時間和平均路徑長度來對比算法的規(guī)劃效率和規(guī)劃出來的路徑質(zhì)量。實驗中RRT-connect算法的迭代次數(shù)設(shè)置為50 000。

圖4 四種場景

3.1.2 實驗結(jié)果

離散障礙物場景實驗的結(jié)果如圖5所示。圖5(a)是路徑規(guī)劃所用時間的對比圖，其縱坐標(biāo)是規(guī)劃路徑所用的平均時長，單位為毫秒；橫坐標(biāo)是路徑起止點的組序，如橫坐標(biāo)的“1”就表示規(guī)劃的是以圖4(a)中的S[1]為起始點、G[1]為目標(biāo)點的路徑；圖例中的KPP代表關(guān)鍵點路徑規(guī)劃算法的運行結(jié)果，RRT-C是RRT-connect算法的運行結(jié)果。圖5(b)是規(guī)劃出來的路徑長度對比圖，其縱坐標(biāo)表示算法規(guī)劃出來的平均路徑長度，橫坐標(biāo)及圖例所表示的意義與圖5(a)相同。如無特殊說明，后續(xù)實驗結(jié)果圖中各元素的意義均與圖5相同，不再贅述。

圖5 離散障礙物場景實驗結(jié)果

由圖5不難看出，在對圖4(a)中的5組路徑規(guī)劃中，KPP算法在規(guī)劃效率和路徑質(zhì)量方面都比RRT-connect算法好很多。KPP算法的平均耗時僅占RRT-connect算法的1.66%，KPP算法規(guī)劃的平均路徑長度僅占RRT-connect算法的13.22%。

窄通道場景的實驗結(jié)果如圖6所示。在窄通道場景下，KPP算法在效率方面的提高更加明顯，KPP算法的平均耗時僅占RRT-connect算法的0.16%。KPP算法規(guī)劃的平均路徑長度占RRT-connect算法的21.67%。這一比例有所升高，其原因是窄通道限定了路徑的大致方向。不管哪種算法規(guī)劃的路徑，都要從窄通道中通過，而該通道的長度是固定的，所以在通道內(nèi)部，這些路徑的長度差就被限定在一個較小的范圍內(nèi)了。

圖6 窄通道場景實驗結(jié)果

Z字形場景的實驗結(jié)果如圖7所示。在Z字形場景實驗中，圖4(c)中的5組起止點中，RRT-connect算法僅成功規(guī)劃出了兩組，分別是起止點相距較近的S[1]到G[1]的路徑和S[2]到G[2]的路徑。這兩組路徑規(guī)劃過程中，KPP算法的平均耗時僅占RRT-connect算法的0.09%；KPP算法規(guī)劃的平均路徑長度占RRT-connect算法的23.18%。與窄通道相比，Z字形通路更多地限制了路徑的走向，所以Z字形場景中，KPP算法規(guī)劃的平均路徑長度與RRT-connect算法規(guī)劃的平均路徑長度進一步提高。

迷宮場景的實驗結(jié)果如圖8所示。KPP算法的平均耗時僅占RRT-connect算法的0.08%；KPP算法規(guī)劃的平均路徑長度占RRT-connect算法的12.32%。

由實驗結(jié)果可以看出，在四種不同的場景中，KPP算法的規(guī)劃效率和規(guī)劃出來的路徑長度均比RRT-connect算法好很多。特別是在窄通道場景和Z字形場景中，當(dāng)以高效著稱的RRT-connect算法都難以成功規(guī)劃處路線時，KPP仍然可以在較短時間內(nèi)規(guī)劃出一條質(zhì)量較高的路徑。

圖7 Z字形場景實驗結(jié)果

圖8 迷宮場景實驗結(jié)果

3.2 與BIT*算法的對比實驗

為了進一步檢驗KPP算法的性能，與BIT*算法做了對比實驗，實驗選用了3組起止點。每組起止點完成50次規(guī)劃，通過對比平均規(guī)劃用時和平均路徑長度來評價算法性能。

實驗結(jié)果如下：進行第一組路徑規(guī)劃時，BIT*算法的平均耗時為27 343.75毫秒，平均路徑長度為26.976 321；KPP算法的平均耗時為0.27毫秒，平均路徑長度為26.131 984。進行第二組路徑規(guī)劃時，BIT*算法的平均耗時為25 312.5毫秒，平均路徑長度為23.484 871；KPP算法的平均耗時為0.21毫秒，平均路徑長度為23.332 335。進行第三組路徑規(guī)劃時，BIT*算法的平均耗時為29 484.375毫秒，平均路徑長度為33.495 748；KPP算法的平均耗時為15.625毫秒，平均路徑長度為34.640 041。三組路徑規(guī)劃中，KPP算法平均耗時明顯要小于BIT*算法；在路徑長度方面，除第三組外，KPP規(guī)劃的路徑長度均略小于BIT*算法規(guī)劃的路徑長度。在第三組路徑規(guī)劃時，由于關(guān)鍵點選擇時，以最小步長為原則，選取距離最近的節(jié)點，所選節(jié)點并不是最好的，從而導(dǎo)致KPP算法所規(guī)劃的路徑略長。

4 結(jié)束語

KPP算法通過引入關(guān)鍵點，將較復(fù)雜的長路徑規(guī)劃轉(zhuǎn)化為規(guī)模較小的子路徑規(guī)劃；同時將隨機搜索轉(zhuǎn)化為啟發(fā)式搜索，所以其規(guī)劃效率較高，特別是在窄通道、Z字形或障礙物較密集的場景中，當(dāng)其他算法無法成功規(guī)劃路徑時，KPP算法仍然可以較快地完成規(guī)劃。同時，路徑壓縮的操作也減少了KPP算法規(guī)劃出來的路徑長度。盡管KPP算法是用于解決2D問題路徑規(guī)劃問題的，可以很容易地將其擴展到立體空間中，求解3D問題路徑規(guī)劃的問題。

由于選出的關(guān)鍵點不一定是最合理的，部分關(guān)鍵點可能會給規(guī)劃帶來錯誤的引導(dǎo)，進而導(dǎo)致KPP規(guī)劃出來的路徑質(zhì)量降低，所以KPP算法不是最優(yōu)的。