基于STM32的平衡小车设计过程分享（3）

一、简介

续上文，电机驱动、MPU6050驱动均已完成，接下来我们给他加上PID让它站起来

二、PID控制

       我们将小车PID控制函数放到MPU6050中断函数中，每当MPU6050有数据输出时，引脚INT有相应的电平输出。依次来触发外部中断作为控制周期。保持MPU6050数据的实时性。

2.1 直立环控制

       通过传入的KP、KD参数，让平衡车的pitch轴保持在水平位置。

       直立环是用于控制机械系统中垂直方向运动的一种控制器。在这些应用中，垂直方向的运动通常是非常稳定的，因此I控制器的积分作用并不是必需的。而PD控制器可以提供足够的控制性能，同时避免了可能出现的积分饱和问题，因此在直立环中通常只使用PD控制器，而不使用I控制器。

/**************************************************************************

函数功能：直立PD控制

入口参数：角度、机械平衡角度（机械中值）、角速度

返回  值：直立控制PWM

**************************************************************************/

int balance_UP(float Angle,float Mechanical_balance,float Gyro)

{ 

  float Bias;

       int balance;

       Bias=Angle-Mechanical_balance;    //求出平衡的角度中值和机械相关

       balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;  //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数

        return balance;

}

2.2 速度环控制

       这里我们只用到了PI的控制参数，如果有微分环(D)在里面的，系统会响应波动太快，微分对极小的速度波动都会有反应，而速度的微微波动又太多了，什么电机微抖，转速检测装置的不稳定。总结，微分太灵敏了，所以我们只用到了PI作为调节参数

/**************************************************************************

函数功能：速度PI控制

入口参数：电机编码器的值

返回  值：速度控制PWM

**************************************************************************/

int velocity(int encoder_left,int encoder_right,int Target_Speed)

{ 

   static float Velocity,Encoder_Least,Encoder;

       static float Encoder_Integral;

  //=============速度PI控制器=======================//   

       Encoder_Least=(Encoder_Left+Encoder_Right); //获取最新速度偏差==测量速度（左右编码器之和）-目标速度

       Encoder *= 0.8;     //===一阶低通滤波器       

       Encoder += Encoder_Least*0.2;  //===一阶低通滤波器   

       Encoder_Integral +=Encoder; //===积分出位移 积分时间：10ms

       Encoder_Integral=Encoder_Integral-Target_Speed;   //接收遥控器数据，控制前进后退

       if(Encoder_Integral>10000)       Encoder_Integral=10000; //===积分限幅

       if(Encoder_Integral<-10000)       Encoder_Integral=-10000; //===积分限幅       

       Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI; //===速度控制     

    if(pitch<-40||pitch>40)       Encoder_Integral=0;    //===电机关闭后清除积分

       return Velocity;

}

2.3 转向环控制

       在转向控制中，PD控制器代表的是比例-微分控制器，它根据当前的偏差和偏差的变化率来调整控制参数，以实现快速响应和稳定性。

       相比之下，积分控制器（I控制器）会根据偏差的积分来调整控制参数，以减小系统的稳态误差。然而，在转向控制中，由于小车的动态响应和环境的不确定性，I控制器容易出现积分饱和的问题，从而导致系统不稳定和震荡。

       因此，为了保证转向控制的稳定性和可靠性，通常只使用PD控制器，而不使用I控制器。同时，还可以通过其他控制策略，如模糊控制、自适应控制等来进一步提高控制性能。

/**************************************************************************

函数功能：转向PD控制

入口参数：电机编码器的值、Z轴角速度

返回  值：转向控制PWM

**************************************************************************/



int Turn_UP(int gyro_Z, int RC)

{

       int PWM_out;

       /*转向约束*/

       if(RC==0)Turn_Kd=TURN_KD;//若无左右转向指令，则开启转向约束

       else Turn_Kd=0;//若左右转向指令接收到，则去掉转向约束

       PWM_out=Turn_Kd*gyro_Z + Turn_KP*RC;

       return PWM_out;

}

2.4 PID参数的调节

通过调节PID的参数，使小车达到稳定状态，该数据仅供参考，具体调节还得从实际中得来

//直立环

#define BLC_KP 300

#define BLC_KD 0.6

//速度环

#define SPD_KP -80

#define SPD_KI -0.40

//转向环

#define TURN_KP -30

#define TURN_KD -0.3

审核编辑：汤梓红