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

陈 雷，卞鸿巍

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



【基础理论与应用研究】

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

陈 雷，卞鸿巍

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

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

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

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

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

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

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

(1)

(2)

由于本文采用PI控制来对误差进轴漂移和磁传感器补偿陀螺仪Z轴漂移，分别设置了两组不同的ki和kp，则有：

(3)

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

图2 模糊自适应互补滤波姿态解算算法结构框图

图的隶属度函数

图的隶属度函数

图的隶属度函数

表和模糊规则

由表1可针对X轴模糊控制器制定如下模糊规则：

同理，由表2可针对Y轴模糊控制器制定如下模糊规则：

3 仿真试验验证

使用Matlab仿真数据，验证模糊自适应互补滤波的效果，仿真数据设置如下：

1) 仿真数据时间为60 s；航向角数据模拟以幅值为30°，周期为2π正弦函数摇摆；俯仰角数据模拟以幅值为45°，周期为2π正弦函数摇摆；横滚角数据模拟以幅值为50°，周期为2π正弦函数摇摆；

3) 陀螺仪零偏设为10°/h，随机噪声方差设为5°/h；

4) 由于固定载体磁环境比较稳定，在三轴磁传感器理想数据归一化后，加随机噪声和零偏，随机噪声的增益和零偏增益均为0.005;

5) 设置数据采样频率为100Hz。

常规互补滤波器和模糊自适应互补滤波器解算的姿态误差如图6～图8所示。

图6 俯仰角误差

图7 横滚角误差

图8 航向角误差

由仿真得到的常规互补滤波器和模糊自适应互补滤波器解算的姿态误差对比曲线，与表3姿态误差特性对比，可知模糊自适应互补滤波器解算的姿态误差峰峰值和方差均小于常规互补滤波器解算的姿态误差峰峰值和方差，故在阈值判断的基础上，模糊自适应互补滤波器性能更佳。当载体运动状态比较复杂时，模糊自适应滤波器能实时调整滤波参数，从而获取较为精确的姿态信息。

表3 姿态误差特性

4 结论

本文针对常规互补滤波姿态解算算法存在的问题进行了多次改进，首先优化加速度计测量值的模值，在此基础上，设计了模糊自适应互补滤波姿态解算算法，根据加速度求取的误差实时调整滤波器参数。仿真结果表明：提出的改进方法及提出的模糊自适应互补滤波姿态解算算法效果更优。

[1] 丁君.AHRS航姿解算中的两种滤波方法的比较研究[J].计算机仿真，2013,30(9)：60-63.

[2] 廖懿华，张铁民.基于模糊比例积分偏差修正的多旋翼飞行器姿态测算系统[J].农业工程学报，2014，30(20)：19-26.

[3] 梁延德,程敏.基于互补滤波器的四旋翼飞行器姿态解算[J].传感器与微系统， 2011，30(11)：56-61.

[4] 余彦霖，祖家奎.微小型无人直升机姿态信号的互补滤波融合算法[J].直升机技术，2014，180(3)：13-17.

[5] 史智宁，韩波.基于重力场自适应互补滤波的无人直升机水平姿态估计方法[J].传感器技术学报，2009，22(7):993-993.

[6] 阎世梁.基于改进互补滤波器的低成本微小飞行器姿态估计方法[J].计算机应用，2013，33(7):2078-2082.

[7] 彭孝东，张铁民，任章.三轴数字MEMS加速度计现场标定方法[J].震动、测试与诊断，2014，34(3)：543-548.

[8] 宋丽君，秦永元.MEMS陀螺仪的一种实用标定法[J].压电与声光，2010，32(3)：372-375.

[9] 戴磊，旋翼飞行机器人磁罗盘误差分析及校准[J].旋翼无机器人，2012，34(4)：418-423.

[10]李翔.航姿参考系统三轴磁强计校正的点积不变法[J].仪器仪表学报，2012，33(8)：1813-1817.

(责任编辑 唐定国)

Attitude Algorithm Based on Fuzzy Adaptive Complementary Filter

CHEN Lei, BIAN Hong-wei

(Department of Navigation Engineering，Naval University of Engineering，Wuhan 430033, China)

The accelerometers measurement values contain noises and motion accelerations, and if the parameters of the complementary filter are set to a group of fixed values, attitude solution results will be affected. To solve this problem, the modulus values of accelerometer were used to optimize attitude algorithm, and then a fuzzy adaptive complementary filter attitude algorithm was proposed according to the properties that the error calculated from accelerometer that can reflect the movement state of carrier. Simulation results show that this attitude algorithm can adjust the parameters of filter in real time and it has a better effect than traditional complementary filter attitude algorithm.

fuzzy adaptive; attitude algorithm; complementary filter

2016-08-25；

2016-10-10

国家自然科学

基金项目(41506220)

陈雷(1992—)，男，硕士研究生，主要从事导航制导与控制研究。

10.11809/scbgxb2017.04.038

陈雷，卞鸿巍.基于模糊自适应互补滤波的姿态解算算法[J].兵器装备工程学报，2017(4):183-187.

format:CHEN Lei, BIAN Hong-wei.Attitude Algorithm Based on Fuzzy Adaptive Complementary Filter[J].Journal of Ordnance Equipment Engineering,2017(4):183-187.

U666

A

2096-2304(2017)04-0183-05