[發明專利]一種管道測繪用慣導里程計組合導航方法

有效

• 申請號: 201310516139.7
• 申請日: 2013-10-28
• 公開（公告）號: CN103727938A
• 公開（公告）日: 2014-04-16
• 發明（設計）人: 李海軍;徐海剛;姜述明;閆春新;朱紅;裴玉鋒;李瑞賢;劉沖;郭元江;李昂;王根
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00
• 代理公司: 核工業專利中心 11007
• 代理人: 高尚梅

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。