李 翔，王勇军，李 智

(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