[发明专利]一种管道测绘用惯导里程计组合导航方法

权利要求书

1.一种管道测绘用惯导里程计组合导航方法，其特征在于：它包括如下步骤，

（1）建立计算模型，

（2）航位计算，

（3）Kalman滤波模型，

（4）反向处理，

（5）标志点修正。

2.如权利要求1所述的一种管道测绘用惯导里程计组合导航方法，其特征在于：所述的步骤（1）包括，

1）误差模型

惯导里程计组合导航算法采用“速度+位置”匹配Kalman滤波方法，该方案采用19阶导航误差模型，选取19个误差状态变量为

X=δVnδVuδVeΦnΦuΦeδLDδhDδλD▿x▿y▿zϵxϵyϵzΘYΘZΔSFΔt]]>

状态方程为：

X·=AX]]>

其中

A=A1A203×3Cbn03×303×4A3A403×303×3-Cbn03×403×3A503×303×303×3A6010×19]]>

A1=-Vu/RM-Vn/RM-2(VetanL/RN+ωiesinL)2Vn/RM02(Ve/RN+ωiecosL)VetanL/RN+2ωiesinL-(Ve/RN+2ωiecosL)VntanL/RN-Vu/RN]]>

A2=0-fefufe0-fn-fufn0,]]>A3=001/RN00tanL/RN-1/RM00]]>

A4=0-Vn/RM-ωiesinL-VetanL/RNVn/RM0ωiecosL+Ve/RNωiesinL+VetanL/RN-ωiecosL-VE/RN0]]>

A5=0-VDeVDuVDe0-VDn-VDuVDn0,]]>A6=-Cbn(1,3)VDCbn(1,2)VDCbn(1,1)VD0-Cbn(2,3)VDCbn(2,2)VDCbn(2,1)VD0-Cbn(3,3)VDCbn(3,2)VDCbn(3,1)VD0]]>

其中，δV_n、δV_u、δV_e为惯导北向、垂向、东向速度误差；Φ_n、Φ_u、Φ_e为惯导北向、垂向、东向失准角；惯导X、Y、Z轴加速度计零偏；ε_x、ε_y、ε_z：惯导X、Y、Z轴陀螺漂移；δL_D、δh_D、δλ_D为里程计航位推算的纬度、高度、经度误差；Θ_Y、Θ_Z为惯导与里程计间沿惯导Y和Z轴的安装误差角；ΔSF：里程计刻度系数误差；Δt：为时间延迟；f_n、f_u、f_e为惯导系统测量的北天东三方向上的比力；V_n、V_u、V_e分别为惯导系统北向、垂向和东向速度；ω_ie为地球自转角速率；L表示惯导系统所处位置的纬度；R_N、R_M分别为卯酉圈曲率半径、子午面内曲率半径，V_D为里程计的前向速度；V_Dn、V_Du、V_De分别表示里程计航位推算的北向、垂向、东向速度；表示矩阵第1行第1列对应的元素，其余相同形式变量的含义与此相同；

2）观测方程

滤波器观测方程分为两部分，当有速度观测时，观测方程如下：

δVnδVuδVeδLDδhDδλD=1000VDe-VDuCbn(1,3)VD-Cbn(1,2)VD-Cbn(1,1)VDVe·010-VDe0VDn03×9Cbn(2,3)VD-Cbn(2,2)VD-Cbn(2,1)VDVe·001VDu-VDn0Cbn(3,3)VD-Cbn(2,2)VD-Cbn(3,1)VDVe·03×19X]]>

当有参考点信息时，观测方程如下：

δVnδVuδVeδLDδhDδλD=03×1910003×601003×10001X]]>

分别表示导航解算时用于速度更新的惯导系统地理系的加速度值；其余各变量定义同上节；

3）方程离散化

状态转移矩阵离散化公式如下

φk+1,k=I+TnATn+TnA2Tn+......+TnATe=I+Σt=TnTeTnAt]]>

其中，T_n为导航周期，T_n=0.005s，T_e为滤波周期，T_e=1s，A_t为t时刻的状态转移矩阵，φ_k+1,k为离散化之后的转移矩阵，每个滤波周期开始时t=0；

噪声阵离散化公式为：

Q_k=QT_e

Q为噪声阵，

观测量求解公式如下：

δVn=Vn-VDnδVu=Vu-VDuδVe=Ve-VDeδLD=LD-LMδhD=hD-hMδλD=λD-λM]]>

其中，λ_D、L_D、h_D为里程计航位推算的经度、纬度、高度；λ_M、L_M、h_M为标志点处的经度、纬度、高度，里程计的速度由下式得到：

VDnVDuVDe=CbnΔS/dT00]]>

式中，ΔS为0.1s内的位移增量，dT＝0.1s。