拓冰网站定制 
做了这么多年机械臂控制我见过太多轨迹规划踩坑的例子。刚入门时大家通常首选三次多项式公式短、代码简单、仿真效果看着也不错但真把轨迹跑上实际设备问题就来了末端抖、跟踪误差大、关节异响。后来我系统梳理了一遍不同阶次多项式的特性才发现三次多项式只是“能走”离“走得稳”还差不少。这篇文章就围绕七次多项式轨迹规划展开讲讲为什么三次不够用、七次好在哪里、系数怎么解以及我调试过程中积累的一些经验。这次分享我尽量把原理讲透同时给出能直接复制运行的MATLAB完整代码。不管你是做机械臂轨迹规划、自动化设备运动控制还是在做科研课题需要平滑轨迹这篇文章都值得花十分钟看完。就算你现在只用三次多项式把七次的思路理解清楚后再回头处理起末速度、加速度、加加速度都受限的任务时会感觉顺手很多。1. 为什么三次多项式不够用从“能走”到“走得稳”1.1 三次多项式的4个边界条件与先天局限三次多项式轨迹的通用形式很简单[ q(t) a_0 a_1 t a_2 t^2 a_3 t^3 ]四个未知系数对应四个边界条件最常见的就是约束起点和终点的位置与速度[ q(0) q_0,\quad \dot{q}(0) v_0,\quad q(T) q_f,\quad \dot{q}(T) v_f ]联立求解就能得到唯一的一组系数。我最早做单轴点到点运动时就用这个方案当时觉得已经够用了位置曲线连续、速度曲线连续机械臂也能从A点运动到B点。问题在于速度连续不代表加速度连续。对三次多项式求二阶导得到的是一条直线[ \ddot{q}(t) 2a_2 6a_3 t ]在起点 (t0) 和终点 (tT)加速度的值通常不为零。这意味着机械臂在启动的那一瞬间关节力矩会突然加到一个非零值然后在到达目标点的瞬间又突然卸掉。这种“瞬间加力、瞬间撤力”的过程反映在实际设备上就是振动和冲击。1.2 加速度跳变与加加速度连续的工程意义很多人第一次听到“加加速度连续”这个概念时觉得玄乎其实用生活场景类比一下就很好懂。你坐公交车司机起步时如果一脚油门踩到底你会明显感到背后被猛推一下这就是加速度突变如果司机踩着油门慢慢加深你感受到的推背感是逐渐变强的体感会舒服很多。机械臂也一样加速度突变意味着惯性力突变机械结构会因此产生弹性形变和残余振动。更严谨地说加速度的导数被称为加加速度Jerk它直接关系到机械系统受到的冲击程度。Jerk不连续即使加速度曲线看起来只有个折点也会在驱动层面产生激励把机械结构的固有模态激发出来。我实测过一台六轴机械臂用三次多项式规划时末端残余振动幅度明显高于五阶、七阶轨迹尤其在负载较大或臂展较长时差距更明显。另一个容易被忽略的影响是轨迹跟踪精度。控制器跟踪轨迹时加速度跳变会导致前馈力矩突变而伺服环路有一定带宽限制跟不上这种突变于是实际轨迹和规划轨迹之间就会出现偏差。做精密装配或高速搬运时这个偏差足以导致抓取失败或碰撞。1.3 什么时候该升级到七次多项式并不是所有场景都需要七次多项式。如果设备本身刚性很好、速度慢、负载小三次多项式完全够用没必要增加计算量。我自己的判断标准大致是这样运动速度较高或者负载较大对振动敏感时建议至少用到五次多项式。需要进行精密定位、末端有相机或传感器或者对轨迹平滑性有硬性要求时直接用七次。起点和终点不仅要约束速度还要约束加速度和加加速度时三次、五次都无能为力只能上七次。还有一个实际场景多段轨迹拼接。如果整条轨迹由多个路径点组成你希望在中间路径点不停顿、平滑过渡那么每段轨迹在端点处的速度、加速度甚至加加速度都要相等。次数不够高时强行保证高阶连续就成了难题。七次多项式因为约束数量多在处理这种“跨段连续”问题时优势非常明显。从数值计算角度看七次多项式也并没有想象中那么复杂。无非是矩阵从4×4变成8×8MATLAB一条反斜杠命令就解决了。本末倒置去省这点计算量反而在设备调试时花大量时间处理振动问题很不划算。2. 七次多项式原理8个边界条件怎么用、系数怎么求2.1 从三次到七次多出来的4个约束是什么七次多项式的标准形式是[ q(t) a_0 a_1 t a_2 t^2 a_3 t^3 a_4 t^4 a_5 t^5 a_6 t^6 a_7 t^7 ]8个系数对应8个边界条件。除了起点和终点的位置、速度还可以约束起点和终点的加速度、加加速度[ \begin{aligned} q(0) q_0, q(T) q_f \ \dot{q}(0) v_0, \dot{q}(T) v_f \ \ddot{q}(0) a_0, \ddot{q}(T) a_f \ \dddot{q}(0) j_0, \dddot{q}(T) j_f \end{aligned} ]这里的 (a_0) 和 (a_f) 是起点、终点的加速度(j_0) 和 (j_f) 是起点、终点的加加速度。注意不要和多项式系数搞混我写代码时会把这两组量在命名上严格分开。这个8个边界条件覆盖了轨迹规划中最常见的需求不仅要求位置连续、速度连续还要求加速度连续、加加速度连续。如果把整个运动过程看成一个整体七次多项式可以保证机械臂从静止出发、最终回到静止的全过程都平滑无冲击。2.2 归一化时间τ防止矩阵病态的巧妙做法直接对 (t) 构造8×8线性方程组会有一个隐患当运动时间 (T) 比较大时矩阵里会出现 (T^2)、(T^7) 这样的高次项不同数量级的元素混在一起矩阵条件数会变得很差求解出来的系数误差很大。我早期吃过这个亏当时规划一个20秒的转台运动解出来的位置曲线在末段明显偏离边界条件查了半天才发现是矩阵病态导致的。解决办法是引入归一化时间[ \tau \frac{t}{T},\quad \tau \in [0, 1] ]把轨迹写作 (q(t) f(\tau))这样所有系数都在1附近矩阵的结构固定求解稳定。时间尺度放缩后导数关系也要跟着变这里需要用到链式法则[ \dot{q} \frac{1}{T} \frac{df}{d\tau},\quad \ddot{q} \frac{1}{T^2} \frac{d^2 f}{d\tau^2},\quad \dddot{q} \frac{1}{T^3} \frac{d^3 f}{d\tau^3} ]边界条件也要相应换算。例如如果实际起点加速度是2 rad/s²运动时间是2秒那么归一化后对应的边界条件应为 (f(0) 2 \times 2^2 8)。这个换算关系在写代码时特别容易漏漏一步整个轨迹就错了。2.3 系数求解矩阵推导归一化后设[ f(\tau) c_0 c_1 \tau c_2 \tau^2 c_3 \tau^3 c_4 \tau^4 c_5 \tau^5 c_6 \tau^6 c_7 \tau^7 ]对 (\tau) 求各阶导数[ f(\tau) c_1 2c_2 \tau 3c_3 \tau^2 4c_4 \tau^3 5c_5 \tau^4 6c_6 \tau^5 7c_7 \tau^6 ][ f(\tau) 2c_2 6c_3 \tau 12c_4 \tau^2 20c_5 \tau^3 30c_6 \tau^4 42c_7 \tau^5 ][ f(\tau) 6c_3 24c_4 \tau 60c_5 \tau^2 120c_6 \tau^3 210c_7 \tau^4 ]分别代入 (\tau0) 和 (\tau1)可以得到一个8×8的线性方程组。用矩阵形式表示就是[ M \cdot c b ]其中 (M) 是一个固定不变的Vandermonde风格矩阵[ M \begin{bmatrix} 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 1 1 1 1 1 1 1 \ 0 1 2 3 4 5 6 7 \ 0 0 2 6 12 20 30 42 \ 0 0 0 6 24 60 120 210 \end{bmatrix} ]右侧边界条件向量 (b) 则是[ b \begin{bmatrix} q_0 \ v_0 T \ a_0 T^2 \ j_0 T^3 \ q_f \ v_f T \ a_f T^2 \ j_f T^3 \end{bmatrix} ]在MATLAB中一行代码就能完成求解c M \ b;我在实际项目中通常会额外做一个验证步骤把解出来的系数代回边界条件检查位置、速度、加速度、加加速度是否都满足预期。这样能最快发现单位换算错误或者符号搞反的问题。3. 完整代码实现一个人机交互友好的轨迹规划函数3.1 主函数septic_traj.m逐行拆解我的代码风格比较直白所有输入都以参数形式显式给出不依赖全局变量方便直接在仿真里调用。下面是主函数function [t, q, qd, qdd, qddd] septic_traj(q0, qf, v0, vf, a0, af, j0, jf, T, dt) % 七次多项式轨迹规划 % 输入 % q0, qf : 起点/终点位置 % v0, vf : 起点/终点速度 % a0, af : 起点/终点加速度 % j0, jf : 起点/终点加加速度 % T : 总运动时间 % dt : 采样时间 % 输出 % t : 时间序列 % q : 位置轨迹 % qd : 速度轨迹 % qdd : 加速度轨迹 % qddd : 加加速度轨迹 % 归一化时间避免矩阵病态 n max(round(T / dt), 2); tau linspace(0, 1, n 1); t tau * T; % 构建8x8系数矩阵 M [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 1 1 1 1 1 1 1; 0 1 2 3 4 5 6 7; 0 0 2 6 12 20 30 42; 0 0 0 6 24 60 120 210]; % 右侧边界条件向量注意T的幂次 b [q0; v0*T; a0*T^2; j0*T^3; qf; vf*T; af*T^2; jf*T^3]; % 求解系数 c0~c7 c M \ b; % polyval要求系数从高次到低次排列所以翻转 coef_high_to_low flipud(c(:)); % 用polyval和polyder依次求各阶导数这里是对归一化时间tau求导 p0 coef_high_to_low; p1 polyder(p0); % df/dtau p2 polyder(p1); % d^2f/dtau^2 p3 polyder(p2); % d^3f/dtau^3 % 求轨迹 q polyval(p0, tau); q_tau1 polyval(p1, tau); q_tau2 polyval(p2, tau); q_tau3 polyval(p3, tau); % 换算回实际物理量 qd q_tau1 / T; qdd q_tau2 / T^2; qddd q_tau3 / T^3; end几个关键点说一下linspace(0,1,n1)比0:dt/T:1更安全可以避免浮点误差导致最后一个点不是1的情况。flipud是为了配合polyval的降幂排列习惯很多初学者在这一步容易忘。polyder可以直接生成导函数的系数省去手动写导函数的多余代码也更不容易出错。所有物理量的换算都集中在最后的T、T^2、T^3上只要这一处不出错整条轨迹就是对的。3.2 可视化对比脚本三次vs七次光有轨迹生成函数还不够我一般会立刻写一个可视化脚本把位置、速度、加速度、加加速度四张图一次性画出来。这里给出一个可以直接跑通的示例% 对比三次多项式和七次多项式的轨迹特性 clear; clc; close all; % 运动参数 q0 0; % 起始位置 qf 90 * pi / 180; % 终止位置 90度 T 2; % 总时间2秒 dt 0.001; % 采样时间1ms % 七次多项式起末速度/加速度/加加速度均为0 [t7, q7, qd7, qdd7, qddd7] septic_traj(q0, qf, 0, 0, 0, 0, 0, 0, T, dt); % 三次多项式只有起末位置和速度约束 n3 round(T/dt); tau3 linspace(0, 1, n31); t3 tau3 * T; q3 q0 (qf - q0) .* (3*tau3.^2 - 2*tau3.^3); qd3 (qf - q0) / T .* (6*tau3 - 6*tau3.^2); qdd3 (qf - q0) / T^2 .* (6 - 12*tau3); qddd3 zeros(size(t3)); % 三次多项式的jerk在端点处是冲击 % 绘图 figure(Color, w, Position, [100 100 900 800]); subplot(4,1,1); plot(t7, q7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, q3*180/pi, r--, LineWidth, 1.5); ylabel(位置 (deg)); legend(七次, 三次); grid on; subplot(4,1,2); plot(t7, qd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qd3*180/pi, r--, LineWidth, 1.5); ylabel(速度 (deg/s)); grid on; subplot(4,1,3); plot(t7, qdd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qdd3*180/pi, r--, LineWidth, 1.5); ylabel(加速度 (deg/s^2)); grid on; subplot(4,1,4); plot(t7, qddd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qddd3*180/pi, r--, LineWidth, 1.5); ylabel(加加速度 (deg/s^3)); grid on; xlabel(时间 (s));运行这段代码你能清楚地看到两个关键差异第一三次多项式的加速度曲线是直线而且起末点加速度不为零如果起点速度、终点速度均为0那么起点加速度是正的、终点加速度是负的也就是说机械臂一开始就要“猛地加速”、最后要“猛地刹车”。第二三次多项式的加加速度在起点和终点是无穷大的冲击数学上表现为脉冲绘制时我直接置为0实际物理上这个冲击就是设备振动的主要来源。我在实际项目里拿到七次轨迹后第一件事就是看加加速度曲线有没有突变。只要加加速度曲线是连续光滑的设备通常不会发生明显振动。3.3 用符号计算做快速验证数值解算出来的系数到底对不对最稳妥的办法是用MATLAB符号计算交叉验证。这段代码不需要每回都跑但第一次写完函数后建议执行一遍syms t T q0 qf v0 vf a0 af j0 jf tau t / T; f c0 c1*tau c2*tau^2 c3*tau^3 c4*tau^4 c5*tau^5 c6*tau^6 c7*tau^7; % 构造边界条件方程组 eq1 subs(f, t, 0) q0; eq2 subs(diff(f, t), t, 0) v0; eq3 subs(diff(f, t, 2), t, 0) a0; eq4 subs(diff(f, t, 3), t, 0) j0; eq5 subs(f, t, T) qf; eq6 subs(diff(f, t), t, T) vf; eq7 subs(diff(f, t, 2), t, T) af; eq8 subs(diff(f, t, 3), t, T) jf; sol solve([eq1 eq2 eq3 eq4 eq5 eq6 eq7 eq8], [c0 c1 c2 c3 c4 c5 c6 c7]); % 代入数值后和septic_traj的结果对比 T_val 2; num_sol structfun((x) double(subs(x, T, T_val)), sol);这里注意符号计算里要直接对 (t) 求导不要对 (\tau) 求导后再换算因为符号求解器会自动处理链式法则简化中间过程。对比数值解和符号解如果误差在 (10^{-10}) 量级说明代码实现没问题。4. 从单轴到机械臂三维轨迹与多段轨迹衔接4.1 三维笛卡尔空间轨迹对每个轴单独插值实际机械臂运动往往是多个关节同时运动或者末端在三维空间中走轨迹。最简单也最常用的做法是对每个轴每个坐标分别调用一次七次多项式规划器。以三关节机械臂为例假设三个关节从初始角度转到目标角度每个关节的起末速度、加速度、加加速度都设为0q0 deg2rad([0, 45, 90]); qf deg2rad([90, 30, 10]); T 2; dt 0.01; % 预分配 n round(T/dt) 1; q zeros(n, 3); qd zeros(n, 3); qdd zeros(n, 3); qddd zeros(n, 3); for i 1:3 [t, q(:,i), qd(:,i), qdd(:,i), qddd(:,i)] ... septic_traj(q0(i), qf(i), 0, 0, 0, 0, 0, 0, T, dt); end % 画三关节位置曲线 figure(Color, w); plot(t, q(:,1)*180/pi, r-, LineWidth, 1.5); hold on; plot(t, q(:,2)*180/pi, g-, LineWidth, 1.5); plot(t, q(:,3)*180/pi, b-, LineWidth, 1.5); xlabel(时间 (s)); ylabel(关节角度 (deg)); legend(关节1, 关节2, 关节3); grid on;各关节独立规划时虽然每个关节的轨迹都满足七次多项式约束但必须注意整条轨迹的插值点需保持一致的时间网格。最稳妥的方式是先确定公共的dt和T然后统一生成时间轴再逐轴计算轨迹。否则各轴的数据点长度对不上后面做运动学正解或碰撞检测时会很麻烦。另外一个容易踩的坑是单位。角度建议统一用弧度不要混着来。有些参数来自Excel表格可能是角度制的直接拿来规划会导致速度、加速度量级差出57倍轨迹形状看着没毛病实际加速度曲线完全错误。4.2 多段轨迹的无冲击衔接单段七次多项式可以保证起末点的高阶连续但很多任务并不是单段运动。例如机械臂需要从A点经过B点再到达C点你又不想在B点停下来这时就需要分段规划并保证段与段之间平滑过渡。我的做法是给中间路径点也分配非零的速度、加速度和加加速度条件。假设第一段从0°运动到60°用时1秒第二段从60°运动到120°用时1秒。为了让两段在60°处平滑衔接第一段的终点速度、第二段的起点速度都设为同一个值加速度和加加速度也都设为0T1 1; T2 1; dt 0.001; v_mid 0.5; % 中间点速度单位rad/s [t1, q1, qd1, qdd1, qddd1] septic_traj(0, deg2rad(60), 0, v_mid, 0, 0, 0, 0, T1, dt); [t2, q2, qd2, qdd2, qddd2] septic_traj(deg2rad(60), deg2rad(120), v_mid, 0, 0, 0, 0, 0, T2, dt); % 拼接 t_all [t1; t1(end) t2(2:end)]; q_all [q1; q2(2:end)]; qd_all [qd1; qd2(2:end)]; qdd_all [qdd1; qdd2(2:end)]; qddd_all [qddd1; qddd2(2:end)];这里有个细节中间点的速度不能随便给要保证物理上可达。如果机械臂实际速度上限是0.3 rad/s你给一个0.5 rad/s的中间速度第一段轨迹可能在接近中间点时加速度超出约束。所以多段轨迹规划时最好先根据设备能力估算各路径点的最大可行速度再带入规划器。从连续性的角度看如果每个路径点只约束位置用三次多项式也能拼出位置连续、速度不连续的多段轨迹但这种轨迹在路径点处会有速度突变设备每次经过中间点都会“咯噔”一下。升级到七次后位置、速度、加速度、加加速度四个量在路径点处全部对齐设备经过路径点时完全感受不到切换这是七次多项式在多段轨迹场景下最突出的优势。4.3 与梯形速度规划的取舍不少工程师在运动控制里习惯用梯形速度规划Trapezoidal Velocity Profile。它把运动过程分成加速段、匀速段、减速段实现简单、参数直观因此在PLC和运动控制卡中非常常见。但梯形速度规划有一个天然缺点加速度在启动、匀速切换、减速开始这几个节点是跳变的加加速度是脉冲对机械系统的冲击和七次多项式相比要明显得多。我用一张表复盘不同方案的特性方案连续性边界约束能力计算量适用场景三次多项式位置、速度连续位置、速度共4个约束很小低速、低精度点到点五次多项式位置、速度、加速度连续位置、速度、加速度共6个约束小中高速点到点七次多项式位置、速度、加速度、加加速度连续位置、速度、加速度、加加速度共8个约束中高速、高精度、多段衔接梯形速度位置连续速度分段线性通常只约束速度和加速度很小简单点动、简易轴控制梯形速度规划的优点是总运动时间更容易预估和控制因为中间有明确的匀速段缺点则是平滑性差。七次多项式在平滑性上是明显胜出的但总时间控制需要靠数值迭代来确定合适的时间 (T)不能随心所欲指定速度和加速度。实际选型时我会看设备类型传统输送线、气动机构用梯形规划没毛病精密机械臂、半导体设备、医疗设备这类对振动敏感的场合能用七次就尽量上七次。5. 常见问题与避坑指南5.1 矩阵求解出现NaN或严重超调怎么办我在论坛上见过不少人问七次多项式解出来的轨迹在中间出现很大的反向摆动甚至是NaN。遇到这类问题我一般按下面的顺序排查现象可能原因处理方法求解结果全是NaN(T0)、边界条件中夹带数值类型错误检查T是否为正数用isnumeric确认边界条件轨迹严重超调加加速度边界条件设置过大或运动时间太短增大T或减小轨迹反向运动中间速度/加速度约束与起末位置不匹配在路径中间增加一个途经点分两段规划单位不一致导致量级失衡位置用度、速度用rad/s混合统一成弧度或统一成角度制调试七次多项式轨迹我的习惯是先把所有加加速度约束设为0、加速度约束设为0只保留起末位置和速度也就是让它退化成五次多项式甚至三次多项式先把基础轨迹跑通再逐步增加高阶约束。这样做的好处是能精确定位到底是哪个边界条件导致了异常。如果基础轨迹正常加上某个jerk约束后轨迹突变问题就出在这个约束值上。5.2 加速度/加加速度超限的判断与调整七次多项式只是保证轨迹数学上平滑不保证实际物理量不超限。规划完成后必须检查加速度和加加速度峰值是否在伺服驱动器允许范围内。我常用的检查方式是直接对轨迹求最大值acc_max max(abs(qdd)); jerk_max max(abs(qddd)); fprintf(最大加速度: %.3f rad/s^2\n, acc_max); fprintf(最大加加速度: %.3f rad/s^3\n, jerk_max);如果超限最简单的调整方式是增大总时间 (T)。因为加速度与 (1/T^2) 成正比加加速度与 (1/T^3) 成正比把T从2秒增大到2.5秒加速度峰值会降到原来的64%(2/2.5) 的平方加加速度峰值会降到51%(2/2.5) 的立方效果非常显著。如果设备对节拍有硬性要求时间不能拉长那就得降低规划速度或减小目标角度。这里推荐一个“最小时间搜索”的简单方法在给定加速度、加加速度约束下用二分法遍历 (T)找到刚好满足约束的最小运动时间。虽然这个方法没有考虑完整的动力学约束但比拍脑袋定时间实用得多。5.3 采样周期、控制周期与离散化注意事项轨迹规划出来的曲线是连续函数但实际控制器只能在离散时间点执行。采样周期dt的选择直接影响轨迹精度和控制器负载。如果机械臂控制周期是1msdt就设为0.001如果控制周期是125μsdt就设为0.000125。不要为了图平滑把dt设得比控制周期还小因为控制器根本用不到那些中间点反而白白增加内存和计算量。另一个常见坑是linspace(0, 1, round(T/dt)1)实际生成的时间点数和0:dt:T可能略有差异导致最后一个点不是精确的T。在需要精确计算终点位姿的场合建议统一用linspace生成时间轴然后用min(t - t_prev)来反推实际步长避免由于浮点误差导致轨迹时间错位。如果轨迹会导入Simulink做仿真验证我习惯把t、q、qd、qdd、qddd打包成一个timeseries对象这样Simulink的Signal Editor可以直接读取不需要自己写接口转换。打包方式也很简单q_ts timeseries(q, t); qd_ts timeseries(qd, t);5.4 七次多项式不是万能的什么时候换方案虽然七次多项式很强大但它也有自己的边界。它只能约束起点和终点的边界条件对轨迹中间区域没有额外的控制能力。如果轨迹中间必须避开某个障碍物或者必须经过某个特定的位形七次多项式本身无能为力需要引入中间路径点把轨迹分成多段再衔接。另外当路径点很多、对轨迹生成效率要求很高时七次多项式逐段求解的代码结构相对繁琐。这种情况下B样条、NURBS这类曲线反而更灵活因为它们具有局部支撑性修改一个控制点只影响附近一段曲线不影响全局轨迹。我遇到视觉引导的抓取任务时末端轨迹经常要实时变化这时我会改用B样条在固定工艺路径的离线规划场景中七次多项式仍然是首选。还有一个经验之谈七次多项式的加加速度边界条件不要给太大的非零值尤其在起点和终点。因为机械臂静止时突然有加加速度就好比你刚站稳就被人推了一下无论推得多平滑体感都很难受。实际工程中把起末的加速度和加加速度都设为0通常是最稳妥的选择。除非你明确知道设备需要预加载或预加速否则不要轻易给非零值。做轨迹规划这些年我最深的体会是方案本身没有绝对的高下之分关键看匹配不匹配应用场景。七次多项式给我的核心价值是把“振动”这个很难调的问题从源头处就消解掉一大半。如果你手头正好有遇到振动困扰的点到点运动任务不妨立刻把代码跑一遍对比一下三次和七次的加速度曲线曲线本身就会告诉你答案。 七次多项式轨迹规划：原理、MATLAB实现与机械臂应用
做了这么多年机械臂控制我见过太多轨迹规划踩坑的例子。刚入门时大家通常首选三次多项式公式短、代码简单、仿真效果看着也不错但真把轨迹跑上实际设备问题就来了末端抖、跟踪误差大、关节异响。后来我系统梳理了一遍不同阶次多项式的特性才发现三次多项式只是“能走”离“走得稳”还差不少。这篇文章就围绕七次多项式轨迹规划展开讲讲为什么三次不够用、七次好在哪里、系数怎么解以及我调试过程中积累的一些经验。这次分享我尽量把原理讲透同时给出能直接复制运行的MATLAB完整代码。不管你是做机械臂轨迹规划、自动化设备运动控制还是在做科研课题需要平滑轨迹这篇文章都值得花十分钟看完。就算你现在只用三次多项式把七次的思路理解清楚后再回头处理起末速度、加速度、加加速度都受限的任务时会感觉顺手很多。1. 为什么三次多项式不够用从“能走”到“走得稳”1.1 三次多项式的4个边界条件与先天局限三次多项式轨迹的通用形式很简单[ q(t) a_0 a_1 t a_2 t^2 a_3 t^3 ]四个未知系数对应四个边界条件最常见的就是约束起点和终点的位置与速度[ q(0) q_0,\quad \dot{q}(0) v_0,\quad q(T) q_f,\quad \dot{q}(T) v_f ]联立求解就能得到唯一的一组系数。我最早做单轴点到点运动时就用这个方案当时觉得已经够用了位置曲线连续、速度曲线连续机械臂也能从A点运动到B点。问题在于速度连续不代表加速度连续。对三次多项式求二阶导得到的是一条直线[ \ddot{q}(t) 2a_2 6a_3 t ]在起点 (t0) 和终点 (tT)加速度的值通常不为零。这意味着机械臂在启动的那一瞬间关节力矩会突然加到一个非零值然后在到达目标点的瞬间又突然卸掉。这种“瞬间加力、瞬间撤力”的过程反映在实际设备上就是振动和冲击。1.2 加速度跳变与加加速度连续的工程意义很多人第一次听到“加加速度连续”这个概念时觉得玄乎其实用生活场景类比一下就很好懂。你坐公交车司机起步时如果一脚油门踩到底你会明显感到背后被猛推一下这就是加速度突变如果司机踩着油门慢慢加深你感受到的推背感是逐渐变强的体感会舒服很多。机械臂也一样加速度突变意味着惯性力突变机械结构会因此产生弹性形变和残余振动。更严谨地说加速度的导数被称为加加速度Jerk它直接关系到机械系统受到的冲击程度。Jerk不连续即使加速度曲线看起来只有个折点也会在驱动层面产生激励把机械结构的固有模态激发出来。我实测过一台六轴机械臂用三次多项式规划时末端残余振动幅度明显高于五阶、七阶轨迹尤其在负载较大或臂展较长时差距更明显。另一个容易被忽略的影响是轨迹跟踪精度。控制器跟踪轨迹时加速度跳变会导致前馈力矩突变而伺服环路有一定带宽限制跟不上这种突变于是实际轨迹和规划轨迹之间就会出现偏差。做精密装配或高速搬运时这个偏差足以导致抓取失败或碰撞。1.3 什么时候该升级到七次多项式并不是所有场景都需要七次多项式。如果设备本身刚性很好、速度慢、负载小三次多项式完全够用没必要增加计算量。我自己的判断标准大致是这样运动速度较高或者负载较大对振动敏感时建议至少用到五次多项式。需要进行精密定位、末端有相机或传感器或者对轨迹平滑性有硬性要求时直接用七次。起点和终点不仅要约束速度还要约束加速度和加加速度时三次、五次都无能为力只能上七次。还有一个实际场景多段轨迹拼接。如果整条轨迹由多个路径点组成你希望在中间路径点不停顿、平滑过渡那么每段轨迹在端点处的速度、加速度甚至加加速度都要相等。次数不够高时强行保证高阶连续就成了难题。七次多项式因为约束数量多在处理这种“跨段连续”问题时优势非常明显。从数值计算角度看七次多项式也并没有想象中那么复杂。无非是矩阵从4×4变成8×8MATLAB一条反斜杠命令就解决了。本末倒置去省这点计算量反而在设备调试时花大量时间处理振动问题很不划算。2. 七次多项式原理8个边界条件怎么用、系数怎么求2.1 从三次到七次多出来的4个约束是什么七次多项式的标准形式是[ q(t) a_0 a_1 t a_2 t^2 a_3 t^3 a_4 t^4 a_5 t^5 a_6 t^6 a_7 t^7 ]8个系数对应8个边界条件。除了起点和终点的位置、速度还可以约束起点和终点的加速度、加加速度[ \begin{aligned} q(0) q_0, q(T) q_f \ \dot{q}(0) v_0, \dot{q}(T) v_f \ \ddot{q}(0) a_0, \ddot{q}(T) a_f \ \dddot{q}(0) j_0, \dddot{q}(T) j_f \end{aligned} ]这里的 (a_0) 和 (a_f) 是起点、终点的加速度(j_0) 和 (j_f) 是起点、终点的加加速度。注意不要和多项式系数搞混我写代码时会把这两组量在命名上严格分开。这个8个边界条件覆盖了轨迹规划中最常见的需求不仅要求位置连续、速度连续还要求加速度连续、加加速度连续。如果把整个运动过程看成一个整体七次多项式可以保证机械臂从静止出发、最终回到静止的全过程都平滑无冲击。2.2 归一化时间τ防止矩阵病态的巧妙做法直接对 (t) 构造8×8线性方程组会有一个隐患当运动时间 (T) 比较大时矩阵里会出现 (T^2)、(T^7) 这样的高次项不同数量级的元素混在一起矩阵条件数会变得很差求解出来的系数误差很大。我早期吃过这个亏当时规划一个20秒的转台运动解出来的位置曲线在末段明显偏离边界条件查了半天才发现是矩阵病态导致的。解决办法是引入归一化时间[ \tau \frac{t}{T},\quad \tau \in [0, 1] ]把轨迹写作 (q(t) f(\tau))这样所有系数都在1附近矩阵的结构固定求解稳定。时间尺度放缩后导数关系也要跟着变这里需要用到链式法则[ \dot{q} \frac{1}{T} \frac{df}{d\tau},\quad \ddot{q} \frac{1}{T^2} \frac{d^2 f}{d\tau^2},\quad \dddot{q} \frac{1}{T^3} \frac{d^3 f}{d\tau^3} ]边界条件也要相应换算。例如如果实际起点加速度是2 rad/s²运动时间是2秒那么归一化后对应的边界条件应为 (f(0) 2 \times 2^2 8)。这个换算关系在写代码时特别容易漏漏一步整个轨迹就错了。2.3 系数求解矩阵推导归一化后设[ f(\tau) c_0 c_1 \tau c_2 \tau^2 c_3 \tau^3 c_4 \tau^4 c_5 \tau^5 c_6 \tau^6 c_7 \tau^7 ]对 (\tau) 求各阶导数[ f(\tau) c_1 2c_2 \tau 3c_3 \tau^2 4c_4 \tau^3 5c_5 \tau^4 6c_6 \tau^5 7c_7 \tau^6 ][ f(\tau) 2c_2 6c_3 \tau 12c_4 \tau^2 20c_5 \tau^3 30c_6 \tau^4 42c_7 \tau^5 ][ f(\tau) 6c_3 24c_4 \tau 60c_5 \tau^2 120c_6 \tau^3 210c_7 \tau^4 ]分别代入 (\tau0) 和 (\tau1)可以得到一个8×8的线性方程组。用矩阵形式表示就是[ M \cdot c b ]其中 (M) 是一个固定不变的Vandermonde风格矩阵[ M \begin{bmatrix} 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 1 1 1 1 1 1 1 \ 0 1 2 3 4 5 6 7 \ 0 0 2 6 12 20 30 42 \ 0 0 0 6 24 60 120 210 \end{bmatrix} ]右侧边界条件向量 (b) 则是[ b \begin{bmatrix} q_0 \ v_0 T \ a_0 T^2 \ j_0 T^3 \ q_f \ v_f T \ a_f T^2 \ j_f T^3 \end{bmatrix} ]在MATLAB中一行代码就能完成求解c M \ b;我在实际项目中通常会额外做一个验证步骤把解出来的系数代回边界条件检查位置、速度、加速度、加加速度是否都满足预期。这样能最快发现单位换算错误或者符号搞反的问题。3. 完整代码实现一个人机交互友好的轨迹规划函数3.1 主函数septic_traj.m逐行拆解我的代码风格比较直白所有输入都以参数形式显式给出不依赖全局变量方便直接在仿真里调用。下面是主函数function [t, q, qd, qdd, qddd] septic_traj(q0, qf, v0, vf, a0, af, j0, jf, T, dt) % 七次多项式轨迹规划 % 输入 % q0, qf : 起点/终点位置 % v0, vf : 起点/终点速度 % a0, af : 起点/终点加速度 % j0, jf : 起点/终点加加速度 % T : 总运动时间 % dt : 采样时间 % 输出 % t : 时间序列 % q : 位置轨迹 % qd : 速度轨迹 % qdd : 加速度轨迹 % qddd : 加加速度轨迹 % 归一化时间避免矩阵病态 n max(round(T / dt), 2); tau linspace(0, 1, n 1); t tau * T; % 构建8x8系数矩阵 M [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 1 1 1 1 1 1 1; 0 1 2 3 4 5 6 7; 0 0 2 6 12 20 30 42; 0 0 0 6 24 60 120 210]; % 右侧边界条件向量注意T的幂次 b [q0; v0*T; a0*T^2; j0*T^3; qf; vf*T; af*T^2; jf*T^3]; % 求解系数 c0~c7 c M \ b; % polyval要求系数从高次到低次排列所以翻转 coef_high_to_low flipud(c(:)); % 用polyval和polyder依次求各阶导数这里是对归一化时间tau求导 p0 coef_high_to_low; p1 polyder(p0); % df/dtau p2 polyder(p1); % d^2f/dtau^2 p3 polyder(p2); % d^3f/dtau^3 % 求轨迹 q polyval(p0, tau); q_tau1 polyval(p1, tau); q_tau2 polyval(p2, tau); q_tau3 polyval(p3, tau); % 换算回实际物理量 qd q_tau1 / T; qdd q_tau2 / T^2; qddd q_tau3 / T^3; end几个关键点说一下linspace(0,1,n1)比0:dt/T:1更安全可以避免浮点误差导致最后一个点不是1的情况。flipud是为了配合polyval的降幂排列习惯很多初学者在这一步容易忘。polyder可以直接生成导函数的系数省去手动写导函数的多余代码也更不容易出错。所有物理量的换算都集中在最后的T、T^2、T^3上只要这一处不出错整条轨迹就是对的。3.2 可视化对比脚本三次vs七次光有轨迹生成函数还不够我一般会立刻写一个可视化脚本把位置、速度、加速度、加加速度四张图一次性画出来。这里给出一个可以直接跑通的示例% 对比三次多项式和七次多项式的轨迹特性 clear; clc; close all; % 运动参数 q0 0; % 起始位置 qf 90 * pi / 180; % 终止位置 90度 T 2; % 总时间2秒 dt 0.001; % 采样时间1ms % 七次多项式起末速度/加速度/加加速度均为0 [t7, q7, qd7, qdd7, qddd7] septic_traj(q0, qf, 0, 0, 0, 0, 0, 0, T, dt); % 三次多项式只有起末位置和速度约束 n3 round(T/dt); tau3 linspace(0, 1, n31); t3 tau3 * T; q3 q0 (qf - q0) .* (3*tau3.^2 - 2*tau3.^3); qd3 (qf - q0) / T .* (6*tau3 - 6*tau3.^2); qdd3 (qf - q0) / T^2 .* (6 - 12*tau3); qddd3 zeros(size(t3)); % 三次多项式的jerk在端点处是冲击 % 绘图 figure(Color, w, Position, [100 100 900 800]); subplot(4,1,1); plot(t7, q7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, q3*180/pi, r--, LineWidth, 1.5); ylabel(位置 (deg)); legend(七次, 三次); grid on; subplot(4,1,2); plot(t7, qd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qd3*180/pi, r--, LineWidth, 1.5); ylabel(速度 (deg/s)); grid on; subplot(4,1,3); plot(t7, qdd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qdd3*180/pi, r--, LineWidth, 1.5); ylabel(加速度 (deg/s^2)); grid on; subplot(4,1,4); plot(t7, qddd7*180/pi, b-, LineWidth, 1.5); hold on; plot(t3, qddd3*180/pi, r--, LineWidth, 1.5); ylabel(加加速度 (deg/s^3)); grid on; xlabel(时间 (s));运行这段代码你能清楚地看到两个关键差异第一三次多项式的加速度曲线是直线而且起末点加速度不为零如果起点速度、终点速度均为0那么起点加速度是正的、终点加速度是负的也就是说机械臂一开始就要“猛地加速”、最后要“猛地刹车”。第二三次多项式的加加速度在起点和终点是无穷大的冲击数学上表现为脉冲绘制时我直接置为0实际物理上这个冲击就是设备振动的主要来源。我在实际项目里拿到七次轨迹后第一件事就是看加加速度曲线有没有突变。只要加加速度曲线是连续光滑的设备通常不会发生明显振动。3.3 用符号计算做快速验证数值解算出来的系数到底对不对最稳妥的办法是用MATLAB符号计算交叉验证。这段代码不需要每回都跑但第一次写完函数后建议执行一遍syms t T q0 qf v0 vf a0 af j0 jf tau t / T; f c0 c1*tau c2*tau^2 c3*tau^3 c4*tau^4 c5*tau^5 c6*tau^6 c7*tau^7; % 构造边界条件方程组 eq1 subs(f, t, 0) q0; eq2 subs(diff(f, t), t, 0) v0; eq3 subs(diff(f, t, 2), t, 0) a0; eq4 subs(diff(f, t, 3), t, 0) j0; eq5 subs(f, t, T) qf; eq6 subs(diff(f, t), t, T) vf; eq7 subs(diff(f, t, 2), t, T) af; eq8 subs(diff(f, t, 3), t, T) jf; sol solve([eq1 eq2 eq3 eq4 eq5 eq6 eq7 eq8], [c0 c1 c2 c3 c4 c5 c6 c7]); % 代入数值后和septic_traj的结果对比 T_val 2; num_sol structfun((x) double(subs(x, T, T_val)), sol);这里注意符号计算里要直接对 (t) 求导不要对 (\tau) 求导后再换算因为符号求解器会自动处理链式法则简化中间过程。对比数值解和符号解如果误差在 (10^{-10}) 量级说明代码实现没问题。4. 从单轴到机械臂三维轨迹与多段轨迹衔接4.1 三维笛卡尔空间轨迹对每个轴单独插值实际机械臂运动往往是多个关节同时运动或者末端在三维空间中走轨迹。最简单也最常用的做法是对每个轴每个坐标分别调用一次七次多项式规划器。以三关节机械臂为例假设三个关节从初始角度转到目标角度每个关节的起末速度、加速度、加加速度都设为0q0 deg2rad([0, 45, 90]); qf deg2rad([90, 30, 10]); T 2; dt 0.01; % 预分配 n round(T/dt) 1; q zeros(n, 3); qd zeros(n, 3); qdd zeros(n, 3); qddd zeros(n, 3); for i 1:3 [t, q(:,i), qd(:,i), qdd(:,i), qddd(:,i)] ... septic_traj(q0(i), qf(i), 0, 0, 0, 0, 0, 0, T, dt); end % 画三关节位置曲线 figure(Color, w); plot(t, q(:,1)*180/pi, r-, LineWidth, 1.5); hold on; plot(t, q(:,2)*180/pi, g-, LineWidth, 1.5); plot(t, q(:,3)*180/pi, b-, LineWidth, 1.5); xlabel(时间 (s)); ylabel(关节角度 (deg)); legend(关节1, 关节2, 关节3); grid on;各关节独立规划时虽然每个关节的轨迹都满足七次多项式约束但必须注意整条轨迹的插值点需保持一致的时间网格。最稳妥的方式是先确定公共的dt和T然后统一生成时间轴再逐轴计算轨迹。否则各轴的数据点长度对不上后面做运动学正解或碰撞检测时会很麻烦。另外一个容易踩的坑是单位。角度建议统一用弧度不要混着来。有些参数来自Excel表格可能是角度制的直接拿来规划会导致速度、加速度量级差出57倍轨迹形状看着没毛病实际加速度曲线完全错误。4.2 多段轨迹的无冲击衔接单段七次多项式可以保证起末点的高阶连续但很多任务并不是单段运动。例如机械臂需要从A点经过B点再到达C点你又不想在B点停下来这时就需要分段规划并保证段与段之间平滑过渡。我的做法是给中间路径点也分配非零的速度、加速度和加加速度条件。假设第一段从0°运动到60°用时1秒第二段从60°运动到120°用时1秒。为了让两段在60°处平滑衔接第一段的终点速度、第二段的起点速度都设为同一个值加速度和加加速度也都设为0T1 1; T2 1; dt 0.001; v_mid 0.5; % 中间点速度单位rad/s [t1, q1, qd1, qdd1, qddd1] septic_traj(0, deg2rad(60), 0, v_mid, 0, 0, 0, 0, T1, dt); [t2, q2, qd2, qdd2, qddd2] septic_traj(deg2rad(60), deg2rad(120), v_mid, 0, 0, 0, 0, 0, T2, dt); % 拼接 t_all [t1; t1(end) t2(2:end)]; q_all [q1; q2(2:end)]; qd_all [qd1; qd2(2:end)]; qdd_all [qdd1; qdd2(2:end)]; qddd_all [qddd1; qddd2(2:end)];这里有个细节中间点的速度不能随便给要保证物理上可达。如果机械臂实际速度上限是0.3 rad/s你给一个0.5 rad/s的中间速度第一段轨迹可能在接近中间点时加速度超出约束。所以多段轨迹规划时最好先根据设备能力估算各路径点的最大可行速度再带入规划器。从连续性的角度看如果每个路径点只约束位置用三次多项式也能拼出位置连续、速度不连续的多段轨迹但这种轨迹在路径点处会有速度突变设备每次经过中间点都会“咯噔”一下。升级到七次后位置、速度、加速度、加加速度四个量在路径点处全部对齐设备经过路径点时完全感受不到切换这是七次多项式在多段轨迹场景下最突出的优势。4.3 与梯形速度规划的取舍不少工程师在运动控制里习惯用梯形速度规划Trapezoidal Velocity Profile。它把运动过程分成加速段、匀速段、减速段实现简单、参数直观因此在PLC和运动控制卡中非常常见。但梯形速度规划有一个天然缺点加速度在启动、匀速切换、减速开始这几个节点是跳变的加加速度是脉冲对机械系统的冲击和七次多项式相比要明显得多。我用一张表复盘不同方案的特性方案连续性边界约束能力计算量适用场景三次多项式位置、速度连续位置、速度共4个约束很小低速、低精度点到点五次多项式位置、速度、加速度连续位置、速度、加速度共6个约束小中高速点到点七次多项式位置、速度、加速度、加加速度连续位置、速度、加速度、加加速度共8个约束中高速、高精度、多段衔接梯形速度位置连续速度分段线性通常只约束速度和加速度很小简单点动、简易轴控制梯形速度规划的优点是总运动时间更容易预估和控制因为中间有明确的匀速段缺点则是平滑性差。七次多项式在平滑性上是明显胜出的但总时间控制需要靠数值迭代来确定合适的时间 (T)不能随心所欲指定速度和加速度。实际选型时我会看设备类型传统输送线、气动机构用梯形规划没毛病精密机械臂、半导体设备、医疗设备这类对振动敏感的场合能用七次就尽量上七次。5. 常见问题与避坑指南5.1 矩阵求解出现NaN或严重超调怎么办我在论坛上见过不少人问七次多项式解出来的轨迹在中间出现很大的反向摆动甚至是NaN。遇到这类问题我一般按下面的顺序排查现象可能原因处理方法求解结果全是NaN(T0)、边界条件中夹带数值类型错误检查T是否为正数用isnumeric确认边界条件轨迹严重超调加加速度边界条件设置过大或运动时间太短增大T或减小轨迹反向运动中间速度/加速度约束与起末位置不匹配在路径中间增加一个途经点分两段规划单位不一致导致量级失衡位置用度、速度用rad/s混合统一成弧度或统一成角度制调试七次多项式轨迹我的习惯是先把所有加加速度约束设为0、加速度约束设为0只保留起末位置和速度也就是让它退化成五次多项式甚至三次多项式先把基础轨迹跑通再逐步增加高阶约束。这样做的好处是能精确定位到底是哪个边界条件导致了异常。如果基础轨迹正常加上某个jerk约束后轨迹突变问题就出在这个约束值上。5.2 加速度/加加速度超限的判断与调整七次多项式只是保证轨迹数学上平滑不保证实际物理量不超限。规划完成后必须检查加速度和加加速度峰值是否在伺服驱动器允许范围内。我常用的检查方式是直接对轨迹求最大值acc_max max(abs(qdd)); jerk_max max(abs(qddd)); fprintf(最大加速度: %.3f rad/s^2\n, acc_max); fprintf(最大加加速度: %.3f rad/s^3\n, jerk_max);如果超限最简单的调整方式是增大总时间 (T)。因为加速度与 (1/T^2) 成正比加加速度与 (1/T^3) 成正比把T从2秒增大到2.5秒加速度峰值会降到原来的64%(2/2.5) 的平方加加速度峰值会降到51%(2/2.5) 的立方效果非常显著。如果设备对节拍有硬性要求时间不能拉长那就得降低规划速度或减小目标角度。这里推荐一个“最小时间搜索”的简单方法在给定加速度、加加速度约束下用二分法遍历 (T)找到刚好满足约束的最小运动时间。虽然这个方法没有考虑完整的动力学约束但比拍脑袋定时间实用得多。5.3 采样周期、控制周期与离散化注意事项轨迹规划出来的曲线是连续函数但实际控制器只能在离散时间点执行。采样周期dt的选择直接影响轨迹精度和控制器负载。如果机械臂控制周期是1msdt就设为0.001如果控制周期是125μsdt就设为0.000125。不要为了图平滑把dt设得比控制周期还小因为控制器根本用不到那些中间点反而白白增加内存和计算量。另一个常见坑是linspace(0, 1, round(T/dt)1)实际生成的时间点数和0:dt:T可能略有差异导致最后一个点不是精确的T。在需要精确计算终点位姿的场合建议统一用linspace生成时间轴然后用min(t - t_prev)来反推实际步长避免由于浮点误差导致轨迹时间错位。如果轨迹会导入Simulink做仿真验证我习惯把t、q、qd、qdd、qddd打包成一个timeseries对象这样Simulink的Signal Editor可以直接读取不需要自己写接口转换。打包方式也很简单q_ts timeseries(q, t); qd_ts timeseries(qd, t);5.4 七次多项式不是万能的什么时候换方案虽然七次多项式很强大但它也有自己的边界。它只能约束起点和终点的边界条件对轨迹中间区域没有额外的控制能力。如果轨迹中间必须避开某个障碍物或者必须经过某个特定的位形七次多项式本身无能为力需要引入中间路径点把轨迹分成多段再衔接。另外当路径点很多、对轨迹生成效率要求很高时七次多项式逐段求解的代码结构相对繁琐。这种情况下B样条、NURBS这类曲线反而更灵活因为它们具有局部支撑性修改一个控制点只影响附近一段曲线不影响全局轨迹。我遇到视觉引导的抓取任务时末端轨迹经常要实时变化这时我会改用B样条在固定工艺路径的离线规划场景中七次多项式仍然是首选。还有一个经验之谈七次多项式的加加速度边界条件不要给太大的非零值尤其在起点和终点。因为机械臂静止时突然有加加速度就好比你刚站稳就被人推了一下无论推得多平滑体感都很难受。实际工程中把起末的加速度和加加速度都设为0通常是最稳妥的选择。除非你明确知道设备需要预加载或预加速否则不要轻易给非零值。做轨迹规划这些年我最深的体会是方案本身没有绝对的高下之分关键看匹配不匹配应用场景。七次多项式给我的核心价值是把“振动”这个很难调的问题从源头处就消解掉一大半。如果你手头正好有遇到振动困扰的点到点运动任务不妨立刻把代码跑一遍对比一下三次和七次的加速度曲线曲线本身就会告诉你答案。