概述

在1.1和1.2的两个章节我们已经学习了笛卡尔坐标转换为Frenet坐标的推导过程，在自动驾驶的决策规划模块中，通过坐标转换我们从欧式空间变换到了SL自然空间，在这个空间下进行路径决策与计算，规划出的路径也是以自然坐标系的形式输出，这些路径点需要输出到控制模块进行路径跟踪，控制模块需要的是笛卡尔坐标系下的坐标形式，因此我们需要将自然坐标系的路径点转换为笛卡尔坐标。

推导过程

将自然坐标系转化为笛卡尔坐标系，可以将其描述为以下问题：

已知参考线，已知自然坐标系下一点的坐标$(s,l)，$以及$\dot{s}、\ddot{s}、\dot{l}、\ddot{l}、l{'}、l{''}$,求笛卡尔坐标下的位矢$\overrightarrow{r_t}$、速度$\overrightarrow{v_t}$、航向角$\theta_t$、加速度$\overrightarrow{a_t}$、加速度方向角$\theta_{at}$

在参考线上找到投影点b

对于给定的参考线，我们需要计算一个索引到弧长的哈希表，即参考线的第i个坐标序列对应的弧长是多少，$index2s[i] = s$

顺序遍历index2s中的每一个元素，当某索引$n$对应的弧长$s_n$满足$s_n<=s<=s_{n+1}$,那么投影点$b$就落在参考点$n$和参考点$n+1$之间，依据向量关系：

$s_n$点的方向向量为$\tau_n$，所以有：

$b$点的曲率$k_b$、曲率变化率$k{'}_b$、和x轴夹角$\theta_b$：

$b$点的单位切向量和单位法向量：

计算车辆点位矢

依据简单的向量关系

计算车辆点的速度和航向角

依据1.1中关于$\dot{s}$和$\dot{l}$的计算公式，我们可以进行反推：

(1)式和(2)式平方和相加得到速度大小，(2)式/(1)式得到航向角。

计算车辆点的加速度和加速度方位角

与推导速度的方法类似，依据1.1中关于$\ddot{s}$和$\ddot{l}$的计算公式，我们可以进行反推：

相关代码

计算index2s：

import numpy as np
import math

def index2s(host_x_prj=None, host_y_prj=None, host_match_index=None,
            reference_x_set=None, reference_y_set=None):
    """
    为了计算弧长, 将参考线的弧长与索引建立映射表, 以当前车辆的的投影点作为原点
    :param host_x_prj:
    :param host_y_prj:
    :param host_match_index:
    :param reference_x_set:
    :param reference_y_set:
    :return:
    """
    # 先计算以参考线起点为坐标的s
    index2s = np.array([((reference_x_set[i] - reference_x_set[i + 1]) ** 2 + (
                reference_y_set[i] - reference_y_set[i + 1]) ** 2) ** 0.5 for i in range(0, len(reference_x_set) - 1)]).cumsum()

    # 计算参考线起点到原点的弧长
    s0 = calc_s_to_origin(host_x_prj=host_x_prj, host_y_prj=host_y_prj,
                          host_match_index=host_match_index,
                          reference_x_set=reference_x_set, reference_y_set=reference_y_set, index2s=index2s)
    return index2s - s0


def calc_s_to_origin(host_x_prj=None, host_y_prj=None, host_match_index=None,
                     reference_x_set=None, reference_y_set=None, index2s=None):
    """
    计算 投影点 到 index2s的s原点 的弧长s
    :param host_x_prj:
    :param host_y_prj:
    :param host_match_index:
    :param reference_x_set:
    :param reference_y_set:
    :param index2s: 索引->弧长, 以参考线起点为原点
    :return:
    """
    # 先要判断投影点是在匹配点的前面还是后面

    # 投影点->匹配点向量
    prj_match_vec = np.array([reference_x_set[host_match_index] - host_x_prj,
                              reference_y_set[host_match_index - host_y_prj]])

    # 如果匹配点不是最后一个序列点, 则计算 投影点->匹配点后序点的向量
    # 匹配点前序点->匹配点的向量
    if host_match_index < len(reference_x_set) - 1:
        # 不是最后一个点
        vec_path = np.array([reference_x_set[host_match_index + 1] - reference_x_set[host_match_index],
                             reference_y_set[host_match_index + 1] - reference_y_set[host_match_index]])
        pass
    else:
        # 是最后一个点
        vec_path = np.array([reference_x_set[host_match_index] - reference_x_set[host_match_index - 1],
                             reference_y_set[host_match_index] - reference_y_set[host_match_index - 1]])

    if np.dot(prj_match_vec, vec_path) > 0:
        # 投影点在匹配点前序y
        s0 = index2s(host_match_index) - math.sqrt((host_x_prj - reference_x_set[host_match_index]) ** 2 +
                                                   (host_y_prj - reference_y_set[host_match_index]) ** 2)
    else:
        # 投影点在匹配点后序
        s0 = index2s(host_match_index) + math.sqrt((host_x_prj - reference_x_set[host_match_index]) ** 2 +
                                                   (host_y_prj - reference_y_set[host_match_index]) ** 2)
    return s0

坐标转换

import math
import numpy as np

def frenet_to_cartesian_single(index2s=None, s=None, l=None, s_dt=None, s_ddt=None, l_dt=None, l_ddt=None, l_ds=None,
                               l_dds=None, frenet_path_x=None, frenet_path_y=None,
                               frenet_path_heading=None, frenet_path_kappa=None):
    """
    将自然坐标系转化为笛卡尔坐标系
    :param index2s:
    :param s:
    :param l:
    :param s_dt:
    :param s_ddt:
    :param l_dt:
    :param l_ddt:
    :param l_ds:
    :param l_dds:
    :param frenet_path_x:
    :param frenet_path_y:
    :param frenet_path_heading:
    :param frenet_path_kappa:
    :return:
    """
    # 先找到sn
    n = -1
    for i in range(0, len(frenet_path_x) - 1):
        if index2s[i] <= s <= index2s[i + 1]:
            n = i
            break
        else:
            pass

    # 计算匹配点(投影点)的曲率\航向角\位置矢量
    if n == -1:
        # 说明投影点在最后一个参考线点的后面
        n = len(frenet_path_x) - 1
        match_kappa = frenet_path_kappa[n] / 2
        match_kappa_ds = 0
    else:
        match_kappa = (frenet_path_kappa[n] + frenet_path_kappa[n + 1]) / 2
        match_kappa_ds = (frenet_path_kappa[n + 1] - frenet_path_kappa[n]) / (index2s[n + 1] - index2s[n])

    match_heading = frenet_path_heading[n] + frenet_path_kappa[n] * (s - index2s[n])
    match_vec = np.array([frenet_path_x[n], frenet_path_y[n]]) + \
                (s - index2s[n]) * np.array([np.cos(match_heading),
                                             np.sin(match_heading)])

    # 计算轨迹点的位矢
    match_tor = np.array([np.cos(match_heading), np.sin(match_heading)])
    match_nor = np.array([-np.sin(match_heading), np.cos(match_heading)])
    host_vec = match_vec + l * match_nor
    host_x, host_y = host_vec[0], host_vec[1]

    # 计算轨迹点的速度, 航向角
    host_v = np.sqrt((1 - l * match_kappa) ** 2 * s_dt ** 2 + l_dt ** 2)
    host_heading = math.atan2(l_ds, 1 - l * match_kappa) + match_heading
    host_vx, host_vy = host_v * np.cos(host_heading), host_v * np.sin(host_heading)

    # 计算轨迹点的加速度以及加速度和x轴的夹角
    fz = l_ddt + match_kappa * (1 - l * match_kappa) * s_dt ** 2
    fm = s_ddt * (1 - match_kappa * l) - s_dt ** 2 * (2 * match_kappa * l_ds + match_kappa_ds * l)
    host_a = np.sqrt(fz ** 2 + fm ** 2)
    host_a_heading = math.atan2(fz, fm) + match_heading
    host_ax, host_ay = host_a * np.cos(host_a_heading), host_a * np.sin(host_a_heading)
    
    return host_x, host_y, host_vx, host_vy, host_heading, host_ax, host_ay, host_a_heading