Reeds-Shepp曲线这个系列写到第三部分了前两部分把运动学模型、路径分类以及公式推导的思路基本过完了这篇我集中说代码实现。公式推导是一回事代码能跑是另一回事中间隔着的坑比想象中多。这篇我不会再花大篇幅去重推公式而是把重点放在“公式如何映射成函数”“枚举路径类型时如何保证不错不漏”“拼接出来的路径如何做误差评估”这几个工程关键点上顺便把我在实际项目中踩过的坑都标出来。适合正在做自动驾驶路径规划、AGV调度、机器人导航的朋友参考哪怕你之前只听过Reeds-Shepp这个名字只要有一点点运动规划基础也能跟着这篇文章把核心代码搭起来。1. 系列第三部分的定位从公式到可运行代码1.1 前两部分的结论快速回顾Reeds-Shepp曲线和前几篇讨论的Dubins曲线最大的区别就是车辆允许倒车。所以最短路径不再局限于“弧线-直线-弧线”这种单调结构而是会出现形如LRL、LS-R、C|C|C这样带有转向反转和掉头操作的组合。前两部分推导出来的核心结论可以浓缩成三句话对满足最小转弯半径约束的车辆任意位姿之间的最短路径一定可以表示成有限段“左转弧L、右转弧R、直线S”的拼接。这些拼接模式按照圆弧方向组合可以归纳成若干基础族再根据符号展开成具体的48种路径类型。每一种路径类型背后都对应一组基于几何关系的解析公式给定起点、终点、车头朝向和转弯半径可以直接算出每一段弧的圆心、切点、角度和长度不需要数值优化求解。第三部分要做的就是把这48种类型的求解过程写成代码然后通过遍历所有路径类型、拼接路径、比较总长度最终选出最短的那条。1.2 为什么“暴力枚举所有路径类型”反而是最靠谱的做法很多人在实现Reeds-Shepp曲线时容易陷入一个误区试图通过某种统一的数值优化方法直接解出最短路径。从理论上讲这不是不行但工程上非常不推荐。原因在于Reeds-Shepp问题的几何结构已经研究得非常透彻最优解的路径类型只会落在有限集合里直接用解析公式枚举不仅速度快而且数值稳定性高行为也可预测。我见过一些代码尝试用样条插值加迭代优化来逼近最短路径最终结果不仅慢还经常收敛到局部最优或者在某些极限位姿下出现振荡。相比之下枚举48种路径类型的方式每一次求解都是纯粹的几何计算最坏情况下也就几十次函数调用现代CPU上跑一次耗时在微秒级。这个效率对于实时规划器完全够用而且每一个中间结果都可以可视化调试时能看到到底是哪条候选路径被选中为什么被选中逻辑透明得多。从这个角度说标题里的“暴力枚举推导公式数学构造”这个组合其实很精准枚举是框架公式是核心数学构造保证了每种路径类型都能被精确求解而不是靠猜测。2. 整体架构设计从位姿输入到路径输出2.1 核心数据结构路径段、路点、候选结果写代码之前先把数据结构定清楚。Reeds-Shepp的整条路径本质上是由若干“圆弧段”和“直线段”拼接而成所以我定义了一个Segment结构来描述单段路径class Segment: def __init__(self, seg_type, center, start_theta, end_theta, length): self.seg_type seg_type # L, R, S self.center center # 圆弧圆心直线段为 None self.start_theta start_theta # 圆弧起始角度 self.end_theta end_theta # 圆弧终止角度 self.length length # 该段长度seg_type用来标记这一段是左转弧、右转弧还是直线center存储圆心坐标直线段不需要圆心start_theta和end_theta是圆弧在圆上的起始极角与终止极角用于后续插值length记录该段的实际弧长或直线长度。在Segment之上我定义了一个RSPath结构用来保存完整的候选路径class RSPath: def __init__(self, segments, total_length): self.segments segments self.total_length total_lengthsegments是一个Segment列表顺序就是车辆实际行驶的顺序total_length是所有段长度之和最终选路径时主要比较这个值。有了这两个结构后续所有构造函数的输出都能统一主流程只需要处理RSPath对象不需要关心具体是CSC类型还是CCC类型。2.2 坐标变换把任意起终点统一到标准位形这是整个代码实现里最值得先做的一步。Reeds-Shepp的公式推导包括我前两篇写的那些计算过程几乎都是建立在“起点在原点、车头朝向x轴正方向”这个标准位形下的。如果每次按任意起终点直接套公式代码里会多出一堆正负号分支非常容易出错。我的做法是在主函数最前面做一次刚体变换把起点平移到原点再把整个坐标系旋转到起点朝向为x轴正方向。对应的代码很简单def normalize_pose(start, goal): x0, y0, yaw0 start x1, y1, yaw1 goal dx x1 - x0 dy y1 - y0 cos_yaw math.cos(-yaw0) sin_yaw math.sin(-yaw0) xf dx * cos_yaw - dy * sin_yaw yf dx * sin_yaw dy * cos_yaw yawf normalize_angle(yaw1 - yaw0) return (0.0, 0.0, 0.0), (xf, yf, yawf)这个变换本身是“保距”的也就是平移旋转不会改变两点之间的实际距离所以变换后再算出的路径长度和直接在原坐标系下算完全一致。求出路径后如果需要把路点坐标映射回原来的世界坐标系只需要做一次逆变换也就是先反向旋转再平移。需要注意normalize_angle这个函数。写角度相关的代码时角度归一化是命门稍后会专门说。这里先给出一个最简单可靠的版本def normalize_angle(angle): while angle math.pi: angle - 2.0 * math.pi while angle -math.pi: angle 2.0 * math.pi return angle2.3 主流程枚举、构造、评估、筛选整体主流程非常直白写成伪代码就是def reeds_shepp_path(start, goal, rho): start_norm, goal_norm normalize_pose(start, goal) xf, yf, yawf goal_norm best_path None best_err float(inf) for path_type in enumerate_all_types(): candidate construct_path(path_type, xf, yf, yawf, rho) if candidate is None: continue err endpoint_error(candidate, xf, yf, yawf) if err 1e-3 and candidate.total_length best_path.total_length: best_path candidate return transform_back(best_path)enumerate_all_types负责产生所有需要尝试的路径类型标识符construct_path根据类型调用对应的构型构造器endpoint_error检查路径终点是否和实际目标位姿一致。这个流程看起来简单但真正常被忽略的细节是不是所有路径类型在任意起终点下都有合法解。比如某些类型要求起点和终点的位置距离必须落在某个范围否则切点不存在。所以每个构造函数的第一步都是做几何可行性检查不合法就返回None由主流程继续尝试下一种类型。还有一个容易被忽略的点枚举顺序。虽然最终我们会比较总长度取最小值但如果把一些特定类型放在前面可能在正反两种枚举顺序下得到同样的类型组合但比较方向相反。更安全的做法是同一类型下把所有“符号选择”都试一遍这个后面讲CCC类型时具体展开。3. 两大核心公式的代码化CSC与CCC3.1 圆弧段和直线段的公共基元在写具体路径类型之前先把两个基础函数准备好一个是直线段一个是圆弧段。直线段最简单输入两个点直接返回距离def straight_length(p1, p2): return math.hypot(p2[0] - p1[0], p2[1] - p1[1])圆弧段稍复杂一点。给定圆心center、半径rho、起始极角theta_start、终止极角theta_end以及转向方向direction1表示左转-1表示右转我需要计算这一段弧的实际弧长并在插值时按方向生成路径点。弧长的计算不能直接用abs(theta_end - theta_start)因为角度可能有超过π的情况而且一旦涉及多圈缠绕必须做带上符号的归一化。我在实践里用的方法是把“有符号角度增量”先算出来再取绝对值作为弧长def arc_angle_and_length(center, radius, start_point, end_point, direction): start_theta math.atan2(start_point[1] - center[1], start_point[0] - center[0]) end_theta math.atan2(end_point[1] - center[1], end_point[0] - center[0]) if direction 0: delta normalize_angle(end_theta - start_theta) if delta 0: delta 2.0 * math.pi else: delta normalize_angle(start_theta - end_theta) if delta 0: delta 2.0 * math.pi return delta * radius这个函数的核心思想是左转时车辆沿逆时针方向从起点绕到终点所以角度差必须归一化到(0, 2π)右转时反过来从终点绕到起点。这一步看似简单但很多初版代码跑出来的路径会绕大圈通常就是这里没有处理“角度负值加2π”的逻辑。插值时也是基于这个逻辑在起点和终点极角之间线性插值再根据方向决定实际角度序列。3.2 CSC类路径的切点公式与实现CSC是“弧-直线-弧”结构的统称比如LSR、LS-L、RSL、RS-R。这类路径的关键是求两段圆弧之间的公切线切点。在标准位形下起点的第一段圆弧圆心可以直接写出来。左转时圆心在起点左侧右转时圆心在起点右侧o1x 0.0 o1y direction1 * rho终点的第二段圆弧圆心稍微复杂一点。假设终点位姿是(xf, yf, yawf)那么终点左转圆心的位置是o3x xf - direction3 * rho * math.sin(yawf) o3y yf direction3 * rho * math.cos(yawf)direction3是终点处第二段弧的转向1为左转-1为右转。这个公式怎么理解呢圆弧圆心始终位于车辆当前朝向的侧向偏移处。车辆朝向的单位方向是(cos(yawf), sin(yawf))左转时圆心在左侧左侧法向量是(-sin(yawf), cos(yawf))右转时圆心在右侧右侧法向量是(sin(yawf), -cos(yawf))。乘上半径rho就得到上面的表达式。有了两个圆心接下来就是求切点。同向转向LL或RR取外公切线异向转向LR或RL取内公切线。等半径情况下外公切线的两个切点可以这样求vx o3x - o1x vy o3y - o1y d math.hypot(vx, vy) if d 2.0 * rho: return None # 两圆相交甚至重叠无公切线 nx -vy / d # 垂直于连心线方向的单位向量 ny vx / d t1x o1x rho * nx t1y o1y rho * ny t2x o3x rho * nx t2y o3y rho * ny这里的n (-vy/d, vx/d)是连心线逆时针旋转90度得到的单位向量。用正负号分别对应两条外公切线实际应该选哪条取决于第一段圆弧起始角度到切点的旋转方向是否和车辆转向一致。我的经验是两条都试构造出完整RSPath后再检查终点误差把不合法的那条丢弃。内公切线的公式稍麻烦一点。设连心线方向角为phi atan2(vy, vx)等半径情况下可以推导出切点相对圆心的方向角是phi ± α其中α asin(2*rho/d)。两个切点分别位于两圆“对侧”具体公式在代码里实现为alpha math.asin(2.0 * rho / d) base math.atan2(vy, vx) theta_t1 base alpha # 或者 base - alpha取决于方向组合 theta_t2 base - alpha t1x o1x rho * math.cos(theta_t1) t1y o1y rho * math.sin(theta_t1) t2x o3x rho * math.cos(theta_t2) t2y o3y rho * math.sin(theta_t2)内公切线符号选择是这类路径最容易写错的地方。我建议不要试图记住一个固定正负号而是在代码里把base alpha和base - alpha两种组合都生成然后通过“第一段弧从起点到切点、第二段弧从切点到终点”的连续性验证来判断取舍。这样做虽然多了一点计算量但代码逻辑清晰不容易因为方向组合遗漏而漏掉合法路径。3.3 CCC类路径的两圆交点公式与实现CCC类路径是三段圆弧连续拼接比如LRL、LR-L等。和CSC不同CCC里没有直线段所以核心问题变成给定第一段圆弧圆心、第三段圆弧圆心如何确定中间那一段圆弧的圆心位置。在标准位形下第一段圆弧圆心O1和第三段圆弧圆心O3可以直接由起点和终点位姿算出来。中间圆弧的圆心O2必须满足两个条件它到O1的距离为2*rho到O3的距离也为2*rho。为什么是2*rho因为两段反向圆弧在切点处内切圆心距等于两段圆弧半径之和即rho rho。于是问题就变成了求两个半径同为2*rho的圆的交点。代码如下D math.hypot(o3x - o1x, o3y - o1y) if D 4.0 * rho: return None # 两圆无交点不存在标准CCC解 mid_x (o1x o3x) / 2.0 mid_y (o1y o3y) / 2.0 h math.sqrt(max(0.0, (2.0 * rho) ** 2 - (D / 2.0) ** 2)) o2x1 mid_x - h * (o3y - o1y) / D o2y1 mid_y h * (o3x - o1x) / D o2x2 mid_x h * (o3y - o1y) / D o2y2 mid_y - h * (o3x - o1x) / D两个交点分别代表两种不同的绕行方式代码里必须都尝试。得到O2后三段圆弧的圆心、半径、起止极角都能确定。特别注意第一段圆弧的起点是车辆起点它在O1圆上的极角是固定的第一段圆弧的终点是切点它在O1圆上的方向角就是O1指向O2的方向角。因此第一段弧的转角可以算出来第二段弧的转角则由O2指向O1和O2指向O3两个方向的夹角决定第三段弧的转角则从O3指向O2的方向角转到终点在O3圆上的方向角。这个过程中最容易出问题的是三个角度之间的衔接不是简单的“取绝对差”而必须考虑旋转方向。比如LRL的第一段是左转那么从起点位置在圆上的极角转到O1-O2方向极角必须逆时针绕行所以角度增量要用“目标角减起始角再取模”。我在实际代码里会把每个弧段封装成一个函数输入圆心、起点、终点、方向输出弧长和关键角度。这样不管三段怎么组合代码都不会出现混乱的正负号分支。3.4 带拐点构型的处理思路真实场景中大概有90%以上的起终点位姿可以用CSC或标准的CCC类路径覆盖剩下那些比较刁钻的组合对应的是带拐点也就是中间某处车速方向反转的类型比如C|C|C、CC|C等。带拐点构型的公式推导在前两部分已经讲过代码实现上不需要另起炉灶。我的做法是复用前面写的两圆交点求解函数只是在构造某些弧段时把“连续绕行”改成“先绕行一段然后在Alphabetic类型里标记掉的掉头点位置改变曲率方向”。具体到代码通常是给construct_path传入一个word字符串比如LR-L然后用一个循环解析这个字符串遇到|就切换速度方向遇到字母就调用对应的圆弧或直线基元。这里想分享一个实用经验与其手写48个构造函数不如把8到9个基础构型风格写成一个通用模板用参数控制每一段的转向、长度以及是否存在中点掉头。比如CCC类可以写成一个construct_three_arc(o1, o3, direction1, direction2, direction3, flip)函数flip控制中间掉头变体。这样既保留了“每种路径都由公式精确构造”的优点又避免了代码大量重复。4. 路径插值、误差评估与最优路径筛选4.1 从路径段到密集路点公式构造出来的路径只是一堆符号描述真正要交给控制器或可视化模块的是离散化后的路点序列。插值逻辑也不复杂对每个Segment根据它的类型和长度以固定步长生成中间点。圆弧段插值时要特别注意每一步的角度不能直接用线性插值start t * (end - start)因为这样在跨过±π边界时会跳变。正确做法是先算好带方向的增量左转正、右转负然后theta start_theta direction * step_dist / rho x center[0] rho * math.cos(theta) y center[1] rho * math.sin(theta)这样算出来的点每间隔固定弧长一个控制器使用起来非常方便。直线段插值就简单了直接按比例在首尾两个切点之间均匀取点即可。4.2 终点误差检查与阈值设置暴力枚举路径类型时一定会出现一些几何上“看似合法”但实际拼接后整体误差很大的情况。例如CSC切点选错了方向生成的第一段弧或第三段弧虽然角度计算正确但拼接出来的路径终点位置和实际目标差了一截。所以主流程里必须有终点误差检查。我的实现是构造完候选路径后顺着路径的每一段做一次正向递推从起点开始累加位置和朝向算到路径终点再和目标(xf, yf, yawf)比较def endpoint_error(path, xf, yf, yawf): # 对path按段逐步积分得到终点位姿 ex end_x - xf ey end_y - yf e_yaw normalize_angle(end_yaw - yawf) return math.hypot(ex, ey) abs(e_yaw) * 0.1位置误差和角度误差的加权系数我取0.1主要是为了让角度误差和位置误差在同一个量级比较。实际使用中只要位置误差小于1e-3米、角度误差小于1e-3弧度就可以认为这条路径是合法的。如果候选路径连这个基本检查都过不了直接丢弃不需要参与后续长度比较。4.3 为什么“最近”不一定等于“最短”筛选最优路径时最直接的想法是比较total_length取最小。但在实际工程里我建议把误差检查放在长度比较之前否则可能出现某条路径理论长度很短但终点误差为零点几米的情况这种路径是不能用的。另外还要注意有些路径类型会在枚举结果里出现“重复等价”的情况。比如在某些特殊位姿下LSR和RSL可能生成完全相同的路径只是绕行方向不同长度也相同。这种情况下保留哪一条都行但如果你在代码里加了“只保留第一条满足误差的路径”的剪枝逻辑就有可能丢掉真正的最短路径。我的习惯是绝不提前剪枝让所有类型都构造完、评估完最后统一比较。5. 实测排坑与工程细节5.1 角度归一化的坑这个坑几乎每个人都会踩。atan2返回的角度范围在-π到π之间但你计算弧长时需要的角度范围可能是0到2π也可能是任意带符号的角度。如果不做归一化就会出现本该是“转5度”的地方算出来是“转355度”的情况导致路径绕超大一圈。我的统一约定是所有涉及“两点之间的方位角”的地方全部先经过normalize_angle处理所有涉及“旋转增量”的地方按照转向方向做0到2π的正向模运算。这个约定写进编码规范后角度相关的bug至少减少八成。5.2 切点方向选错的坑CSC类型的两条公切线都合法但只有一条能构成正确的行驶方向。我见过很多实现把公切线公式固定死结果在某些顶点的yaw组合下结果路径在切点附近出现车体朝向不连续。排查这类问题最快的方法是可视化。把两段圆弧的圆心、起点、终点全部画出来你一眼就能看出切点方向是不是反了。如果没有可视化工具可以在代码里打印每个切点在圆上的极角以及第一段弧的起点极角手动检查角度增量是否符合转向方向。5.3 最小转弯半径与4rho边界检查CCC类型求解时O1和O3之间的距离如果大于4*rho两圆就没有交点标准CCC解不存在。但实际车辆位姿完全可能出现D略大于4*rho的情况这时如果草率跳过CCC只靠CSC可能找不到最优解甚至找不到可行解。我的建议是在D 4*rho时不要直接return None可以尝试带拐点的CCC变体或者把半径稍微放宽一点点重新解一次。当然放宽半径意味着车辆实际行驶轨迹可能超过最小转弯半径约束所以这个放宽只能作为“候选路径”参与比较最终输出前要额外校验每段弧的曲率不超过车辆极限。5.4 可视化调试的小技巧最后分享一个非常实用的调试技巧。Reeds-Shepp曲线这种几何规划算法最适合用可视化来验证。我在开发阶段写了一个很轻量的matplotlib脚本输入起点、终点、转弯半径把所有候选路径全部画出来包括每段弧的圆心、切点、圆弧轨迹然后用不同颜色区分路径类型。调试效率提升非常大。一旦发现某个起终点组合下所有候选路径都不合法就把这个组合固定成单元测试用例每次改完代码都跑一遍。这个做法帮我抓出了很多只在特殊位姿下才出现的边界bug也是我推荐每个人都养成的习惯。Reeds-Shepp曲线这块公式推导是基础但真正让算法稳定的是那些藏在角度处理、切点方向、边界检查里的细节。这些坑我基本都在项目里踩过一遍希望这篇第三部分能帮你少走些弯路。