SimpleFOC源码学习01(v2.3.2) - PID控制器PID.cpp与PID.h

前言

---

github源码：https://github.com/simplefoc/Arduino-FOC/tree/master/src/common

看代码之前，先看一张直觉图：

一、pid.h

---

class PIDController
{

public:
    /**
     *  
     * @param P - Proportional gain
     * @param I - Integral gain
     * @param D - Derivative gain
     * @param ramp - Maximum speed of change of the output value
     * @param limit - Maximum output value
     */
    PIDController(float P, float I, float D, float ramp, float limit);
    ~PIDController() = default;
    float operator() (float error);
    void reset();
    float P; //!< Proportional gain
    float I; //!< Integral gain
    float D; //!< Derivative gain
    float output_ramp; //!< Maximum speed of change of the output value
    float limit; //!< Maximum output value
protected:
    float error_prev; //!< last tracking error value
    float output_prev;  //!< last pid output value
    float integral_prev; //!< last integral component value
    unsigned long timestamp_prev; //!< Last execution timestamp
};

pid.h 是头文件，定义了 PIDController 这个类的结构，就像一张设计图纸。
公开成员（外部可以访问）：

• P, I, D --- 三个增益参数
• output_ramp --- 输出变化速率的上限
• limit --- 输出值的绝对上限
• 构造函数 PIDController(...) --- 初始化控制器
• operator() --- 核心计算函数（调用方式像函数一样：pid(error)）
• reset() --- 重置状态

受保护成员（内部状态，外部不可访问）：

• error_prev --- 上次的误差
• output_prev --- 上次的输出
• integral_prev --- 上次的积分值
• timestamp_prev --- 上次调用的时间戳

二、pid.cpp

---

2.1、理解构造函数

PIDController::PIDController(float P, float I, float D, float ramp, float limit)
    : P(P)
    , I(I)
    , D(D)
    , output_ramp(ramp)    // output derivative limit [volts/second]
    , limit(limit)         // output supply limit     [volts]
    , error_prev(0.0f)
    , output_prev(0.0f)
    , integral_prev(0.0f)
{
    timestamp_prev = _micros();
}

这段代码的作用是：接收 5 个参数，把所有"记忆状态"初始化为 0，并记录当前时间戳，为第一次计算做好准备。

2.2、核心函数 operator() 逐行解读

float PIDController::operator() (float error){
    // calculate the time from the last call
    unsigned long timestamp_now = _micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
    // quick fix for strange cases (micros overflow)
    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = P * error;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
    // antiwindup - limit the output
    integral = _constrain(integral, -limit, limit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = D*(error - error_prev)/Ts;
    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -limit, limit);
    // if output ramp defined
    if(output_ramp > 0){
        // limit the acceleration by ramping the output
        float output_rate = (output - output_prev)/Ts;
        if (output_rate > output_ramp)
            output = output_prev + output_ramp*Ts;
        else if (output_rate < -output_ramp)
            output = output_prev - output_ramp*Ts;
    }
    // saving for the next pass
    integral_prev = integral;
    output_prev = output;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}

计算时间间隔 Ts：

unsigned long timestamp_now = _micros();
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;  // 防止异常值

_micros() 返回微秒，乘以 1e-6 换算成秒。如果时间异常（比如定时器溢出），就用默认值 1ms。

P 比例项

float proportional = P * error;

最简单，直接乘以增益。

I 积分项（Tustin 梯形法）

float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);
integral = _constrain(integral, -limit, limit);  // 防积分饱和

用梯形面积来近似积分，比矩形法更精确。_constrain 防止积分无限增大（即"积分饱和"）。

D 微分项

float derivative = D * (error - error_prev) / Ts;

误差的变化量除以时间 = 变化速率。

求和 + 限幅

float output = proportional + integral + derivative;
output = _constrain(output, -limit, limit);

输出斜率限制（ramp）

if(output_ramp > 0){
    float output_rate = (output - output_prev) / Ts;
    if (output_rate > output_ramp)
        output = output_prev + output_ramp * Ts;
    else if (output_rate < -output_ramp)
        output = output_prev - output_ramp * Ts;
}

防止输出跳变过快，比如电机突然全速启动会损坏硬件。

保存状态供下次使用

integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;

完整数据流动图

总结

2.3、为什么用梯形法？

核心对比是矩形法 vs 梯形法------两者都是用"面积"来近似积分，但精度差别很大。

矩形法（前向欧拉）：

integral = integral_prev + I * Ts * error_prev

只用上一步的误差值，相当于用矩形来近似曲线下面积。

梯形法（Tustin / 双线性变换）：

integral = integral_prev + I * Ts * 0.5 * (error + error_prev)

同时用这一步和上一步取平均，相当于用梯形来近似面积。

直觉上很清楚：矩形法每一步只用旧值，总是"落后"于真实曲线；梯形法把两端连成斜线，天然地跟曲线贴合得更好。

为什么这在电机控制里特别重要？

梯形法还有一个关键数学性质------它在 s 域（连续）和 z 域（离散）之间的映射是双线性变换（Tustin 变换），保证了：

1. 稳定性保持：连续系统稳定，离散化后仍然稳定。矩形法没有这个保证------采样率不够高时可能引入振荡甚至发散。
2. 频率特性保真：频率响应的形状被保留，只是略有压缩，不会产生虚假的谐振峰。
3. 代价几乎为零 ：只是多加了一个 error_prev，乘以 0.5f，没有额外的存储或计算负担。

对应代码再看一遍

// Tustin transform of the integral part
// u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
float integral = integral_prev + I * Ts * 0.5f * (error + error_prev);

注释里直接写了 "Tustin transform"，0.5f * (error + error_prev) 就是取两端平均------这就是梯形的高度。

简单说：矩形法便宜但不准，梯形法同样便宜但准得多，所以在嵌入式控制里几乎是默认选择。

三、调节PID

---

框图：

核心原则：从内到外调，内层必须先稳定，外层才有意义。 就是，先调电流环，再调速度环，最后才调位置环。

第一步：调电流环（最内层，最快）

对应代码：PID_current_q 和 PID_current_d，默认值 P=3, I=300, D=0, ramp=0。

电流环响应速度要求最高（kHz 级别），所以 ramp=0（不限速），D=0（噪声太大）。

调法：只有电流传感器时才需要调这层。没有电流传感器就跳过，用电压模式代替。

• 从小 P 开始（比如 P=1），逐渐增大到电流响应变快但不抖动
• I 消除稳态误差，一般设为 P × 100 左右（就是代码默认的比例关系）
• 如果电流波形振荡，先减 I，再减 P

第二步：调速度环（中间层）

对应代码：PID_velocity，默认 P=0.5, I=10, D=0, ramp=1000。注意 D=0 是故意的------速度信号由传感器微分得到，本身已经有噪声，再加 D 会放大噪声导致抖动。

速度环调参步骤：

1. 先把 I=0，只用 P。逐渐增大 P 直到速度跟得上目标但还没振荡。
2. 加入 I，消除稳态误差（电机在目标速度附近的静差）。I 太大会过冲，适量即可。
3. output_ramp（默认 1000 V/s）控制输出突变速率。启动保护要求高时调小，想要快速响应时调大。
4. 配合低通滤波器 LPF_velocity（默认 Tf=0.005s）------Tf 越大越平滑但越滞后。

第三步：调位置环（最外层）

对应：P_angle，默认 P=20, I=0, D=0，输出限幅是速度上限 vel_limit=20 rad/s。

位置环只用 P 控制器，这不是偷懒------是因为速度环已经处理了积分（消除稳态误差），位置环加 I 反而容易引起积分饱和导致过冲。

• 从小 P 开始（比如 P=5），逐渐增大到位置跟踪准确但没有过冲
• 调 vel_limit（P_angle.limit）来限制最大运动速度，防止大角度命令让电机猛冲