CN105865446A - 基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，该方法包括以下步骤：首先建立无人直升机运动过程中大气数据系统气压高度误差模型；在气压高度误差模型和传统惯性/大气阻尼回路的基础上，随后建立包含大气数据系统气压高度误差和惯性导航系统传感器误差的六状态阻尼卡尔曼滤波器的状态方程和量测方程；最后在无人直升机飞行过程中对高度误差和天向速度误差进行惯性/大气导航系统阻尼卡尔曼滤波，获得阻尼卡尔曼滤波后的组合导航系统导航结果。本发明方法能够在无人直升机飞行过程中实现对大气高度测量误差的跟踪和补偿，从而改善大气辅助惯性导航高度通道中高度的精度，适合于工程应用。

Description

基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法

技术领域

本发明涉及无人直升机基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，属于惯性/大气导航系统高度测量技术领域。

背景技术

无人直升机是一种有动力、操控灵活的无人驾驶飞行器，可实现垂直起降、空中悬停、原地转弯及任意的前飞、后飞和侧飞等特殊飞行方式，并且可以加载多种机载设备，执行多种任务，实现反复长航时飞行。无人直升机能够超低空飞行躲避敌方雷达的探测和跟踪，战场适应能力强，可以完成许多固定翼飞机无法完成的任务，弥补固定翼飞机在飞行范围和用途方面的不足，最大限度满足多方面的军事需求。

飞行高度是无人直升机的重要飞行参数，是无人直升机实现正常安全驾驶、准确导航与控制、有效侦察和攻击等任务不可或缺的信息。通过垂直方向上测得加速度的双时间积分，可以获得的垂直位置(高度)，但垂直加速度的双重积分是不稳定的，加速度偏差可以导致计算高度呈指数增长，因此，惯性导航系统依赖的加速度测量值积分取得高度估计是不稳定的系统。为了提供更稳定的惯性导航系统，气压高度计等外部引用被单独或组合使用，但气压高度计在低高度测量精度差，易受天气变化和阵风影响而造成高度测量不准。

目前基于经典控制理论设计的惯性/大气高度阻尼方法，是大气高度和惯性高度两种信息取长补短，进而得到高度误差不发散且动态性能较好的高度信息，但其本质上无法完全解决大气随机性误差的影响，目前关于大气高度的研究还基本上只是停留在二阶阻尼与三阶阻尼的研究中，鲜有深入研究阻尼系统理论和改进研究。因此研究基于大气高度辅助的惯性导航六状态阻尼卡尔曼滤波方法，估计出大气高度计中高度测量误差，从而提高大气测量输出的准确性，将具有突出的应用价值。

发明内容

本发明所要解决的技术问题是：提供基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，该方法能够在无人直升机飞行过程中实现对大气高度测量误差的跟踪和补偿，满足无人直升机动态飞行对导航系统的高精度要求。

本发明为解决上述技术问题采用以下技术方案：

基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，包括如下步骤：

步骤1，建立无人直升机运动过程中大气数据系统气压高度误差模型，所述大气数据系统的高度误差为气压高度表的标度因素误差、气压高度表偏置误差和压力偏置误差；

步骤2，在步骤1对无人直升机运动过程中大气数据系统气压高度误差建模的基础上，将惯性导航系统高度通道的三个误差量和大气数据系统的三个高度误差量作为系统的状态量，构建无人直升机六状态阻尼卡尔曼滤波器的状态方程和量测方程；

步骤3，对步骤2建立的状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对大气高度测量误差的跟踪和补偿。

作为本发明的一种优选方案，步骤1所述大气数据系统气压高度误差模型为：

δh_ads＝δh_b+h_Tδh_s+δh_p+υ_ads+ω_ads，

其中，δh_ads为气压高度误差、δh_b为气压高度表的标度因素误差、h_T为气压高度真值、δh_s为气压高度表偏置误差、δh_p为压力偏置误差、υ_ads为气压高度观测噪声、ω_ads为大气高度白噪声；δh_b、δh_s、δh_p的数学表达式为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{h}}_s=0\\ \mathrm{\δ}{\stackrel{\·}{h}}_b=-\frac{1}{\mathrm{\τ}}{\mathrm{\δh}}_b+{\mathrm{\ω}}_B\\ \mathrm{\δ}{\stackrel{\·}{h}}_p=f\left(h\right){\mathrm{\δh}}_p+{\mathrm{\ω}}_{ads}\end{array}\right.,$

其中，h为无人直升机的真实高度，m₁＝44.32km，m₂＝0.190263，m₃＝45.395km，，m₄＝14.605km， 为系统极点，ω_B为测量误差噪声。

作为本发明的一种优选方案，步骤2所述无人直升机六状态阻尼卡尔曼滤波器的状态方程为：

$\stackrel{\·}{X}=FX+GW,$

其中，X为系统状态变量、为状态变量X的一阶导数、F为系统矩阵、G为系统噪声系数矩阵、W为系统噪声矩阵，状态方程表示为：

$\left(\begin{array}{c}\mathrm{\δ}\stackrel{\·}{h}\\ \mathrm{\δ}{\stackrel{\·}{V}}_z\\ \mathrm{\δ}\stackrel{\·}{a}\\ \mathrm{\δ}{\stackrel{\·}{h}}_b\\ \mathrm{\δ}{\stackrel{\·}{h}}_s\\ \mathrm{\δ}{\stackrel{\·}{h}}_p\end{array}\right)=\left(\begin{array}{cccccc}-k_1& 1& 0& k_1& k_{1}h& k_1\\ 2{\mathrm{\ω}}_s^2-k_2& 0& -1& k_2& k_{2}h& k_2\\ k_3& 0& 0& -k_3& -k_{3}h& -k_3\\ 0& 0& 0& -\frac{1}{\mathrm{\τ}}& 0& 0\\ 0& 0& 0& 0& 0& 0\\ f\left(h\right)& 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}\mathrm{\δ}h\\ {\mathrm{\δV}}_z\\ \mathrm{\δ}a\\ {\mathrm{\δh}}_b\\ {\mathrm{\δh}}_s\\ {\mathrm{\δh}}_p\end{array}\right)+\left(\begin{array}{cccc}{k}_1& 0& 0& k_1\\ k_2& 1& 0& k_2\\ -k_3& 0& 0& -k_3\\ 0& 0& 1& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{\mathrm{\υ}}_{ads}\\ {\mathrm{\δA}}_z\\ {\mathrm{\ω}}_B\\ {\mathrm{\ω}}_{ads}\end{array}\right),$

其中，δh、δV_Z、δa分别为惯性导航系统高度通道中的垂直位置误差、垂直速度误差、垂直加速度误差状态量；δh_b、δh_s、δh_p分别为大气数据系统中的气压高度表的标度因素误差、气压高度表偏置误差、压力偏置误差状态量；k₁、k₂、k₃分别为大气高度三阶阻尼回路比例参数，h为无人直升机的真实高度，m₁＝44.32km，m₂＝0.190263，m₃＝45.395km，m₄＝14.605km， 为系统极点，为舒勒频率，υ_ads为气压高度观测噪声，δA_z为垂直加速度误差，ω_B为测量误差噪声，ω_ads为大气高度白噪声。

作为本发明的一种优选方案，步骤2所述无人直升机六状态阻尼卡尔曼滤波器的量测方程为：

Z＝HX+V，

其中，Z为观测量矩阵、H为量测矩阵、X为系统状态变量、V为量测噪声矩阵；量测方程表示为：

Z(t)＝[h_I-h_A]＝H(t)X(t)+V(t)＝(1 0 -1 -h -1)X(t)+V(t)，

其中，h_I＝h_t+δh，h_A＝h_t-δh_p，H(t)＝[1]_1×1，V(t)为系统测量白噪声矩阵，h为无人直升机的真实高度，δh为惯性导航系统高度通道中的垂直位置误差，δh_p为压力偏置误差，t为时间。

作为本发明的一种优选方案，所述步骤3的具体过程为：

31)将滤波器状态方程和量测方程离散化处理得到：

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+{\mathrm{\Γ}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},$

其中，X_k为t_k时刻系统状态变量，X_k-1为t_k-1时刻系统状态变量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵，W_k-1为t_k-1时刻系统噪声矩阵，Z_k为t_k时刻的位置、速度及姿态观测量矩阵，H_k为t_k时刻量测矩阵，V_k为t_k时刻量测噪声矩阵；

32)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+B_{k,k-1}{U}_{k-1}+{\mathrm{\Γ}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},$

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵；

33)得到系统的线性化卡尔曼滤波器方程：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

P_k,k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T;$

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵；Z_k为t_k时刻的新息序列；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

34)根据33)得到的线性化卡尔曼滤波器方程估计系统高度误差值，并用大气数据系统的气压高度值减去系统高度误差值，得到大气数据系统的气压高度修正值。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，该方法通过将3个惯性导航系统误差状态量和3个大气数据系统误差状态量作为系统的状态量，建立六状态阻尼卡尔曼滤波器的状态方程和量测方程，将惯性高度与气压高度之差作为组合卡尔曼滤波器的观测量，实现对惯性高度通道测量误差的实时估计和修正，获得大气高度的误差补偿量。该方法可以很好地跟踪并补偿大气高度信息中的误差，提高大气高度数据可靠性，提高惯性/大气组合导航系统精度。

附图说明

图1是本发明基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法的架构图。

图2是垂直通道误差传统三阶阻尼模型原理图。

图3是一阶马尔科夫噪声滤波补偿效果曲线图。

图4是随机白噪声滤波补偿效果曲线图。

图5是阶跃响应滤波补偿效果曲线图。

图6是随机白噪声和一阶马尔科夫噪声滤波混合补偿效果曲线图。

图7是六状态阻尼卡尔曼滤波和传统三阶阻尼高度误差对比曲线图。

图8是六状态阻尼卡尔曼滤波和传统三阶阻尼天向速度误差对比曲线图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，本发明无人直升机基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法的原理是：六状态阻尼卡尔曼滤波研究方案中，包含3个惯性导航系统误差状态量和3个大气数据系统误差状态量，分别为惯性导航系统垂直加速度误差、垂直速度误差、垂直位置误差、气压高度表的标度因数误差、气压高度表偏置误差和压力偏置误差。其中气压高度表的标度因数误差的观测量是由惯性阻尼后速度估计产生的；气压高度表压力偏置误差状态是直接通过压力传感器的偏置误差和压力测量值本身中的噪声建模获取的。对6个误差状态量的实时估计，将惯性高度与气压高度之差作为组合卡尔曼滤波器的观测量，通过对大气数据系统中高度测量误差的实时估计，进而对大气测量误差进行反馈修正，最终通过大气辅助惯性高度通道提高在惯性/大气导航系统中的高度测量的准确性，克服传统阻尼系统无法满足无人直升机在多任务飞行阶段对高度信息测量的精确、可靠的高性能要求。

本发明的具体实施方式如下：

1、建立无人直升机运动过程中大气数据系统气压高度误差模型

气压高度误差模型比较复杂不易建模，通常选取气压高度误差中比较典型的误差种类，主要为气压高度表的标度因素误差、气压高度表偏置误差和压力偏置误差等。根据以上几种大气误差之间的关系，建立大气数据系统气压高度误差模型为：

δh_ads＝δh_b+h_Tδh_s+δh_p+υ_ads+ω_ads (1)

式(1)中δh_ads为气压高度误差、h_T为气压高度真值、δh_b为气压高度表的标度因素误差、δh_s为气压高度表偏置误差、δh_p为压力偏置误差；υ_ads为气压高度观测噪声、ω_ads为大气高度白噪声，几种误差的数学表达式为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{h}}_s=0\\ \mathrm{\δ}{\stackrel{\·}{h}}_b=-\frac{1}{\mathrm{\τ}}{\mathrm{\δh}}_b+{\mathrm{\ω}}_B\\ \mathrm{\δ}{\stackrel{\·}{h}}_p=f\left(h\right){\mathrm{\δh}}_p+{\mathrm{\ω}}_{ads}\end{array}\right.---\left(2\right)$

式(2)中，为系统极点，ω_B为测量误差噪声，·为一阶导数，

$f\left(h\right)=\left\{\begin{array}{c}-m_5{(1-\frac{h}{m_1})}^{\frac{m_2-1}{m_2}},h\≤11km\\ -\frac{m_4}{ln\left(10\right)}{10}^{\frac{h-m_3}{m_4}},h>11km\end{array}\right.---\left(3\right)$

式(3)中，m₁＝44.32km，m₂＝0.190263，m₃＝45.395km，，m₄＝14.605km，h为无人直升机的真实高度，结合上面数学关系，为了简化无人直升机运动过程中大气数据系统动态建模过程和直接说明问题，本发明设计几种典型误差模型作为大气数据系统气压高度误差，包括随机一阶马尔科夫误差模型、随机白噪声误差模型和阶跃响应误差模型，其中随机一阶马尔科夫误差模型：

$\left\{\begin{array}{c}{H}_P=H_T+{\mathrm{\Δh}}_m\\ \mathrm{\Δ}{\stackrel{\·}{h}}_m=-\frac{v_d}{D}{\mathrm{\Δh}}_m+{\mathrm{\ω}}_h\end{array}\right.---\left(4\right)$

式(4)中，H_P为气压高度，单位米，H_T为气压高度真值，Δh_m为一阶马尔科夫过程误差，v_d为飞行速度，D为马尔科夫相关系数，ω_h为大气测量误差噪声，一般为白噪声。

随机白噪声误差模型：

randn＝AF(x) (5)

式(5)中，A为均方差，F(x)为正态分布函数，randn为随机白噪声误差。

阶跃响应误差模型：

$\mathrm{\&epsiv;}\left(t\right)=\left\{\begin{array}{c}0,t<0\\ 1,t>0\end{array}\right.---\left(6\right)$

式(6)中，ε(t)为阶跃响应误差。

2、建立六状态阻尼卡尔曼滤波模型

(2.1)建立六状态阻尼卡尔曼滤波状态方程

六状态阻尼卡尔曼滤波修正大气高度误差，是在传统三阶高度阻尼误差回路模型(如图2所示)基础上考虑了阻尼回路中误差量之间的耦合关系，在高度阻尼的基础上，对大气测量误差进行实时估计和补偿。选取机体坐标系(b系)为机体的“右、前、上”(XYZ)方向，选取导航坐标系(n系)为东北天(ENU)地理坐标系。

如图2所示，分别引入比例系数k₁、k₂、k₃，反馈补偿到垂直加速度和速度综合计算中，通过比例系数k₁环节，使整个回路的输出误差保持衰减振荡，保证系统的阻尼性；通过比例系数k₃环节，加快系统输出误差的衰减速度，增加系统的阻尼性，提高系统的振荡频率。传统阻尼回路垂直通道中各个变量的误差关系为：

h_ads＝h_T+δh_ads

＝h_T+δh_b+h_Tδh_s+δh_p+υ_ads+ω_ads

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{h}={\mathrm{\&delta;V}}_z-k_1(\mathrm{\&delta;}h-{\mathrm{\&delta;h}}_{ads}),$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_z={\mathrm{\&delta;A}}_Z-k_2(\mathrm{\&delta;}h-{\mathrm{\&delta;h}}_{ads})-\mathrm{\&delta;}a+2{\mathrm{\&omega;}}_s^2\mathrm{\&delta;}h---\left(7\right)$

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{a}=k_3(\mathrm{\&delta;}h-{\mathrm{\&delta;h}}_{ads}),\mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_s=0,$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_b=-\frac{1}{\mathrm{\&tau;}}{\mathrm{\&delta;h}}_b+{\mathrm{\&omega;}}_B,\mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_p=f\left(h\right){\mathrm{\&delta;h}}_p+{\mathrm{\&omega;}}_{ads}$

式(7)中，h_ads、δh_ads、h_T、δh_b、δh_s、δh_p分别为气压高度、气压高度误差、气压高度真值、气压高度表的标度因数误差、气压高度表偏置误差和压力偏置误差，δh、δV_z、δA_Z、δa、分别为惯性导航系统的垂直位置误差、垂直速度误差、垂直加速度误差、加速度补偿量和舒勒频率，υ_ads、ω_ads、ω_B分别为气压高度观测噪声、大气数据系统高斯白噪声和惯性导航系统高斯白噪声误差，1/τ为系统极点，f(h)如式(3)。

在式(7)的基础上，建立无人直升机六状态阻尼卡尔曼滤波器的状态方程和量测方程：

$\left.\begin{array}{c}\stackrel{\&CenterDot;}{X}=FX+GW\\ Z=HX+V\end{array}\right\}---\left(8\right)$

式(8)中，X为系统状态变量；为状态变量X的一阶导数；F为系统矩阵；G为系统噪声系数矩阵；W为系统噪声矩阵；Z为观测量矩阵；H为量测矩阵；V为量测噪声矩阵。

阻尼卡尔曼滤波状态量X为：

X＝[δh δV_Z δa δh_b δh_s δh_p]^T (9)

式(9)中，δh、δV_z、δa分别为惯性导航系统高度通道中的垂直位置误差、垂直速度误差、垂直加速度误差状态量；δh_b、δh_s、δh_p分别为大气数据系统中的压力偏置误差、气压高度表的标度因素误差、气压高度表偏置误差状态量。

根据步骤1中所建立的大气数据系统气压高度误差模型和式(7)，与式(9)对应的系统矩阵F为：

$F=\left(\begin{array}{cccccc}-k_1& 1& 0& k_1& k_{1}h& k_1\\ 2{\mathrm{\&omega;}}_s^2-k_2& 0& -1& k_2& k_{2}h& k_2\\ k_3& 0& 0& -k_3& -k_{3}h& -k_3\\ 0& 0& 0& -\frac{1}{\mathrm{\&tau;}}& 0& 0\\ 0& 0& 0& 0& 0& 0\\ f\left(h\right)& 0& 0& 0& 0& 0\end{array}\right)---\left(10\right)$

式(10)中，k₁、k₂、k₃为大气高度三阶阻尼回路比例参数。

系统噪声矩阵为：

W＝[υ_ads δA_z ω_B ω_ads]^T (11)

式(11)中，ω_B为测量误差噪声，一般为白噪声。系统白噪声随机误差矢量v_d为前向飞行速度，D为相关系数，大气压力噪声ω_ads为高度的函数：

${\mathrm{\&omega;}}_{ads}=\left\{\begin{array}{c}{m}_1{m}_6-m_1({m_6}^{\frac{1}{m_2}}+\frac{h}{p_0}),h\&le;11km\\ m_{4}log\left(\frac{{m_6}^{\frac{1}{m_2}}}{{m_6}^{\frac{1}{m_2}}+\frac{h}{p_0}}\right),h>11km\end{array}\right.---\left(12\right)$

式(12)中，p₀＝1013.25。

系统噪声系数矩阵为：

$G=\left(\begin{array}{cccc}{k}_1& 0& 0& k_1\\ k_2& 1& 0& k_2\\ -k_3& 0& 0& -k_3\\ 0& 0& 1& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right)---\left(13\right)$

(2.2)建立六状态阻尼卡尔曼滤波量测方程

将惯性高度h_I与气压高度h_p之差作为组合卡尔曼滤波器的观测量，量测方程推导如下：惯性导航系统的高度通道信息可表示为h_I＝h_t+δh，大气高度计给出的高度通道信息可表示为h_A＝h_t-δh_p，量测方程即为：

Z(t)＝[h_I-h_A]＝[δh+δh_p]＝H(t)X(t)+V(t)＝(1 0 -1 -h -1)X(t)+V(t) (14)

式(14)中，H(t)＝[1]_1×1，V(t)为系统测量白噪声矩阵，该模型中定为10米。

3、无人直升机基于大气辅助的惯性高度通道阻尼卡尔曼滤波

(3.1)将滤波器状态方程和量测方程离散化处理：

$\left.\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\}---\left(15\right)$

式(15)中，X_k为t_k时刻系统状态变量；X_k-1为t_k-1时刻系统状态变量；Φ_k,k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵；Γ_k,k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵；W_k-1为t_k-1时刻系统噪声矩阵；Z_k为t_k时刻的位置、速度及姿态观测量矩阵；H_k为t_k时刻量测矩阵；V_k为t_k时刻量测噪声矩阵。

(3.2)在式(15)的基础上加入控制项U_k-1，采用闭环修正系统状态方程对惯导系统误差进行修正：

$\left.\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+B_{k,k-1}{U}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\}---\left(16\right)$

式(16)中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵，

${\mathrm{\&Phi;}}_{k,k-1}=\underset{n=0}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{{\&lsqb;F_{k}T\&rsqb;}^n}{n!}---\left(17\right)$

${\mathrm{\&Gamma;}}_{k,k-1}=\underset{n=1}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{{\&lsqb;F_{k}T\&rsqb;}^{n-1}}{n!}{G}_{k}T---\left(18\right)$

上述公式中，F_k为t_k时刻系统矩阵的值，G_k为t_k时刻系统噪声系数矩阵的值，T为离散时间，n为阶数。

(3.3)得到系统的线性化卡尔曼滤波器方程：

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}---\left(19\right)$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\&lsqb;Z_k-H_k{\hat{X}}_{k,k-1}\&rsqb;---\left(20\right)$

P_k,k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T (21)

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(22\right)$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(23\right)$

上述公式中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵；Z_k为t_k时刻的新息序列；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵。

(3.4)用大气数据系统的气压高度值减去步骤(3.3)估计出来的系统高度误差值，获得大气数据系统的气压高度修正值。

为了验证发明所提出的无人直升机基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法的正确性及有效性，采用本发明方法建立六状态阻尼卡尔曼滤波模型，用Matlab进行阻尼卡尔曼滤波，对产生的滤波修正量和加入的气压高度误差量进行仿真验证对比。对一阶马尔科夫误差、随机白噪声误差、阶跃响应误差、随机白噪声和一阶马尔科夫噪声混合误差的跟踪和补偿的仿真结果如图3-图6所示。

在无人直升机动态飞行过程中，加入一阶马尔科夫噪声的气压高度误差，进行惯性/大气组合导航系统仿真，采用传统三阶阻尼和六状态阻尼卡尔曼滤波得到的高度误差和天向速度误差对比如图7-图8所示。

图3、图4和图6中灰色实线表示仿真中加入的气压高度误差量，黑色实线表示采用本发明的经过卡尔曼滤波得到的气压高度误差量；图5中黑色实线表示仿真中加入的气压高度误差量，黑色虚线表示采用本发明的经过卡尔曼滤波得到的气压高度误差修正量。从图中可以看出，采用本发明阻尼卡尔曼滤波得到的气压高度误差修正量和大气数据系统中本身加入的一阶马尔科夫误差、随机白噪声误差、阶跃响应误差、随机白噪声和一阶马尔科夫噪声混合误差趋势一致，所以此卡尔曼滤波器模型合理，可以一定程度修正大气数据，为后续辅助惯性导航提供了依据。图7-图8中黑色实线表示加入一阶马尔科夫误差以后采用本发明六状态阻尼滤波得到的高度误差和天向速度误差，灰色实线表示采用传统三阶阻尼模型得到的高度误差和天向速度误差，从图中的对比曲线可以看出，本发明的六状态阻尼滤波方法的高度误差精度相比传统三阶阻尼方法提高约2倍，天向速度误差精度也有提高，因此设计结合经典阻尼特性的六状态卡尔曼滤波器，可以很好的跟踪并补偿大气高度信息中的误差，提高大气高度数据可靠性，提高惯性/大气组合导航系统精度。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，其特征在于，包括如下步骤：

步骤1，建立无人直升机运动过程中大气数据系统气压高度误差模型，所述大气数据系统的高度误差为气压高度表的标度因素误差、气压高度表偏置误差和压力偏置误差；

步骤2，在步骤1对无人直升机运动过程中大气数据系统气压高度误差建模的基础上，将惯性导航系统高度通道的三个误差量和大气数据系统的三个高度误差量作为系统的状态量，构建无人直升机六状态阻尼卡尔曼滤波器的状态方程和量测方程；

步骤3，对步骤2建立的状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对大气高度测量误差的跟踪和补偿。

2.根据权利要求1所述基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，其特征在于，步骤1所述大气数据系统气压高度误差模型为：

δh_ads＝δh_b+h_Tδh_s+δh_p+υ_ads+ω_ads，

其中，δh_ads为气压高度误差、δh_b为气压高度表的标度因素误差、h_T为气压高度真值、δh_s为气压高度表偏置误差、δh_p为压力偏置误差、υ_ads为气压高度观测噪声、ω_ads为大气高度白噪声；δh_b、δh_s、δh_p的数学表达式为：

$\left\{\begin{array}{c}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_s=0\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_b=-\frac{1}{\mathrm{\&tau;}}{\mathrm{\&delta;h}}_b+{\mathrm{\&omega;}}_B\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_p=f\left(h\right){\mathrm{\&delta;h}}_p+{\mathrm{\&omega;}}_{ads}\end{array}\right.,$

其中，h为无人直升机的真实高度，m₁＝44.32km，m₂＝0.190263，m₃＝45.395km，，m₄＝14.605km， 为系统极点，ω_B为测量误差噪声。

3.根据权利要求1所述基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，其特征在于，步骤2所述无人直升机六状态阻尼卡尔曼滤波器的状态方程为：

$\stackrel{\&CenterDot;}{X}=FX+GW,$

其中，X为系统状态变量、为状态变量X的一阶导数、F为系统矩阵、G为系统噪声系数矩阵、W为系统噪声矩阵，状态方程表示为：

$\left(\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{h}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_z\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{a}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_b\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_s\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{h}}_p\end{array}\right)=\left(\begin{array}{cccccc}-k_1& 1& 0& k_1& k_{1}h& k_1\\ 2{\mathrm{\&omega;}}_s^2-k_2& 0& -1& k_2& k_{2}h& k_2\\ k_3& 0& 0& -k_3& -k_{3}h& -k_3\\ 0& 0& 0& -\frac{1}{\mathrm{\&tau;}}& 0& 0\\ 0& 0& 0& 0& 0& 0\\ f\left(h\right)& 0& 0& 0& 0& 0\end{array}\right)\left(\begin{array}{c}\mathrm{\&delta;}h\\ {\mathrm{\&delta;V}}_z\\ \mathrm{\&delta;}a\\ {\mathrm{\&delta;h}}_b\\ {\mathrm{\&delta;h}}_s\\ {\mathrm{\&delta;h}}_p\end{array}\right)+\left(\begin{array}{cccc}{k}_1& 0& 0& k_1\\ k_2& 1& 0& k_2\\ -k_3& 0& 0& -k_3\\ 0& 0& 1& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}{\mathrm{\&upsi;}}_{ads}\\ {\mathrm{\&delta;A}}_z\\ {\mathrm{\&omega;}}_B\\ {\mathrm{\&omega;}}_{ads}\end{array}\right),$

其中，δh、δV_Z、δa分别为惯性导航系统高度通道中的垂直位置误差、垂直速度误差、垂直加速度误差状态量；δh_b、δh_s、δh_p分别为大气数据系统中的气压高度表的标度因素误差、气压高度表偏置误差、压力偏置误差状态量；k₁、k₂、k₃分别为大气高度三阶阻尼回路比例参数，h为无人直升机的真实高度，m₁＝44.32km，m₂＝0.190263，m₃＝45.395km，m₄＝14.605km， 为系统极点，为舒勒频率，υ_ads为气压高度观测噪声，δA_z为垂直加速度误差，ω_B为测量误差噪声，ω_ads为大气高度白噪声。

4.根据权利要求1所述基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，其特征在于，步骤2所述无人直升机六状态阻尼卡尔曼滤波器的量测方程为：

Z＝HX+V，

其中，Z为观测量矩阵、H为量测矩阵、X为系统状态变量、V为量测噪声矩阵；量测方程表示为：

Z(t)＝[h_I-h_A]＝H(t)X(t)+V(t)＝(1 0 -1 -h -1)X(t)+V(t)，

其中，h_I＝h_t+δh，h_A＝h_t-δh_p，H(t)＝[1]_1×1，V(t)为系统测量白噪声矩阵，h为无人直升机的真实高度，δh为惯性导航系统高度通道中的垂直位置误差，δh_p为压力偏置误差，t为时间。

5.根据权利要求1所述基于大气辅助的惯性高度通道阻尼卡尔曼滤波方法，其特征在于，所述步骤3的具体过程为：

31)将滤波器状态方程和量测方程离散化处理得到：

$\left.\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},$

其中，X_k为t_k时刻系统状态变量，X_k-1为t_k-1时刻系统状态变量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的一步状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统噪声系数矩阵，W_k-1为t_k-1时刻系统噪声矩阵，Z_k为t_k时刻的位置、速度及姿态观测量矩阵，H_k为t_k时刻量测矩阵，V_k为t_k时刻量测噪声矩阵；

32)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

$\left.\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+B_{k,k-1}{U}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right\},$

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵；

33)得到系统的线性化卡尔曼滤波器方程：

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\&lsqb;Z_k-H_k{\hat{X}}_{k,k-1}\&rsqb;$

P_k,k-1＝Φ_k,k-1P_k-1Φ_k,k-1 ^T+Γ_k-1Q_k-1Γ_k-1 ^T

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T;$

其中，为t_k-1时刻至t_k时刻一步预测状态量；为t_k-1时刻滤波状态估计量；为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵；Z_k为t_k时刻的新息序列；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1 ^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1 ^T为Γ_k-1的转置矩阵；为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

34)根据33)得到的线性化卡尔曼滤波器方程估计系统高度误差值，并用大气数据系统的气压高度值减去系统高度误差值，得到大气数据系统的气压高度修正值。