CN102997921A - 一种基于反向导航的Kalman滤波算法 - Google Patents

Abstract

本发明涉及一种反向组合导航算法，公开了一种基于反向导航的Kalman滤波算法。目的在于提供一种反向惯性/GPS组合导航算法，对POS存储的惯性、GPS量测数据反序，进行由后向前的反向导航解算，并利用Kalman滤波技术实现惯性/GPS组合导航。通过建立反向惯性导航的姿态矩阵和速度、位置的更新方程，给出了速度、位置和姿态角的误差方程，并在此基础上选择了反向Kalman滤波器的状态量和观测量，进而给出反向Kalman滤波器的滤波模型。对惯性/卫星数据进行时间序列上由后向前的反向处理后，即可根据本发明提供的方法进行反向惯性/GPS组合导航，此方法的优点在于：提供了一种新的后处理方式，为提高POS的精度开辟了一条新路径，同时拓展Kalman滤波系数应用范围。

Description

一种基于反向导航的Kalman滤波算法

技术领域

本发明涉及一种反向组合导航算法，特别是反向惯性/GPS(GlobalPositioning System)组合导航算法。

背景技术

通常的惯性/GPS组合导航算法是对测量数据构成的时间序列进行由前向后的实时正向处理，而POS(Position and Orientation System)存储了整个任务过程中的量测数据。针对POS的事后处理过程无实时性要求且所有量测数据都已知的特点，可通过反向导航对POS存储的量测数据进行由后向前的事后处理，从而为提高POS的精度开辟一条新路径，同时拓展Kalman滤波系数应用范围。

发明内容

本发明的目的在于提供一种反向惯性/GPS组合导航算法，对POS存储的惯性、GPS量测数据反序，在反序过程中同时对陀螺仪的测量值取反号，进行由后向前的反向导航解算，并利用Kalman滤波技术实现惯性/GPS组合导航。

本发明是这样实现的：一种基于反向导航的Kalman滤波算法，是一种对导航信息数据进行后处理Kalman滤波算法，其中，在滤波过程中，使用如下所述的方程式：

反向惯导***的姿态矩阵和速度、位置的更新方程为：

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

式中：

k表示计算的当前时刻，真实的当前时刻；

k-1表示计算的前一时刻，真实的前一时刻；

T为反向惯导***的解算周期，单位：秒，确定后为常量；

为载体系b系向导航系n系，即北天东地理坐标系，转化的姿态转移矩阵，3×3维；

其中ω_ie为地球自转角速率，单位：弧度/秒，为反向惯导***的纬度，单位：弧度；

其中v_N、v_E分别为反向惯导***的北向速度、东向速度，单位：米/秒，R_M、R_N分别为地球子午圈、卯酉圈半径，单位：米，h为反向惯导***的高度，单位：米；

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ω}}_{\mathrm{ibx}}^b& -{\mathrm{\ω}}_{\mathrm{iby}}^b& -{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T,$

分别为载体系x轴陀螺测量值、y轴陀螺测量值、z轴陀螺测量值，单位：弧度/秒；

表示

的反对称矩阵；

$v_{\mathrm{enk}-1}^n=v_{\mathrm{enk}}^n-T[C_{\mathrm{bk}}^n{f}_{k-1}^b-(2{\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×v_{\mathrm{enk}}^n+g_k^n]---\left(1b\right)$

式中：

$v_{\mathrm{en}}^n={\left[\begin{array}{ccc}{v}_N& v_U& v_E\end{array}\right]}^T,$ 其中v_U为反向惯导***的垂向速度，单位：米/秒；

在第一次进行反向导航解算的时候，使速度初值反向，即此速度初值为真实导航过程中速度最后一个值；

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$

分别为载体系x轴加速度计测量值、y轴加速度计测量值、z轴加速度计测量值，单位：米/秒²；

gⁿ＝[0-g 0]^T，其中g为地球的重力加速度，单位：米/秒²；

式中：

λ为反向惯导***的经度，单位：弧度；

反向惯导***的速度、位置和姿态角误差方程为：

式中：

δλ分别为反向惯导***的纬度、经度误差，单位：弧度；

δh为反向惯导***的高度误差，单位：米；

fⁿ＝[f_N f_U f_E]^T，其中f_N、f_U、f_E分别为北向加速度、垂向加速度、东向加速度，单位：米/秒²；

$\mathrm{\δ}{f}^n=C_b^n{\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T,$ 其中分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误差、z轴加速度计零偏误差，单位：米/秒²；

φ＝[φ_N φ_U φ_E]^T，其中φ_N、φ_U、φ_E分别为反向惯导***的北向失调角、垂向失调角、东向失调角，单位：弧度；

$\mathrm{\δ}{v}_{\mathrm{en}}^n={\left[\begin{array}{ccc}\mathrm{\δ}{v}_N& \mathrm{\δ}{v}_U& {\mathrm{\δv}}_E\end{array}\right]}^T,$ 其中δv_N、δv_U、δv_E分别表示反向惯导***的北向速度误差、垂向速度误差和东向速度误差，单位：米/秒；

δgⁿ＝[0 -δg 0]^T；

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ϵ}}_x^b& -{\mathrm{\ϵ}}_y^b& -{\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ 其中

分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒。

本发明的优点是通过建立反向惯性导航的姿态矩阵和速度、位置的更新方程及速度、位置和姿态角的误差方程，给出反向Kalman滤波器的滤波模型，进而实现惯性/GPS信息的反向组合导航。此方法提供了一种新的后处理方式，为提高POS的精度开辟了一条新路径。

同时本发明给出了反向Kalman滤波的实现过程，拓展了Kalman滤波的应用范围。

具体实施方式

下面结合具体实施例对本发明做进一步的说明：

1)反向惯性导航及误差方程

反向惯导***的姿态矩阵和速度、位置的更新方程为：

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

(与正向导航的区别在于：从时刻k到k-1计算)

式中：

k表示计算的当前时刻，真实的当前时刻；

k-1表示计算的前一时刻，真实的当前时刻；

T为反向惯导***的解算周期，单位：秒，确定后为常量；

为载体系b系向导航系n系(北天东地理坐标系)转化的姿态转移矩阵，3×3维；

其中ω_ie为地球自转角速率，单位：弧度/秒，

为反向惯导***的纬度，单位：弧度；(与正向导航的区别在于：正负号)

其中v_N、v_E分别为反向惯导***的北向速度、东向速度，单位：米/秒，R_M、R_N分别为地球子午圈、卯酉圈半径，单位：米，h为反向惯导***的高度，单位：米；

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ω}}_{\mathrm{ibx}}^b& -{\mathrm{\ω}}_{\mathrm{iby}}^b& -{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T,$

分别为载体系x轴陀螺测量值、y轴陀螺测量值、z轴陀螺测量值，单位：弧度/秒；(与正向导航的区别在于：正负号)；

表示

的反对称矩阵。

式中：

$v_{\mathrm{en}}^n={\left[\begin{array}{ccc}{v}_N& v_U& v_E\end{array}\right]}^T,$ 其中v_U为反向惯导***的垂向速度，单位：米/秒；

在第一次进行反向导航解算的时候，使速度初值反向，即

此速度初值为真实导航过程中速度最后一个值；

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ 分别为载体系x轴加速度计测量值、y轴加速度计测量值、z轴加速度计测量值，单位：米/秒²；

gⁿ＝[0-g 0]^T，其中g为地球的重力加速度，单位：米/秒²。

式中：

λ为反向惯导***的经度，单位：弧度。

反向惯导***的速度、位置和姿态角误差方程为：

式中：

δλ分别为反向惯导***的纬度、经度误差，单位：弧度；

δh为反向惯导***的高度误差，单位：米；

fⁿ＝[f_N f_U f_E]^T，其中f_N、f_U、f_E分别为北向加速度、垂向加速度、东向加速度，单位：米/秒²；

$\mathrm{\δ}{f}^n=C_b^n{\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T,$ 其中分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误差、z轴加速度计零偏误差，单位：米/秒²；

φ＝[φ_N φ_U φ_E]^T，其中φ_N、φ_U、φ_E分别为反向惯导***的北向失调角、垂向失调角、东向失调角，单位：弧度；

$\mathrm{\δ}{v}_{\mathrm{en}}^n={\left[\begin{array}{ccc}\mathrm{\δ}{v}_N& \mathrm{\δ}{v}_U& {\mathrm{\δv}}_E\end{array}\right]}^T,$ 其中δv_N、δv_U、δv_E分别表示反向惯导***的北向速度误差、垂向速度误差和东向速度误差，单位：米/秒；

δgⁿ＝[0-δg 0]^T；

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ϵ}}_x^b& -{\mathrm{\ϵ}}_y^b& -{\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ 其中

分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒；

2)反向Kalman滤波

本实施例中，根据POS的***构成，同时依据推导的误差方程，选取位置误差、速度误差、姿态角误差、陀螺漂移误差和加表零偏误差等共15个误差变量作为反向组合导航***的状态量，

选取反向惯性导航的位置、速度与GPS提供的位置、速度作差，作为POS的观测量，

其中上标G表示GPS提供的信息；

λ^G分别为GPS提供的纬度、经度，单位：弧度；h^G为GPS提供的高度，单位：米；

分别为GPS提供的北向速度、垂向速度、东向速度，单位：米/秒。

根据给出的反向惯导***的误差方程，确定其状态方程为

式中：

A^B(t)为15×15维***参数矩阵，根据式(2)计算；

W^B(t)为15×1维***激励噪声向量。

量测方程为：

Z^B＝H^BX^B(t)+V^B(t)                                     (4)

式中：

H^B为6×15维量测矩阵；

V^B(t)为量测噪声向量。

将状态方程和量测方程离散化为：

$X_{k-1}^B={\mathrm{\Φ}}_{k-1,k}^B{X}_k^B+W_k^B---\left(5\right)$

$Z_{k-1}^B=H^B{X}_{k-1}^B+V_{k-1}^B---\left(6\right)$

${\mathrm{\Φ}}_{k-1,k}^B=I+T_d\underset{i=N-1}{\overset{0}{\mathrm{\Σ}}}{A}_i^B---\left(7\right)$

其中为***的状态转移阵；T_d为***的滤波周期，单位：秒；N为反向惯导***的解算频率。

反向Kalman滤波器的滤波模型为：

${\hat{X}}_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{\hat{X}}_k^B---\left(8a\right)$

$P_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{P}_k^B{\left({\mathrm{\Φ}}_{k-1,k}^B\right)}^T+Q_k^B---\left(8b\right)$

$K_{k-1}^B=P_{k-1,k}^B{\left(H^B\right)}^T{[H^B{P}_{k-1,k}^B{\left(H^B\right)}^T+R_{k-1}^B]}^{-1}---\left(8c\right)$

${\hat{X}}_{k-1}^B={\hat{X}}_{k-1,k}^B+K_{k-1}^B(Z_{k-1}^B-H^B{\hat{X}}_{k-1,k}^B)---\left(8d\right)$

$P_{k-1}^B=(I-K_{k-1}^B{H}^B)P_{k-1,k}^B{(I-K_{k-1}^B{H}^B)}^T+K_{k-1}^B{R}_{k-1}^B{\left(K_{k-1}^B\right)}^T---\left(8e\right)$

式中：

为反向滤波器的预测均方误差阵；

为反向滤波器的估计均方误差阵；

为反向滤波器的***噪声方差阵；

为反向滤波器的增益阵；

为反向滤波器的量测噪声方差阵。

选取状态初始值

估计均方误差阵初始值

***噪声方差阵初始值

及量测噪声方差阵初始值

后，采用公式(8)进行反向Kalman滤波。

3)输出校正

采用反向Kalman滤波的误差估计量

对反向惯性导航结果进行输出校正。

位置的输出校正方程为

${\mathrm{\λ}}_k^B={\mathrm{\λ}}_k-{\mathrm{\δ}\widehat{\mathrm{\λ}}}_{k/N}^B---\left(9b\right)$

$h_k^B=h_k-\mathrm{\δ}{\hat{h}}_{k/N}^B---\left(9c\right)$

式中：

分别为反向组合导航的纬度、经度，单位：弧度；

为反向组合导航的高度，单位：米；

分别为纬度、经度误差的反向Kalman滤波估计值，单位：弧度；

为高度误差的反向Kalman滤波估计值，单位：米；

姿态矩阵的输出校正方程为

$C_{\mathrm{bk}}^{\hat{n}}=C_{\mathrm{nk}}^{\hat{n}}{C}_{\mathrm{bk}}^n---\left(10\right)$

$C_{\mathrm{nk}}^{\hat{n}}=\left[\begin{array}{ccc}1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B\\ {\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& 1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B\\ -{\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B& 1\end{array}\right]$ 为姿态误差矩阵，

分别为反向Kalman滤波估计的北向失调角、垂向失调角、东向失调角，单位：弧度；

为输出校正后的姿态矩阵，用于计算反向组合导航的姿态角。

用滤波的误差估计量对反向惯性导航结果进行校正，解算出反向组合导航结果。

以某型号POS的机载惯性(陀螺零偏稳定性0.03°/h，加速度计零偏稳定性40μg)、GPS(定位精度米级)试验数据为例，进一步说明本发明的具体实施过程。

确定反向滤波的估计均方误差初始值

***噪声的初始方差阵

和量测噪声方差阵初始值

后，根据公式(8)进行反向Kalman滤波。当GPS周秒为20000.000秒时，估计的状态向量为

根据公式(8)、(9)，采用反向滤波的误差估计量对反向惯性导航结果进行输出校正，得反向组合导航的纬度为

经度为λ^B＝75.41156148度，高度为h＝11170.922米，滚动角为γ^B＝0.0209度，航向角为ψ^B＝19.2987度，俯仰角为θ^B＝-1.0891度。

Claims (1)

1.一种基于反向导航的Kalman滤波算法，是一种对导航信息数据进行后处理Kalman滤波算法，其特征在于：在滤波过程中，使用如下所述的方程式：

反向惯导***的姿态矩阵和速度、位置的更新方程为：

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

式中：

k表示计算的当前时刻，真实的当前时刻；

k-1表示计算的前一时刻，真实的当前时刻；

T为反向惯导***的解算周期，单位：秒，确定后为常量；

为载体系b系向导航系n系，即北天东地理坐标系，转化的姿态转移矩阵，3×3维；

其中ω_ie为地球自转角速率，单位：弧度/秒，

为反向惯导***的纬度，单位：弧度；

其中v_N、v_E分别为反向惯导***的北向速度、东向速度，单位：米/秒，R_M、R_N分别为地球子午圈、卯酉圈半径，单位：米，h为反向惯导***的高度，单位：米；

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ω}}_{\mathrm{ibx}}^b& -{\mathrm{\ω}}_{\mathrm{iby}}^b& -{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T,$

分别为载体系x轴陀螺测量值、y轴陀螺测量值、z轴陀螺测量值，单位：弧度/秒；

表示

的反对称矩阵；

$v_{\mathrm{enk}-1}^n=v_{\mathrm{enk}}^n-T[C_{\mathrm{bk}}^n{f}_{k-1}^b-(2{\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×v_{\mathrm{enk}}^n+g_k^n]---\left(1b\right)$

式中：

$v_{\mathrm{en}}^n={\left[\begin{array}{ccc}{v}_N& v_U& v_E\end{array}\right]}^T,$ 其中v_U为反向惯导***的垂向速度，单位：米/秒；

在第一次进行反向导航解算的时候，使速度初值反向，即此速度初值为真实导航过程中速度最后一个值；

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ 分别为载体系x轴加速度计测量值、y轴加速度计测量值、z轴加速度计测量值，单位：米/秒²；

gⁿ＝[0-g 0]^T，其中g为地球的重力加速度，单位：米/秒²；

式中：

λ为反向惯导***的经度，单位：弧度；

反向惯导***的速度、位置和姿态角误差方程为：

式中：

δλ分别为反向惯导***的纬度、经度误差，单位：弧度；

δh为反向惯导***的高度误差，单位：米；

fⁿ＝[f_N f_U f_E]^T，其中f_N、f_U、f_E分别为北向加速度、垂向加速度、东向加速度，单位：米/秒²；

$\mathrm{\δ}{f}^n=C_b^n{\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T,$ 其中分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误差、z轴加速度计零偏误差，单位：米/秒²；

φ＝[φ_N φ_U φ_E]^T，其中φ_N、φ_U、φ_E分别为反向惯导***的北向失调角、垂向失调角、东向失调角，单位：弧度；

$\mathrm{\δ}{v}_{\mathrm{en}}^n={\left[\begin{array}{ccc}\mathrm{\δ}{v}_N& \mathrm{\δ}{v}_U& {\mathrm{\δv}}_E\end{array}\right]}^T,$ 其中δv_N、δv_U、δv_E分别表示反向惯导***的北向速度误差、垂向速度误差和东向速度误差，单位：米/秒；

δgⁿ＝[0-δg 0]^T；

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ϵ}}_x^b& -{\mathrm{\ϵ}}_y^b& -{\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ 其中

分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒。