PID演算法的C語言實現

_小溢 發表於 2020-11-21

1.根據我控制演算法類文章中關於PID的理論的一些描述,同時也根據網路上一些其他的PID文章,以及自己最近一個專案的實踐後,總結了幾套基於C語言的PID演算法,由於網路中很少有人進行分享完整的PID演算法實現,我這裡分享下。

(1)標頭檔案,定義pid的結構體,類的概念,包含pid的屬性和方法

#ifndef __PID_H_
#define __PID_H_

#include <stdint.h>



typedef struct _pid
{
    int16_t set_value;            // 給定值,rin(k)
    int16_t actual_value;         // 實際值,反饋值,rout(k)
    int16_t err;                  // 偏差值,rin(k) - rout(k)
    int16_t err_last;             // 上一次偏差值,rin(k - 1) - rout(k - 1)
    int16_t err_last_last;        // 上一次上一次的偏差值,rin(k - 2) - rout(k - 2)
    float kp;                       // 比例係數
    float ki;                       // 積分系數
    float kd;                       // 微分系數
    float uk;                       // pid公式運算結果值
    float incremental_value;      // 增量值
    float integral_value;             // 積分值
    float umax;                 // uk的上限值,抗積分飽和用
    float umin;                 // uk的下限值,抗積分飽和用
    int16_t err_up_value;         // 偏差上限值,積分分離用
    int16_t ki_k;                 // 積分的再次乘機係數,積分分離用
    
    float out_value;              // 
    
    float(*position_type)(struct _pid *ppid);                       // 位置型PID演算法,無積分分離、無抗積分飽和
    float(*incremental_type)(struct _pid *ppid);              // 增量型PID演算法
    float(*integral_separation_type)(struct _pid *ppid);      // 積分分離PID演算法
    float(*int_sep_anti_sat_type)(struct _pid *ppid);         // 積分分離 + 抗積分飽和PID演算法
}_pid_t;







_pid_t *pid_create(void);



extern _pid_t *pg_pid;







#endif

(2).c檔案,包含標頭檔案中4個PID演算法的實現,包含位置型PID演算法、增量型PID演算法、積分分離PID演算法、積分分離+抗飽和PID演算法

#include <stdlib.h>
#include <string.h>

#include "pid.h"

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "portmacro.h"
#include "semphr.h"

#include "Debug.h"



/*************************************
 *
 * Funciton Name : pid_position_type
 * Function      :位置型PID演算法,無積分分離、無抗積分飽和
 * 
 * @author       :why
 * @note         : 積分分離:能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分
 *               : 抗積分飽和:能解決執行器到達極限值時,但uk還在增大,使得系統進入飽和,進入飽和區的時間越長,當系統出現反向偏差,解決這個反向偏差的時間就會越長,此時系統會像失控一樣。
 *               :進入抗積分飽和,當uk超過上限時,積分向只累加負偏差,當uk超過下限時,積分項只累加正偏差。
 * 
 × @param         :pid
 *
 *
 * @return       : uk
 *************************************/
static float pid_position_type_f(struct _pid *ppid)
{
    ppid->err = ppid->set_value - ppid->actual_value;   // 偏差
    
    ppid->integral_value += ppid->err;                  // 積分累加
    
    ppid->uk = ppid->kp * ppid->err + ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last);
    
    ppid->err_last = ppid->err;
    
    return (ppid->uk);
}


/*************************************
 *
 * Funciton Name : pid_incremental_type_f
 * Function      :增量型PID演算法
 * 
 * @author       :why
 * @note         : 相較於位置型,因為與3個偏差相關,增強了系統穩定性
 * 
 × @param         :pid
 *
 *
 * @return       : uk + 增量值
 *************************************/
static float pid_incremental_type_f(struct _pid *ppid)
{
    ppid->err = ppid->set_value - ppid->actual_value;   // 偏差
    
    //ppid->integral_value += ppid->err;                  // 積分累加
    
    ppid->incremental_value = ppid->kp * ( ppid->err - ppid->err_last) + ppid->ki * ppid->err + ppid->kd * (ppid->err - 2 * ppid->err_last + ppid->err_last_last);
    
    ppid->uk += ppid->incremental_value;
    
    ppid->err_last_last = ppid->err_last;
    ppid->err_last = ppid->err;
    
    return (ppid->uk);
}


/*************************************
 *
 * Funciton Name : pid_integral_separation_type_f
 * Function      :積分分離PID演算法
 * 
 * @author       :why
 * @note         : 能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分
 * 
 × @param         :pid
 *
 *
 * @return       : uk
 *************************************/
static float pid_integral_separation_type_f(struct _pid *ppid)
{
    ppid->err = ppid->set_value - ppid->actual_value;   // 偏差
    
    // 誤差過大去除積分效果
    if ( abs(ppid->err) > ppid->err_up_value )
    {
        ppid->ki_k = 0;
    }
    else
    {
        ppid->ki_k = 1;
        ppid->integral_value += ppid->err;              // 積分累加
    }      
    
    ppid->uk = ppid->kp * ppid->err + ppid->ki_k * ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last);
    
    ppid->err_last = ppid->err;
    
    return (ppid->uk);
}


/*************************************
 *
 * Funciton Name : pid_int_sep_anti_sat_type_f
 * Function      :積分分離 + 抗積分飽和PID演算法
 * 
 * @author       :why
 * @note         : 能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分
 *               : 抗積分飽和:能解決執行器到達極限值時,但uk還在增大,使得系統進入飽和,進入飽和區的時間越長,當系統出現反向偏差,解決這個反向偏差的時間就會越長,此時系統會像失控一樣。
 *               :進入抗積分飽和,當uk超過上限時,積分向只累加負偏差,當uk超過下限時,積分項只累加正偏差。
 *
 × @param         :pid
 *
 *
 * @return       : uk
 *************************************/
static float pid_int_sep_anti_sat_type_f(struct _pid *ppid)
{
    ppid->err = ppid->set_value - ppid->actual_value;   // 偏差
    
    Debug("ppid->err = %d\r\n", ppid->err);
    
    if ( ppid->out_value > ppid->umax )                    // 抗積分飽和
    {
        // 誤差過大去除積分效果
        if ( abs(ppid->err) > ppid->err_up_value )  // 積分分離
        {
            ppid->ki_k = 0;
        }
        else
        {
            ppid->ki_k = 1;
            
            if ( ppid->err < 0 )                    
            {
                ppid->integral_value += ppid->err;              // 積分累加
            }
            
        }    
    }
    else if ( ppid->out_value < ppid->umin )                // 抗積分飽和
    {
        // 誤差過大去除積分效果
        if ( abs(ppid->err) > ppid->err_up_value )  // 積分分離
        {
            ppid->ki_k = 0;
        }
        else
        {
            ppid->ki_k = 1;
            
            if ( ppid->err > 0 )                    
            {
                ppid->integral_value += ppid->err;              // 積分累加
            }
            
        } 
    }
    else
    {
        // 誤差過大去除積分效果
        if ( abs(ppid->err) > ppid->err_up_value )
        {
            ppid->ki_k = 0;
        }
        else
        {
            ppid->ki_k = 1;
            ppid->integral_value += ppid->err;              // 積分累加
        } 
    }
    
    ppid->uk = ppid->kp * ppid->err + ppid->ki_k * ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last);
    
    ppid->err_last = ppid->err;
    
//    if ( ppid->uk >= 0.07 && ppid->uk <= 1 )
//    {
//        ppid->uk = 1;
//    }
//    
//    if ( ppid->uk <= -0.07 && ppid->uk >= -1 )
//    {
//        ppid->uk = -1;
//    }
    
    return (ppid->uk);
}

/*************************************
 *
 * Funciton Name    : pid_init
 * Function         : pid例項構造
 * 
 * @author       :why
 *
 *
 *************************************/
static void pid_init(_pid_t *ppid)
{
    ppid->kp = 0.2;
    ppid->ki = 0.05;
    ppid->kd = 0;
    
    ppid->umax = 4000;
    ppid->umin = 0;
    
    
    ppid->position_type = pid_position_type_f;
    ppid->incremental_type = pid_incremental_type_f;
    ppid->integral_separation_type = pid_integral_separation_type_f;
    ppid->int_sep_anti_sat_type = pid_int_sep_anti_sat_type_f;
}


/*************************************
 *
 * Funciton Name    : pid_create
 * Function         : pid例項建立
 * @author          : why
 *
 * @return          : 返回pid例項
 *
 *************************************/
_pid_t *pid_create(void)
{
    _pid_t *ppid = (_pid_t *)pvPortMalloc(sizeof(_pid_t));
    
    memset(ppid, 0, sizeof(_pid_t));
   
    pid_init(ppid);
    
    return ppid;
}
(3)PID的實現演算法有了,但還是要根據實際情況進行除錯選取最適合的PID演算法以及修改可能存在的不恰當的位置。