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

Frenet转Cartesian

1 明确目标

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

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

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

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

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

• 位置向量：$\overrightarrow{r_h}$
• 速度向量：$\overrightarrow{v_h}$
• 切线方向与$x$轴夹角：${\theta }_h$
• 加速度向量：$\overrightarrow{a_h}$
• 曲率：${\kappa }_h$

2 公式推导

2.1 基础公式汇总

为了方便查阅，现将前文《01Frenet与Cartesian坐标系（基础知识）》推导的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 求解$\overrightarrow{r_h}$

根据（2-1）有：
$$\overrightarrow{r_h}=\overrightarrow{r_r}+l\overrightarrow{n_r}\text{\hspace{1em}(2-11)}$$

2.3 求解$\overrightarrow{v_h}$

$$\begin{array}{rl}\overrightarrow{v_h}& =\dot{\overrightarrow{r_h}}\\ & =\dot{\overrightarrow{r_r}}+\dot{l}\overrightarrow{n_r}+l\dot{\overrightarrow{n_r}}\\ & =s\overrightarrow{{\tau }_r}+\dot{l}\overrightarrow{n_r}-l{\kappa }_r\dot{s}\overrightarrow{{\tau }_r}\end{array}\text{\hspace{1em}(2-12)}$$

可以进一步求得
$$\begin{array}{rl}{v}_h& =|\overrightarrow{v_h}|\\ {\theta }_h& =arg(\overrightarrow{v_h})\end{array}\text{\hspace{1em}(2-13)}$$
其中，arg代表求向量的相位角。

求出了$\overrightarrow{v_h}$，可以进一步求得
$$\overrightarrow{{\tau }_h}=\frac{\overrightarrow{v_h}}{v_h}\text{\hspace{1em}(2-14)}$$
对于二维平面，
$$\begin{array}{rl}\overrightarrow{n_h}& =A\overrightarrow{{\tau }_h}\\ & =\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\overrightarrow{{\tau }_h}\end{array}\text{\hspace{1em}(2-15)}$$
其中，$A$ 是一个二维变换矩阵

Tips：如果是编码的话，（2-15）的乘法也可以直接通过调换坐标进行等价替换，可以进一步提升计算效率。例如，$\overrightarrow{{\tau }_h}=(\frac{x}{\sqrt{x^2+y^2}},\frac{y}{\sqrt{x^2+y^2}})$，则$\overrightarrow{n_h}=(\frac{-y}{\sqrt{x^2+y^2}},\frac{x}{\sqrt{x^2+y^2}})$。

2.4 求解$\overrightarrow{a_h}$

因为已经求得了$\overrightarrow{v_h}$，所以可以进一步求得$\overrightarrow{a_h}$,一种方法就是简单粗暴的对$\overrightarrow{v_h}$求导，即$\overrightarrow{a_h}=\dot{\overrightarrow{v_h}}$.我们这里选择另外一种更简便的方式.
根据前文Cartesian转Frenet的公式(2-19)和(2-21),有
$$\overrightarrow{a_h}\overrightarrow{{\tau }_r}=(1-{\kappa }_{r}l)\ddot{s}-(2{\kappa }_r{l}^{\prime }+{\kappa }_r^{\prime }l){\dot{s}}^2$$

$$\overrightarrow{a_h}\overrightarrow{n_r}={\kappa }_r{\dot{s}}^2(1-{\kappa }_{r}l)+\ddot{l}$$

根据单位正交基的定义可以进一步求得
$$\begin{array}{rl}\overrightarrow{a_h}=& [(1-{\kappa }_{r}l)\ddot{s}-(2{\kappa }_r{l}^{\prime }+{\kappa }_r^{\prime }l){\dot{s}}^2]\overrightarrow{{\tau }_r}+\\ & [{\kappa }_r{\dot{s}}^2(1-{\kappa }_{r}l)+\ddot{l}]\overrightarrow{n_r}\end{array}\text{\hspace{1em}(2-16)}$$
进一步可得

$$\begin{array}{rl}{a}_{\tau }& =\overrightarrow{a_h}\overrightarrow{{\tau }_h}\\ a_n& =\overrightarrow{a_h}\overrightarrow{n_h}\end{array}\text{\hspace{1em}(2-17)}$$

2.5 求解 ${\kappa }_h$

由（2-7）可得
$${\kappa }_h=\frac{a_n}{v_h^2}\text{\hspace{1em}(2-18)}$$

3 总结

• 还是坚持使用向量推导的一贯作风，不引入三角函数，会使得表达式更简洁；
• 编码时使用向量实现是否比三角函数更快还需要进一步实验（理论上会更快）。