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 Planner?zhuanlan.zhihu.com半杯茶的小酒杯：路徑規劃-人工勢場法(Artificial Potential Field)?zhuanlan.zhihu.com

此處采用Voronoi Field的定義如下：

其中

和分別是路徑點(x,y)到最近障礙物的距離和到最近Voronoi Diagram的邊的距離。越靠近障礙物，的值越大，越接近1；越靠近Voronoi Edge，的值越接近0。

上圖圖左一為Voronoi Field的實際效果，上圖右一是標準Potential Field的實際效果。可以看到，Voronoi Field對狹窄通道的效果要明顯優于Potential Field。

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求解最優路徑。

代碼參見:

https://github.com/teddyluo/hybrid-a-star-annotation/blob/master/src/smoother.cpp

平滑后的路徑如下：

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.

注：本文首發于微信公眾號【半杯茶的小酒杯】，轉載請注明出處，謝謝！

推薦閱讀：

半杯茶的小酒杯：自動駕駛運動規劃-Hybird A*算法?zhuanlan.zhihu.com半杯茶的小酒杯：機器人動態規劃(Dynamic Programming)入門?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛路徑規劃-Voronoi Planner?zhuanlan.zhihu.com半杯茶的小酒杯：基于采樣的運動規劃算法-RRT(Rapidly-exploring Random Trees)?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛路徑規劃-Graph Based的BFS最短路徑規劃?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛運動規劃-Dubins曲線?zhuanlan.zhihu.com半杯茶的小酒杯：未知環境下的Lidar概率占位柵格圖(Occupancy Grid Map) Python代碼實現?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛Mapping-占位柵格圖(Occupancy Grid Map)?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛中的車輛運動學模型?zhuanlan.zhihu.com半杯茶的小酒杯：自動駕駛定位算法(十五)基于多傳感器融合的狀態估計(muti-Sensors Fusion)?zhuanlan.zhihu.com自動駕駛路徑規劃器-Lattice Planner詳解?www.banbeichadexiaojiubei.com

總結

以上是生活随笔為你收集整理的python 非线性规划_自动驾驶运动规划-Hybird A*算法(续)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。