[发明专利]基于直接导航模型的无迹卡尔曼非线性初始对准方法

权利要求书

1.基于直接导航模型的无迹卡尔曼非线性初始对准方法，包括下列步骤：

步骤1、根据惯性测量单元中陀螺仪测得的角速度信息和加速度计测得的线加速度信息，使用基于凝固解析粗对准算法完成捷联惯性导航系统粗对准，求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，以导航系速度为量测量，建立基于导航参数微分方程的非线性滤波连续系统模型；

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型；

步骤4、根据步骤3中建立的非线性离散系统模型构建用于初始对准的简化无迹卡尔曼非线性滤波器，滤波算法中，时间更新与量测更新不同步；

步骤5、将步骤1提取的粗对准姿态角以及陀螺仪和加速度计的输出代入步骤4中，完成捷联惯性导航系统初始对准，直接输出导航参数。

2.根据权利要求1所述的基于直接导航模型的无迹卡尔曼非线性初始对准方法的步骤，其中所述的步骤2根据捷联惯性导航系统的速度微分方程、欧拉角微分方程、传感器模型，以导航系速度为量测量，建立基于导航参数微分方程（即直接导航模型）的非线性滤波连续系统模型，具体为：

捷联惯性导航系统的速度微分方程为

v.enn=fn-(2ωien+ωenn)×venn+gn---(1)]]>

式中，v_e、v_n和v_u分别为东、北、天速度，[.]^T表示矩阵转置；

ω_ie为地球自转角速率，L为当地地理纬度；

R_n为子午圈曲率半径，R_e为卯酉圈曲率半径；R_n=R_q(1-2e+3esin²L)，R_e=R_q(esin²L+1)，R_q为参考椭球赤道平面半径，e为地球扁率；

gⁿ=[0,0,-g]^T，g为重力加速度；

fn=Cbnfibb,]]>且fibb=[f~ibxb-δfibxb,f~ibyb-δfibyb,f~ibzb-δfibzb]T,]]>和分别为三只加速度计的测量值，δfibxb=▿x+wax,δfibyb=▿y+way,δfibzb=▿z+waz,]]>和分别为三只加速度计的测量误差，分为两部分，一部分为随机常值▽_x、▽_y和▽_z，一部分为随机噪声w_ax、w_ay和w_az；

Cnb=cosγcosψ-sinθsinγsinψcosγsinψ+sinθsinγcosψ-cosθsinγ-cosθsinψcosθcosψsinθsinγcosψ+sinθcosγsinψsinγsinψ-sinθcosγcosψcosθcosγ.]]>

捷联惯性导航系统的欧拉角微分方程

θ.γ.ψ.=1cosθcosθcosγ0sinγcosθsinθsinγcosθ-cos-sinγ0cosγγsinθωnbxbωnbybωnbzb---(2)]]>

式中，ωnbb=ωibb-Cnb(ωien+ωenn),]]>ωibb=[ω~ibxb-δωibxb,ω~ibyb-δωibyb,ω~ibzb-δωibzb]T,]]>和为三只陀螺仪测量值，δωibxb=ϵx+wgx,δωibyb=ϵy+wgy,δωibzb=ϵz+wgz,]]>和为三只陀螺仪的测量误差，分为两部分，一部分为随机常值ε_x、ε_y和ε_z，一部分为随机噪声w_gx、w_gy和w_gz；

捷联惯性导航系统的传感器模型

▿.x=0,▿.y=0,▿.z=0,ϵ.x=0,ϵ.y=0,ϵ.z=0]]>

(3)]]>

取12维系统状态向量为

x＝[v_e,v_n,v_u,P,R,H,▽_x,▽_y,▽_z,ε_x,ε_y,ε_z]^T

式中，x为状态向量；

系统噪声向量为

w＝[w_ax,w_ay,w_az,w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

取3维量测向量为

z＝[v_e,v_n,v_u]^T

式中，z为量测向量；

量测噪声向量为

η＝[w_ve,w_vn,w_vu]^T

式中，η为量测噪声向量，w_ve、w_vn、w_vu分别为东、北、天速度的量测噪声，假设为零均值高斯白噪声；

根据捷联惯性导航系统的速度微分方程、姿态欧拉角微分方程和传感器模型以及量测量建立的基于导航参数微分方程（即直接导航模型）的非线性滤波连续系统模型为：

x.=F(x)+G(x)wz=Hx+η]]>

式中，H=[I_3×30_3×9]；F(.)为非线性的状态函数，F(x)和G(x)参照式（1）-（3）写出。