4. 关节空间轨迹规划：梯形速度、S形速度与多项式插值

关节空间轨迹规划，说白了就是让机器人每个关节从A点转到B点，中间怎么走、速度怎么变。你想想看，如果直接让电机从0速瞬间跳到目标速度，那冲击力能把减速机打坏。所以我们需要一条平滑的曲线。

我个人习惯把这类规划分成两大类：分段式速度规划（梯形、S形）和多项式插值（三次、五次、七次）。前者直观好调，后者数学上更优雅。今天咱们就把这五种方法掰开揉碎了讲。

4.1 梯形速度规划

梯形速度规划是最基础的方法。速度曲线像梯形：匀加速、匀速、匀减速三段。我最早做六轴机器人时，用的就是它。简单、可靠、计算量小。

核心参数就三个：最大速度 \(v_{max}\)、最大加速度 \(a_{max}\)、总位移 \(\Delta q = q_f - q_0\)。如果位移足够大，能跑到最大速度，那就是完整梯形；如果位移太小，还没加速到 \(v_{max}\) 就得减速了，变成三角形。

梯形规划的数学表达很简单：

// 梯形速度规划核心逻辑
// 输入：起始角度 q0，终止角度 qf，最大速度 vmax，最大加速度 amax
// 输出：t 时刻的角度 q(t)

double trapezoidal_profile(double q0, double qf, double vmax, double amax, double t) {
    double dq = qf - q0;
    double ta = vmax / amax;          // 加速段时间
    double td = vmax / amax;          // 减速段时间
    double dq_acc = 0.5 * amax * ta * ta;  // 加速段位移
    double dq_dec = 0.5 * amax * td * td;  // 减速段位移
    
    // 判断是否达到最大速度
    if (dq_acc + dq_dec >= fabs(dq)) {
        // 三角形：未达到最大速度
        ta = sqrt(fabs(dq) / amax);
        td = ta;
        vmax = amax * ta;
    }
    
    double tc = (fabs(dq) - dq_acc - dq_dec) / vmax;  // 匀速段时间
    
    // 根据 t 所在区间计算位置
    if (t <= ta) {
        // 加速段
        return q0 + 0.5 * amax * t * t;
    } else if (t <= ta + tc) {
        // 匀速段
        return q0 + dq_acc + vmax * (t - ta);
    } else if (t <= ta + tc + td) {
        // 减速段
        double t_dec = t - ta - tc;
        return q0 + dq_acc + vmax * tc + vmax * t_dec - 0.5 * amax * t_dec * t_dec;
    } else {
        return qf;
    }
}

4.2 S形速度规划

S形速度规划解决了梯形规划的加速度突变问题。它在加速段和减速段各加了加加速度（Jerk）限制，让加速度平滑变化。说白了，就是速度曲线从梯形变成了S形。

S形规划有7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，但实际用起来很顺手。

阶段	加速度变化	Jerk符号	典型应用
加加速	0 → amax	+J	启动阶段
匀加速	保持 amax	0	快速提速
减加速	amax → 0	-J	过渡到匀速
匀速	0	0	稳定运行
加减速	0 → -amax	-J	开始减速
匀减速	保持 -amax	0	持续减速
减减速	-amax → 0	+J	平稳停止

S形规划的核心代码框架：

// S形速度规划 - 7段式
// 参数：q0, qf, vmax, amax, jerk_max

void s_curve_profile(double q0, double qf, double vmax, double amax, double jerk, double t) {
    // 1. 计算各段时间
    double ta = amax / jerk;           // 加加速/减加速时间
    double tv = (vmax - amax * ta) / amax;  // 匀加速时间（如果存在）
    // ... 实际需要判断是否达到 vmax 和 amax
    
    // 2. 根据 t 所在区间选择公式
    // 加加速段: q = q0 + (1/6)*jerk*t^3
    // 匀加速段: q = q0 + (1/6)*jerk*ta^3 + (1/2)*amax*(t-ta)^2 + ...
    // 减加速段: q = q0 + ... 
    // 匀速段:   q = q0 + ... + vmax*(t - t_acc_total)
    // 加减速段: q = q0 + ... - (1/6)*jerk*(t - t_dec_start)^3
    // 匀减速段: ...
    // 减减速段: ...
}

4.3 多项式插值

多项式插值跟分段规划的思路完全不同。它用一个多项式函数直接拟合整个轨迹。你只需要给定起点和终点的位置、速度、加速度约束，多项式系数就解出来了。

4.3.1 三次多项式

三次多项式是最简单的连续轨迹。它只有4个系数，刚好满足4个约束：起点位置、终点位置、起点速度、终点速度。

// 三次多项式插值
// q(t) = a0 + a1*t + a2*t^2 + a3*t^3
// 约束: q(0)=q0, q(T)=qf, q'(0)=v0, q'(T)=vf

void cubic_poly(double q0, double qf, double v0, double vf, double T) {
    double a0 = q0;
    double a1 = v0;
    double a2 = (3*(qf - q0) - (2*v0 + vf)*T) / (T*T);
    double a3 = (2*(q0 - qf) + (v0 + vf)*T) / (T*T*T);
    
    // 任意时刻 t 的位置
    // q(t) = a0 + a1*t + a2*t^2 + a3*t^3
}

三次多项式的缺点是加速度在起点和终点不连续。如果你需要精确控制加速度，就得用更高次的多项式。

4.3.2 五次多项式

五次多项式有6个系数，可以额外约束起点和终点的加速度。这样加速度就是连续的，不会出现突变。

// 五次多项式插值
// q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
// 约束: 位置、速度、加速度在起点和终点

void quintic_poly(double q0, double qf, double v0, double vf, 
                  double a0_in, double af, double T) {
    double a0 = q0;
    double a1 = v0;
    double a2 = a0_in / 2.0;
    
    double T2 = T*T;
    double T3 = T2*T;
    double T4 = T3*T;
    double T5 = T4*T;
    
    double dq = qf - q0;
    
    double a3 = (20*dq - (8*vf + 12*v0)*T - (3*a0_in - af)*T2) / (2*T3);
    double a4 = (30*dq + (14*vf + 16*v0)*T + (3*a0_in - 2*af)*T2) / (2*T4);
    double a5 = (12*dq - (6*vf + 6*v0)*T - (a0_in - af)*T2) / (2*T5);
}

4.3.3 七次多项式

七次多项式有8个系数，可以额外约束起点和终点的加加速度（Jerk）。这样整个轨迹的加加速度也是连续的，运动极其平滑。

七次多项式的系数求解比较繁琐，但好在是线性方程组，用矩阵求逆就能搞定。我一般用下面的方式组织约束矩阵：

// 七次多项式约束矩阵
// [q0]   [1  0   0    0    0    0    0    0 ] [a0]
// [v0]   [0  1   0    0    0    0    0    0 ] [a1]
// [a0]   [0  0   2    0    0    0    0    0 ] [a2]
// [j0]   [0  0   0    6    0    0    0    0 ] [a3]
// [qf] = [1  T  T^2  T^3  T^4  T^5  T^6  T^7] [a4]
// [vf]   [0  1  2T   3T^2 4T^3 5T^4 6T^5 7T^6] [a5]
// [af]   [0  0   2   6T  12T^2 20T^3 30T^4 42T^5] [a6]
// [jf]   [0  0   0   6   24T  60T^2 120T^3 210T^4] [a7]

// 解这个8x8线性方程组即可得到系数 a0~a7

4.4 五种方法对比

方法	连续阶	计算量	参数数量	适用场景
梯形速度	位置连续，速度连续	极低	3	简单搬运、低速场景
S形速度	位置/速度/加速度连续	中等	4	通用工业机器人
三次多项式	位置/速度连续	低	4	点到点运动
五次多项式	位置/速度/加速度连续	低	6	精密装配、焊接
七次多项式	位置/速度/加速度/Jerk连续	中等	8	高速高精度、光学检测

选哪种方法，取决于你的应用场景。如果只是简单的点到点搬运，梯形规划就够了。如果是精密装配或者高速运动，我建议至少用五次多项式。至于七次多项式，嗯，除非你对加加速度有硬性要求，否则没必要给自己找麻烦。