路徑規劃概述：基於採樣、搜尋、優化全搞定！

1 決策控制與運動規劃概述

目前決策控制方法可以分為三類：sequential planning、behavior-aware planning、和end-to-end planning 。

• sequential planning：最傳統的方法，感知、決策與控制三個部分層次較為清晰；
• behavior-aware planning：相較於第一個亮點在於引進人機共駕、車路協同以及車輛對外部動態環境的風險預估；
• end-to-end planning：DL、DRL技術，借助大量的資料訓練，獲得從影像等感知資訊到方向盤轉角等車輛控制輸入的關係，屬於時下最熱門的方法之一。

本文將對sequential planning進行介紹，按照整個決策控制順序講述自動駕駛汽車的感知控制過程，最後會簡要總結一下前文所提到的待解決的問題。

Control architecture for automated vehicles

2 路徑規劃概述

sequential planning 的過程簡要概括為路徑規劃->決策過程->車輛控制，本文所述的路徑規劃屬於第一步與第三步。

https://www.php.cn/link/aa7d66ed4b1c618962d406535c4d282a

#在無人車的運動軌跡產生問題上，有直接軌跡生成法與路徑-速度分解法兩種，比較第一種，路徑-速度難度較小，因此較常用。

2.1 路徑規劃的型別

路徑規劃可分為四大類：以PRM、RRT為代表的基於取樣的演算法、以為A* 、D* 代表的基於搜尋的演算法、以β樣條曲線為代表的基於插值擬合的軌跡生成演算法，和以MPC為代表的用於局部路徑規劃的最優控制演算法。本小節將依照上述順序逐一講解：

A Review of Motion Planning Techniquesfor Automated Vehicles

2.2 路徑規劃演算法優缺點

3 路徑規劃方法

#3.1 以取樣為基礎的演算法

3.1.1 基本演算法PRM與RRT

#(1) PRM

##PRM演算法（Probabilistic Road Map） 。 PRM主要包含了兩個步驟，一是學習階段，二是查詢階段。

第一步，學習階段：對狀態空間內的安全區域均勻隨機採樣ｎ個點，並刪除採樣落在障礙物上的點，接著對相鄰的點進行連接並做碰撞檢測，剔除不是collision-free的連線，最後得到一個連通圖。

第二步，查詢階段：對於給定的一對初始和目標狀態，利用上一步建構好的取樣節點及連續，運用圖搜尋的方法（Dijkstra或A*）來找出一條可行路徑。

完成PRM建置後，就可以用來解決不同初始、目標狀態的運動規劃問題，但是這個特性對於無人車運動規劃而言是不必要的。另外PRM要求對狀態之間進行精確連接，這對於存在複雜微分約束的運動規劃問題是十分困難的。

(2) RRT

RRT (Rapidly-exploring Random Tree)演算法。 RRT其實代表了一系列基於

隨機生長樹思想的演算法，是目前機器人領域運用最廣泛、優化變種最多的一類演算法.

# #① 樹初始化：初始化樹的結點集和邊集，結點集只包含初始狀態，邊集為空；

② 樹的生長：對狀態空間隨機取樣，當取樣點落在狀態空間安全區域時，選擇目前樹中離取樣點最近的結點，將其向取樣點擴展連接；若產生的軌跡不與障礙物發生碰撞，則將該軌跡加入樹的邊集，該軌跡的終點加入到樹的結點集

③ 重複步驟②，直至擴展到目標狀態集中，相比PRM的無向圖而言，RRT建構的是初始狀態作為根結點、目標狀態作為葉結點的樹狀結構，對於不同的初始和目標狀態，需要建構不同的樹。

RRT不要求狀態之間的精確連接，更適合解決像無人車運動規劃這樣的運動動力學問題。

3.1.2 取樣法的問題與解

求解效率與是否最適解。 PRM與RRT擁有機率完備性的原因在於其幾乎會遍歷構型空間中所有位置。

(1) 求解效率

在提升求解效率方面，最佳化RRT的核心思想在於引導樹向空曠區域，即盡量遠離障礙物，避免障礙物處的節點的重複檢查，以此提升效率。主要解決方法：

① 均勻取樣

標準RRT演算法對狀態空間均勻隨機取樣，目前樹中結點獲得擴展的機率與其Voronoi區域面積成正比，所以樹會向著狀態空間的空曠區域生長，均勻充滿狀態空間的自由區域。

RRT-connect演算法同時建構兩棵分別起始於初始狀態和目標狀態的樹，當兩棵樹生長到一起時則找到可行解。 Go-biaing在隨機取樣序列中以一定比例插入目標狀態，引導樹向目標狀態擴展，加快求解速度，提升求解品質。

heuristic RRT使用啟發式函數增加擴展代價低的結點被採樣的機率，計算樹中每個結點的代價，但是在復雜環境中，代價函數的定義困難的，為解決這一問題，f-biased採樣方法先將狀態空間離散化為網格，再使用Dijkstra演算法計算每個網格上的代價，這個網格所在區域的點的代價值都等於該值，以此建構啟發式函數。

② 最佳化距離度量

距離用來度量兩個構形之間路徑的代價，輔助產生啟發式代價函數，引導樹的走向。但在考慮障礙物的情況下，距離計算的難度高，運動規劃中距離的定義採用類似歐氏距離的定義。 RG-RRT （rechability guided RRT）可以消除不準確的距離對RRT探索能力的影響，它需要計算樹中結點的能達集，當採樣點到結點的距離大於到該結點能達集的距離時，該節點才有可能被選中進行擴展。

③ 降低碰撞檢查次數

碰撞檢查取樣法的效率瓶頸之一，通常的做法是對路徑等距離離散化，再對每個點的構形作碰撞檢查。 resolution complete RRT透過降低靠近障礙物的結點獲得擴展的機率，它對輸入空間離散化，對於某個結點輸入只使用一次；若某個輸入對應的軌跡與障礙物碰撞，則對該節點加上一個懲罰值，該懲罰值越高，該節點獲得擴展的機率越小。 dynamic domain RRT與adaptive dynamic domain RRT限制採樣區域在目前樹所在的局部空間，以防止靠近障礙物的結點反覆擴展失敗，提高演算法效率。

④ 提升即時性

Anytime RRT先快速建立一個RRT，得到一個可行解並記錄其代價，之後繼續取樣，但僅將有利於降低可行解代價的結點插入樹中，從而逐漸獲得較優的可行解。 Replanning將整個規劃任務分解為若干等時間的子任務序列，在執行目前任務的同時規劃下一個任務。

(2) 在解決最優性問題上主要有以下方法：

RGG演算法（random geometric graph）：根據隨機幾何圖理論對標準PRM和RRT 改良的具有漸近最優性質的PRM、RRG和RRT 演算法，在狀態空間中隨機取樣ｎ個點，並將距離小於r(n)的點連起來，就構成了RGG 。

RRT* 演算法：在RRG基礎上引入「重新連接」步驟，檢查新插入結點作為其臨近點的父結點是否會使其臨近點的代價降低，若降低，則去掉臨近點原來的父子關係，將目前插入點作為其父結點，這就是RRT*演算法。

LBT-RRT演算法：大量的結點連接和局部調整使得PRM和RRT的效率十分低。 LBT-RRT演算法將RRG和RRT* 演算法結合起來，在獲得漸進最優性的前提下，獲得更高的效率。

3.2 基於搜尋的演算法

基本思想是將狀態空間透過確定的方式離散成一個圖，然後利用各種啟發式搜尋演算法搜尋可行解甚至是最優解，該種類別演算法比較成熟。

基於搜尋的演算法的基礎是狀態格子，狀態格子是狀態空間離散化，由狀態結點和從該結點出發到達相鄰結點的運動基元組成，一個狀態結點可以透過其運動基元轉換到另一個狀態結點。狀態格子就將原來連續的狀態空間轉化為一個搜尋圖，運動規劃問題就變成了在圖中搜尋出一系列將初始狀態變換到目標狀態運動基元，建構起狀態格子後就可以使用圖搜索演算法來搜尋最優軌跡。

3.2.1 基礎演算法Dijkstra、A*的建構

Dijkstra演算法遍歷整個構型空間，找出每兩個格子之間的距離，最後選擇出發點到目標點的最短路徑，其廣度優先的性質導致效率很低，在該演算法的基礎上加入啟發式函數，即所搜尋結點到目標節點的距離，並以此為基礎再次進行搜尋可避免全域搜尋帶來的效率低下，這即為A*演算法，如下圖所示，紅色為搜尋區域。

圖6：A*與Dijkstra演算法效果比較圖

3.2.2 搜尋法的問題與建議

#與基於採樣的演算法相同，這種類別的演算法也需要做效率與最優性的最佳化。

在提升效率上面，A* 本身屬於靜態規劃的演算法，針對A* 演算法的延申有weighted A* 透過增加啟發式函數的權重進一步引導搜尋方向向這一目標節點進行，搜尋速度很快，但是容易陷入局部極小值，無法保證全域最優解。

對於運動的車輛來說，使用A* 的衍生演算法D（dynamic A）可大幅提升效率。同樣以動態規劃為基礎的還有LPA，該演算法可以處理狀態格子的運動基元的代價是時變的情況，當環境發生變化時可以透過對較少數目節點的重新搜尋規劃出新的最優路徑。在LPA 的基礎上開發出D*-Lite可以獲得與D*相同的結果，但是效率更高。

在進行最優化解的探尋時，ARA* 是在Weighted A* 基礎上發展出的具有Anytime性質的搜尋演算法，它透過多次呼叫Weighted A* 演算法且每次呼叫就縮小啟發式函數的權重，這樣演算法可以快速求出可行解，透過引入集合INCONS使得每次循環可以繼續使用上一次循環的信息，對路徑做出優化，逐漸逼近最優解。

在兼顧演算法效率與最優性的問題上，Sandin aine等提出了MHA* 演算法，引入多個啟發式函數，保證其中有一個啟發式函數在單獨使用時可以找到最優解，從而透過協調不同啟發式函數產生的路徑代價，可以兼顧演算法的效率與最適性。 DMHA在MHA的基礎上在線即時產生合適的啟發式函數，從而避免局部最小值問題。

3.3 基於插值擬合的演算法

#基於內插擬合的演算法可定義為：根據已知的一系列用於描述道路圖的點集，透過使用資料插值與曲線擬合的方式創造出智慧車將行駛的路徑，可提供較好的連續性、較高的可導性。具體的方法如下：

Dubins曲線和Reeds and Sheep(RS)曲線是連接構形空間中任兩點的最短路徑，分別對應無倒車和有倒車的情況。它們都是由最大曲率圓弧和直線組成的，在圓弧和直線連接處存在曲率不連續，實際車輛按照這樣曲線行駛時必須在曲率不連續處停車調整方向輪才能繼續行駛。

多項式插值曲線是最常用的方法，它可以透過滿足結點的要求來設定多項式係數，並且獲得較好的連續可導性，四階多項式常用於縱向約束控制，五階多項式常用於橫向約束控制，三階多項式也被用於超車軌跡。

樣條曲線具有封閉的表達式，容易保證曲率連續性。 β樣條曲線可以實現曲率連續性，三次Bezier曲線可以確保曲率的連續性和有界性，且計算量相對較小。 η^3曲線[43]是一種七次樣條曲線，它有著很好的性質：曲率連續性和曲率導數的連續性，這對於高速行駛車輛是很有意義的。

3.4 基於最佳控制的演算法

將基於最佳控制的演算法歸在路徑規劃中，主要是因為其中的MPC可以進行局部的路徑規劃以進行避障，除此之外，MPC主要的作用是進行軌跡跟踪，其所考慮的問題除了必要的動力學、運動學約束以外，未來還應考慮舒適性、感知資訊的不確定性、車間通訊的不確定性，並且在局部軌跡規劃時還可以將駕駛員納入控制閉環。對於以上所提到的不確定性問題與將駕駛員納入控制閉環將在第四節討論。 關於MPC的學習，主要從最佳化理論與工程實務兩個面向著手。前者，推薦Dimitri P. Bertsekas的Convex Optimization Algorithms，James B. Rawlings的Model Predictive Control：Theory, Computation, and Design。中文領域劉浩洋老師的最優化書個人覺得相對清晰易懂。對於後者，首先龔建偉老師的那本無人駕駛MPC書強推了，老版書裡的demo有問題，不過都在新版裡解決了。

MPC所使用的預測模型有很多種：諸如卷積神經網路、模糊控制、狀態空間等等，其中用的最多的為狀態空間法。 MPC可簡要表述為：滿足必要的動力學、運動學等等約束的情況下，透過數值手段求解模型的最優解，該最優解即為狀態方程的控制量，如方向盤轉角等等，並將控制量作用在車模上以獲得所需的狀態量，如速度、加速度、座標等等。

透過上述描述可知，MPC的關鍵在於模型的建立與模型的求解，如何等效簡化模型的建立以及提升求解的效率是重中之重。在不同的控制輸入下車輛會走不同的軌跡，每個軌跡都與之對應一個目標函數值，無人駕駛車輛會透過求解演算法找出最小目標函數值對應的控制量，並將其作用在車上，如下圖：

為了降低建模難度，也有使用人工位能場模型進行建模，人工位能場的基本思想類似電場，在道路上的障礙物類似電場中與場源相異電荷極性的電荷。障礙物（動態、靜態）處的位能較高，無人車將向低位能位置前進。

4 開源專案

推薦一個開源專案CppRobotics：

• Path Planning
• Dijkstra
• A Star
• RRT
• Dynamic Window Approach
• Model Predictive Trajectory Generator
• Cubic Spline Planner
• State Lattice Planner
• Frenet Frame Trajectory

5 學習方法

入門新領域的學習脈絡是：工程、理論以及視野三駕馬車齊頭並進，以路徑規劃為例：

#5.1 工程

指的是了解各路徑規劃演算法內容，一邊從廣度上了解各演算法內容，一邊從深度上深入學習各演算法細節。關於路徑規劃領域的演算法，目前沒見全面的教程，但是龔建偉的NMPC運動規劃可以參考。

5.2 理論

指的是了解支撐這些演算法運算數學原理以及這些演算法產生的原因（數學觀點）。

• 建構目標函數與約束條件同時求極值來得到最優控制量（路徑），屬於最優化理論；
• 在求解最優控制量時大家常見的牛頓法、最速下降法等等這些數值求解方法，本質來自於數值求解代數等式方程，屬於數值分析;
• 求解過程中所見到的導數雅可比矩陣、判斷條件中的向量範數等等，本質就是把一維數值求解放到了高維，屬於矩陣理論。

5.3 視野

指的是了解路徑規劃在科研以及企業的主要應用，手段分別為科研文獻以及成果報告等等。

6 小結

本文介紹了目前路徑規劃的梗概，了解目前路徑規劃有那些方法。內容很繁雜，很難在沒有實際應用導向的情況下下短期全部學通，只能在需要的時候再專注學習。

#

以上是路徑規劃概述：基於採樣、搜尋、優化全搞定！的詳細內容。更多資訊請關注PHP中文網其他相關文章！