德國開源代碼的四軸飛行

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

德國人代碼中的神態(tài)檢測算法：

首先，將陀螺儀和加速度及的測量值減常值誤差，得到角速度和加速度，并對(duì)角速度進(jìn)行積分，然后對(duì)陀螺儀積分和加速度計(jì)的數(shù)值進(jìn)行融合。融合分為兩部分，實(shí)時(shí)融合和長期融合,實(shí)時(shí)融合每一次算法周期都要執(zhí)行，而長期融合沒256個(gè)檢測周期執(zhí)行一次，（注意檢測周期小于控制周期的2ms）實(shí)時(shí)融合：

1.將陀螺儀積分和加表濾波后的值做差；2.依照狀況對(duì)差值進(jìn)行衰減，并作限幅處理；3.將衰減值參與到角度中。長期融合：

長期融合主要包括兩個(gè)部分，一是對(duì)角速度的漂移進(jìn)行估計(jì)（估計(jì)值是要在每一個(gè)控制周期都耦合到角度中的），二是對(duì)陀螺儀的常值誤差（也就是陀螺儀中立點(diǎn)）進(jìn)行實(shí)時(shí)的修正。

1.將陀螺儀積分的積分和加速度積分做差（PS：為什么這里要使用加表積分和陀螺儀積分的積分，由于在256個(gè)檢測周期內(nèi)，有一些加速度計(jì)的值含有有害的加速度分量，假使只使用一個(gè)時(shí)刻的加表值對(duì)陀螺儀漂移進(jìn)行估計(jì)，顯然估計(jì)值不會(huì)確鑿，使用多個(gè)周期的值進(jìn)行疊加后做座平均處理，可以減小隨機(jī)的有害加速度對(duì)估計(jì)陀螺儀漂移過程中所鎖產(chǎn)生的影響）2.將上面兩個(gè)值進(jìn)行衰減，得到估計(jì)的陀螺儀漂移

3.對(duì)使考慮了陀螺儀漂移和不考慮陀螺儀漂移得到的角度做差，假使這兩個(gè)值較大，說明陀螺儀在前段時(shí)間內(nèi)測到的角速率不夠確鑿，需要對(duì)差值誤差（也就是陀螺儀中立點(diǎn)）進(jìn)行修正，修正幅度和差值有關(guān)長期融合十分關(guān)鍵，假使不能對(duì)陀螺儀漂移做修正，則系統(tǒng)運(yùn)行一段時(shí)間后，速率環(huán)的穩(wěn)定性會(huì)降低。

下面比較一下德國四軸中神態(tài)檢測部分和卡爾曼濾波之間的關(guān)系：

卡爾曼濾波是一種線性系統(tǒng)的最優(yōu)估計(jì)濾波方法。對(duì)于本系統(tǒng)而言，使用卡爾曼濾波的作用是通過對(duì)系統(tǒng)狀態(tài)量的估計(jì)，和通過加速度計(jì)測量值對(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ì)，從而保證速率環(huán)的正常運(yùn)行，并在加速度計(jì)敏感到各種有害加速度的時(shí)候，使神態(tài)檢測更加確鑿。但是卡爾曼濾波器能否工作在最優(yōu)狀態(tài)很大程度上取決于系統(tǒng)模型的確鑿性，模型參數(shù)的標(biāo)定和系統(tǒng)參數(shù)的選取。然而，僅僅通過上位機(jī)觀測而得到最優(yōu)工作參數(shù)是不顯示的，由于參數(shù)的修改會(huì)導(dǎo)致整個(gè)系統(tǒng)中好多地方發(fā)生改變，很難保證幾個(gè)值都恰好為最優(yōu)解，這需要扎實(shí)的理論知識(shí)，大量的測量數(shù)據(jù)和系統(tǒng)的仿真，通過我對(duì)卡爾曼濾波器的使用，發(fā)現(xiàn)要想兼顧鎖有的問題，還是有一定難度的。

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

于卡爾曼濾波器設(shè)計(jì)的難度，使用這種方法還是比較現(xiàn)實(shí)的。

控制算法：

德國人的控制算法的核心是對(duì)角速度做PI計(jì)算，P的作用是使四軸能夠產(chǎn)生對(duì)于外界干擾的抗?fàn)幜?，I的作用是讓四軸產(chǎn)生一個(gè)與角度成正比的抗?fàn)幜Α?/p>

假使只有P的作用，將四軸拿在手上就會(huì)發(fā)現(xiàn)，四軸能夠抗?fàn)幫饨绲母蓴_力矩的作用，而且這個(gè)抗?fàn)幜κ挚焖伲灰滞龍D改變四軸的轉(zhuǎn)速，四軸就會(huì)產(chǎn)生一個(gè)抗?fàn)幜?，但是，假使用手將四軸扳過一個(gè)角度，則四軸無法自己回到水平的角度位置，這就需要I的調(diào)理作用。

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

在對(duì)角速度進(jìn)行了PI調(diào)理之后，德國人將操縱桿的值融合到結(jié)果中去，并對(duì)得到的新的值有進(jìn)行了一次PI計(jì)算，這個(gè)積分參數(shù)很小，使用這個(gè)積分的作用由于，四軸在有一個(gè)十分小的傾角的狀況下產(chǎn)生的抗?fàn)幜睾苄。瑹o法使四軸回到水平位置，這就會(huì)導(dǎo)致無論怎么手動(dòng)調(diào)理微調(diào)，四軸都很難做到懸停，會(huì)不停得做水平漂移運(yùn)動(dòng)，這就必需不停的進(jìn)行調(diào)整。

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

voidPiep(unsignedcharAnzahl){

while(Anzahl--){

if(MotorenEin)return;//aufkeinenFallimFlug!beeptime=100;Delay_ms(250);}}

//函數(shù):SetNeutral設(shè)定傳感器發(fā)出參數(shù)的中立數(shù)值，由于有漂移所以要使其每次工作都要測量出來。//############################################################################

//Nullwerteermitteln/*設(shè)置中立點(diǎn)*/voidSetNeutral(void)

//############################################################################{

/*加速度計(jì)中立點(diǎn)*/NeutralAccX=0;

NeutralAccY=0;NeutralAccZ=0;/*陀螺儀中立點(diǎn)*/AdNeutralNick=0;AdNeutralRoll=0;AdNeutralGier=0;

Parameter_AchsKopplung1=0;

Parameter_AchsGegenKopplung1=0;。。。

/*這個(gè)地方我還沒有弄得太明白，檢測中立點(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è)定氣壓初始的偏差。}

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

/*這兩個(gè)參數(shù)在飛控程序中沒有用到*/StartNeutralRoll=AdNeutralRoll;StartNeutralNick=AdNeutralNick;

if(eeprom_read_byte(NeutralAccX=abs(Mittelwert_AccNick)/ACC_AMPLIFY;NeutralAccZ=Aktuell_az;}else{

/*假使發(fā)現(xiàn)變化不大，則依舊儲(chǔ)存上一次的*/

NeutralAccX=(int)eeprom_read_byte(

NeutralAccY=(int)eeprom_read_byte(

NeutralAccZ=(int)eeprom_read_byte(}

/*將所有數(shù)據(jù)清零，這里帶2的變量都是參與了陀螺儀漂移補(bǔ)償值之后得到的角度*/Mess_IntegralNick=0;Mess_IntegralNick2=0;Mess_IntegralRoll=0;Mess_IntegralRoll2=0;Mess_Integral_Gier=0;MesswertNick=0;MesswertRoll=0;MesswertGier=0;

StartLuftdruck=Luftdruck;HoeheD=0;

Mess_Integral_Hoch=0;

KompassStartwert=KompassValue;GPS_Neutral();beeptime=50;//

/*從EEPROM中讀取陀螺儀積分到達(dá)90°時(shí)候的值，并儲(chǔ)存，當(dāng)?shù)玫降纳駪B(tài)角度大于這個(gè)范圍時(shí)，說明超過了90°

，就要進(jìn)行相應(yīng)的處理*/

Umschlag180Nick=(long)EE_Parameter.WinkelUmschlagNick*2500L;Umschlag180Roll=(long)EE_Parameter.WinkelUmschlagRoll*2500L;ExternHoehenValue=0;}

///////////////////////////////

//函數(shù)描述：求參數(shù)的平均數(shù)值//////////////////////////////

//############################################################################

//BearbeitetdieMesswertevoidMittelwert(void)

//根據(jù)測量值計(jì)算陀螺儀和加速度計(jì)數(shù)據(jù)

//############################################################################{

staticsignedlongtmpl,tmpl2;

/*將陀螺儀數(shù)據(jù)減去常值誤差，得到實(shí)際的叫速率的倍數(shù)*/MesswertGier=(signedint)AdNeutralGier-AdWertGier;MesswertRoll=(signedint)AdWertRoll-AdNeutralRoll;MesswertNick=(signedint)AdWertNick-AdNeutralNick;

//DebugOut.Analog[26]=MesswertNick;DebugOut.Analog[28]=MesswertRoll;//加速度傳感器輸出

/*加速度計(jì)數(shù)據(jù)濾波，ACC_AMPLIFY=12得到的Mittelwert_AccNick是加速度計(jì)數(shù)值的12倍*//*AdWertAccNick為測量值*/

//Beschleunigungssensor++++++++++++++++++++++++++++++++++++++++++++++++

Mittelwert_AccNick=((long)Mittelwert_AccNick*1+((ACC_AMPLIFY*(long)AdWertAccNick)))/

2L;//具有濾波功能的方法，用當(dāng)前加速度和上次的加速度平均

Mittelwert_AccRoll=((long)Mittelwert_AccRoll*1+((