回顾笛卡尔坐标->frenet坐标的转换公式

博文链接： juejin.cn/post/713416…

l=\overrightarrow{n_b}(\overrightarrow{r_t}-\overrightarrow{r_b})\\

\dot{l} = \overrightarrow{v_t}·\overrightarrow{n_b}\\

\ddot{l}=\overrightarrow{a_t}·\overrightarrow{n_b}-k_b(1-k_b l)\dot{s}^2

\dot{s} = \frac{|v_t|·cos(\theta_t-\theta_b)}{1-k_b·l}

\ddot{s} = \frac{\overrightarrow{a_t}·\overrightarrow{\tau_b}}{1-k_b·l} + \frac{\dot{s}^2·k_b·l^{'}}{1-k_b·l} + \frac{\dot{s}^2(k^{'}_b l+k_b l^{'})}{1-k_b l}

l^{'}=\frac{\dot{l}}{\dot{s}}\\

l^{''}=\frac{d^2l}{ds}=\frac{d\frac{\dot{l}}{\dot{s}}}{ds}=\frac{\ddot{l}-l^{'}\ddot{s}}{\dot{s}^2}

要完成转换，不可避免的需要计算投影点b的信息。

投影点计算

考虑某时刻的车辆位置点car(绿色)，高精地图给出的离散参考线坐标序列（灰色）。

首先遍历参考线上的所有点，分别计算其与车辆点的距离，距离最小的即为当前车辆点的匹配点 $m$ .

设: 当前匹配点m切向量与x轴的夹角为 $\theta_m$ (已知)，则单位切向量 $\overrightarrow{\tau}=(cos\theta_m,sin\theta_m)$ (已知)，匹配点到车辆点的向量为 $\overrightarrow{d}$ (已知)，匹配点的位矢为 $\overrightarrow{r_m}$ (已知)，匹配点的曲率为 $k_m$ (已知)

则投影点 $r$ 的位矢可由下式计算得到：

\overrightarrow{r_r}=\overrightarrow{r_m}+(\overrightarrow{d}·\overrightarrow{\tau})·\overrightarrow{\tau}\\

投影点的切向量与 $x$ 轴的夹角 $\theta_r$ 可由下式计算得到：

\overrightarrow{\theta_r}=\overrightarrow{\theta_m}+k_m·(\overrightarrow{d}·\overrightarrow{\tau})\\

投影点的曲率可由下式计算得到：

k_r=k_m 或 k_r=(k_m+k_{m+1}) / 2\\

而投影点处曲率对弧长 $s$ 的导数 $k{'}$ 可由下式子计算得到

k{'}=0 或 k{'}=(k_r - k_m)/(\overrightarrow{d}·\overrightarrow{\tau})

实际上我们认为的参考线点的切向量和曲率都是已知的，这个我们是可以通过坐标推算出来的，可使用中点欧拉法计算离散点的曲率和切向量与x轴的夹角。计算代码如下(python)

import math
import numpy as np

def calc_kappa(x_array=None,
               y_array=None):
    """
    计算离散点的曲率和切向量与x轴的夹角
    :param x_array: 笛卡尔x坐标序列
    :param y_array: 笛卡尔y坐标序列
    :return: 
    """
    
    # 计算差分
    dx = np.diff(x_array)
    dy = np.diff(y_array)

    dx_pre = np.insert(dx, 0, dx[0])
    dx_after = np.append(dx, dx[-1])
    dx_final = (dx_pre + dx_after) / 2

    dy_pre = np.insert(dy, 0, dy[0])
    dy_after = np.append(dy, dy[-1])
    dy_final = (dy_pre + dy_after) / 2

    ds_final = np.sqrt(dx_final ** 2 + dy_final ** 2)

    # 计算heading夹角
    heading_array = np.array([math.atan2(dy, dx) for dy, dx in zip(dy_final, dx_final)])

    d_heading = np.diff(heading_array)
    d_heading_pre = np.insert(d_heading, 0, d_heading[0])
    d_heading_after = np.append(d_heading, d_heading[-1])
    d_heading_final = (d_heading_pre + d_heading_after) / 2

    kappa_array = np.array([d_h / ds for d_h, ds in zip(d_heading_final, ds_final)])
    return heading_array, kappa_array

这里用到了math.atan2(y,x),因为这块的角度处理要特别小心，因为我们这里要求切向量与x轴的夹角在 $[-\pi,\pi]$ 之间，正角度为向量从x轴正方向逆时针旋转的角度，负角度为向量从x轴正方向顺时针旋转的角度。

具体的atan2(y,x)函数解释说明可以看这个博客：

blog.csdn.net/KYJL888/art…

规划决策篇：1.2计算投影点信息

回顾笛卡尔坐标->frenet坐标的转换公式

投影点计算