基于惯性/地磁的弹体组合测姿方法

赵捍东 ，李志鹏，王　芳

(1.中北大学机电工程学院,山西 太原 030051；2.北京理工大学机电学院，北京 100081；3.江南工业集团有限公司工艺技术研究所，湖南 湘潭 411207)



基于惯性/地磁的弹体组合测姿方法

赵捍东1，李志鹏2，王芳3

(1.中北大学机电工程学院,山西 太原 030051；2.北京理工大学机电学院，北京 100081；3.江南工业集团有限公司工艺技术研究所，湖南 湘潭 411207)

摘要:针对单一传感器测姿精度有限和容错性不佳的问题，提出了基于惯性/地磁传感器的弹体姿态组合测量方法。该方法使用矢量因子联邦滤波算法对测量数据进行融合，并针对系统中存在的模型误差和噪声不严格符合高斯分布的问题，使用了改进的强跟踪无迹卡尔曼滤波算法作为联邦滤波的子滤波器。仿真结果表明，本文方法可以获得比任何单一传感器都高的估计精度，并比传统联邦滤波算法的融合精度更高。

关键词:姿态估计；组合测姿；联邦滤波；无迹滤波；强跟踪滤波

0引言

随着作战形式的变化，简易制导弹药因其较低的使用成本和较高的打击精度成为战场新宠，而弹体姿态的准确测量又对简易制导弹药的有效控制具有重要作用，所以提高弹体姿态的测量精度具有十分重要的意义。

惯性传感器和地磁传感器都是重要的弹体姿态测量仪器，但使用单一传感器进行姿态测量时，测姿精度和容错性都不佳，为了获得更高的姿态测量精度，文献[1]提出了使用陀螺和地磁传感器的弹丸姿态测量方法，但是这种方法仅仅利用陀螺数据作为地磁解算的初值，对数据的利用并不充分。文献[2-3]基于融合算法给出了多传感器组合测量方法，对各传感器的输出信息利用的比较充分，也取得了很高的测量精度，但其使用传感器较多而不适合低成本简易制导弹药使用。同时，文献[2-3]中的联邦滤波融合算法作为广泛应用的分散式融合方法[4-9]，其标量式因子分配方法并不能最大限度的发挥这一融合算法的优势。同时就子滤波器而言，噪声不严格符合高斯分布或系统模型存在误差也会影响滤波精度。本文针对此问题，提出了基于惯性传感器和地磁传感器的组合测姿方法，并在融合算法上做了改进。

1组合测姿原理

1.1姿态测量原理

在惯性传感器中，进行姿态测量的器件主要是陀螺仪，陀螺仪通过敏感载体的角速率，再进行积分，便可得到载体的当前姿态角。统计规律表明，陀螺的输出误差主要由一阶马尔可夫随机漂移、随机白噪声、常值漂移误差组成。其误差模型为[2-3,11]：

ε=εb+εr+ωg

(1)

式(1)中,εb为随机常数，εr为一阶马尔科夫过程噪声，ωg为陀螺随机噪声漂移。由此可以看出，陀螺的输出噪声是十分复杂的，这也给应用中的滤波估计带来了难度。

地磁传感器测姿的原理是三轴地磁传感器敏感载体在地磁场中的倾角，再转化到惯性坐标系内姿态角[1]。地磁场是地球本身的固有物理属性，稳定性好，不易人为干扰。基于对地磁的测量进行测姿，已经在低轨卫星、无人机、舰船上得到了广泛的应用[2-3]。地磁测姿误差不累积，但受到地磁建模误差，异磁场干扰，载体干扰的影响，总的误差比较复杂。

考虑到MEMS陀螺的角速率测量精度较高，且价格功耗等都较低，同时磁强计的角度测量误差不随时间累积，本文便利用磁强计和MEMS陀螺进行姿态测量，使其优势互补。基于磁强计和陀螺的姿态解算原理，在文献[1-2]中有详细的描述，本文主要在文献[1]的现有工作的基础上，考虑将磁强计和陀螺的测量数据进行融合，以便充分利用二者的信息，提高弹体姿态测量的精度。在成本上，本文所提出的方法只是在算法上实现，硬件上与文献[1]相比并没有新增，所以本文方法是一种提高精度、控制成本的方法。

1.2基于联邦滤波的融合原理

MEMS陀螺和磁强计是弹体姿态测量的常用传感器，但目前为止只能进行分散式融合，而无法将陀螺仪和磁强计做成一个传感器，实现深度组合。而联邦滤波又是最常用最有效的分散式融合模式，所以本文中使用联邦滤波作为融合框架。

从算法结构上看，联邦滤波结构中的各个子滤波器从主滤波器中分享最优融合后的状态信息[4-5]，并将从主滤波器分配到的信息作为下一步滤波融合的基准值，这就可以在最大程度上抑制陀螺误差的累积。联邦滤波的经典结构如图1所示[4,6,9]。

图1　联邦滤波融合结构Fig.1　the federated filter fusing frame

对于一般的离散系统，我们可以将其系统状态模型描述为：

X(k)=Φ(k/k-1)X(k-1)+W(k)

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

上式中的βi称为信息分配系数，并且符合以下约束条件：

(10)

其中：0≤βi≤1, i=1,2,…,n，βm为主滤波器上的信息分配系数，也可称为分配因子[7-8]。从式(3)—式(9)中可以看出，在联邦滤波融合中，每个子滤波器的信息都得到了有效的利用。

实际计算时，对于每一个子滤波器，主滤波器到子滤波器的信息反馈方式一般取为[5-6]：

(11)

(12)

那么在MEMS陀螺/磁强计组合测量弹体姿态的应用中，陀螺和磁强计分别敏感弹体的姿态，并分别对二传感器的输出量进行滤波，之后根据二传感器对应的子滤波器的方差估计的大小在主滤波器中进行姿态融合。

1.3子滤波器设计

在联邦滤波融合算法中，主滤波器进行信息融合和分配，而真正的滤波过程在子滤波器中完成，所以子滤波器的性能对最终的融合结果有一定的决定性作用。

虽然卡尔曼滤波器的使用十分广泛，但状态方程具有一定的非线性并且噪声并不严格符合高斯分布，将会严重影响经典卡尔曼滤波器的性能，所以工程中常使用无迹卡尔曼滤波器(Unscented Kalman Filter, UKF)进行滤波的实现，本文中便使用UKF作为子滤波器。

(13)

其中λ=α2(n+κ)-n，同时定义对应的权值为：

(14)

X(k)=f(X(k-1))+W(k-1)

(15)

Z(k)=h(X(k))+V(k)

(16)

上式中，W,V分别为状态误差和量测噪声，k表示第k次观测。那么基于UKF的滤波过程可表示为：

1)初始化过程：

(17)

(18)

2)预测更新：

xi(k/k-1)=f(χi(k-1))

(19)

(20)

zi(k/k-1)=h(xi(k/k-1))

(21)

(22)

(23)

3)量测更新：

(24)

(25)

(26)

(27)

P(k)=P*(k)-K(k)PZ(k)Z(k)KT(k)

(28)

子滤波器完成状态估计和方差估计后，将信息输入主滤波器，进行最优融合，以提高最终的姿态估计精度，所以说子滤波器的估计精度和主滤波器的融合精度，都对最终的姿态估计有影响。本文中根据现有研究成果，对子滤波器和主滤波器都进行了改进。

2改进的组合测姿

2.1改进的强跟踪无迹卡尔曼滤波

UKF在进行非线性问题的滤波时具有独特的优点，是当前应用最为广泛的非线性滤波方法。但模型的不确定性和异常扰动也会在很大程度上影响UKF的性能。为此，我们引入了强跟踪UKF方法，并进行了简化的改进。强跟踪原理原本是用在扩展卡尔曼滤波器中的一项改进，文献[13]将这一原理应用到UKF中，形成强跟踪UKF，即STUKF，但该强跟踪UKF变换次数多，所以本文对文献[13]中的强跟踪UKF进行了简化改进。改进过程主要针对上文中的式(26)—式(28)，改进过程如下：

(29)

(30)

P(k)=λ(k)P*(k)-K(k)PZ(k)Z(k)KT(k)

(31)

上式中的λ(k)为对应于k时刻的渐消因子，其计算方法为：

(32)

(33)

(34)

N(k)=Vr(k)-βR(k) ，R(k) 为观测系统噪声，r为观测量预测残差。

(35)

其中0<ρ≤1为遗忘因子，β为弱化因子[13-16]。

强跟踪改进的滤波器因为采用了强迫正交原理，可以有效的削弱建模误差和不确定性噪声带来的不利影响[13-14]，这对实际应用具有十分重要的意义。

2.2改进的联邦滤波融合结构

由于采用了信息反馈机制，可以使得联邦滤波获得最优的融合效果，所以信息分配系数便在一定程度上决定了最后的融合精度。传统的信息分配系数的求法，如式(14)，仅考虑了子滤波器方差的特性。由于每个子系统获得的观测量不同，那么对不同子系统的可观测度就不相同。可观测性越好，子系统对应的估计值就越精确，于是，文献[9]提出了一种将系统可观测度信息引入求解分配系数的过程中，对分配系数进行改进后，本文中基于联邦滤波的组合测姿原理可表示为图2所示。

图2　改进的INS/地磁组合测量模型Fig.2　novel INS/magnetometer based integrated estimation model

其中，Ai=diag(ai1,ai2,…,ain) ，

i=1,2,…,N;j=1,2,…n,

σij为可观测度矩阵的奇异值[9]。

3仿真与分析

为了验证本文方法的有效性，基于以下较大的误差信息进行仿真试验。MEMS陀螺测量误差0.5°，常值漂移为5(°)/h，随机漂移5(°)/h；地磁姿态误差1°，并每隔30s添加一次幅值为5°的异常干扰。陀螺初始姿态误差分别为，1.5°，1°，-2°，基于某火箭弹弹道进行仿真，并设定角运动状态方程的系数误差为2%。分别以传统方法和本文方法进行姿态估计的仿真对比，其中传统方法为经典UKF作为子滤波器的一般联邦滤波方法，这也是工程上较为常用的一种方法。仿真时间600s，误差统计如图3、图4所示。

图3　姿态角误差统计Fig.3　attitude error statistics

图4　姿态角速率误差统计Fig. 4　attitude angle velocity error statistics

从图中可以直观的看出，本文方法比传统的联合滤波算法具有更高的滤波精度，并且误差初期误差收敛更快。这主要是因为本文所使用的强跟踪改进可以有效解决模型误差、异常扰动和非高斯噪声的问题。并且从误差曲线中可以看出，最终的姿态角误差小于任何一个单传感器的输出误差，也说明了多传感组合测姿的优越性。统计解算过程中的姿态角误差，如表1所示。

表1　姿态角估计的误差统计

在工程应用中，使用陀螺和磁强计的组合，可以充分利用陀螺和磁强计的优点，从而提供更精确的姿态测量数据。

4结论

本文提出了基于MEMS陀螺和磁强计的弹体姿态组合测量方法，该方法使用了矢量分配因子型的联邦滤波算法对测量数据进行融合，并引入了改进的强跟踪无迹卡尔曼滤波器作为子滤波器，提高了复杂噪声环境下的融合精度，仿真结果表明，本文方法在三轴方向上的姿态估计精度均高于任何一个传感器的测量精度，并且比传统估计方法精度更高。因为子滤波器的估计精度很大程度上决定了最终的融合精度，采用适用范围更广的粒子滤波器作为子滤波器将是本课题进行进一步研究的方向。

参考文献:

[1]赵捍东, 曹红松, 朱基智, 等. 基于磁强计和陀螺的姿态测量方法[J]. 中北大学学报, 2010,31(6): 631-635.

[2]WangXinlong,ZhangQing,LiHengnian.Anautonomousnavigationschemebasedonstarlight,geomagneticandgyroswithinformationfusionforsmallsatellites[J].ActaAstronautica, 2014,94(1)：708-717.

[3]GrovesPaulD.PrinciplesofGNSS,Inertial,andMultisensorIntegrated,NavigationSystems[M].publishedbyartechhouse, 2008: 280-300.

[4]BenjaminNoack,JulierSimonJ,MarcReinhardt,etal.NonlinearFederatedFiltering[C]//Turkey,16thInternationalConferenceonInformationFusionIstanbul, 2013:350-356.

[5]JongheeBae,YoudanKim.AttitudeEstimationforSatelliteFaultTolerantSystemUsingFederatedUnscentedKalmanFilter[J].InternationalJournalofAeronautical&SpaceSci, 2010, 11(2): 80-86.

[6]张海龙, 郝静如, 李启光. 联邦卡尔曼滤波在捷联惯导/ 全球定位/里程仪组合系统中的优化设计[J].探测与控制学报, 2009,31(6):66-73.

[7]GU Qitai, FANG Jing. Global optimality for generalized federated filter[J]. Acta automatica sinica, 2009,35(10)：1310-1316.

[8]Zhou Benchuan, Cheng Xianghong. Federated filtering algorithm based on fuzzy adaptive UKF for marine SINS/GPS/DVL integrated system[C]//2010 Chinese Control and Decision Conference, 2010: 2082-2085.

[9]Xiong Zhi, Chen Jihui, Wang Rong, et al. A new dynamic vector formed information sharing algorithm in federated filter [J]. Aerospace Science and Technology, 2013, 29(1): 37-46.

[10]Wan E A, Van Der Merwe R.The unscented Kalman filter[J]. Kalman filtering and neural networks, 2001,11(1): 221-280.

[11]周翔, 霍鹏飞, 祁克玉. MEMS 陀螺/ 磁传感器复合弹丸姿态测量[J]. 探测与控制学报, 2010,32(6):10-14.

[12]苏宛新. 自适应 UKF 滤波在 SINS初始对准中的应用[J]. 中国惯性技术学报, 2011, 19(5):533-536.

[13]王小旭, 赵琳, 夏全喜, 等. 基于unscented变换的强跟踪滤波勃起[J]. 控制与决策, 2010,25(7): 1063-1068.

[14]Chen Xiyuan, Shen Chong, Zhang Weibin,et al. Novel hybrid of strong tracking Kalman filter and wavelet neural network for GPS/INS during GPS outages[J]. Measurement, 2013,46(1): 3847-3854.

[15]M Kiani, Seid H Pourtakdoust. Concurrent orbit and attitude estimation using minimum sigma point unscented Kalman filter[J]. Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2014, 228(6): 801-819.

[16]Dah Jing Jwo, Chi Fan Yang, Chih Hsun Chuang, et al. Performance enhancement for ultra-tight GPS/INS integration using a fuzzy adaptive strong tracking unscented Kalman filter[J]. Nonlinear Dynamics, 2013, 73(1):377-395.

*收稿日期:2015-11-26

作者简介:赵捍东(1960—),男，吉林长春人，博士，教授，研究方向：弹箭飞行与控制。E-mail:nuc_zhd@163.com。

中图分类号:TJ714

文献标志码:A

文章编号：1008-1194(2016)03-0047-05

Projectile Attitude Estimation Based on Inertial & Magneto Integrated Measurement

ZHAO Handong1, LI Zhipeng2, WANG Fang3

(1. Mechanical and Electrical Engineering School of North University of China, Taiyuan 030051,China;2. Mechanical Engineering School, Beijing Institute of Technology, Beijing 100081, China;3. Institute of Process Technology, Jiangnan Industries Group Co.LTD, Xiangtan 411207,China)

Abstract:Aiming at the problem of attitude detection accuracy is limited and the fault tolerance is low when using single sensor, a inertial sensor and magnetometer based attitude estimation approach was proposed. Measure data was integrated via the vector formed information share federated filter. As there exist the problem of system model bias and the Non-Gaussian noise, proposed a novel strong tracking unscented kalman filter algorithm as the local filter. Simulation result shows, this method could get higher estimation accuracy than using any single sensor, and which was higher than traditional integration approach.

Key words:attitude estimation; integrated attitude detecting; federated filter; unscented kalman filter; strong tracking kalman filter