電賽練習之旋轉倒立擺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哈希