代 君,劉戰(zhàn)合,王 菁

(鄭州航空工業(yè)管理學(xué)院)

?

基于MATLAB四旋翼姿態(tài)航向仿真*

代 君,劉戰(zhàn)合,王 菁

(鄭州航空工業(yè)管理學(xué)院)

基于互補(bǔ)濾波下的信息融合算法，將實(shí)驗(yàn)獲得的MTI小型姿態(tài)航向參考系統(tǒng)中慣性器件的實(shí)際數(shù)據(jù)進(jìn)行仿真分析，在比較了陀螺儀積分、加速度計(jì)計(jì)算、互補(bǔ)濾波法得到的姿態(tài)角效果后，最終分析了互補(bǔ)濾波理論應(yīng)用于姿態(tài)航向解算中的優(yōu)化效果.

姿態(tài)航向參考系統(tǒng)；互補(bǔ)濾波；MTI

0 引言

近十幾年來(lái)，四旋翼飛行器逐漸走入人們的日常生活，四旋翼飛行器在電子技術(shù)飛速發(fā)展的促進(jìn)下日益趨向小型化.

四旋翼飛行器主要依靠四個(gè)旋翼產(chǎn)生升力，通過(guò)改變各個(gè)旋翼的升力大小，可以控制四旋翼的飛行方向與飛行姿態(tài).而對(duì)于四旋翼姿態(tài)航向參考系統(tǒng)來(lái)講，慣性傳感器是必不可少的.慣性傳感器包括陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì) ，通過(guò)這些安裝在四旋翼上的傳感器所測(cè)得的數(shù)據(jù)，計(jì)算機(jī)可以計(jì)算出四旋翼的姿態(tài)及航向.

四旋翼姿態(tài)航向系統(tǒng)，顧名思義，就是為飛行控制人員測(cè)量、提供并顯示姿態(tài)角(包括俯仰角和滾轉(zhuǎn)角)和航向角的系統(tǒng)，使飛行控制人員能夠根據(jù)這些數(shù)據(jù)及時(shí)調(diào)整操作，達(dá)到飛行目的 .對(duì)于四旋翼姿態(tài)航向的計(jì)算算法進(jìn)行研究，可以使四旋翼飛行器的姿態(tài)角、航向角、偏航角的誤差更小，提高系統(tǒng)定位精度.研究算法需從無(wú)人機(jī)的姿態(tài)信息融合系統(tǒng)著手，這種參考系統(tǒng)由微加速度計(jì)、磁航向計(jì)、微型陀螺儀組成.加速度計(jì)和磁航向計(jì)組合可以計(jì)算得到俯仰角、滾轉(zhuǎn)角、偏航角；陀螺儀通過(guò)測(cè)量三個(gè)方向的角速度，通過(guò)積分即可算出姿態(tài)角. 但是這兩種方法都有所缺陷，計(jì)算得到的姿態(tài)角誤差均比較大.因此要尋求一種更為有效的算法來(lái)解決這些問(wèn)題，即為該文所要論述的方法——互補(bǔ)濾波算法.這種算法可以滿足四旋翼導(dǎo)航控制系統(tǒng)的可靠度、精度的要求，因此具有重要的理論和現(xiàn)實(shí)意義.

1 互補(bǔ)濾波

互補(bǔ)濾波器可以分別給陀螺儀和加速度計(jì)的測(cè)量數(shù)據(jù)賦予加權(quán)系數(shù)，使得四旋翼飛行器的姿態(tài)角在靜態(tài)時(shí)主要依靠加速度計(jì)的測(cè)量值，大機(jī)動(dòng)時(shí)主要依靠陀螺儀的測(cè)量值.

1.1 互補(bǔ)濾波器原理

濾波器包括低通濾波器、高通濾波器、采樣周期、時(shí)間常量等 .

(1)低通濾波器.目標(biāo)是濾除高頻數(shù)據(jù)，留下低頻數(shù)據(jù).如angle=0.98*angle+0.02*x_acc;(加速度傳感器通過(guò)低通濾波器)

(2)高通濾波器.與低通濾波概念相反，能用來(lái)抑制陀螺漂移.

(3)采樣周期.該次仿真采樣率為100 Hz，即采樣周期為0.01 s.

(4)時(shí)間常量.濾波器的時(shí)間常數(shù)是信號(hào)作用的相對(duì)時(shí)間.

(5)互補(bǔ)濾波則是通過(guò)給低通和高通濾波器賦予一定的加權(quán)系數(shù)，使其輸出一個(gè)有意義的、準(zhǔn)確的線性估計(jì).如圖1所示：

圖1 互補(bǔ)濾波原理圖

1.2 互補(bǔ)濾波算法[4]

根據(jù)陀螺儀和加速度計(jì)在頻域上互補(bǔ)的特性，可以用互補(bǔ)濾波算法結(jié)合它們的優(yōu)點(diǎn).互補(bǔ)濾波器的傳遞函數(shù)如下：

(1)

(2)

將式(2)帶入重力場(chǎng)及地磁場(chǎng)分量的微分方程即

(3)

四旋翼平衡時(shí)，加速度計(jì)的輸出是：

fb=a-gb+n1

(4)

其中，fb為比力向量; a為加速度向量； gb為重力加速度； n1為噪聲向量.磁強(qiáng)計(jì)輸出為：

(5)

由式(3)和(4)，得：

(6)

由式(3)和(5)，得：

(7)

由式(6)、(7)，可以得到重力場(chǎng)和地磁場(chǎng)的互補(bǔ)濾波器的時(shí)域方程為：

(8)

(9)

通過(guò)互補(bǔ)濾波，將三種慣性器件進(jìn)行優(yōu)勢(shì)互補(bǔ)，使得長(zhǎng)時(shí)間情況下保持姿態(tài)解算的高精度且不會(huì)發(fā)生陀螺的漂移.

2 姿態(tài)航向仿真分析

2.1 硬件介紹

MTI[5]輸出的數(shù)據(jù)非常全面，包括三維加速度計(jì)、三維陀螺儀和三維磁強(qiáng)計(jì)數(shù)據(jù).此外，它還直接輸出姿態(tài)角和航向角，可以為該次仿真作為參照，見(jiàn)表1.

表1 MTI采樣項(xiàng)目信息

2.2 仿真總體構(gòu)架

本次MATLAB仿真將分別對(duì)穩(wěn)定狀態(tài)下和大機(jī)動(dòng)狀態(tài)下兩種情形進(jìn)行仿真，并分別繪制出在這兩種狀態(tài)下，由陀螺儀、加速度計(jì)以及互補(bǔ)濾波算法解算出來(lái)的姿態(tài)角及航向角隨時(shí)間的變化曲線圖.然后，對(duì)穩(wěn)定狀態(tài)下的陀螺儀積分得到的姿態(tài)角隨時(shí)間變化曲線進(jìn)行分析，由于陀螺儀會(huì)發(fā)生漂移，所以預(yù)測(cè)誤差很大.隨后，對(duì)大機(jī)動(dòng)狀態(tài)下的加速計(jì)計(jì)算得到的姿態(tài)角隨時(shí)間的變化曲線進(jìn)行分析，由于加速度計(jì)在動(dòng)態(tài)響應(yīng)方面存在劣勢(shì)，所以預(yù)測(cè)加速度計(jì)得到的結(jié)果偏差將比較大.最后，使用互補(bǔ)濾波算法求得姿態(tài)角隨時(shí)間的變化曲線，并和前兩中計(jì)算方法進(jìn)行對(duì)比分析，如果PID參數(shù)選擇準(zhǔn)確，預(yù)測(cè)互補(bǔ)濾波效果會(huì)優(yōu)于前兩者.流程圖解如圖2所示.

圖2 仿真流程圖

2.3 仿真實(shí)驗(yàn)

由于此次使用的MTI小型AHRS系統(tǒng)輸出的數(shù)據(jù)有加速度計(jì)、陀螺儀、磁強(qiáng)計(jì)以及三個(gè)姿態(tài)角(Roll、Pitch、Yaw)，所以可將仿真得到的數(shù)據(jù)與MTI小型AHRS輸出的原始姿態(tài)角數(shù)據(jù)進(jìn)行對(duì)比分析.

(1)穩(wěn)定狀態(tài)下采集加速度計(jì)、陀螺儀，分別采用加速度計(jì)計(jì)算和四元數(shù)方法計(jì)算(陀螺儀積分)俯仰角、滾轉(zhuǎn)角，得到結(jié)果如圖3、圖4所示.

圖3 滾轉(zhuǎn)角變化

圖4 俯仰角變化

從圖3和圖4中可以明顯地看出，在四旋翼穩(wěn)定飛行狀態(tài)下，由于陀螺儀存在漂移的現(xiàn)象(即陀螺儀的誤差會(huì)累積)，其在穩(wěn)定的狀態(tài)下所測(cè)得的滾轉(zhuǎn)角和俯仰角，隨著時(shí)間的增長(zhǎng)，會(huì)越來(lái)越偏離真實(shí)值.因此，如果沒(méi)有其他修正的方法，得到姿態(tài)角誤差只會(huì)越來(lái)越大.所以，直接采用陀螺儀數(shù)據(jù)積分所得到的姿態(tài)角結(jié)果是不準(zhǔn)確的.

(2)大機(jī)動(dòng)時(shí)，只使用加速度計(jì)測(cè)量計(jì)算的姿態(tài)角數(shù)據(jù)與原始數(shù)據(jù)對(duì)比如圖5和圖6所示.

圖5 俯仰角變化

圖6 滾轉(zhuǎn)角變化

從圖5和圖6中可以看出，在80 s之后四旋翼做了較大的機(jī)動(dòng)飛行，此時(shí)的加速度計(jì)計(jì)算的數(shù)據(jù)比原始數(shù)據(jù)大，即存在額外的加速度，致使測(cè)量誤差更大，所以只使用加速度計(jì)測(cè)得的姿態(tài)角數(shù)據(jù)也是不準(zhǔn)確的.

(3)同樣的情況下，大機(jī)動(dòng)時(shí)，采用加速度計(jì)數(shù)據(jù)和陀螺儀數(shù)據(jù)融合的方法(互補(bǔ)濾波)來(lái)進(jìn)行仿真，所得到的姿態(tài)角變化曲線結(jié)果與加速度計(jì)直接計(jì)算的結(jié)果對(duì)比如圖7、圖8所示.為了達(dá)到對(duì)比的效果明確，將原始數(shù)據(jù)和加速度計(jì)計(jì)算的姿態(tài)角也放在圖中.

圖7 滾轉(zhuǎn)角變化

圖8 俯仰角變化

顯然，從圖7和圖8中可以看出，從63 s開(kāi)始，四旋翼開(kāi)始做大機(jī)動(dòng)飛行，俯仰角大小幾乎不變，而滾轉(zhuǎn)角大小變化劇烈，說(shuō)明四旋翼在進(jìn)行較頻繁的滾轉(zhuǎn)機(jī)動(dòng)飛行.而互補(bǔ)濾波器得到的滾轉(zhuǎn)角幾乎與MTI直接輸出的原始數(shù)據(jù)重合，而加速度計(jì)由于其本身的動(dòng)態(tài)劣勢(shì)，使得測(cè)量誤差較大.這證明互補(bǔ)濾波算法的優(yōu)化作用是顯而易見(jiàn)的，它能夠綜合陀螺儀和加速計(jì)的優(yōu)點(diǎn)，使得測(cè)量精度更高，在大機(jī)動(dòng)飛行時(shí)，動(dòng)態(tài)響應(yīng)更好.但是相比MTI小型AHRS直接輸出的數(shù)據(jù)還有差距，考慮到這點(diǎn)，就需要進(jìn)行一些對(duì)比.

為了更清楚的對(duì)比互補(bǔ)濾波算法所得到的姿態(tài)角的上下浮動(dòng)情況與MIT系統(tǒng)輸出的姿態(tài)角的數(shù)據(jù)的差別，取出部分仿真過(guò)程中的數(shù)據(jù)來(lái)進(jìn)行計(jì)算，對(duì)各項(xiàng)都計(jì)算均方差，所得到結(jié)果見(jiàn)表2.

表 2 均方差對(duì)比

由表2可以看出，使用濾波算法所得到滾轉(zhuǎn)角和俯仰角，以及用磁強(qiáng)計(jì)數(shù)據(jù)計(jì)算得到的偏航角數(shù)據(jù)的均方差都比MTI系統(tǒng)直接輸出的三個(gè)姿態(tài)角的均方差大.雖然本文所采用的互補(bǔ)濾波算法相比于陀螺儀積分和加速度計(jì)直接計(jì)算有一定的優(yōu)化效果，但是相比于MTI的數(shù)據(jù)來(lái)說(shuō)，仍然不夠穩(wěn)定.這可能與MTI內(nèi)部可能采用更為優(yōu)化的卡爾曼濾波算法有關(guān)，在此后的研究中，將進(jìn)一步深入研究.

(4)對(duì)于磁強(qiáng)計(jì)計(jì)算得到偏航角，與陀螺儀修正法得到的偏航角，該文只將其與原始數(shù)據(jù)進(jìn)行對(duì)比，而不再將其進(jìn)行互補(bǔ)濾波計(jì)算.不過(guò)程序中求解時(shí)對(duì)磁強(qiáng)計(jì)和陀螺儀所得結(jié)果均經(jīng)行了修正.對(duì)比圖如圖9所示.

由圖9可以看出，在63 s之后，由于四旋翼做大機(jī)動(dòng)飛行，雖然只改變滾轉(zhuǎn)角，但由于磁強(qiáng)計(jì)的動(dòng)態(tài)響應(yīng)較差，所以在那之后它所測(cè)得的數(shù)據(jù)也發(fā)生了較明顯的變化.可以說(shuō)明，磁強(qiáng)計(jì)的動(dòng)態(tài)特性也不好.后續(xù)的研究中將進(jìn)一步研究一些優(yōu)化算法，會(huì)使磁強(qiáng)計(jì)得到的偏航角效果更好一些.

圖9 偏航角變化

3 結(jié)束語(yǔ)

實(shí)驗(yàn)結(jié)果表明，在穩(wěn)定狀態(tài)下，相比直接使用陀螺儀數(shù)據(jù)積分計(jì)算得到的俯仰角和滾轉(zhuǎn)角來(lái)說(shuō)，加速度計(jì)計(jì)算得到俯仰角和滾轉(zhuǎn)角還是比較穩(wěn)定的，沒(méi)有發(fā)生類似陀螺儀因漂移而產(chǎn)生的過(guò)大誤差的情況.而在大機(jī)動(dòng)飛行的時(shí)候，由于加速度計(jì)的動(dòng)態(tài)響應(yīng)很差，產(chǎn)生了額外加速度，導(dǎo)致其測(cè)出的數(shù)據(jù)有較大的誤差.互補(bǔ)濾波算法綜合了加速度計(jì)和陀螺儀的優(yōu)點(diǎn)，所得到的結(jié)果更加接近原始數(shù)據(jù)，因此使用互補(bǔ)濾波算法進(jìn)行姿態(tài)解算結(jié)果更加準(zhǔn)確，工程應(yīng)用方面亦可進(jìn)行應(yīng)用.

[1] 沈林成.無(wú)人機(jī)系統(tǒng)導(dǎo)論[M]. 北京：國(guó)防工業(yè)出版社，2014.9.

[2] 李榮冰,劉建業(yè),曾慶化.基于MEMS技術(shù)的微型慣性導(dǎo)航系統(tǒng)發(fā)展?fàn)顩r[J].中國(guó)慣性技術(shù)學(xué)報(bào),2004,12(6)：88-95.

[3] 時(shí)貴敏.基于MEMS傳感技術(shù)的航姿系統(tǒng)算法研究[D].哈爾濱：哈爾濱工程大學(xué)工學(xué)碩士學(xué)位論文，2014.

[4] 徐鵬.某型無(wú)人機(jī)姿態(tài)航向參考系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].西安：西北工業(yè)大學(xué)碩士學(xué)位論文,2007.

[5] 郭曉鴻，楊忠，陳喆，楊成順，龔華軍，EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用[J],中國(guó)慣性技術(shù)學(xué)報(bào),2011,30(11):56-60.

[6] 喬偉，劉鵬，黃艷輝. 微慣性單元姿態(tài)解算方法[J]，集成電路通訊,2011(3):36-40.

(責(zé)任編輯：季春陽(yáng))

The Simulation of Quadrotor Attitude Heading Based on MATLAB

Dai Jun, Liu Zhanhe, Wang Jing

(Zhengzhou University of Aeronautics)

In this paper, the simulation is analyzed on the information fusion algorithm based on the complementary filter and uses the actual experiments data of the inertial device from MTI small attitude heading

ystem. After comparing the results of attitude angle from the results of integral calculation of accelerometer and gyroscope and the results based on Complementary Filter, the optimization effect of the Complementary Filter theory applied in the attitude data measurement is analyzed.

Attitude Heading Reference; System; Complementary Filter; MTI

2016-02-09

*河南省科技廳(162102210237)；航空科學(xué)基金(2014ZA55001)；河南省教育廳(15A590001)

TP249

A

1000-5617(2016)02-0060-05