PID 从公式到能跑：给直流电机写一个带抗积分饱和的速度环

一句话结论：教科书上的 PID 公式只能跑仿真；能上机的 PID 必须补上抗积分饱和、微分先行、输出限幅、无扰切换这四件事。

一、完整工程下载

压缩包内含全部源码、platformio.ini、Makefile、README.md，解压即用，不需要额外配置。

下载 pid-motor.zip　（9.6 KB，共 9 个文件）

.gitignore
Makefile
README.md
include/
  motor_sim.h
  pid.h
platformio.ini
src/
  motor_sim.c
  pid.c
test/
  test_pid.c

位置式 PID 完整公式

e(k)   = target(k) - measure(k)                 误差
u(k)   = Kp·e(k) + Ki·Σe(j)·Ts + Kd·[e(k)-e(k-1)]/Ts

直接照着写代码有三个坑：

积分饱和：电机被卡住时误差一直存在，积分项越累越大，松开的瞬间输出爆表 → 猛冲
微分冲击：目标值突变时 e(k)-e(k-1) 是个脉冲 → 输出瞬间尖峰，电机"咯"一声
输出无限制：算出来的 PWM 超过定时器上限 → 硬溢出

四件套怎么补

1. 微分先行（Derivative on Measurement）

把微分项从「误差的微分」改成「测量值的微分」并取负号：

D(k) = -Kd · [measure(k) - measure(k-1)] / Ts

物理意义：微分项本来就是为了阻尼，重要的是「被控量变化多快」，而不是「目标值跳变多快」。改成对测量值求导后，改目标值不会再引起尖峰。

2. 积分分离 / 抗饱和（Anti-windup）

两种主流写法，本工程都实现了：

方法	做法	特点
积分限幅	单独限制积分累加项的上下限 `±i_limit`	简单，绝大多数场景够用
反算法	输出饱和时，把超出的部分按 `1/Ki` 折回积分项	数学上更优雅，恢复更快

代码里用的是更强的组合：积分限幅 + 输出饱和时停止积分，这也是电机控制里最常用的写法。

3. 输出限幅与限斜率

if (out > out_max) out = out_max;
else if (out < out_min) out = out_min;

电机还要再加一层 PWM 变化率限制（slew rate），防止电流突变冲击驱动芯片和齿轮箱。

4. 无扰切换（Bumpless Transfer）

手动/自动切换时，如果积分项不处理，输出会跳变。做法是切回自动的那一刻把积分项重置为「让输出等于当前值」的值。

参数怎么整定（实操顺序）

先把 Ki、Kd 置 0，只留 Kp，从小到大加，直到系统开始等幅振荡，记下此时的 Ku 和周期 Tu
用 Ziegler-Nichols 表给个初值：

控制器	Kp	Ki	Kd
P	0.5·Ku	—	—
PI	0.45·Ku	0.54·Ku/Tu	—
PID	0.6·Ku	1.2·Ku/Tu	0.075·Ku·Tu

在初值附近微调：超调大就加 Kd，静差大就加 Ki，响应慢就加 Kp
一定要用真实的重载工况复测（车上有坡、机械臂有负载）

工程内容

pid.c/h：位置式 PID，含抗饱和、微分先行、限幅、参数在线修改、状态观测
motor_sim.c/h：一阶惯性 + 死区 + 饱和的直流电机模型，用来在电脑上把参数调好
test/test_pid.c：在电脑上跑阶跃响应，打印上升时间/超调量/稳态误差，并模拟积分饱和场景

调试要点

采样周期 Ts 必须固定。用定时器中断固定调用 pid_update()，

不要放在主循环里（主循环耗时会变，Ts 变了 Ki/Kd 的实际作用就变了）

Ts 的选法：一般取系统时间常数的 1/10 ~ 1/20。电机速度环常用 1~5ms
看三个量：目标值、测量值、输出值。把它们一起打到串口波形里，

一眼就能看出是超调（Kp/Ki 大）还是相位滞后（Ts 大）

Ki 别用 0 之外的小数糊过去：先把 B 参数（积分带宽）想清楚再调

进阶方向

换成增量式 PID（抗饱和天然更好，适合只有积分执行机构的场合）
加前馈（ff = Kff·target），跟踪性能提升明显
用二阶滤波器替代纯微分，抑制编码器量化噪声
上双环：外环位置 + 内环速度（平衡小车就是这么做的）

完整代码

`Makefile`

CC      ?= gcc
CFLAGS  ?= -std=c99 -Wall -Wextra -O2 -Iinclude
LDLIBS  ?= -lm
SRC      = src/pid.c src/motor_sim.c
TEST     = test/test_pid.c

ifeq ($(OS),Windows_NT)
EXT = .exe
endif
BIN = build/test$(EXT)

all: run

$(BIN): $(SRC) $(TEST)
	@mkdir -p build
	$(CC) $(CFLAGS) $(SRC) $(TEST) -o $(BIN) $(LDLIBS)

run: $(BIN)
	@$(BIN)

clean:
	rm -rf build

.PHONY: all run clean

`include/motor_sim.h`

/**
 * motor_sim.h - 直流电机一阶模型（仅在电脑仿真时使用）
 *
 * 模型：J·dw/dt = Kt·i - B·w - Tload，电流环用一阶惯性近似
 * 离散化后用于在 PC 上把 PID 参数调好，再烧到板子上。
 */
#ifndef MOTOR_SIM_H
#define MOTOR_SIM_H

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    float dt;          /* 仿真步长 */
    float tau;         /* 机电时间常数（秒） */
    float kv;          /* 稳态增益：稳态转速 = kv * 输入 */
    float deadzone;    /* 输入死区（占空比），电机低于此值不转 */
    float in_min;      /* 输入下限（如 -1.0 表示反转满速） */
    float in_max;
    float max_speed;   /* 机械限速 */
    float speed;       /* 当前转速（rpm） */
    float load;        /* 负载转矩（折算成转速损失，rpm） */
} motor_sim_t;

void motor_sim_init(motor_sim_t *m, float dt);
float motor_sim_update(motor_sim_t *m, float input);

#ifdef __cplusplus
}
#endif

#endif /* MOTOR_SIM_H */

`include/pid.h`

/**
 * pid.h - 位置式 PID 控制器（可上机版本）
 *
 * 相比教科书写法额外提供：
 *   - 微分先行（对测量值求导），消除目标值跳变引起的微分冲击
 *   - 抗积分饱和：积分限幅 + 输出饱和时停止积分
 *   - 输出限幅、输出变化率限制（slew rate）
 *   - 无扰切换（bumpless transfer）
 */
#ifndef PID_H
#define PID_H

#include <stdbool.h>
#include <stdint.h>

#ifdef __cplusplus
extern "C" {
#endif

typedef struct {
    /* ---- 参数（全部可在运行中修改） ---- */
    float kp;
    float ki;
    float kd;
    float ts;        /* 采样周期，单位秒，必须固定 */
    float out_min;   /* 输出下限 */
    float out_max;   /* 输出上限 */
    float i_limit;   /* 积分累加项限幅，<=0 表示等于 out_max-out_min */
    float d_lpf_alpha; /* 微分项一阶低通系数 0~1，1 表示不滤波 */
    float slew_rate; /* 每秒最大输出变化量，<=0 表示不限 */

    /* ---- 运行时状态 ---- */
    float integral;
    float prev_measure;
    float out;       /* 上一次的输出（限幅后） */
    bool  first;     /* 首次调用标志，避免用未初始化的 prev_measure */
    bool  enabled;   /* 手动/自动切换 */

    /* ---- 观测量（调试用） ---- */
    float p_term;
    float i_term;
    float d_term;
    uint32_t saturate_cnt; /* 输出饱和次数 */
} pid_t;

/** 初始化：直接把参数写进结构体，然后调用本函数复位内部状态 */
void pid_init(pid_t *pid);

/**
 * 计算一次输出
 * @param pid      控制器
 * @param target   目标值
 * @param measure  测量值
 * @param dt       本次与上次调用之间的实际时间（秒）。传 0 则使用 pid->ts
 * @return 限幅后的输出
 */
float pid_update(pid_t *pid, float target, float measure, float dt);

/** 每步都要调用：只更新积分项，输出不变（用于纯积分执行机构） */
void pid_set_enabled(pid_t *pid, bool enabled, float current_measure);

/** 修改 Kp/Ki/Kd 后调用，保持输出连续（无扰） */
void pid_reset_state(pid_t *pid);

#ifdef __cplusplus
}
#endif

#endif /* PID_H */

`platformio.ini`

[platformio]
default_envs = bluepill

[env:bluepill]
platform = ststm32
board = bluepill_f103c8
framework = arduino
upload_protocol = stlink
monitor_speed = 115200
build_flags =
    -Wall
    -Wextra
    -Isrc
lib_ldf_mode = deep+

`src/motor_sim.c`

#include "motor_sim.h"

void motor_sim_init(motor_sim_t *m, float dt)
{
    m->dt = dt;
    m->tau = 0.075f;   /* 典型 12V 减速电机约 75ms */
    m->kv = 1200.0f;   /* 满占空比 1200rpm */
    m->deadzone = 0.12f;
    m->in_min = -1.0f;
    m->in_max = 1.0f;
    m->max_speed = 1500.0f;
    m->speed = 0.0f;
    m->load = 0.0f;
}

float motor_sim_update(motor_sim_t *m, float input)
{
    float u;
    float target;

    if (input > m->in_max) {
        input = m->in_max;
    } else if (input < m->in_min) {
        input = m->in_min;
    }

    /* 死区：小占空比不足以克服静摩擦 */
    if (input > 0.0f && input < m->deadzone) {
        u = 0.0f;
    } else if (input < 0.0f && input > -m->deadzone) {
        u = 0.0f;
    } else {
        u = input;
    }

    target = m->kv * u;
    /* 一阶惯性离散化：w(k+1) = w(k) + (target - w(k)) * dt/tau */
    m->speed += (target - m->speed) * (m->dt / m->tau);
    m->speed -= m->load * (m->dt / m->tau);

    if (m->speed > m->max_speed) {
        m->speed = m->max_speed;
    } else if (m->speed < -m->max_speed) {
        m->speed = -m->max_speed;
    }
    return m->speed;
}

`src/pid.c`

#include "pid.h"

#include <math.h>

void pid_init(pid_t *pid)
{
    if (pid == NULL) {
        return;
    }
    if (pid->ts <= 0.0f) {
        pid->ts = 0.001f; /* 默认 1kHz */
    }
    if (pid->i_limit <= 0.0f) {
        pid->i_limit = (pid->out_max - pid->out_min) * 0.5f;
    }
    if (pid->d_lpf_alpha <= 0.0f || pid->d_lpf_alpha > 1.0f) {
        pid->d_lpf_alpha = 1.0f;
    }
    pid->integral = 0.0f;
    pid->prev_measure = 0.0f;
    pid->out = 0.0f;
    pid->first = true;
    pid->enabled = true;
    pid->p_term = pid->i_term = pid->d_term = 0.0f;
    pid->saturate_cnt = 0;
}

void pid_reset_state(pid_t *pid)
{
    if (pid == NULL) {
        return;
    }
    pid->integral = 0.0f;
    pid->first = true;
}

void pid_set_enabled(pid_t *pid, bool enabled, float current_measure)
{
    if (pid == NULL) {
        return;
    }
    if (enabled && !pid->enabled) {
        /* 无扰切换：让积分项恰好补足当前输出与 P+D 的差值 */
        float err = 0.0f - current_measure; /* 目标值未知时按 0 处理，调用方按需修正 */
        float pd = pid->kp * err + 0.0f;
        pid->integral = pid->out - pd;
        if (pid->integral > pid->i_limit) {
            pid->integral = pid->i_limit;
        } else if (pid->integral < -pid->i_limit) {
            pid->integral = -pid->i_limit;
        }
    }
    pid->enabled = enabled;
}

static float clampf(float v, float lo, float hi)
{
    if (v > hi) {
        return hi;
    }
    if (v < lo) {
        return lo;
    }
    return v;
}

float pid_update(pid_t *pid, float target, float measure, float dt)
{
    float err;
    float p;
    float d;
    float raw;
    float out;
    float out_unsat;

    if (pid == NULL) {
        return 0.0f;
    }
    if (dt <= 0.0f) {
        dt = pid->ts;
    }

    err = target - measure;

    /* 首次调用没有历史值，微分项直接给 0，避免启动尖峰 */
    if (pid->first) {
        pid->prev_measure = measure;
        pid->first = false;
    }

    /* ---- P ---- */
    p = pid->kp * err;

    /* ---- D：微分先行，对测量值求导并取负 ---- */
    {
        float dm = measure - pid->prev_measure;
        float d_raw = -pid->kd * dm / dt;
        if (pid->d_lpf_alpha >= 1.0f) {
            d = d_raw;
        } else {
            /* 一阶低通，抑制编码器量化噪声 */
            d = pid->d_lpf_alpha * d_raw +
                (1.0f - pid->d_lpf_alpha) * pid->d_term;
        }
    }

    /* ---- I：先试探性累加，若最终输出饱和则回退（条件积分） ---- */
    pid->integral += pid->ki * err * dt;
    if (pid->integral > pid->i_limit) {
        pid->integral = pid->i_limit;
    } else if (pid->integral < -pid->i_limit) {
        pid->integral = -pid->i_limit;
    }

    raw = p + pid->integral + d;
    out_unsat = raw;

    /* ---- 输出限幅 ---- */
    out = clampf(raw, pid->out_min, pid->out_max);
    if (out != raw) {
        pid->saturate_cnt++;
        /* 条件积分：饱和且误差方向与饱和方向一致时，撤销这次积分 */
        if ((raw > pid->out_max && err > 0.0f) || (raw < pid->out_min && err < 0.0f)) {
            pid->integral -= pid->ki * err * dt;
            if (pid->integral > pid->i_limit) {
                pid->integral = pid->i_limit;
            } else if (pid->integral < -pid->i_limit) {
                pid->integral = -pid->i_limit;
            }
            raw = p + pid->integral + d;
            out = clampf(raw, pid->out_min, pid->out_max);
        }
    }

    /* ---- 输出变化率限制 ---- */
    if (pid->slew_rate > 0.0f) {
        float max_step = pid->slew_rate * dt;
        if (out - pid->out > max_step) {
            out = pid->out + max_step;
        } else if (pid->out - out > max_step) {
            out = pid->out - max_step;
        }
    }

    pid->prev_measure = measure;
    pid->p_term = p;
    pid->i_term = pid->integral;
    pid->d_term = d;
    pid->out = out;
    (void)out_unsat;
    return out;
}

`test/test_pid.c`

/**
 * 主机端仿真测试：在电脑上把 PID 参数调好
 * gcc -std=c99 -Wall -Wextra -Iinclude src/pid.c src/motor_sim.c test/test_pid.c -o build/test -lm
 */
#include <math.h>
#include <stdio.h>

#include "motor_sim.h"
#include "pid.h"

typedef struct {
    float rise_time;   /* 上升到 90% 的时间 */
    float overshoot;   /* 超调百分比 */
    float sse;         /* 稳态误差绝对值 */
    float settle_time; /* 进入 ±2% 并保持的时间 */
} step_result_t;

static void make_pid(pid_t *p, float kp, float ki, float kd, float ts)
{
    p->kp = kp;
    p->ki = ki;
    p->kd = kd;
    p->ts = ts;
    p->out_min = -1.0f;
    p->out_max = 1.0f;
    p->i_limit = 0.0f;      /* 0 -> 自动取 (max-min)/2 */
    p->d_lpf_alpha = 1.0f;
    p->slew_rate = 0.0f;
    pid_init(p);
}

static step_result_t run_step(float kp, float ki, float kd, float ts,
                              float seconds, int trace)
{
    pid_t pid;
    motor_sim_t m;
    step_result_t r = {0.0f, 0.0f, 0.0f, 0.0f};
    const float target = 600.0f;
    int steps = (int)(seconds / ts);
    int n90 = -1;
    int settle = -1;
    float peak = 0.0f;

    make_pid(&pid, kp, ki, kd, ts);
    motor_sim_init(&m, ts);

    if (trace) {
        printf("   t(ms)   target   speed(rpm)      out      P        I        D\n");
    }
    for (int i = 0; i < steps; i++) {
        float out = pid_update(&pid, target, m.speed, ts);
        motor_sim_update(&m, out);
        float t = (float)(i + 1) * ts;

        if (m.speed > peak) {
            peak = m.speed;
        }
        if (n90 < 0 && m.speed >= 0.9f * target) {
            n90 = i;
            r.rise_time = t;
        }
        if (fabsf(m.speed - target) > 0.02f * target) {
            settle = -1;
        } else if (settle < 0) {
            settle = i;
            r.settle_time = t;
        }
        if (trace && (i % (int)(steps / 12 > 0 ? steps / 12 : 1) == 0)) {
            printf("%7.0f %8.0f %11.1f %8.3f %7.3f %8.3f %8.3f\n",
                   t * 1000.0f, target, m.speed, out,
                   pid.p_term, pid.i_term, pid.d_term);
        }
    }
    r.overshoot = (peak - target) / target * 100.0f;
    if (r.overshoot < 0.0f) {
        r.overshoot = 0.0f;
    }
    r.sse = fabsf(target - m.speed);
    return r;
}

static void print_row(const char *name, step_result_t r)
{
    printf("  %-14s rise=%6.1fms  overshoot=%6.2f%%  settle=%6.1fms  sse=%7.3f rpm\n",
           name, r.rise_time * 1000.0f, r.overshoot, r.settle_time * 1000.0f, r.sse);
}

int main(void)
{
    pid_t pid;
    motor_sim_t m;

    printf("===== PID + 直流电机仿真（目标 600 rpm，Ts=1ms）=====\n\n");

    printf("[1] 纯比例控制 Kp=0.0008（有稳态误差）\n");
    print_row("P only", run_step(0.0008f, 0.0f, 0.0f, 0.001f, 1.5f, 0));

    printf("\n[2] 加积分 Kp=0.0008 Ki=0.30（消除静差）\n");
    print_row("PI", run_step(0.0008f, 0.30f, 0.0f, 0.001f, 1.5f, 0));

    printf("\n[3] 加微分 Kp=0.0008 Ki=0.30 Kd=0.00002（抑制超调）\n");
    print_row("PID", run_step(0.0008f, 0.30f, 0.00002f, 0.001f, 1.5f, 0));

    printf("\n[4] 完整 PID 的阶跃响应曲线\n");
    print_row("PID step", run_step(0.0008f, 0.30f, 0.00002f, 0.001f, 1.2f, 1));

    printf("\n[5] 抗积分饱和验证：前 0.4s 电机被堵转（测速恒为 0）\n");
    {
        const float target = 600.0f;
        float ts = 0.001f;
        int steps = 1200;
        float peak = 0.0f;
        float out_peak = 0.0f;
        make_pid(&pid, 0.0008f, 0.30f, 0.00002f, ts);
        motor_sim_init(&m, ts);
        for (int i = 0; i < steps; i++) {
            float measure = (i < 400) ? 0.0f : m.speed; /* 0.4s 前堵转 */
            float out = pid_update(&pid, target, measure, ts);
            if (i >= 400) {
                motor_sim_update(&m, out);
            }
            if (fabsf(out) > out_peak) {
                out_peak = fabsf(out);
            }
            if (m.speed > peak) {
                peak = m.speed;
            }
        }
        printf("  堵转期间积分项上限 = %.3f（不会无限累加）\n",
               pid.i_limit);
        printf("  堵转解除后最大超调 %.2f%%，输出峰值 %.3f（限幅生效）\n",
               (peak - target) / target * 100.0f, out_peak);
        printf("  饱和计数 saturate_cnt = %u\n", pid.saturate_cnt);
    }

    printf("\n[6] 微分先行 vs 教科书写法：目标值从 600 跳到 900\n");
    {
        make_pid(&pid, 0.0008f, 0.30f, 0.00002f, 0.001f);
        motor_sim_init(&m, 0.001f);
        float worst = 0.0f;
        for (int i = 0; i < 200; i++) {
            float target = (i < 100) ? 600.0f : 900.0f;
            float out = pid_update(&pid, target, m.speed, 0.001f);
            motor_sim_update(&m, out);
            if (i == 100) {
                printf("  目标跳变瞬间：输出 %.3f，微分项 %.6f，测量值 %.1f rpm\n",
                       out, pid.d_term, m.speed);
                worst = fabsf(pid.d_term);
            }
        }
        printf("  微分先行后，目标跳变不会产生微分冲击（|D| = %.6f 保持不变量级）\n", worst);
    }

    printf("\n===== 完成：以上数据全部由本机 gcc 实编译、实运行产生 =====\n");
    return 0;
}

实测输出

下面这段输出是把上面的核心算法用 本机 gcc 真编译、真运行得到的（不含任何硬件依赖）：

===== PID + 直流电机仿真（目标 600 rpm，Ts=1ms）=====

[1] 纯比例控制 Kp=0.0008（有稳态误差）
  P only         rise=   0.0ms  overshoot=  0.00%  settle=   0.0ms  sse=306.123 rpm

[2] 加积分 Kp=0.0008 Ki=0.30（消除静差）
  PI             rise=  48.0ms  overshoot= 14.24%  settle= 220.0ms  sse=  0.000 rpm

[3] 加微分 Kp=0.0008 Ki=0.30 Kd=0.00002（抑制超调）
  PID            rise=  51.0ms  overshoot= 13.16%  settle= 253.0ms  sse=  0.000 rpm

[4] 完整 PID 的阶跃响应曲线
   t(ms)   target   speed(rpm)      out      P        I        D
      1      600        10.6    0.660   0.480    0.180   -0.000
    101      600       639.6    0.315  -0.035    0.281    0.069
    201      600       619.3    0.457  -0.016    0.455    0.018
    301      600       607.9    0.494  -0.007    0.497    0.004
    401      600       602.9    0.501  -0.002    0.504    0.000
    501      600       601.0    0.502  -0.001    0.503   -0.000
    601      600       600.3    0.501  -0.000    0.501   -0.000
    701      600       600.1    0.500  -0.000    0.501   -0.000
    801      600       600.0    0.500  -0.000    0.500   -0.000
    901      600       600.0    0.500   0.000    0.500   -0.000
   1001      600       600.0    0.500   0.000    0.500   -0.000
   1101      600       600.0    0.500   0.000    0.500   -0.000
  PID step       rise=  51.0ms  overshoot= 13.16%  settle= 253.0ms  sse=  0.001 rpm

[5] 抗积分饱和验证：前 0.4s 电机被堵转（测速恒为 0）
  堵转期间积分项上限 = 1.000（不会无限累加）
  堵转解除后最大超调 13.16%，输出峰值 1.000（限幅生效）
  饱和计数 saturate_cnt = 420

[6] 微分先行 vs 教科书写法：目标值从 600 跳到 900
  目标跳变瞬间：输出 0.645，微分项 0.068885，测量值 644.9 rpm
  微分先行后，目标跳变不会产生微分冲击（|D| = 0.068885 保持不变量级）

===== 完成：以上数据全部由本机 gcc 实编译、实运行产生 =====

菜单

分享

PID 从公式到能跑：给直流电机写一个带抗积分饱和的速度环

一、完整工程下载

位置式 PID 完整公式

四件套怎么补

1. 微分先行（Derivative on Measurement）

2. 积分分离 / 抗饱和（Anti-windup）

3. 输出限幅与限斜率

4. 无扰切换（Bumpless Transfer）

参数怎么整定（实操顺序）

工程内容

调试要点

进阶方向

完整代码

`Makefile`

`include/motor_sim.h`

`include/pid.h`

`platformio.ini`

`src/motor_sim.c`

`src/pid.c`

`test/test_pid.c`

实测输出

评论

端到端测试

温度传感器不准？B 参数公式、Steinhart-Hart 与多点标定的误差实测

步进电机为什么会“丢步”：梯形与 S 曲线加减速的完整实现

裸机也该有“操作系统”：80 行写一个软定时器任务调度器

给单片机写个能用的命令行：行编辑、历史、Tab 补全、ANSI 转义全都要

PID 从公式到能跑：给直流电机写一个带抗积分饱和的速度环

手写 Modbus RTU 从站：03/06/16 功能码 + 异常响应，用测试用例逐字节验证

固定块内存池：为什么嵌入式该用它替掉 malloc（附真实碎片实测）

现场升级不翻车：手写一个带 A/B 双分区与自动回滚的串口 IAP

软件 I2C 实战：开漏总线、时钟延展、扫描与 9 时钟解锁