做机器人轨迹规划这些年我见过太多新人在用三次多项式结果机械臂一动就咔咔响、末端抖个不停。为什么会这样因为三次多项式只能保证位置和速度连续加速度在起止点直接跳变。别小看这个跳变在高速运动和高精度定位场景下它带来的冲击和振动足以毁掉一次调试。今天我们聊的七次多项式就是专门解决这类问题的实用方案用MATLAB写出来也就几十行却能帮你把机械臂轨迹的平滑度拉高一个档次。这篇文章我会讲清楚三件事为什么七次多项式比三次、五次更适合要求严格的轨迹规划它的8个边界条件是怎么定出来的以及怎么用MATLAB把它封装成一个工程函数直接拿来用。我会把完整代码贴出来从矩阵求解到结果可视化每一行都讲明白。无论是刚接触机械臂轨迹规划的本科生还是需要在项目里落地平滑轨迹的工程师都能从这里拿走可以直接运行的东西。1. 从三次到七次轨迹规划中的阶次选择1.1 三次多项式的软肋到底在哪三次多项式轨迹规划的公式很简单就是 q(t) a0 a1·t a2·t² a3·t³总共4个系数带4个边界条件起止位置和起止速度。这4个条件分别代入求导后的方程就能把a0到a3解出来。问题是位置和速度连续了加速度呢你自己算一下就知道了加速度是 q(t) 2a2 6a3·t这是一个线性函数。在起点 t0 处加速度等于 2a2在终点 tT 处加速度等于 2a2 6a3·T。这代表什么代表机械臂从静止状态启动的一瞬间加速度从0直接跳到了2a2结束的时候又从某个值瞬间归零。这个加速度跳变对应的是一种无限大的加加速度在物理上就是给机构一个近似阶跃的冲击力。低速低负载场景下可能不明显但一旦速度提高、负载变大这种冲击轻则引起末端抖动重则让减速器齿轮磨损加剧、螺栓松动。你坐汽车时如果司机总是突然踩油门又突然刹停乘客会是什么感觉机械臂也是一样的只不过它不会抱怨只会用振动和精度下降来回报你。1.2 五次和七次分别解决了什么问题既然三次多项式的问题出在加速度不连续上那自然就想到把多项式的阶次抬高多留几个自由度去约束更高阶的导数。五次多项式有6个系数可以加两个边界条件通常就是把起止加速度 a0 和 af 约束进去。这样一来加速度变成连续的了起止点不再有加速度跳变。听起来问题解决了但你再看三阶导数也就是加加速度jerk它变成了一个常数。加速度在起止点确实一致了但加加速度在起点处从0跳到一个非零常数到终点处又从某个常数跳回0。这个“jerk冲击”虽然影响比加速度冲击温和一些但在高精度轨迹跟踪和高动态响应场景下依然会激励机械结构的高频模态引发残余振动。七次多项式就是把jerk也作为边界条件约束进去。8个系数对应8个边界条件位置、速度、加速度、jerk各给定起止两个值。这样从位置到jerk四阶量全部连续整个运动过程的“冲击感”基本消除了。你把任意时刻的jerk画出来会是一条连续的三次曲线起止点jerk相等且通常设为0真正做到了“起得柔、停得稳”。1.3 七次多项式适合哪些实际场景不是所有轨迹规划都非用七次不可但有几类场景它几乎是标配。第一类是高速高精度机械臂。比如SCARA、六轴机器人在搬运小物件时的点到点运动运动时间短、定位要求高jerk冲击带来的残余振动会直接影响到位精度和稳定时间。第二类是医疗机器人和协作机器人这类设备直接和人体或精密元件接触轨迹的柔和度关乎安全和体验。第三类是云台、相机、天线等需要平滑扫描的设备jerk连续能有效避免画面抖动和电机噪音。计算成本方面完全不用担心。七次多项式只需求解一个8×8线性方程组在MATLAB里也就是微秒级的事实时控制完全没有压力。很多人一听七次就觉得很复杂其实它的计算量和三次相比只是多了一个矩阵求逆的过程而且这个矩阵是固定的可以预先算好。2. 七次多项式轨迹规划的核心原理2.1 8个边界条件怎么理解和给定七次多项式的一般形式是q(t) a0 a1·t a2·t² a3·t³ a4·t⁴ a5·t⁵ a6·t⁶ a7·t⁷它的8个系数需要8个方程来解边界条件的标准取法如下时刻边界条件物理含义t0q(0) q0起始位置t0q(0) v0起始速度t0q(0) a0起始加速度t0q(0) j0起始加加速度tTq(T) qf终止位置tTq(T) vf终止速度tTq(T) af终止加速度tTq(T) jf终止加加速度在关节空间做点到点运动规划时最常见的取法是 v0vf0、a0af0、j0jf0也就是机械臂从静止启动、到静止停止并且启动和停止过程没有任何冲击。位置 q0 和 qf 由路径点给出。如果是在多段轨迹拼接中起止速度、加速度、jerk就要由上一段轨迹的末状态来传递了。2.2 系数求解的矩阵表达把8个边界条件分别代入 q(t) 及其导数得到的是关于 a0 到 a7 的8个线性方程。写成矩阵形式就是 A·c b其中 c [a0; a1; a2; a3; a4; a5; a6; a7]。矩阵A的第一到第四行对应t0时刻的位置、速度、加速度、jerk约束第五到第八行对应tT时刻的同类约束。具体来说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 T T² T³ T⁴ T⁵ T⁶ T⁷; 0 1 2T 3T² 4T³ 5T⁴ 6T⁵ 7T⁶; 0 0 2 6T 12T² 20T³ 30T⁴ 42T⁵; 0 0 0 6 24T 60T² 120T³ 210T⁴];右侧向量 b [q0; v0; a0; j0; qf; vf; af; jf]。看到矩阵的规律了吗每一行其实就是对应导数多项式在t0或tT处的取值。以第7行为例它对应加速度在tT处的值二阶导数是 2a2 6a3·t 12a4·t² 20a5·t³ 30a6·t⁴ 42a7·t⁵代入tT之后系数就是 0, 0, 2, 6T, 12T², 20T³, 30T⁴, 42T⁵。只要照着这个规律构造矩阵无论多项式多少次都不会写错。在MATLAB里求解时一定不要写 inv(A)*b要用左除 A\b。左除基于LU分解和列主元法数值稳定性远好于显式求逆而且速度更快。这个习惯在矩阵条件数不理想时能救你一把。2.3 重点时间归一化与数值稳定性如果你直接按上面矩阵在MATLAB里跑多数场景下没问题但遇到大时间尺度就得小心了。轨迹总时间T5秒时矩阵里最大元素是T的7次方15625还好。可一旦T20秒T⁷1.28×10⁹矩阵第5行第8列和第1行第1列差了9个数量级。这时候矩阵条件数会变得非常差解出来的系数可能在小数点后几位就已经带上了很大的数值误差最终轨迹会在起止点附近出现肉眼可见的抖动。解决这个问题最有效的办法是时间归一化。令τt/T那么τ的取值范围是[0,1]七次多项式变成关于τ的多项式。此时矩阵里的T全部变成1数值范围被压缩到O(1)水平条件数大幅下降。需要注意的是边界条件里的速度、加速度和jerk也要一起归一化速度dq/dt (1/T)·dq/dτ所以归一化后的速度边界要乘以T 加速度d²q/dt² (1/T²)·d²q/dτ²归一化后加速度边界要乘以T² jerkd³q/dt³ (1/T³)·d³q/dτ³归一化后jerk边界要乘以T³。一句话总结用归一化时间写程序算出来的多项式系数是关于τ的最后用链式法则还原物理量。我建议就算T只有1秒也养成这个习惯因为一旦你习惯了归一化的写法边界条件的量级还会影响别的数值问题后面排查起来会干净得多。3. MATLAB手把手实现完整代码与逐段解读3.1 工程函数poly7怎么写我直接给出封装好的函数输入起止状态和时间参数输出位置、速度、加速度、jerk轨迹以及时间向量。function [q, qd, qdd, qddd, t] poly7(q0, qf, v0, vf, a0, af, j0, jf, T, dt) % 七次多项式轨迹规划函数 % 输入 % q0, qf - 起始/终止位置单位m 或 rad % v0, vf - 起始/终止速度 % a0, af - 起始/终止加速度 % j0, jf - 起始/终止加加速度 % T - 轨迹总时间s % dt - 采样周期s默认0.01 % 输出 % q, qd, qdd, qddd - 位置、速度、加速度、加加速度轨迹列向量 % t - 时间向量列向量 if nargin 10 dt 0.01; end % 时间向量 t (0:dt:T); % 边界条件向量 b [q0; v0; a0; j0; qf; vf; af; jf]; % 系数矩阵每行对应一个边界条件t0四行tT四行 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 T T^2 T^3 T^4 T^5 T^6 T^7; 0 1 2*T 3*T^2 4*T^3 5*T^4 6*T^5 7*T^6; 0 0 2 6*T 12*T^2 20*T^3 30*T^4 42*T^5; 0 0 0 6 24*T 60*T^2 120*T^3 210*T^4]; % 求解多项式系数用左除不要用inv c A \ b; % 逐项计算轨迹注意向量化要使用 .^ 点乘方 q c(1) c(2)*t c(3)*t.^2 c(4)*t.^3 c(5)*t.^4 c(6)*t.^5 c(7)*t.^6 c(8)*t.^7; qd c(2) 2*c(3)*t 3*c(4)*t.^2 4*c(5)*t.^3 5*c(6)*t.^4 6*c(7)*t.^5 7*c(8)*t.^6; qdd 2*c(3) 6*c(4)*t 12*c(5)*t.^2 20*c(6)*t.^3 30*c(7)*t.^4 42*c(8)*t.^5; qddd 6*c(4) 24*c(5)*t 60*c(6)*t.^2 120*c(7)*t.^3 210*c(8)*t.^4; end这个函数我已经在多个项目里跑过输出是列向量直接plot就能用。唯一要注意的是t.^2这种点乘方很多新手在这里忘记加点导致矩阵幂运算报错。3.2 单关节点到点示例运行一次就看懂下面这段主脚本演示一个完整的点到点运动关节角从0°转到90°总时间5秒起止速度、加速度、jerk全部为0。跑完会画出四条曲线你一眼就能看出轨迹在各个阶次上的平滑度。clear; clc; close all; % 边界条件从0°转到90° q0 0; qf 90; % 单位度 v0 0; vf 0; % 度/s a0 0; af 0; % 度/s^2 j0 0; jf 0; % 度/s^3 % 时间参数 T 5; % 总时间 5s dt 0.01; % 采样周期 10ms % 调用poly7函数 [q, qd, qdd, qddd, t] poly7(q0, qf, v0, vf, a0, af, j0, jf, T, dt); % 可视化 figure(Position, [100 100 900 650]); subplot(2,2,1); plot(t, q, b-, LineWidth, 1.8); grid on; xlabel(时间 (s)); ylabel(位置 (deg)); title(位置轨迹); xlim([0 T]); subplot(2,2,2); plot(t, qd, r-, LineWidth, 1.8); grid on; xlabel(时间 (s)); ylabel(速度 (deg/s)); title(速度轨迹); xlim([0 T]); subplot(2,2,3); plot(t, qdd, g-, LineWidth, 1.8); grid on; xlabel(时间 (s)); ylabel(加速度 (deg/s^2)); title(加速度轨迹); xlim([0 T]); subplot(2,2,4); plot(t, qddd, m-, LineWidth, 1.8); grid on; xlabel(时间 (s)); ylabel(加加速度 (deg/s^3)); title(加加速度轨迹); xlim([0 T]); sgtitle(七次多项式轨迹规划结果);跑完这段代码位置曲线是一个从0平滑上升到90的S形速度和加速度曲线都是平滑的钟形jerk曲线起止都是0、中间平滑变化。如果你换成三次多项式加速度曲线会变成一条直线且两端不是0jerk曲线直接是一条水平线对比非常明显。3.3 三次多项式对比一图看懂差距为了让你直观感受差距我补一个三次多项式对比函数。代码思路和poly7完全一样只是矩阵变成4×4。function [q3, qd3, qdd3, t3] poly3(q0, qf, v0, vf, T, dt) % 三次多项式轨迹规划对比用 % 只能在位置、速度层面连续加速度是线性函数jerk恒为常数 t3 (0:dt:T); A [1 0 0 0; 0 1 0 0; 1 T T^2 T^3; 0 1 2*T 3*T^2]; b [q0; v0; qf; vf]; c A \ b; q3 c(1) c(2)*t3 c(3)*t3.^2 c(4)*t3.^3; qd3 c(2) 2*c(3)*t3 3*c(4)*t3.^2; qdd3 2*c(3) 6*c(4)*t3; end在同一个脚本里同时调用poly7和poly3把加速度和jerk画在同一个坐标系里你会看到三次多项式的加速度在t0和tT处都是非零值本例中分别是某个正数和负数这就是加速度跳变而七次多项式的加速度从0开始、到0结束起止点完全平滑。这个对比图是我给学生讲轨迹规划时必放的图比任何文字都有说服力。4. 参数调优和工程应用技巧4.1 边界条件设置的经验法则实际工程中最常犯的错误不是代码写错而是边界条件给得不符合物理实际。第一个准则是jerk的边界值不要随便给。很多新手看教程里j00、jf0就一直用0这确实是最常见、最好用的设置。但如果你的机械臂在起点时本身就有残余运动比如上一条轨迹末速不为0那j0也要跟着给成上一条轨迹结束时刻的实际jerk不能拍脑袋填0否则拼接处会出现尖峰。判断标准很简单在MATLAB里把jerk曲线画出来如果起点或终点有明显跳变说明边界条件给错了。第二个准则是位置变化大、时间给得短必然会出现过冲。七次多项式是插值多项式它在满足边界条件的同时会在中间段出现超调。比如从0°到90°总时间只有1秒位置曲线很可能冲到95°再回落。这是数学性质决定的不是代码bug。解决办法是增大T或者改用带约束的最优化轨迹规划比如用fmincon求时间最优轨迹但那已经超出这篇文章的范围了。4.2 多路径点拼接与时间分配实际机械臂作业很少只走一个点到点更多是连续经过多个路径点比如从A到B再到C。这时通常的做法是每个路径段分别用七次多项式规划然后在拼接点保证状态连续。拼接点的处理有个关键细节第i段结束时的位置、速度、加速度、jerk要作为第i1段开始时的边界条件。如果你希望机械臂在路径点不停顿就把速度设成连续计算出来的值如果希望它在路径点准停比如点焊场景速度、加速度、jerk全部设为0。多段轨迹的另一个问题是时间分配。每段给多少时间直接决定了速度峰值和加速度峰值。我常用的方法是先按最大速度反算最小时间T_min |qf - q0| / v_max再乘一个1.2到1.5的安全系数。实际跑起来之后看速度曲线有没有超过关节限速没有就逐步压缩时间有就放宽。这个过程在MATLAB里循环一遍边界条件就能批量验证非常方便。4.3 从单关节到机械臂扩展到关节空间七次多项式单关节版本推出来后扩展到多关节机械臂特别简单。机械臂的关节空间轨迹规划本来就是“每个关节各自规划一条轨迹”然后同步执行。你只需要在MATLAB里写一个for循环对每个关节调用一次poly7函数把所有的位置、速度、加速度轨迹存到矩阵里第i行对应第i个关节第j列对应第j个采样时刻。有一点要注意机械臂末端笛卡尔空间的直线运动不能直接用关节空间的七次多项式替代因为关节空间匀速并不等于末端直线匀速。如果你的需求是末端走直线需要在笛卡尔空间插值出路径点序列再把这些路径点逆解成关节角序列最后对每个关节做七次多项式拟合或分段规划。这是两个层面的问题别混在一起。5. 常见问题与排查技巧实录5.1 轨迹过冲严重位置曲线冲过头这个问题我在新手上几乎百分百遇到。现象是位置曲线在中间段超出了起点终点范围看起来像“甩过头再回来”。先确认是不是时间太短。把T增大两倍试试如果过冲幅度明显减小那就是时间分配问题。另一个常见原因是起止速度给得太激进比如起点速度不为0但方向指向终点或者加速度边界值很大这些都会加剧过冲。我的排查顺序是先画位置曲线和速度曲线速度峰值出现在中间还是两端如果速度峰值出现在起止点附近那一定是边界条件给得不合理。如果时间不能改、边界条件必须保留那七次多项式就力不从心了。这时候可以考虑用五次B样条或者带最优化的轨迹规划它们能引入不等式约束把位置限制在安全范围内。5.2 数值精度问题轨迹在起点终点附近抖动如果你把T设得很大比如30秒以上而且没有做时间归一化那你解出来的系数可能已经“坏了”。这种问题从曲线上看是起点或终点附近有微小的高频抖动幅度可能只有0.01但在高精度定位场景下已经不可接受。解决办法就是把矩阵里的T全部换成归一化时间τ边界条件按上一节说的乘上T的幂次。归一化之后矩阵条件数从可能超过10^6降到100左右数值误差会消失。另一个防坑建议是永远用A\b而不是inv(A)*b这个问题在我自己的代码里也犯过改成左除之后精度立竿见影。5.3 选型对比什么时候该用七次多项式我整理了一个简单的对比表方便你在项目里快速决策规划方式连续阶次计算量适用场景三次多项式位置、速度最低低速、对冲击不敏感的场景五次多项式位置、速度、加速度低一般工业搬运要求不高七次多项式位置、速度、加速度、jerk低高速高精度、医疗、云台、协作机器人B样条/最优化可控阶次约束中高复杂约束、避障、时间最优一句话如果你的控制器算力不强但工况简单三次足够如果是要求高平滑度的现代机器人应用七次是性价比很高的选择。B样条当然也能做到jerk连续但实现复杂度高不少对大多数场景来说有点杀鸡用牛刀。我在实际项目中用七次多项式规划之后最明显的变化是机械臂在高速搬运时的末端抖动明显减小到位后的稳定时间从原来的几百毫秒缩短到几十毫秒。这个改进只靠改一行规划函数就拿到了远比在后端调PID参数来得效率高。最后再分享一个小技巧你可以把poly7保存成一个独立的.m文件然后在Simulink的MATLAB Function模块里调用它做轨迹发生器这样仿真和实机用的是同一套代码一致性会好很多。