Planning Algorithm

一. 單一路段規劃

從道路中提取Vehicle可能會到達的目標點，采用Cost Function對不同的路徑進行評估，并挑選出最合適的路徑。

根據當前的點以及此后可能到達的目標點進行路徑規劃，路徑的選取相對來說比較柔和，使得在每一個拐點處它的曲率成線性變換。這個地方多采用多項式擬合的方法，在后面我們將看到三次多項式(cubic)或者高階的五次多項式(quintic)將會被應用到，而且根據車速以及路況的不同，兩種方式可以切換。對于每一個制定的路徑，選取三種運動策略：

1. 提高線速度；

2. 降低線速度；

3. 保持線速度；

如上圖所示，從一點出發到達每一個終點被劃分為三種狀態。

二. 路徑估計

對于路徑的估計非常關鍵，這里我們采用耗損函數來完成這一任務。對于每一條路徑Ts，Cost Function——C(Ts)根據路徑與障礙物的距離定義他的安全指數，指數越高說明安全性越低。如下圖所示，對不同的路徑以及不同的速度模式分別給出了其對應的安全指數。

現在我們引入幾個參變量，Sf(Ts)，Tf(Ts)分別對應于路徑的終止點以及終止時間，定義耗損函數

M(Ts) = C(Ts) + Phi(Ts)

其中Phi(Ts)是對目標點的評估函數。

Phi(Ts) = Phi_c(Ts) + Phi_h(Ts)

Phi_c(Ts) = -Ks*Sf(Ts) + Kt*Tf(Ts)

Phi_h(Ts) = Hd(Sf(Ts)) ?+ Ht(Tf(Ts))?

Phi_c(Ts)根據運行的距離以及時間評估，距離越短，時間越長，得分越高；Phi_h是為了確保運行時間至少持續了一個給定的時間長度，而且使得選擇偏向于直接到達最遠點的路徑。

三. 多路段路徑規劃

由于障礙物的存在，使得在多路段規劃的時候需要多次切換車道。但是如何從多條路徑中選取適合的道路是一個棘手的問題，如下圖所示。

為了簡化這一問題，我們首先采用剪枝法刪去多余的節點。我們建立一張Time——velocity的表，在每一個方框中的每一種類型速度保留一個節點。關于為什么對于每個初始節點要要保留不同類型的速度模式，而不是取出其中最好的，我們在后面的將會有介紹。

從初始的節點引出的9種借點模式，對應V_T表中的每一個格子，刪除必要的點得到有圖所示的節點圖，可以看到每個格子中每種速度模式的節點最多只有一個。

實際效果圖

總結

以上是生活随笔為你收集整理的Planning的全部內容，希望文章能夠幫你解決所遇到的問題。

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