﻿ 一种SINS/CNS/GNSS组合导航滤波算法
 文章快速检索 高级检索
 大地测量与地球动力学  2021, Vol. 41 Issue (7): 676-681  DOI: 10.14075/j.jgg.2021.07.004

### 引用本文

LIU Lili, LIN Xueyuan, YU Feng, et al. A Filtering Method of SINS/CNS/GNSS Integrated Navigation System[J]. Journal of Geodesy and Geodynamics, 2021, 41(7): 676-681.

### Foundation support

National Natural Science Foundation of China, No.60874112, 61673208; Yantai "Double Hundred Plan" Talent Project, No.YT201803.

### Corresponding author

LIN Xueyuan, PhD, professor, majors in integrated navigation system. E-mail: linxy_ytcn@126.com.

### 第一作者简介

LIU Lili, lecturer, majors in information processing and automation technology, E-mail: liulili.yc@163.com.

### 文章历史

1. 烟台南山学院工学院，山东省龙口市大学路12号，265713;
2. 南京航空航天大学航天学院，南京市御道街29号，210016;
3. 北京理工大学化学与化工学院，北京市中关村南大街5号，100081

1 组合导航系统建模 1.1 状态方程

 ${\mathit{\boldsymbol{f}}_i} = {\mathit{\boldsymbol{\dot \upsilon }}_i} - \mathit{\boldsymbol{g}}{}$ (1)

 $\mathit{\boldsymbol{C}}_b^g\mathit{\boldsymbol{C}}_i^b{\mathit{\boldsymbol{f}}_i} = \mathit{\boldsymbol{C}}_b^g{\mathit{\boldsymbol{f}}_b} = \mathit{\boldsymbol{C}}_i^{\rm{g}}{\mathit{\boldsymbol{\dot v}}_i} - \mathit{\boldsymbol{C}}_i^{\rm{g}}\mathit{\boldsymbol{g}}$ (2)

 $\mathit{\boldsymbol{C}}_b^{\rm{g}}{\mathit{\boldsymbol{f}}_b} = \mathit{\boldsymbol{\dot v}} - \mathit{\boldsymbol{C}}_i^{\rm{g}}\mathit{\boldsymbol{g}}$ (3)

 $\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot p}} = \mathit{\boldsymbol{\upsilon }}} \end{array}$ (4)

 $\mathit{\boldsymbol{\dot q}} = 0.5 \cdot \mathit{\boldsymbol{q}} \circ {\mathit{\boldsymbol{\omega }}_b}$ (5)

 $\left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{\omega }}_c} = {\mathit{\boldsymbol{\omega }}_b} + {\mathit{\boldsymbol{\omega }}_r} + {\mathit{\boldsymbol{\omega }}_\varepsilon }}\\ {{{\mathit{\boldsymbol{\dot \omega }}}_r} = {\mathit{\boldsymbol{\omega }}{}_n}} \end{array}} \right.$ (6)
 $\left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{f}}_c} = {\mathit{\boldsymbol{f}}_b} + {\mathit{\boldsymbol{f}}_r} + {\mathit{\boldsymbol{f}}_\varepsilon }}\\ {{{\mathit{\boldsymbol{\dot f}}}_r} = {\mathit{\boldsymbol{f}}_n}} \end{array}} \right.$ (7)

1.2 测量方程

 ${\boldsymbol{p}_c} = \boldsymbol{C}_i^g\boldsymbol{C}_e^i \cdot {\boldsymbol{p}_g} - \boldsymbol{C}_i^g{\boldsymbol{R}_o} + \boldsymbol{C}_i^g\boldsymbol{C}_e^i \cdot {\boldsymbol{p}_\varepsilon }$ (8)

 $\begin{array}{l} \;\;\;{\boldsymbol{q}_c} = \boldsymbol{q}_g^{ - 1} \circ {\boldsymbol{q}_s} \circ {\boldsymbol{q}_\varepsilon } \circ {\boldsymbol{q}_f} = {\boldsymbol{q}^t} \circ {\boldsymbol{q}_\varepsilon } \circ {\boldsymbol{q}_f} = \\ \left( {{\boldsymbol{q}^t} + \boldsymbol{q}_\varepsilon ^1} \right) \circ {\boldsymbol{q}_f} = {\boldsymbol{q}^t} \circ {\boldsymbol{q}_f} + \boldsymbol{q}_\varepsilon ^1 \circ {\boldsymbol{q}_f} = \boldsymbol{q} + {\boldsymbol{q}_\eta } \end{array}$ (9)

 $\begin{array}{l} \boldsymbol{q} = \boldsymbol{q}_g^{ - 1} \circ {\boldsymbol{q}_s} \circ {\boldsymbol{q}_f}\\ {\boldsymbol{q}_t} = \boldsymbol{q}_g^{ - 1} \circ {\boldsymbol{q}_s}\\ \boldsymbol{q}_\varepsilon ^1 = \left[ {\begin{array}{*{20}{l}} { - q_1^t{q_{\varepsilon 1}} - q_2^t{q_{\varepsilon 2}} - q_3^t{q_{\varepsilon 3}}}\\ { + q_0^t{q_{\varepsilon 1}} - q_3^t{q_{\varepsilon 2}} + q_2^t{q_{\varepsilon 3}}}\\ { + q_3^t{q_{\varepsilon 1}} + q_0^t{q_{\varepsilon 2}} - q_1^t{q_{\varepsilon 3}}}\\ { - q_2^t{q_{\varepsilon 1}} + q_1^t{q_{\varepsilon 2}} + {q_0}{q_{\varepsilon 3}}} \end{array}} \right]\\ {\boldsymbol{q}_\eta } = \boldsymbol{q}_\varepsilon ^1 \circ {\boldsymbol{q}_f} \end{array}$ (10)
2 基于EKF的组合导航算法

2.1 误差增量方程推导

 $\delta {\boldsymbol{\dot q}_{13}} = - [\boldsymbol{\dot \omega } \times ] \cdot \delta {\boldsymbol{q}_{13}} + 0.5 \cdot \delta \boldsymbol{\omega }$ (11)

 $\delta {\boldsymbol{\dot q}_{13}} = - [\boldsymbol{\hat \omega } \times ] \cdot \delta {\boldsymbol{q}_{13}} - 0.5 \cdot {\boldsymbol{\omega }_r} - 0.5 \cdot {\boldsymbol{\omega }_\varepsilon }$ (12)

 $\boldsymbol{C}_b^g{\boldsymbol{f}_b} = \left( {\boldsymbol{C}_b^g \cdot \delta \boldsymbol{C}_b^g} \right) \cdot {\boldsymbol{f}_c}$ (13)

 $\delta \boldsymbol{C}_b^g = \left[ {\begin{array}{*{20}{c}} 1&{ - 2\delta {q_3}}&{2\delta {q_2}}\\ {2\delta {q_3}}&1&{ - 2\delta {q_1}}\\ { - 2\delta {q_2}}&{2\delta {q_1}}&1 \end{array}} \right]$

 $\begin{array}{l} \boldsymbol{C}_b^g{\boldsymbol{f}_b} \approx \boldsymbol{C}_b^g \cdot \left( {{{\boldsymbol{\hat f}}_b} - {\boldsymbol{f}_r} - {\boldsymbol{f}_\varepsilon }} \right) + \\ \boldsymbol{C}_b^{\rm{g}} \cdot \left[ {\begin{array}{*{20}{c}} 0&{ - 2\delta {q_3}}&{2\delta {q_2}}\\ {2\delta {q_3}}&0&{ - 2\delta {q_1}}\\ { - 2\delta {q_2}}&{2\delta {q_1}}&0 \end{array}} \right] \cdot {{\boldsymbol{\hat f}}_b} = \\ \boldsymbol{C}_b^g \cdot {{\boldsymbol{\hat f}}_b} - \boldsymbol{C}_b^g \cdot {\boldsymbol{f}_r} - \boldsymbol{C}_b^g \cdot {\boldsymbol{f}_\varepsilon } + 2\boldsymbol{C}_b^g \cdot \left[ {{{\boldsymbol{\hat f}}_b} \times } \right] \cdot \delta {\boldsymbol{q}_{13}} \end{array}$ (14)

 $\left[ {{{\boldsymbol{\hat f}}_b} \times } \right] = \left[ {\begin{array}{*{20}{c}} 0&{ - {{\hat f}_{bz}}}&{{{\hat f}_{by}}}\\ {{{\hat f}_{bz}}}&0&{ - {{\hat f}_{bx}}}\\ { - {{\hat f}_{by}}}&{{{\hat f}_{bx}}}&0 \end{array}} \right]$

 $\boldsymbol{g} = \left[ \begin{array}{l} - \frac{{\mu x}}{{{r^3}}}\left[ {1 - {J_2}{{\left( {\frac{{{R_e}}}{r}} \right)}^2}\left( {7.5\frac{{{z^2}}}{{{r^2}}} - 1.5} \right)} \right]\\ - \frac{{\mu y}}{{{r^3}}}\left[ {1 - {J_2}{{\left( {\frac{{{R_e}}}{r}} \right)}^2}\left( {7.5\frac{{{z^2}}}{{{r^2}}} - 1.5} \right)} \right]\\ - \frac{{\mu z}}{{{r^3}}}\left[ {1 - {J_2}{{\left( {\frac{{{R_e}}}{r}} \right)}^2}\left( {7.5\frac{{{z^2}}}{{{r^2}}} - 4.5} \right)} \right] \end{array} \right]$ (15)

 $0 ＜ \left[ {1 - {J_2}{{\left( {\frac{{{R_e}}}{r}} \right)}^2}\left( {7.5\frac{{{z^2}}}{{{r^2}}} - 4.5} \right)} \right] ＜ 1$

 $- \frac{\mu }{{{r^3}}}{\rm{d}}x + 3\frac{{\mu {x^2}}}{{{r^5}}}{\rm{d}}x + 3\frac{{\mu xy}}{{{r^5}}}{\rm{d}}y + 3\frac{{\mu xz}}{{{r^5}}}{\rm{d}}z$

 $\boldsymbol{\dot v} - \boldsymbol{C}_i^g\boldsymbol{g} = \widehat {\boldsymbol{\dot v}} + \delta \boldsymbol{\dot v} - \boldsymbol{C}_i^g\boldsymbol{g}$ (16)

 $\delta \boldsymbol{\dot v} = 2\boldsymbol{C}_b^{\rm{g}} \cdot \left[ {{{\boldsymbol{\hat f}}_b} \times } \right] \cdot \delta {\boldsymbol{q}_{13}} - \boldsymbol{C}_b^{\rm{g}} \cdot \delta {\boldsymbol{f}_r} - \boldsymbol{C}_b^{\rm{g}} \cdot {\boldsymbol{f}_\varepsilon }$ (17)

 ${\boldsymbol{p}_c} - \boldsymbol{\hat p} = \delta \boldsymbol{p} + \boldsymbol{C}_i^{\rm{g}}\boldsymbol{C}_e^i \cdot {\boldsymbol{p}_\varepsilon }$ (18)

 ${\left( {{{\boldsymbol{\hat q}}^{ - 1}} \circ {\boldsymbol{q}_c}} \right)_{13}} = \delta {\boldsymbol{q}_{13}} + {\left( {{{\boldsymbol{\hat q}}^{ - 1}} \circ {\boldsymbol{q}_\eta }} \right)_{13}}$ (19)

2.2 EKF信息融合算法

 $\boldsymbol{\dot X}(t) = \boldsymbol{F}(t)\boldsymbol{X}(t) + \boldsymbol{G}(t)\boldsymbol{W}(t)$ (20)
 $\boldsymbol{Z}(t) = \boldsymbol{H}(t)\boldsymbol{X}(t) + \boldsymbol{V}(t)$ (21)
 $\boldsymbol{X} = \left[ {\begin{array}{*{20}{l}} {\delta {\boldsymbol{q}_{13}}}&{\delta \boldsymbol{p}}&{\delta \boldsymbol{v}}&{{\boldsymbol{\omega }_r}}&{{\boldsymbol{f}_r}} \end{array}} \right]$ (22)

 ${\boldsymbol{Z}_G}(t) = {\boldsymbol{H}_G}(t)\boldsymbol{X}(t) + {\boldsymbol{V}_G}(t)$ (23)

 ${\boldsymbol{Z}_C}(t) = {\boldsymbol{H}_C}(t)\boldsymbol{X}(t) + {\boldsymbol{V}_C}(t)$ (24)

3 仿真与分析 3.1 实验1

 图 1 飞行轨迹 Fig. 1 Flight path

 图 2 GNSS定位误差 Fig. 2 GNSS positioning error

 图 3 CNS定姿误差 Fig. 3 CNS attitude error
3.2 实验2

 图 4 姿态误差 Fig. 4 Attitude error

 图 5 位置误差 Fig. 5 Position error

 图 6 速度误差 Fig. 6 Velocity error

4 结语