基于MPU6050的四軸硬件姿態解算研究*

2018-01-24 11:20:46陳國定周鵬豪胡朕豪湯粵生秦志飛

機電工程 2018年1期

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

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

0 引言

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

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

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

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

1.1 原理簡述

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

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

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

(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為姿態傳感器，氣壓計MS5611用于定高，藍牙與無線模塊分別用于調試與遙控，執行單元采用小而輕便、控制特性良好的空心杯電機，電源模塊采用多路獨立電源，以上各個模塊都集成在不到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 姿態的表示

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

2.1 歐拉角法

根據歐拉轉動定理，剛體做定點轉動的空間位置需要歐拉角α、β、γ，3個獨立的變量來確定，機體坐標系與地理坐標系的轉換如圖4所示。通過三次旋轉即可使機體坐標旋轉到與地理坐標系重合，三次坐標變換的旋轉矩陣的乘積即為歐拉角姿態矩陣[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 四元數法

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

(1)表示旋轉的四元數

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

(2)

且：

其中：

(2)四元數轉旋轉矩陣

(3)

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

(4)

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

3 姿態的解算

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

3.1 軟件姿態解算

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

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

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

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

(5)

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

(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)便可得到控制飛行器所需要的當前姿態的歐拉角。

3.2 硬件姿態解算

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

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

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

圖6 基于DMP的硬件姿態解算算法程序實現流程圖

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

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