旋轉倒立擺調節經驗

前言

近期在做2013年電賽控制類題目–簡易旋轉倒立擺裝置，自己並不是自動化專業的學生，沒有學過自動控制原理，倒立擺其實是一個十分經典的自動控制模型，我們只能是邊做邊學習，逐漸去了解倒立擺。
我認爲倒立擺有兩個難點，一個是自動起擺一個是機械結構，其中自動起擺涉及到PID算法與運動方程的求解，而機械結構主要是儘量減小轉動阻尼同時避免旋轉時線的纏繞。我們買了平衡小車家的機械結構套件，他們爲了避免線纏繞使用了導線環，這是一個好東西，可以完美解決導線纏繞問題。我在學習平衡小車家程序與算法的過程中也是總結了一些經驗，在這裏分享一下。

程序框架

平衡小車家旋轉倒立擺的代碼符合他們家的一貫作風，所有的控制算法在定時中斷中實現，可以進行確定頻率的控制，使用了一個5ms中斷，在中斷中完成數據讀取、PWM控制量計算、對電機的控制，這樣的代碼結構清晰、響應快，但是也有些缺點，如果你要有功能選擇，在中斷中的代碼是比較複雜的，要麼使用switch-case完全放棄代碼的共用，如果有共用部分的代碼那麼代碼邏輯要仔細考慮。
對於那些執行頻率不必太高的功能，比如50ms向上位機發送數據、100ms進行LED閃爍、100ms讀取電池電壓，你可以用一個每次中斷累加的變量來進行控制，當它取餘爲某一值時執行某一功能，這樣就類似與一個任務調度器了。
還有一點要注意的是，在中斷中那些重要的、每次都必須執行的代碼要放在靠前的位置，而那些對實時性要求不太高的代碼可以往後稍一稍，因爲每次進入中斷就清除中斷標誌位，下一個中斷會在5ms後準時到來，如果你當前的中斷沒有執行完則會被打斷（一般是不會發生這樣的事情，除非在中斷中做了耗時很長的操作），所以爲了避免打斷那些控制代碼要放在靠前的位置。
最後，在中斷中不放耗時久的操作這也是常識了，那些耗時久的以及耗時久同時有時序要求的代碼都可以放在主函數中執行，比如OLED的刷新、向上位機的數據發送，示例代碼如下：

while(1)
{      
	DataScope();	            //===上位機
	delay_flag=1;	            //===50ms中斷精準延時標誌位
	oled_show();               //===顯示屏打開	  	
	while(delay_flag);         //===50ms中斷精準延時  主要是波形顯示上位機需要嚴格的50ms傳輸週期   					
	if(Flash_Send==1)          //===寫入PID參數到Flash,由按鍵控制該指令
	{
		Flash_Write();	        //===把參數寫入到Flash
		Flash_Send=0;	          //===標誌位清零
		Flag_OLED=1;            //===顯示標誌位置1 在顯示屏上面顯示Data Is Saved的字樣
	}							
} 

這裏每50ms進行一次數據發送和刷屏，使用while(delay_flag);來等待50ms標誌位，時間控制的還是比較好的。

關於直立

我們的旋轉倒立擺使用旋轉電位器來獲取擺杆的當前角度，電機使用的是帶編碼盤的直流電機。我們要保持的直立有兩個要求，擺杆垂直與平面同時要儘量靜止與原地，不會隨意的轉動。
所以說要有兩個環來控制，首先是角度環，以旋轉電位器的值與垂直點的偏差爲輸入，目標是把這個偏差控到零，我們使用的PD控制器，因爲該系統有一定的滯後性，而且是一個不穩定的系統，我認爲並沒有消除靜差的需求，所以沒有上積分，代碼如下：

int balance(float Angle)
{  
	float Bias;                       //傾角偏差
	static float Last_Bias,D_Bias;    //PID相關變量
	int balance;                      //PWM返回值 
	Bias=Angle-ZHONGZHI;              //求出平衡的角度中值 和機械相關
	D_Bias=Bias-Last_Bias;            //求出偏差的微分 進行微分控制
	/*===計算傾角控制的電機PWM  PD控制===*/
	balance=-Balance_KP*Bias-D_Bias*Balance_KD;   
	Last_Bias=Bias;                   //保持上一次的偏差
	return balance;
}

角度環參數的整定，首先調節KP這沒什麼說的，首先確定極性，然後從低往高調節到出現低頻抖動（比較嚴重的），接下來確定KD的極性，注意KD是負反饋，偏差增大時應該控制系統往減小偏差的方向運動，這樣才能幫助擺桿直立，所以確定極性後從小往大調節，KD的參數是要比較大的，因爲偏差的微分每次都會是一個比較小的值，調到系統基本可以直立，並且有高頻振動時就可以了，這樣我們就確定了KP和KD的最大參數，將它們乘0.6就可以得到正常的PD參數。
接下來就是位置環了，在前面我們調節好直立環後，擺杆可以直立一段時間，但是它會逐漸往一個方向加速，然後直到轉到全速後倒下，我們就需要位置環來讓它穩在原地，同時給它更快的速度去追要倒下的杆。這裏我們一開始想錯了，我們錯以爲位置環的作用就是使擺杆停在某一個位置，即進行負反饋，但是這樣調的結果是擺杆更加不能倒立了還不如只有直立環的效果好，查閱資料後發現應該使用正反饋，應該讓速度更快去追倒下的擺杆，也就是如果只有位置環，你往任意方向轉動懸臂，懸臂會往這個方向加速旋轉直到全速。位置環的輸入爲編碼器的值與目標點的差值，同樣適用PD調節，代碼如下：

int Position(int Encoder)
{  
   static float Position_PWM,Last_Position,Position_Bias,Position_Differential;
	 static float Position_Least;
  	Position_Least =Encoder-Position_Zero;             			//===最近位置差值
    Position_Bias *=0.8;		   
    Position_Bias += Position_Least*0.2;	             		//===一階低通濾波器  
	Position_Differential=Position_Bias-Last_Position;
	Last_Position=Position_Bias;
	Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===速度控制	
	return Position_PWM;
}

使用低通濾波器的作用是減緩位置差值對平衡的影響，也就是上次位置差值佔80% + 這次速度差佔20% =此次速度差值。
調節位置換我們沒有太好的辦法，只能是根據經驗給一些值，KP參數不要太大，否則會對直立環有較大的影響，KD參數要大一些，使得在最短時間內達到最大速度。這裏建議不要單獨調位置環，最好把直立環和位置環一起調節，邊看效果邊看數據曲線邊調。
如果要實現在保持平衡的時候
最後的效果我們基本可以實現平衡小車家視頻中的效果，可以較好的實現直立。

關於自動起擺

自動起擺的程序我們研究的也不是特別的透徹，尤其是涉及運動方程的部分，還沒有自己去推公式，我們的自動起擺程序大致分爲兩步，第一步是讓擺的擺幅逐漸增大，直到接近於水平，這裏增大擺幅的同時還有略微減小週期，這可以通過運動公式來計算出來，當擺幅達到要求且正在下落時懸臂會迅速的往下落的方向旋轉大約半圈，然後它由於過沖會有一個回擺，通過這個回擺就會把擺杆擺起來，這時開啓直立環，因爲位置環會削弱直立環的作用，所以在剛剛立起擺杆不夠穩定的情況下先不開啓位置環，過300ms後開啓位置環，這時自動起擺的過程就完成了。代碼如下：

void Run(u8 Way)
{ 		   
	  static float Count_FZ,Count_Next,Target_Position=10380;
	  static u8 Flag_Back;
  	static float Count_Big_Angle=0.046542;
	  static int Position_Max;
	if(Way==1)                                 //進入自動起擺程序 按鍵觸發
	{
	if(Flag_qb==1)  //第1步，擺杆自由擺動，振幅越來越大
	{
		Ratio=1; //正常的PID參數
		Count_qb+=Count_Big_Angle;   //自變量
		Count_Big_Angle-=0.0000027;      //振幅越大，擺動週期略微減小
		Count_FZ+=0.025;   //振幅越來越大
		Target_Position=0.6*Count_FZ*sin(Count_qb)+10000;  //運動公式		 
		Encoder=Read_Encoder(2);             	       //===更新編碼器位置信息
		Moto_qb=Position_PID(Encoder,Target_Position);  //位置閉環控制
		if(Moto_qb>7200)Moto_qb=7200;//控制位置閉環控制過程的速度
		if(Moto_qb<-7200)Moto_qb=-7200;//控制位置閉環控制過程的速度
		//2100
		if(Angle_Balance>(Angle_Max+850)&&Angle_Balance<2320&&D_Angle_Balance<=-1)   //振幅大於閾值時，進入下一步 
		{
			Flag_qb++;
			Count_qb=0;
			TIM2->CNT=10000;        //復位一下計數寄存器
			Count_FZ=0;
		}
	Set_Pwm(Moto_qb); //賦值給PWM寄存器 
	}
	 if(Flag_qb==2)            //第3步，通過位置控制，利用慣性，自動起擺
	 {	 
		 Target_Position=10600;                        //設定目標值
		 Encoder=Read_Encoder(2);             	       //===更新編碼器位置信息
	 Moto_qb=Position_PID(Encoder,Target_Position);//===位置PID控制器
		 if(Moto_qb>7200) Moto_qb=7200;
		 if(Moto_qb<-7200)Moto_qb=-7200;
	 Set_Pwm(Moto_qb);  //賦值給PWM寄存器
		if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200))  //到底接近平衡位置 即可開啓平衡系統
		{	
			State=1;   //倒立狀態置1
			Way_Turn=0;//自動起擺標誌位清零
			Flag_qb=0; //自動起擺步驟清零
			Angle_Max=0;
			Balance_KP += 30;
			Balance_KD += 200;
		}
	 }
	 if(Flag_qb==3)            //停
	 {
		AIN1 = 1;AIN2 = 1;
	 }
	}
	if(Way==2) //手動起擺程序
	{
	if(Angle_Balance<(ZHONGZHI+200)&&Angle_Balance>(ZHONGZHI-200))  //到底接近平衡位置 即可開啓平衡系統
		{	
			State=1;   //倒立狀態置1
			Way_Turn=0;//起擺標誌位清零
		}
	}
}
int Position_PID (int Encoder,int Target)
{ 	
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Encoder-Target;                                   //計算偏差
	 Integral_bias+=Bias;	                                  //求出偏差的積分
 //	  Pwm=70*Bias*Ratio+0.00*Integral_bias*Ratio+200*(Bias-Last_Bias)*Ratio;   //位置式PID控制器
  Pwm=90*Bias*Ratio+0.00*Integral_bias*Ratio+250*(Bias-Last_Bias)*Ratio;   //位置式PID控制器
	 Last_Bias=Bias;                                        //保存上一次偏差 
	 return Pwm;                                            //增量輸出
}

有幾個注意的點。這裏運動方程我並不知道該如何調，這個是根據你擺杆重心、電機特性、當地重力加速度等算出來的，這個還是得試着推一下；還有一個點是剛剛立起來時直立環參數應該調大一點，我們就設置成了直立環的最大參數，300ms後在用正常的參數加上位置換來控制，因爲我們在調節時發現起擺後的一瞬間經常沒能直立，擺的抗干擾性不夠強，所以加大了直立環參數，用這樣的方法起擺的成功率大大增加。