自动驾驶之直角坐标与自然坐标转换

上一讲：b站老王 自动驾驶决策规划学习记录（三）
接着上一讲学习记录b站老王对自动驾驶规划系列的讲解

参考视频：

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

0 前言

龙格现象：高次多项式拟合可能会出现震荡，慎用高次多项式。

尽可能用分段低次多项式去拟合，而不是高次多项式。

在数学中，笛卡尔坐标系 (Cartesian coordinate system) 是一种正交坐标系，亦称为直角坐标系。

用向量法推导，降低推导难度

车：host_vehicle

问题：
已知车在Cartesian坐标系下的$\overrightarrow{r_h},\overrightarrow{v_h},\overrightarrow{a_h},k_h$，求车在以道路为坐标轴的frenet坐标下的坐标$s,\dot{s},\ddot{s},l,l{}^{\prime },l{}^{\prime \prime },\dot{l},\ddot{l}$
其中，$\dot{s}=\frac{dS}{dt},\dot{l}=\frac{dl}{dt},l{}^{\prime }=\frac{dl}{dS}$

在推导中，难度最大的就是曲线坐标系（别扭）。
曲线坐标系与直角坐标系的两点不同：

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

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

1 预备知识

1.1 预备知识1

拓展：

1.2 预备知识2

frenet公式

扩展1：

$l,l{}^{\prime },l{}^{\prime \prime }$中的$l{}^{\prime }=\frac{dl}{dS}$，$dS$描述的是frenet坐标系下坐标轴曲线的弧微分。

曲率$k_h,k_r$都只是在直角坐标系下的曲率

扩展2：

1.3 总结

以下变量都以Cartesian坐标为准
$\overrightarrow{r_n}$：车的位矢
$\vec{v}$：车的速度
$\vec{a}$：车的加速度
$k_h$：车的位矢在车的轨迹上的曲率
$\overrightarrow{{\tau }_h}$：车的位矢在车的轨迹上的切线方向单位向量
$\overrightarrow{n_h}$：车的位矢在车的轨迹上的法线方向单位向量
$\overrightarrow{r_r}$：投影位矢
$\dot{S}$：投影速率
$k_r$：投影位矢在道路几何上的曲率
$\overrightarrow{{\tau }_r}$：投影位矢在道路几何上的切线方向单位向量
$\overrightarrow{n_r}$：投影位矢在道路几何上的法线方向单位向量

求自然坐标系和直角坐标系之间转化的关键
七个辅助公式：

问题转化为：

分三步：

1. 7个辅助公式
2. 找到车在$frenet$坐标系下的投影点在Cartesion的坐标，记为$x_r,y_r,{\theta }_r,k_r$
   
3. 利用向量三角形，以及微积分求出$S,\dot{S},\ddot{S},l,l_{}^{\prime },l_{}^{\prime \prime }$
   核心公式：${\vec{r}}_r+l{\vec{n}}_r={\vec{r}}_h$

如何找到$(x_h,y_h)$在frenet坐标下的投影$x_r,y_r,{\theta }_r,k_r$暂时先不管，假设已找到了$x_r,y_r,{\theta }_r,k_r$，自然可得到${\vec{r}}_r,{\vec{\tau }}_r,{\vec{n}}_r,k_r$

博客的公式：

2 推导过程

1.如何找到$x_r,y_r,{\theta }_r,k_r$
2.如何计算S
3.如何从$S,\dot{S},\ddot{S},l,\dot{l},\ddot{l},l_{}^{\prime },l_{}^{\prime \prime }$到${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,k_h$

2.1 如何找到$x_r,y_r,{\theta }_r,k_r$

一般reference_line都是离散点，只涉及离散点的$x_r,y_r,{\theta }_r,k_r$

2.2 如何计算S

有其他更精确的算法，但是不稳定。以分段直线近似曲线虽然比较糙，但是对参数变化不敏感(robust)

2.3 如何从$S,\dot{S},\ddot{S},l,\dot{l},\ddot{l},l_{}^{\prime },l_{}^{\prime \prime }$到${\vec{r}}_h,{\vec{v}}_h,{\vec{a}}_h,k_h$