【内含代码】PID平衡控制在智能巡逻机器人上的应用

姿态平衡控制

在直立控制里面加入速度负反馈无法达到速度闭环的目的，而且还会破坏直立控制系统，因此在保证直立控制的优先级条件下，开发者们要把速度控制放在直立控制的前面，也就是速度控制调节的结果仅仅是改变直立控制的目标值。

根据经验可知，小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度，就需要增加小车向前倾斜的角度，倾斜角度加大之后，车轮在直立控制的作用下需要向前运动保持小车平衡，速度增大；如果要降低小车向前行驶的速度，就需要减小小车向前倾斜的角度，倾斜角度减小之后，车轮在直立控制的作用下向后运动保持小车平衡，速度减小。

开发者们把速度和直立两个控制器串联起来工作，其中速度控制的输出作为直立控制的输入，而直立控制的输出作为系统的输出，这其实就是一个串级控制系统。其中直立控制使用PD控制。因为编码器可能存在的噪声，为防止噪声被放大并消除系统的静差，这里速度控制使用PI控制。

直立PD控制

int balance(float Angle,float Gyro)
{  
     float Bias;
     int balancePID;
     Bias=Angle-Angle_OFFSET;                       //===求出平衡的角度中值 和机械相关
     balancePID=Balance_Kp*Bias+Gyro*Balance_Kd;   //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
     return balancePID;
}

速度PI控制

int velocity(int encoder_left,int encoder_right)
{  
    static float Velocity,Encoder_Least,Encoder,Movement;
    static float Encoder_Integral,Target_Velocity;
    //=============遥控前进后退部分=======================// 
    Target_Velocity=40;                 
    if(Direction.Current==GO_STRAIGHT)      Movement=-Target_Velocity/Flag_speed;           //===前进标志位置1 
    else if(Direction.Current==GO_BACK) Movement=Target_Velocity/Flag_speed;         //===后退标志位置1
    else  Movement=0;   
    //=============速度PI控制器=======================// 
    Encoder_Least =(encoder_left+encoder_right)-0;                   
    Encoder *= 0.8;                                                     //===一阶低通滤波器       
    Encoder += Encoder_Least*0.2;                                       //===一阶低通滤波器    
    Encoder_Integral +=Encoder;                                       //===积分出位移 积分时间：10ms
    Encoder_Integral=Encoder_Integral-Movement;                       //===接收遥控器数据，控制前进后退
    if(Encoder_Integral>8000)   Encoder_Integral=8000;             //===积分限幅
    if(Encoder_Integral<-8000)  Encoder_Integral=-8000;              //===积分限幅  
    Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;        //===速度控制 
    if(Turn_Off(Angle_Balance,BAT_VOL)==1||Direction.Current==TURN_OFF)   
      Encoder_Integral=0;      
    return Velocity;
}

转向控制

除了保持平衡之外，小车也涉及到左右转动，因此还需要加入转向的控制，可参考如下

int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
    static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; 
    float Turn_Amplitude=30/Flag_speed,Kp=32,Kd=0;  
    if(Direction.Current==TURN_LEFT||Direction.Current==TURN_RIGHT)                      
        {
            if(++Turn_Count==1)
            Encoder_temp=myabs(encoder_left+encoder_right);
            Turn_Convert=50/Encoder_temp;
            if(Turn_Convert<0.6)Turn_Convert=0.6;
            if(Turn_Convert>3)Turn_Convert=3;
        }   
      else
        {
            Turn_Convert=0.9;
            Turn_Count=0;
            Encoder_temp=0;
        }           
        if(Direction.Current==TURN_LEFT){
            Turn_Target+=Turn_Convert;
        }
        else if(Direction.Current==TURN_RIGHT){
            Turn_Target-=Turn_Convert; 
        }
        else Turn_Target=0;
    
    if(Turn_Target>Turn_Amplitude)  Turn_Target=Turn_Amplitude;    //===转向速度限幅
      if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
        if(Direction.Current==GO_STRAIGHT||Direction.Current==GO_BACK||Direction.Current==KEEP_STOP)  
          Kd=-1 ;        
        else 
          Kd=0;   
    //=============转向PD控制器=======================//
    Turn=-Turn_Target*Kp-gyro*Kd;                 //===结合Z轴陀螺仪进行PD控制
      return Turn;
}

电机PWM控制

通过上述一系列控制计算后，开发者就得到了使小车平衡的PWM值（即下面程序中的Moto1和Moto2）。

    Balance_Pwm = balance(Angle_Balance,Gyro_Balance);//===平衡PID控制  
    Velocity_Pwm= velocity(Enconder_left,Enconder_right);//===速度环PID控制
    Turn_Pwm   = turn(Enconder_left,Enconder_right,Gyro_Turn);//===转向环PID控制 
    Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm;//===计算左轮电机最终PWM
    Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm;//===计算右轮电机最终PWM
    Limit_Pwm();                            //===PWM限幅  
    Set_Pwm(Moto1,Moto2);                   //===赋值给PWM寄存器 

将该PWM幅值给对应寄存器，就可以查看小车实际运动状态了。

void Set_Pwm(int moto1,int moto2)
{     
    if(moto2>0)     {AIN2_RESET;AIN1_SET;}
    else            {AIN2_SET;AIN1_RESET;}      
    TIM16_PWM_Set(myabs(moto2));
​
    if(moto1>0) {BIN1_RESET;BIN2_SET;}
    else        {BIN1_SET;BIN2_RESET;}
    TIM17_PWM_Set(myabs(moto1));
}
​