(六)【平衡小车制作】位置式PID、直立环与速度环编程

news/2024/3/4 9:17:20

本篇文章我将针对位置式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_lastEnC_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_lastEnC_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、直立环与速度环软件编程讲解,平衡小车系列文章作者在持续更新中。若文章中出现错误或者小伙伴对以上内容有所疑问,欢迎大家在评论区留言,小政看到后会尽快回复大家!


http://www.ppmy.cn/news/62793.html

相关文章

JMeter开发web及手机APP自动化脚本练习

&#xff08;一&#xff09;开发web自动化脚本练习 一、打开浏览器代理服务器设置 我这里用的是360浏览器&#xff0c;打开浏览器代理服务器设置&#xff0c;端口要与jmeter中的端口设置保持一致哦。 二、JMeter设置代理 JMeter设置代理&#xff08;jmeter中的端口要与360浏览…

【设计模式】抽象工厂模式

【设计模式】抽象工厂模式 参考资料&#xff1a; Java设计模式 - 抽象工厂模式 重学 Java 设计模式&#xff1a;实战抽象工厂模式 文章目录 【设计模式】抽象工厂模式一、抽象工厂模式介绍1.1、什么是工厂方法模式1.2、角色概述 二、案例场景模拟2.1、背景一&#xff1a;Redis…

如何创建可引导的 ESXi USB 安装介质 (macOS, Linux, Windows)

如何创建可引导的 ESXi USB 安装介质 (macOS, Linux, Windows) 如何制作 ESXi USB 启动盘 请访问原文链接&#xff1a;https://sysin.org/blog/create-bootable-esxi-usb-installer/&#xff0c;查看最新版。原创作品&#xff0c;转载请保留出处。 作者主页&#xff1a;sysi…

【SpringCloud】初始微服务

目录 一、单体架构 1、概念 2、优点 3、缺点 二、分布式架构 1、概念 2、优点 3、缺点 三、微服务 1、概念 2、优点 3、缺点 四、微服务技术对比 五、SpringCloud 六、服务拆分 1、注意事项 2、服务远程调用 一、单体架构 1、概念 业务的所有功能都集中到一个…

Triloga 的任务 — Satta 系列来袭!

谁战胜了这些凶兽&#xff0c;谁就获得了力量&#xff0c;让我们通过装备体现出来&#xff01;来自神秘洞穴的疯狂昆虫外壳&#xff0c;黄昏之地燃烧中的部落的骨质盔甲&#xff0c;以及深海的美妙灯光。 Triloga 的任务——Satta 系列已在 The Sandbox 市场平台上架&#xff1…

【Java-05】常用API、正则表达式、Collection集合

主要内容 BigInteger类BigDecimal类Arrays类包装类String类的常用方法正则表达式Collection集合 1 BigInteger类 1.1 概述 概述 : java.math.BigInteger类是一个引用数据类型 , 可以用于计算一些大的整数 , 当超出基本数据类型数据范围的整数运算时就可以使用BigInteger了。…

java中枚举类型使用详解

在Java中&#xff0c;枚举类型&#xff08;Enumeration&#xff09;是一种特殊的数据类型&#xff0c;它允许程序员定义一个有限的、可枚举的数据集。枚举类型的定义通常在类的内部&#xff0c;在类的外部使用。 以下是一些关于Java中枚举类型使用的详细说明&#xff1a; 定义…

2023/5/8总结

JAVA基础知识&#xff08;2&#xff09; 1.方法 1、方法定义 格式&#xff1a;public static void 方法名&#xff08;&#xff09;{ //方法体 } 2、方法调用 格式&#xff1a;方法名&#xff08;&#xff09;&#xff1b; 3、方法的通用格式 public static 返回值类型方法名&…

业务连续性

业务连续性 业务连续性管理业务连续性的概念业务连续性计划建设过程组织管理业务影响分析&#xff08;BIA&#xff09;BIA-1确定业务优先级BIA-2风险分析BIA-3资产优先级划分 制定及批准实施制定及批准实施-风险降低制定及批准实施-风险转移制定及批准实施-风险规避与风险接受制…

计算机专业还有前途吗?从业9年的程序员告诉你优缺点

选专业如果你想选择计算机专业 我就从过来人的角度给你些建议 我从专科读到本科又读了在职研究生 正好覆盖面还算比较广 这里要做个限定 计算机专业并非是专业名 是一个类别 我专科读的是网络系统管理 没错就是网管 本科读的是计算机科学与技术 研究生读的是软件工程 本视频后面…

今年的面试难度有点大....

大家好&#xff0c;最近有不少小伙伴在后台留言&#xff0c;又得准备面试了&#xff0c;不知道从何下手&#xff01; 不论是跳槽涨薪&#xff0c;还是学习提升&#xff01;先给自己定一个小目标&#xff0c;然后再朝着目标去努力就完事儿了&#xff01; 为了帮大家节约时间&a…

1062 Talent and Virtue

About 900 years ago, a Chinese philosopher Sima Guang wrote a history book in which he talked about peoples talent and virtue. According to his theory, a man being outstanding in both talent and virtue must be a "sage&#xff08;圣人&#xff09;"…

基于 R 语言 APSIM 模型高级应 用及批量模拟实践技术

随着数字农业和智慧农业的发展&#xff0c;基于过程的农业生产系统模型在模拟作物对气候变化的响应与适应、农田管理优化、作物品种和株型筛选、农田固碳和温室气体排放等领域扮演着越来越重要的作用。APSIM (Agricultural Production Systems sIMulator)模型是世界知名的作物生…

用DG备库做的rman备份恢复一个数据库

环境描述&#xff1a; 1.因为主库存储空间不足&#xff0c;于是将备份放在dg备库上做。 2.主库因为磁盘空间问题&#xff0c;数据文件有两个目录。 3.dg备库因为主库两个数据文件目录里面有两个同名数据文件&#xff0c;所有dg备库也有两个数据文件目录。 4.主库与备库与测…

经典常用的脚本讲解

目录 一&#xff1a;echo 语句 二&#xff1a;while read命令​编辑 三&#xff1a;猴子摘香蕉问题 四:斐波拉切数求前10个数的和 ​五&#xff1a;随机生成8位数的密码 六&#xff1a;二进制转换 &#xff08;1&#xff09;余数倒排法 &#xff08;2&#xff09;减法正…

数组排序sort()方法

sort() 方法对数组的项目进行排序。 排序顺序可以是按字母或数字&#xff0c;也可以是升序&#xff08;向上&#xff09;或降序&#xff08;向下&#xff09;。 默认情况下&#xff0c;sort() 方法将按字母和升序将值作为字符串进行排序。 一、语法 array.sort(compareFunct…

【SWAT水文模型】SwatWeather软件使用教程

SwatWeather软件使用教程 1 SwatWeather天气模型发生器1.1 数据输入 2 各功能介绍2.1 计算降水2.2 计算气温2.3 计算辐射2.4 计算风速2.5 计算露点 参考 1 SwatWeather天气模型发生器 SwatWeather.exe 软件只要输入一定格式要求的文件&#xff0c;就可以根据提示进行所需 数据…

React初识

概述 React 是用于构建用户界面的 JS 库。React 是 MVC 中薄薄的一层 V&#xff0c;把数据变成 DOM 显示出来&#xff0c;它只关注表现层。React 原生就是函数 虚拟 DOM 特点 单向数据流 兼容性更好&#xff1a;支持IE8 JSX语法&#xff1a;处理数据和 DOM 之间的关系&…

画图以及代码分析结合的循环队列的实现

循环队列的实现 概念以及结构实现初始化判空判满入队出队从队头获得元素从队尾获得元素释放 概念以及结构 循环队列是一种线性数据结构&#xff0c;其操作表现基于 FIFO&#xff08;先进先出&#xff09;原则并且队尾被连接在队首之后以形成一个循环。它也被称为“环形缓冲器”…

17.provide.inject.下

学习要点&#xff1a; 1.provide.inject 本节课我们来要了解一下 Vue3.x 中的 provide.inject(提供.注入)功能&#xff1b; 一&#xff0e;provide.inject 1. 使用 Composition API&#xff0c;也是使用 import 导入来实现的&#xff1b; // App.vue import { ref, provide } f…
最新文章