从0编写一份PID控制代码

从0编写一份PID控制代码

一、前言

上一章节我分享了控制算法PID的基本概念，以及调参方式，相信大家对PID有了一个基本的了解，这一章我分享一下现在我使用的PID算法代码（代码是大疆工程师写的PID代码模板，写的非常棒），结合原理分析，让大家对其有一个更加深刻的理解，并且知道如何写PID算法
上一节文章链接：你和PID调参大神之间，就差这篇文章！

二、PID初始化代码

工程或者比赛中我们用到的PID一般不止一个，这些PID只是参数的值不一样，参数类型，参数运算函数基本相同，所以定义一个结构体，将这些有关参数作为结构体的成员，定义一个新的PID参数时，就是建立一个新的结构体，运算和初始化时直接调用对应的成员变量就行，十分方便简洁，具体定义的结构体如下：

typedef struct
{
    //PID运算模式
    uint8_t mode;
    //PID 三个基本参数
    fp32 Kp;
    fp32 Ki;
    fp32 Kd;

    fp32 max_out;  //PID最大输出
    fp32 max_iout; //PID最大积分输出

    fp32 set;	  //PID目标值
    fp32 fdb;	  //PID当前值

    fp32 out;		//三项叠加输出
    fp32 Pout;		//比例项输出
    fp32 Iout;		//积分项输出
    fp32 Dout;		//微分项输出
    //微分项最近三个值 0最新 1上一次 2上上次
    fp32 Dbuf[3];  
    //误差项最近三个值 0最新 1上一次 2上上次
    fp32 error[3];  

} pid_type_def;

定义的结构体成员变量的功能已经备注在代码中，有了结构的体之后，我们使用结构体定义一个PID结构体，编写一个初始化代码，初始运行时调用一次，初始化各个参数，PID_init函数主体如下：

void PID_init(pid_type_def *pid, uint8_t mode, const fp32 PID[3], fp32 max_out, fp32 max_iout)
{
    if (pid == NULL || PID == NULL)
    {
        return;
    }
    pid->mode = mode;
    pid->Kp = PID[0];
    pid->Ki = PID[1];
    pid->Kd = PID[2];
    pid->max_out = max_out;
    pid->max_iout = max_iout;
    pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
    pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
}

参数	功能
*pid	传入要初始化的PID结构体指针
mode	PID运行的模式，增量式还是位置式PID，此处我们定义一个枚举变量用于设置模式
PID[3]	传入一个数组，用于作为三个基本参数P、I、D的初始值
max_out	PID总输出的限幅，防止整体输出过大，传入一个正数，限制范围为[-max_out,+max_out]
max_iout	积分项输出的限幅，因为系统刚启动时与目标误差较大，累计误差计算输出会很大，影响系统稳定性，所以对累计误差进行限幅，传入一个正数，限制范围为[-max_iout,+max_iout]

模式枚举

enum PID_MODE
{
    PID_POSITION = 0,
    PID_DELTA
};

三、PID运算代码

PID初始化完成之后，就是编写具体的运算代码了，代码内容如下，具体含义注释在代码中：

fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    //判断传入的PID指针不为空
    if (pid == NULL)
    {
        return 0.0f;
    }
    //存放过去两次计算的误差值
    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    //设定目标值和当前值到结构体成员
    pid->set = set;
    pid->fdb = ref;
    //计算最新的误差值
    pid->error[0] = set - ref;
    //判断PID设置的模式
    if (pid->mode == PID_POSITION)
    {
        //位置式PID
        //比例项计算输出
        pid->Pout = pid->Kp * pid->error[0];
        //积分项计算输出
        pid->Iout += pid->Ki * pid->error[0];
        //存放过去两次计算的微分误差值
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        //当前误差的微分用本次误差减去上一次误差来计算
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        //微分项输出
        pid->Dout = pid->Kd * pid->Dbuf[0];
        //对积分项进行限幅
        LimitMax(pid->Iout, pid->max_iout);
        //叠加三个输出到总输出
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        //对总输出进行限幅
        LimitMax(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        //增量式PID
        //以本次误差与上次误差的差值作为比例项的输入带入计算
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        //以本次误差作为积分项带入计算
        pid->Iout = pid->Ki * pid->error[0];
        //迭代微分项的数组
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        //以本次误差与上次误差的差值减去上次误差与上上次误差的差值作为微分项的输入带入计算
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        //叠加三个项的输出作为总输出
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        //对总输出做一个先限幅
        LimitMax(pid->out, pid->max_out);
	}
	return pid->out;
}

限幅代码为预编译代码

#define LimitMax(input, max)   \
    {                          \
        if (input > max)       \
        {                      \
            input = max;       \
        }                      \
        else if (input < -max) \
        {                      \
            input = -max;      \
        }                      \
    }

以上就是PID运行时的处理代码了，这里PID有两种处理模式，一个增量式一个是位置式，两个算法具体说起来还比较复杂，这里有一篇很棒的参考文章，链接挂在这里，想要了解原理可以参考这篇文章：

位置式PID与增量式PID区别浅析

四、PID输出清空代码

有时候我们需要停止PID，需要清除中间变量，主要就是目标值和中间变量清零，具体代码如下

void PID_clear(pid_type_def *pid)
{
    if (pid == NULL)
    {
        return;
    }
	//当前误差清零
    pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;
    //微分项清零
    pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
    //输出清零
    pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;
    //目标值和当前值清零
    pid->fdb = pid->set = 0.0f;
}

以上代码我放到CSDN内，需要自取：链接