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

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

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

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

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

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

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

測姿系統;MEMS慣性器件;Kalman濾波;數據融合

0 引言

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

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

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

1 坐標系和姿態角描述

本文定義兩種坐標系,分別是地理坐標系和機體坐標系。地理坐標系是載體所在地區的坐標系,常用符號n來表示。坐標原點通常為載體質心,定義z軸為當地地垂線方向并以指向天空為正向,x軸在當地水平面內以指向東為正向,y軸在當地水平面內以指向北為正向。

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

姿態余弦矩陣表征了參考坐標系到載體坐標系的變換,反映了載體在參考坐標系中的姿態航向信息。

(1)

2 兩種測姿方法

2.1 基于MEMS陀螺儀的測姿

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

四元數可表示為

Q=q0+q1i+q2j+q3k

(2)

(3)

式中,?表示四元數乘法。

由四元數與坐標變換矩陣的關系,姿態余弦矩陣可由一個構成四元數的四個實數表示,表達式如下

(4)

這樣,載體的姿態角就可由四元數表示為:

(5)

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

(6)

其中

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

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

(7)

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

2.2 基于MEMS加速度計和磁強計的測姿

當載體處于零加速運動的狀態時,三軸加速度計的比力矢量和等于當地的重力加速度。如果此時載體水平,僅有天向加速度計有比力輸出,并且大小等于重力加速度;而如果載體不水平,那三個軸的加速度計都會有輸出,并且重力在三軸加速度計的分量與載體的姿態成一定的對應關系,可以表示為

(8)

f、gn分別表示三軸加速度計的比力矢量和當地重力加速度的比力矢量。結合姿態余弦矩陣,載體的俯仰角和橫滾角就可以用三軸加速度計的比力輸出表示:

(9)

(10)

ψ=-arctan(mx/my)

(11)

3 基于Kalman濾波的姿態信息融合

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

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

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

(12)

(13)

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

3.2 系統方程的推導

對四元數微分方程兩邊同時求導得到

(14)

q-qg=δq

(15)

式中,q是載體姿態對應的真實四元數,qg是由陀螺儀單獨測姿得到的四元數,本文選取兩者的差作為系統的狀態變量。

選取系統的狀態變量為

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

那么系統的狀態方程可以表示成為

(16)

其中系統的過程噪聲向量為

3.3 系統的觀測方程

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

δqm=qm-qg

(17)

其中，qm是根據加速度計和磁強計組合測姿得到的四元數。

則系統的觀測方程為

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

(18)

根據推導系統的狀態方程和觀測方程可以畫出磁強計與微慣性器件的組合測姿系統的原理圖如圖1所示。

圖1 組合測姿系統原理圖Fig.1 Schematic diagram of integrated attitude determination system

3.4 仿真參數設置

3.4.1 傳感器參數設置

傳感器參數包括MEMS陀螺儀和加速度計以及磁強計的隨機常數誤差,一階馬科夫過程相關時間。傳感器精度指標如表1[6]所示。

3.4.2 其他參數設置

系統狀態初值x0,狀態估計均方誤差矩陣初值P0、系統噪聲均方誤差矩陣初值Q0和系統觀測噪聲均方誤差矩陣R0初值設置如下:

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])

考慮到四旋翼無人機的續航時間,設置仿真時間為0.5h,姿態算法更新頻率為 10Hz。

表1 傳感器精度指標Tab.1 Accuracy of sensers

3.5 仿真與結果

在本文仿真分析中,假設載體做無加速度的運動,載體的3個姿態角按正弦規律變化為

(19)

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

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

圖2 俯仰角Fig.2 Pitch angle

圖3 橫滾角Fig.3 Roll angle

圖4 偏航角Fig.4 Yaw angle

4 結論

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

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

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

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

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

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

[6] 伊程毅.基于地磁和微慣性器件組合的姿態測量系統研究 [D].哈爾濱工業大學， 2013.

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

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

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

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

[11] 馬敏, 吳海超.基于四元數自補償四旋翼飛行器姿態解算[J].制造業自動化，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

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

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

V249.122

A

2095-8110(2017)03-0027-05