前阵子帮学弟调一台六轴机械臂他用三次多项式做关节空间点到点运动。位置曲线、速度曲线看着都挺顺滑的可一上电机械臂启动瞬间“突”地抖了一下停下来时也明显一顿末端夹具上夹着的小工件都在晃。我一看就明白了三次多项式只约束了位置和速度加速度在端点处是跳变的实际关节驱动力矩在那一瞬间发生突变机械结构共振一下再正常不过。这就是为什么很多入门轨迹规划教程最后都会引到高次多项式——但真正把加加速度jerk也管起来、做到物理上更平滑的还得是七次多项式。这篇博文我会把七次多项式轨迹规划的数学原理、MATLAB完整实现、与三次/五次的实测数据对比还有我在工程里踩过的坑一次性讲清楚。适合正在做机械臂轨迹规划课设、准备机器人方向复试或者自研机械臂想改善启停冲击的工程师们。1. 为什么三次、五次都不够用从“曲线平滑”到“运动冲击”1.1 三次多项式课时最爱的入门模型启动就露馅三次多项式是几乎所有教材第一个讲的轨迹规划方法因为它最简单一个三次多项式 $q(t)a_0a_1ta_2t^2a_3t^3$ 只有4个系数刚好对应4个边界条件——起点位置、终点位置、起点速度、终点速度。把方程组一解系数就出来了计算量极小。但问题恰恰出在这里。当你把起点和终点的速度都设为0时三次多项式虽然能保证位置和速度连续加速度曲线却是一条从某个非零值开始的直线走到末端又变成另一个非零值。物理上这意味着什么机械臂启动瞬间关节角加速度从一个“静止状态下的0”突然跳到非零值关节驱动力矩跟着突变结构就会产生一个冲击响应。你听到的“咔哒”声、看到的末端抖动很大程度就是这么来的。三次多项式的加加速度更没法看。由于加速度在端点突变加加速度在这些点理论上是一个冲激根本不可能用连续函数去表达。哪怕我们在轨迹跟踪时不看这一项机械结构该被激励还是会激励。我的经验是用三次多项式做简单的无负载小惯量演示还行真上负载、上精密装配基本都会被这个启停冲击坑一次。1.2 五次多项式加速度连续了但加加速度又开始跳变五次多项式 $q(t)a_0\dotsa_5t^5$ 有6个系数可以多约束两个边界条件。于是我们可以把起点加速度和终点加速度都设为0这样加速度曲线在端点处不再跳变位置、速度、加速度三阶全部连续。相比三次多项式这已经是质的提升。很多工业伺服运动控制系统在实际使用中就是按“五次多项式端点加速度为0”做的效果也确实比三次好。但如果你把五次多项式的加加速度曲线画出来会发现它是一条抛物线在起点和终点的值不为0。我拿后面代码里那个具体例子算了一下关节从0转到 $\pi/3$时间2秒速度、加速度两端都为0时五次多项式的加加速度在起点大约是 $7.854\ \text{rad/s}^3$终点也是 $7.854\ \text{rad/s}^3$。也就是说从静止状态开始运动的一瞬间加加速度从0直接跳到了7.854。这个跳变对高速高精设备来说就是机械臂底座在高频激励下产生微小振动的来源。多项式位置约束速度约束加速度约束加加速度约束端点加加速度三次有有无无冲激五次有有有无7.854端点跳变七次有有有有0连续1.3 七次多项式把加加速度也钉死在边界上七次多项式有8个系数正好对应8个边界条件起点/终点的位置、速度、加速度、加加速度。这样不仅位置、速度、加速度连续连加加速度也可以显式设为零从静止启动时全程加加速度平滑变化不再有瞬态冲击。可能有人会问那九次、十一次不是更“高级”吗理论上确实可以继续加约束比如约束加加速度的导数snap。但在工程实践中八次以上的多项式带来的收益非常有限反而容易因为高阶项的数值振荡导致轨迹在中途“回摆”。七次多项式是一个比较务实的选择物理意义清晰每个边界条件都有明确的执行器含义数值稳定性也相对可控。如果你拿Simulink或者Gazebo做仿真把三次、五次、七次三种轨迹分别喂给同一个机械臂模型对比末端振动七次的结果通常会是最平稳的。这也是我在后来给多台设备做运动规划时把七次多项式作为点到点运动默认方案的原因。2. 七次多项式的数学原理八个边界条件怎么变成八个系数2.1 多项式通式和边界条件的物理意义七次多项式的通式写成$$q(t)c_0c_1tc_2t^2c_3t^3c_4t^4c_5t^5c_6t^6c_7t^7$$其中 $c_0$ 到 $c_7$ 是待求系数。对时间求一阶导得到速度二阶导得到加速度三阶导得到加加速度。八个边界条件如下时刻位置速度加速度加加速度$t0$$q_0$$v_0$$a_0$$j_0$$tt_f$$q_f$$v_f$$a_f$$j_f$这些边界条件一般来自任务需求比如机械臂要从A点静止开始加速到某个速度再减速到B点静止。大多数点到点场景里$v_0v_f0$$a_0a_f0$$j_0j_f0$。但我后面会强调不是所有场景都适合全部设0有时候中间状态需要非零值七次多项式的好处就是你可以灵活指定。2.2 从边界条件构造线性方程组把8个边界条件代入通式和它的导数式会得到一个 $8\times8$ 的线性方程组。一种比较直观的做法是前4个系数由起点条件直接确定$$c_0q_0,\quad c_1v_0,\quad c_2\frac{a_0}{2},\quad c_3\frac{j_0}{6}$$然后把剩下4个系数 $c_4,c_5,c_6,c_7$ 用终点的4个方程去解。但这个方法每换一种多项式次数都要重新手推公式非常容易出错。我更推荐直接构造完整的8×8矩阵来解这也是下面MATLAB代码使用的方式。矩阵形式是 $A\mathbf{c}\mathbf{b}$其中$$A\begin{bmatrix} 1 0 0 0 0 0 0 0\ 0 1 0 0 0 0 0 0\ 0 0 2 0 0 0 0 0\ 0 0 0 6 0 0 0 0\ 1 t_f t_f^2 t_f^3 t_f^4 t_f^5 t_f^6 t_f^7\ 0 1 2t_f 3t_f^2 4t_f^3 5t_f^4 6t_f^5 7t_f^6\ 0 0 2 6t_f 12t_f^2 20t_f^3 30t_f^4 42t_f^5\ 0 0 0 6 24t_f 60t_f^2 120t_f^3 210t_f^4 \end{bmatrix},\quad \mathbf{b}\begin{bmatrix}q_0\v_0\a_0\j_0\q_f\v_f\a_f\j_f\end{bmatrix}$$这个矩阵的行向量分别对应起点位置、起点速度、起点加速度、起点加加速度、终点位置、终点速度、终点加速度、终点加加速度。只要把这个矩阵看清楚后面无论是三次、五次、七次还是你想扩展到九次都是一样的套路列数等于系数个数行数等于边界条件数解方程即可。2.3 为什么用矩阵解而不是手推公式最早我用三次多项式时也喜欢手推公式因为公式就那么几行写起来快。但后来项目里同时要做三次、五次、七次三种轨迹对比以及处理中间点非零边界条件手推就非常痛苦。矩阵方法有几个明显好处通用性极强。从三次到七次只需要改矩阵尺寸和对应的行向量不需要重新推导系数通项。用MATLAB的A\b解方程组比系数公式更不容易出错而且数值稳定性更好。代码可读性好。过两个月回来看代码矩阵每一行对应哪个边界条件一眼就能看出来。这里要提一个工程小细节当 $t_f$ 比较小、比如0.5秒以内时矩阵里的元素数量级还没什么问题但如果 $t_f$ 很大比如超过10秒矩阵里会出现 $t_f^7$ 这种超大数字条件数变差直接解可能会损失精度。这个问题我在第5节会说怎么处理先用“真实时间域”把原理讲透。3. MATLAB完整实现规划函数、主程序与绘图3.1 poly3、poly5、poly7三个子函数先定义三个计算系数的子函数。注意MATLAB R2016b之后允许在脚本末尾直接写局部函数如果你用的是老版本就把这些函数单独保存成poly7.m之类的函数文件。function c poly3(q0, qf, v0, vf, tf) % 三次多项式系数返回 c [c0 c1 c2 c3] A [ 1, 0, 0, 0; 0, 1, 0, 0; 1, tf, tf^2, tf^3; 0, 1, 2*tf, 3*tf^2 ]; b [q0; v0; qf; vf]; c A\b; end function c poly5(q0, qf, v0, vf, a0, af, tf) % 五次多项式系数返回 c [c0 c1 c2 c3 c4 c5] A [ 1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0; 1, tf, tf^2, tf^3, tf^4, tf^5; 0, 1, 2*tf, 3*tf^2, 4*tf^3, 5*tf^4; 0, 0, 2, 6*tf, 12*tf^2, 20*tf^3 ]; b [q0; v0; a0; qf; vf; af]; c A\b; end function c poly7(q0, qf, v0, vf, a0, af, j0, jf, tf) % 七次多项式系数返回 c [c0 c1 c2 c3 c4 c5 c6 c7] A [ 1, 0, 0, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0, 0, 0; 0, 0, 0, 6, 0, 0, 0, 0; 1, tf, tf^2, tf^3, tf^4, tf^5, tf^6, tf^7; 0, 1, 2*tf, 3*tf^2, 4*tf^3, 5*tf^4, 6*tf^5, 7*tf^6; 0, 0, 2, 6*tf, 12*tf^2, 20*tf^3, 30*tf^4, 42*tf^5; 0, 0, 0, 6, 24*tf, 60*tf^2, 120*tf^3, 210*tf^4 ]; b [q0; v0; a0; j0; qf; vf; af; jf]; c A\b; end这三个函数的核心逻辑完全一样区别只在矩阵大小和边界条件个数。以后如果要做九次多项式你按这个模式把矩阵扩到10×10就行非常方便。3.2 主程序边界条件、轨迹生成、四联图下面的主程序计算三种多项式轨迹并画出位置、速度、加速度、加加速度四个子图。这样可以直接看到七次多项式到底比三次、五次平滑在哪里。% 七次多项式轨迹规划 —— 与三次、五次对比演示 clear; clc; close all; % 边界条件 q0 0; % 初始关节角 (rad) qf pi/3; % 终止关节角 (rad) v0 0; vf 0; % 初始/终止角速度 a0 0; af 0; % 初始/终止角加速度 j0 0; jf 0; % 初始/终止角加加速度 tf 2; % 总时间 (s) dt 0.001; % 采样周期 (s) t 0:dt:tf; % 时间轴 % 三次多项式 coef3 poly3(q0, qf, v0, vf, tf); q3 coef3(1) coef3(2)*t coef3(3)*t.^2 coef3(4)*t.^3; v3 coef3(2) 2*coef3(3)*t 3*coef3(4)*t.^2; a3 2*coef3(3) 6*coef3(4)*t; % 五次多项式 coef5 poly5(q0, qf, v0, vf, a0, af, tf); q5 coef5(1) coef5(2)*t coef5(3)*t.^2 coef5(4)*t.^3 coef5(5)*t.^4 coef5(6)*t.^5; v5 coef5(2) 2*coef5(3)*t 3*coef5(4)*t.^2 4*coef5(5)*t.^3 5*coef5(6)*t.^4; a5 2*coef5(3) 6*coef5(4)*t 12*coef5(5)*t.^2 20*coef5(6)*t.^3; j5 6*coef5(4) 24*coef5(5)*t 60*coef5(6)*t.^2; % 七次多项式 coef7 poly7(q0, qf, v0, vf, a0, af, j0, jf, tf); q7 coef7(1) coef7(2)*t coef7(3)*t.^2 coef7(4)*t.^3 ... coef7(5)*t.^4 coef7(6)*t.^5 coef7(7)*t.^6 coef7(8)*t.^7; v7 coef7(2) 2*coef7(3)*t 3*coef7(4)*t.^2 4*coef7(5)*t.^3 ... 5*coef7(6)*t.^4 6*coef7(7)*t.^5 7*coef7(8)*t.^6; a7 2*coef7(3) 6*coef7(4)*t 12*coef7(5)*t.^2 20*coef7(6)*t.^3 ... 30*coef7(7)*t.^4 42*coef7(8)*t.^5; j7 6*coef7(4) 24*coef7(5)*t 60*coef7(6)*t.^2 120*coef7(7)*t.^3 ... 210*coef7(8)*t.^4; % 绘图 figure(Color, w, Position, [100 100 800 700]); subplot(4,1,1); plot(t, q3, --, t, q5, -., t, q7, -, LineWidth, 1.5); ylabel(位置 (rad)); legend(三次, 五次, 七次, Location, best); title(位置对比); grid on; xlim([0 tf]); subplot(4,1,2); plot(t, v3, --, t, v5, -., t, v7, -, LineWidth, 1.5); ylabel(速度 (rad/s)); legend(三次, 五次, 七次, Location, best); title(速度对比); grid on; xlim([0 tf]); subplot(4,1,3); plot(t, a3, --, t, a5, -., t, a7, -, LineWidth, 1.5); ylabel(加速度 (rad/s^2)); legend(三次, 五次, 七次, Location, best); title(加速度对比); grid on; xlim([0 tf]); subplot(4,1,4); plot(t, j5, -., t, j7, -, LineWidth, 1.5); % 三次多项式jerk端点冲激不画 ylabel(加加速度 (rad/s^3)); legend(五次, 七次, Location, best); title(加加速度对比); grid on; xlim([0 tf]); % 边界条件校验 fprintf(七次多项式边界校验\n); fprintf(q(0)%.6f, q(tf)%.6f\n, q7(1), q7(end)); fprintf(v(0)%.6f, v(tf)%.6f\n, v7(1), v7(end)); fprintf(a(0)%.6f, a(tf)%.6f\n, a7(1), a7(end)); fprintf(j(0)%.6f, j(tf)%.6f\n, j7(1), j7(end)); % 峰值统计 fprintf(峰值速度三次 %.4f, 五次 %.4f, 七次 %.4f rad/s\n, ... max(abs(v3)), max(abs(v5)), max(abs(v7))); fprintf(峰值加速度三次 %.4f, 五次 %.4f, 七次 %.4f rad/s^2\n, ... max(abs(a3)), max(abs(a5)), max(abs(a7))); fprintf(峰值加加速度五次 %.4f, 七次 %.4f rad/s^3\n, ... max(abs(j5)), max(abs(j7)));运行这段程序你会在命令窗口看到类似下面的校验结果七次多项式边界校验 q(0)0.000000, q(tf)1.047198 v(0)0.000000, v(tf)0.000000 a(0)0.000000, a(tf)0.000000 j(0)0.000000, j(tf)0.000000这说明七次多项式的八个边界条件全部精确满足不是“近似满足”。3.3 代码里几个值得留意的“为什么”这个代码看起来简单但有几个点值得展开说一下。第一为什么用A\b而不是inv(A)*bA\b是MATLAB求解线性方程组的标准方式底层用的是LU分解或更稳定的算法数值上更可靠速度也更快。对于8×8矩阵两者差别不大但这是一个好的编码习惯尤其当矩阵阶数变大或者条件数变差的时候差别就体现出来了。第二为什么不用polyval而是一行行直接展开polyval确实能把多项式求值写得更短但它要求系数向量从高次到低次排列求出来的结果还要再flip一次反而容易混淆。在教程和工程验证代码里我更喜欢直接展开写可读性最高。等你确认算法没问题再改成polyval去优化代码量也不迟。第三采样周期为什么设成dt 0.001 1ms是很多运动控制器和仿真器的默认控制周期用来观察轨迹已经足够细腻。如果你的控制周期是500Hz或者2kHz把dt改成对应值即可不影响规划结果。实际下发到控制器时只会从这条连续轨迹里按控制周期取点。4. 实测数据对比三种多项式的差异到底有多大4.1 位置与速度视觉差别不大峰值却很现实把三条位置曲线叠在一起看三次、五次、七次都是从0走到 $\pi/3$位置曲线整体轮廓差别不大。但如果看速度曲线差异就来了三次多项式的速度峰值大约 $0.785\ \text{rad/s}$最大速度出现在总时间的中点五次多项式峰值约 $0.982\ \text{rad/s}$七次多项式峰值约 $1.145\ \text{rad/s}$。这里的规律可能反直觉约束越多的轨迹中间段“冲”得越猛。原因也好理解七次多项式要求在起点和终点各阶导数都为零等于把一个S形曲线的两端都“压平”了为了在同样的2秒内走完同样的角度中段速度只能提上去。对于真实机械臂这意味着七次多项式虽然更平滑但电机的峰值转速需求可能比三次多项式更高选电机时要注意留裕量。指标三次多项式五次多项式七次多项式峰值速度0.785 rad/s0.982 rad/s1.145 rad/s峰值加速度1.571 rad/s²约1.510 rad/s²约1.96 rad/s²端点加速度±1.571跳变00端点加加速度冲激7.854跳变04.2 加速度三次端点跳变五次七次端点为零三次多项式的加速度曲线是一条直线起点处加速度直接是 $1.571\ \text{rad/s}^2$终点处是 $-1.571\ \text{rad/s}^2$。这意味着机械臂在启动瞬间就要承受一个“无中生有”的角加速度力矩突变必然带来振动。五次和七次多项式的加速度曲线两端都是0从数值上看这两者的加速度连续性是一致的。它们真正的区别不在加速度而在加加速度。4.3 加加速度七次真正的优势以及“限制更多反而峰值更高”的反直觉结论加加速度曲线是区分五次和七次的试金石。五次多项式的加加速度是一条抛物线在本例中起点和终点的值都是 $7.854\ \text{rad/s}^3$中间会变小。也就是说机械臂启动那一瞬间加加速度从0跳变到7.854虽然这个跳变不像三次的冲激那么恶劣但依然是一个高频激励来源。七次多项式的加加速度曲线两端精确为0从0开始变化到某个峰值再回到0全程连续。在本例中峰值大约 $5.5\ \text{rad/s}^3$ 量级比五次的端点值还小一点。注意这并不是说七次多项式在任何场景下加加速度峰值都更小——反过来经常会发生。用七次多项式时如果中间段为了满足更多约束而必须“让”得更快加加速度峰值完全可能比五次更大。关键区别从来不是“谁的峰值更小”而是“谁的加加速度是连续的、可控的”。这组对比告诉我们判断轨迹好不好不能只看位置和速度曲线一定要把加速度、加加速度一起画出来看。很多实际振动问题在位置曲线上根本看不出来一画加加速度就全明白了。5. 工程落地七次多项式实战中必须避开的几个坑5.1 过冲与回摆不是所有边界条件都不会超调我见过不少人把七次多项式当万能药结果把边界条件随便一改轨迹就出现了明显的过冲位置先冲过目标点再慢慢回来。更麻烦的是有些过冲不一定明显你不画图根本发现不了。比如上面那个案例$q_00, q_f\pi/3$所有端点导数都为0位置曲线是单调的没问题。但如果你把起点加加速度设成非零值或者终点加速度设成某个值去匹配下一段轨迹位置曲线就可能在中途超出 $[q_0, q_f]$ 的区间。这在机械臂轨迹拼接、避障场景里非常危险因为超调意味着末端可能撞到目标点附近的东西。我的习惯是每次规划完先用min(q7)和max(q7)检查是否超界再决定要不要调整边界条件或时间。这个检查在代码里加一行就行但能挡住很多隐蔽问题。5.2 速度、加速度、加加速度峰值超限检验七次多项式只保证数学上的平滑不保证执行器一定扛得住。我之前就遇到过这种情况轨迹算出来很漂亮速度、加速度曲线也连续但一上实际关节伺服报警“过速”或者“过流”。原因就是峰值速度超过电机额定转速或者峰值加速度对应的力矩超过电机峰值扭矩。所以规划完成后必须做峰值检验。可以用解析法求速度/加速度/加加速度导数的零点也可以在轨迹上用较细的采样点做近似扫描。工程上我一般直接采一大把点取max(abs(...))效率足够代码也短。如果发现某个物理量超限通常的调整手段有三种延长运动时间 $t_f$、调整边界条件让过渡更缓和、或者把轨迹细分多段来“削峰”。5.3 时间归一化tf变大时矩阵条件数的处理前面提到过真实时间域的矩阵里会出现 $t_f^7$ 这种项。当 $t_f$ 只有2秒时没事但如果你规划一个慢速过程比如机械臂用20秒走过一段长路径$20^71.28\times10^9$这时候矩阵元素跨度从1到十亿条件数会变得很差解出来的系数精度可能损失。解决办法是时间归一化。设 $\taut/t_f$让 $\tau\in[0,1]$多项式改写成 $q(\tau)k_0k_1\tau\dotsk_7\tau^7$。边界条件需要换算$k_0 q_0$$k_1 v_0 t_f$$k_2 a_0 t_f^2 / 2$$k_3 j_0 t_f^3 / 6$同理终点的速度、加速度、加加速度边界也要分别乘以 $t_f$、$t_f^2$、$t_f^3$。然后所有矩阵里的 $t_f$ 全部用1代入解出的系数会比直接解真实时间域稳定很多。实际使用轨迹时时间轴按下式还原$t \tau t_f$。我在实际项目里基本都写归一化版本因为它不需要根据运动时间长短去调整算法一次封装到处用。你自己写代码时建议一上来就按归一化时间去写少踩一个坑。5.4 多段轨迹拼接把上一段的末端状态传给下一段的起点很多工程任务不是一个简单的点到点而是多个路径点顺序执行。这时候七次多项式最大的价值就体现出来了它可以把上一段轨迹终点的位置、速度、加速度、加加速度全部“带”到下一段作为下一段的起点边界条件从而保证整条轨迹在拼接点处一直到加加速度都是连续的。具体做法很简单先规划第一段得到 $\mathbf{q}{end}, \mathbf{v}{end}, \mathbf{a}{end}, \mathbf{j}{end}$然后把它作为第二段的 $q_0, v_0, a_0, j_0$再给定第二段的终点状态和时间继续解七次多项式。这个过程可以一直串下去形成一条全局高阶连续的轨迹。不过要注意一点中间点处的速度、加速度、加加速度值不是你凭空定的而是由前一段轨迹的末端状态决定的。如果你希望机械臂经过某个路径点时不停顿、匀速通过那就得先把整条路径的速度规划好然后反推每一段的边界条件。这已经属于轨迹规划里比较进阶的用法但七次多项式给了你实现它的自由度。还有一个类似的小技巧如果你手头的轨迹只是普通的五次多项式拼接中间点加速度连续但加加速度突变换成七次多项式后可以让中间点的加加速度也连续。实测下来对要求高节拍的上下料应用这种连续性的提升带来的稳定性收益非常明显。最后说一个我被坑过的小细节别把边界条件里的单位搞混。角度是弧度不是度速度是弧度每秒加加速度是弧度每三次方秒。这个看似简单但我有一阵子从别的项目拷贝初始化代码忘了改单位结果轨迹规划出来速度曲线飞上天查了半天才发现是角度制换算问题。七次多项式本身不难难的是把每个环节的单位、边界条件、物理意义都弄得清清楚楚。建议你把上面的代码原样跑一遍然后改改 $q_f$、$t_f$再试试非零的起点加加速度很快就能建立起对七次多项式的直觉。