《Practical Search Techniques in Path Planning for Autonomous Driving》
路徑規劃演算法，可以在未知環境中行進的自動駕駛車輛生成平滑的軌跡。

混合狀態A搜尋，Hybrid-State A Search

我們將A演算法應用於車輛的3D動力學狀態空間。搜尋空間 ( x , y , θ ) (x,y,\theta) (x,y,θ)是離散的，但每個離散狀態不侷限於空間網格的中心。

三種圖搜尋演算法的比較：A演算法訪問的狀態在每個網格的中心位置（左側）；Field D和Theta演算法在每個網格的角點處計算成本，並允許網格之間的任意線性路徑（中間）；混合A計算網格內連續狀態的成本，並將該成本作為單元格的成本。

因為一個單元格合併了多個連續狀態，混合A演算法無法保證找到最優路徑。但是混合A演算法能夠保證結果路徑是可行使的。且通常，混合A演算法得到的結果路徑通常在最優路徑附近，我們可以通過優化演算法獲得全域性最優解。
演算法允許倒車，但懲罰倒車以及切換運動方向。

啟發函式

演算法參考了兩種啟發函式：non-holonomicwithout-obstacles，即忽略障礙物，但考慮車輛的non-holonomic約束。我們假設目標狀態為 ( x g , y g , θ g ) = ( 0 , 0 , 0 ) (x_g,y_g,\theta_g)=(0,0,0) (xg​,yg​,θg​)=(0,0,0)，並計算每個點 ( x , y , θ ) (x,y,\theta)

(x,y,θ)到達目標點所需的最短路徑作為啟發函式。
演算法使用max(2D歐式距離，non-holonomicwithout-obstacles成本)作為啟發函式。
而歐氏距離啟發函式，由於考慮了障礙物，它可以發現2D中所有的U型障礙物和死角，然後產生更昂貴的代價函式，引導搜尋遠離這些區域。

A*演算法使用歐幾里得距離作為啟發函式：a，擴充套件了21515個結點，c，擴充套件了68730個結點（複雜環境導致了大量的無用搜索）
non-holonomic-without-obstacles作為啟發函式：b，只擴充套件了1465個結點，d，只擴充套件了10588個結點。

擴充套件結點

上述正向搜尋使用控制動作轉向的離散空間。

使用控制動作的離散空間進行前向搜尋，且使用基於Reed-Shepp曲線。通過使用特定的控制動作在短時間內（對應於網格的解析度）模擬汽車的運動模型來擴充套件樹中的節點。

使用Voronoi場的路徑成本函式

使用Voronoi場來權衡路徑長度與距離障礙物的距離。


a)voronoi場，b)vorinoi圖，c)其他勢場（標準電勢場）。可以看出，其他勢場中，狹窄隘口有很高的勢，不利於通行。

區域性優化和平滑

混合A演算法產生的路徑並非最優的，進行兩個階段進行優化：1. 對軌跡上的頂點進行非線性優化，以改善解的長度和平滑度。2. 執行非引數插值。

對軌跡上每個結點進行位姿優化。

紅色線是混合A產生的軌跡，藍線是對結點優化後的軌跡。
使用上述優化方法（CG平滑），我們得到的路徑比混合A*得到的解更平滑，但它仍然是分段線性的。
我們需要對結點進行曲線插值，因此，我們使用非引數插值。下圖顯示了對路徑結點進行插值的結果。