恩智浦杯智能汽车大赛—直立车模实现原理（mpu6050控制）

本人2019年参赛，做的是双车组的直立，由一开始的什么都不会，到最后能看到自己做的小车可以上赛道。觉得自己对平衡车的理解还可以，在这里跟大家分享一下做直立的心得。

直立车模的调试分为三个部分：1.直立环 2.速度环3.转向环
有人说速度环可以省略，我认为在你的转向环调得好的情况下可以省略，因为失去了速度环的车模就会一直处于加速状态（在弯道时由于摩擦的作用会使速度降下来），这就会导致在直道入弯时的速度无法控制，在转向环不强的时候很容易飞出赛道。

直立环

先上直立环的代码，当然这种代码网上很多，形式也都差不多。

/**************************************************************************
函数功能：小车平衡电机占空比控制，直立PD控制
入口参数：倾角角度、角速度
返回  值：平衡控制用PWM
**************************************************************************/
int Balance_Ctrl_Pwm(float Ang,float Gyro)
{
    float Bias=Ang-QingJiaoZhongZhi;        //中值，求出平衡的角度中值 和机械相关
    int balance;
    float kp = 42, kd = - 26; 
    balance= -(int)(kp*Bias+Gyro*kd);   //计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数 
    
    return balance;
}

先说一下变量的意思
QingJiaoZhongZhi // 小车平衡时候的数值，关闭电机，用手使得小车正好平衡时候的
float Ang,float Gyro //这两个是mpu6050卡尔曼滤波后的倾角角度、角速度。

然后再来说一下P,D两个参数的调试。
先调试kp，需要把kd置为0。首先需要确定正负，我们先随便给一个正值例如kp=10吧。然后我们相对于车模的平衡角度倾斜车模，如果车轮的转向和你倾斜车模的方向一致则可以确定kp应取正值，否则kp取负值。然后再来确定大小 逐渐增大Kp直至车身出现大幅低频摇摆的直立在地上，然后把这个数值乘以0.6基本就是一个比较稳定kp值。调试kd时同样需要把kp置为0。首先需要确定正负：例如先给他个kd=2；当你转动车模时车轮的转向与你车模转向一直则kd是正值，否则就是负值，注意pd与车模的平衡角度无关。大小的话同样也是逐渐增大直至出现低幅高频抖动时停止，也是把这个数值乘以0.6即可。

注意：这里涉及到一个负反馈调节因为直立环是要保证正常站立。因为当实际角度脱离平衡角度时，轮子只有给车身一个更大的同方向的力才会使车身保持平衡。

如果还是不理解可以拿支笔放在手指上试试，手指怎样移动才会使笔立在手指上？

速度环

速度环也是一个标准的负反馈例子。例如：我们想让车模加速，速度环就会降低车轮的速度使车身前倾，就会导致直立环起作用让车轮加速保持直立。这就完成了一个加速过程。需要注意的是速度环会干扰直立环，如果速度环的强度过大，车身就会非常不稳定。我在调用该函数时每一百次直立环才会调用一次速度环，这样就降低了速度环对直立环的干扰。

/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回  值：速度控制PWM
**************************************************************************/
float Target_Velocity = 0;
extern float kp_steep;
int Velocity_Ctrl_Pwm(int encoder_left,int encoder_right)
{
    static float Velocity,Encoder_Least,Encoder;
    static float Encoder_Integral;
    float sudumax = 2700;
    float kp=kp_steep,ki = kp/200;
      //速度PI控制
    Encoder_Least =(encoder_left+encoder_right)-Target_Velocity;                    //获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） 
    
    Encoder *= 0.7;		                                      //一阶低通滤波器       
    Encoder += Encoder_Least*0.3;	                              //一阶低通滤波器    
    Encoder_Integral +=Encoder;                                       //积分出位移 积分时间：？ms
    if(Encoder_Integral>2000)   Encoder_Integral=2000;              //积分限幅
    if(Encoder_Integral<-2000)   Encoder_Integral=-2000;              //积分限幅

    Velocity=Encoder*kp+Encoder_Integral*ki;                          //速度控制	
    
    if(flag_zhongdian!=0)
    Encoder_Integral=0;      //电机关闭后清除积分 
    
    //输出限幅
    if(Velocity > sudumax) Velocity = sudumax;
    else if(Velocity < -sudumax) Velocity = -sudumax;
    
    return (int)Velocity;
}

转向环

转向环其实有很多方法。我是用的比较简单的一种办法：动态的pid算法，P的系数是电磁传过来的偏差，D的系数是MPU6050的Z轴加速度（因为车模静止时我所在的地区Z轴加速度为2，所以在代码最后gyro+ 2进行了一次加速度补偿）。速度越快P越大,D越小。当电磁传过来的偏差小于5时，判为直道，采用很小的P。可有效地减少直道上的晃动。我一开始这样做发现过弯时会有特别严重的抖动，经常飞出赛道，因此在最后加了个判断

if((My_Abs(Turn_Target) - My_Abs(Turn_Target_last)) >= 37) //入弯防抖
else if((My_Abs(Turn_Target_last) - My_Abs(Turn_Target)) >= -40) //出弯防抖

入弯防抖同时增大3倍P,2倍D是车模更快更准的转弯，避免速度快时过弯不及时（毕竟我这一届只有不到30cm的前瞻）。出弯防抖与前者配合减少转弯过度带来的晃动。

变量 RoadPianCha 是左右横电感和斜电感的差除以左右电感的和。也就是说这个变量的范围是 0-100。

/**************************************************************************
函数功能：转向控制  修改转向速度，请修改Turn_MaxPwm即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回  值：转向控制PWM
作    者：
**************************************************************************/
int Turn_Ctrl_Pwm2(int encoder_left,int encoder_right,float gyro)//转向控制
{
    static float Turn_Target,Turn,Turn_Target_last;
    float Turn_MaxPwm,Kp,Kd;  
 //   if(My_Abs(RoadPianCha) <= 2)
   //   RoadPianCha = 0;
    
      Turn_Target_last = Turn_Target;
      Turn_Target=RoadPianCha - (encoder_left-encoder_right)/5;   
      
      if(My_Abs(RoadPianCha) >= 5) {
         Kp = 41.0 * (200+encoder_left + encoder_right)/200.0;
        Kd = -4.2/ ((200+encoder_left + encoder_right)/200.0);
        Turn_MaxPwm = 2000;
      }
      else {
      Kp =3;
      Kd = -5;
      Turn_MaxPwm =200;
      } 
      if((My_Abs(Turn_Target) - My_Abs(Turn_Target_last)) >= 37) //入弯防抖
      {
       // Target_Velocity = 100;
        Kp = Kp * 3.2;
        Kd = Kd * 2.3;
      }
      else if((My_Abs(Turn_Target_last) - My_Abs(Turn_Target)) >= -40)  //出弯防抖
      {
       // Target_Velocity = 1000;
        Kp = Kp * 1.7;
        Kd = Kd * 3.4;
      }

      if(Turn_Target>Turn_MaxPwm)  Turn_Target=Turn_MaxPwm;           //转向速度限幅
      if(Turn_Target<-Turn_MaxPwm) Turn_Target=-Turn_MaxPwm;
    
    
      //=转向PD控制器==//
      Turn=  -Turn_Target*Kp + (gyro+ 2)*Kd;       //结合Z轴陀螺仪进行PD控制
      return (int)Turn;
}

汇总

最后关于这几个环串起来：
在主函数中开一个定时器（时间大概为1-2毫米）把这几个函数轮番调用
下面的是我定时器触发函数

////====数据处理====平衡控制==速度环控制==转向环控制
void PIT1_isr (void)
{   
    flag1ms++;
    if(flag1ms%2 == 1)
    {
            Get_Dip_Angle();            //角度获取    
            Balance_Pwm = Balance_Ctrl_Pwm(Angle_Balance,Gyro_Balance);         //===平衡PID控制	
            if(flag1ms >= 100)
            {
            flag_100ms++;
            flag1ms = 0;
            Velocity_Pwm = Velocity_Ctrl_Pwm( Encoder_Left, Encoder_Right);        //===速度环PID控制	 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点
            }
    }
    else
    {
      Encoder_Right =   FTM_AB_Get(FTM1);          //脉冲为前进+，后退为-    
      Encoder_Left  =   -FTM_AB_Get(FTM2);         

    Get_AD_data();      //获取电感，测距的AD值
    AD_chuli();         //处理AD值，得出最终偏差
    Turn_Pwm = Turn_Ctrl_Pwm2(Encoder_Left, Encoder_Right,Gyro_Turn);  //===转向环PID控制   
    }
    
    //输出电机PWM,————负脉冲向前
    Moto_Left=Balance_Pwm+Velocity_Pwm+Turn_Pwm;                   //===计算左轮电机最终PWM
    Moto_Right=Balance_Pwm+Velocity_Pwm-Turn_Pwm;                  //===计算右轮电机最终PWM      
    //end
    PIT_TFLG(1) |= PIT_TFLG_TIF_MASK;    //清中断标志位    
}

最后传一张直立车模的照片。虽然有点丑。但是还是有点怀念。