自駕車路徑規劃系列(2)-Frenet公式

系列文章

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

前面有提到Frenet座標系就是把「曲線的空間轉換至線性空間」，這時候就要靠Frenet公式了。

在紅色曲線上，任意參考點P點的切向量(tangent) (T)，垂直於T的法向量(normal) (N)，同時垂直於T和N的副法向量(binormal) (B)。

簡單說Frenet公式就是三個單位向量(T、N和B)對曲線(紅線)弧長微分之後的仍然用這三個向量來表示的關係式，也就是

Frenet公式

位置向量(r)(指向P參考點)對弧長(s)的微分等於切線的單位向量(T)

當長度固定的向量微分之後只有兩個結果
(i)等於0
或是
(ii)和原始向量垂直
因為T會隨時間變化更動，切線在不同參考點會變動，所以是(ii) 和原始向量垂直，因此

N為法線的單位向量，N和T垂直。k為dT/ds的大小，也稱為曲率(curvature)，曲率的倒數:1/k為曲率半徑。

第三個單位向量同時與T和N垂直，稱為副法線單位向量B

×為外積

切線的單位向量(T)、N為法線的單位向量、副法線單位向量B三者的關係

依照箭頭方式做外積，則

反箭頭方式做外積，則

所以

τ為曲線扭率(torsion)。

因為切線、法線、副法線的關係

由上面推導已經得到三個Frenet公式:

Frenet公式

k稱為曲率(curvature)，當曲率為0則代表曲線為直線。

τ為曲線扭率(torsion)，曲線不能行程在同一平面運動曲線的度量值，當扭率趨於0，表示曲線近似在同一平面內運動。

求得k和τ的解

我們可以把上述的Frenet公式同時考慮到時間軸(t)上的關係，並且我們將位置向量(r)對時間t微分，得到速率(v)乘上切線單位向量

弧長對t微分(ds/dt)為速度v。

二階微分

三階微分

求k

Note: T×N=B，|B|=1，這邊的|B|是在作L2-norm算向量的長度，因為B是單位向量，所以等於1。

求τ

因為B和T和N都垂直，內積等於0，自己的內積等於1(單位向量的關係)。

Note:

但在車輛規劃上，路面資訊可以視為平面，一般對高度資訊不感興趣，前面有提到扭率為0表示曲線近似在同一平面內運動，因此對路徑規劃上，曲線扭率(τ)可以視為0，Frenet公式可以簡化為

v為速度，k為曲率。

用矩陣方式簡化一點Frenet公式

在車輛控制可以省略高度資訊(忽略副法向量B)，自動駕駛車路徑規劃常看到的Frenet公式為