📚 课程目录第2章控制算法基础回顾

控制算法基础回顾：PID控制原理、状态机设计、滤波器基础

做嵌入式控制，说白了就是三件事：算准、切对、滤干净。算准靠PID，切对靠状态机，滤干净靠滤波器。这三样东西，我几乎在每个项目里都会碰到。今天咱们就把它们掰开揉碎了聊一遍。

PID控制原理：从公式到代码

PID控制，比例-积分-微分。公式很简单：

u(t) = Kp * e(t) + Ki * ∫e(τ)dτ + Kd * de(t)/dt

但实际写代码的时候，坑不少。我习惯用位置式PID，因为它直观，调参时心里有数。

位置式PID实现（C语言）

typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
    float limit;
} PID_Controller;

float PID_Update(PID_Controller *pid, float setpoint, float measurement) {
    float error = setpoint - measurement;
    pid->integral += error;
    // 积分限幅，防止积分饱和
    if (pid->integral > pid->limit) pid->integral = pid->limit;
    if (pid->integral < -pid->limit) pid->integral = -pid->limit;

    float derivative = error - pid->prev_error;
    pid->prev_error = error;

    float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
    // 输出限幅
    if (output > pid->limit) output = pid->limit;
    if (output < -pid->limit) output = -pid->limit;

    return output;
}

这里有个细节：积分限幅。我在做电机控制时遇到过，积分项一直累加，输出直接饱和，电机抖得像筛子。加上限幅后，问题就解决了。

调参口诀：先调Kp让系统响应快，再加Kd抑制超调，最后加Ki消除静差。别一上来三个参数一起调，容易乱。

状态机设计：让逻辑清晰可维护

状态机是嵌入式系统的骨架。我见过太多人用一堆if-else嵌套，最后自己都看不懂。状态机的好处是：每个时刻系统只在一个状态里，逻辑是确定的。

我常用的状态机结构：

typedef enum {
    STATE_IDLE,
    STATE_RUNNING,
    STATE_ERROR,
    STATE_CALIBRATE
} SystemState;

typedef struct {
    SystemState current_state;
    void (*entry)(void);
    void (*do_action)(void);
    void (*exit)(void);
} StateMachine;

每个状态有进入、执行、退出三个动作。这样设计，调试时只要看当前状态，就知道系统在干什么。

避坑指南：我曾经在一个项目中，状态切换时忘了调用exit函数，导致资源没释放，系统跑着跑着就崩了。记住：状态切换必须完整执行entry和exit。

状态机设计的关键点：

状态数量控制在5-7个，太多会复杂
每个状态只做一件事，单一职责
状态切换条件要明确，避免模糊边界
加一个默认状态，处理异常情况

滤波器基础：从硬件到软件

传感器数据，没有不脏的。滤波器就是用来把脏数据洗干净。我常用的几种：

滤波器类型	适用场景	优点	缺点
一阶低通滤波	温度、压力等慢变信号	实现简单，计算量小	有相位延迟
滑动平均滤波	ADC采样值	平滑效果好	需要缓存，响应慢
中值滤波	去除脉冲噪声	抗干扰能力强	排序耗时
卡尔曼滤波	多传感器融合	最优估计	实现复杂，需要模型

一阶低通滤波的代码最简单：

float LowPassFilter(float input, float prev_output, float alpha) {
    // alpha = T / (T + RC)，T是采样周期
    return alpha * input + (1 - alpha) * prev_output;
}

alpha值怎么选？我一般从0.1开始试。alpha越大，滤波越弱，响应越快；alpha越小，滤波越强，但延迟也大。

实战经验：做无人机姿态解算时，加速度计数据噪声大，我用了一阶低通滤波，alpha=0.15。效果不错，但发现延迟导致控制滞后。后来换成互补滤波，把加速度计和陀螺仪数据融合，延迟问题就解决了。

三者如何协同工作

PID、状态机、滤波器不是孤立的。它们在实际系统中是这样配合的：

传感器数据 → 滤波器去噪
滤波后数据 → 状态机判断当前状态
状态机输出 → PID控制器计算控制量
PID输出 → 执行器动作

这个流程，我几乎在每个控制项目里都用。你想想看，如果传感器数据不滤波，状态机就会误判；状态机误判，PID就会输出错误控制量。一环扣一环，哪个都不能省。

这个图，我建议你保存下来。每次调试系统时，对照着看，就知道问题出在哪一环。

调试顺序：先检查滤波器输出是否干净，再验证状态机切换是否正确，最后调PID参数。别跳步，跳步容易走弯路。

嗯，以上就是控制算法的基础。这些东西看着简单，但真正用好，需要大量实践。我做了十几年嵌入式，每次遇到新问题，还是会回到这三个基础上来思考。你想想看，是不是这个道理？