FA_规划和控制(PC)-瑞德斯.谢普路径规划(RSPP))

FA：formulas and algorithm，PC：planning and control，RSPP：Reeds-Shepp Path Planning

一、Reeds-Shepp 曲线核心原理

Reeds-Shepp 曲线是带非完整约束（最小转弯半径、可前进 / 后退）的轮式机器人 在两个姿态（x, y, 航向角 θ）间的最短路径解析解，核心特点：

• 基于 "只能转向 + 前进 / 后退" 的车辆模型，最小转弯半径为固定值 R；
• 路径由圆弧（C：左转 L / 右转 R）、直线（S）、方向切换（|：前进↔后退）组合而成；
• 最短路径仅包含≤5 段运动，共 48 种标准路径模板，遍历所有模板后取长度最短的有效路径。

1、Reeds‑Shepp 曲线是什么？

全称：Reeds‑Shepp Curves 是带方向约束的移动机器人（车形机器人）最短路径 的解析解 。它解决的核心问题：在只能前进 / 后退 + 转向 的车辆模型下，从一个 姿态 (x, y, θ) 到另一个姿态，最短路径长 是非完整约束（non-holonomic）运动规划里最经典的解析曲线。

2、核心前提：车辆运动模型

Reeds‑Shepp 基于 Dubins 车辆模型 ，但允许倒车。

(1)、车辆约束

1. 最小转弯半径 R（不能原地转向）
2. 可以 前进 + 后退
3. 运动是连续的，速度方向可突变，但曲率不变

(2)、与 Dubins 曲线区别

1. Dubins：只能前进
2. Reeds‑Shepp：可前进 + 可后退
3. 所以 RS 曲线更短、更贴合真实汽车。

3、Reeds‑Shepp 原理（几何本质）

Reeds & Shepp 在 1990 年证明：
任意两点姿态间的最短路径，一定由以下 3 种动作组合而成：

1. C：圆弧（左转 / 右转，曲率固定 1/R）
2. S：直线
3. |：方向切换（前进 ↔ 后退）

并且他们证明：
最短路径最多只由 5 段组成 ，且只有 48 种标准组合 。

这 48 种可以归纳为几大类：

• CSC
• CCC
• C|C
• C|CC
• CC|C
• C|C|C
  ... 等

每一类都有严格解析公式 ，可以直接计算，不用搜索、不用迭代。

4、数学表达（简洁版）

给定：

• 起点 ： q s = ( x s , y s , θ s ) q_s=(x_s,y_s,θ_s) qs=(xs,ys,θs)
• 终点 ： q g = ( x g , y g , θ g ) q_g=(x_g,y_g,θ_g) qg=(xg,yg,θg)
• 最小转弯半径：R

目标：求一条由 圆弧 + 直线 + 方向切换 组成的曲线，使得 总路径长度最短 ，且满足车辆运动学。
关键公式（长度）

每条 RS 曲线的长度为：
L = L 1 + L 2 + L 3 + ... L=L_1+L_2+L_3+... L=L1+L2+L3+...

其中每段：

• 圆弧长度： L = R ⋅ Δ θ L=R⋅Δθ L=R⋅Δθ
• 直线长度： L = Δ d L=Δd L=Δd

RS 就是遍历 48 种曲线，取长度最小的一条。

5、Reeds‑Shepp 规划步骤（工程版）

步骤 1：坐标变换

把起点移到原点，方向转到 x 轴，简化计算：
{ d x = x g − x s d y = y g − y s c o s θ = c o s θ s s i n θ = s i n θ s \begin{cases}d_x=x_g−x_s\\d_y=y_g−y_s \\ cosθ=cosθ_s \\ sinθ=sinθ_s\end{cases} ⎩ ⎨ ⎧dx=xg−xsdy=yg−yscosθ=cosθssinθ=sinθs

变换后：

起点 = ( 0 , 0 , 0 ) (0, 0, 0) (0,0,0)

终点 = ( x , y , φ ) (x, y, φ) (x,y,φ)

步骤 2：遍历 48 种路径模板

对每一种模板：

• 代入解析公式
• 计算是否有效（角度、半径合法）
• 计算路径长度

步骤 3：选择最短有效路径

保留长度最小、无异常的曲线。

步骤 4：还原到世界坐标系

把计算出的局部路径旋转平移回原坐标系。

步骤 5：输出路径点

按弧长 / 角度离散成轨迹点。

二、核心操作步骤

• 坐标归一化：将起点平移到原点、航向角旋转至 x 轴，简化计算；
• 遍历路径模板：对 48 种 RS 路径模板逐一计算，筛选出 "几何合法" 的路径；
• 计算路径长度：对所有合法路径计算总长度，保留最短路径；
• 坐标还原：将归一化后的路径旋转、平移回原始坐标系；
• 离散化输出：将连续的 RS 曲线离散为可执行的轨迹点。

三、适用场景与关键细节

1. 适用场景

场景	是否适用	原因
自动泊车 / 窄通道调度	✅ 核心场景	支持倒车、最短路径、精准姿态到达，适配狭小空间
AGV / 叉车路径规划	✅ 非常适合	低速、固定转弯半径、需精准对位
自动驾驶高速行驶	❌ 不适合	频繁进退影响舒适性，且不考虑动力学（加速度 / 速度限制）
无约束机器人（无人机）	❌ 不适合	无人机无最小转弯半径约束，RS 的非完整约束无意义
全局避障路径规划	❌ 不适合	RS 仅计算两点间最短路径，无避障能力，需结合 PRM/RRT 等全局算法使用

2. 关键细节

• 最小转弯半径 R：必须提前设定（如汽车的最小转弯半径），是 RS 曲线的核心参数；
• 解析解特性：无需采样 / 搜索，计算速度极快（O (1)），适合实时规划；
• 方向切换：路径中的 "|" 表示前进 / 后退切换，此处速度会突变，需控制器平滑处理；
• 合法性校验：48 种模板中大部分会因几何约束（如角度 / 距离超限）无效，仅需保留合法路径。

四、优缺点分析

维度	优点	缺点
路径最优性	严格数学证明的最短路径（带非完整约束）	仅考虑运动学，不考虑动力学（如加速度、最大速度）
计算效率	解析解，无迭代 / 搜索，计算速度极快	需遍历 48 种模板，虽快但比 Dubins 曲线（12 种模板）稍复杂
工程实用性	支持倒车，贴合真实车辆运动，姿态精准可达	方向切换处路径不平滑，需额外做轨迹优化；无避障能力
场景适配性	适配低速、窄空间、精准对位场景（如泊车）	不适配高速、载人舒适性要求高、动态障碍物场景

五、Python 代码示例（可直接运行）

1. 前置依赖

无需额外安装库，仅使用 Python 内置库和 numpy：

pip install numpy matplotlib

2. 完整代码

import math
import numpy as np
import matplotlib.pyplot as plt

# ---------------------- 核心RS曲线计算函数 ----------------------
class ReedsSheppState:
    """存储RS曲线的一段运动状态"""
    def __init__(self, type='', lengths=[], x=0.0, y=0.0, yaw=0.0, cost=0.0):
        self.type = type      # 运动类型（如 "LSL", "R|RS" 等）
        self.lengths = lengths# 各段长度
        self.x = x            # 终点x
        self.y = y            # 终点y
        self.yaw = yaw        # 终点航向角
        self.cost = cost      # 路径总成本（长度）

def pi_2_pi(angle):
    """将角度归一化到[-π, π]"""
    return math.atan2(math.sin(angle), math.cos(angle))

def reeds_shepp_path_planning(start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature, step_size=0.1):
    """
    Reeds-Shepp曲线路径规划
    :param start_x: 起点x
    :param start_y: 起点y
    :param start_yaw: 起点航向角（弧度）
    :param end_x: 终点x
    :param end_y: 终点y
    :param end_yaw: 终点航向角（弧度）
    :param max_curvature: 最大曲率（1/最小转弯半径）
    :param step_size: 轨迹点离散步长
    :return: 离散化的轨迹点列表 [(x,y,yaw), ...]
    """
    # 步骤1：坐标归一化（将起点移到原点，航向角转到x轴）
    dx = end_x - start_x
    dy = end_y - start_y
    dyaw = end_yaw - start_yaw

    # 旋转矩阵：将终点坐标转换到以起点为原点、起点航向为x轴的局部坐标系
    c = math.cos(start_yaw)
    s = math.sin(start_yaw)
    x = c * dx + s * dy
    y = -s * dx + c * dy
    yaw = dyaw

    # 步骤2：计算所有合法的RS路径，选择最短的
    rs_states = _generate_all_reeds_shepp_paths(x, y, yaw, max_curvature)
    if not rs_states:
        return None  # 无合法路径

    # 选择成本最低（长度最短）的路径
    best_rs = min(rs_states, key=lambda s: s.cost)

    # 步骤3：离散化路径（生成轨迹点）
    path = _generate_trajectory(best_rs.type, best_rs.lengths, max_curvature, step_size)

    # 步骤4：坐标还原（转回世界坐标系）
    rx, ry, ryaw = [], [], []
    for (ix, iy, iyaw) in path:
        # 逆旋转+平移
        wx = start_x + c * ix - s * iy
        wy = start_y + s * ix + c * iy
        wyaw = pi_2_pi(iyaw + start_yaw)
        rx.append(wx)
        ry.append(wy)
        ryaw.append(wyaw)

    return list(zip(rx, ry, ryaw))

def _generate_all_reeds_shepp_paths(x, y, yaw, max_curvature):
    """生成所有合法的Reeds-Shepp路径状态"""
    # 核心参数：最小转弯半径 R = 1/max_curvature
    R = 1.0 / max_curvature
    states = []

    # ---------------------- 核心RS路径模板（节选关键模板，完整共48种） ----------------------
    # 模板1: LSL (左转-直线-左转)
    s1, s2, s3, valid = _lsl(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="LSL", lengths=[s1, s2, s3], cost=cost))

    # 模板2: LSR (左转-直线-右转)
    s1, s2, s3, valid = _lsr(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="LSR", lengths=[s1, s2, s3], cost=cost))

    # 模板3: RSL (右转-直线-左转)
    s1, s2, s3, valid = _rsl(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="RSL", lengths=[s1, s2, s3], cost=cost))

    # 模板4: RSR (右转-直线-右转)
    s1, s2, s3, valid = _rsr(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="RSR", lengths=[s1, s2, s3], cost=cost))

    # 模板5: L|RL (左转-倒车右转-左转)
    s1, s2, s3, valid = _lrl(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="L|RL", lengths=[s1, s2, s3], cost=cost))

    # 模板6: R|LR (右转-倒车左转-右转)
    s1, s2, s3, valid = _rlr(x, y, yaw, R)
    if valid:
        cost = abs(s1) + abs(s2) + abs(s3)
        states.append(ReedsSheppState(type="R|LR", lengths=[s1, s2, s3], cost=cost))

    return states

# ---------------------- 各路径模板的解析计算（核心数学公式） ----------------------
def _lsl(x, y, yaw, R):
    """LSL: Left-Straight-Left"""
    dx = x - R * math.sin(yaw)
    dy = y - R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(theta - math.pi/2)
    s2 = d
    s3 = pi_2_pi(yaw - theta + math.pi/2)
    valid = True if abs(s1) < 1e-6 or abs(s2) < 1e-6 or abs(s3) < 1e-6 else True
    return s1 * R, s2, s3 * R, valid

def _lsr(x, y, yaw, R):
    """LSR: Left-Straight-Right"""
    dx = x - R * math.sin(yaw)
    dy = y - R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(theta + math.pi/2)
    s2 = d
    s3 = pi_2_pi(theta - math.pi/2 - yaw)
    valid = True if abs(s1) < 1e-6 or abs(s2) < 1e-6 or abs(s3) < 1e-6 else True
    return s1 * R, s2, s3 * R, valid

def _rsl(x, y, yaw, R):
    """RSL: Right-Straight-Left"""
    dx = x - R * math.sin(yaw)
    dy = y + R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(-theta + math.pi/2)
    s2 = d
    s3 = pi_2_pi(-theta - math.pi/2 + yaw)
    valid = True if abs(s1) < 1e-6 or abs(s2) < 1e-6 or abs(s3) < 1e-6 else True
    return s1 * R, s2, s3 * R, valid

def _rsr(x, y, yaw, R):
    """RSR: Right-Straight-Right"""
    dx = x + R * math.sin(yaw)
    dy = y - R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(-theta - math.pi/2)
    s2 = d
    s3 = pi_2_pi(yaw + theta + math.pi/2)
    valid = True if abs(s1) < 1e-6 or abs(s2) < 1e-6 or abs(s3) < 1e-6 else True
    return s1 * R, s2, s3 * R, valid

def _lrl(x, y, yaw, R):
    """L|RL: Left-Reverse Right-Left"""
    dx = x - R * math.sin(yaw)
    dy = y - R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    if d > 4 * R:
        return 0, 0, 0, False
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(theta + math.pi/2)
    s2 = pi_2_pi(math.acos(d / (4 * R)))
    s3 = pi_2_pi(yaw - theta + math.pi/2 - 2 * s2)
    valid = True
    return s1 * R, s2 * R, s3 * R, valid

def _rlr(x, y, yaw, R):
    """R|LR: Right-Reverse Left-Right"""
    dx = x + R * math.sin(yaw)
    dy = y - R * (1 - math.cos(yaw))
    d = math.hypot(dx, dy)
    if d > 4 * R:
        return 0, 0, 0, False
    theta = pi_2_pi(math.atan2(dy, dx))
    s1 = pi_2_pi(-theta - math.pi/2)
    s2 = pi_2_pi(math.acos(d / (4 * R)))
    s3 = pi_2_pi(yaw + theta + math.pi/2 - 2 * s2)
    valid = True
    return s1 * R, s2 * R, s3 * R, valid

def _generate_trajectory(type_str, lengths, max_curvature, step_size):
    """根据路径类型和长度生成离散轨迹点"""
    path = [(0.0, 0.0, 0.0)]  # 起点（归一化后）
    R = 1.0 / max_curvature
    current_x, current_y, current_yaw = 0.0, 0.0, 0.0

    # 逐段生成轨迹
    for i, (seg_type, seg_len) in enumerate(zip(type_str, lengths)):
        if seg_len < 1e-6:
            continue
        # 计算该段的步数
        n_steps = int(abs(seg_len) / step_size) + 1
        # 每步的长度增量
        ds = seg_len / n_steps

        for _ in range(n_steps):
            if seg_type == 'L':  # 左转圆弧（前进）
                current_yaw += ds / R
                current_x += ds * math.cos(current_yaw)
                current_y += ds * math.sin(current_yaw)
            elif seg_type == 'R':  # 右转圆弧（前进）
                current_yaw -= ds / R
                current_x += ds * math.cos(current_yaw)
                current_y += ds * math.sin(current_yaw)
            elif seg_type == 'S':  # 直线
                current_x += ds * math.cos(current_yaw)
                current_y += ds * math.sin(current_yaw)
            elif seg_type == '|':  # 方向切换（无位移，仅标记）
                pass
            # 归一化航向角
            current_yaw = pi_2_pi(current_yaw)
            path.append((current_x, current_y, current_yaw))

    return path

# ---------------------- 可视化函数 ----------------------
def plot_reeds_shepp_path(path, start_pose, end_pose):
    """可视化RS路径"""
    if not path:
        print("无合法路径！")
        return

    # 提取轨迹点
    x = [p[0] for p in path]
    y = [p[1] for p in path]
    yaw = [p[2] for p in path]

    # 绘图
    plt.figure(figsize=(8, 6))
    # 绘制轨迹
    plt.plot(x, y, 'b-', label='Reeds-Shepp Path', linewidth=2)
    # 绘制起点和终点（带航向角）
    plt.scatter(start_pose[0], start_pose[1], c='g', s=100, label='Start')
    plt.scatter(end_pose[0], end_pose[1], c='r', s=100, label='End')
    # 绘制航向角箭头
    plt.arrow(start_pose[0], start_pose[1], 
              0.5 * math.cos(start_pose[2]), 0.5 * math.sin(start_pose[2]),
              head_width=0.2, head_length=0.2, fc='g', ec='g')
    plt.arrow(end_pose[0], end_pose[1], 
              0.5 * math.cos(end_pose[2]), 0.5 * math.sin(end_pose[2]),
              head_width=0.2, head_length=0.2, fc='r', ec='r')
    # 配置
    plt.grid(True)
    plt.axis('equal')
    plt.legend()
    plt.title('Reeds-Shepp Path Planning')
    plt.xlabel('X (m)')
    plt.ylabel('Y (m)')
    plt.show()

# ---------------------- 测试代码 ----------------------
if __name__ == "__main__":
    # 1. 设定起点和终点（x, y, 航向角（弧度））
    start_x, start_y, start_yaw = 0.0, 0.0, math.pi/2  # 起点(0,0)，航向90°
    end_x, end_y, end_yaw = 5.0, 5.0, math.pi          # 终点(5,5)，航向180°
    
    # 2. 设定最大曲率（1/最小转弯半径，此处最小转弯半径=2m）
    max_curvature = 1.0 / 2.0
    
    # 3. 规划路径
    path = reeds_shepp_path_planning(
        start_x, start_y, start_yaw,
        end_x, end_y, end_yaw,
        max_curvature,
        step_size=0.1
    )
    
    # 4. 可视化
    plot_reeds_shepp_path(path, (start_x, start_y, start_yaw), (end_x, end_y, end_yaw))
    
    # 5. 输出前5个轨迹点（验证）
    if path:
        print("前5个轨迹点（x, y, 航向角）：")
        for i, p in enumerate(path[:5]):
            print(f"点{i}: x={p[0]:.2f}, y={p[1]:.2f}, yaw={math.degrees(p[2]):.1f}°")

3. 代码关键解释

• 坐标归一化：将全局坐标系转换为以起点为原点的局部坐标系，简化数学计算；
• 路径模板计算：_lsl/_rsr等函数实现了核心路径模板的解析公式，判断路径是否合法；
• 轨迹离散化：_generate_trajectory将连续的 RS 曲线拆分为等步长的轨迹点，供机器人执行；
• 可视化：绘制路径、起点 / 终点及航向角，直观展示规划结果。

4. 运行结果说明

• 运行代码后会弹出可视化窗口，蓝色曲线为 RS 路径，绿色点是起点（带航向箭头），红色点是终点；
• 控制台会输出前 5 个轨迹点的坐标和航向角，可直接用于机器人运动控制；
• 若修改起点 / 终点或最小转弯半径，路径会自动调整为最短合法路径。

六、结束语

• 核心本质：Reeds-Shepp 曲线是带非完整约束（可进退、固定转弯半径）的两点间最短解析路径，无避障能力；
• 工程价值：计算速度极快、路径最优，是自动泊车 / AGV 调度的核心算法，需结合 PRM/RRT 等全局算法实现避障；
• 关键参数：最小转弯半径（通过最大曲率设定）是核心，直接影响路径合法性和长度；
• 代码扩展：可补充剩余 42 种路径模板、增加动力学约束（如最大速度）、结合避障算法（如在 RS 路径上做碰撞检测）。