3、速度控制模式：速度环设计、速度前馈与加速度前馈、速度规划与平滑处理、速度控制中的抖动抑制

速度控制，说白了就是让电机按照你想要的转速跑起来。很多刚入行的朋友觉得这很简单——给个目标速度，PID 一怼不就完了？嗯，真这么干，你会发现电机要么抖得像筛子，要么响应慢得像蜗牛。今天我就把速度控制里那些坑和技巧，一次性讲清楚。

3.1 速度环设计：从理论到工程

速度环的核心任务只有一个：让实际速度快速、准确地跟随目标速度。我习惯用 PI 控制器作为速度环的基础，因为 D 项在速度环里容易引入高频噪声，弊大于利。

先看一个标准的 PI 速度环结构：

// 速度环 PI 控制器（位置式）
float speed_pid_update(float target_speed, float current_speed) {
    float error = target_speed - current_speed;
    integral += error * dt;
    // 抗积分饱和
    if (integral > integral_max) integral = integral_max;
    if (integral < -integral_max) integral = -integral_max;
    float output = Kp * error + Ki * integral;
    return output;
}

这里有个关键点：积分限幅。我在项目中遇到过一台六轴机器人，空载时跑得好好的，一夹持工件就开始超调震荡。查了半天，发现是积分项在负载变化时积累过大，导致输出饱和。加上抗积分饱和后，问题立刻解决。

你可能会问：为什么不用 PD？因为速度信号本身就有噪声，微分项会把噪声放大，导致电机高频抖动。除非你的编码器分辨率极高、采样率极快，否则别碰 D 项。

3.2 速度前馈与加速度前馈：让系统"预判"你的意图

纯反馈控制有个天然缺陷——它总是等误差出现了才去纠正。这就好比开车时你总是等车跑偏了才打方向盘，永远慢半拍。前馈就是解决这个问题的。

速度前馈：直接把目标速度乘以一个前馈系数，加到 PI 输出上。这样当目标速度变化时，控制器能提前给出一个基础输出，反馈环只需要补偿误差部分。

// 带速度前馈的 PI 控制器
float speed_ff_pid_update(float target_speed, float current_speed) {
    float error = target_speed - current_speed;
    integral += error * dt;
    // 抗积分饱和
    if (integral > integral_max) integral = integral_max;
    if (integral < -integral_max) integral = -integral_max;
    float feedback = Kp * error + Ki * integral;
    float feedforward = Kff_v * target_speed;  // 速度前馈
    return feedback + feedforward;
}

加速度前馈：比速度前馈更进一步。它根据目标加速度（速度的导数）来补偿。当系统需要快速加速时，加速度前馈能提供额外的力矩输出，大幅减小加速过程中的速度跌落。

前馈类型	输入信号	主要作用	适用场景
速度前馈	目标速度	减小稳态跟随误差	匀速跟踪、低速运动
加速度前馈	目标加速度	减小加速/减速时的动态误差	高速启停、频繁加减速

3.3 速度规划与平滑处理

给速度环一个阶跃信号（比如从 0 直接跳到 1000 rpm），再好的控制器也会抖三抖。速度规划就是让目标速度的变化变得平滑，减少对机械系统的冲击。

常用的速度规划方法有三种：

1. T 型速度规划：匀加速-匀速-匀减速。简单粗暴，但加速度有突变，存在冲击。
2. S 型速度规划：加加速度（Jerk）受限，加速度连续变化。工业机器人标配。
3. 多项式规划：用 5 次或 7 次多项式拟合速度曲线。最平滑，但计算量大。

我个人最常用的是 S 型速度规划。它比 T 型更平滑，又比多项式更容易实现。下面是一个简化的 S 型速度规划实现：

// S 型速度规划 - 计算当前时刻的目标速度
float s_curve_plan(float start_speed, float end_speed, float t, float total_t) {
    // 归一化时间 [0, 1]
    float tau = t / total_t;
    if (tau < 0.0f) tau = 0.0f;
    if (tau > 1.0f) tau = 1.0f;
    // S 型曲线：3*t^2 - 2*t^3
    float s = 3.0f * tau * tau - 2.0f * tau * tau * tau;
    return start_speed + (end_speed - start_speed) * s;
}

为什么用 3t² - 2t³？因为它在起点和终点的斜率（加速度）都为 0，保证了速度变化的连续性。你想想看，如果加速度在起点和终点有突变，机械系统就会感受到冲击——这就是抖动的来源之一。

3.4 速度控制中的抖动抑制

抖动是速度控制的头号敌人。它会让机器人末端抖动、加工表面粗糙、甚至引发机械共振。我总结了几种常见的抖动来源和对应的抑制方法：

抖动来源	表现特征	抑制方法
编码器量化噪声	低速时速度值跳动	速度观测器 / 低通滤波
PI 参数过激	高频震荡	降低 Kp / 增加 Ki 限幅
机械共振	特定频率剧烈抖动	陷波滤波器（Notch Filter）
摩擦力突变	过零点时卡顿	摩擦补偿 + 前馈

陷波滤波器是我在项目中用得最多的抖动抑制手段。有一次调试一个大型龙门架，速度一跑到 500 mm/s 就开始剧烈抖动，频谱分析发现是 35 Hz 的机械共振。我在速度环输出后面加了一个 35 Hz 的陷波滤波器，抖动立刻消失了。

// 二阶陷波滤波器（IIR）
typedef struct {
    float b0, b1, b2;
    float a1, a2;
    float x1, x2;
    float y1, y2;
} notch_filter_t;

float notch_filter_process(notch_filter_t *f, float input) {
    float output = f->b0 * input + f->b1 * f->x1 + f->b2 * f->x2
                   - f->a1 * f->y1 - f->a2 * f->y2;
    f->x2 = f->x1;
    f->x1 = input;
    f->y2 = f->y1;
    f->y1 = output;
    return output;
}

还有一种容易被忽略的抖动来源——速度计算方式。直接用位置差分求速度，在低速时噪声极大。我建议用 M/T 法测速（同时测量脉冲数和时间），或者用状态观测器来估计速度。下面是一个简单的速度观测器：

// 一阶速度观测器
float speed_observer(float position, float last_position, float last_speed, float dt) {
    float position_speed = (position - last_position) / dt;  // 直接差分
    float observed_speed = last_speed + K_obs * (position_speed - last_speed) * dt;
    return observed_speed;
}

这个观测器本质上是一个低通滤波器，但它比直接滤波更聪明——它利用了系统的运动模型，能在滤除噪声的同时保持较好的动态响应。

3.5 本章知识体系

下面这张图总结了速度控制模式的核心逻辑和各个模块之间的关系：

从这张图可以看得很清楚：速度控制不是简单的一个 PI 环就完事了。它需要从规划层开始做平滑处理，然后用前馈减少反馈环的负担，最后用抖动抑制手段解决残余问题。每一层都有自己的职责，缺一不可。

最后说一句：速度控制调好了，位置控制就成功了一半。因为位置环的外环依赖速度环的内环性能——内环带宽越高、响应越快，外环才能跑得越稳。所以，别急着调位置环，先把速度环吃透。