4. 轨迹表示方法：路径与轨迹的区别、多项式轨迹、样条曲线、时间参数化
做轨迹跟踪这些年，我见过不少新手一上来就盯着控制算法调参，结果折腾半天车还是走不稳。为什么？因为最根本的问题没解决——你给控制器的轨迹本身就不对。今天咱们就把轨迹表示这件事彻底聊透。
4.1 路径 vs 轨迹：一字之差，天壤之别
先问个问题：路径和轨迹，到底是不是一回事？
很多人混着用，但在工程里这俩概念必须分清楚。
| 维度 | 路径（Path） | 轨迹（Trajectory） |
|---|---|---|
| 定义 | 空间中的几何曲线 | 带时间信息的路径 |
| 包含信息 | 位置 (x, y, z) | 位置 + 速度 + 加速度 + 时间戳 |
| 能否直接跟踪 | 不能，缺少时间基准 | 能，控制器知道"什么时候该到哪" |
| 典型应用 | 全局规划、地图标注 | 运动控制、轨迹跟踪 |
核心理解：路径是"去哪"，轨迹是"什么时候到哪"。你给控制器一条路径，它不知道是该用1秒跑完还是10秒跑完——这就是很多跟踪震荡的根源。
我记得刚入行时有个项目，AGV在仓库里总是急停急启，电机都快烧了。查了两天发现，规划层只给了路径点，没给时间分配。控制器只能自己猜速度，结果可想而知。
4.2 多项式轨迹：最朴素的参数化方法
既然轨迹需要时间信息，最简单的做法就是用时间 t 的多项式来描述位置。
一个 n 次多项式轨迹长这样：
p(t) = a₀ + a₁·t + a₂·t² + ... + aₙ·tⁿ实际工程中最常用的是三次多项式和五次多项式。
4.2.1 三次多项式（Cubic Polynomial）
三次多项式有4个系数，刚好可以约束起点和终点的位置和速度：
p(0) = p₀, p(T) = p₁
v(0) = v₀, v(T) = v₁解这个方程组就能得到 a₀~a₃。说白了就是"给定起止位置和速度，算出一条平滑曲线"。
我的经验：三次多项式适合点对点的简单运动，比如机械臂从A点到B点抓取。但如果要求加速度也连续（比如高速移动的无人机），三次多项式就不够用了——加速度在起止点会跳变。
4.2.2 五次多项式（Quintic Polynomial）
五次多项式有6个系数，可以额外约束起止点的加速度：
p(0) = p₀, p(T) = p₁
v(0) = v₀, v(T) = v₁
a(0) = a₀, a(T) = a₁这样生成的轨迹，位置、速度、加速度全部连续。我在做自动驾驶路径规划时，五次多项式是标配——尤其是换道场景，加速度不连续会让乘客感觉"被推了一下"。
4.3 样条曲线：多段拼接的艺术
单段多项式只能处理两个点。实际轨迹往往有几十上百个路径点，怎么办？
答案是分段拼接——这就是样条曲线（Spline）的由来。
4.3.1 B样条曲线
B样条的核心思想是：用多个控制点来"拉扯"曲线，但每个控制点只影响局部区域。
它的数学形式是：
C(u) = Σᵢ Nᵢₖ(u) · Pᵢ其中 Nᵢₖ(u) 是基函数，Pᵢ 是控制点。
B样条有几个让我特别喜欢的特点：
- 局部性：移动一个控制点，只改变附近一小段曲线。不像贝塞尔曲线，动一个点整条线都变。
- 连续性可控：通过调整节点向量，可以控制曲线在拼接处的光滑程度（C⁰、C¹、C²）。
- 凸包性：曲线始终在控制点构成的凸包内，不会出现意外的"甩出去"。
我曾经踩过的坑：有一次用B样条做轨迹平滑，控制点间距不均匀，结果曲线在密集区域出现"打结"。后来发现是节点向量没归一化。记住：节点向量最好归一化到[0,1]区间，否则数值稳定性会出问题。
4.3.2 贝塞尔曲线
贝塞尔曲线是另一种经典表示法。它的特点是：
- 曲线通过首尾控制点
- 中间控制点只起"牵引"作用
- 全局性：改变任何一个控制点，整条曲线都会变
贝塞尔曲线的数学形式：
B(t) = Σᵢ C(n,i) · (1-t)ⁿ⁻ⁱ · tⁱ · Pᵢ其中 C(n,i) 是二项式系数。
我个人习惯在可视化调试时用贝塞尔曲线——因为它直观，拖拽控制点就能看到曲线变化。但做实际轨迹规划时，我更倾向B样条，因为局部性太重要了。
4.4 时间参数化：给轨迹注入"灵魂"
有了几何曲线（路径），怎么变成轨迹？
答案是时间参数化——把路径参数 s 映射到时间 t。
常见的做法：
- 等速参数化：s(t) = v · t，最简单，但起止点加速度无穷大。
- 梯形速度剖面：加速-匀速-减速三段，工程中最常用。
- S形速度剖面：加加速度（jerk）也连续，适合高精度场景。
举个例子，假设路径长度 L=10m，期望最大速度 v_max=2m/s，加速度 a_max=1m/s²：
梯形速度剖面：
1. 加速段：t ∈ [0, 2s]，s = 0.5·a·t²
2. 匀速段：t ∈ [2s, 5s]，s = 2 + v·(t-2)
3. 减速段：t ∈ [5s, 7s]，s = 8 + v·(t-5) - 0.5·a·(t-5)²避坑指南：时间参数化一定要考虑执行器的物理限制。我曾经在AGV项目里只算了速度限制，没算加速度限制，结果电机电流直接爆表。从那以后，我每次做时间参数化都会把速度、加速度、加加速度三个约束全列出来。
4.5 本章知识体系总览
下面这张图把本章的核心逻辑串起来了：
4.6 小结
这一章我们聊了轨迹表示的核心内容。说白了就三件事：
- 分清路径和轨迹——路径是几何，轨迹是时间+几何。
- 选对参数化方法——多项式适合点对点，样条适合多点拼接。
- 做好时间参数化——别忘了加加速度约束。
嗯，这些基础打牢了，后面讲控制算法时你才能理解"为什么同样的控制器，换条轨迹效果差这么多"。做工程就是这样，地基不牢，楼盖得再高也得塌。