[发明专利]一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法

权利要求书

1.一种用于机载惯性/多普勒雷达组合导航系统的信息融合方法，其特征在于：该方法具体布骤如下：

步骤一：建立惯性器件、多普勒雷达的误差模型；

a.加速度计误差方程

加速度计零偏的误差模型用一阶马尔科夫过程模型方程表示：

▿·xb=-β▿x▿xb+W▿x▿·yb=-β▿y▿yb+W▿y▿·zb=-β▿z▿zb+W▿z---(2)]]>

δK·Ax=0δK·Ay=0δK·Az=0---(3)]]>

上述方程中，分别为加速度计x、y、z轴向在载体坐标系下的零偏；分别为加速度计x、y、z轴向理想值；分别为加速度计x、y、z轴向测量值；δK_Ax、δK_Ay、δK_Az分别为加速度计x、y、z轴向刻度因子误差；A_ij为加速度计j轴向偏向i轴向的失准角；分别为加速度计x、y、z轴向噪声；分别为加速度计x、y、z轴向过程的相关时间；

b.陀螺误差模型

陀螺零偏的误差模型用一阶马尔科夫过程模型方程表示：

ϵ·xb=-βϵxϵxb+Wϵxϵ·yb=-βϵyϵyb+Wϵyϵ·zb=-βϵzϵzb+Wϵz---(5)]]>

δK·gx=0δK·gy=0δK·gz=0---(6)]]>

上述方程中，分别为陀螺x、y、z轴向在载体坐标系下的零偏；分别为x、y、z轴向陀螺理想值；分别为x、y、z轴向陀螺测量值；δK_gx、δK_gy、δK_gz分别为陀螺x、y、z轴向刻度因子误差；M_ij为陀螺j轴向偏向i轴向的失准角；W_εx、W_εy、W_εz分别为陀螺x、y、z轴向噪声；1/β_εx、1/β_εy、1/β_εz分别为陀螺x、y、z轴向过程的相关时间；

c.多普勒雷达误差方程

多普勒雷达零偏的误差模型用一阶马尔科夫过程模型方程表示：

δv·dx=-βdxδvdx+Wdxδv·dy=-βdyδvdy+Wdyδv·dz=-βdzδvdz+Wdz---(8)]]>

δKdi·=0D·ij=0i=x,y,z;j=x,y,z---(9)]]>

上述方程中，δv_dx、δv_dy、δv_dz分别为多普勒雷达x、y、z轴向在载体坐标系下的零偏；分别为x、y、z轴向多普勒雷达理想值；分别为x、y、z轴向多普勒雷达测量值；δK_dx、δK_dy、δK_dz分别为多普勒雷达x、y、z轴向刻度因子误差；D_ij为多普勒雷达j轴向偏向i轴向的失准角；W_dx、W_dy、W_dz分别为多普勒雷达x、y、z轴向噪声；1/β_dx、1/β_dy、1/β_dz分别为多普勒雷达x、y、z轴向过程的相关时间；

步骤二：基于惯性/多普勒组合导航在机载固定翼飞机上的应用，确定状态变量；

机载固定翼飞机的特点是动力沿纵轴方向，横轴和垂轴方向无动力，因此，纵轴方向速度是载体的主要速度，风力外部环境会造成横轴和垂轴方向很小的速度，即

对多普勒雷达而言，由刻度因子误差δK_d和失准角D_ij引起的误差分别为和因此由飞行器的特点知道δK_dx、δK_dz、D_xy、D_yz、D_zx、D_xz引起的误差忽略，只需考虑δK_dy、D_xy、D_zy引起的误差；

对惯导系统而言，失准角引起的误差远小于刻度因子引起的误差，因此不考虑失准角引起的误差；由飞行器的特点知道，航向角和俯仰角相对变化较小，飞机横滚角相对变化较大，即较大，较小，只需考虑陀螺由刻度因子δK_gy以及加速度计由刻度因子δK_Ay、δK_Ay引起的误差；

因此惯导系统与多普勒雷达器件需选取δK_dy、D_xy、D_zy、δv_dx、δv_dy、δv_dz、δK_gy、ϵyb,ϵzb,▿xb,▿yb,▿zb,]]>δK_Az、δK_Ax作为状态量；

步骤三：建立惯性/多普勒雷达组合导航状态方程；

a.Kalman滤波的基本过程如下：

首先，给定动态系统的一阶线性状态方程和量测方程为：

X·(t)=F(t)X(t)+G(t)W(t)---(10)]]>

Z(t)＝H(t)X(t)+V(t)           (11)

进而将状态方程(10)和量测方程(11)离散化得：

X_k＝Ф_k，k-1X_k-1+Г_k-1W_k-1    (12)

Z_k＝H_kX_k+V_k                   (13)

状态预测估计方程为：

X^k/k-1=Φk,k-1X^k-1---(14)]]>

方差预测方程为：

Pk/k-1=Φk,k-1Pk-1Φk,k-1T+Γk-1Qk-1Γk-1T---(15)]]>

状态预测估计方程为：

X^k=X^k/k-1+Kk(Zk-HkX^k/k-1)---(16)]]>

方差迭代方程：

Pk=Pk/k-1-Pk/k-1HkT(HkPk/k-1HkT+Rk)-1HkPk/k-1]]>

=(I-KkHk)Pk/k-1---(17)]]>

滤波增益方程为：

Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1---(18)]]>

初始条件为：

X^0=E(X0)=μ0,varX~0=varX0=P0---(19)]]>

验前统计量为：

E[W_k]＝0，Cov[W_k，W_j]＝E[W_kW_j^T]＝Q_kδ_kj         (20)

E[V_k]＝0，Cov[V_k，V_j]＝E[V_kV_j^T]＝R_kδ_kj         (21)

Cov[W_k，V_j]＝E[W_kV_j^T]＝0                        (22)

δkj=0,k≠j1,k=j---(23)]]>

b.姿态误差方程

ψxn·=(ωien+ωenn)zψyn-(ωien+ωenn)yψzn-T12δKGywibxb-ϵxnψyn·=(ωien+ωenn)zψxn+(ωien+ωenn)xψzn-T22δKGywibyb-ϵynψzn·=(ωien+ωenn)yψxn-(ωien+ωenn)xψyn-T32δKGywibzb-ϵzn---(24)]]>

式中：

ϵxn=T11ϵxb+T12ϵyb+T13ϵzbϵyn=T21ϵxb+T22ϵyb+T23ϵzbϵzn=T31ϵxb+T32ϵyb+T33ϵzb---(25)]]>

其中为i(x、y、z)轴向平台坐标系相对计算地理坐标系之间的姿态误差角；为地球自转角速率；为位移角速度；T为姿态矩阵；

c.位置误差方程

δθ·xn=-VznR+hδθxn+Vyn(R+h)2δh-1R+hδVy1δθ·yn=-VznR+hδθyn-Vxn(R+h)2δh+1R+hδVx1δh·=-Vynδθxn+Vxnδθyn+δVz1---(26)]]>

其中为i(x、y、z)轴向位置误差；h为高度；为i(x、y、z)轴向速度；为i(x、y、z)轴向速度误差；R为地球半径；

d.速度误差方程

δV·x1=-gδθy-fznψyn+fynψzn+(wenn+2wien)zδVy1-(wenn+2wien)yδVz1+T11δKAxfxb+T13δKAzfzb+▿xnδV·y1=gδθx+fznψxn-fxnψzn-(wenn+2wien)zδVx1+(wenn+2wien)xδVz1+T21δKAxfyb+T23δKAzfzb+▿ynδV·z1=2gR2(R+h)3δh-fynψxn+fxnψyn+(wenn+2wien)yδVx1-(wenn+2wien)xδVy1+T31δKAxfxb+T33δKAzfzb+▿zn---(27)]]>

式中

▿xn=T11▿xb+T12▿yb+T13▿zb▿yn=T21▿xb+T22▿yb+T23▿zb▿zn=T31▿xb+T32▿yb+T33▿zb---(28)]]>

Cbn=T=T11T12T13T21T22T23T31T32T33---(29)]]>

ρ=wenn=-VynR+hVxnR+hρz---(30)]]>

e.状态方程

由步骤二确立的器件误差和步骤四公式(24)、(26)、(27)确立的导航误差作为状态量组成状态矢量，如下：

X=[δθxn,δθyn,δh,δVx1,δVy1,δVz1,ψxn,ψyn,ψzn,ϵxb,ϵyb,ϵzb,▿xb,▿yb,▿zb,]]>(31)

δKAx,δKAz,δKGy,Dxy,Dzy,δKdy,δvdx,δvdy,δvdz]T]]>

F1=-VznR+h0Vyn(R+h)20-VznR+h-Vxn(R+h)2-VynVxn0---(32)]]>

F2=0-g0g00002gR2(R+h)3---(33)]]>

F3=0-1R+h01R+h00001---(34)]]>

F4=0(ωenn+2ωien)z-(ωenn+2ωien)y-(ωenn+2ωien)z0(ωenn+2ωien)x(ωenn+2ωien)y(ωenn+2ωien)x0---(35)]]>

F5=0000000000000000000-fznfyn000fzn0-fxn000-fynfxn0000---(36)]]>

F6=0(ωenn+2ωien)z-(ωenn+2ωien)yT11T12T13-(ωenn+2ωien)z0(ωenn+2ωien)xT21T22T23(ωenn+2ωien)y-(ωenn+2ωien)x0T31T32T33000-βϵx000000-βϵy000000-βϵz---(37)]]>

F7=000000000000000000T11T12T13T11fxbT13fzb0T21T22T23T21fxbT23fzb0T31T32T33T31fxbT33fzb0---(38)]]>

F8=00000-T12wibxb00000-T22wibxb00000-T32wibxb000000000000000000---(39)]]>

F9=F1F3F2F4---(40)]]>

F11=000000000000000000000-βdx000000-βdy000000-βdz---(42)]]>

最后得到状态转移矩阵：

F=F9F5F706×606×6F6F806×606×606×6F1006×606×606×606×6F11---(43)]]>

连续Kalman滤波状态方程为

X·(t)=F(t)X(t)+W(t)---(44)]]>

步骤四：建立惯性/多普勒雷达组合导航量测方程；

δVDOPn=V~INUn-V~DOPn]]>

=VINUn+δV1-(δθ×)VINUn-C~bn(VDOPb+δVDOPb)]]>

=VINUn+δV1-(δθ×)VINUn-[I-(φ×)]Cbn(VDOPb+δVDOPb)]]>

≈VINUn-CbnVDOPb+δV1-(δθ×)VINUn-CbnδVDOPb+(φ×)CbnVDOPb---(45)]]>

≈δV1+[(φ-δθ)×]CbnVINUb-CbnδVDOPb]]>

=δV1-(CbnVDOPb×)ψ-CbnδVDOPb]]>

=Z]]>

Zxn=(T31VDOPxb+T32VDOPyb+T33VDOPzb)ψyn-(T21VDOPxb+T22VDOPyb+T23VDOPzb)ψzn+δVx1-T11VDOPybDxy-T13VDOPybDzy-T12VDOPybδKy-T11δvdx-T12δvdy-T13δvdzZyn=-(T31VDOPxb+T31VDOPyb+T33VDOPzb)ψxn+(T11VDOPxb+T12VDOPyb+T13VDOPzb)ψzn+δVy1-T21VDOPybDxy-T23VDOPybDzy-T22VDOPybδKy-T21δvdx-T22δvdy-T23δvdzZzn=(T21VDOPxb+T22VDOPyb+T23CDOPzb)ψxn-(T11VDOPxb+T12VDOPyb+T13VDOPzb)ψyn+δVz1-T31VDOPybDxy-T33VDOPybDzy-T32VDOPybδKy-T31δvdx-T32δvdy-T33δvdz---(46)]]>

H1=0(T31VDOPxb+T32VDOPyb+T33VDOPzb)-(T21VDOPxb+T22VDOPyb+T23VDOPzb)-(T31VDOPxb+T32VDOPyb+T33VDOPzb)0(T11VDOPxb+T12VDOPyb+T13VDOPzb)(T21VDOPxb+T22VDOPyb+T23VDOPzb)-(T11VDOPxb+T12VDOPyb+T13VDOPzb)0---(47)]]>

H2=100010001---(48)]]>

H3=-T11VDOPyb-T13VDOPyb-T12VDOPyb-T21VDOPyb-T23VDOPyb-T22VDOPyb-T31VDOPyb-T33VDOPyb-T32VDOPyb---(49)]]>

H4=-T11-T12-T13-T21-T22-T23-T31-T32-T33---(50)]]>

H＝[0_3×3 H₂ H₂ 0_3×9 H₃ H₄]          (51)

最终确立量测方程

Z＝HX+V                  (52)

步骤五：将卡尔曼滤波连续系统离散化；

将连续系统离散化

Φk+1,k=I+TF+(TF)22+(TF)36---(53)]]>

Qk=Σi=1∞MiΔTii---(54)]]>

其中M1=Q(t),Mi+1=FMi+MiTFT;]]>

步骤六：根据惯导与多普勒雷提供的导航信息，进行Kalman滤波信息融合解算，通过状态估计，得到关于导航参数的最优或次优估计，采用闭环反馈方式对惯导和多普勒雷达同时修正；

a.用卡尔曼滤波估计的惯导系统和多普勒雷达误差对测量数据补偿

1.对加速度计数据补偿

2.对陀螺数据补偿

对多普勒雷达数据补偿

b.用卡尔曼滤波估计的速度误差对速度及时修正

Vⁿ＝[I+(δθ×)](Vⁿ-δV¹)                      (58)

c.用卡尔曼滤波估计的姿态误差对姿态矩阵及时修正补偿

T＝{I+[(ψ+δθ)×]}T                      (59)

d.用卡尔曼滤波估计的位置误差对位置矩阵及时修正补偿

Cen=[I+(δθ×)]Cen.---(60)]]>