Frenet坐標(biāo)系，自動(dòng)駕駛“混亂”的源頭

人類駕駛員在道路上開(kāi)車的時(shí)候，沒(méi)有人會(huì)去基于當(dāng)前距離出點(diǎn)的橫向和縱向距離是多少，而去決定下一步的方向盤(pán)轉(zhuǎn)角大小。通常情況，我們只會(huì)關(guān)注車輛當(dāng)前距離左右車道線的距離，來(lái)判斷是否偏離車道，是否需要打方向盤(pán)進(jìn)行方向微調(diào)。

自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃也在效仿這一過(guò)程，而為了讓這一過(guò)程得以實(shí)施，2010年，BMW的Moritz Werling在2010年的論文《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene′t Frame》中提出了Frenet坐標(biāo)系。

沒(méi)錯(cuò)，還是國(guó)外人發(fā)明，我們整天鼓吹自動(dòng)駕駛第一大國(guó)，但看看自動(dòng)駕駛常用的基礎(chǔ)理論、經(jīng)典架構(gòu)、基礎(chǔ)軟件、經(jīng)典算法……，有多少是我們的發(fā)明創(chuàng)造！每個(gè)場(chǎng)景都炒一波自動(dòng)駕駛概念，每個(gè)概念都拉一波融資，每一波融資都用來(lái)重復(fù)造輪子……。大而不強(qiáng)，是自動(dòng)駕駛繼承自汽車產(chǎn)業(yè)的頑疾。

Frenet坐標(biāo)下的出現(xiàn)，讓規(guī)劃、控制有了方向，但也從此開(kāi)啟了自動(dòng)駕駛?cè)f國(guó)混戰(zhàn)的源頭。

01Frenet坐標(biāo)系的定義

Frenet坐標(biāo)系的建立基于一個(gè)參考線，這個(gè)參考線可以是任意曲線，但在自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃中一般定義為道路的中心線，這條中心線是地圖模塊輸入過(guò)來(lái)的一系列離散點(diǎn)。同時(shí)使用參考線的切線向量和法線向量建立一個(gè)直角坐標(biāo)系，如圖1所示，這個(gè)坐標(biāo)系就是Frenet坐標(biāo)系。

圖1 Frenet坐標(biāo)系示意圖

某一時(shí)刻，假設(shè)車輛質(zhì)心在全局笛卡爾坐標(biāo)系下的坐標(biāo)為（x，y），車輛質(zhì)心到參考線上的投影點(diǎn)稱為原點(diǎn)，原點(diǎn)切線方向稱為s軸方向或縱軸方向，原點(diǎn)法線方向稱為d軸方向或橫軸方向?？v坐標(biāo)s值指的是參考線上的原點(diǎn)與起點(diǎn)之間曲線的長(zhǎng)度，也就是車輛在道路上的縱向行駛距離。橫坐標(biāo)d值指的是原點(diǎn)與車輛質(zhì)心之間的距離，也就是車輛偏離道路中心線的距離。車輛質(zhì)心隨著時(shí)間在不斷變化，因此Frenet坐標(biāo)系的原點(diǎn)也在不斷變換，所以Frenet坐標(biāo)系是一個(gè)移動(dòng)坐標(biāo)系。

02為什么使用Frenet坐標(biāo)系

（一）道路表達(dá)直觀

假設(shè)現(xiàn)在我們知道笛卡爾坐標(biāo)系下車輛的位置為（x，y），在道路通常都有一定曲率的背景下，從這個(gè)坐標(biāo)中我們無(wú)法得知道路在哪里，也不知道車輛行駛了多遠(yuǎn)，更難以確定它是否偏離車道中心。當(dāng)然我們可以從輸入的高精地圖數(shù)據(jù)進(jìn)行計(jì)算獲得上述數(shù)據(jù)，但是計(jì)算量太大，因此用笛卡爾坐標(biāo)系描述道路會(huì)非常復(fù)雜。

而在Frenet坐標(biāo)系中，橫軸和縱軸相互垂直且原點(diǎn)所在參考線與車道中心線平行，容易確定車輛偏離車道中心線的距離以及車輛沿車道中心線的行駛距離。因此使用Frenet坐標(biāo)系可以忽略道路曲率的影響，讓道路表達(dá)更加直觀、簡(jiǎn)潔。

（二）簡(jiǎn)化路徑規(guī)劃問(wèn)題

運(yùn)動(dòng)規(guī)劃是為了獲得車輛在未來(lái)一段時(shí)間內(nèi)具有前瞻性的行駛軌跡，這個(gè)軌跡是一種由二維空間和一維時(shí)間組成的三維空間中的曲線。

在笛卡爾坐標(biāo)系下，整車的橫向運(yùn)動(dòng)、縱向運(yùn)動(dòng)耦合在一起，想要計(jì)算清楚兩者的關(guān)系還是需要費(fèi)一番苦力的。而在Frenet坐標(biāo)系中，距離參考線的位置可以使用縱向距離s和橫向距離d分別表示，而通過(guò)對(duì)時(shí)間分別求導(dǎo)，又可以得到縱向運(yùn)動(dòng)速度和橫向運(yùn)動(dòng)速度。

車輛的二維運(yùn)動(dòng)問(wèn)題在Frenet坐標(biāo)系下被解耦成兩個(gè)一維運(yùn)動(dòng)問(wèn)題，而一維優(yōu)化問(wèn)題要比二維優(yōu)化問(wèn)題更容易容易求解。因此相比于笛卡爾坐標(biāo)系，F(xiàn)renet坐標(biāo)系可以明顯地簡(jiǎn)化運(yùn)動(dòng)規(guī)劃問(wèn)題，這就是運(yùn)動(dòng)規(guī)劃中笛卡爾坐標(biāo)系轉(zhuǎn)換為Frenet坐標(biāo)系的必要性。

下面我們用一個(gè)例子來(lái)解釋Frenet坐標(biāo)系的這種降維作用。假設(shè)行為決策層判斷前方有低速行駛的車輛，隨后在t0時(shí)刻做出了一個(gè)在t1時(shí)刻完成變道的決策。即車輛需要在Δt=（t1-t0）時(shí)間內(nèi)，橫向上需要完成一個(gè)Δd以及縱向上完成一個(gè)Δs的移動(dòng)。如圖2所示，我們可以將s和d分別表示為關(guān)于t的函數(shù)：s（t）和d（t），這樣二維運(yùn)動(dòng)規(guī)劃問(wèn)題被分割成了兩個(gè)獨(dú)立的一維優(yōu)化問(wèn)題。

圖2 Frenet坐標(biāo)系下運(yùn)動(dòng)規(guī)劃示例

03Frenet坐標(biāo)系與笛卡爾坐標(biāo)系轉(zhuǎn)換

要使用Frenet坐標(biāo)系進(jìn)行運(yùn)動(dòng)規(guī)劃，就需要將笛卡爾坐標(biāo)系下的車輛狀態(tài)轉(zhuǎn)化為Frenet坐標(biāo)系下的狀態(tài)。由于公式太多，只能采用圖片方式呈現(xiàn)后續(xù)內(nèi)容。

圖3 Frenet坐標(biāo)系和笛卡爾坐標(biāo)系下的狀態(tài)表示

基于Frenet坐標(biāo)系進(jìn)行運(yùn)動(dòng)規(guī)劃，將輸出單獨(dú)的橫、縱向軌跡，而最終輸出到自動(dòng)駕駛車輛參考運(yùn)動(dòng)軌跡需要可直接被控制模塊所應(yīng)用，因此最后還需要把Frenet坐標(biāo)系下得到的軌跡轉(zhuǎn)化到一個(gè)全局笛卡爾坐標(biāo)系中。逆過(guò)程過(guò)于簡(jiǎn)單，下面直接給出結(jié)論。

04寫(xiě)在最后

Frenet坐標(biāo)系的熟練掌握，應(yīng)該是規(guī)劃控制入門(mén)的第一課，也是開(kāi)啟你自動(dòng)駕駛混亂時(shí)代的第一課。

審核編輯 ：李倩