本发明属于非线性最优估计和电子仪器测量标定与校正领域,具体涉及一种三轴磁力计自身误差在线校正方法。
背景技术:
在地质勘探、磁性目标探测、地磁导航与飞行控制等领域,经常需要利用三轴磁力计测量地磁场。然而,由于三轴磁力计的制造厂商加工工艺和安装工艺的不足,导致磁测结果存在偏差。主要误差因素有三轴非正交误差、灵敏度误差以及零偏误差。这些仪器误差在很大程度上影响了地磁场矢量的测量精度,有必要对三种仪器自身误差进行校正。
一项专利号为cn103885020b的专利提出了利用自适应遗传算法对三轴磁力计的非正交误差进行校正。利用该算法对实验操作和仪器设备的要求较低,可操作性好,但没有考虑零偏误差和灵敏度误差,综合误差补偿效果并不理想。龙达峰等人在《地磁传感器误差参数估计与补偿方法》一文中提出采用卡尔曼滤波器对误差参数进行估计,实现对三轴磁力计的标定,但是卡尔曼滤波对非线性系统的估计效果不佳。
本发明将地磁场矢量模的平方作为系统的观测量,观测方程具有较强的非线性,因此采用对非线性具有较好估计效果的自适应粒子滤波算法对三轴磁力计的误差参数进行估计,实现对三轴磁力计的在线标定与校正。本发明提出的自适应粒子滤波器可以根据粒子的估计状态自动调整样本数目,在保证滤波精度的同时,可以较快地估计传感器误差参数,提高了估计效率,具有较好的实时性效果。
技术实现要素:
本发明的目的在于提供在保证滤波精度的同时,可以较快地估计传感器误差参数,提高了估计效率,具有较好的实时性效果的一种三轴磁力计自身误差在线校正方法。
本发明的目的通过如下技术方案来实现:
一种三轴磁力计自身误差在线校正方法具体过程包括以下步骤:
步骤1,选定一个无外界干扰磁场存在的开阔区域作为三轴磁力计的校正区域,利用非铁质材料制作而成的安装基座将标量磁力计和三轴磁力计捷联于载体的尾部。
步骤2,分析三轴磁力计的主要传感器误差因素,推导含有零偏误差、灵敏度误差和非正交误差这三种误差的三轴磁力计对地磁场矢量的测量输出模型,如式(1)所示:
hm=khe+ho(1)
式中,
根据式(1)的三轴磁力计误差补偿模型,如式(2)所示:
he=k-1(hm-ho)(2)
步骤3,以三轴磁力计对地磁场矢量测量值模的平方(hm)thm作为滤波系统的观测量,则系统的观测方程为:
zm=(he)the-(hm)t(d-i)hm+2(ho)tdhm-(ho)tdho+υ(t)(3)
式中,(he)the为用标量磁力计测得真实地磁场矢量模的平方,d=(k-1)tk-1,观测噪声υ(t)为零均值的高斯噪声。
将待估计的三轴磁力计自身误差参数作为状态量:
x=[x1,x2,x3,x4,x5,x6,x7,x8,x9](4)
式中,ho=[x1,x2,x3]t,xi∈(500nt,650nt),i={1,2,3},
待估计的参数在标定过程中保持不变,因此三轴磁力计误差参数的粒子滤波估计系统的状态方程为:
x(k)=x(k-1)+ω(k)(5)
式中,状态噪声ω(k)为均值为零的高斯噪声。
步骤4,利用自适应粒子滤波算法对三轴磁力计误差参数进行估计,得到误差参数的向量值,具体过程如下:
step1k=0时,初始化待估9维参数向量得到x0,给出置信区间
step2令k=k+1,载体在空间中作0°~360°的全姿态运动,捷联于载体的三轴磁力计获得此时刻地磁场矢量的测量值hmk,以||hmk||2的计算值作为系统的观测量;
step3重要性采样:根据重要性函数从先验粒子中采样抽取n个样本,根据式(3)计算此时的预测值yk|k-1;根据式(6)估计重要性权值。
先验概率作为重要性密度函数:
a.根据先验概率分布来更新粒子
b.记录最大权值
c.根据公式(10)确定自适应系数β,然后利用公式(11)重新计算权值;
式中,ε为阈值,根据经验确定;k为比例常数,k/α>0,α为精度因子。
step4按公式(12),对权值进行归一化处理;
step5重采样:将原来的具有较小权值的带权样本
step6预测样本数:结合当前k时刻的观测值来估计k+1时刻的状态误差的方差
式中,l为置信区间的长度,rα/2为n-1个自由度的t分布的上界值。
step7参数估计:利用式(14)进行当前时刻的状态参数估计;
step8k=k+1,回到step2。
步骤5,将三轴磁力计的误差参数估计值代入公式(2)所示的误差补偿模型,计算得到校正后的地磁场矢量测量值。
本发明的有益效果在于:
为了解决三轴磁力计自身误差带来的磁测结果误差较大的问题,本发明提出了利用自适应粒子滤波算法对三轴灵敏度误差、零偏误差和非正交误差进行参数估计,利用这些参数估计值对地磁场矢量进行在线补偿。本发明将似然分布自适应与样本自适应结合起来,根据参数估计状态的方差来确定样本数的下限,在重采样阶段嵌入似然采样,根据反映观测噪声实时统计性能的精度因子的大小来自适应地调节似然分布的状态,增加了先验和似然的重叠区,提高了参数估计精度;达到了三轴磁力计自身误差参数估计与校正的目的,提高了地磁场矢量的测量精度。
附图说明
图1为在线校正算法的具体流程示意图;
图2为三轴磁力计坐标系与正交系的误差角示意图;
图3为校正之前的三轴磁力计测量值的仿真结果;
图4为校正之前的地磁场矢量模的测量误差;
图5.1为自适应粒子滤波对零偏误差参数的估计结果;
图5.2为自适应粒子滤波对零偏误差参数的估计值的误差;
图6.1为自适应粒子滤波对灵敏度误差参数的估计结果;
图6.2为自适应粒子滤波对灵敏度误差参数的估计值的误差;
图7.1为自适应粒子滤波对非正交误差角参数的估计结果;
图7.2为自适应粒子滤波对非正交误差角参数的估计值的误差;
图8.1为自适应粒子滤波在线校正后的地磁场矢量的测量值;
图8.2为自适应粒子滤波在线校正后的地磁场矢量的测量值误差;
图9为自适应粒子滤波在线校正后的地磁场矢量模的测量误差。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明:
步骤1,选定一个无外界干扰磁场存在的开阔区域作为三轴磁力计的校正区域,利用非铁质材料制作而成的安装基座将标量磁力计和三轴磁力计捷联于载体的尾部。
步骤2,分析三轴磁力计的主要传感器误差因素,推导含有零偏误差、灵敏度误差和非正交误差这三种误差的三轴磁力计对地磁场矢量的测量输出模型,如式(1)所示:
hm=khe+ho(1)
式中,
根据式(1)的三轴磁力计误差补偿模型,如式(2)所示:
he=k-1(hm-ho)(2)
步骤3,以三轴磁力计对地磁场矢量测量值的模平方(hm)thm作为滤波系统的观测量,则系统的观测方程为:
zm=(he)the-(hm)t(d-i)hm+2(ho)tdhm-(ho)tdho+υ(t)(3)
式中,(he)the为用标量磁力计测得真实磁场的总场值的平方,d=(k-1)tk-1,观测噪声υ(t)为均值为零的高斯噪声。
将待估计的三轴磁力计自身误差参数作为状态量:
x=[x1,x2,x3,x4,x5,x6,x7,x8,x9](4)
式中,ho=[x1,x2,x3]t,xi∈(500nt,650nt),i={1,2,3},
待估计的参数在标定过程中保持不变,因此三轴磁力计误差参数的粒子滤波估计系统的状态方程为:
x(k)=x(k-1)+ω(k)(5)
式中,状态噪声ω(k)为零均值的高斯噪声。
步骤4,利用自适应粒子滤波算法对误差参数进行估计,得到误差参数的向量值,具体过程如下:
step1k=0时,初始化待估参数为x0=[0,0,0,0,0,0,0,0,0],置信区间设为
step2令k=k+1,载体在空间中作0°~360°的全姿态运动,捷联于载体的三轴磁力计获得此时刻地磁场矢量的测量值hmk,||hmk||2的计算值作为系统的观测量;
step3重要性采样:根据重要性函数从先验样本中采样抽取n个样本,根据式(3)计算此时的观测值yk|k-1;根据式(6)估计重要性权值。
先验概率作为重要性密度函数:
a.根据先验概率分布来更新粒子
b.记录最大权值
c.根据公式(10)确定自适应系数β,然后利用公式(11)重新计算权值;
式中,ε为阈值,根据经验确定;k为比例常数,k/α>0,α为精度因子。
step4按公式(12),对权值进行归一化处理;
step5重采样:将原来的具有较小权值的带权样本
step6预测样本数:结合当前k时刻的观测值来估计k+1时刻的状态误差的方差
式中,l为置信区间的长度,rα/2为n-1个自由度的t分布的上界值。
step7参数估计:利用式(14)进行当前时刻的状态参数估计;
step8k=k+1,回到step2;
步骤5,将得到的三轴磁力计的误差参数的估计值代入公式(2)误差补偿模型中,得到校正后的地磁场矢量测量值。
设定||he||=40000nt,ho=[550nt,500nt,600nt]t,灵敏度误差为s=[0.05,0.04,0.06],非正交误差为θ=[5°,8°,3°],初始化待估参数为x0=[0,0,0,0,0,0,0,0,0],置信区间设为
仿真得到补偿前的地磁场矢量值及地磁场矢量模的误差值,分别如图3和图4所示。自适应粒子滤波对零偏误差参数、灵敏度误差参数及非正交误差角参数的估计结果分别如图5.1、6.1和7.1所示。自适应粒子滤波在线校正后的地磁场矢量的测量值及矢量值误差分别如图8.1和8.2所示。
从仿真情况来看,在三个误差因素的影响下,补偿之前地磁场矢量模的误差最大值达到了6000nt;通过自适应粒子滤波进行估计,得到的估计结果的误差分别为
从补偿后的地磁场矢量值误差图8.2可以看出,将矢量值误差控制在60nt以内。利用自适应滤波算法能在第100个采样点附近得到准确的估计值,所以该算法能较快地对传感器误差参数进行估计,以便较快对地磁场矢量测量进行补偿,自适应粒子滤波估计算法具有很好的实时性能。图9为校正后的地磁场强度测量误差,从图9所示的结果看,测量误差已降低到70nt以内;因此,自适应粒子滤波估计算法能对三轴磁力计自身误差参数进行在线估计与校正,提高地磁场的测量精度。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。