PID算法的C语言实现

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算法，无积分分离、无抗积分饱和
 * @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算法
 * @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算法
 * @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算法
 * @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实例构造
 *
 *
 *************************************/
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          :
 *
 * @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算法以及修改可能存在的不恰当的位置。