基于MPU6050的四軸硬件姿態(tài)解算研究*

陳國定，周鵬豪，胡朕豪，湯粵生，秦志飛

(浙江工業(yè)大學 信息工程學院，浙江 杭州 310023)

0 引 言

四軸飛行器是多軸飛行器中結構最為簡單、控制最為靈活、應用最為廣泛的一種飛行器。如何準確實時獲取飛行器的姿態(tài)信息是四軸飛行器控制的關鍵。姿態(tài)解算的核心在于旋轉，對于飛行器來說，一般采用四元數來表示旋轉，在獲得四元數之后，會將其轉化為歐拉角，然后輸入到姿態(tài)控制算法中。在四軸飛行器的系統(tǒng)設計里，利用慣性元件(如陀螺儀)，進行軟件姿態(tài)解算是最為普遍的形式，也是四軸控制中最為復雜的部分，常見的一些姿態(tài)解算以及誤差補償的算法有互補濾波算法、梯度下降算法和卡爾曼濾波算法，這些算法都有了比較成熟的研究與應用[1-2]。

然而隨著傳感技術的發(fā)展，MPU6050出現了硬件DMP直接讀取四元數的方法，即MPU6050內置了一個功能模塊，能夠把原始數據進行校正處理后輸出。DMP讀取的數據無法跟蹤飛行器劇烈的姿態(tài)變化，而且有些時候還會出現突變甚至卡死現象。

本研究將搭建基于MPU6050的四軸飛行器，并對其進行軟件姿態(tài)解算和硬件姿態(tài)解算，對比分析說明硬件姿態(tài)解算的可行性。

1 飛行原理分析與平臺搭建

1.1 原理簡述

四軸飛行器通常具有兩種不同的飛行方式：十字型與X型[3]。對于十字型飛行方式，飛行器姿態(tài)改變方向與機身相同，電機分布在機體坐標系的坐標軸上，方便理論分析與控制。而X型飛行方式，飛行器姿態(tài)改變方向與機身成45°，控制的靈敏度與穩(wěn)定性更好，但其物理模型較為復雜。本文以十字型飛行方式為例進行分析，其動力學模型如圖1所示。

圖1 十字型飛行方式動力學模型

四軸飛行器飛行的姿態(tài)控制主要包括俯仰角控制、橫滾角控制、偏航角控制、高度控制，通過調整分布于機體的4個電機轉速(ν1、ν2、ν3、ν4)來實現姿態(tài)變換。

(1)俯仰控制。飛行器繞著Y軸方向旋轉時，所進行的控制為俯仰控制，ν2減小，ν4增加，飛行器左傾，反之右傾；

(2)橫滾控制。飛行器繞著X軸方向旋轉時，所進行的控制為橫滾控制，ν1減小，ν3增加，飛行器前傾，反之則后傾；

(3)航向控制。ν1與ν2同時減小，ν2與ν4同時增加，飛行器左旋，反之則右旋；

(4)高度控制。ν1=ν2=ν3=ν4，當其同時加速時，電機升力變大，當升力大于飛行器重力時，飛行器擁有向上運動的加速度；當其同時減速時，電機升力變小，當升力小于飛行器重力時，飛行器擁有向下運動的加速度。

1.2 平臺搭建

本研究采用了自行設計的一款小型四軸飛行器，整體結構如圖2所示。

圖2 結構框圖

本研究以STM32f103vet6為主控芯片，MPU6050、HMC5883為姿態(tài)傳感器，氣壓計MS5611用于定高，藍牙與無線模塊分別用于調試與遙控，執(zhí)行單元采用小而輕便、控制特性良好的空心杯電機，電源模塊采用多路獨立電源，以上各個模塊都集成在不到10 cm×10 cm的飛控PCB板上，具體實物如圖3所示。

圖3 四軸實物圖

1.3 MPU6050簡述

MPU6050是9軸運動處理傳感器，它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴展的數字運動處理器DMP。

MPU6050對陀螺儀和加速度計分別用了3個16位的ADC，將其測量的模擬量轉化為可輸出的數字量。傳感器的測量范圍都是用戶可控的，陀螺儀可測范圍為±250°/s，±500°/s，±1 000 °/s，±2 000°/s，加速度計可測范圍為±2g，±4g，±8g，±16g。產品輸出可通過最高至400 kHz的I2C接口實現。

數字運動處理器(DMP)可減少復雜的融合演算數據、感測器同步化、姿勢感應等的負荷。運動處理數據庫支持Android、Linux與Windows內建之運作時間偏差與磁力感測器校正演算技術，免除了客戶另外校正的需求[4]。

2 姿態(tài)的表示

剛體姿態(tài)描述的是剛體坐標系與參考坐標系的角位置關系。姿態(tài)解算的核心在于旋轉，四軸飛行器中一般采用四元數來保存飛行器的姿態(tài)，在需要控制的時候，會將四元數轉化為歐拉角，然后輸入到姿態(tài)控制算法中。

2.1 歐拉角法

根據歐拉轉動定理，剛體做定點轉動的空間位置需要歐拉角α、β、γ，3個獨立的變量來確定，機體坐標系與地理坐標系的轉換如圖4所示。通過三次旋轉即可使機體坐標旋轉到與地理坐標系重合，三次坐標變換的旋轉矩陣的乘積即為歐拉角姿態(tài)矩陣[5]。

圖4 機體坐標系與地理坐標系的轉換

坐標系轉換過程中按照Z—Y—X的旋轉順序得到旋轉矩陣如下式所示：

(1)

其中：

A=sinγsinβcosα-cosγsinα，
B=sinγsinβcosα+cosγcosα，
C=cosγsinβcosα+sinγsinα，
D=cosγsinβsinα-sinγcosα。

2.2 四元數法

四元數法是利用一種超復數的乘法來等效反映姿態(tài)變換的方法。相比歐拉角法，可以減少大量的計算量，提高姿態(tài)解算速度。

(1)表示旋轉的四元數

通過旋轉軸和繞該旋轉軸旋轉的角度可以構造一個四元數[6-8]。

(2)

且：

其中：

(2)四元數轉旋轉矩陣

(3)

從而得到四元數與歐拉角的關系如下式所示：

(4)

從旋轉矩陣看出，采用歐拉角描述姿態(tài)矩陣，計算過程中三角函數多，造成運算量較大，為降低姿態(tài)解算復雜度，達到實時解算飛行姿態(tài)，采用四元數表示可以帶來極大方便。

3 姿態(tài)的解算

在該系統(tǒng)中，本研究根據MPU6050提供的數據，分析得到當前姿態(tài)四元數表示，進一步轉化成歐拉角，輸入到姿態(tài)控制器。軟件姿態(tài)解算就是通過MPU6050得到的陀螺儀，加速度計和磁力計的原始數據，通過程序得到飛行器當前姿態(tài)的四元數；硬件姿態(tài)解算就是通過MPU6050的DMP直接讀取飛行器當前姿態(tài)的四元數。

3.1 軟件姿態(tài)解算

MPU6050集成了3軸MEMS陀螺儀和3軸MEMS加速度計，互補濾波算法就是分別利用加速度計和陀螺儀的頻率響應優(yōu)勢[9-11]，對傳感器測量的原始數據進行互相彌補，來提高解算的姿態(tài)精確度。

對于加速度計來說，它的靜態(tài)特性好，動態(tài)特性較差；而陀螺儀存在溫漂和零漂，因此在低頻段動態(tài)響應特性差而高頻動態(tài)響應特性好。因此，對陀螺儀測量的數據進行高通濾波，對加速度計測量的數據進行低通濾波，然后通過互補，實現在整個頻域上表現良好的動態(tài)響應特性。

在四軸飛行器上實現Mahony互補濾波算法的程序流程圖如圖5所示。

圖5 Mahony互補濾波算法程序實現流程圖

(5)

由于上一次姿態(tài)解算的姿態(tài)可以簡單認為是陀螺儀積分，推算出來的重力向量ν可以看作陀螺儀積分后的姿態(tài)。

(6)

(7)

(8)

通過調整Kp,Ki兩個參數，能有效地通過加速計數據快速修正陀螺儀數據。

然后通過四元數微分方程，對修正后的陀螺儀數據積分，得到當前的四元數表示。結果如下：

qω(n)=qω(n-1)+(-qxgx-qygy-qzgz)T
qx(n)=qx(n-1)+(qωgx+qygz-qzgy)T
qy(n)=qy(n-1)+(-qωgy-qxgz+qzgx)T
qz(n)=qz(n-1)+(qωgz+qxgy-qygx)T

(9)

式中：T—控制周期；n—第n個控制周期。

最后進行四元數的歸一化處理：

(10)

利用式(4)便可得到控制飛行器所需要的當前姿態(tài)的歐拉角。

3.2 硬件姿態(tài)解算

MPU6050硬件上集成了一個可擴展的數字運動處理器DMP，它可以融合來自陀螺儀、加速度計的數據，并通過自身的IIC總線向磁力計讀取數據，并進行融合，基于硬件電路的處理計算出準確的四元數，處理結果可以從DMP寄存器讀出。

基于DMP實現的硬件姿態(tài)解算簡化了程序代碼設計，單片機只有在DMP解算完成的外部中斷來臨時，才會去讀取結果，硬件解算的時間可以用來處理其他任務。

在四軸飛行器上實硬件姿態(tài)解算算法的程序流程圖如圖6所示。

圖6 基于DMP的硬件姿態(tài)解算算法程序實現流程圖

本研究在STM32芯片上初始化IIC模塊，通過IIC總線配置DMP模塊的寄存器，設置好DMP的濾波器截止頻率和工作頻率，并使DMP能周期地從MPU6050中讀取陀螺儀和加速度計的數據，處理運動姿態(tài)，將結果保存在DMP寄存器中或者FIFO中，同時生成外部中斷。主程序收到外部中斷后，通過IIC直接從DMP寄存器或者通過FIFO緩沖讀取飛行器姿態(tài)的四元數表示。

本研究對讀取得到的四元數進行歸一化處理：

(11)

然后通過歐拉角與四元數的關系轉化可得到控制飛行器所需要的當前姿態(tài)的歐拉角：

(12)

4 軟硬姿態(tài)解算的對比分析

本研究先在飛行器上測試了軟件姿態(tài)解算算法。

首先比較飛行器的軟、硬件姿態(tài)解算的實際解算出的飛行器姿態(tài)(歐拉角)的情況，實際測試過程通過藍牙串口，將解算的數據發(fā)送到到上位機，并通過Matlab畫出兩者的數據曲線，比較結果如圖(7～9)所示。

圖7 俯仰角測試數據曲線

圖8 橫滾角測試數據曲線

圖9 偏航角數據曲線

實驗結果發(fā)現，硬件姿態(tài)解算的俯仰角、橫滾角和偏航角與軟件解算大致相同，誤差很小，在容許誤差范圍之內。然后對飛行器在不同姿態(tài)時，進行讀取軟、硬件解算的解算時間，如表1所示。

表1 軟、硬件解算的不同姿態(tài)解算時間

實驗通過對比分別在水平放置、機身向前傾、機身向左傾3種姿態(tài)的軟、硬件姿態(tài)解算時間，結果表明，兩種姿態(tài)解算方法都能在極短時間完成解算，且硬件解算所花時間相對更短。

最后本研究對硬件姿態(tài)解算進行了飛行測試，采用硬件姿態(tài)解算，飛行器機身的反應反而更靈活。

5 結束語

本研究介紹了以Mahony互補濾波算法為代表的軟件解算算法，并與硬件姿態(tài)解算進行了分析和比較。采用MPU6050的DMP硬件姿態(tài)解算方式能很好地融合姿態(tài)數據，對加速度計的高頻抖動和陀螺儀的低頻噪聲有一定的濾除效果。實驗采用了將軟件姿態(tài)解算與硬件姿態(tài)解算進行對比來分析硬件姿態(tài)解算的性能。

結果表明，硬件姿態(tài)解算能實時準確解算出四軸飛行器3個軸的姿態(tài)變化，并能及時跟蹤四軸飛行器的姿態(tài)變化，硬件姿態(tài)解算能滿足小型四軸飛行器的姿態(tài)控制要求。

[1] 王曉初,盧 琛.四旋翼姿態(tài)解算算法的對比與研究[J].制造業(yè)自動化,2015,37(1):120-122.

[2] 王大鵬，王茂森，戴勁松，等.囚旋翼飛行器懸停建模及控制[J].兵工自動化，2017，36(5)：92-96.

[3] 張廣玉,張洪濤,李隆球,等.四旋翼微型飛行器設計[J].哈爾濱理工大學學報,2012,17(3):110-114.

[4] LEEDY Li. MPU-6000.6050中文資料[EB/OL]. http://wenku.baidu.com/link?url=ayXQoKndQHWukFbb_sVahdjzu2vuqzjcYZsdPjvTd02OU1Z_Gw0ZVSMfToo6dEs4F

OaJyEWnUbo9h3NUH6oW3BPZ29XSz9mui7JgHNRa8Jm.

[5] 徐云川.四軸飛行器姿態(tài)解算算法設計與仿真[J].科技視界,2016(23):17-18.

[6] 王美山,李文亮,楊傳路,等.繞任意軸μ旋轉φ對應的歐拉角的新求解公式[J].大學物理,2006,25(9):31-33.

[7] 冀 亮,錢正洪,白茹.基于四元數的四軸無人機姿態(tài)的估計和控制[J].現代電子技術,2015,38(11):112-116.

[8] 楊天標.剛體運動的四元素表示[J].德州學院學報,2010,26(2):5-11.

[9] 梁延德,程 敏,何福本,等.基于互補濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,30(11):56-58.

[10] 路 洋.軟件姿態(tài)解算[EB/OL].http://www.crazepony.com/wiki/software-algorithm.html.

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