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

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

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

3. 轨迹求解：

3.1 局部轨迹求解

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

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

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

   计算插值距离列表 (calc_interpolate_dists_list)的过程如下

   • 输入：t_u_v（包含每个段落的长度），step_len（每一步的长度）。
   • 处理：对于每个段落长度，根据长度的正负确定步长d_dist。使用np.arange生成从0到length的序列，步长为d_dist，然后添加length本身，以确保终点包含在内。
   • 输出：interpolate_dists_list，包含每个段落的插值距离列表。

2. 根据上一步生成的插值距离列表的值，计算其对应的局部位姿值。

   插值计算 (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 = 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(t - u - phi)
        return True, [t, u, -v], ['L', 'R', 'L']

    return False, [], []


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

    # 在论文中，(2 < u1 <= 4)的解被认为是次优的
    # 当u1 > 4时，不存在解
    if u1 <= 2:
        u = mod2pi(math.acos((u1 + 2) * 0.25))
        t = mod2pi(theta + u + math.pi / 2)
        v = mod2pi(phi - t + 2 * u)
        if (t >= 0) and (u >= 0) and (v >= 0):
            return True, [t, u, -u, -v], ['L', 'R', 'L', 'R']

    return False, [], []


def lr_l_r(x, y, phi):
    """L+R-L-R+"""
    u1, theta = polar(x + math.sin(phi), y - 1 - math.cos(phi))
    u2 = (20 - u1 ** 2) / 16

    if 0 <= u2 <= 1:
        u = math.acos(u2)
        beta = math.asin(2 * math.sin(u) / u1)
        t = mod2pi(theta + beta + math.pi / 2)
        v = mod2pi(t - phi)
        if (t >= 0) and (v >= 0):
            return True, [t, -u, -u, v], ['L', 'R', 'L', 'R']

    return False, [], []


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

    if u1 >= 2.0:
        u = math.sqrt(u1 ** 2 - 4) - 2
        beta = math.atan2(2, math.sqrt(u1 ** 2 - 4))
        t = mod2pi(theta + beta + math.pi / 2)
        v = mod2pi(t - phi + math.pi / 2)
        if (t >= 0) and (v >= 0):
            return True, [t, -math.pi / 2, -u, -v], ['L', 'R', 'S', 'L']

    return False, [], []


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

    if u1 >= 2.0:
        t = mod2pi(theta + math.pi / 2)
        u = u1 - 2
        v = mod2pi(phi - t - math.pi / 2)
        if (t >= 0) and (v >= 0):
            return True, [t, -math.pi / 2, -u, -v], ['L', 'R', 'S', 'R']

    return False, [], []


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

    if u1 >= 2.0:
        u = math.sqrt(u1 ** 2 - 4) - 2
        beta = math.atan2(math.sqrt(u1 ** 2 - 4), 2)
        t = mod2pi(theta - beta + math.pi / 2)
        v = mod2pi(t - phi - math.pi / 2)
        if (t >= 0) and (v >= 0):
            return True, [t, u, math.pi / 2, -v], ['L', 'S', 'R', 'L']

    return False, [], []


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

    if u1 >= 2.0:
        t = mod2pi(theta)
        u = u1 - 2
        v = mod2pi(phi - t - math.pi / 2)
        if (t >= 0) and (v >= 0):
            return True, [t, u, math.pi / 2, -v], ['L', 'S', 'L', 'R']

    return False, [], []


def lr_90s_l_90r(x, y, phi):
    """L+R-90S-L-90R+"""
    zeta = x + math.sin(phi)
    eeta = y - 1 - math.cos(phi)
    u1, theta = polar(zeta, eeta)

    if u1 >= 4.0:
        u = math.sqrt(u1 ** 2 - 4) - 4
        beta = math.atan2(2, math.sqrt(u1 ** 2 - 4))
        t = mod2pi(theta + beta + math.pi / 2)
        v = mod2pi(t - phi)
        if (t >= 0) and (v >= 0):
            return True, [t, -math.pi / 2, -u, -math.pi / 2, v], ['L', 'R', 'S', 'L', 'R']

    return False, [], []


class Path:
    """
    路径数据容器
    """

    def __init__(self):
        # 路径段长度（负值表示反向段）
        self.t_u_v = []
        # 路径段类型字符（"S": 直线，"L": 左转，"R": 右转）
        self.ctypes = []
        self.L = 0.0  # 路径的总长度
        self.x = []  # x坐标位置
        self.y = []  # y坐标位置
        self.yaw = []  # 方向角[弧度]
        self.directions = []  # 方向（1: 前进，-1: 后退）


def set_path(paths, t_u_v, ctypes, step_len):
    path = Path()
    path.ctypes = ctypes
    path.t_u_v = t_u_v
    path.L = sum(np.abs(t_u_v))

    # 检查是否存在相同路径
    for i_path in paths:
        type_is_same = (i_path.ctypes == path.ctypes)
        length_is_close = (sum(np.abs(i_path.t_u_v)) - path.L) <= step_len
        if type_is_same and length_is_close:
            return paths  # 存在相同路径，不需要插入

    # 检查路径的长度是否太短
    if path.L <= step_len:
        return paths  # 路径太短，不需要插入

    paths.append(path)
    return paths


def time_flip(t_u_v):
    return [-x for x in t_u_v]


def reflect(ctypes):
    def switch_dir(dirn):
        if dirn == 'L':
            return 'R'
        elif dirn == 'R':
            return 'L'
        else:
            return 'S'

    return [switch_dir(dirn) for dirn in ctypes]


def generate_rs_path(q0, q1, max_cur, step_len):
    dx = q1[0] - q0[0]
    dy = q1[1] - q0[1]
    dyaw = q1[2] - q0[2]

    c = math.cos(q0[2])
    s = math.sin(q0[2])
    x = (c * dx + s * dy) * max_cur
    y = (-s * dx + c * dy) * max_cur
    step_len *= max_cur  # 步长归一化

    paths = []
    rs_path_func = [lsl, lsr,  # CSC
                    lr_l, lr_l_, lrl_,  # CCC
                    lrl_r_, lr_l_r,  # CCCC
                    lr_90s_l_, lr_90s_r_,  # CCSC
                    lsr90l_, lsl90r_,  # CSCC
                    lr_90s_l_90r]  # CCSCC

    for path_func in rs_path_func:

        # 遍历筛选后的12种组合
        flag, t_u_v, ctypes = path_func(x, y, dyaw)
        if flag:
            for dis in t_u_v:
                if 0.1 * sum([abs(d) for d in t_u_v]) < abs(dis) < step_len:
                    print("Step len too large for Reeds-Shepp paths.")
                    return []
            paths = set_path(paths, t_u_v, ctypes, step_len)

        # 时间翻转
        flag, t_u_v, ctypes = path_func(-x, y, -dyaw)
        if flag:
            for dis in t_u_v:
                if 0.1 * sum([abs(d) for d in t_u_v]) < abs(dis) < step_len:
                    print("Step len too large for Reeds-Shepp paths.")
                    return []
            t_u_v = time_flip(t_u_v)
            paths = set_path(paths, t_u_v, ctypes, step_len)

        # 反射变换
        flag, t_u_v, ctypes = path_func(x, -y, -dyaw)
        if flag:
            for dis in t_u_v:
                if 0.1 * sum([abs(d) for d in t_u_v]) < abs(dis) < step_len:
                    print("Step len too large for Reeds-Shepp paths.")
                    return []
            ctypes = reflect(ctypes)
            paths = set_path(paths, t_u_v, ctypes, step_len)

        # 逆向变换
        flag, t_u_v, ctypes = path_func(-x, -y, dyaw)
        if flag:
            for dis in t_u_v:
                if 0.1 * sum([abs(d) for d in t_u_v]) < abs(dis) < step_len:
                    print("Step len too large for Reeds-Shepp paths.")
                    return []
            t_u_v = time_flip(t_u_v)
            ctypes = reflect(ctypes)
            paths = set_path(paths, t_u_v, ctypes, step_len)

    return paths


def calc_interpolate_dists_list(t_u_v, step_len):
    interpolate_dists_list = []
    for length in t_u_v:
        d_dist = step_len if length >= 0.0 else -step_len
        interp_dists = np.arange(0.0, length, d_dist)
        interp_dists = np.append(interp_dists, length)
        interpolate_dists_list.append(interp_dists)

    return interpolate_dists_list


def interpolate(dist, length, mode, max_cur, origin_x, origin_y, origin_yaw):
    if mode == "S":
        x = origin_x + dist / max_cur * math.cos(origin_yaw)
        y = origin_y + dist / max_cur * math.sin(origin_yaw)
        yaw = origin_yaw
    else:  # 曲线
        ldx = math.sin(dist) / max_cur
        ldy = 0.0
        yaw = None
        if mode == "L":  # left turn
            ldy = (1.0 - math.cos(dist)) / max_cur
            yaw = origin_yaw + dist
        elif mode == "R":  # right turn
            ldy = (1.0 - math.cos(dist)) / -max_cur
            yaw = origin_yaw - dist
        gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy
        gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy
        x = origin_x + gdx
        y = origin_y + gdy

    return x, y, yaw, 1 if length > 0.0 else -1


def generate_local_course(t_u_v, ctypes, max_cur, step_len):
    interpolate_dists_list = calc_interpolate_dists_list(t_u_v, step_len * max_cur)

    origin_x, origin_y, origin_yaw = 0.0, 0.0, 0.0

    xs, ys, yaws, directions = [], [], [], []
    for (interp_dists, mode, length) in zip(interpolate_dists_list, ctypes,
                                            t_u_v):

        for dist in interp_dists:
            x, y, yaw, direction = interpolate(dist, length, mode,
                                               max_cur, origin_x,
                                               origin_y, origin_yaw)
            xs.append(x)
            ys.append(y)
            yaws.append(yaw)
            directions.append(direction)
        origin_x = xs[-1]
        origin_y = ys[-1]
        origin_yaw = yaws[-1]

    return xs, ys, yaws, directions


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


def calc_paths(sx, sy, syaw, ex, ey, eyaw, max_cur, step_len):
    q0 = [sx, sy, syaw]
    q1 = [ex, ey, eyaw]

    paths = generate_rs_path(q0, q1, max_cur, step_len)
    for path in paths:
        xs, ys, yaws, directions = generate_local_course(path.t_u_v,
                                                         path.ctypes, max_cur,
                                                         step_len)
        # 转换为全局坐标
        path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] for
                  (ix, iy) in zip(xs, ys)]
        path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] for
                  (ix, iy) in zip(xs, ys)]
        path.yaw = [pi_2_pi(yaw + q0[2]) for yaw in yaws]
        path.directions = directions
        path.lengths = [length / max_cur for length in path.t_u_v]
        path.L = path.L / max_cur

    return paths


def reeds_shepp_planning(sx, sy, syaw, ex, ey, eyaw, max_cur, step_len=0.2):
    paths = calc_paths(sx, sy, syaw, ex, ey, eyaw, max_cur, step_len)

    if not paths:
        return None, None, None, None, None  # 不能生成任何轨迹

    # 查找路径最短的路径
    best_path_index = paths.index(min(paths, key=lambda p: abs(p.L)))
    b_path = paths[best_path_index]

    return b_path.x, b_path.y, b_path.yaw, b_path.ctypes, b_path.t_u_v


def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k", linewidth=3.0):
    if isinstance(x, list):
        for (ix, iy, iyaw) in zip(x, y, yaw):
            plot_arrow(ix, iy, iyaw)
    else:
        plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), fc=fc,
                  ec=ec, head_width=width, head_length=width)
        plt.plot([x, x + length * math.cos(yaw)],
                 [y, y + length * math.sin(yaw)],
                 color=ec, linewidth=linewidth)


def plot_car(x, y, yaw, color='-k'):
    car_color = color
    c, s = math.cos(yaw), math.sin(yaw)
    rot = rot_mat_2d(-yaw)
    car_outline_x, car_outline_y = [], []
    width, rear_2_front, rear_2_back = 2.0, 3.3, 1.0
    outline_xs = [rear_2_front, rear_2_front, -rear_2_back, -rear_2_back, rear_2_front]
    outline_ys = [width / 2, -width / 2, -width / 2, width / 2, width / 2]
    for rx, ry in zip(outline_xs, outline_ys):
        converted_xy = np.stack([rx, ry]).T @ rot
        car_outline_x.append(converted_xy[0] + x)
        car_outline_y.append(converted_xy[1] + y)

    plot_arrow(x, y, yaw)
    plt.plot(car_outline_x, car_outline_y, car_color)


def main():
    # 起点
    start_x = 5.0
    start_y = 5.0
    start_yaw = np.deg2rad(0.0)

    # 目标点
    end_x = 25.0
    end_y = 20.0
    end_yaw = np.deg2rad(-90.0)

    max_curvature = 0.1  # 最大曲率
    path_step_length = 0.05  # 路径步长

    xs, ys, yaws, modes, lengths = reeds_shepp_planning(start_x, start_y, start_yaw,
                                                        end_x, end_y, end_yaw,
                                                        max_curvature, path_step_length)

    if not xs:
        print("Rs path palnning failed.")

    else:
        # image_list = []  # 存储图片
        # i = 0
        for i_x, i_y, i_yaw in zip(xs, ys, yaws):
            plt.cla()
            plt.plot(xs, ys, label="final course " + str(modes))
            plot_car(i_x, i_y, i_yaw)
            plot_car(end_x, end_y, end_yaw, 'gray')

            plt.legend()
            plt.grid(True)
            plt.axis("equal")
            plt.pause(0.0001)
        #     plt.savefig("temp.png")
        #     i += 1
        #     if (i % 5) == 0:
        #         image_list.append(imageio.imread("temp.png"))
        #
        # imageio.mimsave("display.gif", image_list, duration=0.1)


if __name__ == '__main__':
    main()

运行结果：

参考文献

Reeds Shepp planning-Mathematical Description of Individual Path Types