Arduino 讓小車走直線的秘密 增量式PID 直流減速編碼電機

直流減速電機增量式PID 通俗易懂版

對于我一個雙非學校沒上過自控的電子專業本科生而言,這個東西真的是太難了,之前研究了一個A4950驅動,價格便宜,驅動能力高,安全性高,便捷性高,一句話就是比L298N好用,我寫完之后發了出去,我的一位老師看完之后,說沒看出來好在哪,應該弄一個PID控制的,好吧我弄.為啥是通俗易懂版,這個,因為我理解的也不是很深入,要是有錯誤希望大佬指點海涵,相信對大部分同學來說應該是夠用的,這篇文章寫一個電機,本文章研究于平衡小車之家

1.直流電機增量式PID倍訓控制原理簡述

PID我們一般分為兩種,一種叫位置PID一種叫速度PID,位置PID一般用于倒立擺,平衡小車等,使一個引數準確的到達某一個指定的靜止狀態,比如讓一個電機準確的轉90度,增量式PID用于讓一個動態變數,盡可能穩定的維持在某一個目標值,那嗎我們想讓小車盡可能準確的勻速行駛,就是讓每一個輪子的轉速穩定在一個目標值,那具體是怎么實作的呢,我把他分為四個步驟

看懂這個流程圖你至少要知道:
(1)編碼電機的基本原理,編碼電機的編碼器,對應轉速不同相同時間內會輸出不同個脈沖
(2)單片機通過讀取編碼器輸出的脈沖(連續的脈沖就是一個方波)的上升沿或下降沿,或上升沿和下降沿來獲取轉速
好的接下來我們就解釋這個圖
第一步:單片機通過直流減速電機的編碼器獲取轉速,并獲得當前轉速與目標轉速的誤差
第二步:根據誤差PI控制器會算出一個用于將直流減速電機調整到目標轉速的PWM值
第三步:將算出的PWM值輸出給直流電機,對直流電機轉速進行調整
第四步:再次讀取直流電機轉速傳給單片機

PI控制器:一個函式,輸入的是當前轉速與目標轉速的誤差,輸出的是調整轉速需要用到的PWM值

接下來我介紹一下,讓小車勻速的活神仙,離散PID公式

PWM+=Kp×[e(k)-e(k-1)]+Ki×e(k)+Kd×[e(k)-2e(k-1)+e(k-2)]

看著公式眼暈!?別走別走我們最后不用這個復雜的公式
我還是先介紹一下它的引數都代表什么

PWM :輸出增量,就是輸出值

Kp ,Ki ,Kd 這三個引數是需要隨機應變的,根據不斷的測驗調整得出一組最佳值,什么!?隨機應變這四個字是在敷衍你???別走別走一會有例子的好吧你一定看的懂,你一定看的懂

e(k) : 本次誤差

e(k-1):上次誤差

e(k-2):上上次誤差

我們要用的公式的祖宗說完了,我們實際用的公式可以簡化為

PWM+=Kp×[e(k)-e(k-1)]+Ki×e(k)

這個簡單的短短的公式就是開啟小車勻速寶藏的鑰匙

C語言實作代碼如下

int Incremental_PI (int Encoder,int Target)
{  
   static float Bias,PWM,Last_bias;
   Bias=Target-Encoder;                                  //計算偏差
   PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制器
   Last_bias=Bias;                                       //保存上一次偏差 
   return PWM;                                           //增量輸出
}

我們來一行一行詳細解釋

int Incremental_PI (int Encoder,int Target)

定義一個回傳值為整形的函式叫Incremental_PI輸入的是兩個整形變數 Encoder,Target (Encoder代表當前速度,Target代表目標速度)

Bias=Target-Encoder; 

計算偏差目標值減去當前值

static float Bias,PWM,Last_bias;

定義三個全域浮點型靜態變數,Bias(本次偏差),PWM(輸出增量),Last_bias(上次誤差)

PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;

計算出調整偏差所需要的PWM值

Last_bias=Bias;   

保存上一次偏差

return PWM;

回傳需要輸出的PWM值

2.實驗準備

1.Arduino mega 2560
2.12V電源一個 我用的是航模電池
3.直流電機驅動 A4950
鏈接: A4950驅動詳解
4.編碼電機
5.面包板
6.杜邦線若干
接線圖如下:

3.實驗預備知識

1.編碼器AB相
編碼器的A相和B相輸出的是有相位差的兩個脈沖,實際上就是兩個方波,那我們是如何從編碼器A相和B相輸出的方波例區分我們所認定的正反的呢

舉個栗子比如我們的計數器是通過檢測A相輸出方波的上升沿計數的,那當我們檢測到A相輸出的上升沿時,發現如果此時B相輸出的是高電平那我們就可以認定為此時電機在正轉,當檢測到A相的上升沿時,如果B相輸出的是低電平那我們就可以認定為電機是反轉

如果只檢測一個相位在單位時間內的一個邊沿出現的次數,我們將這種測速方法稱為M測速法,如果我們將AB相的上升沿和下降沿都做檢測的話就可以達到四倍精度又稱四分頻測速,然而Arduino mega 2560 只有6個中斷IO口,其中還有和I2C通信口和Serial1串口共用的IO口,所以我們做一臺車只使用四個中斷IO口每一個中斷IO口對應一個電機的編碼器,但我們可以檢測A相的上升沿和下降沿采用二分頻測速
2.定時內部中斷
如果你要用Arduino做四輪的PID小車你就至少需要四個中斷IO口,一般來說我們都是采用Mega2560 ,下面是arduino系列單片機對應引腳號和中斷號

需要對設定時間內的脈沖的上升沿和下降沿計數,而且不占用CPU,我們就需要用到定時內部中斷,當然了arduino 最大的好處就是方便 ,我們不需要像使用32一樣去配置一大堆暫存器和一大堆模式,我們就采用一個外國小伙為我們寫好的mega2560內部定時器庫

#include <FlexiTimer2.h>        //定時中斷

配置也十分簡單 在setup()里寫兩行就夠了

  FlexiTimer2::set(5, control); //5毫秒定時中斷函式,每5s執行一次control函式
  FlexiTimer2::start ();      //中斷使能 

好如過你了解了這些基礎知識,那我們就開始

4.實驗代碼

以下代碼實作功能為,從串口輸入一個目標轉速Velocity,使電機的轉速盡可能準確的維持在目標值 ,Velocity所代表的是指定時間內上升沿和下降沿的個數 ,指定時間即為我在內部定時中斷里設定的時間,又稱采樣周期

unsigned int Motor_AIN1=5;       //控制A電機的PWM引腳  一定改成自己用的
unsigned int Motor_AIN2=6;        
String Target_Value;             //串口獲取的速度字串變數
int value;                       //用于存盤通過PI控制器計算得到的用于調整電機轉速的PWM值的整形變數 
#include <FlexiTimer2.h>         //定時中斷頭檔案庫
/***********編碼器引腳************/
#define ENCODER_A 2              //編碼器A相引腳
#define ENCODER_B 50             //編碼器B相引腳
int Velocity,Count=0;            //Count計數變數 Velocity存盤設定時間內A相上升沿和下降沿的個數
float Velocity_KP =7.2, Velocity_KI =0.68,Target=0;//Velocity_KP,Velocity_KI.PI引數  Target目標值
/*********** 限幅************
*以下兩個參與讓輸出的PWM在一個合理區間
*當輸出的PWM小于40時電機不轉 所以要設定一個啟始PWM
*arduino mega 2560 單片機的PWM不能超過255 所以 PWM_Restrict 起到限制上限的作用
*****************************/
int startPWM=40;                 //初始PWM
int PWM_Restrict=215;            //startPW+PWM_Restric=255<256
void setup() 
{
  Serial.begin(9600);            //打開串口
  Serial.println("/*****開始驅動*****/");
  pinMode(ENCODER_A,INPUT);     //設定兩個相線為輸入模式
  pinMode(ENCODER_B,INPUT);
  pinMode(Motor_AIN1,OUTPUT);   //設定兩個驅動引腳為輸出模式
  pinMode(Motor_AIN2,OUTPUT); 
  FlexiTimer2::set(5, control); //5毫秒定時中斷函式
  FlexiTimer2::start ();        //中斷使能 
  attachInterrupt(0, READ_ENCODER_A, CHANGE);      //開啟對應2號引腳的0號外部中斷,觸發方式為CHANGE 即上升沿和下降沿都觸發,觸發的中斷函式為 READ_ENCODER_A 
}

void loop() 
{
  while(Serial.available()>0)          //檢測串口是否接收到了資料
  {
    Target_Value=Serial.readString();  //讀取串口字串
    Target=Target_Value.toFloat();     //將字串轉換為浮點型,并將其賦給目標值
    Serial.print("目標轉速頻率:");      //串口列印出設定的目標轉速
    Serial.println(Target);


  }
}
/**********外部中斷觸發計數器函式************
*根據轉速的方向不同我們將計數器累計為正值或者負值(計數器累計為正值為負值為計數器方向)
*只有方向累計正確了才可以實作正確的調整,否則會出現逆方向滿速旋轉
*
*※※※※※※超級重點※※※※※※
*
*所謂累計在正確的方向即
*(1)計數器方向
*(2)電機輸出方向(控制電機轉速方向的接線是正著接還是反著接)
*(3)PI 控制器 里面的誤差(Basi)運算是目標值減當前值(Target-Encoder),還是當前值減目標值(Encoder-Target)
*三個方向只有對應上才會有效果否則你接上就是使勁的朝著一個方向(一般來說是反方向)滿速旋轉

我例子里是我自己對應好的,如果其他驅動單片機在自己嘗試的時候出現滿速旋轉就是三個方向沒對應上

下列函式中因為在上升沿觸發時由于在A相線上升沿觸發時,B相是低電平,和A相線下降沿觸發時B是高電平是一個方向,在這種觸發方式下,我們將count累計為正,另一種清理將count累計為負
********************************************/
void READ_ENCODER_A() 
{
    
    if (digitalRead(ENCODER_A) == HIGH) 
    {     
     if (digitalRead(ENCODER_B) == LOW)      
       Count++;  //根據另外一相電平判定方向
     else      
       Count--;
    }
    
    else 
    {    
     if (digitalRead(ENCODER_B) == LOW)      
     Count--; //根據另外一相電平判定方向
     else      
     Count++;
    }
    
}
/**********定時器中斷觸發函式*********/
void control()
{     

        Velocity=Count;    //單位時間內讀取位置資訊
        Count=0;           //并清零
        value=Incremental_PI_A(Velocity,Target);  //通過目標值和當前值在這個函式下算出我們需要調整用的PWM值
        Set_PWM(value);    //將算好的值輸出給電機
        
}
/***********PI控制器****************/
int Incremental_PI_A (int Encoder,int Target)
{  
   static float Bias,PWM,Last_bias;                      //定義全域靜態浮點型變數 PWM,Bias(本次偏差),Last_bias(上次偏差)
   Bias=Target-Encoder;                                  //計算偏差,目標值減去當前值
   PWM+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;   //增量式PI控制計算
   
   if(PWM>PWM_Restrict)
   PWM=PWM_Restrict;                                     //限幅
   
   if(PWM<-PWM_Restrict)
   PWM=-PWM_Restrict;                                    //限幅  
   
   Last_bias=Bias;                                       //保存上一次偏差 
   return PWM;                                           //增量輸出
}
/**********實際控制函式*********/
void Set_PWM(int motora)                        
{ 
  if (motora > 0)                                  //如果算出的PWM為正
  {
    analogWrite(Motor_AIN1, motora+startPWM);      //讓PWM在設定正轉方向(我們認為的正轉方向)正向輸出調整
    analogWrite(Motor_AIN2, 0); 
  }
  else if(motora == 0)                             //如果PWM為0停車
  {
    analogWrite(Motor_AIN2, 0);
    analogWrite(Motor_AIN1, 0);   
  }
  else if (motora < 0)                              //如果算出的PWM為負
  {
    analogWrite(Motor_AIN2, -motora+startPWM);      //讓PWM在設定反轉方向反向輸出調整
    analogWrite(Motor_AIN1, 0);
  }
}

上傳代碼使用效果如下

那怎么看出來它是不是勻速的呢
如果沒有上位機示波器的話
你可以將Volocity在串口列印出來
傻是傻了點但是可以用

void loop() 
{
  while(Serial.available()>0)
  {
    Target_Value=Serial.readString();
    Target=Target_Value.toFloat();
    Serial.print("目標轉速頻率:");
    Serial.println(Target);
  }
  Serial.println(Velocity);     //這句是新加的
}

比如我輸入了一個10可以看到輸出的速度會在10上下浮動我截了一張在抖動的,其實大部分時間輸出的速度還是維持在10的,輸出的速度也不會超過[9.11]一直會在這個范圍也就是10的上下1浮動

如果有上位機示波器的話呢看到能更舒服一點
黃線是調整轉速輸出的PWM值,白線是實際值,紅線是目標值
紅線基本和白線重合看不到了

我們再仔細看一下白線和紅線偏差不超過1,四個輪都調成這個程度實際效果親測效果還挺好,我覺得夠用,當然你可以接著調

5.Kp,Ki引數調節

好那關鍵問題來了Kp和Ki的引數怎么調呢
實際上因為我們只用到兩個引數,還是比較好調的
1.一般來說第一個引數Kp起讓實際值保持穩定的浮動在一個固定值上下的作用.Kp這個引數的大小和電機驅動的能力以及驅動電源的電壓值有關,驅動能力越強一般Kp越大,情況是多樣的,唯有不斷嘗試,看哪個引數出來的效果最好,震蕩幅度最小,震蕩次數最少.
和位置PID的個人覺得調整方式不太一樣,這里不想做太多復雜的解釋.
但還是可以給大家點建議以我例子中的引數為例,我用的是A4950,12V的驅動電源,使用的引數是Velocity_KP =7.2,如果你用的驅動組合比我更強勁(驅動模塊比我用的好,驅動電壓比我高)那你就從我的例子的引數開始一點一點往上嘗試,順帶一提L298N驅動能力和A4950差不多

2.第二個引數Ki起讓這個固定值是目標值的作用,用來消除穩態誤差,又稱靜差,具體是怎么表現的呢
我把上面例子的引數用的是

Velocity_KP =7.2, Velocity_KI =0.68

接下來我把Ki變成0

// An highlighted block
Velocity_KP =7.2, Velocity_KI =0

就會出現如下情況,白線雖然也是在一個固定值上下浮動,但是這個固定值離我們的目標值,也就是紅線明顯有偏差.這個原理大家不是很明白的話可以去搜穩態誤差,Ki起的就是消除穩態誤差的作用

我們吧Velocity_KI 再次賦值0.68
可以看到紅線白線又開始"糾纏不清"

在讓想小車勻速的PI控制例子中,這個Ki不用給太大

6.后記

在最后調引數這塊,之前我也看了好多,這個調參的文章,還有一大堆口訣什么什么的,我個人覺得似乎確實并不太好理解,而且基本是用于第一類位置PID的,然后感覺都不如自己直接嘗試來的痛快,大概都是先調Kp使系統穩定,然后Ki讓系統消除靜態誤差穩定在該穩定的值,第一次寫PID的老實說有點害怕,怕自己哪塊理解的不到位,誤導大家,如果有寫的不妥當的地方還希望大家批評指正,希望能幫到大家