frenet坐标与cartesian坐标相互转换与代码实现（第二部分）

由于csdn文章长度的限制，将本篇文章分成两篇发布，另一篇文章见frenet坐标与cartesian坐标相互转换与代码实现（第一部分）。
也可以直接通过公众号iDoitnow的frenet坐标与cartesian坐标相互转换与代码实现直接阅读完整版

1 frenet转cartesian坐标系

frenet转cartesian坐标系是已知$ s$ 、$ \dot{s}$、 $\ddot{s}$ 、$l$、$l^{\prime }$、$ l’’ $，求 $\vec{r}$、 $\theta$ 、$\vec{v}$ 、$\vec{a}$、 $k$ 。

1.1 求$\vec{r}$

由图中的几何关系可得
$$\vec{r}=\overrightarrow{r_r}+l\overrightarrow{n_r}\text{\hspace{1em}(30)}$$

1.2 求$\vec{v}$

由公式(18)和(17)可得
$$\begin{array}{rl}\vec{v}\overrightarrow{n_r}& =\dot{l}\\ \vec{v}\overrightarrow{{\tau }_r}& =(1-k_{r}l)\dot{s}\end{array}\text{\hspace{1em}(31)}$$
将上面两个式子展开可得
$$\begin{array}{rl}|\vec{v}||\overrightarrow{n_r}|sin(\theta -{\theta }_r)& =|\vec{v}|\cdot 1\cdot sin(\theta -{\theta }_r)=\dot{l}\\ |\vec{v}||\overrightarrow{{\tau }_r}|cos(\theta -{\theta }_r)& =|\vec{v}|\cdot 1\cdot cos(\theta -{\theta }_r)=(1-k_{r}l)\dot{s}\end{array}\text{\hspace{1em}(32)}$$
将上面两个式子两侧分别平方并相加可得
$$|\vec{v}|={({\dot{l}}^2+(1-k_{r}l{)}^2{\dot{s}}^2)}^{\frac{1}{2}}\text{\hspace{1em}(33)}$$
将(32)两式相除可得
$$tan(\theta -{\theta }_r)=\frac{\dot{l}}{(1-k_{r}l)\dot{s}}\text{\hspace{1em}(34)}$$
结合公式(22)，可得
$$\theta ={\theta }_r+arctan\frac{\dot{l}}{(1-k_{r}l)\dot{s}}={\theta }_r+arctan\frac{l^{\prime }}{1-k_{r}l}\text{\hspace{1em}(35)}$$

1.3 求$\vec{a}$和$a_{\tau }$

由公式(23)和(25)可得
$$\begin{array}{rl}\vec{a}\overrightarrow{n_r}& =\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2\\ \vec{a}\overrightarrow{{\tau }_r}& =\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2\end{array}\text{\hspace{1em}(36)}$$
与求解$\vec{v}$类似，将上两式分别平方在相加可得
$$|\vec{a}|=((\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2{)}^2+(\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2{)}^2{)}^{\frac{1}{2}}\text{\hspace{1em}(37)}$$
将(36)两式再相除可得
$$tan({\theta }_a-{\theta }_r)=\frac{\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2}{\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2}\text{\hspace{1em}(38)}$$
其中，${\theta }_a$为向量$\vec{a}$与笛卡尔坐标系$X$轴的夹角。

由上式可得
$${\theta }_a={\theta }_r+arctan\frac{\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2}{\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2}\text{\hspace{1em}(39)}$$

对于切线加速度$a_{\tau }=|\vec{\dot{v}}|$，将公式$(10)$两边同时点乘$\vec{\tau }$可得
$$a_{\tau }=|\vec{\dot{v}}|=\vec{a}\vec{\tau }=|\vec{a}|cos({\theta }_a-\theta )\text{\hspace{1em}(40)}$$

1.4 求$k$

将公式$(10)$两边点乘$\vec{n}$可得
$$\vec{a}\vec{n}=k|\vec{v}{|}^2\text{\hspace{1em}(41)}$$
由上式可得
$$k=\frac{\vec{a}\vec{n}}{|\vec{v}{|}^2}\text{\hspace{1em}(42)}$$

1.5 小结

综上，可得
$$\{\begin{array}{ll}\vec{r}& =\overrightarrow{r_r}+l\overrightarrow{n_r}\\ |\vec{v}|& ={({\dot{l}}^2+(1-k_{r}l{)}^2{\dot{s}}^2)}^{\frac{1}{2}}\\ \theta & ={\theta }_r+arctan\frac{\dot{l}}{(1-k_{r}l)\dot{s}}={\theta }_r+arctan\frac{l^{\prime }}{1-k_{r}l}\\ |\vec{a}|& =((\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2{)}^2+(\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2{)}^2{)}^{\frac{1}{2}}\\ {\theta }_a& ={\theta }_r+arctan\frac{\ddot{l}+k_r(1-k_{r}l){\dot{s}}^2}{\ddot{s}(1-k_{r}l)+k_r{\dot{s}}^2{l}^{\prime }+(k_r^{\prime }l-k_r{l}^{\prime }){\dot{s}}^2}\\ a_{\tau }& =|\vec{a}|cos({\theta }_a-\theta )\\ k& =\frac{\vec{a}\vec{n}}{|\vec{v}{|}^2}\end{array}\text{\hspace{1em}(43)}$$

2. 代码实现(python)

cartesian到frenet坐标转换：

import numpy as np
from math import *

def cartesian_to_frenet(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a_v, theta, kappa):
    vec_r = np.array([x, y])
    vec_rr = np.array([rx, ry])
    vec_nr = np.array([cos(rtheta + np.pi / 2), sin(rtheta + np.pi / 2)])
    if np.cross(vec_r - vec_rr, vec_nr) >= 1.0e-6: # 投影点应与目标点组成的向量应与nr平行
        print("The reference point [rx, ry] and [x, y] don't match")

    vec_t = np.array([cos(theta), sin(theta)])
    vec_v = np.array([v * cos(theta), v * sin(theta)])
    vec_n = np.array([cos(theta + np.pi / 2), sin(theta + np.pi / 2)])
    vec_a = a_v * vec_t + v * v * kappa * vec_n

    vec_tr = np.array([cos(rtheta), sin(rtheta)])

    # 计算s、l
    l = np.dot(vec_r - vec_rr, vec_nr)
    s = rs
    # 计算dot_s
    dot_s = np.dot(vec_v, vec_tr) / (1 - rkappa * l)

    dot_l = np.dot(vec_v, vec_nr)
    ddot_l = np.dot(vec_a, vec_nr) - rkappa * (1 - rkappa * l) * dot_s * dot_s
    # 计算dl
    dl = dot_l / dot_s
    # 计算ddl、dot_s
    ddot_s = np.dot(vec_a, vec_tr) / (1 - rkappa * l) \
             + dot_s * dot_s * rkappa * dl / (1 - rkappa * l) \
             + dot_s * dot_s * (rdkappa * l + rkappa * dl) / (1 - rkappa * l)
    ddl = (ddot_l - dl * ddot_s) / (dot_s * dot_s)

    return np.array([s, dot_s, ddot_s]), np.array([l, dl, ddl])

frenet到cartesian坐标转换：

import numpy as np
from math import *

def normalize_angle(angle):
    a = fmod(angle+np.pi, 2*np.pi)
    if a < 0.0:
        a += (2.0*np.pi)
    return a - np.pi

def frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition):
    if fabs(rs - s_condition[0]) >= 1.0e-6:
        print("The reference point s and s_condition[0] don't match")

    vec_rr = np.array([rx, ry])
    vec_nr = np.array([cos(rtheta + np.pi / 2), sin(rtheta + np.pi / 2)])
    # 计算r，得到的就是目标点坐标(x, y)
    vec_r = vec_rr + d_condition[0] * vec_nr

    dot_l = d_condition[1] * s_condition[1]
    ddot_l = d_condition[2] * s_condition[1] * s_condition[1] + s_condition[2] * d_condition[1]
    # 计算速度v
    v = sqrt(dot_l**2 + (1  - rkappa * d_condition[0])**2 * s_condition[1]**2)
    # 计算theta
    theta = normalize_angle(rtheta + atan2(d_condition[1], (1 - rkappa * d_condition[0])))
    # 计算加速度a、theta_a和切线加速度a_v
    a_part1 = ddot_l + rkappa * (1 - rkappa * d_condition[0]) * (s_condition[1] * s_condition[1])
    a_part2 = (s_condition[2] * (1 - rkappa * d_condition[0])
               - rkappa * s_condition[1] * s_condition[1] * d_condition[1]
               - s_condition[1] * s_condition[1] *(rdkappa * d_condition[0] + rkappa * d_condition[1]))
    a = sqrt(a_part1 * a_part1 + a_part2 * a_part2)
    theta_a = normalize_angle(rtheta + atan2(a_part1, a_part2))
    a_v = a * cos(theta_a - theta)
    # 计算kappa
    vec_n = np.array([-sin(theta), cos(theta)])  # 切线单位向量n
    kappa = a * np.dot(np.array([cos(theta_a), sin(theta_a)]), vec_n) / (v*v)

    return  vec_r[0], vec_r[1], v, a_v, theta, kappa

2. 参考文献

B站up主忠厚老实的老王的自动驾驶决策规划视频

---

文章首发公众号：iDoitnow如果喜欢话，可以关注一下