A 3D Trajectory Measurement Method for Underground Pipelines Based on an Integrated Navigation System

Abstract

The present invention relates to a method for measuring the three-dimensional trajectory of underground pipelines based on an integrated navigation system, belonging to the field of trenchless technology, and solves the problem of poor measurement accuracy of the three-dimensional trajectory of underground pipelines in the prior art. The steps of the present invention include: Step 1: Establish an error model of the MEMS gyroscope and accelerometer; Step 2: Establish error equations for velocity, position, and attitude; Step 3: Determine the state variables and state equations according to the motion characteristics of the pipeline measuring instrument, the MEMS sensor error model, and the velocity, position, and attitude error equations; Step 4: Use the MEMS magnetometer, odometer, and quasi-zero velocity information as external measurements to establish a measurement equation for the Kalman filter; Step 5: Discretize the Kalman filter continuous system; Step 6: Solve the Kalman filter equation, compensate the output data of the gyroscope and accelerometer according to the solution results, and correct the velocity, attitude, and position results at the same time. The present invention improves the measurement accuracy of the three-dimensional trajectory of the pipeline.

Description

Technical Field

[0001] The present invention relates to the field of trenchless technology, and particularly to a three-dimensional trajectory measurement method for underground pipelines based on a combined navigation system. Background Art

[0002] Underground pipelines refer to various pipelines buried underground in cities and towns, which undertake the transmission work of water supply, gas, heat, etc. in cities and towns. Mastering the distribution of underground pipelines is particularly important for reducing property losses and protecting the lives and safety of the masses, which requires drawing accurate three-dimensional trajectories of underground pipelines.

[0003] Currently, the detection methods for underground pipelines mainly include ground penetrating radar detection method, resistivity method, magnetic gradient method, infrared radiation method, etc. However, these methods all have some disadvantages in terms of measurement range and usage conditions: (1) The measurement error increases with the increase of depth, and it is generally only applicable to measuring pipelines with shallow burial depths; (2) When working, electromagnetic signals or current signals are received on the ground, and the measurement results are easily interfered by the environment; (3) It is not applicable to all pipe materials. Some are only applicable to metal pipe materials, and some are only applicable to non-metal pipe materials.

[0004] The inertial pipeline positioning method is a new type of three-dimensional trajectory measurement method for pipelines that has emerged in recent years. It uses accelerometers and gyroscopes as IMUs (Inertial Measurement Units), and uses inertial measurement algorithms to solve the data measured by the accelerometers and gyroscopes, so as to obtain the three-dimensional trajectory of the underground pipeline. During the inertial measurement process, since the inertial measurement algorithm is an integration over time, errors will accumulate over time. Therefore, current pipeline information solutions mostly use IMU fusion odometer dead reckoning information, or use GPS in the pipeline to mark position information for error correction. Currently, the most advanced products in the world are produced by the Belgian company Reduct. For pipelines of several hundred meters, the measurement error of the inertial locator produced by Reduct can be controlled within 10 cm, with very high measurement accuracy. Currently, its selling price is about one million, which is very expensive.

[0005] MEMS (Micro-Electro-Mechanical System) gyroscopes are new types of sensors that have emerged in recent years. Although their accuracy is not as high as that of fiber optic gyroscopes, they have certain advantages in terms of price, volume, and weight compared to fiber optic gyroscopes. This has enabled MEMS gyroscopes to gradually gain a foothold in inertial measurement systems. Currently, most pipeline trajectory calculations use odometers or in-pipeline GPS to mark position information to correct the inertial measurement errors calculated by MEMS IMUs. However, odometers are prone to problems such as idling and slipping, which can affect the accuracy of dead reckoning. Many old pipelines do not have GPS positioning markings inside, and only the starting and ending points of the pipeline can obtain GPS information. When there is no GPS positioning marking, the accuracy of the three-dimensional pipeline trajectory measured only by the odometer will be greatly reduced. Summary of the Invention

[0006] In view of the above analysis, an embodiment of the present invention aims to provide a three-dimensional underground pipeline trajectory measurement method based on a combined navigation system to solve the problem of poor measurement accuracy of existing three-dimensional underground pipeline trajectories.

[0007] The present invention provides a three-dimensional underground pipeline trajectory measurement method based on a combined navigation system. The steps include:

[0008] Step 1: Establish an error model for the MEMS gyroscope and accelerometer;

[0009] Step 2: Establish error equations for velocity, position, and attitude parameters;

[0010] Step 3: Determine the state variables and state equations based on the motion characteristics of the pipeline measuring instrument, the error models of the MEMS gyroscope and accelerometer, and the velocity, position, and attitude error equations;

[0011] Step 4: Use the MEMS magnetometer, odometer, and quasi-zero velocity information as external measurements to establish a measurement equation for the Kalman filter;

[0012] Step 5: Discretize the continuous Kalman filter system;

[0013] Step 6: Solve the Kalman filter equation, and compensate the output data of the gyroscope and accelerometer according to the solution results to obtain attitude and position information.

[0014] Further, in the above Step 1, the gyroscope error model is:

[0015]

[0016] The accelerometer error model is:

[0017]

[0018] Further, in the step 4, quasi-zero speed information, fluxgate information, and odometer information are used as external observables to correct the result of inertial navigation solution.

[0019] Further, in the step 4, the difference between the MEMS-IMU solution speed and the quasi-zero speed constraint is:

[0020]

[0021] Further, in the step 4, the latitude, longitude, and altitude information obtained by updating and solving the odometer information are:

[0022]

[0023] Further, in the step 4, the pitch angle θ, roll angle γ, and azimuth angle are calculated by combining a three-axis accelerometer and a three-axis fluxgate The formula is:

[0024]

[0025] Further, in the step 4, the measurement equation of the pipeline three-dimensional trajectory measurement combination measurement method:

[0026]

[0027] Further, in the step 5, the continuous Kalman filter system is discretized to obtain:

[0028]

[0029] Further, in the step 6, the acceleration data is compensated as:

[0030]

[0031] Further, in the step 6, the gyro data is compensated as:

[0032]

[0033] Compared with the prior art, the present invention can at least achieve one of the following beneficial effects:

[0034] (1) The present invention fuses MEMS-IMU, MEMS magnetometer, odometer, and quasi-zero speed information, and uses Kalman filtering technology to optimally estimate the pipeline three-dimensional trajectory parameters, which are used to compensate for the rapidly diverging height channel of the IMU measurement system over time and the speed parameters, position parameters, etc. that continuously accumulate with integral calculation, thereby improving the measurement accuracy of the pipeline three-dimensional trajectory.

[0035] (2) The present invention adopts a forward and reverse Kalman filtering fusion algorithm, performs reverse calculation on IMU data in reverse chronological order, and still fuses MEMS magnetometers, odometers, and quasi-zero speed information for filtering to obtain the optimal result of reverse measurement. Finally, the filtering results obtained by forward and reverse filtering are fused to further improve the pipeline measurement accuracy;

[0036] (3) The present invention uses low-cost MEMS sensors to improve the pipeline measurement accuracy while reducing costs.

[0037] In the present invention, the above technical solutions can also be combined with each other to achieve more preferred combination schemes. Other features and advantages of the present invention will be described in the subsequent specification, and some advantages can be made obvious from the specification or understood by implementing the present invention. The objectives and other advantages of the present invention can be realized and obtained through the content specifically pointed out in the specification and the drawings. Brief Description of the Drawings

[0038] The drawings are only used for the purpose of showing specific embodiments and are not considered as limiting the present invention. Throughout the drawings, the same reference signs represent the same components.

[0039] Figure 1 It is a schematic diagram of the basic principle of the Kalman filter for a specific embodiment;

[0040] Figure 2 It is a schematic diagram of the calculation principle for updating the dead reckoning position information in a specific embodiment;

[0041] Figure 3 It is a flow chart of the forward and reverse filtering two-way fusion algorithm for a specific embodiment. Detailed Embodiment

[0042] The following will specifically describe the preferred embodiments of the present invention with reference to the drawings. Among them, the drawings form a part of the present invention and are used together with the embodiments of the present invention to explain the principle of the present invention, rather than to limit the scope of the present invention.

[0043] A specific embodiment of the present invention discloses a three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system.

[0044] The state equation and observation equation of the Kalman filter for a discrete linear system can be expressed as:

[0045]

[0046] Z(t) = H(t)X(t) + V(t) (2)

[0047] Where: X(t) is the state variable, Z(t) is the measurement, F(t) is the state transition matrix, H(t) is the observation matrix, G(t) is the process noise transition matrix, W(t) is the process noise, and V(t) is the observation noise.

[0048] Furthermore, discretizing the state equation (1) and the measurement equation (2) gives:

[0049] X k = Φ k,k-1 X k-1 + Γ k-1 W k-1 (3)

[0050] Z k = H k X k + V k (4)

[0051] Where, X k is the n-dimensional state vector (the quantity to be estimated) at time k, Z k is the m-dimensional measurement vector at time k, Φ k,k-1 is the one-step state transition matrix of the system from time k-1 to k (n×n order), H k is the system measurement matrix at time k (m×n order), Γ k-1 is the system noise matrix (n×r order), W k-1 is the system noise at time k-1 (r-dimensional), and V k is the m-dimensional measurement noise at time k.

[0052] The state prediction and estimation equation is:

[0053]

[0054] Where, is the estimated value of Xk-1 at time k-1, is the predicted value from time k-1 to k.

[0055] The variance prediction equation is:

[0056]

[0057] Where, P k-1 is the estimated covariance matrix. Q k-1 is the variance matrix of the system noise.

[0058] The state estimation equation is:

[0059]

[0060] The variance iteration equation:

[0061]

[0062] In the formula, K k is the filtering gain, and R k is the variance matrix of the measurement noise.

[0063] The filtering gain equation is:

[0064]

[0065] The initial conditions are:

[0066]

[0067] The a priori statistics are:

[0068] E[W k = 0, Cov[W k , W j = E[W k W j T = Q k δ kj

[0069] E[V k = 0, Cov[V k , V j = E[V k V j T = R k δ kj

[0070] Cov[W k , V j = E[W k V j T = 0

[0071]

[0072] As Figure 1 shown, it can be seen that the Kalman filter contains two update loops and two filtering loops: time update and measurement update, filtering calculation loop and gain calculation loop. The basic equations of the Kalman filter: state one-step prediction equation, variance prediction equation, state estimation equation, estimated error variance equation, and filtering gain equation. The Kalman filter realizes prediction and correction update estimation through iterative operations.

[0073] Figure 1 In is the state estimated value at time t k-1 ; is is the state Kalman filter estimation of Φ k,k-1 t k-1 Time to t k One-step transfer array at a time; Γ k-1 is the system noise driving array; H k is the measurement array; R k is the measurement noise variance; Q k is the system noise variance matrix; K k is the filter gain; Z k Measurement value; P k-1 is the estimated mean square error matrix; P k,k-1 One-step forecast error variance matrix.

[0074] The steps of the underground pipeline three-dimensional trajectory measurement method based on the integrated navigation system include:

[0075] Step 1: Build the error models for the MEMS gyroscope and accelerometer.

[0076] (1) Gyroscope error model:

[0077]

[0078] The gyro bias ε b The error model can be expressed by the first-order Markov process model equation:

[0079]

[0080] In the above equation, are the ideal values ​​of the x, y, and z axis gyroscopes, respectively; They are the zero bias of the gyroscope x, y, and z axes in the carrier coordinate system; are the x, y, and z axis gyro measurement values ​​respectively; K gx , K gy , K gz are the gyro x, y, and z axis scale factors respectively; δK gx ,δK gy ,δK gz are the gyro x, y, and z axis scale factor errors respectively; M ij W is the misalignment angle of the gyroscope j axis toward i axis; εx , W εy , W εz are the random noises of the gyroscope x, y, and z axes respectively; 1 / β εx , 1 / β εy , 1 / β εz are the correlation times of the random processes along the x, y, and z axes of the gyroscope, respectively.

[0081] (2) Accelerometer error model:

[0082]

[0083] Where the accelerometer zero bias ▽ b The error model of can be expressed by the first-order Markov process model equation:

[0084]

[0085] In the above equation, are the ideal values of the accelerometer in the x, y, and z axes respectively; are the zero biases of the accelerometer in the x, y, and z axes in the vehicle coordinate system respectively; are the measured values of the accelerometer in the x, y, and z axes respectively; K Ax 、K Ay 、K Az are the scale factors of the accelerometer in the x, y, and z axes respectively; δK Ax 、δK Ay 、δK Az are the scale factor errors of the accelerometer in the x, y, and z axes respectively; A ij is the misalignment angle of the accelerometer in the j axis deviating from the i axis; are the random noises of the accelerometer in the x, y, and z axes respectively; are the correlation times of the random noises of the accelerometer in the x, y, and z axes respectively.

[0086] Step 2: Establish the error equations of velocity, position, and attitude parameters.

[0087] (1) Velocity error equation:

[0088]

[0089] Where:

[0090]

[0091] is the attitude transition matrix; v E 、v N 、v U are the velocities in the east, north, and up directions in the navigation calculation coordinate system respectively; δv E 、δv N 、δv U are the velocity errors in the east, north, and up directions in the navigation calculation coordinate system respectively; are the pitch angle, roll angle, and azimuth angle errors in the navigation calculation coordinate system respectively; L is the earth's latitude at the location of the fiber optic inertial navigation; δL is the latitude error at the location of the fiber optic inertial navigation; h is the height at the location of the fiber optic inertial navigation; ω ie is the earth's angular velocity of rotation; are the zero biases of the accelerometer in the x, y, and z axes under the navigation calculation coordinate system; R M is the main curvature radius of the Earth's meridian; R N is the main curvature radius of the Earth's prime vertical.

[0092] (2) Position error equation:

[0093]

[0094] where L, λ, and h are the Earth's latitude, longitude, and depth of the location of the fiber optic inertial navigation respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error respectively.

[0095] (3) Attitude error equation:

[0096]

[0097] where is the error of the tilt angle, is the error of the roll angle, is the error of the azimuth angle; is the Earth's angular rotation rate; is the displacement angular velocity; are the zero biases of the gyroscope in the x, y, and z axes under the navigation coordinate system respectively.

[0098] Step 3: Determine the state variables and state equations according to the motion characteristics of the pipeline measuring instrument, the error models of the MEMS gyroscope and accelerometer, and the velocity, position, and attitude error equations.

[0099] State variables and state equations:

[0100] According to the Kalman filter state equation, the state equation of the integrated navigation algorithm can be expressed by the following formula:

[0101]

[0102] where X(t) is the state variable, F(t) is the state transition matrix, W(t) is the system noise, and G(t) is the system noise transfer matrix.

[0103] (1) Determination of state variables:

[0104] For MEMS-IMU, the error caused by the misalignment angle is much smaller than the error caused by the scale factor. Therefore, the error caused by the misalignment angle is not considered. A in the error model ij (i = x, y, z. j = x, y, z.) and M in the error model ij (i = x, y, z. j = x, y, z.) can be ignored.

[0105] According to the working characteristics of the pipeline trajectory measuring instrument, the instrument basically does not rotate during operation. Therefore, the scale factor errors of the x, y, and z axes are ignored, i.e., δK gx δK gy and δK gz are ignored. The instrument only has a velocity along the y-axis. Therefore, the scale factor errors of the three accelerations in the x and z directions can be ignored, i.e., δK Ax and δK Az can be ignored, while δK Ay cannot be ignored.

[0106] Therefore, for the MEMS-IMU, the δK Ay of the y-axis of the device accelerometer and the zero biases of the three axes of the gyroscope and the zero biases of the three axes of the accelerometer are taken as state variables. At the same time, the three velocity errors, attitude errors, and position errors are also determined as state variables. Therefore, the state variables of the combined inclinometer are:

[0107]

[0108] W is the system noise. According to the error model of the MEMS-IMU device in step 1, it can be determined as:

[0109]

[0110] G is the system noise transfer matrix, G = I 16×16 .

[0111] (2) Determination of the state transition matrix:

[0112] F is the state transition matrix of the integrated navigation algorithm, and it can be expressed as:

[0113]

[0114] where F N corresponds to the system dynamic matrix of the 9 system error parameters of the MEMS-IMU, and it is a 9×9 square matrix. The non-zero elements of the F N matrix are as follows:

[0115] f 2,7 = -ω ie sinL f 4,2 = -f U , f 4,3 = f N , f 5,1 = fU , f 5,3 = -f E , f 6,1 = -f N , f 6,2 = f E , f 6,7 = -2v E ω ie sinL f 9,6 = 1

[0116] F s is as follows:

[0117]

[0118] F ay is as follows:

[0119]

[0120] F noise is as follows:

[0121]

[0122] Step 4: Use the MEMS magnetometer, odometer, and quasi-zero velocity information as external measurements to establish the measurement equation of the Kalman filter.

[0123] The measurement equation of the Kalman filter is shown in Equation (30):

[0124] Z(t) = H(t)X(t) + V(t) (30)

[0125] Where Z(t) is the measurement, H(t) is the measurement transfer matrix, and V(t) is the measurement noise.

[0126] (1) Determination of the measurement

[0127] In integrated navigation, the quasi-zero velocity information, fluxgate information, and odometer information are used as external observables to correct the results of inertial navigation calculation.

[0128] a. Quasi-zero velocity information:

[0129] When the pipeline trajectory measuring instrument is working, the measurement system moves along the axial direction Y, and the moving speeds in the X and Z directions on the interface perpendicular to Y are 0. In actual measurement, due to the existence of vibration interference, the speeds of the X-axis and Z-axis are not absolutely zero. Here, the interference is simplified to white noise:

[0130]

[0131] In the formula, v x , v z are the white noises of the velocities of the X and Z axes in the axial direction of the instrument. is the quasi-zero velocity with white noise of vibration superimposed on the X and Z axes in the axial direction of the instrument.

[0132] The velocity v n in the navigation coordinate system to the velocity v b in the carrier coordinate system is transformed as follows:

[0133]

[0134] Solving Equation (32) gives the velocity error expression in the carrier coordinate system as shown below:

[0135]

[0136] where

[0137] The azimuth relationship between the carrier coordinate system of the pipeline trajectory measuring instrument and the navigation coordinate system is the attitude angle of the instrument, including the azimuth angle A, the tilt angle I, and the tool face angle T. The direction cosine matrix of its coordinate transformation is:

[0138]

[0139] Substituting into Equation (32) gives:

[0140]

[0141] where are the velocities of the instrument in the three axial directions of x, y, and z in the carrier coordinate system. are the velocity errors of the instrument in the three axial directions of x, y, and z in the carrier coordinate system.

[0142] The velocity expression in the carrier coordinate system calculated by MEMS-IMU is Then there is:

[0143]

[0144] Therefore, the difference between the velocity solved by MEMS-IMU and the quasi-zero velocity constraint is:

[0145]

[0146] b. Odometer position information:

[0147] As Figure 2 shown, the position update in trajectory measurement can also be performed by dead reckoning using the distance information measured by the odometer. In the formula And θ are the azimuth angle and tilt angle at position 1, and MD is the distance traveled by the odometer between position 1 and position 2.

[0148] According to trigonometric function operations, the calculation formulas for the eastward displacement increment, northward displacement increment, and vertical displacement increment between two adjacent points are as follows:

[0149]

[0150] The eastward displacement increment, northward displacement increment, and vertical displacement increment can be obtained through Equation (37). Then, the latitude, longitude, and altitude information obtained by updating and calculating the odometer information can be obtained through the following formula.

[0151]

[0152] c. Fluxgate attitude information:

[0153] When measuring the azimuth angle with a fluxgate, a static measurement scheme is mostly adopted. The pitch angle θ, roll angle γ, and azimuth angle are calculated by combining a three-axis accelerometer and a three-axis fluxgate. The formula is:

[0154]

[0155] In the formula, is the measured value of the three-axis fluxgate in the body coordinate system.

[0156] According to the working environment of the pipeline trajectory measuring instrument, the above external information is summarized into three types: a) Based on the odometer distance information, the position information of the pipeline trajectory measuring instrument, namely latitude L, longitude λ, and altitude h, is deduced according to trigonometric functions. b) The pitch angle θ, roll angle γ, and azimuth angle obtained from the geomagnetic information measured by the fluxgate and the earth gravity information measured by the accelerometer. c) The quasi-zero speed information obtained according to the motion form of the pipeline trajectory measuring instrument. Select the difference between the three attitude information solved by MEMS-IMU and the attitude information solved by the fluxgate as the attitude quantity measurement, the difference between the three position information and the position information solved by the odometer as the position quantity measurement, and the quasi-zero degree information of the motion characteristics of the pipeline trajectory measuring instrument as the speed quantity measurement. Then, the measurement quantity Z(t) can be expressed as:

[0157]

[0158] (2) Measurement equation:

[0159] According to the measurement quantity, the measurement equation of the combined measurement method for the three-dimensional pipeline trajectory can be obtained:

[0160] Z(t) == H k X + υ (43)

[0161] Among them, the measurement matrix H k has the following expression

[0162]

[0163] Among them,

[0164]

[0165] Step 5: Discretize the continuous Kalman filter system.

[0166] Discretize the continuous system:

[0167]

[0168] Among them, M1 = Q(t), M i+1 = FM i + M i T F T .

[0169] Step 6: Solve the Kalman filter equation, and compensate the output data of the gyroscope and accelerometer according to the solution results to obtain attitude, velocity, and position information.

[0170] According to the measurement information provided by the MEMS-IMU / magnetometer / odometer, perform Kalman filter information fusion solution, and obtain the optimal or sub-optimal estimate of the inclinometer parameters through state estimation, and correct the inertial navigation in a closed-loop feedback manner.

[0171] (1) Compensate the accelerometer data:

[0172]

[0173] (2) Compensate the gyroscope data:

[0174]

[0175] a) Use the velocity error estimated by the Kalman filter to correct the velocity in a timely manner to obtain velocity information with higher accuracy

[0176]

[0177] b) Use the attitude error estimated by the Kalman filter to correct and compensate the attitude matrix in a timely manner to obtain attitude information with higher accuracy

[0178]

[0179] c) The position error estimated by Kalman filtering is used to correct and compensate the position matrix in a timely manner to obtain position information with higher accuracy.

[0180]

[0181] Step 7: Strapdown inertial navigation forward Kalman filtering.

[0182] The solution formulas for the attitude, velocity, and position of the strapdown inertial navigation in the forward IMU inertial navigation in the forward time sequence can be expressed by the following formula:

[0183]

[0184] Where

[0185]

[0186] g n = [0; 0; -g]

[0187] In the formula V n , L, λ, and h represent the inertial navigation attitude matrix, velocity, latitude, longitude, and altitude respectively; and f b represent the gyro angular velocity measurement and the accelerometer specific force measurement respectively; and g n are the earth's angular rotation rate and the magnitude of the gravitational acceleration respectively; R M and R N are the local earth's meridian and prime vertical radii respectively; (·×) represents the skew-symmetric matrix formed by the vector ·; k is the forward sampling sequence. Assume that the sampling periods of the gyro and accelerometer in the strapdown inertial navigation system are both T S .

[0188] The strapdown inertial navigation forward Kalman filter solution uses the inertial measurement algorithm to calculate the data collected by the MEMS-IMU. At the same time, every 1 s, a Kalman filter is performed according to the Kalman filter model established in steps 1-6. Finally, a set of forward filtered attitude, velocity, position calculation results and the corresponding covariance matrix are obtained.

[0189] Step 8: Strapdown inertial navigation reverse Kalman filtering.

[0190] If the data information collected by the gyroscope and accelerometer during the inertial measurement process is regarded as a set of time series, when performing measurement and calculation, generally, the sampling sequence is processed in real time in the order of time. Generally, data storage is not performed during this process. However, when the computer has sufficient storage capacity and sufficient computing power, it can be considered to store the data information output by the gyroscope and accelerometer. In addition to the forward processing in the order of time, the data can also be analyzed and processed reversely, that is, applying the reverse measurement algorithm to deeply mine the data information.

[0191] Assume that from t0 to t N At the moment, the inertial navigation system navigates from point A to point B. Then, for the reverse navigation from point B to point A, only the terms in Equation (52) need to be transposed. After rearrangement, the reverse navigation algorithm is obtained as follows:

[0192]

[0193] In the formula, r represents reverse, and k gradually decreases from N to 0.

[0194] To obtain the navigation result of the reverse mechanical arrangement and ensure the consistency of the time stamps of the data of each sensor, it is necessary to flip the data after reverse processing from the last epoch to the first epoch. The time stamp of the kth epoch after inversion can be expressed as:

[0195] t p = t1 + t N - t k (54)

[0196] Among them, t p , t k are the backward data time stamp and the forward data time stamp of the kth epoch respectively, and t1 and t N are the time stamps of the first epoch and the last epoch respectively.

[0197] Denote:

[0198]

[0199] After rearrangement, the reverse navigation algorithm can be expressed as:

[0200]

[0201] By comparing Equation (52) and (55), it can be seen that the forward and reverse navigation calculations are basically the same in form. In fact, in the ideal state without calculation errors and device errors, the attitudes and positions at the same moment in the forward navigation calculation process and the reverse navigation calculation process are equal. The only difference is that the directions of the velocity vectors are opposite. Only by taking the opposite of the gyroscope sampling output, the earth's angular rotation rate, and the velocity direction in the forward navigation calculation process can the reverse navigation be achieved.

[0202] The inverse Kalman filter navigation algorithm uses the data collected by the MEMS-IMU to perform inverse-time navigation solution. Every 1 s, a Kalman filter is performed according to the Kalman filter algorithm established in steps 1-6, and finally a set of attitude, velocity, position solution results after inverse filtering and the corresponding covariance matrix are obtained.

[0203] Step 9: Two-way fusion of forward and inverse filtering.

[0204] Two sets of attitude, velocity, position and covariance matrix can be obtained from the forward Kalman filter navigation solution and the inverse Kalman filter navigation solution. Using the covariance matrix stored separately in the forward and inverse directions, the two sets of attitude, velocity and position are weighted and averaged to obtain the optimal estimate.

[0205] The process of two-way fusion of forward and inverse filtering is as follows: First, perform initial alignment on the inertial navigation output recorded within the interval, combine the stored external measurement information to perform forward integrated navigation solution filtering, and then perform backward solution filtering in reverse time. Smooth the navigation output results of the two, and use the smoothed results as the final navigation output. The flow chart is as Figure 3 shown.

[0206] The discretized state equation and measurement equation of forward filtering are:

[0207]

[0208] The discretized state equation and measurement equation of inverse filtering are:

[0209]

[0210] The inverse filtering still uses the MEMS magnetometer, odometer information and quasi-zero speed information as external observables. However, the external observation data also needs to be calculated in reverse time order. It can be seen from equations (56) and (57) that the state variables X, measurement variables Z and measurement transfer matrix H of the state equations of forward and inverse filtering are exactly the same. The difference is only that the inverse state transition matrix is the inverse matrix of the forward state transition matrix .

[0211] During the forward and inverse filtering processes, the optimal state variables X and optimal covariance matrices P obtained from the forward and inverse solutions need to be stored. In the fusion weighted smoothing stage of two-way filtering, according to the root mean square values of the diagonal elements of the matrix P stored in the forward filtering and inverse filtering, the attitude, velocity and position of the carrier are recalculated. The specific calculation methods of the state parameter estimation vector and covariance matrix are as follows:

[0212]

[0213] In the formula is the optimal state quantity and covariance obtained by forward filtering, is the optimal state quantity and covariance obtained by backward filtering, P s,k is the optimal state quantity and covariance obtained after fusing the forward and backward filtering results.

[0214] The present invention aims at the three-dimensional trajectory measurement of underground pipelines, and proposes a method that uses a relatively low-cost MEMS inertial measurement unit, combines the proposed multi-information fusion algorithm with the forward and backward Kalman filtering fusion algorithm to improve the positioning accuracy. First, the MEMS gyroscope and the MEMS accelerometer are used for inertial measurement and calculation. At the same time, the Kalman filtering method is adopted to fuse the MEMS magnetometer, odometer information and quasi-zero velocity constraint information to constrain and correct the calculation result of the IMU. Then all the data are stored. In reverse order of time, the result at the final moment of forward multi-information fusion is used as the reverse initial information. Based on the reverse measurement algorithm, the MEMS magnetometer, odometer information and quasi-zero velocity constraint information are fused again to perform a reverse calculation and filtering on the data. Finally, the forward and reverse filtering results are fused to obtain the optimal pipeline measurement result.

[0215] The present invention improves the measurement accuracy of MEMS-IMU in the field of pipeline three-dimensional trajectory measurement, solves the problem that the inertial measurement error diverges with time when there is no GPS signal annotation in old pipelines, and solves the problem that the existing pipeline measurement instruments are costly; the present invention can provide high-precision measurement and positioning information for underground pipelines of any material and any burial depth, and lays an algorithmic theoretical foundation for manufacturing low-cost and high-precision inertial pipeline locators.

[0216] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any change or replacement that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system, characterized in that the steps Including: Step 1: Establish an error model for the MEMS gyroscope and accelerometer; Step 2: Establish error equations for velocity, position, and attitude parameters; Step 3: Determine the state variables and state equations based on the motion characteristics of the pipeline measuring instrument, the error models of the MEMS gyroscope and accelerometer, and the velocity, position, and attitude error equations; Step 4: Use the MEMS magnetometer, odometer, and quasi-zero velocity information as external measurements to establish a measurement equation for the Kalman filter; In the said Step 4, the quasi-zero velocity information, fluxgate information, and odometer information are used as external observables to correct the results of the inertial navigation solution; In the said Step 4, the difference between the MEMS-IMU solution velocity and the quasi-zero velocity constraint is: where, v x , v z are the white noises of the velocities of the instrument's axial X and Z axes respectively, are the quasi-zero velocities of the instrument's axial X and Z axes with vibration white noise superimposed respectively; A is the azimuth angle of the instrument, I is the inclination angle of the instrument, T is the tool face angle of the instrument, is the velocity of the instrument in the three axial directions x, y, and z of the carrier coordinate system; is the velocity error of the instrument in the three axial directions x, y, and z of the carrier coordinate system; δV N , δV E , δV U are the northward velocity error, eastward velocity error, and upward velocity error in the navigation calculation coordinate system respectively, and δI, δT, and δA are the inclination angle error, tool face angle error, and azimuth angle error of the instrument respectively; Step 5: Discretize the Kalman filter continuous system; Step 6: Solve the Kalman filter equation, and compensate the output data of the gyroscope and accelerometer according to the solution results.

2. The three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system according to claim 1, characterized in that In the said Step 1, the gyroscope error model is: Among them, are the ideal values of the gyroscopes in the x, y, and z axes respectively; are the zero biases of the gyroscopes in the x, y, and z axes in the body coordinate system respectively; are the measured values of the gyroscopes in the x, y, and z axes respectively; K gx 、K gy 、K gz are the scale factors of the gyroscopes in the x, y, and z axes respectively; δK gx 、δK gy 、δK gz are the scale factor errors of the gyroscopes in the x, y, and z axes respectively; M ij is the misalignment angle of the j-axis of the gyroscope deviating from the i-axis; The accelerometer error model is: wherein, are the ideal values of the accelerometer in the x, y, and z axes respectively; are the zero biases of the accelerometer in the x, y, and z axes respectively in the vehicle coordinate system; are the measured values of the accelerometer in the x, y, and z axes respectively; K Ax 、K Ay 、K Az are the scale factors of the accelerometer in the x, y, and z axes respectively; δK Ax 、δK Ay 、δK Az are the scale factor errors of the accelerometer in the x, y, and z axes respectively; A ij is the misalignment angle of the j-axis of the accelerometer deviating from the i-axis.

3. The three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system according to claim 1, characterized in that In the said Step 4, the latitude, longitude, and altitude information updated and solved by the odometer information is: where k is the time series, δh, δN, and δE are the vertical displacement increment, northward displacement increment, and eastward displacement increment calculated by the odometer respectively, L, λ, and h are the earth's latitude, longitude, and altitude where the fiber optic inertial navigation is located, R M and R N are the principal curvature radius of the earth's meridian and the principal curvature radius of the earth's prime vertical respectively.

4. The three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system according to claim 1, characterized in that In the step 4, the pitch angle θ, roll angle γ and azimuth angle are calculated by combining a triaxial accelerometer with a triaxial fluxgate. The formula is as follows: Among them, is the measured value of the three-axis fluxgate in the carrier coordinate system, and g is the acceleration due to gravity.

5. The three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system according to claim 1, characterized in that In the said Step 4, the measurement equation of the combined measurement method for the three-dimensional trajectory measurement of the pipeline: Among them, A INS , θ INS , I INS are respectively the azimuth angle, pitch angle and tool face angle measured by the inertial navigation system; A 磁 , θ 磁 , I 磁 are respectively the azimuth angle, pitch angle and tool face angle measured by the magnetometer and accelerometer; L INS , λ INS , h INS are respectively the latitude, longitude and altitude measured by the inertial navigation system; L MCM , λ MCM , h MCM are respectively the latitude, longitude and altitude measured by the odometer; δL, δλ, δh are respectively the earth latitude error, longitude error and altitude error at the position where the fiber optic inertial navigation is located; X is the n-dimensional state vector, the quantity to be estimated; H k is the system measurement matrix at time k, of order m×n; v is the m-dimensional measurement noise.

6. The three-dimensional trajectory measurement method for underground pipelines based on an integrated navigation system according to claim 1, characterized in that In the said Step 6, the compensation for the accelerometer data is:

7. The three-dimensional trajectory measurement method of underground pipelines based on an integrated navigation system according to claim 1, wherein In the said Step 6, the compensation for the gyro data is:

Images