CN106507913B - Combined positioning method for pipeline mapping - Google Patents

Abstract

The invention belongs to field of inertia technology, and in particular to a kind of combined positioning method for pipeline mapping.Its feature is：Using inertia and speedometer Combinated navigation method, course angle is modified using dead reckoning and accurately known position road sign point, in solution low precision inertial navigation system high-precision fixed to a difficult problem.Navigation interative computation is combined to Kalman filtering with smooth fixed-interval smoother backward using front, improve integrated navigation and location precision afterwards.

Description

The combined positioning method surveyed and drawn for pipeline

Technical field

The present invention relates to field of inertia technology, and in particular to a kind of to determine for the combination that pipeline is surveyed and drawn Position method.

Background technology

Because pipeline is typically laid on ground end, it is difficult to by effective high-precision positioner to it Particular location and pipeline track carry out accurate measurement.At present, China is in pipeline survey field Correlative measurement method achievement in research is less, most using external measurement of correlation in engineer applied Equipment and processing software.

The content of the invention

It is an object of the invention to provide a kind of inexpensive, high-precision group surveyed and drawn for pipeline Close localization method.

Realize the technical scheme of the object of the invention：

A kind of combined positioning method surveyed and drawn for pipeline, it is characterised in that：Comprise the following steps：

The first step, obtains sampled data：Inertial navigation system obtains gyro, accelerometer sampled data； Odometer obtains displacement sampled data；Obtain pipeline coordinate reference points；

Second step, sets inertial navigation system sampling period, odometer sampling period, Kalman respectively Filtering cycle；

3rd step, in initial coordinate reference point, appearance is carried out using initial Alignment of Inertial Navigation System result State parameter, location parameter, speed parameter, and Kalman filtering parameter initialization；

4th step, reads inertial navigation system gyro, accelerometer sampled data, according to setting inertial navigation System communication cycle carries out navigation calculating, real-time update inertial navigation attitude, orientation, speed, position Parameter；

5th step, according to navigation result of calculation, carries out dead reckoning, calculates dead reckoning warp Degree, latitude, elevation information；

6th step, calculate when navigation, dead reckoning to it is preceding to the moment in Kalman filtering cycle when, Kalman filtering calculating is carried out, filtering preserves optimal filter estimation parameter, one-step prediction after terminating Parameter, optimal covariance matrix, one-step prediction covariance matrix are estimated, using filter result to inertial navigation Horizontal attitude, speed, site error are modified, and position correction is carried out to dead reckoning result；

7th step, repeats the 4th step to the 6th step, until receiving next pipeline coordinate reference points, Using pipeline coordinate reference points position and dead-reckoning position, calculate inertial navigation azimuthal misalignment angle and Odometer scale coefficient error；

8th step, parameter, one-step prediction estimation are estimated according to the optimal filter obtained in the 6th step Parameter, optimal covariance matrix, one-step prediction covariance matrix, carry out reverse smoothing computation, utilize The gyroscopic drift of smoothing computation modified result, accelerometer zero, horizontal attitude；

9th step, the inertial navigation azimuthal misalignment angle calculated using the 7th step and odometer calibration factor Error, the inertial navigation orientation to the initial coordinate reference point moment is modified, to odometer scale system Number error is modified, initialization navigation and Kalman filtering；

Tenth step, repeats the 4th step and is calculated to the 9th stepping row iteration, iterations reaches 3 times Afterwards, the mapped results between two road sign points are obtained；

11st step, repeats the 4th step to the tenth step, obtains the survey between two neighboring road sign point Result is painted, so on up to last road sign point, whole pipeline mapping is completed.

A kind of combined positioning method surveyed and drawn for pipeline as described above, the 5th step is to utilize to lead Computing formula of navigating (1) carries out dead reckoning, calculates dead reckoning longitude, latitude, height Information：

Wherein,It is car body displacement in j-th of sampling period in gyro Projected in coordinate system,For j moment attitude matrixs,For car in j-th of sampling period Displacement body is projected in navigational coordinate system,Respectively j-th sampling week North orientation, day are to, east orientation car body displacement in phase, and T is the odometer sampling period,For boat position Calculate latitude, h_DFor dead reckoning highly, λ_DFor dead reckoning longitude.R_MEnclosed the land for meridian The radius of a ball, R_NFor prime vertical earth radius.

It is to utilize in a kind of combined positioning method surveyed and drawn for pipeline as described above, the 6th step Formula (2), (3) carry out Kalman filtering calculating：

Wherein, H=[I_3×3 0_3×12 -I_3×3]^T, γ is measurement noise；

δRⁿFor three-dimensional position error (latitude error, Height error, longitude error), δ VⁿFor three-dimensional velocity error (north speed error, the fast error in day, Eastern speed error), φⁿFor north, day, east orientation misalignment, ε^bFor x, y, z gyroscopic drifts,For X, y, z accelerometer zero.Ф_INS(k+1 k) is discrete state transfer matrix, Г_INS(k) it is Discrete system noise coefficient battle array, I is 15 dimension unit matrixs, T_fFor filtering cycle, F is continuous State-transition matrix, w_INS(k) it is system noise acoustic matrix；

For three-dimensional dead-reckoning position error (dead reckoning latitude error, Dead reckoning height error, dead reckoning longitude error)；

Wherein：0_3x3Null matrix, 0 are tieed up for 3x3₃For 3-dimensional null matrix, w_d3For x, y, z plus Speedometer random noise, w_g3For x, y, z Gyro Random noise.V_N, V_U, V_ERespectively Inertial navigation north speed, day speed, east speed, ω_ieFor earth rotation angular speed,For local latitude.

It is to utilize in a kind of combined positioning method surveyed and drawn for pipeline as described above, the 7th step Formula (4) calculates inertial navigation azimuthal misalignment angle θ_YWith odometer scale coefficient error δ k_D：

δk_D=S_D/S_R- 1, θ_Y=S_D×S_R/|S_D||S_R| (4)

δk_DFor odometer scale coefficient error, θ_YFor azimuthal misalignment angle.S_DFor dead reckoning position Move, S_RIt is starting point road sign point and termination road sign point displacement.

A kind of combined positioning method surveyed and drawn for pipeline as described above, is utilized formula (5) Carry out reverse smoothing computation.

Effect of the invention is that：Using inertia and odometer Combinated navigation method, position of navigating is utilized Calculate and accurately known position road sign point is modified to course angle, low precision inertial navigation system in solution System it is high-precision fixed to problem.Using preceding to Kalman filtering and backward smooth fixed strike Algorithm is combined navigation interative computation, improves integrated navigation and location precision afterwards.

Brief description of the drawings

Fig. 1 is a kind of pipeline mapping combined positioning method schematic diagram provided by the present invention；

Fig. 2 is pipeline mapping track result schematic diagram in embodiment；

Fig. 3 is combined altitudes and GPS degree of contrast curve maps in embodiment；

Fig. 4 is combination longitude, latitude error curve map in embodiment；

Fig. 5 is combined altitudes error curve diagram in embodiment.

Embodiment

The invention will be further described with reference to the accompanying drawings and examples.

A kind of pipeline mapping combined positioning method, comprises the following steps：

The first step, obtains sampled data：Inertial navigation system obtains gyro, accelerometer sampled data； Odometer obtains displacement sampled data；Pipeline coordinate reference points are obtained, as shown in table 1：

Table 1

Second step, sets inertial navigation system, odometer sampling period, Kalman filtering cycle respectively.

3rd step, in initial road sign reference point, appearance is carried out using initial Alignment of Inertial Navigation System result State parameter, location parameter, speed parameter, and Kalman filtering parameter initialization.

4th step, reads inertial navigation system gyro, accelerometer sampled data, according to setting inertial navigation System communication cycle carries out navigation calculating, real-time update inertial navigation attitude, orientation, speed, position Parameter.

5th step, according to navigation result of calculation, boat position is carried out using navigation computing formula (1) Calculate, calculate dead reckoning longitude, latitude, elevation information；

Wherein, T is odometer sampling period, Δ S_jxFor car body displacement in j-th of sampling period,Projected for car body displacement in j-th of sampling period in gyro coordinate system,During for j Carve attitude matrix,Projected for car body displacement in j-th of sampling period in navigational coordinate system,North orientation, day are to, east orientation car body position in respectively j-th sampling period Move,For dead reckoning latitude, h_DFor dead reckoning highly, λ_DFor dead reckoning longitude.R_M For meridian circle earth radius, R_NFor prime vertical earth radius.

6th step, navigation is calculated, dead reckoning, to the moment in Kalman filtering cycle, is utilized before Formula (2), (3) carry out Kalman filtering calculating, and filtering preserves optimal filter estimation after terminating Parameter, one-step prediction estimation parameter, optimal covariance matrix, one-step prediction covariance matrix, is utilized Filter result is modified to inertial navigation horizontal attitude, speed, site error, to dead reckoning knot Fruit carries out position correction.

Wherein, H=[I_3×3 0_3×12 -I_3×3]^T, γ is measurement noise；

For three-dimensional dead-reckoning position error (dead reckoning latitude error, Dead reckoning height error, dead reckoning longitude error)；

δRⁿFor three-dimensional position error (latitude error, Height error, longitude error), δ VⁿFor three-dimensional velocity error (north speed error, the fast error in day, Eastern speed error), φⁿFor north, day, east orientation misalignment, ε^bFor x, y, z gyroscopic drifts,For X, y, z accelerometer zero.Ф_INS(k+1 k) is discrete state transfer matrix, Г_INS(k) it is Discrete system noise coefficient battle array, I is 15 dimension unit matrixs, T_fFor filtering cycle, F is continuous State-transition matrix, W_INS(k) it is system noise acoustic matrix；

Wherein：0_3x3Null matrix, 0 are tieed up for 3x3₃For 3-dimensional null matrix, w_a3For x, y, z plus Speedometer random noise, w_g3For x, y, z Gyro Random noise.V_N, V_U, V_ERespectively Inertial navigation north speed, day speed, east speed, ω_ieFor earth rotation angular speed,For local latitude.

7th step, the step of repeat step the 4th, the 5th step and the 6th step, until receiving next pipe Road coordinate reference points, using pipeline coordinate reference points position and dead-reckoning position, utilize public affairs Formula (4) calculates inertial navigation azimuthal misalignment angle θ_YWith odometer scale coefficient error δ k_D。

δk_D=S_D/S_R- 1, θ_Y=S_D×S_R/|S_D||S_R| (4)

δk_DFor odometer scale coefficient error, θ_YFor azimuthal misalignment angle.S_DFor dead reckoning position Move, S_RIt is starting point road sign point and termination road sign point displacement.

8th step, parameter, one-step prediction estimation are estimated according to the optimal filter obtained in the 6th step Parameter, optimal covariance matrix, one-step prediction covariance matrix, are carried out reverse using formula (5) Smoothing computation, utilizes the gyroscopic drift of smoothing computation modified result, accelerometer zero, horizontal appearance State；

9th step, the inertial navigation azimuthal misalignment angle calculated using the 7th step and odometer calibration factor Error is modified to the inertial navigation orientation at initial road sign reference point moment, to odometer calibration factor Error is modified, initialization navigation and Kalman filtering.

Tenth step, repeats the 4th step and is calculated to the 9th stepping row iteration, iterations reaches 3 times Afterwards, the mapped results between two road sign points are obtained.

11st step, repeats the 4th step to the tenth step, obtains the survey between two neighboring road sign point Paint result, so on up to last road sign point, complete whole pipeline mapping, obtain as Pipeline mapping track shown in Fig. 2.

Fig. 3 is combined altitudes and GPS degree of contrast curves, and Fig. 4 is combination longitude, latitude Error curve, Fig. 5 is combined altitudes error curve.From Fig. 3,4 and Fig. 5, it can see Go out and pipeline mapping is carried out using point method, reaction ratio of precision is higher.

The SINS and mileage gauge of low precision are combined in the present embodiment utilization, are passed through The mode handled afterwards completes the accurate mapping to underground piping, positioning precision 1 meter with Interior, wherein longitude error is 0.75 meter, and latitude error is 0.83 meter, and height error is 0.57 Rice.The present invention can be achieved with the high accuracy of pipeline mapping without high accuracy inertial navigation system, reduce Cost.

The operation principle of this method is as shown in Figure 1：Inertial navigation system utilizes gyro, acceleration Count information and complete navigation calculating, utilize navigate result of calculation and odometer displacement parameter progress boat position Calculate and calculate, obtain dead-reckoning position；Joined using inertial navigation location parameter and dead-reckoning position The difference of number, to Kalman filtering observed quantity, is calculated as preceding before carrying out to filtering, and to inertial navigation system System and odometer error carry out Real-time Feedback compensation；When receiving road sign reference point, before utilization Result is preserved to filtering and carries out backward smoothing processing, using sharpening result to inertial navigation system and mileage Meter is modified, and utilizes road sign reference point precise position information, by road sign point correction algorithm, Inertial navigation system orientation and odometer scale coefficient error are modified.

The magnetic absolute altitude precision location information of set a distance can be provided in pipeline mapping, it is possible to use should Information combination odometer dead reckoning carries out course and position correction to inertial navigation system.Meanwhile, pipe Road mapping can further improve positioning precision by handling afterwards, so that complete to pipeline track Into accurate measurement.Achievement of the present invention proposes one kind and utilizes Low-cost SINS and mileage first The integrated navigation processing method afterwards being combined is counted, one kind is provided for China's pipeline survey field Effectively high accuracy duct survey handles combined navigation locating method afterwards.

Obviously, those skilled in the art can to the present invention carry out it is various change and modification without Depart from the spirit and scope of the present invention.If these modifications and modification belong to the claims in the present invention And its within the scope of equivalent technologies, then the present invention is also intended to exist comprising these changes and modification It is interior.

Claims (5)

1. a kind of combined positioning method surveyed and drawn for pipeline, it is characterised in that：Including following Step：

The first step, obtains sampled data：Inertial navigation system obtains gyro, accelerometer sampled data； Odometer obtains displacement sampled data；Obtain pipeline coordinate reference points；

Second step, sets inertial navigation system sampling period, odometer sampling period, Kalman respectively Filtering cycle；

3rd step, in initial coordinate reference point, appearance is carried out using initial Alignment of Inertial Navigation System result State parameter, location parameter, speed parameter, and Kalman filtering parameter initialization；

4th step, reads inertial navigation system gyro, accelerometer sampled data, according to setting inertial navigation System communication cycle carries out navigation calculating, real-time update inertial navigation attitude, orientation, speed, position Parameter；

5th step, according to navigation result of calculation, carries out dead reckoning, calculates dead reckoning warp Degree, latitude, elevation information；

6th step, calculate when navigation, dead reckoning to it is preceding to the moment in Kalman filtering cycle when, Kalman filtering calculating is carried out, filtering preserves optimal filter estimation parameter, one-step prediction after terminating Parameter, optimal covariance matrix, one-step prediction covariance matrix are estimated, using filter result to inertial navigation Horizontal attitude, speed, site error are modified, and position correction is carried out to dead reckoning result；

7th step, repeats the 4th step to the 6th step, until receiving next pipeline coordinate reference points, Using pipeline coordinate reference points position and dead-reckoning position, calculate inertial navigation azimuthal misalignment angle and Odometer scale coefficient error；

8th step, parameter, one-step prediction estimation are estimated according to the optimal filter obtained in the 6th step Parameter, optimal covariance matrix, one-step prediction covariance matrix, carry out reverse smoothing computation, utilize The gyroscopic drift of smoothing computation modified result, accelerometer zero, horizontal attitude；

9th step, the inertial navigation azimuthal misalignment angle calculated using the 7th step and odometer calibration factor Error, the inertial navigation orientation to the initial coordinate reference point moment is modified, to odometer scale system Number error is modified, initialization navigation and Kalman filtering；

Tenth step, repeats the 4th step and is calculated to the 9th stepping row iteration, iterations reaches 3 times Afterwards, the mapped results between two road sign points are obtained；

11st step, repeats the 4th step to the tenth step, obtains the survey between two neighboring road sign point Result is painted, so on up to last road sign point, whole pipeline mapping is completed.

2. according to described in claim 1 it is a kind of for pipeline survey and draw combined positioning method, its It is characterised by：5th step is to carry out dead reckoning using navigation computing formula (1), is calculated Dead reckoning longitude, latitude, elevation information：

Wherein,It is car body displacement in j-th of sampling period in gyro Projected in coordinate system,For j moment attitude matrixs,For car in j-th of sampling period Displacement body is projected in navigational coordinate system,Respectively j-th sampling week North orientation, day are to, east orientation car body displacement in phase, and T is the odometer sampling period,For boat position Calculate latitude, h_DFor dead reckoning highly, λ_DFor dead reckoning longitude, R_MEnclosed the land for meridian The radius of a ball, R_NFor prime vertical earth radius.

3. according to described in claim 1 it is a kind of for pipeline survey and draw combined positioning method, its It is characterised by：It is to carry out Kalman filtering calculating using formula (2), (3) in 6th step：

$\left\{\begin{array}{c}X(k+1)=\mathrm{\Φ}(k+1,k)X\left(k\right)+\mathrm{\Γ}\left(k\right)w\left(k\right)\\ Z\left(k\right)=HX\left(k\right)+\mathrm{\γ}\left(k\right)\end{array}\right.---\left(2\right)$

Wherein, H=[I_3×3 0_3×12 -I_3×3]^T, γ is measurement noise；

δRⁿFor three-dimensional position error (latitude error, Height error, longitude error), δ VⁿFor three-dimensional velocity error (north speed error, the fast error in day, Eastern speed error), φⁿFor north, day, east orientation misalignment, ε^bFor x, y, z gyroscopic drifts,For X, y, z accelerometer zero, Φ_INS(k+1 k) is discrete state transfer matrix, Г_INS(k) it is Discrete system noise coefficient battle array, I is 15 dimension unit matrixs, T_fFor filtering cycle, F is continuous State-transition matrix, w_INS(k) it is system noise acoustic matrix；

For three-dimensional dead-reckoning position error (dead reckoning latitude error, Dead reckoning height error, dead reckoning longitude error)；

${\mathrm{\Γ}}_{INS}\left(k\right)={\left[\begin{array}{ccccc}{0}_{3x5}& 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}\\ 0_{3x3}& C_b^n\left(k\right)& 0_{3x3}& 0_{3x3}& 0_{3x3}\\ 0_{3x3}& 0_{3x3}& C_b^n\left(k\right)& 0_{3x3}& 0_{3x3}\\ 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{5x5}& 0_{3x3}\\ 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{5x5}\end{array}\right]}_{T_f},F=\left[\begin{array}{ccccc}{F}_{11}& F_{12}& 0_{3x3}& 0_{3x3}& 0_{3x3}\\ F_{21}& F_{22}& F_{23}& 0_{3x3}& C_b^n\\ F_{31}& F_{32}& F_{33}& C_b^n& 0_{3x3}\\ 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}\\ 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}\end{array}\right]$

w_INS(k)=[0₃ w_a3 w_g3 0₃ 0₃]^T

Wherein：0_3x3Null matrix, 0 are tieed up for 3x3₃For 3-dimensional null matrix, w_a3For x, y, z plus Speedometer random noise, w_g3For x, y, z Gyro Random noise, V_N, V_U, V_ERespectively Inertial navigation north speed, day speed, east speed, ω_ieFor earth rotation angular speed,For local latitude；

$\left\{\begin{array}{c}{P}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k.k-1}^T+Q\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}\\ {\hat{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{\mathrm{RK}}_k^T\end{array}\right.---\left(3\right).$

4. according to described in claim 1 it is a kind of for pipeline survey and draw combined positioning method, its It is characterised by：It is to calculate inertial navigation azimuthal misalignment angle θ using formula (4) in 7th step_YAnd mileage Count scale coefficient error δ k_D：

δk_S=S_D/S_R ^-1, θ_Y=S_D×S_R/|S_D||S_R| (4)

δk_DFor odometer scale coefficient error, θ_YFor azimuthal misalignment angle, S_DFor dead reckoning position Move, S_RIt is starting point road sign point and termination road sign point displacement.

5. according to described in claim 1 it is a kind of for pipeline survey and draw combined positioning method, its It is characterised by：Reverse smoothing computation is carried out using formula (5)：

$\left\{\begin{array}{c}{A}_{k-1}^p=P_k{\left({\mathrm{\Φ}}_{k,k-1}\right)}^T{P}_{k,k-1}^{-1}\\ {\hat{X}}_{k-1}^p={\hat{X}}_{k-1}+A_{k-1}^p({\hat{X}}_k^p-{\mathrm{\Φ}}_{k,k-1}{X}_{k-1})\\ P_{k-1}^p=P_{k-1}+A_{k-1}^p(P_k^p-P_{k,k-1}){\left(A_{k-1}^p\right)}^T\end{array}\right.---\left(5\right).$