平滑变结构滤波在航天器组合导航中的应用

施常勇，张丽敏

（1. 上海航天控制技术研究所，上海 201109；2. 上海市空间智能控制技术重点实验室，上海 201109）

平滑变结构滤波在航天器组合导航中的应用

施常勇1,2，张丽敏1,2

（1. 上海航天控制技术研究所，上海 201109；2. 上海市空间智能控制技术重点实验室，上海 201109）

针对系统模型不准确或噪声无法建模等导致的组合导航精度下降问题，在闭环间接滤波方案中引入一种基于变结构和滑模控制概念的平滑变结构滤波器（SVSF），该滤波器对于建模的不确定性和给定上限但无法建模的扰动噪声具有较好的稳定性和鲁棒性。将提出的方案应用于航天器SINS/GNSS组合导航系统中，进行仿真验证，并与EKF滤波器进行比较。仿真结果表明：SVSF尽管不是“最优”滤波器，但其精度与EKF相当，可实现位置精度优于10 m，速度精度优于0.01 m/s；在模型误差与测量误差增大和系统突变等情况下，其较EKF更加具有抗干扰能力，可提高导航系统的鲁棒性和稳定性。

平滑变结构滤波器；组合导航；鲁棒性；航天器

随着空间技术的发展，人们对航天器自主导航的需求越来越高，GNSS/SINS组合导航系统充分利用了SINS短期精度高、抗干扰能力强和GNSS长期精度高的优点，取长补短，实现航天器高精度高可靠自主导航[1]。在组合导航系统中常采用卡尔曼滤波技术[2]，但是该方法要求能够提供系统精确的状态方程和量测方程，并且系统噪声和量测噪声是均值为零、协方差已知的高斯噪声，否则难以满足要求，甚至发散。针对此问题，不同的研究者提出了如 EKF[3]、UKF[4]、PF[5]或CKF[6]等滤波算法，但仍存在诸多不足，如：模型误差作为过程噪声来处理，且假设为高斯白噪声，这与组合导航系统的实际噪声情况并不相符；对系统模型不确定性的鲁棒性差等。

平滑变结构滤波器（smooth variable structure filter，SVSF）是由Habibi等人于2007年在变结构滤波器 VSF（variable structure filter，2002）[7]基础上提出的，其基本思想是基于变结构和滑模控制概念，采用变结构的增益，使得预测状态趋近于系统真实轨迹，因此是一种“预测-校正”的估计器，其对于建模的不确定性和给定上限但无法建模的扰动噪声具有较好的稳定性和鲁棒性[7]。本文将结合 SVSF理论，研究其在航天器组合导航中应用。

1 平滑变结构滤波器

设系统方程为：

SVSF估计的基本概念如图1所示，假设某一状态量为x，其估计值为，其中的初始值选取一般基于概率分布或先验知识。通过使用SVSF增益，将被强制到状态轨迹周围（也称存在子空间）内， 一旦进入存在子空间，将沿系统状态轨迹的切换。为减小切换抖动，在该区域中使用饱和项来使结果平滑。当不是所有的动态都被明确建模时，该策略具有较强的鲁棒性。

图1 SVSF状态估计原理图Fig.1 Principle of SVSF state estimation

证明：根据Lyapunov稳定性理论，定义Lyapunov函数如下：

若ΔV＜0，则认为系统估计过程是稳定的，由此：

基于上述前提，可以推导出标准变结构滤波器的增益方程：

ψi为给定的噪声上限值。

式(5)中可以体现SVSF的核心思想，利用到噪声信息和误差信息，在误差上界处（滑模面处）采用饱和项强制估计状态进行切换，在上下滑模面内采用误差新息进行更新。

经过数年的发展，SVSF出现了多种形式，如固定边界层厚度[9]、基于协方差的最优边界层厚度[10]，也出现了SVSF与CKF[11]、UKF[12]、PF[13]组合滤波的研究，本文采用基于协方差的非线性系统迭代方程[14]：

状态预测方程：

协方差预报方程：

误差预报：

增益方程：

系统状态估计方程：

协方差推广方程：

误差方程：

式(7)和(11)中Qk和Rk分别为系统和量测噪声协方差阵。

对于非线性动力学方程，需进行线性化处理：

2 SINS/GNSS 组合导航系统设计

在 SINS/GNSS组合导航系统中，可采用多种滤波结构形式[15]，如开环直接滤波、闭环直接滤波、开环间接滤波或闭环间接滤波等。本文采用闭环间接滤波方案，将SINS和GNSS子系统各自确定的航天器相对于J2000系的位置和速度相减作为量测，送入滤波器进行滤波计算，从而获得导航参数误差的最优估计值；利用滤波估计值实时地对SINS进行误差校正；最后，将校正后的导航结果作为组合导航系统的输出。SINS/GNSS组合导航方案结构如图2所示。

图2 SINS/GNSS组合导航系统框图Fig.2 Architecture of SINS/GNSS integrated navigation system

选择J2000惯性坐标系i作为导航坐标系，利用航天器本体系到惯性系的转换矩阵将加速度计的输出从航天器本体坐标系变换到惯性坐标系即：

则航天器在惯性坐标系的加速度为：

注意到地球引力项gni与当前的位置是相关的，而当前的位置又是未知的，因此计算时将上一时刻近似代替当前时刻gni，则惯导位置求解公式为：

其中，Tzh为SINS解算周期。

惯导系统速度求解：为了提高系统的导航精度，在求解惯导系统速度时，当前时刻的地球引力项gni用该Tzh内平均值来近似代替，则惯导速度求解公式为：

随着生活方式的改变，我国人群中存在MS的比例逐年增加，目前已报道的我国成人MS的比例为9.8%～17.8%[17]，本研究中合并MS的前列腺癌患者占16.08%，与之前的报道相符。ZHANG等[12]的研究结果显示MS和前列腺癌的Gleason评分≥8以及较晚的病理分期有关。本研究的结果证实了MS和病理T分期的相关性，然而，未发现两组之间Gleason评分有明显差异，可能的原因是本研究采用的Gleason评分分组方式与之前的研究不同。为了减少其他临床终点对结果的干扰，我们采用竞争风险模型评估MS对生化复发的影响，结果显示合并MS的前列腺癌患者出现生化复发的概率是无MS者的1.86倍。

GNSS的输出为：

其中，δv为速度残差，δr为位置残差。

选择惯导输出的航天器J2000坐标系下的速度、位置与GNSS输出的相应值之差作为量测，构造的量测为：

量测方程为：

由此可知，系统的测量矩阵为单位阵，即：

3 仿真研究

为验证SVSF在GNSS/SINS组合导航中的可用性和适应性，采用高精度轨道动力学设计了一段空间飞行轨迹，陀螺、加速度计和GNSS的误差设置见3.1节，分别对正常误差工况、过程异常和敏感器噪声增大等三种工况进行了仿真，并将SVSF与EKF进行了对比验证。

3.1 敏感器误差设计

组合仿真周期：TZH=0.5 s。

3.2 仿真结果

1）正常误差工况

采用表1中设置的敏感器误差参数，SVSF与EKF仿真曲线如图3所示。从图中可以看出，尽管SVSF不是“最优”滤波器，但两种滤波器的误差基本在同一量级，位置精度优于10 m，速度精度优于0.01 m/s。

图3 位置速度误差曲线(正常误差工况)Fig.3 True estimation errors of position and velocity (normal errors of sensors)

2）过程异常工况

为验证SVSF的鲁棒性，在500s时GNSS的Y轴速度噪声异常放大100倍(持续50s)，SVSF与EKF在速度上均受到了影响，但由于SVSF是基于变结构的思想，具有较好的鲁棒性，其在位置维的影响远小于EKF，仿真结果如图4所示。

图4 过程异常工况位置速度误差曲线Fig.4 True estimation errors of position and velocity (abnormity states)

3）敏感器噪声增大工况

为进一步验证SVSF的性能，在GNSS正常噪声设置的基础上，对速度误差放大10倍处理，按照原有设计参数，SVSF表现出较好的适应性，滤波稳定性优于EKF滤波器，滤波精度也较EKF的高，仿真曲线如图5所示。

图5 初始噪声增大工况位置速度误差曲线Fig.5 True estimation error of position and velocity (large errors of sensors)

4 结 论

本文针对组合导航系统中系统模型不准确或噪声无法建模导致的导航精度下降问题，研究了 SVSF滤波器在航天器 GNSS/SINS组合导航系统中的应用技术。针对航天器轨道运行的特点，提出了闭环间接滤波方案，并搭建了仿真模型，对所设计的导航系统进行了仿真验证，并将其与EKF进行了对比分析。仿真结果表明，SVSF具有较好的鲁棒性和稳定性，具有较强的工程应用价值。

（References）：

[1]魏伟, 武云云. 惯性/天文/卫星组合导航技术的现状与展望[J]. 现代导航, 2014(1): 63-65.Wei Wei, Wu Yun-yun. Status and prospect of INS/CNS/GNSS integrated navigation technology[J]. Modern Navigation, 2014(1): 63-65.

[2]谢木生, 卡尔曼滤波在GPS/INS 组合导航系统中的应用研究[D]. 中南大学, 2013: 19-26.Xie Mu-sheng. Study on application of Kalman in INS/GPS integrated navition[D]. Central South University,2013: 19-26.

[3]吴春光, 李双明, 潘玉纯. EKF和 UKF在非线性组合导航系统中的对比研究[J]. 长春理工大学学报 (自然科学版), 2016, 39(2): 82-85.Wu Chun-guang, Li Shuang-ming, Pan Yu-chun. Comparison of EKF and UKF applied in nonlinear navigation system[J]. Journal of Changchun University of Science and Technology (Natural Science Edition), 2016, 39(2): 82-85.

[4]胡高歌, 高社生, 赵岩. 一种新的自适应 UKF 算法及其在组合导航中的应用[J]. 中国惯性技术学报, 2014,22(3): 357-360.Hu Gao-ge, Gao She-sheng, Zhao Yan. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 357-360.

[5]薛丽, 高社生, 胡高歌. 自适应 Sage-Husa 粒子滤波及其在组合导航中的应用[J]. 中国惯性技术学报,2013, 21(1): 84-88.Xue Li, Gao She-sheng, Hu Gao-ge. Adaptive Sage-Husa particle filtering and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2013,21(1): 84-88.

[6]孙枫, 唐李军. 基于cubature Kalman filter的INS/GPS组合导航滤波算法[J]. 控制与决策, 2012, 27(7): 1032-1036.Sun Feng, Tang Li-jun, INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J]. Control and Decision, 2012, 27(7): 1032-1036.

[7]Habibi S, Burton R, Chinniah Y. Estimation using a new variable structure filter[C]//Proceedings of the 2002 American Control Conference. 2002, vol.4: 2937-2942.

[8]Habibi S. The smooth variable structure filter[J]. Proceedings of the IEEE, May 2007, 95(5): 1026-1059.

[9]Gadsden S A, Habibi, Kirubarajan T. Kalman and smooth variable structure filters for robust estimation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2014,50(2): 1038-1050.

[10]Gadsden S A, Habibi S R. A new form of the smooth variable structure filter with a covariance derivation[C]//49th IEEE Conference on Decision and Control. 2010:7389-7394.

[11]Al-Shabi M, Gadsden S A, Habibi S R. Kalman filtering strategies utilizing the chattering effects of the smooth variable structure filter[J]. Signal Processing, 2013, 93(2):420-431.

[12]Al-Shabi M, Hatamleh K S. The unscented smooth variable structure filter application into a robotic arm[C]//ASME 2014 International Mechanical Engineering Congress and Exposition. 2014.

[13]Gadsden S A, Habibi S R, Kirubarajan T. The smooth particle variable structure filter[J]. Transactions of the Canadian Society for Mechanical Engineering, 2012,36(2): 177-193.

[14]Gadsden S A, Sayed M E, Habibi S R. Derivation of an optimal boundary layer width for the smooth variable structure filter[C]//American Control Conference. 2011:4922-4927.

[15]Benzerrouk H, Nebylov A V. Aircraft’s approach integrated navigation system INS/GNSS based on joint application of linear and nonlinear filtering[C]//the 18th IFAC World Congress. Milano, Italy, 2011: 1866-1871.

Application of SVSF in spacecraft integrated navigation system

SHI Chang-yong1,2, ZHANG Li-min1,2
(1. Shanghai Institute of Spaceflight Control Technology, Shanghai 201109, China;2. Shanghai Key Laboratory of Aerospace Intelligent Control Technology, Shanghai 201109, China)

In view of the precision degradation of GNSS/SINS integrated navigation system due to the fact that the system model is not accurate or the noises can not be modeled, a smooth variable structure filter(SVSF) based on variable structure and sliding mode control concept is introduced in the closed loop indirect filtering scheme. The filter is stable and robust for the modeling uncertainties and the disturbance noise when with an upper bound on the level of un-modeled dynamics or knowledge of the level of noise.By applying the proposed scheme into the spacecraft SINS/GNSS integrated navigation system, the simulation is carried out and the comparison with the EKF filter is made. The simulation results show that the SVSF, though suboptimal, has equivalent precision to EKF, and the position and velocity accuracies are better than 10 m and 0.01 m/s, respectively. The SVSF’s anti-interference ability is stronger than that of EKF in cases such as the model and the measurement have larger errors, the system has a mutation, etc.,which can improve the robustness and stability of the navigation system.

smooth variable structure filter; integrated navigation; robustness;spacecraft

1005-6734(2017)04-0478-05

10.13695/j.cnki.12-1222/o3.2017.04.010

U666.1

A

2017-05-26；

2017-07-26

施常勇（1982—），男，高级工程师，从事航天器导航系统设计与研究。E-mail: shi05cy@163.com