第三章 参考轨迹生成：路径规划与轨迹规划的区别、时间参数化、样条曲线插值
各位同学，欢迎来到第三章。前两章我们把轨迹跟踪的“骨架”搭起来了，讲了系统架构和坐标系。这一章，我们来聊聊一个非常容易混淆的概念——路径和轨迹到底差在哪？
说实话，我刚入行那两年，也经常把这两个词混着用。直到有一次做AGV调度项目，老板问我“轨迹规划好了没”，我说“路径规划好了”，结果被怼了一顿。嗯，从那以后，我才真正把这两个概念刻在脑子里。
一句话总结：路径只关心“走哪条路”，轨迹还要关心“什么时候走到哪里”。
3.1 路径规划 vs 轨迹规划：本质区别
我们先看一个直观的例子。假设你要从A点走到B点，中间有个障碍物。
- 路径规划：给你画出一条绕过障碍物的折线或曲线。它只包含空间信息——
(x, y)坐标序列。 - 轨迹规划：不仅告诉你走这条线，还告诉你“前2秒加速，中间3秒匀速，最后1秒减速”。它包含时间信息——
(x, y, t)或(x, y, v, a)。
我习惯用一个表格来对比，这样更清楚：
| 对比维度 | 路径规划 | 轨迹规划 |
|---|---|---|
| 输出内容 | 空间点序列 (x, y, z) | 时间-状态序列 (t, x, y, v, a) |
| 是否包含时间 | 否 | 是 |
| 是否包含速度/加速度 | 否 | 是 |
| 典型算法 | A*, Dijkstra, RRT | 多项式插值、梯形速度规划、S形速度规划 |
| 应用场景 | 静态环境避障 | 动态跟踪、时间约束任务 |
你想想看，如果只做路径规划，机器人沿着路径走，遇到上坡或者急转弯，速度还是不变，那肯定要出问题。所以，轨迹规划 = 路径规划 + 时间参数化。
3.2 时间参数化：给路径加上“时间轴”
路径规划完成后，我们得到了一堆离散的路径点。但机器人不能直接跳着走，它需要知道每个时刻该到哪。这就是时间参数化的作用。
我个人常用的方法有两种：
3.2.1 等时间间隔采样
最简单粗暴的方式。假设路径总长L，期望速度v，那么总时间T = L / v。然后按固定时间步长Δt，在路径上均匀采样位置点。
// 伪代码示例：等时间间隔采样
double total_length = compute_path_length(path);
double desired_speed = 1.0; // m/s
double total_time = total_length / desired_speed;
double dt = 0.01; // 控制周期 10ms
for (double t = 0; t <= total_time; t += dt) {
double s = t / total_time; // 归一化路径参数 [0, 1]
Point p = interpolate_path(path, s);
trajectory.push_back({t, p.x, p.y});
}
注意：这种方法有个坑——如果路径曲率变化大，等时间间隔会导致转弯处速度突变。我曾经在一个仓库AGV项目里踩过这个坑，车子在拐弯时直接甩尾了。后来改用了基于曲率的速度自适应。
3.2.2 速度曲线规划
更专业的做法是给路径附加速度曲线。常见的速度曲线有：
- 梯形速度曲线：匀加速-匀速-匀减速。简单实用，但加速度有跳变。
- S形速度曲线：加加速度连续，运动更平滑。适合高精度场合。
- 多项式速度曲线：用高阶多项式拟合，灵活性高。
我建议初学者先从梯形速度曲线入手，理解透了再上S形。毕竟，能跑起来的代码，比完美的代码更有价值。
3.3 样条曲线插值：让路径更平滑
路径规划出来的点往往是折线，直接让机器人沿着折线走，每个拐点都要急停急转，效率极低。所以我们需要用样条曲线把这些点“连”成一条光滑的曲线。
常见的样条曲线有：
| 样条类型 | 特点 | 适用场景 |
|---|---|---|
| 三次样条 (Cubic Spline) | 二阶连续，计算简单 | 通用轨迹插值 |
| B样条 (B-Spline) | 局部控制，不全局影响 | 复杂形状拟合 |
| 贝塞尔曲线 (Bezier) | 直观，通过控制点调整 | 路径编辑、可视化 |
| NURBS | 可表示圆锥曲线，精度高 | 工业机器人、数控加工 |
3.3.1 三次样条插值实战
我个人最常用的是三次样条。它保证位置、速度、加速度都连续，对大多数移动机器人来说已经足够。
// 三次样条插值核心逻辑（简化版）
struct CubicSplineSegment {
double a, b, c, d; // 多项式系数
double t_start, t_end;
};
// 计算每个段在时间t处的值
double evaluate(const CubicSplineSegment& seg, double t) {
double dt = t - seg.t_start;
return seg.a + seg.b * dt + seg.c * dt*dt + seg.d * dt*dt*dt;
}
小技巧：实际工程中，我通常不会手动实现样条插值。Eigen库有现成的插值模块，ROS的nav_msgs/Path消息也支持直接发布样条路径。但理解原理很重要——出了问题才知道怎么调。
3.4 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
3.5 避坑指南与个人经验
最后，分享几个我踩过的坑，希望能帮你少走弯路：
- 路径点太密导致轨迹抖动：我曾经在一条10米的路径上插了1000个点，结果样条曲线出现了过冲。后来把点减少到50个，效果反而更好。记住：不是点越多越好。
- 时间参数化忽略加速度约束：有个项目，我直接按最大速度规划时间，结果电机加速太猛，直接过流保护。后来加了加速度和加加速度约束，问题解决。
- 样条边界条件没处理好：三次样条需要两个边界条件（比如起点和终点的速度）。我一开始用了自然样条（二阶导为0），结果起点速度不为0，机器人起步时猛冲了一下。后来改成指定起点速度=0，才平稳起步。
我的建议：如果你刚开始做轨迹规划，先别追求花哨的算法。用三次样条 + 梯形速度曲线，把整个流程跑通。等遇到性能瓶颈了，再考虑B样条、S形曲线这些进阶方案。工程上，简单可靠比复杂完美更重要。
好了，这一章就到这里。路径和轨迹的区别、时间参数化的方法、样条插值的选型，这些概念搞清楚了，后面讲具体跟踪算法时你会轻松很多。