航向姿态参考系统(Attitude and Heading Reference System,AHRS)能够提供航向、横滚和侧翻等姿态信息,机械陀螺仪及光纤陀螺仪等高精度惯性导航器件价格昂贵,难以得到推广。目前MEMS传感器在消费类电子产品中得到广泛应用,但是MEMS角速率陀螺仪存在严重的零点漂移和随机误差,在捷联惯性导航解算中会产生积分误差,难以达到应用的精度。加速度计和磁场计能分别测量出重力加速度和地磁场这两个不相关的三维矢量,可以作为平台姿态的观测矢量来校准陀螺仪。扩展卡尔曼滤波可以结合这几种传感器的特点,以陀螺仪测量得到的角速率作预测更新,以重力加速度和磁场观测更新,得到更高精度的姿态角信息。
AHRS的软件设计主要分为:
① 传感器初始化,包括设置传感器的更新速率、量程。
② 初始化卡尔曼滤波的相关矩阵,根据传感器的特点设置过程激励噪声协方差矩阵Q,设为对角元素为0.1的四维对角方阵。
③ 若成功读取陀螺仪数据,进行卡尔曼滤波的时间更新。
④ 采集加速度传感器和磁阻传感器的数据,若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在于观测方差的R,可根据两种传感器的置信度设置相应的值,航向姿态参考系的程序流程如图2所示。