http://v.youku.com/v_show/id_XMTE4NzMyNjE2.html
http://v.youku.com/v_show/id_XMTE4NzMyNDc2.html
這是我在優(yōu)酷上傳的兩段視頻。

從開始做四軸到現(xiàn)在，已經(jīng)累計使用了三個月的時間，從開始的嘗試用四元數(shù)法進行姿態(tài)檢測，到接著使用的卡爾曼濾波算法，我們走過了很多彎路，我在從上周開始了對德國人四軸代碼的研究和移植，發(fā)現(xiàn)德國人的代碼的確有他的獨到之處，改變了很多我對模型的想法，因為本人是第一次嘗試著制作模型，因此感覺很多想法還是比較簡單。經(jīng)過了一周的時間，我將德國人的代碼翻譯并移植到了我目前的四軸上，并進行了調(diào)試，今天，專門請到了一個飛直升機的教練，對我們的四軸進行試飛，并與一個華科爾的四軸進行了現(xiàn)場比較，現(xiàn)在我們四軸的穩(wěn)定性已經(jīng)達到了商品四軸的程度。下面是我這一周時間內(nèi)對德國人代碼的一些理解：

德國人代碼中的姿態(tài)檢測算法：
首先，將陀螺儀和加速度及的測量值減常值誤差，得到角速度和加速度，并對角速度進行積分，然后對陀螺儀積分和加速度計的數(shù)值進行融合。融合分為兩部分，實時融合和長期融合,實時融合每一次算法周期都要執(zhí)行，而長期融合沒256個檢測周期執(zhí)行一次，（注意檢測周期小于控制周期的2ms）
實時融合：
1.將陀螺儀積分和加表濾波后的值做差；
2.按照情況對差值進行衰減，并作限幅處理；
3.將衰減值加入到角度中。
長期融合：
長期融合主要包括兩個部分，一是對角速度的漂移進行估計（估計值是要在每一個控制周期都耦合到角度中的），二是對陀螺儀的常值誤差（也就是陀螺儀中立點）進行實時的修正。
1.將陀螺儀積分的積分和加速度積分做差（PS：為什么這里要使用加表積分和陀螺儀積分的積分，因為在256個檢測周期內(nèi)，有一些加速度計的值含有有害的加速度分量，如果只使用一個時刻的加表值對陀螺儀漂移進行估計，顯然估計值不會準確，使用多個周期的值進行疊加后做座平均處理，可以減小隨機的有害加速度對估計陀螺儀漂移過程中所鎖產(chǎn)生的影響）
2.將上面兩個值進行衰減，得到估計的陀螺儀漂移
3.對使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，如果這兩個值較大，說明陀螺儀在前段時間內(nèi)測到的角速率不夠準確，需要對差值誤差（也就是陀螺儀中立點）進行修正，修正幅度和差值有關(guān)
長期融合十分關(guān)鍵，如果不能對陀螺儀漂移做修正，則系統(tǒng)運行一段時間后，速率環(huán)的穩(wěn)定性會降低。

下面比較一下德國四軸中姿態(tài)檢測部分和卡爾曼濾波之間的關(guān)系：
卡爾曼濾波是一種線性系統(tǒng)的最優(yōu)估計濾波方法。對于本系統(tǒng)而言，使用卡爾曼濾波的作用是通過對系統(tǒng)狀態(tài)量的估計，和通過加速度計測量值對系統(tǒng)狀態(tài)進行驗證，從而得到該系統(tǒng)的最優(yōu)狀態(tài)量，并實時更新系統(tǒng)的各參數(shù)（矩陣），而最重要的一點，改濾波器能夠?qū)ν勇輧x的常值漂移進行估計，從而保證速率環(huán)的正常運行，并在加速度計敏感到各種有害加速度的時候，使姿態(tài)檢測更加準確。但是卡爾曼濾波器能否工作在最優(yōu)狀態(tài)很大程度上取決于系統(tǒng)模型的準確性，模型參數(shù)的標定和系統(tǒng)參數(shù)的選取。然而，僅僅通過上位機觀測而得到最優(yōu)工作參數(shù)是不顯示的，因為參數(shù)的修改會導致整個系統(tǒng)中很多地方發(fā)生改變，很難保證幾個值都恰好為最優(yōu)解，這需要扎實的理論知識，大量的測量數(shù)據(jù)和系統(tǒng)的仿真，通過我對卡爾曼濾波器的使用，發(fā)現(xiàn)要想兼顧鎖有的問題，還是有一定難度的。

而德國人的姿態(tài)檢測部分是在嘗試使用一種簡單方法去解決復雜問題，他既沒有使用傳統(tǒng)的四元數(shù)法進行姿態(tài)檢測，也么有使用卡爾曼濾波。他的計算量不比最簡單的卡爾曼濾程序波程序的計算量小，但與卡爾曼濾波相比，更加直觀，易于理解，參數(shù)調(diào)節(jié)也更加方便。我個人理解，這個方法是在嘗試著對卡爾曼濾波這一復雜相互耦合的多狀態(tài)變量的線性系統(tǒng)狀態(tài)估計過程進行了簡單的解耦，從而將姿態(tài)的最優(yōu)估計和陀螺儀漂移的最優(yōu)估計分隔開，這樣，就可以通過比較直觀的觀測手段對兩個部分的參數(shù)進行調(diào)整，但是，這種方法的理論性肯定不如使用四元數(shù)法和卡爾曼濾波，在一些特殊的情況下還可能出現(xiàn)問題，但是，由于卡爾曼濾波器設(shè)計的難度，使用這種方法還是比較現(xiàn)實的。

控制算法：
德國人的控制算法的核心是對角速度做PI計算，P的作用是使四軸能夠產(chǎn)生對于外界干擾的抵抗力矩，I的作用是讓四軸產(chǎn)生一個與角度成正比的抵抗力。
如果只有P的作用，將四軸拿在手上就會發(fā)現(xiàn)，四軸能夠抵抗外界的干擾力矩的作用，而且這個抵抗力非常快速，只要手妄圖改變四軸的轉(zhuǎn)速，四軸就會產(chǎn)生一個抵抗力矩，但是，如果用手將四軸扳過一個角度，則四軸無法自己回到水平的角度位置，這就需要I的調(diào)節(jié)作用。

對角速度做I（積分）預算實際得到的就是角度，德國人四軸里面用的也是角度值，如果四軸有一個傾斜角度，那么四軸就會自己進行調(diào)整，直到四軸的傾角為零，它所產(chǎn)生的抵抗力是與角度成正比的，但是，如果只有I的作用，會使四軸迅速產(chǎn)生振蕩，因此，必須將P和I結(jié)合起來一起使用，這時候基本上就會得到德國四軸的效果了。

在對角速度進行了PI調(diào)節(jié)之后，德國人將操縱桿的值融合到結(jié)果中去，并對得到的新的值有進行了一次PI計算，這個積分參數(shù)很小，使用這個積分的作用因為，四軸在有一個非常小的傾角的情況下產(chǎn)生的抵抗力矩很小，無法使四軸回到水平位置，這就會導致無論怎么手動調(diào)節(jié)微調(diào)，四軸都很難做到懸停，會不停得做水平漂移運動，這就必須不停的進行調(diào)整。

下面是我給德國四軸中飛控程序的一些注釋：

void Piep(unsigned char Anzahl)  
{  
while(Anzahl--)  
{  
  if(MotorenEin) return; //auf keinen Fall im Flug!  
  beeptime = 100;  
  Delay_ms(250);  
}  
}  



//函數(shù):SetNeutral設(shè)定傳感器發(fā)出參數(shù)的中立數(shù)值，因為有漂移所以要使其每次工作都要測量出來。  
//############################################################################  
//  Nullwerte ermitteln
/*設(shè)置中立點*/  
void SetNeutral(void)  
//############################################################################  
{  
/*加速度計中立點*/
NeutralAccX = 0;  
NeutralAccY = 0;
NeutralAccZ = 0;
/*陀螺儀中立點*/
AdNeutralNick = 0;
AdNeutralRoll = 0;
AdNeutralGier = 0;
    Parameter_AchsKopplung1 = 0;
    Parameter_AchsGegenKopplung1 = 0;。。。  

/*這個地方我還沒有弄得太明白，檢測中立點的函數(shù)被調(diào)用了兩次，但是第一次的數(shù)據(jù)好像沒有保存，只用到了

第二次的數(shù)據(jù)*/
/*記錄中立點*/
CalibrierMittelwert();
    Delay_ms_Mess(100);
/*記錄中立點*/
CalibrierMittelwert();
/*既然只使用了后一次的數(shù)據(jù)，為什么要進行兩次記錄中立點的函數(shù)*/
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  
     {
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();//如果氣壓表輸出在

950外750內(nèi)，則設(shè)定氣壓初始的偏差。  
     }

/*將量測值作為陀螺儀的中立點*/  
AdNeutralNick= AdWertNick;
AdNeutralRoll= AdWertRoll;
AdNeutralGier= AdWertGier;