四旋翼飛行器姿態(tài)檢測(cè)系統(tǒng)的研究

四旋翼飛行器姿態(tài)檢測(cè)系統(tǒng)的研究

在四個(gè)翼翼翼翼的飛行控制過(guò)程中，正確、實(shí)時(shí)地確定飛機(jī)的飛行角度是確定控制精度和系統(tǒng)穩(wěn)定性的關(guān)鍵。唯一的ss傳感器可以單獨(dú)測(cè)量姿態(tài)角度，但其精度主要取決于可靠性設(shè)備的精度。僅從提高硬件結(jié)構(gòu)設(shè)計(jì)和工藝方面難以顯著提高。隨著時(shí)間的推移，系統(tǒng)誤差的積累不足，因此很難確定長(zhǎng)期荷載的姿態(tài)。因此，使用單一傳感器難以獲得相對(duì)真實(shí)的姿態(tài)角度。在這項(xiàng)工作中，我們考慮到了姿態(tài)角測(cè)量的準(zhǔn)確性，并使用了多傳感器信號(hào)的集成處理方法來(lái)獲得最佳姿態(tài)角?；诙鄠鞲衅鞯男畔⑷诤鲜且粋€(gè)非常重要的研究?jī)?nèi)容,只有采用適宜的融合方法才能達(dá)到最好的效果.文獻(xiàn)采用的加權(quán)平均法是一種簡(jiǎn)單、直觀的融合方法,它將傳感器信息進(jìn)行加權(quán)平均的結(jié)果作為融合值,適用于動(dòng)態(tài)環(huán)境,但運(yùn)算精度不高.文獻(xiàn)采用的神經(jīng)網(wǎng)絡(luò)方法,具有良好的非線性和有效的自學(xué)習(xí)能力,但涉及到參數(shù)優(yōu)化和結(jié)構(gòu)模型的選擇問(wèn)題,其結(jié)構(gòu)過(guò)于復(fù)雜或簡(jiǎn)單都會(huì)對(duì)融合精度產(chǎn)生影響.本文針對(duì)由加速度計(jì)和陀螺儀建立的姿態(tài)角度測(cè)量系統(tǒng),建立其特征模型,采用卡爾曼濾波方法,對(duì)來(lái)自加速度計(jì)和陀螺儀的信號(hào)進(jìn)行融合,解決噪聲干擾與姿態(tài)最優(yōu)估計(jì)問(wèn)題,并將此方法應(yīng)用于四旋翼飛行器的角度測(cè)量系統(tǒng)進(jìn)行驗(yàn)證性試驗(yàn).1傾角傳感器的使用本文所采用的姿態(tài)角度測(cè)控系統(tǒng)主要由加速度計(jì)、陀螺儀、微控制器、濾波電路、電機(jī)調(diào)速器、無(wú)刷電機(jī)等部分組成.姿態(tài)檢測(cè)系統(tǒng)的硬件平臺(tái)如圖1,由微處理器對(duì)陀螺儀、濾波電路和加速度計(jì)構(gòu)成的傳感器組進(jìn)行高速A/D采樣后,通過(guò)卡爾曼濾波器對(duì)傳感器數(shù)據(jù)進(jìn)行補(bǔ)償和信息融合,得到準(zhǔn)確的姿態(tài)角度信號(hào),此角度信號(hào)再通過(guò)PID控制器運(yùn)算,輸出給電子調(diào)速器轉(zhuǎn)換成PWM信號(hào),進(jìn)而對(duì)電機(jī)進(jìn)行控制.加速度計(jì)用于測(cè)量物體的線性加速度,加速度計(jì)的輸出值與傾角呈非線性關(guān)系,隨著傾角的增加而表現(xiàn)為正弦函數(shù)變化.因此對(duì)加速度計(jì)的輸出進(jìn)行反正弦函數(shù)處理,才能得到其傾角值.測(cè)量數(shù)據(jù)噪聲與帶寬的平方根成正比,即噪聲會(huì)隨帶寬的增加而增加.VΝoise=350ug√Bw×1.5VNoise=350ugBw×1.5????????√式中:Bw為傳感器帶寬(單位為Hz).因此在設(shè)計(jì)卡爾曼濾波器時(shí),首先要確定被測(cè)加速度的頻率范圍,然后再設(shè)計(jì)濾波器的參數(shù),盡量使濾波器的帶寬略高于被測(cè)頻率,這樣不僅有助于濾除高頻干擾,而且也有利于降低系統(tǒng)噪聲干擾.但是如果要得到精確的傾角值,帶寬就需要設(shè)置得比較小,而這時(shí)加速度計(jì)動(dòng)態(tài)響應(yīng)慢,不適合跟蹤動(dòng)態(tài)角度運(yùn)動(dòng),如果期望快速的響應(yīng),又會(huì)引入較大的噪聲.再加上其測(cè)量范圍的限制,使得單獨(dú)應(yīng)用加速度計(jì)檢測(cè)飛行器傾角并不合適,需要與其他傳感器共同使用.陀螺儀的作用是用來(lái)測(cè)量角速度信號(hào),通過(guò)對(duì)角速度積分,便能得到角度值.陀螺儀本身極易受噪聲干擾,微機(jī)械陀螺不能承受較大的震動(dòng),同時(shí)由于溫度變化、不穩(wěn)定力矩等因素,陀螺儀會(huì)產(chǎn)生漂移誤差,并會(huì)隨著時(shí)間的推移而累加變大,通過(guò)積分會(huì)使得誤差變得很大.因此,也不能單獨(dú)使用陀螺儀作為本系統(tǒng)的傾角傳感器.2卡爾曼濾波器狀態(tài)更新前評(píng)估首先建立系統(tǒng)的狀態(tài)方程和測(cè)量方程.由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系,系統(tǒng)傾斜真實(shí)角度φ可以用來(lái)做一個(gè)狀態(tài)向量.在該系統(tǒng)中,采用加速度計(jì)估計(jì)出陀螺儀常值偏差b,以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測(cè)方程:{[˙φ˙b]=[00-10][φb]+ωgyro+[wg0]φacce=[φb]+wa?????????????????[φ˙b˙]=[00?10][φb]+ωgyro+[wg0]φacce=[φb]+wa式中,ωgyro為包含固定偏差的陀螺儀輸出角速度,φacce為加速度計(jì)經(jīng)處理后得到的角度值,wg為陀螺儀測(cè)量噪聲,wa為加速度計(jì)測(cè)量噪聲,b為陀螺儀漂移誤差,wg與wa相互獨(dú)立,此處假設(shè)二者為滿(mǎn)足正態(tài)分布的白色噪聲,令Ts為系統(tǒng)采樣周期,得到離散系統(tǒng)的狀態(tài)方程和測(cè)量方程:{X(k)=[10-Τs1]X(k-1)+[Τs0]ωgyro(k-1)+[wg(k)Τs0]Vi(k)=X(k)+wa(k)???????????X(k)=[10?Ts1]X(k?1)+[Ts0]ωgyro(k?1)+[wg(k)Ts0]Vi(k)=X(k)+wa(k)同時(shí),要估算k時(shí)刻的實(shí)際角度,就必須根據(jù)k-1時(shí)刻的角度值,再根據(jù)預(yù)測(cè)得到的k時(shí)刻的角度值得到k時(shí)刻的高斯噪聲的方差,在此基礎(chǔ)之上卡爾曼濾波器進(jìn)行遞歸運(yùn)算直至估算出最優(yōu)的角度值.在此,須知道系統(tǒng)過(guò)程噪聲協(xié)方差陣Q以及測(cè)量誤差的協(xié)方差矩陣R,對(duì)卡爾曼濾波器進(jìn)行校正.Q與R矩陣的形式如下:Q=[q_acce00q_gyro]R=[r_acce]式中,q_acce和q_gyro分別是加速度計(jì)和陀螺儀測(cè)量的協(xié)方差,其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度,值越小,表明信任程度越高.在該系統(tǒng)中陀螺儀的值更為接近準(zhǔn)確值,因此取q_gyro的值小于q_acce的值.當(dāng)前狀態(tài):X(k|k-1)=AX(k-1|k-1)+BU(k)(1)式中,是利用k預(yù)測(cè)的結(jié)果,X(k-1|k-1)是k-1時(shí)刻的最優(yōu)結(jié)果.則有對(duì)應(yīng)于X(k|k-1)的協(xié)方差為:Ρ(k|k-1)=AΡ(k-1|k-1)AΤ+Q(2)式中,P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的協(xié)方差,AT表示A的轉(zhuǎn)置矩陣,Q是系統(tǒng)過(guò)程的協(xié)方差.式子(1)、(2)即對(duì)系統(tǒng)的狀態(tài)更新.則狀態(tài)k的最優(yōu)化估算值X(k|k):X(k|k)=X(k|k-1)+Κ(k)(Ζ(k)-ΗX(k|k-1))(3)其中H=,K為卡爾曼增益(KalmanGain):Κ(k)=Ρ(k|k-1)ΗΤ/(ΗΡ(k|k-1)ΗΤ+R)(4)此時(shí),我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k).但是為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值,我們還要更新k狀態(tài)下X(k|k)的協(xié)方差:Ρ(k|k)=(Ι-Κg(k)Η)Ρ(k|k-1)(5)其中,I為單位陣,對(duì)于本系統(tǒng)則有,.當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P(k|k)就是式子(2)的P(k-1|k-1).(3)、(4)、(5)式為卡爾曼濾波器狀態(tài)更新方程.計(jì)算完時(shí)間更新方程和測(cè)量更新方程后,再次重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì),作為下一次計(jì)算的先驗(yàn)估計(jì),這樣,周而復(fù)始、循環(huán)反復(fù)地運(yùn)算下去直至找到最優(yōu)的結(jié)果.3卡爾曼濾波器的改進(jìn)為了驗(yàn)證卡爾曼濾波信號(hào)融合方法的有效性,構(gòu)建驗(yàn)證系統(tǒng)如下:如圖2為加入外界干擾,即人為改變測(cè)量系統(tǒng)的初始角度時(shí)加速度計(jì)與陀螺儀輸出波形,在外加干擾的情況下陀螺儀能夠測(cè)量出角速度值,但受噪聲影響所測(cè)量值不準(zhǔn)確且由于波動(dòng)較大,積分之后誤差信號(hào)將被放大使得角度值不準(zhǔn)確.圖3為引入卡爾曼濾波器后的波形曲線,加速度計(jì)成功地跟蹤出陀螺儀的偏差,濾波輸出曲線平滑濾波效果明顯,傾角估計(jì)的精度得到很大提高.4濾波對(duì)姿態(tài)角度測(cè)量的影響本文研究了基于卡爾曼濾波的加速度計(jì)與陀螺儀的信號(hào)融合方法,在動(dòng)態(tài)數(shù)據(jù)采集試驗(yàn)的基礎(chǔ)上,通過(guò)卡