CN105021198A - 一种基于多传感器综合导航的位置估计方法 - Google Patents

Abstract

本发明公开了一种基于多传感器综合导航的位置估计方法，包含以下步骤：1、采集DR、GPS、DME、VOR、ADC输出的参数；2、建立状态方程；3、求取GPS的位置、速度量测方程、DME的斜距量测方程、VOR的方位角量测方程；4、根据各量测方程建立综合量测方程；5、结合状态方程和综合量测方程，估计位置误差量、速度误差量；6、根据估计的位置误差量、速度误差量对DR***进行输出校正，完成位置估计。本发明无需使用惯性导航***，具有定位精度高、可靠性高、实时性好、经济有效且易于工程实现的特点，可以满足民用飞机高精度、高可靠性的定位需求。

Description

一种基于多传感器综合导航的位置估计方法

技术领域

本发明涉及一种多传感器综合导航方法，特别是一种基于DR/GPS/DME/VOR综合导航的位置估计方法，是一种非常适用于民用飞机高精度、高可靠性定位需求的方法。

背景技术

对于民用飞机来说，安全性永远是第一位的。为了确保民用飞行的飞行安全，大型民用飞机一般配备多种导航***。如空客320配备有惯性导航***、全球定位***(GPS)、测距仪(DME)、甚高频全向信标(VOR)、航位推算(DR)等多个导航设备用于定位。如何有效综合利用多个导航设备信息进行位置计算，从而确保民用飞机的定位精度和可靠性，是民用飞机设计过程中亟待解决的关键技术。

惯性导航***虽然能够自主提供全面的导航信息，但其误差会随着时间不断积累，所以不能长期独立的进行精确导航。GPS在能够全天候、实时提供机载的位置和速度信息，并且误差不随时间积累，但GPS存在依赖外部信息、易受无线电干扰、数据输出率低等不足；VOR、DME等无线电定位***的定位精度不受使用时间影响，但工作范围受地面台覆盖区域的限制，且易受地场环境、电磁干扰等空间噪声影响。DR***是利用机载已有的航向、速度、姿态等传感器数据进行位置推算的自主导航定位***，DR***具有自主性好、抗干扰性强、连续输出定位数据、短时间精度较高、成本低等优点，但***误差随时间积累、不能单独地长时间使用；上述各种单一的导航源各有优缺点，综合发挥各种导航***的特点，互相取长补短，将导航***的信息综合起来，将提高导航定位的精度和可靠性，更好地满足民用飞机对导航***的要求。

目前，传统的综合导航定位方式通常以惯性导航***作为主导航***，使用GPS、DME及VOR等其他导航***辅助进行组合，利用卡尔曼滤波实现飞机位置的估计，但是这种方式不仅需要配备价格昂贵的惯性导航***，并且模型和组合算法复杂。在工程实现中，采用传统卡尔曼滤波方法进行组合导航时，由于受模型误差和计算过程中的舍入误差影响，容易发生状态协方差矩阵失去对称性和正定性，从而导致滤波器失效。为确保导航的可靠性，通常采用联邦滤波的方法实现多传感器的导航综合，但是这种方法计算量大，实时性差。

发明内容

本发明为了解决现有的民用飞机综合导航定位方法复杂、滤波器容易失效、计算量大、实时性差，并且需要配置昂贵的惯性导航***等问题，提出了一种基于多传感器综合导航的位置估计方法，利用该方法能够为民用飞机提供高精度、可靠性高的位置数据。

本发明的发明目的通过以下技术方案实现：

一种基于多传感器综合导航的位置估计方法，包含以下步骤：

步骤1、采集机载各导航设备输出的参数，所述参数包括：(1)DR***输出位置、速度信息；(2)GPS***输出的位置、速度信息；(3)DME***输出的斜距信息；(4)VOR***输出的方位角信息；(5)ADC***输出的高度信息；

步骤2、根据DR***误差模型和ADC***输出的高度信息建立状态方程；

步骤3、利用DR***的位置、速度信息与GPS的位置、速度之差求得GPS的位置、速度量测方程；利用DR***的位置信息与DME***的斜距信息求得DME的斜距量测方程；利用DR***的位置信息与VOR***的方位角信息求得VOR的方位角量测方程；

步骤4、根据GPS的位置、速度量测方程、DME的斜距量测方程和VOR的方位角量测方程建立综合量测方程；

步骤5、结合状态方程和综合量测方程，估计位置误差量、速度误差量；

步骤6、根据估计的位置误差量、速度误差量对DR***进行输出校正，完成位置估计。

该方法无需使用惯性导航***，具有定位精度高、可靠性高、实时性好、经济有效且易于工程实现的特点。

依据上述特征，所述状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

其中：状态变量为：

X＝[δL δλ δV_N δV_E]^T

***阵为：

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

***噪声矢量为：

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

δL、δλ分别为纬度误差、经度误差；δV_N、δV_E分别为北向速度误差、东向速度误差；V_E为DR***的东向速度；L为DR***的纬度；h为ADC***的高度；R_N＝R_e(1+fsin²L)，R_M＝R_e(1-2f+3fsin²L)，f为地球扁率，R_e为地球半径；w_L、w_λ分别为DR***纬度白噪声、经度白噪声；分别为DR***北向速度白噪声、东向速度白噪声；T为矩阵的转置；t为时间。

依据上述特征，所述步骤3中GPS的位置、速度量测方程为：

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

其中为L、λ、V_N、V_E分别为DR***的纬度、经度、北向速度、东向速度，L_GPS、λ_GPS、分别为GPS***的纬度、经度、北向速度、东向速度，H_GPS＝[H¹ _GPS H² _GPS H³ _GPS H⁴ _GPS]^T＝diag[1 1 1 1]，V_GPS为零均值的高斯白噪声，且互不相关；

所述DME的斜距量测方程为：

Z_DME(t)＝D_DR-D_DME＝H_DME(t)X(t)+V_DME(t)

式中 $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H_DME＝[a₁ a₂ 0 0]，其中

a₁＝(R_N+h)[-e₁sin L cosλ-e₂sin L sinλ]+[R_N(1-f)²+h]e₃cos L

a₂＝(R_N+h)[e₂cos L cosλ-e₁cos L sinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ x_DR、y_DR、z_DR为DR位置在地球固定坐标系下x、y、z方向的位置量，x_DME、y_DME、z_DME为DME导航台位置在地球固定坐标系下x、y、z方向的位置量，D_DME为DME***的斜距信息，V_DME为零均值的高斯白噪声，地球固定坐标系原点在地心，z轴沿地球自转轴方向，x轴在赤道平面内，与零度子午线相交，y轴与x轴、z轴构成右手直角坐标系；

所述VOR的方位角量测方程为：

Z_VOR(t)＝θ_DR-θ_VOR＝H_VOR(t)X(t)+V_VOR(t)

式中H_VOR＝[b₁ b₂ 0 0]，其中

b₁＝c₁[(R_N+h)sin L sinλcosλ-(R_N+h)sin L sinλcosλ]

+c₂[(R_N+h)sin²L cos²λ+(R_N+h)sin²L sin²λ+(R_N(1-f)²+h)cos²L]

b₂＝c₁[(R_N+h)cos L sin²λ+(R_N+h)cos L cos²λ]

+c₂[(R_N+h)sin L cos L sinλcosλ-(R_N+h)sin L cos L sinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(xⁿ _DR-xⁿ _VOR)²+(yⁿ _DR-yⁿ _VOR)²

xⁿ _DR、yⁿ _DR为DR位置在地理坐标系下x、y方向的位置量，xⁿ _VOR、yⁿ _VOR为VOR导航台位置在地理坐标系下x、y方向的位置量，θ_VOR为VOR***的方位角，V_VOR为零均值的高斯白噪声，地理坐标系以当前位置为原点，x轴指向正东，y轴指向正北，z轴指向天向与x轴、y轴构成右手直角坐标系。

优选地，为了避免不合理的量测信息引入到滤波***中，提高了***的稳定性和可靠性，所述步骤4还包含在建立综合量测方程前先对位置、速度量测信息、斜距量测信息、方位角量测信息进行量测信息合理性检查，如果存在合理的量测信息，将通过量测信息合理性检查的量测信息纳入综合量测方程，并继续执行步骤5；如果各量测信息均不合理时，则不建立综合量测方程，通过采用预测估计的方法进行位置误差量和速度误差量的估计，并执行步骤6。

依据上述特征，所述量测信息合理性检查的方法为：

根据k-1时刻的状态估计值由状态方程得到k时刻***状态的递推值由此得到k时刻第i个量测量的预测值进而得到第i个实际量测值与量测量的预测值之间的差值即

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

第i个量测量的理论预测误差的方差为如果则第i个量测量满足合理性要求，否则量测信息不合理；

其中γ为判断量测信息是否合理的门限值，i＝1、2、3、4、5、6表示依次对GPS纬度、经度、北向速度、东向速度信息，DME斜距信息，VOR方位角信息进行合理性检查。

依据上述特征，所述步骤4中综合量测方程为：

当GPS的位置、速度量测信息均满足合理性要求时，位置和速度量测信息Z_GPS纳入***量测量Z中；当DME斜距量测信息满足合理性要求时，斜距量测信息Z_DME纳入***量测量Z中；当VOR方位角量测信息满足合理性要求时，方位角量测信息Z_VOR纳入***量测量Z中；构建综合量测方程：

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

***量测量为m维向量，m根据GPS位置和速度量测信息、DME斜距量测信息、VOR方位角量测信息的合理性而变化：

当位置、速度量测信息、斜距量测信息、方位角量测信息均合理时m＝6；

当位置、速度量测信息合理，斜距量测信息、方位角量测信息只有一个合理时m＝5；

当位置、速度量测信息合理，斜距量测信息、方位角量测信息均不合理时m＝4；

当斜距量测信息、方位角量测信息均合理，位置、速度量测信息不合理时m＝2；

当斜距量测信息、方位角量测信息只有一个合理，位置、速度量测信息不合理时m＝1；

m维量测相互独立，量测噪声方差阵为对角阵

优选地，为了有效解决了递推过程中量测信息均不合理情况下滤波的连续性问题，采用所述预测估计方法进行位置误差量、速度误差量估计的步骤为：根据前一时刻的状态估计值和状态方程对位置误差量、速度误差量进行估计：

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

$P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

优选地，为了有效解决了递推过程中滤波器发散的问题，提高了滤波精度，并且易于工程实现，在存在合理的量测信息时采用基于序贯处理的平方根滤波方法进行位置误差量和速度误差量的估计，所述采用基于序贯处理的平方根滤波方法中估计位置误差量、速度误差量的步骤为：

(5.1)将状态方程和综合量测方程离散化，得到：

X_k＝Φ_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为状态向量，Φ_k,k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，W_k-1为***噪声向量，V_k为量测噪声向量，W_k-1和V_k是不相关的零均值高斯白噪声，***噪声和量测噪声的方差阵分别为Q_k和R_k；

(5.2)采用基于序贯处理的平方根滤波方法估计位置误差量、速度误差量：

记式中Δ_k和Δ_k/k-1都是下三角矩阵；

设 $P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α、β为位置估计误差、速度估计误差的方差初值；

时间更新方程为：

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

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

用乔莱斯基分解方法对P_k/k-1作平方根分解得到Δ_k/k-1；

量测更新：

设根据k-1时刻的序贯处理结果已获得和Δ_k/k-1，则k时刻的量测更新序贯处理按下述步骤执行：

取 ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

对于j＝1,2,...,m迭代计算下述方程：

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j={\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

当j＝m时，即获得k时刻的量测更新结果：

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

其中为状态估计值，Δ_k为估计均方误差P_k的平方根，

本发明的优点及显著效果：

1、本发明方法提出了不使用价格昂贵的惯性导航***，充分使用现有的DR、GPS、DME、VOR及ADC***的输出参数，采用综合导航的方法实现飞机位置的估计，具有定位精度高、经济有效的特点；

2、本发明方法提出了对量测信息合理性检查的方法，实现对不合理信息的检查和隔离，避免不合理的量测信息引入到滤波***中，提高了***的稳定性和可靠性；

3、本发明方法提出了对量测信息合理和不合理两种情况下的两种不同的滤波方法，有效的解决了量测信息不合理情况下的解算问题，保证了位置估计数据的连续性；

4、本发明方法采用平方根滤波的方法有效解决了递推过程中滤波器发散的问题，提高了滤波精度，并且易于工程实现；

5、本发明方法采用序贯处理的方法对多个量测信息进行更新，大幅降低了滤波求解的计算量，提高了定位的实时性。

附图说明

图1是本发明方法的结构原理图；

图2是本发明方法的程序流程图。

具体实现方式

实施例一：

参看图1，本发明在不使用惯性导航***的前提下，充分利用现有的DR、GPS、DME、VOR及ADC***的输出参数，实现位置信息的估计，包含以下步骤：

步骤1、采集机载各导航设备输出的参数，所述参数包括：(1)DR***输出位置、速度信息；(2)GPS***输出的位置、速度信息；(3)DME***输出的斜距信息；(4)VOR***输出的方位角信息；(5)ADC***输出的高度信息；

步骤2、根据DR***误差模型及ADC***输出的高度信息建立综合导航***的状态方程；

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

状态变量为：

X＝[δL δλ δV_N δV_E]^T

***阵为：

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

***噪声矢量为：

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

其中，δL、δλ分别为纬度误差、经度误差；δV_N、δV_E分别为北向速度误差、东向速度误差；V_E为DR***的东向速度；L为DR***的纬度；h为ADC***的高度；R_N＝R_e(1+fsin²L)，R_M＝R_e(1-2f+3fsin²L)，f＝1/298.257，R_e＝6378137米；w_L、w_λ分别为DR***纬度白噪声、经度白噪声；分别为DR***北向速度白噪声、东向速度白噪声；T为矩阵的转置；t为时间；

步骤3、利用DR***的位置、速度信息与GPS的位置、速度之差求得GPS的位置、速度量测方程；利用DR***的位置信息与DME***的斜距信息求得DME的斜距量测方程；利用DR***的位置信息与VOR***的方位角信息求得VOR的方位角量测方程；

GPS的位置、速度量测方程具体为：

将DR***的位置与GPS测量的位置之差 $\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\end{array}\right]$ 作为位置量测量，将DR***的速度与GPS测量的速度之差 $\left[\begin{array}{c}{V}_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]$ 作为速度量测量，将位置量测量与速度量测量合并作为GPS总的量测量Z_GPS，建立GPS的量测方程：

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

其中为L、λ、V_N、V_E分别为DR***的纬度、经度、北向速度、东向速度，L_GPS、λ_GPS、分别为GPS***的纬度、经度、北向速度、东向速度，H_GPS＝[H¹ _GPS H² _GPS H³ _GPS H⁴ _GPS]^T＝diag[1 1 1 1]，V_GPS为零均值的高斯白噪声，且互不相关。

DME的斜距量测方程具体为：

根据DR的位置信息计算的斜距D_DR与DME输出的斜距D_DME之差D_DR-D_DME作为斜距量测值Z_DME，建立DME的量测方程：

Z_DME(t)＝D_DR-D_DME＝H_DME(t)X(t)+V_DME(t)

式中 $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H_DME＝[a₁ a₂ 0 0]，其中

a₁＝(R_N+h)[-e₁sin L cosλ-e₂sin L sinλ]+[R_N(1-f)²+h]e₃cos L

a₂＝(R_N+h)[e₂cos L cosλ-e₁cos L sinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ x_DR、y_DR、z_DR为DR位置在地球固定坐标系下x、y、z方向的位置量，x_DME、y_DME、z_DME为DME导航台位置在地球固定坐标系下x、y、z方向的位置量，V_DME为零均值的高斯白噪声，地球固定坐标系原点在地心，z轴沿地球自转轴方向，x轴在赤道平面内，与零度子午线相交，y轴与x轴、z轴构成右手直角坐标系。

VOR的方位角量测方程具体为：

根据DR的位置信息计算的方位角θ_DR与VOR输出的方位角θ_VOR之差作为方位角量测值Z_VOR，建立VOR的量测方程：

Z_VOR(t)＝θ_DR-θ_VOR＝H_VOR(t)X(t)+V_VOR(t)

式中H_VOR＝[b₁ b₂ 0 0]，其中

b₁＝c₁[(R_N+h)sin L sinλcosλ-(R_N+h)sin L sinλcosλ]

+c₂[(R_N+h)sin²L cos²λ+(R_N+h)sin²L sin²λ+(R_N(1-f)²+h)cos²L]

b₂＝c₁[(R_N+h)cos L sin²λ+(R_N+h)cos L cos²λ]

+c₂[(R_N+h)sin L cos L sinλcosλ-(R_N+h)sin L cos L sinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(xⁿ _DR-xⁿ _VOR)²+(yⁿ _DR-yⁿ _VOR)²

xⁿ _DR、yⁿ _DR为DR位置在地理坐标系下x、y方向的位置量，xⁿ _VOR、yⁿ _VOR为VOR导航台位置在地理坐标系下x、y方向的位置量，V_VOR为零均值的高斯白噪声，地理坐标系以当前位置为原点，x轴指向正东，y轴指向正北，z轴指向天向与x轴、y轴构成右手直角坐标系。

步骤4、建立综合导航***的量测方程；

$Z\left(t\right)=\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]$

步骤5、结合综合导航***的状态方程和量测方程，实现位置误差量、速度误差量的估计。

在本步骤中，为了有效解决递推过程中滤波器发散的问题，提高滤波精度，并且大幅降低滤波求解的计算量易于工程实现，本步骤优选采用基于序贯处理的平方根滤波方法来估计位置误差量、速度误差量，对于本领域的技术人员来说也可以采用其它方法来估计位置误差量、速度误差量。

基于序贯处理的平方根滤波方法具体步骤为：

(a)状态方程和量测方程离散化

将状态方程和量测方程离散化，得到：

X_k＝Φ_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为状态向量，Φ_k,k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，W_k-1为***噪声向量，V_k为量测噪声向量，W_k-1和V_k是不相关的零均值高斯白噪声，***噪声和量测噪声的方差阵分别为Q_k和R_k。

(b)基于序贯处理的平方根滤波方法

记 $P_k={\mathrm{\Δ}}_k{\mathrm{\Δ}}_k^T,P_{k/k-1}={\mathrm{\Δ}}_{k/k-1}{\mathrm{\Δ}}_{k/k-1}^t,$ 式中Δ_k和Δ_k/k-1都是下三角矩阵。设 ${\hat{X}}_0=0_{4\×1},$ $P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α、β为位置估计误差、速度估计误差的方差初值。

时间更新方程为：

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

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

用乔莱斯基分解方法对P_k/k-1作平方根分解得到Δ_k/k-1。

量测更新：

设根据k-1时刻的序贯处理结果已获得和Δ_k/k-1，则k时刻的量测更新序贯处理可按下述步骤执行：

取 ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

对于j＝1,2,...,m迭代计算下述方程

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j={\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

当j＝m时，即获得k时刻的量测更新结果

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

其中为状态估计值，Δ_k为估计均方误差P_k的平方根，

步骤6、根据估计的位置误差量、速度误差量对DR***进行输出校正，由此完成基于多传感器综合导航的位置估计。

因此，通过步骤1到步骤6可以实现飞机位置的实时精确估计。

实施例二

如图2所示，本发明还提出了一种对实施例一进行改进的方案，在实施例二中对量测信息合理性检查，实现对不合理信息的检查和隔离，避免不合理的量测信息引入到滤波***中，提高了***的稳定性和可靠性；以及对量测信息合理和不合理两种情况下的采用不同的滤波方法，有效的解决了量测信息不合理情况下的解算问题，保证了位置估计数据的连续性，实施例二的实施步骤如下：

步骤1-3同实施例一。

步骤4、采用量测信息合理性检查方法依次对位置、速度量测信息(简称：GPS量测信息)、斜距量测信息(简称：DME量测信息)、方位角量测信息(简称VOR量测信息)进行合理性检查，如果存在合理的量测信息，将通过量测信息合理性检查的量测信息建立综合量测方程，如果不存在合理的量测信息，则综合量测方程不存在。

所述的量测信息合理性检查方法为：

根据k-1时刻的状态估计值由***状态方程可得到k时刻***状态的递推值由此可得k时刻第i个量测量的预测值进而得到第i个实际量测值与量测量的预测值之间的差值即

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

第i个量测量的理论预测误差的方差为如果则第i个量测量满足合理性要求，否则量测信息不合理。其中γ为判断量测信息是否合理的门限值，i＝1、2、3、4、5、6表示依次对GPS纬度、经度、北向速度、东向速度信息，DME斜距信息，VOR方位角信息进行合理性检查。

当GPS的位置、速度量测信息均满足合理性要求时，位置和速度量测信息Z_GPS纳入***量测量Z中；当DME斜距量测信息满足合理性要求时，斜距量测信息Z_DME纳入***量测量Z中；当VOR方位角量测信息满足合理性要求时，方位角量测信息Z_VOR纳入***量测量Z中；构建综合量测方程：

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

***量测量为m维向量，m根据GPS、DME、VOR量测信息的合理性而变化：

当GPS、DME、VOR量测信息均合理时m＝6；

当GPS量测信息合理、DME和VOR量测信息只有一个合理时m＝5；

当GPS量测信息合理、DME、VOR量测信息均不合理时m＝4；

当DME、VOR量测信息均合理，GPS量测信息不合理时m＝2；

当DME和VOR量测信息只有一个合理，GPS量测信息不合理时m＝1；

m维量测相互独立，量测噪声方差阵为对角阵 $R_k=diag\[\begin{array}{cccc}{R}_k^1& R_k^1& \mathrm{...}& R_k^m\end{array}\].$

步骤5、根据量测信息合理性采用不同的方法进行位置误差量和速度误差量的估计，具体为：

如果存在合理的量测信息时采用基于序贯处理的平方根滤波方法进行位置误差量和速度误差量的估计，所述基于序贯处理的平方根滤波方法同实施例一；

如果不存在合理的量测信息时采用预测估计的方法进行位置误差量和速度误差量的估计，根据前一时刻的状态估计值和***状态方程实现位置误差量、速度误差量的估计：

$\begin{array}{c}{\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}\end{array}.$

步骤6、根据步骤5中是否存在合理的量测信息两种情况得到的位置误差量、速度误差量对DR***进行输出校正，由此完成基于多传感器综合导航的位置估计。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims (9)

1.一种基于多传感器综合导航的位置估计方法，包含以下步骤：

步骤1、采集机载各导航设备输出的参数，所述参数包括：(1)DR***输出位置、速度信息；(2)GPS***输出的位置、速度信息；(3)DME***输出的斜距信息；(4)VOR***输出的方位角信息；(5)ADC***输出的高度信息；

步骤2、根据DR***误差模型和ADC***输出的高度信息建立状态方程；

步骤3、利用DR***的位置、速度信息与GPS的位置、速度之差求得GPS的位置、速度量测方程；利用DR***的位置信息与DME***的斜距信息求得DME的斜距量测方程；利用DR***的位置信息与VOR***的方位角信息求得VOR的方位角量测方程；

步骤4、根据GPS的位置、速度量测方程、DME的斜距量测方程和VOR的方位角量测方程建立综合量测方程；

步骤5、结合状态方程和综合量测方程，估计位置误差量、速度误差量；

步骤6、根据估计的位置误差量、速度误差量对DR***进行输出校正，完成位置估计。

2.根据权利要求1所述的位置估计方法，其特征在于，所述步骤2中状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

其中：状态变量为：

X＝[δL δλ δV_N δV_E]^T

***阵为：

$F=\left[\begin{array}{cccc}0& 0& \frac{1}{R_M+h}& 0\\ \frac{V_E\tan L}{(R_N+h)\cos L}& 0& 0& \frac{1}{(R_N+h)\cos L}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$

***噪声矢量为：

$W={\left[\begin{array}{cccc}{w}_L& w_{\mathrm{\λ}}& w_{V_N}& w_{V_E}\end{array}\right]}^T$

δL、δλ分别为纬度误差、经度误差；δV_N、δV_E分别为北向速度误差、东向速度误差；V_E为DR***的东向速度；L为DR***的纬度；h为ADC***的高度；R_N＝R_e(1+fsin²L)，R_M＝R_e(1-2f+3fsin²L)，f为地球扁率，R_e为地球半径；w_L、w_λ分别为DR***纬度白噪声、经度白噪声；分别为DR***北向速度白噪声、东向速度白噪声；T为矩阵的转置；t为时间。

3.根据权利要求1所述的位置估计方法，其特征在于，所述步骤3中GPS的位置、速度量测方程为：

$Z_{GPS}\left(t\right)=\left[\begin{array}{c}L-L_{GPS}\\ \mathrm{\λ}-{\mathrm{\λ}}_{GPS}\\ V_N-V_{N_{GPS}}\\ V_E-V_{E_{GPS}}\end{array}\right]=H_{GPS}\left(t\right)X\left(t\right)+V_{GPS}\left(t\right)$

其中为L、λ、V_N、V_E分别为DR***的纬度、经度、北向速度、东向速度，L_GPS、λ_GPS、V_NGPS、V_EGPS分别为GPS***的纬度、经度、北向速度、东向速度，H_GPS＝[H¹ _GPS H² _GPS H³ _GPS H⁴ _GPS]^T＝diag[1 1 1 1]，V_GPS为零均值的高斯白噪声，且互不相关；

所述步骤3中DME的斜距量测方程为：

Z_DME(t)＝D_DR-D_DME＝H_DME(t)X(t)+V_DME(t)

式中 $D_{DR}=\sqrt{{(x_{DR}-x_{DME})}^2+{(y_{DR}-y_{DME})}^2+{(z_{DR}-z_{DME})}^2},$ H_DME＝[a₁ a₂ 0 0]，其中

a₁＝(R_N+h)[-e₁sinLcosλ-e₂sinLsinλ]+[R_N(1-f)²+h]e₃cosL

a₂＝(R_N+h)[e₂cosLcosλ-e₁cosLsinλ]

$e_1=\frac{x_{DR}-x_{DME}}{D_{DR}},e_2=\frac{y_{DR}-y_{DME}}{D_{DR}},e_3=\frac{z_{DR}-z_{DME}}{D_{DR}},$ x_DR、y_DR、z_DR为DR位置在地球固定坐标系下x、y、z方向的位置量，x_DME、y_DME、z_DME为DME导航台位置在地球固定坐标系下x、y、z方向的位置量，D_DME为DME***的斜距信息，V_DME为零均值的高斯白噪声，地球固定坐标系原点在地心，z轴沿地球自转轴方向，x轴在赤道平面内，与零度子午线相交，y轴与x轴、z轴构成右手直角坐标系；

所述步骤3中VOR的方位角量测方程为：

Z_VOR(t)＝θ_DR-θ_VOR＝H_VOR(t)X(t)+V_VOR(t)

式中H_VOR＝[b₁ b₂ 0 0]，其中

b₁＝c₁[(R_N+h)sinLsinλcosλ-(R_N+h)sinLsinλcosλ]

+c₂[(R_N+h)sin²Lcos²λ+(R_N+h)sin²Lsin²λ+(R_N(1-f)²+h)cos²L]

b₂＝c₁[(R_N+h)cosLsin²λ+(R_N+h)cosLcos²λ]

+c₂[(R_N+h)sinLcosLsinλcosλ-(R_N+h)sinLcosLsinλcosλ]

$c_1=\frac{{y^n}_{DR}-{y^n}_{VOR}}{s},c_2=-\frac{{x^n}_{DR}-{x^n}_{VOR}}{s},$ s＝(xⁿ _DR-xⁿ _VOR)²+(yⁿ _DR-yⁿ _VOR)²

xⁿ _DR、yⁿ _DR为DR位置在地理坐标系下x、y方向的位置量，xⁿ _VOR、yⁿ _VOR为VOR导航台位置在地理坐标系下x、y方向的位置量，θ_VOR为VOR***的方位角，V_VOR为零均值的高斯白噪声，地理坐标系以当前位置为原点，x轴指向正东，y轴指向正北，z轴指向天向与x轴、y轴构成右手直角坐标系。

4.根据权利要求1所述的位置估计方法，其特征在于，所述步骤4包含在建立综合量测方程前先对位置、速度量测信息、斜距量测信息、方位角量测信息进行量测信息合理性检查，如果存在合理的量测信息，将通过量测信息合理性检查的各量测信息纳入综合量测方程，并继续执行步骤5；如果各量测信息均不合理时，则不建立综合量测方程，通过采用预测估计的方法进行位置误差量和速度误差量的估计，并执行步骤6。

5.根据权利要求4所述的位置估计方法，其特征在于所述量测信息合理性检查的方法为：

根据k-1时刻的状态估计值由状态方程得到k时刻***状态的递推值由此得到k时刻第i个量测量的预测值进而得到第i个实际量测值与量测量的预测值之间的差值即

${\hat{Z}}_k^i=Z_k^i-{\hat{Z}}_{k/k-1}^i=Z_k^i-H_k^i{\hat{X}}_{k/k-1}$

第i个量测量的理论预测误差的方差为如果则第i个量测量满足合理性要求，否则量测信息不合理；

其中γ为判断量测信息是否合理的门限值，i＝1、2、3、4、5、6表示依次对GPS纬度、经度、北向速度、东向速度信息，DME斜距信息，VOR方位角信息进行合理性检查。

6.根据权利要求4所述的位置估计方法，其特征在于，所述步骤4中综合量测方程为：

当GPS的位置、速度量测信息均满足合理性要求时，位置和速度量测信息Z_GPS纳入***量测量Z中；当DME斜距量测信息满足合理性要求时，斜距量测信息Z_DME纳入***量测量Z中；当VOR方位角量测信息满足合理性要求时，方位角量测信息Z_VOR纳入***量测量Z中；构建综合量测方程：

$Z\left(t\right)={\left[\begin{array}{c}{Z}_{GPS}\left(t\right)\\ Z_{DME}\left(t\right)\\ Z_{VOR}\left(t\right)\end{array}\right]}_{m\×1}$

***量测量为m维向量，m根据GPS位置和速度量测信息、DME斜距量测信息、VOR方位角量测信息的合理性而变化：

当位置、速度量测信息、斜距量测信息、方位角量测信息均合理时m＝6；

当位置、速度量测信息合理，斜距量测信息、方位角量测信息只有一个合理时m＝5；

当位置、速度量测信息合理，斜距量测信息、方位角量测信息均不合理时m＝4；

当斜距量测信息、方位角量测信息均合理，位置、速度量测信息不合理时m＝2；

当斜距量测信息、方位角量测信息只有一个合理，位置、速度量测信息不合理时m＝1；

m维量测相互独立，量测噪声方差阵为对角阵 $R_k=\mathrm{diag}\left[\begin{array}{cccc}{R}_k^1& R_k^1& ...& R_k^m\end{array}\right].$

7.根据权利要求4所述的位置估计方法，其特征在于，所述预测估计方法进行位置误差量、速度误差量估计的步骤为：根据前一时刻的状态估计值和状态方程对位置误差量、速度误差量进行估计：

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

$P_k={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

8.根据权利要求1所述的位置估计方法，其特征在于，所述步骤5中采用基于序贯处理的平方根滤波方法进行位置误差量和速度误差量的估计。

9.根据权利要求8所述的位置估计方法，其特征在于，所述基于序贯处理的平方根滤波方法进行位置误差量、速度误差量估计的步骤为：

(5.1)将状态方程和综合量测方程离散化，得到：

X_k＝Φ_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为状态向量，Φ_k,k-1为状态转移矩阵，Z_k为量测向量，H_k为量测矩阵，W_k-1为***噪声向量，V_k为量测噪声向量，W_k-1和V_k是不相关的零均值高斯白噪声，***噪声和量测噪声的方差阵分别为Q_k和R_k；

(5.2)采用基于序贯处理的平方根滤波方法估计位置误差量、速度误差量：

记式中Δ_k和Δ_k/k-1都是下三角矩阵；

设 ${\hat{X}}_0=0_{4\×1},P_0=\left[\begin{array}{cc}{\mathrm{\αI}}_{2\×2}& 0_{2\×2}\\ 0_{2\×2}& {\mathrm{\βI}}_{2\×2}\end{array}\right],$ α、β为位置估计误差、速度估计误差的方差初值；

时间更新方程为：

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

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

用乔莱斯基分解方法对P_k/k-1作平方根分解得到Δ_k/k-1；

量测更新：

设根据k-1时刻的序贯处理结果已获得和Δ_k/k-1，则k时刻的量测更新序贯处理按下述步骤执行：

取 ${\hat{X}}_k^0={\hat{X}}_{k/k-1},{\mathrm{\Δ}}_k^0={\mathrm{\Δ}}_{k/k-1}$

对于j＝1,2,...,m迭代计算下述方程：

$a_k^j={\left(H_k^j{\mathrm{\Δ}}_k^{j-1}\right)}^T$

$b_k^j={(a_k^{jT}{a}_k^j+R_k^j)}^{-1}$

${\mathrm{\γ}}_k^j={(1+\sqrt{b_k^j{R}_k^j})}^{-1}$

$K_k^j=b_k^j{\mathrm{\Δ}}_k^{j-1}{a}_k^j$

${\hat{X}}_k^j-{\hat{X}}_k^{j-1}+K_k^j(Z_k^j-H_k^j{\hat{X}}_k^{j-1})$

${\mathrm{\Δ}}_k^j={\mathrm{\Δ}}_k^{j-1}-{\mathrm{\γ}}_k^j{K}_k^j{a}_k^{jT}$

当j＝m时，即获得k时刻的量测更新结果：

${\hat{X}}_k={\hat{X}}_k^m$

${\mathrm{\Δ}}_k={\mathrm{\Δ}}_k^m$

其中为状态估计值，Δ_k为估计均方误差P_k的平方根，