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

原创

已于 2022-03-18 11:33:37 修改 · 853 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#几何学 #算法 #线性代数

于 2022-03-18 11:26:41 首次发布

本文详细解析了自动驾驶中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 变量说明：

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

2.2 基础公式

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

2.3 $C a r t e s i a n$ 坐标系转换到 $F r e n e t$ 坐标系

已知 $\vec{r_h}, \vec{v_h}, \vec{a_h}, \kappa_h, \vec{\tau_h}, \vec{n_h}$ 和 $F r e n e t$ 坐标系起点 $x_0,y_0)$ ，求 $\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{cases} \vec{d} = (x_h - x_m, y_h - y_m) \\ \vec{\tau_m} = (cos{\theta_m}, sin{\theta_m}) \tag{2-2} \end{cases}$

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

$\vec{r_r} = \begin{pmatrix} x_r \\ y_r \end{pmatrix} = \vec{r_m} + (\vec{d} \cdot \vec{\tau_m}) \vec{\tau_m} = \begin{pmatrix} x_m \\ y_m \end{pmatrix} + (\vec{d} \cdot \vec{\tau_m}) \begin{pmatrix} cos{\theta_m} \\ sin{\theta_m} \end{pmatrix} \tag{2-3}$