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

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

本算法的一些假設如下：

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

最大行駛速度為80KMH

最大加速度為10m/s2

最大jerk為10m/s3

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

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

車輛不能逆向行駛

自動駕駛汽車的功能層

首先，我們來詳細的研究下自動駕駛汽車的功能層（Functional Layers）。

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

動作控制層：負責控制汽車，使其盡可能的安按照“設定的”軌跡形式。該層需要最快的反映速度；

傳感器融合層：負責合并個傳感器的輸出（如雷達和激光雷達）

定位層：負責盡可能準確的在地圖上定位車輛的位置，并計算其他物體相對于車輛的位置

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

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

軌跡層：負責計算既定條件（速度、距離、車道、jerk等等）下的行車軌跡

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

傳感器融合層的意義

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

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

車輛的位置、速度和方向

其他車輛的位置和速度

上次提交的車輛行駛軌跡

通過以上信息，我們可以計算車輛與其他車輛的準確距離，并通過行車軌跡來預測與其他車輛的碰撞可能性。

下面我們詳細介紹軌跡生成器（Trajectory Generation）。

Frenet坐標系

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

笛卡爾坐標系中的曲線車道

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

不同坐標系下的行車軌跡：

Frenet（左）vs 笛卡爾坐標系（右）

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

行駛軌跡平滑處理

我們假設車道已經(jīng)被預先映射，并且提供了沿著中黃線的路徑點，這條中黃線分隔了公路的兩邊。這有助于我們確定我們在最近的路徑點上的位置。

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

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

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)控制機制

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

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

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

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

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

損失函數(shù)

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

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

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

最終結(jié)果

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