陈 雷，卞鸿巍

(海军工程大学 导航工程系，武汉 430033)



【基础理论与应用研究】

基于模糊自适应互补滤波的姿态解算算法

陈 雷，卞鸿巍

(海军工程大学 导航工程系，武汉 430033)

在互补滤波姿态解算算法中，如果将滤波器参数设置为固定值，加速度计测量值包含振动噪声和运动加速度会影响姿态解算效果；针对此问题，首先根据加速度计测量值的模值对姿态解算算法进行改进，然后根据加速度计测量值求取的误差能反应载体运动状态这一特性，提出了一种模糊自适应互补滤波姿态解算算法；仿真试验表明：这种解算算法能实时调整滤波参数，相对于互补滤波姿态解算算法，姿态解算效果更佳。

模糊自适应；姿态解算算法；互补滤波

1 改进互补滤波姿态求解器模型

改进的姿态解算互补滤波器结构图[7]，如图1所示。其中，ab、mb、ωb分别为加速度计、陀螺仪、磁传感器经过标较后的数据[8-10]，γ、θ、φ分别为互补滤波器输出俯仰角、横滚角和航向角。为了消除滤波器的静态误差，取C(s)=kp+ki/s，其中，kp决定了滤波器的截止频率，ki决定了滤波器消除静态误差的时间。由于加速度计和磁传感器具有不同的响应特性，为了滤波器获得较好的效果，针对加速度计和磁传感器分别设计比例增益kp和积分增益ki。

图1 改进姿态解算互补滤波器结构

由于低成本陀螺仪漂移比较大，需要对陀螺仪进行校准。当载体处于平稳状态时，加速度计测量的重力矢量与互补滤波器估计的重力矢量在载体坐标系的投影叉乘Paccerror，可以表征陀螺仪X/Y轴的误差。……