姜香菊， 曾幼涵， 劉二林

(1.蘭州交通大學(xué) 自動(dòng)化與電氣工程學(xué)院，甘肅 蘭州 730070;2.蘭州交通大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730070)

?

四旋翼飛機(jī)姿態(tài)檢測(cè)方法設(shè)計(jì)與實(shí)現(xiàn)*

姜香菊1， 曾幼涵1， 劉二林2

(1.蘭州交通大學(xué) 自動(dòng)化與電氣工程學(xué)院，甘肅 蘭州 730070;2.蘭州交通大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730070)

為了解決四旋翼飛機(jī)的姿態(tài)檢測(cè)問題，設(shè)計(jì)了基于MPU6050和HCM5883L的九軸傳感器，并提出了基于正弦算法的四旋翼飛機(jī)姿態(tài)角計(jì)算方法，該方法不需要判斷即可由檢測(cè)值直接獲取角度大小及正負(fù)，較方向余弦算法具有更好的實(shí)用性。針對(duì)陀螺儀和加速度計(jì)的優(yōu)缺點(diǎn)，設(shè)計(jì)了互補(bǔ)數(shù)據(jù)融合算法。仿真及實(shí)踐均證明了檢測(cè)方案的有效性。

四旋翼飛機(jī); 姿態(tài)檢測(cè); MPU6050; 互補(bǔ)算法

0 引 言

四旋翼飛機(jī)是一種由4個(gè)螺旋槳對(duì)稱安裝而構(gòu)成的飛行器。該飛行器主要有兩種結(jié)構(gòu)，一種為“+”字結(jié)構(gòu)，一種為“X”結(jié)構(gòu)。從Breguet-Richet發(fā)明世界上第一架四旋翼飛機(jī)至今，四旋翼飛機(jī)已經(jīng)發(fā)展了一個(gè)多世紀(jì)。四旋翼飛機(jī)機(jī)械結(jié)構(gòu)相對(duì)簡(jiǎn)單，可以通過對(duì)4個(gè)螺旋槳的控制來控制飛機(jī)的飛行姿態(tài)，因此四旋翼飛機(jī)可用于搜救、航拍、偵查以及軍事對(duì)抗等各種場(chǎng)合[1～3]。四旋翼飛行器的姿態(tài)檢測(cè)是飛行器控制的核心工作之一，很多研究人員都對(duì)該問題進(jìn)行過深入的探討[4]。于雅麗等人利用MMA7260和ENC—03M等對(duì)四旋翼飛機(jī)進(jìn)行姿態(tài)檢測(cè)[5]。汪紹華等人闡述了利用卡爾曼濾波算法解決四旋翼飛機(jī)的姿態(tài)檢測(cè)問題的具體方案[8]。

本文從實(shí)際應(yīng)用的角度出發(fā)，通過慣性坐標(biāo)系和機(jī)體坐標(biāo)系詳細(xì)說明了所需檢測(cè)的飛機(jī)姿態(tài)參數(shù)，利用正弦算法對(duì)數(shù)據(jù)進(jìn)行處理；同時(shí)使用MPU6050和HMC5883進(jìn)行了硬件電路設(shè)計(jì)，并對(duì)數(shù)據(jù)處理算法進(jìn)行了分析說明。最后通過仿真及實(shí)際應(yīng)用證明了所采用方法的正確性。

1 四旋翼飛機(jī)姿態(tài)檢測(cè)參數(shù)分析

四旋翼飛機(jī)由檢測(cè)裝置，主控裝置，電機(jī)驅(qū)動(dòng)裝置以及螺旋槳構(gòu)成。本文中以“+”型四旋翼飛機(jī)為例進(jìn)行說明。在進(jìn)行控制理論研究時(shí)，一般要建立慣性坐標(biāo)系和四旋翼飛機(jī)機(jī)體坐標(biāo)系。如圖1所示，坐標(biāo)系OXYZ為慣性坐標(biāo)系。OXYZ坐標(biāo)系中，O為地面空間的任意一點(diǎn)，X軸平運(yùn)行于水平面，方向指向北方；Y軸平行于水平面，方向指向東方；Z軸垂直于OXY平面，方向與重力方向相反。坐標(biāo)系oxyz為機(jī)體坐標(biāo)系，z軸垂直于飛機(jī)的旋翼平面且通過四旋翼飛機(jī)的重心，x軸與y軸則分別平行于兩個(gè)對(duì)稱的螺旋槳的旋轉(zhuǎn)中心?？梢杂忙?(x,y,z)∈R3表示飛機(jī)質(zhì)心的位置矢量?？梢杂脷W拉角η=(ψ,θ,φ)∈R3表示四旋翼飛機(jī)的飛行姿態(tài)。

圖1 四旋翼飛機(jī)結(jié)構(gòu)簡(jiǎn)圖及受力分析

2 四旋翼飛機(jī)姿態(tài)檢測(cè)的硬件設(shè)計(jì)與檢測(cè)原理

2.1 四旋翼飛機(jī)姿態(tài)檢測(cè)硬件設(shè)計(jì)

在四旋翼飛機(jī)的幾何中心處，安裝傳感器MPU6050以及傳感器HMC5883L。MPU—6050是6軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器(digital motion processor，DMP)。MPU—6050對(duì)陀螺儀和加速度計(jì)分別用了3個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)換為可輸出的數(shù)字量。在MPU—6050上，通過其I2C接口連接非慣性數(shù)字傳感器HMC5883L組建9軸傳感器。HMC5883L 是表面貼裝的高集成模塊，并帶有數(shù)字接口的弱磁傳感器芯片，包括放大器、自動(dòng)消磁驅(qū)動(dòng)器、偏差校準(zhǔn)、能使羅盤精度控制在1°～2°的12位模數(shù)轉(zhuǎn)換器等。具體電路圖見圖2[4～6]。

圖2 傳感器硬件電路

2.2 角度檢測(cè)基本原理

2.2.1 仰俯角和翻滾角檢測(cè)基本原理

檢測(cè)過程中6軸加速度傳感器MPU6050和3軸地磁傳感器HMC5883L均靠近四旋翼飛機(jī)中心，平貼于控制板安裝，且HMC5883L的XYZ3軸與MPU6050的3軸相平行，2只傳感器的距離盡量近。MPU6050的3軸與建立的機(jī)體坐標(biāo)系的3軸相平行。假定四旋翼飛機(jī)勻速運(yùn)動(dòng)時(shí)，可以用方向余弦來計(jì)算角度θ與φ，具體原理見圖3(圖中oxyz坐標(biāo)系為機(jī)體坐標(biāo)系，MPU6050的3個(gè)軸與oxyz的3個(gè)軸平行，且MPU6050的中心與機(jī)體坐標(biāo)系的中心重合)。

圖3 MPU6050檢測(cè)角度基本原理

如果MPU6050三個(gè)軸上檢測(cè)到的加速度為gx，gy，gz，則有

(1)

由方向余弦公式可得

(2)

由于需要檢測(cè)的數(shù)據(jù)為四旋翼飛機(jī)ox軸與水平面之間的夾角θ；oy軸與水平面之間的夾角φ，且在四旋翼飛機(jī)實(shí)際運(yùn)行過程中-90°<θ<90°，-90°<φ<90°，所以，對(duì)式(2)進(jìn)行處理且去掉∠gox項(xiàng)，可得

(3)

微控制器(MCU)編程時(shí)，可以根據(jù)各軸檢測(cè)值的正負(fù)直接由式(3)計(jì)算出角度，式(3)計(jì)算的角度正負(fù)符合右手定則。在θ與φ較小時(shí)，可直接取θ=sinθ，φ=sinφ。顯然由式(3)直接計(jì)算俯仰角和翻滾角較式(2)方便很多[4]。

2.2.2 偏航角檢測(cè)基本原理

3 四旋翼飛機(jī)姿態(tài)檢測(cè)軟件設(shè)計(jì)

對(duì)于MPU6050與HMC5883L而言，2只傳感器中的地磁傳感器和加速度傳感器的低頻響應(yīng)較好，但是高頻響應(yīng)特性較差。陀螺儀的高頻與低頻特性均較好，但是在實(shí)際使用過程中存在溫漂和零漂，所以，必須通過數(shù)據(jù)的處理來獲取精確的四旋翼飛機(jī)的姿態(tài)?？紤]到在MCU中編程實(shí)現(xiàn)的方便性，可以用互補(bǔ)數(shù)據(jù)融合算法對(duì)數(shù)據(jù)進(jìn)行處理。

(4)

陀螺儀動(dòng)態(tài)特性好，但是存在漂移，因此，公式(4)計(jì)算出的俯仰角會(huì)出現(xiàn)積分誤差。根據(jù)加速度計(jì)檢測(cè)出的測(cè)量值，由式(3)可以計(jì)算出俯仰角，但是由于加速度傳感器自身動(dòng)態(tài)特性較差，同時(shí)四旋翼飛機(jī)自身為非剛體，在飛行過程中不可避免地會(huì)產(chǎn)生抖動(dòng)，所以，式(3)計(jì)算出的俯仰角噪聲較大，但是該值從整體上可以反映俯仰角大小。如果采用平均濾波等方法，則檢測(cè)滯后，無法用于四旋翼飛機(jī)控制?；パa(bǔ)數(shù)據(jù)融合算法的目的是利用式(3)計(jì)算出的噪聲較大的俯仰角消除由式(4)得到的俯仰角的積分誤差，具體原理見式(5)[7～9]。

(5)

該算法的數(shù)字實(shí)現(xiàn)方法為

(6)

4 仿真分析

為了對(duì)該互補(bǔ)算法的正確性進(jìn)行分析，對(duì)式(5)進(jìn)行拉式變換可得

(7)

利用Matlab進(jìn)行仿真分析算法有效性，其Simulink仿真結(jié)果見圖4、圖5。圖4中Out1模擬實(shí)際陀螺儀的輸出。由仿真結(jié)果可以看出，由于陀螺儀的溫漂，在四旋翼飛機(jī)的機(jī)體旋轉(zhuǎn)角速度為零時(shí)，積分獲取的角度值沒有停留在實(shí)際角度處，而是會(huì)不斷增加，所以，直接由角速度積分獲取角度值不可用。在圖5中，將有溫漂的陀螺儀的值和有噪音的加速度值進(jìn)行互補(bǔ)融合處理，處理后的結(jié)果消除了積分誤差，且消除了加速度計(jì)獲取信號(hào)的噪音。

圖4 陀螺儀檢測(cè)值直接積分仿真

圖5 數(shù)據(jù)融合算法仿真

通過仿真結(jié)果進(jìn)行對(duì)比可知，數(shù)據(jù)處理前，角度信號(hào)噪聲較大；直接進(jìn)行積分的信號(hào)因?yàn)榉e分誤差逐漸偏離正常值。數(shù)據(jù)融合后，所得數(shù)據(jù)可以反映實(shí)際信號(hào)，效果較好。按照硬件設(shè)計(jì)方案和數(shù)據(jù)處理方法，結(jié)合傳統(tǒng)PD算法進(jìn)行飛機(jī)姿態(tài)控制，實(shí)踐表明，該設(shè)計(jì)方案合理有效。驗(yàn)證效果見圖6。

圖6 飛機(jī)懸浮控制效果

5 結(jié) 論

姿態(tài)檢測(cè)是四旋翼飛機(jī)控制過程中的關(guān)鍵問題。四旋翼飛機(jī)作為欠驅(qū)動(dòng)系統(tǒng)，運(yùn)行在三維空間。其基本姿態(tài)檢測(cè)量包括俯仰角、翻滾角和偏航角。本文利用HMC5883L和MPU6050構(gòu)成的9軸傳感器完成了原始數(shù)據(jù)獲取工作。由于通過方向余弦算出的俯仰角或者翻滾角等存在噪音，無法直接用于控制系統(tǒng)。文中設(shè)計(jì)了互補(bǔ)數(shù)據(jù)融合算法。仿真及實(shí)際運(yùn)行均證明了該設(shè)計(jì)方案的合理性。相較于卡爾曼濾波，該方法容易理解，更容易在MCU中實(shí)現(xiàn)[10～12]。創(chuàng)新點(diǎn)在于設(shè)計(jì)了四旋翼飛機(jī)姿態(tài)檢測(cè)方案，方案的硬件設(shè)計(jì)緊湊合理；采用了正弦算法計(jì)算俯仰角及翻滾角，方法可直接計(jì)算出對(duì)應(yīng)的角度，且角度正負(fù)直接算出，符合右手定則，方法較方向余弦方便很多；設(shè)計(jì)互補(bǔ)數(shù)據(jù)融和算法，方法簡(jiǎn)單有效，且方便在MCU中實(shí)現(xiàn)。

[1] 劉曉杰.基于視覺的微小型旋翼飛機(jī)位姿估計(jì)研究與實(shí)現(xiàn)[D].長春：吉林大學(xué),2009.

[2] Saad Sardar,Muhammad Bilal Kadri. Autonomous control of a quad-copter via fuzzy gain scheduled PD control[C]∥Proc IEEE 12th international Conference on Frontiers of Information Techno-logy,Computer Society,2014:73-78.

[3] 吳成富,劉小齊,馬松輝,等.四旋翼無人機(jī)建模及其四元數(shù)空置率設(shè)計(jì)[J].飛行力學(xué),2013,31(2):122-125.

[4] 李運(yùn)堂,賈宇寧,王鵬峰,等.基于多傳感器的四旋翼飛行器硬件系統(tǒng)設(shè)計(jì)[J].傳感器與微系統(tǒng),2015,34(2):119-142.

[5] 于雅莉,孫 楓,王元昔.基于多傳感器的四旋翼飛行器硬件電路設(shè)計(jì)[J].傳感器與微系統(tǒng),2011,30(8):113-115,123.

[6] 劉春陽,徐軍領(lǐng),程洪濤,等.MPU9250傳感器的姿態(tài)檢測(cè)與數(shù)據(jù)融合[J].河南科技大學(xué)學(xué)報(bào):自然科學(xué)版,2015,36(4):14-17.

[7] 姜香菊,劉二林.兩輪自平衡機(jī)器人角度檢測(cè)數(shù)據(jù)融合算法[J].計(jì)算機(jī)工程與應(yīng)用,2013,49(8):203-205.

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

[9] 萬曉鳳,康利平，余運(yùn)俊,等.互補(bǔ)濾波算法在四旋翼飛行器姿態(tài)解算中的應(yīng)用[J].測(cè)控技術(shù),2015,34(2):8-211.

[10] 楊凌霄,李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測(cè)方法[J].計(jì)算機(jī)仿真,2014,31(6):406-409.

[11] 劉二林,姜香菊.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測(cè)[J].自動(dòng)化與儀器儀表,2015(2):52-54.

[12] 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法[J].傳感技術(shù)學(xué)報(bào),2012,25(4):524-528.

姜香菊(1979-)，副教授，碩士生導(dǎo)師，主要從事計(jì)算機(jī)控制和檢測(cè)技術(shù)的研究工作。

Design and realization of quad-rotor airplane attitude detection method*

JIANG Xiang-ju1, ZENG You-han1， LIU Er-lin2

(1.School of Automatic & Electrical Engineering,Lanzhou Jiaotong University,Lanzhou 730070,China;2.School of Mechatronic Engineering,Lanzhou Jiaotong University,Lanzhou 730070,China)

In order to solve the problem of quad-rotor airplane attitude detection,a nine-axis sensor are designed based on MPU6050 and HCM5883L.An aircraft body attitude angle calculation method which is based on sine algorithm are proposed.The calculation method based on sine is better than based on cosine.It can gain angle and positive or negative.According to advantages and disadvantages of gyroscope and accelerometer,a complementary data fusion algorithm is designed.The validity of the scheme is proved by the simulation and practice.

quad-rotor airplane; attitude detection; MPU6050; complementary algorithm

10.13873/J.1000—9787(2017)07—0079—04

2016—07—04

甘肅省高等學(xué)?？蒲许?xiàng)目(2014A—041)；校青年基金資助項(xiàng)目(2014034)

TP274

A

1000—9787(2017)07—0079—04