企业官网建设流程全解析

做机械臂轨迹规划，十个人里有八个人第一节课交上来的作业都是三次多项式。课本上确实讲得多，给定起止位置和起止速度，四个约束解四个系数，一条三次曲线就出来了，看起来又简洁又合理。但如果你把这条三次轨迹放到真机上跑一圈，尤其是让关节从静止突然启动、再在终点突然刹停，你会听到基座那边传来若有若无的"咯噔"声，高速搬运场景里这种冲击会被放大得非常明显。

这篇文章我想把背后的原因讲透，再给出用MATLAB实现五次多项式轨迹规划的完整方案，包括全部代码、绘图对比、多路径点扩展，以及我在实际调机中踩过的坑。不管你是准备做机械臂课程设计，还是要给真实设备写轨迹生成模块，这套内容都适用。先说结论：三次多项式到五次多项式的升级，本质上是把加速度从"跳变"变成"连续"，这一个改动就能解决大部分起停冲击问题。

1. 三次多项式的"暗伤"：加速度突变才是真问题

1.1 三次多项式到底做对了什么

三次多项式轨迹规划是机器人学里最经典的入门内容，公式简单，求解只需要解一个4×4线性方程组，位置曲线平滑，速度曲线连续，这些优点让它在教科书和课程设计里出现频率极高。给定起止位置θ₀、θf和起止速度v₀、vf，假设T为总运动时间，三次多项式通式可以写成：

θ(t)=a₀+a₁t+a₂t²+a₃t³

代入四个边界条件，就能解出a₀到a₃。以最常见的零起止速度为例，解出来之后轨迹的形式非常漂亮：

θ(t)=θ₀+3(θf-θ₀)/T²·t²-2(θf-θ₀)/T³·t³

它在t=0和t=T时刻的位置、速度都精确满足边界条件，而且整个运动过程速度没有突变。从数学上看，这个轨迹没什么毛病。

但在真机上调过轨迹的人，通常会对三次多项式有另一个印象：机器人每到一段轨迹的起点和终点，关节都会传来轻微的冲击感。如果你这时候把手搭在机械臂基座上，能明显感觉到一下一下的"点头"振动。振动幅度可能不大，但对减速机和末端精度的影响是实打实的，尤其是节拍快的上下料、搬运场景，长时间跑下来机构间隙会越变越大。

问题出在哪里？就出在加速度上。

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²。

关键就在这两个点：t=0瞬间，加速度从0直接跳到1.57 rad/s²；t=2瞬间，加速度从-1.57 rad/s²直接跳回0。驱动关节的力矩和加速度成正比，加速度阶跃意味着力矩阶跃，这就是真机上"咯噔"一响的来源，也是机构振动的主要激励。

提示：加速度在起止点跳变，等价于切换点出现理论上的无穷大加加速度（jerk）。伺服电机和减速机的柔性会把它摊平一部分，但长期跑下来，齿隙扩大、结构件疲劳都会加速。

1.3 急动度：衡量轨迹柔顺性的关键指标

加加速度，也就是Jerk，是加速度的导数，物理上代表加速度的变化率。三次多项式内部jerk是常数-12Δθ/T³，更准确地说，在起止点它从0突然进入这个常数，再在终点突然回到0，本质上是一个"加上又去掉"的阶跃信号。

阶跃型的jerk对机构很不友好。打个比方，你开车从静止起步，如果一脚油门踩到底再一脚刹车踩到底，车上的人会猛点头；如果你先缓踩油门、再逐渐收油、缓慢刹停，整个过程就舒服得多。三次多项式相当于前者，五次多项式相当于后者。

想要消除加速度跳变，就得在边界条件里把起止加速度也约束住。这就是五次多项式登场的原因。

2. 五次多项式的数学内核：六个约束确定六个系数

2.1 从4个约束扩展到6个约束

五次多项式的一般形式是：

θ(t)=a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵

有6个未知系数，就需要6个方程。三次多项式只用了位置和速度的4个约束，五次多项式在此基础上把起止加速度也各加了一个，变成2个位置、2个速度、2个加速度，一共6个。对比起来很直观：

这6个约束写成方程组就是：

θ(0)=θ₀，θ'(0)=v₀，θ''(0)=a₀ θ(T)=θf，θ'(T)=vf，θ''(T)=af

把通式代入，整理成矩阵形式Ax=b。注意A矩阵的写法，前三行对应t=0，后三行对应t=T：

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=[θ₀; v₀; a₀; θf; vf; af]，解a=A\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） % 边界条件矩阵：前三行是t=0，后三行是t=T 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×4：

function [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; % 终点关节角，单位rad（60度） 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在轨迹内部是连续变化的抛物线，不存在冲激。换句话说，五次多项式把"瞬间冲击"换成了"有限且连续的斜率变化"，从机构耐受角度看是质变。

把关键数值汇总成一张表，方便对比：

提示：五次多项式消除了加速度跳变，但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(i+1) - 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(k+1), ... vpts(k), vpts(k+1), ... apts(k), apts(k+1), ... 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 end

6.3 采样周期与控制周期必须对齐

规划出来的dt一定要和控制器下发指令的周期一致。比如控制器是1kHz，dt就取0.001，规划数组长度是T/dt+1，直接按固定步长索引下发即可。如果机器人在线运动时还需要实时响应，可以只保存6个系数，在实时循环里按下发时刻t代入多项式计算，这样内存占用几乎为零。这个做法的好处是：不需要预先缓存几千个点，只要每次把当前时刻推进dt，随时都能算出来目标位置。

6.4 关节空间规划的"最后一公里"问题

五次多项式轨迹是在关节空间做的，它保证的是每个关节单独平滑，但不保证机械臂末端在笛卡尔空间走直线。如果你的工艺要求末端走直线，比如涂胶、焊接、切割，必须在笛卡尔空间做直线插补，再通过逆运动学换回关节角，然后再考虑是否用五次多项式对逆解结果做平滑。这个关系很多人第一次做项目时会搞混，提前确认到底是关节平滑优先，还是末端轨迹精确优先，能省下大量返工时间。

6.5 什么时候值得上七次多项式

如果轨迹要对接前后两个运动段，而且前一段结束时的jerk并没有归零，五次多项式在拼接点仍会出现jerk跳变。比如从直线段进入圆弧段，速度方向突变导致加速度方向突变，这时候光靠五次约束起止加速度就不够了。七次多项式可以额外约束端点jerk，把这种跳变也消掉。七次方程组规模只大了2个未知数，解法跟我给的矩阵方式完全一致，在A矩阵后面再加两行约束就行。

不过从我个人的经验看，大部分实际工况并不需要直接上七次。先调整轨迹时间、改路径点位置、加一段过渡停留，往往能消除大半问题。七次是最后的手段，不是默认选项——它会让系数对边界条件更敏感，调试起来也更费劲。

就我自己调机的体会来说，把三次换成五次之后，机械臂起停瞬间的异响和抖动会明显减轻，但别指望它解决所有振动问题。轨迹规划只是运动控制链路的一环，伺服增益、机构共振频率、减速机背隙都可能让理论曲线和实际表现差出一截。建议你按本文的脚本把对比图跑一遍，再找台真机听一听起停时的声音，对"平滑"这两个字的理解会比只看曲线深刻得多。