CN102636165A - 一种用于油气管道轨迹测绘的后处理组合导航方法 - Google Patents

Abstract

本发明提供一种用于油气管道轨迹测绘的后处理组合导航方法。本发明利用捷联惯导、航位推算以及定点的GPS信号进行组合导航在没有GPS修正点处，进行捷联惯导\航位推算组合导航；在GPS修正点处，进行捷联惯导\航位推算\GPS组合导航。利用卡尔曼滤波器对捷联惯导系统误差、姿态角误差以及里程计标度因数误差进行补偿，同时利用系统数据处理非实时的特点，对系统各项误差进行重复估计并补偿，直至系统误差小于设定的门限值，有效的提高了系统定位精度，满足实际工程需要。

Description

一种用于油气管道轨迹测绘的后处理组合导航方法

技术领域

本发明涉及油气管道轨迹测绘的后处理组合导航方法，特别是在长输油气管道轨迹测绘的特殊使用环境下利用惯性测量装置进行轨迹测绘的后处理方法。

背景技术

随着我国国民经济的快速发展，输油气管道建设进入高峰期，管道作为石油、天然气最经济合理的运输方式，具有安全、环保、高效、节能的优点。由于石油、天然气具有易燃、易爆性和有毒性等特点，管道的安全运行非常重要。油气管道长期服役后，会因外部干扰、腐蚀、管材和施工质量等原因发生失效事故，不仅影响能源的供应，还可能导致火灾、爆炸、中毒事件的发生，造成重大经济损失、人员伤亡和环境污染。借助于各种管道内检测技术，能有效检测出管壁内的各种金属损失缺陷和机械损伤，为管道的安全运行提供科学的维护依据。但现有技术中，存在测量误差较大，精确度不高的问题，导致定位精度无法满足工程使用要求。这对管道业主有效监测其管道是否存在安全隐患，是否因地震、土壤坍塌和人为地表作业造成的管道位移造成极大的不便，同时也不能为管道业主提供实施完整性管理的重要基础数据即管道走向检测数据。

发明内容

鉴于现有技术中存在的问题，本发明的目的在于提供一种用于油气管道轨迹测绘的后处理组合导航方法。通过本发明，利用本发明的实物载体捷联惯导系统，可实现对长输油气管道的探伤定位和轨迹测绘，结合GPS技术实现管道可视化完整性管理。

为实现上述目的，本发明提供一种用于油气管道轨迹测绘的后处理组合导航方法。该方法包括以下步骤：在每个捷联惯导系统采样周期内：

步骤1，判断系统时间是否小于最后一个GPS点时间，如果否，结束测绘，如果是则进行下一步；

步骤2，捷联惯导解算，获得所述捷联惯导系统的姿态、速度、位置，由捷联惯导解算公式得出所述捷联惯导系统误差方程；

步骤3，航位推算，利用所述捷联惯导解算的姿态角和里程计进行所述航位推算，获得所述航位推算速度和位置，由航位推算解算公式得出航位推算误差方程；

步骤4，利用所述捷联惯导系统误差方程和所述航位推算误差方程建立卡尔曼滤波模型：

组合导航系统状态方程为：

${\stackrel{\·}{X}}_{\mathrm{SINS}/\mathrm{DR}}={\mathrm{FX}}_{\mathrm{SINS}/\mathrm{DR}}+\mathrm{G\ω}---\left(1\right)$

组合导航系统观测方程为：

Z_k＝H_kX_SINS/DR+v_k    (2)

上述方程(1)中：

$X_{\mathrm{SINS}/\mathrm{DR}}={\left[\begin{array}{ccccccc}\mathrm{\φ}& {\mathrm{\δV}}^n& \mathrm{\δP}& \mathrm{\ϵ}& \▿& {\mathrm{\δP}}_D& \mathrm{\δSF}\end{array}\right]}^T,$ 其中φ＝[φ_E φ_N φ_U]为系统姿态角误差，φ_E、φ_N、φ_U分别为捷联惯导系统横滚、俯仰、航向角；

为捷联惯导系统速度误差，δP＝[δλ δL δh]为捷联惯导系统位置误差，δλ、δL、δh分别为捷联惯导系统纬度、经度、高度位置误差；ε＝[ε_x ε_y ε_z]为陀螺随机常值漂移， $\▿=\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]$ 为加速度计随机常值偏差，x、y、z载体坐标轴向；δP_D＝[δλ_D δL_D δh_D]为航位推算位置误差，δL_D、δλ_D、δh_D分别为航位推算位置经度、纬度、高度误差，δSF为里程计标度系数误差；

$F=\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0\\ F_{\mathrm{SINS}/\mathrm{DR}}& F_{\mathrm{DR}}\end{array}\right],$ 其中，F_SINS为捷联惯导系统误差状态矩阵，F_DR为航位推算误差状态矩阵，F_SINS/DR为捷联惯导与航位推算误差状态交叉矩阵； $G={\left[\begin{array}{ccc}{-C}_b^n& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& C_b^n& 0_{3\×13}\end{array}\right]}^T,$ 其中，

为捷联姿态矩阵；

ω＝[ω_Gx ω_Gy ω_Gz ω_Ax ω_Ay ω_Az]T，其中ω_G＝[ω_Gx ω_Gy ω_Gz]为陀螺随机游走漂移，ω_A＝[ω_Ax ω_Ay ω_Az]为加速度计随机游走偏差；

上述方程(2)中：

Z_k＝P_SINS-P_DR＝δP-δP_D，δP和δP_D与所述方程(1)中相同；

H_k＝[0_3×6 I₃ 0_3×6-I₃ 0_3×1]，I₃为3X3的单位矩阵；

v_k为系统观测量测噪声；

对所述方程(1)和(2)进行卡尔曼滤波计算，估计所述捷联惯导姿态角和里程计标度因数误差并补偿；

步骤5，判断是否存在GPS信号，如果不存在所述GPS信号，则回到步骤2，如果存在，则利用所述捷联惯导系统误差方程和所述航位推算误差方程以及GPS信号建立所述方程(1)和(2)的卡尔曼滤波模型：

其中Z_k＝P_SINS-P_GPS＝δP-δP_D，其余变量与所述步骤4中相同；

进行卡尔曼滤波计算，估计所述捷联惯导姿态角和里程计标度误差并补偿；

步骤6，判断补偿后的捷联惯导系统位置误差是否小于预先设定的门限值，若不小于，则回到步骤2，若小于，则读取下一个GPS点。

本发明与现有技术相比的有益效果：

本发明解决了长输油气管道进行轨迹测绘时无法实时提供GPS信号，导致定位精度无法满足工程使用要求的难题。本发明利用捷联惯导、里程计标度因数以及定点的GPS信息进行组合导航，对惯性器件误差、姿态角误差以及里程计标度因数误差进行补偿，同时利用系统数据处理非实时的特点，对各项误差进行重复估计并补偿，定位精度(经度、纬度、高度误差均方差)能达到小于2m，满足实际工程需要。

附图说明

图1为一种用于油气管道轨迹测绘的后处理组合导航方法流程图；

图2为轨迹测绘的时间示意图；

具体实施方式

下面将结合附图对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。

惯性管道轨迹测绘系统的基本组成为光纤捷联惯导系统，辅助信号有里程计和定距离(1Km～2Km)的GPS信号。在管道检测过程中，惯性管道测绘系统搭载在管道检测总体上，靠油压或气压，以一定的速度前进，在前进的过程中系统采集计算机高速采集惯性传感器数据、里程计标度因数，并将数据保存在系统磁盘中。管道检测结束后，系统通过地面数据处理软件将保存的数据下载到地面计算机中，结合地面定距离、高精度的GPS信号，利用本发明进行数据处理，生成管道的轨迹曲线和GPS坐标文本，达到对管道探伤定位和轨迹测绘的目的。

1.捷联惯导\航位推算组合导航

在捷联惯导解算周期内，分别进行捷联惯导解算和航位推算，得到捷联惯导系统的速度和位置信息，以及航位推算的速度和位置信息。利用航位推算得到的速度信息可以得到捷联惯导系统的速度误差，从而推算系统姿态角误差并进行补偿。

2.捷联惯导\航位推算\GPS组合导航

由于实时的修正系统姿态角误差，航位推算位置误差主要由航向角误差和里程计标度因数误差引起。其中，航向角误差引起经纬度误差交叉耦合，而里程计标度因数误差引起经纬度径向的误差。对航位推算方程进行误差扰动，得到系统航向角误差和里程计标度因数误差估计公式。在定距离GPS修正点处，利用航位推算位置误差信息，推算出航向角误差和里程计标度因数误差并进行补偿。

下面参照附图对本发明的实施例进行说明。

图1为本发明实施例提供的一种用于油气管道轨迹测绘的后处理组合导航方法的流程图，如图1所示，该方法包括

步骤1，判断系统时间是否小于最后一个GPS点时间，如果是，则进行步骤2，如果否，则系统测绘结束。

步骤2，进行捷联惯导解算，系统导航周期为捷联惯导系统的采样周期，该参数可以通过地面软件设置，一般约为200-400Hz。

所述捷联惯导解算分为三部分，包括姿态角更新、速度更新和位置更新。

$\left\{\begin{array}{c}{\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\Ω}}_{\mathrm{nb}}^b\\ {\stackrel{\·}{v}}^n=f^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\\ {\stackrel{\·}{C}}_n^e=C_n^e({\mathrm{\ω}}_{\mathrm{en}}^n\×),\stackrel{\·}{h}=v_U^n\end{array},\×v^n+g^n\right.---\left(3\right)$

其中：

为姿态矩阵，初始值通过所述捷联惯导初始对准进行装订；

为一个导航周期内的旋转矢量；

vⁿ为载体速度；

为载体天向角速度；

fⁿ为载体加速度；

为地球角速度；

载体角速度；

位置矩阵，初始值通过初始装订得到，h为地理高度。

步骤3，航位推算，包括速度更新和位置更新。

$\left\{\begin{array}{c}{v}_D^n=C_b^n{v}_D^b\\ {\stackrel{\·}{L}}_D=v_{\mathrm{DN}}^n/(R_M+h_D),{\stackrel{\·}{\mathrm{\λ}}}_D=v_{\mathrm{DE}}^n\sec L_D/(R_N+h_D),{\stackrel{\·}{h}}_D=v_{\mathrm{DU}}^n\end{array}\right.---\left(4\right)$

其中：姿态矩阵

由步骤2所述捷联惯导解算得到，为里程计测量的速度信息，

分别为北向和东向速度分量，L_D、λ_D为航位推算系统位置的经、纬度，R_M、R_N为地球半径，h_D地理高度。

步骤4，由(3)式得到捷联惯导系统的误差方程：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=\mathrm{\φ}\×{\mathrm{\ω}}_{\mathrm{in}}^n+{\mathrm{\δ\ω}}_{\mathrm{in}}^n-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{V}}^n={-}^{\mathrm{\φn}}\×f^n+{\mathrm{\δV}}^n\×({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)+V^n({2\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)+{\▿}^n\\ \mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2},\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_U,\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_N+h}\sec L+\mathrm{\δL}\frac{V_E}{R_N+h}\tan L\sec L-\mathrm{\δh}\frac{V_E\sec L}{{(R_N+h)}^2}\end{array}\right.---\left(5\right)$

由(4)式得到航位推算系统的误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}{V}_D^n={\tilde{V}}_D^n-V_D^n=-\mathrm{\φ}\×V_D^n+\mathrm{\δSF}\·V_D^n\\ \mathrm{\δ}{\stackrel{\·}{L}}_D=\frac{{\mathrm{\δV}}_{\mathrm{Dn}}^n}{R_M+h_D}-\mathrm{\δ}{h}_D\frac{V_{\mathrm{DN}}^n}{{(R_M+h_D)}^2},\mathrm{\δ}{\stackrel{\·}{h}}_D=\mathrm{\δ}{V}_{\mathrm{DU}}^n,\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\λ}}}_D=\frac{{\mathrm{\δV}}_{\mathrm{DE}}^n}{R_N+h_D}\sec L_D+\mathrm{\δ}{L}_D\frac{V_{\mathrm{DE}}^n}{R_N+h_D}\tan L_D\sec L_D-\mathrm{\δ}{h}_D\frac{V_{\mathrm{DE}}^n\sec L_D}{{(R_N+h_D)}^2}\end{array}\right.---\left(6\right)$

由所述方程(5)和(6)式建立捷联惯导和航位推算组合导航系统误差模型：

组合导航系统状态方程为：

${\stackrel{\·}{X}}_{\mathrm{SINS}/\mathrm{DR}}={\mathrm{FX}}_{\mathrm{SINS}/\mathrm{DR}}+\mathrm{G\ω}---\left(1\right)$

组合导航系统观测方程为：

Z_k＝H_kX_SINS/DR+v_k    (2)

其中，所述组合导航系统状态方程(1)中各变量定义：

$X_{\mathrm{SINS}/\mathrm{DR}}={\left[\begin{array}{ccccccc}\mathrm{\φ}& {\mathrm{\δV}}^n& \mathrm{\δP}& \mathrm{\ϵ}& \▿& {\mathrm{\δP}}_D& \mathrm{\δSF}\end{array}\right]}^T$

$F=\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0\\ F_{\mathrm{SINS}/\mathrm{DR}}& F_{\mathrm{DR}}\end{array}\right]$

$G={\left[\begin{array}{ccc}{-C}_b^n& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& C_b^n& 0_{3\×13}\end{array}\right]}^T$

ω＝[ω_GX ω_Gy ω_Gz ω_Ax ω_Ay ω_Az]^T。

式中φ＝[φ_E φ_N φ_U]为系统姿态角误差，φ_E、φ_N、φ_U分别为捷联惯导系统横滚、俯仰、航向角；

${{\mathrm{\δV}}^n}_{\·}\left[\begin{array}{ccc}{\mathrm{\δV}}_E^n& {\mathrm{\δV}}_N^n& {\mathrm{\δV}}_U^n\end{array}\right]$ 为捷联惯导系统速度误差；

δP＝[δλ δL δh]为捷联惯导系统位置误差；δλ、δL、δh分别为捷联惯导系统纬度、经度、高度位置误差；

ε＝[ε_x ε_y ε_z]为陀螺随机常值漂移，x、y、z载体坐标轴向；；

$\▿=\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]$ 为加速度计随机常值偏差，x、y、z载体坐标轴向；

δP_D＝[δλ_D δL_D δh_D]为航位推算位置误差；

δSF为里程计标度因数误差；

F_SINS为捷联惯导系统误差状态矩阵；

F_DR为航位推算误差状态矩阵；

F_SINS/DR为捷联惯导与航位推算误差状态交叉矩阵；

组合导航系统误差矩阵F的非零元素如下：

$F\left(\mathrm{1,2}\right)={\mathrm{\ω}}_{\mathrm{ie}}\sin L+V_E^n\tan L/(R_N+h),$ $F\left(\mathrm{1,3}\right)=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_E^n/(R_N+h)),$

F(1，5)＝-1/(R_M+h)， $F\left(\mathrm{1,9}\right)=V_N^n/{(R_M+h)}^2,$

F(2，1)＝-F(1，2)， $F\left(\mathrm{2,3}\right)=-V_N^n/(R_M+h),$

F(2，4)＝1/(R_N+h)，            F(2，7)＝-ω_iesinL，

$F\left(\mathrm{2,9}\right)=-V_E^n/{(R_N+h)}^2,$ F(3，1)＝-F(1，3)，

F(3，2)＝-F(2，3)，            F(3，4)＝tanL/(R_N+h)，

$F\left(\mathrm{3,7}\right)={\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_E^n{\sec }^{2}L/(R_N+h),$ $F\left(\mathrm{3,9}\right)=-V_E^n\tan L/{(R_N+h)}^2,$

F(4，2)＝-f_U，                 F(4，3)＝f_N，

F(4，4)＝(V_NtanL-V_U)/(R_N+h)， $F\left(\mathrm{4,5}\right)=2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+V_E^n\tan L/(R_N+h),$

$F\left(\mathrm{4,6}\right)=-({2\mathrm{\ω}}_{\mathrm{is}}\sin L+V_E^n/(R_N+h)),$ $F\left(\mathrm{4,7}\right)={2\mathrm{\ω}}_{\mathrm{ie}}(V_U^n\sin L+V_N^n\cos L)+V_E^n{V}_N^n{\sec }^{2}L/(R_N+h),$

$F\left(\mathrm{4,9}\right)=(V_E^n{V}_U^n-V_E^n{V}_N^n\tan L)/{(R_N+h)}^2,$ F(5，1)＝f_U，

F(5，3)＝-f_E，            F(5，4)＝-F(4，5)，

F(5，5)＝-V_U/(R_M+h)，     F(5，6)＝-V_N/(R_M+h)，

$F\left(\mathrm{5,7}\right)=-({2V}_E^n{\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_E^{n^2}{\sec }^{2}L/(R_N+h)),$ $F\left(\mathrm{5,9}\right)=V_N^n{V}_U^n/{(R_M+h)}^2+V_E^{n^2}\tan L/{(R_N+h)}^2,$

F(6，1)＝-f_N，

F(6，2)＝f_E， $F\left(\mathrm{6,4}\right)=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+V_E^n/(R_N+h)),$

F(6，5)＝2V_N/(R_M+h)， $F\mathrm{6,7})=-2V_E^n{\mathrm{\ω}}_{\mathrm{ie}}\sin L,$

$F\left(\mathrm{6,9}\right)={-V}_N^{n^2}/{(R_M+h)}^2-V_E^{n^2}/{(R_N+h)}^2,$ F(7，5)＝1/(R_M+h)，

$F\left(\mathrm{7,9}\right)=-V_N^n/{(R_M+h)}^2,$ F(8，4)＝secL/(R_N+h)，

$F\left(\mathrm{8,7}\right)=V_E^n\tan L\sec L/(R_N+h),$ $F\left(\mathrm{8,9}\right)={-V}_E^n\sec L/{(R_N+h)}^2,$

F(9，6)＝1， $F\left(\mathrm{16,1}\right)=V_{\mathrm{DU}}^n/(R_M+h_D),$

$F\left(\mathrm{16,3}\right)=-V_{\mathrm{DE}}^n/(R_M+h_D),$ $F\left(\mathrm{16,18}\right)={-V}_{\mathrm{DN}}^n/{(R_M+h_D)}^2,$

$F\left(\mathrm{16,19}\right)=V_{\mathrm{DN}}^n/(R_M+h_D),$ $F\left(\mathrm{17,2}\right)={-V}_{\mathrm{Du}}^n\sec L_D/(R_N+h_D),$

$F\left(\mathrm{17,3}\right)={-V}_{\mathrm{Dn}}^n\sec L_D(R_N+h_D),$ $F\left(\mathrm{17,16}\right)=V_{\mathrm{DE}}^n\tan L_D\sec L_D/(R_N+h_D),$

$F\left(\mathrm{17,18}\right)={-V}_{\mathrm{DE}}^n\sec L_D/{(R_N+h_D)}^2,$ $F\left(\mathrm{17,19}\right)=V_{\mathrm{DN}}^n\sec L_D/(R_N+h_D),$

$F\left(\mathrm{18,1}\right)=-V_{\mathrm{DN}}^n,$ $F\left(\mathrm{18,2}\right)=V_{\mathrm{DE}}^n,$

$F\left(\mathrm{18,19}\right)=V_{\mathrm{DU}}^n,$ $F(i,j)={-C}_b^n(i=\mathrm{1,2,3};j=\mathrm{10,11,12})$

$F(i,j)=C_b^n(i=\mathrm{4,5,6},;j=\mathrm{13,14,15})$

上面F矩阵中未提到元素均为零；

为捷联姿态矩阵；

ω_G＝[ω_Gx ω_Gy ω_Gz]为陀螺随机游走漂移；

ω_A＝[ω_Ax ω_Ay ω_Az]为加速度计随机游走偏差。

其中，组合导航系统观测方程(2)中各变量定义：

Z_k＝P_SINS-P_DR＝δP-δP_D

H_k＝[0_3×6 I₃ 0_3×6 -I₃ 0_3×1]，I₃为3X3的单位矩阵。v_k为系统观测量测噪声。

根据所述方程(5)(6)利用卡尔曼滤波器估计姿态角和里程计标度因数误差，并补偿。

步骤5，判断是否存在GPS信号，如果不存在，回到步骤2，如果存在GPS信号，则利用所述捷联惯导系统误差方程和所述航位推算误差方程以及GPS信号建立所述方程(1)和(2)的卡尔曼滤波模型：

其中Z_k＝P_SINS-P_GPS＝δP-δP_D，其余变量与所述步骤4中相同；

进行卡尔曼滤波计算，估计所述捷联惯导姿态角和里程计标度因数误差并补偿；

步骤6，如图2所示，判断补偿后的捷联惯导系统位置误差是否小于预先设定的门限值，若不小于，则回到步骤2，若小于，则读取下一个GPS点，回到步骤1.

上述公式中未提及的变量和参数可参照教科书《惯性导航》，此处不再赘述。

由上述实施例，可知利用捷联惯导航位推算组合导航和捷联惯导\航位推算\GPS组合导航，数据处理方式具有非实时的特点，所以在数据处理中，对系统各误差可以重复进行估计和补偿，直至系统误差小于设定门限值，有效的提高了系统定位精度。

本发明未详细说明部分为本领域技术人员公知技术。

Claims (1)

1.一种用于油气管道轨迹测绘的后处理组合导航方法，其特征在于，在每个捷联惯导系统采样周期内：

步骤1，判断系统时间是否小于最后一个GPS点时间，如果否，结束测绘，如果是进行下一步；

步骤2，捷联惯导解算，获得所述捷联惯导系统的姿态、速度、位置，由捷联惯导解算公式得出所述捷联惯导系统误差方程；

步骤3，航位推算，利用所述捷联惯导解算的姿态角和里程计标度因数进行所述航位推算，获得所述航位推算速度和位置，由航位推算解算公式得出航位推算误差方程；

步骤4，利用所述捷联惯导系统误差方程和所述航位推算误差方程建立卡尔曼滤波模型：

组合导航系统状态方程为：

${\stackrel{\·}{X}}_{\mathrm{SINS}/\mathrm{DR}}={\mathrm{FX}}_{\mathrm{SINS}/\mathrm{DR}}+\mathrm{G\ω}---\left(1\right)$

组合导航系统观测方程为：

Z_k＝H_kX_SINS/DR+v_k    (2)

上述方程(1)中：

$X_{\mathrm{SINS}/\mathrm{DR}}={\left[\begin{array}{ccccccc}\mathrm{\φ}& {\mathrm{\δV}}^n& \mathrm{\δP}& \mathrm{\ϵ}& \▿& {\mathrm{\δP}}_D& \mathrm{\δSF}\end{array}\right]}^T,$ 其中φ＝[φ_E φ_N φ_U]为系统姿态角误差，φ_E、φ_N、φ_U分别为捷联惯导系统横滚、俯仰、航向角；

为捷联惯导系统速度误差，δP＝[δλ δL δh]为捷联惯导系统位置误差，δλ、δL、δh分别为捷联惯导系统纬度、经度、高度位置误差；ε＝[ε_x ε_y ε_z]为陀螺随机常值漂移， $\▿=\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]$ 为加速度计随机常值偏差，x、y、z载体坐标轴向；δP_d＝[δλ_D δL_D δh_D]为航位推算位置误差，δL_D、δλ_D、δh_D分别为航位推算位置经度、纬度、高度误差，δSF为里程计刻度系数误差；

$F=\left[\begin{array}{cc}{F}_{\mathrm{SINS}}& 0\\ F_{\mathrm{SINS}/\mathrm{DR}}& F_{\mathrm{DR}}\end{array}\right],$ 其中，F_SINS为捷联惯导系统误差状态矩阵，F_DR为航位推算误差状态矩阵，F_SINS/DR为捷联惯导与航位推算误差状态交叉矩阵；

$G={\left[\begin{array}{ccc}{-C}_b^n& 0_{3\×3}& 0_{3\×13}\\ 0_{3\×3}& C_b^n& 0_{3\×13}\end{array}\right]}^T,$ 其中，

为捷联姿态矩阵；

ω＝[ω_Gx ω_Gy ω_Gz ω_Ax ω_Ay ω_Az]^T，其中ω_G＝[ω_Gx ω_Gy ω_Gz]为陀螺随机游走漂移，ω_A＝[ω_Ax ω_Ay ω_Az]为加速度计随机游走偏差；

上述方程(2)中：

Z_k＝P_SINS-P_Dr＝δP-δP_D，δP和δP_D与所述方程(1)中含义相同；

H_k＝[0_3×6 I₃ 0_3×6 -I₃ 0_3×1]，I₃为3X3的单位矩阵；

v_k为系统观测量测噪声；

对所述方程(1)和(2)进行卡尔曼滤波计算，估计所述捷联惯导姿态角和里程计标度因数误差并补偿；

步骤5，判断是否存在GPS信号，如果不存在所述GPS信号，则回到步骤2，如果存在，则利用所述捷联惯导系统误差方程和所述航位推算误差方程以及GPS信号建立所述方程(1)和(2)的卡尔曼滤波模型：

其中Z_k＝P_SINS-P_GPS＝δP-δP_D，其余变量与所述步骤4中相同；

进行卡尔曼滤波计算，估计所述捷联惯导姿态角和里程计标度误差并补偿；

步骤6，判断补偿后的捷联惯导系统位置误差是否小于预先设定的门限值，若不小于，则回到步骤2，若小于，则读取下一个GPS点。

Images