3、Reeds-Shepp曲线（上）：基本概念、6种基本运动基元、组合规则、最短路径长度计算
各位同学，欢迎来到《混合A星算法泊车路径生成实战》的第三讲。
今天我们来聊聊一个在泊车路径规划里绕不开的话题——Reeds-Shepp曲线。说实话，我第一次接触这东西的时候，觉得它就是个数学玩具。直到我在一个垂直泊车项目里，用混合A星搜出来的路径总是带着一堆不必要的锯齿，后来换成RS曲线做平滑，效果立竿见影。嗯，从那以后我就再也不敢小看它了。
3.1 基本概念：为什么需要RS曲线？
先问大家一个问题：泊车时，车能不能原地掉头？
不能。因为汽车有最小转弯半径约束，而且只能前进或后退。这就意味着，我们规划的路径必须满足两个条件：
- 曲率连续（至少分段连续）
- 曲率有界（不能超过车辆最小转弯半径）
Reeds-Shepp曲线，说白了就是在满足上述约束的前提下，连接两个位姿的最短路径。它由Reeds和Shepp两位大佬在1990年提出，至今仍是泊车路径规划的基石。
核心思想：RS曲线假设车辆可以前进（Forward）和后退（Reverse），且转弯半径固定为R。它用一系列圆弧和直线段拼接出最短路径。
我个人习惯把RS曲线看作「带倒车功能的Dubins曲线」。Dubins曲线只允许前进，而RS曲线允许倒车——这恰恰是泊车场景最需要的。
3.2 六种基本运动基元
RS曲线由六种基本运动基元组合而成。每种基元代表一个连续的运动段：
| 符号 | 含义 | 说明 |
|---|---|---|
L |
左转圆弧（前进） | 车辆向左转弯，半径为R |
R |
右转圆弧（前进） | 车辆向右转弯，半径为R |
S |
直线段（前进） | 车辆直行 |
l |
左转圆弧（后退） | 倒车时向左转弯 |
r |
右转圆弧（后退） | 倒车时向右转弯 |
s |
直线段（后退） | 倒车直行 |
注意大小写：大写代表前进，小写代表后退。这个约定在代码里很常见，别搞混了。
我的经验：在实际项目中，我通常把基元封装成枚举类型，方便后续组合和调试。比如：
class MotionPrimitive(Enum):
LEFT_FORWARD = 'L'
RIGHT_FORWARD = 'R'
STRAIGHT_FORWARD = 'S'
LEFT_REVERSE = 'l'
RIGHT_REVERSE = 'r'
STRAIGHT_REVERSE = 's'3.3 组合规则：如何拼出有效路径？
有了基元，怎么组合？RS曲线给出了严格的组合规则。一共有48种可能的组合，但常用的只有9种。我给大家列一下核心的几类：
3.3.1 基本组合（CSC类）
这类路径由「圆弧-直线-圆弧」组成，是最常见的类型：
L+S+L：左转前进 → 直行前进 → 左转前进L+S+R：左转前进 → 直行前进 → 右转前进R+S+L：右转前进 → 直行前进 → 左转前进R+S+R：右转前进 → 直行前进 → 右转前进
还有带倒车的版本，比如 l+s+l、l+s+r 等。
3.3.2 带倒车的组合（CCC类）
这类路径由三段圆弧组成，中间可能包含倒车：
L+R+L：左转前进 → 右转前进 → 左转前进R+L+R：右转前进 → 左转前进 → 右转前进l+r+l：左转后退 → 右转后退 → 左转后退r+l+r：右转后退 → 左转后退 → 右转后退
注意：CCC类路径在泊车中很常见，但要注意圆弧之间的切向连续性。我曾经在调试时发现，如果两段圆弧的曲率方向不一致，路径会出现尖点——车根本开不过去。
3.3.3 带直线和倒车的混合组合
比如 L+s+l（前进左转 → 后退直行 → 后退左转），这种组合在狭窄车位里特别实用。
3.4 最短路径长度计算
好了，现在我们知道有哪些组合了。但问题是：给定起点和终点，怎么选出最短的那条？
答案是：穷举所有有效组合，计算每条路径的长度，取最小值。
具体来说，对于每种组合，我们需要：
- 根据起点和终点的位姿，解算出每段圆弧的圆心和角度
- 计算每段圆弧的弧长（弧长 = 半径 × 转角）
- 计算直线段的长度
- 累加得到总路径长度
这里我给出一个简化版的Python实现，用于计算 L+S+L 组合的长度：
import math
def calc_lsl_length(start, end, R):
"""
计算 L+S+L 组合的路径长度
:param start: (x, y, yaw) 起点位姿
:param end: (x, y, yaw) 终点位姿
:param R: 最小转弯半径
:return: 路径总长度
"""
dx = end[0] - start[0]
dy = end[1] - start[1]
dyaw = end[2] - start[2]
# 计算第一段圆弧的圆心
cx1 = start[0] + R * math.cos(start[2] + math.pi/2)
cy1 = start[1] + R * math.sin(start[2] + math.pi/2)
# 计算第三段圆弧的圆心
cx3 = end[0] + R * math.cos(end[2] + math.pi/2)
cy3 = end[1] + R * math.sin(end[2] + math.pi/2)
# 计算直线段长度（两个圆心之间的距离）
d = math.hypot(cx3 - cx1, cy3 - cy1)
# 如果直线段长度小于2R，说明路径不可行
if d < 2 * R:
return float('inf')
# 计算三段路径的长度
theta1 = math.atan2(cy3 - cy1, cx3 - cx1) - start[2]
theta3 = end[2] - math.atan2(cy3 - cy1, cx3 - cx1)
length1 = R * abs(theta1)
length2 = math.sqrt(d**2 - (2*R)**2)
length3 = R * abs(theta3)
return length1 + length2 + length3避坑指南：我曾经在计算圆弧角度时，忘记处理角度归一化（把角度限制在 [-π, π] 范围内），结果算出来的路径长度忽大忽小。后来加了一行 theta = (theta + math.pi) % (2 * math.pi) - math.pi 才搞定。
3.5 实际应用中的注意事项
最后，分享几个我在项目中踩过的坑：
- 数值稳定性：当起点和终点很近时，某些组合的几何解算会出现除零错误。记得加小量 epsilon 保护。
- 路径可行性检查：算出来的路径长度是无穷大？那说明这个组合不可行，直接跳过。
- 性能优化：48种组合全部计算一遍，在嵌入式平台上可能有点慢。我一般会先根据起点和终点的朝向，快速过滤掉明显不合理的组合。
好了，这一讲我们搞清楚了RS曲线的基元、组合规则和长度计算方法。下一讲，我会带大家手写一个完整的RS曲线求解器，并把它集成到混合A星算法中。到时候，你会看到这些数学公式是怎么变成实际可用的泊车路径的。
有什么问题，欢迎在评论区留言。我们下期见。