技术领域
本发明属于磁力计标定领域,特别涉及姿态角已知时磁力计标定方法。
背景技术
磁力计是用来测量磁感应强度的传感器,它可以提供对磁北的参考,是低成本高性能导航系统中用于姿态估计的关键信息源。磁力计由于加工工艺的限制,会存在零偏误差、交轴耦合误差、比例因子误差等问题,影响磁力计的测量精度;而且磁力计容易受到周围环境的磁干扰,这些干扰分为软磁干扰和硬磁干扰。因此为测量更准确的磁场信息,需对磁力计进行标定。此外,在多源融合传感器定位中,将磁力计和惯性传感器的信息放在一个公共框架中是实现高精度定位的基础,但由于安装工艺、软磁干扰等原因,磁力计与惯性传感器的轴系难以实现完全一致,故磁力计与惯性传感器进行交叉对准标定是一个关键点和亟需解决的问题。
目前根据国内外的文献来看,基于极大似然估计法的椭球拟合是目前最常用的磁力计内在校准方法,它的标定与姿态无关,利用了磁力计在局部位置或均匀磁场中的测量值无论方向如何都是恒定的这一事实。对于传感器之间的交叉标定,主要通过依靠局部重力信息完成,但这种标定方法依赖加速计测量值。
综上,在磁力计标定时,如何快速完成磁力计内在标定及交叉标定是需要解决的关键问题。本发明以最小二乘法,包括但不限于此应用场景,无需依赖姿态、重力信息,提出一种简单有效的磁力计标定及与惯性传感器交叉标定方法。
发明内容
针对磁力计快速校准的需求以及磁力计轴系与IMU惯性传感器轴系不对齐的问题,本发明提出了一种在姿态角已知的情况下在基于最小二乘原理完成磁力计的内在标定以及IMU惯性传感器的交叉标定,即可以快速实现标定磁力计交轴耦合误差、比例因子误差、零位偏置误差、软磁误差、硬磁误差,并实现磁力计与IMU惯性传感器轴系对齐。本发明无需借助任何外界设备,无需参数设置,简单可行,具有很好的普适性,同时能够满足现场快速(20s左右)标定的要求,具有良好的标定精度。
本发明采用如下的技术方案:一种基于最小二乘方法完成磁力计的内在标定以及与IMU惯性传感器的轴系对齐,主要是通过旋转智能手机完成标定,在基于某一位置的地磁场强度在当地坐标系下三轴磁场分量不变及磁场总强度不变的假设下,通过对磁力计的测量模型进行建模,使用泰勒展开及最小二乘方法,得到磁力计标定参数;所述的技术方案包括以下步骤:
步骤1,在室外空旷区域选择任意选择一个位置,并根据位置的经纬度坐标查询地球磁场参考模型,得到该位置的磁场总强度参考值;
步骤2,以传感器测量中心为旋转中心,使其绕着载体坐标系的X、Y、Z三轴各旋转多圈,并获取标定过程中每个历元的姿态角(即俯仰角、横滚角、航向角)及磁场信息;
步骤3,建立磁力计测量模型,确定待估参数,将磁力计的测量模型进行泰勒展开,使用最小二乘法迭代求得待估参数。
进一步的,对磁力计的测量模型进行建模时,考虑磁力计受到软磁误差、比例因子误差、交轴耦合误差、零位偏置误差、硬铁误差及传感器噪声,载体坐标系下磁力计的测量模型如下:
对磁力计原始观测数据m
其中,b表示载体坐标系b系,b系是以惯性传感器IMU相位中心为原点,x轴指向载体前进方向,y轴垂直于x轴指向载体右侧,z轴与x轴和y轴垂直并构成右手系;
进一步的,对磁力计测量模型进行迭代计算时,首先以m
式中,k表示第k个历元,j表示历元的总个数;
在基于磁强总强度不变的假设下,根据M
其中,
进一步的,通过一阶泰勒展开将简化模型
待估参数
将式子
待求参数共计12个,记为X,
X=[a
由于该式子为非线性,按泰勒公式展开,舍弃二阶项和高阶项,上标i表示迭代次数,如下式:
其中Δ表示残差,令:
令:x
式(7)简化为:
令
通过最小二乘法即可求式(9)的解算结果,各观测值等权,P为单位阵,求得
x=(H
X
式中,X
进一步的,最小二乘法的初值通过以下方式获取,
对于
对于B
将原始数据m
当本次迭代求得的B
本发明具有以下有益效果:
(1)本发明在获取高精度姿态角的基础上完成了磁力计的内在标定及与惯性传感器的交叉标定,可以减少零偏误差、交轴耦合误差、比例因子误差、软磁干扰和硬磁干扰以及磁力计与惯性传感器的轴系不对齐误差,标定效果较好。
(2)本发明操作简单,易于实现,操作时间短。只需以智能手机(即载体)的中心为旋转中心,绕着载体坐标系的X、Y、Z三轴各旋转720°即可完成标定工作,对姿态无要求,标定效率较高。
(3)本发明实现了将磁力计坐标系与惯性传感器坐标系对齐,使得磁力计和惯性传感器的信息放在一个公共框架中,可以在传感器阵列发挥重要作用。
附图说明
图1为手机标定动作示意图。
图2为迭代求解磁力计标定参数的流程图。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步说明。
步骤1,选择室外某一空旷位置作为标定位置,获取该位置的经纬度坐标及高度,通过查询国际地磁参考场IGRF(International Geomagnetic Reference Field)模型获取该位置的磁场强度M
步骤2,如图1所示,以智能手机(即载体)的中心为旋转中心,使其绕着载体坐标系的X、Y、Z三轴各旋转720°;
步骤3,通过专利“MEMS陀螺自动标定方法”的方法,通过伪位置约束和反向平滑获取该标定过程中高精度的每个历元的载体坐标系到当地坐标系的姿态旋转矩阵
步骤4,建立磁力计测量模型,建立当地坐标系下磁场强度与载体坐标系下磁场强度之间的关系,通过对磁力计测量模型的迭代计算得到标定参数,计算流程如图2所示;
而且,步骤4的实现方式包括以下子步骤,
41)对磁力计的测量模型进行建模。受制造工艺、加工精度等的影响,磁力计存在交轴耦合误差、比例因子误差、零位偏置误差及传感器噪声。此外,磁力计容易受到周围环境的磁干扰,这些干扰分为软磁干扰和硬磁干扰,它们的表现形式与交轴耦合误差、比例因子误差、零位偏置误差相同。故需同时完成上述误差的标定。此外,还存在磁力计轴系与惯性传感器轴系不对齐误差。本发明认为载体坐标系与惯性传感器坐标系轴系相同,故在载体坐标系下磁力计的测量模型如下:
式中,b表示载体坐标系b系,b系是以惯性传感器IMU相位中心为原点,x轴指向载体前进方向,y轴垂直于x轴指向载体右侧,z轴与x轴和y轴垂直并构成右手系;n表示当地坐标n系,n系是以惯性传感器IMU相位中心为原点,x轴平行于当地水平面指向正北,y轴平行于当地水平面指向正东,z轴垂直于当地水平面向下,三者构成右手系;
42)对磁力计原始观测数据m
43)将式(1)中
44)以m
式中,k(k=1,2...j)表示第k个历元,j表示历元的总个数;
45)在基于磁强总强度不变的假设下,根据M
其中,
46)对简化后的模型(式2)进行一阶泰勒展开,完成对模型的线性化处理,然后用最小二乘法求出待估参数
待估参数
将式子
待求参数共计12个,记为X,
X=[a
由于该式子为非线性,按泰勒公式展开,舍弃二阶项和高阶项,上标i表示迭代次数,如下式:
其中Δ表示残差,令:
令:x
式(7)简化为:
令
通过最小二乘法即可求式(9)的解算结果,本发明认为各观测值等权,P为单位阵,求得
x=(H
X
X
进一步的,最小二乘法的初值通过如下方式获得,
由于
对于B
对于手机磁力计而言,零偏造成的误差远大于交轴耦合和比例因子等的误差,因此在进行泰勒展开获取初值时,只考虑磁力计受零偏误差影响,故进一步简化磁力计的测量模型为:
将磁力计原始观测数据m
在基于磁强总强度不变的假设下,根据M
根据每个历元的
将原始数据m
当本次迭代求得的B
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代,但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
机译: 基于磁力计测量的姿态故障检测方法
机译: 基于姿态标定的步长推荐方法及系统
机译: 飞机制动方法,涉及通过获取对俯仰进行正向计算的飞机姿态来适应飞机控制逻辑的破坏控制,并将基于姿态的制动控制修改为另一种制动控制