無人駕駛frenet座標系解釋

待整理

本文從畢設中整理，尚未整理完成。

Frenet座標系介紹

Frenet–Serret公式用於描述粒子在三維歐氏空間內沿一條連續可微曲線的運動學特徵

$\vec T$稱爲切向量（Tangent），表示沿曲線運動方向的單位向量；$\vec N$稱爲法向量（Normal），表示當前曲線運動平面內垂直於的單位向量；$\vec B$稱爲當前運動平面的副法向量（Binormal），表示同時垂直於和的單位向量。令$r(t)$爲這個歐式空間中隨着時間$t$改變的非退化曲線，可令$s(t)$爲這條非退化曲線在時間t的弧長。

退化曲線即：兩條相交直線,當一個平面過圓錐的頂點截圓錐面得到曲線，其方程爲（A1x+B1y+C1) *(A2x+B2y+C2) = 0
形似二次曲線,實際爲兩條直線

$s(t) = \int_0^t ||\gamma'(\sigma)|| d\sigma$

$t$還可以表示爲$s$的函數，也即是存在$t(s)$，從而有$\vec r(s)=\vec r(t(s))$，對於這條將利用弧長表達的曲線函數，可以定義其切向量$\vec T$、法向量$\vec N$、副法向量$\vec B$如下：

假設存在：$y(x) 、z(x)$兩個函數
那麼存在：用z表示y的函數$y(z) = y(x(z)) = y(z)$
注：$y = 3*x 、 x(y) = 1/3 * y$ $\qquad z = 2*x 、x(z) = 1/2 * z$
$\qquad y(z) = y(x(z)) = 3 * x(z) = 3 * 1/2 * z$

$\vec T$$\frac{\frac{d\vec r}{ds}}{||\frac{d\vec r}{ds}||}$$\qquad \frac{d\vec r}{ds}$ 除以自己的範數（長度），得到一個歸一化的向量

$\vec N$$\frac{\frac{d\vec T}{ds}}{||\frac{d\vec T}{ds}||}$

$\vec B = \vec T ×\vec N$

根據公式可知：
$\frac{d\vec T}{ds}=\kappa \vec N$

$\frac{d\vec N}{ds}=-\kappa \vec T+\tau \vec B$

$\frac{d\vec B}{ds}=-\tau \vec N$

其中，$\kappa、\tau$爲$\vec r(t(s))$曲線的曲率和撓率。其矩陣表達式如下所示：
$\begin{bmatrix} \vec T' \\ \vec N' \\ \vec B' \end{bmatrix}=$$\begin{bmatrix} 0 &\kappa&0 \\ -\kappa&0&\tau \\ 0&\tau &0 \end{bmatrix}$$\begin{bmatrix}\vec T \\ \vec N\\ \vec B\end{bmatrix}$

frenet座標在無人駕駛中的用途

對於當前的無人駕駛研究而言，通常不會單獨將高度作爲規劃因素，因此可以將車輛運動曲線投影到同一平面內，即令$\tau$=0，這樣上式就可以簡化爲：
$\begin{bmatrix}\vec T'\\\vec N'\end{bmatrix}$=$\begin{bmatrix}0&\kappa \\ -\kappa &0\end{bmatrix}$$\begin{bmatrix}\vec T\\ \vec N\end{bmatrix}$

假定$\vec r(t(s))$是參考線在弧長$s$處的位置，$x$是當前車輛軌跡點，該向量一般採用UTM座標表示，$\vec x = [x,y]^T$，在Frenet座標系下，一般採用弧長s和橫向偏移l描述車輛當前位置對其描述，即$\vec x =\vec x(s,l)$。

$\theta_r、\vec T_r、\vec N_r$分別爲規劃軌跡的方位角、單位切向角和單位法向角，$\theta_r、\vec T_r、\vec N_r$分別爲當前軌跡的方位角、單位切向角和單位法向角。由正交定義可得:

$\vec T _r=\begin{bmatrix}\cos\theta_r\\\sin\theta_r\end{bmatrix}$

$\vec N_r =\begin{bmatrix}-\sin\theta_r&\cos\theta_r\end{bmatrix}$

$\vec T_x =\begin{bmatrix}\cos\theta_x\\\sin\theta_x\end{bmatrix}$

$\vec N_x =\begin{bmatrix}-\sin\theta_x&\cos\theta_x\end{bmatrix}$

因爲正交，所以可知：$\vec T_r×\vec N_r$=0、$\vec T_x×\vec N_x$=0

通過對平面向量的加減可得：
$\vec x(s,l)=\vec r(s)+l(s)\vec N_r(s)$

根據公式得：
$l\vec N^T \vec N=\vec N^T(\vec x-\vec r)$

根據外積的反稱性可知：
$l=\begin{bmatrix}\vec x\\\vec r\end{bmatrix}\vec N_r$

假設有任意$\forall a$：
$\dot{a}=$$\frac{d(a)}{dt}$

$\ddot{a}=$$\frac{d(\dot{a})}{dt}$

$a'=$$\frac{d(a)}{ds}$

$a''=$$\frac{d(a')}{ds}$

在上式中，俺們規定，$\dot{a}$表示$a$對時間$t$的一階求導，$\ddot{a}$表示$a$對時間$t$的一階求導，$a'$表示$a$對弧長$s$的一階求導，$a''$表示$a$對弧長$s$的二階求導。
則有：
$\dot{l}=\begin{bmatrix}\dot{\vec x} -\dot{\vec r}\end{bmatrix}^T\vec N_r-\begin{bmatrix}\vec x-\vec r\end{bmatrix}^T \dot{\vec {N_r}}$

由單位切向量和單位法向量的定義可得:
$\dot{\vec x}=$$\frac {d||\vec x||}{dt}$$\vec T_x=v_x\vec T_x$

$\dot{\vec r}=$$\frac {d||\vec r||}{dt}$$\vec T_r=\dot{s}\vec T$

將公式互相帶入，結合平面幾何知識可知：
$\dot{l}=v_x\sin\Delta \theta$

其中$\Delta\theta$爲$\theta_x-\theta_r$。
同樣地可知：
$\dot{\vec x}=$$\frac{d(\vec r+l\vec N_r)}{dt}$$=\dot{\vec r}+\dot{l}\vec N_r+l\dot{\vec N_r}$

即是：

$v_x=||\dot{\vec x}||=$$\sqrt{[\dot{s}(1-\kappa_t)l]^2+\dot{l}^2}$

最終可以將$l'$和$l''$表示如下：
$l'=$$\frac{v_x}{\dot{s}}$$\sin\Delta\theta$$(1-\kappa_rl)\tan\Delta\theta$

$l''=$$-(\kappa_r'l+\kappa_rl')\tan\Delta\theta+$$\frac{(1-\kappa_rl)}{\cos^2\Delta\theta}$$(\kappa_x$$\frac{1-\kappa_rl}{\cos\Delta\theta}$$-\kappa_r)$