[发明专利]一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法

权利要求书

1.一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法，其特征在于，包括以下几个步骤：

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

载体的初始位置参数包括初始的经度、纬度、高度；

步骤二、光纤捷联惯组进行预热，然后采集光纤陀螺仪和石英加速度计的输出数据；

光纤陀螺仪输出的数据为载体相对于惯性参考系的角速率石英加速度计输出的数据为载体相对于惯性参考下的比力f^b；

步骤三、对采集到的光纤陀螺仪和石英加速度计的数据进行处理，采用二阶调平法和方位估算法完成光纤捷联惯导系统的粗对准，初步确定载体姿态角；

步骤四、粗对准结束后进入精对准阶段；保持载体在第一位置上静止不动，进行第一次零速修正；

步骤五、载体从第一位置以45度/秒的转速绕航向轴旋转180度至第二位置；保持载体在第二位置上静止不动，进行第二次零速修正；

步骤六、载体从第二位置以-45度/秒的转速绕航向轴旋转-180度至第一位置；保持载体在第一位置上静止不动，进行第三次零速修正；

步骤七、载体从第一位置以45度/秒的转速绕航向轴旋转180度至第二位置；保持载体在第二位置上静止不动，进行第四次零速修正；

步骤八、通过步骤七的第四次零速修正，得到载体的真实航向角，光纤捷联惯导系统完成寻北，同时估计出器件的漂移误差。

2.根据权利要求1所述的一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法，其特征在于，步骤一中，外部设备为GPS接收机。

3.根据权利要求1所述的一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法，其特征在于，所述的零速修正基于卡尔曼滤波，即利用载体此时速度为零作为卡尔曼滤波器的观测量，通过卡尔曼滤波迭代，估计出载体的航向误差角ψ_z、光纤陀螺仪的漂移误差B_x、B_y，加速度计的漂移误差A_x、A_y，最后对航向角、光纤陀螺仪和加速度计的输出进行修正。

4.根据权利要求1或者3所述的一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法，其特征在于，所述零速修正具体包括以下几个步骤：

步骤1、建立精对准的系统状态方程和量测方程；

导航坐标系取为游动自由方位坐标系；

根据载体现在的位置，建立系统状态方程和量测方程分别为：

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

Z(t)＝HX(t)+η(t)

式中，表示系统状态的微分、F(t)表示状态矩阵、X(t)表示系统状态向量、W(t)表示系统噪声、Z(t)表示量测值、H表示观测矩阵、η(t)表示量测噪声；

其中：系统状态向量与系统噪声分别为：

X(t)＝[δθ_x δθ_y δθ_z δv_x δv_y δv_z ψ_x ψ_y ψ_z A_x A_y A_z B_x B_y B_z]^T

W(t)=000wδvxwδvywδvzwψxwψywψz000000T]]>

式中，δθ表示角位置误差矢量、δθ_x表示角位置误差矢量X轴分量、δθ_y表示角位置误差矢量Y轴分量、δh表示高度误差、δV表示速度误差矢量、δV_x表示速度误差矢量X轴分量、δV_y表示速度误差矢量Y轴分量、δV_z表示速度误差矢量Z轴分量、ψ表示姿态角误差、ψ_x表示俯仰角误差、ψ_y横滚角误差、ψ_z航向角误差、A_x、A_y、A_z为加速度计的漂移误差状态变量，A_x为X轴加速度计载体坐标系下漂移误差、A_y为Y轴加速度计载体坐标系下漂移误差、A_z为Z轴加速度计载体坐标系下漂移误差，B_x、B_y、B_z表示光纤陀螺仪的漂移误差状态变量，B_x表示载体系下X轴陀螺的漂移误差，B_y表示载体系下Y轴陀螺的漂移误差，B_z表示载体系下Z轴陀螺的漂移误差；系统噪声W(t)六个分量分别为加速度计和陀螺在载体坐标系下均值为O，方差为Q、呈正态分布的白噪声；

状态矩阵为：

F(t)=F1(t)F2(t)09×9F3(t)]]>

式中，F₁(t)、F₂(t)、F₃(t)具体为：

F1(t)=-vzR0vyR20-1R00-vzR-vxR21R00-vyvx00010-g002(ρ+2Ω)z-(ρ+2Ω)yg00-2(ρ+2Ω)z0(ρ+2Ω)x002gR(ρ+2Ω)y-(ρ+2Ω)x0]]>

F2(t)=03×303×303×3F21(t)Cbn03×3]]>F21(t)=0-fzfyfz0-fx-fyfx0]]>

F3(t)=F31(t)03×3Cbn06×606×606×6]]>F31(t)=0Ωz-ωy-Ωz0ωxωy-ωx0]]>

Cbn=C11C12C13C21C22C23C31C32C33]]>

式中：v表示载体运动速度矢量、v_x表示载体运动速度矢量X轴分量、v_y表示载体运动速度矢量Y轴分量、v_z表示载体运动速度矢量Z轴分量、R表示地球半径、Ω表示地球自转角速率矢量、Ω_z表示地球自转角速率矢量Z轴分量、g表示地球重力加速度、ρ表示载体运动角速率矢量、ω表示ρ+Ω、(ρ+2Ω)_x表示矢量(ρ+2Ω)的X轴分量、(ρ+2Ω)_y表示矢量(ρ+2Ω)的Y轴分量、(ρ+2Ω)_z表示矢量(ρ+2Ω)的Z轴分量、f表示载体敏感的比力矢量、f_x表示载体感受的比力矢量X轴分量、f_y表示载体感受的比力矢量Y轴分量、f_z表示载体感受的比力矢量Z轴分量、ω_x表示ω矢量X轴分量、ω_y表示ω矢量Y轴分量；表示捷联姿态矩阵，C_ij(i，j＝1，2，3)表示捷联姿态矩阵各个分量；

在静基座对准时，使用的外部信息是零速信息，取速度误差为观测量，则系统的观测方程及观测矩阵分别为：

Z(t)＝HX(t)+η(t)

H＝[O_3×3 I_3×3 O_3×9]

其中，η(t)分别为零速量测噪声矢量、I表示单位矩阵；

步骤2：对系统状态方程进行离散化；

对步骤1建立的系统状态变量进行估计，需要对光纤捷联惯组的系统状态方程进行离散化；离散化采用泰勒级数展开，则：

Φ(k+1,k)=I+TF(k)+T22!F2(k)+T33!F3(k)+···]]>

其中：Φ(k+1，k)为状态一步转移矩阵、I为十五阶单位阵、F(k)为状态转移矩阵，T为滤波周期；

系统模型噪声的方差为：

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

其中：Q(k)为离散系统噪声方差阵、Q连续系统噪声方程强度阵、F为状态转移矩阵；

步骤3：进行卡尔曼滤波状态估计；

对卡尔曼滤波器进行迭代，第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]]>

最后估计得到载体此刻所在位置的航向误差角ψ_z、光纤陀螺仪的漂移误差B_x、B_y，加速度计的漂移误差A_x、A_y，最后对航向角、光纤陀螺仪和加速度计的输出进行修正，利用ψ_z对载体的航向角进行修正，得到载体的真实航向角，光纤捷联惯导系统完成寻北。

5.根据权利要求1所述的一种抑制器件斜坡误差影响的光纤捷联惯组往复式两位置寻北方法，其特征在于，步骤四中，进行第一次零速修正，在步骤四的修正基础上，步骤五再进行第二次零速修正，在步骤五的修正基础上，步骤六再进行第三次零速修正进行，在步骤六的修正基础上，步骤七再进行第四次零速修正进行。