第三章 路径规划 vs 轨迹规划：区别与联系
说实话，我刚入行那会儿，也把路径规划和轨迹规划混为一谈。总觉得「不就是让机器人从A走到B吗？」直到第一次在产线上调试机械臂，机器人倒是绕过了所有障碍物，但一动起来就像喝醉了酒——急停、抖动、末端执行器差点把工件甩飞。那时候我才意识到，这两件事根本不是一个量级的问题。
这一章，我们就来掰扯清楚路径规划和轨迹规划到底有什么区别，又怎么配合工作。同时我会带你快速过一遍主流的路径规划算法（A*、RRT），然后深入轨迹规划的核心任务。嗯，这些都是后续实战章节的基石。
3.1 路径规划 vs 轨迹规划：一张表说清楚
先给个最直白的定义：
- 路径规划：找一条从起点到终点的几何曲线，只关心空间位置，不关心时间。
- 轨迹规划：给这条路径加上时间信息，告诉机器人「什么时刻应该到达什么位置、速度、加速度是多少」。
你想想看，路径规划回答的是「走哪条路」，轨迹规划回答的是「怎么走才稳、才快、才不抖」。我在项目中遇到过不少新手，路径规划跑通了就以为大功告成，结果一上轨迹规划就崩——说白了，缺的就是这个「时间维度」。
| 对比维度 | 路径规划 | 轨迹规划 |
|---|---|---|
| 输入 | 起点、终点、障碍物地图 | 路径点序列、运动约束（速度/加速度限值） |
| 输出 | 一系列空间点 (x, y, z, ...) | 时间参数化后的状态序列 (t, q, q̇, q̈) |
| 核心约束 | 避障、连通性 | 动力学、平滑性、时间最优 |
| 典型算法 | A*、RRT、Dijkstra、PRM | 梯形速度规划、S形速度规划、多项式插值 |
| 评价指标 | 路径长度、无碰撞 | 执行时间、冲击度、能耗 |
核心观点：路径规划是轨迹规划的前置条件。没有路径，轨迹就是空中楼阁；但只有路径没有轨迹，机器人动起来就是「点头式」运动。
3.2 路径规划算法概览
路径规划算法五花八门，但工业界和学术界最常用的，我归纳为两大类：基于搜索和基于采样。下面重点讲 A* 和 RRT，这两个你以后会反复打交道。
3.2.1 A* 算法：启发式搜索的标杆
A* 说白了就是在 Dijkstra 的基础上加了一个「方向感」。它用代价函数 f(n) = g(n) + h(n) 来指导搜索：
g(n)：从起点到当前节点 n 的实际代价h(n)：从 n 到终点的启发式估计代价（比如欧氏距离）
我建议你记住一个原则：h(n) 必须满足「可容许性」，即估计值不超过真实值。否则 A* 会退化成 Dijkstra，甚至找不到最优解。曾经我在一个仓储机器人项目里，因为用了曼哈顿距离做启发式，结果地图有斜向通道，机器人绕了一大圈——嗯，那就是启发式函数没选对。
小技巧：在栅格地图上，如果允许八方向移动，用欧氏距离做启发式；如果只允许四方向，用曼哈顿距离。别混用。
3.2.2 RRT 算法：高维空间的救星
RRT（快速随机扩展树）的思路很暴力——在地图里随机撒点，从起点开始「生长」出一棵树，直到树梢碰到终点。它不保证最优，但胜在快，尤其适合高维空间（比如机械臂的关节空间）。
核心步骤就三步：
- 采样：在地图自由空间随机取一个点
q_rand - 扩展：在已有树中找到离
q_rand最近的节点q_near，朝q_rand方向步进得到q_new - 碰撞检测：如果
q_near到q_new的边不碰障碍物，就把q_new加入树中
重复这三步，直到 q_new 接近终点。我在调试 RRT 时踩过一个坑：步长设太大，树直接「跳」过狭窄通道；设太小，树长半天够不到终点。后来我总结了一个经验——步长取地图典型障碍物间隙的 1/3 左右，效果比较稳。
注意：RRT 的随机性导致每次运行结果可能不同。如果用在生产环境，建议加后处理平滑（比如样条插值），或者改用 RRT*（带最优性保证的变种）。
3.3 轨迹规划的核心任务
路径规划搞定了「走哪条路」，接下来轨迹规划要解决三个核心问题：
3.3.1 时间参数化
把路径上的离散点 [q0, q1, ..., qn] 映射成时间函数 q(t)。最简单的做法是线性插值，但那样速度会突变，产生无穷大加速度——机器人会「抖」。
我个人习惯用 五次多项式 做插值，因为它能同时约束位置、速度、加速度的边界条件。代码大概长这样：
// 五次多项式轨迹规划示例（伪代码）
function quintic_trajectory(q0, qf, t0, tf, v0, vf, a0, af):
// 计算系数矩阵
T = tf - t0
// 解线性方程组得到 a0~a5
a0 = q0
a1 = v0
a2 = a0 / 2
a3 = (20*qf - 20*q0 - (8*vf + 12*v0)*T - (3*a0 - af)*T^2) / (2*T^3)
a4 = (30*q0 - 30*qf + (14*vf + 16*v0)*T + (3*a0 - 2*af)*T^2) / (2*T^4)
a5 = (12*qf - 12*q0 - (6*vf + 6*v0)*T - (a0 - af)*T^2) / (2*T^5)
// 返回时间函数
return function(t):
dt = t - t0
return a0 + a1*dt + a2*dt^2 + a3*dt^3 + a4*dt^4 + a5*dt^5
3.3.2 运动约束处理
机器人不是赛车，它有物理极限：电机最大转速、关节最大力矩、末端最大速度……轨迹规划必须把这些约束考虑进去。常用的方法是 S形速度规划，把运动分成加加速、匀加速、减加速、匀速、加减速、匀减速、减减速七个阶段。
我曾经在一个码垛机器人项目里，直接用梯形速度规划（只有加速、匀速、减速），结果每次加减速切换时末端都会剧烈抖动，码垛精度直接超差。换成 S 形规划后，冲击度（jerk）被限制住了，问题就解决了。
3.3.3 时间最优与平滑性权衡
理论上，时间最优轨迹就是全程以最大加速度加速到最大速度，然后以最大减速度减速。但这样会产生「方波」加速度曲线，对机械结构冲击很大。实际工程中，我们往往在时间和平滑性之间取折中。
我的经验是：先按时间最优算一版，然后对加速度曲线做低通滤波，或者用优化方法（比如二次规划）显式地最小化加加速度的平方和。这样既保证了效率，又不会把机器人「开散架」。
一句话总结：路径规划管「空间」，轨迹规划管「时间+物理」。两者缺一不可，顺序不能乱。
3.4 本章知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以看到路径规划和轨迹规划在整体流程中的位置，以及它们各自包含的关键算法和任务。
从这张图可以看得很清楚：整个流程是串行的——先有路径，后有轨迹。路径规划负责在空间中找到一条无碰撞的「路」，轨迹规划负责在这条路上赋予「时间节奏」和「物理约束」。两者配合好了，机器人才能走得稳、走得快、走得准。
学习建议：如果你刚开始接触，建议先动手实现一个简单的 A* 路径规划（在栅格地图上），然后再用五次多项式做轨迹插值。把这两个跑通了，后面的 RRT、S形规划、时间最优优化都是锦上添花。