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

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

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

?

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

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

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

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

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

0 引言

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

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

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

1 互補濾波

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

1.1 互補濾波器原理

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

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

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

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

(4)時間常量.濾波器的時間常數(shù)是信號作用的相對時間.

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

圖1 互補濾波原理圖

1.2 互補濾波算法[4]

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

(1)

(2)

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

(3)

四旋翼平衡時，加速度計的輸出是：

fb=a-gb+n1

(4)

其中，fb為比力向量; a為加速度向量； gb為重力加速度； n1為噪聲向量.磁強計輸出為：

(5)

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

(6)

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

(7)

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

(8)

(9)

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

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

2.1 硬件介紹

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

表1 MTI采樣項目信息

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

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

圖2 仿真流程圖

2.3 仿真實驗

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

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

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

圖4 俯仰角變化

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

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

圖5 俯仰角變化

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

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

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

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

圖8 俯仰角變化

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

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

表 2 均方差對比

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

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

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

圖9 偏航角變化

3 結(jié)束語

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

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

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

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

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

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

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

(責任編輯：季春陽)

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)；航空科學基金(2014ZA55001)；河南省教育廳(15A590001)

TP249

A

1000-5617(2016)02-0060-05