位置式pid
秋微微博-微型音响
2023年2月21日发(作者：拇指的拼音)【平衡⼩车制作】（六）位置式PID、直⽴环与速度环编程（超详解）
⼤家好，我是⼩政。本篇⽂章我将针对位置式PID算法、直⽴环、速度环等的编程进⾏详细的讲解，让每位⼩伙伴能够对这三个概念的
编程逻辑有更加清晰的理解。
⼀、直⽴环（PD控制器）
1.中⽂公式
直⽴环输出=Kp1×⾓度偏差+Kd×⾓度偏差的微分
//⾓度偏差=真实⾓度-期望⾓度
2.英⽂公式
直⽴环PD控制器：Kp×Ek+Kd×Ek_D
（Ek：⾓度偏差；Ek_D：⾓度偏差的微分）
Ek=真实⾓度-期望⾓度（Angle-Med，由陀螺仪MPU6050测得）
Ek_D=真实⾓速度（gyro_Y，由陀螺仪MPU6050测得）
3.软件编程
根据理论公式进⾏软件编程，相信看完上⾯的讲解后这段代码应该⽐较清晰易懂。
/*****************
直⽴环PD控制器：Kp*Ek+Kd*Ek_D
⼊⼝：Med:机械中值(期望⾓度)，Angle:真实⾓度，gyro_Y:真实⾓速度
出⼝：直⽴环输出
******************/
intVertical(floatMed,floatAngle,floatgyro_Y)
{
intPWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
returnPWM_out;
}
⼆、速度环（PI控制器）
1.中⽂公式
速度环输出=Kp2×电机速度偏差+Ki2×电机速度偏差的积分
//电机速度偏差=真实速度-期望速度
2.英⽂公式
速度环PI控制器：Kp×Ek+Ki×Ek_S
（Ek：电机速度偏差；Ek_S：电机速度偏差的积分）
Ek=真实速度-期望速度（真实速度：左电机速度+右电机速度；期望速度：0）
Ek_S=速度偏差的累加
3.低通滤波
期间需要低频滤波，我们是以直⽴环为主，速度环为辅，速度环相对于直⽴环来说是⼀个⼲扰，最终⽬的是直⽴。低频滤波作⽤是使得
波形更加平滑，滤除⾼频⼲扰，防⽌速度过⼤影响直⽴环正常⼯作。
4.积分限幅
通过⽐较限制积分在规定范围内变动，不得超出。
5.软件编程
根据理论公式进⾏软件编程，相信看完上⾯的讲解后这段代码应该⽐较清晰易懂。
/*****************
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
intVelocity(intTarget,intencoder_left,intencoder_right)
{
//定义成静态变量，保存在静态存储器，使得变量不丢掉
staticintPWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
floata=0.7;
//1.计算速度偏差
//舍去误差--我的理解：能够让速度为"0"的⾓度，就是机械中值。
Encoder_Err=((encoder_left+encoder_right)-Target);
//2.对速度偏差进⾏低通滤波
//low_out=(1-a)*Ek+a*low_out_last
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除⾼频⼲扰，放置速度突变
EnC_Err_Lowout_last=EnC_Err_Lowout;//防⽌速度过⼤影响直⽴环的正常⼯作
//3.对速度偏差积分出位移
Encoder_S+=EnC_Err_Lowout;
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
returnPWM_out;
}
三、转向环
1.中⽂公式
转向环输出=系数×Z轴⾓速度
(Z轴⾓速度由陀螺仪MPU6050测得)
2.软件编程
转向环的编程⽐较简单，我们只需设置⼀个参数调节Z轴⾓速度即可。
/*****************
转向环：系数*Z轴⾓速度
******************/
intTurn(intgyro_Z)
{
intPWM_out;
PWM_out=(-0.6)*gyro_Z;
returnPWM_out;
}
四、控制函数
1、采集编码器数据和MPU6050⾓度信息
编码器数据：左电机速度，右电机速度
（两个电机是相对安装，刚好相差180度，为了编码器输出极性⼀致，就需要对其中⼀个取反）
MPU6050数据：⾓度数据，⾓速度数据，⾓加速度数据
2、将数据压⼊闭环控制中，计算出控制输出量
直⽴环输出
速度环输出
转向环输出
3、把控制输出量加载到电机上，完成最终的控制
左电机输出（编码器放置相对）
右电机输出
限幅
赋值
4、控制中断函数
⾸先要判断是否接受到中断请求，即检测MPU6050的ANT引脚是否处在低电平（即为发⽣中断），然后清除中断标志位，进⾏接下来三
步（即上⾯的三个步骤）。
voidEXTI9_5_IRQHandler(void)
{
intPWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//⼀级判定
{
if(PBin(5)==0)//⼆级判断
{
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//1.采集编码器数据&MPU6050⾓度信息
//电机是相对安装，刚好相差180度，为了编码器输出极性⼀致，就需要对其中⼀个取反
Encoder_Left=-Read_Speed(2);
Encoder_Right=Read_Speed(4);
mpu_dmp_get_data(&Pitch,&Roll,&Yaw);//读取⾓度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);//读取⾓速度
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);//读取加速度
//2.将数据压⼊闭环控制中，计算出控制输出量
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);//速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);//直⽴环
Turn_out=Turn(gyroz);
PWM_out=Vertical_out;//最终输出
//3.把控制输出量加载到电机上，完成最终控制
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2);//PWM限幅
Load(MOTO1,MOTO2);//加载到电机上
}
}
}
五、整个控制函数源代码
1、control.c
#include"control.h"
floatMed_Angle=0;//机械中值，能使得⼩车真正平衡住的⾓度
floatTarget_Speed=0;//期望速度。---⼆次开发接⼝，⽤于控制⼩车前进后退及其速度。
float
Vertical_Kp=0,
Vertical_Kd=0;//直⽴环Kp、Kd
float
float
Velocity_Kp=0,//速度环Kp、Ki（正反馈）
Velocity_Ki=0;
float
Turn_Kp=0;
intVertical_out,Velocity_out,Turn_out;//直⽴环&速度环&转向环的输出变量
intVertical(floatMed,floatAngle,floatgyro_Y);//函数声明
intVelocity(intTarget,intencoder_left,intencoder_right);
intTurn(intgyro_Z);
voidEXTI9_5_IRQHandler(void)
{
intPWM_out;
if(EXTI_GetITStatus(EXTI_Line5)!=0)//⼀级判定
{
if(PBin(5)==0)//⼆级判断
{
EXTI_ClearITPendingBit(EXTI_Line5);//清除中断标志位
//1.采集编码器数据&MPU6050⾓度信息
//电机是相对安装，刚好相差180度，为了编码器输出极性⼀致，就需要对其中⼀个取反
Encoder_Left=-Read_Speed(2);
Encoder_Right=Read_Speed(4);
mpu_dmp_get_data(&Pitch,&Roll,&Yaw);//读取⾓度
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);//读取⾓速度
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);//读取加速度
//2.将数据压⼊闭环控制中，计算出控制输出量
Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right);//速度环
Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);//直⽴环
Turn_out=Turn(gyroz);
PWM_out=Vertical_out;//最终输出
//3.把控制输出量加载到电机上，完成最终控制
MOTO1=PWM_out-Turn_out;//左电机
MOTO2=PWM_out+Turn_out;//右电机
Limit(&MOTO1,&MOTO2);//PWM限幅
Load(MOTO1,MOTO2);//加载到电机上
}
}
}
/*****************
直⽴环PD控制器：Kp*Ek+Kd*Ek_D
⼊⼝：Med:机械中值(期望⾓度)，Angle:真实⾓度，gyro_Y:真实⾓速度
出⼝：直⽴环输出
******************/
intVertical(floatMed,floatAngle,floatgyro_Y)
{
intPWM_out;
PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
returnPWM_out;
}
/*****************
速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
******************/
intVelocity(intTarget,intencoder_left,intencoder_right)
{
//定义成静态变量，保存在静态存储器，使得变量不丢掉
staticintPWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
floata=0.7;
floata=0.7;
//1.计算速度偏差
//舍去误差--我的理解：能够让速度为"0"的⾓度，就是机械中值。
Encoder_Err=((encoder_left+encoder_right)-Target);
//2.对速度偏差进⾏低通滤波
//low_out=(1-a)*Ek+a*low_out_last
EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_last;//使得波形更加平滑，滤除⾼频⼲扰，放置速度突变
EnC_Err_Lowout_last=EnC_Err_Lowout;//防⽌速度过⼤影响直⽴环的正常⼯作
//3.对速度偏差积分出位移
Encoder_S+=EnC_Err_Lowout;
//4.积分限幅
Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
//5.速度环控制输出
PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
returnPWM_out;
}
/*****************
转向环：系数*Z轴⾓速度
******************/
intTurn(intgyro_Z)
{
intPWM_out;
PWM_out=Turn_Kp*gyro_Z;
returnPWM_out;
}