4. 轨迹规划：关节空间与操作空间

轨迹规划这章，说白了就是让机器人「怎么动」的问题。你给机器人一个起点和终点，它得自己算出一条路来——不能撞到自己，不能抖得像帕金森，还得在合理时间内走完。

我个人习惯把轨迹规划分成两大类：关节空间和操作空间。这两者的区别，我刚开始做项目时也迷糊过一阵子，后来踩了几个坑才真正搞明白。

4.1 关节空间轨迹规划

关节空间规划，就是直接在机器人的每个关节角度上做文章。你不管末端执行器在空间里画了什么曲线，只关心每个关节从角度 A 转到角度 B 时，速度、加速度怎么变化。

这样做的好处很明显——计算量小，而且不会遇到奇异性问题。但代价是末端轨迹在笛卡尔空间里是不可预测的，可能走一条很诡异的弧线。

4.1.1 多项式轨迹

最简单的思路：给每个关节的起点角度和终点角度之间，拟合一条光滑曲线。

三次多项式是最常用的入门方案。它需要四个约束条件：起点位置、终点位置、起点速度、终点速度（通常设为零）。

// 三次多项式系数求解
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³

// 约束条件
θ(0) = θ₀          // 起点角度
θ(tf) = θf         // 终点角度
θ'(0) = 0          // 起点速度
θ'(tf) = 0         // 终点速度

// 解出系数
a₀ = θ₀
a₁ = 0
a₂ = 3·(θf - θ₀) / tf²
a₃ = -2·(θf - θ₀) / tf³

嗯，这里要注意：三次多项式只能保证位置和速度连续，加速度在起点和终点会有跳变。如果你做的是高速抓取，这种跳变会引起末端抖动。

五次多项式多了两个约束——起点加速度和终点加速度都设为零。这样位置、速度、加速度全部连续，运动更平滑。

// 五次多项式
θ(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵

// 六个约束条件
θ(0) = θ₀,  θ(tf) = θf
θ'(0) = 0,  θ'(tf) = 0
θ''(0) = 0, θ''(tf) = 0

4.1.2 梯形速度轨迹

多项式轨迹虽然平滑，但有个问题——你不知道最大速度是多少。实际项目中，电机有速度上限、加速度上限，你不能让规划结果超出硬件能力。

梯形速度轨迹就是来解决这个问题的。它把运动分成三段：加速段、匀速段、减速段。你直接设定最大速度 v_max 和最大加速度 a_max，规划器保证不越界。

// 梯形速度轨迹三段式
// 加速段：0 ≤ t ≤ ta
θ(t) = θ₀ + 0.5·a_max·t²
θ'(t) = a_max·t

// 匀速段：ta ≤ t ≤ tf - ta  
θ(t) = θ₀ + 0.5·a_max·ta² + v_max·(t - ta)
θ'(t) = v_max

// 减速段：tf - ta ≤ t ≤ tf
θ(t) = θf - 0.5·a_max·(tf - t)²
θ'(t) = a_max·(tf - t)

你想想看，梯形速度最大的优点就是直观。我在调试现场时，经常直接看速度曲线是不是梯形——如果不是，说明参数设得有问题。

4.2 操作空间轨迹规划

关节空间规划虽然简单，但有个硬伤——末端走的路径不可控。如果你需要机器人末端走一条直线、画一个圆弧，或者沿着工件表面运动，就必须在操作空间（笛卡尔空间）里做规划。

操作空间规划的核心流程是：

1. 在笛卡尔空间里生成路径点（位置 + 姿态）
2. 对每个路径点做逆运动学求解，得到关节角度
3. 在关节空间做插值，保证运动平滑

说白了，操作空间规划是「先定路径，再算关节」。关节空间规划是「先定关节，不管路径」。

4.2.1 直线轨迹规划

直线轨迹是最常见的需求。给定起点 P₀ 和终点 Pf，在两者之间线性插值：

// 位置线性插值
P(s) = P₀ + s·(Pf - P₀),  s ∈ [0, 1]

// 姿态用球面线性插值（SLERP）
// 避免欧拉角的万向锁问题
R(s) = slerp(R₀, Rf, s)

这里 s 是归一化参数，你可以用前面讲的多项式或梯形速度来控制 s 随时间的变化。这样末端走的是直线，但速度曲线可以自己定。

4.2.2 圆弧轨迹规划

圆弧轨迹在焊接、打磨、涂胶等场景中很常见。三点确定一个圆弧：起点、中间点、终点。

// 圆弧参数计算
1. 由三点确定圆心 O 和半径 r
2. 计算起始角 θ₀ 和终止角 θf
3. 沿圆弧角度插值：
   θ(s) = θ₀ + s·(θf - θ₀)
   P(s) = O + r·[cos(θ(s)), sin(θ(s)), 0]^T

嗯，这里要注意：圆弧轨迹在三维空间里可能不在一个平面上。如果三点不共面，你需要先拟合一个最佳平面，或者用样条曲线代替。

4.3 关节空间 vs 操作空间：怎么选？

我经常被问到这个问题。其实没有的好坏，关键看场景。

4.4 本章知识体系

下面这张图总结了轨迹规划的完整脉络，你可以把它当作快速索引：

4.5 避坑指南

最后分享几个我这些年积累的经验，希望能帮你少走弯路：

• 不要迷信高阶多项式。七次多项式虽然平滑，但参数多、调起来费劲。我一般先用五次多项式，如果抖动明显再升级。
• 梯形速度的加速度不能设太大。我曾经为了赶节拍，把加速度设到电机极限的 90%，结果机器人跑起来像在跳舞——末端抖动严重。后来控制在 60%-70% 就稳了。
• 操作空间规划一定要做碰撞检测。因为末端走直线时，其他关节可能撞到周围设备。我习惯在规划阶段用包围盒做快速碰撞检查，而不是等到运行时才发现。
• 逆运动学求解失败时，不要直接报错。可以尝试在路径点附近微调姿态，或者切换到关节空间规划做降级处理。系统鲁棒性比完美路径更重要。