电赛练习之旋转倒立摆PID调节

前言：在家准备电赛控制题，第一个选择的旋转倒立摆，结构和电路相对简单，对于新手比较友好。本人今年大二，自学的STM32和PID算法，本文算是对这个题目练习的记录吧，文章和程序有误的地方还请大家多多指教。
一、基本原理（可能有误）
原理这一块没有做的太复杂，只是稍微估计了电机需要的速度值。当摆臂失去平衡后，就需要一个惯性加速度平衡重力加速度，这个惯性加速度就是电机施加的加速度。以竖直方向右偏30度为例，则平衡所需要的惯性加速度大约是5.77m/(s^2)。又根据功能关系，可估算此时摆杆重心处速度为1m/s。设定在0.5s内将摆杆恢复至竖直方向,以gsin(pi/6)来估算重力加速度分量，以asin(pi/6)来估算施加的加速度，最终算得a=6.9m/(s*s)，则电机需要的速度是v=3.5m/s。又考虑到实际过程中电机不能始终在满速和停止之间来回运动，且在控制过程中要考虑到干扰的施加，那么摆杆的速度不止1m/s，这样看来至少需要旋转臂25cm情况下满速为10m/s以上的电机比较合适。

二、器件选择
1、电机采用转速530rad/min12v的JGB37-520电机，驱动选择tb6612驱动模块，传感器选择线性误差在0.1%的WDD35D4电位器。
2、这一次的缺陷就是没有加入稳压电路，这个太重要了，因为在调节PID参数的过程中，需要保持整个系统变化的只有参数值。试想一下，当我们在调节PID参数控制PWM输出时，电压不断波动，那么控制效果将随电压波动时好时坏。又比如这次我采用的是12V电池，那么每次充完电后，电压值都会比我上次调节好参数值时有变化，那么我又需要再次调节参数值，所以这个题目花费了很长时间，而且在电压波动的情况下，无论怎么改变参数值，摆臂的震动都比较大，调节效果并不是很理想。

三、机械结构
考虑到正式比赛时需要自己搭建机械结构，我就没有直接购买现成的机械结构。关于机械结构还是平时接触太少了，随便在淘宝上买的不锈钢打孔支架作摆臂和旋转臂，最后发现传感器没法安放，强行用电机支架和胶带固定住。最困难的是怎么把电机和旋转臂连接得牢靠，最开始用的一个联轴器，发现转的猛了就会松动，想尽各种办法，最后是用三个联轴器连接后才能连接得牢靠，为了增加高度，在三个基本的联轴器上又重了三个联轴器。底座用四个直角电机支架连接，这样就算来回摆动也不会引起振动。最后整个结构用钉子钉在书桌上（心疼书桌一秒），这样就算比较稳定的机械结构了。

四、倒立稳定实现

(一)、PID算法
PID的讲解在网上有很多专业又详细的教程，我就不再说了，这里讲一下这一个月调节PID参数后对PID参数的理解：
P值——快速接近目标值，误差越大，效果越强——容易导致超调；
I值——消除静差——提高准确性（但我没用，容易使响应过慢）；
D值——消除震荡，预测误差变化——使系统趋于稳定;

(二)、角度PID
这个PID是主要的控制量，应该作为整个控制过程的主导量。代码并不复杂，我采用的是位置式PID，如下：

float angle_pid(float adc_value)
{ 
   
  static float error,pwm,error_1;
  error=middle_value-adc_value;
  pwm=error*a_p+(error-error_1)*a_d;
  error_1=error;
  return pwm;
}

如何调节PID参数，我记录一下自己调节的过程：
1、首先将D值和I值置0，单独调节P值，最开始可以设置得大一点，例如在4999为上限的PWM值下，我一般先设置P值为55~60，这时大概率系统响应过度，会造成摆臂在中值处来回剧烈震荡，这时就可以按照每次减1的速度来调节P值，直到调节到电机能快速做出反应的情况下不会造成过度的震动。我没有去具体分析P值调节下角度变化的波形，因为我的电压不稳定，不可能调节得很合适。

2、P值调节好后，摆臂应该能在中值处稳定一小段时间，之后会左右做小幅度摆动，或者朝一个方向运动，最后落下。这时需要加入D值，提前预测角度变化，消除震荡。同样，D值可以预设得很大，这时摆臂会出现低频震动，但是比单独P值调节要稳定，依次减少D值，直到摆臂能稳定在中值附件，最终朝一个方向加速运动下去并落下。这时角度环的PID参数就调好了。

(三)、位置PID
最开始很不理解这个位置环是怎么工作的，查网上资料说是串级PID，但都没有很清楚讲诉为什么需要位置环PID，这个位置环又是怎样和角度环配合的。这里结合搜集到的资料，记录一下自己的理解：
1、位置环作用本质
首先角度环PID已经能让摆臂稳定在中值附近，但是还是会朝一个方向转动下去，这是因为角度环控制的PWM值不能再纠正最后和中值的一点差值。有人会说那就加大PID参数啊，但是这样会导致超调，一点点的变化都会让摆臂剧烈摆动，最后还是落下。
解决这个问题的办法就是定期根据电机旋转的方向和距离加大PWM调节量，纠正最后一点误差。比如以电机编码器的值作为判断数据，编码器输出正值代表正转，在角度环调节的情况下，如果摆臂引导旋转臂向正方向一直转动，编码器累计值超过预设值时，就可以判断角度环没有及时纠正最后一点误差，这时就让PWM值增大速率更大，电机加速度更大，那么最后一点差值就纠正了。
而这个定期加大PWM值的作用就是所谓的位置环，有了位置环，就能把旋转臂的调节范围控制在一定范围内
2、位置环如何配合角度环
其实理解了位置环是如何作用的之后，就知道怎样去设置位置环了。至于网上说的什么角度环输出作为位置环输入我硬是没搞懂。我就按自己的想法来弄，根据定时器定时开启位置控制，比如每50ms开启一次，每次持续25ms，这样就能和角度环配合起来用了。
以上就是我的理解，代码如下：

float position_pid(void)
{ 
   
 static float bian_value,bian_error,last_bian_error,pwm,bian_last_value;
 static float bian_basic,bian_weifen;
 if(position_pd==1)//当起摆成功后刷新位置信息
 { 
   
  bian_basic=0;
  position_pd=0;
 }
 //位置环需要编码器累计值作判断
 bian_value=bian_read();//读取当前编码器值 
 bian_basic=bian_basic*0.5+bian_value;
 bian_error=bian_basic-position_value;
 bian_weifen=bian_error-last_bian_error;
 pwm=bian_basic*p_p+bian_weifen*p_d;
 last_bian_error=bian_basic;
 bian_last_value=bian_value;
 return pwm;
 
}

五、关于自动起摆
(一)、基本思路
自动起摆的过程主要分为三步。
第一步，先定时来回震荡，增大摆臂摆动幅度，直到摆臂超过水平线；
第二步，继续定时震荡，这时摆臂将有机会达到倒立稳定控制的角度范围内，一旦达到这个范围，立即单独开启角度环PID控制，由于考虑到摆臂此时速度较大，角度变化较快，应该增大D值控制，能比较好的稳定摆臂。经过对比实验，确实增大D值利于对摆臂的稳定。角度PID将持续300ms左右；
第三步，恢复预设PID参数值，开启角度环和位置环配合的PID控制，持续稳定摆臂。
(二)、中途异常掉落处理
为了能及时检测摆臂异常掉落，但是UCOSIII还没学会，我想到可以将延时控制过程分成很多次循环，每次循环延时5ms，那么每5ms就能检测一次角度值，如果出现异常，就退出循环，重新起摆。
程序如下：

void move_auto()
{ 
   
 static int i=0,time=5,flag=0,open_contrl=0,open_angle_control=0,n=0;
 static float adc_value=0,pwm;
 
 adc_value=Get_Adc_Average(ADC_Channel_10,5);
 //flag==0 未达到预定角度 不开启稳定控制 
 //定时器起摆
 if(flag==0) 
 { 
   
  adc_value=Get_Adc_Average(ADC_Channel_10,5);
  while((adc_value>xia_xian&&adc_value<shang_xian)!=1)
  { 
   
   //400ms定时
   for(i=0;i<80;i++)
   { 
   
    motor(4048);
    delay_ms(time);
    adc_value=Get_Adc_Average(ADC_Channel_10,5);
    //每5ms检测一次角度值
    //如果达到预定角度
    //立即开启稳定控制
    if(adc_value>xia_xian&&adc_value<shang_xian) 
    { 
   
     open_contrl=1;
     open_angle_control=1;
     while(open_contrl)
     { 
   
      adc_value=Get_Adc_Average(ADC_Channel_10,5);
      //判断稳定成功否 未成功则退出 继续定时起摆
      if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) open_contrl=0;
      //首先单独使用角度环控制
      //此时需要的PID参数中 D值将要更大一些
      //因为考虑到速度影响大
      if(open_angle_control==1)
      { 
   
      for(n=0;n<50;n++)
      { 
   
       a_p=73;  
       a_i=0; 
       a_d=380; 
       p_p=0;
        p_d=0;
       //同样判断摆臂是否异常落下
       adc_value=Get_Adc_Average(ADC_Channel_10,5);
       if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
       pwm=banlance_pid(adc_value);
       motor(pwm);
       delay_ms(5);
      }
      n=0;
      open_angle_control=0;
      //更改参数位稳定控制参数
      a_p=56;  
      a_i=0; 
      a_d=100; 
      p_p=2000;
      p_d=500;
      position_pd=1;
      }
      //稳定控制开启
      pwm=banlance_pid(adc_value);
         motor(pwm);
     }
    }
   }
   
   //反向定时起摆 和上一段配合摆动
   for(i=0;i<80;i++)
   { 
   
    motor(-4048);
    delay_ms(time);
    adc_value=Get_Adc_Average(ADC_Channel_10,5);
    if(adc_value>xia_xian&&adc_value<shang_xian) 
    { 
   
     open_contrl=1;
     open_angle_control=1;
     while(open_contrl)
     { 
   
      adc_value=Get_Adc_Average(ADC_Channel_10,5);
      if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) open_contrl=0;
      if(open_angle_control==1)
      { 
   
      for(n=0;n<50;n++)
      { 
   
       a_p=73;  
       a_i=0; 
       a_d=380; 
       p_p=0;
       p_d=0;
       adc_value=Get_Adc_Average(ADC_Channel_10,5);
       if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
       pwm=banlance_pid(adc_value);
       motor(pwm);
       delay_ms(5);
      }
      n=0;
      open_angle_control=0;
      a_p=56;  
      a_i=0; 
      a_d=100; 
      p_p=2000;
      p_d=500;
      position_pd=1;
      }
      pwm=banlance_pid(adc_value);
         motor(pwm);
     }
    }
   }
   adc_value=Get_Adc_Average(ADC_Channel_10,5);
  }
  flag=1;
 }
 
 //当flag==1时开启稳定控制
 while(flag==1)
 { 
   
  open_angle_control=1;
  adc_value=Get_Adc_Average(ADC_Channel_10,5);
  if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) flag=0;
   if(open_angle_control==1)
  { 
   
   for(n=0;n<50;n++)
   { 
   
    a_p=73;  
    a_i=0; 
    a_d=330; 
    p_p=0;
    p_d=0;
     adc_value=Get_Adc_Average(ADC_Channel_10,5);
    if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
    pwm=banlance_pid(adc_value);
    motor(pwm);
    delay_ms(5);
   }
   n=0;
   open_angle_control=0;
   a_p=56;  
   a_i=0; 
   a_d=100;
   p_p=2000;
   p_d=500;
   position_pd=1;
  }
  pwm=banlance_pid(adc_value);
  motor(pwm);
 }
} 

六、抗干扰性测试
根据题目要求需要抗干扰测试。在5g砝码拉起90度后向下释放撞击摆臂上方1——2cm处时，摆臂能迅速调整，并在2s内恢复正常，在极端情况下会出现摆臂落下的情况，但都能再次自动起摆并稳定。
七、稳定时旋转一周
基本思路是定时该变位置控制的目标位置值。虽然有一点控制效果，能稳定旋转几周，最后还是变得不稳定，摆臂落下，算时勉强有点效果吧。在网上很少找到这方面的资料，还望路过的大神能赐教。
八、总结
1、控制题首先需要建立物理模型，做出运动分析，这一点我做的不够好，导致这次电机选型一开始选的转速过小，不能满足加速度需求，后来被迫重新购买电机。网上有大佬用拉格朗日方程建立物理模型，需要好好学习一下（大概率看不懂）。
2、PID参数调节一定要在其他变量都基本不变的情况下进行，尤其时电压。机械结构底盘必须够稳，不然摆臂还没稳定，就被底座的摆动摇乱了。理论分析和机械结构搭建在前期可以多花一些时间，不然后期改机械结构和重新理论分析真的太花费时间了。
3、这次加深了PID的理解，各类模块的配置过程和程序都掌握的不错，也算是为电赛打下基础了。
最后还是贴一个程序吧，供大家探讨和赐教：
链接：https://pan.baidu.com/s/1q_LFi_9Xzfan7bUUh4foRw
提取码：6804