• 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏吧

PID算法的C语言实现

开发技术 开发技术 5天前 7次浏览

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 = %drn", 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算法以及修改可能存在的不恰当的位置。

喜欢 (0)