The application of improved Sage-Husa algorithm in aircraft integrated navigation
-
摘要: 针对机载组合导航系统，考虑不同飞行阶段的气压高度，提出一种改进的Sage-Husa自适应滤波算法，以提高组合导航系统定位精度. 该算法通过引入气压高度，实时计算并修正滤波异常判定的调节因子，以满足飞机不同飞行阶段的滤波需求. 通过捷联式惯性导航系统(SINS)、全球卫星导航系统(GNSS)定位误差特性仿真、卡尔曼滤波组合算法仿真、以及改进的Sage-Husa自适应滤波算法仿真，并对相关结果进行比较验证. 仿真结果表明，改进Sage-Husa自适应滤波可以提高滤波的自适应性，降低组合导航系统定位误差，取得较好的效果.
-
关键词:
- 组合导航 /
- 气压高度 /
- 改进的Sage-Husa滤波算法 /
- 定位误差 /
- 卡尔曼滤波
Abstract: Aiming at the airborne integrated navigation system, considering the air pressure altitude in different flight stages, an improved Sage-Husa adaptive filtering algorithm is proposed to improve the positioning accuracy of the integrated navigation system. This algorithm calculates and corrects the adjustment factor of filter anomaly determination in real time by introducing air pressure altitude to meet the filtering requirements of different flight stages of the aircraft. Through strap-down inertial navigation system (SINS), global navigation satellite system (GNSS) positioning error characteristics simulation, Kalman filter combination algorithm simulation, and improved Sage-Husa The adaptive filtering algorithm is simulated, and the relevant results are compared and verified. The simulation results show that improving the Sage-Husa adaptive filtering can improve the adaptiveness of the filtering, reduce the positioning error of the integrated navigation system, and achieve better results. -
0. 引 言
全球卫星导航定位技术在航空领域中的应用日益广泛. 以美国的全球定位系统（GPS）、俄罗斯的格洛纳斯系统（GLONASS）、欧盟的伽利略系统（Galileo）和中国的北斗卫星导航系统（BDS）为典型代表，共同组成了全球卫星导航系统（GNSS）[1]. GNSS具有全天候、导航定位精度高等优点，但其信号易受遮蔽与干扰，对精确导航定位产生影响. 机载惯性导航技术具有完全自主、无源性、实时性强，输出参数全面等优点，已成为现代民航飞机、军用飞机航电系统的重要组成部分，但其也有导航误差随时间快速发散、初始对准时间长等缺点[2]. 捷联惯性导航系统（SINS）具有结构简单、重量轻、可靠性高等优点，是目前最常使用的惯性导航系统. GNSS/SINS组合导航系统结合二者的优点，使组合后系统定位精度、连续性及有效性优于单系统工作状态[3-4].
GNSS/SINS组合导航系统一般采用卡尔曼滤波来实现[5]. 传统卡尔曼滤波需通过建立精确的系统模型与噪声模型来保持良好的滤波精度，但在实际应用中系统状态与干扰噪声统计特性难以预先进行准确判断和建模[6]，会出现滤波精度低甚至滤波发散等情况. 为解决上述问题，文献[7]应用了传统Sage-Husa自适应滤波算法，但算法计算量大，实际应用中难以满足滤波实时性的要求. 文献[8]和文献[9]中对传统Sage-Husa自适应滤波算法进行简化，并对系统量测噪声进行优化处理，提高了滤波的准确性，但算法并未考虑滤波异常的情况，使其无法在滤波出现异常情况时做出及时调整. 文献[10]结合滤波异常判据的条件，对Sage-Husa自适应滤波算法进行改进，使其适用于特定的系统工作环境，提高了滤波精度.
针对飞机全飞行过程，本文提出一种改进的Sage-Husa算法，引入飞机气压高度作为滤波异常判断条件，结合GNSS/SINS组合导航，提高定位精度，增强组合导航滤波算法自适应性.
1. GNSS/SINS组合导航系统模型
GNSS/SINS组合导航系统整体架构如图1所示. 其中GNSS接收机通过接收导航卫星信号，解算出用户的位置与速度，SINS通过惯性测量元件测量出用户的比力与角速度，经过惯性导航解算得到用户位置与速度. 最后将上述两种导航系统求得用户位置与速度通过组合导航系统进行数据融合，并利用融合后的数据进行SINS误差补偿，最终得到最优导航输出信息.
1.1 GNSS/SINS组合导航系统状态方程
GNSS/SINS组合导航系统中，选取的状态变量包括经度误差
$\delta {L_{\rm{I}}}$ 、纬度误差$\delta {\lambda _{\rm{I}}}$ 、高度误差$\delta {h_{\rm{I}}}$ 、东（E）、北（N）、天（U）坐标系下的E方向速度误差$\delta {v_{{\rm{Ie}}}}$ 、N方向速度误差$\delta {v_{{\rm{In}}}}$ 、U方向速度误差$\delta {v_{{\rm{Iu}}}}$ 、E方向姿态角误差$\delta {\varphi _{\rm{e}}}$ 、N方向姿态角误差$\delta {\varphi _{\rm{n}}}$ 、U方向姿态角误差$\delta {\varphi _{\rm{u}}}$ 、机体坐标系下三个坐标轴方向的陀螺仪漂移误差${\varepsilon _x}$ 、${\varepsilon _y}$ 、${\varepsilon _z}$ 以及加速度计漂移误差${\nabla _x}$ 、${\nabla _y}$ 、${\nabla _z}$ . 状态变量的微分方程如下：$$ \delta \overset{\centerdot }{{L}_{\rm{I}}}=\frac{\delta {v}_{\rm{In}}}{R+{h}_{\rm{I}}}-\frac{{v}_{\rm{In}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}},$$ (1) $$ \begin{split}\delta \overset{\centerdot }{{\lambda }_{\rm{I}}}=&\frac{\delta {v}_{\rm{Ie}}}{R+{h}_{\rm{I}}}{\rm{sec}}({L}_{\rm{I}})+\frac{{v}_{\rm{Ie}}}{R+{h}_{\rm{I}}}{\rm{sec}}{L}_{\rm{I}}{\rm{tan}}({L}_{\rm{I}})\delta {L}_{\rm{I}}-\\ &\frac{{v}_{\rm{Ie}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}}{\rm{sec}}({L}_{\rm{I}})\delta {h}_{\rm{I}}\end{split},$$ (2) $$ \delta \overset{\centerdot }{{h}_{\rm{I}}}=\delta {v}_{\rm{Iu}},$$ (3) $$ \begin{split}\delta {\overset{\centerdot }{v}}_{\rm{Ie}}=&{f}_{\rm{e}}\times {\varphi }_{\rm{e}}-(\frac{{v}_{\rm{In}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}}+\frac{\delta {v}_{\rm{In}}}{R+{h}_{\rm{I}}})\times {v}_{\rm{Ie}}+\\ &\frac{{v}_{\rm{In}}}{R+{h}_{\rm{I}}}\delta {v}_{\rm{Ie}}+{{C}}_{\rm{b}}^{\rm{n}}{\nabla }_{x}\end{split},$$ (4) $$ \begin{split}\delta {\overset{\centerdot }{v}}_{\rm{In}}=&{f}_{\rm{n}}\times {\varphi }_{\rm{n}}-(\frac{\delta {v}_{\rm{Ie}}}{R+{h}_{\rm{I}}}-\frac{{v}_{\rm{Ie}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}})\times {v}_{\rm{In}}+\\& {\rm{2\omega}}_{\rm{ie}}{\rm{sin}}({L}_{\rm{I}})\delta {L}_{\rm{I}}{v}_{\rm{In}}+2{\rm{\omega}}_{\rm{ie}}{\rm{cos}}({L}_{\rm{I}})\delta {v}_{\rm{In}}+\\ &\frac{{v}_{\rm{In}}}{R+{h}_{\rm{I}}}\delta {v}_{\rm{In}}+{{C}}_{\rm{b}}^{\rm{n}}{\nabla }_{y}\end{split},$$ (5) $$ \begin{split}\delta {\overset{\centerdot }{v}}_{\rm{Iu}}=&{f}_{\rm{u}}\times {\varphi }_{\rm{u}}-{\rm{2\omega }}_{\rm{ie}}{\rm{cos}}({L}_{\rm{I}})\delta {L}_{\rm{I}}{v}_{\rm{Iu}}+\\&\frac{\delta {v}_{\rm{Ie}}}{R+{h}_{\rm{I}}}{\rm{tan}}({L}_{\rm{I}}){v}_{\rm{Iu}}-\frac{{v}_{\rm{Ie}}{\rm{tan}}({L}_{\rm{I}})}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}}{v}_{\rm{Iu}}+\\& \frac{{v}_{\rm{Ie}}{{\rm{sec}}}^{\rm{2}}({L}_{\rm{I}})}{R+{h}_{\rm{I}}}{v}_{\rm{Iu}}\delta {L}_{\rm{I}}-{\rm{2\omega }}_{\rm{ie}}{\rm{sin}}({L}_{\rm{I}})\delta {v}_{\rm{Iu}}+\\& \frac{{v}_{\rm{Ie}}}{R+{h}_{\rm{I}}}{\rm{tan}}({L}_{\rm{I}})\delta {v}_{\rm{Iu}}+{{C}}_{\rm{b}}^{\rm{n}}{\nabla }_{z}\end{split},$$ (6) $$ \begin{split}\delta {\overset{\centerdot }{\varphi }}_{\rm{Ie}}=&\frac{{v}_{\rm{In}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}}-\frac{\delta {v}_{\rm{In}}}{R+{h}_{\rm{I}}}+\\& \frac{{v}_{\rm{In}}}{R+{h}_{\rm{I}}}\delta {\varphi }_{\rm{e}}+{{C}}_{\rm{b}}^{\rm{n}}{\varepsilon }_{x}\end{split},$$ (7) $$ \begin{split}\delta {\overset{\centerdot }{\varphi }}_{\rm{n}}=&-{\rm{\omega }}_{\rm{ie}}{\rm{sin}}({L}_{\rm{I}})+\frac{\delta {v}_{\rm{Ie}}}{{{R+}}{h}_{\rm{I}}}-\frac{{v}_{\rm{Ie}}}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}}-\\& ({\rm{\omega }}_{\rm{ie}}{\rm{cos}}({L}_{\rm{I}})+\frac{{v}_{\rm{Ie}}}{R+{h}_{\rm{I}}})\times {\varphi }_{\rm{n}}+{{C}}_{\rm{b}}^{\rm{n}}{\varepsilon }_{y}\end{split},$$ (8) $$ \begin{split}\delta {\overset{\centerdot }{\varphi }}_{\rm{u}}=&{\rm{\omega }}_{\rm{ie}}{\rm{cos}}({L}_{\rm{I}})\delta {L}_{\rm{I}}+\frac{{v}_{\rm{Ie}}{\rm{tan}}({L}_{\rm{I}})}{R+{h}_{\rm{I}}}-\frac{{v}_{\rm{Ie}}{\rm{tan}}({L}_{\rm{I}})}{{(R+{h}_{\rm{I}})}^{\rm{2}}}\delta {h}_{\rm{I}}+\\& \frac{{v}_{\rm{Ie}}{{\rm{sec}}}^{\rm{2}}({L}_{\rm{I}})}{R+{h}_{\rm{I}}}\delta {L}_{\rm{I}}-\frac{{v}_{\rm{Ie}}{\rm{tan}}({L}_{\rm{I}})}{R+{h}_{\rm{I}}}{\varphi }_{\rm{Iu}}+\\& {\rm{\omega }}_{\rm{ie}}{\rm{sin}}({L}_{\rm{I}}){\varphi }_{\rm{Iu}}+{{C}}_{\rm{b}}^{\rm{n}}{\varepsilon }_{y}\end{split},$$ (9) $$ \overset{\centerdot }{{{\varepsilon}} }=-\frac{\rm{1}}{T}{{\varepsilon}} +{{w}}_{\rm{r}},$$ (10) $$ \overset{\centerdot }{\nabla }=-\frac{\rm{1}}{T}\nabla +{{w}}_{\rm{a}},$$ (11) $${{\varepsilon }} = {\begin{bmatrix} {{\varepsilon _x},}&{{\varepsilon _y},}&{{\varepsilon _z}} \end{bmatrix}^{\rm{T}}},$$ (12) $$\nabla = {\begin{bmatrix} {{\nabla _x},}&{{\nabla _y},}&{{\nabla _z}} \end{bmatrix}^{\rm{T}}}.$$ (13) 式中：
$R$ 为地球半径；${{\rm{\omega }}_{{\rm{ie}}}}$ 为地球自转角速度；${{C}}_{\rm{b}}^{\rm{n}}$ 为机体坐标系到导航坐标系转换的方向余弦矩阵，${{C}}_{\rm{b}}^{\rm{n}}$ 的表达式为$${{C}}_{\rm{b}}^{\rm{n}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {{{{C}}_{\rm{1}}},}&{{{{C}}_{\rm{2}}},}&{{{{C}}_{\rm{3}}}} \end{array}} \right],$$ (14) $${{{C}}_{\rm{1}}} = {\begin{bmatrix} {\cos \alpha \cos \phi - \sin \alpha \sin \theta } \\ { - \cos \theta \sin \phi } \\ {\sin \alpha \cos \phi + \cos \alpha \sin \theta \sin \phi } \end{bmatrix}} ,$$ (15) $${{{C}}_{\rm{2}}} = {\begin{bmatrix} {\cos \alpha \sin \phi + \sin \alpha \sin \theta \cos \alpha } \\ {\cos \theta \cos \phi } \\ {\sin \alpha \sin \theta - \cos \alpha \sin \theta \cos \phi } \end{bmatrix}} ,$$ (16) $${{{C}}_{\rm{3}}} = {\begin{bmatrix} { - \sin \alpha \cos \theta } \\ {\sin \theta } \\ {\cos \alpha \cos \theta } \end{bmatrix}} ,$$ (17) $\alpha $ 为飞机滚转角，$\theta $ 为飞机俯仰角，$\phi $ 为飞机航向角，$T$ 为相关时间；${{{w}}_{\rm{r}}}$ 与${{{w}}_{\rm{a}}}$ 分别为陀螺仪与加速度计的白噪声（WN). 由此建立系统状态方程为$$ \overset{\centerdot }{{{X}}}(t)={{F}}(t){{X}}(t)+{{G}}(t){{W}}(t).$$ (18) 式(18)中：
$${{X}}(t) = { {\begin{bmatrix} {{{{X}}_1}(t),}\;\;{{{{X}}_{\rm{2}}}(t),}\;\;{{{{X}}_{\rm{3}}}(t),}\;\;{\begin{array}{*{20}{c}}\!\!\!\! {{{\varepsilon }},}\;\nabla \end{array}} \!\!\!\!\!\!\end{bmatrix}}^{\rm{T}}},$$ (19) $${{{X}}_1}(t) = {\begin{bmatrix} {\delta L,}&{\delta \lambda ,}&{\delta h} \end{bmatrix}^{\rm{T}}},$$ (20) $${{{X}}_2}(t) = {\begin{bmatrix} {\delta {v_{\rm{e}}},}&{\delta {v_{\rm{n}}},}&{\delta {v_{\rm{u}}}} \end{bmatrix}^{\rm{T}}},$$ (21) $${{{X}}_3}(t) = {\begin{bmatrix} {\delta {\varphi _{\rm{e}}},}&{\delta {\varphi _{\rm{n}}},}&{\delta {\varphi _{\rm{u}}}} \end{bmatrix}^{\rm{T}}};$$ (22) ${{G}}(t)$ 表达式为$${{G}}(t) = {\begin{bmatrix} {{{{0}}_{9 \times 6}}} \\ {{{{I}}_{6 \times 6}}} \end{bmatrix}} ;$$ (23) ${{W}}(t)$ 表达式为$${{W}}(t) = {\begin{bmatrix} {{w_{{\rm{r}}x}},}\;{{w_{{\rm{r}}y}},}\!\!{\begin{array}{*{20}{c}} {{w_{{\rm{r}}z}},}\;{{w_{{\rm{a}}x}},}\;{{w_{{\rm{a}}y}},}\;{{w_{{\rm{a}}z}}} \end{array}} \!\!\!\!\!\!\end{bmatrix}^{\rm{T}}};$$ (24) ${{F}}(t)$ 为系统状态转移矩阵，表达式为$${{F}}(t) = { {\begin{bmatrix} {{{{F}}_1}(t)}&{{{{F}}_2}(t)} \\ {{{{0}}_{6 \times 9}}}&{{{{0}}_{6 \times 9}}} \end{bmatrix}} _{15 \times 15}},$$ (25) 矩阵
${{{F}}_1}(t)$ 各元素可由系统状态变量的微分方程推导出,${{{F}}_2}(t)$ 表达式为$${{{F}}_2}(t) = \left[ {\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}}&{{{{0}}_{3 \times 3}}} \\ {\begin{array}{*{20}{c}} {{{{0}}_{3 \times 3}}} \\ {{{C}}_{\rm{b}}^{\rm{n}}} \end{array}}&{\begin{array}{*{20}{c}} {{{C}}_{\rm{b}}^{\rm{n}}} \\ {{{{0}}_{3 \times 3}}} \end{array}} \end{array}} \right].$$ (26) 1.2 GNSS/SINS组合导航系统量测方程
GNSS/SINS组合导航系统采用松组合方式，故将GNSS解算的位置和速度与SINS测量得到的位置和速度之差，作为组合导航系统量测信息. 将飞机在导航坐标系下的真实位置设为
$\left( {L,\lambda ,h} \right)$ ，真实速度设为$({v_{\rm{e}}},{v_{\rm{n}}},{v_{\rm{u}}})$ ，SINS解算的位置与速度可分别为$\left( {{L_{\rm{I}}},{\lambda _{\rm{I}}},{h_{\rm{I}}}} \right)$ 、$({v_{{\rm{Ie}}}},{v_{{\rm{In}}}},{v_{{\rm{Iu}}}})$ . GNSS解算的位置与速度可分别为$\left( {{L_{\rm{G}}},{\lambda _{\rm{G}}},{h_{\rm{G}}}} \right)$ 、$({v_{{\rm{Ge}}}},{v_{{\rm{Gn}}}},{v_{{\rm{Gu}}}})$ ，可得GNSS/SINS组合导航系统量测方程为$$\begin{split}\begin{array}{l} {{Z}}(t) = \begin{bmatrix} ({L_{\rm{I}}} - {L_{\rm{G}}})R \\ ({\lambda _{\rm{I}}} - {\lambda _{\rm{G}}})R\cos( L )\\ \begin{array}{*{20}{c}} {}&{{h_{\rm{I}}} - {h_{\rm{G}}}}&{} \end{array} \\ \begin{array}{*{20}{c}} {}&{{v_{{\rm{Ie}}}} - {v_{{\rm{Ge}}}}}&{} \end{array} \\ \begin{array}{*{20}{c}} {}&{{v_{{\rm{In}}}} - {v_{{\rm{Gn}}}}}&{} \end{array} \\ \begin{array}{*{20}{c}} {}&{{v_{{\rm{Iu}}}} - {v_{{\rm{Gu}}}}}&{} \end{array} \end{bmatrix} = \begin{bmatrix} (\delta {L_{\rm{I}}} - \delta {L_{\rm{G}}})R \\ (\delta {\lambda _{\rm{I}}} - \delta {\lambda _{\rm{G}}})R\cos( L )\\ \begin{array}{*{20}{c}} {}&{\begin{array}{*{20}{c}} {\delta {h_{\rm{I}}} - \delta {h_{\rm{G}}}}&{} \end{array}} \end{array} \\ \begin{array}{*{20}{c}} {}&{\delta {v_{{\rm{Ie}}}} - \delta {v_{{\rm{Ge}}}}}&{} \end{array} \\ \begin{array}{*{20}{c}} {}&{\delta {v_{{\rm{In}}}} - \delta {v_{{\rm{Bn}}}}}&{} \end{array} \\ \begin{array}{*{20}{c}} {}&{\delta {v_{{\rm{Iu}}}} - \delta {v_{{\rm{Bu}}}}}&{} \end{array} \end{bmatrix} \\ \;\;\;\;\;\;\;\;\; = {{H}}(t){{X}}(t) + {{V}}(t). \end{array} \\[-20pt] \end{split} $$ (27) 式中：
$ {{H}}(t)$ 为$${{H}}(t) = {\begin{bmatrix}\!\!\!\!\!\!\!\! {\begin{array}{*{20}{c}} R \\ {\begin{array}{*{20}{c}} 0 \\ {{{{0}}_{4 \times 1}}} \end{array}} \end{array}}\!\!\!\!\!\!\!\!{\begin{array}{*{20}{c}} 0 \\ {\begin{array}{*{20}{c}} {R\cos (L)} \\ {{{{0}}_{4 \times 1}}} \end{array}} \end{array}}\!\!\!\!{\begin{array}{*{20}{c}} {{{{0}}_{1 \times 4}}} \\ {\begin{array}{*{20}{c}} {{{{0}}_{1 \times 4}}} \\ {{{{I}}_{4 \times 4}}} \end{array}} \end{array}}\!\!\!\!{\begin{array}{*{20}{c}} {{{{0}}_{1 \times 9}}} \\ {\begin{array}{*{20}{c}} {{{{0}}_{1 \times 9}}} \\ {{{{0}}_{4 \times 9}}} \end{array}} \end{array}} \!\!\!\!\!\!\!\!\end{bmatrix}} ;$$ (28) 量测噪声矩阵
${{V}}(t)$ 为$${{V}}(t) = \left[\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\! {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} {\delta {L_{\rm{G}}}R} \\ {\delta {\lambda _{\rm{G}}}R\cos (L)} \end{array}} \\ {\delta {h_{\rm{G}}}} \end{array}} \\ {\delta {v_{{\rm{Ge}}}}} \end{array}} \\ {\delta {v_{{\rm{Gn}}}}} \end{array}} \\ {\delta {v_{{\rm{Gu}}}}} \end{array}} \!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\right].$$ (29) 将式(18)与式(27)进行离散化，可得GNSS/SINS组合导航离散时间的系统模型为
$$\left\{ \begin{array}{l} {{{X}}_{{k}}} = {{{\varPhi }}_{k,k - 1}}{{{X}}_{{{k - }}1}}{\rm{ + }}{{{\varGamma }}_{k - 1}}{{{W}}_{k - 1}} \\ {{{Z}}_k} = {{{H}}_k}{{{X}}_{{k}}} + {{{V}}_k} \end{array} \right..$$ (30) 式中：
${{{\varPhi }}_{k,k - 1}}$ 为离散化后的系统状态一步转移矩阵；${{{\varGamma }}_{k - 1}}$ 为离散系统噪声分配矩阵；${{{W}}_{k - 1}}$ 为系统噪声矩阵；${{{H}}_k}$ 为离散系统量测转移矩阵；${{{V}}_k}$ 为量测噪声矩阵.2. 改进的Sage-Husa自适应滤波算法
2.1 Sage-Husa自适应滤波算法特性分析
针对离散化后的GNSS/SINS组合导航系统模型，其系统噪声
${{{W}}_{k - 1}}$ 与量测噪声${{{V}}_k}$ 满足以下统计特性：${\rm{E(}}{{{W}}_k}{\rm{) = }}{q_k}$ ，${\rm{D(}}{{{W}}_k}{\rm{) = }}{Q_k}$ ，${\rm{E(}}{{{V}}_k}{\rm{) = }}{r_k}$ ，${\rm{D(}}{{{V}}_k}{\rm{) = }}{R_k}$ 且${\rm{E[(}}{{{W}}_k} - {q_k}{\rm{)(}}{{V}}_k^{\rm{T}} - {r_k}{\rm{)] = 0}}$ . 即系统噪声${{{W}}_k}$ 的均值为${q_k}$ ，方差为${Q_k}$ ，系统量测噪声${{{V}}_k}$ 的均值为${r_k}$ ，方差为${R_k}$ ，且两者相互独立. 传统卡尔曼滤波算法中，要求系统噪声${{{W}}_{k - 1}}$ 与量测噪声${{{V}}_k}$ 均为均值是零的高斯白噪声（WGN)，针对噪声对象单一，实际应用中噪声类型复杂，噪声统计特性难以判断，卡尔曼滤波算法难以获得系统状态最优估计.Sage-Husa算法对系统状态进行最优估计过程中，对系统噪声
${{{W}}_{k - 1}}$ 与量测噪声${{{V}}_k}$ 的均值与方差进行实时估计与校正，确保系统滤波处于正常状态. 然而在飞机实际飞行过程中，当其飞行环境平稳，系统滤波正常时，无需对系统噪声的统计特性进行校正，因此文献[11]中引入滤波异常的判据条件：$${{{v}}_k}{{v}}_k^{\rm{T}}{{ > }}\gamma \cdot {\rm{E[}}{{{v}}_k}{{v}}_k^{\rm{T}}{\rm{],}}\gamma \geqslant {\rm{1}}.$$ (31) $${\rm{E[}}{{{v}}_k}{{v}}_k^{\rm{T}}{\rm{] = }}{{{H}}_k}{{{P}}_{k.k - 1}}{{H}}_k^{\rm{T}} + {{{R}}_k}.$$ (32) 式中：
$\gamma $ 为调节因子，表示滤波异常判断的严格程度；${{{v}}_k}$ 表示量测新息序列，${\rm{E[}}{{{v}}_k}{{v}}_k^{\rm{T}}{\rm{]}}$ 表示新息序列方差的理论预计值. 当式(31)成立时，说明新息序列的实际方差大于理论预计值的$\gamma $ 倍，系统滤波出现异常，需对量测噪声做出调整，保证系统正常滤波.2.2 改进的Sage-Husa自适应滤波算法
民用航空飞机的飞行阶段可划分为：起飞、爬升、巡航、进近、着陆阶段，其中起飞和着陆时间约占飞机总飞行时间的6%，但是却有近一半的飞行事故发生在该阶段[12]. 另外，飞机的起飞和着陆阶段，相比于爬升与巡航阶段，距离地面高度较低，导航定位精度对飞行安全的影响增大.
因此，当飞机飞行高度越低时，组合导航系统滤波结果要求更加精确，保证飞行所需的定位精度. 通常情况下，在WN模型已知的前提下，卡尔曼滤波输出为最优线性无偏估计，但是在复杂的飞行环境中，飞机受到的干扰噪声(如：空间中的电子干扰，机场终端区由于多路径效应产生的反射叠加噪声等)并不能全面提前预测，对于未知噪声建立精确数学模型并掌握其数理统计特性是非常困难的. 当飞机实际飞行所受到的干扰噪声与提前建立的卡尔曼滤波算法中的噪声模型不匹配甚至相差特别大时，卡尔曼滤波输出将会出现跳变、发散等滤波异常情况.
为满足飞机在不同飞行阶段中GNSS/SINS组合导航系统滤波的精确性与自适应性，本文利用飞机气压高度
${h_{\rm{p}}}$ ，对Sage-Husa自适应滤波算法进行改进. 飞机在巡航阶段，气压高度的参考基准面为国际标准气压海平面（101.325 kPa），而当飞机进入机场管制区域时，气压高度的参考基准面为当地修正海平面[13]，在特定飞行阶段中，气压高度的参考基准面具有确定性与统一性. 因此，引入气压高度来对式(31)中的调节因子$\gamma $ 进行优化，令$$\gamma {\rm{ = }}B\;{\log _a}{h_{\rm{p}}} + C.$$ (33) 式中：
$B$ 为大于0的常数，$a$ 与$C$ 也为常数，$a$ 的取值范围为${\rm{1}} < a \leqslant {\rm{10}}$ ，${h_{\rm{p}}}$ 为气压高度. 常数$B$ 、$a$ 、$C$ 在飞机起飞前可根据实际滤波异常判别标准要求范围进行设定. 则改进后的Sage-Husa自适应滤波算法为：$${{\mathop {{X}}\limits^ {\wedge}} _{k,k - {\rm{1}}}} = {{{\varPhi }}_{k,k - 1}}{{{X}}_{k{\rm{ - }}1}}{\rm{ + }}{{{\varGamma }}_{k - 1}}{{{W}}_{k - 1}},$$ (34) $${{{P}}_{k,k - {\rm{1}}}} = {{{\varPhi }}_{k,k - {\rm{1}}}}{{{P}}_{k - {\rm{1}}}} {{\varPhi }}^{\rm{T}}_{k,k -1} + {{{Q}}_{k - {\rm{1}}}},$$ (35) $${{{v}}_k} = {{{Z}}_k} - {{{H}}_k}{{\mathop {{X}}\limits^ {\wedge}} _{k,k - {\rm{1}}}},$$ (36) $${{{K}}_k} = {{{P}}_{k,k - {\rm{1}}}}{{H}}_k^{\rm{T}}({{{H}}_k}{{{P}}_{k,k - {\rm{1}}}}{{H}}_k^{\rm{T}} + {{\mathop {{R}}\limits^ {\wedge}} _{k - {\rm{1}}}}),$$ (37) $${{\mathop {{X}}\limits^ {\wedge}} _k} = {{\mathop {{X}}\limits^ {\wedge}} _{k,k - {\rm{1}}}} + {{{K}}_k}{{{v}}_k},$$ (38) $${{{P}}_k} = ({{I}} - {{{K}}_k}{{{H}}_k}){{{P}}_{k,k - {\rm{1}}}},$$ (39) $${{\mathop {{R}}\limits^ {\wedge}} _k} = ({\rm{1}} - {d_k}){{\mathop {{R}}\limits^ {\wedge}} _{k - {\rm{1}}}} + {d_k}({{{v}}_k}{{v}}_k^{\rm{T}} - {{{H}}_k}{{{P}}_{k,k - {\rm{1}}}}{{H}}_k^{\rm{T}}).$$ (40) $$\begin{split} {{\mathop {{Q}}\limits^ {\wedge}} _k} = &({\rm{1}} - {d_k}){{\mathop {{Q}}\limits^ {\wedge}} _{k - {\rm{1}}}} + {d_k}({{{K}}_k}{{{v}}_k}{{v}}_k^{\rm{T}}{{K}}_k^{\rm{T}} + \\& {{{P}}_k} - {{{\varPhi }}_{k,k - {\rm{1}}}}{{{P}}_{k - {\rm{1}}}}{{\varPhi }}_{k,k - {\rm{1}}}^{\rm{T}}) \end{split} ,$$ (41) $$\begin{split}{d_k}\! =\! \left\{\!\!\!\!\!\! {\begin{array}{*{20}{c}} {({\rm{1}}\! -\! b)/({\rm{1}} \!-\! {b^{k + {\rm{1}}}}),{{v}}{{{v}}^{\rm{T}}}{{ > }}\gamma {\rm{(}}{{H}}{{{P}}_{k.k - 1}}{{{H}}^{\rm{T}}} + {{{R}}_k})} \\ {\;\;\;{\rm{ 0, }}\;\;\;\;\;\;\;{{v}}{{{v}}^{\rm{T}}} \leqslant \gamma {\rm{(}}{{H}}{{{P}}_{k.k - 1}}{{{H}}^{\rm{T}}} + {{{R}}_k})} \end{array}}\!\!\!\!\!\! \right..\end{split}$$ (42) 其中，
$b$ 称为遗忘因子，一般在0.95～0.99选取，调节因子$\gamma {\rm{ = }}B\;{\log _a}{h_{\rm{p}}} + C$ . 改进后的自适应滤波算法，滤波异常判断严格程度随着气压高度的变化而变化. 当飞机气压高度变低时，调节因子$\gamma $ 随之变低，系统滤波异常判断严格程度变高[14]，当其气压高度变高，飞机在相应飞行高度层间隔增大，可进行调节飞行状态的空间与时间比在起飞与着陆阶段更加充足，此时调节因子$\gamma $ 增大，系统滤波异常判断严格程度降低，由此来增强飞机在不同飞机阶段组合导航系统滤波的自适应性. 改进后的Sage-Husa自适应滤波算法流程如图2所示3. 改进Sage-Husa自适应滤波算法仿真与结果分析
3.1 仿真模块与仿真条件设置
本文利用从我国西南某城市到北方某城市真实机载快速数据记录器（QAR）的数据对改进后Sage-Husa自适应滤波算法有效性进行验证. 验证平台包含多个仿真模块：SINS模块、GNSS模块、组合导航滤波模块. QAR数据可提供飞机位置、气压高度等信息，SINS模块提供惯性元件测量的角速度、比力信息以及惯导系统解算的、位置信息，GNSS模块提供导航卫星系统解算的用户速度与位置信息，组合导航滤波模块对两种导航信息进行数据融合，输出组合导航数据信息.
系统仿真时间为500 s，滤波频率为1 Hz；陀螺仪误差为均值为0 °/h，方差为0.03 °/h的WGN；加速度计误差为均值为0 g，方差为1×10−5 g的WGN；GNSS测量伪距误差为均值为10 m，方差为5 m的WGN. SINS输出频率为20 Hz，GNSS输出频率为1 Hz. 在200～210 s内与350～360 s内使其量测噪声扩大到原来的5倍与10倍，用来模拟实际飞行环境中飞机受到噪声干扰及量测噪声发生突变的情况.
3.2 仿真结果分析
利用真实QAR数据获取验证用飞行轨迹，如图3所示，初始位置为北纬30.56°，东经103.94°，气压高度为489.51 m.
在本文仿真条件下，SINS与GNSS在导航坐标系下的定位误差如图4、图5所示.
由图4可知，SINS定位误差随着时间增长迅速积累，在东（E）、北（N）、天（U）三个方向都存在不同程度发散，其中在系统仿真时间结束时，E方向定位误差发散到77.50 m，N方向定位误差发散到91.68 m，U方向定位误差发散到140.92 m. 由图5可知，在本文条件下GNSS水平方向定位精度要优于垂直方向的定位精度，其中东向定位误差范围为−20.40～24.33 m，N方向定位误差范围为−24.01～25.59 m，U方向定位误差范围为−33.21～31.59 m. 通过仿真结果可以看出，SINS定位误差随着时间的积累而发散，故不适合长时间依靠该系统进行定位，GNSS定位误差虽不会出现发散现象，但当可见星数量减少或出现导航信号受到遮蔽、干扰时，GNSS定位误差也会出现增大的现象. 因此不管是SINS还是GNSS，其单系统定位精度都不能满足飞机飞行时定位精度要求，故需对不同导航方式进行组合，来提高飞机导航定位精度.
改进Sage-Husa自适应滤波算法的遗忘因子选取b=0.98，调节因子
$\gamma $ 为$$\gamma {\rm{ = 1}}{\rm{.5}}\;{\log _{{\rm{10}}}}\left( {{h_{\rm{p}}}} \right) - {\rm{1}}.$$ (42) 图6为改进前后的调节因子变化曲线对比图. 通过引入QAR中气压高度数据，对调节因子进行改进. 从改进后的曲线可以看出，飞机起飞（0～50 s）、进近着陆(420～500 s)阶段调节因子数值偏小，此时滤波异常判断标准严格程度高. 飞机在航路阶段(51～419 s)时，调节因子数值偏大，滤波异常判断标准严格程度变低，符合高空飞机之间间距要求较大的预期. 根据气压高度调整滤波异常判断标准的严格程度，在确保飞行安全的前提下，提高了算法的自适应性. 而改进前的调节因子为常值，难以根据气压高度的变化对系统滤波异常判断标准的严格程度进行动态调整.
在前述仿真条件下，分别进行卡尔曼滤波组合计算、改进前Sage-Husa自适应滤波算法、改进后Sage-Husa自适应滤波算法的组合导航解算的仿真，定位误差的仿真结果如图7～图9所示.
由图7、图8、图9以及表1可知，在文中仿真条件下，在200～210 s与350～360 s时间段内增大量测噪声后，卡尔曼滤波算法在E、N、U方向定位误差发生明显跳变，最大值分别为32.63 m、26.32 m、78.37 m，误差的均方根（RMS）值分别为11.25 m、12.58 m、13.32 m，且误差值随时间增长呈发散趋势，定位精度较低且误差波动范围较大，难以满足飞机起降阶段高精度定位的要求. 改进前Sage-Husa自适应滤波算法在E、N、U三个方向的定位误差最大值分别为6.83 m、9.35 m、20.42 m，误差的RMS值下降到4.39 m、4.72 m、4.58 m，提高系统定位精度的同时，减小了误差波动范围. 改进后的Sage-Husa自适应滤波算法在E、N、U三个方向定位误差最大值分别为3.86 m、6.42 m、12.97 m，误差的RMS值分别为3.05 m、3.98 m、3.62 m. 可以看出，Sage-Husa自适应滤波算法在量测噪声发生突变时，仍能使系统保持良好的滤波输出，且对误差发散现象起到良好的抑制效果. 改进后的算法不仅降低了定位误差，使误差波动更加平滑，提高了定位精度，且随着飞行气压高度变化，借助调节因子的改变，使其适用于复杂的飞行环境与不同飞行阶段，增强了组合导航算法自适应性，可以更好地确保飞机安全平稳飞行.
表 1 导航系下定位误差比较m 算法 统计量 E方向误差 N方向误差 U方向误差 卡尔曼
滤波算法最大值 32.63 26.32 78.37 RMS 11.25 12.58 13.32 改进前
Sage-Husa算法最大值 6.83 9.35 20.42 RMS 4.39 4.72 4.58 改进后
Sage-Husa算法最大值 3.86 6.42 12.97 RMS 3.05 3.98 3.62 4. 结束语
本文针对飞机起飞、爬升、巡航、进近、着陆不同飞行阶段，利用气压高度对Sage-Husa自适应滤波算法进行改进，同时对SINS与GNSS单系统的工作以及通过传统卡尔曼滤波算法进行组合导航工作时的定位误差特性进行仿真与结果分析，可获得以下结论：
1) SINS定位误差随时间而积累，长时间工作会导致其误差发散严重，定位精度随系统时间增加严重降低. 利用GNSS定位时，其定位误差波动较大且导航信号易受遮蔽与干扰，不适用于在复杂电磁环境中工作，定位精度易受影响；
2) 卡尔曼滤波将SINS与GNSS进行组合导航，在一定程度上能够提高定位精度，但传统卡尔曼滤波算法对系统过程噪声与量测噪声的统计特性规定较严格，在实际复杂噪声环境应用中存在局限性，难以保证飞机在受到干扰时，仍保持良好滤波效果；
3) 利用气压高度对Sage-Husa自适应滤波算法进行改进，将判断滤波异常严格程度的调节因子从原来固定不变的常数，转变为结合飞机实际飞行气压高度的动态变化值，以此来满足飞机不同飞行阶段对滤波标准的不同要求，效果良好.
在本文的仿真条件下，相比于改进前的算法，改进后的Sage-Husa自适应滤波算法在E方向定位误差降低43.48%，N方向定位误差降低31.33%，U方向定位误差降低36.48%，且使误差波动更加平缓，增强自适应性的同时，提高了系统定位精度.
-
表 1 导航系下定位误差比较
m 算法 统计量 E方向误差 N方向误差 U方向误差 卡尔曼
滤波算法最大值 32.63 26.32 78.37 RMS 11.25 12.58 13.32 改进前
Sage-Husa算法最大值 6.83 9.35 20.42 RMS 4.39 4.72 4.58 改进后
Sage-Husa算法最大值 3.86 6.42 12.97 RMS 3.05 3.98 3.62 
下载: 导出CSV
-
[1] 卢鋆, 申建华. 世界卫星导航格局发展趋势研究[J]. 卫星应用, 2020(8): 31-37. DOI: 10.3969/j.issn.1674-9030.2020.08.010 [2] 雷宏杰, 张亚崇. 机载惯性导航技术综述[J]. 航空精密制造技术, 2016, 52(1): 7-12. DOI: 10.3969/j.issn.1003-5451.2016.01.002 [3] 周先林, 张慧君, 和涛, 等. GPS/INS松耦合组合导航的自适应卡尔曼滤波算法研究[J]. 时间频率学报, 2020, 43(3): 222-230. [4] 李松, 唐小妹, 孙鹏跃, 等. GNSS/INS紧组合最大熵卡尔曼滤波算法[J]. 全球定位系统, 2020, 45(4): 1-8. [5] 程保喜. GNSS与惯性导航组合系统在复杂环境下的定位研究[J]. 中北大学学报(自然科学版), 2021, 42(1): 89-96. [6] SUN J, XU X S, LIU Y T, et al. FOG random drift signal denoising based on the improved AR model and modified Sage-Husa adaptive Kalman filter[J]. Sensors, 2016, 16(7): 1073. DOI: 10.3390/s16071073
[7] 索艳春. 基于Sage-Husa自适应滤波器的MEMS陀螺随机误差建模补偿[J]. 电子器件, 2018, 41(6): 1457-1460. DOI: 10.3969/j.issn.1005-9490.2018.06.021 [8] 梁俊元, 游林儒, 文小琴, 等. 改进的Sage-Husa算法在提高激光位移传感器精度中的应用[J]. 科学技术与工程, 2017, 17(31): 115-119. DOI: 10.3969/j.issn.1671-1815.2017.31.018 [9] 邱望彦, 李荣冰, 刘建业. 基于改进自适应渐消卡尔曼滤波的通用航空GNSS/微惯性组合导航算法研究[J]. 电子测量技术, 2020, 43(10): 95-100. [10] 魏延辉, 刘静, 郝晟功. 基于改进自适应滤波的SINS/DVL组合导航算法研究[J]. 自动化与仪表, 2019, 34(5): 95-100. [11] 鲁平, 赵龙, 陈哲. 改进的Sage-Husa自适应滤波及其应用[J]. 系统仿真学报, 2007, 19(15): 3503-3505. DOI: 10.3969/j.issn.1004-731X.2007.15.034 [12] 赵奉鲁. 中国民航安全趋势分析及国内外对比研究[D]. 天津: 中国民航大学, 2018. [13] 秦敏恒. 关于修正海平面气压计算的方法讨论[J]. 内蒙古科技与经济, 2019, 420(2): 80-82. DOI: 10.3969/j.issn.1007-6921.2019.02.037 [14] 于之靖, 张冬晓. 缩小垂直间隔适航性研究及相关问题分析[J]. 航空工程进展, 2015, 6(1): 116-121. DOI: 10.3969/j.issn.1674-8190.2015.01.019 -
期刊类型引用(7)
1. 王玮，潘新龙，林雪原，张日军. GNSS/SINS组合导航系统的改进变分贝叶斯自适应滤波算法. 大地测量与地球动力学. 2024(06): 560-565 . 
百度学术
2. 文胜，刘彩云，栾添添，孙明晓. 改进Sage-Husa算法在SINS/GPS组合导航中的应用. 哈尔滨理工大学学报. 2024(03): 37-44 . 百度学术
3. 荆蕾，林雪原，潘新龙，乔玉新. 基于改进Sage-Husa的GNSS/SINS组合导航系统自适应UKF算法. 中国空间科学技术(中英文). 2024(05): 127-135 . 百度学术
4. 朱元，吴博宇，陆科，吴名芝. 城市复杂环境下的车辆组合定位算法研究. 重庆理工大学学报(自然科学). 2023(01): 9-18 . 百度学术
5. 周豪，韩志刚，胡锦仁. 基于改进扩展卡尔曼滤波的爬架运行安全姿态估计. 科学技术与工程. 2023(25): 10817-10824 . 百度学术
6. 靳凯迪，柴洪洲，宿楚涵，惠俊，白腾飞. 基于可变遗忘因子的渐消记忆变分贝叶斯自适应滤波算法. 北京航空航天大学学报. 2023(11): 2989-2999 . 百度学术
7. 陈永刚，韩思成，贾水兰，许继业. 基于衰减因子的虚拟应答器信息融合方法研究. 全球定位系统. 2022(05): 88-93 . 本站查看
其他类型引用(13)
计量
- 文章访问数: 557
- HTML全文浏览量: 306
- PDF下载量: 52
- 被引次数: 20
