代码收藏家技术教程 2023-04-07

一文入门pid（二）：pid算法的代码实现

承接上文，我们已经得到了pid的连续型公式：

$\mu _{t} = kp\cdot e(t)+ki\int_{0}^{t}e(\tau )d\tau+kd\cdot \frac{de(t)}{dt}$

但是计算机是离散的，如何把他写到代码里呢。

位置式

比例项的e(t)不用处理即可获得，积分项可以根据前文的物理意义去表示，即面积。函数图像e(t)的面积可以看成若干个 $\Delta t$ 时间内的矩形面积相加，即为 $\sum_{0}^{t}e(t)\Delta t$ 。同样把微分项看作切线，就是 $\Delta t$ 时间内的两点斜率，为 $[e(t)-e(t-1)]/\Delta t$ 。在代码中可以不要 $\Delta t$ ，用迭代次数模拟时间。这就是pid离散化的位置式：

$\mu (t)=kp\cdot e(t)+ki\cdot \sum_{i=0}^{t}e(i)+kd\cdot [e(t)-e(t-1)]$

下面给出代码展示，用了staer表示静态误差，也可以不用

#include <stdio.h>
#include <stdlib.h>

typedef struct pid_parameter
{
	float Target;		//目标值
	float Current;		//实际值(反馈值)
	float Error;		//偏差值(当前偏差)
	float Last_error;	//上次偏差值
	float Integral;		//积分值
	float Kp,Ki,Kd;		//比例,积分,微分系数
	float Out;			//输出值
}pid;

pid user_pid;

float PID(float User_target)//pid算法输出
{
	user_pid.Target = User_target;//将用户输入的目标传入目标值
	user_pid.Error = user_pid.Target - user_pid.Current;//目标值 - 实际值
	user_pid.Integral += user_pid.Error;//累加误差
	user_pid.Out = (user_pid.Kp*user_pid.Error) + (user_pid.Ki*user_pid.Integral) + (user_pid.Kd*(user_pid.Error-user_pid.Last_error));//pid输出值计算
	user_pid.Last_error = user_pid.Error;//本次偏差值将成为 "Last_error"
	return user_pid.Out;//输出实际值
}

void Pid_init()
{
	printf("user_pid init...\n");
	user_pid.Kp=0.8;//比例项
	user_pid.Ki=0.2;//积分项
	user_pid.Kd=0.4;//微分项
	user_pid.Target=0;   
	user_pid.Current=60;
	user_pid.Error=0;
	user_pid.Last_error=0;
	user_pid.Integral=0;
	user_pid.Out=0;
	printf("user_pid init end.\n");
}

int main()
{
	int i = 0;
	float u = 0;
	float Display;
	Pid_init();
	float StaEr = rand()%10-5;
	
	while(i<200)
	{
		u = PID(100);
		user_pid.Current = user_pid.Current + u - StaEr;// 调用pid算法,并输入目标值
		Display = user_pid.Current;
		printf("%f    \n",Display);
		i++;
	}
	
    system("pause");
	return 0;
}

增量式：

计算 $\mu(t)- \mu(t-1)$ 可以得到增量式：

$\Delta \mu=\mu (t)-\mu (t-1)$

$= kp\cdot [e(t)-e(t-1)]+ki\cdot e(i)+kd\cdot [e(t)-2e(t-1)+e(t-2)]$

代码如下：

#include <stdio.h>
#include <stdlib.h>

struct pid_para
{
    float target; // 目标值
    float current;// 测量值
    float error;  // 误差
    float last_error; // 上一次的误差
    float last_lerror;// 前一次的误差
    //float integral; // 积分
    float kp,ki,kd;
    float out;
};

pid_para pid;

float pid_increment(float target)
{
    pid.target = target;
    pid.error = pid.target - pid.current;
    //pid.integral += pid.error;
    float increment = pid.kp * (pid.error - pid.last_error) + (pid.ki * pid.error) + (pid.kd * (pid.error - 2*pid.last_error + pid.last_lerror));
    pid.out += increment;
    pid.last_lerror = pid.last_error;
    pid.last_error = pid.error;
    
    return pid.out;
}

void pid_init()
{
    pid.target = 0;
    pid.current = 60;
    pid.error = 0;
    pid.last_error = 0;
    pid.out = 0;
    pid.kp = 0.8;
    pid.ki = 0.2;
    pid.kd = 0.4;
    printf("kp = %.2f , ki = %.2f , kd = %.2f\n", pid.kp,pid.ki,pid.kd);
}

int main(void)
{
    int i = 0;
    float u = 0;
    float StaEr = rand()%10-5;

    pid_init();

    for(i = 0; i < 100; i++){
        u = pid_increment(100);
	    pid.current = pid.current + u - StaEr;// 调用pid算法,并输入目标值
		printf("%f    \n",pid.current);
    }

    system("pause");
    return 0;
}

其他改进思路

在工程中实际情况是十分复杂的，只用经典pid可能也有不足。例如我们要控制一个四轴无人机从30米飞到100米，以高度误差作为e(t)。pid输出的电机数据可能是从2000r/s到8000r/s（随便编的数字）。但是电机的最大转速就是5000r/s，达不到pid输出的要求，那他只能以最大转速运转。这导致e的函数图像不能快速下降，积分部累计了非常多。这样即使e(t)很小时，比例部分对电机输出作用很小，积分部却成为主要矛盾，让电机仍然保持很大的转速，引起严重的超调。这就是积分饱和现象。我们无法改变比例项和微分项，就只能从积分项入手，用积分分离的思路解决。给系统设置一个最大值，若pid输出超过最大值，就令ki=0，不累加积分，等到pid输出可以接受了，再累加积分。

 if(pid.error > 50){
        flag = 0;
    }
    else{
        pid.integral += pid.error;
    }
    pid.out = (pid.kp * pid.error) + flag * (pid.ki * pid.integral) + (pid.kd * (pid.error - pid.last_error));

当然我们也可以依照情况赋给ki不同的值而不只是0，这就是变积分的思想。

一个pid不够，还可以引入双环pid，以外环的输出作为内环的输入。工程模型会变得很复杂，这里不做详细说明了。