MATLAB五次多项式轨迹规划：原理、实现与验证

简介：面向机械臂控制与仿真领域，这份资料以五次多项式为核心，系统讲解其在轨迹规划与数据拟合中的应用，适合刚开始接触机器人运动学或MATLAB仿真的学习者。压缩包共2个文件，其中MATLAB源码（.m）用于实现多项式计算与仿真，另附一个RAR压缩包，整体仅2KB，便于直接下载查看。目前已有897人学习下载。内容不仅给出五次多项式的位置、速度、加速度公式，还详细梳理了从数据准备、polyfit拟合、参数优化、轨迹生成到实时控制的完整流程，并对比三次多项式说明五次多项式在平滑性与精度上的优势，有助于理解机械臂运动学中的连续轨迹控制思路。借助上述代码和推导，读者可以快速上手编写自己的机械臂轨迹规划脚本，并进一步结合仿真结果优化运动性能。

1. 五次多项式什么意思：从数学定义到MATLAB里的角色

先说数学定义：五次多项式是最高次数为 5 的多项式，标准形式是 f(t) = a5·t^5 + a4·t^4 + a3·t^3 + a2·t^2 + a1·t + a0，一共 6 个系数。轨迹规划里选它，不是因为“次数越高越好”，而是因为 5 次正好能同时约束起点和终点的位置、速度、加速度这 6 个边界条件，得到一条处处连续、加速度无跳变的曲线。三次多项式只能约束位置和速度，加速度会突变；七次以上又会引入不必要的振荡。在 MATLAB 中，一个多项式对应一个系数向量，比如p = [a5 a4 a3 a2 a1 a0]，配合polyval、polyder就能直接算曲线和导数。如果你刚接触这个概念，建议先忘掉“5 次方程求根”的主意，五次多项式在工程里多数用来做路径生成，而不是用来解方程。

2. 在MATLAB中构造五次多项式：polyfit与解析求解两条路

2.1 用 polyfit 拟合已知采样点

常见做法是给一组离散的位置数据，用polyfit做最小二乘拟合。比如已知 6 个点，想用 5 次多项式插值穿过它们，polyfit加一步求逆即可：

t_data = (0:5)'; % 6 个采样时刻，单调递增 y_data = [0; 0.5; 1.8; 3.1; 2.2; 0]; % 对应位置 p = polyfit(t_data, y_data, 5); t_fine = linspace(min(t_data), max(t_data), 200); y_fine = polyval(p, t_fine); plot(t_data, y_data, 'o', t_fine, y_fine, '-');

代码里polyfit(t_data, y_data, 5)返回长度为 6 的系数向量，5代表次数。注意，polyfit在这里是插值等效，因为点数和系数个数相等，所以曲线精确穿越每个点，而不是近似拟合。polyval把系数应用到稠密时间轴上，得到光顺曲线。

参数说明： - t_data、y_data 必须等长，且 t_data 单调； - degree=5 时返回 6 个系数，顺序从高次到常数； - 若采样点数多于 6，polyfit 变成最小二乘拟合，不再严格穿过点。

2.2 解析法：由边界条件直接解系数

轨迹规划里更常用的是解析法。已知起点和终点的时间t0和t1，以及各自的pos、vel、acc，一共 6 个条件，正好确定 6 个系数。最常见的做法是把时间区间平移到[0, T]，并归一化，然后用矩阵求解：

syms a5 a4 a3 a2 a1 a0 syms T t0 t = (t - t0) / T; % 归一化时间，范围[0,1] % 代入条件和导数，再求解线性方程组 % 为简化，这里直接用数值矩阵，常见代码会在下面给出

实际操作中不写符号计算，而是直接构造系数矩阵：

function p = quintic_coeff(q0, v0, a0, q1, v1, a1, 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; a0; q1; v1; a1]; c = M \ b; p = [c(6), c(5), c(4), c(3), c(2), c(1)]; end

这里 M 是依据归一化时间 t ∈ [0,1] 构造的系数矩阵，最后一行对应五阶导系数120其实没用到，因为边界条件只到加速度。函数返回的p已经是标准系数向量，可直接交给polyval。关键点是：矩阵求解在时间未归一化时容易条件数过大，务必先把实际时间缩放成[0,1]，再反算实际位移。

参数说明： - M 的每一行代表一个边界条件在 t=0 或 t=1 时的约束 - q0/v0/a0 是起点的位置、速度、加速度，q1/v1/a1 同理 - T 是运动总时间，若 T 很小，归一化能避免数值溢出

3. MATLAB五次多项式的核心参数与边界条件设计

3.1 归一化时间与物理时间的关系

设计五次多项式时最容易被忽略的一步是时间归一化。很多人直接把原始时间t代入多项式，结果系数数量级差异巨大，导致曲线在数值上不精确。正确的写法是令tau = (t - t_start) / T，tau 属于[0,1]，然后在 tau 域上构造多项式。最终位置公式是：

q(t) = a5·tau^5 + a4·tau^4 + a3·tau^3 + a2·tau^2 + a1·tau + a0

速度是dq/dtau · 1/T，加速度是d2q/dtau2 · 1/T^2。系数是在 tau 域里求出来的，但求导时要记得除以 T。下面这段代码演示如何在 MATLAB 里把这个过程闭环：

% 初始化状态 q_start = 0; v_start = 0; a_start = 0; q_end = 10; v_end = 0; a_end = 0; T_total = 2; % 用上面的 quintic_coeff 函数得到 lambda 域系数 p = quintic_coeff(q_start, v_start, a_start, q_end, v_end, a_end, T_total); % 生成 tau 序列 tau = linspace(0, 1, 500); % 位置 q_tau = polyval(p, tau); % 速度：对系数求导后除以 T dp = polyder(p); v_tau = polyval(dp, tau) / T_total; % 加速度：再求导一次除以 T^2 ddp = polyder(dp); a_tau = polyval(ddp, tau) / (T_total^2); % 画出三条曲线 subplot(3,1,1); plot(tau*T_total, q_tau); ylabel('位置'); subplot(3,1,2); plot(tau*T_total, v_tau); ylabel('速度'); subplot(3,1,3); plot(tau*T_total, a_tau); ylabel('加速度');

代码中的polyder是 MATLAB 自带函数，传入多项式系数向量，返回导数的系数向量。注意这里没有乘以 T，因为求导是对 tau 求的，物理导数要额外除以 T 或 T^2。如果不做这一步，速度会整体放大 T 倍，轨迹明显失真。

从表格里能看到，五次多项式正好是“位置、速度、加速度”都被约束的最优选择。如果控制要求里还包含加(jerk)连续，就得升级到七次多项式，MATLAB 里的写法完全一致，只是矩阵维数从 6×6 变成 8×8。

3.2 边界条件求导陷阱：别忘了雅可比缩放

有经验的 MATLAB 用户往往会在求物理速度时，直接把polyval(polyder(p), t_physical)里的 t_physical 代入，这是错的。因为p是在 tau 域定义的，物理位置的一阶导数是：

dq/dt = (dq/dtau) / T

同理，二阶导数要除以 T^2。这个缩放系数在编写五阶多项式轨迹生成函数时必须写清楚。建议把归一化、系数求解、导数缩放封装成一个 MATLAB 函数返回结构体，避免多次重复出错。我一般会这样组织：

function traj = quintic_traj(q0, v0, a0, q1, v1, a1, T, N) % 输入物理边界条件和采样点个数N tau = linspace(0, 1, N); p = quintic_coeff(q0, v0, a0, q1, v1, a1, T); q = polyval(p, tau); v = polyval(polyder(p), tau) / T; a = polyval(polyder(polyder(p)), tau) / T^2; traj = struct('t', tau*T, 'q', q, 'v', v, 'a', a, 'p', p); end

函数返回结构体traj，后续直接画图或发给控制器。结构体里的p是 tau 域系数，t是物理时间，方便在 Simulink 外部调用。

4. MATLAB高阶五次多项式的数值稳定性与陷阱

4.1 时间区间过大时的病态矩阵

直接用 5 次多项式处理很长的运动时间，比如 T = 100 秒，系数矩阵会变成病态矩阵。原因是系数 a5、a4 等数量级跨越 10 的 10 次方，MATLAB 在反斜杠求解M \ b时舍入误差会被放大。先看一个对比实验：

T_long = 100; tic; p_long = quintic_coeff(0,0,0,10,0,0,T_long); toc; % 不归一化直接解，矩阵几乎奇异 M_raw = [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]; p_raw = M_raw \ [0;0;0;10;0;0];

在这个例子里，T^5是 1e10 量级，和 1、2 这样的数字混在同一矩阵里，cond(M_raw)会轻松超过 1e15，结果可能完全不可信。而归一化后 tau ∈ [0,1]，矩阵元素都在 1 附近，条件数在几十以内。

4.2 polyfit 与 polyval 的高阶注意点

polyfit在阶数高时也可能病态，但它内部用 QR 分解，比直接构造范德蒙德矩阵稳定一些。不过五次多项式不属于高阶，只要时间归一化就没问题。真正要小心的是polyval的求值顺序：MATLAB 用 Horner 算法，速度很快，但如果你用polyval去算 tau 接近 1 的高次幂，结果可能和符号解有微小偏差。这不是 bug，而是浮点计算的正常结果。验证方法是用符号数学工具箱做对照：

syms tau p_syms = sym(p); q_syms = poly2sym(p_syms, tau); Q_func = matlabFunction(q_syms, 'Vars', tau); Q_numeric = Q_func(0.99999);

若Q_numeric与直接polyval的结果误差大于 1e-10，优先检查系数是否输入反了。MATLAB 的系数向量是降幂排列，p = [p5 p4 p3 p2 p1 p0]，最容易犯的错误是把低次项写在前面。

4.3 同阶导数之间的超调问题

五次多项式的加速度是一个三次多项式，在边界条件为“起点终点速度加速度均为 0”时，中间加速度会有对称的正负峰值。峰值大小与 T 的平方成反比。如果 T 太小，加速度会爆炸，实际系统根本跟不上。这时需要检查加速度最大值是否超过执行器极限。MATLAB 中一行代码就能找到最大值：

a_max = max(abs(a_tau)) / T^2;

当a_max超过物理限制时，需要增大 T 或改梯形曲线。五次多项式没有圆角或中间匀速段，所以T必须满足最小运动时间约束。这个约束也是一个重要的给定参数，而不是随便填的。

5. MATLAB验证五次多项式连续性：一个高效自动化检查技巧

验证五次多项式轨迹是否合格，除了肉眼画图，还要定量检查几个不变量。这里给出一个具体的检查函数，能一键确认位置、速度、加速度的连续性和边界条件是否满足：

function check_quintic(traj, q0, v0, a0, q1, v1, a1, T) % traj 是 quintic_traj 的返回值 eps_tol = 1e-9; % 检查边界条件 assert(abs(traj.q(1) - q0) < eps_tol, '起点位置不满足'); assert(abs(traj.q(end) - q1) < eps_tol, '终点位置不满足'); assert(abs(traj.v(1) - v0) < eps_tol, '起点速度不满足'); assert(abs(traj.a(end) - a1) < eps_tol, '终点加速度不满足'); % 检查一阶导数和二阶导数之间是否有断点 % 用数值差分对比解析导数 q_diff = gradient(traj.q, traj.t); assert(max(abs(q_diff - traj.v)) < 1e-6, '位置与速度不一致'); v_diff = gradient(traj.v, traj.t); assert(max(abs(v_diff - traj.a)) < 1e-5, '速度与加速度不一致'); disp('所有连续性检查通过'); end

gradient是 MATLAB 用中心差分求数值导数，解析推导的导数和数值导数的差不超过 1e-5 量级就说明没有断点。这个技巧的原理是：五次多项式的一阶导数是四次，二阶导数是三次，它们本身连续，但如果你在系数或时间缩放上出错，数值差分和解析结果就会明显偏差。实际项目里可以把这条检查嵌入到代码测试套件里，每次参数变化自动执行。

另一个更有针对性的技巧：直接用matlabFunction把轨迹公式转成可部署函数，替代反复polyval的查表调用。如果最终需要把轨迹烧录到控制器，建议把 tau 离散成查询表，配合polyfit做插值，而不是在嵌入式里实时算 5 次幂。这样做既保留解析轨迹的光顺性，又把运行时开销降到最低。上面这个检查函数和查询表生成器，能让你的五次多项式代码从“看起来对”升级到“可验证、可部署”。

本文还有配套的精品资源，点击获取