基于指数加权卡尔曼滤波的组合定姿算法

王　骞

(中国运载火箭技术研究院 研究发展中心，北京　100076)



基于指数加权卡尔曼滤波的组合定姿算法

王骞

(中国运载火箭技术研究院 研究发展中心，北京100076)

星敏感器/陀螺组合定姿系统利用扩展卡尔曼滤波进行数据融合，但在轨运行时星敏感器的量测噪声模型不断变化，由于扩展卡尔曼滤波不能自适应调整，导致滤波器无法正常工作；基于模糊逻辑提出了一种指数加权卡尔曼滤波算法，实时监测系统滤波残差，利用模糊逻辑计算指数因子，自适应更新滤波器的量测噪声模型，从而有效地抑制了滤波器发散，提高了滤波精度;通过以TMS320C6713为处理器的DSP系统进行的半物理仿真实验，验证了指数加权卡尔曼滤波算法的有效性。

卡尔曼滤波; 指数加权;模糊逻辑; 量测噪声;半物理仿真

0　引言

陀螺能以较高的频率提供角速度和姿态角信息，并具有非常好的短期精度和稳定性，但是由于陀螺漂移的存在，长期工作由角速度积分得到的姿态角会有较大的累积误差[1]。星敏感器利用对星体的观测，通过与导航星库进行比对来确定姿态，精度高且无累积误差[2]，但是输出频率较低。星敏感器/陀螺组合定姿系统利用扩展卡尔曼滤波通过星敏感器提供的高精度姿态信息实时对陀螺漂移进行校正并补偿，使二者优势互补，从而实现高精度、高频率、长期稳定工作的定姿系统[3]。

扩展卡尔曼滤波通过系统模型和噪声模型建立状态方程和量测方程。系统模型可以通过运动学方程推导，而噪声模型只能通过实验数据估计。在轨运行时星敏感器的量测噪声模型不断变化，扩展卡尔曼滤波由于不能进行自适应调整，导致滤波精度下降甚至滤波发散。众多学者也提出过很多自适应改进算法。文献[4]通过衰减因子调整协方差阵，增加了新息的权重，克服了数据饱和的现象，但只能对动力学模型进行修正。文献[5]利用目标的机动信息计算缩放因子，修正过程噪声模型，但会造成决策滞后，且对量测噪声模型无法进行调整。文献[6]针对目标动态定位时系统噪声统计特性不明确的问题，应用Sage自适应滤波方法(又称极大后验估计)，根据每次测量值所获得的新信息，推算出当前时刻的系统噪声统计特性的估算值，并求出状态估值。文献[7]在进行状态滤波的同时对系统噪声的统计特性进行在线估计，确保算法收敛于期望用户，提高了检测器在动态环境下的跟踪性能。

扩展卡尔曼滤波包括预测和修正两个过程，在轨运行量测噪声的变化将直接导致滤波器修正过程出现错误，而以上文献对动力学模型和过程噪声的调整仅能纠正预测过程出现的错误，对修正过程出现的错误无能为力。针对此问题，本文提出一种基于模糊逻辑的指数加权卡尔曼滤波算法，通过模糊逻辑系统计算指数因子，从而实时更新量测噪声模型，使之不断与实际模型进行匹配，有效调整了滤波器的修正过程，使其能够稳定工作。

1　指数加权卡尔曼滤波

该算法基本框图如图1所示。

图1　基于模糊理论的指数加权卡尔曼滤波算法框图

定义指数因子eα，量测噪声阵更新方程为Rk=eα·Rk-1，通过模糊逻辑系统计算出α来调整Rk。

由此可得指数加权卡尔曼滤波的递推方程为(1)式～(6)式：

状态变量的一步预测：

(1)

协方差阵的一步预测：

(2)

量测噪声阵的更新：

(3)

滤波增益的更新：

(4)

状态变量的更新：

(5)

协方差阵的更新：

(6)

指数因子α与估计量测噪声偏离实际量测噪声的程度有关，而比较滤波残差的实际值与理论值可以描述这种偏离程度，滤波残差rk的定义为(7)式：

(7)

残差方差阵的理论值为(8)式：

(8)

2　模糊逻辑系统

模糊逻辑系统是一种基于知识并运用语言变量的系统，与其它传统的自适应方法相比，模糊逻辑系统的优点在于它的设计简单并充分运用了被控制系统的相关知识[8]。

当实际量测噪声大于估计量测噪声时，Trace(Sr)>Trace(Pr)，模糊逻辑系统的输入input>0，应使输出α>0，适当增大估计量测噪声；当实际量测噪声小于估计量测噪声时，Trace(Sr)

模糊逻辑系统的作用为保持估计量测噪声与实际量测噪声的匹配，即应使模糊逻辑系统的输入input保持在0附近，以维持量测噪声模型的准确性。

首先对模糊逻辑系统的输入进行模糊化，其隶属度函数如图2所示，即三角形隶属度函数，其中横坐标为input(无量纲变量，单位为1)，纵坐标为隶属度membership(无量纲变量，单位为1)，分别定义为模糊集合less，表示基本小于0；模糊集合equ，表示在0附近；模糊集合more，表示基本大于0。

图2　输入隶属度函数

解模糊化的方法这里采用重心法，即模糊控制系统的输出α取为从输入隶属度函数上得到的隶属度值在输出隶属度函数与相应集合曲线及横轴围成的三角形重心的横坐标。根据这一原则，通过不断仿真实验，确定了输出隶属度函数如图3所示，其中横坐标为output(无量纲变量，单位为1)，纵坐标为隶属度membership(无量纲变量，单位为1)：

图3　输出隶属度函数

模糊控制系统的输入与3个模糊集合的位置关系共有5种情况如式(9)所示：

(9)

所以通过重心法解模糊化来计算模糊控制系统的输出α也对应有5条解算规则如式(10)所示：

(10)

例如符号XCG(Δless∪Δequ)表示与输出隶属度函数的less曲线和equ曲线分别围成的三角形并集的重心横坐标。

3　半物理仿真

为了评价本文提出的指数加权卡尔曼滤波算法的性能，我们利用TI公司型号为TMS320C6713的DSP实现了该算法，陀螺和星敏感器分别通过SPI和HPI接口与DSP通信，对该算法进行了半物理仿真。

组合定姿系统的状态向量取为(11)式：

(11)

其中:Δq1,Δq2,Δq3为估计四元数与真实四元数之间旋转偏差四元数的矢量部分，Δb1,Δb2,Δb3为陀螺三轴漂移的估计值与真实值之间的差值。

量测向量取为(12)式：

(12)

星敏感器的量测噪声来源主要有暗电流噪声、读出噪声、光子散粒噪声和非均匀性噪声，而影响这些噪声最主要的外部因素是温度[9]。星敏感器在轨运行时由于受到太阳辐射、地球红外辐射和地球阳光反照等因素的影响，工作环境的温度有可能产生渐变或者突变，所以可能导致量测噪声有渐变或突变的情况[10]。为了符合实际工作环境的需求，本文对星敏感器量测噪声渐变和突变两种情况都进行了仿真测试。

3.1星敏感器量测噪声从10″渐变至60″

整个过程的姿态角误差曲线和角速度误差曲线如图4和图5所示。

图4　量测噪声渐变时姿态角误差曲线

图5　量测噪声渐变时角速度误差曲线

图中实线指数加权卡尔曼滤波结果，虚线为扩展卡尔曼滤波结果。对比可知随着量测噪声的渐变扩展卡尔曼滤波在整个过程中误差逐渐增大，有发散的趋势，而指数加权卡尔曼滤波在整个过程中误差很小且保持稳定。

图6为指数加权卡尔曼滤波中指数因子在整个量测噪声渐变过程中的变化曲线。

图6　指数加权因子α曲线

由图可知整个过程中α始终稳定在0附近，证明噪声模型匹配准确。

3.2星敏感器量测噪声在750秒时从10″突变至60″

整个过程的姿态角误差曲线和角速度误差曲线如图7和图8所示：

图7　量测噪声突变时姿态角误差曲线

图8　量测噪声突变时角速度误差曲线

由图可知在750 s以前两种方法滤波精度相差不大，在750 s以后由于实际量测噪声突然变大，扩展卡尔曼滤波的误差随之增大且有发散趋势，指数加权卡尔曼滤波的误差则没有太大影响，能够全程保持稳定。

图9为指数加权卡尔曼滤波中指数因子在整个过程中的变化曲线。

图9　指数加权因子α曲线

由图可知在750 s实际量测噪声发生突变的同时α也有一个突变，随后又逐渐回到0，并重新稳定在0附近，可见指数加权卡尔曼滤波有很强的自适应调整能力。

表1为两种方法的误差统计数据。

表1　两种方法的误差统计数据

对比两组数据可知指数加权卡尔曼滤波的精度要高于扩展卡尔曼滤波，且稳定性要明显优于扩展卡尔曼滤波。

4　结论

本文提出了指数加权卡尔曼滤波算法，在每个滤波周期实时监控系统残差的变化，利用模糊逻辑系统输出指数调节因子α，在线调整滤波器的量测噪声模型，自适应地逼近实际噪声模型，改善了滤波器的精度和稳定性。

对于星敏感器/惯性陀螺组合定姿系统利用星敏感器在线校正陀螺漂移，其实际工作环境量测噪声模型未知且不断变化，本文提出的指数加权卡尔曼滤波算法很好地解决了这一问题，通过监控星敏感器量测值与一步预测值之间的微小旋转四元数，利用模糊逻辑系统调节滤波器量测噪声模型，增强了滤波器的性能，有效地防止了滤波器发散。本文的研究对提高星敏感器/陀螺组合定姿系统的精度具有重要意义。

[1] Georgy J,Noureldin A,Korenberg M J,et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation[J]. IEEE Transactions on Intelligent Transportation Systems,2010,11(4): 856-872.

[2] Sigel D A,Wettergreen D. Star tracker celestial localization system for a Lunar rover[A]. Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems[C]. CA. 2007:2851-2856.

[3] 邢广华，刘建业，孙永荣,等.多信息融合组合导航半物理仿真系统设计与实现[J].航天控制，2005，23(2)：64-68.

[4] 马忠孝，刘宗玉，陈明.基于模糊逻辑的自适应卡尔曼滤波在GPS/INS组合导航中的应用[J].信息与控制，2006，35(4)：457-461.

[5] 顾潮琪，周德云，曲艺海.一种精确跟踪机动目标的滤波算法的研究[J].电光与控制，2007，14(2)：8-11.

[6] 宋悦铭,刘晶红,沈宏海，等. 航空光电平台目标定位自适应卡尔曼滤波方法的应用研究[J]. 航空精密制造技术,2011,47(1): 47-54.

[7] 高维廷,李辉,翟海天. 基于自适应卡尔曼滤波的盲多用户检测算法[J]. 计算机工程,2011,37(12): 104-106.

[8] 童正环,江颖,张佩,等. 自适应模糊控制器的实现[J]. 仪器仪表学报,2006,27(6): 1939-1940.

[9] 刘海波,谭吉春,郝云彩,等. 环境温度对星敏感器测量精度的影响[J]. 光电工程,2008,35(12): 40-44.

[10] 李学夔,谭海曙,于昕梅,等. 星敏感器定位精度噪声影响因素研究[J]. 光电子技术,2010,30(2): 97-101.

Integrated Attitude Determination Algorithm Based on Kalman Filter with Weighted Index

Wang Qian

(Research and Development Center，China Academy of Launch Vehicle Technology,Beijing100076，China)

Integrated attitude determination system of star sensor/gyroscope completes data fusion with Extended Kalman filter. Measurement noise model on-orbit of star sensor is variable. Extended Kalman filter is unadaptable so that the filter is out of order. This paper presents Kalman filter with weighted index based on fuzzy logic. The algorithm monitors residuals real time,exports index factor by fuzzy logic and then adjusts measurement noise model in order to restrain diffusion and improve precision. The semi-physical simulation results based on DSP system with TMS320C6713 processor validate availability of the algorithm.

Kalman filter;weighted index; fuzzy logic;measurement noise; semi-physical simulation

1671-4598(2016)04-0133-04DOI：10.16526/j.cnki.11-4762/tp.2016.04.040

V249.32

A

2015-08-08；

2015-11-04。

王骞(1986-)，男，黑龙江人，硕士，工程师，主要从事航天器电气总体设计、航天器测试等方向的研究。