4. 轨迹表示方法：路径点序列、多项式轨迹、样条曲线
做轨迹跟踪这些年，我最大的体会是：轨迹怎么表示，直接决定了你能跑多快、跟多准。很多人一上来就调PID、调MPC，结果发现怎么调都抖、怎么调都滞后。其实问题往往出在轨迹本身——你给的参考轨迹就不平滑，控制器再牛也白搭。
这一章，我把工作中最常用的三种轨迹表示方法掰开揉碎了讲。每种方法都有它的脾气，选对了事半功倍。
核心观点：轨迹表示不是简单的“存一堆点”，而是要在平滑性、计算效率、约束满足之间找平衡。没有银弹，只有取舍。
4.1 路径点序列：最朴素，也最容易踩坑
路径点序列，说白了就是存一串离散的坐标点 (x, y, theta)，然后让机器人一个点一个点地跟。这是最直观的方法，我刚入行时也这么干。
优点很明显：实现简单、存储量小、任何轨迹都能近似。但坑也在这里——点与点之间怎么走，完全取决于你的插值方式。
我曾经踩过的坑：有一次做AGV导航，路径点间隔设了0.5米，结果机器人每到一个点就急停一下，再加速到下一个点。现场看起来就像在“点头”。后来才发现，问题出在路径点太稀疏，而且没有做速度过渡。
路径点序列的常见插值方式：
| 插值方式 | 连续性 | 计算量 | 适用场景 |
|---|---|---|---|
| 线性插值 | C0（位置连续） | 极低 | 低速、大间距路径 |
| 三次样条插值 | C2（位置+速度+加速度连续） | 中等 | 中高速、需要平滑 |
| Hermite插值 | C1（位置+速度连续） | 低 | 已知端点速度的场景 |
我个人习惯是：如果路径点间距小于0.2米，线性插值就够了。但如果间距超过0.5米，必须用三次样条，否则加速度跳变会让控制器很难受。
4.2 多项式轨迹：平滑性的数学保证
多项式轨迹的核心思想是：用一条连续的多项式曲线来拟合整段轨迹。最常见的做法是最小加加速度（Minimum Jerk）轨迹。
为什么选加加速度？你想想看，加速度连续变化，力就不会突变，电机就不会“咯噔”一下。我做过一个机械臂的轨迹规划，用五次多项式之后，末端抖动直接降了60%。
一个典型的多项式轨迹公式长这样：
// 五次多项式轨迹（位置、速度、加速度均连续）
double poly5(double t, double t0, double tf,
double p0, double pf,
double v0, double vf,
double a0, double af) {
double tau = (t - t0) / (tf - t0);
// 系数由边界条件解出
double a0_coeff = p0;
double a1_coeff = v0 * (tf - t0);
double a2_coeff = 0.5 * a0 * (tf - t0) * (tf - t0);
// ... 实际工程中建议用矩阵求解
return a0_coeff + a1_coeff * tau + a2_coeff * tau * tau
+ a3_coeff * tau * tau * tau
+ a4_coeff * tau * tau * tau * tau
+ a5_coeff * tau * tau * tau * tau * tau;
}实战建议：多项式阶数不是越高越好。三次多项式只能保证速度连续，五次多项式保证加速度连续，七次多项式保证加加速度连续。对于大多数移动机器人，五次多项式已经足够。阶数太高反而容易产生龙格现象（端点振荡）。
多项式轨迹的另一个好处是时间参数化。你可以显式地控制轨迹的总时间，这对于时间最优规划非常有用。我在做竞速机器人时，就用七次多项式配合时间缩放，在满足动力学约束的前提下把时间压到最短。
4.3 样条曲线：局部可控的利器
样条曲线，尤其是B样条和贝塞尔曲线，是我现在最常用的轨迹表示方法。为什么？因为局部可控——你修改一个控制点，只影响附近一小段轨迹，不会牵一发动全身。
4.3.1 贝塞尔曲线：直观但全局
贝塞尔曲线的特点是：起点和终点一定经过，中间的控制点只“吸引”曲线。这个特性在视觉上非常直观，设计师很喜欢用。
但贝塞尔曲线有个硬伤：移动一个控制点，整条曲线都会变。这意味着如果你在跟踪过程中需要局部避障，就得重新计算整条曲线，计算量上不划算。
// 三阶贝塞尔曲线（4个控制点）
double bezier3(double t, double p0, double p1, double p2, double p3) {
double u = 1.0 - t;
return u*u*u * p0 + 3*u*u*t * p1 + 3*u*t*t * p2 + t*t*t * p3;
}4.3.2 B样条：工业级的选择
B样条是我个人最推荐的方法。它解决了贝塞尔曲线的全局性问题——每个控制点只影响局部区间。而且B样条的连续性可以显式控制：三次B样条天然就是C2连续。
B样条的核心参数是节点向量（Knot Vector）。节点向量的分布决定了曲线的形状：
| 节点向量类型 | 特点 | 适用场景 |
|---|---|---|
| 均匀节点 | 节点等距分布，曲线不过首尾点 | 循环轨迹、周期性路径 |
| 准均匀节点 | 首尾节点重复，曲线经过首尾点 | 起点终点固定的轨迹 |
| 非均匀节点 | 节点可任意分布 | 需要局部加密的场景 |
我的经验：做移动机器人轨迹跟踪，准均匀节点的三次B样条是最稳妥的选择。它既保证了轨迹经过起点和终点，又提供了C2连续性，而且计算量可控。我在多个项目中都用这个方案，效果很稳定。
4.4 三种方法的对比与选型
说了这么多，到底怎么选？我整理了一个对比表，方便你快速决策：
| 维度 | 路径点序列 | 多项式轨迹 | 样条曲线 |
|---|---|---|---|
| 平滑性 | 取决于插值方式 | 高（可显式控制阶数） | 高（B样条天然C2） |
| 局部可控 | 否 | 否 | 是（B样条） |
| 计算量 | 极低 | 低 | 中等 |
| 存储量 | 低 | 极低（只需系数） | 中等（控制点+节点向量） |
| 实时修改 | 容易 | 困难（需重新求解） | 容易（B样条局部修改） |
| 典型场景 | 简单路径、低速AGV | 机械臂、时间最优规划 | 自动驾驶、动态避障 |
注意：不要盲目追求高阶平滑。有一次我在项目中用了七次B样条，结果轨迹确实平滑，但计算耗时增加了3倍，实时性完全跟不上。后来换成三次B样条，效果几乎一样，实时性却好了很多。够用就好，别过度设计。
4.5 实战选型建议
最后，我根据不同的项目阶段，给几条具体的建议：
- 原型验证阶段：用路径点序列+线性插值。快速跑通流程，别在轨迹表示上花太多时间。
- 性能调优阶段：切换到三次B样条（准均匀节点）。这是性价比最高的方案，平滑性和计算量都很好。
- 极限性能阶段：用多项式轨迹做时间最优规划。配合动力学约束，把轨迹的时间压到最短。
- 动态环境：必须用B样条。局部可控的特性让你能在避障时只修改局部轨迹，不用全局重算。