PID控制算法（C++）

目录

介绍及公式

库文件 

位置式

pid.h

pid.cpp

增量式

pid.h

pid.cpp

用法

1.把库文件放进自己的工程

2.在主程序中包含pid库文件

3.新建pid控制器并初始化pid参数

4.计算error

5.调用pid控制器计算输出

6.将输出施加到系统

---

介绍及公式

pid算法用于简单的控制非常方便，它常用于底层的控制。

因此我们常常把pid控制做成固定的库，方便调用。

库文件 

说明：这里的库程序，是基于esp32开发板写的，其他开发板类似，主要区别就是获取当前时间戳不一样。其余的算法是一样的。如果你想移植到其他平台，只需要修改获取时间戳的函数即可。时间戳单位是微秒。

位置式

pid.h

#define PID_H

#include "time_utils.h"
#include "foc_utils.h"

/**
 *  PID controller class
 */
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);

    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 integral_prev; //!< last integral component value
    float error_prev; //!< last tracking error value
    unsigned long timestamp_prev; //!< Last execution timestamp
    float output_prev;  //!< last pid output value
};

pid.cpp

#include "pid.h"

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]
    , integral_prev(0.0)
    , error_prev(0.0)
    , output_prev(0.0)
{
    timestamp_prev = _micros();
}

// PID controller function
float PIDController::operator() (float error){
    // 计算上次调用到现在的时间Ts(秒)
    unsigned long timestamp_now = _micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6;
    // 快速修复异常 (micros overflow)，如果溢出或者出现其他异常，则Ts=0.001ms
    if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 

    // u(s) = (P + I/s + Ds)e(s)  //计算输入函数u(s)
    // Discrete implementations   //分元素计算
    // proportional part          //比例项
    // u_p  = P *e(k)             //u_p = P * e(k)
    float proportional = P * error;
    // Tustin transform of the integral part //积分项的双线性变换
    // u_ik = u_ik_1  + I*Ts*(ek + ek _1)/2  //u_ik = u_ik_1  + I*Ts*(ek + ek _1)/2
    float integral = integral_prev + I*Ts*(error + error_prev)*0.5;
    // antiwindup - limit the output
    integral = _constrain(integral, -limit, limit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts               //微分项，除以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;
}

增量式

pid.h

#define PID_H

#include "time_utils.h"
#include "foc_utils.h"

/**
 *  PID controller class
 */
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);

    float P;                //!< 比例增益
    float I;                //!< 积分增益
    float D;                //!< 微分增益
    float output_ramp;      //!< 输出值最大变化率
    float limit;            //!< 输出限制(最大输出绝对值)

protected:
    float error_prev;       //!< k-1时刻误差
    float error_prev1;      //!< k-2时刻误差
    unsigned long timestamp_prev; //!< 上次执行计算时的时间戳
    float output_prev;      //!< k-1时刻输出
    float output;           //
};

pid.cpp

#include "pid.h"

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)         // 输出限制(常用pwm分辨率)
    , error_prev(0.0)
    , output_prev(0.0)
    , output(0.0)
{
    timestamp_prev = _micros();
}

// PID controller function
float PIDController::operator() (float error){
    // 计算上次调用到现在的时间Ts(秒) 
    unsigned long timestamp_now = _micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6;
    // 快速修复异常 (micros overflow)，如果溢出或者出现其他异常，则Ts=0.001ms
    if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 

    // 计算输出函数u(s)
    // u(k) = u(k-1) + Kp * (e(k)-e(k-1)) + Ki * Ts * (e(k)+e(k-1))/2 + Kd * (e(k)-2*e(k-1)+e(k-2))/Ts
    // 分别计算
    // 比例项: u_p  = P * (e(k)-e(k-1))
    float proportional = P * (error - error_prev);
    // 积分项: u_ik = I * Ts * (e(k)+e(k-1))/2
    float integral = I * Ts * (error + error_prev)*0.5;
    // 微分项: u_dk = D * (e(k)-2*e(k-1)+e(k-1))/Ts
    float derivative = D * (error - 2 * error_prev + error_prev1)/Ts;

    // 各部分加和
    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;
    }
    
    // 存储数据以便下次调用
    output_prev = output;
    error_prev1 = error_prev;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}

用法

程序用起来比较简单。

1.把库文件放进自己的工程

2.在主程序中包含pid库文件

3.新建pid控制器并初始化pid参数

PIDController pidv{2.8, 8.0, 0.01, 50000.0, 255.0};  //单纯速度环

4.计算error

自己计算一下error，比如我期望电机转速为5r/s,现在通过编码器检测到电机实际速度为3r/s。则error=5-3=2r/s.

5.调用pid控制器计算输出

通过下面一行代码，把误差放进去，pid控制器就会自己计算出来输出。

output = pidv(error);

6.将输出施加到系统

比如我是pid调速的，输入的是有效电压（通过调节pwm占空比调节），输出的是速度。

那么我们拿到pid控制器输出的数值，作为系统的输入，也就是pwm占空比施加到系统即可。