参考资料

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）

1. Frenet坐标系介绍

1.1 Cartesian坐标系

一般情况下，我们使用Cartesian坐标系（笛卡尔坐标系）来描述物体的坐标，但对于车辆来说，笛卡尔坐标系并不是最佳选择。因为即使知道了笛卡尔坐标系下车辆的位置信息，也难以表达车辆与道路之间的相对位置，导致二者之间的相对关系不明确。
因此，传统规划算法在笛卡尔坐标系下规划出的路径对于开放道路有良好的效果，但是对于公路环境，忽略车道信息导致路径的自由度太高而容易违反道路交通规则。

1.2 Frenet坐标系

Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。

Frenet坐标系使用道路的中心线作为参考线，使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。

下图是自动驾驶车辆在全局坐标系与Frenet坐标系中的位置示意。

假设自动驾驶车辆在全局坐标系下的坐标为 $(x, y)$ ，从车辆的位置 $(x, y)$ 向参考线 $T$ 作投影，投影点为 $F$ ，则点 $F$ 与车辆位置 $(x, y)$ 的距离即为横向位移 $d$ (方向为参考线当前的法向，称为横向，Lateral)；从参考线的起始点到投影点 $F$ 的曲线距离即为纵向位移 $s$ （方向沿着参考线，称为纵向，Longitudinal）。

基于Frenet坐标系，将自动驾驶车辆每时每刻的位置状态分解在 $s$ 和 $d$ 两个方向来描述车辆的运动状态，从而在轨迹曲线拟合时，减少处理坐标信息的工作量。

1.3 Frenet公式

在向量微积分中，弗勒内-塞雷公式（Frenet–Serret 公式）用来描述欧几里得空间 $R^3$ 中的粒子在连续可微曲线上的运动。更具体的说，弗勒内公式描述了曲线的切向，法向，副法方向之间的关系。

下图是空间曲线的切向量 $T$ ，法向量 $N$ 和副法向量 $B$ ，以及切向量和法向量张成的密切平面。

单位切向量 $T$ ，单位法向量 $N$ ，单位副法向量 $B$ ，被称作 弗勒内标架，他们的具体定义如下：

$T$ 是单位切向量，方向指向粒子运动的方向。
$N$ 是切向量 $T$ 对弧长参数的微分单位化得到的向量。
$B$ 是 $T$ 和 $N$ 的外积 $T\times N$ 。

下图是螺旋线上弗勒内标架的运动。蓝色的箭头表示切向量，红色的箭头表示法向量，黑色的箭头表示副法向量。
在这里插入图片描述

Frenet公式如下：
$\tag{1.1} \left\{\begin{aligned} \frac{d T}{d s}&= \kappa N \\ \frac{d N}{d s}&= -\kappa T+\tau B \\ \frac{d B}{d s}&= -\tau N \end{aligned}\right.$
其中， $\frac{d(\cdot)}{d s}$ 表示是对弧长的微分， $\kappa$ 为曲率（curvature)， $\tau$ 为挠率 ( torsion)。弗勒内公式描述了空间曲线曲率挠率的变化规律。

曲率：曲线不能形成一条直线的度量值，曲率越趋于 0 ，则曲线越趋近于直线；
挠率：是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于 0 ，则曲线越趋近于在同一平面内运动。

将公式(1.1)写作矩阵的形式，有
$\tag{1.2} \left[\begin{array}{l} \mathbf{T}^{\prime} \\ \mathbf{N}^{\prime} \\ \mathbf{B}^{\prime} \end{array}\right]=\left[\begin{array}{ccc} 0 & \kappa & 0 \\ -\kappa & 0 & \tau \\ 0 & -\tau & 0 \end{array}\right]\left[\begin{array}{c} \mathbf{T} \\ \mathbf{N} \\ \mathbf{B} \end{array}\right]$
其中系数矩阵是反对称矩阵。

对于无人驾驶路径规划应用，一般对高度信息不感兴趣，因此可以将车辆运动曲线投影到同一平面内，挠率可设定为 0 ，这样Frenet公式可以进一步简化为:
$\tag{1.3} \left\{\begin{aligned} \frac{d T}{d s}&=\kappa N \\ \frac{d N}{d s}&=-\kappa T \end{aligned}\right.$

2. Frenet坐标系与全局笛卡尔坐标系转换

在Frenet坐标系下，经规划得到横向、纵向运动轨迹后，需要将其重新映射到全局笛卡尔坐标系，以供控制模块调用。

如图所示, 受车辆运动学、动力学特性及道路环境 (避障) 限制, 车辆的实际行驶轨迹与参考线难以重合。

图中主要参数说明

首先, $\mathrm{\vec{t}}, \mathrm{\vec{n}}$ 都是单位向量且有:
$\tag{1.4} \left\{ \begin{aligned} \vec{t}_{r} &=\left[\cos \theta_{r}, \sin \theta_{r}\right]^T \\ \vec{n}_{r} &=\left[-\sin \theta_{r}, \cos \theta_{r}\right]^T \\ \vec{t}_{x} &=\left[\cos \theta_{x}, \sin \theta_{x}\right]^T \\ \vec{n}_{x} &=\left[-\sin \theta_{x}, \cos \theta_{x}\right]^T \end{aligned} \right.$

$\theta_{r}:$ 表示参考线 $\vec{r}$ 的方向角
$\vec{n}_{r}$ : 表示r(s)单位法向量
$\vec{t}_{r}$ : 表示r(s)单位切向量
$\theta_{x}$ : 表示 $\vec{x}$ 的方向角
$\vec{n}_{x}$ : 表示 $\vec{x}$ 单位法向量
$\vec{t}_{x}$ : 表示 $\vec{x}$ 单位切向量

全局坐标系下

在全局坐标系下, 任意时刻 $t$ 的车辆运动状态可以描述为 $\left[\vec{x}, \theta_{x}, \kappa_{x}, v_{x}, a_{x}\right]$ 。

其中:

$\vec{x}$ 为车辆当前位置 $Q$ , 表示车辆在全局坐标系下的位置信息 $(x, y)$ , 是一个向量;
$\theta_{x}$ 为方位角，即全局坐标系下的朝向；
$\kappa_{x}$ 为曲率
$v_{x}=||\vec{\dot x}||$ 为Cartesian坐标系下的线速度;
$a_{x}=\frac{dv_x}{dt}$ 为加速度.
图中的 $\vec{r}(t)$ 为位置 $Q$ 投影到参考线上的投影点 $P$ 在全局坐标系下的位置向量。

Frenet坐标系下

在 Frenet坐标系下, 车辆的运动状态可以描述为 $\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right]$ .

其中:

$s$ 为纵向位移，即Frenet纵坐标；
$\dot{s}=\frac{ds}{dt}$ 为Frenet纵向速度；
$\ddot{s}=\frac{\dot{s}}{dt}$ 为Frenet纵向加速度；
$d$ 为横向位移, 即Frenet横坐标；
$\dot{d}$ 为Frenet横向速度；
$\ddot{d}$ 为Frenet横向加速度；
$d^{\prime}=\frac{\mathrm{d}}{\mathrm{d}s}(d)$ 为横向位移对纵向坐标的一阶导数,
$d^{\prime \prime}=\frac{\mathrm{d}}{\mathrm{d}s}(d^{\prime})$ 为横向位移对纵向坐标的二阶导数.

tips： $\dot{x}=\frac{dx}{dt}$ ， $x'=\frac{dx}{ds}$

由于正文篇幅字数的限制，这里只给出转换公式，具体推导见下一篇博客。

2.1 Cartesian转 Frenet公式

已知 $\theta_x, v_x, a_x, k_x), (s_r, x_r, y_r, \theta_r, k_r, k_r')$ 的前提下，求 $\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right]$ .

公式总结如下：
$\tag{1.5} \left\{ \begin{aligned} s&=s_r\\ \dot{s} &=\frac{v_x\cos{\Delta \theta}}{\left(1-\kappa_{r} d\right) }\\ \ddot{s}&=\frac{a_{x} \cos \Delta \theta-\dot{s}^{2}\left[d^{\prime}\left(k_{x} \frac{1-k_{r} d}{\cos \Delta \theta}-k_{r}\right)-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right]}{1-k_{r} d}\\ d&=\operatorname{sign}\left(\left(y-y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)\right) \sqrt{\left(x-x_{r}\right)^{2}+\left(y-y_{r}\right)^{2}}\\ \dot d &= v_x\sin{\Delta \theta}\\ \ddot d &= a_x\sin{\Delta \theta}\\ d^{\prime}&=\left(1-\kappa_{r} d\right) \tan \Delta \theta \\ d^{\prime \prime} &=-(\kappa_{r}^{\prime} d+\kappa_{r} d^{\prime}) \tan \Delta \theta+ \frac{1-\kappa_{r} d}{\cos ^{2} \Delta \theta}\left(\kappa_{x} \frac{1-\kappa_{r} d}{\cos \Delta \theta}-\kappa_{r}\right) \end{aligned} \right.$

python实现

import numpy as np
from math import *

def cartesian2Frenet(x,y,theta_x,v_x,a_x,k_x,s_r,x_r,y_r,theta_r,k_r,k_r_):
    """全局坐标系转Frenet坐标系

    Args:
        x (_type_): Cartesian坐标系下的车辆横坐标位置
        y (_type_): Cartesian坐标系下的车辆纵坐标位置
        theta_x (_type_): 为方位角，即全局坐标系下的朝向；
        v_x (_type_): Cartesian坐标系下的线速度大小;
        a_x (_type_): Cartesian坐标系下的加速度
        k_x (_type_): 曲率
        s_r (_type_): 投影点的弧长
        x_r (_type_): 投影点P点在Cartesian坐标系下的x坐标
        y_r (_type_): 投影点P点在Cartesian坐标系下的y坐标
        theta_r (_type_): 投影点P点在Cartesian坐标系下的朝向角
        k_r (_type_): 曲率
        k_r_ (_type_): 曲率对弧长s的一阶导数_

    Returns:
        _type_: Frenet坐标系下车辆的运动状态
    """
    delta_theta = theta_x-theta_r
    one_kr_d = 1-k_r*d
    s=s_r
    d=np.sign((y-y_r)*cos(theta_r)-(x-x_r)*sin(theta_r))*sqrt((x-x_r)**2+(y-y_r)**2)
    dot_d = v_x*sin(delta_theta)
    ddot_d = a_x*sin(delta_theta)
    dot_s=v_x*cos(delta_theta)/one_kr_d
    d_=one_kr_d*tan(delta_theta)
    d__=-(k_r_*d+k_r*d_)*tan(delta_theta)+one_kr_d/(cos(delta_theta))**2*(k_x*one_kr_d/cos(delta_theta)-k_r)
    ddot_s = (a_x*cos(delta_theta)-dot_s**2*(d_*(k_x*one_kr_d/cos(delta_theta)-k_r)-(k_r_*d+k_r*d_)))/one_kr_d

    return s,dot_s,ddot_s,d,dot_d,ddot_d,d_,d__

2.2 Frenet转Cartesian公式

首先，作为参考系，投影点P点在笛卡尔坐标系下的坐标,角度,切向量角度以及曲率都已知，即 $x_r, y_r, θ_r,k_r,k_r'$ 。Frenet坐标系下, 车辆的运动状态 $\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right]$ 也已知。求在全局坐标系下的 $\left[x,y, \theta_{x}, \kappa_{x}, v_{x}, a_{x}\right]$ 。

公式总结如下：
$\tag{1.6} \left\{\begin{aligned} x&= x_{r}-d\sin \left(\theta_{r}\right) \\ y&= y_{r}+d \cos \left(\theta_{r}\right) \\ \theta_{x}&= \arctan(\frac{d'}{1-k_r d})+\theta_r\\ v_{x}&= \sqrt{\left[\dot{s}\left(1-k_{r} d\right)\right]^{2}+\left(\dot{s} d^{\prime}\right)^{2}} \\ a_{x}&=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\frac{\dot{s}^{2}}{\cos \Delta \theta}\left[d^{\prime}\left(k_{x} \frac{1-k_{r} d}{\cos \Delta \theta}-k_{r}\right)-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right] \\ k_{x}&= \left(\left(d^{\prime \prime}+\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right) \tan \Delta \theta\right) \frac{\cos ^{2}\Delta \theta}{1-k_{r}d}+k_{r}\right) \frac{\cos \Delta \theta}{1-k_{r} d} \\ \end{aligned}\right.$

python实现

import numpy as np
from math import *

def frenet2Cartesian(s,dot_s,ddot_s,d,dot_d,ddot_d,d_,d__,x_r,y_r,theta_r,k_r,k_r_):
    """Frenet转Cartesian

    Args:
        s (_type_): 为纵向位移，即Frenet纵坐标；
        dot_s (_type_): Frenet纵向速 度
        ddot_s (_type_): Frenet纵向加速度,
        d (_type_): 横向位移, 即Frenet横坐标
        dot_d (_type_): Frenet横向速度
        ddot_d (_type_): Frenet横向加速度
        d_ (_type_): 横向位移对纵向坐标的一阶导数
        d__ (_type_): 横向位移对纵向坐标的二阶导数
        x_r (_type_): 投影点P点在Cartesian坐标系下的x坐标
        y_r (_type_): 投影点P点在Cartesian坐标系下的y坐标
        theta_r (_type_): 投影点P点在Cartesian坐标系下的朝向角
        k_r (_type_): 曲率
        k_r_ (_type_): 曲率对弧长s的一阶导数_

    Returns:
        _type_: _description_
    """
    x = x_r-d*sin(theta_r)
    y = y_r+d*cos(theta_r)
    one_kr_d= 1-k_r*d
    theta_x = theta_r+atan2(d_,one_kr_d)
    delta_theta = theta_x-theta_r
    v_x = sqrt((dot_s*one_kr_d)**2+(dot_s*d_)**2)
    k_x = cos(delta_theta)/one_kr_d*(k_r+(cos(delta_theta))**2/one_kr_d*(d__+(k_r_*d+k_r*d_)*tan(delta_theta)))
    a_x = ddot_s*one_kr_d/cos(delta_theta)+dot_s**2/cos(delta_theta)*(d_*(k_x*one_kr_d/cos(delta_theta)-k_r)-(k_r_*d+k_r*d_))
    
    return x,y,theta_x,v_x,a_x,k_x