1. 把 PID 做成一个功能库:为什么不再每个工程抄一遍代码
PID 控制写第一版只要二十分钟,难的是第二台车、第三个工程还要不要重抄一遍。我最初就是把 PID_Calc 连同几个全局变量直接贴进 main.c,换一块板子、换一路舵机,就得复制、改名、再改一遍限幅——两个工程之后,代码和参数已经完全对不上,调参时连自己都不确定当前跑的是哪一版。
后来做法变了:把它收成一个独立的功能库。结构体是句柄,对外只暴露一个计算入口,参数、限幅、内部状态全放在句柄里,库本身不碰 IO、不碰全局变量。这套做法在一颗 STC32F12K 上跑电磁车的舵机与电机控制,接口足够通用,换平台照样能搬。下面从字段设计、接入步骤,一路写到参数整定和现场踩坑。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 功能库的骨架:PID_TypeDef 里该放什么、不该放什么
功能库好不好用,八成就看结构体这一层。字段放多了,调用方要维护一堆它根本不该关心的东西;放少了,限幅、抗饱和这些必须留痕的状态又没地方存。我的划分标准很简单:凡是"每个控制周期都可能变"的,留在句柄里由库自己写;凡是"上电配一次就不动"的,初始化时灌进去;凡是"跟具体执行器有关"的,绝不写死在库内部。
2.1 三堆字段:参数、状态、约束
库里的句柄就长这样,字段一个不多一个不少:
c复制typedef struct {
float target; /* 目标值,舵机控制里通常是 0,即赛道中心 */
float current; /* 当前值,即计算出的位置偏差 g_fPositionError */
float err; /* 当前误差 */
float err_last; /* 上一次误差 */
float err_sum; /* 误差积分项 */
float Kp, Ki, Kd; /* PID 参数 */
float output; /* 输出值,舵机角度或电机 PWM 增量 */
float output_max; /* 输出限幅上限 */
float output_min; /* 输出限幅下限 */
float integral_max; /* 积分限幅,防止积分饱和 */
} PID_TypeDef;
按职责拆开看,就是三堆:
| 分组 | 字段 | 由谁赋值 | 写错之后的现场表现 |
|---|---|---|---|
| 目标与当前 | target、current |
调用方,每个控制周期 | 车往反方向打舵,越偏越往外 |
| 增益 | Kp、Ki、Kd |
初始化时一次 | 直道摆振,或者入弯反应迟钝 |
| 输出约束 | output_max、output_min |
初始化时按执行器行程标定 | 舵机顶到机械限位,齿轮嗡嗡响 |
| 积分约束 | integral_max |
初始化时按能忍受的恢复时间估 | 冲出赛道后回中要好几秒 |
| 内部状态 | err、err_last、err_sum、output |
只由 PID_Calc 写 |
上电第一拍输出打满 |
这张表最有用的一列是最后一列。它逼着我在设计阶段就想清楚:某个字段如果没人负责,或者两个人都想写,现场一定出怪事。err_sum 尤其典型——它只能由库自己累加和限幅,调用方一旦手动去改,抗饱和就形同虚设。
至于那些"看起来该放进去"的东西,我一般不加:采样周期、传感器量程、舵机中位角度、电机死区。它们要么属于调用点的换算,要么属于执行器的标定表,塞进 PID 句柄只会让这个库换一个平台就搬不动。库的边界干净,才谈得上复用。
2.2 唯一入口 PID_Calc:误差从哪来、输出往哪去
对外只留一个函数,输入是句柄指针,输出是限幅后的控制量:
c复制float PID_Calc(PID_TypeDef *pid)
{
float derivative;
pid->err = pid->target - pid->current; /* 目标和当前都统一成"偏差量" */
/* 比例项 */
pid->output = pid->Kp * pid->err;
/* 积分项:先累加,立刻限幅,最后才乘 Ki */
pid->err_sum += pid->err;
if (pid->err_sum > pid->integral_max) pid->err_sum = pid->integral_max;
if (pid->err_sum < -pid->integral_max) pid->err_sum = -pid->integral_max;
pid->output += pid->Ki * pid->err_sum;
/* 微分项:对误差的变化量做差分 */
derivative = pid->err - pid->err_last;
pid->output += pid->Kd * derivative;
pid->err_last = pid->err;
/* 总输出限幅 */
if (pid->output > pid->output_max) pid->output = pid->output_max;
if (pid->output < pid->output_min) pid->output = pid->output_min;
return pid->output;
}
这段代码里有四个顺序细节,改任何一个都会在现场体现出来:
err = target - current,符号方向在这里一次定死。后面所有项都基于这个err,调用方不许再翻一次符号,否则 P 和 I 会互相打架。err_sum是"先累加、马上限幅、再乘Ki"。限的是积分量本身,不是Ki * err_sum。这两者看着差不多,实际上差很远:限在后一步,积分量会在后台悄悄长到很大,输出早就饱和了,等误差反号还得慢慢往回拉。derivative用的是相邻两拍的误差差值,err_last在算完微分之后才更新。顺序反了,微分项恒为 0,你会以为自己加了个假 D。- 总输出限幅放在最后,两道闸各管各的。
integral_max管"积分项能攒多大",output_max/output_min管"最终能打多远",缺一道都不行。
还有一个容易被忽略的点:这个函数不读传感器、不写 PWM、不依赖任何硬件头文件。它只认 current 这一个浮点数。正因为如此,我才能在 PC 上把它单独编出来回放验证,这事放到最后一章讲。
2.3 从全局变量到句柄:两个环怎么共用一套代码
原始写法里是 PID_TypeDef g_pidServo; 一个全局结构体,舵机环用,函数直接操作它。单个环没问题,一旦要加转向环、速度环、差速环,就得复制三份结构体和三份函数,或者改成 PID_Calc_Servo()、PID_Calc_Motor() 这种一眼看出敷衍的命名。
改成句柄之后,一个库文件管所有环:
c复制PID_TypeDef g_pidServo; /* 舵机环:current 喂位置偏差 */
PID_TypeDef g_pidMotor; /* 速度环:current 喂编码器算出的速度偏差 */
g_pidMotor.current = g_fSpeedError; /* 各自更新自己的 current */
g_nPwmDelta = PID_Calc(&g_pidMotor); /* 同一份代码,不同句柄 */
这样做的直接好处是调参时不会串——舵机的 Kp 和速度环的 Kp 各自躺在自己的句柄里,改一个不会顺手改坏另一个。间接好处是测试可以脱离硬件:给 g_pidMotor 灌一段速度偏差序列,就能在没接电机的情况下验证限幅和收敛。
代价是多了一层指针传递,以及每个实例必须显式初始化。全局变量靠 BSS 段隐式清零,改成局部变量或者动态分配之后就不成立了,这一点在第 5 章会单独说——它是上电第一拍打满舵的常见原因。
3. 功能库接入实战:三步把 PID_Calc 挂进控制周期
库写好了不等于能用。我见过太多情况是代码本身没问题,接入的地方错了:偏差单位没统一、调用周期不对、输出没做中位合成。这一章按我实际搬库的顺序走一遍,每一步都给可执行的东西。
3.1 第一步:库文件落位,头文件只暴露两样东西
目录上我习惯把库单独放一层,别和业务代码混在一个文件夹里,不然过两个月自己也分不清哪个文件是"能搬走"的:
text复制project/
├── lib/pid/
│ ├── pid.h # 句柄类型 + 函数原型,不含任何硬件头文件
│ └── pid.c # PID_Init / PID_Calc 实现
├── app/
│ ├── control.c # 控制周期,负责喂 current、接 output
│ └── main.c # 初始化、外设配置
└── ...
pid.h 里只放两样东西,句柄类型和两个函数原型:
c复制#ifndef PID_H
#define PID_H
typedef struct { /* 见 2.1,字段原样搬过来 */ } PID_TypeDef;
void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd,
float out_max, float out_min, float integral_max);
float PID_Calc(PID_TypeDef *pid);
#endif
这里刻意不 #include 任何芯片头文件。一旦头文件里出现寄存器定义或者厂商 SDK,这个库就再也搬不到第二块板上;而它本来只需要 float 和 if 两样东西。编译上也没什么特别的,把 pid.c 加进工程编译列表即可,不需要额外的库依赖。
提示:如果工程开了浮点优化或者
-ffast-math,先把pid.c单独排除掉。这类优化会重排浮点运算顺序,限幅比较的结果可能和你手算的不一致,排查时非常费神。
3.2 第二步:初始化,先限幅后增益
材料里的原始代码没有初始化函数,参数是在定义处直接赋值的。搬成库之后,我一般补一个 PID_Init,把"必须显式清零"这件事固定下来:
c复制void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd,
float out_max, float out_min, float integral_max)
{
/* 状态全部显式清零,不依赖 BSS 段的隐式初始化 */
pid->target = 0.0f;
pid->current = 0.0f;
pid->err = 0.0f;
pid->err_last = 0.0f;
pid->err_sum = 0.0f;
pid->output = 0.0f;
/* 增益 */
pid->Kp = kp;
pid->Ki = ki;
pid->Kd = kd;
/* 约束:先标定执行器行程,再谈增益 */
pid->output_max = out_max;
pid->output_min = out_min;
pid->integral_max = integral_max;
}
调用点上,我坚持"先限幅后增益"的顺序:
c复制/* 舵机环:输出是舵机角度增量,限幅按舵机左右行程标定 */
PID_Init(&g_pidServo, KP_SERVO, KI_SERVO, KD_SERVO,
SERVO_OUT_MAX, SERVO_OUT_MIN, SERVO_INTEGRAL_MAX);
/* 速度环:输出是 PWM 增量,限幅按占空比上下限标定 */
PID_Init(&g_pidMotor, KP_MOTOR, KI_MOTOR, KD_MOTOR,
PWM_OUT_MAX, PWM_OUT_MIN, MOTOR_INTEGRAL_MAX);
为什么顺序不能反?因为增益的"手感"是相对限幅而言的。output_max 还没定,Kp 调到多大都说不清是"够"还是"过"——同一个 Kp,限幅宽一点显得软,限幅窄一点显得猛。先把执行器能接受的行程卡死,再往里填增益,调参才有参照。这些宏的具体数值跟车重、舵机型号、齿比都有关,没法给通用值,标定方法放在下一章。
3.3 第三步:在控制周期里调用,喂偏差不喂原始值
真正的调用只有两行,但这两行里的坑比整个库都多:
c复制/* 控制周期中断里执行:先更新 current,再算输出 */
g_pidServo.current = g_fPositionError; /* 偏差量,不是传感器原始值 */
g_nServoAngle = PID_Calc(&g_pidServo); /* 算出的控制量接舵机 */
第一条的措辞值得较真:current 要的是"偏差量",不是电感采样值、不是归一化后的位置、更不是 ADC 原始码值。原始值要先在业务层换算成"偏离赛道中心多少",单位、正负号都统一了,再交给库。如果把原始值直接塞进去,target = 0 这个约定立刻失效,表现就是车越偏越往外打——方向反了,而且量纲不对,Kp 怎么调都不对。
第二条要注意调用的时机。PID_Calc 会改写 err、err_sum、err_last、output 四个状态字段,它不是纯函数,不能在一个控制周期里被调用两次,也不能在主循环和中断里同时调用同一个句柄。我一般把调用点锁死在一个固定周期的定时器中断里,主循环只读 output,不碰句柄。
c复制/* 主循环里只消费结果,不再调 PID_Calc */
servo_set_angle(SERVO_MID + (int16_t)g_nServoAngle);
3.4 收尾一步:输出怎么接到执行器
PID_Calc 返回的是"控制量",不是最终角度或最终占空比。舵机这边要叠中位:
c复制/* 中位 + 控制量,再夹一次机械行程,双重保护 */
int16_t angle = SERVO_MID + (int16_t)g_nServoAngle;
if (angle > SERVO_MECH_MAX) angle = SERVO_MECH_MAX;
if (angle < SERVO_MECH_MIN) angle = SERVO_MECH_MIN;
servo_set_angle(angle);
电机这边通常是增量式用法,把返回值当 PWM 的增量叠到当前占空比上,再整体限幅。注意库里的 output_max / output_min 限的是"这个增量",不是"最终占空比",两层限幅的含义别混。机械行程这道夹取建议保留:库的限幅是按参数算的,参数标错了还有机械限位兜底,总比烧舵机强。
到这一步,链路就跑通了:初始化一次、每个控制周期喂一次偏差、拿回一个限幅后的控制量、叠到执行器上。剩下的全是参数问题。
4. 参数怎么设:Kp、Ki、Kd 与三个限幅的落地顺序
调参这件事,网上教程大多从"先加 P"讲起,但我在车上真正翻车的地方几乎都不在增益上,而在限幅和调用周期。所以我的顺序是:先把三个限幅标出来,再动 Kp,最后才考虑 I 和 D。
4.1 先把限幅标出来,再谈增益
三个限幅各有各的标定依据,不能拍脑袋:
| 参数 | 怎么定 | 定大了 | 定小了 |
|---|---|---|---|
output_max / output_min |
舵机从 tim 中位打到机械极限所需的控制量,留一成余量 | 库的限幅失效,靠机械限位兜底,舵机容易堵转发热 | 打舵打不满,急弯明显不够用 |
integral_max |
按"能忍受的恢复时间"估:允许的最大持续偏差 × 期望恢复的控制周期数 | 积分饱和,冲出赛道后回中慢、二次过冲 | 稳态误差压不下去,直道长期偏一侧 |
| 调用周期 | 定时器中断周期,全工程统一 | 响应慢,入弯迟 | 噪声被放大,舵机高频抖 |
integral_max 是最容易被忽略的一个。很多人只设 output_max,觉得输出都限住了还能出什么事——但积分项是在限幅之前累加的,输出饱和期间它照样在长,这就是第 5 章第一条要讲的现象。
标定 output_max 有个笨但可靠的办法:把车架起来,手动从中位往一个方向给控制量,记录舵机打到机械极限时的数值,取九成作为上限,反方向同理取 output_min(注意符号,一般是负值)。这样标出来的值跟具体舵机、连杆长度绑定,换车就得重标,不能照抄。
4.2 只用 Kp:把车调到"能跑但不摆"
Ki 和 Kd 都置零,只留 Kp,从很小的值开始往上加。判断标准不是"看起来跟得紧",而是这三条:
- 直道上车能基本保持居中,允许小幅左右修正;
- 手动把车推偏一点,松手后能自己回到中间,不回中就是
Kp偏小; - 回中过程不带明显过冲和来回摆,摆起来就是
Kp偏大。
这里有个反直觉的地方:Kp 加到"反应很快"往往已经过头了。因为位置偏差本身带噪声,Kp 越大,噪声进舵机的比例越高,直道上会看到舵机小幅高频地抖。这条边界要靠第 5 章的滤波手段往后推,而不是继续降 Kp——降下去入弯就跟不上了。
4.3 Ki 什么时候加、加多少看什么
Ki 的存在意义是消掉稳态误差:直道上车长期偏一侧,Kp 再大也拉不回来,因为偏差小到产生的比例输出不够克服机械中位偏差。这时候加一点 Ki,让积分项慢慢把这点偏置补上。
加 Ki 的判断依据是"稳态",不是"瞬态"。具体做法:让车在直道上跑一段,观察舵机角度是否稳定收敛到某个非中位值并且不再变化——如果是,说明有稳态误差,可以加 Ki;如果车在直道上本来就左右摆,加 Ki 只会让摆幅变大、周期变长,那是 Kp 没调好,先退回去。
Ki 一般从很小的值起步,每加一次都要重新观察"冲出赛道后回中"这个过程。回中太慢是 Ki 偏小或 integral_max 偏大,回中带过冲是 Ki 偏大。这两个现象要分开看,别混在一起调。
4.4 Kd 与采样噪声:先过滤再微分
材料里的注释写得很实在:微分项"常用不完全微分或微分先行"。原因就是 derivative = err - err_last 这个差分对噪声极其敏感——控制周期越短,相邻两拍的误差差值里噪声占比越大,Kd 一加上去,等于把噪声放大后直接塞进舵机。
我的处理顺序是:先确认 Kp、Ki 已经稳定,再判断需不需要 D。如果入弯时车头"甩"得过冲,加一点 Kd 能压住;如果直道上本来就有高频抖动,加 Kd 只会更糟,这时候要么先给偏差做一阶低通,要么改微分先行(对 current 差分而不是对 err 差分),把设定值突变从微分项里摘出去。
还有一个容易被忽视的耦合:改了控制周期,Kp 基本不受影响,但 Ki 和 Kd 的等效强度会跟着变。err_sum 是"按调用次数累加"的,周期缩短一半,同样的 Ki 积分速度快一倍。这条留到第 5 章细说。
5. 功能库避坑排查:五个现场翻车点
下面五条都是这套库在实际车上出过的状况,每条按"看到什么、为什么、怎么改"写。它们的共同点是:代码逻辑本身没错,错在接入方式或者参数约束。
5.1 现象:冲出赛道后回中要好几秒 —— 积分饱和
现象:车被推出赛道,扶回直道后舵机仍保持一个明显偏角,要跑出七八米才慢慢回中,紧接着入弯就是一次过冲。
原因:出赛道那段时间偏差持续同号,err_sum 一路累加到 integral_max 的天花板,而 integral_max 是按直道手感估的,远大于脱困需要的量。输出早就顶在 output_max 上了,积分还在后台攒,等误差反号要靠 Ki * err_sum 一点点把它拉回来。
解决:第一步收紧 integral_max,按"允许的最大持续偏差 × 能忍受的恢复周期数"重估;第二步在库里加条件积分——输出已经顶到 output_max 的那几拍不再累加 err_sum。工业控制器里这叫抗饱和,写进 PID_Calc 也就多一个判断:
c复制/* 只有输出未饱和时才累加积分,防止饱和期间积分继续上涨 */
if (pid->output < pid->output_max && pid->output > pid->output_min) {
pid->err_sum += pid->err;
}
注意这个判断放在计算比例项之后、积分项之前,顺序错了等于没加。
5.2 现象:舵机高频抖 —— 微分放大噪声
现象:直道上舵机持续小幅高频摆动,能听到明显的嗡嗡声,Kp 稍微加一点抖得更厉害,Kd 一加直接失控。
原因:derivative = err - err_last 对相邻两拍的差值做差分,控制周期越短,噪声在差值里的占比越高。位置偏差来自采样,本身带抖动,Kd 把它按倍数放大后塞进舵机。
解决:三条路,按代价从低到高。先给进库之前的偏差做一阶低通或几次滑动平均,成本最低;不行就改微分先行,把差分对象从 err 换成 current,设定值突变不会冲进微分项;再不行就上不完全微分,给微分项本身串一个低通。改完之后 Kd 需要重新从小往大调,原来那个值已经不适用了。
5.3 现象:上电第一拍输出打满 —— 状态没清零
现象:上电瞬间舵机猛地打向一侧再回中,或者第一次进弯方向判断正常但幅度明显过大。
原因:从单个全局 g_pidServo 改成多实例之后,句柄放在局部变量或者动态分配的内存里,err_sum、err_last、output 是随机值。PID_Calc 第一拍就把这些垃圾值带进积分项和微分项,输出自然离谱。全局变量时代这个问题被 BSS 段的隐式清零掩盖了,一改分配方式就暴露。
解决:所有实例一律走 PID_Init,把 target、current、err、err_last、err_sum、output 显式写零,不依赖任何隐式初始化。这条要写进接入规范里,光靠记性迟早漏。
5.4 现象:限幅设了却还在累积 —— 顺序和位置写反
现象:output_max 明明设了,车还是"越纠越猛",松手后要等很久才恢复,看起来限幅完全没生效。
原因:两种写法都会这样。一是只限总输出、不限 err_sum,积分在后台继续长;二是积分限幅写在了乘法之后,限的其实是 Ki * err_sum 这个输出量,积分量本身没被卡住。两种都属于"闸装错了位置"。
解决:严格按库里的顺序——先 err_sum += err,立刻对 err_sum 限幅,再乘 Ki,最后限总输出。两道闸各管一段,不能合成一道。改代码时建议在注释里把这三步的先后写清楚,这地方最容易在重构时被顺手合并掉。
5.5 现象:换了中断周期,参数全变味 —— 积分跟的是次数不是时间
现象:把 PID_Calc 的调用周期从 5 ms 改到 2 ms,硬件一点没动,原来的 Kp、Ki 直接震荡,直道摆得厉害。
原因:err_sum 是"按调用次数累加",Ki 的物理含义其实是"每次调用增加多少输出",跟时间无关。周期缩短一倍,等效积分速度翻倍;Kd 那边同理,差分步长变了,微分强度也跟着变。只有 Kp 不受周期影响。
解决:改周期必须重调 Ki、Kd,没有捷径。想少折腾就把积分写成 Ki * err * dt、微分写成 Kd * (err - err_last) / dt 的形式,让参数和时间挂钩,换周期只需改 dt 一个宏。不管用哪种,都要在库的头文件里写死一句注释:本库按固定周期调用,当前周期是多少——半年后回来改的人(往往就是自己)会感谢这句话。
6. 验证与进阶:把功能库从"能跑"逼到"敢改"
这套库最大的价值不是省了几行代码,而是它不碰硬件,所以可以在上车之前先验证。我现在的固定动作是离线回放:把实车跑出来的一段偏差序列存成数组或者 CSV,在 PC 上喂给 PID_Calc,看输出曲线。
c复制/* 离线回放:库不依赖硬件,直接在 PC 上编译运行 */
PID_TypeDef pid;
PID_Init(&pid, KP_SERVO, KI_SERVO, KD_SERVO,
SERVO_OUT_MAX, SERVO_OUT_MIN, SERVO_INTEGRAL_MAX);
for (int i = 0; i < seq_len; i++) {
pid.current = err_seq[i]; /* 录下来的偏差序列 */
float out = PID_Calc(&pid);
printf("%d %.4f %.4f\n", i, err_seq[i], out); /* 逐拍对照手算 */
}
看三件事:输出有没有越过 output_max / output_min;误差持续同号时 err_sum 是否停在 integral_max 上不再增长;误差反号后输出回到零附近需要多少拍。这三条对上,参数方向基本就对了,上车只是微调。
上车前我还会过一遍这张表,任何一项不通过就先别急着跑:
| 检查项 | 判据 | 不通过时先查什么 |
|---|---|---|
| 偏差单位与符号 | 车偏左时输出方向正确 | current 是不是喂了原始值 |
| 限幅有效性 | 离线回放输出不越界 | 限幅顺序是否被重构改掉 |
| 积分抗饱和 | 饱和期间 err_sum 不再上涨 |
条件积分的判断位置 |
| 多实例隔离 | 改舵机 Kp 不影响速度环 |
是否误用了全局句柄 |
| 调用周期 | 全工程只有一个调用点 | 主循环里是否又调了一次 |
进阶方向可以考虑不完全微分和微分先行,这两条材料里点到过,改法也不复杂:微分先行是把差分对象从 err 换成 current,不完全微分是给微分项再串一个一阶低通。但改之前一定先把上面这套回放跑通,否则你分不清是改坏了还是本来就没调好。从那以后我每次把库搬进新工程,都强制先跑一遍离线回放、再上车看现象,两次省下来的调试时间远比写这个回放脚本多。希望帮到你。
