【平衡小车制作】(六)位置式PID、直立环与速度环编程(超详解)
内容导读
互联网集市收集整理的这篇技术教程文章主要介绍了【平衡小车制作】(六)位置式PID、直立环与速度环编程(超详解),小编现在分享给大家,供广大互联网技能从业者学习和参考。文章包含7117字,纯文字阅读大概需要11分钟。
内容图文
![【平衡小车制作】(六)位置式PID、直立环与速度环编程(超详解)](/upload/InfoBanner/zyjiaocheng/605/fbf2fa2cabb545feabbf0fea9be7ba6b.jpg)
??大家好,我是小政。本篇文章我将针对位置式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:真实角速度
出口:直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
return PWM_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:偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
// 定义成静态变量,保存在静态存储器,使得变量不丢掉
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
float a=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;
return PWM_out;
}
三、转向环
1.中文公式
?转向环输出=系数×Z轴角速度
?(Z轴角速度由陀螺仪MPU6050测得)
2.软件编程
??转向环的编程比较简单,我们只需设置一个参数调节Z轴角速度即可。
/*****************
转向环:系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
int PWM_out;
PWM_out = (-0.6)*gyro_Z;
return PWM_out;
}
四、控制函数
1、采集编码器数据和MPU6050角度信息
- ?编码器数据:左电机速度,右电机速度
(两个电机是相对安装,刚好相差180度,为了编码器输出极性一致,就需要对其中一个取反) - ?MPU6050数据:角度数据,角速度数据,角加速度数据
2、将数据压入闭环控制中,计算出控制输出量
- ?直立环输出
- ?速度环输出
- ?转向环输出
3、把控制输出量加载到电机上,完成最终的控制
- ?左电机输出(编码器放置相对)
- ?右电机输出
- ?限幅
- ?赋值
4、控制中断函数
?首先要判断是否接受到中断请求,即检测MPU6050的ANT引脚是否处在低电平(即为发生中断),然后清除中断标志位,进行接下来三步(即上面的三个步骤)。
void EXTI9_5_IRQHandler(void)
{
int PWM_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"
float Med_Angle=0; // 机械中值,能使得小车真正平衡住的角度
float Target_Speed=0; // 期望速度。---二次开发接口,用于控制小车前进后退及其速度。
float
Vertical_Kp=0,
Vertical_Kd=0; // 直立环Kp、Kd
float
Velocity_Kp=0, // 速度环Kp、Ki(正反馈)
Velocity_Ki=0;
float
Turn_Kp=0;
int Vertical_out,Velocity_out,Turn_out; // 直立环&速度环&转向环的输出变量
int Vertical(float Med,float Angle,float gyro_Y); // 函数声明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z);
void EXTI9_5_IRQHandler(void)
{
int PWM_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:真实角速度
出口:直立环输出
******************/
int Vertical(float Med,float Angle,float gyro_Y)
{
int PWM_out;
PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
return PWM_out;
}
/*****************
速度环PI控制器:Kp*Ek+Ki*Ek_S(Ek_S:偏差的积分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
// 定义成静态变量,保存在静态存储器,使得变量不丢掉
static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
float a=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;
return PWM_out;
}
/*****************
转向环:系数*Z轴角速度
******************/
int Turn(int gyro_Z)
{
int PWM_out;
PWM_out = Turn_Kp*gyro_Z;
return PWM_out;
}
??以上就是平衡小车系列文章第六讲——位置式PID、直立环与速度环软件编程讲解,若文章中出现错误或者小伙伴对以上内容有所疑问,欢迎大家在评论区留言,小政看到后会尽快回复大家!
内容总结
以上是互联网集市为您收集整理的【平衡小车制作】(六)位置式PID、直立环与速度环编程(超详解)全部内容,希望文章能够帮你解决【平衡小车制作】(六)位置式PID、直立环与速度环编程(超详解)所遇到的程序开发问题。 如果觉得互联网集市技术教程内容还不错,欢迎将互联网集市网站推荐给程序员好友。
内容备注
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 gblab@vip.qq.com 举报,一经查实,本站将立刻删除。
内容手机端
扫描二维码推送至手机访问。