自駕車路徑規劃系列(3)-笛卡爾座標系和Frenet座標系相互轉換

系列文章

• 自駕車路徑規劃系列(1)-Lattice Planner之笛卡爾座標系和Frenet座標系的關係
• 自駕車路徑規劃系列(2)-Frenet公式
• 自駕車路徑規劃系列(3)-笛卡爾座標系和Frenet座標系相互轉換

這篇很長，我把文章進行章節的編碼，但因為Medium文章好像沒有提供文章內的超連結，所以大家可以用搜尋的方式直接到找

I. 簡介
II. 笛卡爾座標系←→Frenet座標系公式
II-1. 笛卡爾座標系→Frenet座標系
II-2. Frenet座標系→笛卡爾座標
II-3. 笛卡爾座標系→Frenet座標系在計算的流程
III. Frenet公式推導部分
III-1. 笛卡爾座標轉Frenet座標系
III-1–1. 橫向距離d的部分
III-1–2. 橫向距離對於時間t的微分
III-1–3. 橫向距離對於時間t的二次微分
III-1–4. 橫向距離對於弧長s的微分
III-1–5. 橫向距離對於弧長s的二次微分
III-2. 縱向距離s的部分
III-2–1. 縱向距離對於時間t的微分
III-2–2. 縱向距離對於時間t的二次微分
III-3. Frenet座標系轉笛卡爾座標
III-3–1 笛卡爾座標x向量
III-3–2 車輛在車輛軌跡的轉向角
III-3–3 車輛速度
III-3–4 車輛加速度
III-3–5 車輛軌跡曲率

I. 簡介

前面已經提過為什麼要將笛卡爾座標系轉換為Frenet座標系，主要就是將非線性的問題轉換到單純的線性問題，例如下圖將參考線(二次或是三次問題)，轉換到一次式問題，可參考第一篇有解釋。

在第二篇已經提到Frenet的式子，是由切線向量T和法線向量N來決定Frenet轉換，然後副法線B也就是高度資訊因在自駕規劃是可以忽略，因此我們不考慮副法線B。在自駕車的Frenet座標就是將切向量T(上右圖的s)和法向量N(上右圖的d)組成的直角坐標系。

那從笛卡爾座標轉到Frenet座標需要算的東西有什麼?我們先呈現下面這張圖。

稍微簡易看一下總共有13個參數，分別是

這13個參數的意義分別為:

接下來我們要把這13個參數的方程式都找出來，超級多的。

Note: 我們在Frenet式子有介紹，Frenet是針對弧長s微分的式子，我們也介紹針對時間t做微分，為了簡化看到微分的式子，我們會將微分的方程式簡化

Note: 我們在Frenet式子有講到在曲線上某一點的切線(T)、法線(N)、副法線(B)，我們先忽略副法線，然後依據切線夾角 ，我們可以算出切線(T)和法線(N)的向量表示式。下圖因為T和N我們都假設是單位向量，所以 。

後面會用到這個方程式。如果有看到sin和cos的出現就是用這個方程式出現的。

II. 笛卡爾座標系←→Frenet座標系公式

我們把上面那張圖畫的在複雜一點，切線、法線、方位角都畫出來，如下。

由上面提到切線和法線轉換向量的式子可以推出

先將Frenet式子寫出來(見第二部分內容)

我們開始找笛卡爾座標的Q轉換到Frenet座標為Q。公式推導很複雜，可以省略推導直接看下面的內容，也可以看後半段的推導內容。

II-1. 笛卡爾座標系→Frenet座標系

式子?是為了讓讀者方便去推導公式部分找公式的推導來源

II-2. Frenet座標系→笛卡爾座標

式子?是為了讓讀者方便去推導公式部分找公式的推導來源

II-3. 笛卡爾座標系→Frenet座標系在計算的流程

先算一些會用到的方程式簡化，

然後計算要需要求的參數

III. Frenet公式推導部分

III-1. 笛卡爾座標轉Frenet座標系

III-1–1. 橫向距離d的部分

由上圖可以得到x和 r(從原點0拉出的那兩條黑色虛線)之間的關係可以寫成，

式子1。(因為N是單位向量，所以向量長度要乘上位移大小d)

上圖看不懂也沒關係，我們簡化那個圖變成下圖，大家看這個高中數學(向量的合成)，注意一下箭頭方向，這樣就知道為什麼有上面的(式子1)。

向量的合成

式子2

但因為我們用d在表示偏移，所以會有負數的情形，也就是在參考線的左邊還是右邊，所以

式子3

從(式子1)

所以利用式子2可以來決定d是正數還是負數。

III-1–2. 橫向距離對於時間t的微分

對時間t的微分

根據(式子1)可以得知x-r=dN，所以

因為切向量和法向量正交，所以

式子4

III-1–3. 橫向距離對於時間t的二次微分

式子5

note:

這個關係推導是從式子9來的，因為我希望都先處理橫向問題，我們先在這邊寫出來，有疑慮可以先看後面部分。

III-1–4. 橫向距離對於弧長s的微分

式子6

III-1–5. 橫向距離對於弧長s的二次微分

曲率定義:

式子7

note:

所以

帶入(式子7)

式子8

III-2. 縱向距離s的部分

III-2–1. 縱向距離對於時間t的微分

根據(式子1)可以得知，x-r=dN，對時間t微分，

式子9

III-2–2. 縱向距離對於時間t的二次微分

速度(一次微分)對t微分等於加速度(二次微分)，這邊很有趣，我一開始也以為直接對式子9微分就好了，但推出來都怪怪的，後來看別人是從加速度(也是二次微分)出發。

上一張圖裡面有些方程式我們在這段列出來

式子10

III-3. Frenet座標系轉笛卡爾座標

III-3–1 笛卡爾座標x向量

式子11

III-3–2 車輛在車輛軌跡的轉向角

式子12

III-3–3 車輛速度

式子13

III-3–4 車輛加速度

III-3–5 車輛軌跡曲率

式子13