简介在机械臂控制与仿真中五次多项式常用于生成平滑无突变的运动轨迹其六个自由度可同时约束位置、速度与加速度适合高精度关节路径规划。这份MATLAB资源面向机器人、自动化及机电专业学生与工程仿真人员帮助理解五次多项式原理并快速上手机械臂三维轨迹生成与动态仿真实现。资源为RAR压缩包共2个文件以MATLAB的.m脚本为核心对应多项式系数计算、轨迹生成与仿真示例另含辅助RAR压缩资料整体大小仅2KB轻量易用适合作为入门练习或课堂演示代码。已有约900人学习浏览适合需要快速验证五次多项式轨迹规划效果或进行机械臂运动学仿真练习的中初级学习者。通过对应源码读者可直观看到五次多项式位置、速度、加速度之间的导数关系以及如何利用MATLAB完成多项式拟合、系数求解和运动数据输出便于进一步修改参数并应用到关节插补、工业机械臂仿真等实际场景。1. 机械臂轨迹规划里的五次多项式为什么三次不够机械臂轨迹规划里五次多项式quintic polynomial几乎是入门与落地之间最常见的分水岭而 MATLAB 仿真是检验它最顺手的工具。给机械臂关节规划一段从 0 rad 到 pi/2 rad 的运动最直接的三次多项式能保证位置和速度连续但加速度在起点和终点会突然跳变体现在实体机上就是抖动和传动冲击。五次多项式多出两个自由度和两个边界条件把加速度也约束成连续量因此是关节空间轨迹规划的默认选择。很多机械臂仿真的课程作业和实际项目最终都会落到一份 MATLAB 源码上用五次多项式生成关节角度、速度和加速度曲线再送去运动学或动力学模型验证。这篇就从源码拆起把系数怎么求、代码怎么写、仿真怎么验、超限怎么救讲透。2. 五次多项式的约束方程与矩阵求解方法2.1 为什么是六个自由度和六个边界条件五次多项式的标准形式是 P(t)a0a1ta2t^2a3t^3a4t^4a5t^5六个待定系数决定了它刚好能塞进六个独立约束。对轨迹规划来说这六个约束天然来自路径两端的状态起点和终点的位置、速度、加速度。位置约束保证机械臂确实从指定点走到目标点速度约束让关节在启停瞬间与前后轨迹接续加速度约束则进一步抑制冲击。三次多项式只有四个自由度能约束位置和速度但加速度不可控必然在两个端点出现阶跃。这里的本质区别不是“曲线更弯”而是边界约束从 4 个变成了 6 个求解从唯一解变成了另一个唯一解代价只是多解一个三阶线性方程组。2.2 把边界条件写进表格下表是一次典型运动的边界条件假设起始时间 t00终止时间 tT。物理量起点 t0终点 tT位置q0qf速度v0vf加速度a0af六个条件对应六行方程。常见设置是 q00、qfpi/6v0vf0a0af0也就是“静止到静止”。此时五次多项式退化为完全对称的形状但不是所有任务都必须让端点加速度为零比如机械臂需要连续经过两个路径点前一段终点速度就不为零这时边界速度应继承上一段轨迹的末端速度而不是粗暴置零。2.3 构造矩阵和右端项把 P(t) 对 t 求一阶导和二阶导v(t)a12a2t3a3t^24a4t^35a5t^4 a(t)2a26a3t12a4t^220a5t^3代入 t0 和 tT 后得到线性系统 M*cb。为了减少数值误差我习惯先把时间归一化成 taut/T这样矩阵元素都在 0 和 1 附近而不是 T 的高次幂。归一化后 b 中的速度项要乘以 T加速度项要乘以 T^2很多初学者在这一点上写错。% 归一化时间 tau t / T % 系数向量 c [a0; a1; a2; a3; a4; a5] M [1 0 0 0 0 0; % tau0 位置 0 1 0 0 0 0; % tau0 速度 0 0 2 0 0 0; % tau0 加速度 1 1 1 1 1 1; % tau1 位置 0 1 2 3 4 5; % tau1 速度 0 0 2 6 12 20];% tau1 加速度 b [q0; v0*T; a0*T^2; qf; vf*T; af*T^2]; c M \ b;这里 b 的第三行和第六行是加速度边界乘 T^2因为 d²P/dtau² T² * d²P/dt²。M 的第三行 [0 0 2 0 0 0] 对应 a2 的二阶系数 2其余为 0。如果直接用实际 t 构造矩阵T10 时矩阵里会出现 T^4、T^5 量级条件数会变得很难看归一化后 cond(M) 固定不变。用 MATLAB 的M\b解这个稠密小矩阵比用符号计算快得多也足够精确。2.4 从系数到位置、速度、加速度得到 c 之后任意 tau 处的值都是多项式求值tau 0:0.01:1; q c(1) c(2)*tau c(3)*tau.^2 c(4)*tau.^3 c(5)*tau.^4 c(6)*tau.^5; qd c(2) 2*c(3)*tau 3*c(4)*tau.^2 4*c(5)*tau.^3 5*c(6)*tau.^4; qdd 2*c(3) 6*c(4)*tau 12*c(5)*tau.^2 20*c(6)*tau.^3;注意这里 qd 和 qdd 是关于 tau 的导数回到物理时间 t 还要除以 T 和 T^2。如果直接用polyval(c(end:-1:1), tau)MATLAB 要求系数从高次到低次排列所以需要先翻转。更稳妥的方式是写一个统一封装函数见下一章。3. 用 MATLAB 实现五次多项式轨迹生成Untitled2.m 的复现思路3.1 把散装脚本封装成可复用函数拿到手的 Untitled2.m 通常是课程里一段式脚本手动填写边界条件循环画曲线。代码能跑但换一组关节角或时间就要改动多处不适合机械臂多关节批量仿真。我会先把它重构成一个quintic_traj函数输入起止边界、总时间和采样周期输出时刻序列和对应位置、速度、加速度。function [q, qd, qdd, t] quintic_traj(q0, qf, v0, vf, a0, af, T, dt) % 五次多项式轨迹生成函数 % 输入: % q0, qf : 起止位置 % v0, vf : 起止速度 % a0, af : 起止加速度 % T : 总运动时间 % dt : 采样间隔 % 输出: % q, qd, qdd : 位置、速度、加速度序列 % t : 时间序列 tau (0:dt:T) / T; M [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 1 1 1 1 1; 0 1 2 3 4 5; 0 0 2 6 12 20]; b [q0; v0*T; a0*T^2; qf; vf*T; af*T^2]; c M \ b; c_high c(end:-1:1); % polyval 需要从高次到低次 q polyval(c_high, tau); qd polyval(polyder(c_high), tau) / T; qdd polyval(polyder(polyder(c_high)), tau) / T^2; t tau * T; endpolyder返回导函数的系数向量也是高次到低次因此可以直接传给polyval。除以 T 和 T^2 对应 2.3 里讲的时间尺度换算。函数末尾返回 t 而不是 tau这样调用者不需要关心归一化细节。3.2 单关节示例从 0 到 90 度的静止到静止运动下面是一个可以直接复制到 MATLAB 运行的调用脚本对应两秒内从 0 rad 运动到 pi/2 radT 2; dt 0.01; q0 0; qf pi/2; v0 0; vf 0; a0 0; af 0; [q, qd, qdd, t] quintic_traj(q0, qf, v0, vf, a0, af, T, dt); figure; subplot(3,1,1); plot(t, q); ylabel(位置 rad); subplot(3,1,2); plot(t, qd); ylabel(速度 rad/s); subplot(3,1,3); plot(t, qdd); ylabel(加速度 rad/s^2); xlabel(时间 s);参数值说明T2 s总运动时间dt0.01 s采样周期q00 rad起点位置qfpi/2 rad终点位置v0/vf0 rad/s起止速度a0/af0 rad/s²起止加速度运行后会看到位置曲线是 S 形速度曲线是抛物线形的单峰加速度曲线是一个完整的三角波在 tT/2 附近过零。加速度曲线没有阶跃这就是五次多项式区别于三次多项式最直观的证据。3.3 求解结果的验证和 polyfit 的误区拿到系数后先用边界断言确认没有解错assert(abs(q(1) - q0) 1e-9); assert(abs(q(end) - qf) 1e-9); assert(abs(qd(1) - v0) 1e-9); assert(abs(qd(end)- vf) 1e-9); assert(abs(qdd(1) - a0) 1e-9); assert(abs(qdd(end)- af) 1e-9);这些断言能捕捉 b 向量中 T 和 T^2 乘错的问题。我见过不少人在这里用polyfit去拟合几个稀疏点生成轨迹效果是曲线经过中间点但端点速度完全不可控机械臂启动瞬间还是会有冲击。polyfit 适合数据拟合轨迹规划需要的是插值约束二者不要混用。如果中间必须经过某些路径点正确做法是分段五次多项式并在连接点处把速度、加速度设为连续值。比如前一段终点速度是 0.2 rad/s后一段起点速度就写 0.2 rad/s而不是置零加速度同理。这就是多点连续轨迹的基本写法。Untitled2.m 如果只是单段演示可以在此基础上再封装一个quintic_traj_waypoint函数按路径点循环调用把上一段的末端状态传给下一段。注意解五次多项式系数用M\b不要用inv(M)*b。\在 MATLAB 里对稠密小矩阵走 LU 分解数值更稳、更快。这一章给到的函数足够替换大部分课程代码里的核心逻辑下一步就是把轨迹喂给一个真实的三维机械臂模型。4. 机械臂三维仿真把五次多项式轨迹喂给机械臂模型4.1 用 rigidBodyTree 搭建仿真机械臂拿到关节角轨迹后下一步是把它作用到机械臂模型上做三维仿真。MATLAB 官方 Robotics System Toolbox 里推荐用rigidBodyTree而不是老旧的SerialLink。下面是一个两连杆机械臂的搭建和动画过程刚好能验证五次多项式轨迹的空间效果。robot rigidBodyTree; link1 rigidBody(link1); jnt1 rigidBodyJoint(jnt1, revolute); setFixedTransform(jnt1, trvec2tform([0 0 0]) * axang2tform([0 0 1 0])); link1.Joint jnt1; addBody(robot, link1, base); link2 rigidBody(link2); jnt2 rigidBodyJoint(jnt2, revolute); setFixedTransform(jnt2, trvec2tform([0.3 0 0]) * axang2tform([0 0 1 0])); link2.Joint jnt2; addBody(robot, link2, link1);这里trvec2tform给定连杆偏移axang2tform([0 0 1 0])表示旋转轴是 z 轴、角度为 0。两段连杆长度设成 0.3 m可以观察末端轨迹。然后给两个关节分别规划轨迹[q1, qd1, qdd1, t] quintic_traj(0, pi/3, 0, 0, 0, 0, 2, 0.02); [q2, qd2, qdd2, ~] quintic_traj(0, -pi/6, 0, 0, 0, 0, 2, 0.02); figure; for i 1:length(t) config [q1(i) q2(i)]; show(robot, config, PreservePlot, false); xlim([-0.1 0.7]); ylim([-0.3 0.4]); zlim([-0.1 0.1]); title(sprintf(t %.2f s, t(i))); drawnow; endshow的第二个参数是关节坐标行向量DataFormat 默认是 row 姿态。这个循环把两个关节角序列逐帧推进能看到机械臂末端平滑地扫过一段圆弧。如果要记录末端轨迹可以在每个循环里调用getTransform(robot, config, link2)取出位置并保存。4.2 三次与五次多项式对比问题出在加速度和 jerk为了说明为什么非用五次不可把同一个运动用三次多项式重做一遍。三次多项式只有位置和速度约束加速度在两端会突变。对比代码% 三次多项式系数 M3 [1 0 0 0; 0 1 0 0; 1 T T^2 T^3; 0 1 2*T 3*T^2]; b3 [q0; v0; qf; vf]; c3 M3 \ b3; tau3 t / T; q3 polyval(c3(end:-1:1), tau3); qd3 polyval(polyder(c3(end:-1:1)), tau3) / T; qdd3 polyval(polyder(polyder(c3(end:-1:1))), tau3) / T^2; figure; subplot(2,1,1); plot(t, qdd, t, qdd3); legend(五次,三次); ylabel(加速度); subplot(2,1,2); plot(t(1:end-1), diff(qdd)/dt, linewidth, 1.5); hold on; plot(t(1:end-1), diff(qdd3)/dt); ylabel(jerk);运行后三次多项式的加速度曲线是直线两端跳变jerk 在起点和终点是尖峰五次多项式加速度曲线是斜线jerk 连续且有限。对六轴机械臂做三维仿真时这些尖峰在逆动力学计算中直接变成力矩噪声严重时会让ode45动态仿真的步长被迫缩小仿真速度变慢。因此五次多项式不仅是“更平滑”它直接决定了后续动力学仿真能不能稳定跑完。4.3 结合优化工具箱做约束可行化多关节仿真时经常遇到某个关节速度或加速度超限。最实用的做法是把总时间 T 设为优化变量用 MATLAB 优化工具箱的fmincon最小化 T约束是每个关节的速度和加速度峰值不超过额定值。目标是求最短可行时间代码框架如下function [c, ceq] traj_limits(T, q0_set, qf_set, vel_lim, acc_lim) c []; for j 1:length(q0_set) [~, qd, qdd] quintic_traj(q0_set(j), qf_set(j), 0, 0, 0, 0, T, 0.001); c [c; max(abs(qd)) - vel_lim(j); max(abs(qdd)) - acc_lim(j)]; end ceq []; end调用时给 T 一个下限比如 0.5 sfmincon 会返回满足约束的最短时间。这个方案比手动试 T 更可复现也是课程作业从“能画图”变成“像工程方案”的关键一步。关节速度上限 rad/s加速度上限 rad/s²joint11.53.0joint22.04.0上表是机器人手册里常见的关节额定参数。把这些值写进约束函数fmincon 就能自动找到一条既不超速度又不超加速度的轨迹。5. 验证和修正轨迹的三个工程化技巧5.1 峰值约束检查生成轨迹后先检查超限而不是直接送去仿真。一个快速检查函数如下function ok check_traj_limits(qd, qdd, vel_lim, acc_lim) ok max(abs(qd)) vel_lim max(abs(qdd)) acc_lim; end传入单关节的 qd、qdd 和对应的额定值返回布尔结果。这个函数短但能在仿真前拦住最粗糙的参数错误。5.2 时间缩放救回超限轨迹当峰值超限时不要重新解方程组直接按比例放大 T。速度峰值与 T 成反比加速度峰值与 T^2 成反比所以新的总时间取两者各自缩放比例的较大者T_new T * max(max(abs(qd))/vel_lim, sqrt(max(abs(qdd))/acc_lim));注意加速度比例要开根号因为 T 放大一倍加速度峰值降到四分之一。更新 T 后重新调用quintic_traj即可。5.3 实时控制器只传系数把整条轨迹数组发给控制器会占用缓冲实时系统通常只传六个系数和运动时长。控制器每个周期读取节拍时间计算 tau再用polyval得到当前期望位置并与编码器反馈做位置环。这样quintic_traj在控制器里退化成一条语句q_des polyval(c_high, tau);这就是五次多项式工程落地的最后一步离线规划算系数在线运行只做多项式求值。本文还有配套的精品资源点击获取 机械臂轨迹规划中的五次多项式：MATLAB实现与仿真验证
简介在机械臂控制与仿真中五次多项式常用于生成平滑无突变的运动轨迹其六个自由度可同时约束位置、速度与加速度适合高精度关节路径规划。这份MATLAB资源面向机器人、自动化及机电专业学生与工程仿真人员帮助理解五次多项式原理并快速上手机械臂三维轨迹生成与动态仿真实现。资源为RAR压缩包共2个文件以MATLAB的.m脚本为核心对应多项式系数计算、轨迹生成与仿真示例另含辅助RAR压缩资料整体大小仅2KB轻量易用适合作为入门练习或课堂演示代码。已有约900人学习浏览适合需要快速验证五次多项式轨迹规划效果或进行机械臂运动学仿真练习的中初级学习者。通过对应源码读者可直观看到五次多项式位置、速度、加速度之间的导数关系以及如何利用MATLAB完成多项式拟合、系数求解和运动数据输出便于进一步修改参数并应用到关节插补、工业机械臂仿真等实际场景。1. 机械臂轨迹规划里的五次多项式为什么三次不够机械臂轨迹规划里五次多项式quintic polynomial几乎是入门与落地之间最常见的分水岭而 MATLAB 仿真是检验它最顺手的工具。给机械臂关节规划一段从 0 rad 到 pi/2 rad 的运动最直接的三次多项式能保证位置和速度连续但加速度在起点和终点会突然跳变体现在实体机上就是抖动和传动冲击。五次多项式多出两个自由度和两个边界条件把加速度也约束成连续量因此是关节空间轨迹规划的默认选择。很多机械臂仿真的课程作业和实际项目最终都会落到一份 MATLAB 源码上用五次多项式生成关节角度、速度和加速度曲线再送去运动学或动力学模型验证。这篇就从源码拆起把系数怎么求、代码怎么写、仿真怎么验、超限怎么救讲透。2. 五次多项式的约束方程与矩阵求解方法2.1 为什么是六个自由度和六个边界条件五次多项式的标准形式是 P(t)a0a1ta2t^2a3t^3a4t^4a5t^5六个待定系数决定了它刚好能塞进六个独立约束。对轨迹规划来说这六个约束天然来自路径两端的状态起点和终点的位置、速度、加速度。位置约束保证机械臂确实从指定点走到目标点速度约束让关节在启停瞬间与前后轨迹接续加速度约束则进一步抑制冲击。三次多项式只有四个自由度能约束位置和速度但加速度不可控必然在两个端点出现阶跃。这里的本质区别不是“曲线更弯”而是边界约束从 4 个变成了 6 个求解从唯一解变成了另一个唯一解代价只是多解一个三阶线性方程组。2.2 把边界条件写进表格下表是一次典型运动的边界条件假设起始时间 t00终止时间 tT。物理量起点 t0终点 tT位置q0qf速度v0vf加速度a0af六个条件对应六行方程。常见设置是 q00、qfpi/6v0vf0a0af0也就是“静止到静止”。此时五次多项式退化为完全对称的形状但不是所有任务都必须让端点加速度为零比如机械臂需要连续经过两个路径点前一段终点速度就不为零这时边界速度应继承上一段轨迹的末端速度而不是粗暴置零。2.3 构造矩阵和右端项把 P(t) 对 t 求一阶导和二阶导v(t)a12a2t3a3t^24a4t^35a5t^4 a(t)2a26a3t12a4t^220a5t^3代入 t0 和 tT 后得到线性系统 M*cb。为了减少数值误差我习惯先把时间归一化成 taut/T这样矩阵元素都在 0 和 1 附近而不是 T 的高次幂。归一化后 b 中的速度项要乘以 T加速度项要乘以 T^2很多初学者在这一点上写错。% 归一化时间 tau t / T % 系数向量 c [a0; a1; a2; a3; a4; a5] M [1 0 0 0 0 0; % tau0 位置 0 1 0 0 0 0; % tau0 速度 0 0 2 0 0 0; % tau0 加速度 1 1 1 1 1 1; % tau1 位置 0 1 2 3 4 5; % tau1 速度 0 0 2 6 12 20];% tau1 加速度 b [q0; v0*T; a0*T^2; qf; vf*T; af*T^2]; c M \ b;这里 b 的第三行和第六行是加速度边界乘 T^2因为 d²P/dtau² T² * d²P/dt²。M 的第三行 [0 0 2 0 0 0] 对应 a2 的二阶系数 2其余为 0。如果直接用实际 t 构造矩阵T10 时矩阵里会出现 T^4、T^5 量级条件数会变得很难看归一化后 cond(M) 固定不变。用 MATLAB 的M\b解这个稠密小矩阵比用符号计算快得多也足够精确。2.4 从系数到位置、速度、加速度得到 c 之后任意 tau 处的值都是多项式求值tau 0:0.01:1; q c(1) c(2)*tau c(3)*tau.^2 c(4)*tau.^3 c(5)*tau.^4 c(6)*tau.^5; qd c(2) 2*c(3)*tau 3*c(4)*tau.^2 4*c(5)*tau.^3 5*c(6)*tau.^4; qdd 2*c(3) 6*c(4)*tau 12*c(5)*tau.^2 20*c(6)*tau.^3;注意这里 qd 和 qdd 是关于 tau 的导数回到物理时间 t 还要除以 T 和 T^2。如果直接用polyval(c(end:-1:1), tau)MATLAB 要求系数从高次到低次排列所以需要先翻转。更稳妥的方式是写一个统一封装函数见下一章。3. 用 MATLAB 实现五次多项式轨迹生成Untitled2.m 的复现思路3.1 把散装脚本封装成可复用函数拿到手的 Untitled2.m 通常是课程里一段式脚本手动填写边界条件循环画曲线。代码能跑但换一组关节角或时间就要改动多处不适合机械臂多关节批量仿真。我会先把它重构成一个quintic_traj函数输入起止边界、总时间和采样周期输出时刻序列和对应位置、速度、加速度。function [q, qd, qdd, t] quintic_traj(q0, qf, v0, vf, a0, af, T, dt) % 五次多项式轨迹生成函数 % 输入: % q0, qf : 起止位置 % v0, vf : 起止速度 % a0, af : 起止加速度 % T : 总运动时间 % dt : 采样间隔 % 输出: % q, qd, qdd : 位置、速度、加速度序列 % t : 时间序列 tau (0:dt:T) / T; M [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 2 0 0 0; 1 1 1 1 1 1; 0 1 2 3 4 5; 0 0 2 6 12 20]; b [q0; v0*T; a0*T^2; qf; vf*T; af*T^2]; c M \ b; c_high c(end:-1:1); % polyval 需要从高次到低次 q polyval(c_high, tau); qd polyval(polyder(c_high), tau) / T; qdd polyval(polyder(polyder(c_high)), tau) / T^2; t tau * T; endpolyder返回导函数的系数向量也是高次到低次因此可以直接传给polyval。除以 T 和 T^2 对应 2.3 里讲的时间尺度换算。函数末尾返回 t 而不是 tau这样调用者不需要关心归一化细节。3.2 单关节示例从 0 到 90 度的静止到静止运动下面是一个可以直接复制到 MATLAB 运行的调用脚本对应两秒内从 0 rad 运动到 pi/2 radT 2; dt 0.01; q0 0; qf pi/2; v0 0; vf 0; a0 0; af 0; [q, qd, qdd, t] quintic_traj(q0, qf, v0, vf, a0, af, T, dt); figure; subplot(3,1,1); plot(t, q); ylabel(位置 rad); subplot(3,1,2); plot(t, qd); ylabel(速度 rad/s); subplot(3,1,3); plot(t, qdd); ylabel(加速度 rad/s^2); xlabel(时间 s);参数值说明T2 s总运动时间dt0.01 s采样周期q00 rad起点位置qfpi/2 rad终点位置v0/vf0 rad/s起止速度a0/af0 rad/s²起止加速度运行后会看到位置曲线是 S 形速度曲线是抛物线形的单峰加速度曲线是一个完整的三角波在 tT/2 附近过零。加速度曲线没有阶跃这就是五次多项式区别于三次多项式最直观的证据。3.3 求解结果的验证和 polyfit 的误区拿到系数后先用边界断言确认没有解错assert(abs(q(1) - q0) 1e-9); assert(abs(q(end) - qf) 1e-9); assert(abs(qd(1) - v0) 1e-9); assert(abs(qd(end)- vf) 1e-9); assert(abs(qdd(1) - a0) 1e-9); assert(abs(qdd(end)- af) 1e-9);这些断言能捕捉 b 向量中 T 和 T^2 乘错的问题。我见过不少人在这里用polyfit去拟合几个稀疏点生成轨迹效果是曲线经过中间点但端点速度完全不可控机械臂启动瞬间还是会有冲击。polyfit 适合数据拟合轨迹规划需要的是插值约束二者不要混用。如果中间必须经过某些路径点正确做法是分段五次多项式并在连接点处把速度、加速度设为连续值。比如前一段终点速度是 0.2 rad/s后一段起点速度就写 0.2 rad/s而不是置零加速度同理。这就是多点连续轨迹的基本写法。Untitled2.m 如果只是单段演示可以在此基础上再封装一个quintic_traj_waypoint函数按路径点循环调用把上一段的末端状态传给下一段。注意解五次多项式系数用M\b不要用inv(M)*b。\在 MATLAB 里对稠密小矩阵走 LU 分解数值更稳、更快。这一章给到的函数足够替换大部分课程代码里的核心逻辑下一步就是把轨迹喂给一个真实的三维机械臂模型。4. 机械臂三维仿真把五次多项式轨迹喂给机械臂模型4.1 用 rigidBodyTree 搭建仿真机械臂拿到关节角轨迹后下一步是把它作用到机械臂模型上做三维仿真。MATLAB 官方 Robotics System Toolbox 里推荐用rigidBodyTree而不是老旧的SerialLink。下面是一个两连杆机械臂的搭建和动画过程刚好能验证五次多项式轨迹的空间效果。robot rigidBodyTree; link1 rigidBody(link1); jnt1 rigidBodyJoint(jnt1, revolute); setFixedTransform(jnt1, trvec2tform([0 0 0]) * axang2tform([0 0 1 0])); link1.Joint jnt1; addBody(robot, link1, base); link2 rigidBody(link2); jnt2 rigidBodyJoint(jnt2, revolute); setFixedTransform(jnt2, trvec2tform([0.3 0 0]) * axang2tform([0 0 1 0])); link2.Joint jnt2; addBody(robot, link2, link1);这里trvec2tform给定连杆偏移axang2tform([0 0 1 0])表示旋转轴是 z 轴、角度为 0。两段连杆长度设成 0.3 m可以观察末端轨迹。然后给两个关节分别规划轨迹[q1, qd1, qdd1, t] quintic_traj(0, pi/3, 0, 0, 0, 0, 2, 0.02); [q2, qd2, qdd2, ~] quintic_traj(0, -pi/6, 0, 0, 0, 0, 2, 0.02); figure; for i 1:length(t) config [q1(i) q2(i)]; show(robot, config, PreservePlot, false); xlim([-0.1 0.7]); ylim([-0.3 0.4]); zlim([-0.1 0.1]); title(sprintf(t %.2f s, t(i))); drawnow; endshow的第二个参数是关节坐标行向量DataFormat 默认是 row 姿态。这个循环把两个关节角序列逐帧推进能看到机械臂末端平滑地扫过一段圆弧。如果要记录末端轨迹可以在每个循环里调用getTransform(robot, config, link2)取出位置并保存。4.2 三次与五次多项式对比问题出在加速度和 jerk为了说明为什么非用五次不可把同一个运动用三次多项式重做一遍。三次多项式只有位置和速度约束加速度在两端会突变。对比代码% 三次多项式系数 M3 [1 0 0 0; 0 1 0 0; 1 T T^2 T^3; 0 1 2*T 3*T^2]; b3 [q0; v0; qf; vf]; c3 M3 \ b3; tau3 t / T; q3 polyval(c3(end:-1:1), tau3); qd3 polyval(polyder(c3(end:-1:1)), tau3) / T; qdd3 polyval(polyder(polyder(c3(end:-1:1))), tau3) / T^2; figure; subplot(2,1,1); plot(t, qdd, t, qdd3); legend(五次,三次); ylabel(加速度); subplot(2,1,2); plot(t(1:end-1), diff(qdd)/dt, linewidth, 1.5); hold on; plot(t(1:end-1), diff(qdd3)/dt); ylabel(jerk);运行后三次多项式的加速度曲线是直线两端跳变jerk 在起点和终点是尖峰五次多项式加速度曲线是斜线jerk 连续且有限。对六轴机械臂做三维仿真时这些尖峰在逆动力学计算中直接变成力矩噪声严重时会让ode45动态仿真的步长被迫缩小仿真速度变慢。因此五次多项式不仅是“更平滑”它直接决定了后续动力学仿真能不能稳定跑完。4.3 结合优化工具箱做约束可行化多关节仿真时经常遇到某个关节速度或加速度超限。最实用的做法是把总时间 T 设为优化变量用 MATLAB 优化工具箱的fmincon最小化 T约束是每个关节的速度和加速度峰值不超过额定值。目标是求最短可行时间代码框架如下function [c, ceq] traj_limits(T, q0_set, qf_set, vel_lim, acc_lim) c []; for j 1:length(q0_set) [~, qd, qdd] quintic_traj(q0_set(j), qf_set(j), 0, 0, 0, 0, T, 0.001); c [c; max(abs(qd)) - vel_lim(j); max(abs(qdd)) - acc_lim(j)]; end ceq []; end调用时给 T 一个下限比如 0.5 sfmincon 会返回满足约束的最短时间。这个方案比手动试 T 更可复现也是课程作业从“能画图”变成“像工程方案”的关键一步。关节速度上限 rad/s加速度上限 rad/s²joint11.53.0joint22.04.0上表是机器人手册里常见的关节额定参数。把这些值写进约束函数fmincon 就能自动找到一条既不超速度又不超加速度的轨迹。5. 验证和修正轨迹的三个工程化技巧5.1 峰值约束检查生成轨迹后先检查超限而不是直接送去仿真。一个快速检查函数如下function ok check_traj_limits(qd, qdd, vel_lim, acc_lim) ok max(abs(qd)) vel_lim max(abs(qdd)) acc_lim; end传入单关节的 qd、qdd 和对应的额定值返回布尔结果。这个函数短但能在仿真前拦住最粗糙的参数错误。5.2 时间缩放救回超限轨迹当峰值超限时不要重新解方程组直接按比例放大 T。速度峰值与 T 成反比加速度峰值与 T^2 成反比所以新的总时间取两者各自缩放比例的较大者T_new T * max(max(abs(qd))/vel_lim, sqrt(max(abs(qdd))/acc_lim));注意加速度比例要开根号因为 T 放大一倍加速度峰值降到四分之一。更新 T 后重新调用quintic_traj即可。5.3 实时控制器只传系数把整条轨迹数组发给控制器会占用缓冲实时系统通常只传六个系数和运动时长。控制器每个周期读取节拍时间计算 tau再用polyval得到当前期望位置并与编码器反馈做位置环。这样quintic_traj在控制器里退化成一条语句q_des polyval(c_high, tau);这就是五次多项式工程落地的最后一步离线规划算系数在线运行只做多项式求值。本文还有配套的精品资源点击获取