微型姿态解算系统的抗磁干扰算法研究

　　0　引　　言

　　与第一代机械转子陀螺惯性测量组件、第二代光电陀螺惯性测量组件相比,微机械陀螺(MEMS陀螺)具有体积小、重量轻、功耗少、成本低、集成化程度高等优点[1]。在汽车工业、人体姿态跟踪和室内机器人等民用领域需求的推动下,MEMS陀螺已经获得了很大发展。但是,其较低的精度和固有的陀螺漂移会令其短时间内产生很大的姿态

　　偏差,所以MEMS陀螺的应用受到很大制约。为了解决这个问题,可以采用基于MARG传感器的卡尔曼滤波来进行姿态解算。近年来,利用MARG传感器进行姿态解算方面取得了一些成果。2001年,Marins等人[2]采用MARG传感器基于扩展卡尔曼滤波算法(EKF)提出了四元数扩展卡尔曼滤波器。在此滤波器设计的基础上,Yun和Bachmann[3]利用QUEST算法取代Gauss-Newton算法,并利用基于四元数的卡尔曼滤波器来进行人体运动的实时跟踪。

　　上述算法虽然能够解算出载体的姿态,但是他们都没有考虑补偿地磁测量中所受到的干扰。由于地磁场强度很弱,所以容易受到外界磁场的干扰。当存在干扰磁场时,磁强计的数据反而会使姿态解算产生较大误差。本文利用基于四元数的扩展卡尔曼滤波[4-9]算法,通过对磁强计输出的磁数据利用抗磁干扰算法,有效地抑制了外界干扰磁场产生的误差。最后通过仿真实验验证了该方法的正确性。

　　1　基于MARG传感器的EKF算法

　　MARG传感器是1种微小型三自由度传感器,由相互正交的3个陀螺仪、3个加速度计和3个磁强计组成[10]。陀螺仪动态测量载体的角速度,并通过积分得到载体的绝对姿态,但误差随时间累积。加速度计能测量重力方向,同时还有载体运动的加速度。磁强计用以测量地磁场的大小和方向,动态性能较差。基于MARG传感器的微型姿态解算系统可以利用加速度计和磁强计对地球重力场和地磁场的测量结果来抑制陀螺漂移导致的误差累积,利用陀螺来保证系统的动态性能及稳定性。MARG传感器的输出数据通过Kalman滤波器融合得到姿态的最优估计,既保证了姿态测量的精度,又保持了惯性系统自主工作的特点[11]。本文采用的姿态解算系统是Yun和Bachmann在文献[3]中提出的,该系统的结构如图1所示。首先利用QUEST算法处理磁强计数据和加速度计数据得到1个四元数qma,然后四元数qma再与MEMS陀螺测量的角速率解算得到的四元数qω利用EKF算法融合得出最后的姿态。

　　取状态向量x是由陀螺的三轴角速率ω和四元数qω的4个元组成的7维向量,即x= [ωx,ωy,ωz,q0,q1,q2,q3]T。而观测向量是由测量到的三轴角速率ωc及加速度计和磁强计解算得到的四元数qma的4个元组成的7维向量,即z=[ωcx,ωcy,ωcz,qma0,qma1,qma2,qma3]T。该系统的状态方程和测量方程分别为: