深入解析：用 C 语言实现三级级联 PID 控制器

示例代码如下

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

// PID Controller Structure
typedef struct {
    float kp;            // Proportional gain
    float ki;            // Integral gain
    float kd;            // Derivative gain
    float integral;      // Integral accumulator
    float prevError;     // Previous error for derivative calculation
    float outMax;        // Maximum output limit
    float outMin;        // Minimum output limit
} PIDController;

// Initialize a PID controller
void PID_Init(PIDController *c, float kp, float ki, float kd, float outMin, float outMax) {
    c->kp = kp;
    c->ki = ki;
    c->kd = kd;
    c->integral = 0.0f;
    c->prevError = 0.0f;
    c->outMin = outMin;
    c->outMax = outMax;
}

// PID update function: computes control output given error and dt
float PID_Update(PIDController *c, float error, float dt) {
    // Proportional term
    float Pout = c->kp * error;

    // Integral term with anti-windup via clamping
    c->integral += error * dt;
    float Iout = c->ki * c->integral;

    // Derivative term (band-limited differentiation could be added separately)
    float derivative = (error - c->prevError) / dt;
    float Dout = c->kd * derivative;
    c->prevError = error;

    // Compute total output
    float output = Pout + Iout + Dout;

    // Clamp output and anti-windup: if output at limit, prevent integral further windup
    if (output > c->outMax) {
        output = c->outMax;
        if (error > 0) c->integral -= error * dt;
    } else if (output < c->outMin) {
        output = c->outMin;
        if (error < 0) c->integral -= error * dt;
    }

    return output;
}

// Placeholder hardware interface functions
float getPositionFeedback(void);
float getVelocityFeedback(void);
float getTorqueFeedback(void);
void setActuatorOutput(float command);
float getDeltaTime(void);

int main(void) {
    // Desired setpoints
    float positionRef = 0.0f;
    float velocityRef = 0.0f;
    float torqueRef   = 0.0f;

    // PID controllers for each loop
    PIDController posPID, velPID, torPID;
    PID_Init(&posPID, 1.0f, 0.5f, 0.1f, -10.0f, 10.0f);   // Position loop gains & limits
    PID_Init(&velPID, 0.8f, 0.3f, 0.05f, -5.0f, 5.0f);    // Velocity loop gains & limits
    PID_Init(&torPID, 0.5f, 0.2f, 0.01f, -2.0f, 2.0f);    // Torque loop gains & limits

    // Control loop
    while (true) {
        // Get elapsed time
        float dt = getDeltaTime();
        if (dt <= 0) continue;

        // Read feedback
        float posMeas    = getPositionFeedback();
        float velMeas    = getVelocityFeedback();
        float torqueMeas = getTorqueFeedback();

        // Position loop: compute velocity reference
        float posError = positionRef - posMeas;
        velocityRef = PID_Update(&posPID, posError, dt);

        // Velocity loop: compute torque reference
        float velError = velocityRef - velMeas;
        torqueRef = PID_Update(&velPID, velError, dt);

        // Torque loop: compute final actuator command
        float torError = torqueRef - torqueMeas;
        float actuatorCmd = PID_Update(&torPID, torError, dt);

        // Send command to actuator (e.g., motor driver)
        setActuatorOutput(actuatorCmd);

        // Optional: log or debug print
        // printf("posErr=%.3f velRef=%.3f velErr=%.3f torRef=%.3f act=%.3f\n", posError, velocityRef, velError, torqueRef, actuatorCmd);
    }

    return 0;
}

深入解析：用 C 语言实现三级级联 PID 控制器

在精密运动控制和力矩控制领域，PID（比例-积分-微分）控制器因其结构简单、鲁棒性高而被广泛应用。本篇博客将带你逐行剖析一段采用 C 语言实现的三级级联 PID 控制器代码，展示其在位置/速度/力矩环中的应用思路，以及如何实现抗积分饱和（anti-windup）和输出限幅。

---

一、代码结构概览

// PID Controller Structure
typedef struct {
    float kp, ki, kd;    // PID 三项增益
    float integral;      // 积分累加
    float prevError;     // 上次误差，用于微分计算
    float outMax, outMin;// 输出限幅
} PIDController;

void PID_Init(PIDController *c, float kp, float ki, float kd, float outMin, float outMax);
float PID_Update(PIDController *c, float error, float dt);

// 主控制循环
int main(void) {
    // 初始化三环 PID
    PID_Init(&posPID, 1.0f, 0.5f, 0.1f, -10.0f, 10.0f);
    // 位置环：输出范围被限制在 -10 … +10 
    // （比如限定位置环的“速度命令”或“位置增量”在 ±10 单位内）

    PID_Init(&velPID, 0.8f, 0.3f, 0.05f, -5.0f, 5.0f);
    // 速度环：输出范围被限制在 -5 … +5 
    // （比如限定速度环的“扭矩/力矩命令”在 ±5 单位内）

    PID_Init(&torPID, 0.5f, 0.2f, 0.01f, -2.0f, 2.0f);
    // 力矩环：输出范围被限制在 -2 … +2 
    // （比如限定力矩环的“电流指令”在 ±2 单位内）

    while (true) {
        float dt = getDeltaTime();
        // 读取反馈
        // 位置环 → 速度参考
        // 速度环 → 力矩参考
        // 力矩环 → 输出命令
    }
}

整段程序通过 三级级联（位置→速度→力矩）实现精细的运动控制：

1. 位置环 生成速度参考；

2. 速度环 生成力矩（电流/扭矩）参考；

3. 力矩环 输出最终驱动命令。

---

二、PID 控制器核心：PIDController 与 PID_Update

1. 数据结构

typedef struct {
    float kp;        // 比例增益
    float ki;        // 积分增益
    float kd;        // 微分增益
    float integral;  // 积分累计值
    float prevError; // 上次误差
    float outMax;    // 输出上限
    float outMin;    // 输出下限
} PIDController;

• kp, ki, kd：三项增益，决定控制器对误差、误差累积、误差变化率的敏感度。

• integral：累加误差项，用于消除稳态偏差，但需防止过度累积（抗风up）。

• prevError：保存上一时刻误差，用于计算微分项。

• outMax/outMin：输出限制，确保控制信号在硬件可承受范围内。

2. 初始化函数

void PID_Init(PIDController *c, float kp, float ki, float kd, float outMin, float outMax) {
    c->kp = kp; c->ki = ki; c->kd = kd;
    c->integral = 0.0f;
    c->prevError = 0.0f;
    c->outMin = outMin;
    c->outMax = outMax;
}

将所有状态量清零，并设置 PID 增益及输出边界。

3. 更新函数

float PID_Update(PIDController *c, float error, float dt) {
    // 比例
    float Pout = c->kp * error;

    // 积分（含抗风up处理）
    c->integral += error * dt;
    float Iout = c->ki * c->integral;

    // 微分
    float derivative = (error - c->prevError) / dt;
    float Dout = c->kd * derivative;
    c->prevError = error;

    // 总输出
    float output = Pout + Iout + Dout;

    // 限幅与抗风up：输出达到限幅且误差同方向时，撤销积分累加
    if (output > c->outMax) {
        output = c->outMax;
        if (error > 0) c->integral -= error * dt;
    } else if (output < c->outMin) {
        output = c->outMin;
        if (error < 0) c->integral -= error * dt;
    }

    return output;
}

• Pout：当前误差乘以比例增益。

• Iout：积分项，累加误差并乘以积分增益；若持续饱和导致积分过大，则通过撤销刚刚累加的一步来实现抗积分饱和。

• Dout：微分项，反映误差变化率，对突然变化的误差提供阻尼作用。

• 限幅：确保输出不会超出 outMin / outMax，同时结合抗风up逻辑防止积分项越界。

---

三、三级级联控制流程

while (true) {
    float dt = getDeltaTime();
    if (dt <= 0) continue;

    // 1. 位置环
    float posError = positionRef - getPositionFeedback();
    velocityRef = PID_Update(&posPID, posError, dt);

    // 2. 速度环
    float velError = velocityRef - getVelocityFeedback();
    torqueRef = PID_Update(&velPID, velError, dt);

    // 3. 力矩环
    float torError = torqueRef - getTorqueFeedback();
    float actuatorCmd = PID_Update(&torPID, torError, dt);

    setActuatorOutput(actuatorCmd);
}

1. 位置环：以目标位置 positionRef 与实际位置差值计算速度参考 velocityRef。

2. 速度环：以速度参考与实际速度差值计算扭矩/电流参考 torqueRef。

3. 力矩环：以扭矩参考与实际测量扭矩差值生成最终驱动命令 actuatorCmd。

这种级联结构可以解耦各环的响应速度与带宽：

• 位置环带宽最小，负责缓慢跟踪；

• 速度环带宽居中，提供动态响应；

• 力矩环带宽最大，消除快速扰动。

---

四、关键要点与调试建议

1. 参数整定

   • 建议先调试力矩环，获得稳定的闭环响应；

   • 再在力矩环基础上调速度环，最后调位置环；

   • 调参时可使用阶跃响应或频率响应分析。

2. 抗积分饱和

   • if (output > outMax) … c->integral -= error*dt 有效防止积分过度累积，避免系统过冲或失控。

3. 采样时间 dt

   • 必须保证周期稳定，否则微分项会放大噪声；

   • 可在硬件定时器中获取精准 dt。

4. 滤波与噪声

   • 实际中建议对微分项做低通滤波，或采用带限微分；

   • 位置/速度反馈信号需加硬件或软件滤波，降低高频干扰。

5. 安全限幅

   • 将 outMin/outMax 设为硬件允许范围，避免驱动信号越界损坏设备。

---

五、总结

本文通过一段简单清晰的 C 代码，演示了三级级联 PID 控制在位置-速度-力矩控制中的典型实现，涵盖了 PID 三项计算、积分抗风up、输出限幅，以及级联控制的设计思路。在实际应用中，还可以根据系统特性加入更多优化，例如微分滤波、前馈补偿、自适应增益等，进一步提升性能。希望这篇博客能帮助你快速掌握三级级联 PID 的实现与调试要点，为精准运动控制项目提供参考！