一種復雜場景下的路徑規劃問題解決方法

郭書杰，田 華，王 偉

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

0 引 言

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

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

1 路徑規劃問題

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

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

2 關鍵點路徑規劃算法

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

圖1 KPP處理過程

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

2.1 獲取關鍵點

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

2.1.1 獲取潛在關鍵點

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

圖2 左上點的外推過程

2.1.2 關鍵點選擇

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

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

2.2 實施路徑規劃

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

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

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

圖3 冗余路徑點

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

2.3 算法的總流程

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

2.4 算法分析

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

關鍵點路徑規劃算法的缺點是需要進行頻繁的碰撞檢測。在進行關鍵點獲取和路徑壓縮時，都需要多次進行碰撞檢測。當地圖上障礙物數量過多時，會在一定程度上影響規劃效率。

3 實 驗

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

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

3.1.1 實驗過程

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

圖4 四種場景

3.1.2 實驗結果

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

圖5 離散障礙物場景實驗結果

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

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

圖6 窄通道場景實驗結果

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

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

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

圖7 Z字形場景實驗結果

圖8 迷宮場景實驗結果

3.2 與BIT*算法的對比實驗

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

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

4 結束語

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

由于選出的關鍵點不一定是最合理的，部分關鍵點可能會給規劃帶來錯誤的引導，進而導致KPP規劃出來的路徑質量降低，所以KPP算法不是最優的。