微小型無人機航向姿態(tài)解算研究*

郭志杰， 莊哲民， Alex Noel Joseph Raj， 黃慕斌

(1. 汕頭大學 電子工程系， 廣東 汕頭 515063； 2. 汕頭市杰泰電子科技有限公司， 廣東 汕頭 515041)

0 引 言

無人機(Unmanned Aerial Vehicle, UAV)在飛行過程中， 航向系統(tǒng)起著航向角計算的作用[1]. 航向角的獲取方法有3種， 分別為： ① 無線電航向角獲??； ② 工業(yè)陀螺儀航向角獲??； ③ 磁感器的航向角獲取.

無線電航向角是通過對比機體和信標臺的角度而計算出的機體航向角， 其缺點是易于被其他無線電干擾. 工業(yè)陀螺儀航向角用陀螺的定軸性獲得載體的姿態(tài)角度， 此方法得到的角度雖然精度高， 但是成本也很高[2]. 磁感器也叫磁羅盤或磁力計， 利用地磁場來測量航向. 地磁場是在近地空間連續(xù)分布的矢量場， 在一定的地面空間內(nèi)是相同的， 因此可作為很理想的方位導向基準. 并且磁感器的結(jié)構(gòu)簡單、 體積小、 功耗低， 是一種常見的航向角獲取方法， 在軍事、 民用、 商業(yè)等方面都應用很廣泛.

一般來說， 微小型無人機的航向測量系統(tǒng)中常用陀螺儀輸出角速率積分得到航向姿態(tài)角[3]， 但此方法得到的航向角精度低、 穩(wěn)定性差， 從而導致無人機緩慢自旋. 為了克服上述問題， 微小型無人機的航向系統(tǒng)開始使用磁感器定航向. 此方法成本低、 穩(wěn)定性較高， 但由于磁感器周圍存在著磁性材料， 會對測量產(chǎn)生干擾. 最常見的干擾是軟磁干擾和硬磁干擾[4]， 前者指的是磁感器附近存在軟磁物質(zhì)， 外部磁場作用在軟磁物質(zhì)上， 使得磁感器附近磁場發(fā)生畸變， 從而產(chǎn)生的干擾， 具體表現(xiàn)為磁感器數(shù)據(jù)擬合出來不再是標準圓球； 后者是由硬磁物質(zhì)引起的， 硬磁物質(zhì)會給磁感器帶來一個磁場偏移從而產(chǎn)生干擾， 最終效果就是磁力計數(shù)據(jù)擬合后偏離原心， 因此需要引入磁干擾校正補償[5].

由于磁感器自身工藝、 安裝角度、 測量時的抖動以及上述一系列干擾等因素使得解算出的航向角精度低甚至結(jié)果錯誤. 因此需要一個高精度、 高準確度的磁校準算法對其進行校正， 故本文設計了基于優(yōu)化的HMC5883L和MPU6050融合磁校準算法.

1 微小型無人機航向角校準系統(tǒng)的組成

普通無人機的航向角通常由單個磁力計數(shù)據(jù)計算而來， 但是這樣計算出的航向角精度低， 穩(wěn)定性差. 本文設計的微小型四旋翼無人機航向角校準系統(tǒng)運行流程為：

1) 讓磁感器進行硬磁干擾補償；

2) 讓陀螺儀數(shù)據(jù)進行零點校正；

3) 把經(jīng)過硬磁干擾補償?shù)耐勇輧x3軸數(shù)據(jù)ωx(t),ωy(t),ωz(t)轉(zhuǎn)換到導航坐標系下；

4) 把補償后的磁感器數(shù)據(jù)和陀螺儀數(shù)據(jù)通過互補濾波融合去掉高頻干擾；

5)用文獻[6]得到的Roll和Pitch角計算出機體傾角補償， 進而計算出準確的航向角. 其航向角控制系統(tǒng)方框圖如圖 1 所示.

圖 1 無人機航向角校準系統(tǒng)Fig.1 UAV direction angle calibration system

首先， 將遙控器的校正標志f=1通過2.4GHz無線傳輸傳送到主控芯片STM32F031k6中觸發(fā)硬磁干擾校正， 其次， 將無人機按一定順序旋轉(zhuǎn)， 根據(jù)主控板采集到的30 s數(shù)據(jù)計算出磁力計補償數(shù)據(jù)， 從而對磁力計的輸出進行補償， 得到校正后的磁力計輸出數(shù)據(jù)[7]； 最后把已經(jīng)零點校正后的陀螺儀數(shù)據(jù)通過旋轉(zhuǎn)矩陣轉(zhuǎn)換到導航坐標系下， 利用互補濾波融合磁力計和陀螺儀數(shù)據(jù)得到準確的航向角.

2 基于優(yōu)化的HMC5883L和MPU6050融合航向姿態(tài)解算算法

外部干擾磁場和磁力計誤差是磁力計在測量磁場強度和方向過程中的主要誤差來源[8].

圖 2 硬磁干擾源示意圖Fig.2 Diagram of a hard magnetic interference source

制作材料、 制作工藝會造成磁力計測量偏差， 如零點偏移、 靈敏度等誤差. HMC5883L中設計了自檢電路， 能自動粗校準3軸的輸出靈敏度. 器件里還設計了置位/復位H-橋式電路[9]， 可以消除外界磁場引起的磁感器靈敏度降低問題.

軟磁干擾和硬磁干擾是磁場干擾的主要來源， 其中軟磁干擾的補償不易實現(xiàn)且補償效果較差， 因此通常將磁力計安裝在離無人機4個電機最遠的地方， 以降低磁感器的軟磁干擾. 在微小型四旋翼無人機中軟磁干擾比較小， 一般只需進行硬磁校正補償. 磁感器硬磁干擾源示意圖如圖 2 所示.

對磁感器硬磁干擾補償時全方位采集磁場數(shù)據(jù)的過程如圖 3 所示： 首先將無人機水平放在地上，z軸朝上， 繞z軸水平勻速旋轉(zhuǎn)無人機360°； 再將機體y軸朝下， 繞y軸水平勻速旋轉(zhuǎn)無人機360°， 進行數(shù)據(jù)采集.

圖 3 無人機磁力計數(shù)據(jù)采集過程Fig.3 Data acquisition process of UAV magnetometer

在校正微小型四旋翼無人機硬磁干擾過程中， 無人機主控不斷比較3軸的磁場強度的大小， 最終得到磁感器的3軸數(shù)據(jù)的最大值和最小值， 分別為Xmax,Xmin,Ymax,Ymin,Zmax和Zmin. 通過式(1)獲得3軸的硬磁干擾偏置值.

在采集數(shù)據(jù)的過程中， 因磁力計所處的環(huán)境磁場并不完全均衡， 導致HMC5883L的3個軸輸出靈敏度不會一樣， 所以造成同一個器件的3軸數(shù)據(jù)的輸出值不一樣， 故需要讓輸出數(shù)據(jù)減去不同干擾補償， 如式(2)，Xcal,Ycal,Zcal為補償后的磁場3軸數(shù)據(jù). 當HMC5883L 3軸輸出量中的干擾量得到補償后， 其3軸輸出數(shù)據(jù)擬合后的重心點在原點.

(1)

(2)

(3)

式中： 設L為SWF的窗口隊列長度；wx(t),wy(t),wz(t)分別為加速度計每次輸入SWF的3軸陀螺儀數(shù)據(jù).

(4)

(5)

(6)

(7)

由式(6， 7)可得展開式

(8)

校正后HMC5883L的數(shù)據(jù)準確度高但滯后嚴重. 經(jīng)過零點校正后的陀螺儀數(shù)據(jù)反應靈敏但測量的數(shù)據(jù)隨時間漂移量太大， 如圖 4 所示， 為分別在初始航向30°, 60°, 90°時測得的航向角漂移靜態(tài)測試曲線. 從圖 4 可以看出航向角隨時間的增加而偏移嚴重， 故需要加入兩者互補濾波讓當前校正后磁力計數(shù)據(jù)和陀螺儀數(shù)據(jù)融合得到經(jīng)過校正后的航向角3軸數(shù)據(jù). 得到校正后的數(shù)據(jù)[ψ′,φ′,θ′]與[Xcal,Ycal,Zcal]后， 給兩部分一個權(quán)重a， 得到一個最優(yōu)的近似結(jié)果[X′,Y′,Z′]， 如式(9)所示：

(9)

最終得到經(jīng)過陀螺儀和磁力計互補濾波后的航向3軸數(shù)據(jù)， 將此數(shù)據(jù)輸入式(10)計算出航向角. 其中X′,Y′,Z′為從本文設計的無人機航向角校準系統(tǒng)中輸出的數(shù)據(jù)，φ,θ分別為無人機的橫滾角和俯仰角， 由論文[6]可得到.Xh,Yh為求航向角ψ產(chǎn)生的中間變量.

(10)

圖 4 陀螺儀航向漂移靜態(tài)測試Fig.4 Static test of gyro heading drift

磁偏角是指地磁北極與高德地圖的地理北極所產(chǎn)生的一個角度偏差. 物體的磁偏角大小和方向由物體在地球表面的空間決定. 磁針指北極N向東偏則磁偏角為正， 向西偏則磁偏角為負[13]， 故需要在求出的航向角ψ中減去磁偏角得到準確航向角ψ′， 如式(11)所示.

ψ′=ψ-Magnetic Declinaition.

(11)

考慮到航向角象限的問題， 當ψ′<0時補償360°， 如式(12). 設計的整個系統(tǒng)采樣周期為6 ms， 并且UAV航向角的范圍為： 0°～ 360°.

ψ′=ψ′+360 (ψ′<0).

(12)

3 實驗結(jié)果

由于微小型四旋翼無人機需要考慮成本和功耗， 故硬件上用HoneyWell公司的磁感器HMC5883L進行磁場測量， 傳感器電路圖如圖 5 所示， 其中2.2 K兩個電阻為I2C總線的上拉電阻； C1， C3兩個電容為濾波電容， 起保護芯片的作用.

圖 5 HMC5883L電路圖Fig.5 HMC5883L circuit diagram

角度測量使用InvenSense公司的低功耗6軸傳感器MPU6050， 使用低功耗MCU STM32f031k6作為磁感器、 陀螺儀3軸數(shù)據(jù)采集、 加速度計和濾波等的主控芯片. 用Beken公司的2.4 GHz傳輸芯片 BK2423進行遙控數(shù)據(jù)傳輸， 其功耗低(供電1.9～2.6V)， 可提供最大8 MHz的時鐘速率的4腳SPI接口， 短距離穩(wěn)定性強、 抗干擾能力強且通訊速率高， 滿足微小型四旋翼無人機實時傳輸?shù)囊? 設計的無人機所用電池為800 mAh， 滿電電壓為4.2 V.

從圖 6 可以看出： 微小型無人機飛行控制的硬件平臺包括低成本、 低功耗的6軸傳感器MPU6050、 微控制單元(Microprogrammed Control Unit， MCU)STM32f0， 2.4 GHz模塊、 磁力計模塊HMC5883L和氣壓計模塊SPL06-001等.

圖 6 無人機飛行控制系統(tǒng)硬件圖Fig.6 Hardware diagram of UAV flight control system

在沒有金屬鋼架結(jié)構(gòu)的地方進行實驗， 圖 7 為采集到的7 000個3軸磁場數(shù)據(jù)， 由上述數(shù)據(jù)擬合而成的半球體， 球的中心不在(0， 0， 0)點. 根據(jù)式(1)計算補償數(shù)據(jù)， 再根據(jù)式(2)對原始數(shù)據(jù)進行補償， 采集干擾補償后的數(shù)據(jù)， 通過Matlab擬合3軸數(shù)據(jù)可得到圖 8. 從圖 8 中可以看出： 擬合出的曲線為一個球形， 球體球心與坐標原點(0， 0， 0)重合[14]， 此時磁場的補償工作順利完成.

圖 7 原始磁場數(shù)據(jù)三維圖Fig.7 The three-dimensional map of the original magnetic field data

圖 8 補償后磁場數(shù)據(jù)三維圖Fig.8 The three-dimensional map of magnetic field data after compensation

當磁力計補償完成后， 讓無人機飛起來， 在空中懸停， 用串口USART通過藍牙將數(shù)據(jù)傳送到上位機， 然后將數(shù)據(jù)保存下來. 采集3組8 000個數(shù)據(jù)， 如圖 9 所示， 陀螺儀計算出的航向角漂移太嚴重， 大約漂移速率為0.187 5 °/s， 這會讓無人機在定高時自旋、 由磁力計測出的數(shù)據(jù)精度低. 本文算法得到的航向角較磁力計測出的航向角精度提升50%.

讓無人機繼續(xù)定于空中懸停， 用遙控器操控， 讓其先向負方向自旋轉(zhuǎn)30°左右， 一段時間后， 將其航向回歸初始位置. 同上， 將數(shù)據(jù)傳送到上位機然后將數(shù)據(jù)保存下來， 采集3組5 800個數(shù)據(jù)， 如圖 10 所示， 陀螺儀計算出的航向角隨著自旋轉(zhuǎn)角度繼續(xù)漂移， 且存在少量角度超調(diào)； 由磁力計測出的數(shù)據(jù)存在較大超調(diào)量， 這將直接導致無人機航向抖動； 本文算法得到的航向角超調(diào)較小， 能保證微小型無人機航向穩(wěn)定.

綜上所述， 采用本文方法對微小型無人機航向定航時， 算出的航向角數(shù)據(jù)精度高、 穩(wěn)定性強， 在玩具無人機定航中具有較好的優(yōu)勢. 其原因在于：

1) 通過“最大最小值”法去除HMC5883L的硬磁干擾；

2) 采用互補濾波將兩者數(shù)據(jù)進行融合， 把陀螺儀數(shù)據(jù)的高頻性、 實時性與磁力計的準確性、 穩(wěn)定性通過一個權(quán)重a相結(jié)合， 從而保證了航向角度具有很好的穩(wěn)定性、 實時性和準確性.

圖 9 微小型無人機懸停時航向角對比曲線Fig.9 Comparison curves of heading angle for micro UAV hovering

4 結(jié) 論

本文針對微小型四旋翼無人機在定航時由于計算出的航向角準確度低導致機體微自旋的缺點， 設計了基于優(yōu)化的HMC5883L和MPU6050融合航向姿態(tài)解算新方法. 實驗結(jié)果表明： 本文提出的磁校準算法能準確去除硬磁干擾且能結(jié)合陀螺儀和磁力計兩者的優(yōu)點， 能輸出實時性高、 誤差小、 穩(wěn)定性強的航向角， 有效保證了微小型四旋翼無人機的航向穩(wěn)定.