Planning Algorithm

一. 單一路段規(guī)劃

從道路中提取Vehicle可能會(huì)到達(dá)的目標(biāo)點(diǎn)，采用Cost Function對(duì)不同的路徑進(jìn)行評(píng)估，并挑選出最合適的路徑。

根據(jù)當(dāng)前的點(diǎn)以及此后可能到達(dá)的目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃，路徑的選取相對(duì)來(lái)說(shuō)比較柔和，使得在每一個(gè)拐點(diǎn)處它的曲率成線(xiàn)性變換。這個(gè)地方多采用多項(xiàng)式擬合的方法，在后面我們將看到三次多項(xiàng)式(cubic)或者高階的五次多項(xiàng)式(quintic)將會(huì)被應(yīng)用到，而且根據(jù)車(chē)速以及路況的不同，兩種方式可以切換。對(duì)于每一個(gè)制定的路徑，選取三種運(yùn)動(dòng)策略：

1. 提高線(xiàn)速度；

2. 降低線(xiàn)速度；

3. 保持線(xiàn)速度；

如上圖所示，從一點(diǎn)出發(fā)到達(dá)每一個(gè)終點(diǎn)被劃分為三種狀態(tài)。

二. 路徑估計(jì)

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

現(xiàn)在我們引入幾個(gè)參變量，Sf(Ts)，Tf(Ts)分別對(duì)應(yīng)于路徑的終止點(diǎn)以及終止時(shí)間，定義耗損函數(shù)

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

其中Phi(Ts)是對(duì)目標(biāo)點(diǎn)的評(píng)估函數(shù)。

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)根據(jù)運(yùn)行的距離以及時(shí)間評(píng)估，距離越短，時(shí)間越長(zhǎng)，得分越高；Phi_h是為了確保運(yùn)行時(shí)間至少持續(xù)了一個(gè)給定的時(shí)間長(zhǎng)度，而且使得選擇偏向于直接到達(dá)最遠(yuǎn)點(diǎn)的路徑。

三. 多路段路徑規(guī)劃

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

為了簡(jiǎn)化這一問(wèn)題，我們首先采用剪枝法刪去多余的節(jié)點(diǎn)。我們建立一張Time——velocity的表，在每一個(gè)方框中的每一種類(lèi)型速度保留一個(gè)節(jié)點(diǎn)。關(guān)于為什么對(duì)于每個(gè)初始節(jié)點(diǎn)要要保留不同類(lèi)型的速度模式，而不是取出其中最好的，我們?cè)诤竺娴膶?huì)有介紹。

從初始的節(jié)點(diǎn)引出的9種借點(diǎn)模式，對(duì)應(yīng)V_T表中的每一個(gè)格子，刪除必要的點(diǎn)得到有圖所示的節(jié)點(diǎn)圖，可以看到每個(gè)格子中每種速度模式的節(jié)點(diǎn)最多只有一個(gè)。

實(shí)際效果圖

總結(jié)

以上是生活随笔為你收集整理的Planning的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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