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

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

0 引 言

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

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

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

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

1.1 原理簡述

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

圖1 十字型飛行方式動力學(xué)模型

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

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

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

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

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

1.2 平臺搭建

本研究采用了自行設(shè)計的一款小型四軸飛行器，整體結(jié)構(gòu)如圖2所示。

圖2 結(jié)構(gòu)框圖

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

圖3 四軸實物圖

1.3 MPU6050簡述

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

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

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

2 姿態(tài)的表示

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

2.1 歐拉角法

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

圖4 機體坐標(biāo)系與地理坐標(biāo)系的轉(zhuǎn)換

坐標(biāo)系轉(zhuǎn)換過程中按照Z—Y—X的旋轉(zhuǎn)順序得到旋轉(zhuǎn)矩陣如下式所示：

(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 四元數(shù)法

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

(1)表示旋轉(zhuǎn)的四元數(shù)

通過旋轉(zhuǎn)軸和繞該旋轉(zhuǎn)軸旋轉(zhuǎn)的角度可以構(gòu)造一個四元數(shù)[6-8]。

(2)

且：

其中：

(2)四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣

(3)

從而得到四元數(shù)與歐拉角的關(guān)系如下式所示：

(4)

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

3 姿態(tài)的解算

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

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

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

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

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

圖5 Mahony互補濾波算法程序?qū)崿F(xiàn)流程圖

(5)

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

(6)

(7)

(8)

通過調(diào)整Kp,Ki兩個參數(shù)，能有效地通過加速計數(shù)據(jù)快速修正陀螺儀數(shù)據(jù)。

然后通過四元數(shù)微分方程，對修正后的陀螺儀數(shù)據(jù)積分，得到當(dāng)前的四元數(shù)表示。結(jié)果如下：

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個控制周期。

最后進(jìn)行四元數(shù)的歸一化處理：

(10)

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

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

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

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

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

圖6 基于DMP的硬件姿態(tài)解算算法程序?qū)崿F(xiàn)流程圖

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

本研究對讀取得到的四元數(shù)進(jìn)行歸一化處理：

(11)

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

(12)

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

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

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

圖7 俯仰角測試數(shù)據(jù)曲線

圖8 橫滾角測試數(shù)據(jù)曲線

圖9 偏航角數(shù)據(jù)曲線

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

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

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

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

5 結(jié)束語

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

結(jié)果表明，硬件姿態(tài)解算能實時準(zhǔn)確解算出四軸飛行器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] 張廣玉,張洪濤,李隆球,等.四旋翼微型飛行器設(shè)計[J].哈爾濱理工大學(xué)學(xué)報,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)解算算法設(shè)計與仿真[J].科技視界,2016(23):17-18.

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

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

[8] 楊天標(biāo).剛體運動的四元素表示[J].德州學(xué)院學(xué)報,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)解算中的應(yīng)用[J].測控技術(shù),2015,34(2):8-11.