企业官网建设流程全解析

Reeds-Shepp曲线的公式推导难吗？说实话，数学本身不算难，难的是理清思路。我第一次动手实现RS曲线时，对着别人源码里几十个if分支发懵：一条最短路径，为什么要拆成这么多情况？后来想明白了，RS曲线的核心思路就六个字：暴力枚举加数学构造。这篇文章是系列第二篇，重点解决从公式到代码的两道坎：第一，L+S+L+、L+S+R+这类基础路径的参数公式是怎么推出来的；第二，怎么把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、RSR，CCC类型有两个骨架LRL、RLR，再加上CCSC、CSCC这类包含直线和连续圆弧的复合骨架，一共是12类标准型。你翻开LaValle那本《Planning Algorithms》第15章的表格，看到的就是这12类，符号写成L+S+L+、L+S+R+、L+S+L−这类形式。上标加号表示前进，上标减号表示后退。

这个化简的意义很实在：我们不需要为48种路径写48套公式，只需要为12类代表路径各写一套公式，剩下36种通过坐标变换和符号变换去复用。这也是为什么开源项目里Reeds-Shepp实现的代码量看起来很大、但仔细看逻辑非常规整，因为每一类模板函数都在处理一个“标准代表”。

2. 三段式路径的公式推导：L+S+L+ 与 L+S+R+

2.1 统一前提：归一化、坐标变换、圆心构造

进入推导之前，先统一几个前提，这些约定直接决定后面公式的样子。假设车辆最小转弯半径为rho，第一步先把所有坐标除以rho，让最小转弯半径归一化为1。计算完路径长度后再乘回rho，这样公式里不会到处都是rho，清爽很多。

位姿仍然表示成(x, y, theta)，其中theta是车辆朝向与x轴的夹角。为了推导方便，把起点固定在原点、朝向x轴正方向，也就是起点位姿为(0, 0, 0)。任何一般位姿都可以通过平移和旋转变换到这个标准形式，算出来的路径再变换回去，物理意义不变。

这里最核心的几何工具是“圆心构造”。车辆以最小转弯半径左转时，圆心一定在车辆左侧一个半径处；右转时，圆心在右侧一个半径处。归一化半径后，这个距离就是1。比如起点在原点、朝向x轴正方向，那么第一段左转圆弧的圆心就在(0, 1)。这个“拿圆规画圆”的直觉，后面所有推导都建立在这个关系上。

2.2 L+S+L+：外公切线情形的完整推导

先推导最简单的三段式路径L+S+L+，也就是前进左转、前进直行、前进左转。这里三个动作全部是前进方向，上标都是加号。这段路径的几何结构是：两段左转圆弧被一段直线连接起来。

设三段参数分别为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弧度。

到这里L+S+L+的参数公式就全部推出来了。注意：这个推导要求t、u、v全部非负，这条路径才是可用的候选。比如终点位姿是(5, 0, pi/2)，代入算出来t是负的，说明这条路不成立，直接丢掉。

2.3 L+S+R+：内公切线情形与增根陷阱

L+S+L+是外公切线情形，L+S+R+就变成了内公切线情形。左转接直线再接右转，两段圆弧圆心分别在直线的两侧。工程上L+S+R+的出现频率很高，但它比L+S+L+麻烦不少。

第一段还是左转，起点圆心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 theta，B = y - cos theta - 1，得到方程组：

u cos t = A - 2 sin t u sin t = B + 2 cos t

这比L+S+L+复杂，因为两个方程的右边都还带着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，否则这个方程无解，L+S+R+路径不存在。这个条件其实有很直观的几何意义：两个半径为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。第二，某些路径还要满足额外的几何不等式，比如L+S+R+要求R大于等于2。第三，有的情况下需要处理角度归一化，避免因为角度多转一圈产生虚假的正值。比如theta原始值是3pi/2，不归一化直接参与计算，可能算出一个很大的t和v，虽然也满足非负，但那条路径可能转了好几圈，明显不是最短路径。

所以在实现时，所有角度在用之前都做一次wrap到(-pi, pi]的处理，避免周期性问题。边界上的合法解也要小心，比如t恰好等于0，这时“圆弧”退化为一个点，路径类型实际上退化成了L+S+或者S+L+，这种退化路径在数值上可能出现在临界位姿附近，代码需要能正确处理。

3. 路径搜索框架：把公式变成“暴力枚举”

3.1 最短路径搜索的总流程

有了单个路径类型的公式，整个搜索流程就非常简单了。把起点和终点经过刚体变换，转换到标准坐标系下，然后逐个调用12类标准路径的模板函数，每个模板函数返回一个候选路径对象。如果某类路径在当前位姿下不合法，模板函数返回空。最后在所有非空候选中，比较路径总长度，取最小值。

这个流程的好处是把“推导公式”和“组合搜索”彻底解耦。每个模板函数只负责一件事：针对某一种单词类型，计算它的三段或五段参数，并检查合法性。搜索层完全不关心具体公式是什么，只做枚举和比较。这也就解释了为什么RS曲线实现代码可以写得很模块化，后续想加新型路径、想换数值精度都容易。

需要注意，暴力枚举虽然“暴力”，但12类模板函数每个都会执行几个atan2、sqrt、sin、cos，计算量非常小，一微秒级别的耗时在普通CPU上就能跑完。实际工程中RS曲线通常作为局部规划器的一步，实时性完全不用担心。

3.2 用数学构造处理CCC类路径

刚才推导的L+S+L+和L+S+R+都是含直线段的CSC类型。但最短路径有时候不需要直线，三段全是圆弧，比如L+R−L+和R+L−R+。这类CCC路径的处理思路和CSC不一样，核心是“圆交点构造”。

拿L+R−L+来说，三段圆弧半径都是1，第一段左转、第二段右转、第三段左转。第一段圆弧的圆心由起点位姿唯一确定，第三段圆弧的圆心由终点位姿唯一确定。第二段圆弧是右转，圆心必须同时满足两个条件：到第一段圆心距离为2，到第三段圆心距离也为2。这实际上就是求两个圆的交点。

圆心距为2的两个圆，交点可以解出来，一般有两个。每个交点对应一条不同的中间圆弧路径，分别算一遍，检查三段圆弧的转向方向是否与L、R、L一致，转角是否非负，然后保留合法路径。两个交点解出来之后，三段圆弧各自的转角用圆心夹角关系计算。这个思路就是典型的“数学构造”：不求解优化问题，而是把几何关系变成圆的求交运算。

CCC类型的公式推导虽然也涉及反三角函数，但所有步骤都可以通过尺规作图的逻辑一步步推下来，比CSC类更直观。缺点是需要处理三角形解的存在性，比如两个圆心距离大于4时无解，因为半径2加半径2够不到。这时路径不存在，返回空即可。

3.3 从12类到48种：对称变换的工程实现思路

前面提到12类标准路径是化简后的结果，那完整48种怎么办？工程上有两种做法。第一种简单粗暴：把12类标准路径的公式各写一遍，再单独写48种路径的枚举入口。第二种更优雅：只实现少量基准模板，其他路径类型通过坐标变换复用基准模板。

比如L−S−L−和L+S+L+的关系，前者等于把后者按时间反转，也就是从终点倒着走回起点。在代码里，可以先把终点和起点对调，调用L+S+L+的模板函数，再把生成的路径段方向整体取反。同理，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 核心模板函数实现

先实现L+S+L+，这个函数代码很短，几乎就是把推导结果直接翻译过来：

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，这是给浮点误差留余地，避免边界情况被误杀。

L+S+R+稍微复杂，因为有增根处理：

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, rho=1.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, key=lambda 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正方向的定义可能不一样。有的定义逆时针为正，有的定义顺时针为正，左转右转的半径向量方向也会跟着变。我建议在自己实现的文件头部用注释写清楚整套约定，并且每篇博客、每个函数都保持一致。我在调L+S+R+时，就因为圆心方向写反，找了一晚上bug，最后是random test抓出来的。

5.2 临界值处理：R=2、u=0、角度差为0

临界值是RS曲线实现里最容易翻车的地方。R等于2时，L+S+R+的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 一份常见问题速查表

这套速查表是我实际调代码时积累下来的。强烈建议每踩一个坑就往表里加一行，等表里的内容积累到二十条左右，你对RS曲线的理解就会比大部分照抄代码的人深得多。

最后再说一个我个人的体会：Reeds-Shepp曲线这东西，光看公式很容易觉得自己懂了，一写代码就露馅。尤其是L+S+R+那种带增根问题的类型，手推公式和代码验证完全是两回事。想真正掌握它，最好的路径就是像我这样：先推一个最简单的L+S+L+，再推一个带坑的L+S+R+，然后搭一个暴力枚举框架，最后用随机位姿把算法按在地上摩擦几天。等这一套流程走完，后面再学其他运动规划算法，你会觉得瓶颈根本不在公式推导本身，而在于如何把几何约束干净地翻译成代码逻辑。