前言：在家準備電賽控制題，第一個選擇的旋轉倒立擺，結構和電路相對簡單，對於新手比較友好。本人今年大二，自學的STM32和PID算法，本文算是對這個題目練習的記錄吧，文章和程序有誤的地方還請大家多多指教。
一、基本原理（可能有誤）
原理這一塊沒有做的太複雜，只是稍微估計了電機需要的速度值。當擺臂失去平衡後，就需要一個慣性加速度平衡重力加速度，這個慣性加速度就是電機施加的加速度。以豎直方向右偏30度爲例，則平衡所需要的慣性加速度大約是5.77m/(s^2)。又根據功能關係，可估算此時擺杆重心處速度爲1m/s。設定在0.5s內將擺杆恢復至豎直方向,以gsin(pi/6)來估算重力加速度分量，以asin(pi/6)來估算施加的加速度，最終算得a=6.9m/(s*s)，則電機需要的速度是v=3.5m/s。又考慮到實際過程中電機不能始終在滿速和停止之間來回運動，且在控制過程中要考慮到干擾的施加，那麼擺杆的速度不止1m/s，這樣看來至少需要旋轉臂25cm情況下滿速爲10m/s以上的電機比較合適。
二、器件選擇
1、電機採用轉速530rad/min12v的JGB37-520電機，驅動選擇tb6612驅動模塊，傳感器選擇線性誤差在0.1%的WDD35D4電位器。
2、這一次的缺陷就是沒有加入穩壓電路，這個太重要了，因爲在調節PID參數的過程中，需要保持整個系統變化的只有參數值。試想一下，當我們在調節PID參數控制PWM輸出時，電壓不斷波動，那麼控制效果將隨電壓波動時好時壞。又比如這次我採用的是12V電池，那麼每次充完電後，電壓值都會比我上次調節好參數值時有變化，那麼我又需要再次調節參數值，所以這個題目花費了很長時間，而且在電壓波動的情況下，無論怎麼改變參數值，擺臂的震動都比較大，調節效果並不是很理想。
三、機械結構
考慮到正式比賽時需要自己搭建機械結構，我就沒有直接購買現成的機械結構。關於機械結構還是平時接觸太少了，隨便在淘寶上買的不鏽鋼打孔支架作擺臂和旋轉臂，最後發現傳感器沒法安放，強行用電機支架和膠帶固定住。最困難的是怎麼把電機和旋轉臂連接得牢靠，最開始用的一個聯軸器，發現轉的猛了就會鬆動，想盡各種辦法，最後是用三個聯軸器連接後才能連接得牢靠，爲了增加高度，在三個基本的聯軸器上又重了三個聯軸器。底座用四個直角電機支架連接，這樣就算來回擺動也不會引起振動。最後整個結構用釘子釘在書桌上（心疼書桌一秒），這樣就算比較穩定的機械結構了。
四、倒立穩定實現
(一)、PID算法
PID的講解在網上有很多專業又詳細的教程，我就不再說了，這裏講一下這一個月調節PID參數後對PID參數的理解：
P值——快速接近目標值，誤差越大，效果越強——容易導致超調；
I值——消除靜差——提高準確性（但我沒用，容易使響應過慢）；
D值——消除震盪，預測誤差變化——使系統趨於穩定;
(二)、角度PID
這個PID是主要的控制量，應該作爲整個控制過程的主導量。代碼並不複雜，我採用的是位置式PID，如下：
float angle_pid(float adc_value)
{
static float error,pwm,error_1;
error=middle_value-adc_value;
pwm=error*a_p+(error-error_1)*a_d;
error_1=error;
return pwm;
}如何調節PID參數，我記錄一下自己調節的過程：
1、首先將D值和I值置0，單獨調節P值，最開始可以設置得大一點，例如在4999爲上限的PWM值下，我一般先設置P值爲55~60，這時大概率系統響應過度，會造成擺臂在中值處來回劇烈震盪，這時就可以按照每次減1的速度來調節P值，直到調節到電機能快速做出反應的情況下不會造成過度的震動。我沒有去具體分析P值調節下角度變化的波形，因爲我的電壓不穩定，不可能調節得很合適。
2、P值調節好後，擺臂應該能在中值處穩定一小段時間，之後會左右做小幅度擺動，或者朝一個方向運動，最後落下。這時需要加入D值，提前預測角度變化，消除震盪。同樣，D值可以預設得很大，這時擺臂會出現低頻震動，但是比單獨P值調節要穩定，依次減少D值，直到擺臂能穩定在中值附件，最終朝一個方向加速運動下去並落下。這時角度環的PID參數就調好了。
(三)、位置PID
最開始很不理解這個位置環是怎麼工作的，查網上資料說是串級PID，但都沒有很清楚講訴爲什麼需要位置環PID，這個位置環又是怎樣和角度環配合的。這裏結合蒐集到的資料，記錄一下自己的理解：
1、位置環作用本質
首先角度環PID已經能讓擺臂穩定在中值附近，但是還是會朝一個方向轉動下去，這是因爲角度環控制的PWM值不能再糾正最後和中值的一點差值。有人會說那就加大PID參數啊，但是這樣會導致超調，一點點的變化都會讓擺臂劇烈擺動，最後還是落下。
解決這個問題的辦法就是定期根據電機旋轉的方向和距離加大PWM調節量，糾正最後一點誤差。比如以電機編碼器的值作爲判斷數據，編碼器輸出正值代表正轉，在角度環調節的情況下，如果擺臂引導旋轉臂向正方向一直轉動，編碼器累計值超過預設值時，就可以判斷角度環沒有及時糾正最後一點誤差，這時就讓PWM值增大速率更大，電機加速度更大，那麼最後一點差值就糾正了。
而這個定期加大PWM值的作用就是所謂的位置環，有了位置環，就能把旋轉臂的調節範圍控制在一定範圍內
2、位置環如何配合角度環
其實理解了位置環是如何作用的之後，就知道怎樣去設置位置環了。至於網上說的什麼角度環輸出作爲位置環輸入我硬是沒搞懂。我就按自己的想法來弄，根據定時器定時開啓位置控制，比如每50ms開啓一次，每次持續25ms，這樣就能和角度環配合起來用了。
以上就是我的理解，代碼如下：
float position_pid(void)
{
static float bian_value,bian_error,last_bian_error,pwm,bian_last_value;
static float bian_basic,bian_weifen;
if(position_pd==1)//當起擺成功後刷新位置信息
{
bian_basic=0;
position_pd=0;
}
//位置環需要編碼器累計值作判斷
bian_value=bian_read();//讀取當前編碼器值
bian_basic=bian_basic*0.5+bian_value;
bian_error=bian_basic-position_value;
bian_weifen=bian_error-last_bian_error;
pwm=bian_basic*p_p+bian_weifen*p_d;
last_bian_error=bian_basic;
bian_last_value=bian_value;
return pwm;
}五、關於自動起擺
(一)、基本思路
自動起擺的過程主要分爲三步。
第一步，先定時來回震盪，增大擺臂擺動幅度，直到擺臂超過水平線；
第二步，繼續定時震盪，這時擺臂將有機會達到倒立穩定控制的角度範圍內，一旦達到這個範圍，立即單獨開啓角度環PID控制，由於考慮到擺臂此時速度較大，角度變化較快，應該增大D值控制，能比較好的穩定擺臂。經過對比實驗，確實增大D值利於對擺臂的穩定。角度PID將持續300ms左右；
第三步，恢復預設PID參數值，開啓角度環和位置環配合的PID控制，持續穩定擺臂。
(二)、中途異常掉落處理
爲了能及時檢測擺臂異常掉落，但是UCOSIII還沒學會，我想到可以將延時控制過程分成很多次循環，每次循環延時5ms，那麼每5ms就能檢測一次角度值，如果出現異常，就退出循環，重新起擺。
程序如下：
void move_auto()
{
static int i=0,time=5,flag=0,open_contrl=0,open_angle_control=0,n=0;
static float adc_value=0,pwm;
adc_value=Get_Adc_Average(ADC_Channel_10,5);
//flag==0 未達到預定角度 不開啓穩定控制
//定時器起擺
if(flag==0)
{
adc_value=Get_Adc_Average(ADC_Channel_10,5);
while((adc_value>xia_xian&&adc_value<shang_xian)!=1)
{
//400ms定時
for(i=0;i<80;i++)
{
motor(4048);
delay_ms(time);
adc_value=Get_Adc_Average(ADC_Channel_10,5);
//每5ms檢測一次角度值
//如果達到預定角度
//立即開啓穩定控制
if(adc_value>xia_xian&&adc_value<shang_xian)
{
open_contrl=1;
open_angle_control=1;
while(open_contrl)
{
adc_value=Get_Adc_Average(ADC_Channel_10,5);
//判斷穩定成功否 未成功則退出 繼續定時起擺
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) open_contrl=0;
//首先單獨使用角度環控制
//此時需要的PID參數中 D值將要更大一些
//因爲考慮到速度影響大
if(open_angle_control==1)
{
for(n=0;n<50;n++)
{
a_p=73;
a_i=0;
a_d=380;
p_p=0;
p_d=0;
//同樣判斷擺臂是否異常落下
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
pwm=banlance_pid(adc_value);
motor(pwm);
delay_ms(5);
}
n=0;
open_angle_control=0;
//更改參數位穩定控制參數
a_p=56;
a_i=0;
a_d=100;
p_p=2000;
p_d=500;
position_pd=1;
}
//穩定控制開啓
pwm=banlance_pid(adc_value);
motor(pwm);
}
}
}
//反向定時起擺 和上一段配合擺動
for(i=0;i<80;i++)
{
motor(-4048);
delay_ms(time);
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if(adc_value>xia_xian&&adc_value<shang_xian)
{
open_contrl=1;
open_angle_control=1;
while(open_contrl)
{
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) open_contrl=0;
if(open_angle_control==1)
{
for(n=0;n<50;n++)
{
a_p=73;
a_i=0;
a_d=380;
p_p=0;
p_d=0;
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
pwm=banlance_pid(adc_value);
motor(pwm);
delay_ms(5);
}
n=0;
open_angle_control=0;
a_p=56;
a_i=0;
a_d=100;
p_p=2000;
p_d=500;
position_pd=1;
}
pwm=banlance_pid(adc_value);
motor(pwm);
}
}
}
adc_value=Get_Adc_Average(ADC_Channel_10,5);
}
flag=1;
}
//當flag==1時開啓穩定控制
while(flag==1)
{
open_angle_control=1;
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) flag=0;
if(open_angle_control==1)
{
for(n=0;n<50;n++)
{
a_p=73;
a_i=0;
a_d=330;
p_p=0;
p_d=0;
adc_value=Get_Adc_Average(ADC_Channel_10,5);
if((adc_value>pan_xia_xian&&adc_value<pan_shang_xian)!=1) n=100;
pwm=banlance_pid(adc_value);
motor(pwm);
delay_ms(5);
}
n=0;
open_angle_control=0;
a_p=56;
a_i=0;
a_d=100;
p_p=2000;
p_d=500;
position_pd=1;
}
pwm=banlance_pid(adc_value);
motor(pwm);
}
} 六、抗干擾性測試
根據題目要求需要抗干擾測試。在5g砝碼拉起90度後向下釋放撞擊擺臂上方1——2cm處時，擺臂能迅速調整，並在2s內恢復正常，在極端情況下會出現擺臂落下的情況，但都能再次自動起擺並穩定。
七、穩定時旋轉一週
基本思路是定時該變位置控制的目標位置值。雖然有一點控制效果，能穩定旋轉幾周，最後還是變得不穩定，擺臂落下，算時勉強有點效果吧。在網上很少找到這方面的資料，還望路過的大神能賜教。
八、總結
1、控制題首先需要建立物理模型，做出運動分析，這一點我做的不夠好，導致這次電機選型一開始選的轉速過小，不能滿足加速度需求，後來被迫重新購買電機。網上有大佬用拉格朗日方程建立物理模型，需要好好學習一下（大概率看不懂）。
2、PID參數調節一定要在其他變量都基本不變的情況下進行，尤其時電壓。機械結構底盤必須夠穩，不然擺臂還沒穩定，就被底座的擺動搖亂了。理論分析和機械結構搭建在前期可以多花一些時間，不然後期改機械結構和重新理論分析真的太花費時間了。
3、這次加深了PID的理解，各類模塊的配置過程和程序都掌握的不錯，也算是爲電賽打下基礎了。
最後還是貼一個程序吧，供大家探討和賜教：
鏈接：https://pan.baidu.com/s/1q_LFi_9Xzfan7bUUh4foRw
提取碼：6804