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坐标转换下

2 坐标转换

2.1 变量说明：

$\overrightarrow{r_h}$	车的位置矢量	$\overrightarrow{r_h}$	投影的位置矢量
$\vec{v}$	车的速度	$\dot{s}$	投影的速率
$\vec{a}$	车的加速度	—	—
${\kappa }_h$	车的位置矢量在车轨迹上的曲率	${\kappa }_r$	投影位置矢量在道路几何上的曲率
$\overrightarrow{{\tau }_h}$	车的位置矢量在车轨迹上的切线方向的单位向量	$\overrightarrow{{\tau }_r}$	投影位置矢量在道路几何上的切线方向的单位向量
$\overrightarrow{n_h}$	车的位置矢量在车轨迹上的法线方向的单位向量	$\overrightarrow{n_r}$	投影位置矢量在道路几何上的法线方向的单位向量

2.2 基础公式

计算所需要的公式，即为上述$(1-1)—(1-4)$：
$$\{\begin{array}{l}①\overrightarrow{\dot{r_h}}=|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}=\overrightarrow{v_h}\\ ②\overrightarrow{\dot{r_r}}=\dot{s}\overrightarrow{{\tau }_r}\\ ③\overrightarrow{\dot{{\tau }_h}}=\frac{d\overrightarrow{{\tau }_h}}{dt}={\kappa }_h|\overrightarrow{v_h}|\overrightarrow{n_h}\\ ④\overrightarrow{\dot{n_h}}=-{\kappa }_h|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}\\ ⑤\overrightarrow{\dot{{\tau }_r}}={\kappa }_r\dot{s_r}\overrightarrow{n_r}\\ ⑥\overrightarrow{\dot{n_r}}=-{\kappa }_r\dot{s_r}\overrightarrow{{\tau }_r}\\ ⑦\vec{a}=|\overrightarrow{\dot{v_h}}|\overrightarrow{{\tau }_h}+|\overrightarrow{v_h}{|}^2{\kappa }_h\overrightarrow{n_h}\end{array}\text{\hspace{1em}(2-1)}$$

2.3 $Cartesian$坐标系转换到$Frenet$坐标系

已知$\overrightarrow{r_h},\overrightarrow{v_h},\overrightarrow{a_h},{\kappa }_h,\overrightarrow{{\tau }_h},\overrightarrow{n_h}$和$Frenet$坐标系起点$(x_0,y_0)$，求$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$。

2.3.1 计算ADC在Frenet坐标系中的投影点

需要根据车辆坐标$(x_h,y_h)$，在参考路径上找到 $x_r,y_r,{\theta }_r,{\kappa }_r$。首先找到距离车辆坐标$(x_h,y_h)$最近的点，称为匹配点 $x_m,y_m,{\theta }_m,{\kappa }_m$

根据匹配点和ADC坐标可以得到：

$$\{\begin{array}{l}\vec{d}=(x_h-x_m,y_h-y_m)\\ \overrightarrow{{\tau }_m}=(cos{\theta }_m,sin{\theta }_m)\end{array}\text{\hspace{1em}(2-2)}$$

将车辆坐标在曲线投影点处的$\overrightarrow{t_r}$近似为$\vec{\tau }$，可以得到：

$$\overrightarrow{r_r}=\left(\begin{array}{c}{x}_r\\ y_r\end{array}\right)=\overrightarrow{r_m}+(\vec{d}\cdot \overrightarrow{{\tau }_m})\overrightarrow{{\tau }_m}=\left(\begin{array}{c}{x}_m\\ y_m\end{array}\right)+(\vec{d}\cdot \overrightarrow{{\tau }_m})\left(\begin{array}{c}cos{\theta }_m\\ sin{\theta }_m\end{array}\right)\text{\hspace{1em}(2-3)}$$

其中的向量点乘可以转化为以下形式：
$$\vec{d}\cdot \overrightarrow{{\tau }_m}=|\vec{d}||\overrightarrow{{\tau }_m}|cos<{\theta }_h,{\theta }_r>=|\vec{d}|cos({\theta }_h-{\theta }_r)\text{\hspace{1em}(2-4)}$$

为了后续计算表达简明，车辆在$Frenet$坐标系下的投影点在$cartesian$坐标系下的坐标，记为$x_r,y_r,{\theta }_r,{\kappa }_r$，因此可以得到：$\overrightarrow{r_r}=(x_r,y_r),\overrightarrow{{\tau }_r}=(cos{\theta }_r,sin{\theta }_r),\overrightarrow{n_r}=(-sin{\theta }_r,cos{\theta }_r)$。

利用向量三角形和微积分求出$s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime },\dot{l},\ddot{l}$。

核心公式：$\overrightarrow{r_r}+l\times \overrightarrow{n_r}=\overrightarrow{r_h}$。

2.3.2 计算$l$

公式$\overrightarrow{r_r}+l\overrightarrow{n_r}=\overrightarrow{r_h}$两边点乘$\overrightarrow{n_r}$可得：
$$l=(\overrightarrow{r_h}-\overrightarrow{r_r})\cdot \overrightarrow{n_r}\text{\hspace{1em}(2-5)}$$
写成三角函数形式：
$$l=sign((y_h-y_r)cos({\theta }_r)-(x_h-x_r)sin({\theta }_r))\sqrt{(y_h-y_r{)}^2-(x_h-x_r{)}^2}\text{\hspace{1em}(2-6)}$$

3.3.3 计算$\dot{s}(\dot{s_r})$:

公式$\overrightarrow{r_r}+l\times \overrightarrow{n_r}=\overrightarrow{r_h}$两边同时对时间求导得：
$$\overrightarrow{\dot{r_r}}+l\overrightarrow{\dot{n_r}}+\dot{l}\overrightarrow{n_r}=\overrightarrow{\dot{r_h}}\text{\hspace{1em}(2-7)}$$
利用基础公式①②⑤⑥得：
$$\dot{s_r}\overrightarrow{{\tau }_r}+l(-{\kappa }_r\dot{s_r}\overrightarrow{{\tau }_r})+\dot{l}\overrightarrow{n_r}=\overrightarrow{v_h}\text{\hspace{1em}(2-8)}$$
两边同时点乘$\overrightarrow{{\tau }_r}$得：$\dot{s_r}+l(-{\kappa }_r\dot{s_r})=\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}$，得到：
$$\dot{s_r}=\frac{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}\text{\hspace{1em}(2-9)}$$
转化为三角函数形式：
$$vec{v}_h\cdot \overrightarrow{{\tau }_r}=|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}\cdot \overrightarrow{{\tau }_r}=|\overrightarrow{v_h}||\overrightarrow{{\tau }_h}||\overrightarrow{{\tau }_r}|cos<\overrightarrow{{\tau }_h},\overrightarrow{{\tau }_r}>=|\overrightarrow{v_h}|cos({\theta }_h-{\theta }_r)\text{\hspace{1em}(2-10)}$$
其中$|\overrightarrow{{\tau }_h}|=|\overrightarrow{{\tau }_r}|=1$。

因此，公式$(2-9)$可以等价写为：
$$\dot{s_r}=\frac{|\overrightarrow{v_h}|cos({\theta }_h-{\theta }_r)}{1-{\kappa }_{r}l}\text{\hspace{1em}(2-11)}$$

3.3.4 计算$\dot{l}$

由公式$(2-8)$：$\dot{s_r}\overrightarrow{{\tau }_r}+l(-{\kappa }_r\dot{s_r}\overrightarrow{{\tau }_r})+\dot{l}\overrightarrow{n_r}=\overrightarrow{v_h}$，两边同时点乘$\overrightarrow{n_r}$得到：
$$\dot{l}=\overrightarrow{v_h}\cdot \overrightarrow{n_r}\text{\hspace{1em}(2-12)}$$
写成三角函数形式有：
$$\dot{l}=\overrightarrow{v_h}\cdot \overrightarrow{n_r}=|\overrightarrow{v_h}||\overrightarrow{n_r}|cos<\overrightarrow{v_h},\overrightarrow{n_r}>=|\overrightarrow{v_h}|cos({\theta }_h-({\theta }_r+\frac{\pi }{2}))=|\overrightarrow{v_h}|sin({\theta }_h-{\theta }_r)\text{\hspace{1em}(2-13)}$$

3.3.5计算$l^{\prime }$

$$l^{\prime }=\frac{dl}{ds}=\frac{\frac{dl}{dt}}{\frac{d{s}_r}{dt}}=\frac{\dot{l}}{\dot{s_r}}=(1-{\kappa }_{r}l)\frac{\overrightarrow{v_h}\cdot \overrightarrow{n_r}}{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}\text{\hspace{1em}(2-14)}$$

写成三角函数形式：
$$l^{\prime }=(1-{\kappa }_{r}l)\frac{\overrightarrow{v_h}\cdot \overrightarrow{n_r}}{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}=(1-{\kappa }_{r}l)tan({\theta }_h-{\theta }_r)\text{\hspace{1em}(2-14)}$$

3.3.6 计算$\ddot{s}(\ddot{s_r})$

$$\begin{array}{rl}\ddot{s_r}& =\frac{d(\frac{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l})}{dt}\\ & =\frac{1}{(1-{\kappa }_{r}l{)}^2}(\frac{d(\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r})}{dt}(1-{\kappa }_{r}l)-(\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r})(-\dot{{\kappa }_r}l-{\kappa }_r\dot{l}))\\ & =\frac{1}{1-{\kappa }_{r}l}(\frac{d\overrightarrow{v_h}}{dt}\cdot \overrightarrow{{\tau }_r}+\overrightarrow{v_h}\cdot \frac{d\overrightarrow{{\tau }_r}}{dt})+\frac{1}{1-{\kappa }_{r}l}\frac{\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}(\dot{{\kappa }_r}l+{\kappa }_r\dot{l}))\\ & =\frac{1}{1-{\kappa }_{r}l}(\vec{a}\cdot \overrightarrow{{\tau }_r}+\overrightarrow{v_h}\cdot ({\kappa }_r\dot{s_r}\overrightarrow{n_r}))+\frac{1}{1-{\kappa }_{r}l}\dot{s_r}(\frac{d{\kappa }_r}{d{s}_r}\frac{d{s}_r}{dt}l+{\kappa }_r\frac{dl}{d{s}_r}\frac{d{s}_r}{dt})\\ & =\frac{\vec{a}\cdot \overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}+\frac{({\kappa }_r\dot{s_r})(\overrightarrow{v_h}\cdot \overrightarrow{n_r})}{1-{\kappa }_{r}l}+\frac{{\dot{s_r}}^2}{1-{\kappa }_{r}l}({{\kappa }_r}^{\prime }l+{\kappa }_r{l}^{\prime })\end{array}\text{\hspace{1em}(2-15)}$$

由于$\dot{l}=\overrightarrow{v_h}\cdot \overrightarrow{n_r},\dot{l}=\frac{dl}{ds}\frac{ds}{dt}=l^{\prime }\dot{s_r}$，因此：
$$\ddot{s_r}=\frac{\vec{a}\cdot \overrightarrow{{\tau }_r}}{1-{\kappa }_{r}l}+\frac{{\kappa }_r{\dot{s_r}}^2{l}^{\prime }}{1-{\kappa }_{r}l}+\frac{{\dot{s_r}}^2}{1-{\kappa }_{r}l}({{\kappa }_r}^{\prime }l+{\kappa }_r{l}^{\prime })\text{\hspace{1em}(2-16)}$$
根据基础公式⑦：$\vec{a}=|\overrightarrow{\dot{v_h}}|\overrightarrow{{\tau }_h}+|\overrightarrow{v_h}{|}^2{\kappa }_h\overrightarrow{n_h}$，由公式$(2-11)$，得:$|\overrightarrow{v_h}|=\frac{(1-{\kappa }_{r}l)\dot{s_r}}{cos({\theta }_h-{\theta }_r)}$，因此$\vec{a}\cdot \overrightarrow{{\tau }_r}$转化为三角函数形式：
$$\begin{array}{rl}\vec{a}\cdot \overrightarrow{{\tau }_r}& =|\overrightarrow{\dot{v_h}}|cos<\overrightarrow{{\tau }_h},\overrightarrow{{\tau }_r}>+|\overrightarrow{v_h}{|}^2{\kappa }_h\overrightarrow{n_h}\cdot \overrightarrow{{\tau }_r}\\ & =|\overrightarrow{\dot{v_h}}|cos<\overrightarrow{{\tau }_h},\overrightarrow{{\tau }_r}>+{\kappa }_h\frac{(1-{\kappa }_{r}l{)}^2}{co{s}^2({\theta }_h-{\theta }_r)}{\dot{s_r}}^2(-sin({\theta }_h-{\theta }_r))\\ & =|\overrightarrow{\dot{v_h}}|cos<\overrightarrow{{\tau }_h},\overrightarrow{{\tau }_r}>+{\kappa }_h{\dot{s_r}}^2\frac{1-{\kappa }_{r}l}{cos({\theta }_h-{\theta }_r)}(-(1-{\kappa }_{r}l)tan({\theta }_h-{\theta }_r))\\ & =|\overrightarrow{a_h}|cos({\theta }_h-{\theta }_r)+{\kappa }_h{\dot{s_r}}^2\frac{1-{\kappa }_{r}l}{cos({\theta }_h-{\theta }_r)}(-l^{\prime })\end{array}\text{\hspace{1em}(2-17)}$$

因此公式$(2-16)$的三角函数形式为：
$$\ddot{s_r}=\frac{|\overrightarrow{a_h}|cos({\theta }_h-{\theta }_r)-{\dot{s_r}}^2(l^{\prime }({\kappa }_h\frac{1-{\kappa }_{r}l}{cos({\theta }_h-{\theta }_r)}-{\kappa }_r)-({{\kappa }_r}^{\prime }l+{\kappa }_r{l}^{\prime }))}{1-{\kappa }_{r}l}\text{\hspace{1em}(2-18)}$$

3.3.7 计算$\ddot{l}$

$$\begin{array}{rl}\ddot{l}& =\frac{d\dot{l}}{dt}\\ & =\frac{d\overrightarrow{v_h}}{dt}\cdot \overrightarrow{n_r}+\overrightarrow{v_h}\cdot \frac{d\overrightarrow{n_r}}{dt}\\ & =\vec{a}\cdot \overrightarrow{n_r}+\overrightarrow{v_h}\cdot (-{\kappa }_r\dot{s_r}\overrightarrow{{\tau }_r})\\ & =\vec{a}\cdot \overrightarrow{n_r}-{\kappa }_r\dot{s_r}(\overrightarrow{v_h}\cdot \overrightarrow{{\tau }_r})\\ & =\vec{a}\cdot \overrightarrow{n_r}-{\kappa }_r(1-{\kappa }_{r}l)(\dot{s_r}{)}^2\end{array}\text{\hspace{1em}(2-19)}$$

$\vec{a}\cdot \overrightarrow{n_r}$的三角函数形式为：
$$\vec{a}\cdot \overrightarrow{n_r}=|\overrightarrow{\dot{v_h}}|cos<\overrightarrow{{\tau }_h},\overrightarrow{n_r}>=|\overrightarrow{\dot{v_h}}|cos({\theta }_h-({\theta }_r+\frac{\pi }{2}))=|\vec{a}|sin({\theta }_h-{\theta }_r)\text{\hspace{1em}(2-20)}$$

因此，$\ddot{l}$的三角函数形式为：
$$\ddot{l}=|\vec{a}|sin({\theta }_h-{\theta }_r)-{\kappa }_r(1-{\kappa }_{r}l)(\dot{s_r}{)}^2\text{\hspace{1em}(2-21)}$$

3.3.8 计算$l^{\prime \prime }$

$$\begin{array}{rl}\ddot{l}& =\frac{d\dot{l}}{dt}\\ & =\frac{d\dot{l^{\prime }\dot{s_r}}}{dt}\\ & =\frac{d{l}^{\prime }}{d{s}_r}\frac{d{s}_r}{dt}\dot{s_r}+l^{\prime }\ddot{s_r}\\ & =l^{\prime \prime }{\dot{s_r}}^2+l^{\prime }\ddot{s_r}\end{array}\text{\hspace{1em}(2-22)}$$

因此，可以得到：
$$l^{\prime \prime }=\frac{\ddot{l}-l^{\prime }\ddot{s_r}}{{\dot{s_r}}^2}\text{\hspace{1em}(2-23)}$$

写成三角函数形式：
$$l^{\prime \prime }=-({{\kappa }_r}^{\prime }l+{\kappa }_r{l}^{\prime })tan({\theta }_h-{\theta }_r)+\frac{1-{\kappa }_{r}l}{co{s}^2({\theta }_h-{\theta }_r)}(\frac{1-{\kappa }_{r}l}{cos({\theta }_h-{\theta }_r)}{\kappa }_h-{\kappa }_r)\text{\hspace{1em}(2-24)}$$

Reference

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

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