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

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

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

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

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

5、融合十分關(guān)鍵，如果不能對(duì)陀螺儀漂移做修正，則系統(tǒng)運(yùn)行一段時(shí)間后，速率環(huán)的穩(wěn)定性會(huì)降低。下面比較一下德國(guó)四軸中姿態(tài)檢測(cè)部分和卡爾曼濾波之間的關(guān)系：卡爾曼濾波是一種線性系統(tǒng)的最優(yōu)估計(jì)濾波方法。對(duì)于本系統(tǒng)而言，使用卡爾曼濾波的作用是通過(guò)對(duì)系統(tǒng)狀態(tài)量的估計(jì)，和通過(guò)加速度計(jì)測(cè)量值對(duì)系統(tǒng)狀態(tài)進(jìn)行驗(yàn)證，從而得到該系統(tǒng)的最優(yōu)狀態(tài)量，并實(shí)時(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(cè)量出來(lái)。############################################################################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。。。這個(gè)地方我還沒(méi)有弄得太明白，檢測(cè)中立點(diǎn)的函數(shù)被調(diào)用了兩次，但是第一次的數(shù)據(jù)好像沒(méi)有保存，只用到了第二次的數(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è)定氣壓初始的偏差