简介：基于MATLAB的五次多项式轨迹规划仿真脚本，面向机器人路径规划、自动驾驶等动态控制系统开发者和相关专业初学者，也可用于课程实验或毕业设计的辅助参考。资源包共1个文件，为可直接运行的M脚本，压缩包仅1KB，体量小、部署简单。目前已有3511人学习/下载，使用热度较高。脚本围绕五次多项式系数求解展开，利用优化函数或约束条件可处理起点、终点、初始/终端速度与加速度要求，生成平滑连续的运动轨迹；同时提供绘图代码，能够输出时间-位置关系曲线和二维轨迹投影，直观展示轨迹形态与运动特性。五次多项式具有六个自由度，可同时满足位置、速度、加速度等多种边界条件，因此能在满足约束的前提下保持曲率连续；通过调整参数还可适配不同应用场景，是一份轻量而完整的轨迹规划入门示例。 做机器人运动控制，绕不开轨迹规划。早年做单轴点位运动，梯形速度曲线加个S型过渡已经够用，后来做多关节机械臂的连续路径，问题一下就暴露出来了——路点之间速度掉到零再重新起步，末端执行器一顿一顿的，焊缝、涂胶这类工艺根本没法看。我最近在重构平滑过渡模块，核心思路就是 duoduanwuci，也就是“多段五次”的拼音缩写，用多段五次多项式轨迹规划把每一段路径做到位置、速度、加速度连续。这篇文章会把原理、推导、工程实现和踩坑记录都整理出来，适合正在折腾运动控制、机器人轨迹规划的工程师参考。不管你是做六轴机械臂、SCARA，还是直角坐标平台，这套思路都能直接套。
1. 项目概述：多段五次轨迹规划到底在解决什么问题
1.1 单段轨迹规划的局限
先从单段说起。经典三次多项式轨迹 (\theta(t)=a_0+a_1t+a_2t^2+a_3t^3)，边界条件只能约束位置和速度，加速度在端点处必然跳变。带来的后果很直接：执行机构在启动和停止瞬间受到冲击，实际设备上你能观察到明显的抖动和噪音，负载稍大一点，机械谐振都被激出来了。
更麻烦的是连续路径场景。如果每段都单独用三次多项式拼接，段间为了保证速度连续已经够费劲，加速度连续性根本顾不上。机器走多路点时，轨迹在路点附近会出现不自然的“顿挫”，这在高精度涂胶、激光切割、焊接摆弧这类工艺里几乎是不可接受的。所以当路径点一多，三次方案的先天不足就彻底暴露出来了。
1.2 为什么偏偏选五次多项式
五次多项式 (\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5) 的优势在于它有六个待定系数，正好能对应六个边界条件：起点和终点的位置、速度、加速度。也就是说，端点的加速度可以被精确指定为0或者某个目标值，整段轨迹的加速度曲线变成连续光滑的曲线，机械冲击和残余振动能压到很低的水平。
多段场景下，只要把上一段的末端加速度当作下一段的起始加速度，二阶连续就天然满足了。很多工业控制器里的“平滑运动”底层算法，用的就是这一套。duoduanwuci 项目实际在干的，就是把这些五次段拼装进一个统一规划框架，让整条路径从起点到终点都保持平滑。我做这个模块之前，对比过B样条、S型速度曲线等方案，最终选五次多项式，原因很实际：数学形式简单、解析解好求、边界条件直观，不会像样条那样需要额外处理端点约束。
2. 五次多项式的核心数学原理与公式推导
2.1 边界条件与系数求解
直接列式子。设当前段时间为 (T)，起点位置 (q_0)、速度 (v_0)、加速度 (acc_0)，终点位置 (q_f)、速度 (v_f)、加速度 (acc_f)，位移 (h=q_f-q_0)。六个系数可以直接写出解析解：
[ a_0=q_0 ]
[ a_1=v_0 ]
[ a_2=\frac{acc_0}{2} ]
[ a_3=\frac{20h-(8v_f+12v_0)T-(3acc_0-acc_f)T^2}{2T^3} ]
[ a_4=\frac{-30h+(14v_f+16v_0)T+(3acc_0-2acc_f)T^2}{2T^4} ]
[ a_5=\frac{12h-(6v_f+6v_0)T-(acc_0-acc_f)T^2}{2T^5} ]
这个推导最核心的一点，是把六个方程写成矩阵形式然后求逆。手推一遍会很有帮助，但平时写代码不需要每次算逆矩阵，直接用上面的解析公式效率最高。注意我刻意用 (acc_0) 而不是 (a_2) 来表示边界加速度，避免和多项式系数 (a_0 \sim a_5) 混淆，这个习惯在代码里也很重要。
2.2 多段连接时的连续性条件
多段拼接时，每一段自身是光滑的，真正要处理好的是段间边界。位置连续性要求上一段末端位置等于下一段起点位置，这个天然满足；速度连续性要求 (v_{end,i}=v_{0,i+1})；加速度连续性要求 (acc_{end,i}=acc_{0,i+1})。
工程上的做法是，先对整个路径做一次路点速度规划，决定每个路点的速度值，再把这些值作为五次多项式的边界条件，逐段求解。路点速度通常用梯形速度法或者S型速度曲线粗算，不推荐拍脑袋给个固定值。为什么？因为如果某段给定起终点速度后，最大加速度可能超过电机能力，直接跑就会丢步或者报警。
2.3 为什么三次不够、七次不用
有人会问，既然五次这么好，为什么不直接用七次？七次多项式能额外约束加加速度 (jerk)，理论上更平滑。但实际工程里有两个问题：一是系统矩阵更高阶，数值敏感性明显上升，特别是当段时间很短时，系数容易变得非常大；二是边界条件太多，你得为每个路点都准备合理的 (jerk) 值，这在实际调试中很难给出。
三次多项式计算简单，但加速度连续性无法保证。五次多项式在两个方向上都处于“够用且好实现”的甜点区：加速度连续，满足绝大多数设备对平滑性的要求，同时解析公式简单，在低算力MCU上也能跑得动。
3. 多段五次轨迹规划的工程实现
3.1 总体流程：时间分配与路点速度预规划
多段规划不是简单把几个五次拼起来就完事。我实际调试下来的完整流程是这样的：
- 根据各段位移、最大速度/加速度约束估算每段最小时间；
- 规划路点速度（一般取相邻两段速度的折中，或按最大允许速度的百分比）；
- 逐段用五次多项式求解系数；
- 按统一时间戳重采样，得到位置、速度、加速度序列。
第二步最容易被忽视。如果随意给路点速度，某些段可能来不及从 (v_0) 加速到 (v_f)，曲线会产生严重“鼓包”。我通常用“梯形速度+限幅”的方式先粗算路点速度，再回来校验各段加速度峰值。不行就加时间或者降速度，直到所有运动学约束都满足为止。
3.2 核心代码实现：一个可以直接抄的五次轨迹类
我自己的项目里用Python快速验证算法，确认没问题后再移植到C++。这里给出一个可以直接运行的核心函数：
import numpy as np def quintic_coeffs(q0, qf, v0, vf, acc0, accf, T): if T <= 0: raise ValueError("T must be positive") h = qf - q0 T2 = T * T T3 = T2 * T T4 = T3 * T T5 = T4 * T a0 = q0 a1 = v0 a2 = acc0 / 2.0 a3 = (20*h - (8*vf + 12*v0)*T - (3*acc0 - accf)*T2) / (2*T3) a4 = (-30*h + (14*vf + 16*v0)*T + (3*acc0 - 2*accf)*T2) / (2*T4) a5 = (12*h - (6*vf + 6*v0)*T - (acc0 - accf)*T2) / (2*T5) return np.array([a0, a1, a2, a3, a4, a5]) def evaluate_quintic(coeffs, t): a0, a1, a2, a3, a4, a5 = coeffs pos = a0 + a1*t + a2*t*t + a3*t**3 + a4*t**4 + a5*t**5 vel = a1 + 2*a2*t + 3*a3*t*t + 4*a4*t**3 + 5*a5*t**4 acc = 2*a2 + 6*a3*t + 12*a4*t*t + 20*a5*t**3 return pos, vel, acc多段拼接时，把每段按自己的起始时间偏移即可：
def plan_multisegment(waypoints, vels, accs, times): # waypoints: [q0, q1, ..., qn] # vels: [v0, v1, ..., vn] # accs: [acc0, acc1, ..., accn] # times: 每段时长 [T0, T1, ..., T(n-1)] segments = [] for i in range(len(waypoints) - 1): seg = quintic_coeffs( waypoints[i], waypoints[i+1], vels[i], vels[i+1], accs[i], accs[i+1], times[i] ) segments.append((seg, times[i])) return segments给路点数组传入的实际是每个路点的位置、速度和加速度。注意首尾路点的加速度绝大多数场景都设为0，中间路点如果要严格二阶连续，必须让上一段的 (acc_f) 和下一段的 (acc_0) 相等。
3.3 时间分配的工程估算
每段最短时间不能靠感觉。一个常见做法是分别按速度上限和加速度上限估算：
[ T_{min,vel} = \frac{|h|}{v_{max}} ]
[ T_{min,acc} = \sqrt{\frac{2|h|}{a_{max}}} ]
实际取两者较大值，再乘一个1.1到1.3的安全系数。为什么要有安全系数？因为五次多项式的峰值速度可能比平均速度高不少，如果按梯形速度曲线的最小时间卡得太死，加速度或加加速度很容易超限。我之前调试一台六轴机器人时，某段位移只有20毫米，理论上0.4秒就能走完，但实际运行时电机在中间位置附近出现明显啸叫，后来把时间加到0.55秒才消停。这个例子我一直留着，现在所有时间分配都留余量。
3.4 数值稳定性与嵌入式注意点
在FPGA或MCU上直接算 (T^3)、(T^4)、(T^5) 有可能溢出或损失精度，尤其当段时间只有几十毫秒时，系数数量级差异很大。建议改成归一化参数 (s=t/T) 计算，或者把系数预计算成定点数。另一个细节是浮点误差在多段累加时会被放大，能使用双精度就尽量用双精度，特别是长路径、多段落场景。我在STM32上做移植时，单精度float跑五段以上的规划，末端位置误差能到几十个脉冲当量，换成double之后问题直接消失。
4. 常见问题与排查技巧实录
4.1 典型现象速查表
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 段间速度突变或停顿 | 路点速度没预规划好，或 vels 数组设置不合理 | 用梯形速度/S型曲线粗算路点速度，再校验各段加速度 |
| 中间位置“鼓包”严重 | 每段时间太短，或路点速度方向不对 | 增大该段 T，优先于降低速度幅值 |
| 加速度曲线有毛刺 | 端点加速度未强制为0，或段间加速度连续性未满足 | 检查 accs 数组，中间路点保持相邻段 acc 一致 |
| 长轨迹末端漂移 | 浮点精度不足，多段误差累积 | 改用双精度或归一化系数计算 |
| 机械臂实际运行抖动 | 加速度连续但 jerk 冲击过大 | 平滑路点速度，给加速度变化留出缓冲区 |
这张表基本覆盖了我这几年调试轨迹规划遇到的大部分问题。很多问题表面看起来是机械或电气的原因，最后定位到规划层时已经折腾了很久，所以建议先查轨迹数据，再查伺服参数。
4.2 关于“鼓包”的定量排查
“鼓包”的实质是速度曲线在某点出现反向，或者运动学参数超出物理极限。排查方法很简单：打印每段的最大速度、最大加速度和最大 jerk 值，和设定上限对比。如果某段最大速度明显超过路点速度设置，大概率是时间分配不足，优先增大该段时间而不是降低速度。
一个具体的例子：某段路径起点速度0.1 m/s，终点速度0.1 m/s，位移0.3米，时间给0.6秒。算出来最大速度能达到0.4 m/s，说明这段路径中段要冲高速再刹回来，这本身就是一种浪费。如果把时间加到1.0秒，最大速度降到0.25 m/s左右，加速度峰值也同步下降。这类问题从位置曲线上不容易看出来，但从速度曲线一眼就能发现。
4.3 我实际用下来的几条经验
路点速度取相邻两段能稳定达到的最小速度，而不是每段都冲到最大。很多新手喜欢把每个路点速度都设成设备极限，结果就是每段都在全速冲刺和急停之间切换，电机发热严重，轨迹精度也没提升。
起点和终点加速度在绝大多数场景下直接给0，不要省事留非零值，除非你明确知道自己在干什么。留非零值意味着设备在启停瞬间带加速度冲击，对减速机和联轴器都不友好。
如果做实时插补，建议把每段系数一次性算好缓存，不要在插补周期里重复求解。五次多项式求值本身只是几次乘加，花费不大，但如果你在20 kHz的中断里反复调用三角函数或者矩阵运算，时间开销会很明显。
调整参数时，先动时间，再动速度，最后动加速度边界条件。我踩过几次坑之后发现，这个顺序可以让问题更容易定位。先改时间最能直观看到曲线变化，如果改时间解决不了，再去检查路点速度是否合理，最后才动加速度约束。
我做 duoduanwuci 这段时间最大的体会是，轨迹规划看着偏理论，但真正决定设备能不能稳定跑的，往往是对边界条件、时间分配和数值精度这些“小事”的把握。把这几个点控制好，多段五次这套方案在实际项目里会非常省心，希望这篇整理能帮你少走一些弯路。
本文还有配套的精品资源，点击获取
