本篇文章我將針對位置式PID算法、直立環(huán)、速度環(huán)等的編程進(jìn)行詳細(xì)的講解，讓每位小伙伴能夠?qū)@三個概念的編程邏輯有更加清晰的理解。

一、直立環(huán)（PD控制器）

1.中文公式
?直立環(huán)輸出=Kp1×角度偏差+Kd×角度偏差的微分
?// 角度偏差=真實角度-期望角度

2.英文公式
?直立環(huán)PD控制器：Kp×Ek+Kd×Ek_D
?（Ek：角度偏差；Ek_D：角度偏差的微分）

Ek=真實角度-期望角度（Angle-Med，由陀螺儀MPU6050測得）
Ek_D=真實角速度（gyro_Y，由陀螺儀MPU6050測得）---角度的微分值代表角速度

3.軟件編程
??根據(jù)理論公式進(jìn)行軟件編程，相信看完上面的講解后這段代碼應(yīng)該比較清晰易懂。

/*****************  
直立環(huán)PD控制器：Kp*Ek+Kd*Ek_D
入口：Med:機(jī)械中值(期望角度)，Angle:真實角度，gyro_Y:真實角速度
出口：直立環(huán)輸出
******************/
int Vertical(float Med,float Angle,float gyro_Y) 
{
  int PWM_out;
  
  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  
  return PWM_out;
} 

?

二、速度環(huán)（PI控制器）

1.中文公式

?速度環(huán)輸出=Kp2×電機(jī)速度偏差+Ki2×電機(jī)速度偏差的積分
?// 電機(jī)速度偏差=真實速度-期望速度

2.英文公式

?速度環(huán)PI控制器：Kp×Ek+Ki×Ek_S
?（Ek：電機(jī)速度偏差；Ek_S：電機(jī)速度偏差的積分）

Ek=真實速度-期望速度（真實速度：左電機(jī)速度+右電機(jī)速度；期望速度：0）
Ek_S=速度偏差的累加
3.低通濾波

??期間需要低頻濾波，我們是以直立環(huán)為主，速度環(huán)為輔，速度環(huán)相對于直立環(huán)來說是一個干擾，最終目的是直立。低頻濾波作用是使得波形更加平滑，濾除高頻干擾，防止速度過大影響直立環(huán)正常工作。

4.積分限幅

??通過比較限制積分在規(guī)定范圍內(nèi)變動，不得超出。

5.軟件編程

??根據(jù)理論公式進(jìn)行軟件編程，相信看完上面的講解后這段代碼應(yīng)該比較清晰易懂。

/*****************  
速度環(huán)PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的積分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
  // 定義成靜態(tài)變量，保存在靜態(tài)存儲器，使得變量不丟掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.計算速度偏差
  //舍去誤差--我的理解：能夠讓速度為"0"的角度，就是機(jī)械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  
  // 2.對速度偏差進(jìn)行低通濾波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，濾除高頻干擾，放置速度突變
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度過大影響直立環(huán)的正常工作
  
  // 3.對速度偏差積分出位移
  Encoder_S+=EnC_Err_Lowout;
  
  // 4.積分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度環(huán)控制輸出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  
  return PWM_out;
}

三、轉(zhuǎn)向環(huán)

1.中文公式

?轉(zhuǎn)向環(huán)輸出=系數(shù)×Z軸角速度
?(Z軸角速度由陀螺儀MPU6050測得)

2.軟件編程

??轉(zhuǎn)向環(huán)的編程比較簡單，我們只需設(shè)置一個參數(shù)調(diào)節(jié)Z軸角速度即可。

/*****************  
轉(zhuǎn)向環(huán)：系數(shù)*Z軸角速度
******************/
int Turn(int gyro_Z)
{
  int PWM_out;
  
  PWM_out = (-0.6)*gyro_Z;
  
  return PWM_out;
}

四、控制函數(shù)

1、采集編碼器數(shù)據(jù)和MPU6050角度信息

?編碼器數(shù)據(jù)：左電機(jī)速度，右電機(jī)速度
（兩個電機(jī)是相對安裝，剛好相差180度，為了編碼器輸出極性一致，就需要對其中一個取反）
?MPU6050數(shù)據(jù)：角度數(shù)據(jù)，角速度數(shù)據(jù)，角加速度數(shù)據(jù)
2、將數(shù)據(jù)壓入閉環(huán)控制中，計算出控制輸出量

?直立環(huán)輸出
?速度環(huán)輸出
?轉(zhuǎn)向環(huán)輸出
3、把控制輸出量加載到電機(jī)上，完成最終的控制

?左電機(jī)輸出（編碼器放置相對）
?右電機(jī)輸出
?限幅
?賦值
4、控制中斷函數(shù)
?首先要判斷是否接受到中斷請求，即檢測MPU6050的ANT引腳是否處在低電平（即為發(fā)生中斷），然后清除中斷標(biāo)志位，進(jìn)行接下來三步（即上面的三個步驟）。
?

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一級判定
  {
    if(PBin(5)==0)    // 二級判斷
    { 
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中斷標(biāo)志位
      // 1.采集編碼器數(shù)據(jù)&MPU6050角度信息
      // 電機(jī)是相對安裝，剛好相差180度，為了編碼器輸出極性一致，就需要對其中一個取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 讀取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 讀取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 讀取加速度
      // 2.將數(shù)據(jù)壓入閉環(huán)控制中，計算出控制輸出量
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度環(huán)
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立環(huán)
			Turn_out=Turn(gyroz);	
      
      PWM_out=Vertical_out;//最終輸出
      
      // 3.把控制輸出量加載到電機(jī)上，完成最終控制
      MOTO1 = PWM_out-Turn_out; // 左電機(jī)
      MOTO2 = PWM_out+Turn_out; // 右電機(jī)
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加載到電機(jī)上
    }
  }
}

五、整個控制函數(shù)源代碼

1、control.c

#include "control.h"

float Med_Angle=0;      // 機(jī)械中值，能使得小車真正平衡住的角度 
float Target_Speed=0;	  // 期望速度。---二次開發(fā)接口，用于控制小車前進(jìn)后退及其速度。
float 
  Vertical_Kp=0,
  Vertical_Kd=0;     // 直立環(huán)Kp、Kd
float 
  Velocity_Kp=0,     // 速度環(huán)Kp、Ki（正反饋）
  Velocity_Ki=0;
float 
  Turn_Kp=0;

int Vertical_out,Velocity_out,Turn_out; // 直立環(huán)&速度環(huán)&轉(zhuǎn)向環(huán)的輸出變量

int Vertical(float Med,float Angle,float gyro_Y); // 函數(shù)聲明
int Velocity(int Target,int encoder_left,int encoder_right);
int Turn(int gyro_Z);

void EXTI9_5_IRQHandler(void)
{
  int PWM_out;
  if(EXTI_GetITStatus(EXTI_Line5)!=0) // 一級判定
  {
    if(PBin(5)==0)    // 二級判斷
    { 
      EXTI_ClearITPendingBit(EXTI_Line5); // 清除中斷標(biāo)志位
      // 1.采集編碼器數(shù)據(jù)&MPU6050角度信息
      // 電機(jī)是相對安裝，剛好相差180度，為了編碼器輸出極性一致，就需要對其中一個取反
      Encoder_Left  = -Read_Speed(2); 
      Encoder_Right = Read_Speed(4);
      
      mpu_dmp_get_data(&Pitch,&Roll,&Yaw);	    // 讀取角度
      MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);  // 讀取角速度
      MPU_Get_Accelerometer(&aacx,&aacy,&aacz); // 讀取加速度
      // 2.將數(shù)據(jù)壓入閉環(huán)控制中，計算出控制輸出量
			Velocity_out=Velocity(Target_Speed,Encoder_Left,Encoder_Right); // 速度環(huán)
      Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,gyrox);			  // 直立環(huán)
			Turn_out=Turn(gyroz);	
      
      PWM_out=Vertical_out;//最終輸出
      
      // 3.把控制輸出量加載到電機(jī)上，完成最終控制
      MOTO1 = PWM_out-Turn_out; // 左電機(jī)
      MOTO2 = PWM_out+Turn_out; // 右電機(jī)
      Limit(&MOTO1,&MOTO2);     // PWM限幅
      Load(MOTO1,MOTO2);        // 加載到電機(jī)上
    }
  }
}

/*****************  
直立環(huán)PD控制器：Kp*Ek+Kd*Ek_D

入口：Med:機(jī)械中值(期望角度)，Angle:真實角度，gyro_Y:真實角速度
出口：直立環(huán)輸出
******************/
int Vertical(float Med,float Angle,float gyro_Y) 
{
  int PWM_out;
  
  PWM_out = Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
  
  return PWM_out;
} 

/*****************  
速度環(huán)PI控制器：Kp*Ek+Ki*Ek_S(Ek_S：偏差的積分)
******************/
int Velocity(int Target,int encoder_left,int encoder_right)
{
  // 定義成靜態(tài)變量，保存在靜態(tài)存儲器，使得變量不丟掉
  static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;
  float a=0.7;
  
  // 1.計算速度偏差
  //舍去誤差--我的理解：能夠讓速度為"0"的角度，就是機(jī)械中值。
  Encoder_Err = ((encoder_left+encoder_right)-Target);
  // 2.對速度偏差進(jìn)行低通濾波
  // low_out = (1-a)*Ek+a*low_out_last
  EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last; // 使得波形更加平滑，濾除高頻干擾，放置速度突變
  EnC_Err_Lowout_last = EnC_Err_Lowout;   // 防止速度過大影響直立環(huán)的正常工作
  // 3.對速度偏差積分出位移
  Encoder_S+=EnC_Err_Lowout;
  // 4.積分限幅
  Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
  
  // 5.速度環(huán)控制輸出
  PWM_out = Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
  
  return PWM_out;
}

/*****************  
轉(zhuǎn)向環(huán)：系數(shù)*Z軸角速度
******************/
int Turn(int gyro_Z)
{
  int PWM_out;
  
  PWM_out = Turn_Kp*gyro_Z;
  
  return PWM_out;
}

以上就是平衡小車系列文章第六講——位置式PID、直立環(huán)與速度環(huán)軟件編程講解，平衡小車系列文章作者在持續(xù)更新中。若文章中出現(xiàn)錯誤或者小伙伴對以上內(nèi)容有所疑問，歡迎大家在評論區(qū)留言，小政看到后會盡快回復(fù)大家！文章來源地址http://www.zghlxwxcb.cn/news/detail-435442.html

到了這里，關(guān)于（六）【平衡小車制作】位置式PID、直立環(huán)與速度環(huán)編程的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！