第三节：关节空间轨迹规划——梯形速度、S形速度与多项式插值
各位工程师，大家好。今天我们来聊聊关节空间轨迹规划。说实话，这是自动化产线里最基础也最容易被忽视的环节。我见过不少项目，机器人动起来咔咔响，或者末端抖动得厉害，十有八九是轨迹规划没做好。
关节空间规划，说白了就是让每个关节从A点转到B点，怎么转、转多快、加减速怎么控制。你想想看，如果直接让电机从0速瞬间跳到目标速度，那冲击力能把减速机打坏。所以我们需要一条平滑的速度曲线。
一、梯形速度规划
梯形速度规划是最简单、最常用的方法。它的速度曲线像个梯形——匀加速、匀速、匀减速三段。
核心思想： 先加速到最大速度，保持匀速，最后减速到零。
我在项目中遇到过一个问题：梯形规划虽然简单，但加速度在拐点处突变。也就是说，从加速切换到匀速那一瞬间，加速度从最大值直接跳变到0。这会导致关节产生冲击，俗称“抖一下”。
不过，对于大多数搬运、码垛场景，梯形规划完全够用。它的计算量小，实时性好。
// 梯形速度规划示例（伪代码）
float t_acc = v_max / a_max; // 加速时间
float t_dec = v_max / d_max; // 减速时间
float s_acc = 0.5 * a_max * t_acc * t_acc; // 加速段位移
float s_dec = 0.5 * d_max * t_dec * t_dec; // 减速段位移
float s_const = total_dist - s_acc - s_dec; // 匀速段位移
float t_const = s_const / v_max; // 匀速段时间我的经验： 梯形规划中，如果总位移太短，可能连加速都没完成就要减速了。这时候需要降低最大速度，或者干脆用三角形速度曲线（只有加速和减速）。
二、S形速度规划
S形速度规划比梯形更“温柔”。它的加速度不是突变的，而是连续变化的。说白了，就是加加速度（Jerk）可控。
为什么要用S形？我曾经调试一条高速分拣线，机器人每分钟要抓取120次。用梯形规划时，末端抖动导致视觉识别偶尔丢帧。换成S形规划后，抖动明显减少，识别率从97%提升到了99.5%。
S形规划通常分为7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但实际实现时可以用查表法或解析法。
| 阶段 | 加速度变化 | 速度变化 |
|---|---|---|
| 加加速 | 0 → a_max | 缓慢增加 |
| 匀加速 | 保持 a_max | 线性增加 |
| 减加速 | a_max → 0 | 缓慢增加 |
| 匀速 | 0 | 保持不变 |
| 加减速 | 0 → -a_max | 缓慢减小 |
| 匀减速 | 保持 -a_max | 线性减小 |
| 减减速 | -a_max → 0 | 缓慢减小 |
注意： S形规划的计算量比梯形大不少。如果控制器性能有限，建议用查表法代替实时计算。我曾经在ARM Cortex-M4上跑过，7段解析计算大约需要50微秒，还算能接受。
三、多项式插值
多项式插值适合对轨迹平滑度要求极高的场景。比如精密装配、激光焊接等。
三次多项式
三次多项式是最简单的插值方式。它保证位置和速度连续，但加速度不连续。公式是：
θ(t) = a0 + a1*t + a2*t² + a3*t³四个系数由起点和终点的位置、速度四个条件唯一确定。嗯，这里要注意：三次多项式只能保证速度连续，加速度在起点和终点会有跳变。
五次多项式
五次多项式比三次多两个自由度。它可以同时约束起点和终点的位置、速度、加速度。公式是：
θ(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵六个系数由六个边界条件确定。我个人习惯在精密装配中用五次多项式，因为加速度连续，冲击小。
七次多项式
七次多项式更进一步，可以约束加加速度（Jerk）。公式是：
θ(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵ + a6*t⁶ + a7*t⁷八个系数由位置、速度、加速度、加加速度的边界条件确定。说实话，七次多项式在实际产线中用得不多，因为计算量大，而且效果和五次多项式差别不大。除非你搞高精度光学对准，否则没必要。
避坑指南： 我曾经在一条汽车焊装线上，用七次多项式规划焊接轨迹。结果因为计算周期太长，导致控制器超时报警。后来换成五次多项式，效果一样好，计算量却少了一半。所以，别盲目追求高阶次。
四、多段轨迹衔接
实际产线中，机器人很少只走一段轨迹。通常是多段轨迹连续执行，比如从A到B再到C。这时候就涉及轨迹衔接问题。
多段衔接有两种方式：
- 点对点衔接： 每段轨迹独立规划，到达终点后停止，再启动下一段。简单但效率低。
- 连续衔接： 前一段的终点速度作为后一段的起点速度，中间用过渡曲线平滑连接。效率高但实现复杂。
我建议在高速产线中使用连续衔接。比如在码垛场景中，抓取点和放置点之间通常有一段直线运动，如果每段都停一下再走，节拍会慢很多。
连续衔接的常用方法是：在两段轨迹之间插入一段过渡曲线（比如圆弧或样条曲线），保证位置、速度、加速度连续。过渡段的长度一般取总轨迹的5%~10%。
// 多段轨迹衔接示例（简化）
// 段1：从P0到P1，终点速度v1
// 段2：从P1到P2，起点速度v1
// 在P1附近插入过渡曲线
float blend_radius = 10.0; // 过渡半径
float blend_time = blend_radius / v1; // 过渡时间
// 在过渡时间内，用五次多项式平滑连接注意： 过渡半径不能太大，否则会偏离原轨迹太多。也不能太小，否则加速度会过大。我一般取轨迹长度的5%作为初始值，然后根据实际运动效果微调。
五、知识体系总览
下面这张图总结了关节空间轨迹规划的核心内容。你可以把它当作一个快速参考。
好了，关节空间轨迹规划的核心内容就这些。梯形规划简单实用，S形规划平滑高效，多项式插值适合精密场景，多段衔接则是产线落地的关键。下次遇到机器人抖动或者节拍上不去的问题，不妨从轨迹规划入手排查一下。
公众号:蓝海资料掘金营，微信deep3321