PID算法简介及实现代码

PID简介

智能车比赛中，用到了PID算法，写下来当一个总结。PID是很经典且应用很广泛的控制算法，依据误差来减少误差。


PID

PID分为三部分：

1. P 比例。P增大，可以加快系统响应速度，但是不能从根本上消除静态误差，而且P过大，容易超调。
2. I 积分 。用求和代替积分。一段时间内误差的累积。但是对于变化很快的系统，积分时间不宜太长，I不宜给的太大，否则会降低系统的稳定。
3. D 微分。用增量代替微分。D可以预测偏差，就相当于求了一个加速度，可以做一个预判。但是要注意选择合适的微分时间。

基本的PID算法有两种，位置式PID和增量式PID：

位置式PID：



增量式PID：

位置式PID和增量式PID的区别：

个人经验之谈，对于误差变化很快的系统，选增量式PID比较合适，因为增量式PID只涉及了前后几次的误差，而位置式PID的积分，是一段时间内的误差，如果误差变化很快的话，位置式的PID的I这一项，反倒会引入更大的误差，不如使用增量式PID。

更详细的，可以 看这篇

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

代码实现：

//pid.h
enum pid_mode
{
	PID_POSITION = 0,
	PID_DELTA
};

typedef struct
{
	char mode;//position or delta

	float kp;//P
	float ki;//I
	float kd;//D

	float iout_max;
	float out_max;

	float target;//goal
	float now;//now

	float out_p;
	float out_i;
	float out_d;
	float out;


	float erro[3];
	float Dbuff[3];
}pid_param_t;

void pid_init(pid_param_t * pid, char mode, const float PID[3], float iout_max, float out_max);
float pid_Calc(pid_param_t * pid, float now, float target);

//pid.c
#include "pid.h"

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

void pid_init(pid_param_t * pid ,char mode,const float PID[3],float iout_max,float out_max)
{
	pid->mode = mode;

	pid->kp = PID[0];
	pid->ki = PID[1];
	pid->kd = PID[2];

	pid->out_max = out_max;
	pid->iout_max = iout_max;

	pid->out_p = pid->out_i = pid->out_d = 0;
	pid->out = pid->last_out = pid->before_out = 0;
	pid->Dbuff[0] = pid->Dbuff[1] = pid->Dbuff[2] = 0;
	pid->erro[0] = pid->erro[1] = pid->erro[2] = 0;

}


float pid_Calc(pid_param_t * pid ,float now,float target)
{
	pid->target = target;
	pid->now = now;

	pid->erro[1] = pid->erro[0];
	pid->erro[2] = pid->erro[1];

	pid->erro[0] = pid->target - pid->now;

	if (pid->mode = PID_POSITION)
	{
		pid->Dbuff[0] = pid->erro[0] - pid->erro[1];

		pid->out_p = pid->kp*pid->erro[0];
		pid->out_i += pid->ki*pid->erro[0];
		pid->out_d = pid->kd*pid->Dbuff[0];

		LimitMax(pid->out_i, pid->iout_max);
	
		pid->out = pid->out_p + pid->out_i +  pid->out_d;
	}
	else if (pid->mode = PID_DELTA)
	{

		pid->Dbuff[1] = pid->Dbuff[0];
		pid->Dbuff[2] = pid->Dbuff[1];

		pid->Dbuff[0] = pid->erro[0] - pid->erro[1];

		pid->out_p = pid->kp*pid->Dbuff[0];
		pid->out_i= pid->ki*pid->erro[0];
		pid->out_d = pid->kd*(pid->Dbuff[0]- pid->kd*pid->Dbuff[1]);

		pid->out = pid->out_p + pid->out_i + pid->out_d;

		LimitMax(pid->out, pid->out_max);

	}
	return pid->out;
}