﻿ 一种基于INS/GPS/CNS的全信息导航滤波算法
 舰船科学技术  2019, Vol. 41 Issue (10): 159-162 PDF

1. 清华大学精密仪器系，北京 100084;
2. 中国舰船研究院，北京 100192;
3. 哈尔滨工业大学仪器科学与工程学院，哈尔滨 150001

A full information navigation filtering algorithm based on INS/GPS/CNS
YIN Hong-liang1,2, ZHANG Rong1, HAO Qiang3, YU Dong-kang2
1. Department of Precision Instrument, Tsinghua University, Beijing 100084, China;
2. China Ship Research and Development Academy, Beijing 100192, China;
3. School of Instrument Science and Engineering, Harbin Institute of Technology, Harbin 150001, China
Abstract: In order to solve the weak system stability problem of the INS/GPS navigation system caused by incomplete observation information, this paper uses CNS to build a full observation information navigation system based on INS/GPS/CNS. Through establishing the position error, velocity error and platform misalignment angle mathematical models, a full information navigation filtering algorithm based on INS/GPS/CNS is proposed, which improves the navigation accuracy of the system and improves the stability of the system. The experimental results show that the accuracy of each navigation parameter is improved to different degrees compared with the traditional two-combination filtering algorithm, and the attitude error is convergent, the stability is high under long-haul conditions.
Key words: INS/GPS/CNS     full information filtering     EKF     integrated navigation
0 引　言

1 INS/GPS/CNS滤波模型

1.1 状态方程模型

1）速度误差模型

 $\begin{split} \delta {{\dot{ V}}^n} = {{{f}}^n} \times {{\bf{\varphi }}^n} - \left( {2{\bf{\omega }}_{ie}^n + {\bf{\omega }}_{en}^n} \right) \times \delta {{{V}}^n}+ \\ \;\;\;\;\;{{{V}}^n} \times \left( {2\delta {\bf{\omega }}_{ie}^n + \delta {\bf{\omega }}_{en}^n} \right) + {{{\nabla}} ^n}\text{。} \end{split}$ (1)

 ${\bf{\omega }}_{ie}^n = {\left[ {\begin{array}{*{20}{c}} 0&{{\omega _{ie}}\cos L}&{{\omega _{ie}}\sin L} \end{array}} \right]^{\rm T}}\text{，}$ (2)
 ${\bf{\omega }}_{en}^n = {\left[ {\begin{array}{*{20}{c}} { - \displaystyle\frac{{{V_N}}}{{R + h}}}&{\displaystyle\frac{{{V_E}}}{{R + h}}}&{\displaystyle\frac{{{V_E}}}{{R + h}}\tan L} \end{array}} \right]^{\rm T}}\text{，}$ (3)
 $\delta {\bf{\omega }}_{ie}^n = {\left[ {\begin{array}{*{20}{c}} 0&{ - {\omega _{ie}}\sin L\delta L}&{{\omega _{ie}}\cos L\delta L} \end{array}} \right]^{\rm T}}\text{，}$ (4)
 $\delta {\bf{\omega }}_{en}^n = \left[ {\begin{array}{*{20}{c}} { - \displaystyle\frac{{\delta {V_N}}}{{R + h}} + \frac{{{V_N}\delta h}}{{{{\left( {R + h} \right)}^2}}}} \\ {\displaystyle\frac{{\delta {V_E}}}{{R + h}} - \frac{{{V_E}\delta h}}{{{{\left( {R + h} \right)}^2}}}} \\ {\displaystyle\frac{{\delta {V_E}}}{{R + h}}\tan L - \frac{{{V_E}\tan L\delta h}}{{{{\left( {R + h} \right)}^2}}} + - \frac{{{V_E}{{\sec }^2}L\delta L}}{{R + h}}} \end{array}} \right]\text{。}\!\!\!\!\!$ (5)

2）平台失准角误差方程

 ${{\dot{\bf \varphi }}^n} = \delta {\bf{\omega }}_{in}^n - ({\bf{\omega }}_{ie}^n + {\bf{\omega }}_{en}^n) \times {{\bf{\varphi }}^n} + {{\bf{\varepsilon }}^n}\text{。}$ (6)

3）位置误差方程

 $\begin{split} &\delta {{\dot{ P}}^n} = \left[ {\begin{array}{*{20}{c}} 0&{\displaystyle\frac{1}{{R + h}}}&0 \\ {\displaystyle\frac{{\sec L}}{{R + h}}}&0&0 \\ 0&0&1 \end{array}} \right] \cdot \delta {{{V}}^n}+\\ &\quad \left[ {\begin{array}{*{20}{c}} 0&0&{ - \displaystyle\frac{{{V_N}}}{{{{\left( {R + h} \right)}^2}}}} \\ {\displaystyle\frac{{{V_E}\sec L\tan L}}{{R + h}}}&0&{ - \displaystyle\frac{{{V_E}\sec L}}{{{{\left( {R + h} \right)}^2}}}} \\ 0&0&0 \end{array}} \right] \cdot \delta {{{P}}^n} \end{split}\text{。}$ (7)

 ${\dot{ X}} = {{AX}} + {{BW}}\text{。}$ (8)

 \begin{align} & {\bf X}=[\begin{matrix} \delta \lambda & \delta L & \delta h & \delta {{V}_{E}} & \delta {{V}_{N}} & \delta {{V}_{U}} & {{\varphi }_{E}} & {} & {} \\ \end{matrix} \\ & \begin{matrix} \ \ {{\varphi }_{N}} & {{\varphi }_{U}} & \varepsilon _{x}^{b} & \varepsilon _{y}^{b} & \varepsilon _{z}^{b} & \nabla _{x}^{b} & \nabla _{y}^{b} & \nabla _{z}^{b} \\ \end{matrix}{{]}^{\rm T}}\text{。} \\ \end{align} (9)

1.2 量测方程模型

1）INS/GPS量测方程

 ${Z_1} = \left[ {\begin{array}{*{20}{c}} {{L_I} - {L_G}} \\ {{\lambda _I} - {\lambda _G}} \\ {{h_I} - {h_G}} \\ {{v_{xI}} - {v_{xG}}} \\ {{v_{yI}} - {v_{yG}}} \\ {{v_{zI}} - {v_{zG}}} \end{array}} \right] = {{ H}_1}X + \gamma \text{。}$ (10)

2）INS/CNS量测方程

 ${\varPhi _{In}} = { C}_b^n{\varPhi _{Ib}}\;{\varPhi _{Cn}} = { C}_i^n{\varPhi _{Ci}}\text{。}$ (11)

 ${{ Z}_2} = \left[ {\begin{array}{*{20}{c}} {{\varPhi _{EIn}} - {\varPhi _{ECn}}} \\ {{\varPhi _{NIn}} - {\varPhi _{NCn}}} \\ {{\varPhi _{UIn}} - {\varPhi _{UCn}}} \end{array}} \right] = {{ H}_2}X + \kappa$ (12)

3）INS/GPS/CNS量测方程

 ${ Z} = \left[ {\begin{array}{*{20}{c}} {{L_I} - L} \\ {{\lambda _I} - \lambda } \\ {{h_I} - h} \\ {{v_{xI}} - {v_x}} \\ {{v_{yI}} - {v_y}} \\ {{v_{zI}} - {v_z}} \\ {{\varPhi _{EIn}} - {\varPhi _{En}}} \\ {{\varPhi _{NIn}} - {\varPhi _{Nn}}} \\ {{\varPhi _{UIn}} - {\varPhi _{Un}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\varPhi _{EIn}} - {\varPhi _{ECn}}} \\ {{\varPhi _{NIn}} - {\varPhi _{NCn}}} \\ {{\varPhi _{UIn}} - {\varPhi _{UCn}}} \end{array}} \right] = { H}X + \eta \text{。}$ (13)

2 基于EKF的INS/GPS/CNS滤波算法

 $\left\{ \begin{array}{l} {{{{\dot X}}}_k}{{ = }}{{{A}}_{k|k - 1}}{{{X}}_{k - 1}}{{ + }}{{{B}}_{k - 1}}{{{W}}_{k - 1}} \text{，} \\ {Z_k} = {H_k}{X_k} + {\eta _k} \text{。}\\ \end{array} \right.$ (14)

 ${{\hat{\rm X}}_{k|k - 1}}{\rm{ = }}{{{F}}_{k|k - 1}}{{\hat{\rm X}}_{k - 1}}\text{，}$ (15)
 ${{{P}}_{k|k - 1}}{{ = }}{{{F}}_{k|k - 1}}{{{P}}_{k - 1}}{{{F}}_{k|k - 1}^{\rm T}} + {G_{k - 1}}{Q_{k - 1}}G_{k - 1}^{\rm T}\text{，}$ (16)
 ${{{K}}_k}{{ = }}{{{P}}_{k|k - 1}}H_k^{\rm T}{\left( {H_k^{}{{\rm{P}}_{k|k - 1}}H_k^{\rm T} + {Q_k}} \right)^{\rm T}}\text{，}$ (17)
 ${{\hat{\rm X}}_k}{\rm{ = }}{{\hat{\rm X}}_{k|k - 1}} + {K_k}\left( {{Z_k} - {H_k}{{{\hat{\rm X}}}_{k|k - 1}}} \right)\text{，}$ (18)
 ${{{P}}_k} = \left( {I - {{{K}}_k}H_k^{}} \right){{{P}}_{k|k - 1}}{\left( {I - {{{K}}_k}H_k^{}} \right)^{\rm T}} + {K_k}{{ Q}_k}K_k^{\rm T}\text{。}$ (19)

3 试验结果与分析

 ${A_k} = \frac{{{e_k}}}{{{e_0}}}\text{。}$ (20)

 图 1 位置误差试验结果对比图 Fig. 1 Comparison of position error test results

 图 2 速度误差试验结果对比图 Fig. 2 Comparison of speed error test results

 图 3 姿态误差试验结果对比图 Fig. 3 Comparison of attitude error test results

4 结　语

 [1] CHRISTOPHER, MOORE, TERRY, et al. Adaptive Kalman filtering for low-cost INS/GPS[J]. Journal of Navigation, 2003, 56(1): 143-152. DOI:10.1017/S0373463302002151 [2] NEMRA A, AOUF N. Robust INS/GPS sensor fusion for UAV localization using SDRE nonlinear filtering[J]. IEEE Sensors Journal, 2010, 10(4): 789-798. DOI:10.1109/JSEN.2009.2034730 [3] ABDOLKARIMI E S, ABAEI G, MOSAVI M R. A wavelet-extreme learning machine for low-cost INS/GPS navigation system in high-speed applications[J]. Gps Solutions, 2018, 22(1): 15. DOI:10.1007/s10291-017-0682-x [4] WANG H, CAI Y. Study on INS/GPS/CNS integrated navigation systems considering of ship′s deformation[J]. Journal of Jiangsu University of Science & Technology, 2008. [5] GAO B, HU G, GAO S, et al. Multi-sensor optimal data fusion for INS/GNSS/CNS integration based on unscented Kalman filter[J]. International Journal of Control Automation & Systems, 2018, 16(1): 129-140. [6] WANG D, LV H, JIE W. A novel SINS/CNS integrated navigation method using model constraints for ballistic vehicle applications[J]. Journal of Navigation, 2017, 70(6): 1-23. [7] 张科, 刘海鹏, 李恒年, 钱山. SINS/GPS/ CNS组合导航联邦滤波算法[J]. 中国惯性技术学报, 2013, 21(2): 226-230. DOI:10.3969/j.issn.1005-6734.2013.02.019 [8] 吴坤峰, 杨峰, 梁彦, 张共愿, 程咏梅. INS/GPS/CNS组合导航系统仿真[J]. 火力与指挥控制, 2010, 35(7): 13-19. DOI:10.3969/j.issn.1002-0640.2010.07.004 [9] ZHANG Tong, CHEN Kang, FU Wen-xing. Optimal two-iteration sculling compensation mathematical framework for SINS velocity updating[J]. Journal of Systems Engineering and Electronics, 2014, 25(6): 1065-1071. DOI:10.1109/JSEE.2014.00122 [10] 严恭敏. 捷联惯导系统动基座初始对准及其它相关问题研究[D]. 西安: 西北工业大学. 2008.