四翼飛行器姿態(tài)控制算法研究--葉樹球(共15頁(yè))

1、飛行器位置(wi zhi)姿態(tài)圖。 四翼飛行器主要硬件(yn jin)及電路組成一個(gè)(y )基本的飛行器機(jī)體設(shè)計(jì)方案中,包括至少以下部分:主控制器、姿態(tài)傳感器、電源電機(jī)以及通信接口,其系統(tǒng)結(jié)構(gòu)圖如下所示:由于四翼飛行器的傳感器采樣數(shù)據(jù)包含大量的噪聲,使得控制系統(tǒng)對(duì)飛行器的姿態(tài)(zti)解算會(huì)出現(xiàn)很大的誤差,為減小誤差對(duì)控制系統(tǒng)的影響,需要對(duì)加速度、角加速度、地磁傳感器的測(cè)量數(shù)據(jù)進(jìn)行融合濾波處理才能得到更加有效的飛行器姿態(tài)信息。本章首先介紹四翼飛行器姿態(tài)解算中常用的數(shù)據(jù)濾波算法包括,卡爾曼濾波和互補(bǔ)濾波,接著(ji zhe)研究了利用四元數(shù)來(lái)對(duì)飛行器進(jìn)行姿態(tài)解算的過(guò)程。1.卡爾曼濾波卡爾曼濾波是

2、一種遞歸(自回歸)的濾波器,該濾波器的顯著(xinzh)優(yōu)點(diǎn)是高效率。卡爾曼濾波的思想是釆用遞歸自更新的方法來(lái)估計(jì)出當(dāng)前的狀態(tài),同時(shí)保證目標(biāo)的真實(shí)狀態(tài)與濾波估計(jì)出的狀態(tài)的均方誤差收斂值最小?？柭鼮V波器的過(guò)程由兩個(gè)階段組成,共由五個(gè)公式表示,它們分別是預(yù)測(cè)和更新,前階段含兩個(gè)公式,后階段包含三個(gè)公式。在前一階段:濾波器需要對(duì)對(duì)象的當(dāng)前狀態(tài)作出評(píng)估,評(píng)估過(guò)程依照上一次對(duì)對(duì)象狀態(tài)的估計(jì);在后一階段:濾波器需要對(duì)對(duì)象作出更加精確的評(píng)估,評(píng)估過(guò)程是通過(guò)優(yōu)化當(dāng)前狀態(tài)的觀測(cè)值來(lái)實(shí)現(xiàn)的。如下式(3.1)和式(3.2)是預(yù)測(cè)階段的公式公式(gngsh)中:Rk:對(duì)象(duxing)測(cè)量噪聲的協(xié)方差。Kk:最優(yōu)

3、卡爾曼增益(zngy)。I:單位矩陣。其中Zk是滿足式(3.6)的對(duì)對(duì)象的一個(gè)測(cè)量值。其中:Hk是對(duì)象的觀測(cè)矩陣,Vk是均值為0的觀測(cè)噪聲矩陣。公式(3.3)的作用是更新當(dāng)前的最優(yōu)卡爾曼增益;公式(3.4)的作用是根據(jù)當(dāng)前卡爾曼增益對(duì)系統(tǒng)當(dāng)前狀態(tài)進(jìn)行估計(jì),也就是卡爾曼濾波的估計(jì)輸出;公式(3.5)的作用是對(duì)濾波器當(dāng)前誤差的協(xié)方差進(jìn)行更新。這三個(gè)公式得出了當(dāng)前的系統(tǒng)狀態(tài),并更新濾波器自身的狀態(tài),以便下一個(gè)卡爾曼濾波周期的正常進(jìn)行。2. 互補(bǔ)濾波簡(jiǎn)單來(lái)說(shuō)互補(bǔ)濾波就是把濾波器的兩部分加起來(lái)成為一個(gè)濾波器,使其輸出一個(gè)準(zhǔn)確的線性估計(jì)在四翼飛行器中,由于加速度傳感器的靈敏度高,其受外部的影響比較大,其測(cè)

4、量值也容易被混入噪聲,直接使用測(cè)量數(shù)據(jù)進(jìn)行姿態(tài)解算會(huì)使得控制系統(tǒng)的輸出誤差非常大。而對(duì)于陀螺儀來(lái)說(shuō),它反應(yīng)的是短時(shí)間內(nèi)角速度的積分,因此陀螺儀的瞬時(shí)測(cè)量是比較準(zhǔn)確的,但是這也帶來(lái)一個(gè)問(wèn)題,陀螺儀會(huì)產(chǎn)生累積誤差的漂移現(xiàn)象。也就是說(shuō),陀螺儀的短時(shí)間內(nèi)測(cè)量值是比較準(zhǔn)確的,而加速度計(jì)濾波測(cè)量值在長(zhǎng)時(shí)間范圍內(nèi)是比較準(zhǔn)確的。因此陀螺儀和加速計(jì)的互補(bǔ)濾波思想是使用加速度計(jì)的測(cè)量值作一個(gè)均值,利用這個(gè)值來(lái)修正陀螺儀的測(cè)量值并換算出角度,以達(dá)到降低陀螺儀角度累積誤差的目的?；パa(bǔ)濾波器的設(shè)計(jì)。系統(tǒng)首先對(duì)陀螺儀的測(cè)量值進(jìn)行積分,得到當(dāng)前狀態(tài)的角速度測(cè)量數(shù)據(jù);接著系統(tǒng)對(duì)加速度計(jì)測(cè)量值進(jìn)行低通濾波處理,其結(jié)果與角速度送

5、入高通濾波器處理后的輸出值進(jìn)行求和融合,就可以計(jì)算(j sun)出當(dāng)前的角度。這里使用高斯濾波來(lái)處理加速計(jì)的測(cè)量值。1.高斯(o s)濾波-低通濾波器高通濾波器是一種低通濾波器,它是對(duì)數(shù)據(jù)進(jìn)行加權(quán)平均的線性平滑過(guò)程,對(duì)符合正太分布(fnb)的噪聲有很好的過(guò)濾效果,它的表達(dá)形式如下式(3.7)所示一般情況(qngkung)下,高斯濾波器可以使用多次均值濾波器的逼近來(lái)代替,大于等于三次平均值濾波器逼近就可以得到很好的結(jié)果,如下式(3.8)是使一般情況下,高斯濾波器可以使用多次均值濾波器的逼近來(lái)代替,大于等于三次平均值濾波器逼近就可以得到很好的結(jié)果,如下式(3.8)是使用三次平均濾波的公式式中n表示

6、當(dāng)前第n個(gè)采樣點(diǎn),N表示釆樣窗口(chungku)寬度,令N=2k,那么由(3.8)可得到式(3.9): N的選取需要結(jié)合傳感器的更新速率和姿態(tài)檢測(cè)系統(tǒng)(xtng)的姿態(tài)更新頻率,如果N取值過(guò)小那么濾波器就無(wú)法有效濾除噪聲,若N選取過(guò)大則會(huì)導(dǎo)致數(shù)據(jù)延遲過(guò)大,使得姿態(tài)檢測(cè)系統(tǒng)的實(shí)時(shí)性無(wú)法保證。2.高通濾波器這一步首先要對(duì)陀螺儀的測(cè)量角速度經(jīng)過(guò)積分得出當(dāng)前的角度,公式(3.10) 給出了積分公式。式中W為當(dāng)前陀螺儀測(cè)量角速度矩陣,e t為當(dāng)前t時(shí)刻的角速度積分,其結(jié)果是飛行器的當(dāng)前角度(jiod)?；パa(bǔ)濾波公式如下(3.11)所示:0 t即為角速度計(jì)和陀螺儀融合的角度(jiod)值。式中G和A是時(shí)

7、間根據(jù)時(shí)間常數(shù)算出來(lái)的參數(shù),并且其和恒等于1,其值可以使用公式(2.12)計(jì)算出: 公式中T為時(shí)間常數(shù),dt為釆樣窗口頻率,一般情況(qngkung)下可以去時(shí)間常數(shù)T=0.5sec,取釆樣窗口 N=100,有dt=0.01sec,這時(shí)可以求得:G約等于0.98,A約等于0.2。PID控制原理PID控制方法是工業(yè)控制系統(tǒng)中歷史最長(zhǎng)的一種控制方法,它通過(guò)比較控制系統(tǒng)的測(cè)量量和目標(biāo)值,計(jì)算偏差,再對(duì)偏差進(jìn)行比例、積分、微分調(diào)節(jié),用以決定向被控系統(tǒng)發(fā)送那種控制信號(hào),它屬于一種典型的負(fù)反饋閉環(huán)控制方法21。如下圖4.2所示為典型PID控制系統(tǒng)原理框圖。如上圖所示,r (t)為被控對(duì)象t時(shí)刻設(shè)定的目標(biāo)(

8、mbio)狀態(tài),y (t)為控制對(duì)象t時(shí)刻的狀態(tài),通過(guò)反饋線與r (t)比較求出控制目標(biāo)的偏差e (t), e (t)同時(shí)輸入比例、積分、微分調(diào)節(jié)器,并據(jù)此計(jì)算控制量,產(chǎn)生(chnshng)控制信號(hào)送入控制對(duì)象,完成一個(gè)PID控制周期。其控制表達(dá)式如下式(4.1)所示:公式中e(t)即為控制對(duì)象(duxing)的目標(biāo)偏差,如圖(4.1)所示:即e(t)=r(t)-y(t),Kp為比例增益,Ti為積分時(shí)間常數(shù),Td為微分時(shí)間常數(shù);Kp對(duì)應(yīng)PID中的P,Ti對(duì)應(yīng)PID中的I,Td對(duì)應(yīng)PID中的D。1.比例項(xiàng)在pro控制方法中,Kpe(t)即為比例項(xiàng),它反應(yīng)的是控制系統(tǒng)對(duì)系統(tǒng)偏差的影響,比例系數(shù)Kp

9、的大小直接表明系統(tǒng)對(duì)偏差的反應(yīng)的靈敏度。Kp越大,系統(tǒng)對(duì)偏差的靈敏度越高,系統(tǒng)能很快對(duì)偏差作出反應(yīng),控制對(duì)象的靜差減小;Kp減小,系統(tǒng)對(duì)偏差的靈敏度也降低,系統(tǒng)對(duì)偏差反應(yīng)減弱,同時(shí)控制對(duì)象靜差變大。但是,Kp過(guò)大將會(huì)是系統(tǒng)產(chǎn)生震蕩,Kp過(guò)小會(huì)導(dǎo)致系統(tǒng)反應(yīng)太慢,因此,在實(shí)際應(yīng)用中要合理地選擇比例系數(shù)Kp,以達(dá)到控制速度,同時(shí)又不使系統(tǒng)產(chǎn)生震蕩,以達(dá)到穩(wěn)定的控制效果。2.積分(jfn)項(xiàng)在PID控制方法中,即為積分項(xiàng),它反應(yīng)(fnyng)的是控制系統(tǒng)對(duì)累積偏差的調(diào)節(jié)作用?？刂葡到y(tǒng)(kn zh x tn)通過(guò)累加最近一段過(guò)去時(shí)間控制對(duì)象的偏差,根據(jù)這個(gè)累積偏差作出反應(yīng)。由表達(dá)式可知,當(dāng)越大,即一段時(shí)

10、間內(nèi)系統(tǒng)靜差比較大,積分部分的作用就會(huì)增強(qiáng),該項(xiàng)就可以彌補(bǔ)比例項(xiàng)的不足,隨著時(shí)間的推移會(huì)越來(lái)越小,當(dāng)系統(tǒng)達(dá)到穩(wěn)定即:e(t)=0,le(t)dt = 0,積分項(xiàng)為零。同時(shí)從積分項(xiàng)的表達(dá)式中可以看出,積分常數(shù)由比例系數(shù)和積分系數(shù)組成,在比例系數(shù)一定的情況下,積分系數(shù)越大,靜差消除越慢,系統(tǒng)超調(diào)量減小,同時(shí)系統(tǒng)出現(xiàn)震蕩的可能性也越小;Ti越小,消除靜差的速度也越快,但是系統(tǒng)的超調(diào)量越大,系統(tǒng)容易出現(xiàn)震蕩。由上可知,積分項(xiàng)的作用在于消除系統(tǒng)的靜差,但是該項(xiàng)非常容易引起系統(tǒng)的震蕩,導(dǎo)致系統(tǒng)不穩(wěn)定。同樣,積分系數(shù)Ti也要精心選取,以做到控制系統(tǒng)既不引起超調(diào),也不導(dǎo)致系統(tǒng)震蕩。3.微分項(xiàng)在pro控制方法中

11、,即為微分項(xiàng),它反應(yīng)的控制系統(tǒng)對(duì)偏差變化的調(diào)節(jié)作用。由式可知,系統(tǒng)越不穩(wěn)定即兩次系統(tǒng)偏差比較大,也就越大,這at時(shí)系統(tǒng)就很有可能出現(xiàn)震蕩(zhndng),而微分項(xiàng)就可以來(lái)消除或在減輕系統(tǒng)的超調(diào)可能,避免系統(tǒng)(xtng)進(jìn)入震蕩狀態(tài);在控制過(guò)程中,若系統(tǒng)接近目標(biāo)值,也就越小,微at分項(xiàng)可以達(dá)到(d do)輔助調(diào)節(jié)的作用。綜上可知,微分項(xiàng)的作用就是減小系統(tǒng)的超調(diào)量。微分常數(shù)?7。由比例系數(shù)和微分系數(shù)7組成,在比例系數(shù)一定的情況下,微分系數(shù)越大,控制系統(tǒng)對(duì)系統(tǒng)偏差e(t)的變化擬制作用就越強(qiáng),微分系數(shù)越小,控制系統(tǒng)對(duì)系統(tǒng)偏e(t)差的變化擬制作用就越弱。但是微分項(xiàng)的缺點(diǎn)就是的作用會(huì)很明顯被噪聲干擾,

12、甚至傷害控制系統(tǒng)的穩(wěn)定性。因此,控制系統(tǒng)需要謹(jǐn)慎引入微分項(xiàng),或者嚴(yán)格選取微分項(xiàng)的調(diào)節(jié)時(shí)間常數(shù)Td,使微分部分能對(duì)系統(tǒng)發(fā)揮積極穩(wěn)定的作用。綜上可知:PID控制器中比例項(xiàng)用來(lái)調(diào)節(jié)系統(tǒng)的偏差,它能迅速地對(duì)系統(tǒng)的偏差作出反應(yīng),是PID控制器的最主要成分。但是它不能消除系統(tǒng)的靜態(tài)誤差,過(guò)大或者過(guò)小都會(huì)影響控制器的性能;而積分項(xiàng)的作用是可以消除系統(tǒng)的靜態(tài)誤差,同樣積分項(xiàng)過(guò)大或者過(guò)小將導(dǎo)致系統(tǒng)超調(diào)甚至系統(tǒng)的震蕩;微分項(xiàng)的作用就是減小系統(tǒng)的超調(diào)量,增加系統(tǒng)的穩(wěn)定性。通過(guò)合理地設(shè)定PID的比例、積分、微分系數(shù)可以明顯改善控制器的性能。在PID的參數(shù)整定方法中最常用的方法有:試湊法、頻率法和時(shí)域法45。PID控制系統(tǒng)(kn zh x tn)設(shè)計(jì)根據(jù)PID控制方法表達(dá)式結(jié)合飛行(fixng)器的飛行原理有公式(4.2): 上式中:口0,把(4.17)代入(4.16)可得到:根據(jù)上面的推導(dǎo),當(dāng)被控對(duì)象的輸入U(xiǎn)如(4.17)式,并結(jié)合Lyapunov穩(wěn)定性理論5,可以得到系統(tǒng)是全局漸近穩(wěn)定的。對(duì)于n階單輸入單輸出非線性系統(tǒng),可以參照前面的二階過(guò)程推導(dǎo)得出。文獻(xiàn)12給出了推導(dǎo)過(guò)程,這里直接