一种新的电子罗盘校准算法研究*

时间：2024-05-22

晁正正,张晓明,马喜宏,吕辰,陈雷,李小燕

(中北大学电子测试技术科技重点实验室,太原 030051)

在地磁导航中,能够准确的获取地磁场信息至关重要,通常采用MEMS三轴磁强计(TAM)进行地磁场矢量测量,将采集到的地磁场矢量信息进行姿态解算或者以偏航角形式辅助惯性导航。由于TAM通常具有刻度系数、零点和三轴不正交性等制造误差,这些因素最终导致了电子罗盘具有大约1°～2°的误差。此外,TAM不可避免的受到周围铁磁材料干扰,严重影响电子罗盘航向角解算精度。针对这些误差,常用的TAM标定补偿技术有多传感器融合方法、椭圆拟合方法、椭球拟合方法等。多传感器融合方法融合了多个传感器获得的量测数据(含有TAM),但是标定结果严重受其他传感器数据影响;椭圆拟合只适用于二维平面运动下的标定,具有一定的局限性;而椭球拟合方法是一种新颖的标定方法,它将误差模型与椭球方程系数联系起来,从而将其系数标定出来进行磁补偿。传统的基于迭代递推思想的椭球拟合方法受初始状态影响易陷入局部最优解,从而导致磁补偿不准确。

针对上述问题,退火算法作为一种模拟进化算法,可用来误差模型参数的最优辨识,在跳出局部参数最优情况下实现全局参数最优估计,实现参数的准确估计[1-2]。因此本文提出了基于模拟退火算法的椭球拟合算法来提高电子罗盘测量精度,仿真和试验结果验证了本方法的有效性。

1 TAM的误差分析和建模

TAM的误差分为传感器误差、磁材料干扰误差和安装误差[3]。其中传感器误差由刻度系数、零点和不正角三种误差组成。磁材料干扰误差由软磁干扰和硬磁干扰误差组成。软磁干扰主要影响TAM刻度系数矩阵,硬磁干扰主要影响TAM零点偏移矩阵。

本文建立了所有误差源在内的TAM模型为:

(1)

考虑到这些误差之间相互作用对TAM的干扰[5],本文根据椭球拟合方法简化误差模型为:

(2)

式中:A为安装误差与不正交角误差矩阵;K为TAM的软磁干扰和刻度系数误差矩阵;P为TAM的硬磁干扰和零点偏移矩阵。通过标定出以上三个误差矩阵即可实现对TAM测量值的补偿[6-8]。

1.1 椭球体模型拟合

在轴间存在不正交角的情况下,TAM误差模型为:

(3)

当TAM在某一固定地理位置作各种姿态变化时,将地磁场矢量视为一常矢量,其磁场强度为一常值,因此有:

(4)

而椭球的一般方程为:

(5)

对比上式,设:

(6)

C2=-2KKTP=[lmn]T

(7)

用9个未知变量表示K矩阵和P矩阵则有:

(8)

K=VDV-1

(9)

(10)

式中:D=diag(d1d2d3)的元素是矩阵C1特征值的平方根,V是矩阵C1特征向量组成矩阵。因此TAM制造误差补偿步骤为:首先,由采集的原始数据确定式(6)中的椭球参数。然后用式(8)和式(9)确定矩阵K和P。最后通过式(10)补偿输出,得到了地磁场矢量测量信息的校准。

1.2 偏差模型

偏差模型中主要包括TAM轴间不正交角误差和系统安装误差。如图1所示,实线表示地心地固坐标系的轴线,虚线表示TAM实际三轴轴线。在地心地固坐标系中,z轴与传感器坐标系三轴之间夹角分别表示为a,b,c。当传感器在地心地固坐标系内绕z轴旋转时,TAM的测量值满足:

(11)

(12)

同理可得Rx和Ry,因此可以通过式(13)求出A,通过式(14)进行不正交角和系统安装误差角补偿得到更为精确地磁场矢量测量信息[9-12]。

A=[RxRyRz]

(13)

(14)

图1 三轴磁强计的非正交性和安装误差

2 模型参数的最优估计

模拟退火算法的原理和固体退火的原理近似:将热力学的理论套用到数据统计学上,随着固体被加热至最高温,其内部粒子变为杂乱无序状态,内能变大,在其渐渐冷却过程中,固体内部粒子逐渐趋于恒定状态,使其处于每个温度时都能达到平衡状态,最后在常温时达到基态,内能减为最小的状况类比为参数寻优中的收敛过程[13]。

根据Metropolis准则,模拟退火算法接受新解的概率为:

(15)

将内能E类比为评价函数f,温度T的下降类比为算法迭代次数k的增加,即得到求解参数优化问题的模拟退火算法:由初始解θ0和初始k开始,对当前解重复“产生参数新状态—计算评价函数差—接受或舍弃”的迭代,并逐步缓慢衰减T值,算法终止时的当前状态即为所得近似全局最优解。模拟退火过程由参数的初值T0,衰减因子R,每个T值时的迭代次数L和停止条件Tf参数控制。其总体流程图如图2所示。

图2 模拟退火算法流程图

2.1 评价函数

要准确的估计TAM软磁干扰和刻度系数误差矩阵,硬磁干扰和零点偏移向量有关的10个参数,利用最小二乘法构造评价函数:

(16)

2.2 模拟退火算法具体步骤

Step 1 状态初始化:初始温度T0,初始状态参数θ0,每个温度T下的迭代次数L。

Step 2 外循环:判断T

Step 3 内循环

Step 3.1 初始化内循环次数L=0。

Step 3.3L=L+1,判断L是否达到设定次数,若是则退出内循环,否则转Step 3.1。

Step 4 降温处理,令T=RT,本文取R=0.99,转Step 2。

3 试验及结果分析

3.1 计算机仿真实验

利用MATLAB进行仿真设置:利用IGRF模型及当地经纬度、海拔高度参数,得到当地地磁场三分量(单位:nT)为:He=[29 061.7 -2 684.3 45 662.8]T。仿真中令俯仰角、滚转角、偏航角分别做1°,300转弧度,0～360°;300转弧度,1°,0～360°;300转弧度,91°,0～360°角度变化,模拟标定实验过程。设置误差干扰参数a=1,b=1.5,c=2,d=1,e=1.5,f=2,l=1,m=1.5,n=2,生成磁干扰数据。设置初始参数θ0=[3 3 3 3 3 3 3 3 3 3]T,内循环设置为L=5,进行模拟退火椭球方程参数估计,估计结果如图3所示。

如图3所示,退火算法在初始温度较大情况下,参数收敛波动较大,随着温度T0呈指数下降,每一时刻温度T下接收新状态参数的概率越低,磁干扰误差参数趋于稳定,最后θopt=[1.009 3 1.500 7 2.003 3 1.002 4 1.498 4 2.0017 0.999 3 1.495 8 2.006 7 0.310 3]T,与初始设置的干扰误差系数基本一致,验证了此算法的有效性。

图3 模拟退火参数估计

将估计好的参数代入式(10),进行地磁场测量矢量的校准,由图4所示,椭圆体被校正成球形,软磁干扰和刻度系数误差得到了补偿校准;球型归于原点,硬磁干扰和零点偏移得到了补偿校准。补偿前后三轴误差均值为1 000 nT以内,方差为500 nT以内,此算法具有明显补偿效果,校准前后对比图如图4所示。

图4 磁场测量值补偿前后对比图

表1 三轴补偿前后误差均值与方差

3.2 半物理试验验证

本文使用的电子罗盘系统,其硬件系统包括基于ARM公司开发的Cortex-M4F内核的STM32F405RG主控芯片,具有32位的微处理器最高时钟可达168MHz,指令处理速度大210DMIPS,高主频可以保证信号采集中的快速获取。IMU模块使用MPU9255中的三轴加速度计和三轴陀螺仪,其中加速度计具有可编程参数选择±2gn,±4gn,±8gn,±16gn,本系统令AFS_SEL=1选择±4gn量程;陀螺仪具有可编程参数选择±250 °/s,±500 °/s,±1 000 °/s,±2 000 °/s,本系统令FS_SEL=1选择±500 °/s量程,分别具有16位AD输出数字量使其免去ADC采集精度的下降,而且陀螺仪和加速度计一体化结构可以避免繁琐的芯片选择和外设成本,保证最佳性能。TAM模块选择霍尼韦尔公司的HMC5983芯片,该芯片表面封装12位AD与地干扰AMR传感器,能在±8 gauss的磁场中实现2 mgauss的分辨率。主控芯片通过SPI模式将采集回来的加速度计,陀螺仪和磁强计数据存储到SD卡中进行数据存储。

本文实验地点在开阔地段任意旋转电子罗盘,以获得不同姿态下的地磁场测量数据。利用该算法准确拟合出估计参数,并绘制出蓝色原始数据和渐变紫色拟合椭球面的对照图。如图5所示,每个原始数据点几乎位于拟合椭球面上,表明椭球体拟合良好,即K和P可由式(8)和式(9)确定。

图5 原始数据和拟合椭球面

然后分别将系统的x、y和z轴调整为垂直向上,旋转电子罗盘系统,通过系统获得测量数据,由式(13)求得A。则原始数据可以通过式(10)和式(14)来修正。

如图6所示,红色虚线所在的椭圆体表示为原始数据拟合所得。底部的红色粗实线表示由z轴垂直方向旋转的原始数据所组成的圆。蓝色实线描绘了符合修正数据的球体。椭圆体明显地平移和拉伸,z轴最大值明显大于地磁场模值(54 193 nT)。但球体几乎是中心对称的,它的半径大约等于地磁场强度。虚线所在的平面明显不平行于x-y平面,也不正交于z轴。修正后的实线所在平面几乎与z轴正交。由图6可以看出,经过标定过程后,地磁场测量矢量的位置、方向和大小都得到了很好的校正。

图6 补偿前和补偿后数据

最后,我们将电子罗盘固定在无磁转台上,手动将转台打到俯仰角30°,保持水平旋转转台一周,每隔10°取点,共取36个点,做出补偿前后航向角误差曲线,如图7所示。由图7可知,通过补偿前后航向角与理论值的对比,实验表明电子罗盘受干扰模型中的误差对航向角的解算有很大影响,补偿前航向角最大误差为4.5°,补偿后的航向角误差为0.4°,补偿后航向角的精度提高了一个数量级。

图7 补偿前后航向角的误差曲线