自動駕駛高速路上的路徑規(guī)劃算法

1、如何安全有效的規(guī)劃行駛路線，是自動駕駛汽車需解決的最大的難題之一。事實上，路徑規(guī)劃技術(shù)，現(xiàn)階段是一個非常活躍的研究領(lǐng)域。路徑規(guī)劃之所以如此復(fù)雜，是 因為其涵蓋了自動駕駛的所有技術(shù)領(lǐng)域，從最基礎(chǔ)的制動器，到感知周圍環(huán)境的傳感器，再 到定位及預(yù)測模型等等。準確的路徑規(guī)劃，要求汽車要理解我們所處的位置以及周邊的物體 (其他車輛、行人、動物等)會在接下來的幾秒鐘內(nèi)采取什么樣的行為。另一項關(guān)鍵技術(shù)是 軌跡生成器(trajectory generator),其產(chǎn)生輸入路徑規(guī)劃算法的參考軌跡。本期重點介紹一種基于C+開發(fā)的高速公路路徑規(guī)劃算法。該算法利用jerk minimisation 技術(shù)，可在模擬器中

2、生成安全且高效的行駛路徑。本算法的一些假設(shè)如下：任何情況下，不會與其他車輛發(fā)生事故最大行駛速度為80KMH最大加速度為10m/s2最大 jerk 為 10m/s3車輛在不同車道之間不超過3s車輛不能超出高速的3條車道車輛不能逆向行駛本算法的開發(fā)難度非常之大1.自動駕駛汽車的功能層首先，我們來詳細的研究下自動駕駛汽車的功能層(Functional laxxxxyers)。8EHAVIORPREDICTIONLOCALIZATIONTRAJECTORYCENSOR FUSIONMOTION CONTROL路徑規(guī)劃需要自動駕駛汽車不同功能層之間的合作、協(xié)調(diào)。上圖給岀了一個自動駕駛系統(tǒng)的 功能層的配置

3、形式：動作控制層：負責(zé)控制汽車，使其盡可能的按照“設(shè)定的”軌跡形式。該層需要最快的 反應(yīng)速度；傳感器融合層：負責(zé)合并個傳感器的輸出（如雷達和激光雷達）定位層：負責(zé)盡可能準確的在地圖上定位車輛的位置，并計算其他物體相對于車輛的位 置預(yù)測層：負責(zé)識別傳感器檢測到的物體的性質(zhì)（又名感知），并根據(jù)汽車當前的軌跡、其 他車輛的軌跡和場景中的各種元素（如交通燈）預(yù)測場景中近未來的變化。這個層的一個重要 任務(wù)是預(yù)測沖突。行為層：該層的主要作用是協(xié)調(diào)。根據(jù)底層的輸入信息來決定如何調(diào)整行車軌跡軌跡層：負責(zé)計算既定條件（速度、距離、車道、jerk等等）下的行車軌跡生成行車軌跡的方法很多，這里我們采用了 Frene

4、t Coordiante System方法。2 傳感器融合層的意義我們在模擬器中為車輛設(shè)置了一系列傳感器，它們的輸出融合在一起以產(chǎn)生更精確的測量結(jié) 果。大多數(shù)在Level4 _h的自動駕駛汽車公司在他們的傳感器套件中使用雷達、激光雷達和 照相機。擁有多種不同類型的傳感器至關(guān)重要，因為毎種傳感器都有各自的優(yōu)缺點。此外， 對于同種傳感器進行冗余設(shè)計，可以減輕傳感器故障帶來的影響。在本算法中，模擬器可以提供以下傳感器融合功能:車輛的位置、速度和方向其他車輛的位置和速度上次提交的車輛行駛軌跡通過以上信息，我們可以計算車輛與其他車輛的準確距離，并通過行車軌跡來預(yù)測與其他車 輛的碰撞可能性。下面我們詳細介

5、紹軌跡生成器(Trajectory Generation)。坐標系通常，我們習(xí)慣使用笛卡爾坐標系來定義空間點的位置。但在現(xiàn)實中，道路往往不是“筆直 的，因此對于人類非常簡單的操作(如判斷車輛在哪條車道)，在電腦的笛卡爾坐標系中，往往是難以準確定義的。下圖展示了我們使用笛卡爾坐標系時所面臨的問題:笛卡爾坐標系中的曲線車道設(shè)想一下，如果我們采用的坐標系可以反映道路的曲率，那么在新的坐標系下車輛向前行駛 并保持在車道內(nèi)的軌跡就會變成一條直線，這會大大簡化路徑規(guī)劃的難度。而Frenet坐標 系正可以實現(xiàn)我們的設(shè)想。vs笛卡爾坐標系(右)在Frenet坐標系中，可以平面上的點的位置可以由縱軸和橫軸定位分

6、別記為S和D。其背 后的數(shù)學(xué)原理非常復(fù)雜，在此我們不進行累述。你可以認為穿過道路中心的曲線決定了 S軸, 并指出了車輛在路上走了多遠；D軸則映射為車輛的側(cè)向位移。下圖顯示了彎曲道路在 Frenet坐標系上的樣子：4 軌跡平滑處理我們假設(shè)車道已經(jīng)被預(yù)先映射，并且提供了沿著中黃線的路徑點，這條中黃線分隔了公路的 兩邊。這有助于我們確定我們在最近的路徑點上的位置。由于我們所設(shè)置的路徑點非常稀疏，當我們試圖將Frenet轉(zhuǎn)換回真實世界坐標時，會產(chǎn)生 帶折角的軌跡。這反過來又會導(dǎo)致車輛突然的加速和顛簸。由于函數(shù)toRealWorld(s d)- (x y)使用兩個路徑點之間的線性插值，來確定x和y的最佳

7、逼近值，我們總是冒著產(chǎn)生非 平滑軌跡的風(fēng)險。如何進行改善呢在之前分享的算法中，我們發(fā)現(xiàn)由多項式擬合的曲線往往會產(chǎn)生非常平滑的 軌跡。因此，我們用多項式擬合來替代線性插值。利用在Frenet坐標系中取位置s來創(chuàng)建 樣條，得到真實世界的坐標（x y）和偏移量（dx和dy）,然后代入這個公式，得到最接近 真實世界的坐標點。x = spline s x(s) + cl * spline s dx(s) y = spline s y(s) + cl * spline s dy(s)仿真后可以看到，經(jīng)過優(yōu)化的行車軌跡非常平滑。5 狀態(tài)控制機制路徑規(guī)劃的另一個問題是，人類可以根據(jù)駕駛方式、環(huán)境信息以及目的地

8、，將汽車轉(zhuǎn)換到不 同的狀態(tài)。事實證明，我們可以將機車輛的狀態(tài)編入字典，并指導(dǎo)它們根據(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)。6 損失函數(shù)鑒于我們通常會返回多個候選狀態(tài)以及軌跡，我們必須找到一種選擇“最佳行動路線的方 法。我們用損失函數(shù)來解決該問題。損失函數(shù)可以教會汽車我們想要的合理行為，以及通過 不同的權(quán)重來懲罰哪些行為。我們所有的成本損失函數(shù)都遵循在cost functions文件中定義的接口 :f