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

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

0 引 言

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

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

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

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

1 微小型無(wú)人機(jī)航向角校準(zhǔn)系統(tǒng)的組成

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

1) 讓磁感器進(jìn)行硬磁干擾補(bǔ)償；

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

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

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

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

圖 1 無(wú)人機(jī)航向角校準(zhǔn)系統(tǒng)Fig.1 UAV direction angle calibration system

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

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

外部干擾磁場(chǎng)和磁力計(jì)誤差是磁力計(jì)在測(cè)量磁場(chǎng)強(qiáng)度和方向過(guò)程中的主要誤差來(lái)源[8].

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

制作材料、 制作工藝會(huì)造成磁力計(jì)測(cè)量偏差， 如零點(diǎn)偏移、 靈敏度等誤差. HMC5883L中設(shè)計(jì)了自檢電路， 能自動(dòng)粗校準(zhǔn)3軸的輸出靈敏度. 器件里還設(shè)計(jì)了置位/復(fù)位H-橋式電路[9]， 可以消除外界磁場(chǎng)引起的磁感器靈敏度降低問(wèn)題.

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

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

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

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

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

(1)

(2)

(3)

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

(4)

(5)

(6)

(7)

由式(6， 7)可得展開(kāi)式

(8)

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

(9)

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

(10)

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

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

ψ′=ψ-Magnetic Declinaition.

(11)

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

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

(12)

3 實(shí)驗(yàn)結(jié)果

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

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

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

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

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

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

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

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

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

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

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

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

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

圖 9 微小型無(wú)人機(jī)懸停時(shí)航向角對(duì)比曲線Fig.9 Comparison curves of heading angle for micro UAV hovering

4 結(jié) 論

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