Reeds-Shepp曲线路径规划详解：从运动学模型到枚举求解

1. 回归坐标系第二部分到底在推导什么做自动泊车、AGV 路径规划、或者机械臂末端轨迹规划的工程师对 Reeds-Shepp 曲线这个名字应该不陌生。只要车辆受最小转弯半径约束、又允许倒车Reeds-Shepp 就是一个绕不开的数学工具。它是系列文章的第二部分第一部分已经铺垫了车辆运动学模型、姿态 (x, y, θ) 的定义以及最小转弯半径 r 的归一化思路。这一部分我们要回答两个核心问题Reeds-Shepp 为什么只需要从有限几种路径类型里选每种类型对应的弧长参数怎么根据起点、终点位姿求出来很多初学者拿到 Reeds-Shepp 的论文或者开源代码第一反应都是“符号太抽象、公式太多、抄完也不知道为什么”。所以第二部分不做太多工程上的花活而是把公式当成“可操作的数学构造”来拆。我用到的方法可以概括成一句话暴力枚举路径类型 推导几何公式 数学构造求解流程。把这个组合思路理清楚后面第三部分写完整代码、做性能优化、接可视化都会顺很多。适合读这篇文章的人我觉得有三类一是正在做路径规划算法复现的开发者二是刚接触泊车/园区物流车项目的学生三是已经在用现成 RS 库、但遇到边界情况不知道怎么排查的人。如果只是想跑通代码也可以直接跳到第 4 节但强烈建议先把第 3 节的推导逻辑过一遍因为排查问题的时候你对几何关系的理解程度决定了你能多快定位 bug。1.1 从第一部分的运动学模型说起第一部分我们建立的模型很简单车辆在平面内运动位姿用 (x, y, θ) 表示θ 是车头朝向单位是弧度。车辆以速度 v 前进或者后退同时方向盘的转角决定了瞬时转弯半径。当方向盘打满时车辆沿最小半径 r 的圆弧运动方向盘回正时车辆走直线。用微分方程写就是dx v · cos(θ) · dt dy v · sin(θ) · dt dθ (v / r) · σ · dt这里 σ 表示转向方向σ1 代表左转σ-1 代表右转σ0 代表直行。v 的正负代表前进和倒车。由于实际计算中我们关心的是“从某个位姿出发执行一段固定模式运动后到达哪里”所以更常用的是把上面的微分方程直接积分出来的位姿传播公式。这一套在第二部分非常重要因为后面所有路径类型的推导本质上都在干一件事把若干段“圆弧直线”的位姿传播结果拼接起来让它正好等于已知的目标位姿。一个小提醒第一部分如果把半径 r 归一化成 1那么所有弧长参数直接就是“弧度数”最后算真实路径长度时再乘回 r 就行。这样处理的好处是公式里少一截 r看起来清爽很多。后面第三部分的工程代码里我会专门留一个接口把归一化长度恢复成物理长度现在先统一用 r1 来推导。1.2 四个运动原语与路径单词表达从上面的运动学模型出发车辆在任意时刻的“基本动作”可以抽象成向前左转、向前右转、向后左转、向后右转、前进直行、后退直行。由于直线运动不改变朝向它本质上只是“方向不变的一段位移”所以我们对路径用字母简写L左转圆弧段R右转圆弧段S直线段字母后面跟的角度参数表示这段弧的转角或直线的长度。这样一条完整路径就可以写成一个“单词”比如 LS R 表示“左转圆弧、直线、右转圆弧”每个字母右上角的标记表示前进还是后退。至于圆弧转角的方向本身已经由 L/R 决定了。为什么用这种字符串表达因为它把“路径形状”和“具体参数”解耦了。路径形状决定几何结构比如三段圆弧怎么相切、圆心在哪儿具体参数决定这段路径到底转多大角度、直线多长。第二部分的主要工作就是枚举所有可能的“单词”利用几何关系反推出每个单词里的具体参数然后比较总长选出最短的。这里要特别强调一个容易踩的坑字符串里的 L/R 是“车辆转向方向”而不是“绕绝对坐标系的顺逆时针方向”。倒车左转和前进左转车辆在空间里画出来的圆弧旋转方向是相反的。很多初学者在推导公式时把 L前进左转和 L-后退左转的圆心位置搞混导致接出来的路径根本不对。为了避免这个问题我在代码里统一用“速度方向 转向符号”的组合去描述每段运动后面 3.1 节的通用公式会直接给出这套符号下的圆弧传播结果。2. 路径类型分类为什么 Reeds-Shepp 比 Dubins 复杂这么多2.1 倒车带来的组合爆炸如果不允许倒车问题就是 Dubins 曲线。Dubins 的结论很经典最优路径只可能是 CSC圆弧-直线-圆弧或者 CCC圆弧-圆弧-圆弧两大类其中每一类再按转向方向分一分类一共六种情况。为什么这么少因为前进情况下连续两个同向圆弧可以合并成一个大圆弧连续两个反向圆弧又会让路径明显冗长所以需要枚举的模式很有限。Reeds-Shepp 加了倒车之后情况一下子变复杂了。同样一段运动车辆可能是“倒着左转”也可能是“倒着右转”原语数量从两个方向变成了四个方向。更重要的是由于允许中途改变行驶方向路径可以在同一个位置“原地换挡”这就让“单词”的组合数成倍增长。如果我天真地去枚举所有长度不超过 N 的原语序列组合数会迅速爆炸。更麻烦的是并不是每个单词都有解也不是每个有解的单词都满足车辆运动学约束比如转向方向突变问题、弧长必须为正的问题。所以 Reeds-Shepp 曲线的第一个难点不是“怎么求参数”而是“到底需要看哪些路径类型”。如果全部塞给优化器去暴力搜索计算量和数值稳定性都扛不住。2.2 三条对称性定律把组合数砍下来Reeds 和 Shepp 在 1957 年的论文里给出了一个非常漂亮的结论任意起点到任意终点的最短路径一定可以在一个很小的候选集合里找到。这个集合之所以能缩到很小靠的是三条对称性时间翻转Time Flip把所有“前进/后退”标记取反等价于把路径倒过来看。从数学上说如果 P 是从 A 到 B 的一条路径那么把每个原语的前进/后退方向反转得到的是从 A 到 B 的另一条路径长度相同且几何形状是关于原点镜像的。这条对称性告诉我们只需要实现前进模式的一个类型就能通过取反得到对应的倒车类型。反射Reflect把整个坐标系做左右镜像也就是把所有的 L 换成 R、R 换成 L。这样一条 LS R 路径就变成 RS L。反射不改变路径长度所以我们只要实现一种“偏向”另一种可以直接镜像得到。反向行驶Backwards把整条路径的起点和终点对调并且把路径走向反过来。这等价于把单词倒序写出来同时把每个原语的前进/后退方向取反。这三条对称性极其重要不只是理论上的“少写几行代码”而是直接决定了路径类型归类方式。比如我只需要实现一个基础函数 LpSpLp前进左转-前进直线-前进左转通过时间翻转就能得到 LnSnLn通过反射就能得到 RpSpRp再组合就能得到一大片家族。代码里我会用“旋转/翻转标志位”来复用同一条计算逻辑而不是为每个类型单独写一个逆解函数。2.3 48种情形与9种基本类型利用上面三条对称性Reeds-Shepp 把最优路径归并成了 9 大类具体来说就是 CSC 族的 8 种和 CCC 族的 1 种。每一类再做对称变换一共对应 48 种具体“单词”。初看 48 这个数字还是不小但实际代码里只需要处理 9 个核心公式48 种情形全部由对称变换自动生成。这 9 类路径大致如下大类基本类型示例说明CSCLS R左转、直线、右转三段的转向方向相反呈 S 形CSCLS R-左转、直线、倒车右转方向组合不同CSCL-S R倒车左转、直线、前进右转CSCL-S R-倒车左转、直线、倒车右转CSCLS L两段同向圆弧中间夹直线整体形成大弧线CSCLS L-左转、直线、倒车左转CSCL-S L倒车左转、直线、前进左转CSCL-S L-倒车左转、直线、倒车左转CCCLR- L三段圆弧中间反向两端同向形成“肘形”路径表格里只列了偏向 L 的基础形式R 系列通过反射得到。到这里路径分类这件事就清楚了我们需要解的其实是 9 个几何问题而不是几千个。正因为集合足够小第二部分后面才可以放心地“暴力枚举”每个类型、求一遍解、再取最短。3. 核心公式推导从几何关系到位姿参数3.1 圆弧与直线的增量公式推导路径参数之前必须先有一套准确无误的位姿传播公式。我的建议是把圆弧运动的公式记成下面这种“圆心偏移 旋转”的形式而不是硬背几个三角函数展开式设当前位姿为 (x, y, θ)圆弧半径 r1转向符号 σ1 表示左转σ-1 表示右转弧长为 s。那么终点位姿为θ θ σ · s x x - σ · sin(θ) σ · sin(θ σ · s) y y σ · cos(θ) - σ · cos(θ σ · s)这个公式的几何含义是圆心在当前位姿的“垂直于前进方向的左侧或右侧”距离 1 的位置车辆沿着以该圆心为圆心的圆弧走一段 s终点相对圆心的方向角等于 θ σ·s 对应的方向。直线段更简单x x u · cos(θ) y y u · sin(θ) θ θ其中 u 是直线长度可正可负负代表倒车直行。我特别提醒一句网上很多版本把圆弧公式写成“x x r·sin(s)”这种简化形式那是因为它默认起点朝向 θ0并且挪到了局部坐标系里。一旦你的路径是多段拼接就必须回到上面这个带 θ 的完整形式。我见过很多复现工程前面几段路径没问题最后一段因为嵌套简化公式导致终点对不上排查了半天才发现是局部坐标和全局坐标混用了。3.2 CCC族三段圆弧相切的几何约束先看最复杂的 CCC 族以 LR-L 为例。三段圆弧两两相切第一段是左转第二段是右转第三段又是左转。由于半径都是 1第一段和第二段的圆心距离一定是 2第二段和第三段的圆心距离也是 2。换句话说三个圆心构成一个等腰三角形两条腰长都是 2。这个几何关系非常有用。设三段圆弧的角度分别是 β、γ、δ终点朝向和起点朝向之间的关系是φ β - γ δ因为 L 贡献 βR- 贡献 -γL 贡献 δ直接加起来就是终点朝向相对起点的变化量。位置方程则需要用 3.1 节的圆弧传播公式逐段拼接。如果起点是 (0, 0, 0)执行 LR-L 后终点坐标 (x, y) 是 β、γ、δ 的函数。再加上 φ β - γ δ我们一共有 3 个未知数、3 个方程。解这个方程组的过程就是 CCC 路径的逆解过程。实际代码里我不会直接展开成一个巨大的三角函数表达式去求解析解而是利用等腰三角形约束把问题转化为“固定第二段圆心位置求解第一段和第三段的角度”。具体做法是在第二段角度 γ 的可行区间内采样或者二分搜索每次用几何关系推算出 β 和 δ检查是否满足位置方程。虽然多了一个搜索层但数值稳定性好而且候选类型只有那么几个计算开销完全可以接受。这里有个工程经验CCC 族的求解区间并不是整个 [0, 2π]而是通常限制在 β∈[0, π/2]、δ∈[0, π/2]中间角 γ∈(0, π)。超出这个范围的解往往不是最短路径或者会导致圆弧相互重叠。我在初版代码里没加区间限制结果优化器经常收敛到一些奇怪的大角度路径后来对照论文加上了约束结果立刻正常了。3.3 CSC族直线段把两个圆弧串起来CSC 族是实际使用频率最高的一类因为大多数正常目标位姿下最优解都是 CSC 而不是 CCC。我们以 LS R 为例推导一次完整的参数求解过程其他 CSC 类型套路完全一样。起点 (0, 0, 0)第一段左转角度 t第二段直线长度 u第三段右转角度 v。根据 3.1 节的公式第一段终点 x₁ sin(t) y₁ 1 - cos(t) θ₁ t第二段终点 x₂ sin(t) u · cos(t) y₂ 1 - cos(t) u · sin(t) θ₂ t第三段是右转 v起点朝向 t代入 σ-1 x x₂ - (-sin(t)) (-sin(t - v)) x₂ sin(t) - sin(t - v) y y₂ (-cos(t)) - (-cos(t - v)) y₂ - cos(t) cos(t - v) θ t - v把 x₂、y₂ 代进去并且利用终点朝向 φ t - v可以把 v 替换成 t - φ从而消去 v。整理后得到关于 t 和 u 的两个方程x 2·sin(t) u·cos(t) - sin(φ) y 1 - 2·cos(t) u·sin(t) cos(φ)这是一个典型的“一个未知数 t、一个未知数 u”的方程组。先把 u 消掉第一个方程乘以 sin(t)第二个方程乘以 cos(t)然后相减得到只含 t 的方程(x - 2·sin(t) sin(φ))·sin(t) (y - 1 2·cos(t) - cos(φ))·cos(t)这个方程看起来复杂但它本质上是一个关于 t 的线性三角方程展开之后可以写成 A·sin(t) B·cos(t) C 的形式可以用解析方法求根也可以用数值方法在一个小范围里扫根。求出 t 之后u 回代到上面任意一个方程即可v 直接等于 t - φ。需要特别说明的是这个方程可能有两个根、一个根、或者没有根。没有根意味着当前目标位姿对 LS R 这个类型不可行需要换下一个候选类型。这也是“暴力枚举路径类型”这个思路的灵魂所在先枚举类型再对每个类型求根有根才继续比较没根直接跳过。3.4 暴力枚举推导公式数学构造的推导流程把 2.3 节的 9 个基本类型和 3.3 节这种“消元求解”的过程合在一起就是完整的 Reeds-Shepp 逆解流程准备 9 个基础类型的“单词模板”例如 LS R、LS L、LR-L。对每个模板通过反射、时间翻转、反向行驶三个对称操作生成同一个几何问题下的方向组合。对每个具体方向组合根据目标位姿 (x, y, φ) 列方程消去中间变量得到单变量方程。求解单变量方程回代得到全部弧长参数。检查参数是否满足物理约束弧长非负、角度区间合理等。对满足约束的候选路径计算总长。从所有候选里选出总长最短的一条。这个流程里“暴力枚举”负责覆盖全部可能性“推导公式”负责把每个可能性的参数精确算出来“数学构造”负责保证公式推导的正确性和效率。三者缺一不可。很多商业路径规划库里第 4 步用的是闭式解析解速度极快但对教学和复现来说先用数值扫根把流程跑通再慢慢替换成解析解是性价比最高的路径。这里也顺带回应一下网上经常搜到的“Reeds-Shepp 暴力枚举”其实并不是说对连续状态空间做随机采样而是指“对有限个几何类型做遍历求解”。这个差异想清楚之后你对整个算法的复杂度就会有正确的认识候选类型是常数级别的所以整个算法的耗时也基本是常数级别和路况环境没有关系。4. 代码实现把公式变成能跑的枚举器4.1 数据结构设计在写算法之前我建议先定义清楚数据结构。因为后面要比较 48 种候选路径如果数据结构设计得太松散代码很快就会失控。我用 Python 做演示因为它最能突出逻辑而且后续要改成 C 也很容易平移。from enum import Enum from math import sin, cos, atan2, sqrt, pi class Turn(Enum): LEFT 1 # 左转 RIGHT -1 # 右转 STRAIGHT 0 class Gear(Enum): FORWARD 1 BACKWARD -1 # 基本路径段 class Segment: def __init__(self, turn, gear, length): self.turn turn self.gear gear self.length length # 弧长或直线长度半径已归一化 def __repr__(self): g F if self.gear Gear.FORWARD else B t {Turn.LEFT: L, Turn.RIGHT: R, Turn.STRAIGHT: S}[self.turn] return f{t}{g}({self.length:.4f})用 Segment 列表表示一条完整路径每个 Segment 包括转向类型、前进/倒车标志、长度。这样长度的正负就被 Gear 独立表达了Segment.length 永远保持为正计算总长时直接累加即可。4.2 位姿传播函数有了数据结构先把 3.1 节的位姿传播公式实现出来。这个函数是整个算法里最底层、最核心的工具一定要确保正确。def propagate(x, y, theta, seg): 根据当前位姿和一段路径段返回新的位姿 (x, y, theta) d seg.gear.value # 1 前进, -1 倒车 if seg.turn Turn.STRAIGHT: return x seg.length * d * cos(theta), \ y seg.length * d * sin(theta), \ theta else: sigma seg.turn.value * d # 注意倒车会让实际空间转向方向反转 theta_new theta sigma * seg.length x_new x - sigma * sin(theta) sigma * sin(theta_new) y_new y sigma * cos(theta) - sigma * cos(theta_new) return x_new, y_new, theta_new这里最容易出错的是 sigma 的计算。如果车辆倒车左转从外部看它实际上是在沿着顺时针方向画弧所以空间里的转向符号 sigma 等于“转向类型”乘“行驶方向”。我一开始漏了这一步导致倒车类型的路径全部算反了。4.3 CSC类型逆解函数接下来写一个针对 LS R 的逆解函数。按照 3.3 节的推导我们对 t 扫根然后回代 u 和 v。这里用简单的网格扫描加局部精化避免一上来就上 scipy 之类的牛顿迭代。def solve_LpSpRp(x_target, y_target, phi_target): best None # t 的合理搜索范围 N 2000 for i in range(N): t -pi 2 * pi * i / N # 消元方程见 3.3 节 lhs (x_target - 2 * sin(t) sin(phi_target)) * sin(t) rhs (y_target - 1 2 * cos(t) - cos(phi_target)) * cos(t) if abs(lhs - rhs) 1e-4: # 回代 u if abs(cos(t)) 1e-6: u (x_target - 2 * sin(t) sin(phi_target)) / cos(t) else: u (y_target - 1 2 * cos(t) - cos(phi_target)) / sin(t) v t - phi_target if u 0 and v 0: segs [Segment(Turn.LEFT, Gear.FORWARD, t), Segment(Turn.STRAIGHT, Gear.FORWARD, u), Segment(Turn.RIGHT, Gear.FORWARD, v)] total_len t u v if best is None or total_len best[0]: best (total_len, segs) return best这段代码不是最快的但逻辑非常直观。网格扫描能保证大部分情况下找到初解然后你想换成二分/牛顿迭代去精化只需要把“判断 abs(lhs-rhs)”换成“求根区间收缩”即可。第三部分我会给出完整的 9 类型实现和精化版本。4.4 总长度比较与路径选择有了单个类型的逆解函数再写一个统一入口把所有候选类型跑一遍选出最短路径def reeds_shepp_plan(x_target, y_target, phi_target): candidates [] # 这里先放 LS R 的例子第三部分会填完整列表 r solve_LpSpRp(x_target, y_target, phi_target) if r is not None: candidates.append((LS R, r[0], r[1])) if not candidates: return None # 按总长排序选最短 candidates.sort(keylambda item: item[1]) return candidates[0]实际候选列表会包含 CSC 族的多种方向组合每种组合通过反射和时间翻转生成。需要注意返回前要把归一化长度乘上真实转弯半径 r_min公式是 physical_len normalized_len * r_min弧段的角度不变直线的真实长度也要乘 r_min。5. 完整示例走查从 (0,0,0°) 到 (2,3,0°)5.1 手工构造一个可行目标位姿为了验证代码是否正确最好的办法是“反推”先设计一段路径算出终点位姿再把这组终点位姿丢给逆解函数看看能否解出原来设计的参数。这个测试思路在机器人算法开发里非常常用。我设计这样一条路径起点 (0,0,0)第一段左转 90°第二段直线 1 米第三段右转 90°。也就是t π/2 u 1 v π/2根据 3.3 节的终点公式φ t - v 0 x 2·sin(π/2) 1·cos(π/2) - sin(0) 2 y 1 - 2·cos(π/2) 1·sin(π/2) cos(0) 1 1 1 3所以目标位姿是 (2, 3, 0)。这个位姿下LS R 类型至少有一个已知可行解反向验证的时候如果代码能找回 tπ/2、u1、vπ/2就说明传播公式和逆解逻辑基本正确。5.2 实际运行结果我用 4.3 节的 solve_LpSpRp 跑了一下这组目标值返回结果如下类型: LS R t 1.5708 u 1.0000 v 1.5708 总长度 4.1416三段参数正好是 π/2、1、π/2和构造值一致。这说明两点第一3.1 节的圆弧传播公式是对的第二CSC 类型“消元后单变量求根”的思路在代码里落地没有问题。另外我故意测试了一个不满足该类型几何条件的目标位姿比如 (5, 0, π/2)solve_LpSpRp 返回 None。这说明代码里的可行性判断在起作用不会给你乱输出一条跑不通的路径。正常使用 Reeds-Shepp 算法时总会有若干候选类型返回 None这是很正常的最终结果就是从非 None 的候选中选出最短的那一条。6. 常见问题与排查技巧实录6.1 角度归一化不一致导致方向判断错误RS 曲线所有计算都对角度大小很敏感。有的代码习惯把角度规约到 [0, 2π)有的规约到 [-π, π)两个体系混用会导致三角函数符号判断错误。比如你算出来 v 是 -0.2如果代码判断“v0 不合法”直接跳过很可能只是因为角度没有归一化到合适的区间。我的建议是所有目标角度在进入枚举器之前统一规约到 [-π, π)所有从公式里解出来的角度也统一用这个规范做检查。别在计算过程中反复规约那样只会引入更多判断分支。6.2 圆弧长度出现负号时该怎么处理Reeds-Shepp 里很多类型的公式假设弧长参数为正。如果求出来是负值一种处理方式是直接丢弃另一种方式是反转该段的 Gear 并且取绝对值。比如 L 段算出长度 -0.5在物理上等价于倒车右转 0.5。但这里要小心涉及转向方向的时候不是简单取绝对值就完事必须联动修改字母类型。我的经验是在“类型固定、求参数”的函数里出现负值直接丢弃逻辑最简单也不容易埋雷如果你写的是“对所有类型做对称变换”的框架那么负值问题在对称变换那一层已经被消解掉了不会大量出现。6.3 数值精度导致的消元方程“假无解”扫根时我用的是 abs(lhs - rhs) 1e-4 这种阈值。对很多位姿组合误差可能是 1e-5也可能因为网格太粗漏掉。如果你发现某个明明可行的目标位姿却返回 None先尝试加密网格或者把等号条件改成“检测符号变化”再检查一下是否真的没有根。实车调试时这个“假无解”最容易让人怀疑公式推错了其实只是数值分辨率不够。6.4 对称变换时把 L 和 R 搞反时间翻转和反射这两类变换很容易在代码里写反。我的排查技巧是构造一个“原地掉头”的目标位姿比如起点 (0,0,0)终点 (0,0,π)然后打印所有候选路径的类型、参数和总长。如果 L/R 系列的结果完全对不上说明反射逻辑有问题如果前进/倒车系列结果对不上说明时间翻转逻辑有问题。这个测试用例比随机目标位姿更容易暴露方向性错误。最后分享一点个人体会我在第一次实现 Reeds-Shepp 时试图一步到位把 9 个基础类型的闭式解全部写出来结果越写越乱最后推倒重来改成“通用位姿传播公式 暴力扫根 逐步精化”的节奏代码反而稳定得多。路径规划算法里公式推导的作用是帮你理解几何结构而不一定是把每个公式都展开到最简形式。先把流程跑通再去追求性能是我这几年做规划算法最深的感受。