做机械臂轨迹规划十个人里有八个人第一节课交上来的作业都是三次多项式。课本上确实讲得多给定起止位置和起止速度四个约束解四个系数一条三次曲线就出来了看起来又简洁又合理。但如果你把这条三次轨迹放到真机上跑一圈尤其是让关节从静止突然启动、再在终点突然刹停你会听到基座那边传来若有若无的咯噔声高速搬运场景里这种冲击会被放大得非常明显。这篇文章我想把背后的原因讲透再给出用MATLAB实现五次多项式轨迹规划的完整方案包括全部代码、绘图对比、多路径点扩展以及我在实际调机中踩过的坑。不管你是准备做机械臂课程设计还是要给真实设备写轨迹生成模块这套内容都适用。先说结论三次多项式到五次多项式的升级本质上是把加速度从跳变变成连续这一个改动就能解决大部分起停冲击问题。1. 三次多项式的暗伤加速度突变才是真问题1.1 三次多项式到底做对了什么三次多项式轨迹规划是机器人学里最经典的入门内容公式简单求解只需要解一个4×4线性方程组位置曲线平滑速度曲线连续这些优点让它在教科书和课程设计里出现频率极高。给定起止位置θ₀、θf和起止速度v₀、vf假设T为总运动时间三次多项式通式可以写成θ(t)a₀a₁ta₂t²a₃t³代入四个边界条件就能解出a₀到a₃。以最常见的零起止速度为例解出来之后轨迹的形式非常漂亮θ(t)θ₀3(θf-θ₀)/T²·t²-2(θf-θ₀)/T³·t³它在t0和tT时刻的位置、速度都精确满足边界条件而且整个运动过程速度没有突变。从数学上看这个轨迹没什么毛病。但在真机上调过轨迹的人通常会对三次多项式有另一个印象机器人每到一段轨迹的起点和终点关节都会传来轻微的冲击感。如果你这时候把手搭在机械臂基座上能明显感觉到一下一下的点头振动。振动幅度可能不大但对减速机和末端精度的影响是实打实的尤其是节拍快的上下料、搬运场景长时间跑下来机构间隙会越变越大。问题出在哪里就出在加速度上。1.2 起止加速度跳变的数值有多大三次多项式只约束了位置和速度没有约束加速度。把上面的轨迹求两阶导θ(t)6(θf-θ₀)/T²-12(θf-θ₀)/T³·t可以看到起点加速度等于6Δθ/T²终点加速度等于-6Δθ/T²都不是0。这个数值有多大我拿一个很常见的工况算一下让关节从0运动到π/3也就是60°总时间2秒。代入进去起点加速度是π/2约等于1.57 rad/s²终点加速度是-1.57 rad/s²。关键就在这两个点t0瞬间加速度从0直接跳到1.57 rad/s²t2瞬间加速度从-1.57 rad/s²直接跳回0。驱动关节的力矩和加速度成正比加速度阶跃意味着力矩阶跃这就是真机上咯噔一响的来源也是机构振动的主要激励。提示加速度在起止点跳变等价于切换点出现理论上的无穷大加加速度jerk。伺服电机和减速机的柔性会把它摊平一部分但长期跑下来齿隙扩大、结构件疲劳都会加速。1.3 急动度衡量轨迹柔顺性的关键指标加加速度也就是Jerk是加速度的导数物理上代表加速度的变化率。三次多项式内部jerk是常数-12Δθ/T³更准确地说在起止点它从0突然进入这个常数再在终点突然回到0本质上是一个加上又去掉的阶跃信号。阶跃型的jerk对机构很不友好。打个比方你开车从静止起步如果一脚油门踩到底再一脚刹车踩到底车上的人会猛点头如果你先缓踩油门、再逐渐收油、缓慢刹停整个过程就舒服得多。三次多项式相当于前者五次多项式相当于后者。想要消除加速度跳变就得在边界条件里把起止加速度也约束住。这就是五次多项式登场的原因。2. 五次多项式的数学内核六个约束确定六个系数2.1 从4个约束扩展到6个约束五次多项式的一般形式是θ(t)a₀a₁ta₂t²a₃t³a₄t⁴a₅t⁵有6个未知系数就需要6个方程。三次多项式只用了位置和速度的4个约束五次多项式在此基础上把起止加速度也各加了一个变成2个位置、2个速度、2个加速度一共6个。对比起来很直观约束条件三次多项式五次多项式起止位置22起止速度22起止加速度02未知系数46这6个约束写成方程组就是θ(0)θ₀θ(0)v₀θ(0)a₀ θ(T)θfθ(T)vfθ(T)af把通式代入整理成矩阵形式Axb。注意A矩阵的写法前三行对应t0后三行对应tTA [1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0; 1, T, T^2, T^3, T^4, T^5; 0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4; 0, 0, 2, 6*T, 12*T^2, 20*T^3]右端向量是b[θ₀; v₀; a₀; θf; vf; af]解aA\b就是六个系数。这个矩阵每行都很规整位置、速度、加速度依次排列以后想加七次、八次也只是往上扩展行的关系。2.2 时间归一化与闭式解直接解6×6矩阵没有问题但如果你做的是实时规划或者运动时间T特别长最好先把时间归一化。令τt/T把原规划映射到[0,1]区间好处是T的高次幂不会进入系数矩阵条件数稳定得多。实际代码里我会直接用原始时间矩阵求解因为这个例子规模小对精度影响不大但理解归一化对后面做实时规划有帮助尤其是T达到几十秒甚至更长的场景。在工程里最常用的一个场景是起止速度都为0起止加速度也都为0。这种条件下闭式解非常漂亮θ(τ)θ₀(θf-θ₀)(10τ³-15τ⁴6τ⁵)括号里这一项10τ³-15τ⁴6τ⁵就是经典的最小加加速度轨迹minimum-jerk trajectory它是在[0,1]上使jerk平方积分最小的五阶多项式解。τ从0到1时这一项单调地从0升到1两端速度、加速度都为0是一条标准的S形曲线这也是为什么很多论文里说五次多项式轨迹天然带柔顺性。如果起止速度和加速度不为0完整闭式解的系数公式也可以推只是又长又容易抄错。我更推荐直接用矩阵求解万一边界条件变了改矩阵的某一两行比改公式安全得多。这也是后文代码采用矩阵求解的原因。2.3 为什么是五次不是七次或九次有人会问既然五次能消除加速度跳变那七次、九次是不是更好当然可以更高阶可以额外约束jerk端点值甚至jerk的导数但工程代价也随之上升。每高一阶轨迹的振荡倾向和系数灵敏度都会增加对边界条件的估计误差也更敏感而且你未必知道该给高阶的边界条件定多少合适。五次多项式在约束数量和使用成本之间最平衡这就是它在工业机器人上应用最广的原因。七次多项式也不是没有用我后面会在实际部署中的坑里专门说什么样的场景才值得上七次。3. MATLAB完整实现函数封装、主程序与可视化3.1 轨迹规划函数traj_quintic.m完整代码如下直接保存成traj_quintic.m就能用function [t, q, qd, qdd, qddd] traj_quintic(q0, qf, v0, vf, a0, af, T, dt) % 五次多项式轨迹规划 % 输入 % q0, qf : 起止位置 % v0, vf : 起止速度 % a0, af : 起止加速度 % T : 轨迹总时长单位秒 % dt : 采样周期单位秒 % 输出 % t : 时间序列 % q : 位置 % qd : 速度 % qdd : 加速度 % qddd : 加加速度Jerk % 边界条件矩阵前三行是t0后三行是tT A [1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0; 1, T, T^2, T^3, T^4, T^5; 0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4; 0, 0, 2, 6*T, 12*T^2, 20*T^3]; b [q0; v0; a0; qf; vf; af]; % 解线性方程 Ax b得到多项式系数 a A \ b; % 生成时间序列 t (0:dt:T); % 计算位置、速度、加速度、加加速度 q a(1) a(2)*t a(3)*t.^2 a(4)*t.^3 a(5)*t.^4 a(6)*t.^5; qd a(2) 2*a(3)*t 3*a(4)*t.^2 4*a(5)*t.^3 5*a(6)*t.^4; qdd 2*a(3) 6*a(4)*t 12*a(5)*t.^2 20*a(6)*t.^3; qddd 6*a(4) 24*a(5)*t 60*a(6)*t.^2; end这里重点解释一下A\b这个操作。很多教材会写成inv(A)*b但MATLAB官方和实际工程经验都更推荐用反斜杠运算符它内部会根据矩阵结构选择合适解法数值稳定性更好。对6×6矩阵来说两者差异不大但这是个好习惯以后扩展到更大规模时收益明显。3.2 对照组traj_cubic.m为了直观对比我把三次多项式也封装成一个函数。注意它没有加速度约束A矩阵只有4×4function [t, q, qd, qdd, qddd] traj_cubic(q0, qf, v0, vf, T, dt) % 三次多项式轨迹规划对照组 % 输入 % q0, qf : 起止位置 % v0, vf : 起止速度 % T : 轨迹总时长单位秒 % dt : 采样周期单位秒 % 输出 % t : 时间序列 % q : 位置 % qd : 速度 % qdd : 加速度 % qddd : 加加速度Jerk 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]; a A \ b; t (0:dt:T); q a(1) a(2)*t a(3)*t.^2 a(4)*t.^3; qd a(2) 2*a(3)*t 3*a(4)*t.^2; qdd 2*a(3) 6*a(4)*t; qddd 6*a(4) * ones(size(t)); % 三次多项式的jerk是常数 end三次多项式的三阶导数是常数6a₄所以它的jerk是一条水平直线这也是它起止点jerk阶跃的来源。3.3 主程序一键对比出图把两个函数都准备好之后主程序很简单。我用2秒时间规划关节从0转到60°起止速度、加速度都为0画四张子图分别对比位置、速度、加速度、jerk%% 五次多项式 vs 三次多项式 轨迹规划对比 clc; clear; close all; % 轨迹边界条件 q0 0; % 起始关节角单位rad qf pi/3; % 终点关节角单位rad60度 v0 0; vf 0; % 起止速度设为0 a0 0; af 0; % 起止加速度设为0 T 2; % 总运动时间单位s dt 0.01; % 采样周期单位s % 五次多项式规划 [t5, q5, qd5, qdd5, qddd5] traj_quintic(q0, qf, v0, vf, a0, af, T, dt); % 三次多项式规划对照组 [t3, q3, qd3, qdd3, qddd3] traj_cubic(q0, qf, v0, vf, T, dt); % 可视化 figure(Color,w,Position,[80 80 900 900]); subplot(4,1,1); plot(t3, q3, --, LineWidth, 1.5); hold on; plot(t5, q5, -, LineWidth, 1.5); ylabel(位置 (rad)); legend(三次多项式,五次多项式, Location,best); title(五次 vs 三次 轨迹规划对比); grid on; subplot(4,1,2); plot(t3, qd3, --, LineWidth, 1.5); hold on; plot(t5, qd5, -, LineWidth, 1.5); ylabel(速度 (rad/s)); grid on; subplot(4,1,3); plot(t3, qdd3, --, LineWidth, 1.5); hold on; plot(t5, qdd5, -, LineWidth, 1.5); ylabel(加速度 (rad/s^2)); grid on; subplot(4,1,4); plot(t3, qddd3, --, LineWidth, 1.5); hold on; plot(t5, qddd5, -, LineWidth, 1.5); ylabel(Jerk (rad/s^3)); xlabel(时间 (s)); grid on; % 输出关键数值 fprintf(三次多项式: 峰值速度 %.3f rad/s, 峰值加速度 %.3f rad/s^2, jerk常数 %.3f rad/s^3\n, ... max(abs(qd3)), max(abs(qdd3)), qddd3(1)); fprintf(五次多项式: 峰值速度 %.3f rad/s, 峰值加速度 %.3f rad/s^2, 峰值jerk %.3f rad/s^3\n, ... max(abs(qd5)), max(abs(qdd5)), max(abs(qddd5)));画图的小技巧是最后只在最底下的子图加xlabel上面三张子图只保留ylabel这样图面干净不会被重复时间轴标签刷屏。四个子图叠在一起位置、速度、加速度、jerk四个物理量的差异一眼就能看全。3.4 关于运行环境这段代码不依赖任何工具箱矩阵求解、subplot、plot这些全是MATLAB基础功能R2016b之后的版本都能直接跑。不需要额外安装Robotics System Toolbox也不建议为了一个轨迹规划去装大工具箱自己写五十行代码反而更清楚出了问题也知道去哪里查。如果电脑上还没装MATLAB或者刚装好不熟悉界面建议先跑一遍最简单的plot(rand(1,10))确认环境正常再跑上面的主程序。轨迹规划本身对版本和硬件要求很低旧一点的版本只要能运行基础矩阵运算代码都可以兼容。4. 跑出结果后的解读五次和三次到底差在哪4.1 位置和速度的差异比想象中小第一次跑完对比很多人会愣一下两条位置曲线几乎重合速度曲线也都是拱形好像差别不大。这不是代码写错了而是二者形状本来就接近。速度上的差别主要在中段五次的最大速度比三次大约25%。我这个例子里2秒走60°五次的峰值速度约0.98 rad/s三次约0.79 rad/s。如果你的应用受驱动器最大速度约束比如电机额定转速就摆在那里选型时要把这个余量留出来。别指望换了五次多项式之后峰值速度会自动变小——它反而变大了因为轨迹为了照顾加速度连续性在时间轴上把运动摊得更集中了一些。4.2 加速度一个突变一个平滑归零加速度的差异就非常明显了。三次的加速度是从1.57 rad/s²线性降到-1.57 rad/s²在起点和终点各有一个阶跃五次的加速度从0开始按抛物线形状上升到约1.51 rad/s²再平滑回到0全程连续。这里有个反直觉的点五次的峰值加速度并没有比三次小多少本例三次峰值1.57五次峰值1.51几乎一样。五次的真正优势不在峰值更小而在起止连续。对机构来说加速度连续意味着惯性力不会瞬间切换冲击集中在更高阶的jerk层而不是加速度层这对减速机柔性和末端定位精度都更友好。4.3 Jerk的形态变了代价也变了看jerk对比图时会发现五次的jerk在端点大约是7.85 rad/s³反而比三次内部的常数1.57 rad/s³大不少。这怎么解释难道五次更差不是。三次的jerk问题在于它在起点和终点出现0→常数的阶跃以及理论上的冲激这个冲激才是损伤来源五次虽然起点jerk值不为0但它的冲击是有限值的突变jerk在轨迹内部是连续变化的抛物线不存在冲激。换句话说五次多项式把瞬间冲击换成了有限且连续的斜率变化从机构耐受角度看是质变。把关键数值汇总成一张表方便对比指标三次多项式五次多项式最大速度 (rad/s)0.7850.982峰值加速度 (rad/s²)1.571出现在端点1.512出现在τ≈0.211与0.789起止点加速度±1.571跳变0连续Jerk特征恒定-1.571起止点冲激连续抛物线端点约7.854提示五次多项式消除了加速度跳变但jerk的端点值通常不为0。如果你的工艺对jerk也有硬约束比如高精密对准平台对振动极敏感那就需要考虑七次多项式或梯形jerk规划后面第6节会展开说。5. 多路径点的分段五次规划5.1 最简单做法为什么不好机器人在实际任务里通常要依次经过好几个关节角比如从A到B再到C再到D。很多人会把每一段当成独立的五次轨迹去规划但这样做有个前提B、C两点需要给出速度和加速度边界。图省事的做法是把中间点速度全部设为0结果就是机器人走一段停一下、再走一段节拍被拖慢反复加减速对电机也不友好。5.2 中间点速度怎么给才合理工程上常用的一种做法是用相邻两段的平均速度来估计中间点速度。比如中间点θᵢ的速度估计为vᵢ ≈ [(θᵢ₊₁-θᵢ)/Tᵢ (θᵢ-θᵢ₋₁)/Tᵢ₋₁] / 2这个值相当于把匀速穿过中间点的速度做了平滑再乘一个0.8到1.2的经验系数用来调整轨迹的激进程度。中间点的加速度通常设为0这样左右两段在中间点的加速度都为0能自然衔接。5.3 分段规划的MATLAB示例下面这个脚本演示了3段时间依次经过4个路径点的完整过程直接把结果拼接到一个时间轴上%% 多段分段五次多项式规划示例 clear; clc; close all; qpts [0, 30, 90, 60] * pi/180; % 依次经过的关节角单位rad Tseg [1.2, 1.5, 1.3]; % 每段时间单位s dt 0.01; % 估计中间点速度两端速度为0 vpts zeros(size(qpts)); for i 2 : length(qpts)-1 v_pre (qpts(i) - qpts(i-1)) / Tseg(i-1); v_next (qpts(i1) - qpts(i)) / Tseg(i); vpts(i) 0.85 * (v_pre v_next) / 2; % 经验系数0.85 end % 中间点加速度统一设为0保证衔接连续 apts zeros(size(qpts)); t_all []; q_all []; qd_all []; qdd_all []; qddd_all []; t_start 0; for k 1 : length(Tseg) [t, q, qd, qdd, qddd] traj_quintic(... qpts(k), qpts(k1), ... vpts(k), vpts(k1), ... apts(k), apts(k1), ... Tseg(k), dt); % 去掉每段最后一个点避免拼接点重复 if k length(Tseg) keep 1 : length(t)-1; else keep 1 : length(t); end t_all [t_all, t_start t(keep)]; q_all [q_all, q(keep)]; qd_all [qd_all, qd(keep)]; qdd_all [qdd_all, qdd(keep)]; qddd_all [qddd_all, qddd(keep)]; t_start t_start Tseg(k); end figure(Color,w,Position,[80 80 900 700]); subplot(3,1,1); plot(t_all, q_all, LineWidth, 1.5); grid on; ylabel(位置 (rad)); title(多段五次多项式轨迹); subplot(3,1,2); plot(t_all, qd_all, LineWidth, 1.5); grid on; ylabel(速度 (rad/s)); subplot(3,1,3); plot(t_all, qdd_all, LineWidth, 1.5); grid on; ylabel(加速度 (rad/s^2)); xlabel(时间 (s));拼接结果里中间点位置、速度都是连续的加速度也都归零所以整条轨迹没有突兀的加速跳变。需要提醒的是jerk在中间点仍然可能有突变因为左右两段的加速度虽然同为0但加速度曲线的斜率不一定相同。想进一步消除jerk突变就要把全局所有段的系数联立求解让中间点加速度作为未知变量一起优化这个思路展开可以单独写一篇这里先不展开。6. 实际部署中的坑与经验6.1 别用符号计算做在线规划我见过不少同学喜欢用syms定义θ然后调用solve去解系数。符号计算在课堂演示时没问题但放在仿真循环里会越来越慢因为每步都要重建符号表达式并化简CPU会被拖垮。矩阵求解A\b一次也就是微秒级在线规划一定要用纯数值解法。6.2 轨迹时间T的反算与迭代调整五次曲线峰值速度和峰值加速度都跟T直接相关。工程上一般先根据驱动器允许的最大速度和最大加速度反算一个最小T粗略估计T ≥ max( Δθ/v_max, √(Δθ/a_max) )这个公式只是起点因为实际峰值系数不完全等同于匀速估计。更稳妥的做法是写一个循环先用初值T规划一条五次轨迹检查峰值速度、峰值加速度是否超限如果超限就把T乘1.1再算一遍直到满足约束。计算量很小但能保证轨迹一定可执行。我贴一个带自动迭代的版本函数内部直接帮你处理function [t, q, qd, qdd, qddd] traj_quintic_auto(... q0, qf, v0, vf, a0, af, vmax, amax, dt) % 自动迭代调整运动时间直到满足速度与加速度约束 % 注意vmax、amax必须是正数 T max(1e-3, abs(qf - q0) / vmax * 1.5); % 初始估计 while true [t, q, qd, qdd, qddd] traj_quintic(q0, qf, v0, vf, a0, af, T, dt); if max(abs(qd)) vmax max(abs(qdd)) amax break; end T T * 1.1; if T 1e6 error(约束过于严格自动迭代无法找到合适时间); end end end6.3 采样周期与控制周期必须对齐规划出来的dt一定要和控制器下发指令的周期一致。比如控制器是1kHzdt就取0.001规划数组长度是T/dt1直接按固定步长索引下发即可。如果机器人在线运动时还需要实时响应可以只保存6个系数在实时循环里按下发时刻t代入多项式计算这样内存占用几乎为零。这个做法的好处是不需要预先缓存几千个点只要每次把当前时刻推进dt随时都能算出来目标位置。6.4 关节空间规划的最后一公里问题五次多项式轨迹是在关节空间做的它保证的是每个关节单独平滑但不保证机械臂末端在笛卡尔空间走直线。如果你的工艺要求末端走直线比如涂胶、焊接、切割必须在笛卡尔空间做直线插补再通过逆运动学换回关节角然后再考虑是否用五次多项式对逆解结果做平滑。这个关系很多人第一次做项目时会搞混提前确认到底是关节平滑优先还是末端轨迹精确优先能省下大量返工时间。6.5 什么时候值得上七次多项式如果轨迹要对接前后两个运动段而且前一段结束时的jerk并没有归零五次多项式在拼接点仍会出现jerk跳变。比如从直线段进入圆弧段速度方向突变导致加速度方向突变这时候光靠五次约束起止加速度就不够了。七次多项式可以额外约束端点jerk把这种跳变也消掉。七次方程组规模只大了2个未知数解法跟我给的矩阵方式完全一致在A矩阵后面再加两行约束就行。不过从我个人的经验看大部分实际工况并不需要直接上七次。先调整轨迹时间、改路径点位置、加一段过渡停留往往能消除大半问题。七次是最后的手段不是默认选项——它会让系数对边界条件更敏感调试起来也更费劲。就我自己调机的体会来说把三次换成五次之后机械臂起停瞬间的异响和抖动会明显减轻但别指望它解决所有振动问题。轨迹规划只是运动控制链路的一环伺服增益、机构共振频率、减速机背隙都可能让理论曲线和实际表现差出一截。建议你按本文的脚本把对比图跑一遍再找台真机听一听起停时的声音对平滑这两个字的理解会比只看曲线深刻得多。