devbox
寸_铁
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

（六）【平衡小车制作】位置式PID、直立环与速度环编程_平衡小车pid算法程序

作者：寸_铁 | 2024-07-27 13:57:47

赞

踩

平衡小车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.软件编程
  根据理论公式进行软件编程，相信看完上面的讲解后这段代码应该比较清晰易懂。

  1. /*****************  
  2. 直立环PD控制器：Kp*Ek+Kd*Ek_D
  3. 入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
  4. 出口：直立环输出
  5. ******************/
  6. int Vertical(float Med,float Angle,float gyro_Y) 
  7. {
  8.   int PWM_out;
  9.   
  10.   PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  11.   
  12.   return PWM_out;
  13. }

 

二、速度环（PI控制器）


1.中文公式

 速度环输出=Kp2×电机速度偏差+Ki2×电机速度偏差的积分
 // 电机速度偏差=真实速度-期望速度

2.英文公式

 速度环PI控制器：Kp×Ek+Ki×Ek_S
 （Ek：电机速度偏差；Ek_S：电机速度偏差的积分）

Ek=真实速度-期望速度（真实速度：左电机速度+右电机速度；期望速度：0）
Ek_S=速度偏差的累加
3.低通滤波

  期间需要低频滤波，我们是以直立环为主，速度环为辅，速度环相对于直立环来说是一个干扰，最终目的是直立。低频滤波作用是使得波形更加平滑，滤除高频干扰，防止速度过大影响直立环正常工作。

4.积分限幅

  通过比较限制积分在规定范围内变动，不得超出。

5.软件编程

  根据理论公式进行软件编程，相信看完上面的讲解后这段代码应该比较清晰易懂。

  1. /*****************  
  2. 速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
  3. ******************/
  4. int Velocity(int Target,int encoder_left,int encoder_right)
  5. {
  6.   // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  7.   static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  8.   float a=0.7;
  9.   
  10.   // 1.计算速度偏差
  11.   //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  12.   Encoder_Err = ((encoder_left+encoder_right)-Target);
  13.   
  14.   // 2.对速度偏差进行低通滤波
  15.   // low_out = (1-a)*Ek+a*low_out_last
  16.   EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  17.   EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  18.   
  19.   // 3.对速度偏差积分出位移
  20.   Encoder_S+=EnC_Err_Lowout;
  21.   
  22.   // 4.积分限幅
  23.   Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  24.   
  25.   // 5.速度环控制输出
  26.   PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  27.   
  28.   return PWM_out;
  29. }

三、转向环

1.中文公式

 转向环输出=系数×Z轴角速度
 (Z轴角速度由陀螺仪MPU6050测得)

2.软件编程

  转向环的编程比较简单，我们只需设置一个参数调节Z轴角速度即可。

  1. /*****************  
  2. 转向环：系数*Z轴角速度
  3. ******************/
  4. int Turn(int gyro_Z)
  5. {
  6.   int PWM_out;
  7.   
  8.   PWM_out = (-0.6)*gyro_Z;
  9.   
  10.   return PWM_out;
  11. }

四、控制函数


1、采集编码器数据和MPU6050角度信息

 编码器数据：左电机速度，右电机速度
（两个电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反）
 MPU6050数据：角度数据，角速度数据，角加速度数据
2、将数据压入闭环控制中，计算出控制输出量

 直立环输出
 速度环输出
 转向环输出
3、把控制输出量加载到电机上，完成最终的控制

 左电机输出（编码器放置相对）
 右电机输出
 限幅
 赋值
4、控制中断函数
 首先要判断是否接受到中断请求，即检测MPU6050的ANT引脚是否处在低电平（即为发生中断），然后清除中断标志位，进行接下来三步（即上面的三个步骤）。
 

  1. void EXTI9_5_IRQHandler(void)
  2. {
  3.   int PWM_out;
  4.   if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  5.   {
  6.     if(PBin(5)==0)    // 二级判断
  7.     { 
  8.       EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
  9.       // 1.采集编码器数据&MPU6050角度信息
  10.       // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
  11.       Encoder_Left  = -Read_Speed(2); 
  12.       Encoder_Right = Read_Speed(4);
  13.       
  14.       mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
  15.       MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
  16.       MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
  17.       // 2.将数据压入闭环控制中，计算出控制输出量
  18. 			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
  19.       Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
  20. 			Turn_out=Turn(gyroz);	
  21.       
  22.       PWM_out=Vertical_out;//最终输出
  23.       
  24.       // 3.把控制输出量加载到电机上，完成最终控制
  25.       MOTO1 = PWM_out-Turn_out; // 左电机
  26.       MOTO2 = PWM_out+Turn_out; // 右电机
  27.       Limit(&MOTO1,&MOTO2);     // PWM限幅
  28.       Load(MOTO1,MOTO2);        // 加载到电机上
  29.     }
  30.   }
  31. }

五、整个控制函数源代码

1、control.c

  1. #include "control.h"
  2.  
  3. float Med_Angle=0;      // 机械中值，能使得小车真正平衡住的角度 
  4. float Target_Speed=0;	  // 期望速度。---二次开发接口，用于控制小车前进后退及其速度。
  5. float 
  6.   Vertical_Kp=0,
  7.   Vertical_Kd=0;     // 直立环Kp、Kd
  8. float 
  9.   Velocity_Kp=0,     // 速度环Kp、Ki（正反馈）
  10.   Velocity_Ki=0;
  11. float 
  12.   Turn_Kp=0;
  13.  
  14. int Vertical_out,Velocity_out,Turn_out; // 直立环&速度环&转向环的输出变量
  15.  
  16. int Vertical(float Med,float Angle,float gyro_Y); // 函数声明
  17. int Velocity(int Target,int encoder_left,int encoder_right);
  18. int Turn(int gyro_Z);
  19.  
  20. void EXTI9_5_IRQHandler(void)
  21. {
  22.   int PWM_out;
  23.   if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一级判定
  24.   {
  25.     if(PBin(5)==0)    // 二级判断
  26.     { 
  27.       EXTI_ClearITPendingBit(EXTI_Line5); // 清除中断标志位
  28.       // 1.采集编码器数据&MPU6050角度信息
  29.       // 电机是相对安装，刚好相差180度，为了编码器输出极性一致，就需要对其中一个取反
  30.       Encoder_Left  = -Read_Speed(2); 
  31.       Encoder_Right = Read_Speed(4);
  32.       
  33.       mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 读取角度
  34.       MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 读取角速度
  35.       MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 读取加速度
  36.       // 2.将数据压入闭环控制中，计算出控制输出量
  37. 			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度环
  38.       Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立环
  39. 			Turn_out=Turn(gyroz);	
  40.       
  41.       PWM_out=Vertical_out;//最终输出
  42.       
  43.       // 3.把控制输出量加载到电机上，完成最终控制
  44.       MOTO1 = PWM_out-Turn_out; // 左电机
  45.       MOTO2 = PWM_out+Turn_out; // 右电机
  46.       Limit(&MOTO1,&MOTO2);     // PWM限幅
  47.       Load(MOTO1,MOTO2);        // 加载到电机上
  48.     }
  49.   }
  50. }
  51.  
  52. /*****************  
  53. 直立环PD控制器：Kp*Ek+Kd*Ek_D
  54.  
  55. 入口：Med:机械中值(期望角度)，Angle:真实角度，gyro_Y:真实角速度
  56. 出口：直立环输出
  57. ******************/
  58. int Vertical(float Med,float Angle,float gyro_Y) 
  59. {
  60.   int PWM_out;
  61.   
  62.   PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  63.   
  64.   return PWM_out;
  65. } 
  66.  
  67. /*****************  
  68. 速度环PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的积分)
  69. ******************/
  70. int Velocity(int Target,int encoder_left,int encoder_right)
  71. {
  72.   // 定义成静态变量，保存在静态存储器，使得变量不丢掉
  73.   static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  74.   float a=0.7;
  75.   
  76.   // 1.计算速度偏差
  77.   //舍去误差--我的理解：能够让速度为"0"的角度，就是机械中值。
  78.   Encoder_Err = ((encoder_left+encoder_right)-Target);
  79.   // 2.对速度偏差进行低通滤波
  80.   // low_out = (1-a)*Ek+a*low_out_last
  81.   EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，滤除高频干扰，放置速度突变
  82.   EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度过大影响直立环的正常工作
  83.   // 3.对速度偏差积分出位移
  84.   Encoder_S+=EnC_Err_Lowout;
  85.   // 4.积分限幅
  86.   Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  87.   
  88.   // 5.速度环控制输出
  89.   PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  90.   
  91.   return PWM_out;
  92. }
  93.  
  94. /*****************  
  95. 转向环：系数*Z轴角速度
  96. ******************/
  97. int Turn(int gyro_Z)
  98. {
  99.   int PWM_out;
  100.   
  101.   PWM_out = Turn_Kp*gyro_Z;
  102.   
  103.   return PWM_out;
  104. }
  105.

以上就是平衡小车系列文章第六讲——位置式PID、直立环与速度环软件编程讲解，平衡小车系列文章作者在持续更新中。若文章中出现错误或者小伙伴对以上内容有所疑问，欢迎大家在评论区留言，小政看到后会尽快回复大家！

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/寸_铁/article/detail/890428
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号