CN103727938A - Combination navigation method of inertial navigation odometer for pipeline surveying and mapping - Google Patents

Abstract

The invention belongs to a navigation method, and particularly relates to a combination navigation method of an inertial navigation odometer for pipeline surveying and mapping. The method comprises steps as follows: 1), establishment of a calculation model; 2), calculation of a navigation position; 3), establishment of a Kalman filter model; 4), reverse treatment; and 5), correction of a mark point. The combination navigation method has the advantages as follows: the provided combination navigation method of the inertial navigation odometer for pipeline surveying and mapping recycles whole test data, all errors of an inertial navigation system and an odometer are estimated and compensated through forward and reverse combination navigation filtering, then a track is further corrected in combination of the mark point position information, and finally, high-precision pipeline track data are obtained.

Description

A kind of inertial navigation odometer Combinated navigation method for pipeline mapping

Technical field

The invention belongs to a kind of air navigation aid, be specifically related to a kind of pipeline mapping inertial navigation odometer Combinated navigation method, it can be applied in oil and gas pipeline survey field, can effectively utilize detection data that the error of inertial navigation system and odometer is accurately estimated and compensated, greatly improve the measuring accuracy of pipeline track.

Background technology

In order to ensure oil and gas pipeline safety, need regularly oil and gas pipes to be detected, to obtain the data such as pipeline track.Because oil and gas pipes is generally laid on below earth's surface, be difficult to by effective high-precision positioner, its particular location and pipeline track be measured.And inertial navigation system is a kind of measurement mechanism of relative efficiency, there is full independence, be not subject to the features such as external condition restriction, but inertial navigation system positioning precision is dispersed in time, therefore utilize the auxiliary inertial navigation of odometer to carry out a kind of method that integrated navigation is effective raising integrated navigation precision, and pipe detection is normally processed after obtaining omnidistance data afterwards, therefore how to utilize omnidistance measurement data to obtain the primary study content that high-precision measurement track is pipe detection.

The application > > (Yue Bujiang etc. of < < integrated navigation technology in oil and gas pipes mapping system, China's inertial technology journal, the 16th the 6th phase of volume, in Dec, 2008) a kind of integrated navigation technology for oil and gas pipes mapping has been proposed, application forward filtering completes estimation of error, and utilize smoothing technique to revise the error of initial time, the method has been equivalent to utilize once omnidistance data to complete each estimation of error constantly, the method has improved the unidirectional filtering filtering accuracy of the zero hour.In order more to make full use of omnidistance test data, the present invention proposes a kind of utilization and forward and reversely combines navigation and the method for filtering completes inertial navigation system and the estimation of each margin of error of odometer and the method for compensation, the method has been equivalent to utilize twice estimation that omnidistance data complete every error, with respect to the method in document, there is higher estimation of error precision, finally in conjunction with the positional information of monumented point, gained track is revised again, further to improve trace information.

Summary of the invention

The object of this invention is to provide a kind of method that can inertial navigation system and odometer error effectively be estimated and be compensated, the method can detect data to the whole process of pipeline and carry out forward and reverse combine navigation and filtering, complete the estimation of each error term and compensation, to obtain high-precision pipeline trace information.

The present invention is achieved in that inertial navigation odometer Combinated navigation method for the mapping of a kind of pipeline, and it comprises the steps,

(1) set up computation model,

(2) calculate boat position,

(3) Kalman Filtering Model,

(4) reverse process,

(5) monumented point correction.

Described step (1) comprises,

1) error model

Inertial navigation odometer Integrated Navigation Algorithm adopts " speed+position " coupling Kalman filtering method, and this scheme adopts 19 rank navigation error models, chooses 19 error state variablees to be

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\&delta;V}}_n& {\mathrm{\&delta;V}}_u& \mathrm{\&delta;}{V}_e& {\mathrm{\&Phi;}}_n& {\mathrm{\&Phi;}}_u& {\mathrm{\&Phi;}}_e& {\mathrm{\&delta;L}}_D& {\mathrm{\&delta;h}}_D& {\mathrm{\&delta;\&lambda;}}_D& {\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& {\mathrm{\&Theta;}}_Y& {\mathrm{\&Theta;}}_Z& \mathrm{\&Delta;SF}& \mathrm{\&Delta;t}\end{array}\right]$

State equation is:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}$

Wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\&times;3}& C_b^n& 0_{3\&times;3}& 0_{3\&times;4}\\ A_3& A_4& 0_{3\&times;3}& 0_{3\&times;3}& -C_b^n& 0_{3\&times;4}\\ 0_{3\&times;3}& A_5& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& A_6\\ & & & & & \\ & & 0_{10\&times;19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_n/R_M& -2(V_e\tan L/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)\\ {2V}_n/R_M& 0& 2(V_e/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& -(V_e/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],$ $A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+V_e/R_N\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+V_e\tan L/R_N& -{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{\mathrm{De}}& V_{\mathrm{Du}}\\ V_{\mathrm{De}}& 0& -V_{\mathrm{Dn}}\\ -V_{\mathrm{Du}}& V_{\mathrm{Dn}}& 0\end{array}\right],$ $A_6=\left[\begin{array}{cccc}-C_b^n\left(\mathrm{1,3}\right)V_D& C_b^n\left(\mathrm{1,2}\right)V_D& C_b^n\left(\mathrm{1,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{2,3}\right)V_D& C_b^n\left(\mathrm{2,2}\right)V_D& C_b^n\left(\mathrm{2,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{3,3}\right)V_D& C_b^n\left(\mathrm{3,2}\right)V_D& C_b^n\left(\mathrm{3,1}\right)V_D& 0\end{array}\right]$

Wherein, δ V _n, δ V _u, δ V _efor inertial navigation north orientation, vertical, east orientation velocity error; Φ _n, Φ _u, Φ _efor inertial navigation north orientation, vertical, east orientation misalignment; inertial navigation X, Y, Z axis accelerometer bias; ε _x, ε _y, ε _z: the gyroscopic drift of inertial navigation X, Y, Z axis; δ L _d, δ h _d, δ λ _dfor the latitude of odometer dead reckoning, highly, longitude error; Θ _y, Θ _zfor between inertial navigation and odometer along the alignment error angle of inertial navigation Y and Z axis; Δ SF: odometer scale coefficient error; Δ t: be time delay; f _n, f _u, f _ethe specific force that the north day eastern tripartite who measures for inertial navigation system makes progress; V _n, V _u, V _ebe respectively inertial navigation system north orientation, vertical and east orientation speed; ω _iefor earth rotation angular speed; L represents the latitude of inertial navigation system present position; R _n, R _mbe respectively radius of curvature in prime vertical, meridian ellipse intrinsic curvature radius, V _dforward speed for odometer; V _dn, V _du, V _derepresent respectively the north orientation of odometer dead reckoning, vertical, east orientation speed; representing matrix

element corresponding to the 1st row the 1st row, the implication of all the other same form variablees is identical therewith;

2) observation equation

Wave filter observation equation is divided into two parts, and when having speed observation, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{\mathrm{De}}& -V_{\mathrm{Du}}& & C_b^n\left(\mathrm{1,3}\right)V_D& -C_b^n\left(\mathrm{1,2}\right)V_D& -C_b^n\left(\mathrm{1,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 1& 0& -V_{\mathrm{De}}& 0& V_{\mathrm{Dn}}& 0_{3\&times;9}& C_b^n\left(\mathrm{2,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{2,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 0& 1& V_{\mathrm{Du}}& -V_{\mathrm{Dn}}& 0& & C_b^n\left(\mathrm{3,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{3,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ & & & & & & & & & & \\ & & & & & & 0_{3\&times;19}& & & & \\ & & & & & & & & & & \end{array}\right]X$

When having reference point information, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\&times;19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\&times;6}& 0& 1& 0& 0_{3\&times;10}\\ & 0& 0& 1& \end{array}\right]X$

the accekeration of the inertial navigation system Department of Geography upgrading for speed while representing navigation calculation respectively; The same joint of all the other each variable-definitions;

3) equation discretize

State-transition matrix discretize formula is as follows

${\mathrm{\&phi;}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\mathrm{\&Sigma;}}}{T}_n{A}_t$

Wherein, T _nfor navigation cycle, T _n=0.005s, T _efor filtering cycle, T _e=1s, A _tfor t state-transition matrix constantly, φ _{k+1, k}for the transition matrix after discretize, t=0 when each filtering cycle starts;

Noise battle array discretize formula is:

Q _k=QT _e

Q is noise battle array,

Observed quantity solution formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\&delta;V}}_n=V_n-V_{\mathrm{Dn}}\\ {\mathrm{\&delta;V}}_u=V_u-V_{\mathrm{Du}}\\ {\mathrm{\&delta;V}}_e=V_e-V_{\mathrm{De}}\\ {\mathrm{\&delta;L}}_D=L_D-L_M\\ {\mathrm{\&delta;h}}_D=h_D-h_M\\ {\mathrm{\&delta;\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.$

Wherein, λ _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly, the speed of odometer is obtained by following formula:

$\left[\begin{array}{c}{V}_{\mathrm{Dn}}\\ V_{\mathrm{Du}}\\ V_{\mathrm{De}}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\&Delta;S}/\mathrm{dT}\\ 0\\ 0\end{array}\right]$

In formula, Δ S is the displacement increment in 0.1s, dT=0.1s.

Described step (2) comprises,

When carrying out inertial navigation, carry out dead reckoning, computation period is with the navigation calculation cycle, Tn=5 (ms); The same navigation calculation of dead-reckoning position information initializing, utilizes extraneous bookbinding value to obtain initial longitude, latitude and height,

If the displacement that odometer is exported carrier in the i sampling period is Δ S _i, its being expressed as in inertial navigation carrier coordinate system b system:

$\mathrm{\&Delta;}{S}_i^b={\left[\begin{array}{ccc}\mathrm{\&Delta;}{S}_i& 0& 0\end{array}\right]}^T$

Utilize the attitude matrix of inertial navigation system to be transformed under geographic coordinate system:

$\mathrm{\&Delta;}{S}_i^n=C_b^n\mathrm{\&Delta;}{S}_i^b$

For eliminating odometer quantizing noise, the 0.1s of take makes smoothing processing to speed as the cycle.

Dead reckoning formula is:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D\left(t\right)={\mathrm{\&lambda;}}_D(t-T_n)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.$

the displacement increment that represents respectively odometer Department of Geography in the navigation calculation cycle.

Described step (3) comprises, the formula of Kalman filtering equations is as follows:

State one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

One-step prediction error covariance matrix

$P_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k,k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T$

Estimation error variance battle array

P _k=[I-K _kH _k]P _k,k-1

Wherein,

be a step status predication value,

for state estimation matrix, Φ _{k, k-1}for state Matrix of shifting of a step, H _kfor measurement matrix, Z _kfor measurement amount, K _kfor filter gain matrix, R _kfor observation noise battle array, P _{k, k-1}for one-step prediction error covariance matrix, P _kfor estimation error variance battle array, Γ _{k, k-1}for system noise drives battle array, Q _k-1for system noise acoustic matrix.

Described step (4) comprises,

After the navigation and filtering that complete forward sequence, navigation calculation, dead reckoning and Kalman filtering do not stop, but proceed reverse navigation calculation, dead reckoning and Kalman filtering, and main methods comprises:

1) reverse navigation

The computing formula of navigation is with positive navigation, and difference is in computation process, to need some quantity of states to carry out negate, and concrete processing comprises:

A) acceleration of gravity is reverse: g=-g

B) overload oppositely: fb=-fb

C) angular velocity is reverse: wibb=-wibb

D) rotational-angular velocity of the earth is reverse: wien=-wien

E) involve angular velocity reverse: wenn=-wenn

F) position is upgraded oppositely: speed negate

G) time: t=t-Tn

2) oppositely dead reckoning

Oppositely the form of dead reckoning is with the dead reckoning of forward, with the difference of forward dead reckoning be to get negative sign when displacement is cumulative, formula becomes:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D(t-T_n)={\mathrm{\&lambda;}}_D\left(t\right)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.$

3) inverse filtering

Inverse filtering flow process is calculated with forward filtering, only need be by all elements negate in state matrix and measurement matrix.

Described step (5) comprises, utilize forward and reverse combine be filtered paired error term estimation and compensation after, can obtain relatively accurate trace information, but because the remainder error of odometer is still cumulative, therefore conventionally need to track, further revise by monumented point, concrete method is as follows:

At monumented point place, calculate the error of odometer dead reckoning:

$\left\{\begin{array}{c}{\mathrm{dL}}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.$

λ wherein _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly.

Can obtain $\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{\mathrm{De}}& S_{\mathrm{Dn}}& \\ & S_{\mathrm{Du}}& S_{\mathrm{DL}}\\ -S_{\mathrm{Dn}}& S_{\mathrm{De}}& \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{dL}}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\&lambda;}}_D\end{array}\right]$

ΔΦ wherein _u, ΔΦ _l, Δ SF is respectively alignment error angle, the remaining course of odometer, pitching alignment error angle and scale coefficient error; S _dn, S _du, S _de, S _dLbe respectively north orientation that odometer dead reckoning obtains, vertical, east orientation and radial displacement;

After the error parameter estimating, make the following judgment, when radial error is greater than 0.01m,

DS _d=sqrt (dL _d* dL _d+ dh _d* dh _d+ d λ _d* d λ _d) during >0.01m, to calculating the odometer remainder error obtaining, carry out cumulative sum compensation, and re-start calculating from a upper monumented point, the compensation method of odometer remainder error is as follows:

First error is added up:

$\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]$

Then error is compensated:

$d{C}_o^n=\left[\begin{array}{ccc}1& -d{\mathrm{\&Phi;}}_L& d{\mathrm{\&Phi;}}_u\\ d{\mathrm{\&Phi;}}_L& 1& 0\\ -{\mathrm{d\&Phi;}}_u& 0& 1\end{array}\right]d{C}_o^n$

ΔS _i=(1-dSF)ΔS _i

So between two monumented points, carry out iterative computation, until the radial position error at this monumented point place is less than 0.01m.Then carry out next monumented point correction.

Advantage of the present invention is, inertial navigation odometer Combinated navigation method for a kind of pipeline survey field has been proposed, the method recycles global test data, by forward and reverse associating Navigation, every error of inertial navigation system and odometer is estimated and compensated, finally in conjunction with monumented point positional information, track is further revised, finally obtained high-precision pipeline track data.

Accompanying drawing explanation

Fig. 1 is a kind of pipeline mapping inertial navigation odometer Combinated navigation method process flow diagram provided by the present invention.

Embodiment

Below in conjunction with the drawings and specific embodiments, the present invention is described in detail:

First utilize the positional information of initial point initially to set, then carry out the navigation calculation of forward, dead reckoning and Kalman filtering, in whole filtering to three velocity errors, three misalignments carry out Closed-cycle correction, when being calculated to end of data place, proceed reverse navigation calculation, dead reckoning and Kalman filtering, after completing once complete forward and oppositely combining navigation and filtering, following error is once revised, comprise: the gyroscopic drift of inertial navigation system, accelerometer bias, odometer site error, the scale coefficient error of odometer alignment error angle and odometer, then proceed navigation and the filtering of forward, and preserve inertial navigation system attitude matrix, the data such as odometer displacement increment and integrated navigation result, finally, the track obtaining in conjunction with the navigation of monumented point data pair combinations is further revised.

The error model of model inertial navigation system and odometer, then utilize the omnidistance data of pipe detection to carry out positive navigation and the dead reckoning of order, carry out Kalman filtering simultaneously and estimate each error term, after being calculated to the end of data, carry out reverse navigation and dead reckoning, Kalman wave filter continues estimation of error, wave filter is not restarted during this time, until data starting position, then carry out compensation and the correction of each error term, carry out again navigation and the filtering of forward, now because each main error all compensates, to obtain relatively high-precision track data, finally also can to the track of gained, further revise in conjunction with monumented point information.

An inertial navigation odometer Combinated navigation method for pipeline mapping, it comprises the steps:

1. error model

Inertial navigation odometer Integrated Navigation Algorithm adopts " speed+position " coupling Kalman filters solutions, and this scheme adopts 19 rank navigation error models, chooses 19 error state variablees to be

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\&delta;V}}_n& {\mathrm{\&delta;V}}_u& \mathrm{\&delta;}{V}_e& {\mathrm{\&Phi;}}_n& {\mathrm{\&Phi;}}_u& {\mathrm{\&Phi;}}_e& {\mathrm{\&delta;L}}_D& {\mathrm{\&delta;h}}_D& {\mathrm{\&delta;\&lambda;}}_D& {\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& {\mathrm{\&Theta;}}_Y& {\mathrm{\&Theta;}}_Z& \mathrm{\&Delta;SF}& \mathrm{\&Delta;t}\end{array}\right]$

State equation is:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}$

Wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\&times;3}& C_b^n& 0_{3\&times;3}& 0_{3\&times;4}\\ A_3& A_4& 0_{3\&times;3}& 0_{3\&times;3}& -C_b^n& 0_{3\&times;4}\\ 0_{3\&times;3}& A_5& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& A_6\\ & & & & & \\ & & 0_{10\&times;19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_n/R_M& -2(V_e\tan L/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)\\ {2V}_n/R_M& 0& 2(V_e/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& -(V_e/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],$ $A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+V_e/R_N\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+V_e\tan L/R_N& -{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{\mathrm{De}}& V_{\mathrm{Du}}\\ V_{\mathrm{De}}& 0& -V_{\mathrm{Dn}}\\ -V_{\mathrm{Du}}& V_{\mathrm{Dn}}& 0\end{array}\right],$ $A_6=\left[\begin{array}{cccc}-C_b^n\left(\mathrm{1,3}\right)V_D& C_b^n\left(\mathrm{1,2}\right)V_D& C_b^n\left(\mathrm{1,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{2,3}\right)V_D& C_b^n\left(\mathrm{2,2}\right)V_D& C_b^n\left(\mathrm{2,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{3,3}\right)V_D& C_b^n\left(\mathrm{3,2}\right)V_D& C_b^n\left(\mathrm{3,1}\right)V_D& 0\end{array}\right]$

δ V wherein _n, δ V _u, δ V _efor inertial navigation north orientation, vertical, east orientation velocity error; Φ _n, Φ _u, Φ _efor inertial navigation north orientation, vertical, east orientation misalignment;

inertial navigation X, Y, Z axis accelerometer bias; ε _x, ε _y, ε _z: the gyroscopic drift of inertial navigation X, Y, Z axis; δ L _d, δ h _d, δ λ _dfor the latitude of odometer dead reckoning, highly, longitude error; Θ _y, Θ _zfor between inertial navigation and odometer along the alignment error angle of inertial navigation Y and Z axis; Δ SF: odometer scale coefficient error; Δ t: be time delay; f _n, f _u, f _ethe specific force that the north day eastern tripartite who measures for inertial navigation system makes progress; V _n, V _u, V _ebe respectively inertial navigation system north orientation, vertical and east orientation speed; ω _iefor earth rotation angular speed; L represents the latitude of inertial navigation system present position; R _n, R _mbe respectively radius of curvature in prime vertical, meridian ellipse intrinsic curvature radius, V _dforward speed for odometer; V _dn, V _du, V _derepresent respectively the north orientation of odometer dead reckoning, vertical, east orientation speed;

representing matrix

element corresponding to the 1st row the 1st row, the implication of all the other same form variablees is identical therewith.

2) observation equation

Wave filter observation equation is divided into two parts, and when having speed observation, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{\mathrm{De}}& -V_{\mathrm{Du}}& & C_b^n\left(\mathrm{1,3}\right)V_D& -C_b^n\left(\mathrm{1,2}\right)V_D& -C_b^n\left(\mathrm{1,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 1& 0& -V_{\mathrm{De}}& 0& V_{\mathrm{Dn}}& 0_{3\&times;9}& C_b^n\left(\mathrm{2,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{2,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 0& 1& V_{\mathrm{Du}}& -V_{\mathrm{Dn}}& 0& & C_b^n\left(\mathrm{3,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{3,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ & & & & & & & & & & \\ & & & & & & 0_{3\&times;19}& & & & \\ & & & & & & & & & & \end{array}\right]X---\left(1\right)$ When having reference point information, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\&times;19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\&times;6}& 0& 1& 0& 0_{3\&times;10}\\ & 0& 0& 1& \end{array}\right]X---\left(2\right)$

the accekeration of the inertial navigation system Department of Geography upgrading for speed while representing navigation calculation respectively; The same joint of all the other each variable-definitions.

3) equation discretize

State-transition matrix discretize formula is as follows

${\mathrm{\&phi;}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\mathrm{\&Sigma;}}}{T}_n{A}_t---\left(3\right)$

Wherein, T _nfor navigation cycle, T _n=0.005s, T _efor filtering cycle, T _e=1s.A _tfor t state-transition matrix constantly, φ _{k+1, k}for the transition matrix after discretize, t=0 when each filtering cycle starts.

Noise battle array discretize formula is:

Q _k=QT _e

Q is noise battle array.

Observed quantity solution formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\&delta;V}}_n=V_n-V_{\mathrm{Dn}}\\ {\mathrm{\&delta;V}}_u=V_u-V_{\mathrm{Du}}\\ {\mathrm{\&delta;V}}_e=V_e-V_{\mathrm{De}}\\ {\mathrm{\&delta;L}}_D=L_D-L_M\\ {\mathrm{\&delta;h}}_D=h_D-h_M\\ {\mathrm{\&delta;\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.---\left(4\right)$

Wherein, λ _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly.The speed of odometer is obtained by following formula:

$\left[\begin{array}{c}{V}_{\mathrm{Dn}}\\ V_{\mathrm{Du}}\\ V_{\mathrm{De}}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\&Delta;S}/\mathrm{dT}\\ 0\\ 0\end{array}\right]---\left(5\right)$

In formula, Δ S is the displacement increment in 0.1s, dT=0.1s.

2. dead reckoning

When carrying out inertial navigation, carry out dead reckoning.Computation period is with the navigation calculation cycle, Tn=5 (ms); The same navigation calculation of dead-reckoning position information initializing, utilizes extraneous bookbinding value to obtain initial longitude, latitude and height.

If the displacement that odometer is exported carrier in the i sampling period is Δ S _i, its being expressed as in inertial navigation carrier coordinate system b system:

$\mathrm{\&Delta;}{S}_i^b={\left[\begin{array}{ccc}\mathrm{\&Delta;}{S}_i& 0& 0\end{array}\right]}^T---\left(6\right)$

Utilize the attitude matrix of inertial navigation system to be transformed under geographic coordinate system:

$\mathrm{\&Delta;}{S}_i^n=C_b^n\mathrm{\&Delta;}{S}_i^b---\left(7\right)$

For eliminating odometer quantizing noise, the 0.1s of take makes smoothing processing to speed as the cycle.

Dead reckoning formula is:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D\left(t\right)={\mathrm{\&lambda;}}_D(t-T_n)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.---\left(8\right)$

the displacement increment that represents respectively odometer Department of Geography in the navigation calculation cycle.

3.Kalman Filtering Model

Kalman filtering equations adopts the form in document < < Kalman filtering and integrated navigation principle > > (first published, Qin Yongyuan etc. write), and concrete formula is as follows:

State one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}---\left(9\right)$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]---\left(10\right)$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}---\left(10\right)$

One-step prediction error covariance matrix

$P_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k,k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T---\left(12\right)$

Estimation error variance battle array

P _k=[I-K _kH _k]P _k,k-1 (13)

Wherein,

be a step status predication value, for state estimation matrix, Φ _{k, k-1}for state Matrix of shifting of a step, H _kfor measurement matrix, Z _kfor measurement amount, K _kfor filter gain matrix, R _kfor observation noise battle array, P _{k, k-1}for one-step prediction error covariance matrix, P _kfor estimation error variance battle array, Γ _{k, k-1}for system noise drives battle array, Q _k-1for system noise acoustic matrix.

4. reverse process

After the navigation and filtering that complete forward sequence, navigation calculation, dead reckoning and Kalman filtering do not stop, but proceed reverse navigation calculation, dead reckoning and Kalman filtering, and main methods comprises:

1) reverse navigation

The computing formula of navigation is with positive navigation, and difference is in computation process, to need some quantity of states to carry out negate, and concrete processing comprises:

H) acceleration of gravity is reverse: g=-g

I) overload oppositely: fb=-fb

J) angular velocity is reverse: wibb=-wibb

K) rotational-angular velocity of the earth is reverse: wien=-wien

L) involve angular velocity reverse: wenn=-wenn

M) position is upgraded oppositely: speed negate

N) time: t=t-Tn

2) oppositely dead reckoning

Oppositely the form of dead reckoning is with the dead reckoning of forward, with the difference of forward dead reckoning be to get negative sign when displacement is cumulative, formula becomes:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D(t-T_n)={\mathrm{\&lambda;}}_D\left(t\right)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.---\left(14\right)$

3) inverse filtering

Inverse filtering flow process is calculated with forward filtering, only need be by all elements negate in state matrix and measurement matrix.

5. monumented point correction

Utilize forward and reverse combine be filtered paired error term estimation and compensation after, can obtain relatively accurate trace information, but because the remainder error of odometer is still cumulative, therefore conventionally need to track, further revise by monumented point, concrete method is as follows.

At monumented point place, calculate the error of odometer dead reckoning:

$\left\{\begin{array}{c}{\mathrm{dL}}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.---\left(15\right)$

λ wherein _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly.

Can obtain $\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{\mathrm{De}}& S_{\mathrm{Dn}}& \\ & S_{\mathrm{Du}}& S_{\mathrm{DL}}\\ -S_{\mathrm{Dn}}& S_{\mathrm{De}}& \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{dL}}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\&lambda;}}_D\end{array}\right]---\left(16\right)$

ΔΦ wherein _u, ΔΦ _l, Δ SF is respectively alignment error angle, the remaining course of odometer, pitching alignment error angle and scale coefficient error; S _dn, S _du, S _de, S _dLbe respectively north orientation that odometer dead reckoning obtains, vertical, east orientation and radial displacement;

After the error parameter estimating, make the following judgment, when radial error is greater than 0.01m,

dS _D=sqrt(dL _D*dL _D+dh _D*dh _D+dλ _D*dλ _D)>0.01m

Time, to calculating the odometer remainder error obtaining, to carry out cumulative sum compensation, and re-start calculating from a upper monumented point, the compensation method of odometer remainder error is as follows:

First error is added up:

$\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]---\left(17\right)$

Then error is compensated:

$d{C}_o^n=\left[\begin{array}{ccc}1& -d{\mathrm{\&Phi;}}_L& d{\mathrm{\&Phi;}}_u\\ d{\mathrm{\&Phi;}}_L& 1& 0\\ -{\mathrm{d\&Phi;}}_u& 0& 1\end{array}\right]d{C}_o^n$

ΔS _i=(1-dSF)ΔS _i (18)

So between two monumented points, carry out iterative computation, until the radial position error at this monumented point place is less than 0.01m.Then carry out next monumented point correction.

Claims (6)

1. an inertial navigation odometer Combinated navigation method for pipeline mapping, is characterized in that: it comprises the steps,

(1) set up computation model,

(2) calculate boat position,

(3) Kalman Filtering Model,

(4) reverse process,

(5) monumented point correction.

2. inertial navigation odometer Combinated navigation method for the mapping of a kind of pipeline as claimed in claim 1, is characterized in that: described step (1) comprises,

1) error model

Inertial navigation odometer Integrated Navigation Algorithm adopts " speed+position " coupling Kalman filtering method, and this scheme adopts 19 rank navigation error models, chooses 19 error state variablees to be

$X=\left[\begin{array}{ccccccccccccccccccc}{\mathrm{\&delta;V}}_n& {\mathrm{\&delta;V}}_u& \mathrm{\&delta;}{V}_e& {\mathrm{\&Phi;}}_n& {\mathrm{\&Phi;}}_u& {\mathrm{\&Phi;}}_e& {\mathrm{\&delta;L}}_D& {\mathrm{\&delta;h}}_D& {\mathrm{\&delta;\&lambda;}}_D& {\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z& {\mathrm{\&epsiv;}}_x& {\mathrm{\&epsiv;}}_y& {\mathrm{\&epsiv;}}_z& {\mathrm{\&Theta;}}_Y& {\mathrm{\&Theta;}}_Z& \mathrm{\&Delta;SF}& \mathrm{\&Delta;t}\end{array}\right]$

State equation is:

$\stackrel{\&CenterDot;}{X}=\mathrm{AX}$

Wherein

$A=\left[\begin{array}{cccccc}{A}_1& A_2& 0_{3\&times;3}& C_b^n& 0_{3\&times;3}& 0_{3\&times;4}\\ A_3& A_4& 0_{3\&times;3}& 0_{3\&times;3}& -C_b^n& 0_{3\&times;4}\\ 0_{3\&times;3}& A_5& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& A_6\\ & & & & & \\ & & 0_{10\&times;19}& & & \\ & & & & & \end{array}\right]$

$A_1=\left[\begin{array}{ccc}-V_u/R_M& -V_n/R_M& -2(V_e\tan L/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L)\\ {2V}_n/R_M& 0& 2(V_e/R_N+{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)\\ V_e\tan L/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& -(V_e/R_N+2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L)& V_n\tan L/R_N-V_u/R_N\end{array}\right]$

$A_2=\left[\begin{array}{ccc}0& -f_e& f_u\\ f_e& 0& -f_n\\ -f_u& f_n& 0\end{array}\right],$ $A_3=\left[\begin{array}{ccc}0& 0& 1/R_N\\ 0& 0& \tan L/R_N\\ -1/R_M& 0& 0\end{array}\right]$

$A_4=\left[\begin{array}{ccc}0& -V_n/R_M& -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L-V_e\tan L/R_N\\ V_n/R_M& 0& {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+V_e/R_N\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+V_e\tan L/R_N& -{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L-V_E/R_N& 0\end{array}\right]$

$A_5=\left[\begin{array}{ccc}0& -V_{\mathrm{De}}& V_{\mathrm{Du}}\\ V_{\mathrm{De}}& 0& -V_{\mathrm{Dn}}\\ -V_{\mathrm{Du}}& V_{\mathrm{Dn}}& 0\end{array}\right],$ $A_6=\left[\begin{array}{cccc}-C_b^n\left(\mathrm{1,3}\right)V_D& C_b^n\left(\mathrm{1,2}\right)V_D& C_b^n\left(\mathrm{1,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{2,3}\right)V_D& C_b^n\left(\mathrm{2,2}\right)V_D& C_b^n\left(\mathrm{2,1}\right)V_D& 0\\ -C_b^n\left(\mathrm{3,3}\right)V_D& C_b^n\left(\mathrm{3,2}\right)V_D& C_b^n\left(\mathrm{3,1}\right)V_D& 0\end{array}\right]$

Wherein, δ V _n, δ V _u, δ V _efor inertial navigation north orientation, vertical, east orientation velocity error; Φ _n, Φ _u, Φ _efor inertial navigation north orientation, vertical, east orientation misalignment;

inertial navigation X, Y, Z axis accelerometer bias; ε _x, ε _y, ε _z: the gyroscopic drift of inertial navigation X, Y, Z axis; δ L _d, δ h _d, δ λ _dfor the latitude of odometer dead reckoning, highly, longitude error; Θ _y, Θ _zfor between inertial navigation and odometer along the alignment error angle of inertial navigation Y and Z axis; Δ SF: odometer scale coefficient error; Δ t: be time delay; f _n, f _u, f _ethe specific force that the north day eastern tripartite who measures for inertial navigation system makes progress; V _n, V _u, V _ebe respectively inertial navigation system north orientation, vertical and east orientation speed; ω _iefor earth rotation angular speed; L represents the latitude of inertial navigation system present position; R _n, R _mbe respectively radius of curvature in prime vertical, meridian ellipse intrinsic curvature radius, V _dforward speed for odometer; V _dn, V _du, V _derepresent respectively the north orientation of odometer dead reckoning, vertical, east orientation speed;

representing matrix

element corresponding to the 1st row the 1st row, the implication of all the other same form variablees is identical therewith;

2) observation equation

Wave filter observation equation is divided into two parts, and when having speed observation, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& V_{\mathrm{De}}& -V_{\mathrm{Du}}& & C_b^n\left(\mathrm{1,3}\right)V_D& -C_b^n\left(\mathrm{1,2}\right)V_D& -C_b^n\left(\mathrm{1,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 1& 0& -V_{\mathrm{De}}& 0& V_{\mathrm{Dn}}& 0_{3\&times;9}& C_b^n\left(\mathrm{2,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{2,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ 0& 0& 1& V_{\mathrm{Du}}& -V_{\mathrm{Dn}}& 0& & C_b^n\left(\mathrm{3,3}\right)V_D& -C_b^n\left(\mathrm{2,2}\right)V_D& -C_b^n\left(\mathrm{3,1}\right)V_D& \stackrel{\&CenterDot;}{V_e}\\ & & & & & & & & & & \\ & & & & & & 0_{3\&times;19}& & & & \\ & & & & & & & & & & \end{array}\right]X$

When having reference point information, observation equation is as follows:

$\left[\begin{array}{c}{\mathrm{\&delta;V}}_n\\ {\mathrm{\&delta;V}}_u\\ {\mathrm{\&delta;V}}_e\\ {\mathrm{\&delta;L}}_D\\ {\mathrm{\&delta;h}}_D\\ {\mathrm{\&delta;\&lambda;}}_D\end{array}\right]=\left[\begin{array}{ccccc}& & & & \\ & & 0_{3\&times;19}& & \\ & & & & \\ & 1& 0& 0& \\ 0_{3\&times;6}& 0& 1& 0& 0_{3\&times;10}\\ & 0& 0& 1& \end{array}\right]X$

the accekeration of the inertial navigation system Department of Geography upgrading for speed while representing navigation calculation respectively; The same joint of all the other each variable-definitions;

3) equation discretize

State-transition matrix discretize formula is as follows

${\mathrm{\&phi;}}_{k+1,k}=I+T_n{A}_{T_n}+T_n{A}_{2T_n}+......+T_n{A}_{T_e}=I+\underset{t=T_n}{\overset{T_e}{\mathrm{\&Sigma;}}}{T}_n{A}_t$

Wherein, T _nfor navigation cycle, T _n=0.005s, T _efor filtering cycle, T _e=1s, A _tfor t state-transition matrix constantly, φ _{k+1, k}for the transition matrix after discretize, t=0 when each filtering cycle starts;

Noise battle array discretize formula is:

Q _k=QT _e

Q is noise battle array,

Observed quantity solution formula is as follows:

$\left\{\begin{array}{c}{\mathrm{\&delta;V}}_n=V_n-V_{\mathrm{Dn}}\\ {\mathrm{\&delta;V}}_u=V_u-V_{\mathrm{Du}}\\ {\mathrm{\&delta;V}}_e=V_e-V_{\mathrm{De}}\\ {\mathrm{\&delta;L}}_D=L_D-L_M\\ {\mathrm{\&delta;h}}_D=h_D-h_M\\ {\mathrm{\&delta;\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.$

Wherein, λ _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly, the speed of odometer is obtained by following formula:

$\left[\begin{array}{c}{V}_{\mathrm{Dn}}\\ V_{\mathrm{Du}}\\ V_{\mathrm{De}}\end{array}\right]=C_b^n\left[\begin{array}{c}\mathrm{\&Delta;S}/\mathrm{dT}\\ 0\\ 0\end{array}\right]$

In formula, Δ S is the displacement increment in 0.1s, dT=0.1s.

3. inertial navigation odometer Combinated navigation method for the mapping of a kind of pipeline as claimed in claim 1, is characterized in that: described step (2) comprises,

When carrying out inertial navigation, carry out dead reckoning, computation period is with the navigation calculation cycle, Tn=5 (ms); The same navigation calculation of dead-reckoning position information initializing, utilizes extraneous bookbinding value to obtain initial longitude, latitude and height,

If the displacement that odometer is exported carrier in the i sampling period is Δ S _i, its being expressed as in inertial navigation carrier coordinate system b system:

$\mathrm{\&Delta;}{S}_i^b={\left[\begin{array}{ccc}\mathrm{\&Delta;}{S}_i& 0& 0\end{array}\right]}^T$

Utilize the attitude matrix of inertial navigation system to be transformed under geographic coordinate system:

$\mathrm{\&Delta;}{S}_i^n=C_b^n\mathrm{\&Delta;}{S}_i^b$

For eliminating odometer quantizing noise, the 0.1s of take makes smoothing processing to speed as the cycle.

Dead reckoning formula is:

$\left\{\begin{array}{c}{L}_D\left(t\right)=L_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D\left(t\right)={\mathrm{\&lambda;}}_D(t-T_n)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D\left(t\right)=h_D(t-T_n)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.$

the displacement increment that represents respectively odometer Department of Geography in the navigation calculation cycle.

4. inertial navigation odometer Combinated navigation method for the mapping of a kind of pipeline as claimed in claim 1, is characterized in that: described step (3) comprises, the formula of Kalman filtering equations is as follows:

State one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$

One-step prediction error covariance matrix

$P_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k,k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T$

Estimation error variance battle array

P _k=[I-K _kH _k]P _k,k-1

Wherein,

be a step status predication value,

for state estimation matrix, Φ _{k, k-1}for state Matrix of shifting of a step, H _kfor measurement matrix, Z _kfor measurement amount, K _kfor filter gain matrix, R _kfor observation noise battle array, P _{k, k-1}for one-step prediction error covariance matrix, P _kfor estimation error variance battle array, Γ _{k, k-1}for system noise drives battle array, Q _k-1for system noise acoustic matrix.

5. inertial navigation odometer Combinated navigation method for the mapping of a kind of pipeline as claimed in claim 1, is characterized in that: described step (4) comprises,

After the navigation and filtering that complete forward sequence, navigation calculation, dead reckoning and Kalman filtering do not stop, but proceed reverse navigation calculation, dead reckoning and Kalman filtering, and main methods comprises:

1) reverse navigation

The computing formula of navigation is with positive navigation, and difference is in computation process, to need some quantity of states to carry out negate, and concrete processing comprises:

A) acceleration of gravity is reverse: g=-g

B) overload oppositely: fb=-fb

C) angular velocity is reverse: wibb=-wibb

D) rotational-angular velocity of the earth is reverse: wien=-wien

E) involve angular velocity reverse: wenn=-wenn

F) position is upgraded oppositely: speed negate

G) time: t=t-Tn

2) oppositely dead reckoning

Oppositely the form of dead reckoning is with the dead reckoning of forward, with the difference of forward dead reckoning be to get negative sign when displacement is cumulative, formula becomes:

$\left\{\begin{array}{c}{L}_D(t-T_n)=L_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iN}}^n/R_M\\ {\mathrm{\&lambda;}}_D(t-T_n)={\mathrm{\&lambda;}}_D\left(t\right)+\left(\mathrm{\&Delta;}{S}_{\mathrm{iE}}^n\sec L\right)/R_N\\ h_D(t-T_n)=h_D\left(t\right)+\mathrm{\&Delta;}{S}_{\mathrm{iU}}^n\end{array}\right.$

3) inverse filtering

Inverse filtering flow process is calculated with forward filtering, only need be by all elements negate in state matrix and measurement matrix.

6. a kind of pipeline as claimed in claim 1 is surveyed and drawn with inertial navigation odometer Combinated navigation method, it is characterized in that: described step (5) comprises, utilize forward and reverse combine be filtered paired error term estimation and compensation after, can obtain relatively accurate trace information, but because the remainder error of odometer is still cumulative, therefore conventionally need to track, further revise by monumented point, concrete method is as follows:

At monumented point place, calculate the error of odometer dead reckoning:

$\left\{\begin{array}{c}{\mathrm{dL}}_D=L_D-L_M\\ {\mathrm{dh}}_D=h_D-h_M\\ {\mathrm{d\&lambda;}}_D={\mathrm{\&lambda;}}_D-{\mathrm{\&lambda;}}_M\end{array}\right.$

λ wherein _d, L _d, h _dfor the longitude of odometer dead reckoning, latitude, highly; λ _m, L _m, h _mfor the longitude at monumented point place, latitude, highly.

Can obtain

$\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]={\left[\begin{array}{ccc}{S}_{\mathrm{De}}& S_{\mathrm{Dn}}& \\ & S_{\mathrm{Du}}& S_{\mathrm{DL}}\\ -S_{\mathrm{Dn}}& S_{\mathrm{De}}& \end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{dL}}_D\\ {\mathrm{dh}}_D\\ {\mathrm{d\&lambda;}}_D\end{array}\right]$

ΔΦ wherein _u, ΔΦ _l, Δ SF is respectively alignment error angle, the remaining course of odometer, pitching alignment error angle and scale coefficient error; S _dn, S _du, S _de, S _dLbe respectively north orientation that odometer dead reckoning obtains, vertical, east orientation and radial displacement;

After the error parameter estimating, make the following judgment, when radial error is greater than 0.01m,

DS _d=sqrt (dL _d* dL _d+ dh _d* dh _d+ d λ _d* d λ _d) during >0.01m, to calculating the odometer remainder error obtaining, carry out cumulative sum compensation, and re-start calculating from a upper monumented point, the compensation method of odometer remainder error is as follows:

First error is added up:

$\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]=\left[\begin{array}{c}{\mathrm{d\&Phi;}}_u\\ \mathrm{dSF}\\ {\mathrm{d\&Phi;}}_L\end{array}\right]+\left[\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&Phi;}}_u\\ \mathrm{\&Delta;SF}\\ \mathrm{\&Delta;}{\mathrm{\&Phi;}}_L\end{array}\right]$

Then error is compensated:

$d{C}_o^n=\left[\begin{array}{ccc}1& -d{\mathrm{\&Phi;}}_L& d{\mathrm{\&Phi;}}_u\\ d{\mathrm{\&Phi;}}_L& 1& 0\\ -{\mathrm{d\&Phi;}}_u& 0& 1\end{array}\right]d{C}_o^n$

ΔS _i=(1-dSF)ΔS _i

So between two monumented points, carry out iterative computation, until the radial position error at this monumented point place is less than 0.01m.Then carry out next monumented point correction.

Images