前面两篇把Reeds-Shepp曲线的理论基础和公式分类都梳理完了但读者问得最多的还是那句话你推导出一堆 (\alpha)、(d)、(\beta)到底怎么变成能跑的程序这篇就是来做这件事的。作为系列第三部分我会把“公式推导”和“代码实现”之间的最后一公里走完先聊清楚符号路径怎么映射成数值参数再用LSL一个例子把闭式解翻译成函数最后给出一个可扩展的枚举求解框架顺便把我在实际项目里踩过的角度归一化、半径缩放、数值边界这几个坑都填上。适合正在写路径规划器、想搞清楚Reeds-Shepp曲线到底怎么落地的人如果你连前两部分都还没看也不影响本部分会把需要使用的结论重新列清楚。1. 前两部分留下的一张公式清单到底要怎样变成程序1.1 从符号路径到数值路径的映射关系还记得前两篇的核心结论吗我们把问题归一化成了这样起点固定为 ((0,0))、航向为 (0)终点相对位置是 ((x,y,\theta))最小转弯半径 (r1)。在这个归一化坐标下Reeds-Shepp曲线的最优路径一定可以写成若干个“单词”每个单词由 L、R、S 组成分别表示左转圆弧、右转圆弧和直线段。这些单词的总数是有限的完整集合一共有 48 种。有些人一听“48种”就头疼但其实这恰恰是最好写代码的地方不用做任何启发式搜索直接暴力枚举所有单词套公式算出每一段的长度再取总长度最短的那条。暴力枚举在这里不是笨办法而是数学结构决定的。因为最优路径的“形状”是有限的枚举 48 次在现代处理器上连一微秒都用不到反而是工程上最稳、最可控的做法。真正决定代码质量的是另外两件事每个单词对应的公式是不是写对了公式解出来的长度值能不能正确处理“负长度”和“周期角度”。前两部分把公式表推导出来了这一部分我就讲讲怎么把它们翻译成函数以及翻译过程中有哪些坑。1.2 坐标系归一化是代码里最容易出错的一步不管起点和终点在哪里第一步永远是坐标系归一化。一个常见的错误是直接在原始坐标系里套公式最后算出一堆奇怪的路径。Reeds-Shepp的所有公式都是在“起点为原点、起点航向为x轴正向、最小转弯半径为1”的假定下推导的所以必须先把问题搬到这个局部坐标系。核心变换代码可以写成这样def normalize_input(start, goal, radius): # start, goal: (x, y, yaw) dx goal[0] - start[0] dy goal[1] - start[1] dth normalize_angle(goal[2] - start[2]) x math.cos(start[2]) * dx math.sin(start[2]) * dy y -math.sin(start[2]) * dx math.cos(start[2]) * dy yaw dth # 除以最小转弯半径使半径归一到1 x / radius y / radius return x, y, yaw这里的normalize_angle通常是把角度约束到 ((-\pi, \pi])。很多实现翻车都翻在这一步如果角度不做归一化后面公式里的 (\sin\theta)、(\cos\theta) 虽然没问题但比较路径长度时会出现莫名其妙的“多绕一整圈”现象。我习惯在进入具体求解器之前把起点航向、终点航向全都统一过一遍确保后面的三角函数不会因为 (3\pi) 和 (\pi) 的等价性产生歧义。归一化完成后几何公式就只用在单位圆上计算。最后生成路径点时再把所有长度乘回原始转弯半径即可。2. 拿LSL开刀三步把向量方程翻译成函数2.1 圆心坐标与直线向量约束的推导LSL 是最容易理解的路径先左转、再直行、再左转。设三段参数分别是圆弧角 (\alpha)、直线长度 (d)、圆弧角 (\beta)。我下面把推导过程重新走一遍这样你才能明白代码里的atan2到底是从哪来的。第一段左转圆弧。车辆起点在 ((0,0))航向为 (0)左转圆心在起点左侧1个单位处也就是 (C_1(0,1))。左转 (\alpha) 角度后车辆位置变为[ P_1(\sin\alpha,\ 1-\cos\alpha),\quad \text{航向}\alpha ]第二段直线。当前航向是 (\alpha)走 (d) 长度后位置为[ P_2P_1d(\cos\alpha,\ \sin\alpha) ]第三段左转圆弧。先找第三段的圆心 (C_2)它在 (P_2) 左侧1个单位处[ C_2P_2(-\sin\alpha,\ \cos\alpha) ]第三段结束时车辆航向为 (\alpha\beta)也就是终点航向 (\theta)。终点位置可以写成[ P_3C_2(\sin(\alpha\beta),\ -\cos(\alpha\beta)) ]把 (P_2)、(C_2) 代进去并利用 (\alpha\beta\theta)化简得到[ xd\cos\alpha\sin\theta ][ yd\sin\alpha1-\cos\theta ]再把含 (d) 的项单独放在一侧[ d\cos\alphax-\sin\theta ][ d\sin\alphay-1\cos\theta ]令[ ux-\sin\theta,\quad vy-1\cos\theta ]那么[ d\sqrt{u^2v^2},\quad \alpha\operatorname{atan2}(v,u),\quad \beta\theta-\alpha ]这就是 LSL 路径的闭式解。2.2 两行核心代码与角度周期陷阱按照上面的公式代码其实非常短def solve_lsl(x, y, theta): u x - math.sin(theta) v y - 1.0 math.cos(theta) d math.hypot(u, v) if d 1e-6: return 0.0, 0.0, theta alpha math.atan2(v, u) beta theta - alpha return alpha, d, beta但如果你直接把这个函数返回的 (\alpha,d,\beta) 拿去画路径大概率会在某些目标点上画出多绕一圈的奇怪结果。原因刚才说过(\alpha) 和 (\beta) 不是唯一的因为给 (\alpha) 加上 (2\pi)再给 (\beta) 减去 (2\pi)角度关系依然满足。所以我在真正返回之前会做一个小范围的候选枚举candidates [] for k in range(-2, 3): a alpha 2.0 * math.pi * k b theta - a candidates.append((abs(a) d abs(b), a, d, b)) _, alpha, d, beta min(candidates, keylambda item: item[0])这个循环虽然看着不起眼但它能把因为atan2取主值导致的路径长度浪费直接消掉。如果你准备把这段代码放到路径规划库里这一步绝不是可选项而是必须的。它的本质就是在所有可能的整圈旋转组合里挑一组让 (\lvert\alpha\rvert d \lvert\beta\rvert) 最小的解。2.3 有效性判定负长度路径留给谁处理上面解出来的一组数如果 (\alpha)、(d)、(\beta) 都大于等于0说明这条LSL路径可以靠纯前进完成。但如果出现负数比如 (\alpha-0.5)那这条路实际是在右转和LSL这个单词不符。这种负长度解并不是“不能用”它往往对应着另一种带倒车动作的路径类型。比如 LSL 中负的 (\alpha) 可以解释成一段右转倒车但在完整的 Reeds-Shepp 枚举里它会被更规范地表示成类似RS-L-的单词。我在基础版本里习惯把纯前向和带倒车分开处理这样调试时思路更清晰。基础版本对负长度直接判失败EPS 1e-6 if alpha -EPS or beta -EPS or d -EPS: return None等后面要扩展到完整48种路径时再把负长度作为一种合法状态让step函数用长度符号来表达前进/倒车。3. 路径求解器主循环枚举、打分、选最优3.1 路径类型表和对应的求解函数映射LSL 只是其中一种。纯前进不换挡的路径至少还有 RSR、LSR、RSL、LRL、RLR。它们的推导过程完全一样区别只是圆心坐标、角度正负号和方程形式。我建议在代码里建一张路径类型表把字符串名字和求解函数绑定起来solvers { LSL: solve_lsl, RSR: solve_rsr, LSR: solve_lsr, RSL: solve_rsl, LRL: solve_lrl, RLR: solve_rlr, }以 RSR 为例它和 LSL 是对称的。把 LSL 推导里的左转圆心 ((0,1)) 换成右转圆心 ((0,-1))把航向从 (\alpha) 换成 (-\alpha)最后能得到[ d\cos\alphax\sin\theta ][ d\sin\alpha-1\cos\theta-y ]其他几个也全是同一个套路列圆心、列向量等式、解三角形。与其强行背公式不如记住方法实现时一个个推。当然纯前向的这六种并不覆盖所有最优路径。带倒车动作的路径类型还有很多比如LSR-、L-S-R这类混合挡位路径。扩展到完整集合的方式有两种一是把单词表扩充到48项对每一项写独立求解函数二是利用对称性对已经实现的路径做左右镜像和时间反转生成新路径。两种方法我都试过工程上我更喜欢显式枚举因为对称变换在边界条件下很容易出幺蛾子显式写出来的函数虽然多但每个都好调试。3.2 从参数长度到真实路径点通用运动学推进函数枚举得到的最优解只是一组参数要变成实际能用的路径点序列必须写一个通用推进函数。我用一个状态元组(x, y, yaw)表示车辆位姿然后按路径段逐个推进。单位半径下的直线和圆弧推进逻辑如下def step(pose, steer, length): x, y, yaw pose if steer S: return (x length * math.cos(yaw), y length * math.sin(yaw), yaw) if steer L: cx x - math.sin(yaw) cy y math.cos(yaw) new_yaw yaw length return (cx math.sin(new_yaw), cy - math.cos(new_yaw), new_yaw) if steer R: cx x math.sin(yaw) cy y - math.cos(yaw) new_yaw yaw - length return (cx - math.sin(new_yaw), cy math.cos(new_yaw), new_yaw)这里length的正负就代表前进和倒车。直线段length为负就是倒车直线圆弧段length为负就是反向走弧线。因此这个函数天然支持带符号长度后续扩展完整48种路径时不用再改。生成离散路径点时要注意插值密度。圆弧段每段至少插值20个点直线段按步长0.1米插值这样在控制层才不至于出现相邻点之间的朝向突变。如果你在写的是低速AGV或者自动泊车这个密度基本够用。3.3 总代价计算与归一化半径的还原整个主循环的逻辑可以浓缩成下面几步把起点终点变换到局部坐标系并除以最小转弯半径遍历solvers表里的所有求解器每个求解器返回一组或多组候选解计算总长度总长度是所有段绝对长度之和因为倒车同样消耗时间记录最小代价和对应的路径模板、参数最后把所有直线长度乘以原始转弯半径圆弧角度保持不变再用带真实半径的step函数生成全局路径点。伪代码是这样的def plan(start, goal, radius): x, y, theta normalize_input(start, goal, radius) best_cost float(inf) best_params None best_name None for name, solver in solvers.items(): params solver(x, y, theta) if params is None: continue segments interpret(name, params) cost sum(abs(seg) for _, seg in segments) if cost best_cost: best_cost cost best_params params best_name name # 用 best_name 和 best_params 生成全局路径点 ...这一步最容易漏的是“忘了乘以半径”。归一化时坐标已经除过 (r)求解出来的直线长度 (d) 是归一化长度圆弧参数是圆心角不是物理弧长。生成路径点时直线长度要乘 (r)但圆弧计算中圆心偏移和正弦项内部已经包含了半径1的假定因此更稳妥的做法是给step函数再加一个radius参数def step(pose, steer, angle_or_length, radius):函数内部把圆心偏移写成radius * sin(...)把直线长度直接写成length圆弧弧长由角度乘以radius得到。这样能少踩一半的坑。4. 实际跑通的例子与肉眼可见的坑4.1 三个测试用例倒车入库、直角弯、S形绕障我在调这段代码时固定用三个场景来验证。第一个是水平车位倒车入库起点 ( (0,0,\pi/2) )终点 ( (3,1,\pi/2) )。这个场景如果用纯前进的 LSL/RSR经常会找不到合理路径因为倒车段是刚需。完整版带倒车模板的枚举能找到类似“右转-直线-左转倒车”的路径总长度明显更短。第二个是直角弯起点 ( (0,0,0) )终点 ( (5,5,\pi/2) )。这种场景下 RSL 往往是最优解先右转一点直线斜插再左转对准出口。如果角度归一化没做好终点航向可能被算成 (-3\pi/2) 而不是 (\pi/2)导致解出来的转弯角大了整整一圈。第三个是S形绕障起点和终点航向相同但横向偏移较大。LRL/RLR 这种纯圆弧路径会在这种场景里胜出。它的公式比 CSC 复杂核心思路是把三段圆弧的圆心看成一个三角形通过三角形的边长关系反推各段圆弧角。具体推导不在这一篇展开但实现的套路依然不变列圆心、列向量等式、解三角形。这三个场景跑通之后我才敢说这版求解器的基本枚举逻辑没问题。4.2 曲率半径不为1时的缩放处理车辆最小转弯半径 (r) 通常由底盘参数给定可能是5米、6米甚至更大。归一化时用x / r没问题但还原时很多人会忘掉直线段乘半径。更隐蔽的问题是圆弧段圆心角和物理弧长之间的关系是“弧长圆心角×半径”但如果你直接用更新角度差来推进位姿里程计算结果会缺一个半径因子。因此我的建议是所有几何计算都在归一化坐标系里完成最后生成路径点时一次性换算到原始坐标系。具体来说可以先在归一化坐标里用step生成一条单位半径下的路径点序列再把每个点的坐标乘radius航向保持不变。这样做可以把“归一化”和“生成路径”彻底解耦调试时也能分开验证。4.3 航向角归一化所有sin/cos前的盾牌这是整个实现里最阴险的坑。C 的std::atan2返回范围是 ([- \pi, \pi])Python 的math.atan2也一样但下游公式希望角度能在某个区间内连续变化。比如终点航向是 (270^\circ)你归一化成 (-90^\circ) 后(\sin) 和 (\cos) 的值都不变路径形状也不会变但在比较两条候选路径谁更短时角度跳跃会导致某些解被错误地排除。我的经验是分层处理输入层把所有航向先统一归一化到 ((- \pi, \pi])求解器层每个求解器内部再用多周期候选枚举消除atan2主值带来的周期问题。这两层归一化各司其职前者负责让所有输入干净后者负责选出最短周期。只做其中任何一个都不够稳。4.4 求解失败时的降级策略即使把48种路径全部枚举一遍也会因为数值误差导致某些边界情况无解。常见的是起点和终点距离为0但航向不同或者目标点落在最小转弯半径内。这种时候不要直接返回失败我一般会做两级降级先用 Dubins 曲线给一版纯前向路径如果 Dubins 也失败就采用“原地旋转直线”的应急路径。在实际项目中我还遇到过车辆实际转弯半径比标称值大10%导致规划出来的圆弧终点落不到目标位姿上。这种情况光靠规划器是修不掉的必须在路径跟踪层做横向误差修正。所以规划器输出一个理论最优解只是第一步后面接控制时一定要留足容差。最后再分享一个调试技巧我在写这套求解器时并没有一次性把六种、四十八种全部写完而是先只实现 LSL然后用随机位姿和高精度数值优化结果做对比。单条路径验证正确之后再逐步加入 RSR、LSR、RSL、LRL、RLR。这样每个新公式的错误都能第一时间定位而不是等整个枚举跑完看到一团乱麻再回头猜是谁错了。Reeds-Shepp曲线看起来公式繁多但只要抓住“圆心向量约束候选角度枚举归一化还原”这三板斧代码实现并没有想象中那么可怕。