位置式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:真实角速度
出口：直立环输出
******************/
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;
}