本人是參加過三年（也可以說是兩年）NX智能車競賽的老菜雞了，做的都有直立車。感覺大學就要過去了，是該給自己的大學做一些留念與總結了，回顧自己這幾年也沒有什麼拿的，就拿直立說說事。第一次寫部落格，寫的可能不是特别好，歡迎各位大佬指導交流。

直立車控制原理分析對比

直立車的控制分為兩種控制方式。一種為傳統的并行控制理論，直立環(5ms)+速度環(10ms)，更短的控制周期其控制效果會相應的變好；另一種是從四軸飛行器上衍生移植而成的串級控制理論，速度環(8ms)——>角度環(4ms)——>角速度環(2ms),各個PID控制的時間周期因人而異，時間遵循最内環控制周期最短、最外環控制周期最長的原則。

感覺傳統的控制方案裡的速度環是最讓人頭痛的。衆所周知，閉環回報調節可分為兩種回報調節，一種是平常大家熟悉的負回報調節，還有一種就是正回報調節了。傳統方案的速度環的回報是正回報調節，感覺有點違背正常人思考的理念，剛接觸的可能就會一臉懵逼。以溫度調節為例子，要維持恒溫的話，溫度高了就要降溫，溫度低了就要升溫，這就是正常的負回報調節。正回報就是，溫度高了不僅不降溫，反而要升溫。因為這個原因，傳統的速度環就沒有那麼好調試。而反觀串級控制的速度環，它隻是普通的負回報的而已，讓人能夠更加容易了解。

個人感覺對比兩種控制方案都是殊途同歸，讓我非要分一個優劣的話我會選擇後一種。

傳統控制方式

傳統的控制方式比較簡單，隻有兩個環，直立環隻有單獨一個而已，是以直立環的調試會更加的簡單，但是速度環的調試是比較煩人的。因為他是并行控制，兩個環無關的。但是實際上的話兩個環都會作用在同一對電機上，互相之間的影響特别大。如果隻是想要單純的立起來的話傳統的控制方式不失為一個不錯的選擇，但是一旦需要較好的速度控制的話則串級控制會更加好。

//直立環----位置式PID
float balance_pid(float Angle, float GYRO)
{
    float pwm；
    pwm=Kp*(Angle-balance_angle) + Kd*GYRO；
    return pwm;
}

//速度環----位置式PID
float speed_pid(float Speeed_Left,float Speed_Right)
{
        static float Speed_Error_last,Speed_Integral;
        float Speed_Error,Velocity；
        
       //計算速度偏差
        // speed_target為自己設定的速度目标值
        Speed_Error=(Speeed_Left+Speed_Right) - speed_target;      
        
        //速度低通濾波
        Speed_Error *= 0.8;		                                             
        Speed_Error += Speed_Error_last*0.2;	        
        
        //更新上次速度的值  
        Speed_Error_last =  Speed_Error ;           
        
        //速度積分    
        Speed_Integral+=Speed_Error ;                   
        
        //積分限幅               
        if(Speed_Integral>4000)  	Speed_Integral=4000;          
        if(Speed_Integral<-4000)	Speed_Integral=-4000;            
         
        Velocity=Speed_Error*speed_Kp+Speed_Integral*speed_Ki;                       
        return Velocity;
}

參數調試的話可以參照平衡小車之家的平衡車調試指南來進行相應的調試，裡邊寫的還是比較好的哦https://wenku.baidu.com/view/3e5fc765bb4cf7ec4bfed00b.html

串級控制方式

串級控制方式能讓小車的控制會更加穩定，但是相應的過多的參數可能會使不熟悉串級參數調試的人感覺更加複雜（參數調試的下文再說），但是前一種的話直立環隻有PD兩個參數，直立環的調試會更加簡單。也有人說串級參數的調試更加簡單，其實我感覺兩種控制方式的差不多而已。串級的話單單一個角速度環就能讓小車能挺不錯的立住了，調試也是從最内環（角速度環）開始調的，最後調的才是最外環。角速度環已經能讓車立住了那還需要其他的環為什麼呢。個人了解，角速度環參照代碼可知是讓車保持角速度為0的，但是小車在機械平衡位置角速度為0才是正常的直立，如果是在其他位置保持角速度為0的話就不能保持平衡。是以角度環的作用就是控制角速度環在什麼角度去保持角速度為0，一般我們要他保持的角度就是機械平衡的角度值了，是以這兩個環能讓小車穩穩的直立住了。最外環——速度環的作用毫無疑問就是保持速度了。前兩個環調試完畢之後小車就能保持直立了，但是如果想讓他跑的話就需要打破目前的平衡狀态。不難了解，直立車要跑起來就會低頭與擡頭，反映到角度上就是角度的變化，是以速度環的輸出一個微小的角度作為角度環的一個輸入。簡單的說就是讓小車以什麼速度來保持直立。三個環的共同作用就能實作小車的直立控制。

角速度環的話本人用的是位置PID,第一年我用到了PID三個參數,但是個人調試過程中發現D對整車的影響并不大，是以在第二年的話隻使用到了PI而已。有些人的話角速度環隻用到了PD而已，放棄使用積分量，個人感覺積分量是比較重要的，積分對小車立起來起到非常大的作用，但是過大的積分量又會破壞系統的平衡，是以其中的取舍要看個人而已。

//角速度環----位置式PID
//Gyro_Data——陀螺儀擷取的自身實際角速度、
//Gyro_m——角度環輸出的值，作為角速度環的輸入
float Gyro_PID(float Gyro_Data,float Gyro_m)
{
      static float Gyro_Error_Count;
      float Realize,Gyro_Error;
      
      //偏差計算
      Gyro_Error = Gyro_m - Gyro_Data;
      
      //積分
      Gyro_Error_Count += Gyro_Error;
      
      //積分飽和限幅
      if(Gyro_Error_Count>10000) Gyro_Error_Count=10000;
      else if(Gyro_Error_Count<-10000) Gyro_Error_Count=-10000;
      
      //位置PID計算
      Realize = Gyro.P * Gyro_Error +  Gyro.I * Gyro_Error_Count;
     
      return Realize;
}

角度環的控制基本都是采用的PD控制，我看過很多人也都是如此，這沒有什麼争議。

//角度環——位置式PID
//Angle_Data——互補濾波計算出的實際角度
//Angle_m——速度環的輸出，作為角度環的一個輸入
//balance_angle——小車自身的機械平衡角度
float Angle_PID(float Angle_Data,float Angle_m，float balance_angle)
{
      static float Angle_Error_Least;
      float Angle_Error，Realize;
      
      //偏差計算
      Angle_Error = Angle_m - Angle_Data - balance_angle;
      
      Realize = Angle.P * Angle_Error + Angle.D * (Angle_Error - Angle_Error_Least);
      
      //更新上一次的偏差
      Angle_Error_Least = Angle_Error;
      
      return Realize;
}

速度環的話很多人隻用到了P，而第一年本人用到了PI，第二年隻用了P。對比單P控制與PI控制效果感覺差别不大，單P就能夠很好的控制速度了，I的加入隻是更穩定而已，同樣的積分的過飽和反而會破壞平衡起到反效果，建議慎用！！！一般都會對速度環的做種輸出值做一個限幅保護，防止給角度環的值過大。

//速度環——位置式PID
//Speed_L——左輪的速度
//Speed_R——右輪的速度
//Speed_m——設定的目标速度
float Speed_PID(float Speed_L,float Speed_R,float Speed_m)
{
      static float Speed_Error_Integral;
      float Speed_Error;Realize,Speed_New;

	  //計算左右電機的平均速度
      Speed_New = (Speed_L + Speed_R)/2;
      
      //偏差計算
      Speed_Error = Speed_m - Speed_New;
      
      //積分的計算
      Speed_Error_Integral+= Speed_Error;
      if(Speed_Error_Integral>=4000) Speed_Error_Integral=4000;
      if(Speed_Error_Integral<=-4000) Speed_Error_Integral=-4000;
      
      Realize = Speed.P * Speed_Error + Speed_Error_Integral* Speed.I ;
      
      if(Realize>30) Realize=30;
      if(Realize<-30) Realize=-30;
      
      return Realize;
}

參數調試，先調試角速度環，再調試角度環，最後調試速度環，調試的時候無關的環可以設定參數為0。角速度的PI均是讓小車立起來的，單P無法立住，單I的話可以勉強立一下，但是現象就會是前後拼命搖晃。P項反映靈敏快速，I項由于是積分，是以I項反映一般會滞後，但是I項的穩态效果特别好，可按效果對參數修改。要注意的是因為沒有角度環的參與，角速度環隻會在I項的積分

角度環的調試就比較簡單了，單單角速度環已經能在一個位置立住了，角度環調試完畢的效果隻是不需要人為的操作車模自己就能在原地立住，單單給一個參數P就能立的不錯了。

速度環的調試則比較費勁了，它需要跑起來才能更好的看出參數的效果，如果隻是設定速度為0，很難獲得比較好的參數。

角度的擷取

角度的擷取主要則是三種方式而已，一是四元數，二是卡爾曼濾波，三是互補濾波。之前看過網上有人對比了三種方式擷取的波形，幾乎是沒有差别的，本人用的是簡單粗暴的互補濾波（線代困難戶專屬）來融合角度的。

首先說四元數，四元數的話就把陀螺儀和加速度計的值進行一系列的矩陣運算，最後擷取去角度。線代不好的我果斷放棄。大疆RM機器人上的角度用的就是四元數進行運算的，可能是比較好吧。個人對四元數了解不多。

其次說說卡爾曼濾波，他也是用的矩陣運算，但是網上都能随便找到他的例子以及源碼，附錄一下我自己用過的一段卡爾曼濾波的代碼。

void Kalman_Filter_x(float Accel,float Gyro)

{

Angle_x+=(Gyro - Q_bias_x) * dt_x; //先驗估計

Pdot_x[0]=Q_angle_x - PP_x[0][1] - PP_x[1][0]; // Pk-先驗估計誤差協方差的微分

Pdot_x[1]=- PP_x[1][1];

Pdot_x[2]=- PP_x[1][1];

Pdot_x[3]=Q_gyro_x;

PP_x[0][0] += Pdot_x[0] * dt_x; // Pk-先驗估計誤差協方差微分的積分

PP_x[0][1] += Pdot_x[1] * dt_x; // =先驗估計誤差協方差

PP_x[1][0] += Pdot_x[2] * dt_x;

PP_x[1][1] += Pdot_x[3] * dt_x;

Angle_err_x = Accel - Angle_x; //zk-先驗估計

PCt_0_x = C_0_x * PP_x[0][0];

PCt_1_x = C_0_x * PP_x[1][0];

E_x = R_angle_x + C_0_x * PCt_0_x;

K_0_x = PCt_0_x / E_x;

K_1_x = PCt_1_x / E_x;

t_0_x = PCt_0_x;

t_1_x = C_0_x * PP_x[0][1];

PP_x[0][0] -= K_0_x * t_0_x; //後驗估計誤差協方差

PP_x[0][1] -= K_0_x * t_1_x;

PP_x[1][0] -= K_1_x * t_0_x;

PP_x[1][1] -= K_1_x * t_1_x;

Angle_x += K_0_x * Angle_err_x; //後驗估計

Q_bias_x += K_1_x * Angle_err_x; //後驗估計

Gyro_y_x = Gyro - Q_bias_x; //輸出值(後驗估計)的微分=角速度

}

最後再來說一下互補濾波，用的就是兩個陀螺儀積分出來的角度與加速度計反三角函數算出的角度進行互補。有些人可能會說都算出有兩個角度了，還那麼麻煩的融合他幹啥啊。陀螺儀跟加速度計都能計算出角度，但是都各自有着各自的問題。陀螺儀出來的角度因為是積分出來的，因為傳感器誤差存在的溫漂、零飄等，長時間的積分就會導緻角度偏移，與實際值完全對應不上。加速度計出來的角度因為自身原因算出來的角度會抖動非常的劇烈，是以也不能直接用。兩者互補之後的波形則能滿足我們的要求了。以下就是互補濾波的代碼。

void get_data_ang_z(void)

{

MPU_Get_Gyroscope(&gx,&gy,&gz); // 讀取陀螺儀資料

MPU_Get_Accelerometer(&ax,&ay,&az);

gx -= GYRO_Zero_X;

gy -= GYRO_Zero_Y;

gz -= GYRO_Zero_Z;

ang_accel=atan2(ax,az)*180/3.14; //加速度計計算角度

gyro_x=-gx/16.4;

gyro_y=-gy/16.4;

gyro_z=-gz/16.4;

angle = K1 * ang_accel+ (1-K1) * (angle_last + gyro_y * ang_dt);

angle_last = angle;

}

機械結構的影響

這裡說一下機械結構對控制的影響。機械結構對直立控制的影響還是比較巨大的，特别是車速越快對結構的要求就越高，如果隻是單純的想立起來的話對結構的要求則沒那麼高。

主要遵循的原則就是機械重心越集中、越靠近兩車輪軸線越好。車子除去底盤的話基本就電池和電路闆了。電池是比較重的，是以可以優先考慮電池的放置，其次就是考慮電路闆的放置了。重心越低越靠近車輪軸線相應的PID參數的調試就會更加的簡單。反之的話則會更難。

說一下個人經曆，（打一波廣告）剛接觸直立車的我用的是平衡小車之家的那種底盤調試的，因為經費的原因也隻有底盤給我了，我給他配了一個特别特别特别沉的電池，用的傳統控制方案，最後弄了特别久還是不能立起來（可能是我太菜了）。之後換了一個底盤，重心變低也變得靠近車輪軸線字後，兩天就能勉強站起來立住了。

題外話

高速行駛的直立車轉向環跟串級直立的三個環是并行的，是以也會存在幹擾。特别是路面不平（颠婆）的狀态下兩者的幹擾尤為嚴重。直立車的轉向依靠的是兩輪的差速，故也可以把轉向環的輸出串到速度環上邊，作為速度環的一個傳入參數。但是這會帶來一個新的問題，轉向環作為最外環的話其控制周期就會變得特别長。過長的轉向周期也使得車模的轉向變得遲鈍。高速狀态下的直立車如何獲得最優的控制呢？？？

直立車控制方案原理附錄源碼直立車控制原理分析對比

直立車控制原理分析對比

傳統控制方式

串級控制方式

角度的擷取

機械結構的影響

題外話

繼續閱讀

GPS 射頻調試總結

解決STM32調試,無法進入main函數的問題

你所不知道的Android Studio調試技巧單步調試區斷點管理區變量觀察區斷點的分類調試的兩種方式

Android Studio你不知道的進階調試技巧

解決bug的一些總結

java.lang.outofmemoryerror unable to create new native thread

Ubuntu下用dmesg檢視printk的輸出

android studio 調試手機模拟器找不到

通過wifi連接配接手機進行調試

ADT實作手機調試Android應用程式

無線調試android應用——ADB OVER NETWORK

maven插件編寫與調試

netbeanside學習php入門篇

AndroidStudio Debug調試使用單步調試區斷點管理區修改變量值變量觀察區斷點的分類調試的兩種方式

歡迎使用CSDN-markdown編輯器Nginx配置檔案nginx.conf中文詳解（總結）

LoadRunner腳本調試-關聯