简介：本资源是一套面向本科及硕士阶段机器人运动控制与轨迹规划教学的Matlab实践算法包，聚焦于多段路径间基于S型加减速曲线的连续插补与平滑过渡问题，适用于数控系统、工业机器人轨迹优化等典型应用场景。压缩包共66个文件，含59个核心Matlab函数（.m）实现速度规划、加加速度约束、分段拼接与插值计算，3个.mat数据文件用于参数配置与结果验证，1份PDF说明文档梳理算法原理与接口调用逻辑，另含README.md结构说明、GIF动态效果演示及AI格式矢量图解，整体仅1.77MB，轻量易部署。已有1256人学习下载，配套完整运行结果与matlab2019a环境适配代码，开箱即用，特别适合初学者理解S型曲线在实际轨迹规划中的建模逻辑、约束处理与工程实现细节。 直接说结论：这个项目解决的是运动控制里非常经典、也非常让人头疼的一个问题——多段小线段连续插补时，怎么既保证轨迹精度，又不在每个拐角点把速度降成零。哪怕你只是做简单的3D打印路径规划、激光切割的轮廓加工、或者机械臂的轨迹插补，只要路径是一堆连在一起的点/线段，几乎都会撞上这个问题。
我最早接触这个话题，是因为做数控加工仿真时发现一个特别别扭的现象：一段几十个点的轮廓路径，如果用传统方式在每个拐角处都减速到零再启动，整个加工过程就像在“点头”一样，一顿一顿的。后来换成了带平滑过渡的连续插补算法，效果立竿见影——同样的路径，加工时间缩短了将近四成，表面质量也顺滑得多。
下面我就以这个项目的核心思路为主线，把基于S型曲线的多段曲线插补平滑过渡算法掰开揉碎了讲。重点包括：为什么非要用S型曲线、拐角过渡段怎么做、Matlab里怎么一步步实现、以及真实验证时最容易踩的几个坑。
1. 连续多段插补的痛点：为什么拐角处不能直接“刹车”
1.1 传统“点到点”插补的性能瓶颈
先从一个最简单的场景说起。假设你有一条路径，由8个坐标点构成，控制器要把刀具/执行器从P0走到P1、P2……一直走到P7。传统做法是“逐段运动、段间停稳”，也就是走完P0到P1之后，速度降到零，再重新加速走P1到P2。
这种方案的好处是控制逻辑极其简单：每段都可以独立做加减速规划，不存在段与段之间的耦合，误差也只需要考虑单段范围。但坏处也很明显：
- 每段都有“加速—匀速—减速”三个过程，段数一多，加减速的时间占总时间的比例非常大；
- 减速到零再加速，意味着电机/执行器在每个拐角处都要经历一次完全停止和重新启动，机械冲击很大；
- 对加工类应用来说，停顿会造成明显的驻留痕迹，比如激光切割的烧蚀点、3D打印的凸起接缝。
最直观的量化对比：如果一段路径的平均长度只有2mm，而机床最大进给速度是200mm/s，加速度是2000mm/s²，那么从零加速到最大速度需要0.1秒、跑过10mm。也就是说这段2mm的路径根本“跑不起来”，速度曲线是个三角形，最大速度只有约89mm/s。如果还要段段刹停，实际平均速度可能连50mm/s都不到，效率极其拉胯。
1.2 “平滑过渡”到底在平滑什么
连续多段插补的核心思想，就是不把每条线段独立看待，而是把整条多段路径看成一个整体，在拐角处用一段过渡曲线（最常见的就是过渡圆弧）把相邻线段的轨迹连接起来。这样插补器沿着“直线段→过渡圆弧→直线段”一路走到底，中间不需要完全停机。
这里“平滑”包含两层含义：
- 位置连续：过渡曲线和直线段之间必须保证切向连续，也就是进给方向不能突变。如果只保证位置连续而方向突变，就相当于在拐角处让速度矢量瞬间转向，这在物理上意味着无穷大的加速度，根本不可行。
- 速度平滑：过渡段上的速度变化必须受到加速度和加加速度（Jerk）的约束，不能因为进入圆弧就出现加速度突变。
很多初学者容易忽略的是速度平滑和位置平滑是两件事。你完全可以在直线段上做S型速度规划，但到了拐角处如果切向进给方向突变，S型曲线再平滑也没用。所以平滑过渡算法的本质，是同时做轨迹几何上的过渡和速度规划上的过渡。
1.3 这个算法适合哪些场景
说句实在话，这类算法不是给所有运动控制项目用的。如果你的应用场景是点到点的点位运动（比如某个气缸推进、某个轴单独定位），完全用不上。真正有刚需的场景是：
- 数控加工（CNC）：G代码里大量G01小线段，需要连续插补保持高进给；
- 3D打印：模型切片后大量微线段，拐角停顿会造成层间附着力问题和表面纹路；
- 激光切割/水刀切割：连续轮廓轨迹，停顿会降低切口质量；
- 机械臂轨迹规划：笛卡尔空间的多段轨迹，比如机器人沿焊缝运动时不能一停一顿；
- 视觉引导运动：相机采到一系列路径点，执行机构需要依次经过这些点。
如果你正好在做其中某一类，那这个主题就跟你强相关。
2. S型曲线速度规划：从梯形到七段式的进化逻辑
2.1 梯形加减速的问题在哪
很多入门级的运动控制会用梯形速度规划：速度以恒定加速度上升到目标值，匀速跑一段，再以恒定的减速度降下来。
梯形曲线的位移是连续的、速度是连续的，但加速度是阶跃的——起步瞬间从0跳到最大加速度，停止瞬间又跳回0。实际机械系统对加速度的突变是“有记性”的：机构会产生柔性冲击，长时间运行后传动间隙会变大，工件表面也会出现振纹。
更麻烦的是，某些负载比较重、惯量比较大的设备，加速度突变可能直接引发系统振荡。所以更成熟的控制方案会在加速度变化率上做文章，这就是加加速度Jerk概念的来源。
2.2 七段式S型曲线的公式拆解
标准的S型速度规划一般分七个阶段：
- 加加速段：加速度从0线性增加到最大加速度Amax，Jerk为+Jmax；
- 匀加速段：加速度保持在Amax，速度线性增加；
- 减加速段：加速度从Amax线性减小到0；
- 匀速段：速度保持Vmax；
- 加减速段：加速度从0反向增加到-Amax（即减速方向），速度线性减小；
- 匀减速段：加速度保持在-Amax；
- 减减速段：加速度从-Amax线性回到0。
每个阶段的速度曲线都是一段抛物线或直线拼接，所以整体速度曲线呈S形——这也是名称的由来。
用公式来表达的话，单个阶段的加加速度为常数 (J)，加速度 (a(t))、速度 (v(t))、位移 (s(t)) 分别为：
[ a(t) = a_0 + J \cdot t ]
[ v(t) = v_0 + a_0 t + \frac{1}{2} J t^2 ]
[ s(t) = v_0 t + \frac{1}{2} a_0 t^2 + \frac{1}{6} J t^3 ]
其中 (v_0)、(a_0) 分别是该阶段起始时的速度和加速度。注意这里单位要统一：如果J的单位是mm/s³，t的单位是秒，那么位移的单位就是毫米。
实际工程实现中，我一般不在线实时求解上述三组公式，而是用“查表+累计”的方式：预先把速度关于时间的函数离散成插补周期(T_s)的序列，每个插补周期按位置增量输出。这样做的好处是计算量小、实时性好，也方便处理“距离不够”的特殊情况。
2.3 短距离下S型曲线的退化处理
理论上，只要路径长度足够长，S型曲线能完整跑完七段。但实际路径往往没那么理想，经常出现“我这条线段只有0.5mm，但按照最大加速度和最大速度计算，需要至少2mm才能从零加速到目标速度再停下来”的情况。
这时候S型曲线就不能跑满七段，而是退化成“只加速到中间某个速度就要开始减速”的形式。具体来说，系统会先按“加速可达距离”和“减速需要的距离”判断，如果总距离小于最小加减速距离，就直接取消匀速段，甚至取消匀加速/匀减速段，只剩加加速段+减减速段。
这个退化逻辑实现起来很考验代码功底。我见过不少初学者写S型规划时只处理了完整七段的情况，一遇到短线段就报错或者输出不连续的速度，原因就是没做退化分支。这里建议在Matlab里先用穷举法生成所有可能的段组合（7段、6段、5段、4段……）并逐一判断可行条件，验证通过之后再固化到代码里。
3. 多段曲线插补的拐角过渡策略：过渡圆弧与速度约束
3.1 过渡段选型：直线-直线用圆弧过渡
在多段直线路径的拐角处，最经典的做法是插入一段圆弧过渡段。如下图所示意：两条线段相交于拐角点，在交点前后各取一点作为过渡圆弧的起点和终点，圆弧与两条线段相切。
选择圆弧的原因有四个：
- 几何简单，曲率恒定，便于计算法向加速度；
- 只通过圆心的偏移量和半径就能完整描述，存储量小；
- 圆弧上的速度约束可以用经典的离心加速度公式直接算；
- 圆弧与直线相切，保证了一阶连续性（G1连续），对大多数加工场景够用。
过渡比例是关键参数。假设拐角处两条线段长度分别为(L_1)和(L_2)，过渡圆弧起点到拐角点的长度占(L_1)的比例为(r)，终点到拐角点占(L_2)的比例也为(r)，那么实际圆弧半径(R)跟转角(\theta)之间满足：
[ R = \frac{r \cdot \min(L_1, L_2)}{\tan(\theta / 2)} ]
这个公式很有用：给定过渡比例和目标半径，就能反过来限制线段最短长度；反过来，给定线段长度和过渡比例，也能算出过渡半径。
3.2 过渡段速度的三大约束
过渡段上能跑多快，不是一个自由量，而是受三重约束里取最小值：
约束一：法向加速度约束。
圆弧上以速度(v)运动时，法向加速度为 (a_n = v^2 / R)。如果你希望法向加速度不超过某个阈值(a_{n,max})，那么过渡段速度上限就是：
[ v_{n} = \sqrt{a_{n,\max} \cdot R} ]
约束二：弓高误差约束。
对加工类应用来说，过渡圆弧替代了原来的尖角，这本身就是在“削”工件轮廓。如果你允许的最大轮廓误差是(\delta_{max})，那么过渡半径和误差之间有关系：(\delta = R - \sqrt{R^2 - (L_{chord}/2)^2})，L_chard是关键。但更常用的做法是直接把过渡半径限制为不超过误差允许值对应的最大半径，或者在算完半径后单独校验一次弓高误差。
简化工程公式为：给定允许误差(\delta)，过渡段圆弧半径(R)对应的最大速度还需满足：
[ \delta = R\left(1 - \cos(\theta/2)\right) ]
约束三：前后线段速度约束。
过渡段并不是孤岛——进入过渡段前，上一段直线上的速度也要遵循S型曲线的加减速能力；离开过渡段后，下一段直线段还要考虑后续路径的约束。所以实际上过渡段的最大允许速度要跟前瞻速度传播的结果取交集。
综合下来：
[ v_{limit} = \min(v_{n}, v_{\delta}, v_{lookahead}) ]
3.3 多段之间的“前瞻”约束传播
所谓前瞻（lookahead），就是在执行当前段之前，向后多看几段路径，把所有段的速度约束提前算出来。这么做的原因是：从当前段高速进入下一段时，如果下一段曲率更大、速度上限更低，你必须在到达下一段之前就提前减速。如果不能提前减速，等到了下一段再减就晚了。
前瞻算法的经典流程是双向扫描：
- 先做一次前向扫描：从第一段开始，按每段最大加速度能力计算“下一段能跑到的最大速度”；
- 再做一次反向扫描：从最后一段开始（末速度通常为0），反向计算“要能顺利减速到0，这段的起始速度最多是多少”；
- 两次扫描取交集，得到每一段实际允许的最大进给速度。
这个交集结果再作为S型规划的目标速度，就能保证全局速度曲线不会出现“突刹”或“无法按时停下”的问题。
用图来想象的话，整个路径的速度包络是一个由多个“山峰”组成的曲线——每个线段/过渡段都有一个速度上限，峰和谷互相约束。S型曲线规划的任务就是在这个包络下生成一条连续、平滑的速度轨迹。
4. Matlab实现的完整骨架：从数据结构到插补循环
4.1 算法主流程
基于这个思路，我用Matlab实现了一套原型算法。整体流程如下：
- 输入路径点：给出N个三维路径点，或N个二维路径点；
- 路径预处理：计算各段长度、拐角角度，确定哪些位置需要插入过渡圆弧；
- 前瞻速度规划：按双向扫描算出每个过渡段和直线段允许的最大速度；
- S型速度调度：把每段的起止速度作为边界条件，规划出逐插补周期的速度序列；
- 位置输出：每个插补周期根据当前速度和路径几何，合成插补点坐标；
- 误差校验：检查弓高误差、法向加速度、位移累积误差是否超限。
下面是我实现时的关键代码骨架，已做简化，但保留了核心逻辑。
4.2 数据结构设计
定义两个结构体：一个是路径描述，一个是插补器状态。
% 路径描述结构体 path = struct(); path.points = []; % Nx2 或 Nx3 路径点 path.segLength = []; % (N-1)x1 每段长度 path.cornerAngle = []; % (N-2)x1 拐角角度(弧度) path.cornerRadius = []; % (N-2)x1 过渡圆弧半径 path.cornerLimitV = []; % (N-2)x1 过渡段限速 path.segLimitV = []; % (N-1)x1 直线段限速 % 插补器状态结构体 interp = struct(); interp.Ts = 0.001; % 插补周期 1ms interp.vMax = 200; % 最大速度 mm/s interp.aMax = 2000; % 最大加速度 mm/s^2 interp.jMax = 50000; % 最大加加速度 mm/s^3 interp.vCur = 0; % 当前速度 interp.posCur = [0 0]; % 当前位置 interp.segIdx = 1; % 当前所在段 interp.segPos = 0; % 当前段内的累计位移4.3 S型速度调度核心函数
这一节是算法的心脏。我实现了一个简单的S型速度规划函数，输入为该段起始速度、终止速度、最大速度和加减速约束，输出该段每个插补周期对应的速度序列。
function vSeq = s_curve_planner(v0, v1, vMax, aMax, jMax, Ts) % 基于S型曲线生成从v0到v1的速度点序列 % 简化实现：仅处理三段式(加加速-匀速-减减速)的近似S型 % 完整实现需要处理七段式以及短距离退化 tAcc = 0; distAcc = 0; % 判断是否能达到目标速度 % 加速过程所需的距离: 从v0加速到v1 % 用临界加速度时间 t = (v1 - v0) / aMax 做初步估计 dv = v1 - v0; if dv > 0 % 加速阶段 tAcc = dv / aMax; % 如果耗时太短，用jerk限制平滑阶段 if tAcc * aMax < aMax^2 / jMax tAcc = aMax / jMax; end distAcc = v0 * tAcc + 0.5 * aMax * tAcc^2; else % 减速阶段 tAcc = -dv / aMax; if tAcc * aMax < aMax^2 / jMax tAcc = aMax / jMax; end distAcc = v0 * tAcc - 0.5 * aMax * tAcc^2; end % 生成速度序列(此处为简化示意，实际应按七段分别积分) n = round(tAcc / Ts); vSeq = linspace(v0, v1, max(n, 2)); vSeq = vSeq(1:end-1); % 去掉最后一个点，由下一段衔接 end注意：上面的代码是简化示意版本，真正的工程版本需要把七个阶段逐一展开，并且处理好短距离退化问题。我在完整实现里用了一个更稳的做法——不管路径长短，先按“完整七段”初始化参数，然后逐段检查“剩余位移是否为负”，为负就裁掉对应阶段并重新计算时间，循环迭代直到所有阶段都可行。
这个“迭代裁段”的思路在实际调试中帮我省了很多时间。比如遇到极短线段时，不需要为每一种退化组合单独写公式，而是统一按七段初始化、逐段裁剪，代码路径单一，不容易出bug。
4.4 过渡段参数计算与插补点合成
每两个相邻直线段之间，需要计算过渡圆弧的圆心位置、起终点坐标。我用的是向量法：
function [center, pStart, pEnd] = compute_transition_arc(p0, p1, p2, r) % p0, p1, p2 为三个连续路径点 % 计算以 r 为半径的过渡圆弧参数 v1 = p0 - p1; % 入向向量 v2 = p2 - p1; % 出向向量 v1 = v1 / norm(v1); v2 = v2 / norm(v2); theta = acos(dot(v1, v2)); % 拐角补角 if theta < 1e-6 % 接近直线，不需要过渡 center = p1; pStart = p1; pEnd = p1; return; end % 过渡起点和终点距拐角的距离 d = r / tan(theta / 2); pStart = p1 + v1 * d; pEnd = p1 + v2 * d; % 圆心在角平分线上，偏移距离为 r / sin(theta/2) midDir = (v1 + v2) / norm(v1 + v2); center = p1 + midDir * (r / sin(theta / 2)); end有了过渡圆弧参数后，插补循环里每到一个新的插补周期，先判断当前是在直线段还是过渡圆弧上，然后分别做直线插补或圆弧插补。
直线插补相对简单：速度乘上插补周期得到位移增量，再沿当前直线方向合成坐标。圆弧插补则略复杂，我采用的是“角度增量法”：根据当前速度求出角速度(\omega = v/R)，然后更新当前角度，再换算为坐标。
4.5 主插补循环
把所有部分串起来，就得到主插补循环的伪代码：
% 主插补循环 while segIdx <= size(path.points,1) - 1 % 1. 根据当前段和当前速度，确定下一插补周期的目标速度 vTarget = min(vSegLimit(segIdx), vCornerLimit(segIdx)); vNext = s_curve_step(vCur, vTarget, aMax, jMax, Ts); % 2. 更新位置 if is_on_arc(segIdx) % 圆弧插补 [posCur, arcAngle] = arc_interp_step(arcData, vNext, Ts); else % 直线插补 [posCur, segPos] = line_interp_step(segStart, segEnd, vNext, Ts); end % 3. 判断是否进入下一段 if segPos >= segLength(segIdx) segIdx = segIdx + 1; segPos = 0; % 可能需要更新过渡圆弧上下文 end % 4. 记录位置和速度 posLog(end+1, :) = posCur; velLog(end+1) = vNext; vCur = vNext; end这个循环本质上就是一个离散时间系统，每个Ts时间步更新一次。只要S型速度调度输出的速度序列是连续的、过渡段速度不超过限速，整条路径的位置输出就是平滑且可执行的。
5. 实测效果与调优经验：三维案例、参数灵敏度与避坑
5.1 一个五角星轮廓的仿真对比
为了验证算法的有效性，我用一段五角星轮廓路径做了测试。五角星有10个拐角，其中5个尖角角度很小，对过渡段的限速影响比较明显。
对比两种方案：
- 方案A：逐段停机插补，每段都做完整S型加减速；
- 方案B：本文的平滑过渡插补，拐角处插入过渡圆弧。
在同样的最大速度200mm/s、加速度2000mm/s²、加加速度50000mm/s³、插补周期1ms条件下，结果对比如下：
| 指标 | 方案A（逐段停机） | 方案B（平滑过渡） |
|---|---|---|
| 总插补时间 | 1.85s | 1.12s |
| 平均速度 | 约65mm/s | 约108mm/s |
| 最大法向加速度 | 0 | 约1520mm/s² |
| 最大轮廓误差 | 0 | 0.032mm（可控制） |
| 速度曲线连续性 | 速度反复归零 | 速度波动但始终>0 |
可以看到，方案B在总时间上缩短了约40%，平均速度提升了将近70%。代价是引入了可控的轮廓误差（因为我允许过渡圆弧偏离原始尖角），以及较大的法向加速度。
这里有个关键认知：平滑过渡本质上是用“轨迹误差”换“连续速度”。如果你要求零轮廓误差，那就不可能做拐角过渡，只能走回“段间停机”的老路。实际工程中要做的是根据工艺要求确定一个允许误差，再反向推算过渡参数。
5.2 加加速度Jerk的影响
加加速度Jmax是S型曲线里最直观也最需要谨慎设置的一个参数。
- Jmax越大，速度曲线越接近梯形，加速更猛，但冲击也变大；
- Jmax越小，速度曲线越圆润，运动更柔和，但响应变慢，总插补时间变长。
对比Jmax=50000和Jmax=20000两种情况，在同一个五角星轮廓下，后者总时间增加了约8%。看起来不多，但如果你是在做高速加工，8%的效率损失可能直接影响节拍。
我的建议是：优先按设备机械特性设定Jmax，比如丝杆传动机构可以取大一点，皮带传动机构应该取小一点；不要为了追求快而把Jmax拉满，否则电机噪声和震动会让你后悔。
5.3 过渡比例r的选取
过渡比例r决定了过渡圆弧占用线段长度的比例。r越大，过渡圆弧半径越大，过渡段限速越高，但轮廓误差也越大。
实际调试中我发现：
- r=0.3到0.5是一个比较均衡的范围；
- 如果线段极短（比如长度只有0.1mm），r=0.5会导致过渡圆弧几乎占据了整段线，直线段反而没有存在感，这种情况下需要限制r的上限，或者强制要求过渡段长度不得超过线段长度的30%；
- 如果拐角特别尖锐（夹角接近0度），tan(theta/2)非常小，过渡半径会急剧增大，很容易超出误差允许范围，这种情况要么强制缩小r，要么直接放弃过渡、允许速度降到零。
5.4 我踩过的几个典型的坑
坑一：速度序列在段与段之间出现突变。
因为我在前瞻阶段比较乐观地相信了反向扫描的结果，但实际上有一段的速度约束计算漏掉了“从过渡段进入下一段直线时，速度方向变化引起的等效加速度突变”。后来的解决办法是给所有限速值加一个安全系数（比如乘0.95），让规划结果别贴着上限跑。虽然简单粗暴，但真的很管用。
坑二：圆弧插补累加误差导致最终位置漂移。
圆弧插补如果用浮点数连续累加角度，运行几十个插补周期后，结束点的角度会偏离理论值，导致端点坐标和直线段起点对不上。解决方案是在进入圆弧之前记录起始角度和终止角度，插补过程中用“目标角度 - 当前角度”判断是否应该退出圆弧，而不是简单地累加位移。
坑三：Matlab的循环性能问题。
用Matlab写插补算法最大的痛点是循环太慢。如果路径有几千个点、插补周期1ms，纯for循环需要跑很久。我试过几种优化方式：
- 用向量化运算预处理所有S型速度序列（每段的速度序列可以预先生成好，插补时只是查表）；
- 如果必须实时计算，可以把内层循环写成mex函数或改用生成C代码；
- 仿真阶段就无所谓了，直接for循环，但用tic/toc评估性能，心里有数。
坑四：极短线段中的“速度锯齿”现象。
当线段长度接近插补周期内能走的距离时（比如1ms内走0.2mm，但线段只有0.15mm），会出现一个插补周期越过一段、下一周期越过下一段的“跳段”现象，导致速度曲线出现锯齿。这个问题的根源是离散化后位置分辨率和速度的比值不匹配。解决办法是增加“微段合并”预处理：把太短的相邻线段合并成一条更长的直线段，或合并成一条样条段后再插补。
5.5 后续可以怎么扩展
这个项目本身已经能跑通“直线-直线”的多段路径，但从通用性的角度还有几个方向可以扩展：
- 圆弧-圆弧过渡：很多CAM生成的路径已经包含圆弧段（G02/G03），需要处理直线和圆弧、圆弧和圆弧之间的过渡；
- 样条过渡：用贝塞尔曲线或B样条代替圆弧过渡，可以让过渡段更平滑，降低法向加速度峰值，但计算量也更大；
- 前瞻深度的自适应：现在的双向扫描是全局的，如果路径特别长，反向扫描一次可能比较耗时。可以做“窗口式前瞻”，只往前后各看N段，降低计算延迟；
- 插补输出到硬件：Matlab仿真验证之后，可以把核心C代码烧到STM32或者FPGA运动控制器里，配合脉冲输出/总线控制实现真机运动。如果你在搜“stm32h7 运动控制源码”这类词，说明已经走到这一步了，那这个Matlab原型就是你的算法验证基础。
写在最后的几点体会
这个项目给我的最大收获是：多段连续插补的难点不在某一个点上，而在“全局约束怎么传递”这条暗线上。S型曲线本身是数学上的成熟模型，过渡圆弧也是教科书里的经典几何，但把它们真正组合成一套可用的算法，需要处理大量边界情况：线段太短怎么办、拐角太尖怎么办、速度贴上限怎么办、离散化误差怎么控制……这些细节才是工程实现里最花时间的部分。
如果你是在Matlab里搭原型，我建议先别急着写完整的多段插补。把路径拆成两段、三段，先在几个简单的拐角上验证过渡圆弧和S型速度调度的正确性，确认速度曲线连续、位置曲线不超差，再逐步增加路径点数量。这个“从简到繁”的推进方式，能帮你避免一上来就被各种边界条件淹没。
最后还有一个小技巧：在Matlab中验证算法正确性时，把速度曲线和加速度曲线叠加到同一张图上检查。加速度曲线如果出现尖峰，说明某个位置的速度变化太急，多半是加加速度约束没生效。看加速度曲线比看速度曲线更容易发现这类问题。
我当时做的时候把全套验证脚本都留着：一个画路径的、一个画速度/加速度的、一个用来对比“逐段停机”和“平滑过渡”耗时差的。这些脚本在后期调参时帮了大忙。等你把算法确认无误、准备移植到嵌入式平台时，也可以参考同样的思路先做个离线仿真，再上真机——能省下不少调试时间和设备损耗。
本文还有配套的精品资源，点击获取
