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

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

本算法的一些假設如下：

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

最大行駛速度為80KMH

最大加速度為10m/s2

最大jerk為10m/s3

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

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

車輛不能逆向行駛

自動駕駛汽車的功能層

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

路徑規劃需要自動駕駛汽車不同功能層之間的合作、協調。上圖給出了一個自動駕駛系統的功能層的配置形式：

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

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

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

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

行為層：該層的主要作用是協調。根據底層的輸入信息來決定如何調整行車軌跡

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

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

傳感器融合層的意義

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

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

車輛的位置、速度和方向

其他車輛的位置和速度

上次提交的車輛行駛軌跡

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

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

Frenet坐標系

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

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

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

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

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

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

行駛軌跡平滑處理

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

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

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

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

可以看到，經過優化的行車軌跡非常平滑。

狀態控制機制

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

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

最終的狀態控制機制

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

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

損失函數

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

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

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

最終結果

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