舰船科学技术  2019, Vol. 41 Issue (10): 159-162   PDF    
一种基于INS/GPS/CNS的全信息导航滤波算法
尹洪亮1,2, 张嵘1, 郝强3, 于东康2     
1. 清华大学精密仪器系,北京 100084;
2. 中国舰船研究院,北京 100192;
3. 哈尔滨工业大学仪器科学与工程学院,哈尔滨 150001
摘要: 为解决INS/GPS两组合导航系统因观测信息不完整造成的姿态误差发散、系统稳定性较弱的问题,本文利用CNS搭建了基于INS/GPS/CNS的全观测信息导航系统。通过建立以位置误差、速度误差及平台失准角误差为观测量的系统数学模型,提出一种基于INS/GPS/CNS的全信息导航滤波算法,提高了系统的导航精度,改善了系统的稳定性。试验结果表明,本文所提算法相较于传统两组合滤波算法,各导航参数的精度在不同程度上得到改善,长航时条件下姿态误差收敛、稳定性较高。
关键词: INS/GPS/CNS     全信息滤波     EKF     组合导航    
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 引 言

由于惯性导航系统(Inertal Navigation System,INS)与全球定位系统(Global Positioning System,GPS)组合可以提供较高精度的速度、位置信息,所以INS/GPS组合系统被广泛应用在船舶、飞机、导弹等领域。但因INS/GPS组合系统量测信息仅包括位置和速度信息,缺少姿态信息观测量,导致系统的可观测性较差,容易造成系统在长时间导航应用中精度不高,甚至姿态信息发散的问题[13]

天文导航系统(Celestial Navigation System,CNS)采用星体敏感器测量天体方位信息,在经过解算后输出载体姿态信息,其特点是姿态测量精度高、测量误差误差不随时间积累、系统可靠性高,是一种新型的高精度姿态测量系统[46]

为增加INS/GPS组合系统的量测信息,提高系统导航精度,改善系统航向误差发散的问题,将CNS与传统INS/GPS系统组合是目前的研究热点。张科设计了基于联邦滤波的INS/GPS/CNS组合导航算法,但由于其采用的是线性卡尔曼滤波框架,不能很好适应INS/GPS/CNS这类典型非线性系统,造成滤波结果精度较低;吴坤峰考虑到CNS可能存在输出不连续的问题,利用GPS和CNS输出的高精度导航信息,分别设计了GPS/INS和CNS/INS滤波方案,在获得滤波后的导航信息后,将其作为全信息观测量进行二次滤波,最终获得高精度的导航信息。该方法计算较为繁琐,且二次滤波会造成系统实时性变差的问题,不宜开展算法的工程化应用[78]

针对上述问题,本文提出一种基于扩展卡尔曼滤波(Extended Kalman filter,EKF)的INS/GPS/CNS全信息导航滤波算法,利用GPS和CNS输出的高精度导航信息,结合非线性扩展卡尔曼滤波框架,设计全观测量的滤波算法,通过研制的INS/GPS/CNS三组合导航系统进行试验验证,验证了所提算法的高精度和长时间工作条件下的稳定性。

1 INS/GPS/CNS滤波模型

根据INS,GPS和CNS的系统特点,建立INS/GPS/CNS系统的数学模型。

1.1 状态方程模型

根据文献[9],建立INS数学模型如下:

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)

式中: $i$ 为惯性坐标系; $e$ 为地球坐标系; ${{{f}}^n}$ 为加速度计测得的比力在导航系中的投影; $\delta {{{V}}^n} = {\left[ {\delta {V_E}\;\,\delta {V_N}\;\,\delta {V_U}} \right]^{\rm T}}$ ,为东、北、天向的速度误差; ${{\bf{\varphi }}^n} = {\left[ {{\varphi _E}\;\,{\varphi _N}\;\,{\varphi _U}} \right]^{\rm T}}$ ,为东、北、天向的平台失准角; ${\nabla ^n}$ 为加速度计零偏在导航坐标系中的投影; ${\omega _{en}}$ 为牵连角速度; ${\omega _{ie}}$ 为地球自转角速度,根据INS系统特点,有

${\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)

其中: $L$ 为当地地理纬度, $h$ 为系统重心距地面高度, $R$ 为地球半径。

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)

式中: ${\varepsilon ^n}$ 为陀螺常值漂移在导航坐标系中的投影; $\delta {\bf{\omega }}_{in}^n = \delta {\bf{\omega }}_{ie}^n + \delta {\bf{\omega }}_{en}^n$

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)

式中: $\delta {{{P}}^n} = {\left[ {\begin{array}{*{20}{c}} {\delta \lambda }&{\delta L}&{\delta h} \end{array}} \right]^{\rm T}}$ $\delta \lambda $ $\delta L$ $\delta h$ 分别为当地经度、纬度和高度误差。

基于上述分析,选用INS模型作为INS/CNS/GPS系统滤波的基础框架,可获得状态模型如下:

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

其中: ${{A}}$ 为状态矩阵; ${{B}}$ 为系统噪声矩阵; ${{W}}$ 为系统噪声,具体表达式可参见文献[9]; ${{X}}$ 为系统状态参量,表达式如下:

$ \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)

其中: ${{\bf{\varepsilon }}^b} = {\left[ {\begin{array}{*{20}{c}} {\varepsilon _x^b}&{\varepsilon _y^b}&{\varepsilon _z^b} \end{array}} \right]^{\rm T}}$ ,为 $b$ 系三轴陀螺漂移; ${\nabla ^b} = {\left[ {\begin{array}{*{20}{c}} {\nabla _x^b}&{\nabla _y^b}&{\nabla _z^b} \end{array}} \right]^{\rm T}}$ ,为 $b$ 系加速度计零偏。

1.2 量测方程模型

1)INS/GPS量测方程

由于GPS可提供高精度的位置和速度信息,所以取INS和GPS输出的位置和速度信息之差作为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)

式中: ${{ H}_1}$ 为量测矩阵, ${{ H}_1} = \left[\right.$ $ \left.{\begin{array}{*{20}{c}} {{\rm {diag}}\left[\!\! {\begin{array}{*{20}{c}} 1\;\;1\;\;1\;\;1\;\;1\;\;1 \end{array}}\!\! \right]}\;\;{{0_{6 \times 9}}} \end{array}} \!\!\right]$ $\gamma $ 为量测噪声。

2)INS/CNS量测方程

由于INS输出为机体系下姿态( ${\varPhi _{Ib}}$ ),CNS为惯性系下姿态( ${\varPhi _{Cb}}$ ),所以需将两坐标系转化后构建量测方程。将姿态信息转化到导航系下:

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

式中: ${ C}_b^n$ 为机体系转化到导航系的姿态转移矩阵, ${ C}_i^n$ 为惯性系转化到导航系的姿态转移矩阵。

获得量测方程如下:

${{ 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)

式中: ${{ H}_2}$ 为量测矩阵, ${{ H}_2} = \left[ {{0_{3 \times 6}}}\;\;{C \bullet {\rm{diag}}\left[ {\begin{array}{*{20}{c}} 1\;\;1\;\;1 \end{array}} \right]}\right.$ $\left.{{0_{3 \times 6}}} \right]$ C为姿态角误差与平台误差角之间的转移矩阵,具体表达形式可参加文献[10]; $\kappa $ 为量测噪声。

3)INS/GPS/CNS量测方程

基于上述量测方程,构建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)

式中: ${ H}$ 为量测矩阵, ${{{ H}}_{2}}=\left[ \begin{matrix} {\rm {diag}}\end{matrix}\left[ 1 \;\; 1 \;\; 1 \;\; 1 \;\; 1 \;\; 1 \\ \right] \;\; \right.$ $\left.C\centerdot {\rm {diag}}\left[ 1 \;\; 1 \;\; 1 \right] \;\; {{0}_{3\times 9}} \right]$ $\eta $ 为量测噪声。

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

将式(8)和式(13)离散化获得系统离散化模型:

$\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)

其中: ${{{F}}_k}$ 为状态矩阵 ${{{A}}_k}$ 的雅克比行列式, ${{{P}}_k}$ ${{{Q}}_k}$ 分别为系统噪声和量测噪声协方差矩阵。

在初始状态向量 ${{{X}}_0}$ 和初始系统噪声 ${{{P}}_0}$ 已知时,再根据k时刻的量测信息 ${{{Z}}_k}$ ,即可计算得到k时刻状态向量的最优估计 ${{\hat{ X}}_k}$ ,获得高精度稳定的导航信息输出。

3 试验结果与分析

为验证本文提出的基于EKF全信息导航算法的有效性,搭建INS/GPS/CNS三组合导航系统进行精度验证,系统由激光惯导(陀螺漂移 $0.001^\circ /{\rm h}$ )、GPS接收机(定位误差100 m)和星敏感器组成,为便于精度验证,采用高精度转台作为姿态基准进行摇摆试验测试,速度、位置基准分别为0 m/s和0 m。为验证系统在长航时下的精度稳定性,试验时间设为24 h。采用传统两组合滤波算法作为对比,为更直观体现算法稳定性,在进行统计时,以初始时刻的导航误差 ${e_0}$ 为基准,计算后续误差 ${e_k}$ 相对于 ${e_0}$ 的稳定性A,如下式:

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

试验结果如图1图2图3所示。

图 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

图1可知,2种滤波算法的处理结果均收敛,但从数据波动程度来看,两组合算法波动较大,且根据稳定性比值峰峰值,三组合算法峰峰值较小,证明算法稳定性较好。由图2东向速度试验结果,三组合算法误差收敛很快,误差远小于两组合结果,证明三组合算法精度较高。由图3姿态误差试验结果可知,在长航时条件下,姿态信息有不同程度的发散,这是由于INS等效东向陀螺漂移的长期稳定性很难保证所致,在增加了CNS的姿态信息作为观测量后,滤波效果得到明显改善,姿态信息收敛且保持稳定。所以,相较于传统的INS/GPS两组合扩展卡尔曼滤波算法,INS/GPS/CNS三组合扩展卡尔曼滤波算法结果精度更高,长航时工作条件下稳定性更好。

4 结 语

本文针对INS/GPS两组合系统长航时条件下姿态精度发散、导航精度较低、系统稳定性较差的问题,引入CNS系统的姿态信息,构建一种基于INS/GPS/CNS的全信息导航滤波模型,提出一种基于EKF的INS/GPS/CNS滤波算法。搭建INS/GPS/CNS三组合系统对算法有效性进行验证,试验结果表明,相较于传统INS/GPS两组合算法,所提算法在长航时条件下稳定性强、收敛性好且精度高,工程应用意义较大。

参考文献
[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.