瑞萨公司的增量式PID算法

原创已于 2023-09-18 10:56:52 修改 · 134 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #c语言

于 2023-09-18 10:45:20 首次发布

本文介绍了针对RL78的固定点PID控制器的初始化过程和控制函数，包括参数设置、误差计算、积分、比例和微分项的处理，以及输出限值的处理。


#ifndef PID_H
#define PID_H

#define Target_dist (100000)
#define max (256*4)
#define MAX_LIMIT 9999999
#define MIN_LIMIT -9999999

typedef struct PID_PAR
{
    long Ref; // 输入：参考输入
    long Fdb; // 输入：反馈输入
    long Err; // 变量：误差
    long Kp; // 参数：比例增益
    long Up; // 变量：比例输出
    long Ui; // 变量：积分输出
    long Ud; // 变量：微分输出
    long OutPreSat; // 变量：预饱和输出
    long IntegralMax; // 参数：积分最大值
    long IntegralMin; // 参数：积分最小值
    long IntegralErr; // 变量：积分误差累加器
    long OutMax; // 参数：输出最大值
    long OutMin; // 参数：输出最小值
    long Out; // 输出：PID输出
    long SatErr; // 变量：饱和差值
    long Ki; // 参数：积分增益
    long Kc; // 参数：积分修正增益
    long Kd; // 参数：微分增益
    long Up1; // 历史：上一次的比例输出
    long Dt; // 参数：时间常数
    void (*calc)(); // 指向计算函数的指针
} PID_PAR;

void PID_Init(void);
void PID_Control(PID_PAR *pid);

extern struct PID_PAR pid_par;

#endif



#include "PID.h"

//
/* Description: 固定点PID控制器 for RL78 /
/ 初始化 /
*/

void PID_Init(void)
{
    /* 初始化数值 */
    pid_par.Ref = Target_dist; // 输入：参考输入
    pid_par.Fdb = 0; // 输入：反馈输入
    pid_par.Err = 0; // 变量：误差
    pid_par.Kp = 1; // 参数：比例增益
    pid_par.Up = 0; // 变量：比例输出
    pid_par.Ui = 0; // 变量：积分输出
    pid_par.Ud = 0; // 变量：微分输出
    pid_par.OutPreSat = 0; // 变量：预饱和输出
    pid_par.IntegralMax = MAX_LIMIT; // 参数：积分最大值
    pid_par.IntegralMin = MIN_LIMIT; // 参数：积分最小值
    pid_par.IntegralErr = 0; // 变量：积分误差累加器
    pid_par.OutMax = MAX_LIMIT; // 参数：输出最大值
    pid_par.OutMin = MIN_LIMIT; // 参数：输出最小值
    pid_par.Out = 0; // 输出：PID输出
    pid_par.SatErr = 0; // 变量：饱和差值
    pid_par.Ki = 1; // 参数：积分增益
    pid_par.Kc = 1; // 参数：积分修正增益
    pid_par.Kd = 1; // 参数：微分增益
    pid_par.Up1 = 0; // 历史：上一次的比例输出
    pid_par.Dt = 1; // 时间常数（单位）
    pid_par.calc = (&PID_Control); // 指向计算函数的指针
}


/* Description: 固定点PID控制器 for RL78 /
 输入：PID参数结构体指针 /
/ - 常量（固定）Kp、Ki、Kd /
/ - 变量（固定）Up、Ui、Ud /
/ 计算后的（固定）结果 /
/ 额外参数（待定） /
/ 输出：pid->Err：0 表示计算成功 /
/ pid->Err：-1 表示计算超出限制 /
//
/ 注意：该函数应定期调用 /
/*/
void PID_Control(PID_PAR *pid)
{
    long cal_result;
    long present_err;

    /*** 不使用 SatErr 和 Kc ***/

    /* 当前误差 */
    present_err = pid->Ref - pid->Fdb;


    /* 积分项计算 */
    /* 添加积分项（Ui * Ki）*/

    cal_result = pid->Ki * present_err;

    if (cal_result > pid->IntegralMax)
    {
        cal_result = pid->IntegralMax;
    }
    else if (cal_result < pid->IntegralMin)
    {
        cal_result = pid->IntegralMin;
    }

    /* 添加比例项（Pe * Kp） */
    cal_result += pid->Kp * present_err;


    /* 瞬时误差之和 */
    pid->Ui += present_err * pid->Dt;

    /* 误差随时间的斜率 */
    pid->Ud = (present_err - pid->Up1) / (pid->Dt);

    /* 添加微分项（De * Kd） */
    cal_result += pid->Kd * pid->Ud;

    pid->Up1 = present_err;

    /* 检查结果 */
    if (cal_result > pid->OutMax)
    {
        pid->Out = pid->OutMax;
        pid->Err = -1;
    }
    else if (cal_result < pid->OutMin)
    {
        pid->Out = pid->OutMin;
        pid->Err = -1;
    }
    else
    {
        pid->Out = cal_result;
        pid->Err = 0;
    }
}

#define TARGET SPEED 100
struct PID_PAR pid_par;

/* PID variable */
void main()
{
    fPID_Init();
    pid_par.Ref = TARGET_SPEED;

    While(1)
    {
        /* Get Current Speed */
        get_peed(&Feedback);

        /* PID */
        pid_par.Fdb = Feedback;
        pid_par.calc(&pid_par);
        Cal_Result = pid_par.Out;

        /* Set speed */
        set speed(Cal Result);
    }
}