PID庫與PID基本優化（一）

本系列旨在以我自己寫的PID lib為例，講一下PID的幾點基本優化，PID的基本原理網上有很多資料，因此本系列將不會涉及PID的基本實現原理，在這里特別推薦Matlab tech talk的PID教程：https://ww2.mathworks.cn/videos/series/understanding-pid-control.html。

由於筆者大一在讀，還沒有學習自動控制原理等課程，因此本系列將不會從自控原理角度展開，相反的，本系列將試圖從“直覺”展開，通過直觀的描述讓大家從直覺上感受並理解PID的一些包括微分先行、積分分離等基礎的優化。

由於筆者水平有限，文中難免存在一些不足和錯誤之處，誠請各位批評指正。

（一）中主要講解代碼結構與代碼使用，算法有關內容於（二）開始講解

1 代碼結構

該PID lib全部代碼詳見：https://github.com/CharlesW1970/PID_Library

1.1 PID結構體與有關枚舉

該lib通過pid結構體保存於pid運算有關的參數數據，通過枚舉表示其他有關量：

//PID結構體
typedef struct _PID_TypeDef
{
    float Target;
    float LastNoneZeroTarget;
    float Kp;
    float Ki;
    float Kd;

    float Measure;
    float Last_Measure;
    float Err;
    float Last_Err;

    float Pout;
    float Iout; //Iout = ITerm_0 + ITerm_1 +....+ ITerm_n
    float Dout;
    float ITerm; //ITerm = Err * Ki

    float Output;
    float Last_Output;

    float MaxOut;
    float IntegralLimit;
    float DeadBand;
    float ScalarA; //變積分公式參數
    float ScalarB; //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B

    uint8_t Improve; //用於使能優化

    PID_ErrorHandler_t ERRORHandler;

    void (*PID_param_init)(
        struct _PID_TypeDef *pid,
        uint16_t maxOut,
        uint16_t integralLimit,
        float deadband,
        float Kp,
        float ki,
        float kd,
        float A,
        float B,
        uint8_t improve);

    void (*PID_reset)(
        struct _PID_TypeDef *pid,
        float Kp,
        float ki,
        float kd);
} PID_TypeDef;

//PID優化功能枚舉
typedef enum pid_Improvement_e
{
    NONE = 0X00,                        //無
    Integral_Limit = 0x01,              //積分限幅
    Derivative_On_Measurement = 0x02,   //微分先行
    Trapezoid_Intergral = 0x04,         //梯形積分
    Proportional_On_Measurement = 0x08, //該系列不涉及
    OutputFilter = 0x10,                //輸出濾波
    ChangingIntegralRate = 0x20,        //變積分
    ErrorHandle = 0x80,                 //異常處理
} PID_Improvement_e;

//異常情況枚舉，這里只寫了電機堵轉保護一種
typedef enum errorType_e
{
    PID_ERROR_NONE = 0x00U,
    Motor_Blocked = 0x01U
} ErrorType_e;

//異常情況結構體
typedef struct
{
    uint64_t ERRORCount;
    ErrorType_e ERRORType;
} PID_ErrorHandler_t;

1.2 PID初始化

在使用之前需要先調用PID_Init函數進行參數初始化和函數連接

void PID_Init(
    PID_TypeDef *pid,
    uint16_t max_out,
    uint16_t intergral_limit,
    float deadband,

    float kp,
    float Ki,
    float Kd,

    float A,
    float B,

    uint8_t improve)
{
    pid->PID_param_init = f_PID_param_init;
    pid->PID_reset = f_PID_reset; //連接Kp Ki Kd參數重設函數
    pid->PID_param_init(pid, max_out, intergral_limit, deadband,
                        kp, Ki, Kd, A, B, improve); //連接並調用參數初始化函數
}

static void f_PID_param_init(
    PID_TypeDef *pid,
    uint16_t max_out,
    uint16_t intergral_limit,
    float deadband,

    float kp,
    float Ki,
    float Kd,

    float Changing_Integral_A,
    float Changing_Integral_B,

    uint8_t improve)
{
    //參數初始化
    pid->DeadBand = deadband;
    pid->IntegralLimit = intergral_limit;
    pid->MaxOut = max_out;
    pid->Target = 0;

    pid->Kp = kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->ITerm = 0;

    pid->ScalarA = Changing_Integral_A;
    pid->ScalarB = Changing_Integral_B;

    pid->Improve = improve;

    //異常處理初始化
    pid->ERRORHandler.ERRORCount = 0;
    pid->ERRORHandler.ERRORType = PID_ERROR_NONE;

    pid->Output = 0;
}

1.3 PID 計算

PID_Calculate函數與網上大多數代碼大體結構相同，只是添加了不同的優化函數，具體優化在各函數（如：f_PID_ErrorHandle、f_Trapezoid_Intergral）中實現，PID_Calculate函數具體代碼如下：

float PID_Calculate(PID_TypeDef *pid, float measure, float target)
{
    if (pid->Improve & ErrorHandle) 
    {
        //異常處理
        f_PID_ErrorHandle(pid);
        if (pid->ERRORHandler.ERRORType != PID_ERROR_NONE)
        {
            //電機堵轉保護
            pid->Output = 0;
            return 0; 
        }
    }

    //誤差更新
    pid->Measure = measure;
    pid->Target = target;
    pid->Err = pid->Target - pid->Measure;

    //死區內進行計算
    if (ABS(pid->Err) > pid->DeadBand)
    {
        //計算比例、微分輸出與該周期積分項結果
        pid->Pout = pid->Kp * pid->Err;
        pid->ITerm = pid->Ki * pid->Err;
        pid->Dout = pid->Kd * (pid->Err - pid->Last_Err);

        //判斷是否使能梯形積分
        if (pid->Improve & Trapezoid_Intergral)
            f_Trapezoid_Intergral(pid);
        //判斷是否使能變積分
        if (pid->Improve & ChangingIntegralRate)
            f_Changing_Integral_Rate(pid);
        //判斷是否使能積分限幅
        if (pid->Improve & Integral_Limit)
            f_Integral_Limit(pid);
        //判斷是否使能微分先行
        if (pid->Improve & Derivative_On_Measurement)
            f_Derivative_On_Measurement(pid);

        //計算積分輸出
        pid->Iout += pid->ITerm;

        //計算pid總輸出
        pid->Output = pid->Pout + pid->Iout + pid->Dout;

        //判斷是否使能輸出濾波
        if (pid->Improve & OutputFilter)
            f_OutputFilter(pid);

        //輸出限幅
        f_Output_limit(pid);
    }
    //數據保存供下一周期調用
    pid->Last_Measure = pid->Measure;
    pid->Last_Output = pid->Output;
    pid->Last_Err = pid->Err;

    return pid->Output;
}

2 如何使用

這里給出以發布在GitHub上的示例，具體不在詳細講解

//pid函數連接
PID_Init(&PID_Example, 9600, 5000, 3, 1, 5, 0.3, 0.3, 100, 100, 
   		 ErrorHandle | Integral_Limit | OutputFilter);

//修改kp ki kd
PID_Example.PID_reset(&PID_Example, 3, 1, 0);

//計算
PID_Calculate(&PID_Example, measure, target);

該篇對該lib結構和使用就講到這里，下一篇將會開始算法講解。