航姿系統矢量傳感器非對準誤差及其校正*

李 翔，王勇軍，李 智

(1.桂林電子科技大學電子工程與自動化學院，廣西桂林541004; 2.桂林航天工業學院無人機遙測重點實驗室，廣西桂林541004)

航姿系統矢量傳感器非對準誤差及其校正*

李 翔1*，王勇軍1，2，李 智2

(1.桂林電子科技大學電子工程與自動化學院，廣西桂林541004; 2.桂林航天工業學院無人機遙測重點實驗室，廣西桂林541004)

航姿參考系統中三軸磁強計與三軸加速度計常采用橢球擬合法進行誤差校正與補償，其缺點是校正后不同傳感器之間以及傳感器坐標系與載體坐標系之間往往還存在非對準誤差。由定量分析得知此類非對準誤差所引起的航姿參考系統航向角誤差具有常值誤差與半圓羅差之和的形式。利用地磁矢量與重力矢量的點積為常數這一性質，并結合三維旋轉矩陣的小角度近似表達式，對橢球擬合法校正后的傳感器非對準誤差進行了補償，在不同磁干擾條件下均可使校正效果得到改善，航向剩余誤差(均方根)平均減小30%以上。

矢量傳感器;誤差校正;橢球擬合;航姿參考系統

三軸磁強計、三軸加速度計可分別用于測量地磁場與重力矢量，并進而在此基礎上實現姿態與方位(航向)的測量。二者可構成測量三維姿態的電子羅盤，或與 MEMS陀螺儀共同構成 9自由度(Degree of Freedom)的航向與姿態參考系統(Attitude and Heading Reference System，簡稱航姿參考系統或AHRS)［1－2］。

為保證AHRS使用精度，必須對三軸磁強計與三軸加速度計進行誤差校正與補償。在實驗室條件下可采用精密儀器進行校準(標定)，而在實際應用場合則更多采用基于橢球擬合的校正方法［3－7］。橢球擬合法只需利用三維矢量場(即重力或地磁場)的原始測量數據即可完成誤差校正，無需借助任何外部設備或精密基準，簡便易行，因而應用廣泛［8－14］。

橢球擬合法雖能實現單個矢量傳感器的誤差校正，但其原理決定了該方法不能辨識和補償同一航姿參考系統中不同矢量傳感器之間的非對準誤差。換言之，橢球擬合法并未完全解決AHRS中矢量傳感器的誤差校正問題。然而針對橢球擬合法的研究往往未能重視這一問題;雖有部分文獻提出了若干針對性措施，但這些措施或僅限于理論分析，或增加了較多操作步驟及要求，不利于實際應用。

本文對AHRS中加速度計與磁強計之間的非對準誤差所造成的影響進行了理論分析，并提出了一種新校正方法。此方法能有效地補償傳感器間非對準誤差，并可保留橢球擬合法操作簡便的優勢，有助于提高AHRS使用精度。

1 橢球擬合法概述

1.1 橢球擬合法的原理

矢量傳感器誤差校正的橢球擬合法是基于以下兩個基本假設:(1)傳感器無任何誤差時，其測量值分布于以原點為球心的一球面上;(2)傳感器有誤差時，其測量數據分布于一橢球面上。擬合該橢球面并將其復原為球面，便實現了傳感器誤差的校正。

橢球擬合法的實現還需要如下兩個前提:其一，傳感器所測量的矢量場必須為常矢量;其二，傳感器誤差可用式(1)給出的線性模型(亦稱泊松模型)描述，其中u=(u1u2u3)T為被測矢量場，v=(v1v2v3)T為傳感器輸出，矩陣K=(kij)3×3及矢量b= (b1b2b3)T描述傳感器誤差。

在式(1)的基礎上，取被測矢量u的模方‖u‖2并令其為常數，即得到表示橢球面的二次型，從而可進行橢球擬合以實現誤差校正。

1.2 橢球擬合法的缺陷

三維空間中二次曲面的標準方程只含9個獨立系數，對于橢球面方程亦是如此。然而式(1)中描述誤差的矩陣K及矢量b共有12個參數，故橢球擬合法不能完全確定所有參數［15－16］。其后果是經橢球擬合法校正后的傳感器坐標系存在一個未知的三維轉動。從數學角度描述，就是矩陣K的計算結果包含一個表示三維旋轉的正交矩陣R［17］。

因此，若采用橢球擬合法分別校正AHRS中的加速度計與磁強計，則校正后兩傳感器之間及它們與載體坐標系之間都可能存在未知的非對準誤差。

2 傳感器非對準誤差

2.1 非對準誤差的影響

將航向角、俯仰角和橫滾角依次記為ψ、θ、φ載體坐標系中的地磁場矢量h與地理坐標系中的地磁場矢量h0之間的關系可由式(2)描述:

式中:

由重力矢量的測量值可計算θ與φ，從而消去式(2)中的Cθ與Cφ，得到

式中:h0=(h∥0 h⊥)T，h∥與h⊥分別為地磁矢量的水平及豎直分量。又若記hψ=(h1h2h3)T，則h1=h∥cosψ，h2=－h∥sinψ，從而tanψ=－h2/h1。當h1與h2存在微小誤差δh1、δh2時，引起的航向誤差為

當加速度計相對于載體坐標系存在非對準誤差時，會導致俯仰角θ與橫滾角φ的計算值存在誤差。設θ與φ的誤差均為小量，分別記之為δθ與δφ，根據式(2)～(4)，δθ與δφ還會導致航向角ψ出現式(5)所示的誤差(其中已略去二階小量):

式(5)右邊第1項不含航向角ψ，故當載體水平回轉時，該項為一常值誤差(但隨俯仰角θ而增大)。另一方面，式(5)右邊括號中出現了航向角ψ的正、余弦，這一部分相當于磁羅盤的“半圓羅差”，即載體水平回轉一周時該項誤差恰好按正弦規律變化一個周期。

當磁強計相對于載體坐標系存在非對準誤差時，若該誤差為小量，則對應的三維旋轉矩陣可近似表示為式(6):

式中δ1、δ2和δ3分別表示繞3個坐標軸轉過的小角度(均以弧度計)。將式(6)代入式(2)～式(4)，可知由此造成的航向誤差近似為(略去二階及以上小量):

式(7)較式(5)更復雜，但仍可分為不顯含航向角ψ的常值誤差以及含有航向角ψ的正、余弦的“半圓羅差”兩部分。

綜上所述，不論加速度計或磁強計的非對準誤差，均會引起航向誤差，且其變化規律較為復雜。這也正是橢球擬合法所無法克服的缺陷。

2.2 非對準誤差的校正

加速度計的非對準誤差直觀地反映為俯仰角θ與橫滾角φ的誤差，相對而言較易修正。磁強計的非對準誤差則常與各種磁干擾引起的誤差相混合，增加了其辨識和補償的難度。

從數學的角度來看，磁強計非對準誤差的校正可歸為正交Procustes問題。因此，Procustes問題的經典解法可用于磁強計安裝誤差的校正［18－19］。然而，該方法必須同時獲取地磁場矢量在傳感器坐標系以及載體坐標系下的若干組讀數，才能由此求出從傳感器坐標系到載體坐標系的旋轉矩陣。顯而易見，若僅靠磁強計自身的讀數，則無法實施這一校正方法;而若地磁矢量在載體坐標系和傳感器坐標系下的讀數均為已知，則意味著式(1)右邊的被測矢量場u為已知，于是便可結合磁強計讀數直接求解矩陣K及矢量 b，而不需單獨考慮正交 Procustes問題。

目前較實用的非正交誤差校正方法有“繞軸旋轉法”［20］和“常數夾角法”［21］。

“繞軸旋轉法”的原理是:將AHRS繞加速度計某一軸旋轉時，如旋轉過程中該軸磁強計讀數保持不變，則表明磁強計與加速度計在該軸上不存在非對準誤差;否則，根據該軸磁場讀數的變化，可以推算出磁強計在該軸的非對準誤差。由此可依次求出式(6)中的3個小角度δ1、δ2和δ3。“繞軸旋轉法”雖然只需用到加速度計與磁強計讀數，但操作上需保證旋轉軸與加速度計敏感軸方向重合，否則影響校正效果。

“常數夾角法”是利用地磁矢量與重力矢量的夾角為常數這一性質，求取磁強計與加速度計之間的旋轉矩陣。但由于旋轉矩陣包含9個元素，故該方法的計算量較大。另一方面，三維旋轉矩陣理論上應是一個正交矩陣，但由實測數據進行擬合得到的旋轉矩陣則不一定能確保其正交性，因而不能確保校正效果。

2.3 改進的非對準誤差校正法

記磁強計輸出為v=(v1v2v3)T，又假定橢球擬合法已給出式(1)中的K與b。如前所述，K可能包含一個未知旋轉矩陣Cδ，該矩陣可近似由式(6)表示。于是載體坐標系中的地磁矢量為

式中記hC=K(v－b)。又因地磁矢量與重力矢量夾角為常數，故二者的點積亦為常數，即

將式(9)按元素展開即得關于δ1、δ2和δ3的線性表達式:

由式(10)即可利用加速度計與磁強計讀數，通過最小二乘擬合求出3個小角度δ1、δ2和δ3，并進而修正橢球擬合法中的矩陣K。

需要指出，式(10)右邊的常數僅影響校正后地磁矢量的大小，不影響其方向，故其值不需精確選取。

以上方法計算簡便，與橢球擬合法配合使用時基本不需增加系統軟件及校正操作的開銷。盡管該方法對矩陣Cδ采用了近似形式即式(6)，但該式確保Cδ非對角元是反對稱的，這有利于算法的魯棒性。

3 算法驗證

3.1 實驗條件

實驗中，AHRS分別采用3種不同的傳感器配置:①磁強計為HMC1021+HMC1022，加速度計為ADXL213;②磁強計為 HMC5883，加速度計為ADXL345;③9軸模塊MPU9250，該芯片內集成了3軸加速度計、3軸磁強計和3軸陀螺儀。

針對每種傳感器配置，分別在無干擾、小干擾和大干擾3種情形下進行磁強計校正實驗:①無干擾，即移除系統周邊所有可能存在磁干擾的物件;②小干擾，即AHRS采用有較弱磁性的螺釘等小部件;③大干擾，即在AHRS外殼上附加一定尺寸的鋼鐵部件。

對磁強計進行校正時，分別采用3種不同方式:①只采用橢球擬合法;②先用橢球擬合法，再用“常數夾角法”;③先用橢球擬合法，再用2.3節所述新方法。對三者校正后的航向剩余誤差(以均方根誤差即RMSE計)進行對比。

校正過程中，AHRS分別在水平和向左、右、前、后傾斜等5種狀態下各繞豎直軸回轉一圈采集數據。校正后，按同樣方式驗證航向剩余誤差。

3.2 實驗結果

無干擾、小干擾和大干擾3種情形下的實驗結果依次見表1～表3。

表1 無干擾時航向誤差(均方根)

表2 小干擾下航向誤差(均方根)

表3 大干擾下航向誤差(均方根)

由表1～表3可見，常數夾角法在無干擾時效果較好，但在有外加干擾磁場時，其校正效果則不如本文所述方法。其主要原因正如2.2節中所述，常數夾角法在求解三維旋轉矩陣時，不能確保其正交性，這便有可能造成附加的誤差。

尤其值得注意的是，表2所列實驗結果中，在3種不同的傳感器配置下，經過橢球擬合法校正后的航向剩余誤差(RMSE)均已減小到1°以內;然而再經過常數夾角法進一步校正后，航向剩余誤差反而有所增大。

與之形成對比的是，由表1至表3所列結果可見，不論在無干擾、小干擾或大干擾情形下，本文提出的非對準誤差校正方法均能獲得較好效果，可使航向剩余誤差(RMSE)相對于僅采用橢球擬合法時平均減小30%以上。

3.3 討論

常數夾角法與本文所述方法，都是將橢球擬合法校正后的剩余誤差視作一個三維旋轉矩陣Cδ。因此，這兩種方法的補償效果取決于2個關鍵因素:①待校正的誤差是否能用三維旋轉矩陣準確描述(即校正算法對誤差的建模是否與實際情況符合);②該矩陣的求解結果是否準確(即校正算法本身精度如何)。

首先討論第1個因素即誤差建模問題。在2.1節理論推導中已經得知，由三維旋轉矩陣引起的航向誤差應當是常值誤差和半圓羅差之和。其中，半圓羅差的直觀表現是:在載體旋轉一周的過程中，航向角的誤差呈現出類似于正弦函數(或余弦函數)一個周期的變化規律。

以HMC1021+HMC1022這一傳感器配置為例，小干擾及大干擾條件下，經過橢球擬合法校正后的航向剩余誤差變化曲線分別如圖1和圖2所示。從直觀上看，圖2所示航向剩余誤差較大，但其曲線十分符合半圓羅差的特征;圖1中的航向剩余誤差雖然較小，但其變化規律較復雜，僅用半圓羅差不足以準確描述。由此不難預料，常數夾角法以及本文所述算法對于圖2所示情形將得到更好的效果。這與表2、表3中的實驗結果相符，即:在大干擾情形下HMC1021+HMC1022磁強計的校正結果要好于小干擾情形。

圖1 小干擾下HMC1021/2磁強計經橢球擬合法校正后剩余誤差

圖2 大干擾下HMC1021/2磁強計經橢球擬合法校正后剩余誤差

對于上述第2個因素，即校正算法本身的精度，在表2、表3所列實驗結果中已經有所展現。為更直觀地比較常數夾角法和本文算法的性能，現將小干擾情形下兩種算法各自對HMC1021+HMC1022磁強計校正后的誤差曲線示于圖3和圖4;大干擾下兩種算法對HMC1021+HMC1022的校正結果則分別示于圖5與圖6。

圖3 小干擾下HMC1021/2磁強計經常數夾角法校正后剩余誤差

圖4 小干擾下HMC1021/2磁強計經本文算法校正后剩余誤差

圖5 大干擾下HMC1021/2磁強計經常數夾角法校正后剩余誤差

圖6 大干擾下HMC1021/2磁強計經本文算法校正后剩余誤差

從圖3和圖5可以看到，常數夾角法校正后的剩余誤差曲線呈現出較為明顯的半圓羅差形態，這表明該算法對三維旋轉矩陣的求解結果不準確，不但未能起到補償作用，反而引入了更大誤差。而由圖4和圖6可見，無論對于小干擾或大干擾情形，本文算法均有較好的效果。即便實際誤差與理論模型存在一定出入(圖1)，本文算法仍能在橢球擬合法的基礎上進一步縮小航向剩余誤差(圖4)。

4 結論

橢球擬合法只需利用傳感器自身讀數即可實現三軸磁強計與加速度計的校正，但其校正后剩余的傳感器非對準誤差會影響AHRS航姿精度。本文對此類誤差進行了定量分析，并提出了一種改進的校正方法。實驗表明，本文方法對于不同型號傳感器以及不同磁干擾條件均能起到改善AHRS航向精度的作用，可使航向剩余誤差(以均方根計)相對于僅用橢球擬合法時平均降低30%以上。

［1］ 吳濤，白茹，朱禮堯，等.基于卡爾曼濾波的航姿參考系統設計［J］.傳感技術學報，2016，29(4):531－535.

［2］ 米剛，田增山，金悅，等.基于MIMU和磁力計的姿態更新算法研究［J］.傳感技術學報，2015，28(1):43－48.

［3］ Fang Jiancheng，Sun Hongwei，Cao Juanjuan，et al.A Novel Calibration Method of Magnetic Compass Based on Ellipsoid Fitting［J］.IEEE Transactions on Instrumentation and Measurement，2011，60(6):2053－2061.

［4］ 李勇，劉文怡，李杰，等.基于橢球擬合的三軸磁傳感器誤差補償方法［J］.傳感技術學報，2012，25(7):917－920.

［5］ 朱建良，王興全，吳盤龍，等.基于橢球曲面擬合的三維磁羅盤誤差補償算法［J］.中國慣性技術學報，2012，20(5):562－566.

［6］ 張琦，潘孟春，陳棣湘，等.基于線性化參數模型的三軸磁場傳感器校準方法［J］.傳感技術學報，2012，25(2):215－219.

［7］ Sipos M，Paces P，Rohac J，et al.Analyses of Triaxial Accelerometer Calibration Algorithms［J］.IEEE Sensors Journal，2012，12(5): 1157－1165.

［8］ 龍禮，張合.三軸地磁傳感器誤差的自適應校正方法［J］.儀器儀表學報，2013，34(1):161－165.

［9］ 李季，潘孟春，羅詩途，等.半參數模型在載體干擾磁場補償中的應用研究［J］.儀器儀表學報，2013，34(9):2147－2152.

［10］Feng Wenguang，Liu Shibin，Liu Shiwei，et al.A Calibration Method of Three-Axis Magnetic Sensor Based on Ellipsoid Fitting［J］.Journal of Information and Computational Science，2013，10(6): 1551－1558.

［11］Tabatabaei S A H，Gluhak A，Tafazolli R.A Fast Calibration Method for Triaxial Magnetometers［J］.IEEE Transactions on Instrumentation and Measurement，2013，62(11):2929－2937.

［12］劉艷霞，方建軍，楊清梅.基于橢球假設的三軸加速度計誤差標定與補償［J］.傳感器與微系統，2014，33(6):52－54，64.

［13］Hemerly E M，Coelho F A A.Explicit Solution for Magnetometer Calibration［J］.IEEE Transactions on Instrumentation and Measurement，2014，63(8):2093－2095.

［14］翟子雄，張丕狀，張煜林.基于橢球假設的MIMU現場標定方法［J］.中國測試，2015，41(12):25－27，48.

［15］劉艷霞，李希勝，馮毅博，等.三軸磁羅盤標定位置分布的研究［J］.儀器儀表學報，2013，34(3):684－690.

［16］劉玉霞，周軍，葛致磊.基于軌跡約束的地磁場測量誤差修正方法［J］.中國慣性技術學報，2012，20(2):205－210.

［17］李智，李翔.基于橢球假設的三軸電子羅盤羅差補償研究［J］.儀器儀表學報，2011，32(10):2210－2215.

［18］Vasconcelos J F，Elkaim G，Silvestre C，et al.Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame［J］.IEEE Transactions on Aerospace and Electronic Systems，2011，47(2): 1293－1306.

［19］謝啟源，陸宇平，喬兵，等.一種基于橢球約束和Procrustes分析的磁矢量傳感器校正算法［J］.傳感技術學報，2012，25 (13):1684－1687.

［20］張韋，劉詩斌，馮文光，等.基于磁通門的三軸電子羅盤自動誤差補償方法［J］.傳感技術學報，2012，25(12):1692－1695.

［21］ Liu Yanxia，Li Xisheng，Zhang Xiaojuan，et al.Novel Calibration Algorithm for a Three-Axis Strapdown Magnetometer［J］.Sensors，2014，14(5):8485－8504.

李 翔(1984－)，男，廣西陸川人，博士，講師，主要研究方向為姿態測量與智能傳感器，xli1984@hotmail.com;

王勇軍(1985－)男，山東聊城人，博士生，講師，主要研究方向為無人機組合導航技術，dongwang@guat.edu.cn;

李 智(1965－)，男，廣西靈川人，博士，教授，主要研究方向為智能儀器系統，cclizhi@guat.edu.cn。

Inter-Triad Misalignment of Vector Field Sensors in Attitude and Heading Reference Systems and Its Calibration*

LI Xiang1*，WANG Yongjun1，2，LI Zhi2
(1.School of Electronic Engineering and Automation，Guilin University of Electronic Technology，Guilin Guangxi 541004，China; 2.Key Laboratory of Unmanned Aerial Vehicle Telemetry，Guilin University of Aerospace Technology，Guilin 541004，China)

Ellipsoid fitting method is frequently used to eliminate the error of tri-axial magnetometer and accelerometer in attitude and heading reference system(AHRS).But the defect is that it cannot resolve the inter-triad misalignment between different sensors，as well as between the sensor frame and the carrier’s body frame.According to quantitative analysis，the heading error of AHRS caused by inter-triad misalignment consists of constant and sinusoidal components.Using the constant dot product of geomagnetic vector and gravity vector，additional with small angle approximation of three dimensional rotation matrices，the inter-triad misalignment left by ellipsoid fitting method can be compensated.With different magnetic disturbances，the residual heading error(RMSE)can be reduced over 30%on average.

vector field sensor;error calibration;ellipsoid fitting;attitude and heading reference system

TP212.9

A

1004－1699(2017)02－0266－06

C:7220;7310L

10.3969/j.issn.1004－1699.2017.02.017

項目來源:國家自然科學基金地區基金項目(61361006);廣西自然科學基金青年基金項目(2015GXNSFBA139253);廣西自動檢測技術與儀器重點實驗室主任基金項目(YQ15107，YQ14203)

2016－07－06 修改日期:2016－09－05