胡佳興，魏延輝，劉 鑫，李光春

（哈爾濱工程大學(xué) 自動化學(xué)院，哈爾濱 150001）

捷聯(lián)姿態(tài)系統(tǒng)采用慣性敏感器件MEMS三軸加速度計(jì)，三軸陀螺儀和三軸地磁傳感器進(jìn)行組合，構(gòu)成微慣性測量單元MIMU，其應(yīng)用效果與微機(jī)電MEMS傳感器的性能息息相關(guān)[1]。相比于傳統(tǒng)的捷聯(lián)姿態(tài)系統(tǒng)，基于MEMS傳感器的捷聯(lián)姿態(tài)系統(tǒng)具有成本低廉、體積微型化、低功耗、質(zhì)量可靠和優(yōu)秀的抗沖擊能力等特性[2]。

目前MEMS捷聯(lián)姿態(tài)系統(tǒng)已經(jīng)成為近年來國內(nèi)外的研究熱點(diǎn)之一，文獻(xiàn)[3]中結(jié)合異構(gòu)數(shù)據(jù)融合算法，采用互補(bǔ)濾波器實(shí)現(xiàn)了基于MEMS慣性器件的行人測量單元；文獻(xiàn)[4]中應(yīng)用MEMS慣性傳感器，采用卡爾曼濾波器獲得了人體部分的準(zhǔn)確姿態(tài)估計(jì)；文獻(xiàn)[5]中基于互補(bǔ)濾波器設(shè)計(jì)的MEMS/GPS/地磁組合導(dǎo)航系統(tǒng)，具有較高的姿態(tài)、速度和位置精度；文獻(xiàn)[6]中提出了一種新的閉環(huán)姿態(tài)補(bǔ)償算法進(jìn)行MEMS慣性器件和GPS的組合導(dǎo)航，相比于傳統(tǒng)方案有效抑制了慣導(dǎo)系統(tǒng)的誤差積累；文獻(xiàn)[7]中采用改進(jìn)的自適應(yīng)擴(kuò)展卡爾曼算法應(yīng)用于多旋翼無人機(jī)的姿態(tài)解算，具有較為優(yōu)秀的動態(tài)性能。因此，隨著MEMS慣性器件技術(shù)的不斷發(fā)展，基于MEMS的捷聯(lián)姿態(tài)系統(tǒng)將會有更加廣闊的應(yīng)用前景。

本文以MEMS陀螺加速度計(jì)和MEMS電子羅盤傳感器組建捷聯(lián)姿態(tài)系統(tǒng)，并對其誤差建模和標(biāo)定方法進(jìn)行研究。

1 MEMS捷聯(lián)姿態(tài)系統(tǒng)的設(shè)計(jì)

1.1 系統(tǒng)總體設(shè)計(jì)

本系統(tǒng)采用基于ARM-CortexM4內(nèi)核的MK10-DN512VLK10微控制器為核心，采用MEMS傳感器MPU6050和HMC5883L作為慣性傳感器，利用液晶作為人機(jī)交互界面，通過藍(lán)牙實(shí)現(xiàn)與上位機(jī)的信息通訊，完成系統(tǒng)的姿態(tài)測量和數(shù)據(jù)通訊、采集的功能。系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)簡圖Fig.1 System structure diagram

基于MEMS的捷聯(lián)姿態(tài)系統(tǒng)由于傳感器精度較低的缺點(diǎn)無法敏感到地球自轉(zhuǎn)角速度和因地球自轉(zhuǎn)而產(chǎn)生的哥氏加速度分量[3]。因此若利用該系統(tǒng)實(shí)現(xiàn)位置、速度的數(shù)據(jù)解算在短時(shí)間之內(nèi)就會積累較大的誤差，無法進(jìn)行實(shí)際應(yīng)用。因此該系統(tǒng)在沒有GPS、WiFi等參考信息輔助定位的情況下，只能夠?qū)崿F(xiàn)較為精確的載體姿態(tài)以及航向角的計(jì)算。

由于MEMS陀螺儀存在漂移的問題，在進(jìn)行姿態(tài)估計(jì)中，隨著時(shí)間的推進(jìn)積累誤差會不斷增大，從而導(dǎo)致捷聯(lián)姿態(tài)系統(tǒng)的姿態(tài)發(fā)散。因此，為了使捷聯(lián)姿態(tài)系統(tǒng)穩(wěn)定可用，需要與其它姿態(tài)傳感器（加速度計(jì)、電子羅盤）組合使用，形成組合導(dǎo)航系統(tǒng)，才能夠使獲取到的姿態(tài)信息穩(wěn)定可靠。捷聯(lián)姿態(tài)系統(tǒng)的結(jié)構(gòu)流程圖如圖2所示。

圖2 捷聯(lián)姿態(tài)系統(tǒng)的結(jié)構(gòu)流程圖Fig.2 Flow chart of the strapdown attitude system

1.2 MEMS慣性傳感器

采用將加速度計(jì)和陀螺儀集成到一個(gè)芯片的器件作為傳感器。Invensense公司的MPU6050是全球第一個(gè)整合型六軸運(yùn)動處理組件，如圖3所示，由于其將加速度計(jì)和陀螺儀整合到了一個(gè)器件之中，所以有效地減少了器件安裝時(shí)的誤差。同時(shí)MPU6050能夠?qū)崿F(xiàn)加速度計(jì)和陀螺儀多個(gè)量程配置，能夠滿足捷聯(lián)姿態(tài)系統(tǒng)的需求。

圖3 MPU6050系統(tǒng)圖Fig.3 MPU6050 system diagram

1.3 MEMS三軸電子羅盤

采用霍尼韋爾公司的HMC5883L三軸電子羅盤芯片，該傳感器為I2C通訊協(xié)議，應(yīng)用于低成本羅盤和磁場檢測領(lǐng)域。其內(nèi)部集成了HMC118X系列的高分辨率磁阻傳感器，并具有高精度信號放大、自動消磁和偏差校準(zhǔn)功能，能使羅盤精度控制在1°～2°。因此該芯片被廣泛應(yīng)用于移動電話、筆記本電腦、消費(fèi)電子、汽車導(dǎo)航系統(tǒng)和個(gè)人導(dǎo)航系統(tǒng)。HMC5883L的原理圖如圖4所示。

圖4 HMC5883系統(tǒng)圖Fig.4 HMC5883 system diagram

2 系統(tǒng)傳感器輸出數(shù)據(jù)的標(biāo)定方法

2.1 加速度計(jì)誤差模型及標(biāo)定方法

加速度計(jì)精度較低，其誤差模型為

式中：載體坐標(biāo)系下加速度矢量的投影分量為vx、vy、vz；加速度計(jì)多次采樣的均值為 Vx、Vy、Vz；加速度計(jì)零偏為 Vx0、Vy0、Vz0；標(biāo)度因數(shù)為 Skx、Sky、Skz；器件安裝的誤差系數(shù)為 Kxy、Kxz、Kyx、Kyz、Kzx、Kzy。

選用的MEMS加速度計(jì)為六軸MEMS傳感器MPU6050中的三軸加速度計(jì)。量程配置為±8 g，標(biāo)度因數(shù)為4096 LSB/g。因其精度較低，可以忽略因地球自轉(zhuǎn)產(chǎn)生的哥氏加速度對加速度計(jì)敏感軸的分量[8]。

將捷聯(lián)姿態(tài)系統(tǒng)固定于轉(zhuǎn)臺上，轉(zhuǎn)動2次轉(zhuǎn)臺進(jìn)行2次測量，得到加速度計(jì)X軸天向、地向的實(shí)際測量輸出 VX+、VY+、VZ+與 VX-、VY-、VZ-。假設(shè) α 為重力加速度在載體系的投影向量，g為當(dāng)?shù)氐闹亓铀俣?，則α的表達(dá)形式在X軸天向時(shí)為［g 0 0］，X軸地向時(shí)為［-g 0 0］。

由加速度計(jì)的誤差模型可以得到Y(jié)、Z軸加速度計(jì)測量值、零偏和系統(tǒng)安裝誤差之間的關(guān)系，即：

加速度計(jì)安裝誤差系數(shù)，可以由系統(tǒng)2次放置方式的平均值求差得到，即：

將式（1）展開，可得加速度計(jì)X軸均值與其它量的關(guān)系，即：

由式（4）可以得到X軸加速度計(jì)標(biāo)度因數(shù)與零點(diǎn)偏置的求取方法，即：

若求取加速度計(jì)Y、Z軸的標(biāo)度因數(shù)和零點(diǎn)偏置，只需要在轉(zhuǎn)臺上以 α＝［0 g 0］、α＝［0-g 0］、α＝［0 0 g］、α＝［0 0-g］4 個(gè)位置進(jìn)行放置采集數(shù)據(jù)，通過計(jì)算即可獲取。計(jì)算方法為

在系統(tǒng)進(jìn)行轉(zhuǎn)臺實(shí)驗(yàn)時(shí)，首先調(diào)整系統(tǒng)的標(biāo)定軸位置使其與轉(zhuǎn)臺軸平行，之后將其固定在轉(zhuǎn)臺的內(nèi)框平臺上，通過轉(zhuǎn)臺轉(zhuǎn)動6個(gè)位置采集數(shù)據(jù)完成加速度計(jì)的標(biāo)定。

2.2 陀螺儀誤差模型及標(biāo)定方法

采用的MEMS陀螺儀測量精度較低，因此不需要考慮交叉二次誤差項(xiàng)對模型的影響[9]。陀螺儀的誤差模型為

式中：陀螺儀多次采樣的平均值為 Wx、Wy、Wz；陀螺儀零偏為 Wx0、Wy0、Wz0；標(biāo)度因數(shù)為 Skx、Sky、Skz；安裝誤差系數(shù)為 Kxy、Kxz、Kyx、Kyz、kzx、kzy；載體坐標(biāo)系下輸入角速率的投影分量為 ωx、ωy、ωz。

MEMS陀螺儀為六軸MEMS傳感器MPU6050中的三軸陀螺。其最大量程為±2000°/s，此時(shí)陀螺的標(biāo)度因數(shù)約為 16.4 LSB·（°/s）-1。地球自轉(zhuǎn)角速度約為 15.04°/h，即 0.00417°/s。通過參數(shù)的對照可以說明該MEMS陀螺的精度無法測量到地球自轉(zhuǎn)產(chǎn)生的角速度，因此，在標(biāo)定過程中，地球自轉(zhuǎn)角速度對陀螺角速度輸出的影響可以忽略不計(jì)。

將捷聯(lián)姿態(tài)系統(tǒng)固定于轉(zhuǎn)臺上，令ω為載體坐標(biāo)系相對于地理坐標(biāo)系的角速率在載體坐標(biāo)系的投影向量，令三軸轉(zhuǎn)臺分別以ω＝［ω10 0］、ω＝［-ω10 0］的參數(shù)進(jìn)行旋轉(zhuǎn)，轉(zhuǎn)動2次轉(zhuǎn)臺進(jìn)行2次測量，得到MEMS陀螺儀輸出值Wx+、Wy+、Wz+與Wx-、Wy-、Wz-。

由陀螺儀的誤差模型可以得到，Y、Z軸陀螺儀測量值、零偏和系統(tǒng)安裝誤差之間的關(guān)系，即：

陀螺的安裝誤差系數(shù)，可以由系統(tǒng)2次放置方式的平均值求差得到，即：

將式（11）展開，可得加速度計(jì)X軸均值與其它量的關(guān)系，即：

由式（12）可以得到X軸陀螺標(biāo)度因數(shù)與零點(diǎn)偏置的求取方法，即：

若求取陀螺儀Y、Z軸的標(biāo)度因數(shù)和零點(diǎn)偏置，只需要在轉(zhuǎn)臺上以 ω＝［0 ωy0］、ω＝［0-ωy0］、ω＝［0 0 ωz］、ω＝［0 0-ωz］旋轉(zhuǎn)采集數(shù)據(jù)，通過計(jì)算即可獲取。計(jì)算方法為

在系統(tǒng)進(jìn)行轉(zhuǎn)臺實(shí)驗(yàn)時(shí)，首先調(diào)整系統(tǒng)的標(biāo)定軸位置使其與轉(zhuǎn)臺軸平行，之后將其固定在轉(zhuǎn)臺的內(nèi)框平臺上，在轉(zhuǎn)臺進(jìn)行旋轉(zhuǎn)實(shí)驗(yàn)時(shí)采集數(shù)據(jù)完成陀螺的標(biāo)定。

2.3 電子羅盤誤差模型及標(biāo)定方法

若要對橢圓進(jìn)行修正，首先應(yīng)該將橢圓的一個(gè)中心與坐標(biāo)系的原點(diǎn)重合，之后通過系統(tǒng)的標(biāo)度因數(shù)對橢圓進(jìn)行擬合使其恢復(fù)圓形從而完成補(bǔ)償[10]。電子羅盤的誤差模型公式為

式中：Hxoff和Hyoff為偏移量；Hxsf和Hysf為標(biāo)度因數(shù)；用作計(jì)算航向角的 Hx和 Hy可表示為 Hx′、Hy′。

在捷聯(lián)姿態(tài)系統(tǒng)的實(shí)際應(yīng)用環(huán)境中恒定磁場的干擾強(qiáng)度一般是穩(wěn)定不變的，因此可以在該環(huán)境下對電子羅盤的輸出數(shù)據(jù)進(jìn)行標(biāo)定來提高航向角的精度。在實(shí)際標(biāo)定中，首先將電子羅盤的數(shù)據(jù)進(jìn)行采集，圍繞水平面360°旋轉(zhuǎn)一周，然后根據(jù)X軸和Y軸的磁場強(qiáng)度數(shù)據(jù)計(jì)算得到最大值和最小值Hxmax、Hxmin、Hymax、Hymin，通過這 4 個(gè)數(shù)值便可以求出對應(yīng)的偏移量和標(biāo)度因數(shù)。計(jì)算方法為

經(jīng)過補(bǔ)償后的電子羅盤數(shù)據(jù)如圖5所示，此時(shí)電子羅盤輸出的磁場數(shù)據(jù)已經(jīng)修正為接近圓形的形狀。

圖5 經(jīng)過橢圓修正后的電子羅盤數(shù)據(jù)圖像Fig.5 Electronic compass data corrected by the changes in the elliptical image

由于環(huán)境的地磁矢量隨著地理位置的不同會存在一定的變化，因此在更換載體的地理位置之后，需要對電子羅盤重新進(jìn)行修正，以保證航向角的精確度。

3 系統(tǒng)性能試驗(yàn)

3.1 靜態(tài)性能驗(yàn)證

系統(tǒng)的靜態(tài)精度測試方面，首先將系統(tǒng)靜止放置采集數(shù)據(jù)，然后對3個(gè)軸分別進(jìn)行單次轉(zhuǎn)動，采集數(shù)據(jù)發(fā)送至上位機(jī)，通過Matlab繪制出波形。如圖6所示為捷聯(lián)姿態(tài)系統(tǒng)在靜態(tài)下經(jīng)過卡爾曼濾波輸出的歐拉角的波形圖像。

圖6 捷聯(lián)姿態(tài)系統(tǒng)在靜態(tài)下的波形圖像Fig.6 Strapdown attitude system waveform images under the condition of static

在系統(tǒng)的靜態(tài)測試中，用手對系統(tǒng)沿1個(gè)軸進(jìn)行1個(gè)角度的轉(zhuǎn)動，由于是用手旋轉(zhuǎn)精度較低，因此在沿某1個(gè)軸旋轉(zhuǎn)時(shí)另外2個(gè)軸也會有較小的角度變化。通過圖6可以看出系統(tǒng)輸出的姿態(tài)信息在系統(tǒng)旋轉(zhuǎn)后迅速反應(yīng)，并能夠較快地收斂并趨于穩(wěn)定，說明該系統(tǒng)具有較好的穩(wěn)態(tài)性能。經(jīng)過對實(shí)驗(yàn)數(shù)據(jù)的分析，系統(tǒng)在靜態(tài)條件下橫滾角浮動范圍為±0.2°，俯仰角浮動范圍為±0.2°，航向角浮動范圍為±1°。

3.2 動態(tài)性能驗(yàn)證

在捷聯(lián)姿態(tài)系統(tǒng)的動態(tài)性能驗(yàn)證上，對系統(tǒng)施加了較大的姿態(tài)變化，進(jìn)行了單軸運(yùn)動和多軸運(yùn)動2種測試方式。由于是用手旋轉(zhuǎn)捷聯(lián)姿態(tài)系統(tǒng)存在一定的誤差，因此當(dāng)進(jìn)行單軸旋轉(zhuǎn)測試時(shí)其它軸的角度輸出也會有較小的變化，實(shí)驗(yàn)圖像如圖7所示。

圖7 捷聯(lián)姿態(tài)系統(tǒng)在動態(tài)下的波形圖像Fig.7 Strapdown attitude system waveform images under the condition of dynamic

通過圖像可以得知，系統(tǒng)的歐拉角在載體運(yùn)動后迅速收斂并且維持穩(wěn)定，由于是用手旋轉(zhuǎn)系統(tǒng)，因此由于運(yùn)動精度的影響導(dǎo)致在繞1個(gè)軸運(yùn)動時(shí)其它軸輸出的角度也會有較小的角度變化。

4 結(jié)語

本文闡述了基于低成本的MEMS捷聯(lián)姿態(tài)系統(tǒng)的發(fā)展現(xiàn)狀，針對低成本高性能的要求，設(shè)計(jì)了具有互補(bǔ)功能的MEMS捷聯(lián)姿態(tài)系統(tǒng)。通過對系統(tǒng)試驗(yàn)得到的數(shù)據(jù)進(jìn)行分析，該系統(tǒng)在靜態(tài)時(shí)能夠較快地收斂并趨于穩(wěn)定，在動態(tài)測試時(shí)能夠快速地進(jìn)行跟蹤和收斂，證明了該系統(tǒng)具有較好的穩(wěn)態(tài)性能和動態(tài)性能。

[1]唐大全，劉錦濤，張博.微型捷聯(lián)姿態(tài)系統(tǒng)的硬件設(shè)計(jì)[J].自動化儀表，2007，28（1）：15-18.

[2]郭訪社，于云峰，劉書盼，等.捷聯(lián)慣性導(dǎo)航系統(tǒng)姿態(tài)算法研究[J].航天控制，2010，28（1）：37-39.

[3]Fourati H.Heterogeneous data fusion algorithm for pedestrian navigation viafoot-mounted inertialmeasurementunitand complementary filter[J].IEEE Transactions on Instrumentation and Measurement，2014，64（1）：221-229.

[4]Zihajehzadeh S，Loh D，Lee M，et al.A cascaded two-step Kalman filter for estimation of human body segment orientation using MEMS-IMU[C]//Engineering in Medicine and Biology Society（EMBC），2014 36th Annual International Conference of the IEEE，2014：6270-6273.

[5]張勇剛，張?jiān)坪?，李?基于互補(bǔ)濾波器的MEMS/GPS/地磁組合導(dǎo)航系統(tǒng)[J].系統(tǒng)工程與電子技術(shù)，2014，36（11）：166-173.

[6]趙文杰，方舟，李平.基于地磁場矢量誤差反饋的姿態(tài)補(bǔ)償算法[J].浙江大學(xué)學(xué)報(bào)：工學(xué)版，2014，48（9）：169-174.

[7]張欣，白越，趙常均，等.多旋翼姿態(tài)解算中的改進(jìn)自適應(yīng)擴(kuò)展Kalman 算法[J].光學(xué)精密工程，2014，22（12）：238-244.

[8]宋麗君，秦永元.MEMS加速度計(jì)的六位置測試法[J].測控技術(shù)，2009，28（7）：11-13.

[9]宋麗君，秦永元.MEMS陀螺儀的一種實(shí)用標(biāo)定法[J].壓電與聲光，2010，32（3）：372-374.

[10]聞秋香，羅向前，孫君道.磁羅盤工程校準(zhǔn)方法研究[J].科學(xué)技術(shù)與工程，2011，11（28）：158-161.