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

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

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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

(1)

2 兩種測(cè)姿方法

2.1 基于MEMS陀螺儀的測(cè)姿

MEMS陀螺儀為捷聯(lián)式陀螺儀,沒(méi)有陀螺環(huán)架,因此不能直接得到載體姿態(tài)信息,而需要對(duì)陀螺儀所測(cè)得的角速率進(jìn)行積分來(lái)得到載體的姿態(tài)。利用捷聯(lián)式陀螺儀測(cè)姿的解算方法有歐拉角法、方向余弦法、四元數(shù)法、等效旋轉(zhuǎn)矢量法等,四元數(shù)算法所需參數(shù)少、計(jì)算簡(jiǎn)單、便于實(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è)構(gòu)成四元數(shù)的四個(gè)實(shí)數(shù)表示,表達(dá)式如下

(4)

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

(5)

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

(6)

其中

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

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

(7)

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

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

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

(8)

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

(9)

(10)

ψ=-arctan(mx/my)

(11)

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

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

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

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

(12)

(13)

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

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

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

(14)

q-qg=δq

(15)

式中,q是載體姿態(tài)對(duì)應(yīng)的真實(shí)四元數(shù),qg是由陀螺儀單獨(dú)測(cè)姿得到的四元數(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)的過(guò)程噪聲向量為

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

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

δqm=qm-qg

(17)

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

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

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

(18)

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

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

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

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

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

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

系統(tǒng)狀態(tài)初值x0,狀態(tài)估計(jì)均方誤差矩陣初值P0、系統(tǒng)噪聲均方誤差矩陣初值Q0和系統(tǒng)觀測(cè)噪聲均方誤差矩陣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])

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

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

3.5 仿真與結(jié)果

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

(19)

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

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

圖2 俯仰角Fig.2 Pitch angle

圖3 橫滾角Fig.3 Roll angle

圖4 偏航角Fig.4 Yaw angle

4 結(jié)論

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

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

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

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

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

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

[6] 伊程毅.基于地磁和微慣性器件組合的姿態(tài)測(cè)量系統(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] 李會(huì).基于地磁/低成本MEMS慣性器件復(fù)合姿態(tài)測(cè)量方法研究 [D].沈陽(yáng)理工大學(xué)， 2014.

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

[10] 吳友強(qiáng), 紀(jì)浩, 鄒力涵.基于卡爾曼濾波的四旋翼飛行器懸?？刂蒲芯縖J].自動(dòng)化與儀表，2013，28(11):1-5.

[11] 馬敏, 吳海超.基于四元數(shù)自補(bǔ)償四旋翼飛行器姿態(tài)解算[J].制造業(yè)自動(dòng)化，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

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

戴邵武(1966-)，男，博士，教授，研究方向?yàn)閷?dǎo)彈測(cè)試與控制。E-mail:dswhy@163.com

V249.122

A

2095-8110(2017)03-0027-05