Planning-Frenet坐标和Cartesian坐标转换上

本文根据B站up：忠厚老实的老王 的视频《自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换》整理，连接：https://www.bilibili.com/video/BV1tQ4y1r7fh?spm_id_from=333.1007.top_right_bar_window_history.content.click

Planning-Frenet坐标和Cartesian坐标转换中
Planning-Frenet坐标和Cartesian坐标转换下

1 基础知识

1.1 曲线坐标系和直角坐标系的两点不同

1.1.1 曲线坐标系的基向量一般不是常向量

在直角坐标系中，基向量为$\vec{i},\vec{j}$，则$\frac{d\vec{i}}{dx}=\vec{0}$，在曲线坐标系中，基向量$\vec{\tau }$的模$|\vec{\tau }|=1$，但是其方向会随着$s$的变化而变化，即$\frac{d\vec{\tau }}{ds}\ne \vec{0}$。

1.1.2 点的曲线坐标变化与点的实际位移一般不一致

在直角坐标系中一个点移动距离等于其在相应坐标轴上移动的距离，然而在曲线坐标系中则不一定相等，例如在直角坐标系中的点$(x,y)$移动到$(x+\Delta x,y)$，则在$x$上投影也移动了$\Delta x$的距离。曲线坐标系中的点$(s,l)$移动到点$(s+\Delta s_h,l)$，然而其投影在曲线上移动了$\Delta s_r$的距离，$\Delta s_h$和$\Delta s_r$一般不相等。 因此在曲线坐标系中的弧微分有两个：一是点实际运动曲线的弧微分$\frac{d}{d{s}_h}$，一是点在参考曲线(坐标系)上的弧微分$\frac{d}{d{s}_r}$。

在自动驾驶系统中，当车辆沿着参考线行驶时，点在车辆行驶轨迹上$s_h$对时间$t$的微分是车速，即$\frac{d{s}_h}{dt}=|\vec{v}|$，此点在参考线上投影的微分却于此不相等，即$\frac{d{s}_r}{dt}=\dot{s}$。

1.2 位矢的导数

轨迹上一个点的位置矢量为$\vec{r}$，方向为$\vec{\tau }$，沿轨迹移动了$ds$，位置矢量移动了$\Delta \vec{r}$，则$\vec{\dot{r}}=\frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\times \frac{ds}{dt}$，当$dt\to 0$时，$\frac{|\Delta \vec{r}|}{ds}\to 1$，方向趋近于轨迹在位置矢量$\vec{r}$处的切线方向。因此：
$$\vec{\dot{r}}=1\times \vec{\tau }\times \frac{ds}{dt}=|\vec{v}|\vec{\tau }=\vec{v}\text{\hspace{1em}(1-1)}$$

1.3 Frenet公式

向量$\vec{\tau }$是切线方向，向量$\vec{n}$是法线方向，有：
$$\{\begin{array}{l}\frac{d\vec{\tau }}{ds}=\kappa \vec{n}\\ \frac{d\vec{n}}{ds}=-\kappa \vec{\tau }\end{array}$$
对$\frac{d\vec{\tau }}{ds}=\kappa \vec{n}$的证明如下：

$$|d\vec{\tau }|=2\times 1\times sin(\frac{d\theta }{2})$$，当$ds\to 0$时，$d\vec{\tau }$的方向趋近于$\vec{\tau }$垂直方向，即$\vec{n}$的方向。因此：
$$\frac{d\vec{\tau }}{ds}=\frac{|d\vec{\tau }|}{ds}\times \vec{n}=\frac{2sin(\frac{d\theta }{2})}{ds}\times \vec{n}=\frac{d\theta }{ds}\times \vec{n}=\kappa \times \vec{n}$$

1.3 ADC在轨迹和道路几何上向量$\vec{\tau }$和$\vec{n}$的导数

在质点轨迹上：
$$\{\begin{array}{l}\overrightarrow{\dot{{\tau }_h}}=\frac{d\overrightarrow{{\tau }_h}}{dt}=\frac{d\overrightarrow{{\tau }_h}}{d{s}_h}\times \frac{d{s}_h}{dt}={\kappa }_h|\overrightarrow{v_h}|\overrightarrow{n_h}\\ \overrightarrow{\dot{n_h}}=\frac{d\overrightarrow{n_h}}{ds}=\frac{d\overrightarrow{n_h}}{d{s}_h}\times \frac{d{s}_h}{dt}=-{\kappa }_h|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}\end{array}\text{\hspace{1em}(1-2)}$$
在参考线轨迹上：
$$\{\begin{array}{l}\overrightarrow{\dot{{\tau }_r}}=\frac{d\overrightarrow{{\tau }_r}}{dt}=\frac{d\overrightarrow{{\tau }_r}}{d{s}_r}\times \frac{d{s}_r}{dt}={\kappa }_r\dot{s_r}\overrightarrow{n_r}\\ \overrightarrow{\dot{n_r}}=\frac{d\overrightarrow{n_r}}{dt}=\frac{d\overrightarrow{n_r}}{d{s}_r}\times \frac{d{s}_r}{dt}=-{\kappa }_r\dot{s_r}\overrightarrow{{\tau }_r}\end{array}\text{\hspace{1em}(1-3)}$$
已知$\vec{r},\vec{\tau },\vec{n},\kappa$，则$\vec{v}=\vec{\dot{r}}=|\vec{v}|\vec{\tau }$，
$$\vec{a}=\frac{d\vec{v}}{dt}=\frac{d|\vec{v}|\vec{\tau }}{dt}=\frac{d|\vec{v}|}{dt}\vec{\tau }+|\vec{v}|\frac{d\vec{\tau }}{dt}=|\vec{\dot{v}}|\vec{\tau }+|\vec{v}|\vec{\dot{\tau }}=|\vec{\dot{v}}|\vec{\tau }+|\vec{v}{|}^2\kappa \vec{n}\text{\hspace{1em}(1-4)}$$

Reference

[1] 自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换

[2] Frenet坐标系与Cartesian坐标系互转