【ESP32 Arduino平衡小车制作】（四) 直流电机PID控制_esp32 pid

一、PID算法

PID 是 结合Proportional（比例）、 Integral（积分）、Differential（微分）三种环节于一体的闭环控制算法，它是目前为止在连续控制系统中计数最为成熟的一种控制算法；
PID 控制的实质是对目标值和实际值误差进行比例、积分、微分运算后的结果用来作用在输出上。
连续控制的理想 PID 控制规律

$u(t)$ = $Kp(e(t)+\frac{1}{T}{\int }_{}e(t)dt+Td\frac{de(t)}{dt}$

• Kp——比例增益， Kp 与比例度成倒数关系
• Tt——积分时间常数
• TD——微分时间常数
• u（t）——PID 控制器的输出信号
• e（t）——给定值 r（t）与测量值误差

1-1 积分分离pid算法

控制系统在大幅度改变给定值时，系统会出现较大的偏差，不可能在短时间内消除，经过PID算法中积分项的累积后，可能会使控制作用u(t)很大，控制量达到了饱和

·积分饱和使控制量不能根据被控量的误差，按控制算法进行调节，从而影响控制效果，其中最明显的结果是:系统超调增大，响应延迟。

积分分离算法的思想是在e(kt)较大时，取消积分作用;而在e(k)较小时将积分作用投入。

1-2 带有死区的pid算法

在要求控制作用少变动的场合，常采用带死区的PID控制，实际上是一个非线性系统。

1-3 微分先行pid算法

在给定值频繁变换的场合，为了避免系统超调量过大甚至发生振荡，执行机构剧烈动作，对模拟PID控制器进行改进，出现微分先行PID控制器。

基本上属于不完全微分控制算法，但是强调的是微分环节的先行位置。在整个PID控制器前面串接一个低通滤波器，或者在反馈通道串联一个低通滤波器，实现微分先行。

二、程序编写

关于pid的程序编写资料可以查看，百度网盘链接：
https://pan.baidu.com/s/1jV0wTftGX8nU6hdUol_c4Q ：提取码：csef
下面我写了位置式PID控制算法的全过程

第一步、定义 PID 变量结构体

typedef struct
{
	float kp;
	float ki;
	float kd;
	
	float actual;
	float target;
	
	float gyro_Y;
	float err;
	int32_t PID_OUT;
	
}Stand_PID;

typedef struct
{
	float kp;
	float ki;
	
	int32_t err;
	int32_t err_integral;
	
	float target;
	
	float PID_OUT;
	
	float moto_left;
	float moto_right;
	
	int filter_err;
	int filter_err_last;
	float filter_a;
}Speed_PID;

typedef struct
{
	float kp;
	float kd;
	
	int32_t PID_OUT;
	
	int rc;
	float gyro_Z;
}Turn_PID;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

第二步、PID参数的初始化

void PID_Init()
{
	Stand_pid.kp=12.0;
	Stand_pid.ki=0;
	Stand_pid.kd=15.0;
	Stand_pid.target=0.0;
	Stand_pid.max=10;
	Stand_pid.min=-10;
	Stand_pid.err_integral=0;
	Stand_pid.PID_OUT = 0;
	
	Speed_pid.kp=-0.04;
	Speed_pid.ki=Speed_pid.kp/200.0;
	Speed_pid.filter_a=0.7;
	Speed_pid.err_integral=0;
	Speed_pid.target=0;
	Speed_pid.PID_OUT = 0;
	
	Turn_pid.kp=2;
	Turn_pid.kd=4;
	Turn_pid.PID_OUT = 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

第三步、PID控制算法编写

/*********************
直立环PD控制器：Kp*Ek+Kd*Ek_D

入口：期望角度、真实角度、真实角速度
出口：直立环输出
*********************/
void stand_pid(Stand_PID *Stand_pid)
{
	Stand_pid->err= Stand_pid->actual - Stand_pid->target;
	
	//死区控制 误差绝对值小于0.20的全部忽略
	if( 0.001< Stand_pid->err && Stand_pid->err < 0.0500){Stand_pid->err = 0.00001;}
	if(-0.050< Stand_pid->err && Stand_pid->err < 0.001){Stand_pid->err = 0.00001;}
	
	Stand_pid->PID_OUT =(int32_t)( Stand_pid->kp*Stand_pid->err + Stand_pid->kd*Stand_pid->gyro_Y);	
}
/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
void speed_pid(Speed_PID *Speed_pid)
{
	//1.计算速度偏差
	Speed_pid->err=(Speed_pid->moto_left+Speed_pid->moto_right)-Speed_pid->target;
	//2.对速度偏差进行低通滤波
	//使得波形更加平滑，滤除高频干扰，防止速度突变。
	Speed_pid->filter_err=(1-Speed_pid->filter_a)*Speed_pid->err+Speed_pid->filter_a*Speed_pid->filter_err_last;
	
	Speed_pid->filter_err_last=Speed_pid->filter_err;
	//3.对速度偏差积分，积分出位移
	Speed_pid->err_integral+=Speed_pid->filter_err;
	//4.积分限幅	
	Speed_pid->err_integral=Speed_pid->err_integral>10000?10000:(Speed_pid->err_integral<(-10000)?(-10000):Speed_pid->err_integral);
	if(stop==1)Speed_pid->err_integral=0,stop=0;//清零积分量
	
	Speed_pid->PID_OUT=(int32_t)(Speed_pid->kp*Speed_pid->err+Speed_pid->ki*Speed_pid->err_integral);
}


/*********************
转向环：系数*Z轴角速度+系数*遥控数据
*********************/
void turn_pid(Turn_PID *Turn_pid)
{
	Turn_pid->PID_OUT= Turn_pid->kp*Turn_pid->rc + Turn_pid->kd*Turn_pid->gyro_Z ;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

第五步、电机控制程序，在中断中开展

#define SPEED_Y 1000//俯仰(前后)最大设定速度
#define SPEED_Z 5//偏航(左右)最大设定速度 

uint8 Fore,Back,Left,Right;

int16_t Encoder_Left,Encoder_Right;	//编码器数据（速度）
int MOTO1,MOTO2;				//电机装载变量
int PWM_out;

float Target_Speed= 0;	//期望速度（俯仰）
float Turn_Speed=-1;		//期望速度（偏航）


void Control_hanlder (void)
{
	    //1、采集编码器数据 最大值840
	    //电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反。
		Encoder_Left=tim_encoder_get_count(ENCODER1_TIM);
		Encoder_Right=-tim_encoder_get_count(ENCODER2_TIM);
		//速度缩放
		Speed_pid.moto_left  = Encoder_Left/10.0;
		Speed_pid.moto_right = Encoder_Right/10.0;	
	
		//2、将数据压入闭环控制中，计算出控制输出量
		
		//2-1速度环控制
		Speed_pid.target = Target_Speed;
		speed_pid(&Speed_pid);
		
		//2-2直立环控制
		Stand_pid.gyro_Y=gyro_y;
		Stand_pid.actual=pitch;
		stand_pid(&Stand_pid);
		
		//2-3转向环控制
		if((Left==0)&&(Right==0))Turn_pid.kd=4;		//开启转向约束
		else if((Left==1)||(Right==1))Turn_pid.kd=0;//去掉转向约束
		
		Turn_pid.gyro_Z = icm_data.gyro_z;
		Turn_pid.rc = Turn_Speed;
		turn_pid(&Turn_pid);							
		
		PWM_out=Stand_pid.PID_OUT-(int32_t)Speed_pid.PID_OUT;  //最终输出		
		
		//3、把控制输出量加载到电机上，完成最终的的控制。
		MOTO1=PWM_out-Turn_pid.PID_OUT;//左电机
		MOTO2=PWM_out+Turn_pid.PID_OUT;//右电机
		
		Set_PWM(MOTO1,MOTO2);		 //加载到电机上
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

三、硬件设计

剩下的就是漫长的参数调试过程，因为之前都是用分立模块搭建的小车。
从明天开始给更新硬件电路板制作过程