參考資料：
https://blog.csdn.net/nemol1990/article/details/45131603
https://blog.csdn.net/qq_27114397/article/details/53378941
https://blog.csdn.net/sunnyxiaohu/article/details/50601577
http://forum.eepw.com.cn/thread/259834/1
常用四軸的兩種PID演算法講解(單環PID、串級PID)
這裡主要講解的PID演算法屬於一種線性控制器，這種控制器被廣泛應用於四軸上。要控制四軸，顯而易見的是控制它的角度，那麼最簡單，同時也是最容易想到的一種控制策略就是角度單環PID控制器，系統框圖如圖所示
或許有些朋友看得懂框圖，但是程式設計實現有一定困難，在這裡筆者給出了虛擬碼：
上述角度單環PID控制演算法僅僅考慮了飛行器的角度資訊，如果想增加飛行器的穩定性(增加阻尼)並提高它的控制品質，我們可以進一步的控制它的角速度，於是角度/角速度-串級PID控制演算法應運而生。在這裡，相信大多數朋友已經初步瞭解了角度單環PID的原理，但是依舊無法理解串級PID究竟有什麼不同。其實很簡單：它就是兩個PID控制演算法，只不過把他們串起來了(更精確的說是套起來)。那這麼做有什麼用？答案是，它增強了系統的抗干擾性(也就是增強穩定性)，因為有兩個控制器控制飛行器，它會比單個控制器控制更多的變數，使得飛行器的適應能力更強。為了更為清晰的講解串級PID，這裡筆者依舊畫出串級PID的原理框圖，如圖所示：
同樣，為了幫助一些朋友程式設計實現，給出串級PID虛擬碼：
二、程式碼學習整理
根據上述的描述，並且參考給出的程式碼：https://github.com/yzhajlydy/Micro-Quadrotor
以下我個人的理解，其中外環的 角度誤差 = 期望誤差 - 當前誤差
當前誤差其實就是當前的角度如pitch, roll, yaw
那麼期望誤差其實就是遙控器的控制角度，遙控器的角度範圍是1000~2000（天地飛07）。那麼比如如果我要控制±40°的訊號
/**************************實現函式******************************************** *函式原型: void Direction_Control(void) *功 能: 前後左右方向控制 *******************************************************************************/ void Direction_Control(void) { //根據遙控器傳過來的前後方向值，改變ZHONGZHI_PIT的給定值 //按40度計算，500/40 = 12.5(pwm/度) 2度是25 ZHONGZHI_PIT = (myControl.remoteControl[1]-1500)/12.5; //根據遙控器傳過來的左右方向值，改變ZHONGZHI_ROL的給定值 ZHONGZHI_ROL = (myControl.remoteControl[0]-1500)/12.5; } /**************************實現函式******************************************** *函式原型: void Outter_PID(void) *功 能: 外環角度控制 *******************************************************************************/ void Outter_PID(void) { //計算X軸和Y軸角度偏差值 e_pit = ZHONGZHI_PIT-myControl.pitch; e_rol = ZHONGZHI_ROL-myControl.roll; //外環PID運算 angular_speed_X = pit_p*e_pit; angular_speed_Y = rol_p*e_rol; }
然後外環的輸出，是內環的輸入。
/**************************實現函式******************************************** *函式原型: void Inner_PID(void) *功 能: 內環角速度控制 *******************************************************************************/ void Inner_PID(void) { //計算X軸和Y軸角速度偏差 e_X[0] = angular_speed_X - myControl.gyro_X; e_Y[0] = angular_speed_Y - myControl.gyro_Y; //===========================繞X軸內環PID運算============================================ //積分分離，以便在偏差較大的時候可以快速的縮減偏差，在偏差較小的時候，才加入積分，消除誤差 if(e_X[0]>=150.0||e_X[0]<=-150.0){ flag_X = 0; }else{ flag_X = 1; e_I_X += e_X[0]; } //積分限幅 if(e_I_X>MOTOR_MAXVALUE) e_I_X=MOTOR_MAXVALUE; if(e_I_X<MOTOR_MIDVALUE) e_I_X=MOTOR_MIDVALUE; //位置式PID運算 PWM_X = kp1*e_X[0]+flag_X*ki1*e_I_X+kd1*(e_X[0]-e_X[1]); //===========================繞Y軸內環PID運算======================================== //積分分離，以便在偏差較大的時候可以快速的縮減偏差，在偏差較小的時候，才加入積分，消除誤差 if(e_Y[0]>=150.0||e_Y[0]<=-150.0){ flag_Y = 0; }else{ flag_Y = 1; e_I_Y += e_Y[0]; } //積分限幅 if(e_I_Y>MOTOR_MAXVALUE) e_I_Y=MOTOR_MAXVALUE; if(e_I_Y<MOTOR_MIDVALUE) e_I_Y=MOTOR_MIDVALUE; //位置式PID運算 PWM_Y = kp2*e_Y[0]+flag_Y*ki2*e_I_Y+kd2*(e_Y[0]-e_Y[1]); //======================================================================================= //記錄本次偏差 e_X[1] = e_X[0]; //用本次偏差值替換上次偏差值 e_Y[1] = e_Y[0]; //用本次偏差值替換上次偏差值 }
三、PID除錯整理
PID控制器我大概就是這麼實現的，首先將四軸固定在單軸平衡平臺上，讓飛行器完成單軸平衡，主要觀察姿態角的
（1）穩定性，能否平衡在期望角度；
（2）響應性，當操縱命令改變時，四軸能否即時的響應期望的變化；
（3）操縱性，由操縱員感受四軸的姿態是否已與操縱，會不會產生響應過沖。
P
因為PID有內外環之分，所以應該先從內環調節，內環穩定後再調外環。
內環的順序是先調P,其他I、D設為0，那麼如何確定P的值呢？主要的現象是等幅震盪，就是四軸會繞著軸來回擺動。
調節P時遇到的情況：
1）四軸開啟後加速翻轉，說明P的值方向錯了。如果P值過大，可能出現自轉的情況，就是繞著軸最大馬力旋轉（太可怕了）
2）四軸在某個角度稍稍停頓，然後掉落，說明P給小了
3）當P太小時，四軸在很大傾斜的地方，在重力與P的作用下也會震盪，這種震盪不是等幅的，也不是對稱的，震盪波谷的絕對值明顯要比波峰絕對值大而且距離0度會很遠。這種情況要加以區分。
參考資料中說，在有一點反饋力到等幅震盪的區間內確定四軸的P值。
D
然後開始調D，如果說P是回覆力的話，那麼D就是阻尼。雖然它能抑制震盪，但同時也會抑制P的效果。
當震動產生時，在震動的中心點四軸震動速度最大，也是D作用最強的時刻。
加了D效果就比較明顯了，四軸在外力的干擾先能明顯的有回覆力且，能快速穩定在平衡點了，調D就是試，當然D大了也會產生震盪，但是此時不加D時光P作用時的震盪就很小，很明顯就可以看出隨著D的增大，震盪減小又增大的過程。
I
調 I 的時候我先把積分限幅去掉。然後從小往大加，當調平衡時，隨著油門的變大，靜差應該是越來小的。
我將油門推到差不多快要將四軸推離地面的位置，看I能不能消除靜差。因為如果油門給小了，靜差較大，調出來的I雖然能消除靜差但是I比較大，在加油門時有可能也會產生超調震盪。
最後I要再能消除靜差又不產生震盪，個人認為要近可能小。最後加上合適的積分限幅。
其他資料的PID整定：
而筆者在整定串級PID時的經驗則是：先整定內環PID，再整定外環P。
內環P：從小到大，拉動四軸越來越困難，越來越感覺到四軸在抵抗你的拉動；到比較大的數值時，四軸自己會高頻震動，肉眼可見，此時拉扯它，它會快速的振盪幾下，過幾秒鐘後穩定；繼續增大，不用加人為干擾，自己發散翻機。
特別注意：只有內環P的時候，四軸會緩慢的往一個方向下掉，這屬於正常現象。這就是系統角速度靜差。
內環I：前述PID原理可以看出，積分只是用來消除靜差，因此積分項係數個人覺得沒必要弄的很大，因為這樣做會降低系統穩定性。從小到大，四軸會定在一個位置不動，不再往下掉；繼續增加I的值，四軸會不穩定，拉扯一下會自己發散。
特別注意：增加I的值，四軸的定角度能力很強，拉動他比較困難，似乎像是在釘釘子一樣，但是一旦有強干擾，它就會發散。這是由於積分項太大，拉動一下積分速度快，給 的補償非常大，因此很難拉動，給人一種很穩定的錯覺。
內環D：這裡的微分項D為標準的PID原理下的微分項，即本次誤差-上次誤差。在角速度環中的微分就是角加速度，原本四軸的震動就比較強烈，引起陀螺的值變化較大，此時做微分就更容易引入噪聲。因此一般在這裡可以適當做一些滑動濾波或者IIR濾波。從小到大，飛機的效能沒有多大改變，只是回中的時候更加平穩；繼續增加D的值，可以肉眼看到四軸在平衡位置高頻震動(或者聽到電機發出滋滋的聲音)。前述已經說明D項屬於輔助性項，因此如果機架的震動較大，D項可以忽略不加。
外環P：當內環PID全部整定完成後，飛機已經可以穩定在某一位置而不動了。此時內環P，從小到大，可以明顯看到飛機從傾斜位置慢慢回中，用手拉扯它然後放手，它會慢速回中，達到平衡位置；繼續增大P的值，用遙控器給不同的角度給定，可以看到飛機跟蹤的速度和響應越來越快；繼續增加P的值，飛機變得十分敏感，機動效能越來越強，有發散的趨勢。
整體方法：
1，將內外環PID都歸0，適當增加內環的P，調整P至四軸從正面朝上自然轉動到正面朝下時能感受到阻力，且沒有抖動，有抖動就應減小P，當P減小到無抖動或者輕微抖動時即可。
2，讓內環的D慢慢增加，到你用手能明顯感受到轉動四軸產生排斥外力的阻力即可，D能抑制P產生的振盪，但是D過大也會導致高頻振盪，調整D至系統無振盪且能抑制外界的力即可。
3，給內環一點點I，注意的是I的積分要在油門開啟後才開始，油門關閉就清0，且必須有積分限幅。I推薦取越小越好，我取的是0.01，I取大了會導致系統振盪。
4，將內環P減半，將外環P調至內環的50-70倍，根據系統產生的高頻振盪降低內環的D，直至高頻振盪消除即可。
5，給外環一點點I，同3.
6，根據實際情況對引數進行優化調整，調整過程中要注意區分各個引數的作用，時刻記住，P是回覆力，大了會低頻振盪，D是抑制力，大了會高頻振盪，I是靜差消除力，越小越好，大了會產生振盪。
四、衰減曲線法
https://blog.csdn.net/zhuifeng_tjy163163/article/details/78961017
我自己的實驗記錄：
我把油門搖桿打上3格，P調到1的時候反應有點小，2的時候。用手保持飛機朝上，輕輕放手，再拉住。感覺有明顯的來回感，有時候掉下去快了，回饋力也比較大。
調到4的時候還是會往下掉，調到5的時候落下去回饋過頭，翻過去了。4.5還是翻過去了，4.3下落的時候頓了一下。大概4.6到4.5的時候會反轉，而4.3到4.5就會頓一下再下落。
-------------------------------------------------------------------------------------------------------------
2018年8月3日 20點51分 好痛苦，調了這麼久都沒效果。後來用示波器看了波形速率。發現9250的DMP讀取有問題。
STM32F103如果把讀取速率取到100，加上其他工作諸如串列埠啊（改成DMA方式傳送還是佔用資源）、定時器啊、外部中斷啊都會被影響
後來我取了80效果一頓一頓的了，取到50發現匹配的剛剛好。如果有硬體I2C或許會快點，不過出來的曲線好多了。很平滑，沒有之前的折線了。
為什麼會發現這個問題？我在修改PID的時候，飛機要到30度傾角才有反應。這就讓我很困惑，加大到了200Hz效果依然不好，最後用示波器除錯。配合上位機才定位到問題。
感覺自己都在走彎路，硬體效能都沒弄好還一個勁的調PID，調的自己還很洩氣。
-----------------------------------------------------------------------------------------------------------------
硬體問題解決了，飛機的反應立馬好了不少。再也沒有之前的頓頓的強烈反應，取而代之的是平滑的往下滑，好蛋疼。雖然角速度基本沒動，但是角度一直會慢慢往下滑。我在想是不是要加上外環才能穩定，加了外環也好像不能改善。
------------------------------------------------------------------------------------------------------------------
我又除錯了一下，現在四軸的情況是，內環把角速度控制在比較小的變動範圍，但是飛機還是會往下翻。然後我用外環控制姿態，意味著飛機一直會調整一下調整一下。
這樣的情況單軸除錯效果還行，但是試飛階段時，我發現除錯油門比起飛油門小了不少，導致飛機起飛的時候非常不穩。然後再改回單軸除錯發現，用起飛的油門除錯時。
P是太大了，我還是要重新調整。