CN114184209B - Inertial error suppression method for low-speed detection platform system - Google Patents

Abstract

The application provides an inertial error suppression method for a low-speed detection platform system, which comprises the following steps: acquiring mileage output by an odometer under a carrier coordinate system; acquiring the position increment of the odometer under a carrier coordinate system; obtaining an observation matrix according to the observed quantity of the system; acquiring a system state transition matrix; estimating a state variable through a Kalman filtering algorithm, and correcting the mounting error of the odometer based on the estimated state variable; and acquiring lateral accumulated position errors caused by gyro drift after the odometer errors are corrected, acquiring gyro drift equivalent angular velocity based on the lateral accumulated position errors caused by gyro drift, and finishing inertial error correction of the track detection platform system based on the gyro drift equivalent angular velocity. By applying the technical scheme of the application, the technical problem that the heading error of the inertial navigation system cannot be deeply corrected by directly using the site positioning result as the observed quantity in the prior art is solved.

Description

Inertial error suppression method for low-speed detection platform system

Technical field

The invention relates to the technical field of inertial track detection, and in particular to an inertial error suppression method for a low-speed detection platform system.

Background technique

In the process of orbit fine adjustment, precise positioning using total stations and CPIII points is still the main method of absolute measurement. A total station can be used to obtain single-point sub-millimeter precision position information. In actual measurement, a station is generally set up every 60m to 120m, and the measurement accuracy between stations is achieved through an inertial integrated navigation system. The accuracy of differential satellite receivers is susceptible to interference from a variety of factors. The magnitude of the error caused by these interferences is larger than the working distance of the low-speed detection platform and cannot be ignored in most cases. Therefore, low-speed detection platforms generally use inertial/odometer combination measurements. Track parameters between total station stations, and use high-precision positioning information at the station to correct the cumulative error of the inertial/odometer combination. However, due to the sparsity of the stations, the station positioning results cannot be directly used as observations to make more in-depth corrections to the heading errors of the inertial navigation system. As a result, the corrected measurement trajectory will have a standing wave-like trajectory error, and the station output error is 0.

Contents of the invention

The invention provides an inertial error suppression method for a low-speed detection platform system, which can solve the technical problem in the prior art that the site positioning results cannot be directly used as observation quantities to make more in-depth corrections to the heading error of the inertial navigation system.

The invention provides an inertia error suppression method for a low-speed detection platform system. The inertia error suppression method includes: obtaining the mileage output by the odometer in the carrier coordinate system; calculating and obtaining the mileage based on the mileage output by the odometer in the carrier coordinate system. The position increment of the meter in the carrier coordinate system; the difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is used as the system observation quantity, and the observations are obtained based on the system observation quantity matrix; obtain the system state transition matrix; based on the system observation matrix and the system state transition matrix, estimate the state variables through the Kalman filter algorithm, and correct the odometer installation error based on the estimated state variables; based on the corrected odometer Installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error, obtain the gyro drift equivalent angular velocity based on the lateral cumulative position error caused by gyro drift, and complete the track detection platform based on the gyro drift equivalent angular velocity System inertia error correction.

Further, after completing the inertial error correction of the orbit detection platform system, the inertia error suppression method also includes: comparing the corrected inertial error of the orbit detection platform system with the set inertial error accuracy threshold range. When the corrected orbit When the inertial error of the detection platform system exceeds the set inertial error accuracy threshold range, repeat the above steps until the corrected inertial error of the track detection platform system is within the set inertial error accuracy threshold range.

Furthermore, the mileage output by the odometer in the carrier coordinate system can be calculated according to to get, where,/> is the mileage output by the odometer in the carrier coordinate system at time k,/> is the installation relationship matrix between the odometer and the inertial navigation system, K _D is the odometer scale coefficient,/> is the pulse number vector form of the odometer in the odometer coordinate system, and N _k is the number of pulses output by the odometer in the k-th sampling period.

Furthermore, the position increment of the odometer in the carrier coordinate system can be calculated according to to get, where,/> is the position increment of the odometer in the carrier coordinate system, δα _θ is the pitch angle error, δα _ψ is the heading angle error, δK _D is the odometer scale coefficient error,/> is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k, is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,/> is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

Furthermore, the system observations can be based on Get, where, /> is the position increment of the inertial navigation system in the carrier coordinate system, H _k is the observation matrix,/>

Further, the position increment of the inertial navigation system in the carrier coordinate system can be based on to get, where,/> is the position increment of the inertial navigation system in the navigation coordinate system,/> is the speed of the inertial navigation system in the navigation coordinate system at time k,/> is the speed of the inertial navigation system in the navigation coordinate system at k+1 time, and T _s is the calculation period.

Further, correcting the odometer installation error based on the estimated state variables specifically includes: correcting the installation relationship matrix between the inertial navigation system and the odometer and the odometer scale coefficient based on the estimated state variables to complete the odometer calibration. Installation errors are corrected.

Further, the installation relationship matrix between the inertial navigation system and the odometer can be based on To correct, the odometer scale coefficient can be corrected according to K _D,k+1 = (1+δK _D,k )K _D,k, where, /> is the installation relationship matrix between the odometer and the inertial navigation system at k+1 time,/> is the installation relationship matrix between the odometer and the inertial navigation system at time k, K _D,k+1 is the odometer scale coefficient at time k+1, and K _D,k is the odometer scale coefficient at time k.

Furthermore, the gyro drift equivalent angular velocity can be calculated according to To obtain, where Δx(t) is the lateral cumulative position error caused by gyro drift, ω is the equivalent angular rate of gyro drift, t is the time elapsed since starting from the previous station, and v is the average execution of the current data segment Speed, τ is any time.

By applying the technical solution of the present invention, an inertial error suppression method for a low-speed detection platform system is provided. The inertial error suppression method estimates the state variables based on the Kalman filter algorithm by obtaining the observation matrix and the system state transition matrix. And correct the odometer installation error based on the estimated state variables; after correcting the odometer installation error, the navigation error can be reversely deduced through the position error at the site, that is, the equivalent angular velocity of the gyro drift is obtained, based on the gyro drift, etc. The effective angular velocity is used to complete the inertial error correction of the track detection platform system. This method can significantly reduce the standing wave error caused by the fixed-point correction of the total station, and has a more significant suppression effect on the error of the inertial navigation system than the traditional inertial/odometer combined navigation algorithm.

Description of the drawings

The accompanying drawings are included to provide a further understanding of the embodiments of the invention, and constitute a part of this specification, illustrate embodiments of the invention and together with the description, serve to explain the principles of the invention. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without exerting creative efforts.

Figure 1 shows a schematic diagram of changes in the heading difference of two curves provided according to a specific embodiment of the present invention.

Detailed ways

It should be noted that, as long as there is no conflict, the embodiments and features in the embodiments of this application can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, rather than all the embodiments. The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application or uses. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts fall within the scope of protection of the present invention.

It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular forms are also intended to include the plural forms unless the context clearly indicates otherwise. Furthermore, it will be understood that when the terms "comprises" and/or "includes" are used in this specification, they indicate There are features, steps, operations, means, components and/or combinations thereof.

The relative arrangement of components and steps, numerical expressions, and numerical values set forth in these examples do not limit the scope of the invention unless specifically stated otherwise. At the same time, it should be understood that, for convenience of description, the dimensions of various parts shown in the drawings are not drawn according to actual proportional relationships. Techniques, methods and devices known to those of ordinary skill in the relevant art may not be discussed in detail, but where appropriate, such techniques, methods and devices should be considered part of the authorized specification. In all examples shown and discussed herein, any specific values are to be construed as illustrative only and not as limiting. Accordingly, other examples of the exemplary embodiments may have different values. It should be noted that similar reference numerals and letters refer to similar items in the following figures, so that once an item is defined in one figure, it does not need further discussion in subsequent figures.

According to a specific embodiment of the present invention, an inertial error suppression method for a low-speed detection platform system is provided. The inertial error suppression method includes: obtaining the mileage output by the odometer in the carrier coordinate system; based on the odometer in the carrier coordinate system Obtain the position increment of the odometer in the carrier coordinate system through the mileage calculation output below; use the difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system as the system observation quantity , obtain the observation matrix according to the system observations; obtain the system state transition matrix; based on the system observation matrix and the system state transition matrix, estimate the state variables through the Kalman filter algorithm, and correct the odometer installation error based on the estimated state variables. ;Based on the corrected odometer installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error, obtain the gyro drift equivalent angular velocity based on the lateral cumulative position error caused by gyro drift, based on gyro drift, etc. The effective angular velocity is used to complete the inertial error correction of the track detection platform system.

Applying this configuration method, an inertial error suppression method for low-speed detection platform systems is provided. This inertial error suppression method estimates the state variables based on the Kalman filter algorithm by obtaining the observation matrix and the system state transition matrix, and The odometer installation error is corrected based on the estimated state variables; after the odometer installation error is corrected, the navigation error can be inferred through the position error at the site, that is, the gyro drift equivalent angular velocity is obtained. Based on the gyro drift equivalent The angular velocity completes the inertial error correction of the track detection platform system. This method can significantly reduce the standing wave error caused by the fixed-point correction of the total station, and has a more significant suppression effect on the error of the inertial navigation system than the traditional inertial/odometer combined navigation algorithm. Among them, in the present invention, the low-speed detection platform generally refers to the detection platform with a speed less than or equal to 2m/s.

In the present invention, in order to further improve the estimation accuracy, after completing the inertial error correction of the orbit detection platform system, the inertia error suppression method also includes: comparing the corrected inertia error of the orbit detection platform system with the set inertia error accuracy threshold Compare the range. When the inertial error of the corrected track detection platform system exceeds the set inertial error accuracy threshold range, repeat the above steps until the inertial error of the corrected track detection platform system is within the set inertial error accuracy threshold range. In this configuration, by evaluating the inertial error after each correction, when the set accuracy requirements are not met, iterative calculations can be performed to further improve the estimation accuracy.

In the present invention, in order to achieve inertial error suppression for the track detection platform system, it is first necessary to obtain the mileage output by the odometer in the carrier coordinate system. As a specific embodiment of the present invention, the low-speed track detection system includes an inertial navigation system and an odometer. The low-speed track detection system is in close contact with the track through a clamping device. Therefore, the lateral speed and vertical speed are always 0, and the forward speed is always 0. It can be obtained by collecting the odometer output. The mileage output by the odometer in the carrier coordinate system can be calculated according to to get, where,/> is the mileage output by the odometer in the carrier coordinate system at time k,/> is the installation relationship matrix between the odometer and the inertial navigation system, K _D is the odometer scale coefficient,/> is the pulse number vector form of the odometer in the odometer coordinate system, and N _k is the number of pulses output by the odometer in the k-th sampling period.

Furthermore, the installation relationship matrix between the odometer and the inertial navigation system It can be expressed in the form of Euler angles/> Then, the position increment of the odometer in the carrier coordinate system is obtained based on the mileage calculation output by the odometer in the carrier coordinate system. Let δK _D represent the odometer scale coefficient error, then After sorting, it can be expressed in the form of an error vector, that is, the position increment of the odometer in the carrier coordinate system is/> Among them,/> is the position increment of the odometer in the carrier coordinate system, δα _θ is the pitch angle error, δα _ψ is the heading angle error, δK _D is the odometer scale coefficient error,/> is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k,/> is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,/> is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

Furthermore, after obtaining the position increment of the odometer in the carrier coordinate system, the system observation volume is calculated using the position increment per unit time, which can make full use of the high short-term accuracy of the inertial navigation system, making it easier to install The error and scale factor error are estimated more accurately. Within k moments, the position increment of the inertial navigation system is Among them,/> is the position increment of the inertial navigation system in the navigation coordinate system,/> is the speed of the inertial navigation system in the navigation coordinate system at time k,/> is the speed of the inertial navigation system in the navigation coordinate system at time k-1, and T _s is the calculation period. Convert the position increment of the inertial navigation system in the navigation coordinate system to the carrier coordinate system b, we can get

The difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is selected as the system observation quantity Z _k :

According to the system observation quantity, the observation matrix H _k can be obtained, Among them,/> is the position increment of the inertial navigation system in the carrier coordinate system.

Furthermore, after obtaining the system observation matrix, considering that the error vector does not change much in the short term, the system state transition matrix can be approximated as a unit matrix, that is, F _k =I.

Based on _the system _observation matrix and ^the _system state transition matrix, the state _variable Installation errors are corrected.

specifically,

Among them, X _k,k-1 is the one- _step prediction _state , K _k is the filter gain matrix, X _{k is the state variable at time k} , Forecast mean square error matrix, P _k is the mean square error matrix at time k, P _k-1 is the mean square error matrix at time k - 1, Q _k is the system noise matrix, and R _k is the measurement noise matrix.

In the present invention, correcting the odometer installation error based on the estimated state variables specifically includes: correcting the installation relationship matrix between the inertial navigation system and the odometer and the odometer scale coefficient based on the estimated state variables to complete the calibration. The odometer installation error is corrected. Among them, the installation relationship matrix between the inertial navigation system and the odometer can be based on To correct, the odometer scale coefficient can be corrected according to K _D,k+1 = (1+δK _D,k )K _D,k, where, /> is the installation relationship matrix between the odometer and the inertial navigation system at k+1 time,/> is the installation relationship matrix between the odometer and the inertial navigation system at time k, K _D,k+1 is the odometer scale coefficient at time k+1, and K _D,k is the odometer scale coefficient at time k.

After correcting the installation error of the odometer, the measurement position error each time the total station is set up is basically caused by the drift of the heading gyro during the measurement process. As shown in Figure 1, the error growth curve is a curve. It cannot be eliminated by correcting the heading error. In addition, due to the influence of noise during the measurement process, comparing the heading difference changes of the two curves (the first curve is the true value curve, the second curve is the measured value curve containing errors) will introduce greater heading errors, so it is necessary to use The position error is the inverse derivation of the heading gyro drift error.

In the present invention, according to the error generation principle, the relationship between the heading gyro drift and the form mileage can be expressed as Among them, Δ Execution speed, Δt is the system sampling period, /> The discretized form of can be converted into a continuous time function form/> Among them, Δx(t) is the lateral cumulative position error caused by gyro drift, and τ is any time.

In order to have a further understanding of the present invention, the inertial error suppression method for a low-speed detection platform system provided by the present invention will be described in detail below with reference to specific embodiments.

According to a specific embodiment of the present invention, an inertia error suppression method for a low-speed detection platform system is provided. The inertia error suppression method specifically includes the following steps.

Obtain the mileage output by the odometer in the carrier coordinate system; the mileage output by the odometer in the carrier coordinate system can be calculated according to to get, where,/> is the mileage output by the odometer in the carrier coordinate system at time k,/> is the installation relationship matrix between the odometer and the inertial navigation system, K _D is the odometer scale coefficient,/> is the pulse number vector form of the odometer in the odometer coordinate system, and N _k is the number of pulses output by the odometer in the k-th sampling period.

Installation relationship matrix between odometer and inertial navigation system can be expressed in the form of Euler angles

Based on the mileage calculation output by the odometer in the carrier coordinate system, the position increment of the odometer in the carrier coordinate system is obtained; let δK _D represent the odometer scale coefficient error, then After sorting, it can be expressed in the form of an error vector, that is, the position increment of the odometer in the carrier coordinate system is/> Among them,/> is the position increment of the odometer in the carrier coordinate system, δα _θ is the pitch angle error, δα _ψ is the heading angle error, δK _D is the odometer scale coefficient error,/> is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k,/> is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,/> is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

The difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is used as the system observation quantity, and the observation matrix is obtained based on the system observation quantity; the system state transition matrix is obtained; based on the system observation Matrix and system state transition matrix, the state variables are estimated through the Kalman filter algorithm, and the odometer installation error is corrected based on the estimated state variables. System observations can be based on Get, where, is the position increment of the inertial navigation system in the carrier coordinate system, H _k is the observation matrix, The position increment of the inertial navigation system in the carrier coordinate system/> can be based on to get, where,/> is the position increment of the inertial navigation system in the navigation coordinate system,/> is the speed of the inertial navigation system in the navigation coordinate system at time k,/> is the speed of the inertial navigation system in the navigation coordinate system at time k-1, and T _s is the calculation period.

Based on the corrected odometer installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error. Based on the lateral cumulative position error caused by gyro drift, obtain the gyro drift equivalent angular velocity. Based on the gyro drift equivalent The angular velocity completes the inertial error correction of the track detection platform system.

Compare the corrected inertial error of the track detection platform system with the set inertial error accuracy threshold range. When the corrected inertial error of the track detection platform system exceeds the set inertia error accuracy threshold range, repeat the above steps until The corrected inertial error of the track detection platform system is within the set inertial error accuracy threshold range.

To sum up, the present invention provides an inertial error suppression method for low-speed detection platform systems. The inertial error suppression method estimates the state variables based on the Kalman filter algorithm by obtaining the observation matrix and the system state transition matrix. And correct the odometer installation error based on the estimated state variables; after correcting the odometer installation error, the navigation error can be reversely deduced through the position error at the site, that is, the equivalent angular velocity of the gyro drift is obtained, based on the gyro drift, etc. The effective angular velocity is used to complete the inertial error correction of the track detection platform system. This method can significantly reduce the standing wave error caused by the fixed-point correction of the total station, and has a better suppression effect on the error of the inertial navigation system than the traditional inertial/odometer combined navigation algorithm.

For the convenience of description, spatially relative terms can be used here, such as "on...", "on...", "on the upper surface of...", "above", etc., to describe what is shown in the figure. The spatial relationship between one device or feature and other devices or features. It will be understood that the spatially relative terms are intended to encompass different orientations of the device in use or operation in addition to the orientation depicted in the figures. For example, if a feature in the figure is turned upside down, then one feature described as "above" or "on top of" other features or features would then be oriented "below" or "below" the other features or features. under other devices or structures". Thus, the exemplary term "over" may include both orientations "above" and "below." The device may be otherwise oriented (rotated 90 degrees or at other orientations) and the spatially relative descriptors used herein interpreted accordingly.

In addition, it should be noted that the use of words such as "first" and "second" to define parts is only to facilitate the distinction between corresponding parts. Unless otherwise stated, the above words have no special meaning and therefore cannot be understood. To limit the scope of protection of the present invention.

The above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

Claims (7)

1. An inertial error mitigation method for a low-speed detection platform system, the inertial error mitigation method comprising:

acquiring mileage output by an odometer under a carrier coordinate system;

acquiring the position increment of the odometer under the carrier coordinate system based on the odometer output under the carrier coordinate system by calculation;

taking the difference value between the position increment of the inertial navigation system under the carrier coordinate system and the position increment of the odometer under the carrier coordinate system as a system observed quantity, and acquiring an observation matrix according to the system observed quantity;

acquiring a system state transition matrix;

based on the system observation matrix and the system state transition matrix, estimating a state variable through a Kalman filtering algorithm, wherein the state variable comprises an odometer scale coefficient error, a pitch angle error and a course angle error;

correcting the mounting error of the odometer based on the estimated state variable, specifically comprising: correcting an installation relation matrix between the inertial navigation system and the odometer based on the estimated state variable, so as to finish correction of the installation error of the odometer;

based on the corrected odometer installation error, acquiring a lateral accumulated position error caused by gyro drift after the odometer error is corrected,

acquiring a gyro drift equivalent angular velocity based on the lateral accumulated position error caused by gyro drift, wherein the gyro drift is obtained by using the gyro drift equivalent angular velocityEquivalent angular velocity according toAcquiring, wherein deltax (t) is a lateral accumulated position error caused by gyro drift, omega is a gyro drift equivalent angular rate, t is the time elapsed after starting from the last station, v is the average pushing speed of the current data segment, and tau is any moment;

and finishing inertial error correction of the low-speed detection platform system based on the gyro drift equivalent angular velocity.

2. The inertial error mitigation method for a low-speed detection platform system according to claim 1, wherein after completion of inertial error correction for the low-speed detection platform system, the inertial error mitigation method further comprises: comparing the corrected inertial error of the low-speed detection platform system with a set inertial error precision threshold range, and repeating the steps when the corrected inertial error of the low-speed detection platform system exceeds the set inertial error precision threshold range until the corrected inertial error of the low-speed detection platform system is within the set inertial error precision threshold range.

3. The inertial error mitigation method for a low-speed inspection platform system according to claim 1, wherein the mileage output by the odometer in the carrier coordinate system is based onTo obtain, wherein->For the mileage output by the odometer at time k under the carrier coordinate system, < >>K is an installation relation matrix between the odometer and the inertial navigation system _D For the odometer scale factor, +.>In the form of pulse number vector of the odometer under the odometer coordinate system, N _k The number of pulses output by the odometer during the kth sampling period.

4. The inertial error mitigation method for a low-speed inspection platform system according to claim 3, wherein the position increment of the odometer in the carrier coordinate system is based onTo obtain, wherein->Delta alpha is the position increment of the odometer under the carrier coordinate system _θ As pitch angle error, delta alpha _ψ Delta K is the heading angle error _D Error of scale factor for odometer,/->For the mileage output by the odometer at time k in the carrier coordinate system along the x-axis +.>For the mileage output by the odometer at time k in the carrier coordinate system along the y-axis, +.>For the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, X is the state variable.

5. The inertial error mitigation method for a low-speed inspection platform system according to claim 4, wherein said system observables Z _k According toAcquisition of (I) in (I)>For the position increment of the inertial navigation system under the carrier coordinate system, H _k For observing matrix +.>

6. The inertial error mitigation method for a low-speed inspection platform system according to claim 5, wherein the inertial navigation system is position-delta in a carrier coordinate systemAccording to-> To obtain, wherein->For the position increment of the inertial navigation system in the navigation coordinate system, < >>For the speed of the inertial navigation system in the navigation coordinate system at time k,/>For the speed of the inertial navigation system at the moment k+1 in the navigation coordinate system, T _s For calculating the period.

7. The method for inertial error mitigation of a low-speed inspection platform system of claim 4 wherein,the installation relation matrix between the inertial navigation system and the odometer is based onCorrecting the scale coefficient of the odometer according to K _D,k+1 ＝(1+δK _D,k )K _D,k Make corrections in which->For the installation relation matrix between the k+1 moment odometer and the inertial navigation system, +.>For the installation relation matrix between the moment K odometer and the inertial navigation system, K _D,k+1 For the scale factor of the milemeter at the moment k+1, K _D,k And (5) the scale coefficient of the mileometer at the moment k.