Reeds-Shepp曲线公式推导及代码实现-第三部分（共三部分）

文章目录

注：由于CSDN篇幅限制，文章只能拆分成三部分，完整的文章可见Reeds-Shepp曲线公式推导及代码实现

CSDN剩余部分：
Reeds-Shepp曲线公式推导及代码实现-第一部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第二部分（共三部分）
Reeds-Shepp曲线公式推导及代码实现-第三部分（共三部分）

3. 轨迹求解：

3.1 局部轨迹求解

在上一章节我们求解到 $t$ 、 $u$ 、 $v$ 后，我们需要根据字段类型（ $L$ 、 $R$ 或 $S$ ）求解出局部坐标下路径点位姿。

其具体过程如下（建议结合后面的代码查看）：

根据 $t$ 、 $u$ 、 $v$ 的值进行等步长插值，得到每段轨迹对应的生成一系列插值距离列表。

计算插值距离列表 (calc_interpolate_dists_list)的过程如下
- 输入：t_u_v（包含每个段落的长度），step_len（每一步的长度）。
- 处理：对于每个段落长度，根据长度的正负确定步长d_dist。使用np.arange生成从0到length的序列，步长为d_dist，然后添加length本身，以确保终点包含在内。
- 输出：interpolate_dists_list，包含每个段落的插值距离列表。
根据上一步生成的插值距离列表的值，计算其对应的局部位姿值。

插值计算 (interpolate)的过程如下：
- 输入：dist（当前插值距离），length（段落长度），mode（模式，"S"代表直线，"L"代表左转，"R"代表右转），max_cur（最大曲率），以及当前位置和朝向。
- 处理：
  - 如果模式是"S"（直线），则使用直线方程计算新位置和朝向。
  - 如果模式是"L"或"R"（曲线），则计算曲线上对应距离的点的新位置和朝向。
- 输出：新位置(x, y)，新朝向yaw，以及方向（1表示前进，-1表示后退）。

3.2 全局轨迹求解

根据原始起点的坐标和航向，将上一小节计算出来的局部进行整体的平移和旋转（与归一化的步骤刚好相反），最终得到全局路径的轨迹。

3.3 求解最优路径

根据计算出来的路径，选择路径最短的轨迹（也可根据我们实际的需要定制选优规则），作为最终的轨迹输出。

4. 代码实现

reeds_shepp.py

import math
import matplotlib.pyplot as plt
import numpy as np
# import imageio.v2 as imageio
from scipy.spatial.transform import Rotation as Rot


def rot_mat_2d(angle):
    """
    平面内的旋转矩阵
    """
    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]


def polar(x, y):
    r = math.hypot(x, y)
    theta = math.atan2(y, x)
    return r, theta


def mod2pi(x):
    v = np.mod(x, np.copysign(2.0 * math.pi, x))
    if v < -math.pi:
        v += 2.0 * math.pi
    else:
        if v > math.pi:
            v -= 2.0 * math.pi
    return v


def lsl(x, y, phi):
    """L+S+L+"""
    u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
    if 0.0 <= t <= math.pi:
        v = mod2pi(phi - t)
        if 0.0 <= v <= math.pi:
            return True, [t, u, v], ['L', 'S', 'L']

    return False, [], []


def lsr(x, y, phi):
    """L+S+R+"""
    u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi))
    u1 = u1 ** 2
    if u1 >= 4.0:
        u = math.sqrt(u1 - 4.0)
        theta = math.atan2(2.0, u)
        t = mod2pi(t1 + theta)
        v = mod2pi(t - phi)

        if (t >= 0.0) and (v >= 0.0):
            return True, [t, u, v], ['L', 'S', 'R']

    return False, [], []


def lr_l(x, y, phi):
    """L+R-L+"""
    u1, theta = polar(x - math.sin(phi), y - 1 + math.cos(phi))

    if u1 < 4.0:
        beta = math.acos(0.25 * u1)
        t = mod2pi(beta + theta + math.pi / 2)
        u = mod2pi(math.pi - 2 * beta)
        v = mod2pi(phi - t - u)
        return True, [t, -u, v], ['L', 'R', 'L']

    return False, [], []


def lr_l_(x, y, phi):
    """L+R-L-"""
    u1, theta = polar(x - math.sin(phi), y - 1 + math.cos(phi))

    if u1 <= 4.0:
        beta = math.acos(0.25 * u1)
        t = mod2pi(beta + theta + math.pi / 2)
        u = mod2pi(math.pi - 2 * beta)
        v = mod2pi(-phi + t + u)
        return True, [t, -u, -v], ['L', 'R', 'L']

    return False, [], []


def lrl_(x, y, phi):
    """L+R+L-"""
    u1, theta