事實(shí)上，路徑規(guī)劃技術(shù)，現(xiàn)階段是一個(gè)非?；钴S的研究領(lǐng)域。路徑規(guī)劃之所以如此復(fù)雜，是因?yàn)槠浜w了自動(dòng)駕駛的所有技術(shù)領(lǐng)域，從最基礎(chǔ)的制動(dòng)器，到感知周圍環(huán)境的傳感器，再到定位及預(yù)測(cè)模型等等。準(zhǔn)確的路徑規(guī)劃，要求汽車要理解我們所處的位置以及周邊的物體（其他車輛、行人、動(dòng)物等）會(huì)在接下來的幾秒鐘內(nèi)采取什么樣的行為。另一項(xiàng)關(guān)鍵技術(shù)是軌跡生成器（trajectory generator），其產(chǎn)生輸入路徑規(guī)劃算法的參考軌跡。

本期算法分享，我們重點(diǎn)介紹一種基于C++開發(fā)的高速公路路徑規(guī)劃算法。該算法利用jerk minimisation技術(shù)，可在模擬器中生成安全且高效的行駛路徑。

本算法的一些假設(shè)如下：

任何情況下，不會(huì)與其他車輛發(fā)生事故

最大行駛速度為80KMH

最大加速度為10m/s2

最大jerk為10m/s3

車輛在不同車道之間不超過3s

車輛不能超出高速的3條車道

車輛不能逆向行駛

自動(dòng)駕駛汽車的功能層

首先，我們來詳細(xì)的研究下自動(dòng)駕駛汽車的功能層（Functional Layers）。

路徑規(guī)劃需要自動(dòng)駕駛汽車不同功能層之間的合作、協(xié)調(diào)。上圖給出了一個(gè)自動(dòng)駕駛系統(tǒng)的功能層的配置形式：

動(dòng)作控制層：負(fù)責(zé)控制汽車，使其盡可能的安按照“設(shè)定的”軌跡形式。該層需要最快的反映速度；

傳感器融合層：負(fù)責(zé)合并個(gè)傳感器的輸出（如雷達(dá)和激光雷達(dá)）

定位層：負(fù)責(zé)盡可能準(zhǔn)確的在地圖上定位車輛的位置，并計(jì)算其他物體相對(duì)于車輛的位置

預(yù)測(cè)層：負(fù)責(zé)識(shí)別傳感器檢測(cè)到的物體的性質(zhì)(又名感知)，并根據(jù)汽車當(dāng)前的軌跡、其他車輛的軌跡和場(chǎng)景中的各種元素(如交通燈)預(yù)測(cè)場(chǎng)景中近未來的變化。這個(gè)層的一個(gè)重要任務(wù)是預(yù)測(cè)沖突。

行為層：該層的主要作用是協(xié)調(diào)。根據(jù)底層的輸入信息來決定如何調(diào)整行車軌跡

軌跡層：負(fù)責(zé)計(jì)算既定條件（速度、距離、車道、jerk等等）下的行車軌跡

生成行車軌跡的方法很多，這里我們采用了Frenet Coordiante System方法。

傳感器融合層的意義

我們?cè)谀M器中為車輛設(shè)置了一系列傳感器，它們的輸出融合在一起以產(chǎn)生更精確的測(cè)量結(jié)果。大多數(shù)在Level4上的自動(dòng)駕駛汽車公司在他們的傳感器套件中使用雷達(dá)、激光雷達(dá)和照相機(jī)。擁有多種不同類型的傳感器至關(guān)重要，因?yàn)槊糠N傳感器都有各自的優(yōu)缺點(diǎn)。此外，對(duì)于同種傳感器進(jìn)行冗余設(shè)計(jì)，可以減輕傳感器故障帶來的影響。

在本算法中，模擬器可以提供以下傳感器融合功能：

車輛的位置、速度和方向

其他車輛的位置和速度

上次提交的車輛行駛軌跡

通過以上信息，我們可以計(jì)算車輛與其他車輛的準(zhǔn)確距離，并通過行車軌跡來預(yù)測(cè)與其他車輛的碰撞可能性。

下面我們?cè)敿?xì)介紹軌跡生成器（Trajectory Generation）。

Frenet坐標(biāo)系

通常，我們習(xí)慣使用笛卡爾坐標(biāo)系來定義空間點(diǎn)的位置。但在現(xiàn)實(shí)中，道路往往不是“筆直”的，因此對(duì)于人類非常簡(jiǎn)單的操作（如判斷車輛在哪條車道），在電腦的笛卡爾坐標(biāo)系中，往往是難以準(zhǔn)確定義的。下圖展示了我們使用笛卡爾坐標(biāo)系時(shí)所面臨的問題：

笛卡爾坐標(biāo)系中的曲線車道

設(shè)想一下，如果我們采用的坐標(biāo)系可以反映道路的曲率，那么在新的坐標(biāo)系下車輛向前行駛并保持在車道內(nèi)的軌跡就會(huì)變成一條直線，這會(huì)大大簡(jiǎn)化路徑規(guī)劃的難度。而Frenet坐標(biāo)系正可以實(shí)現(xiàn)我們的設(shè)想。

不同坐標(biāo)系下的行車軌跡：

Frenet（左）vs 笛卡爾坐標(biāo)系（右）

在Frenet坐標(biāo)系中，可以平面上的點(diǎn)的位置可以由縱軸和橫軸定位,分別記為S和D 。其背后的數(shù)學(xué)原理非常復(fù)雜，在此我們不進(jìn)行累述。你可以認(rèn)為穿過道路中心的曲線決定了S軸，并指出了車輛在路上走了多遠(yuǎn)；D軸則映射為車輛的側(cè)向位移。下圖顯示了彎曲道路在Frenet坐標(biāo)系上的樣子：

行駛軌跡平滑處理

我們假設(shè)車道已經(jīng)被預(yù)先映射，并且提供了沿著中黃線的路徑點(diǎn)，這條中黃線分隔了公路的兩邊。這有助于我們確定我們?cè)谧罱穆窂近c(diǎn)上的位置。

由于我們所設(shè)置的路徑點(diǎn)非常稀疏，當(dāng)我們?cè)噲D將Frenet轉(zhuǎn)換回真實(shí)世界坐標(biāo)時(shí)，會(huì)產(chǎn)生帶折角的軌跡。這反過來又會(huì)導(dǎo)致車輛突然的加速和顛簸。由于函數(shù)toRealWorld(s, d) -> (x, y)使用兩個(gè)路徑點(diǎn)之間的線性插值，來確定x和y的最佳逼近值，我們總是冒著產(chǎn)生非平滑軌跡的風(fēng)險(xiǎn)。

如何進(jìn)行改善呢?在之前分享的算法中，我們發(fā)現(xiàn)由多項(xiàng)式擬合的曲線往往會(huì)產(chǎn)生非常平滑的軌跡。因此，我們用多項(xiàng)式擬合來替代線性插值。利用在Frenet坐標(biāo)系中取位置s來創(chuàng)建樣條，得到真實(shí)世界的坐標(biāo)（x, y）和偏移量（dx和dy），然后代入這個(gè)公式，得到最接近真實(shí)世界的坐標(biāo)點(diǎn)。

x=spline_s_x(s)+d*spline_s_dx(s)y=spline_s_y(s)+d*spline_s_dy(s)

可以看到，經(jīng)過優(yōu)化的行車軌跡非常平滑。

狀態(tài)控制機(jī)制

路徑規(guī)劃的另一個(gè)問題是，人類可以根據(jù)駕駛方式、環(huán)境信息以及目的地，將汽車轉(zhuǎn)換到不同的狀態(tài)。事實(shí)證明，我們可以將機(jī)車輛的狀態(tài)編入字典，并指導(dǎo)它們根據(jù)當(dāng)前狀態(tài)，將自動(dòng)駕駛系統(tǒng)的其他層轉(zhuǎn)換到哪些狀態(tài)。

在算法中，我們的狀態(tài)控制機(jī)制非常簡(jiǎn)單，如下所示：

最終的狀態(tài)控制機(jī)制

最常見的狀態(tài)是保持車道，但每當(dāng)我們想要改變車道，汽車將首先過渡到準(zhǔn)備換車道的左/右狀態(tài)，并在確保轉(zhuǎn)換車道是安全的。在換車道前進(jìn)入的中間狀態(tài)，類似于車輛在換車道前打開左/右信號(hào)。

關(guān)于狀態(tài)機(jī)的實(shí)現(xiàn)，我們?cè)贔renet坐標(biāo)上獲得了靈感。我們將一個(gè)給定的狀態(tài)分解為它的縱向和橫向分量。縱向狀態(tài)代表保持當(dāng)前車道，橫向狀態(tài)代表我們可能會(huì)進(jìn)入的下一個(gè)狀態(tài)，而損失函數(shù)（Cost Function）則傾向于選擇縱向狀態(tài)。

損失函數(shù)

鑒于我們通常會(huì)返回多個(gè)候選狀態(tài)以及軌跡，我們必須找到一種選擇“最佳”行動(dòng)路線的方法。我們用損失函數(shù)來解決該問題。損失函數(shù)可以教會(huì)汽車我們想要的合理行為，以及通過不同的權(quán)重來懲罰哪些行為。

我們所有的成本損失函數(shù)都遵循在cost_functions文件中定義的接口：

typedeffunction&,constTrajectory&,constState&,constdouble&)>CostFunction;

最終結(jié)果

可以看到，當(dāng)前我們?cè)O(shè)計(jì)的路徑規(guī)劃器運(yùn)行得相當(dāng)好，可以確保車輛在車道上安全有效的行駛。后續(xù)，我們計(jì)劃通過調(diào)整權(quán)重、改進(jìn)損失函數(shù)以及在預(yù)測(cè)層中加入機(jī)器學(xué)習(xí)算法來進(jìn)一步優(yōu)化。