1.一种基于九轴IMU的磁场自动标定方法,其特征在于,包括如下步骤:S1、数据采集,利用九轴IMU采集载体平台的相关数据,其中所述九轴IMU由三轴加速度计、三轴陀螺仪和三轴磁力计组成;
S2、数据滤波,对三轴加速度计的数据进行低通滤波,对三轴陀螺仪的数据进行带通滤波;
S3、姿态解算,依据三轴加速度计和三轴陀螺仪的数据,计算出载体平台的俯仰角和滚转角;
步骤S3中姿态解算的具体步骤如下:S31、姿态更新,将步骤S1中三轴陀螺仪测量的三轴角速度,代入四元数微分方程求解,根据龙哥库塔法,由陀螺仪积分更新四元数,设三轴陀螺仪测量的数据为g=(gx,gy,gz),采样时间为t,则:T
其中,w=sqrt(gx*gx+gy*gy+gz*gz),q′为更新后的四元数,q=[q0 q1 q2 q3] 为上一时刻的四元数;
S22、误差修正,采用EKF方法对误差进行修正,设修正的四元数向量为err_q,修正后的四元数为q″,则修正后真实的四元数为:q″=q′+err_q (2)S33、求解姿态角
根据步骤S32获得修正后的四元数后,求解出所需的姿态,并转化为欧拉角,计算公式为:其中pitch代表俯仰角,roll代表滚转角,yaw代表偏航角;
S4、磁力计倾斜补偿和模型优化,利用步骤S3求得的俯仰角和滚转角,将三轴磁力计的数据由载体坐标系,转换到导航坐标系下,从而将三轴磁力计在空间各方位旋转所围成的椭球图像转换为平面的椭圆;
步骤S4中磁力计倾斜补偿的具体步骤如下:设三轴磁力计k时刻的输出为 该输出是为载体坐标系的坐标,而根据磁力计航向计算公式:
其中,D为磁偏角,由查表获得,By,Bx分别为地磁场在水平方向上的投影,在导航坐标系内分解到水平y轴及x轴的正交分量;
利用步骤S23解算的姿态信息,转换到导航系下,得到By,Bx,计算公式为:分别为By,Bx在k时刻的值;
步骤S4中的模型优化的方法如下:
在理想情况下,当磁力计处于理想地磁场中时的输出满足:其中MMAG(mx,my,mz)为磁力计测量数据,G为理想地磁场;
由于量化因子的误差及零偏存在,因此磁力计的输出模型满足为:
2 2 2 2
(kx(mx+bx)) +(ky(my+by)) +(kz(mz+bz)) =G (7)其中,kx,ky,kz为尺度因子,即软铁误差,bx,by,bz为零偏,即硬铁误差;
利用姿态解算的俯仰、滚转角进行了倾斜补偿,将磁力计的输出模型简化为:
2 2 2
(k′xBx+b′x) +(k′yBy+b′y) =G′ (8)令
则得到如下优化模型:
S5、基于SVD分解的椭圆参数求解,根据椭圆目标优化函数,通过变形及变量变换,将步骤S4得到的椭圆转化为齐次方程组,利用SVD分解,求方程组的解;
步骤S5中基于SVD分解的椭圆参数求解方法如下:将(10)式通过变形及变量变换得到线性齐次方程组:AgV=0 (11)
其中
T
V=[a,b,c,d,e] (12)其中
向量V即该齐次方程的解,该解通过SVD分解的方法求得,向量V并不是方程组(10)的直接解,需要经过如下变换,求得磁力计校准参数k′x,k′y,b′x,b′y后,代入(10)中求得校准后的数据;
S6、计算航向角,步骤S5中求得的方程组的解,得到校准参数,根据校准参数,对磁力计数据修正,然后计算航向角。