自行研制的四軸飛行器算法交流

1、自行研制的四軸飛行器自行研制的四軸飛行器算法交流算法交流從開始做四軸到現(xiàn)在，已經(jīng)累計使用了三個月的時間，從開始的嘗試用四元數(shù)法進行姿態(tài)檢測，到接著使用的卡爾曼濾波算法，我們走過了很多彎路，我在從上周開始了對德國人四軸代碼的研究和移植，發(fā)現(xiàn)德國人的代碼的確有他的獨到之處，改變了很多我對模型的想法，因為本人是第一次嘗試著制作模型，因此感覺很多想法還是比較簡單。經(jīng)過了一周的時間，我將德國人的代碼翻譯并移植到了我目前的四軸上，并進行了調(diào)試，今天

2、，專門請到了一個飛直升機的教練，對我們的四軸進行試飛，并與一個華科爾的四軸進行了現(xiàn)場比較，現(xiàn)在我們四軸的穩(wěn)定性已經(jīng)達到了商品四軸的程度。下面是我這一周時間內(nèi)對德國人代碼的一些理解：德國人代碼中的姿態(tài)檢測算法：首先，將陀螺儀和加速度及的測量值減常值誤差，得到角速度和加速度，并對角速度進行積分，然后對陀螺儀積分和加速度計的數(shù)值進行融合。融合分為兩部分，實時融合和長期融合實時融合每一次算法周期都要執(zhí)行，而長期融合沒256個檢測周期執(zhí)行一次，（

3、注意檢測周期小于控制周期的2ms）實時融合：1.將陀螺儀積分和加表濾波后的值做差；2.按照情況對差值進行衰減，并作限幅處理；3.將衰減值加入到角度中。長期融合：長期融合主要包括兩個部分，一是對角速度的漂移進行估計（估計值是要在每一個控制周期都耦合到角度中的），二是對陀螺儀的常值誤差（也就是陀螺儀中立點）進行實時的修正。1.將陀螺儀積分的積分和加速度積分做差（PS：為什么這里要使用加表積分和陀螺儀積分的積分，因為在256個檢測周期內(nèi)，有一

4、些加速度計的值含有有害的加速度分量，如果只使用一個時刻的加表值對陀螺儀漂移進行估計，顯然估計值不會準確，使用多個周期的值進行疊加后做座平均處理，可以減小隨機的有害加速度對估計陀螺儀漂移過程中所鎖產(chǎn)生的影響）2.將上面兩個值進行衰減，得到估計的陀螺儀漂移3.對使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，如果這兩個值較大，說明陀螺儀在前段時間內(nèi)測到的角速率不夠準確，需要對差值誤差（也就是陀螺儀中立點）進行修正，修正幅度和差值有關(guān)長期

5、融合十分關(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的常值漂移進行估計，從而保證速率if(MotenEin

6、)returnaufkeinenFallimFlug!beeptime=100Delay_ms(250)函數(shù):SetNeutral設(shè)定傳感器發(fā)出參數(shù)的中立數(shù)值，因為有漂移所以要使其每次工作都要測量出來。############################################################################Nullwerteermitteln設(shè)置中立點voidSetNeutral(void)

7、############################################################################加速度計中立點NeutralAccX=0NeutralAccY=0NeutralAccZ=0陀螺儀中立點AdNeutralNick=0AdNeutralRoll=0AdNeutralGier=0Parameter_AchsKopplung1=0Parameter_AchsGegenKop

8、plung1=0。。。這個地方我還沒有弄得太明白，檢測中立點的函數(shù)被調(diào)用了兩次，但是第一次的數(shù)據(jù)好像沒有保存，只用到了第二次的數(shù)據(jù)記錄中立點CalibrierMittelwert()Delay_ms_Mess(100)記錄中立點CalibrierMittelwert()既然只使用了后一次的數(shù)據(jù)，為什么要進行兩次記錄中立點的函數(shù)if((EE_Parameter.GlobalConfig如果氣壓表輸出在950外750內(nèi)，則設(shè)定氣壓初始的偏差