【自动驾驶】Frenet坐标系与Cartesian坐标系（二）

参考资料

• Frenet坐标系与Cartesian坐标系互转
• Trajectory planning and optimization algorithm for automated driving based on Frenet coordinate system
• 轨迹规划1：Frenet坐标转化公式推导

2. Frenet坐标系与全局笛卡尔坐标系转换

---

【自动驾驶】Frenet坐标系与Cartesian坐标系（一）

---

阅读本篇博客之前，请先阅读之前的博客。（由于正文字数的限制，拆成了几篇博客来写。）

在Frenet坐标系下，经规划得到横向、纵向运动轨迹后，需要将其重新映射到全局笛卡尔坐标系，以供控制模块调用。

如图 所示, 受车辆运动学、动力学特性及道路环境 (避障) 限制, 车辆的实际行驶轨迹与参考线难以重合。

图中主要参数说明

首先, $\mathrm{t},\mathrm{n}$ 都是单位向量且有:
$$\{\begin{array}{rl}{t}_r& ={\left[\cos {\theta }_r,\sin {\theta }_r\right]}^T\\ n_r& ={\left[-\sin {\theta }_r,\cos {\theta }_r\right]}^T\\ t_x& ={\left[\cos {\theta }_x,\sin {\theta }_x\right]}^T\\ n_x& ={\left[-\sin {\theta }_x,\cos {\theta }_x\right]}^T\end{array}\text{\hspace{1em}(1.4)}$$

• ${\theta }_r:$ 表示参考线 $r$ 的方向角
• $n_r$ : 表示$r$的单位法向量
• $t_r$ : 表示$r$的单位切向量
• ${\theta }_x$ : 表示 $x$的方向角
• $n_x$ : 表示 $x$的单位法向量
• $t_x$ : 表示 $x$ 的单位切向量
• $\Delta \theta ={\theta }_x-{\theta }_r$：为向量 ($x-r$) 的方向角度

全局坐标系下

在全局坐标系下, 任意时刻 $t$ 的车辆运动状态可以描述为 $\left[x,{\theta }_x,{\kappa }_x,v_x,a_x\right]$。

其中:

• $x$ 为车辆当前位置 $Q$, 表示车辆在全局坐标系下的位置信息 $(x,y)$, 是一个向量;
• ${\theta }_x$ 为方位角，即全局坐标系下的朝向；
• ${\kappa }_x$ 为曲率
• $v_x=||\dot{x}||$ 为Cartesian坐标系下的线速度;
• $a_x=\frac{d{v}_x}{dt}$ 为加速度.
• 图中的$r(t)$ 为位置 $Q$ 投影到参考线上的投影点 $P$ 在全局坐标系 下的位置向量。

Frenet坐标系下

在 Frenet坐标系下, 车辆的运动状态可以描述为 $\left[s,\dot{s},\ddot{s},d,\dot{d},\ddot{d},d^{\prime },d^{\prime \prime }\right]$.

其中:

• $s$ 为纵向位移，即Frenet纵坐标；
• $\dot{s}=\frac{ds}{dt}$ 为Frenet纵向速 度；
• $\ddot{s}=\frac{\dot{s}}{dt}$ 为Frenet纵向加速度,；
• $d$ 为横向位移, 即Frenet横坐标；
• $\dot{d}$ 为Frenet横向速度；
• $\ddot{d}$ 为Frenet横向加速度；
• $d^{\prime }=\frac{\mathrm{d}}{\mathrm{d}s}(d)$ 为横向位移对纵向坐标的一阶导数,
• $d^{\prime \prime }=\frac{\mathrm{d}}{\mathrm{d}s}(d^{\prime })$ 为 横向位移对纵向坐标的二阶导数.

tips：$\dot{x}=\frac{dx}{dt}$，$x^{\prime }=\frac{dx}{ds}$

2.1 Cartesian转 Frenet

已知 $(x,y,{\theta }_x,v_x,a_x,k_x),(s_r,x_r,y_r,{\theta }_r,k_r,k_r^{\prime })$,求 $\left[s,\dot{s},\ddot{s},d,\dot{d},\ddot{d},d^{\prime },d^{\prime \prime }\right]$.

1. 求 $s$

首先，找到曲线上离位置$(x,y)$最近的参考点$r=(x_r,y_r)$（即将位置$Q$投影到参考线上的投影点$P$），该参考点处的 $s_r$即为 $(x,y)$在Frenet坐标系下的 $s$。

2. 求 $d$

$x$和$r$分别为位置$Q$与投影点$P$在Cartesian坐标系下的向量，他们满足
$$x=r+d{n}_r\text{\hspace{1em}(2.1)}$$
变换可得
$$\begin{array}{rl}d& =(x-r{)}^T\cdot n_r\\ & =||x-r|{|}_2\cos (\Delta \theta -({\theta }_r+\frac{\pi }{2}))\\ & =||x-r|{|}_2\sin (\Delta \theta -{\theta }_r)\\ & =||x-r|{|}_2(\sin \Delta \theta \cos {\theta }_r-\cos \Delta \theta \sin {\theta }_r)\end{array}\text{\hspace{1em}(2.2)}$$
上式中， $\Delta \theta ={\theta }_x-{\theta }_r$为向量 $x-r$ 的方向角度（即上图中的$\Delta \theta$），$n_r$为参考点$P$的单位法向量，${\theta }_r+\frac{\pi }{2}$​为单位向量 $n_r$​的方向角度。

设$||d||$为车辆当前 位置 $Q$ 与投影点 $P$ 之间的距离
$$||d||=\sqrt{{\left(x-x_r\right)}^2+{\left(y-y_r\right)}^2}.\text{\hspace{1em}(2.3)}$$

在Frenet坐标系下，每个点到参考线上参考点的向量都与该参考点的法向量 $n_r$ 同向或反向（即$d$与$n_r$在同一条线上），因此，
$$\sin \left(\Delta \theta \right)\cos \left({\theta }_r\right)-\cos \left(\Delta \theta \right)\sin \left({\theta }_r\right)=\pm 1\text{\hspace{1em}(2.4)}$$

因为$\Delta \theta$ 为向量 $x-r$ 的角度，因此，
$$\frac{\sin \left(\Delta \theta \right)}{\cos \left(\Delta \theta \right)}=\frac{y-y_r}{x-x_r}$$
可以根据 $||d||$ 来确定 $d$ 的大小， $(y-$ $y_r)\cos \left({\theta }_r\right)-\left(x-x_r\right)\sin \left({\theta }_r\right)$ 的正负来确定 $d$ 的正负号，$(y-$ $y_r)\cos \left({\theta }_r\right)-\left(x-x_r\right)\sin \left({\theta }_r\right)>0$，$d$ 取正号，否则$d$取负号。公式如下:

$$d=\mathrm{sign}\left(\left(y-y_r\right)\cos \left({\theta }_r\right)-\left(x-x_r\right)\sin \left({\theta }_r\right)\right)\sqrt{{\left(x-x_r\right)}^2+{\left(y-y_r\right)}^2}\text{\hspace{1em}(2.5)}$$

至于为什么是$(y-$ $y_r)\cos \left({\theta }_r\right)-\left(x-x_r\right)\sin \left({\theta }_r\right)$ 这一项来判断符号。很简单，因为$d=(x-x_r,y-y_r)$，$n_r={\left[-\sin {\theta }_r,\cos {\theta }_r\right]}^T$，两者的夹角公式为$\cos {\theta }_{dn}=\frac{\left(y-y_r\right)\cos \left({\theta }_r\right)-\left(x-x_r\right)\sin \left({\theta }_r\right)}{\sqrt{(x-x_r{)}^2+(y-y_r{)}^2}}$，显然由分子这一项判断。

3. 求 $\dot{d}$

令 $t_x、n_x$ 为点 $Q$ 处的正交单位向量; $t_r、n_r$ 为 参考线在投影点 $P$ 处的正交单位向量。

根据 $v_x$​与 $\dot{s}$定义，因为$x=||x||t_x$，所以
$$\dot{x}=\frac{dx}{dt}=\frac{d||x||}{dt}{t}_x=v_x{t}_x$$

同理可以推导出$\dot{r}=\dot{s}{t}_r$.

综上，有
$$\{\begin{array}{r}\dot{r}=\dot{s}{t}_r\\ \dot{x}=v_x{t}_x\end{array}\text{\hspace{1em}(3.1)}$$

根据公式(2.2)，横向速度 $\dot{d}$ 为
$$\begin{array}{rl}\dot{d}& =(\dot{x}-\dot{r}{)}^{\mathrm{T}}\cdot n_r+(x-r{)}^{\mathrm{T}}\cdot {\dot{n}}_r\\ & =v_x{t_x}^T{n}_r-\dot{s}{t_r}^T{n}_r+d{n_r}^T{\dot{n}}_r\end{array}\text{\hspace{1em}(3.2)}$$

由Frenet公式(1.3)，有
$${n_r}^{\prime }=\frac{d{n}_r}{ds}=-{\kappa }_r{t}_r\text{\hspace{1em}(3.3)}$$
于是有
$$\dot{n_r}=\frac{ds}{dt}\frac{d{n}_r}{ds}=-{\kappa }_r\dot{s}{t}_r\text{\hspace{1em}(3.4)}$$
代入公式(3.2)可得：
$$\begin{array}{rl}\dot{d}& =v_x{t_x}^T{n}_r-\dot{s}{t_r}^T{n}_r+d{n_r}^T{\dot{n}}_r\\ & =v_x{t_x}^T{n}_r-\dot{s}{t_r}^T{n}_r-d\dot{s}{\kappa }_r{n_r}^T{t}_r\end{array}\text{\hspace{1em}(3.5)}$$

由于 $n_r$与 $t_r$正交, 可得
$${n_r}^T{t}_r={t_r}^T{n}_r=0\text{\hspace{1em}(3.6)}$$

又因为
$${t_x}^T{n}_r=||{t_x}^T||\cdot ||n_r||\cos ({\theta }_x-({\theta }_r+\frac{\pi }{2}))=\cos ({\theta }_x-{\theta }_r-\frac{\pi }{2})=\sin ({\theta }_x-{\theta }_r)=\sin \Delta \theta \text{\hspace{1em}(3.7)}$$

故公式(3.5)化简后，$\dot{d}$最终为

$$\dot{d}=v_x\sin \Delta \theta \text{\hspace{1em}(3.8)}$$

4. 求 $\dot{s}$

对公式(2.1)的$x$两边对时间$t$求导，结合公式(3.1)和(3.4)可得
$$\begin{array}{rl}\dot{x}& =\frac{\mathrm{d}}{\mathrm{d}t}\left(r+d\cdot n_r\right)\\ \Rightarrow v_x{t}_x& =\dot{r}+\dot{d}\cdot n_r+d\cdot \dot{n_r}\\ & =\dot{s}\left(1-{\kappa }_{r}d\right)\cdot t_r+\dot{d}\cdot n_r\end{array}\text{\hspace{1em}(4.1)}$$

公式(4.1)右乘$t_r$，且由正交性质得到
$$\begin{array}{rl}{v}_x{t}_x\cdot t_r& =\dot{s}\left(1-{\kappa }_{r}d\right)\cdot t_r\cdot t_r+\dot{d}\cdot n_r\cdot t_r\\ & \Downarrow \\ v_x\cos ({\theta }_x-{\theta }_r)& =\dot{s}\left(1-{\kappa }_{r}d\right)\\ & \Downarrow \\ v_x\cos (\Delta \theta )& =\dot{s}\left(1-{\kappa }_{r}d\right)\end{array}\text{\hspace{1em}(4.2)}$$

故$\dot{s}$为

$$\dot{s}=\frac{v_x\cos \Delta \theta }{\left(1-{\kappa }_{r}d\right)}\text{\hspace{1em}(4.3)}$$

5. 求 $d^{\prime }$

因为
$$d^{\prime }=\frac{\mathrm{d}}{\mathrm{d}s}(d)=\frac{\mathrm{d}t}{\mathrm{d}s}\frac{\mathrm{d}}{\mathrm{d}t}(d)=\frac{\dot{d}}{\dot{s}}\text{\hspace{1em}(5.1)}$$

将公式(3.8)和(4.3)分别求得的$\dot{d}和\dot{s}$代入，化简后可得

$$d^{\prime }=\left(1-{\kappa }_{r}d\right)\tan \Delta \theta \text{\hspace{1em}(5.2)}$$

6. 求 $d^{\prime \prime }$

由式(5.2) 可得
$$\begin{array}{rl}{d}^{\prime \prime }& =\frac{\mathrm{d}}{\mathrm{d}s}\left(d^{\prime }\right)\\ & ={\left(1-{\kappa }_{r}d\right)}^{{}^{\prime }}\tan \Delta \theta +\left(1-{\kappa }_{r}d\right)(\tan \Delta \theta {)}^{{}^{\prime }}\\ & ={\left(1-{\kappa }_{r}d\right)}^{{}^{\prime }}\tan \Delta \theta +\left(1-{\kappa }_{r}d\right)\frac{(\Delta \theta {)}^{\prime }}{{\cos }^2(\Delta \theta )}\end{array}\text{\hspace{1em}(6.1)}$$

设 $s_x$​为车辆当前轨迹 $x$的弧长，结合公式(4.3)，有：
$$\frac{\mathrm{d}(\cdot )}{\mathrm{d}s}=\frac{\mathrm{d}{s}_x}{\mathrm{d}t}\frac{\mathrm{d}t}{\mathrm{d}s}\frac{\mathrm{d}(\cdot )}{\mathrm{d}{s}_x}=\frac{v_x}{\dot{s}}\frac{\mathrm{d}(\cdot )}{\mathrm{d}{s}_x}=\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }\frac{\mathrm{d}(\cdot )}{\mathrm{d}{s}_x}\text{\hspace{1em}(6.2)}$$

又根据曲率的定义:
$$\{\begin{array}{r}{\kappa }_r={\theta }_r^{{}^{\prime }}=\frac{\mathrm{d}{\theta }_r}{\mathrm{d}s}\\ {\kappa }_x={\theta }_x^{{}^{\prime }}=\frac{\mathrm{d}{\theta }_x}{\mathrm{d}{s}_x}\end{array}\text{\hspace{1em}(6.3)}$$

注意到 $\Delta \theta ={\theta }_x-{\theta }_r$，故结合公式(6.2)和(6.3)有
$$\begin{array}{rl}(\Delta \theta {)}^{\prime }& =\frac{\mathrm{d}}{\mathrm{d}s}({\theta }_x-{\theta }_r)\\ & =\frac{\mathrm{d}}{\mathrm{d}s}{\theta }_x-\frac{\mathrm{d}}{\mathrm{d}s}{\theta }_r\\ & =\frac{\mathrm{d}}{\mathrm{d}s}{\theta }_x-{\kappa }_r\\ & =\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }\frac{\mathrm{d}{\theta }_x}{\mathrm{d}{s}_x}-{\kappa }_r\\ & ={\kappa }_x\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }-{\kappa }_r\end{array}\text{\hspace{1em}(6.4)}$$

对于${\left(1-{\kappa }_{r}d\right)}^{{}^{\prime }}$，有
$$\begin{array}{rl}{\left(1-{\kappa }_{r}d\right)}^{{}^{\prime }}& =\frac{\mathrm{d}}{\mathrm{d}s}(-{\kappa }_{r}d)\\ & =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\end{array}\text{\hspace{1em}(6.5)}$$
故公式(6.1)最终化简得

$$\begin{array}{rl}{d}^{\prime \prime }& =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +\frac{1-{\kappa }_{r}d}{{\cos }^2\Delta \theta }\left({\kappa }_x\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }-{\kappa }_r\right)\end{array}\text{\hspace{1em}(6.6)}$$

7. 求 $\ddot{s}$

因为$a_x=\dot{v_x}=\frac{\mathrm{d}{v}_x}{\mathrm{d}t}$​​，故根据公式(4.3)以及公式(6.2)、(6.3)有

$$\begin{array}{rl}{a}_x& =\frac{\mathrm{d}{v}_x}{\mathrm{d}t}\\ & =\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\dot{s}(1-k_{r}d)}{\cos \Delta \theta }\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+\dot{s}\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{1-k_{r}d}{\cos \Delta \theta }\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+\dot{s}\frac{\mathrm{d}s}{\mathrm{d}t}\frac{\mathrm{d}}{\mathrm{d}s}\left(\frac{1-k_{r}d}{\cos \Delta \theta }\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+{\dot{s}}^2\frac{\mathrm{d}}{\mathrm{d}s}\left(\frac{1-k_{r}d}{\cos \Delta \theta }\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+{\dot{s}}^2\left(\frac{(1-k_{r}d{)}^{{}^{\prime }}\cos \Delta \theta -(1-k_{r}d)(\cos \Delta \theta {)}^{{}^{\prime }})}{(\cos {\Delta \theta )}^2}\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+{\dot{s}}^2\left(\frac{-(k_r^{{}^{\prime }}d+k_r{d}^{{}^{\prime }})}{\cos \Delta \theta }+\frac{(1-k_{r}d)\sin (\Delta \theta )(\Delta \theta {)}^{{}^{\prime }}}{(\cos \Delta \theta {)}^2}\right)\\ & =\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+\frac{{\dot{s}}^2}{\cos \Delta \theta }\left((1-k_{r}d)\tan \Delta \theta (\Delta \theta {)}^{{}^{\prime }}-\left(k_r^{\prime }d+k_r{d}^{\prime }\right)\right)\end{array}\text{\hspace{1em}(7.1)}$$
最后，将公式(5.2)和(6.4)代入，得

$$a_x=\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+\frac{{\dot{s}}^2}{\cos \Delta \theta }\left[d^{\prime }\left(k_x\frac{1-k_{r}d}{\cos \Delta \theta }-k_r\right)-\left(k_r^{\prime }d+k_r{d}^{\prime }\right)\right]\text{\hspace{1em}(7.2)}$$

故$\ddot{s}为$

$$\ddot{s}=\frac{a_x\cos \Delta \theta -{\dot{s}}^2\left[d^{\prime }\left(k_x\frac{1-k_{r}d}{\cos \Delta \theta }-k_r\right)-\left(k_r^{\prime }d+k_r{d}^{\prime }\right)\right]}{1-k_{r}d}\text{\hspace{1em}(7.3)}$$

8. 求$\ddot{d}$

根据公式(3.8)，易得$\ddot{d}$为

$$\ddot{d}=a_x\sin \Delta \theta \text{\hspace{1em}(8.1)}$$

再将公式(7.2)得到的$a_x$代入即可。

2.2 Frenet转Cartesian

首先，作为参考系，投影点P点在笛卡尔坐标系下的坐标,角度,切向量角度以及曲率都已知，即$(x_r,y_r),{\theta }_r,k_r$。Frenet坐标系下, 车辆的运动状态 $\left[s,\dot{s},\ddot{s},d,\dot{d},\ddot{d},d^{\prime },d^{\prime \prime }\right]$也已知。

10. 求$x$

先推$x=(x,y)$。

显然，结合公式(1.4)有
$$\begin{array}{rl}x& =r+d{n}_r\\ & \Downarrow \\ (x,y)& =(x_r,y_r)+(-d\sin {\theta }_r,d\cos {\theta }_r)\end{array}\text{\hspace{1em}(10.1)}$$

故

$$\{\begin{array}{rl}x& =x_r-d\sin \left({\theta }_r\right)\\ y& =y_r+d\cos \left({\theta }_r\right)\end{array}\text{\hspace{1em}(10.2)}$$

11. 求$v_x$

$$v_x=\frac{d||x||}{dt}=||\frac{dx}{dt}||=||\dot{x}||=\sqrt{{\dot{x}}^T\dot{x}}\text{\hspace{1em}(11.1)}$$

因为

$$\dot{x}=\frac{\mathrm{d}(r+d{n}_r)}{\mathrm{d}t}=\dot{r}+\frac{\mathrm{d}(d)}{\mathrm{d}t}{n}_r+d\frac{\mathrm{d}{n}_r}{\mathrm{d}t}=\dot{r}+\frac{\mathrm{d}(d)}{\mathrm{d}t}{n}_r+d{\dot{n}}_r\text{\hspace{1em}(11.2)}$$
结合公式(3.1)和公式(3.4)，有
$$\dot{x}=\dot{s}{t}_r+\frac{\mathrm{d}(d)}{\mathrm{d}t}{n}_r-{\kappa }_r\dot{s}{t}_r=\dot{s}(1-{\kappa }_{r}d)t_r+\dot{d}{n}_r\text{\hspace{1em}(11.3)}$$

将公式(11.3)代入公式(11.1)，并且注意$t_r$和$n_r$是单位正交的，有:
$$\begin{array}{rl}{v}_x& =\sqrt{{\dot{x}}^T\dot{x}}\\ & =\sqrt{\{\dot{s}(1-{\kappa }_{r}d)t_r+\dot{d}{n}_r{\}}^T\{\dot{s}(1-{\kappa }_{r}d)t_r+\dot{d}{n}_r\}}\\ & =\sqrt{\{\dot{s}(1-{\kappa }_{r}d){t_r}^T+\dot{d}{n_r}^T\}\{\dot{s}(1-{\kappa }_{r}d)t_r+\dot{d}{n}_r\}}\\ & =\sqrt{{\dot{s}}^2(1-{\kappa }_r{)}^2+{\dot{d}}^2}\end{array}\text{\hspace{1em}(11.4)}$$

故最终

$$\begin{array}{rl}{v}_x& =\sqrt{{\dot{s}}^2(1-{\kappa }_r{)}^2+{\dot{d}}^2}\\ & =\sqrt{{\left[\dot{s}\left(1-k_{r}d\right)\right]}^2+{\left(\dot{s}{d}^{\prime }\right)}^2}\end{array}\text{\hspace{1em}(11.5)}$$

12. 求${\theta }_x$

${\theta }_x$由公式(5.2)即得

$${\theta }_x=\arctan (\frac{d^{\prime }}{1-k_{r}d})+{\theta }_r\text{\hspace{1em}(12.1)}$$

13. 求$a_x$

$a_x$的求解过程已在前面的过程给出，即公式(7.2)

$$a_x=\ddot{s}\frac{1-k_{r}d}{\cos \Delta \theta }+\frac{{\dot{s}}^2}{\cos \Delta \theta }[d^{}$$