【平衡小车制作】（六）位置式PID、直立环与速度环编程（超详解）

最新推荐文章于 2024-07-06 17:54:40 发布
最新推荐文章于 2024-07-06 17:54:40 发布
阅读量4.1w 收藏 1.2k
点赞数 161
分类专栏： 嵌入式 文章标签： stm32 pid 嵌入式 单片机
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_44270218/article/details/113665051
版权

  大家好，我是小政。本篇文章我将针对位置式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调参及平衡成果展示（超详解）https://blog.csdn.net/weixin_44270218/article/details/113786386

鲁乎乎
关注
专栏目录
一起玩儿平衡车（ESP32）——08 直立控制环PID参数的调试
一起玩儿科技
03-08 1396
摘要：本文介绍如何调整平衡车直立控制环PID参数
两轮平衡车简单串级PID
06-03
本人大一 最近做了两轮平衡小车主控芯片为STM32F103RCT6，局限于本人大一能力有限，但是我决定将他传上来赚点积分去下载我需要的东西，需要的可以下载，注解挺详细的。也可私聊本QQ:1006348650
自平衡车PID 直立环/平衡环+速度环+转向环 代码整定，完整代码程序
04-21
自平衡车PID 直立环/平衡环+速度环+转向环 代码整定，完整代码程序 .
平衡小车调试指南(直立环-速度环)1
08-03
1.1 平衡小车直立控制调试 1.1.1 确定平衡小车的机械中值 1.1.2 确定 kp 值的极性（令 kd=0） 1.1.3 确定 kp 值的大小（令 kd=
MiniFly微型四轴学习开发日志（二）——四轴PID算法
最新发布
木牛的博客
07-06 71
位置式PID与增量式PID区别浅析_增量式和位置式pid的优缺点-CSDN博客位置式PID是对系统当前的实际位置与你想达到的语气位置的偏差进行校正的PID控制。其公式为由公式看出，其输出与过去所有状态有关，积分项为误差的累加值，输出u(k)因此对应执行器的实际位置，一旦输出大幅度变化，系统将会剧变。另一方面，积分项饱和时，误差仍在积分作用下累计，一旦误差开始反向变化，系统需要时间从饱和区退出。因而输出到达最小最大时，应停止积分作用，进行积分限幅和输出限幅。以下为算法基本例程：}PID;
平衡车PID代码
03-17
平衡车PID代码
学习记录--PID（角度双环）
weixin_52645147的博客
07-15 8914
一，理论理解 参考：从不懂到会用！PID从理论到实践~_哔哩哔哩_bilibili 1. 三个参数 Kp：比例系数：pid->p_out = pid->kp * pid->err[0];（p项输出为kp*（本次误差））可知Kp可调曲线的斜率，但大了会很跳跃 Ki：积分系数：pid->i_out += pid->ki * pid->err[0];（i项输出为ki*（所有误差的累积））可知Ki可以使受控目标达到target。由控制无人机悬停高度的例子可知，只由kp..
平衡小车调节PID参数【详细版】
m0_74732150的博客
03-19 6170
平衡小车调节PID参数【详细版】
两轮平衡车资料(stm32+mpu6050 的PID控制方案)
12-22
很久之前做的一些两轮平衡车资料，主要是一些文档，也有部分的驱动代码
自平衡车PID 直立环+速度环 代码整定，完整代码程序
04-21
自平衡车PID 直立环+速度环 代码整定，完整代码程序 .
PID算法（本人自写验证可用平衡车）
02-17
void PID init { PID e0 0; PID e1 0; PID e2 0; PID x chuzhi 28000; PID x out 28000; PID y shedingzhi 0; PID kp 100; PID ki 28; PID kd 10000; PID2 e0 0; PID2 e1 0; PID2 e2 0; PID2 x chuzhi 27000; PID2 x out 27000; PID2 y shedingzhi 0; PID2 kp 1; PID2 ki 40; PID2 kd 100; }
arduino开源平衡车源代码
09-08
如何在家自己动手做一个arduino平衡车？简单的源代码分享，优酷有我的视频！
平衡车代码/智能
09-11
基于stm32的平衡车代码/电机540/驱动为btn7960.。
平衡车代码.rar_两轮平衡_两轮自平衡_平衡小车_平衡车_自平衡车代码
07-15
使用STM32103C8T6实现两轮自平衡小车。
自平衡车PID 直立环/平衡环 代码整定，完整代码程序
04-21
自平衡车PID 直立环/平衡环 代码整定，完整代码程序 .
平衡小车PID学习源码
07-24
平衡小车PID学习源码，PID算法是自平衡小车控制电机的核心，同时PID也被广泛应用于很多项目中，本源码书写规范，注释详细，是学习自平衡小车以及电机PID很好的参考资料。
二轮平衡小车的平衡原理详解
02-08
二轮平衡小车的平衡原理详解 一、平衡小车的平衡原理 平衡小车是通过两个电机运动下实现小车不倒下直立行走的多功能智能小车。在外力的推拉下，小车依然保持不倒下。这个平衡小车实现的原理其实是在人们生活中的...
2，Big-blance程序_Big!_originubm_自平衡小车_PID电机_
10-03
《自平衡小车开发：Big-blance程序与PID电机控制详解》 自平衡小车，作为机器人技术领域的一个热门研究对象，一直以来都吸引着众多爱好者和工程师的目光。它以其独特的直立行驶能力和灵活的遥控操作，成为了技术...
平衡小车速度控制原理
09-18
### 平衡小车速度控制原理详解 #### 一、引言 平衡小车作为一种常见的自动化设备，在机器人学中有着广泛的应用。它不仅涉及到机械结构的设计，还涉及到复杂的控制算法，尤其是如何确保小车能够在保持直立的同时...
用ch32v307控制实现智能车竞赛中单车的直立，写个工程出来，使用自平衡串级pid控制无刷电机，一个无刷电机有一个pwm口，一个DIR电机方向输出口，一个SPDOUT编码器计时端口，一个DIROUT编码器方向采值口，其中串级PID在这个控制模型上面分为三个环，内环速度环1ms一次，中间环角度环5ms一次，外环速度环10ms一次。同时通过空转比来控制电机转速。串级PID的每一个环都采用的位置式PID来计算的。请写出完成控制代码
07-10
根据您的要求，以下是一个基本的控制代码框架，使用CH32V307控制单车的直立，实现自平衡串级PID控制无刷电机。请注意，以下代码仅为示例框架，您需要根据实际情况进行相应的修改和完善。 ```c // 引入所需库 // 定义常量 // 定义变量 // 初始化函数 // 主函数 int main() { // 初始化 while (1) { // 获取传感器数据（角度、角速度、编码器计数等） // 计算速度环PID控制输出 // 每1ms执行一次速度环控制 if (Time_1ms_Flag) { // 更新速度环控制信号 // 控制电机方向和PWM输出 // 控制速度环PID参数 Time_1ms_Flag = 0; } // 每5ms执行一次角度环控制 if (Time_5ms_Flag) { // 计算角度环PID控制输出 // 控制角度环PID参数 Time_5ms_Flag = 0; } // 每10ms执行一次外环速度环控制 if (Time_10ms_Flag) { // 计算外环速度环PID控制输出 Time_10ms_Flag = 0; } } return 0; } // 速度环PID计算函数 // 角度环PID计算函数 // 外环速度环PID计算函数 // 速度环PID参数调节函数 // 角度环PID参数调节函数 // 外环速度环PID参数调节函数 ``` 请注意，以上代码仅为一个示例框架，并没有包含具体的实现细节和函数调用。您需要根据实际情况添加传感器读取、PID计算、电机控制等功能，并根据串级PID的要求设置不同的控制频率。同时，您需要根据CH32V307的具体使用方法，配置相应的GPIO口、PWM输出、编码器接口等。 此外，根据您的需求，您可能还需要了解单车直立控制、自平衡串级PID控制、无刷电机控制、编码器读取等相关知识，以便正确实现您的项目。
评论 48
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

鲁乎乎

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值