[发明专利]一种基于比力观测的捷联惯导真空滤波修正方法

权利要求书

1.一种基于比力观测的捷联惯导真空滤波修正方法，其特征在于：该方法具体步骤如下：

步骤1、通过外部设备确定载体的初始位置参数并将其装订至导航计算机；

步骤2、捷联惯导系统进行预热，然后采集陀螺仪和加速度计的输出数据；

步骤3、对采集到的陀螺仪和加速度计的输出数据进行处理，根据捷联惯导系统的误差传播特性和古典控制理论，采用二阶调平法和方位估算法来完成系统的粗对准，初步确定载体的姿态，粗对准时间不少于30秒；

步骤4、粗对准结束后进入精对准阶段，保持载体在水平位置上静止不动，作零速修正，对准时间不少于250秒；此时惯组初始俯仰角和滚转角分别为0度和-45度，航向角为45度，对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤5、载体从水平位置绕俯仰轴在水平面的投影旋转至竖直位置，转动时间、角度根据具体情况确定；

步骤6、保持载体在第二个竖直位置上静止不动，作零速修正和航向修正，对准时间不少于250秒；对准过程中包括建立精对准过程零速修正和航向修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤7、两位置对准结束后立即进入飞行过程；飞行轨迹经历主动段大过载、中段前期，期间当导航解算高度大于85km时，视为载体已进入真空段，这时利用观测到的加速度计信息进行基于卡尔曼滤波的真空修正；真空修正过程包括建立真空修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计；

步骤8、惯组进入中段后期，当导航解算高度低于85km时，视为载体已出真空段，这时无真空滤波修正，但延续前期基于真空修正的结果继续对器件误差进行修正。

2.根据权利要求1所述的一种基于比力观测的捷联惯导真空滤波修正方法，其特征在于：步骤4中所述的“对准过程包括建立精对准过程零速修正的系统状态方程和量测方程，以及卡尔曼滤波状态估计”，其具体实现过程如下：

1)导航坐标系取为游动自由方位坐标系，建立步骤4中用到的系统状态方程和量测方程如下：

ddtδθxδθyδhδvxδvyδvzψxψyψz=-vzR0vyR20-1R00000-vzR-vxR21R00000-vyvx00010000-g002Ωz-(ρ+2Ω)y0-fzfyg00-2Ωz0(ρ+2Ω)xfz0-fx002gR(ω+Ω)y-(ω+Ω)x0-fyfx00000000Ωz-ωy000000-Ωz0ωx000000ωy-ωx0δθxδθyδhδvxδvyδvzψxψyψz+000δfxδfyδfz000+000000-ϵx-ϵy-ϵz]]>

式中，δθ——角位置误差矢量；δh——高度误差；δv——速度误差矢量；ψ——ψ角姿态误差；v——载体运动速度矢量；ρ——载体运动角速率矢量；Ω——地球自转角速率矢量；ω——ρ+Ω；g——地球重力加速度；R——地球半径；f——载体感受的比力矢量；δf——加速度计输出误差；ε——陀螺输出误差；

滤波器中陀螺仪的误差模型为：

ϵxϵyϵzn=Cbnϵxϵyϵzb=C11C12C13C21C22C23C31C32C33BxByBz+SgxSgxySgxzSgyxSgySgyzSgzxSgzySgzωibxωibyωibz]]>

式中，B_x、B_y、B_z为陀螺的常值漂移误差项；S_gx、S_gy、S_gz为陀螺的标度误差项；

S_gxy、S_gxz、S_gyx、S_gyz、S_gzx、S_zy砂为陀螺的安装误差；

ω_ib为陀螺仪敏感的相对惯性空间的角速度矢量；

为捷联惯导的位置矩阵

Cbn=cosγcosθsinγcosψ+cosγsinθsinψsinγsinψ-cosγsinθcosψ-sinγcosθcosγcosψ-sinγsinθsinψcosγsinψ+sinγsinθcosψsinθ-cosθsinψcosθcosψ]]>

上式中γ、θ、ψ分别表示载体的横滚角，俯仰角和航向角；

滤波器中加速度计的误差模型为：

δfxδfyδfzn=Cbnδfxδfyδfzb=C11C12C13C21C22C23C31C32C33AxAyAz+SaxSaxySaxzSayxSaySayzSazxSazySazfbxfbyfbz]]>

式中，A_x、A_y、A_z为加速度计的常值漂移误差项；

S_ax、S_ay、S_az为加速度计的标度误差项；

S_axy、S_axz、S_ayx、S_ayz、S_azx、S_azy为加速度计的安装误差；

f_b为加速度计感受的比力矢量；

上述陀螺仪、加速度计的常值误差项根据实际情况需要选用，这里选取的误差状态变量包括：光纤陀螺的3个常值漂移误差B_x、B_y、B_z，加速度计的3个常值漂移误差A_x、A_y、A_z，光纤陀螺仪的1个标度误差S_gx，加速度计的3个标度误差S_ax、S_ay、S_az，陀螺仪的2个安装误差S_gyx、S_gzx，以及加速度计的1个安装误差S_axy；将上述误差状态变量作为增广状态变量添加入上述系统状态方程，共计22阶的滤波器状态变量为

X(t)＝[δθ_x δθ_y δh δv_x δv_z ψ_x ψ_y ψ_z B_x B_y B_z S_gx S_gyx S_gzx A_x A_y A_z S_ax S_ay S_az S_axy]在静基座对准时，首先使用的外部信息是零速信息，其量测模型为

Z₁(t)＝H₁X(t)+η₁(t)＝[0_3×3|I_3×3|0_3×16]X(t)+η₁(t)     (1)

式中，η₁为零速量测噪声矢量；

2)步骤4中，对系统的状态变量进行估计，需要对捷联惯组的系统方程进行离散化；离散化采用泰勒级数展开：

Φ(k+1,k)=I+TA(k)+T22!A2(k)+T33!A3(k)+...]]>

式中T为滤波周期；

系统模型噪声的方差为

Q(k)=QT+[FQ+(FQ)T]T22!+{F[FQ+(FQ)T]+F[(FQ+QFT)]T}T33!+...]]>

3)步骤4中卡尔曼滤波器的迭代，第k+1步的量测值为Z_k+1，则x(k+1)的卡尔曼滤波估计值按下述方程求解：

Pk+1/k=Φk+1,kPk|kΦk+1,kT+ΓkQkΓkT]]>

Kk+1=Pk+1/kHk+1T(Hk+1PK+1/kHk+1T+Rk+1)-1]]>

Pk+1/k+1=(I-Kk+1Hk+1)Pk+1/k(I-Kk+1Hk+1)T+Kk+1Rk+1Kk+1T.]]>