基于单轴旋转的光纤陀螺捷联惯性导航系统误差抑制方法

摘要

本发明提供的是一种基于单轴旋转的光纤陀螺捷联惯性导航系统误差抑制方法。确定载体的初始位置参数；采集光纤陀螺仪和石英加速度计输出的数据；对加速度计的输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系确定载体的姿态信息并完成系统的初始对准；惯性测量单元坐标系绕载体坐标系oyb轴正向旋转45度并确定两坐标系之间的初始相对位置；IMU绕载体坐标系方位轴ozb正向以角速度ω＝6°/s连续转动；将IMU旋转后光纤陀螺仪和石英加速度计生成的数据转换到载体坐标系下，得到惯性器件常值偏差的调制形式；利用光纤陀螺的输出值ωibb对捷联矩阵Tbn进行更新；计算IMU旋转调制后载体的速度和位置；本发明将三轴方向上的惯性器件常值偏差进行调制，提高导航定位精度。

说明书

(一)技术领域

本发明涉及的是一种误差抑制方法，特别是涉及一种基于单轴旋转的光纤陀螺捷联惯性导航系统的误差抑制方法。

(二)背景技术

捷联惯性导航系统SINS是一种完全自主的导航系统，利用陀螺仪和加速度计测量载体相对惯性空间的线运动和角运动参数，在给定初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。由于SINS完全依靠自身的惯性元件，不依靠任何外界信息测量导航参数，因此，它具有隐蔽性好，不受气候条件限制，不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航过程中惯性器件常值偏差的存在是导致惯导系统导航精度难以提高的主要因素。如何有效限制惯性导航误差发散、提高惯性导航系统精度是惯性导航领域一项非常重要的课题。

为了提高捷联系统自身的精度，一方面可以提高惯性元件的精度，但是由于受加工技术水平的限制，无限制的提高元件的精度是很难实现的；另一方面就是采取捷联惯性导航系统的误差抑制技术，自动抵消惯性器件的误差对系统精度的影响。这样就可以应用现有精度的惯性元件构成较高精度的捷联惯性导航系统。

惯导系统的误差抑制，不是依赖于外部辅助对误差状态进行估计，而是研究惯性导航误差在特定运动条件下的传播规律，并依据此规律限制误差发散，提高导航精度的方法。转动抑制是最典型的误差抑制方法：通过绕一个轴或多个轴转动惯性测量单元(IMU)，对导航误差进行调制，达到控制导航误差发散、提高导航精度的目的。

单轴旋转仅能补偿两个敏感轴方向上惯性器件的常值偏差；双轴旋转虽然可以补偿三个敏感轴方向上惯性器件的常值偏差，但是旋转机构的复杂化导致了系统的可靠性及导航解算效率的降低。因此，如何设计合理的单轴旋转补偿方式提高光纤惯导系统的导航精度有重要的意义。

(三)发明内容

本发明的目的在于提供一种将惯性测量单元绕载体方位轴连续旋转，既保证了三个敏感轴方向上惯性器件的常值偏差得以调制，又避免了双轴旋转所需的复杂的旋转机构及导航解算算法的基于单轴旋转的光纤陀螺捷联惯性导航系统误差抑制方法。

本发明的技术解决方案为：一种捷联惯导系统的单轴旋转调制方法，其特征在于将惯性测量单元绕不与自身重合的载体方位轴连续转动，利用惯性测量单元连续转动过程中IMU坐标系与载体坐标系的相对位置关系，即可确定惯性器件常值偏差的抑制形式，其具体步骤如下：

(1)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(2)光纤陀螺捷联惯性导航系统进行预热后采集光纤陀螺仪和石英加速度计输出的数据。其中，三个陀螺的常值漂移相等、三个加速度计零位偏差相等。根据加速度计的输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系初步确定此时载体的姿态信息(纵摇角θ、横摇角γ和航向角ψ)完成系统的初始对准，建立惯导系统的初始捷联矩阵T_bⁿ：

$T_b^n=\left(\begin{array}{ccc}\cos \gamma \cos \psi -\sin \gamma \sin \theta \sin \psi & -\cos \theta \sin \psi & \sin \gamma \cos \psi +\cos \gamma \sin \theta \sin \psi \\ \cos \gamma \cos \psi +\sin \gamma \sin \theta \sin \psi & \cos \theta \cos \psi & \sin \gamma \sin \psi -\cos \gamma \sin \theta \cos \psi \\ -\sin \gamma \cos \theta & \sin \theta & \cos \gamma \cos \theta \end{array}\right)$

(3)惯性测量单元绕载体坐标系oy_b轴正向旋转45度(如附图2)，确定IMU坐标系与载体坐标系之间的初始相对位置：

载体坐标系与IMU坐标系具有同一坐标原点o，oy_s轴与oy_b轴相重合，ox_s轴、oz_s轴、ox_b轴和oz_b轴位于同一平面内，但oz_s轴与oz_b轴的夹角为45°，oz_s轴与ox_b轴的夹角为90°-45°＝45°。

(4)确定两坐标系相对初始位置关系后，惯性测量单元绕载体坐标系方位轴oz_b正向以角速度ω＝6°/s连续转动(如附图3)：

IMU转动过程中，IMU坐标系到载体坐标系的转换矩阵为：

(5)将惯性测量单元旋转后光纤陀螺仪和石英加速度计生成的数据转换到载体坐标系下，得到惯性器件常值偏差的调制形式：

光纤陀螺仪和加速度计的输出值分别为ω_is^s和f_is^s：

${\omega }_{\mathrm{is}}^s={\left(T_s^b\right)}^T{\omega }_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{ϵ}_x& {ϵ}_y& {ϵ}_z\end{array}\right)}^T+{\omega }_{\mathrm{bs}}^s$

$f_{\mathrm{is}}^s={\left(T_s^b\right)}^T{f}_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{▿}_x& {▿}_y& {▿}_z\end{array}\right)}^T+f_{\mathrm{bs}}^s$

其中，${\omega }_{\mathrm{bs}}^s=-{\left(T_s^b\right)}^T{\omega }_{\mathrm{sb}}^b={\left(\begin{array}{ccc}0& 0& \omega \end{array}\right)}^T,$(·)^T表示矩阵·的转秩，ω_ibd^b、f_ibd^b为载体运动的真实输出。ε_x、ε_y、ε_z为陀螺仪的漂移误差，为加速度计零位误差。由于s系相对b系只有旋转运动，没有相对直线运动，所以$f_{\mathrm{bs}}^s=0,$故加速度计输出可表示为：$f_{\mathrm{is}}^s=T_b^s{f}_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{▿}_x& {▿}_y& {▿}_z\end{array}\right)}^T.$

光纤陀螺仪和加速度计的输出从IMU坐标系到载体坐标系的变换可以表示为：

${\omega }_{\mathrm{ib}}^b=T_s^b{\omega }_{\mathrm{is}}^s+{\omega }_{\mathrm{sb}}^b,$$f_{\mathrm{ib}}^b=T_s^b{f}_{\mathrm{is}}^s$

惯性测量单元绕载体坐标系oy_b轴正向旋转45度，此时可以得到ε_zcos45°＝ε_xcos(90°-45°)、载体坐标系oz_b轴方向上不受陀螺常值漂移和加速度计零位偏差的影响，此时载体相对惯性空间的运动角速度及载体相对惯性空间的线运动加速度在载体坐标系的投影分别如下：

$\left(\begin{array}{c}{\omega }_{\mathrm{ib}}^{\mathrm{bx}}={\omega }_{\mathrm{ibd}}^{\mathrm{bx}}+\sqrt{2}/2\cos \mathrm{\omega t}({ϵ}_x+{ϵ}_z)-\sin \mathrm{\omega t}{ϵ}_y\\ {\omega }_{\mathrm{ib}}^{\mathrm{by}}={\omega }_{\mathrm{ibd}}^{\mathrm{by}}+\sqrt{2}/2\sin \mathrm{\omega t}({ϵ}_x+{ϵ}_z)+\cos \mathrm{\omega t}{ϵ}_y\\ {\omega }_{\mathrm{ib}}^{\mathrm{bz}}={\omega }_{\mathrm{ibd}}^{\mathrm{bz}}\end{array}\right)$

$\left(\begin{array}{c}{f}_{\mathrm{ib}}^{\mathrm{bx}}=f_{\mathrm{ibd}}^{\mathrm{bx}}+\sqrt{2}/2\cos \mathrm{\omega t}({▿}_x+{▿}_z)-\sin \mathrm{\omega t}{▿}_y\\ f_{\mathrm{ib}}^{\mathrm{by}}=f_{\mathrm{ibd}}^{\mathrm{by}}+\sqrt{2}/2\sin \mathrm{\omega t}({▿}_x+{▿}_z)+\cos \mathrm{\omega t}{▿}_y\\ f_{\mathrm{ib}}^{\mathrm{bz}}=f_{\mathrm{ibd}}^{\mathrm{bz}}\end{array}\right)$

其中，ω_ib^bx、ω_ib^by、ω_ib^bz分别为载体相对惯性空间的运动角速度在载体坐标系的ox_b轴、oy_b轴、oz_b轴上的分量；f_ib^bx、f_ib^by、f_ib^bz分别为载体相对惯性空间的线加速度在载体坐标系的ox_b轴、oy_b轴、oz_b轴上的分量。

至此，载体坐标系中方位轴上惯性器件的常值偏差得到抵消；水平方向上惯性器件的常值偏差被调制成周期变化的量，经过惯导系统中积分环节，该常值偏差对系统的作用为零。

(6)将步骤(5)获得的载体系下光纤陀螺的输出值ω_ib^b带入惯导系统中采用四元数法对捷联矩阵T_bⁿ进行更新：

${\omega }_{\mathrm{nb}}^b={\omega }_{\mathrm{ib}}^b-{\left(T_b^n\right)}^T({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)$

其中：ω_ieⁿ为地球自转角速度在导航系下的分量；ω_enⁿ为导航坐标系相对地球坐标系的运动角速度在导航系下的分量；ω_nb^b为载体相对导航坐标系的运动角速度在载体坐标系上的分量。

更新四元数和姿态矩阵：

设载体坐标系相对导航坐标系的转动四元数为：

Q＝q₀+q₁i_b+q₂j_b+q₃k_b

其中：i_b、j_b、k_b分别表示载体坐标系ox_b轴、oy_b轴、oz_b轴上的单位方向向量。

四元数的即时修正可以通过解四元数微分方程$\stackrel{·}{Q}=\frac{1}{2}Q{\omega }_{\mathrm{nb}}^b$来实现：

$\left(\begin{array}{c}{\stackrel{·}{q}}_0\\ {\stackrel{·}{q}}_1\\ {\stackrel{·}{q}}_2\\ {\stackrel{·}{q}}_3\end{array}\right)=\frac{1}{2}\left(\begin{array}{cccc}0& -{\omega }_{\mathrm{nb}}^{\mathrm{bx}}& -{\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bz}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\omega }_{\mathrm{nb}}^{\mathrm{bz}}& -{\omega }_{\mathrm{nb}}^{\mathrm{by}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\omega }_{\mathrm{nb}}^{\mathrm{bx}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{bz}}& {\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right)\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$

其中：ω_nb^bx、ω_nb^by、ω_nb^bz分别表示载体相对导航系的运动角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量。

姿态矩阵T_bⁿ的更新过程为：

$T_b^n=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)$

(7)利用石英加速度计的输出值f_ib^b和步骤(6)计算的姿态矩阵T_bⁿ，计算出经过IMU旋转调制后载体的速度和位置。

1)计算导航系下加速度：

$\left(\begin{array}{c}{f}^{\mathrm{nx}}\\ f^{\mathrm{ny}}\\ f^{\mathrm{nz}}\end{array}\right)=T_b^n\left(\begin{array}{c}{f}_{\mathrm{ib}}^{\mathrm{bx}}\\ f_{\mathrm{ib}}^{\mathrm{by}}\\ f_{\mathrm{ib}}^{\mathrm{bz}}\end{array}\right)$

2)计算载体的水平速度和位置：

根据t₁时刻的载体东向水平速度V_x(t₁)和北向水平速度V_y(t₁)，求取t₁时刻载体水平速度的变化率为：

$\left(\begin{array}{c}{\stackrel{·}{V}}_x\left(t_1\right)=f^{\mathrm{nx}}+(2{\omega }_{\mathrm{ie}}^{\mathrm{nz}}+{\omega }_{\mathrm{en}}^{\mathrm{nz}})V_y\left(t_1\right)\\ {\stackrel{·}{V}}_y\left(t_1\right)=f^{\mathrm{ny}}-(2{\omega }_{\mathrm{ie}}^{\mathrm{nz}}+{\omega }_{\mathrm{en}}^{\mathrm{nz}})V_x\left(t_1\right)\end{array}\right)$

在t₂时刻水平速度和载体位置分别为：

$\left(\begin{array}{c}{V}_x\left(t_2\right)=V_x\left(t_1\right)+{\stackrel{·}{V}}_x\left(t_1\right)(t_2-t_1)\\ V_y\left(t_2\right)=V_y\left(t_1\right)+{\stackrel{·}{V}}_y\left(t_1\right)(t_2-t_1)\end{array}\right)$

3)计算载体速度误差和位置误差：

$\left(\begin{array}{c}\Delta V_x=V_x\left(t_2\right)-V_{x0}\\ \Delta V_y=V_y\left(t_2\right)-V_{y0}\end{array}\right)$

其中：V_x0、V_y0分别表示初始时刻载体的东向和北向速度；ΔV_x、ΔV_y分别表示载体东向、北向速度的变化量；λ₀分别表示初始时刻载体所处位置的经度和纬度；Δλ分别表示载体的纬度、经度的变化量；R_xp、R_yp分别表示地球子午圈、卯酉圈的曲率半径；t₁、t₂为惯导系统的解算过程中两个相邻的时间点。

本发明与现有技术相比的优点在于：本发明打破了传统单轴旋转不能补偿三个方向上惯性器件常值偏差及双轴旋转所需的复杂旋转机构和导航解算算法的约束，提出一种旋转轴与陀螺敏感轴成一定角度的捷联惯导系统误差旋转调制方案，该方法可以将三轴方向上的惯性器件常值偏差进行调制，有效地提高导航定位精度。

对本发明有益的效果说明如下：

在Matlab仿真条件下，对该方法进行仿真实验：

载体作三轴摇摆运动。载体以正弦规律绕纵摇轴、横摇轴和航向轴摇摆，其数学模型为：

$\left(\begin{array}{c}\theta ={\theta }_m\sin ({\omega }_{\theta }+{\phi }_{\theta })\\ \gamma ={\gamma }_m\sin ({\omega }_{\gamma }+{\phi }_{\gamma })\\ \psi ={\psi }_m\sin ({\omega }_{\psi }+{\phi }_{\psi })+k\end{array}\right)$

其中：θ、γ、ψ分别表示纵摇角、横摇角和航向角的摇摆角度变量；θ_m、γ_m、ψ_m分别表示相应的摇摆角度幅值；ω_θ、ω_γ、ω_ψ分别表示相应的摇摆角频率；φ_θ、φ_γ、φ_ψ分别表示相应的初始相位；ω_i＝2π/T_i，i＝θ、γ、ψ，T_i表示相应的摇摆周期，k为初始航向角。仿真时取：θ_m＝12°，γ_m＝15°，ψ_m＝10°，T_θ＝8s，T_γ＝10s，T_ψ＝6s，k＝0。

载体初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：三个初始姿态误差角均为零；

赤道半径：R_e＝6378393.0m；

椭球度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

加速度计零偏：10^-4g₀；

常数：π＝3.1415926；

利用发明所述方法得到载体姿态角误差曲线、速度误差曲线和位置误差曲线分别如图4、图5、图6所示。结果表明有摇摆干扰条件下，采用本发明方法可以获得较高的定位精度。

(四)附图说明

图1为本发明的基于IMU单轴旋转的捷联惯性导航系统误差抑制方法流程图；

图2为初始时刻IMU坐标系与载体坐标系的初始相对位置关系；

图3为IMU转动过程中，IMU坐标系与载体坐标系的相对位置关系；

图4为载体摇摆条件下，基于IMU静止时的载体姿态角误差实验曲线；

图5为载体摇摆条件下，基于IMU静止时的载体速度误差实验曲线；

图6为载体摇摆条件下，基于IMU静止时的载体位置误差实验曲线。

图7为载体摇摆条件下，本发明的基于IMU单轴连续旋转的载体姿态角误差实验曲线；

图8为载体摇摆条件下，本发明的基于IMU单轴连续旋转的载体速度误差实验曲线；

图9为载体摇摆条件下，本发明的基于IMU单轴连续旋转的载体位置误差实验曲线。

(五)具体实施方式

下面结合附图对本发明的具体实施方式进行详细地描述：

(1)利用全球定位系统GPS确定载体的初始位置参数(包括经度、纬度)，将它们装订至导航计算机中。

(2)光纤陀螺捷联惯性导航系统进行预热后采集光纤陀螺仪和石英加速度计输出的数据。其中，三个陀螺的常值漂移相等、三个加速度计零位偏差相等。根据加速度计的输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系初步确定此时载体的姿态信息(纵摇角θ、横摇角γ和航向角ψ)完成系统的初始对准，建立惯导系统的初始捷联矩阵T_bⁿ：

$T_b^n=\left(\begin{array}{ccc}\cos \gamma \cos \psi -\sin \gamma \sin \theta \sin \psi & -\cos \theta \sin \psi & \sin \gamma \cos \psi +\cos \gamma \sin \theta \sin \psi \\ \cos \gamma \cos \psi +\sin \gamma \sin \theta \sin \psi & \cos \theta \cos \psi & \sin \gamma \sin \psi -\cos \gamma \sin \theta \cos \psi \\ -\sin \gamma \cos \theta & \sin \theta & \cos \gamma \cos \theta \end{array}\right)---\left(1\right)$

(3)惯性测量单元绕载体坐标系oy_b轴正向旋转45度(如附图2)，确定IMU坐标系与载体坐标系之间的初始相对位置：

载体坐标系与IMU坐标系具有同一坐标原点o，oy_s轴与oy_b轴相重合，ox_s轴、oz_s轴、ox_b轴和oz_b轴位于同一平面内，但oz_s轴与oz_b轴的夹角为45°，oz_s轴与ox_b轴的夹角为90°-45°＝45°。

(4)确定两坐标系相对初始位置关系后，惯性测量单元绕载体坐标系方位轴oz_b正向以角速度ω＝6°/s连续转动(如附图3)：

IMU转动过程中，IMU坐标系到载体坐标系的转换矩阵为：

(5)将惯性测量单元旋转后光纤陀螺仪和石英加速度计生成的数据转换到载体坐标系下，得到惯性器件常值偏差的调制形式：

光纤陀螺仪和加速度计的输出值分别为ω_is^s和f_is^s：

${\omega }_{\mathrm{is}}^s={\left(T_s^b\right)}^T{\omega }_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{ϵ}_x& {ϵ}_y& {ϵ}_z\end{array}\right)}^T+{\omega }_{\mathrm{bs}}^s$

$f_{\mathrm{is}}^s={\left(T_s^b\right)}^T{f}_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{▿}_x& {▿}_y& {▿}_z\end{array}\right)}^T+f_{\mathrm{bs}}^s---\left(3\right)$

其中，${\omega }_{\mathrm{bs}}^s=-{\left(T_s^b\right)}^T{\omega }_{\mathrm{sb}}^b={\left(\begin{array}{ccc}0& 0& \omega \end{array}\right)}^T,$(·)^T表示矩阵·的转秩，ω_ibd^b、f_ibd^b为载体运动的真实输出。ε_x、ε_y、ε_z为陀螺仪的漂移误差，为加速度计零位误差。由于s系相对b系只有旋转运动，没有相对直线运动，所以$f_{\mathrm{bs}}^s=0,$故加速度计输出可表示为：$f_{\mathrm{is}}^s=T_b^s{f}_{\mathrm{ibd}}^b+{\left(\begin{array}{ccc}{▿}_x& {▿}_y& {▿}_z\end{array}\right)}^T.$

光纤陀螺仪和加速度计的输出从IMU坐标系到载体坐标系的变换可以表示为：

${\omega }_{\mathrm{ib}}^b=T_s^b{\omega }_{\mathrm{is}}^s+{\omega }_{\mathrm{sb}}^b,$$f_{\mathrm{ib}}^b=T_s^b{f}_{\mathrm{is}}^s---\left(4\right)$

惯性测量单元绕载体坐标系oy_b轴正向旋转45度，三个陀螺的常值漂移相等，三个加速度计零位偏差相等，此时可以得到ε_zcos45°＝ε_xcos(90°-45°)、载体坐标系oz_b轴方向上不受陀螺常值漂移和加速度计零位偏差的影响，此时载体相对惯性空间的运动角速度及载体相对惯性空间的线运动加速度在载体坐标系的投影分别如下：

$\left(\begin{array}{c}{\omega }_{\mathrm{ib}}^{\mathrm{bx}}={\omega }_{\mathrm{ibd}}^{\mathrm{bx}}+\sqrt{2}/2\cos \mathrm{\omega t}({ϵ}_x+{ϵ}_z)-\sin \mathrm{\omega t}{ϵ}_y\\ {\omega }_{\mathrm{ib}}^{\mathrm{by}}={\omega }_{\mathrm{ibd}}^{\mathrm{by}}+\sqrt{2}/2\sin \mathrm{\omega t}({ϵ}_x+{ϵ}_z)+\cos \mathrm{\omega t}{ϵ}_y\\ {\omega }_{\mathrm{ib}}^{\mathrm{bz}}{=\omega }_{\mathrm{ibd}}^{\mathrm{bz}}\end{array}\right)$

$\left(\begin{array}{c}{f}_{\mathrm{ib}}^{\mathrm{bx}}=f_{\mathrm{ibd}}^{\mathrm{bx}}+\sqrt{2}/2\cos \mathrm{\omega t}({▿}_x+{▿}_z)-\sin \mathrm{\omega t}{▿}_y\\ f_{\mathrm{ib}}^{\mathrm{by}}=f_{\mathrm{ibd}}^{\mathrm{by}}+\sqrt{2}/2\sin \mathrm{\omega t}({▿}_x+{▿}_z)+\cos \mathrm{\omega t}{▿}_y\\ f_{\mathrm{ib}}^{\mathrm{bz}}=f_{\mathrm{ibd}}^{\mathrm{bz}}\end{array}\right)---\left(5\right)$

其中，ω_ib^bx、ω_ib^by、ω_ib^bz分别为载体相对惯性空间的运动角速度在载体坐标系的ox_b轴、oy_b轴、oz_b轴上的分量；f_ib^bx、f_ib^by、f_ib^bz分别为载体相对惯性空间的线加速度在载体坐标系的ox_b轴、oy_b轴、oz_b轴上的分量。

至此，载体坐标系中方位轴上惯性器件的常值偏差得到抵消；水平方向上惯性器件的常值偏差被调制成周期变化的量，经过惯导系统中积分环节，该常值偏差对系统的作用为零。

(6)将步骤(5)获得的载体系下光纤陀螺的输出值ω_ib^b带入惯导系统中采用四元数法对捷联矩阵T_bⁿ进行更新：

${\omega }_{\mathrm{nb}}^b={\omega }_{\mathrm{ib}}^b-{\left(T_b^n\right)}^T({\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)---\left(6\right)$

其中：ω_ieⁿ为地球自转角速度在导航系下的分量；ω_enⁿ为导航坐标系相对地球坐标系的运动角速度在导航系下的分量；ω_nb^b为载体相对导航坐标系的运动角速度在载体坐标系上的分量。

更新四元数和姿态矩阵：

设载体坐标系相对导航坐标系的转动四元数为：

Q＝q₀+q₁i_b+q₂j_b+q₃k_b                     (7)

其中：i_b、j_b、k_b分别表示载体坐标系ox_b轴、oy_b轴、oz_b轴上的单位方向向量。

四元数的即时修正可以通过解四元数微分方程$\stackrel{·}{Q}=\frac{1}{2}Q{\omega }_{\mathrm{nb}}^b$来实现：

$\left(\begin{array}{c}{\stackrel{·}{q}}_0\\ {\stackrel{·}{q}}_1\\ {\stackrel{·}{q}}_2\\ {\stackrel{·}{q}}_3\end{array}\right)=\frac{1}{2}\left(\begin{array}{cccc}0& {-\omega }_{\mathrm{nb}}^{\mathrm{bx}}& -{\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bz}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\omega }_{\mathrm{nb}}^{\mathrm{bz}}& -{\omega }_{\mathrm{nb}}^{\mathrm{by}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\omega }_{\mathrm{nb}}^{\mathrm{bx}}\\ {\omega }_{\mathrm{nb}}^{\mathrm{bz}}& {\omega }_{\mathrm{nb}}^{\mathrm{by}}& -{\omega }_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right)\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)---\left(8\right)$

其中：ω_nb^bx、ω_nb^by、ω_nb^bz分别表示载体相对导航系的运动角速度在载体坐标系ox_b轴、oy_b轴、oz_b轴上的分量。

姿态矩阵T_bⁿ的更新过程如下：

$T_b^n=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(9\right)$

(7)利用石英加速度计的输出值f_ib^b和步骤(6)计算的姿态矩阵T_bⁿ，计算出经过IMU旋转调制后载体的速度和位置。

1)计算导航系下加速度：

$\left(\begin{array}{c}{f}^{\mathrm{nx}}\\ f^{\mathrm{ny}}\\ f^{\mathrm{nz}}\end{array}\right)=T_b^n\left(\begin{array}{c}{f}_{\mathrm{ib}}^{\mathrm{bx}}\\ f_{\mathrm{ib}}^{\mathrm{by}}\\ f_{\mathrm{ib}}^{\mathrm{bz}}\end{array}\right)---\left(10\right)$

2)计算载体的水平速度和位置：

根据t₁时刻的载体东向水平速度V_x(t₁)和北向水平速度V_y(t₁)，求取t₁时刻载体水平速度的变化率为：

$\left(\begin{array}{c}{\stackrel{·}{V}}_x\left(t_1\right)=f^{\mathrm{nx}}+(2{\omega }_{\mathrm{ie}}^{\mathrm{nz}}+{\omega }_{\mathrm{en}}^{\mathrm{nz}})V_y\left(t_1\right)\\ {\stackrel{·}{V}}_y\left(t_1\right)=f^{\mathrm{ny}}-(2{\omega }_{\mathrm{ie}}^{\mathrm{nz}}+{\omega }_{\mathrm{en}}^{\mathrm{nz}})V_x\left(t_1\right)\end{array}\right)---\left(11\right)$

在t₂时刻水平速度和载体位置分别为：

$\left(\begin{array}{c}{V}_x\left(t_2\right)=V_x\left(t_1\right)+{\stackrel{·}{V}}_x\left(t_1\right)(t_2-t_1)\\ V_y\left(t_2\right)=V_y\left(t_1\right)+{\stackrel{·}{V}}_y\left(t_1\right)(t_2-t_1)\end{array}\right)---\left(12\right)$

3)计算载体速度误差和位置误差：

$\left(\begin{array}{c}\Delta V_x=V_x\left(t_2\right)-V_{x0}\\ \Delta V_y=V_y\left(t_2\right)-v_{y0}\end{array}\right)---\left(14\right)$

其中：V_x0、V_y0分别表示初始时刻载体的东向和北向速度；ΔV_x、ΔV_y分别表示载体东向、北向速度的变化量；λ₀分别表示初始时刻载体所处位置的经度和纬度；Δλ分别表示载体的纬度、经度的变化量；R_xp、R_yp分别表示地球子午圈、卯酉圈的曲率半径；t₁、t₂为惯导系统的解算过程中两个相邻的时间点。