★ FEATURED ARTICLE 做机器人运动规划的人迟早会和“曲线生成”这四个字正面相遇。路径规划算法给你一串离散的路径点可机器人真正要跟着走的时候需要的是连续、平滑、满足运动学约束的轨迹。这个“从路径点到连续轨迹”的转换过程就是曲线生成要解决的问题它直接决定了机器人是丝滑地转过拐角还是在每个路径点处停下来急转弯。这节教程里我挑了两种极具代表性的曲线来拆解BSpline和ReedsShepp。BSpline是参数曲线的代表擅长把离散点拟合成光顺曲线常用于路径平滑ReedsShepp则是非完整约束下的最优路径求解器专门解决“车不能横着走”这类约束下的最短路径问题。两者一个重“形”一个重“法”组合在一起基本覆盖了运动规划库中最核心的曲线生成场景。这篇内容适合正在写运动规划库、或者想搞懂常见曲线算法底层的读者我会把原理、代码和踩坑经验一起聊透。1. 曲线生成问题的总体判断与方案拆解1.1 为什么需要“曲线生成”这一层先想清楚一个问题规划算法输出的路径到底是什么咱们常见的地图搜索类规划器输出是一串离散的路径点比如[(0,0), (1,0), (2,1), ...]。这些点只包含空间位置信息没有时间信息也没有方向信息——它们只是一条折线的顶点。但机器人要的是能真正执行的东西。以差速小车为例它需要知道每一时刻的线速度和角速度这等价于要知道轨迹上每个点处的方向朝向角、曲率以及曲率的变化率是否连续。如果直接把折线路径丢给底层控制机器人到每个拐点都得停一下再转向否则瞬时角速度冲击会令电机过载、车身打滑。曲线生成层要做的就是把离散路径点转化为一条参数化的连续曲线并在此基础上做插值、平滑和重采样最终输出带朝向信息和曲率信息的轨迹点序列。这一步做好了下游的轨迹跟踪控制器才能有稳定的输入。这就是BSpline的主要应用场景它用一个较低维度的控制点集合去描述一条具有全局连续性的曲线。1.2 BSpline和ReedsShepp在解决什么问题上有本质区别要理解这两种算法为什么会被放在同一节里讲得先看清楚它们解决的问题落脚点完全不同。BSpline解决的是“如何让曲线更光顺”的问题。它关注的是曲线的数学形态给定一组离散点通过最小二乘拟合或者插值的方式生成一条在节点处具有连续性通常到C2甚至更高的平滑曲线。它不关心机器人是否走得出来只关心曲线本身长得好不好看、光顺不顺。从数学上说C2连续意味着曲率连续机器人执行起来加速度不会突变这对乘坐舒适性和电机寿命都非常友好。ReedsShepp解决的是“在非完整约束下怎么从A点到B点距离最短”的问题。它针对的是像汽车这样的非完整系统特点是转弯半径有下限、不能横向移动。ReedsShepp曲线是在给定最小转弯半径的前提下只由直线段Straight和圆弧段Curve拼接而成的在几何上证明了这些组合中可以找到两状态之间的最短路径。它输出的是一条满足运动学约束的可行路径但不保证曲率连续——圆弧和直线交界处曲率直接跳变。所以BSpline追求光顺度ReedsShepp追求可行性与最短路程。在运动规划库的设计里这两者通常不是替代关系而是管道关系先有ReedsShepp这样的几何路径再用BSpline做平滑后处理。这个组合拳打好了路径既可行又丝滑。2. BSpline曲线生成内核拆解控制点、节点向量与递推求值2.1 基础概念为什么是“B样条”而不是“贝塞尔曲线”很多初学者问过我一个问题明明贝塞尔曲线用的人那么多我们为什么还要用BSpline核心原因是贝塞尔曲线的控制点对曲线形状影响是全局的——你拖任何一个控制点整条曲线都会受影响。这个特性在运动规划里很尴尬因为路径点往往包含局部障碍信息我们希望局部修形不影响远处。BSpline引入了一个“节点向量”的概念把参数域切分成若干区间每个控制点只在一定参数区间内影响曲线形状这就是它所说的“局部支撑性”。通俗地讲BSpline像是一段一段拼接的曲线每一段只由附近的几个控制点决定控制点的修改影响范围是局部的。实际工程里我不可能因为一个路径点有障碍就重新优化整条轨迹所以局部支撑这个性质非常关键。BSpline曲线的数学定义长这样C(u) Σ P_i * N_{i,p}(u)其中 P_i 是控制点N_{i,p}(u) 是p阶B样条基函数定义在节点向量 U[u_0, u_1, ..., u_m] 上。基函数由Cox-de Boor递推关系给出这是所有BSpline实现的核心。2.2 节点向量分类均匀、clamped与三种常见设定节点向量直接决定了曲线起点终点是否穿过控制点这是新手极易踩坑的地方。均匀节点向量节点在参数轴上均匀分布但曲线不穿过首尾控制点。路径规划里很少直接用因为轨迹起点终点不受控。Clamped节点向量也叫非均匀、开这是运动规划中最常用的类型首尾节点分别重复 p1 次p为曲线阶数。这样曲线严格经过第一个和最后一个控制点并且起点切向由前两个控制点决定终点切向由最后两个控制点决定。这个特性对轨迹执行非常重要——机器人必须在起点出发也必须准确到达终点。非均匀节点向量根据控制点间距调整节点分布可以让曲线在控制点密集的区域有更大的自由度形状逼近。常见做法是用向心参数化节点间距与控制点间距的平方根成正比避免曲线在控制点密集处出现尖角。实际使用时的默认选择我建议直接上clamped的3次p3BSpline。3次意味着在内部节点处达到C2连续也就是曲率连续这是轨迹跟踪控制最舒服的输入信号。2.3 Python实现Cox-de Boor递推与de Boor求值说到代码实现最常用的BSpline曲线求值算法是de Boor递推法它比直接算基函数再求和更稳定、计算量更小。核心思路是给定参数u找到它落在哪个节点区间 [u_k, u_{k1}) 内然后从那一层的控制点开始反复做线性插值。def bspline_de_boor(ctrl_pts, knots, degree, u): # ctrl_pts: list of [x, y] control points # knots: clamped knot vector # degree: curve degree (usually 3) # u: parameter value in [0, 1] n len(ctrl_pts) - 1 # find knot span index k such that knots[k] u knots[k1] if u knots[-1]: k n else: k 0 while k len(knots) - 2 and u knots[k 1]: k 1 # copy control points in the local span d [ctrl_pts[j][:] for j in range(max(k - degree, 0), min(k, n) 1)] for r in range(1, degree 1): for j in range(degree, r - 1, -1): denom knots[j k - degree 1] - knots[j k - degree r] alpha (u - knots[j k - degree]) / denom if denom ! 0 else 0.0 d[j] [ (1 - alpha) * d[j - 1][0] alpha * d[j][0], (1 - alpha) * d[j - 1][1] alpha * d[j][1] ] return d[degree]这段代码虽然看起来只有二十多行但我建议你有条件的话在本地跑一跑观察节点向量、阶数对曲线形状的影响。有一个实操经验当节点向量首尾重复度不够 p1 时曲线不会穿过首尾控制点这会导致轨迹起点和终点偏离路径规划的起终点。这个bug在视觉上不太明显但机器人启动时会突然有一个小的位置偏差这种问题调试起来极其消耗时间。2.4 参数选择阶数、控制点数量与节点向量的权衡曲线阶数越高曲线越光滑但计算量也越大而且单点控制的影响范围越宽局部修形能力越差。3次是一个工程上的黄金选择C2连续够用计算也轻。控制点数量则直接决定拟合精度和曲线形态复杂度。控制点太少曲线会“削”掉路径的拐角偏离原始路径太多控制点太多曲线会出现不自然的波浪状抖动而且优化的自由度过多实时计算压力也大。我自己的经验是先用Ramer-Douglas-Peucker算法做一次路径抽稀把明显冗余的共线点去掉再以剩余点作为控制点做BSpline拟合。这样做出来的轨迹既平滑又不丢失关键转角信息。3. ReedsShepp曲线非完整约束下的最短路径求解3.1 非完整约束到底是什么意思要理解ReedsShepp先得理解“非完整约束”这个听着高大上的词。完整约束是指系统位置变量之间存在硬性的几何限制关系比如两轮车只能在平面运动z坐标恒为0这是完整约束。非完整约束则是对速度的约束而不是对位置的约束。车的典型特征是不能横向移动原地转弯半径不大于某个最小值。车的朝向角 θ 和速度 v 之间的关系是x_dot v * cos(θ) y_dot v * sin(θ)这个约束无法直接积分成位置层面的几何限制所以叫非完整约束。生活中很好理解车不能像人一样横着平移它有最小转弯半径倒车入库的时候要打好多把方向这就是非完整约束在起作用。ReedsShepp曲线的核心贡献是证明了在这个约束下任意两个位姿x, y, θ之间的最短路径一定可以由固定模式的 圆弧Curve和 直线Straight拼接而成。圆弧的半径是最小转弯半径前进或后退。这个结论极具工程价值因为它把无穷多可能的路径压缩成了有限个模式的组合每个模式独立计算长度取最小值即可。3.2 路径模式分类CSC、CCC与那12种基本组合ReedsShepp路径模式的分类工程上可以简化描述为几大族CSC弯-直-弯直线段连接两个圆弧适用于大多数常规场景例如从某个车位倒出来再正向开走都是这个模式。CCC弯-弯-弯三段圆弧拼接适用于极狭窄的空间比如两车之间的平行泊车操作需要来回揉库时会出现这种模式。其他复合模式如 CCSC、CCCC 等通常是在CS和CCC的基础上增加额外的圆弧段来处理更特殊的边界条件。完整证明需要计算机辅助枚举但工程实现上只需要知道一件事对每种模式基于起点与终点的相对位姿用几何关系求解圆弧的弧长和直线长度然后取总长最小者。大部分开源运动规划库里实现的是12种左右的模式覆盖所有可行状态组合。3.3 一个可用的简化实现思路我不打算贴一个几百行的完整ReedsShepp实现那样篇幅过长且大部分是模式枚举容易劝退。我这里给一个核心的几何计算片段以CSC模式中 左转-直线-右转 为例展示如何根据起终点位姿计算路径参数。import math def cs_path_length(x, y, phi, rho): # x, y: relative target position # phi: target heading relative to start # rho: minimum turning radius # transform to vehicle frame at origin, heading 0 # start circle center: (0, rho) for left turn c1 (0, rho) # target circle center: solve for (cx, cy) with heading phi c2 (x - rho * math.sin(phi), y rho * math.cos(phi)) # straight line is common tangent between the two circles dx c2[0] - c1[0] dy c2[1] - c1[1] d math.hypot(dx, dy) # distance between centers must be 2*rho for solution to exist if d 2 * rho: return None alpha math.atan2(dy, dx) beta math.acos(d / (2 * rho)) # tangent point on start circle p1 (c1[0] rho * math.cos(alpha - beta), c1[1] rho * math.sin(alpha - beta)) # tangent point on target circle p2 (c2[0] - rho * math.cos(alpha - beta), c2[1] - rho * math.sin(alpha - beta)) arc1 rho * (alpha - beta) # length of initial left turn arc straight math.hypot(p2[0] - p1[0], p2[1] - p1[1]) arc2 rho * ((phi - (alpha - beta)) if phi (alpha - beta) else 2*math.pi - (alpha - beta) phi) return arc1 straight arc2这段代码只展示了模式里最核心的几何求解求两个同半径圆的公切线切点。真实库中你需要对每种 C 的符号左转右转、每个圆弧的走向前进后退枚举情况并规范化角度范围然后统一取最小。我要强调一个新手容易忽略的细节ReedsShepp路径是无曲率连续性的。在圆弧与直线的连接点上曲率从 1/ρ 直接跳变到0。这意味着路径“可通行”但履带式或阿克曼式的车辆在实际跟踪时需要在那个点附近慢下来或者允许一定的路径跟踪误差否则转向执行机构会吃不消。3.4 ReedsShepp能做轨迹优化吗很多同学以为ReedsShepp本身就是轨迹优化的结果——它确实在“最短路径”这个指标下是最优的但它优化的维度有限。你可以通过调整 ρ 的大小来观察路径形态的变化ρ 越小路径越“刁钻”转弯越急长度越短ρ 越大路径越舒展但里程更长。因此工程上常用做法是拿ReedsShepp结果做参考线然后再做二次优化。比如以ReedsShepp路径为初值用数值优化方法在保持非完整约束曲率上限的前提下把路径打磨到曲率连续。这就是“轨迹优化”这4个字在标题里的含义——不是选一种曲线就完事了而是选型、生成、平滑、校验的全流程。4. 轨迹优化管道如何把两种曲线串联出实用价值4.1 一条完整的曲线生成管线应该长什么样在运动规划库里典型的曲线生成管线是这样的第一步输入一条无碰撞的离散路径点路径可能来自混合A*或者栅格地图搜索也可能是全局规划器的输出。第二步提取路径中的关键转折点做抽稀处理去掉冗余点。第三步调用ReedsShepp或者类似几何路径生成器计算出满足车辆运动学约束的粗路径。这一步保证了路径“走得了”把最小转弯半径的因素考虑进来。第四步对粗路径采样得到密集轨迹点再将这些点作为BSpline拟合的目标点生成一条平滑、曲率连续的最终轨迹。第五步对平滑后的轨迹做可行性校验曲率是否超过车辆极限、是否偏离原始路径太远、是否碰撞。这个管道里每种曲线都只做自己最擅长的事。ReedsShepp管“能不能走”BSpline管“走得顺不顺”。如果在第四步直接对原始路径点做BSpline拟合忽略了最小转弯半径约束得到的轨迹很可能“看着平滑但实际转不过去”。4.2 结合时的约束处理技巧用BSpline平滑ReedsShepp路径时最容易犯的错是放任曲线自由拟合结果起点和终点的朝向角跑偏了。解决方案是采用带端点约束的B样条拟合把起点位置、起点朝向、终点位置、终点朝向作为硬约束剩余内部点做最小二乘拟合。实现上有个小技巧先把采样点转换到局部坐标系以起点为原点航向角为x轴正方向在这个坐标系下拟合拟合完再变换回全局坐标。这样做的好处是数值稳定性好避免了全局坐标下坐标值数量级差异引起的病态矩阵问题。BSpline平滑前后的曲率检查是个必须的环节。我一般会计算每个采样点处的曲率再对比最小转弯半径对应的最大曲率。如果超限就回到ReedsShepp把 ρ 调大一些重新生成。这个迭代过程才是真正意义上的轨迹优化闭环。4.3 参数标定建议从实际机器人出发不同底盘的运动学参数差异巨大。我之前做过一个小型四轮差速机器人最小转弯半径接近零理论上可以直接原地旋转。但如果把 ρ 设得太小ReedsShepp生成的路径里圆弧段曲率很大BSpline平滑后虽然曲率连续了最大值却没有显著下降控制器的角速度上限反而成了瓶颈。有个土办法很好用把底盘的最大转向角换算成最小转弯半径然后乘上1.2~1.5的安全系数用这个放大后的 ρ 来做ReedsShepp。虽然路径会稍微长一点但控制系统跟得上实际执行效果反而更好。这里的本质是理论最优不等于工程最优要给执行留余量。5. 常见问题与排查技巧实录5.1 BSpline的节点向量和拟合震荡问题有人在实际拟合时发现曲线在某个区间里剧烈波动明明输入的控制点都很规整曲线却出现“龙蛇”般的抖动。我排查这类问题通常分两步。第一步检查节点向量是否首尾clamped。如果没有clamped曲线不仅不过控制点而且奇偶幂次造成的Runge现象会更明显。第二步检查控制点间距是否极端不均匀。如果两个控制点间距特别近而相邻间距特别远曲线会在密集区出现不必要的弯曲。处理办法是用向心参数化重算节点向量并且在拟合前做一次控制点去重和均匀化处理。还有一种常见状况BSpline拟合时用原始路径点直接作为控制点而不是做最小二乘逼近。这么做会让曲线严格过每个路径点噪声被完完整整保留下来反而放大不光滑性。正确的做法是把原始点作为“目标点”通过最小二乘求控制点让曲线逼近而不是穿过这些点。这里其实是“插值”和“逼近”的区别运动规划里绝大多数情况要的是逼近。5.2 ReedsShepp无解与高耗时ReedsShepp求解失败的典型原因目标点位姿与起始位姿距离小于2倍最小转弯半径而且朝向又比较刁钻。比如车位于狭窄巷道里目标点在正侧方很近的地方这种状态下任何CSC模式都找不到公交切线。此时要检查是不是 ρ 设得过大或者是不是应该切换到CCC模式。另一个性能坑是模式枚举的冗余计算。完整ReedsShepp实现要遍历几十种模式每种模式内部又有弧度规范化、符号分支如果不做模式对称性裁剪计算量很容易翻好几倍。优化手段很简单利用路径的对称性。比如如果起点终点互换时路径长度不变那么倒着算一遍的结果就能复用。5.3 采样的均匀性与时间分配问题曲线生成完之后还有一个隐藏环节采样。均匀地按弧长采样而不是按参数u均匀采样。BSpline的参数u均匀采样会在弧长较长的区间产生稀疏点在弧长较短的区间产生密集点这个不均匀会造成下游速度规划的不顺滑。我在工程里通常先把曲线离散到极密比如每0.01米一个点再用累计弧长做线性插值重采样到目标密度。这一小步对后端的速度规划帮助极大值得写进库里。5.4 坐标系变换与数值稳定性最后也是最重要的常见错题集源头坐标系变换。ReedsShepp求解时起点位姿必须变换到 (0, 0, 0)终点位姿变换为相对坐标。很多人忘了对终点朝向角做角度归一化导致后续圆弧长度计算错误。角度差在 π 附近的时候atan2和acos的组合很容易出现数值抖动最直接的规避办法就是在每个模式计算前把朝向角差值 normalize 到 [-π, π]。数值稳定性还有个容易被忽略的点当两个圆心距离刚好等于2ρ时acos的参数是1.0d/2ρ 在浮点上可能略大于1导致acos返回NaN。好习惯是代码里做 clamp 判断这在曲线生成库里算是一个防御性编程的典型示例。曲线生成这条线看起来就是调个库的事但真正动手实现一遍、调试一遍才能理解数学描述和工程可执行之间的距离。我个人在实际操作中的体会是先想清楚“我要这条曲线的什么性质”再去选算法而不是什么流行用什么。BSpline和ReedsShepp的组合基本上是一个平衡了运动学可行性与轨迹质量的标准答案。后续如果想继续深入可以在这个管道的下游加上速度规划和时间最优求解那就是另一个精彩的话题了。先把手头的曲线生成做稳是运动规划库地基中的地基。
阅读完成 · 觉得有帮助？
