基于模糊自適應(yīng)互補(bǔ)濾波的姿態(tài)解算算法

陳 雷，卞鴻巍

(海軍工程大學(xué) 導(dǎo)航工程系，武漢 430033)

?

【基礎(chǔ)理論與應(yīng)用研究】

基于模糊自適應(yīng)互補(bǔ)濾波的姿態(tài)解算算法

陳 雷，卞鴻巍

(海軍工程大學(xué) 導(dǎo)航工程系，武漢 430033)

在互補(bǔ)濾波姿態(tài)解算算法中，如果將濾波器參數(shù)設(shè)置為固定值，加速度計(jì)測量值包含振動(dòng)噪聲和運(yùn)動(dòng)加速度會(huì)影響姿態(tài)解算效果；針對此問題，首先根據(jù)加速度計(jì)測量值的模值對姿態(tài)解算算法進(jìn)行改進(jìn)，然后根據(jù)加速度計(jì)測量值求取的誤差能反應(yīng)載體運(yùn)動(dòng)狀態(tài)這一特性，提出了一種模糊自適應(yīng)互補(bǔ)濾波姿態(tài)解算算法；仿真試驗(yàn)表明：這種解算算法能實(shí)時(shí)調(diào)整濾波參數(shù)，相對于互補(bǔ)濾波姿態(tài)解算算法，姿態(tài)解算效果更佳。

模糊自適應(yīng)；姿態(tài)解算算法；互補(bǔ)濾波

1 改進(jìn)互補(bǔ)濾波姿態(tài)求解器模型

改進(jìn)的姿態(tài)解算互補(bǔ)濾波器結(jié)構(gòu)圖[7]，如圖1所示。其中，ab、mb、ωb分別為加速度計(jì)、陀螺儀、磁傳感器經(jīng)過標(biāo)較后的數(shù)據(jù)[8-10]，γ、θ、φ分別為互補(bǔ)濾波器輸出俯仰角、橫滾角和航向角。為了消除濾波器的靜態(tài)誤差，取C(s)=kp+ki/s，其中，kp決定了濾波器的截止頻率，ki決定了濾波器消除靜態(tài)誤差的時(shí)間。由于加速度計(jì)和磁傳感器具有不同的響應(yīng)特性，為了濾波器獲得較好的效果，針對加速度計(jì)和磁傳感器分別設(shè)計(jì)比例增益kp和積分增益ki。

圖1 改進(jìn)姿態(tài)解算互補(bǔ)濾波器結(jié)構(gòu)

由于低成本陀螺儀漂移比較大，需要對陀螺儀進(jìn)行校準(zhǔn)。當(dāng)載體處于平穩(wěn)狀態(tài)時(shí)，加速度計(jì)測量的重力矢量與互補(bǔ)濾波器估計(jì)的重力矢量在載體坐標(biāo)系的投影叉乘Paccerror，可以表征陀螺儀X/Y軸的誤差。如式(1)所示：

(1)

(2)

由于本文采用PI控制來對誤差進(jìn)軸漂移和磁傳感器補(bǔ)償陀螺儀Z軸漂移，分別設(shè)置了兩組不同的ki和kp，則有：

(3)

2 模糊自適應(yīng)互補(bǔ)濾波姿態(tài)解算算法

圖2 模糊自適應(yīng)互補(bǔ)濾波姿態(tài)解算算法結(jié)構(gòu)框圖

圖的隸屬度函數(shù)

圖的隸屬度函數(shù)

圖的隸屬度函數(shù)

表和模糊規(guī)則

由表1可針對X軸模糊控制器制定如下模糊規(guī)則：

同理，由表2可針對Y軸模糊控制器制定如下模糊規(guī)則：

3 仿真試驗(yàn)驗(yàn)證

使用Matlab仿真數(shù)據(jù)，驗(yàn)證模糊自適應(yīng)互補(bǔ)濾波的效果，仿真數(shù)據(jù)設(shè)置如下：

1) 仿真數(shù)據(jù)時(shí)間為60 s；航向角數(shù)據(jù)模擬以幅值為30°，周期為2π正弦函數(shù)搖擺；俯仰角數(shù)據(jù)模擬以幅值為45°，周期為2π正弦函數(shù)搖擺；橫滾角數(shù)據(jù)模擬以幅值為50°，周期為2π正弦函數(shù)搖擺；

3) 陀螺儀零偏設(shè)為10°/h，隨機(jī)噪聲方差設(shè)為5°/h；

4) 由于固定載體磁環(huán)境比較穩(wěn)定，在三軸磁傳感器理想數(shù)據(jù)歸一化后，加隨機(jī)噪聲和零偏，隨機(jī)噪聲的增益和零偏增益均為0.005;

5) 設(shè)置數(shù)據(jù)采樣頻率為100Hz。

常規(guī)互補(bǔ)濾波器和模糊自適應(yīng)互補(bǔ)濾波器解算的姿態(tài)誤差如圖6～圖8所示。

圖6 俯仰角誤差

圖7 橫滾角誤差

圖8 航向角誤差

由仿真得到的常規(guī)互補(bǔ)濾波器和模糊自適應(yīng)互補(bǔ)濾波器解算的姿態(tài)誤差對比曲線，與表3姿態(tài)誤差特性對比，可知模糊自適應(yīng)互補(bǔ)濾波器解算的姿態(tài)誤差峰峰值和方差均小于常規(guī)互補(bǔ)濾波器解算的姿態(tài)誤差峰峰值和方差，故在閾值判斷的基礎(chǔ)上，模糊自適應(yīng)互補(bǔ)濾波器性能更佳。當(dāng)載體運(yùn)動(dòng)狀態(tài)比較復(fù)雜時(shí)，模糊自適應(yīng)濾波器能實(shí)時(shí)調(diào)整濾波參數(shù)，從而獲取較為精確的姿態(tài)信息。

表3 姿態(tài)誤差特性

4 結(jié)論

本文針對常規(guī)互補(bǔ)濾波姿態(tài)解算算法存在的問題進(jìn)行了多次改進(jìn)，首先優(yōu)化加速度計(jì)測量值的模值，在此基礎(chǔ)上，設(shè)計(jì)了模糊自適應(yīng)互補(bǔ)濾波姿態(tài)解算算法，根據(jù)加速度求取的誤差實(shí)時(shí)調(diào)整濾波器參數(shù)。仿真結(jié)果表明：提出的改進(jìn)方法及提出的模糊自適應(yīng)互補(bǔ)濾波姿態(tài)解算算法效果更優(yōu)。

[1] 丁君.AHRS航姿解算中的兩種濾波方法的比較研究[J].計(jì)算機(jī)仿真，2013,30(9)：60-63.

[2] 廖懿華，張鐵民.基于模糊比例積分偏差修正的多旋翼飛行器姿態(tài)測算系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報(bào)，2014，30(20)：19-26.

[3] 梁延德,程敏.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng)， 2011，30(11)：56-61.

[4] 余彥霖，祖家奎.微小型無人直升機(jī)姿態(tài)信號的互補(bǔ)濾波融合算法[J].直升機(jī)技術(shù)，2014，180(3)：13-17.

[5] 史智寧，韓波.基于重力場自適應(yīng)互補(bǔ)濾波的無人直升機(jī)水平姿態(tài)估計(jì)方法[J].傳感器技術(shù)學(xué)報(bào)，2009，22(7):993-993.

[6] 閻世梁.基于改進(jìn)互補(bǔ)濾波器的低成本微小飛行器姿態(tài)估計(jì)方法[J].計(jì)算機(jī)應(yīng)用，2013，33(7):2078-2082.

[7] 彭孝東，張鐵民，任章.三軸數(shù)字MEMS加速度計(jì)現(xiàn)場標(biāo)定方法[J].震動(dòng)、測試與診斷，2014，34(3)：543-548.

[8] 宋麗君，秦永元.MEMS陀螺儀的一種實(shí)用標(biāo)定法[J].壓電與聲光，2010，32(3)：372-375.

[9] 戴磊，旋翼飛行機(jī)器人磁羅盤誤差分析及校準(zhǔn)[J].旋翼無機(jī)器人，2012，34(4)：418-423.

[10]李翔.航姿參考系統(tǒng)三軸磁強(qiáng)計(jì)校正的點(diǎn)積不變法[J].儀器儀表學(xué)報(bào)，2012，33(8)：1813-1817.

(責(zé)任編輯 唐定國)

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

國家自然科學(xué)

基金項(xiàng)目(41506220)

陳雷(1992—)，男，碩士研究生，主要從事導(dǎo)航制導(dǎo)與控制研究。

10.11809/scbgxb2017.04.038

陳雷，卞鴻巍.基于模糊自適應(yīng)互補(bǔ)濾波的姿態(tài)解算算法[J].兵器裝備工程學(xué)報(bào)，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