一种简单的低成本航姿系统标定及修正方法

翟张斌，张晓明，安永泉，薛英娟

(1.中北大学 信息与通信工程学院，太原 030051；2.中北大学 电子测量技术国家重点实验室，太原 030051；3.中北大学 创新创业学院，太原 030051)

0 引言

三轴加速度计与三轴磁力计分别可以测量矢量加速度信号与矢量磁信号，在导航和姿态估计等许多应用中，加速度计和磁力计的测量通常相结合起来，构成航姿系统用于得到姿态与航向信息。但由于制造、安装、数据采集及环境等因素的影响，使用传感器测量时会出现测量误差。加速度计的测量误差主要包括传感器零位误差、三轴灵敏度误差、轴间不正交误差以及测量噪声等；磁力计除此之外，还受环境中软硬磁材料引起的干扰；同时，不同传感器在安装时也会不可避免地产生非对准误差，这就需要在它们的敏感轴之间进行校准。因此，在使用前对传感器进行联合标定是十分必要的。

目前，国内外对加速度计与磁力计的联合标定问题进行了很多的研究。李翔与协同研究人员先后用点积不变法[1]、常数夹角法[2]、绕轴旋转法[3]、双内积法[4]等算法来联合标定传感器误差和传感器间非对准误差，提供了常用的联合标定方法，但对于低成本传感器，绕轴方式更容易增加噪声，影响标定结果；陆欣在通过采集正20面体下的测量数据，利用约束范数求解得到磁强计误差校正参数，引入四元数对非对准矩阵进行参数化，并基于点积不变原理通过约束梯度下降估计出非对准误差参数[5]，但需要特定的工装模具来实现均匀静态数据的采集；孙逸帆首先对加速度计的误差参数使用极大似然法进行标定，再利用点积不变的性质，通过递推最小二乘法估计磁力计的误差参数和其与加速度计间的非对准误差参数[6]，但这种方法中的传感器参数误差会影响到非对准误差参数的标定结果；Papafotis将加速度计、磁力计与磁倾角信息构建在一个代价函数中，通过最小二乘算法求解标定参数并进行轴向校准[7]，但将所有的误差参数置于同一代价函数内进行修正，运算较为复杂。

针对低成本航姿系统中加速度计和磁力计在无专业标定设备进行辅助情况下的联合标定，本文提出了一种简便易行的标定方法及误差参数修正方法。首先从减小数据源噪声的方面，在静态多位置下进行采样，同时为了便于操作，采取正交采样策略；接着研究了测量噪声在该采样策略下椭球拟合标定过程中的误差传播，找到了误差传播过程中可观性最差的磁力计误差参数，对该项的修正会最有效地改善磁力计的标定效果；同时也对传感器间的非对准误差进行标定与修正，在修正过程中，使用椭球约束条件下点积法构造地磁矢量与水平面夹角的均方差来量化修正效果，通过三维步进搜索方法进行寻优，最终完成了低成本航姿系统的标定与修正。

1 标定原理

1.1 传感器的误差模型

三轴加速度计和三轴磁力计的误差模型如式(1)所示，v为传感器的实际输出量，u为传感器的理想输出量[8]。对于加速度计，v用f表示，u用fG表示；对于磁力计，v用m表示，u用mB表示。

v=Ku+v0+ε

(1)

式中，矩阵K中体现灵敏度误差和轴间不正交误差、矢量v0中体现零位误差(零偏)、矢量ε中体现传感器噪声及高阶误差项。传感器噪声近似地满足高斯分布，在校准过程中使用每个静态间隔的平均测量值，与高阶误差项视为小量，所以ε可忽略掉[9]。

如图1所示，理想传感器的测量轴为x、y、z轴，而实际传感器的测量轴为x1、y1、z1轴，三者近似正交，但存在着轴间不正交角α、β、γ。

图1 三轴传感器测量轴非正交示意图

假设z1与z轴重合，x1位于xOz平面内与x轴夹角为α，则y1轴在xOy平面内的投影与y轴的夹角为β，与xOy平面的夹角为γ，用kx1、ky1、kz1分别表示实际传感器的三轴灵敏度误差[10-11]，则矩阵K可以表示为：

(2)

通常轴间不正交角的角度都非常小，所以可以得到三轴传感器的实际测量简化模型为：

(3)

1.2 椭球拟合原理

因为同一地点的重力加速度和磁场强度都可视为常量，所以有下式：

(4)

理想加速度计在任意静止姿态下测量的比力所构成的三维坐标，全部分布在以原点为中心、当地重力加速度大小为半径的标准球面上[12]，磁力计同理。但由于传感器误差的影响，使得正球体发生偏离变为中心偏离原点的椭球[13]。由于测量数据满足一个二次型椭球曲面方程，设该曲面方程为[14]：

F(ξ，z)=ξTz=ax2+by2+cz2+2dxy+

2exz+2fyz+2px+2qy+2rz+g=0

(5)

式中，ξ=[a，b，c，d，e，f，p，q，r，g]T为待求的椭球拟合曲面参数向量；z=[x2，y2，z2，2xy，2xz，2yz，2x，2y，2z，g]T为测量数据得运算组合向量。将式(5)整理为矢量形式：

(X-X0)TA(X-X0)T=1

(6)

展开可得：

(7)

由式(4)展开的二次标准型方程与式(7)对比可得[15]：

(8)

通过式(8)即可由最佳椭球拟合参数计算出三轴传感器误差参数kx1、ky1、kz1、α、β、γ和v0。

1.3 非对准误差

多个传感器在安装时会产生非对准误差，如图2所示，理想传感器1坐标系O-xyz与理想传感器2坐标系O2-x2y2z2之间存在着旋转误差，即非对准误差[10-11]。

图2 传感器间非对准误差示意图

该旋转矩阵可看作是依次绕三轴旋转特定角度。分别定义绕X轴、绕Y轴、绕Z轴的旋转角为λ、θ、φ。以传感器1坐标系为基准，依次绕Z轴、Y轴、X轴旋转得到传感器2坐标系。对应地，传感器1坐标系下的输出向量(vx，vy，vz)T在传感器2坐标系下的输出为：

(9)

1.4 点积法

磁倾角是地磁矢量与水平面之间的夹角，如图3所示，在地球上的某一位置，磁倾角的大小是稳定的。

图3 磁倾角示意图

磁倾角δ可以由导航坐标系中重力矢量Gn与地磁矢量Bn的归一化点积定义为[7]：

(10)

由静止姿态下理想加速度计与磁力计输出值的归一化点积可以求得fG与mB夹角φ的余弦值：

(11)

在北东地-导航n坐标系、前右下-载体b坐标系中，设δ为地磁倾角(向下为正)，χ为磁偏角(北偏东为正)，g为重力加速度，B为地磁场强度。在导航坐标中，重力矢量可以表示为：

(12)

地磁矢量可以表示为：

(13)

以加速度计作为基准，当存在非对准误差角时，在任意静止姿态下：

(14)

(15)

sinλsin(φ+χ))cosδ-cosλcosθsinδ

(16)

结果表明，它们的单位矢量点积结果与载体姿态无关，而与非对准误差角存在相关关系，且当非对准误差角λ、θ、φ为0时，有式(11)成立。

2 标定流程

图4给出了所采用的联合标定方法流程：首先采集15组静止姿态下的传感器原始数据，其次对原始数据进行动静态数据检测并分类，接着对加速度计和磁力计的测量数据使用椭球拟合法进行标定，根据标定结果，以补偿后的加速度计轴向作为基准，结合计算磁场矢量与水平面夹角均方差，最后在椭球约束条件下对误差参数进行标定与修正。

图4 标定流程图

2.1 采样策略

由于加速度计测量值也对载体的运动加速度敏感，加速度计的测量值实际上是载体运动加速度与重力加速度负值的合矢量[16]。为了避免载体加速度的影响，这里使用多个姿态下的静止测量数据。

在标定加速度计的静止位置选择上，应选择尽可能多且均匀分布在球面上的点[17]。这里选用最优15点位置集[18]，该位置集考虑到了加速度计测量数据与标定参数间的敏感度，当加速度计的敏感轴方向与重力矢量平行时，加速度计测量对线性尺度因子、非线性尺度因子和偏置的灵敏度最大；当重力矢量在加速度计轴向形成的平面内，敏感轴与重力矢量夹角为45 °或135 °时，加速度计测量对轴间不正交误差的灵敏度最大。

为了同时标定磁力计误差参数和传感器间的非对准误差，对原15位置法在水平面上进行了方向限制。因为重力矢量垂直向下，绕重力矢量轴方向旋转时，不会对加速度计的输出造成明显影响，但对于地磁矢量，磁力计绕重力矢量轴方向旋转，在不同的水平方向下三轴输出会产生明显变化。为了使磁力计测量数据也与标定参数间保持较高灵敏度，限制其中一轴轴向需要朝向东西方向，其余两轴即可获得对于性能参数较大灵敏度的测量数据。最后为了标定过程的连贯性，重新设计了该15位置的顺序编排，如图5所示。

图5 多位置编排示意图

先将传感器的x轴在水平面上靠近东西方向，y轴垂直向上，得到第1个位置姿态；沿南北轴向上的z轴旋转180°得到第2个位置姿态；再沿东西轴向上的x轴逆时针旋转135 °得到第3个位置姿态，再继续沿东西轴向上的x轴逆时针旋转90 °得到第4个位置姿态，再继续沿东西轴向上的x轴逆时针旋转90 °得到第5个位置姿态。传感器轴向与每次旋转的角度不要求严格精确，可用手动的方式完成，各位置需静置保持30 s以上。更替第1个位置姿态中的三轴朝向作为第6、第11个位置姿态，同理按第1个位置姿态到第5个位置姿态的变动方式，最终完成15个不同静止姿态下的数据采集。

2.2 静态检测算法

(17)

式中，varN(ft)和varN(mt)是计算各个分量方差的算子。当ζf(t)、ζm(t)均小于设定阈值时，判断该时刻为静止点，且当静止点持续时间达10秒以上，判断该区间为静止姿态区间。实际应用中，滑动窗N的大小与阈值的大小都需要根据采样频率和传感器噪声大小进行适当的设置，利用该算法可以准确有效地将15个静止姿态的数据筛选出来。

2.3 椭球拟合标定过程中的误差传播

传感器性能参数与真实参数出现偏差时，会影响多位置下计算地磁矢量与水平面的夹角的分布情况。使用Matlab软件仿真进行定性分析，按误差类别分别设置一定范围的偏差，分别观察对地磁矢量与水平面夹角的均方差结果的影响，发现当零位误差、轴间不正交误差和非对准误差与真实误差参数的误差越小时，多位置下计算地磁矢量与水平面夹角的均方差就越小。当三轴灵敏度误差的误差比例大小越接近时，多位置下计算磁场矢量与水平面夹角的均方差越小。所以可以使用计算磁场矢量与水平面夹角的均方差来量化标定结果，对零位误差、轴间不正交误差和非对准误差的修正可以减小多位置下计算磁场矢量与水平面夹角的均方差，提高标定的效果。

接下来研究磁力计的零位误差和轴间不正交误差在椭球拟合标定过程中的误差传播。因为过程中涉及复杂的计算，从数学推导上不易计算推导出，所以通过Matlab软件进行仿真计算分析。

按照本文前面所述的采样策略仿真出理想的15点归一化数据，添加均值为0、标准差为0.05的正态分布噪声，对每点取100次的平均值进行椭球拟合标定，一共进行100 000次的仿真计算，统计零位误差和轴间不正交误差的标定结果分布特征。统计结果显示，它们的结果均符合正态分布，零偏标定结果的标准差为0.002 383(单位为1)，轴间不正交误差角标定结果的标准差为0.007 485(单位为1弧度)，两者数值之比为3.140 4，标准差数值特别接近π倍的关系。

接着仿真出理想球面上均匀的10 000个点，替代实际姿态下的所有情况。分别只用零偏标定结果或轴间不正交误差角标定结果进行“补偿”。因为零位误差属于线性误差，不同次序下的结果补偿后的模值大小均符合正态分布，标准差稳定不变。但轴间不正交误差属于非线性误差，不同次序下的结果补偿后的模值大小均不符合正态分布，而服从“介”字形的分布，且“介”字峰处的位置不固定，所以标准差也不稳定，其均值也大于前者。这表明在该采样策略下，轴间不正交误差的可观性相对较差，所以对它的修正是更有必要的，仅对它进行修正更能简单且有效地提升标定结果。

2.4 误差参数修正

椭球拟合基本能精确计算出零位误差和灵敏度误差，借助地磁矢量与水平面夹角的不变性就可以修正轴间不正交误差和非对准误差。以补偿后的加速度计轴向为参考坐标系，修正磁力计的轴间不正交误差参数和传感器间的非对准误差，以如图6所示的流程进行误差参数修正。

图6 误差参数迭代修正流程图

以椭球拟合标定的磁力计误差参数作为初值，经过第1次寻优迭代，得到非对准误差参数。接着以上一次寻优迭代后的非对准误差参数寻优得到轴间不正交角，再以新的轴间不正交角寻优得到新的非对准误差角作为一次迭代，如此进行迭代寻优，直至在椭球约束条件下计算磁场矢量与水平面夹角的均方差减小到最小值，确定输出标定参数。

在上述流程两类参数的相互寻优中，每次仅需修正3个参数，且由该3个参数决定的三维函数中仅有一个极小值点，对于这种简单的寻找最值问题，这里采取简单且直观的三维步进搜索方法进行寻优。在初始点处计算函数的值，并将其作为当前的最优值。然后，在某一维上向左和向右分别移动一个步长，并计算函数在这两个位置的值，比较这3个位置的函数值，更新当前的最优值，往复更新3个维度上的参数，使达到函数的最小值，即可得到取最小值时的3个参数值。

地磁矢量与水平面的夹角可通过点积法计算得到：

(18)

其中：fk和mk分别为第k对数据点下加速度计和磁力计经标定补偿后的测量值，n为总数据点对数，多位置下的地磁矢量与水平面夹角的平均值为：

(19)

以均方差来量化修正效果，其值越小修正效果越好，如下式：

(20)

定义式(21)为数据分布区间函数，描述数据的分布特征，表示距均值一个标准差的区间范围，其中mean和std分别表示数据统计分布的均值与标准差。

F(mean，std)=[mean-std，mean+std]

(21)

为了保证修正后的磁力计误差参数仍能最大程度拟合测量数据，将修正后距均值一个标准差区间的数据限制在原数据距均值一个标准差区间的范围内，构造椭球约束条件为：

F(mean1，std1)⊆F(mean0，std0)

(22)

式中，mean0和std0为原椭球拟合标定补偿后模值分布的均值与标准差，mean1和std1为修正标定参数补偿后的模值分布的均值与标准差。

3 仿真试验

本节通过Matlab软件完成仿真试验，验证该标定方法的有效性。首先仿真出设定15个编排位置下加速度计与磁力计的理想测量数据。接着对加速度计与磁力计分别设定误差干扰及噪声，得到受误差影响下的仿真测量数据。分别采用下面2种方法进行标定：1)先使用椭球拟合法标定，再使用点积不变法得到传感器的误差与非对准误差；2)采用上述方法，先使用椭球拟合法标定，再利用磁场矢量与水平面夹角及均方差标定并修正非对准误差与传感器误差参数。最后对比分析不同方法的标定效果。

3.1 数据仿真条件

假设某地的磁倾角为30°，在两个理想的传感器间不存在非对准误差的情况下，按上述15位置采样策略仿真出理想的数据点，为方便分析对数据进行了归一化处理。接着，对理想仿真数据添加传感器误差及噪声，误差参数设定值见表1，同时设定加速度计的归一化噪声标准差σacc=0.005，磁力计的归一化噪声标准差σmag=0.05，每点取10次仿真的平均值作为该点测量数据。

表1 传感器的仿真参数和椭球拟合标定结果

用圆点●表示加速度计测量的仿真值，用星点*表示磁力计测量的仿真值，理想的15对数据点分布如图7所示，所有数据点均分布在单位球面上。此时由15对数据点计算得地磁矢量与水平面夹角均为30°，均方差为0°。含误差的15对数据点分布如图8所示。可以看出磁力计的部分数据点已经偏离单位球的表面，此时由15对数据点计算得地磁矢量与水平面夹角大小的平均值为30.553 7 °，均方差为7.780 8 °。

图7 理想的仿真数据点分布图

图8 含误差的仿真数据点分布图

3.2 仿真结果分析

先对含误差的仿真数据点进行椭球拟合标定，椭球拟合残差如图9所示，可以看出磁力计仿真数据较高的噪声导致了较大的拟合残差。误差参数标定结果如表1所示，可以看出噪声较低的加速度计标定结果更接近真实值。

图9 仿真数据拟合残差图

因为加速度计性能参数标定得更为准确，所以将标定补偿后的加速度计三轴轴向作为基准坐标系，分别通过点积不变法和本文方法对非对准误差和磁力计的参数进行标定与修正。使用点积不变法求得3个非对准角度数分别为：[-1.606 1，0.574 4，1.817 2]。通过本文的方法，3个非对准角度数修正为：[-1.336 5，0.331 2，1.945 8]，轴间不正交角度数分别修正为：[-1.535 8，1.672 5，2.374 2]，标定结果进一步接近设定值。表2对比了3种方法的标定效果。

表2 标定效果对比

本文方法相比于椭球拟合+点积不变法，计算磁场矢量与水平面夹角的均方差下降了25%以上，进一步提升了标定质量，15对数据点在不同标定方法下计算的地磁矢量与水平面夹角大小如图10所示。可以看出，其数值大小围绕30 °波动，经本文方法标定后的数值波动最小。经过标定补偿后的加速度计与磁力计的数据分布位置如图11所示，与图7比较可认为已完成标定补偿。

图10 不同标定方法的结果对比

图11 补偿后的仿真数据点分布图

4 现场试验

4.1 试验条件

现场试验采用深圳维特智能科技设计的9轴传感器记录仪WT901SDCL模块，该模块内部使用的6轴IMU传感器芯片型号为ICM42607，使用的磁传感器芯片型号为MMC3630KJ。数据采集过程中，在磁屏蔽桶内使用磁通门对锂电池工作时周围磁场进行了测量，结果显示会有约3 000 nT的干扰，所以对该模块进行了适当的改装，使用铜材质螺钉替换了原铁材质螺钉，通过延长导线将锂电池置于距离模块50 cm以外的位置，减少磁性材料与锂电池放电时周围磁场对磁传感器的影响。

试验地点需要选择周围无强磁物质干扰的稳定磁环境中，需要注意应避免借助路面或墙面等可能含铁氧体材质性质的平台，在石质地面进行数据测量时观察到磁场出现突变，研究称约占水泥熟料质量15%左右的铁铝酸四钙属于铁氧体性质[20-21]，铁氧体的相对磁导率为3.0×102～5.0×103，在空气与墙面两种介质界面上，介质性质的突变会引起磁场突变，理论可合理解释，所以最终选定在空旷的草地上进行数据采集，使用WT901SDCL模块采集数据，按上文的采样策略，每个位置分别静置采集约30 s的数据并存储在SD卡中。9轴传感器模块与测试环境如图12所示。

图12 9轴传感器模块与测试环境

借助WT901SDCL模块配套的上位机软件，对存储文件进行读取与格式转换，得到传感器的原始输出量。使用Matlab软件，先按照芯片手册对原始数据进行处理得到真实物理量，接着完成剩余的数据处理及标定工作。

4.2 标定结果分析

进行动静态数据分离后，分别对加速度计和磁力计的实测数据，取不同静态区间的15个平均值作为待拟合数据进行椭球拟合标定。因为磁传感器输出不受运动加速度的影响，也可以使用所有数据点进行拟合，但需要注意筛选后数据点的密度需要均匀，避免椭球拟合结果更“照顾”数据点密集的区域，因为参与到椭球距离平方和的点数也会变多，从而偏离真实的位置和形状[17]。

经过椭球拟合后，由椭球参数计算得到传感器误差参数。椭球拟合的效果如图13所示，用〇表示多数据点标定前的模值大小，用*表示多数据点标定后的模值大小，用+表示椭球拟合的残差值大小。试验场地当地的重力加速度模值为1 g，地磁矢量模值为54 000 nT，加速度计椭球拟合的残差在0.3%以内，磁力计的椭球拟合残差在5%以内。标定前，加速度计模值的平均值为1.000 8 g，标准差为0.020 3 g，标定后，加速度计模值的平均值为1.000 0 g，标准差为0.0007g。标定前，磁力计模值的平均值为55 000 nT，标准差为12 260 nT，标定后，磁力计模值的平均值为54 000 nT，标准差为430 nT。显然标定后的加速度计数据的离散程度更低，所以将经标定补偿后的加速度计坐标系作为基准坐标系，对传感器间的非对准误差参数和磁力计轴间不正交误差参数进行标定与修正。

图13 实测数据标定前后对比图与椭球拟合残差图

图14展示了未进行椭球约束下磁力计数据与磁场矢量与水平面夹角数据在如上所述迭代中的变化情况。随着迭代次数的增加，补偿后的磁场模值均值逐渐偏离原地磁场均值，标准差也先减小后增大，第6次迭代后，修正补偿后的数据已偏离原椭球补偿后数据的分布特征范围。在迭代过程中，计算磁场矢量与水平面夹角大小的均值在0.01°的量级轻微变动，几乎没有影响，虽在持续减小，但减小的幅度逐渐减弱。根据约束条件，第6次迭代后的误差参数即为最终的标定参数。

图14 实测数据标定结果对比分析

标定后磁场矢量与水平面夹角比较如表3所示，第1次迭代结果和点积不变法标定结果等效，使用点积不变法标定后的计算磁场矢量与水平面夹角均值为 54.391 8°，均方差0.752 7°。第6次迭代结果为本文方法标定结果，标定补偿后的计算磁场矢量与水平面夹角均值为 54.408 4°，增加了0.016 6°，可视为几乎没有影响，因无法确定试验场地的磁倾角准确值，故没有进行对比，计算磁场矢量与水平面夹角均方差为0.484 2°，相比于点积不变法降低了30%以上。

表3 标定后磁场矢量与水平面夹角对比

考虑到实测环境中还存在着未知的磁干扰和使用的传感器精度较低，可认为已完成了加速度计与磁力计的误差参数标定和非对准误差参数标定，实测标定试验效果与仿真试验标定效果一致，验证了该方法的有效性。

5 结束语

对于在无专业标定设备进行辅助情况下低成本航姿系统中加速度计和磁力计的联合标定，本文提出了一种在静态多位置下，基于椭球约束和点积法的标定与修正方法。首先设计了一套快捷高效的多位置采样策略，通过该正交采样策略进行数据采集，可以保证数据的高效性。接着通过椭球拟合法对加速度计和磁力计进行标定，得到初始标定参数，并将补偿后的加速度计坐标轴作为参考坐标轴。最后在椭球约束条件下，根据计算磁场矢量与水平面夹角均方差量化校正效果，对磁力计的轴间不正交角误差参数和传感器间的非对准误差参数进行标定与修正。通过进行仿真与实测试验，结果表明，在同等椭球拟合效果约束下，使用此方法后计算地磁矢量与水平面夹角均方差相比于点积不变法减小了25%以上，提高了磁力计的标定效果与非对准误差参数的准确性，该方法结合了静态位置法、椭球拟合法和点积不变法的优点，且操作过程简便，标定效果良好，具有一定的实际工程意义。该方法虽然有效降低了计算地磁矢量与水平面夹角的均方差，但未对零位误差及灵敏度误差进行修正，下一步将研究如何能够使用简单的方法对其进行修正，进一步提升标定效果。