四旋翼飛機姿態檢測方法設計與實現*

姜香菊， 曾幼涵， 劉二林

(1.蘭州交通大學 自動化與電氣工程學院，甘肅 蘭州 730070;2.蘭州交通大學 機電工程學院，甘肅 蘭州 730070)

?

四旋翼飛機姿態檢測方法設計與實現*

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

(1.蘭州交通大學 自動化與電氣工程學院，甘肅 蘭州 730070;2.蘭州交通大學 機電工程學院，甘肅 蘭州 730070)

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

四旋翼飛機; 姿態檢測; MPU6050; 互補算法

0 引 言

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

本文從實際應用的角度出發，通過慣性坐標系和機體坐標系詳細說明了所需檢測的飛機姿態參數，利用正弦算法對數據進行處理；同時使用MPU6050和HMC5883進行了硬件電路設計，并對數據處理算法進行了分析說明。最后通過仿真及實際應用證明了所采用方法的正確性。

1 四旋翼飛機姿態檢測參數分析

四旋翼飛機由檢測裝置，主控裝置，電機驅動裝置以及螺旋槳構成。本文中以“+”型四旋翼飛機為例進行說明。在進行控制理論研究時，一般要建立慣性坐標系和四旋翼飛機機體坐標系。如圖1所示，坐標系OXYZ為慣性坐標系。OXYZ坐標系中，O為地面空間的任意一點，X軸平運行于水平面，方向指向北方；Y軸平行于水平面，方向指向東方；Z軸垂直于OXY平面，方向與重力方向相反。坐標系oxyz為機體坐標系，z軸垂直于飛機的旋翼平面且通過四旋翼飛機的重心，x軸與y軸則分別平行于兩個對稱的螺旋槳的旋轉中心。可以用ξ=(x,y,z)∈R3表示飛機質心的位置矢量。可以用歐拉角η=(ψ,θ,φ)∈R3表示四旋翼飛機的飛行姿態。

圖1 四旋翼飛機結構簡圖及受力分析

2 四旋翼飛機姿態檢測的硬件設計與檢測原理

2.1 四旋翼飛機姿態檢測硬件設計

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

圖2 傳感器硬件電路

2.2 角度檢測基本原理

2.2.1 仰俯角和翻滾角檢測基本原理

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

圖3 MPU6050檢測角度基本原理

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

(1)

由方向余弦公式可得

(2)

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

(3)

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

2.2.2 偏航角檢測基本原理

3 四旋翼飛機姿態檢測軟件設計

對于MPU6050與HMC5883L而言，2只傳感器中的地磁傳感器和加速度傳感器的低頻響應較好，但是高頻響應特性較差。陀螺儀的高頻與低頻特性均較好，但是在實際使用過程中存在溫漂和零漂，所以，必須通過數據的處理來獲取精確的四旋翼飛機的姿態。考慮到在MCU中編程實現的方便性，可以用互補數據融合算法對數據進行處理。

(4)

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

(5)

該算法的數字實現方法為

(6)

4 仿真分析

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

(7)

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

圖4 陀螺儀檢測值直接積分仿真

圖5 數據融合算法仿真

通過仿真結果進行對比可知，數據處理前，角度信號噪聲較大；直接進行積分的信號因為積分誤差逐漸偏離正常值。數據融合后，所得數據可以反映實際信號，效果較好。按照硬件設計方案和數據處理方法，結合傳統PD算法進行飛機姿態控制，實踐表明，該設計方案合理有效。驗證效果見圖6。

圖6 飛機懸浮控制效果

5 結 論

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

[1] 劉曉杰.基于視覺的微小型旋翼飛機位姿估計研究與實現[D].長春：吉林大學,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].飛行力學,2013,31(2):122-125.

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

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

[6] 劉春陽,徐軍領,程洪濤,等.MPU9250傳感器的姿態檢測與數據融合[J].河南科技大學學報:自然科學版,2015,36(4):14-17.

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

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

[9] 萬曉鳳,康利平，余運俊,等.互補濾波算法在四旋翼飛行器姿態解算中的應用[J].測控技術,2015,34(2):8-211.

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

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

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

姜香菊(1979-)，副教授，碩士生導師，主要從事計算機控制和檢測技術的研究工作。

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

甘肅省高等學校科研項目(2014A—041)；校青年基金資助項目(2014034)

TP274

A

1000—9787(2017)07—0079—04