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

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

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

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

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

5、融合十分關(guān)鍵，如果不能對陀螺儀漂移做修正，則系統(tǒng)運(yùn)行一段時間后，速率環(huán)的穩(wěn)定性會降低。下面比較一下德國四軸中姿態(tài)檢測部分和卡爾曼濾波之間的關(guān)系：卡爾曼濾波是一種線性系統(tǒng)的最優(yōu)估計(jì)濾波方法。對于本系統(tǒng)而言，使用卡爾曼濾波的作用是通過對系統(tǒng)狀態(tài)量的估計(jì)，和通過加速度計(jì)測量值對系統(tǒng)狀態(tài)進(jìn)行驗(yàn)證，從而得到該系統(tǒng)的最優(yōu)狀態(tài)量，并實(shí)時更新系統(tǒng)的各參數(shù)（矩陣），而最重要的一點(diǎn)，改濾波器能夠?qū)ν勇輧x的常值漂移進(jìn)行估計(jì)，從而保證速率if(MotenEin

6、)returnaufkeinenFallimFlug!beeptime=100Delay_ms(250)函數(shù):SetNeutral設(shè)定傳感器發(fā)出參數(shù)的中立數(shù)值，因?yàn)橛衅扑砸蛊涿看喂ぷ鞫家獪y量出來。############################################################################Nullwerteermitteln設(shè)置中立點(diǎn)voidSetNeutral(void)

7、############################################################################加速度計(jì)中立點(diǎn)NeutralAccX=0NeutralAccY=0NeutralAccZ=0陀螺儀中立點(diǎn)AdNeutralNick=0AdNeutralRoll=0AdNeutralGier=0Parameter_AchsKopplung1=0Parameter_AchsGegenKop

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