Frenet坐标系和Cartesian坐标系的相互转换

2023.12.12

1 变量含义

• Frenet和Cartesian相互转换即$$[s,\dot{s},\ddot{s},d,\dot{d},\ddot{d}]\leftrightarrow [\mathbf{X},{\theta }_x,{\kappa }_x,v_x,a_x]$$

• 还要用到的中间变量有$${\mathbf{n}}_{\mathbf{x}},{\mathbf{t}}_{\mathbf{x}},{\mathbf{n}}_{\mathbf{r}},{\mathbf{t}}_{\mathbf{r}},\mathbf{r},d^{\prime },d^{\prime \prime },{\theta }_r,\Delta \theta ,{\kappa }_r,{\kappa }_x^{\prime },{\kappa }_r^{\prime }$$

• 各变量含义：

  变量	含义
  $$\mathbf{X}$$	Cartesian坐标系下的坐标($$\mathbf{X}=[x,y]$$)
  $$\mathbf{r}$$	trajectory上的点$$\mathbf{X}$$对应center line上最近点的Cartesian坐标
  $$v_x,a_x$$	Cartesian坐标系下的速度和加速度
  $$s,\dot{s},\ddot{s}$$	Frenet坐标系下的纵向坐标及其对时间的高阶导($$\frac{d}{dt}$$)
  $$s_x$$	trajectory的弧长(注：centerline弧长$$s_r$$默认写成$$s$$)
  $$d,\dot{d},\ddot{d}$$	Frenet坐标系下的横向坐标及其对时间的高阶导($$\frac{d}{dt}$$)
  $$d^{\prime },d^{\prime \prime }$$	Frenet坐标系下的横向坐标对纵向坐标的高阶导($$\frac{d}{ds}$$)
  $${\theta }_x,{\theta }_r$$	Cartesian坐标系下trajectory/center line的切线与x轴的夹角
  $${\theta }_r^{\prime }$$	$${\theta }_r^{\prime }=\frac{d{\theta }_r}{d{s}_r}={\kappa }_r$$
  $$\Delta \theta$$	$$\Delta \theta ={\theta }_x-{\theta }_r$$
  $${\mathbf{n}}_{\mathbf{x}},{\mathbf{t}}_{\mathbf{x}}$$	Cartesian坐标系下trajectory的法线和切线 $${\mathbf{n}}_{\mathbf{x}}=[-sin{\theta }_x,cos{\theta }_x{]}^T$$ $${\mathbf{t}}_{\mathbf{x}}=[cos{\theta }_x,sin{\theta }_x{]}^T$$
  $${\mathbf{n}}_{\mathbf{r}},{\mathbf{t}}_{\mathbf{r}}$$	Cartesian坐标系下center line的法线和切线 $${\mathbf{n}}_{\mathbf{r}}=[-sin{\theta }_r,cos{\theta }_r{]}^T$$ $${\mathbf{t}}_{\mathbf{r}}=[cos{\theta }_r,sin{\theta }_r{]}^T$$
  $${\kappa }_x,{\kappa }_r$$	Cartesian坐标系下trajectory/center line的曲率
  $${\kappa }_x^{\prime },{\kappa }_r^{\prime }$$	Cartesian坐标系下trajectory/center line的曲率对纵向坐标的导数($$\frac{d}{ds}$$)

2 推导

先来推导$\dot{d}$，由图像容易看出，$d=(\mathbf{x}-\mathbf{r}{)}^T{\mathbf{n}}_{\mathbf{r}}$，两边对t求导得：
$$\dot{d}=[\dot{x}-\dot{r}{]}^T{\mathbf{n}}_r+[x-r{]}^T{\dot{\mathbf{n}}}_r$$
$\dot{x}$很直观，$\dot{x}=v_x{\mathbf{t}}_{\mathbf{x}}$，由[变化量很小时，弧长=割线=切线]的思想，$\dot{r}=\dot{s}{\mathbf{t}}_{\mathbf{r}}$，最难求的是${\dot{\mathbf{n}}}_r$，由于${\mathbf{n}}_{\mathbf{r}}=[-sin{\theta }_r,cos{\theta }_r{]}^T$，两边对时间求导，得到${\dot{\mathbf{n}}}_r=[-\cos {\theta }_r,-sin{\theta }_r{]}^T\dot{{\theta }_r}=-{\mathbf{t}}_{\mathbf{r}}{\theta }_r^{\prime }\dot{s}=-{\mathbf{t}}_{\mathbf{r}}{\kappa }_r\dot{s}$，从而：
$$\begin{gathered} \dot{d}=v_x{{\mathbf{t}}_{\mathbf{x}}}^T{\mathbf{n}}_r-\dot{s}{{\mathbf{t}}_{\mathbf{r}}}^T{\mathbf{n}}_r-[x-r{]}^T{\mathbf{t}}_{\mathbf{r}}{\kappa }_r\dot{s} \\ =v_x{{\mathbf{t}}_{\mathbf{x}}}^T{\mathbf{n}}_r（{\mathbf{t}}_{\mathbf{r}},{\mathbf{n}}_r正交;[x-r],{\mathbf{t}}_{\mathbf{r}}正交） \\ =v_x[cos{\theta }_x,sin{\theta }_x][-sin{\theta }_r,cos{\theta }_r{]}^T \\ =v_x\sin \Delta \theta \end{gathered}$$
下面推导$d^{\prime }$：
$$d^{\prime }=\frac{dd}{ds}=\frac{dd}{dt}\frac{dt}{ds}=\frac{\dot{d}}{\dot{s}}=\frac{1}{\dot{s}}{v}_x\sin \Delta \theta$$
要把未知量 $\dot{s}$消掉，通过$v_x$来消：
$$\begin{gathered} v_x=||\dot{x}|{|}_2=||\frac{d(\mathbf{r}+d{\mathbf{n}}_r)}{dt}|{|}_2 \\ =\dot{s}{\mathbf{t}}_r+\dot{d}{\mathbf{n}}_r-d{\mathbf{t}}_{\mathbf{r}}{\kappa }_r\dot{s} \\ =||[{\mathbf{t}}_{\mathbf{r}},{\mathbf{n}}_{\mathbf{r}}]\left[\begin{array}{cc}1-{\kappa }_{r}d& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}\dot{s}\\ \dot{d}\end{array}\right]|{|}_2 \\ =\sqrt{(1-{\kappa }_{r}d{)}^2{\dot{s}}^2+{\dot{d}}^2} \\ =\sqrt{(1-{\kappa }_{r}d{)}^2{\dot{s}}^2+d^{\prime 2}{\dot{s}}^2} \end{gathered}$$
代入可得：
$$\begin{gathered} d^{\prime 2}=(\frac{1}{\dot{s}}{v}_x\sin \Delta \theta {)}^2=[(1-{\kappa }_{r}d{)}^2+d^{\prime 2}]{\sin }^2\Delta \theta \\ \downarrow \\ {d}^{\prime }(1-{\sin }^2\Delta \theta )=(1-{\kappa }_{r}d{)}^2{\sin }^2\Delta \theta \\ \downarrow \\ {d}^{\prime }=(1-{\kappa }_{r}d)\tan \Delta \theta \end{gathered}$$
下面推导$d^{\prime \prime }$，
$$\begin{gathered} d^{\prime \prime }=\frac{d\frac{dd}{ds}}{ds}=\frac{d{d}^{\prime }}{ds}=(-{\kappa }_r^{\prime }d-{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }\frac{d\Delta \theta }{ds} \\ =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }(\frac{d{\theta }_x}{ds}-\frac{d{\theta }_r}{ds}) \\ =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }(\frac{d{\theta }_x}{d{s}_x}\frac{d{s}_x}{ds}-{\kappa }_r) \\ =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }(\frac{d{\theta }_x}{d{s}_x}\frac{d{s}_x/dt}{ds/dt}-{\kappa }_r) \\ =-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }({\kappa }_x\frac{v_x}{\dot{s}}-{\kappa }_r) \end{gathered}$$

需要把未知量 $\dot{s}$消掉，由$0=(x-r{)}^T{\mathbf{t}}_r$两边对时间求导：
$$\begin{gathered} 0=(\dot{x}-\dot{r}{)}^T{\mathbf{t}}_r+(x-r{)}^T{\dot{\mathbf{t}}}_r \\ 0=v_x{\mathbf{t}}_x^T{\mathbf{t}}_r-\dot{s}{\mathbf{t}}_r^T{\mathbf{t}}_r+(x-r{)}^T{\mathbf{n}}_r{\kappa }_r\dot{s} \\ 0=v_x\cos \Delta \theta -\dot{s}(1-{\kappa }_{r}d) \\ \downarrow \\ {v}_x=\dot{s}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta } \end{gathered}$$
代入，得到：
$$d^{\prime \prime }=-({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan \Delta \theta +(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }({\kappa }_x\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }-{\kappa }_r)$$
由上上式也可以求得$\dot{s}$：
$$\dot{s}=v_x\frac{\cos \Delta \theta }{1-{\kappa }_{r}d}$$
最后来求$\ddot{s}$，由$a_x={\dot{v}}_x$：
$$\begin{gathered} a_x={\dot{v}}_x=\ddot{s}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }+\dot{s}\frac{ds}{dt}\frac{d}{ds}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta } \\ =\ddot{s}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }+{\dot{s}}^2[(-{\kappa }_r^{\prime }d-{\kappa }_r{d}^{\prime })\frac{1}{\cos \Delta \theta }+(1-{\kappa }_{r}d)\frac{1}{{\cos }^2\Delta \theta }\sin (\Delta \theta )\Delta {\theta }^{\prime }] \\ =\ddot{s}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }+{\dot{s}}^2\frac{1}{\cos (\Delta \theta )}[(-{\kappa }_r^{\prime }d-{\kappa }_r{d}^{\prime })+(1-{\kappa }_{r}d)\tan (\Delta \theta )\Delta {\theta }^{\prime }] \end{gathered}$$
其中：
$$\Delta {\theta }^{\prime }=\frac{d({\theta }_x-{\theta }_r)}{ds}=\frac{d{\theta }_x}{d{s}_x}\frac{d{s}_x}{ds}-{\kappa }_r={\kappa }_x\frac{v_x}{\dot{s}}-{\kappa }_r={\kappa }_x\frac{1-{\kappa }_{r}d}{cos\Delta \theta }-{\kappa }_r$$

3 Cartesian转Frenet

• $$[\mathbf{X},{\theta }_x,{\kappa }_x,v_x,a_x],{\theta }_r,{\kappa }_r\to [s,\dot{s},\ddot{s},d,\dot{d},\ddot{d}]$$

• s,d通过找最近点求得

• $$\dot{s}=v_x\frac{\cos (\Delta \theta )}{1-{\kappa }_{r}d},\Delta \theta ={\theta }_x-{\theta }_r$$

  $$\ddot{s}=\frac{\cos (\Delta \theta )}{1-{\kappa }_{r}d}[a_x-\frac{{\dot{s}}^2}{\cos (\Delta \theta )}[(-{\kappa }_r^{\prime }d-{\kappa }_r{d}^{\prime })+(1-{\kappa }_{r}d)\tan (\Delta \theta )\Delta {\theta }^{\prime }]]$$

  $${\kappa }_r^{\prime }=\frac{d^2{\theta }_r}{d{s}^2}$$

  $$d^{\prime }=(1-{\kappa }_{r}d)\tan \Delta \theta$$

  $$\Delta {\theta }^{\prime }={\kappa }_x\frac{1-{\kappa }_{r}d}{cos\Delta \theta }-{\kappa }_r,{\kappa }_x=\frac{d{\theta }_x}{dt}\frac{1}{v_x}$$

• $$\dot{d}=v_x\sin (\Delta \theta )$$

  $$\ddot{d}=a_x\sin (\Delta \theta )+v_x\cos (\Delta \theta )\dot{s}\Delta {\theta }^{\prime }$$

4 Frenet转Cartesian

• $$[s,\dot{s},\ddot{s},d,\dot{d},\ddot{d}]\to [\mathbf{X},{\theta }_x,{\kappa }_x,v_x,a_x]$$

• $$\mathbf{X}=\mathbf{r}+{\mathbf{n}}_{r}d,{\mathbf{n}}_r=[-\sin ({\theta }_r),\cos ({\theta }_r){]}^T$$

• $${\theta }_x={\theta }_r+\arctan \frac{d^{\prime }}{1-{\kappa }_{r}d},d^{\prime }=\frac{\dot{d}}{\dot{s}}$$

• $${\kappa }_x=\frac{{\cos }^3\Delta \theta [d^{\prime \prime }+({\kappa }_r^{\prime }d+{\kappa }_r{d}^{\prime })\tan (\Delta \theta )]}{1-{\kappa }_{r}d}+\frac{{\kappa }_{r}cos\Delta \theta }{1-{\kappa }_{r}d},d^{\prime \prime }=\frac{d\frac{dd}{ds}}{ds}=\frac{d\frac{\dot{d}}{\dot{s}}}{ds}=\frac{\ddot{d}\dot{s}-\dot{d}\ddot{s}}{{\dot{s}}^3}$$(由$d^{\prime \prime }$的式子得到)

• $$v_x=\dot{s}\frac{1-{\kappa }_{r}d}{\cos (\Delta \theta )},\Delta \theta ={\theta }_x-{\theta }_r$$

• $$a_x=\ddot{s}\frac{1-{\kappa }_{r}d}{\cos \Delta \theta }+{\dot{s}}^2\frac{1}{\cos (\Delta \theta )}[(-{\kappa }_r^{\prime }d-{\kappa }_r{d}^{\prime })+(1-{\kappa }_{r}d)\tan (\Delta \theta )\Delta {\theta }^{\prime }]$$

  $${\kappa }_r^{\prime }=\frac{d^2{\theta }_r}{d{s}^2}$$