第3章轨迹规划入门

第3章：轨迹规划入门：梯形速度曲线、S型速度曲线原理与实现

大家好，我是老张。今天咱们聊聊轨迹规划里最基础、也最实用的两个东西——梯形速度曲线和S型速度曲线。

说实话，我刚入行那会儿，觉得轨迹规划挺玄乎的。后来在产线上调了几年机器人，才明白这东西说白了就是：怎么让电机从A点跑到B点，跑得又快又稳，还不抖。

你想想看，如果直接给电机发一个阶跃速度指令，电机瞬间从0冲到3000转，那后果是什么？轻则抖动，重则过流报警，甚至机械结构都可能受损。所以，我们需要一个平滑的速度过渡过程。

3.1 梯形速度曲线

梯形速度曲线，顾名思义，速度曲线像个梯形。它分为三个阶段：匀加速、匀速、匀减速。

我个人习惯叫它“三段式”。为什么叫梯形？你看速度-时间图，先斜着上去（加速），然后平着走（匀速），最后斜着下来（减速），形状就是个梯形。

核心公式（位移-时间关系）：

加速段：s = 0.5 * a * t₁²

匀速段：s = v_max * t₂

减速段：s = v_max * t₃ - 0.5 * a * t₃²

其中 a 为加速度，v_max 为最大速度。

我在项目中遇到过一个问题：梯形曲线在加速段和匀速段的切换点，加速度会突变。说白了，就是加加速度（Jerk）是无穷大。这在一些对冲击敏感的场合（比如精密装配、晶圆搬运）是致命的。

避坑指南：

我曾经在一个高速点胶机上用了梯形曲线，结果胶水飞溅严重。后来发现是加速度突变导致机械臂末端抖动。如果你对运动平滑度有要求，梯形曲线要慎用。

梯形曲线的实现代码其实很简单。我一般这样写：

// 梯形速度曲线生成（伪代码）
void TrapezoidalProfile(float total_dist, float v_max, float a) {
    float t_acc = v_max / a;          // 加速时间
    float s_acc = 0.5 * a * t_acc * t_acc;  // 加速段位移
    
    // 判断是否达到最大速度
    if (total_dist < 2 * s_acc) {
        // 三角形曲线（未达到最大速度）
        // 处理逻辑...
    } else {
        // 标准梯形曲线
        float s_const = total_dist - 2 * s_acc;  // 匀速段位移
        float t_const = s_const / v_max;         // 匀速段时间
        // 输出三段的时间点...
    }
}

嗯，这里要注意：当总位移太小时，梯形会退化成三角形。很多新手会忽略这个边界条件，导致曲线生成失败。

3.2 S型速度曲线

S型速度曲线，说白了就是给梯形曲线“圆角”。它把加速度的变化也做了平滑处理，引入了加加速度（Jerk）的概念。

为什么叫S型？因为速度曲线从加速到匀速的过渡是S形的，不是直角拐弯。你想想看，这样机械冲击就小多了。

S型曲线分为七个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。听着复杂，其实核心就是让加速度也连续变化。

我的经验：

在实际项目中，我很少用完整的七段S型曲线。太复杂了，计算量大，而且参数调起来很痛苦。我一般用五段S型曲线（去掉匀加速和匀减速段），效果已经足够好。

S型曲线的核心参数有三个：最大速度 v_max、最大加速度 a_max、最大加加速度 j_max。其中j_max决定了曲线的“圆润程度”。

我曾经在一个六轴机器人上调试S型曲线，发现j_max设得太小，曲线太“软”，运动效率低；设得太大，又跟梯形曲线没区别。后来总结出一个经验公式：j_max ≈ 5 * a_max / t_acc，效果比较均衡。

3.3 两种曲线的对比

特性	梯形速度曲线	S型速度曲线
加速度连续性	不连续（有突变）	连续
加加速度	无穷大（切换点）	有限值（可设定）
计算复杂度	低	中高
运动平滑度	一般	优秀
适用场景	传送带、简单搬运	精密装配、CNC、机器人
参数数量	2个（v_max, a）	3个（v_max, a_max, j_max）

你看这个表就明白了。梯形曲线简单粗暴，适合对成本敏感、对精度要求不高的场合。S型曲线复杂但平滑，适合高端应用。

3.4 核心逻辑流程图

下面我用一张SVG图，把这两种曲线的核心逻辑串起来。你一看就明白。

这张图把决策逻辑讲得很清楚。你从输入参数开始，先判断能不能达到最大速度，然后决定用三角形还是梯形。最后根据平滑需求，决定是否升级成S型曲线。

3.5 实现中的几个坑

最后，我分享几个实战中踩过的坑：

浮点数精度问题：在计算加速段位移时，如果总位移很小，浮点数误差会导致曲线异常。我习惯用定点数或双精度来算。
时间片对齐：梯形曲线的三段时间，必须与伺服周期对齐。否则会出现最后一个周期跑不完的情况。
S型曲线的Jerk限制：j_max不能设得太小，否则加速段会变得很长，影响效率。我一般取a_max的5-10倍作为初始值。
实时性：S型曲线计算量比梯形大，在低端MCU上要注意优化。我试过在STM32F1上跑七段S型，CPU占用率直接飙到40%。后来改用查表法才压下来。

总结一句话：

梯形曲线是基础，S型曲线是进阶。先吃透梯形，再玩S型。别一上来就搞七段S型，容易把自己绕进去。

好了，这一章就到这里。梯形和S型曲线是轨迹规划的基石，后面讲更复杂的样条曲线、前瞻算法时，都会用到这些概念。你先把这两种曲线在代码里跑通，后面就顺了。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录