戴邵武,李松林,邢志川,丁廣威,袁 鵬,康宇航

(1.海軍航空工程學(xué)院,山東 煙臺 264001；2.91467部隊,山東 青島 266311；3.91206部隊, 山東 青島 266108;4.92330部隊,山東 青島 266001)

基于MEMS傳感器的四旋翼組合測姿研究

戴邵武1,李松林1,邢志川2,丁廣威3,袁 鵬4,康宇航1

(1.海軍航空工程學(xué)院,山東 煙臺 264001；2.91467部隊,山東 青島 266311；3.91206部隊, 山東 青島 266108;4.92330部隊,山東 青島 266001)

針對四旋翼無人機(jī)機(jī)體尺寸較小、帶載荷能力有限的特點,設(shè)計基于低成本MEMS慣性器件的測姿系統(tǒng),以滿足四旋翼無人機(jī)在系統(tǒng)控制方面的需求。但是低成本的MEMS慣性器件具有精度低,隨機(jī)漂移大,容易受到外界環(huán)境干擾等缺點。本文充分發(fā)揮四旋翼無人機(jī)搭載的MEMS慣性器件的功能,分別利用兩種方法測量載體姿態(tài),并通過卡爾曼濾波的方法實現(xiàn)這兩種測姿方法的數(shù)據(jù)融合。經(jīng)過仿真分析,融合后組合測姿系統(tǒng)的測姿精度得到提高。

測姿系統(tǒng);MEMS慣性器件;Kalman濾波;數(shù)據(jù)融合

0 引言

最近幾年在全世界范圍內(nèi),無人機(jī)的應(yīng)用迅速蔓延開來。民用領(lǐng)域,無人機(jī)主要用于航拍、環(huán)境監(jiān)測、新聞報道、天氣預(yù)報、噴灑農(nóng)藥等；軍用領(lǐng)域,無人機(jī)則用于靶機(jī)、電子偵察、騙敵誘餌、實施干擾等任務(wù)[1]。無人機(jī)按結(jié)構(gòu)分為固定翼無人機(jī)和旋翼無人機(jī)。四旋翼無人機(jī)是旋翼無人機(jī)的典型代表。四旋翼無人機(jī)與固定翼無人機(jī)的不同之處在于四旋翼無人機(jī)的升力由旋翼旋轉(zhuǎn)產(chǎn)生,而固定翼無人機(jī)的升力則來自機(jī)翼,飛機(jī)必須具有一定速度才可以獲得能夠克服自身重力的升力[2]。四旋翼無人機(jī)的空氣動力結(jié)構(gòu)使其能夠?qū)崿F(xiàn)定點懸停和垂直起降的功能。除此之外,四旋翼無人機(jī)還具有體積小、質(zhì)量小、成本低、易操作、控制邏輯清晰等優(yōu)點。四旋翼無人機(jī)的這些優(yōu)勢使得其在無人機(jī)市場占有相當(dāng)大的席位,在軍事偵察、管線巡邏、災(zāi)情監(jiān)測、商業(yè)航拍等諸多領(lǐng)域都有應(yīng)用價值[3]。

四旋翼無人機(jī)的廣泛應(yīng)用對其自身的性能提出了更高的要求,特別是姿態(tài)精度要求,所以對四旋翼無人機(jī)姿態(tài)的測量是無人機(jī)技術(shù)研究的重點。由于四旋翼無人機(jī)體積質(zhì)量小,升力靠旋翼旋轉(zhuǎn)產(chǎn)生,并且大多四旋翼無人機(jī)的動力來源都是機(jī)載電池,導(dǎo)致四旋翼無人機(jī)的升力和能源供應(yīng)能力很有限[4]。傳統(tǒng)的姿態(tài)測量系統(tǒng)由于體積、質(zhì)量、功耗等原因,超過了四旋翼無人機(jī)的負(fù)載能力,因此尋找一種體積小、質(zhì)量小、功耗低的姿態(tài)測量系統(tǒng)是四旋翼無人機(jī)技術(shù)研究的重點[5]。

隨著科學(xué)技術(shù)的發(fā)展,基于微機(jī)械和微電子技術(shù)的微機(jī)電(MEMS)傳感器被制造出來并廣泛應(yīng)用[6],微機(jī)電傳感器體積僅為幾毫米,質(zhì)量為幾毫克至幾克,功率也僅為毫瓦級。因此基于微機(jī)電傳感器的姿態(tài)測量系統(tǒng)就負(fù)載方面而言是四旋翼無人機(jī)的理想選擇[7]。但是基于MEMS傳感器的姿態(tài)測量系統(tǒng)除了存在陀螺漂移、積累誤差、振動影響、外磁場干擾等誤差外,還存在測量精度低的問題,這又對姿態(tài)的測量精度造成了影響。因此,四旋翼無人機(jī)負(fù)載能力與其姿態(tài)測量精度之間產(chǎn)生了矛盾,體積質(zhì)量大、功耗高的傳統(tǒng)姿態(tài)測量系統(tǒng),不能滿足四旋翼無人機(jī)的負(fù)載;而體積質(zhì)量小、功耗低的微機(jī)電傳感器組成的姿態(tài)測量元件又導(dǎo)致精度低。為了解決這個矛盾,本文研究一款基于微機(jī)電傳感器的四旋翼無人機(jī)組合測姿系統(tǒng),并結(jié)合數(shù)據(jù)融合的方法來提高測姿精度。

1 坐標(biāo)系和姿態(tài)角描述

本文定義兩種坐標(biāo)系,分別是地理坐標(biāo)系和機(jī)體坐標(biāo)系。地理坐標(biāo)系是載體所在地區(qū)的坐標(biāo)系,常用符號n來表示。坐標(biāo)原點通常為載體質(zhì)心,定義z軸為當(dāng)?shù)氐卮咕€方向并以指向天空為正向,x軸在當(dāng)?shù)厮矫鎯?nèi)以指向東為正向,y軸在當(dāng)?shù)厮矫鎯?nèi)以指向北為正向。

載體坐標(biāo)系是固連在運動載體上的坐標(biāo)系,常用符號b來表示。坐標(biāo)原點通常選為載體質(zhì)心,定義y軸沿載體對稱線方向并以指向載體的前方為正,x軸垂直載體對稱面并以指向載體右側(cè)為正,z軸在載體對稱面內(nèi)以指向上為正。

姿態(tài)余弦矩陣表征了參考坐標(biāo)系到載體坐標(biāo)系的變換,反映了載體在參考坐標(biāo)系中的姿態(tài)航向信息。

(1)

2 兩種測姿方法

2.1 基于MEMS陀螺儀的測姿

MEMS陀螺儀為捷聯(lián)式陀螺儀,沒有陀螺環(huán)架,因此不能直接得到載體姿態(tài)信息,而需要對陀螺儀所測得的角速率進(jìn)行積分來得到載體的姿態(tài)。利用捷聯(lián)式陀螺儀測姿的解算方法有歐拉角法、方向余弦法、四元數(shù)法、等效旋轉(zhuǎn)矢量法等,四元數(shù)算法所需參數(shù)少、計算簡單、便于實現(xiàn)[8],所以本文采用四元數(shù)算法作為研究姿態(tài)角算法的基礎(chǔ)。

四元數(shù)可表示為

Q=q0+q1i+q2j+q3k

(2)

(3)

式中,?表示四元數(shù)乘法。

由四元數(shù)與坐標(biāo)變換矩陣的關(guān)系,姿態(tài)余弦矩陣可由一個構(gòu)成四元數(shù)的四個實數(shù)表示,表達(dá)式如下

(4)

這樣,載體的姿態(tài)角就可由四元數(shù)表示為:

(5)

由于四旋翼飛行器飛行高度低,飛行速度慢,可以忽略地球自轉(zhuǎn)等因素的影響,那么四元數(shù)具有如下微分方程關(guān)系[9]。

(6)

其中

ωx、ωy、ωz為三軸MEMS陀螺儀的輸出。

設(shè)T為采樣周期,四元數(shù)微分方程的一階龍格-庫塔法計算式為

(7)

只要知道每個周期的三軸MEMS陀螺儀的輸出,就可以用式(7)迭代運算得到姿態(tài)四元數(shù)從而得到載體的姿態(tài)角。

2.2 基于MEMS加速度計和磁強(qiáng)計的測姿

當(dāng)載體處于零加速運動的狀態(tài)時,三軸加速度計的比力矢量和等于當(dāng)?shù)氐闹亓铀俣?。如果此時載體水平,僅有天向加速度計有比力輸出,并且大小等于重力加速度;而如果載體不水平,那三個軸的加速度計都會有輸出,并且重力在三軸加速度計的分量與載體的姿態(tài)成一定的對應(yīng)關(guān)系,可以表示為

(8)

f、gn分別表示三軸加速度計的比力矢量和當(dāng)?shù)刂亓铀俣鹊谋攘κ噶?。結(jié)合姿態(tài)余弦矩陣,載體的俯仰角和橫滾角就可以用三軸加速度計的比力輸出表示:

(9)

(10)

ψ=-arctan(mx/my)

(11)

3 基于Kalman濾波的姿態(tài)信息融合

3.1MEMS微慣性器件的誤差建模

MEMS陀螺儀的誤差種類繁多,在工程中對姿態(tài)解算精度影響比較大的主要有零偏誤差、隨機(jī)漂移誤差、噪聲等,也是建立MEMS陀螺儀誤差模型的主要因素。MEMS加速度計的誤差來源和種類與MEMS陀螺儀很相似,表述其性能好壞的主要性能指標(biāo)有:零偏、零偏穩(wěn)定性、線性度、噪聲、隨機(jī)漂移等等。磁強(qiáng)計的誤差一般包括非正交誤差、垂直軸效應(yīng)誤差、溫度誤差、零位誤差和靈敏度誤差等[6]。

在本文的研究中,將MEMS微慣性器件與磁強(qiáng)計的輸出都看成是由理論值加上隨機(jī)常數(shù)誤差、一階馬爾科夫過程和高斯白噪聲的綜合[10]。

(12)

(13)

其中,TH、wh分別為MEMS微慣性器件漂移的相關(guān)時間和偏移噪聲。

3.2 系統(tǒng)方程的推導(dǎo)

對四元數(shù)微分方程兩邊同時求導(dǎo)得到

(14)

q-qg=δq

(15)

式中,q是載體姿態(tài)對應(yīng)的真實四元數(shù),qg是由陀螺儀單獨測姿得到的四元數(shù),本文選取兩者的差作為系統(tǒng)的狀態(tài)變量。

選取系統(tǒng)的狀態(tài)變量為

X=[δq0,δq1,δq2,δq3,nbx,nby,nbz,nrx,nry,nrz]T

那么系統(tǒng)的狀態(tài)方程可以表示成為

(16)

其中系統(tǒng)的過程噪聲向量為

3.3 系統(tǒng)的觀測方程

將加速度計和磁強(qiáng)計組合測姿得到的四元數(shù)與陀螺儀測姿得到的四元數(shù)作差得到的差值作為四元數(shù)誤差的觀測值,即

δqm=qm-qg

(17)

其中，qm是根據(jù)加速度計和磁強(qiáng)計組合測姿得到的四元數(shù)。

則系統(tǒng)的觀測方程為

Z=δqm=HX(t)+V(t)

(18)

根據(jù)推導(dǎo)系統(tǒng)的狀態(tài)方程和觀測方程可以畫出磁強(qiáng)計與微慣性器件的組合測姿系統(tǒng)的原理圖如圖1所示。

圖1 組合測姿系統(tǒng)原理圖Fig.1 Schematic diagram of integrated attitude determination system

3.4 仿真參數(shù)設(shè)置

3.4.1 傳感器參數(shù)設(shè)置

傳感器參數(shù)包括MEMS陀螺儀和加速度計以及磁強(qiáng)計的隨機(jī)常數(shù)誤差,一階馬科夫過程相關(guān)時間。傳感器精度指標(biāo)如表1[6]所示。

3.4.2 其他參數(shù)設(shè)置

系統(tǒng)狀態(tài)初值x0,狀態(tài)估計均方誤差矩陣初值P0、系統(tǒng)噪聲均方誤差矩陣初值Q0和系統(tǒng)觀測噪聲均方誤差矩陣R0初值設(shè)置如下:

x0=[0,0,0,0,0,0,0,0,0,0]

P0=diag([0.0022,0.0022,0.0022,0.0022,

(0.004(°)/s)2,(0.004(°)/s)2,

(0.004(°)/s)2,(0.004(°)/s)2,

(0.001(°)/s)2,(0.001(°)/s)2,

(0.001(°)/s)2])

Q0=diag([(0.004(°)/s)2,(0.004(°)/s)2,

(0.004(°)/s)2,(0.001(°)/s)2,

(0.001(°)/s)2,(0.001(°)/s)2])

R0=diag([0.0022,0.0022,0.0022,0.0022])

考慮到四旋翼無人機(jī)的續(xù)航時間,設(shè)置仿真時間為0.5h,姿態(tài)算法更新頻率為 10Hz。

表1 傳感器精度指標(biāo)Tab.1 Accuracy of sensers

3.5 仿真與結(jié)果

在本文仿真分析中,假設(shè)載體做無加速度的運動,載體的3個姿態(tài)角按正弦規(guī)律變化為

(19)

分別利用MEMS陀螺儀單獨測姿和利用MEMS陀螺儀、加速度計、磁強(qiáng)計組合測姿,并對兩種測姿的精度進(jìn)行分析,得到以下仿真圖形如圖2～圖4所示。

由仿真圖可以看出,單獨利用MEMS陀螺儀測姿,由于MEMS陀螺儀的隨機(jī)漂移和積累誤差等原因,測姿精度低并且波動也比較大。而在融合進(jìn)MEMS加速度計和磁強(qiáng)計后的組合測姿系統(tǒng)中,測姿精度得到了明顯提高,實現(xiàn)了MEMS微慣性器件之間的性能互補(bǔ)。

圖2 俯仰角Fig.2 Pitch angle

圖3 橫滾角Fig.3 Roll angle

圖4 偏航角Fig.4 Yaw angle

4 結(jié)論

本文充分利用四旋翼無人機(jī)上搭載的MEMS慣性器件,組成了兩種測姿分系統(tǒng)。MEMS陀螺儀穩(wěn)定性較好,但是具有精度低和積累誤差的缺點,而MEMS加速度計和磁強(qiáng)計則沒有積累誤差,卻容易受到外界環(huán)境的影響。通過Kalman濾波的方法將兩種測姿系統(tǒng)所測得的姿態(tài)信息進(jìn)行數(shù)據(jù)融合,通過仿真分析,驗證了融合后測姿精度得到提高。

[1] 張浩, 任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動化，2013(1):28-31.

[2] 劉建業(yè), 賈文峰, 賴際舟, 等.微小型四旋翼飛行器多信息非線性融合導(dǎo)航方法及實現(xiàn)[J].南京航空航天大學(xué)學(xué)報，2013，45(5):575-582.

[3] 宋英麟, 鮮斌, 茹濱超, 等.無人機(jī)微型姿態(tài)航向系統(tǒng)數(shù)據(jù)處理[J].中南大學(xué)學(xué)報(自然科學(xué)版)，2013(S2):89-93.

[4] 肖支才, 姜鵬, 戴洪德, 等.室內(nèi)四旋翼無人飛行器定位導(dǎo)航的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].飛航導(dǎo)彈，2014(8):25-29.

[5] 汪紹華, 楊瑩.基于卡爾曼濾波的四旋翼飛行器姿態(tài)估計和控制算法研究(英文)[J].控制理論與應(yīng)用，2013，30(9):1109-1115.

[6] 伊程毅.基于地磁和微慣性器件組合的姿態(tài)測量系統(tǒng)研究 [D].哈爾濱工業(yè)大學(xué)， 2013.

[7] Skog I, H?ndel P.Calibration of a MEMS inertial measurement unit[J].Electrical Engineering Electronic Engineering Information Engineering，2007.

[8] 李會.基于地磁/低成本MEMS慣性器件復(fù)合姿態(tài)測量方法研究 [D].沈陽理工大學(xué)， 2014.

[9] 王凱, 馬騰達(dá), 周德新.基于多傳感器融合的四旋翼飛行器姿態(tài)監(jiān)控[J].測控技術(shù)，2015，34(3):21-24.

[10] 吳友強(qiáng), 紀(jì)浩, 鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸停控制研究[J].自動化與儀表，2013，28(11):1-5.

[11] 馬敏, 吳海超.基于四元數(shù)自補(bǔ)償四旋翼飛行器姿態(tài)解算[J].制造業(yè)自動化，2013(23):18-21.

Research on Integrated Attitude Determination Methods Based on MEMS Device for Quadrotor UAVs

DAI Shao-wu1, LI Song-lin1, XING Zhi-chuan2, DING Guang-wei3, YUAN Peng4, KANG Yu-hang1

(1.Naval Aeronautical Engineering Institute, Yantai，Shandong 264001,China；2.The Troop of 91467, Qingdao，Shandong 266311,China；3.The Troop of 91206, Qingdao，Shandong 266108,China; 4.The Troop of 92330, Qingdao，Shandong 266001,China)

A micro attitude determination system is developed based on low-cost MEMS devices to meet the requirement for the flight control of micro unmanned aerial vehicles(UAV).But low-cost MEMS inertial devices have shortcomings such as low accuracy, great random drift, and is vulnerable to external environmental interference.In this paper,we make the full advantage of MEMS inertial devices equipped on Quadrotor UAV function,and realise data fusion of two attitude determination methods by Kalman filter.Through analysis of simulation, the attitude measurement accuracy of integrated attitude determination system has been improved.

Attitude determination system； MEMS inertial device； Kalman filter； Data fusion

10.19306/j.cnki.2095-8110.2017.03.005

2016-08-22；

2016-09-25

國家自然科學(xué)基金(61102167)；航空科學(xué)基金(21035184007)

戴邵武(1966-)，男，博士，教授，研究方向為導(dǎo)彈測試與控制。E-mail:dswhy@163.com

V249.122

A

2095-8110(2017)03-0027-05