Hybird A*算法保證生成的路徑是車輛可實際行駛的，但它仍然包含很多不必要的車輛轉向操作，我們可以對其進行進一步的平滑和優化。
Objective Function
對於Hybird A*生成的車輛軌跡序列：
,論文【1】中提出如下的目標優化函數(Objective Function)：
該優化函數是Voronoi Term、Obstacle Term、Curvature Term和Smoothness Term四個部分的加權平均：第一個部分引導車輛儘可能的避開障礙物區域；第二個部分懲罰車輛與障礙物的碰撞行為；第三部分約束規劃的每個點的最大曲率，並提供車輛非完整約束的保證；第四個部分是軌跡的平滑性約束。
、
、
、
分別是這四個部分的權重因子。
1、Voronoi Term
Voronoi Term中引入了Voronoi Field的概念，Voronoi Field是機器人Motion Planning領域兩種經典算法Voronoi Diagram和Potential Field的結合。
此處採用Voronoi Field的定義如下：
其中
和
分別是路徑點(x,y)到最近障礙物的距離和到最近Voronoi Diagram的邊的距離。越靠近障礙物，
的值越大，越接近1；越靠近Voronoi Edge，
的值越接近0。
上圖圖左一為Voronoi Field的實際效果，上圖右一是標準Potential Field的實際效果。可以看到，Voronoi Field對狹窄通道的效果要明顯優於Potential Field。
實際停車場的Voronoi Field和Junior的規劃路徑2、Obstacle Term
Obstacle Term中
是路徑點坐標位置，
是附近障礙物的位置，
是決定Obstacle Term是否影響路徑Cost的閾值。當路徑點距離障礙物的距離小於
時，Obstacle Term才會對軌跡的Cost進行懲罰。距離障礙物越近，
的值越小，Obstacle Term的值就越大，整個軌跡的Cost也就越大。這樣就達到了使得平滑後的路徑遠離障礙物的效果。
這裡
一般使用二次函數。即:
3、Curvature Term
對於一系列的點
，
，即為規劃路徑的方向向量；
為路徑點的方向角變化。
為
處的曲率。與Obstacle Term類似，Curvature Term也設置了一個最大允許的路徑曲率
，當曲率大於
時，Curvature Term才會對路徑的Cost施加懲罰。
4、Smoothness Term
平滑項利用當前點前後兩個方向向量的差值來衡量，方向向量既可以衡量方向的改變，也可以體現軌跡點的分布變換。
5、非線性優化
確定Objective Function函數之後，就可以利用Conjugate Gradient(CG，共軛梯度法)或者Gradient Descent求解最優路徑。
Path Planning inUnstructured Environments【2】中採用了如下的梯度下降算法：
代碼參見:
https://github.com/teddyluo/hybrid-a-star-annotation/blob/master/src/smoother.cpp
平滑後的路徑如下：
Hybrid-A*(紅色路徑) VS CG path(藍色路徑)
Non-Parametric Interpolation
對路徑進行非線性優化後，我們得到一條比Hybird A*算法路線更加平滑的路徑，但是這條路徑仍然由一段段的折線組成。在論文【1】中提到在它們的實現中組成路徑的折線大約在0.5m-1m，這些折線仍然會導致車輛會出現非常生硬的轉向，所以需要使用插值算法進一步平滑路徑。
參數化的插值算法對噪聲非常敏感，比如當路徑中兩個頂點非常接近時，三次樣條曲線(Cubic Spline)算法的輸出就會產生非常大的震蕩。
【1】中提出通過固定原始路徑頂點，然後在固定頂點之間插入新的頂點，最後使用Conjugate Gradient(CG，共軛梯度法)最小化曲率的非參數插值(Non-Parametric Interpolation)方法對曲線進一步平滑，平滑效果如下：
參考資料
1、Practical Search Techniques in Path Planning for Autonomous Driving。Dmitri Dolgov，Sebastian Thrun，Michael Montemerlo，James Diebel.
2、Path Planning in Unstructured Environments, A Real-time Hybrid A* Implementation for Fast and Deterministic Path Generation for the KTH Research Concept Vehicle.