深入理解路径平滑

可视化演示

通过原始折线与平滑路径对比、Catmull-Rom样条拟合过程、以及曲率分布分析，展示路径平滑如何消除尖角冗余提升跟踪控制精度。

问题缘起：尖角与冗余

拿到一张网格地图时，基于栅格或拓扑搜索的规划器往往会给出“踏步式”路径：直行一段、转弯九十度、再直行，偶尔还夹杂着为了绕开一个栅格而出现的来回冗余。你可以把它想成老城区里用石板错落铺成的台阶路，人走没问题，但让带轮子的搬运车走，每个台阶都会让车头猛地一摆。

路径平滑是运动规划的一步“后处理”：输入是几何路径点，输出是更适合跟踪控制器执行的平滑曲线。它要处理两类核心问题：一类是尖角，也就是路径上曲率过大甚至趋于无穷大的折点；另一类是冗余，也就是不必要的抖动、锯齿和绕圈。平滑的目标不是简单“把路拉直”，而是在保持原路径意图的前提下，降低尖角、去除抖动，并控制与原路径的偏差。

曲率是衡量尖角程度的天然指标。假设路径参数化为 \(x(s), y(s)\)，点沿路径的切向变化越剧烈，曲率就越大。

直线段上曲率为零，圆弧半径为 \(r\) 时曲率为 \(1/r\)，而直角折点处方向会在瞬间改变，曲率趋近于无穷大。修路时第一件事不是画线，而是先判断哪些尖角必须拆掉，哪些冗余弯道可以抹平。

对跟踪控制器来说，曲率越大，需要的横向加速度就越大；一旦超过车辆物理极限，实际轨迹就会偏离规划路径。因此，路径平滑首先要回答的问题就是：这条路最急的弯在哪里，它是否在车能承受的范围内。

样条插值：让路径穿过关键点

老修路师傅手里有一根弹性很好的钢制样条。他把木桩钉在地上，钢条被迫弯曲，结果会给出一条柔和的曲线，并且精确地穿过每一根木桩。样条插值做的事情正是如此：构造一条曲线，让它严格经过所有给定的路径点。

工程中最常用的是三次样条。它把路径分成若干小区间，每个区间用一条三次多项式曲线表示，同时要求相邻区间在连接点处不仅位置一致，切线方向甚至曲率也保持一致。这样拼出来的曲线整体连续、平滑，又不会像高次多项式那样随意摆动。

三次样条能够保证 \(C^2\) 连续，也就是位置、切线和曲率都连续，这是它受欢迎的重要原因。但它也有一个“强求”的缺点：因为曲线必须穿过每个点，如果某个路径点是噪声或冗余点，曲线就会被迫拐一个不自然的弯。而且样条是全局求解的，移动一个木桩，整根钢条的形状都会发生变化。这就像修路时如果硬要穿过每一块凸起的石头，路面反而会起伏不平。

B样条拟合：局部调控曲线

如果木桩太多太难伺候，师傅会换一种工具：不再用一根贯穿所有桩的钢条，而是用几段短柔性杆拼接，每一段只受附近几个控制桩的影响。拔掉其中一个桩，只有身边一小段曲线会变化，远处纹丝不动。这就是 B 样条的设计思路。

B 样条曲线由一组控制点 \(\mathbf{P}_i\) 和对应的基函数 \(N_{i,p}(u)\) 加权组合而成。控制点相当于修路时插在地面上的控制桩，曲线并不一定穿过它们，而是被它们“牵引”着走。因此 B 样条天然适合做拟合而非插值：面对大量带噪声的路径点时，可以让曲线从点群中间穿过，而不是勉强穿过每一个点。

在实际路径平滑中，节点向量可以预先确定，再通过最小二乘拟合求出控制点；也可以通过调节平滑因子控制曲线对原始路径点的“跟随程度”。B 样条兼顾了平滑性、局部性与计算效率，因此成为机器人运动规划后处理中最常用的工具之一。它相当于让修路师傅拥有了“哪里不平改哪里”的能力，而不必为了一个凸起重铺整条路。

数值优化：约束下的平衡

前面几种方法主要靠几何公式构造曲线，而数值优化提供了另一种思路：把路径平滑看成寻找一个“受力平衡”的位置。想象路径是一根有弹性的橡皮筋，它被弹簧拉向原始参考线，又被墙面和障碍物限制在安全区域内，反复松手后，橡皮筋最终停在一个既不过分偏离、又不碰到障碍的平衡位置。

数值优化把路径点当作决策变量，把“平滑程度”“路径长度”“与参考路径的偏差”等要求写成目标函数，再把“不能碰到障碍”“最大曲率不能超限”等写成约束条件。求解器通过迭代不断微调每个路径点的位置，直到满足条件且目标函数不再显著下降。

除了目标函数，实际工程通常还会加入约束，例如 \(\kappa_{\max}\) 不超过车辆转向极限，以及路径点必须在安全可行域内。这类带约束的二次规划或非线性优化问题，可以用梯度下降、增广拉格朗日、SQP 等迭代算法求解。数值优化的优点是“约束想加就加”，缺点是计算开销更大、参数需要仔细调。它适合在城市窄巷里修路——既要绕开墙，又不能为了平滑而跑到墙外去。

平滑评估：如何才算“平滑”

路修好了不能直接交工，验收时要检查三件事：最急的弯是否足够缓、路线偏离原勘测线是否在允许范围内、以及施工时间是否可接受。路径平滑的评估也类似，不能只看路径点是否连着，还要看曲率、偏差和计算开销。

一条“好看”的路径未必是曲率处处为零的直线，而应该是曲率连续、最大曲率有限、且整体没有突兀抖动的曲线。对于跟踪控制器来说，曲率连续尤其重要：如果曲率发生跳变，控制器就要在瞬间改变前轮转向指令，产生冲击和跟踪误差。因此，路径平滑质量常用曲率平方沿弧长的积分来衡量。

除了曲率能量，还需要关注与原路径的偏差包络：平滑后的路径不能离原始路径太远，否则可能压到障碍物或偏离任务走廊。工程上常用最大偏差 \(\max_s \|\mathbf{q}(s)-\mathbf{q}_{ref}(s)\|\) 来限定安全范围。此外，计算开销也是评估指标之一，尤其对于实时控制，平滑算法必须在几十毫秒内完成。验收一条路，弯道质量、边界安全、施工时效三者缺一不可。

实践选型：按场景选方法

导航到不同工地，修路师傅会带不同工具：在开阔厂区修一条通道，用 B 样条局部调整就够；在障碍密集的狭窄巷道，则必须用数值优化把墙的约束直接放进计算。选型要先回答场景需求，再谈方法优劣。

样条插值适合路径点数量少、点位置准确且必须穿过的场景，比如机器人要依序经过几个固定工位；B 样条拟合适合路径点多、存在噪声、希望曲线平滑但不要求穿过每个点的场景；数值优化则适合带强约束的场景，比如汽车类机器人必须满足最大曲率限制，同时还要绕开障碍物。三者不是互斥的，也可以组合使用：先用 B 样条做快速平滑，再用优化做精细调整。

在实际项目中，跟踪控制精度与路径平滑质量直接相关：最大曲率越低、曲率越连续，控制器就越容易跟踪；反之，即使路径长度很短，一个过急的弯也会让车辆大幅偏离。建议先测量机器人的最小转弯半径和控制频率，再据此为平滑算法设定曲率上限。参数调优时，先固定一个平滑算法，再逐步调整权重，观察“平滑质量—路径偏差—计算耗时”三者之间的平衡，就像反复试铺一段路，直到车辆开上去又稳又不越界。

AI 应用场景

Python 演示

import numpy as npfrom scipy.interpolate import splprep, splev def build_smooth_path(points, smoothing=1.0):    pts = np.asarray(points, dtype=float).T    tck, _ = splprep(pts, s=smoothing, k=3)    u = np.linspace(0.0, 1.0, 200)    x, y = splev(u, tck)    return np.column_stack((x, y)) def path_curvature(path):    x = path[:, 0]    y = path[:, 1]    dx = np.gradient(x)    dy = np.gradient(y)    ddx = np.gradient(dx)    ddy = np.gradient(dy)    numerator = np.abs(dx * ddy - dy * ddx)    denominator = (dx * dx + dy * dy) ** 1.5    return numerator / np.maximum(denominator, 1e-12) def main():    raw = np.array([[0.0, 0.0], [1.0, 2.0], [2.0, 0.0],                    [3.0, 2.0], [4.0, 0.0]])    smooth = build_smooth_path(raw, smoothing=1.0)    curv = path_curvature(smooth)    print("平滑后点数:", len(smooth))    print("最大曲率:", round(float(np.max(curv)), 4))    print("起点:", smooth[0].round(3), "终点:", smooth[-1].round(3)) if __name__ == "__main__":    main()

1. tck, _ = splprep(pts, s=smoothing, k=3)用三次 B 样条拟合路径点，smoothing 控制平滑程度。
2. x, y = splev(u, tck)在参数区间内均匀采样，生成平滑曲线的坐标点。
3. numerator = np.abs(dx * ddy - dy * ddx)按曲率公式计算弯曲程度的分子部分。
4. denominator = (dx * dx + dy * dy) ** 1.5用速度的立方做归一化，消除点密度影响。
5. np.maximum(denominator, 1e-12)避免直线段处分母为零导致除零错误。

常见误区

面试题