Hybird A*算法保證生成的路徑是車輛可實(shí)際行駛的，但它仍然包含很多不必要的車輛轉(zhuǎn)向操作，我們可以對其進(jìn)行進(jìn)一步的平滑和優(yōu)化。

Objective Function

對于Hybird A*生成的車輛軌跡序列：

,論文【1】中提出如下的目標(biāo)優(yōu)化函數(shù)(Objective Function)：

該優(yōu)化函數(shù)是Voronoi Term、Obstacle Term、Curvature Term和Smoothness Term四個部分的加權(quán)平均：第一個部分引導(dǎo)車輛盡可能的避開障礙物區(qū)域；第二個部分懲罰車輛與障礙物的碰撞行為；第三部分約束規(guī)劃的每個點(diǎn)的最大曲率，并提供車輛非完整約束的保證；第四個部分是軌跡的平滑性約束。

、、、分別是這四個部分的權(quán)重因子。

1、Voronoi Term

Voronoi Term中引入了Voronoi Field的概念，Voronoi Field是機(jī)器人Motion Planning領(lǐng)域兩種經(jīng)典算法Voronoi Diagram和Potential Field的結(jié)合。

半杯茶的小酒杯：自動駕駛路徑規(guī)劃-Voronoi Planner?zhuanlan.zhihu.com半杯茶的小酒杯：路徑規(guī)劃-人工勢場法(Artificial Potential Field)?zhuanlan.zhihu.com

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

其中

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

上圖圖左一為Voronoi Field的實(shí)際效果，上圖右一是標(biāo)準(zhǔn)Potential Field的實(shí)際效果。可以看到，Voronoi Field對狹窄通道的效果要明顯優(yōu)于Potential Field。

2、Obstacle Term

Obstacle Term中

是路徑點(diǎn)坐標(biāo)位置，是附近障礙物的位置，是決定Obstacle Term是否影響路徑Cost的閾值。當(dāng)路徑點(diǎn)距離障礙物的距離小于時，Obstacle Term才會對軌跡的Cost進(jìn)行懲罰。距離障礙物越近，的值越小，Obstacle Term的值就越大，整個軌跡的Cost也就越大。這樣就達(dá)到了使得平滑后的路徑遠(yuǎn)離障礙物的效果。

這里

一般使用二次函數(shù)。即:

3、Curvature Term

對于一系列的點(diǎn)

，，即為規(guī)劃路徑的方向向量；為路徑點(diǎn)的方向角變化。

為處的曲率。與Obstacle Term類似，Curvature Term也設(shè)置了一個最大允許的路徑曲率，當(dāng)曲率大于時，Curvature Term才會對路徑的Cost施加懲罰。

4、Smoothness Term

平滑項(xiàng)利用當(dāng)前點(diǎn)前后兩個方向向量的差值來衡量，方向向量既可以衡量方向的改變，也可以體現(xiàn)軌跡點(diǎn)的分布變換。

5、梯度下降

確定Objective Function函數(shù)之后，就可以利用Conjugate Gradient(CG，共軛梯度法)或者Gradient Descent求解最優(yōu)路徑。

代碼參見:

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

平滑后的路徑如下：

Non-Parametric Interpolation

對路徑進(jìn)行非線性優(yōu)化后，我們得到一條比Hybird A*算法路線更加平滑的路徑，但是這條路徑仍然由一段段的折線組成。在論文【1】中提到在它們的實(shí)現(xiàn)中組成路徑的折線大約在0.5m-1m，這些折線仍然會導(dǎo)致車輛會出現(xiàn)非常生硬的轉(zhuǎn)向，所以需要使用插值算法進(jìn)一步平滑路徑。

參數(shù)化的插值算法對噪聲非常敏感，比如當(dāng)路徑中兩個頂點(diǎn)非常接近時，三次樣條曲線(Cubic Spline)算法的輸出就會產(chǎn)生非常大的震蕩。

【1】中提出通過固定原始路徑頂點(diǎn)，然后在固定頂點(diǎn)之間插入新的頂點(diǎn)，最后使用Conjugate Gradient(CG，共軛梯度法)最小化曲率的非參數(shù)插值(Non-Parametric Interpolation)方法對曲線進(jìn)一步平滑，平滑效果如下：

參考資料

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.

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

推薦閱讀：

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。