Reeds-Shepp曲线的公式推导难吗说实话数学本身不算难难的是理清思路。我第一次动手实现RS曲线时对着别人源码里几十个if分支发懵一条最短路径为什么要拆成这么多情况后来想明白了RS曲线的核心思路就六个字暴力枚举加数学构造。这篇文章是系列第二篇重点解决从公式到代码的两道坎第一LSL、LSR这类基础路径的参数公式是怎么推出来的第二怎么把12类标准候选路径组织成一套可复现的代码。适合正在做机器人局部规划、想脱离“抄库”状态、真正自己写出reeds_shepp_shortest_path的读者。1. 准备知识从运动基元到12类标准路径1.1 先想清楚RS曲线为什么能暴力枚举很多人第一次接触Reeds-Shepp曲线时会期待一个类似Dubins曲线的“解析公式”给定起点终点位姿一步算出最短路径。但这个期待很快会落空因为RS曲线比Dubins曲线多了一个“倒车”能力。车辆可以前进、可以后退、又可以左转右转路径组合的数量立刻爆炸。Reeds和Shepp在1990年的论文里证明了这样一个关键结论满足最小转弯半径约束、允许前进和后退的车辆从任意位姿到任意位姿的最短路径一定可以被表示为有限个基本运动基元的组合而且这些组合是有限的。所谓基本运动基元就是三种动作直线行驶、以最小转弯半径左转、以最小转弯半径右转。每个动作可以前进也可以后退。这个有限性结论正是“暴力枚举”的底气。既然最短路径一定跑不出那个有限的单词集合那我们就不需要在线求解复杂的优化问题只需要把集合里的每种路径类型都算一遍比较出最短的那个就行了。这听着像是笨办法但实际非常可靠因为它不依赖初值、不会陷入局部最优更不需要反复迭代。工程实现上这就是一套模板函数加一个min操作。1.2 48种词法如何化简成12类对称性砍掉一大半如果直接按照“三基元 × 双方向 × 段数组合”去穷举路径数量远不止几十种。Reeds-Shepp论文给出的是48种单词工程上更常用的是化简后的12类标准路径。这个化简靠的是对称性。路径规划里常见四种对称变换。第一种是时间翻转把一条路径从终点往起点倒着走前进变后退后退变前进路径长度不变。第二种是左右反射把左转全部换成右转右转全部换成左转路径长度同样不变。第三种是方向反转把整条路径的前进后退状态全部取反。第四种是刚体变换把整个路径平移到任意位置、旋转到任意朝向。利用这几种性质可以把大量看似不同的单词归并为同一类。具体来说CSC类型有四个骨架分别是LSL、LSR、RSL、RSRCCC类型有两个骨架LRL、RLR再加上CCSC、CSCC这类包含直线和连续圆弧的复合骨架一共是12类标准型。你翻开LaValle那本《Planning Algorithms》第15章的表格看到的就是这12类符号写成LSL、LSR、LSL−这类形式。上标加号表示前进上标减号表示后退。这个化简的意义很实在我们不需要为48种路径写48套公式只需要为12类代表路径各写一套公式剩下36种通过坐标变换和符号变换去复用。这也是为什么开源项目里Reeds-Shepp实现的代码量看起来很大、但仔细看逻辑非常规整因为每一类模板函数都在处理一个“标准代表”。2. 三段式路径的公式推导LSL 与 LSR2.1 统一前提归一化、坐标变换、圆心构造进入推导之前先统一几个前提这些约定直接决定后面公式的样子。假设车辆最小转弯半径为rho第一步先把所有坐标除以rho让最小转弯半径归一化为1。计算完路径长度后再乘回rho这样公式里不会到处都是rho清爽很多。位姿仍然表示成(x, y, theta)其中theta是车辆朝向与x轴的夹角。为了推导方便把起点固定在原点、朝向x轴正方向也就是起点位姿为(0, 0, 0)。任何一般位姿都可以通过平移和旋转变换到这个标准形式算出来的路径再变换回去物理意义不变。这里最核心的几何工具是“圆心构造”。车辆以最小转弯半径左转时圆心一定在车辆左侧一个半径处右转时圆心在右侧一个半径处。归一化半径后这个距离就是1。比如起点在原点、朝向x轴正方向那么第一段左转圆弧的圆心就在(0, 1)。这个“拿圆规画圆”的直觉后面所有推导都建立在这个关系上。2.2 LSL外公切线情形的完整推导先推导最简单的三段式路径LSL也就是前进左转、前进直行、前进左转。这里三个动作全部是前进方向上标都是加号。这段路径的几何结构是两段左转圆弧被一段直线连接起来。设三段参数分别为t、u、v。t是第一段左转圆弧的转角u是直线段长度v是第二段左转圆弧的转角。归一化半径下圆弧长度等于转角弧度。第一段圆弧从起点开始起点切线方向是0转过t弧度后切线方向角变成t。第一段圆弧的圆心O1是(0, 1)。圆弧上任意一点满足这个关系如果该点处切线方向角为α那么从圆心到该点的向量是(sin α, -cos α)。所以第一段结束时位置P1就等于P1 O1 (sin t, -cos t) (sin t, 1 - cos t)这个式子不复杂但建议自己动手推一遍画个起点在圆心的右侧、车辆逆时针运动的图很快就能验证。第二段直线沿着方向角t走长度u所以直线段终点P2 P1 u * (cos t, sin t)。第三段还是左转圆弧。终点位姿是(x, y, theta)车辆到达终点时朝向theta。左转圆弧的圆心在终点的左侧一个半径处所以第三段圆弧圆心O2是O2 (x cos(theta pi/2), y sin(theta pi/2)) (x - sin theta, y cos theta)第三段圆弧在起点处的切线方向角也必须是t因为要和直线段平滑衔接。利用同样的“圆心到点向量”关系第三段起点P2可以写成P2 O2 (sin t, -cos t) (x - sin theta sin t, y cos theta - cos t)现在把P2的两个表达式联立起来直线段向量等于终点减起点x - sin theta sin t - sin t u cos t y cos theta - cos t - (1 - cos t) u sin t化简后得到一个非常干净的方程组x - sin theta u cos t y cos theta - 1 u sin t这个方程组几何含义很明显向量(u cos t, u sin t)就是直线段的方向和长度它的两个分量分别等于(x - sin theta, y cos theta - 1)。所以直接解出u sqrt((x - sin theta)^2 (y cos theta - 1)^2) t atan2(y cos theta - 1, x - sin theta) v theta - t为什么v等于theta减t因为第三段左转圆弧从切线方向t开始转到终点切线方向theta逆时针转过theta减t弧度。到这里LSL的参数公式就全部推出来了。注意这个推导要求t、u、v全部非负这条路径才是可用的候选。比如终点位姿是(5, 0, pi/2)代入算出来t是负的说明这条路不成立直接丢掉。2.3 LSR内公切线情形与增根陷阱LSL是外公切线情形LSR就变成了内公切线情形。左转接直线再接右转两段圆弧圆心分别在直线的两侧。工程上LSR的出现频率很高但它比LSL麻烦不少。第一段还是左转起点圆心O1还是(0, 1)P1还是(sin t, 1 - cos t)。第三段换成右转圆弧。右转时圆心在车辆右侧一个半径处所以终点处圆心O2是O2 (x cos(theta - pi/2), y sin(theta - pi/2)) (x sin theta, y - cos theta)右转圆弧上任意一点处如果切线方向角是α那么从圆心到该点的向量是(-sin α, cos α)。注意这里的负号和左转相反。所以第三段起点P2等于P2 O2 (-sin t, cos t) (x sin theta - sin t, y - cos theta cos t)联立直线段关系x sin theta - sin t - sin t u cos t y - cos theta cos t - (1 - cos t) u sin t整理一下令A x sin thetaB y - cos theta - 1得到方程组u cos t A - 2 sin t u sin t B 2 cos t这比LSL复杂因为两个方程的右边都还带着sin t和cos t。怎么消掉t用第二个方程乘以cos t、减去第一个方程乘以sin t这个过程可以直接手推验证最后得到一个非常关键的条件式A sin t - B cos t 2这个式子可以用三角恒等式继续化简。令R sqrt(A^2 B^2)令alpha atan2(B, A)那么A sin t - B cos t等于R sin(t - alpha)。于是sin(t - alpha) 2 / R到这里就能看出约束条件了必须有R大于等于2否则这个方程无解LSR路径不存在。这个条件其实有很直观的几何意义两个半径为1的圆做内公切线圆心距至少要达到2否则公切线画不出来。当R大于等于2时得到两个候选解t1 alpha asin(2 / R) t2 alpha pi - asin(2 / R)u可以直接算出来u sqrt(R^2 - 4)这里就踩到了第一个真正的深坑。按方程平方关系推导u确实是sqrt(R^2 - 4)但你拿着这个u直接代入原方程组经常发现符号对不上。原因在于“平方消元”会引入增根sin(t - alpha) 2/R的两个解里只有一个是真正满足原方程组符号约束的。按我上面举的例子t2算出来之后对应向量方向其实和直线段方向差了一个pi也就是说u应该是负的但u在物理上必须非负所以这个候选不能用。解决办法很简单每个候选t都做一次方向一致性检查。具体做法是先算出向量(px, py) (A - 2 sin t, B 2 cos t)然后计算它和方向向量(cos t, sin t)的点积。如果点积大于0说明方向一致候选合法如果点积小于0说明实际方向反了直接丢弃。这个检查必须放在t、u、v非负检查之前因为方向反了的情况下后面所有判断都是错的。第三段的转角v右转意味着朝着角度减小的方向转所以v t - theta。同样要求非负。2.4 参数合法性判定不是所有解都能用很多初学者会在这一步犯迷糊公式明明推出来了怎么代入一些位姿得不到路径这就是合法性判定在起作用。每条路径类型都有自己适用的位姿范围Reeds-Shepp的12类标准路径合在一起才覆盖整个位姿空间。合法性判定一般有三条。第一每一段的长度或转角必须非负即t大于等于0、u大于等于0、v大于等于0。第二某些路径还要满足额外的几何不等式比如LSR要求R大于等于2。第三有的情况下需要处理角度归一化避免因为角度多转一圈产生虚假的正值。比如theta原始值是3pi/2不归一化直接参与计算可能算出一个很大的t和v虽然也满足非负但那条路径可能转了好几圈明显不是最短路径。所以在实现时所有角度在用之前都做一次wrap到(-pi, pi]的处理避免周期性问题。边界上的合法解也要小心比如t恰好等于0这时“圆弧”退化为一个点路径类型实际上退化成了LS或者SL这种退化路径在数值上可能出现在临界位姿附近代码需要能正确处理。3. 路径搜索框架把公式变成“暴力枚举”3.1 最短路径搜索的总流程有了单个路径类型的公式整个搜索流程就非常简单了。把起点和终点经过刚体变换转换到标准坐标系下然后逐个调用12类标准路径的模板函数每个模板函数返回一个候选路径对象。如果某类路径在当前位姿下不合法模板函数返回空。最后在所有非空候选中比较路径总长度取最小值。这个流程的好处是把“推导公式”和“组合搜索”彻底解耦。每个模板函数只负责一件事针对某一种单词类型计算它的三段或五段参数并检查合法性。搜索层完全不关心具体公式是什么只做枚举和比较。这也就解释了为什么RS曲线实现代码可以写得很模块化后续想加新型路径、想换数值精度都容易。需要注意暴力枚举虽然“暴力”但12类模板函数每个都会执行几个atan2、sqrt、sin、cos计算量非常小一微秒级别的耗时在普通CPU上就能跑完。实际工程中RS曲线通常作为局部规划器的一步实时性完全不用担心。3.2 用数学构造处理CCC类路径刚才推导的LSL和LSR都是含直线段的CSC类型。但最短路径有时候不需要直线三段全是圆弧比如LR−L和RL−R。这类CCC路径的处理思路和CSC不一样核心是“圆交点构造”。拿LR−L来说三段圆弧半径都是1第一段左转、第二段右转、第三段左转。第一段圆弧的圆心由起点位姿唯一确定第三段圆弧的圆心由终点位姿唯一确定。第二段圆弧是右转圆心必须同时满足两个条件到第一段圆心距离为2到第三段圆心距离也为2。这实际上就是求两个圆的交点。圆心距为2的两个圆交点可以解出来一般有两个。每个交点对应一条不同的中间圆弧路径分别算一遍检查三段圆弧的转向方向是否与L、R、L一致转角是否非负然后保留合法路径。两个交点解出来之后三段圆弧各自的转角用圆心夹角关系计算。这个思路就是典型的“数学构造”不求解优化问题而是把几何关系变成圆的求交运算。CCC类型的公式推导虽然也涉及反三角函数但所有步骤都可以通过尺规作图的逻辑一步步推下来比CSC类更直观。缺点是需要处理三角形解的存在性比如两个圆心距离大于4时无解因为半径2加半径2够不到。这时路径不存在返回空即可。3.3 从12类到48种对称变换的工程实现思路前面提到12类标准路径是化简后的结果那完整48种怎么办工程上有两种做法。第一种简单粗暴把12类标准路径的公式各写一遍再单独写48种路径的枚举入口。第二种更优雅只实现少量基准模板其他路径类型通过坐标变换复用基准模板。比如L−S−L−和LSL的关系前者等于把后者按时间反转也就是从终点倒着走回起点。在代码里可以先把终点和起点对调调用LSL的模板函数再把生成的路径段方向整体取反。同理R开头的路径类型可以通过左右反射变换映射到L开头的基准模板。实际开源库里多数选择折中一部分类型显式实现另一部分通过反射、时间反转复用。工程实现的第一版不建议一上来就做高密度复用因为很容易把符号和方向搞混。先把12类模板全部写出来通过测试之后再考虑优化合并这个顺序更稳妥。4. 代码实现一个能跑的最小版本4.1 数据结构和模板函数怎么写先说数据结构。每个路径段需要记录三样东西这段的长度圆弧段就是转角弧度直线段就是直线距离、转向类型L、S、R、方向前进为1后退为-1。整个路径就是路径段的列表再保存一个总长度。用Python写一个最小实现这几个类足够import math class PathSegment: def __init__(self, length, steer, direction): self.length length self.steer steer self.direction direction class ReedsSheppPath: def __init__(self, segments): self.segments segments self.total_length sum(seg.length for seg in segments)模板函数接收归一化后的(x, y, theta)返回一个ReedsSheppPath对象不合法就返回None。这里theta已经经过角度归一化x、y已经除以最小转弯半径。4.2 核心模板函数实现先实现LSL这个函数代码很短几乎就是把推导结果直接翻译过来def path_LpSpLp(x, y, theta): dx x - math.sin(theta) dy y math.cos(theta) - 1.0 u math.hypot(dx, dy) t math.atan2(dy, dx) v theta - t if t -1e-9 or u -1e-9 or v -1e-9: return None return ReedsSheppPath([ PathSegment(t, L, 1), PathSegment(u, S, 1), PathSegment(v, L, 1), ])注意判断条件用了-1e-9而不是0这是给浮点误差留余地避免边界情况被误杀。LSR稍微复杂因为有增根处理def path_LpSpRp(x, y, theta): A x math.sin(theta) B y - math.cos(theta) - 1.0 R math.hypot(A, B) if R 2.0 - 1e-9: return None alpha math.atan2(B, A) delta math.asin(2.0 / R) best None for t in (alpha delta, alpha math.pi - delta): px A - 2.0 * math.sin(t) py B 2.0 * math.cos(t) # 方向一致性检查排除平方增根 if px * math.cos(t) py * math.sin(t) 0: continue u math.sqrt(R * R - 4.0) v t - theta if t -1e-9 or u -1e-9 or v -1e-9: continue cand ReedsSheppPath([ PathSegment(t, L, 1), PathSegment(u, S, 1), PathSegment(v, R, 1), ]) if best is None or cand.total_length best.total_length: best cand return best这段代码里有两个细节值得反复琢磨。一是两个候选t都要看不能只取第一个二是方向一致性检查必须在构造路径之前做否则生成出来的路径终点对不上。4.3 总入口与路径合法性检查总入口做三件事归一化、枚举全部模板、取最短。模板先只放我们已经写好的两个函数后续把其余10类补进来即可def reeds_shepp_shortest_path(x, y, theta, rho1.0): x / rho y / rho theta math.atan2(math.sin(theta), math.cos(theta)) candidates [ path_LpSpLp(x, y, theta), path_LpSpRp(x, y, theta), # 后续在这里补上其余10类标准路径 ] valid [cand for cand in candidates if cand is not None] if not valid: return None best min(valid, keylambda cand: cand.total_length) for seg in best.segments: seg.length * rho best.total_length * rho return best有人会问模板函数返回None到底意味着什么它表示这类路径在当前位姿下不可行可能是几何条件不满足也可能是角度方向不对。这是正常的不是bug。正因为每一类路径只覆盖一部分位姿空间所以才需要用12类模板去互补覆盖。4.4 调试利器随机位姿验证与插值写完核心路径搜索后千万别急着跑可视化。第一步先做“终点验证”。写一个插值函数把路径按小步长采样从起点开始模拟车辆运动看最终能不能精确落到目标位姿。这一步能暴露大部分公式错误和符号错误。插值逻辑不复杂但需要注意方向。本文示例代码先覆盖全前进方向的情况后退段的插值需要额外处理坐标翻转这个我们放到系列第三篇再展开。def interpolate_end(path): x, y, theta 0.0, 0.0, 0.0 for seg in path.segments: s seg.length d seg.direction if seg.steer S: x d * s * math.cos(theta) y d * s * math.sin(theta) elif seg.steer L: x d * (math.sin(theta d * s) - math.sin(theta)) y d * (-math.cos(theta d * s) math.cos(theta)) theta d * s elif seg.steer R: x d * (math.sin(theta - d * s) - math.sin(theta)) y d * (math.cos(theta - d * s) - math.cos(theta)) theta - d * s return x, y, theta这个函数对全前进路径是正确的。然后随机生成几百组起点终点位姿调用reeds_shepp_shortest_path再调用interpolate_end验证终点误差。如果误差超过1e-6说明公式或者实现有问题。这套验证流程强烈建议保留在工程里后续做C移植或者版本迭代时能省下大量排查时间。5. 踩坑记录与调试心得5.1 角度归一化与符号约定RS曲线实现里80%的bug都出在角度处理上。theta可能是任意实数如果不做归一化atan2计算出来的结果和期望的路径转角很容易差2pi的整数倍。比如终点朝向是5pi/2和pi/2其实是同一个方向但不归一化会让公式里某些三角项符号异常导致路径类型误判。我个人的习惯是所有角度进入模板函数之前统一用math.atan2(math.sin(theta), math.cos(theta))做一次wrap。这样后续所有关于v theta - t的判断都基于一个单调的周期内逻辑简单很多。符号约定是另一个大坑。不同论文、不同代码库里theta正方向的定义可能不一样。有的定义逆时针为正有的定义顺时针为正左转右转的半径向量方向也会跟着变。我建议在自己实现的文件头部用注释写清楚整套约定并且每篇博客、每个函数都保持一致。我在调LSR时就因为圆心方向写反找了一晚上bug最后是random test抓出来的。5.2 临界值处理R2、u0、角度差为0临界值是RS曲线实现里最容易翻车的地方。R等于2时LSR的u等于0几何上路径退化为三段圆弧紧贴中间直线段消失。代码里如果直接用R 2作为不合法条件R恰好等于2时没有严格等于的浮点数判断就变得不稳定。所以我习惯用R 2.0 - 1e-9这个边界给误差留一点空间。u等于0的情况同样值得注意。当u接近0时路径变成CCC类型CSC和CCC两种类型会同时给出几乎一样的路径。此时枚举取最短可能因为浮点误差在两个候选之间跳变。实际工程里如果对路径平滑性有要求可以在u小于某个阈值时强制丢弃CSC候选让CCC候选接管。角度差为0的情况也很常见。起点和终点方向完全相同时某些路径的v会等于0导致路径段退化成点。这类退化路径不是错误但长度计算时要注意不要把这一个长度为0的段也做插值采样否则会出现除以0。5.3 一份常见问题速查表症状可能原因解决办法生成的路径终点误差很大角度没归一化或前进后退符号搞反统一wrap角度检查插值函数的方向与模板函数定义一致LSR在某位姿下始终无解R小于2或两个候选t都被增根检查丢弃确认R的条件打印A、B、alpha、delta定位路径长度明显偏长绕了大圈角度多转了整数圈theta没有wrap模板函数入口处强制归一化theta个别位姿下程序直接崩溃某个sqrt里出现负数R 2的临界值用容差处理不要用严格小于枚举结果在两次运行间跳变浮点误差导致两个候选长度几乎相等对微小长度差做容差比较或引入退化路径阈值这套速查表是我实际调代码时积累下来的。强烈建议每踩一个坑就往表里加一行等表里的内容积累到二十条左右你对RS曲线的理解就会比大部分照抄代码的人深得多。最后再说一个我个人的体会Reeds-Shepp曲线这东西光看公式很容易觉得自己懂了一写代码就露馅。尤其是LSR那种带增根问题的类型手推公式和代码验证完全是两回事。想真正掌握它最好的路径就是像我这样先推一个最简单的LSL再推一个带坑的LSR然后搭一个暴力枚举框架最后用随机位姿把算法按在地上摩擦几天。等这一套流程走完后面再学其他运动规划算法你会觉得瓶颈根本不在公式推导本身而在于如何把几何约束干净地翻译成代码逻辑。