尧图网站建设 
先说个我自己调代码的教训。最早实现Reeds-Shepp时我拿着网上那张路径类型表直接套公式结果在连续几个终点上算出来的最短路径比常识长了快一倍。排查到最后问题出在我没把“正方向角”和“航向角变化量”这两个概念掰清楚差一个符号就让整条路径走了大圆弧。所以说公式推导不是走流程坐标系约定、角度规范化、圆心几何关系任何一个环节含糊后面代码就是纸糊的。这篇文章是整个系列的第二部分。第一部分如果已经讲清楚了“Reeds-Shepp曲线解决什么问题”“为什么最短路径一定会落在一个有限集合里”那第二部分就更像一个几何施工队把每一种核心路径类型的公式一步步推出来再落成可以跑的代码骨架。第三部分我们再补完整路径点插值、带倒车段的枚举、以及和混合A*的配合。这篇你可以当作“从数学到代码的中间层”来读适合已经知道Reeds-Shepp大概是什么、但自己动手写实现时总是差口气的人。1. 先把公式的“地基”夯实word符号与归一化坐标很多推导文章上来就开始讲LSL、RSR怎么算但我建议你先花十分钟把地基打牢。因为后面所有公式都是在特定的坐标系、特定的符号约定下成立的换一个约定公式里的加号减号全得跟着变。1.1 三段式word表示法以及±上标最容易栽的跟头Reeds-Shepp路径的基本字母只有三个L代表左转圆弧R代表右转圆弧S代表直线。一段路径就是这几个字母的组合比如LSL就是“左转直线左转”。这个大家应该不陌生。麻烦的是上标和分隔符。完整写法里L⁺表示“左转且前进”L⁻表示“左转且倒车”而L | R这种带竖线的写法表示这里有一个尖点cusp也就是车辆在这里从前进切换到倒车或者反过来。竖线前后两个圆弧的转向可以相同也可以不同。为什么我要单独提这个因为网上很多简化版实现会把上标和竖线全部省略只保留LSL、RSR这种“无倒车”的核心类型。这对很多场景够用但它只是完整路径族的一个子集。你在看推导时如果没意识到自己看的是哪个子集第三部分一旦涉及完整枚举很容易把坐标系搞混。这篇文章第二部分推导的核心类型限定在无倒车、每段都是前进方向的六种LSL、RSR、LSR、RSL、LRL、RLR。它们是完整Reeds-Shepp路径族里最基础、最常用的一组。带倒车段的类型本质上是这六种在时间翻转、反射变换下的变体第三部分再展开。1.2 归一化把任意起终点“钉”回原点推导公式时最烦人的是起点终点任意、车辆转弯半径任意。你当然可以在每个公式里都带着半径r、带着起点坐标去算但那样推导冗长不说代码里也到处都是重复的旋转平移很容易出错。归一化的思路很粗暴把问题“变小变规矩”。车辆最小转弯半径为r我们就直接令r1。任意起点(x₁,y₁,θ₁)、终点(x₂,y₂,θ₂)我们通过坐标变换让起点变成(0,0,0)终点变成归一化后的(x,y,θ)。这样每条路径的圆弧半径都是1圆心坐标、直线长度、转角之间的关系就干净了。具体变换公式是这样的。设起点为(x₁,y₁,θ₁)终点为(x₂,y₂,θ₂)转弯半径为r。先在减去起点坐标的基础上把整个坐标系旋转到起点航向方向为x轴正方向再除以r做缩放[ \begin{bmatrix} x \ y \end{bmatrix}\frac{1}{r} \begin{bmatrix} \cos\theta_1 \sin\theta_1 \ -\sin\theta_1 \cos\theta_1 \end{bmatrix} \begin{bmatrix} x_2 - x_1 \ y_2 - y_1 \end{bmatrix} ]终点航向角同样做差[ \theta \theta_2 - \theta_1 ]算出来的终点(x,y,θ)就是所有推导和代码的输入。等你算完路径再用反变换把路径点映射回原坐标系。反变换就是先乘以r再旋转θ₁角度最后加上起点坐标。这个“先归一化、后反归一化”的套路能让所有公式和代码统一到一个坐标系里是Reeds-Shepp实现的约定俗成。1.3 对称变换只用推导一半路径的底牌另一个关键工具是对称变换。因为车辆具有左右对称性一条LSL路径的镜像就是RSR路径。所以严格来说你只需要推导出LSL、LSR、LRL这三种另外三种RSR、RSL、RLR可以直接通过左右镜像得到。数学上反射变换的实现方式是对终点做(x, -y, -θ)处理算完路径后把路径里的L和R互换。这个操作在代码里非常便宜但能省掉一半的公式推导和调试量。我在下面的推导中会先完整推导左边的三种右边的三种直接给镜像结果和关键差异点。2. CSC路径推导圆心、切线与一条直路CSC指的是“圆弧-直线-圆弧”结构包含LSL、LSR、RSL、RSR四种。这类路径的几何直观最清楚关键就是找到第一段圆弧的圆心、最后一段圆弧的圆心然后两圆心之间拉一条直线所有参数就都冒出来了。2.1 LSL从起点圆和终点圆直接读出三段参数先推最干净的LSL。归一化后起点在(0,0,0)终点在(x,y,θ)转弯半径r1。第一段是左转圆弧。车辆从原点出发一开始朝x轴正方向左转的圆心一定在车左侧、距离为1的位置也就是[ C_1 (0, 1) ]第三段也是左转圆弧。终点(x,y)处的航向是θ左转圆心在终点左侧、垂直于航向的地方。用方向向量来算终点航向的单位向量是(cosθ, sinθ)左侧垂直方向是(-sinθ, cosθ)。所以第三段圆心[ C_3 (x - \sin\theta, \ y \cos\theta) ]两圆心之间的距离就是中间直线段的长度u[ u \sqrt{(x - \sin\theta)^2 (y \cos\theta - 1)^2} ]设两圆心连线与x轴正方向的夹角为α[ \alpha \text{atan2}(y \cos\theta - 1, \ x - \sin\theta) ]第一段圆弧从航向0转到航向α左转是让航向增大所以第一段的带符号转角s₁就是α。第三段圆弧从航向α转到航向θ所以s₃是θ-α。路径总长度为|s₁| u |s₃|。这里有个细节值得注意“带符号转角”这个概念。左转为正、右转为负。因为LSL两段都是左转s₁和s₃理应为非负。如果算出来s₁或s₃是负的说明这个终点的几何结构根本不适用LSL这条路径候选就可以直接丢弃。举个数值例子。设终点为(4, 1, 0.5)那么(C_1 (0, 1))(C_3 (4 - \sin 0.5, 1 \cos 0.5) (3.5206, 1.8776))(u \sqrt{3.5206^2 0.8776^2} 3.6283)(\alpha \text{atan2}(0.8776, 3.5206) 0.2443)(s_1 0.2443, \ s_3 0.5 - 0.2443 0.2557)总长度 (0.2443 3.6283 0.2557 4.1283)。这是一个干干净净的LSL解。2.2 RSR/LSL镜像对照以及转角正负的检查逻辑RSR不用重新推。把LSL在y轴方向做个镜像反射左转圆心换到起点下方第三段圆心换到终点右侧[ C_1 (0, -1), \quad C_3 (x \sin\theta, \ y - \cos\theta) ]直线长度u、两圆心连线方向α的公式结构一模一样只是代入的坐标不同。但第一段和第三段都是右转s₁和s₃从几何上算出来应该是负值。所以在代码里RSR求解器对s₁、s₃的检查条件与LSL正好相反如果算出来是正的就返回不可行。这里我特别强调一点LSL和RSR的公式不是靠“记得加号减号”来区分的而是靠“带符号转角的检查”来自动筛选。与其死记R和L在公式里哪个取正哪个取负不如统一用带符号转角再把非负检查写成统一的约束。这个习惯能帮你避开很多符号事故。2.3 LSR/RSL公切线带来的可行性条件LSR也就是左转、直线、右转是CSC里最容易出边界问题的一类。第一段左转圆心[ C_1 (0, 1) ]第三段右转圆心。右转时圆心在终点右侧所以第三段圆心为[ C_3 (x \sin\theta, \ y - \cos\theta) ]这两圆心之间的距离记为d。因为两段圆弧的转向相反直线段是这两个半径均为1的圆的公切线。画一下几何关系就会发现直线段的长度u、两圆心距d、两圆半径之和2恰好构成一个直角三角形斜边是d直角边是u和2。所以[ u \sqrt{d^2 - 4} ]这个公式同时引出了一个硬性可行性条件d必须大于等于2。如果d 2两圆相交或重叠直的公切线不存在这条LSR路径就无解。再来看直线方向角。设两圆心连线方向角为α直线方向与圆心连线之间夹着一个角β且[ \beta \arcsin\left(\frac{2}{d}\right) \quad \text{或等价地} \quad \beta \text{atan2}(2, \sqrt{d^2 - 4}) ]对于LSR直线方向角是α β。这背后的直觉是左转圆在起点上方右转圆在终点下方附近直线段要让车辆从“左转圆右下侧”切到“右转圆左上侧”方向会比圆心连线略微偏左一点点。第一段带符号转角 (s_1 \alpha \beta)第三段带符号转角 (s_3 \theta - (\alpha \beta))。LSR中s₁应为正左转s₃应为负右转。RSL就是镜像对称直线方向角换成α - β第一段右转s₁应为负第三段左转s₃应为正。这个一正一负的差别靠代码里的符号检查自动区分就好。3. CCC路径推导三角形几何与候选解的判定CCC类路径只有两个成员LRL和RLR也就是三段圆弧连续转弯、没有直线。这种路径的几何比CSC稍微绕一点因为你需要自己确定中间那段圆弧的圆心。这个小节我重点讲清楚判定逻辑这部分也是代码里最容易出bug的地方。3.1 LRL的三角形构造以及两个候选圆心LRL的三段圆弧左转、右转、左转。第一段圆心[ C_1 (0, 1) ]第三段左转圆心[ C_3 (x - \sin\theta, \ y \cos\theta) ]关键是中间这段右转圆弧的圆心C₂。由于第一段圆弧是左转半径1第二段是右转半径1相邻两圆是外切关系所以C₁到C₂的距离等于C₂到C₃的距离都等于2。这个约束把问题变成了一个纯三角形问题已知C₁和C₃的位置求到两者距离都为2的点C₂。设C₁到C₃的距离为d。那么三角形C₁-C₂-C₃的边长分别是2、2、d。用余弦定理可以得到C₁处的内角φ[ \cos\phi \frac{2^2 d^2 - 2^2}{2 \cdot 2 \cdot d} \frac{d}{4} ] [ \phi \arccos\left(\frac{d}{4}\right) ]设C₁到C₃连线的方向角为α那么C₂相对于C₁的方向有两种可能α φ或者α - φ。为什么有两个因为“到两个定点距离都为2的点”本来就是两个圆的交点几何上有两个解。这就有意思了。很多初学者会想当然地认为取哪个解都行实际上只有一个解能满足“三段转角符号正确”的约束。比如我之前实测的一个例子终点(2, 0, 0)LRL路径的C₁(0,1)C₃(2,1)d2φ60°两个候选C₂分别位于连线上方和下方。算下来只有下方的C₂能满足“左转-右转-左转”的符号约束。3.2 带符号转角检查比死记公式稳得多确定候选C₂之后怎么算三段转角并判断可行性我的建议是别去背什么闭式公式直接用几何关系硬算。设P₁ (C₁C₂)/2P₂ (C₂C₃)/2。因为相邻圆外切切点正好在两圆心连线的中点。第一段起点S相对C₁的方向角是-π/2P₁相对C₁的方向角是atan2(P₁.y - C₁.y, P₁.x - C₁.x)。左转是逆时针所以s₁等于这两个角度的逆时针差经角度规范化后应为正。第二段P₁相对C₂的方向角记为a₁P₂相对C₂的方向角记为a₂。右转是顺时针所以s₂ normalize(a₂ - a₁)应为负。第三段P₂相对C₃的方向角记为a₃终点T相对C₃的方向角为atan2(T.y - C₃.y, T.x - C₃.x)。左转所以s₃ normalize(a_T - a₃)应为正。如果s₁、s₂、s₃的符号都满足“正、负、正”这个候选就是可行的。如果不行就试另一个候选C₂。两个都不行才返回无解。这套逻辑的好处在于它完全绕开了“什么情况下取αφ”的繁琐分支讨论。你用符号约束去做筛选公式可能不是最简的但绝对是代码里最不容易错的。3.3 RLR整个推导演示一遍RLR是LRL的镜像。第一段圆心[ C_1 (0, -1) ]第三段右转圆心[ C_3 (x \sin\theta, \ y - \cos\theta) ]中间左转圆弧圆心C₂同样是满足C₁C₂2、C₂C₃2的点。三角形构造、φ的计算方式完全一样。区别在符号检查三段转角应该是“负、正、负”也就是第一段右转、第二段左转、第三段右转。代码层面LRL和RLR可以共用一个“给定C₁、C₃、期望符号模式返回可行路径或None”的工具函数把符号模式作为参数传进去。这样可以少写一半重复代码。4. 代码骨架六个求解器与最短路径枚举公式推完现在把公式落成代码。我这里给出一个可以跑的Python骨架目标是让读者看到“公式→代码”的映射关系。第三部分会在这个骨架上补充路径点插值和完整枚举。4.1 路径数据结构和角度工具先定义三个基础东西路径段、路径对象、角度规范化函数。import math class Segment: def __init__(self, stype, direction, length): self.stype stype # L 左转圆弧 / R 右转圆弧 / S 直线 self.direction direction # 1 前进 / -1 倒车 self.length length class Path: def __init__(self, segments): self.segments segments self.total_length sum(seg.length for seg in segments)角度规范化是重中之重。我统一用normalize_angle把任意角度映射到 [-π, π) 区间这样转角的正负号一目了然。def mod2pi(x): v x % (2 * math.pi) if v 0: v 2 * math.pi return v def normalize_angle(x): v mod2pi(x math.pi) - math.pi return v注意这里有个选择我把规范化区间定在 [-π, π)而不是 [0, 2π)。这两个选择会直接影响代码里所有“期望正还是负”的判断所以一定要全文统一。4.2 CSC类求解器的Python实现以LSL为例完整实现如下def lsl_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x - math.sin(theta), y math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 u math.hypot(dx, dy) alpha math.atan2(dy, dx) s1 normalize_angle(alpha) s3 normalize_angle(theta - alpha) if s1 -1e-6 or s3 -1e-6: return None return Path([ Segment(L, 1, abs(s1)), Segment(S, 1, u), Segment(L, 1, abs(s3)), ])RSR只是把两个圆心都镜像到下方s1、s3的检查方向反过来def rsr_path(x, y, theta): cx1, cy1 0.0, -1.0 cx3, cy3 x math.sin(theta), y - math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 u math.hypot(dx, dy) alpha math.atan2(dy, dx) s1 normalize_angle(alpha) s3 normalize_angle(theta - alpha) if s1 1e-6 or s3 1e-6: return None return Path([ Segment(R, 1, abs(s1)), Segment(S, 1, u), Segment(R, 1, abs(s3)), ])LSR需要先检查两圆心距d是否大于等于2再算公切线def lsr_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x math.sin(theta), y - math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 d math.hypot(dx, dy) if d 2.0: return None alpha math.atan2(dy, dx) beta math.atan2(2.0, math.sqrt(d * d - 4.0)) lam alpha beta u math.sqrt(d * d - 4.0) s1 normalize_angle(lam) s3 normalize_angle(theta - lam) if s1 -1e-6 or s3 1e-6: return None return Path([ Segment(L, 1, abs(s1)), Segment(S, 1, u), Segment(R, 1, abs(s3)), ])RSL就是lam alpha - beta且s1应为负、s3应为正其余结构一致。这六个函数共享同一套坐标系约定输入输出格式完全统一。4.3 CCC类求解器的Python实现LRL的难点在于候选C₂的枚举和符号筛选。我把核心逻辑写成“遍历两个候选方向”的形式def lrl_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x - math.sin(theta), y math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 d math.hypot(dx, dy) if d 1e-6 or d 4.0: return None alpha math.atan2(dy, dx) phi math.acos(d / 4.0) for sign in (1.0, -1.0): c2_angle alpha sign * phi cx2 cx1 2.0 * math.cos(c2_angle) cy2 cy1 2.0 * math.sin(c2_angle) px1, py1 (cx1 cx2) / 2.0, (cy1 cy2) / 2.0 px2, py2 (cx2 cx3) / 2.0, (cy2 cy3) / 2.0 s1 normalize_angle( math.atan2(py1 - cy1, px1 - cx1) - (-math.pi / 2.0) ) a1 math.atan2(py1 - cy2, px1 - cx2) a2 math.atan2(py2 - cy2, px2 - cx2) s2 normalize_angle(a2 - a1) a3 math.atan2(py2 - cy3, px2 - cx3) a_end math.atan2(y - cy3, x - cx3) s3 normalize_angle(a_end - a3) if s1 -1e-6 or s2 1e-6 or s3 -1e-6: continue return Path([ Segment(L, 1, abs(s1)), Segment(R, 1, abs(s2)), Segment(L, 1, abs(s3)), ]) return None这里我用s2 1e-6作为“右转失败”的判断因为第二段应为负。浮点容差取1e-6基本能覆盖double计算误差又不会放过真正的符号错误。RLR的代码就是把所有圆心坐标镜像、符号检查方向反过来不再重复列举。4.4 主函数与一个手动验证过的算例主函数负责归一化、枚举求解器、选最短路径def reeds_shepp_path(x, y, theta, r1.0): xn, yn, tn x / r, y / r, theta solvers [ lsl_path, rsr_path, lsr_path, rsl_path, lrl_path, rlr_path, ] best None for solver in solvers: path solver(xn, yn, tn) if path is None: continue if best is None or path.total_length best.total_length - 1e-9: best path return best我之前手算过终点(4, 1, 0.5)在r1时的LSL路径总长度为4.1283。实际跑这段代码枚举六个求解器后选出的最短路径就是LSL总长度也正好是4.1283。这是一条很好的冒烟测试用例因为每个数值我都手推过一遍拿到代码里可以验证公式到代码的映射是否出错。另一个测试用例是终点(2, 0, 0)。手推或者拿代码跑一下你会发现最优解不是LSL而是LRL路径形状是左右各一个小圆弧、中间一段反向圆弧的S形。这个例子能检验CCC类求解器是否正确——很多实现如果CCC部分有bug在这里就会出现路径长度明显偏大或直接返回None。5. 实测中绕不开的数值细节公式和代码都齐了但这部分才是真正决定你的实现能不能在实车或者仿真里稳定跑起来的重点。5.1 角度规范化的范围选择真的会改变公式我在上面把角度统一规范化到[-π, π)并且所有“期望正/期望负”的判断都是基于这个区间。如果你把规范化改成[0, 2π)LSL的s₁、s₃判断逻辑就会出问题一个真实的-0.1弧度的右转会被映射成2π-0.1而你显然不希望把它当成一次几乎一整圈的“左转”。所以我的建议是代码里所有角度相关的地方从输入到中间变量到输出全部统一走normalize_angle不要在某个局部突然用mod2pi。如果你在调试时发现某个终点算出的最短路径诡异得长第一时间检查是不是某个转角没有被规范化直接落在区间外了。5.2 可行性判断的容差硬比较必踩坑d ≥ 2这个条件理论上很干净代码里直接写if d 2.0: return None也没毛病。但工程上我建议加一个微小容差if d 2.0 - 1e-9: return None为什么因为在边界附近d有可能是2.0000000001也可能因为浮点误差变成1.9999999999。后者的差只有1e-10但硬比较会直接丢掉一条可行的LSR路径。这种边界在Reeds-Shepp里非常常见尤其是那些“直行或者近似直行”的工况d天生就贴近2。同样的道理也适用于LRL/RLR中d 4.0的边界检查以及不同路径之间比较长短时的1e-9容差。另外还有一个更隐蔽的问题math.sqrt(d*d - 4)在d非常接近2时会算出一个接近0的数这个没问题但如果你用math.asin(2/d)当d略小于2时可能会得到nan。所以我在代码里用了math.atan2(2, sqrt(d*d-4))替代asin(2/d)就是为了避免这个数值陷阱。5.3 往返自检判断实现有没有内在bug的土办法代码写完怎么快速验证实现有没有问题我推荐一个土办法叫往返测试。选一个起点A再选一个终点B用你的实现算出从A到B的最短路径长度L1然后反过来从B到A再算一次路径长度L2。理论上Reeds-Shepp路径长度是可逆的L1和L2应该严格相等或者相差一个极小量。这个测试能一次性暴露绝大多数坐标系变换错误、符号约定错误、转角规范化错误。我第一次写完这六个求解器时跑往返测试就在一组特定终点上差了0.4追根溯源发现是RSR的s₁符号检查方向写反了。5.4 和Dubins、混合A*的边界感最后想聊一句边界感。Reeds-Shepp假设车辆可以原地切换前进倒车所以它在低速、车位入库、原地调头这类场景很有效。如果你的场景不允许倒车那应该用Dubins曲线那是同一套几何框架的“无倒车子集”。而混合A*这种规划算法本质上是把Reeds-Shepp当作“从当前节点到终点的启发式连接段”来用并不会把整条路径都交给Reeds-Shepp去规划。原因也很简单Reeds-Shepp完全不考虑障碍物它给出的是一条几何最短路径不是一条可通行的路径。后者还需要碰撞检测、轨迹平滑、速度分配来兜底。所以读到这里的你如果正准备把这段代码塞进一个完整的规划系统里我的建议是先把这个几何求解器当独立模块测试好再横向集成。Reeds-Shepp这个模块的输出是“一段带转向和方向的路径”不是“一条可跟踪轨迹”。有了这个认知后面接平滑、接控制都不容易跑偏。 Reeds-Shepp曲线公式推导与代码实现：从几何原理到路径求解
先说个我自己调代码的教训。最早实现Reeds-Shepp时我拿着网上那张路径类型表直接套公式结果在连续几个终点上算出来的最短路径比常识长了快一倍。排查到最后问题出在我没把“正方向角”和“航向角变化量”这两个概念掰清楚差一个符号就让整条路径走了大圆弧。所以说公式推导不是走流程坐标系约定、角度规范化、圆心几何关系任何一个环节含糊后面代码就是纸糊的。这篇文章是整个系列的第二部分。第一部分如果已经讲清楚了“Reeds-Shepp曲线解决什么问题”“为什么最短路径一定会落在一个有限集合里”那第二部分就更像一个几何施工队把每一种核心路径类型的公式一步步推出来再落成可以跑的代码骨架。第三部分我们再补完整路径点插值、带倒车段的枚举、以及和混合A*的配合。这篇你可以当作“从数学到代码的中间层”来读适合已经知道Reeds-Shepp大概是什么、但自己动手写实现时总是差口气的人。1. 先把公式的“地基”夯实word符号与归一化坐标很多推导文章上来就开始讲LSL、RSR怎么算但我建议你先花十分钟把地基打牢。因为后面所有公式都是在特定的坐标系、特定的符号约定下成立的换一个约定公式里的加号减号全得跟着变。1.1 三段式word表示法以及±上标最容易栽的跟头Reeds-Shepp路径的基本字母只有三个L代表左转圆弧R代表右转圆弧S代表直线。一段路径就是这几个字母的组合比如LSL就是“左转直线左转”。这个大家应该不陌生。麻烦的是上标和分隔符。完整写法里L⁺表示“左转且前进”L⁻表示“左转且倒车”而L | R这种带竖线的写法表示这里有一个尖点cusp也就是车辆在这里从前进切换到倒车或者反过来。竖线前后两个圆弧的转向可以相同也可以不同。为什么我要单独提这个因为网上很多简化版实现会把上标和竖线全部省略只保留LSL、RSR这种“无倒车”的核心类型。这对很多场景够用但它只是完整路径族的一个子集。你在看推导时如果没意识到自己看的是哪个子集第三部分一旦涉及完整枚举很容易把坐标系搞混。这篇文章第二部分推导的核心类型限定在无倒车、每段都是前进方向的六种LSL、RSR、LSR、RSL、LRL、RLR。它们是完整Reeds-Shepp路径族里最基础、最常用的一组。带倒车段的类型本质上是这六种在时间翻转、反射变换下的变体第三部分再展开。1.2 归一化把任意起终点“钉”回原点推导公式时最烦人的是起点终点任意、车辆转弯半径任意。你当然可以在每个公式里都带着半径r、带着起点坐标去算但那样推导冗长不说代码里也到处都是重复的旋转平移很容易出错。归一化的思路很粗暴把问题“变小变规矩”。车辆最小转弯半径为r我们就直接令r1。任意起点(x₁,y₁,θ₁)、终点(x₂,y₂,θ₂)我们通过坐标变换让起点变成(0,0,0)终点变成归一化后的(x,y,θ)。这样每条路径的圆弧半径都是1圆心坐标、直线长度、转角之间的关系就干净了。具体变换公式是这样的。设起点为(x₁,y₁,θ₁)终点为(x₂,y₂,θ₂)转弯半径为r。先在减去起点坐标的基础上把整个坐标系旋转到起点航向方向为x轴正方向再除以r做缩放[ \begin{bmatrix} x \ y \end{bmatrix}\frac{1}{r} \begin{bmatrix} \cos\theta_1 \sin\theta_1 \ -\sin\theta_1 \cos\theta_1 \end{bmatrix} \begin{bmatrix} x_2 - x_1 \ y_2 - y_1 \end{bmatrix} ]终点航向角同样做差[ \theta \theta_2 - \theta_1 ]算出来的终点(x,y,θ)就是所有推导和代码的输入。等你算完路径再用反变换把路径点映射回原坐标系。反变换就是先乘以r再旋转θ₁角度最后加上起点坐标。这个“先归一化、后反归一化”的套路能让所有公式和代码统一到一个坐标系里是Reeds-Shepp实现的约定俗成。1.3 对称变换只用推导一半路径的底牌另一个关键工具是对称变换。因为车辆具有左右对称性一条LSL路径的镜像就是RSR路径。所以严格来说你只需要推导出LSL、LSR、LRL这三种另外三种RSR、RSL、RLR可以直接通过左右镜像得到。数学上反射变换的实现方式是对终点做(x, -y, -θ)处理算完路径后把路径里的L和R互换。这个操作在代码里非常便宜但能省掉一半的公式推导和调试量。我在下面的推导中会先完整推导左边的三种右边的三种直接给镜像结果和关键差异点。2. CSC路径推导圆心、切线与一条直路CSC指的是“圆弧-直线-圆弧”结构包含LSL、LSR、RSL、RSR四种。这类路径的几何直观最清楚关键就是找到第一段圆弧的圆心、最后一段圆弧的圆心然后两圆心之间拉一条直线所有参数就都冒出来了。2.1 LSL从起点圆和终点圆直接读出三段参数先推最干净的LSL。归一化后起点在(0,0,0)终点在(x,y,θ)转弯半径r1。第一段是左转圆弧。车辆从原点出发一开始朝x轴正方向左转的圆心一定在车左侧、距离为1的位置也就是[ C_1 (0, 1) ]第三段也是左转圆弧。终点(x,y)处的航向是θ左转圆心在终点左侧、垂直于航向的地方。用方向向量来算终点航向的单位向量是(cosθ, sinθ)左侧垂直方向是(-sinθ, cosθ)。所以第三段圆心[ C_3 (x - \sin\theta, \ y \cos\theta) ]两圆心之间的距离就是中间直线段的长度u[ u \sqrt{(x - \sin\theta)^2 (y \cos\theta - 1)^2} ]设两圆心连线与x轴正方向的夹角为α[ \alpha \text{atan2}(y \cos\theta - 1, \ x - \sin\theta) ]第一段圆弧从航向0转到航向α左转是让航向增大所以第一段的带符号转角s₁就是α。第三段圆弧从航向α转到航向θ所以s₃是θ-α。路径总长度为|s₁| u |s₃|。这里有个细节值得注意“带符号转角”这个概念。左转为正、右转为负。因为LSL两段都是左转s₁和s₃理应为非负。如果算出来s₁或s₃是负的说明这个终点的几何结构根本不适用LSL这条路径候选就可以直接丢弃。举个数值例子。设终点为(4, 1, 0.5)那么(C_1 (0, 1))(C_3 (4 - \sin 0.5, 1 \cos 0.5) (3.5206, 1.8776))(u \sqrt{3.5206^2 0.8776^2} 3.6283)(\alpha \text{atan2}(0.8776, 3.5206) 0.2443)(s_1 0.2443, \ s_3 0.5 - 0.2443 0.2557)总长度 (0.2443 3.6283 0.2557 4.1283)。这是一个干干净净的LSL解。2.2 RSR/LSL镜像对照以及转角正负的检查逻辑RSR不用重新推。把LSL在y轴方向做个镜像反射左转圆心换到起点下方第三段圆心换到终点右侧[ C_1 (0, -1), \quad C_3 (x \sin\theta, \ y - \cos\theta) ]直线长度u、两圆心连线方向α的公式结构一模一样只是代入的坐标不同。但第一段和第三段都是右转s₁和s₃从几何上算出来应该是负值。所以在代码里RSR求解器对s₁、s₃的检查条件与LSL正好相反如果算出来是正的就返回不可行。这里我特别强调一点LSL和RSR的公式不是靠“记得加号减号”来区分的而是靠“带符号转角的检查”来自动筛选。与其死记R和L在公式里哪个取正哪个取负不如统一用带符号转角再把非负检查写成统一的约束。这个习惯能帮你避开很多符号事故。2.3 LSR/RSL公切线带来的可行性条件LSR也就是左转、直线、右转是CSC里最容易出边界问题的一类。第一段左转圆心[ C_1 (0, 1) ]第三段右转圆心。右转时圆心在终点右侧所以第三段圆心为[ C_3 (x \sin\theta, \ y - \cos\theta) ]这两圆心之间的距离记为d。因为两段圆弧的转向相反直线段是这两个半径均为1的圆的公切线。画一下几何关系就会发现直线段的长度u、两圆心距d、两圆半径之和2恰好构成一个直角三角形斜边是d直角边是u和2。所以[ u \sqrt{d^2 - 4} ]这个公式同时引出了一个硬性可行性条件d必须大于等于2。如果d 2两圆相交或重叠直的公切线不存在这条LSR路径就无解。再来看直线方向角。设两圆心连线方向角为α直线方向与圆心连线之间夹着一个角β且[ \beta \arcsin\left(\frac{2}{d}\right) \quad \text{或等价地} \quad \beta \text{atan2}(2, \sqrt{d^2 - 4}) ]对于LSR直线方向角是α β。这背后的直觉是左转圆在起点上方右转圆在终点下方附近直线段要让车辆从“左转圆右下侧”切到“右转圆左上侧”方向会比圆心连线略微偏左一点点。第一段带符号转角 (s_1 \alpha \beta)第三段带符号转角 (s_3 \theta - (\alpha \beta))。LSR中s₁应为正左转s₃应为负右转。RSL就是镜像对称直线方向角换成α - β第一段右转s₁应为负第三段左转s₃应为正。这个一正一负的差别靠代码里的符号检查自动区分就好。3. CCC路径推导三角形几何与候选解的判定CCC类路径只有两个成员LRL和RLR也就是三段圆弧连续转弯、没有直线。这种路径的几何比CSC稍微绕一点因为你需要自己确定中间那段圆弧的圆心。这个小节我重点讲清楚判定逻辑这部分也是代码里最容易出bug的地方。3.1 LRL的三角形构造以及两个候选圆心LRL的三段圆弧左转、右转、左转。第一段圆心[ C_1 (0, 1) ]第三段左转圆心[ C_3 (x - \sin\theta, \ y \cos\theta) ]关键是中间这段右转圆弧的圆心C₂。由于第一段圆弧是左转半径1第二段是右转半径1相邻两圆是外切关系所以C₁到C₂的距离等于C₂到C₃的距离都等于2。这个约束把问题变成了一个纯三角形问题已知C₁和C₃的位置求到两者距离都为2的点C₂。设C₁到C₃的距离为d。那么三角形C₁-C₂-C₃的边长分别是2、2、d。用余弦定理可以得到C₁处的内角φ[ \cos\phi \frac{2^2 d^2 - 2^2}{2 \cdot 2 \cdot d} \frac{d}{4} ] [ \phi \arccos\left(\frac{d}{4}\right) ]设C₁到C₃连线的方向角为α那么C₂相对于C₁的方向有两种可能α φ或者α - φ。为什么有两个因为“到两个定点距离都为2的点”本来就是两个圆的交点几何上有两个解。这就有意思了。很多初学者会想当然地认为取哪个解都行实际上只有一个解能满足“三段转角符号正确”的约束。比如我之前实测的一个例子终点(2, 0, 0)LRL路径的C₁(0,1)C₃(2,1)d2φ60°两个候选C₂分别位于连线上方和下方。算下来只有下方的C₂能满足“左转-右转-左转”的符号约束。3.2 带符号转角检查比死记公式稳得多确定候选C₂之后怎么算三段转角并判断可行性我的建议是别去背什么闭式公式直接用几何关系硬算。设P₁ (C₁C₂)/2P₂ (C₂C₃)/2。因为相邻圆外切切点正好在两圆心连线的中点。第一段起点S相对C₁的方向角是-π/2P₁相对C₁的方向角是atan2(P₁.y - C₁.y, P₁.x - C₁.x)。左转是逆时针所以s₁等于这两个角度的逆时针差经角度规范化后应为正。第二段P₁相对C₂的方向角记为a₁P₂相对C₂的方向角记为a₂。右转是顺时针所以s₂ normalize(a₂ - a₁)应为负。第三段P₂相对C₃的方向角记为a₃终点T相对C₃的方向角为atan2(T.y - C₃.y, T.x - C₃.x)。左转所以s₃ normalize(a_T - a₃)应为正。如果s₁、s₂、s₃的符号都满足“正、负、正”这个候选就是可行的。如果不行就试另一个候选C₂。两个都不行才返回无解。这套逻辑的好处在于它完全绕开了“什么情况下取αφ”的繁琐分支讨论。你用符号约束去做筛选公式可能不是最简的但绝对是代码里最不容易错的。3.3 RLR整个推导演示一遍RLR是LRL的镜像。第一段圆心[ C_1 (0, -1) ]第三段右转圆心[ C_3 (x \sin\theta, \ y - \cos\theta) ]中间左转圆弧圆心C₂同样是满足C₁C₂2、C₂C₃2的点。三角形构造、φ的计算方式完全一样。区别在符号检查三段转角应该是“负、正、负”也就是第一段右转、第二段左转、第三段右转。代码层面LRL和RLR可以共用一个“给定C₁、C₃、期望符号模式返回可行路径或None”的工具函数把符号模式作为参数传进去。这样可以少写一半重复代码。4. 代码骨架六个求解器与最短路径枚举公式推完现在把公式落成代码。我这里给出一个可以跑的Python骨架目标是让读者看到“公式→代码”的映射关系。第三部分会在这个骨架上补充路径点插值和完整枚举。4.1 路径数据结构和角度工具先定义三个基础东西路径段、路径对象、角度规范化函数。import math class Segment: def __init__(self, stype, direction, length): self.stype stype # L 左转圆弧 / R 右转圆弧 / S 直线 self.direction direction # 1 前进 / -1 倒车 self.length length class Path: def __init__(self, segments): self.segments segments self.total_length sum(seg.length for seg in segments)角度规范化是重中之重。我统一用normalize_angle把任意角度映射到 [-π, π) 区间这样转角的正负号一目了然。def mod2pi(x): v x % (2 * math.pi) if v 0: v 2 * math.pi return v def normalize_angle(x): v mod2pi(x math.pi) - math.pi return v注意这里有个选择我把规范化区间定在 [-π, π)而不是 [0, 2π)。这两个选择会直接影响代码里所有“期望正还是负”的判断所以一定要全文统一。4.2 CSC类求解器的Python实现以LSL为例完整实现如下def lsl_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x - math.sin(theta), y math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 u math.hypot(dx, dy) alpha math.atan2(dy, dx) s1 normalize_angle(alpha) s3 normalize_angle(theta - alpha) if s1 -1e-6 or s3 -1e-6: return None return Path([ Segment(L, 1, abs(s1)), Segment(S, 1, u), Segment(L, 1, abs(s3)), ])RSR只是把两个圆心都镜像到下方s1、s3的检查方向反过来def rsr_path(x, y, theta): cx1, cy1 0.0, -1.0 cx3, cy3 x math.sin(theta), y - math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 u math.hypot(dx, dy) alpha math.atan2(dy, dx) s1 normalize_angle(alpha) s3 normalize_angle(theta - alpha) if s1 1e-6 or s3 1e-6: return None return Path([ Segment(R, 1, abs(s1)), Segment(S, 1, u), Segment(R, 1, abs(s3)), ])LSR需要先检查两圆心距d是否大于等于2再算公切线def lsr_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x math.sin(theta), y - math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 d math.hypot(dx, dy) if d 2.0: return None alpha math.atan2(dy, dx) beta math.atan2(2.0, math.sqrt(d * d - 4.0)) lam alpha beta u math.sqrt(d * d - 4.0) s1 normalize_angle(lam) s3 normalize_angle(theta - lam) if s1 -1e-6 or s3 1e-6: return None return Path([ Segment(L, 1, abs(s1)), Segment(S, 1, u), Segment(R, 1, abs(s3)), ])RSL就是lam alpha - beta且s1应为负、s3应为正其余结构一致。这六个函数共享同一套坐标系约定输入输出格式完全统一。4.3 CCC类求解器的Python实现LRL的难点在于候选C₂的枚举和符号筛选。我把核心逻辑写成“遍历两个候选方向”的形式def lrl_path(x, y, theta): cx1, cy1 0.0, 1.0 cx3, cy3 x - math.sin(theta), y math.cos(theta) dx, dy cx3 - cx1, cy3 - cy1 d math.hypot(dx, dy) if d 1e-6 or d 4.0: return None alpha math.atan2(dy, dx) phi math.acos(d / 4.0) for sign in (1.0, -1.0): c2_angle alpha sign * phi cx2 cx1 2.0 * math.cos(c2_angle) cy2 cy1 2.0 * math.sin(c2_angle) px1, py1 (cx1 cx2) / 2.0, (cy1 cy2) / 2.0 px2, py2 (cx2 cx3) / 2.0, (cy2 cy3) / 2.0 s1 normalize_angle( math.atan2(py1 - cy1, px1 - cx1) - (-math.pi / 2.0) ) a1 math.atan2(py1 - cy2, px1 - cx2) a2 math.atan2(py2 - cy2, px2 - cx2) s2 normalize_angle(a2 - a1) a3 math.atan2(py2 - cy3, px2 - cx3) a_end math.atan2(y - cy3, x - cx3) s3 normalize_angle(a_end - a3) if s1 -1e-6 or s2 1e-6 or s3 -1e-6: continue return Path([ Segment(L, 1, abs(s1)), Segment(R, 1, abs(s2)), Segment(L, 1, abs(s3)), ]) return None这里我用s2 1e-6作为“右转失败”的判断因为第二段应为负。浮点容差取1e-6基本能覆盖double计算误差又不会放过真正的符号错误。RLR的代码就是把所有圆心坐标镜像、符号检查方向反过来不再重复列举。4.4 主函数与一个手动验证过的算例主函数负责归一化、枚举求解器、选最短路径def reeds_shepp_path(x, y, theta, r1.0): xn, yn, tn x / r, y / r, theta solvers [ lsl_path, rsr_path, lsr_path, rsl_path, lrl_path, rlr_path, ] best None for solver in solvers: path solver(xn, yn, tn) if path is None: continue if best is None or path.total_length best.total_length - 1e-9: best path return best我之前手算过终点(4, 1, 0.5)在r1时的LSL路径总长度为4.1283。实际跑这段代码枚举六个求解器后选出的最短路径就是LSL总长度也正好是4.1283。这是一条很好的冒烟测试用例因为每个数值我都手推过一遍拿到代码里可以验证公式到代码的映射是否出错。另一个测试用例是终点(2, 0, 0)。手推或者拿代码跑一下你会发现最优解不是LSL而是LRL路径形状是左右各一个小圆弧、中间一段反向圆弧的S形。这个例子能检验CCC类求解器是否正确——很多实现如果CCC部分有bug在这里就会出现路径长度明显偏大或直接返回None。5. 实测中绕不开的数值细节公式和代码都齐了但这部分才是真正决定你的实现能不能在实车或者仿真里稳定跑起来的重点。5.1 角度规范化的范围选择真的会改变公式我在上面把角度统一规范化到[-π, π)并且所有“期望正/期望负”的判断都是基于这个区间。如果你把规范化改成[0, 2π)LSL的s₁、s₃判断逻辑就会出问题一个真实的-0.1弧度的右转会被映射成2π-0.1而你显然不希望把它当成一次几乎一整圈的“左转”。所以我的建议是代码里所有角度相关的地方从输入到中间变量到输出全部统一走normalize_angle不要在某个局部突然用mod2pi。如果你在调试时发现某个终点算出的最短路径诡异得长第一时间检查是不是某个转角没有被规范化直接落在区间外了。5.2 可行性判断的容差硬比较必踩坑d ≥ 2这个条件理论上很干净代码里直接写if d 2.0: return None也没毛病。但工程上我建议加一个微小容差if d 2.0 - 1e-9: return None为什么因为在边界附近d有可能是2.0000000001也可能因为浮点误差变成1.9999999999。后者的差只有1e-10但硬比较会直接丢掉一条可行的LSR路径。这种边界在Reeds-Shepp里非常常见尤其是那些“直行或者近似直行”的工况d天生就贴近2。同样的道理也适用于LRL/RLR中d 4.0的边界检查以及不同路径之间比较长短时的1e-9容差。另外还有一个更隐蔽的问题math.sqrt(d*d - 4)在d非常接近2时会算出一个接近0的数这个没问题但如果你用math.asin(2/d)当d略小于2时可能会得到nan。所以我在代码里用了math.atan2(2, sqrt(d*d-4))替代asin(2/d)就是为了避免这个数值陷阱。5.3 往返自检判断实现有没有内在bug的土办法代码写完怎么快速验证实现有没有问题我推荐一个土办法叫往返测试。选一个起点A再选一个终点B用你的实现算出从A到B的最短路径长度L1然后反过来从B到A再算一次路径长度L2。理论上Reeds-Shepp路径长度是可逆的L1和L2应该严格相等或者相差一个极小量。这个测试能一次性暴露绝大多数坐标系变换错误、符号约定错误、转角规范化错误。我第一次写完这六个求解器时跑往返测试就在一组特定终点上差了0.4追根溯源发现是RSR的s₁符号检查方向写反了。5.4 和Dubins、混合A*的边界感最后想聊一句边界感。Reeds-Shepp假设车辆可以原地切换前进倒车所以它在低速、车位入库、原地调头这类场景很有效。如果你的场景不允许倒车那应该用Dubins曲线那是同一套几何框架的“无倒车子集”。而混合A*这种规划算法本质上是把Reeds-Shepp当作“从当前节点到终点的启发式连接段”来用并不会把整条路径都交给Reeds-Shepp去规划。原因也很简单Reeds-Shepp完全不考虑障碍物它给出的是一条几何最短路径不是一条可通行的路径。后者还需要碰撞检测、轨迹平滑、速度分配来兜底。所以读到这里的你如果正准备把这段代码塞进一个完整的规划系统里我的建议是先把这个几何求解器当独立模块测试好再横向集成。Reeds-Shepp这个模块的输出是“一段带转向和方向的路径”不是“一条可跟踪轨迹”。有了这个认知后面接平滑、接控制都不容易跑偏。