简介基于MATLAB的五次多项式轨迹规划仿真脚本面向机器人路径规划、自动驾驶等动态控制系统开发者和相关专业初学者也可用于课程实验或毕业设计的辅助参考。资源包共1个文件为可直接运行的M脚本压缩包仅1KB体量小、部署简单。目前已有3511人学习/下载使用热度较高。脚本围绕五次多项式系数求解展开利用优化函数或约束条件可处理起点、终点、初始/终端速度与加速度要求生成平滑连续的运动轨迹同时提供绘图代码能够输出时间-位置关系曲线和二维轨迹投影直观展示轨迹形态与运动特性。五次多项式具有六个自由度可同时满足位置、速度、加速度等多种边界条件因此能在满足约束的前提下保持曲率连续通过调整参数还可适配不同应用场景是一份轻量而完整的轨迹规划入门示例。 做机器人运动控制绕不开轨迹规划。早年做单轴点位运动梯形速度曲线加个S型过渡已经够用后来做多关节机械臂的连续路径问题一下就暴露出来了——路点之间速度掉到零再重新起步末端执行器一顿一顿的焊缝、涂胶这类工艺根本没法看。我最近在重构平滑过渡模块核心思路就是 duoduanwuci也就是“多段五次”的拼音缩写用多段五次多项式轨迹规划把每一段路径做到位置、速度、加速度连续。这篇文章会把原理、推导、工程实现和踩坑记录都整理出来适合正在折腾运动控制、机器人轨迹规划的工程师参考。不管你是做六轴机械臂、SCARA还是直角坐标平台这套思路都能直接套。1. 项目概述多段五次轨迹规划到底在解决什么问题1.1 单段轨迹规划的局限先从单段说起。经典三次多项式轨迹 (\theta(t)a_0a_1ta_2t^2a_3t^3)边界条件只能约束位置和速度加速度在端点处必然跳变。带来的后果很直接执行机构在启动和停止瞬间受到冲击实际设备上你能观察到明显的抖动和噪音负载稍大一点机械谐振都被激出来了。更麻烦的是连续路径场景。如果每段都单独用三次多项式拼接段间为了保证速度连续已经够费劲加速度连续性根本顾不上。机器走多路点时轨迹在路点附近会出现不自然的“顿挫”这在高精度涂胶、激光切割、焊接摆弧这类工艺里几乎是不可接受的。所以当路径点一多三次方案的先天不足就彻底暴露出来了。1.2 为什么偏偏选五次多项式五次多项式 (\theta(t)a_0a_1ta_2t^2a_3t^3a_4t^4a_5t^5) 的优势在于它有六个待定系数正好能对应六个边界条件起点和终点的位置、速度、加速度。也就是说端点的加速度可以被精确指定为0或者某个目标值整段轨迹的加速度曲线变成连续光滑的曲线机械冲击和残余振动能压到很低的水平。多段场景下只要把上一段的末端加速度当作下一段的起始加速度二阶连续就天然满足了。很多工业控制器里的“平滑运动”底层算法用的就是这一套。duoduanwuci 项目实际在干的就是把这些五次段拼装进一个统一规划框架让整条路径从起点到终点都保持平滑。我做这个模块之前对比过B样条、S型速度曲线等方案最终选五次多项式原因很实际数学形式简单、解析解好求、边界条件直观不会像样条那样需要额外处理端点约束。2. 五次多项式的核心数学原理与公式推导2.1 边界条件与系数求解直接列式子。设当前段时间为 (T)起点位置 (q_0)、速度 (v_0)、加速度 (acc_0)终点位置 (q_f)、速度 (v_f)、加速度 (acc_f)位移 (hq_f-q_0)。六个系数可以直接写出解析解[ a_0q_0 ][ a_1v_0 ][ a_2\frac{acc_0}{2} ][ a_3\frac{20h-(8v_f12v_0)T-(3acc_0-acc_f)T^2}{2T^3} ][ a_4\frac{-30h(14v_f16v_0)T(3acc_0-2acc_f)T^2}{2T^4} ][ a_5\frac{12h-(6v_f6v_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,i1})加速度连续性要求 (acc_{end,i}acc_{0,i1})。工程上的做法是先对整个路径做一次路点速度规划决定每个路点的速度值再把这些值作为五次多项式的边界条件逐段求解。路点速度通常用梯形速度法或者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[i1], vels[i], vels[i1], accs[i], accs[i1], 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) 有可能溢出或损失精度尤其当段时间只有几十毫秒时系数数量级差异很大。建议改成归一化参数 (st/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 这段时间最大的体会是轨迹规划看着偏理论但真正决定设备能不能稳定跑的往往是对边界条件、时间分配和数值精度这些“小事”的把握。把这几个点控制好多段五次这套方案在实际项目里会非常省心希望这篇整理能帮你少走一些弯路。本文还有配套的精品资源点击获取