輪式驅動單元電機PID控制說明

PID控制是一種簡單有效且具有較強魯棒性較強的控制手段，在任何一本關於自動控制的教材中均可找到相應的介紹，在此不過多介紹基本原理，而側重於程序的使用方法及其在輪式驅動單元中的測試結果。
       目前機器人的電機大多采用脈寬調製（Pulse width modulation）或簡稱PWM進行控制，而不是使用模擬功率電路。在軟件中通過改變脈衝寬度（如圖1，上圖對下圖），我們可以改變等效的模擬電機信號，從而達到控制電機轉速的目的。電機系統的行爲就像是一個積分器，在一定時間的範圍內對數字信號進行積分。而 則稱爲佔空比。
 

圖1 PWM

問題1 電機控制的非線性
      比較遺憾的是，通常情況下，電機所產生的速度與PWM信號比並非是線性關係，如圖2是我們對輪式驅動單元進行實驗的結果，而圖3右圖則是我們查閱得到的Faulhaber 2230電機的測量曲線，圖3左圖是電機的階躍響應。由於實驗的電機的減速箱和速度的單位的不同，圖2、3所得到的具體數值不盡相同，但它們卻共同反映了電機PWM控制的一個特性——非線性！

 

圖2 輪式單元車輪轉速（周每秒）/PWM

 

圖3 Faulhaber 2230電機的階躍響應與速度/PWM比

 
問題2 開環控制的不準確
       通常電機可以實現正/反轉、速度調節。但是，我們卻並不知道電機實際的運行速度。應當注意到電機的實際運行速度並不僅僅只與輸入的PWM信號有關，還會受到例如負載（例如機器人的重量或是驅動區域的傾斜程度）等外部因素的影響。如果使用開環控制的話，在低速情況下，電機的扭矩通常也非常的小。
 
解決方法 增加PID控制
    電機增加編碼盤後，便爲閉環控制的實現提供了物質基礎。但僅僅是簡單的反饋是不夠的，簡單的比例P反饋控制利用誤差進行校正，而誤差卻是始終存在的（只是能將控制在有限的範圍內），若想實現零穩態誤差響應，還需要增加一個積分I環節。如果想進一步提高機器人的靈敏度（動態響應性能）還可以增加一個微分環節D，這樣便可以組成一個PID控制器。但在這裏不建議大家使用微分D環節，因爲稍有不慎便有可能造成振盪，而在此應用中D環節對性能的提升也十分有限。下面我們介紹一下我們編寫的程序。
 
PID控制程序
    注：我們是在Arduino下進行的開發，但此程序進行簡單的修改便可移植到任何類型的單片機上。
1、連接接口針號設置
       A、B、C分別表示三個不同的輪式單元，ctrl_1、ctrl_2、ctrl_3表示三個電機控制線，counter是編碼盤輸入，analog是電機電流檢測（用於觀測電機是否堵轉，當讀數超過532並持續一段時間時，表示堵轉，應及時斷開電機以避免燒燬）

01 02 03 04 05 06 07 08 09 10 11 12 13 14 15

const int A_ctrl_1=3;          //PWM OUT const int A_ctrl_2=4;          //Motor control 2 const int A_ctrl_3=12;          //Motor control 3 const int A_counter= 2;        //Motor counter input const int A_analog = 0;       //Motor current measure input const int B_ctrl_1=5;          //PWM OUT const int B_ctrl_2=4;          //Motor control 2 const int B_ctrl_3=12;          //Motor control 3 const int B_counter= 19;        //Motor counter input const int B_analog = 4;       //Motor current measure input const int C_ctrl_1=6;          //PWM OUT const int C_ctrl_2=7;          //Motor control 2 const int C_ctrl_3=8;          //Motor control 3 const int C_counter= 20;        //Motor counter input const int C_analog = 5;       //Motor current measure input

2、主要參數設置
    Kp爲比例值，我們設爲1，通常越大調節速度越快，代價是有可能出現超調，或超調過大甚至是不穩定。Ki是積分項，我們設爲4.

01 02 03 04 05 06 07 08 09 10

/////you can chane the following const paramters  //////////////// const int CURRENT_LIMIT=530; // high current warning, if the value of Motor curren is bigger than CURRENT_LIMIT more than 10 sample, stop. const int NUM_C=200;      //the number of counter of a roll.                           //IF select CHANGE in attachInterrupt(0, rencoder, CHANGE) ,the  NUM_C should be multiple by 2                            //Other selection in attachInterrupt(0, rencoder, FALLING), the  NUM_C is equal to the number of counter of a roll const int LOOPTIME =50;   //the loop time(ms) of PID control float Kp=1;          // PID proportional control gain float Ki=4;          // PID i control gain const float K_modal=1;     // K_modal is the modal number of motor. it is eaual 255/(the full RPS of motor).  RPS is roll per second ///////////////////////////////////////////////////////////

3、PI控制程序
      我們首先需要將碼盤連接至一箇中斷，每來一次中斷便counter++。當中斷設爲FALLING或RISING時NUM_C設爲100，因爲碼盤100個齒每轉一週產生100個FALLING中斷。當中斷設爲CHANGE時，則是NUM_C設爲200，因爲碼盤100個齒每轉一週產生200個CHANGE中斷。然後我們通過記錄一個週期looptime內counter的增加數便可得到實際的轉動速度（轉/秒），如下。

1	speed_act=float(count- count_fomer)*1000/float(looptime*NUM_C);

誤差項爲期望速度（轉/秒）減去實際速速。

1	error = abs(desire_act) - abs(speed_act);

PID控制量如下：
    

1	pidTerm =  Kp * error  + Kp * Ki * (error +last_I);

         
更新積分值，注意last_I應使用靜態變量，以不斷積分。
    

1	last_I = error + last_I;

PI完整代碼如下，我們的代碼非常簡單，關鍵的只有幾行（便於學習），並且在經過了實際的測試，即使在低速的情況下仍能保持較大的力矩，無論上坡還是下坡均能保持指定的速度巡航。我們發現在Arduino論壇裏有一個PID類（附件中），感興趣的讀者可以拿來研究使用一下。

01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29

//////////////////////////////////////////////////////// float PID_updata(float command, float targetValue, float currentValue) //computePWM value {        float pidTerm = 0;                             // PID correction     float error=0;                         static float last_I=0;     error = abs(targetValue) - abs(currentValue);     pidTerm =  Kp * error  + Kp * Ki * (error + last_I);                               last_I = error + last_I;     return pidTerm; }   int control_loop(int looptime , float speed_req, int PWM_val) {     long lastMilli;   long count_fomer;   float speed_act;   lastMilli=millis();   count_fomer=count;   interrupts();   while ((millis()-lastMilli) <= looptime)    {  ;  } // enter tmed loop                                                         noInterrupts();   speed_act=float(count- count_fomer)*1000/float(looptime*NUM_C);   PWM_val= PID_updata(PWM_val, speed_req, speed_act);   // compute PWM   return constrain(PWM_val, 0, 255); } //////////////////////////////////////////////////////////