四軸PID控制算法

四軸控制原理

四軸飛行器的螺旋槳與空氣發生相對運動，產生了向上的升力，當升力大於四軸的重力時四軸就可以起飛了。

四軸飛行器飛行過程中如何保持水平呢？

我們先假設一種理想狀況，四個電機的轉速是完全相同的，是不是我們控制四軸飛行器的四個電機保持同樣的轉速，當轉速超過一個臨界點時（升力剛好抵消重力）四軸就可以平穩的飛起來了呢？

答案是否定的，由於四個電機轉向相同，四軸會發生旋轉。我們控制四軸電機1和電機3同向（逆時針旋轉），電機2電機4反向（順時針旋轉），剛好正反扭矩抵消，巧妙的實現了平衡，如下圖所示。

實際上由於電機和螺旋槳本身製造的差異我們無法做到四個電機轉速完全相同，很有可能飛行器起飛之後就側翻。這時候大家可能會想到要用遙控器來控制電機，我們來嘗試一下下面向右側翻的情況。

由於電機的不平衡，在人眼的觀察下發現飛機向右側翻，我們控制右側電機1電機2提高轉速增加升力，飛機歸於平衡。由於 飛機是一個動態系統，在接下來我們會一直重複：觀察->大腦計算->控制->觀察->大腦計算->控制。

但事實上這是不可能的，因爲人無法長時間精確的同時控制四個電機。我們需要一個自動反饋系統替代人操作來完成飛機的自穩定，我們人只需要控制飛機的方向和高度就可以了。這個系統中反饋由姿態傳感器替代眼睛，而大腦則由單片機來替代。這時候該PID控制系統出場。

PID控制理論

PID控制是最常見，應用最爲廣泛的自動反饋系統。PID控制器由偏差的比例（P，Proportional）、積分（I，Integral）和微分（D，Derivative）來對被控對象進行控制。這裏的積分或微分，都是偏差對時間的積分或微分。

對於一個自動反饋控制系統來說，有幾個基本的指標。

• 穩定性（P和I降低系統穩定性，D提高系統穩定性）：在平衡狀態下，系統受到某個干擾後，經過一段時間其被控量可以達到某一穩定狀態；
• 準確性（P和I提高穩態精度，D無作用）：系統處於穩態時，其穩態誤差（Steady-state error）；
• 快速性（P和D提高響應速度，I降低響應速度）：系統對動態響應的要求。一般由過渡時間的長短來衡量。

比例（P）控制

比例控制是一種最簡單的控制方式。其控制器的輸出與輸入誤差信號成比例關係。當僅有比例控制時系統輸出存在穩態誤差。比例項輸出：

積分（I）控制

在積分控制中，控制器的輸出與輸入誤差信號的積分成正比關係。對於只有比例控制的系統存在穩態誤差，爲了消除穩態誤差，在控制器中必須引入“積分項”。積分項是誤差對時間的積分，隨着時間的增加，積分項會增大。這樣，即便誤差很小，積分項也會隨着時間的增加而加大，它推動控制器的輸出增大使穩態誤差進一步減小，直到等於零。因此，比例積分(PI)控制器，可以使系統在進入穩態後無穩態誤差。 積分項輸出：

微分（D）控制

在微分控制中，控制器的輸出與輸入誤差信號的微分成正比關係。微分調節就是偏差值的變化率。使用微分調節能夠實現系統的超前控制。如果輸入偏差值線性變化，則在調節器輸出側疊加一個恆定的調節量。大部分控制系統不需要調節微分時間。因爲只有時間滯後的系統才需要附加這個參數。微分項輸出：

綜上所述得到一條公式，這個就是PID控制數學表達式：

更多關於PID控制理論知識，可以參考維基百科PID控制。

四軸PID控制-單環

在Crazepony四軸飛行器5.0版本及以前，我們使用的是單環增量式PD控制，下面是角度單環PID控制框圖。這裏對圖中幾個數據進行說明，期望角度就是遙控器控制飛行器的角度值，反饋當前角度就是傳感器測得的飛行器角度，這裏的角度指的是Roll/Pitch/Yaw三個角度，而且在PID控制計算的時候，是相互獨立的。

以ROLL方向角度控制爲例：

• 測得ROLL軸向偏差：

偏差=目標期望角度-傳感器實測角度 
DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll;

• 比例項的計算：

比例項輸出 = 比例係數P * 偏差
Proportion =  PID_Motor.P * DIF_ANGLE.X;

• 微分項計算： 由於陀螺儀測得的是ROLL軸向旋轉角速率，角速率積分就是角度，那麼角度微分即角速率，所以微分量剛好是陀螺儀測得的值。

微分輸出=微分系數D*角速率
DifferentialCoefficient = PID_Motor.D * DMP_DATA.GYROx;

• 整合結果總輸出爲：

ROLL方向總控制量=比例項輸出+微分量輸出

ROLL和PITCH軸按照以上公式計算PID輸出，但YAW軸比較特殊，因爲偏航角法線方向剛好和地球重力平行，這個方向的角度無法由加速度計直接測得，需要增加一個電子羅盤來替代加速度計。如果不使用羅盤的話，我們可以單純的通過角速度積分來測得偏航角，缺點是由於積分環節中存在積分漂移，偏航角隨着時間的推移會偏差越來越大，就會出現航向角漂移的問題。我們不使用羅盤就沒有比例項，只僅使用微分環節來控制。

• YAW軸輸出：

微分輸出=微分系數D*角速率

YAW方向控制量 = PID_YAW.D * DMP_DATA.GYROz;

四軸PID控制-串級

角度單環PID控制算法僅僅考慮了飛行器的角度信息，如果想增加飛行器的穩定性(增加阻尼)並提高它的控制品質，我們可以進一步的控制它的角速度，於是角度/角速度-串級PID控制算法應運而生。在這裏，相信大多數朋友已經初步瞭解了角度單環PID的原理，但是依舊無法理解串級PID究竟有什麼不同。其實很簡單：它就是兩個PID控制算法，只不過把他們串起來了(更精確的說是套起來)。那這麼做有什麼用？答案是，它增強了系統的抗干擾性(也就是增強穩定性)，因爲有兩個控制器控制飛行器，它會比單個控制器控制更多的變量，使得飛行器的適應能力更強。爲了更爲清晰的講解串級PID，這裏筆者依舊畫出串級PID的原理框圖，如圖所示：

Crazepony在5.1版本及以後，就是採用角度/角速度串級PID控制。詳見Control.c文件中的CtrlAttiAng(void)函數和CtrlAttiRate(void)函數。

如何整定串級PID時的經驗，來自CSDN網友Nemo之家博客四軸PID講解。原則是先整定內環PID，再整定外環P。

• 內環P：從小到大，拉動四軸越來越困難，越來越感覺到四軸在抵抗你的拉動；到比較大的數值時，四軸自己會高頻震動，肉眼可見，此時拉扯它，它會快速的振盪幾下，過幾秒鐘後穩定；繼續增大，不用加人爲干擾，自己發散翻機。特別注意：只有內環P的時候，四軸會緩慢的往一個方向下掉，這屬於正常現象。這就是系統角速度靜差。
• 內環I：前述PID原理可以看出，積分只是用來消除靜差，因此積分項係數個人覺得沒必要弄的很大，因爲這樣做會降低系統穩定性。從小到大，四軸會定在一個位置不動，不再往下掉；繼續增加I的值，四軸會不穩定，拉扯一下會自己發散。特別注意：增加I的值，四軸的定角度能力很強，拉動他比較困難，似乎像是在釘釘子一樣，但是一旦有強幹擾，它就會發散。這是由於積分項太大，拉動一下積分速度快，給 的補償非常大，因此很難拉動，給人一種很穩定的錯覺。
• 內環D：這裏的微分項D爲標準的PID原理下的微分項，即本次誤差-上次誤差。在角速度環中的微分就是角加速度，原本四軸的震動就比較強烈，引起陀螺的值變化較大，此時做微分就更容易引入噪聲。因此一般在這裏可以適當做一些滑動濾波或者IIR濾波。從小到大，飛機的性能沒有多大改變，只是回中的時候更加平穩；繼續增加D的值，可以肉眼看到四軸在平衡位置高頻震動(或者聽到電機發出滋滋的聲音)。前述已經說明D項屬於輔助性項，因此如果機架的震動較大，D項可以忽略不加。
• 外環P：當內環PID全部整定完成後，飛機已經可以穩定在某一位置而不動了。此時內環P，從小到大，可以明顯看到飛機從傾斜位置慢慢回中，用手拉扯它然後放手，它會慢速回中，達到平衡位置；繼續增大P的值，用遙控器給不同的角度給定，可以看到飛機跟蹤的速度和響應越來越快；繼續增加P的值，飛機變得十分敏感，機動性能越來越強，有發散的趨勢。

電機的輸出整合

油門控制Throttle是電機輸出的基準值，增加油門即可四軸升高，減小油門即下降。最後整合上面通過PID算得的ROLL/PITCH/YAW三軸輸出量進行電機控制。整合如下，在Control.c文件的函數CtrlMotor()中實現。

Motor[2] = (int16_t)(Thr - Pitch - Roll - Yaw );    //M3
Motor[0] = (int16_t)(Thr + Pitch + Roll - Yaw );    //M1
Motor[3] = (int16_t)(Thr - Pitch + Roll + Yaw );    //M4
Motor[1] = (int16_t)(Thr + Pitch - Roll + Yaw );    //M2

Crazepony左下角爲M1機臂，在代碼中對應電機爲Motor[0]，然後逆時針依次爲M2，M3，M4，如圖所示。如果四軸繞ROLL軸向右傾斜5度，那麼電機1電機2應該提高升力，電機3電機0減小升力恢復平衡狀態，所以有以下規則：

• Roll方向旋轉，爲了恢復平衡，則電機1電機2同側出力，電機0電機3反向出力（Motor[1]和Motor[2]中的Roll爲-，Motor[0]和Motor[3]中的Roll爲+）
• Pitch方向旋轉，爲了恢復平衡，則電機2電機3同側出力，電機0電機1反向出力（Motor[2]和Motor[3]中的Pitch爲-，Motor[0]和Motor[1]中的Pitch爲+）

• Yaw方向旋轉，爲了恢復平衡，則電機1電機3同側出力，電機0電機2反向出力，（Motor[1]和Motor[3]中的Yaw爲+，Motor[0]和Motor[2]中的Yaw爲-）
• 轉載http://www.crazepony.com/book/wiki/algorithm-pid.html
•