02Frenet与Cartesian坐标系(Cartesian转Frenet公式推导)

Cartesian转Frenet

1 明确目标

已知车辆质点在Cartesian坐标系下的状态：

• 位置向量：$\overrightarrow{r_h}$，通过定位模块可以获得车辆位置坐标$(x_h,y_h)$，即$\overrightarrow{r_h}$。

• 切线方向与$x$轴夹角：${\theta }_h$，通过定位模块可以获得车辆的朝向角$yaw$角或者$heading$角，即${\theta }_h$。

• 车辆所在轨迹的单位切向量：$\overrightarrow{{\tau }_h}$
  $$\overrightarrow{{\tau }_h}=(cos{\theta }_h,sin{\theta }_h)\text{\hspace{1em}(1-1)}$$

• 车辆所在轨迹的单位法向量：$\overrightarrow{n_h}$
  $$\overrightarrow{n_h}=(-sin{\theta }_h,cos{\theta }_h)\text{\hspace{1em}(1-2)}$$

• 速度向量：$\overrightarrow{v_h}$
  $$\overrightarrow{v_h}=|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}=v_h\overrightarrow{{\tau }_h}\text{\hspace{1em}(1-3)}$$
  其中，$v_h$ 为标量，代表车辆纵向速度，可以通过轮速传感器和阿克曼转向模型计算得到。

• 曲率：${\kappa }_h$ 为车辆行驶轨迹的曲率，可以通过下述曲率公式计算得到。
  $$\kappa =\frac{|x^{\prime }(t)y^{\prime \prime }(t)-y^{\prime }(t)x^{\prime \prime }(t)|}{{\left(x^{\prime }(t{)}^2+y^{\prime }(t{)}^2\right)}^{3/2}}\text{\hspace{1em}(1-4)}$$

• 加速度向量：$\overrightarrow{a_h}$
  $$\overrightarrow{a_h}=\dot{\overrightarrow{v_h}}=a_{\tau }\overrightarrow{{\tau }_h}+{\kappa }_h{v}_h^2\overrightarrow{n_h}\text{\hspace{1em}(1-5)}$$
  从上式中可以看出$a_h$ 可以通过两种方式得到：一种通过$\overrightarrow{v_h}$对时间求导（可以用差分代替）得到；一种是通过纵向加速度计获得$a_{\tau }$，然后结合${\kappa }_h$计算得到。这两种方式前者属于预测的方式，后一种属于观测的方式，可以选择一种，也可以选择使用卡尔曼滤波器将这两者结合起来使用，获得更准确的$\overrightarrow{a_h}$ 。

已知参考线上任意点状态（下述状态在参考线生成后都可以通过计算得到）：

• 参考线上的任意点：$s_r$
• 参考线上任意点的单位切向量：$\overrightarrow{{\tau }_r}$
• 参考线上任意点的单位法向量：$\overrightarrow{n_r}$
• 参考线上任意点的曲率：${\kappa }_r$
• 参考线上任意点的曲率变化率：${\kappa }_r^{\prime }$

求车辆质点在Frenet坐标系下的状态：

• Frenet 坐标系下的纵向坐标：$s$
• 纵向速度：$\dot{s}$
• 纵向加速度：$\ddot{s}$
• 横向坐标：$l$
• 横向速度：$\dot{l}$
• 横向加速度：$\ddot{l}$
• 横向坐标变化率：$l^{\prime }$
• 横向坐标的二阶变化率：$l^{\prime \prime }$

2 公式推导

2.1 基础公式汇总

为了方便查阅，现将前文推导的Frenet基础公式汇总如下：

• 核心关系

$$\overrightarrow{r_h}=\overrightarrow{r_r}+l\overrightarrow{n_r}\text{\hspace{1em}(2-1)}$$

• Frenet最小状态集

$$\dot{l}=l^{\prime }\dot{s}\text{\hspace{1em}(2-2)}$$

$$\ddot{l}=l^{\prime \prime }{\dot{s}}^2+l^{\prime }\ddot{s}\text{\hspace{1em}(2-3)}$$

• 车辆在Cartesian下的向量导数
  $$\dot{\overrightarrow{r_h}}=\overrightarrow{v_h}=|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}=v_h\overrightarrow{{\tau }_h}\text{\hspace{1em}(2-4)}$$

  $$\dot{\overrightarrow{{\tau }_h}}={\kappa }_h{v}_h\overrightarrow{n_h}\text{\hspace{1em}(2-5)}$$

  $$\dot{\overrightarrow{n_h}}=-{\kappa }_h{v}_h\overrightarrow{{\tau }_h}\text{\hspace{1em}(2-6)}$$

  $$\dot{\overrightarrow{v_h}}=\overrightarrow{a_h}=a_{\tau }\overrightarrow{{\tau }_h}+{\kappa }_h{v}_h^2\overrightarrow{n_h}\text{\hspace{1em}(2-7)}$$

• 匹配点在Cartesian下的向量导数
  $$\dot{\overrightarrow{r_r}}=\overrightarrow{v_r}=|\overrightarrow{v_r}|\overrightarrow{{\tau }_r}=\dot{s}\overrightarrow{{\tau }_r}\text{\hspace{1em}(2-8)}$$

  $$\dot{\overrightarrow{{\tau }_r}}={\kappa }_r\dot{s}\overrightarrow{n_r}\text{\hspace{1em}(2-9)}$$

  $$\dot{\overrightarrow{n_r}}=-{\kappa }_r\dot{s}\overrightarrow{{\tau }_r}\text{\hspace{1em}(2-10)}$$

2.2 寻找突破口(求解s)

根据$s$的定义：车辆质点在实际轨迹上的位置投影到参考线（可以理解为车道中心线）的点$(x_r,y_r)$，距离参考线起点的长度，不难算出s。具体的算法在工程实现时往往会单独抽象为一个“计算匹配点/l投影点”的模块，在该模块内不但要完成$s$计算的基本功能，还会加入一些辅助加速的算法，这块后续有机会再说，现在我们认为$s$是可以根据现有信息计算得到的就可以了。即
$$s=s_r\text{\hspace{1em}(2-11)}$$

2.3 撕开一道口子（求解l）

将之前推导出的核心关系公式(2-1)两边同时点乘$\overrightarrow{n_r}$可得
$$l=(\overrightarrow{r_h}-\overrightarrow{r_r})\overrightarrow{n_r}\text{\hspace{1em}(2-12)}$$

2.4 按部就班（求解$\dot{s}$）

将核心关系公式（2-1）两边同时对$t$ 求导，可得
$$\dot{\overrightarrow{r_h}}=\dot{\overrightarrow{r_r}}+\dot{l}\overrightarrow{n_r}+l\dot{\overrightarrow{n_r}}\text{\hspace{1em}(2-13)}$$
将(2-4)、(2-8)、(2-10)带入，可得
$$\overrightarrow{v_h}=\dot{s}\overrightarrow{{\tau }_r}+\dot{l}\overrightarrow{n_r}-l{\kappa }_r\dot{s}\overrightarrow{{\tau }_r}\text{\hspace{1em}(2-14)}$$
为了消除未知量$\dot{l}$，将公式两边同时乘以$\overrightarrow{{\tau }_r}$，可得
$$\overrightarrow{v_h}\overrightarrow{{\tau }_r}=\dot{s}-l{\kappa }_r\dot{s}\text{\hspace{1em}(2-14)}$$
整理后可得
$$\dot{s}=\frac{\overrightarrow{v_h}\overrightarrow{{\tau }_r}}{1-l{\kappa }_r}\text{\hspace{1em}(2-15)}$$

2.5 按部就班（求解$\dot{l}$）

对(2-14)等式两侧同时点乘$\overrightarrow{n_r}$，可得
$$\dot{l}=\overrightarrow{v_h}\overrightarrow{n_r}\text{\hspace{1em}(2-16)}$$

2.6 按部就班（求解$l^{\prime }$）

将（2-15）和（2-16）带入（2-2）可得
$$l^{\prime }=\frac{\overrightarrow{v_h}\cdot \overrightarrow{n_r}}{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}(1-k_{r}l)\text{\hspace{1em}(2-17)}$$

2.7 渐入佳境（求解$\ddot{s}$）

$$\begin{array}{rl}\ddot{s}& =\frac{d\dot{s}}{dt}\\ & =\frac{d}{dt}\left(\frac{\overrightarrow{v_h}\overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}\right)\\ & =\frac{1}{(1-{\kappa }_{r}l{)}^2}\left[(\dot{\overrightarrow{v_h}}\overrightarrow{{\tau }_r}+\overrightarrow{v_h}\dot{\overrightarrow{{\tau }_r}})(1-{\kappa }_{r}l)+\overrightarrow{v_h}\overrightarrow{{\tau }_r}(\dot{{\kappa }_r}l+{\kappa }_r\dot{l})\right]\\ & =\frac{1}{1-{\kappa }_{r}l}\left(\overrightarrow{a_h}\overrightarrow{{\tau }_r}+\overrightarrow{v_h}{\kappa }_r\dot{s}\overrightarrow{n_r}\right)+\frac{1}{(1-{\kappa }_{r}l)}\frac{\overrightarrow{v_h}\overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}\left(k_r^{\prime }\dot{s}l+{\kappa }_r{l}^{\prime }\dot{s}\right)\\ & =\frac{1}{1-{\kappa }_{r}l}\left(\overrightarrow{a_h}\overrightarrow{{\tau }_r}+\overrightarrow{v_h}{\kappa }_r\dot{s}\overrightarrow{n_r}\right)+\frac{1}{(1-{\kappa }_{r}l)}{\dot{s}}^2\left(k_r^{\prime }l+{\kappa }_r{l}^{\prime }\right)\\ & =\frac{\overrightarrow{a_h}\overrightarrow{{\tau }_r}+{\kappa }_r\overrightarrow{v_h}\overrightarrow{n_r}\dot{s}+\left(k_r^{\prime }l+{\kappa }_r{l}^{\prime }\right){\dot{s}}^2}{1-{\kappa }_{r}l}\\ & =\frac{\overrightarrow{a_h}\overrightarrow{{\tau }_r}+{\kappa }_r\dot{l}\dot{s}+\left(k_r^{\prime }l+{\kappa }_r{l}^{\prime }\right){\dot{s}}^2}{1-{\kappa }_{r}l}\end{array}\text{\hspace{1em}(2-18)}$$

又因为(2-2)，有

$$\ddot{s}=\frac{\overrightarrow{a_h}\overrightarrow{{\tau }_r}+(2{\kappa }_r{l}^{\prime }+{\kappa }^{\prime }l){\dot{s}}^2}{1-{\kappa }_{r}l}\text{\hspace{1em}(2-19)}$$

2.8 渐入佳境（求解$\ddot{l}$）

$$\begin{array}{rl}\ddot{l}& =\frac{d\dot{l}}{dt}\\ & =\frac{d}{dt}\left(\overrightarrow{v_h}\overrightarrow{n_r}\right)\\ & =\dot{\overrightarrow{v_h}}\overrightarrow{n_r}+\overrightarrow{v_h}\dot{\overrightarrow{n_r}}\\ & =\overrightarrow{a_h}\overrightarrow{n_r}-{\kappa }_r\dot{s}\overrightarrow{v_h}\overrightarrow{{\tau }_r}\end{array}\text{\hspace{1em}(2-20)}$$

又因为（2-15），有
$$\ddot{l}=\overrightarrow{a_h}\overrightarrow{n_r}-{\kappa }_r{\dot{s}}^2(1-l{\kappa }_r)\text{\hspace{1em}(2-21)}$$

2.9 鸣金收兵（求解$l^{\prime \prime }$）

由（2-3）和 （2-20）可得
$$\begin{array}{rl}{l}^{\prime \prime }& =\frac{\ddot{l}-l^{\prime }\ddot{s}}{{\dot{s}}^2}\\ & =\frac{\overrightarrow{a_h}\overrightarrow{n_r}-{\kappa }_r\dot{s}\overrightarrow{v_h}\overrightarrow{{\tau }_r}-l^{\prime }\ddot{s}}{{\dot{s}}^2}\end{array}\text{\hspace{1em}(2-22)}$$

3 总结

推导涉及的几个思路：

• 找等价关系，做恒等变形；
• 找不到关系，就用定义；
• 乘以线性无关向量进行消元；
• 链式法则求导；
• 先近似，再求极限。