无人驾驶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)$