电赛练习之旋转倒立摆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参数后对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

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

继续阅读

CQ V1.0分词bates(基于双数组tire树)—应该是目前最快的中文分词算法

Command Network(POJ 3164)---定根最小树形图模板题题目描述输入格式输出格式输入样例输出样例分析源程序

开源低带宽语音编解码器

241 Different Ways to Add Parentheses（C代码版）

【趋高机器视觉】机器视觉技术原理解析及解决方案

CSMA/CD1． CSMA/CD的概述2． CSMA 的工作原理3． CSMA/CD控制规程及特点4． CSMA/CD协议5． CSMA/CD的优点6．结束语

极大似然法(ML)与最大期望法(EM)

C++ 第十五周报告1--《冒泡法排序》

ICG表面修饰稀土上转换纳米颗粒，ICG-PCL-PEG-FA 吲哚菁绿-聚已内酯-聚乙二醇-叶酸

笔试面试题目：滑动窗口(二)

数据结构与算法（27）——排序（二）

Android电视机（机顶盒）初次开发的一些经验分享

Dijkstra--简易版（最短路径）

GitHub连夜封杀！这份阿里 10W 字内部 Java 字面试手册到底有多强？

STM32-UART串口通信STM32-UART串口通信

hdu7108哈希