3. 位置式PID实现：从公式到代码，再到抗积分饱和

好，咱们今天来啃一块硬骨头——位置式PID的实现。

说实话，很多新手一上来就写PID代码，结果飞控在天上乱晃，自己还不知道问题出在哪。我当年也踩过这个坑，调了三天三夜，最后发现是积分项在作怪。嗯，今天咱们就把这事彻底讲明白。

3.1 位置式PID公式推导

先看公式。位置式PID的输出，说白了就是三项加起来：

u(k) = Kp * e(k) + Ki * Σe(i) + Kd * [e(k) - e(k-1)]

这里：

• u(k)：当前时刻的控制输出（比如油门值、舵机角度）
• e(k)：当前时刻的误差（目标值 - 当前值）
• Σe(i)：从开始到现在所有误差的累加和
• Kp, Ki, Kd：比例、积分、微分系数

为什么叫“位置式”？因为它的输出直接对应执行机构的绝对位置。比如你控制一个舵机，输出值就是舵机应该转到的角度。这和增量式PID不一样，增量式输出的是“这次要比上次多转多少”。

3.2 C语言实现位置式PID

直接上代码。我个人习惯把PID参数和状态封装成一个结构体，这样清晰又好维护。

// pid.h
typedef struct {
    float Kp;           // 比例系数
    float Ki;           // 积分系数
    float Kd;           // 微分系数
    
    float target;       // 目标值
    float feedback;     // 反馈值（当前值）
    
    float error;        // 当前误差
    float last_error;   // 上一次误差
    float integral;     // 积分累加和
    
    float output;       // 输出值
    float output_limit; // 输出限幅（防止溢出）
} PID_TypeDef;

// pid.c
float PID_Calculate(PID_TypeDef *pid, float target, float feedback) {
    // 1. 计算误差
    pid->error = target - feedback;
    
    // 2. 积分累加
    pid->integral += pid->error;
    
    // 3. 计算微分
    float derivative = pid->error - pid->last_error;
    
    // 4. 位置式PID公式
    pid->output = pid->Kp * pid->error 
                + pid->Ki * pid->integral 
                + pid->Kd * derivative;
    
    // 5. 输出限幅
    if (pid->output > pid->output_limit) {
        pid->output = pid->output_limit;
    } else if (pid->output < -pid->output_limit) {
        pid->output = -pid->output_limit;
    }
    
    // 6. 更新状态
    pid->last_error = pid->error;
    
    return pid->output;
}

3.3 积分饱和问题

积分饱和，说白了就是积分项“撑爆了”。

你想想看，当系统长时间达不到目标值时（比如电机堵转、舵机卡死），误差一直存在，积分项就会一直累加。等障碍解除后，这个巨大的积分值会让输出瞬间冲到极限，导致系统严重超调，甚至震荡。

我在项目中遇到过最典型的情况：四旋翼落地时，桨叶被草缠住，积分项疯狂累加。等桨叶挣脱后，飞控直接给了一个满油门，飞机差点窜上天。嗯，从那以后我再也不敢忽视积分饱和了。

3.4 抗积分饱和方法

常用的方法有三种，我按推荐程度排序：

方法	原理	优点	缺点
积分限幅	给积分项设置最大值	实现简单	可能影响稳态精度
积分分离	误差大时停止积分	响应快，超调小	阈值难调
变积分系数	根据误差大小动态调整Ki	效果最好	实现复杂

我最常用的是积分限幅 + 积分分离的组合。 为什么？因为简单可靠，嵌入式系统最怕花里胡哨的算法。

来看改进后的代码：

float PID_Calculate_AntiWindup(PID_TypeDef *pid, float target, float feedback) {
    pid->error = target - feedback;
    
    // 积分分离：误差大于阈值时，不积分
    if (fabs(pid->error) < pid->integral_threshold) {
        pid->integral += pid->error;
        
        // 积分限幅
        if (pid->integral > pid->integral_limit) {
            pid->integral = pid->integral_limit;
        } else if (pid->integral < -pid->integral_limit) {
            pid->integral = -pid->integral_limit;
        }
    } else {
        // 误差太大，积分清零或保持
        pid->integral = 0;  // 也可以保持原值，看需求
    }
    
    float derivative = pid->error - pid->last_error;
    
    pid->output = pid->Kp * pid->error 
                + pid->Ki * pid->integral 
                + pid->Kd * derivative;
    
    // 输出限幅
    if (pid->output > pid->output_limit) {
        pid->output = pid->output_limit;
    } else if (pid->output < -pid->output_limit) {
        pid->output = -pid->output_limit;
    }
    
    pid->last_error = pid->error;
    return pid->output;
}

3.5 总结与建议

位置式PID，说白了就是“把过去、现在、未来的误差都算进去”。

• 比例项管现在，误差越大输出越大
• 积分项管过去，消除稳态误差
• 微分项管未来，抑制超调

但积分项是把双刃剑。用好了，系统稳如老狗；用不好，系统疯如野马。抗积分饱和不是可选项，而是必选项。

最后说一句：代码写完了，一定要在仿真环境里跑一跑。我见过太多人直接上真机，结果炸机了才回来改代码。嗯，别问我怎么知道的。