Track detection IMU fault self-diagnosis method

By using the Kalman filtering combination algorithm and zero-phase filtering technology of gyroscopes and accelerometers in the inertia/odometer combination navigation, the correlation coefficients of navigation attitude and filtering attitude are calculated, and the attitude measurement accuracy is affected in the inertia/odometer combination navigation is solved. The problem of this is to realize high-precision and high-reliability orbital geometric parameter detection.

CN118999624APending Publication Date: 2024-11-22BEIJING AUTOMATION CONTROL EQUIP INST
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411233440.1
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-04
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

In inertia/odometer combined navigation, due to the high smoothness of the railway track surface, the odometer frequently slips, resulting in abnormal fluctuations in the combination results, affecting the attitude measurement accuracy. A detection method is urgently needed to verify the effectiveness of the combined navigation attitude calculation results. sex.

Method used

The measurements were performed separately by gyroscopes and accelerometers, and the Kalman filtering combined navigation algorithm and zero-phase filtering technology were used to calculate the correlation coefficients of navigation attitude and filtering attitude to realize self-diagnosis of the combined navigation attitude results.

Benefits of technology

No external detection equipment is required, and can be performed online, reducing the impact of odometer failure on the attitude measurement of inertial navigation system, improving equipment environmental adaptability and convenience of use, and ensuring high-precision and high-reliability track geometric parameter measurements.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118999624A_ABST
    Figure CN118999624A_ABST
Patent Text Reader

Abstract

The invention provides an orbit detection IMU (inertial measurement unit) fault self-diagnosis method, which comprises the following steps of: acquiring navigation attitude data by adopting a gyroscope / accelerometer / odometer through a Kalman filtering combined navigation algorithm; filtering attitude data are obtained through zero-phase filtering by only adopting an accelerometer; and synchronously selecting data Xi and Yi of which the window width is w from the acquired navigation attitude and filtering attitude, calculating a correlation coefficient of the Xi and the Yi, and performing fault judgment according to the correlation coefficient. According to the method, the autonomy and reliability of inertial measurement are utilized, the system attitude measurement fault self-diagnosis capability is realized under the condition that other sensors are not added, and the method has remarkable popularization and application values.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of inertial navigation, and relates to a track detection IMU fault self-diagnosis method, in particular to an inertial combined navigation attitude correctness detection method for a track geometry parameter detection system. Background Art

[0002] High-precision and high-reliability attitude measurement is a necessary condition for achieving millimeter-level track geometry parameter measurement. The inertial / odometer combination is a commonly used measurement combination. The inertial navigation system is used to obtain the position and attitude of the carrier in the inertial space, and the odometer is used to measure the mileage of the carrier. The inertial navigation system has high attitude measurement in the short term, but the error grows rapidly over time. The odometer has high mileage measurement accuracy and the error does not grow over time. Therefore, combining the two can achieve continuous high-precision attitude measurement. However, due to the high smoothness of the railway track surface, the odometer frequently slips, resulting in abnormal fluctuations in the combined results, affecting the accuracy of attitude measurement. Therefore, it is urgent to provide a detection method to verify the validity of the combined navigation attitude calculation results. Summary of the invention

[0003] The present invention aims to solve at least one of the technical problems existing in the prior art or related art.

[0004] To this end, the present invention provides a track detection IMU fault self-diagnosis method. The present invention measures the system attitude through a gyroscope and an accelerometer respectively, and realizes self-diagnosis of the correctness of the combined navigation attitude result due to the non-correlation of the measurements of the gyroscope and the accelerometer.

[0005] The technical solution of the present invention is as follows:

[0006] According to one aspect, a track detection IMU fault self-diagnosis method is provided, the method comprising:

[0007] Using gyroscope / accelerometer / odometer, and Kalman filter combined navigation algorithm, navigation attitude data is obtained;

[0008] Only the accelerometer is used to obtain filtered attitude data through zero-phase filtering;

[0009] Synchronously select data X with a window width of w from the acquired navigation posture and filtering posture i and Y i , and calculate X using the following formula i and Y i The correlation coefficient r i :

[0010]

[0011] Among them, X irepresents the navigation posture corresponding to the i-th group of windows, Y i represents the filtering posture corresponding to the i-th group of windows, Respectively represent the mean of the data in the corresponding window. ij Indicates the measured value of the navigation posture of the measurement point within the current window w; Y ij Represents the measured value of the filtered posture of the measurement point within the current window w;

[0012] According to the correlation coefficient, within the window w, fault diagnosis is performed by the following formula:

[0013]

[0014] Among them, σ is the set threshold.

[0015] Furthermore, the gyroscope / accelerometer / odometer is used to obtain navigation attitude data through a Kalman filter combined navigation algorithm, including:

[0016] 1) Establishing an inertial / odometer Kalman filter combined navigation state space model, wherein the space model includes a state equation and a measurement equation;

[0017] 2) Use the gyroscope and accelerometer of the inertial navigation system to perform inertial navigation solution, and estimate the error state through Kalman filtering in combination with the model in step 1).

[0018] Furthermore, the state equation is constructed by the following formula:

[0019] X k =Φ k,k-1 X k-1 +w k-1

[0020] In the formula,

[0021] State variables Where the misalignment angle φ=[φ E φ N φ U ] Τ , speed error δv n =[δV E δV N δV U ] T , Inertial navigation position error Including latitude error Longitude error δλ, altitude error δh, dead reckoning position error Including latitude error Longitude error δλ D , height error δh D , carrier system gyro drift ε b =[εx ε y ε z ] T , load system accelerometer zero bias and the odometer error κ D =[α θ δK D α ψ ] T , including the pitch angle α between the inertial navigation carrier coordinate system and the odometer coordinate system θ , heading angle α ψ and odometer scale factor error δK D .

[0022] X k and X k-1 are the state vectors estimated at time k and time k-1 respectively;

[0023] Φ k,k-1 is the state transfer matrix of the state equation;

[0024] w k-1 is the system error.

[0025] Furthermore, the measurement equation is constructed by the following formula:

[0026] Z k =H k X k +V k

[0027] Among them, Z k is the measurement vector at time k; H k is the measurement matrix at time k, V k is the random noise vector measured at time k.

[0028] Further, the method of using only the accelerometer to obtain filtered attitude data through zero-phase filtering includes:

[0029] Construct a zero-phase filter, including:

[0030] Design zero-phase filtering methods;

[0031] Set initial conditions;

[0032] Filtering attitude data is resolved based on the zero-phase filter.

[0033] Furthermore, the method for designing a zero-phase filter comprises:

[0034] Construct a low-pass filter h(n) for the two horizontal accelerometers a=[a x a y ] is filtered, and the filter output is

[0035] y1(n)=h(n)*a(n)

[0036] Reverse the order of y1(n) to obtain the reversed sequence y2(n):

[0037] y2(n)=y1(N-1-n), where N is the total length of the data;

[0038] Take y2(n) as input and send it to the high-pass filter again to get:

[0039] y3(n)=h(n)*y2(n)

[0040] Among them, y3(n) is called the acceleration reverse filtering output sequence. Reverse y3(n) to obtain a sequence arranged in the forward direction of time:

[0041] a f (n) = y3(N-1-n)

[0042] Among them, a f =[a fx a fy ], a f (n) means a f The nth row of .

[0043] Furthermore, the initial conditions of the filter are set in the following way:

[0044] By extending the data, the initial filtering conditions at the start or end of the data are calculated.

[0045] According to another aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above method when executing the computer program.

[0046] The above technical solution proposes a fault self-diagnosis method for the demand of high-precision and high-reliability attitude measurement for orbital geometry parameter detection, and calculates the carrier attitude by using two ways, namely, gyroscope / accelerometer / odometer and accelerometer only, and uses the non-correlation between the two calculation methods to calculate the consistency of the two through the correlation coefficient, so as to realize self-diagnosis of the validity of the attitude measurement result. The solution proposed by the present invention does not need to introduce external detection equipment and can be executed online, which provides a solution for high-precision and high-reliability orbital geometry parameter measurement, can reduce the impact of odometer failure on the attitude measurement of the inertial navigation system, and improve the environmental adaptability and ease of use of the equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0048] Figure 1 This is the Kalman filter flow chart. DETAILED DESCRIPTION

[0049] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] 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, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0051] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be regarded as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0052] In one embodiment of the present invention, a track detection IMU fault self-diagnosis method is provided, the method comprising:

[0053] Step 1: Use the gyroscope / accelerometer / odometer to obtain navigation attitude data through the Kalman filter combined navigation algorithm;

[0054] Step 2: Only the accelerometer is used to obtain filtered attitude data through zero-phase filtering;

[0055] Step 3: Synchronously select data X with a window width of w from the acquired navigation posture and filtering posture i and Y i , and calculate X using the following formula i and Y i The correlation coefficient r i :

[0056]

[0057] Among them, X i represents the navigation posture corresponding to the i-th group of windows, Y i represents the filtering posture corresponding to the i-th group of windows, Respectively represent the mean of the data in the corresponding window. ij Indicates the measured value of the navigation posture of the measurement point within the current window w; Y ij Represents the measured value of the filtered posture of the measurement point within the current window w;

[0058] According to the correlation coefficient, within the window w, fault diagnosis is performed by the following formula:

[0059]

[0060] Among them, σ is the set threshold.

[0061] In the embodiment of the present invention, the navigation attitude has high dynamic characteristics, and the carrier attitude can be accurately measured when the odometer information is accurate. The accelerometer used to calculate the filtered attitude has lost its high-frequency characteristics due to low-pass filtering, but the attitude filtering only relies on the accelerometer information of the inertial navigation system itself and is not affected by other error factors. Therefore, it has high reliability and can be used as a benchmark to judge the effectiveness of the navigation attitude. Among them:

[0062] When i =1, indicating that the two sets of data are completely correlated. i = 0 means completely uncorrelated. The attitude sequences calculated by the navigation and filtering methods should be close to completely correlated, that is, r i →1, so the correlation coefficient of the two sets of data can be used to judge the correctness of the posture calculation, among which the specific value of σ is designed according to the actual situation.

[0063] By applying the above configuration, a fault self-diagnosis method is proposed to meet the requirements of high-precision and high-reliability attitude measurement for orbital geometry parameter detection. The carrier attitude is calculated by using two methods, namely, gyroscope / accelerometer / odometer and accelerometer only. The non-correlation between the two calculation methods is used to calculate the consistency of the two through the correlation coefficient, so as to realize self-diagnosis of the validity of the attitude measurement result. The scheme proposed by the present invention does not need to introduce external detection equipment and can be executed online. It provides a solution for high-precision and high-reliability orbital geometry parameter measurement, reduces the impact of odometer failure on the attitude measurement of the inertial navigation system, and improves the environmental adaptability and ease of use of the equipment.

[0064] According to an embodiment of the present invention, the step 1 specifically includes:

[0065] Establish the inertial / odometer Kalman filter combined navigation state space model.

[0066] The state space model consists of two parts: state equation and measurement equation:

[0067] First calculate the state equation X k =Φ k,k-1 X k-1 +w k-1 .

[0068] 1. Equation of state X k =Φ k,k-1 X k-1 +w k-1 The specific calculation of each variable is as follows:

[0069] 1.1 State quantity X k

[0070] State variables There are 21 dimensions in total, including:

[0071] Misalignment angle φ=[φ E φ N φ U ] Τ (Unit: rad), speed error δv n =[δV E δV N δV U ] T (Unit: m / s), inertial navigation position error Including latitude error (unit: rad), longitude error δλ (unit: rad), altitude error δh (unit: m), dead reckoning position error Including latitude error (Unit: rad), longitude error δλD (Unit: rad), height error δh D (Unit: m), carrier system gyro drift ε b =[ε x ε y ε z ] T (Unit: rad / s), load system accelerometer zero bias (Unit: m / s 2 ) and odometer error κ D =[α θ δK D α ψ ] T , including the pitch angle α between the inertial navigation carrier coordinate system (b system) and the odometer coordinate system (m system) θ , heading angle α ψ and odometer scale factor error δK D .

[0072] X k and X k-1 are the state vectors estimated at time k and time k-1 respectively;

[0073] 1.2 State equation state transfer matrix Φ k,k-1

[0074] State equation state transfer matrix Φ k,k-1 Through the discretization calculation of F matrix, the F matrix is ​​specifically defined as:

[0075]

[0076] in:

[0077]

[0078] Where: (×) indicates calculating the antisymmetric matrix for the vector in brackets, Represents the direction cosine matrix from b system to n system (navigation coordinate system), For comparison; v E 、v N 、v U is the geographic velocity, unit: m / s; L is the latitude; h is the height; ω ie is the Earth's rotation angular rate; R Mh , R Nh are the principal curvature radius of the meridian plane including the altitude and the curvature radius of the meridian circle, v D Represents the odometer speed scalar.

[0079] 2. Calculate the measurement equation Z k =H k X k +Vk

[0080] Measurement equation Z k =H k X k +V k The specific meanings of the variables are as follows:

[0081] 2.1 Measurement vector Z at time k k

[0082] The measurement vector Z at time k k The specific definition of is:

[0083]

[0084] Where: is the inertial navigation velocity vector including error, is the odometry velocity vector including error.

[0085] 2.2k time measurement matrix H k

[0086] The measurement matrix H at time k k The specific definition is:

[0087]

[0088] 2.3 Use Kalman filtering to estimate and correct the inertial navigation system error:

[0089] The gyroscope and accelerometer of the inertial navigation system are used to perform inertial navigation solution, and the error state is estimated through Kalman filtering in combination with the model in step 1. The execution process of the Kalman filter algorithm is as follows: Figure 1 As shown in the figure: Estimate the state vector for one-step forecast; is the optimal estimated state vector; P k,k-1 is the one-step prediction covariance matrix; P k is the optimal estimated covariance matrix; K k is the optimal gain matrix; R k is the measurement noise covariance matrix after discretization; Q k is the system noise covariance matrix after discretization.

[0090] Considering that the parameter estimation results are related to the carrier maneuver, and the parameter convergence requires a process, the P k The convergence of the diagonal elements is judged when When the corresponding state error is corrected, p k P k Diagonal elements, is the diagonal element convergence threshold.

[0091] According to an embodiment of the present invention, the step 2 specifically includes:

[0092] 1. Constructing a zero-phase filter

[0093] 1.1 Zero Phase Filtering

[0094] Construct a low-pass filter h(n) for the acceleration in the two horizontal directions a=[a x a y ] is filtered, and the filter output is

[0095] y1(n)=h(n)*a(n)

[0096] Reverse the order of y1(n) to obtain the reversed sequence y2(n)

[0097] y2(n)=y1(N-1-n)

[0098] Where N is the total length of the data.

[0099] Take y2(n) as input and send it to the high-pass filter again

[0100] y3(n)=h(n)*y2(n)

[0101] Among them, y3(n) is called the acceleration reverse filtering output sequence. Reverse y3(n) to obtain a sequence arranged in the forward direction of time.

[0102] a f (n) = y3(N-1-n)

[0103] Among them, a f =[a fx a fy ].

[0104] 1.2 Initial condition setting

[0105] The initial condition refers to the initial setting of the internal state of the filter before the filter starts working. If the initial condition is not set, it is generally defaulted to 0. In the track detection scenario, the output of the horizontal accelerometer is not always 0, so incorrect initial condition settings will cause fluctuations in the filtering results in the initial stage of the data, resulting in large attitude calculation errors. Therefore, it is necessary to set the initial condition of the filter.

[0106] The zero-phase filter performs forward and reverse calculations on the data, so the initial filter condition needs to be set at the beginning of the data. Considering the track detection scenario, the following reasonable assumptions can be made:

[0107] a) In the plane, track types are divided into three categories: straight line, circular curve and transition curve, among which transition curve is used to connect straight line and circular curve;

[0108] b) In the vertical direction, the tracks are divided into two categories: flat tracks and ramp tracks;

[0109] c) By extending the data, the initial filtering condition of the data start point (or end point) is calculated. The specific extension method is shown in Table 1, where m represents the length of the extended data:

[0110] Table 1 Data extension method for setting initial conditions of filtering, k = 1, 2, ... m

[0111]

[0112] 2. Posture calculation

[0113] Due to the constraints of the track, the horizontal attitude can be obtained through the accelerometer and odometer. When the carrier is moving on the track, the mean lateral acceleration is 0, so the lateral accelerometer output is proportional to the roll angle; the forward acceleration can be obtained through the odometer differential, so the forward accelerometer output is proportional to the pitch angle. Therefore, the calculation method of the filtered attitude is as follows:

[0114]

[0115] Where g represents the local gravitational acceleration, φ α (i) is the roll angle of the i-th measurement point, φ β (i) is the pitch angle of the i-th measurement point.

[0116] According to another embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the method of the above embodiment is implemented when the processor executes the computer program.

[0117] In summary, the solution proposed in the present invention does not require the introduction of external detection equipment and can be executed online, providing a solution for high-precision and high-reliability track geometry parameter measurement, which can reduce the impact of odometer failure on the attitude measurement of the inertial navigation system and improve the environmental adaptability and ease of use of the equipment.

[0118] Features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or used in place of features in other embodiments.

[0119] It should be emphasized that the term "include / comprises" when used herein refers to the presence of features, integers, steps or components, but does not exclude the presence or addition of one or more other features, integers, steps, components or combinations thereof.

[0120] The above method of the present invention can be implemented by hardware, or by hardware combined with software. The present invention relates to such a computer-readable program, which, when executed by a logic component, enables the logic component to implement the above-mentioned device or component, or enables the logic component to implement the above-mentioned various methods or steps. The present invention also relates to a storage medium for storing the above program, such as a hard disk, a magnetic disk, an optical disk, a DVD, a flash memory, etc.

[0121] The many features and advantages of these embodiments are apparent from this detailed description, and thus the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since numerous modifications and changes will readily occur to those skilled in the art, it is not intended that the embodiments of the invention be limited to the exact construction and operation illustrated and described, but rather all suitable modifications and equivalents falling within the scope thereof are intended to be covered.

[0122] Parts of the present invention that are not described in detail are well known to those skilled in the art.

Claims

1. A self-diagnosis method for track detection IMU faults, It is characterized in that The method comprises: Using gyroscope / accelerometer / odometer, and Kalman filter combined navigation algorithm, navigation attitude data is obtained; Only the accelerometer is used to obtain filtered attitude data through zero-phase filtering; Synchronously select data X with a window width of w from the acquired navigation posture and filtering posture i and Y i , and calculate X using the following formula i and Y i The correlation coefficient r i : Among them, X i represents the navigation posture corresponding to the i-th group of windows, Y i represents the filtering posture corresponding to the i-th group of windows, Respectively represent the mean of the data in the corresponding window. ij Indicates the measured value of the navigation posture of the measurement point within the current window w; Y ij Represents the measured value of the filtered posture of the measurement point within the current window w; According to the correlation coefficient, within the window w, fault diagnosis is performed by the following formula: Among them, σ is the set threshold.

2. A track detection IMU fault self-diagnosis method according to claim 1, It is characterized in that The gyroscope / accelerometer / odometer is used to obtain navigation attitude data through a Kalman filter combined navigation algorithm, including: 1) Establishing an inertial / odometer Kalman filter combined navigation state space model, wherein the space model includes a state equation and a measurement equation; 2) Use the gyroscope and accelerometer of the inertial navigation system to perform inertial navigation solution, and estimate the error state through Kalman filtering in combination with the model in step 1).

3. A track detection IMU fault self-diagnosis method according to claim 2, It is characterized in that The equation of state is constructed by: X k =Φ k,k-1 X k-1 +w k-1 In the formula, State variables Where the misalignment angle φ=[φ E φ N φ U ] Τ , speed error δv n =[δV E δV N δV U ] T , Inertial navigation position error Including latitude error Longitude error δλ, altitude error δh, dead reckoning position error Including latitude error Longitude error δλ D , height error δh D , carrier system gyro drift ε b =[ε x ε y ε z ] T , load system accelerometer zero bias and the odometer error κ D =[α θ δK D α ψ ] T , including the pitch angle α between the inertial navigation carrier coordinate system and the odometer coordinate system θ , heading angle α ψ and odometer scale factor error δK D . X k and X k-1 are the state vectors estimated at time k and time k-1 respectively; Φ k,k-1 is the state transfer matrix of the state equation; w k-1 is the system error.

4. A track detection IMU fault self-diagnosis method according to claim 2 or 3, It is characterized in that The measurement equation is constructed by the following formula: Z k =H k X k +V k Among them, Z k is the measurement vector at time k; H k is the measurement matrix at time k, V k is the random noise vector measured at time k.

5. A track detection IMU fault self-diagnosis method according to claim 2, It is characterized in that The method of using only the accelerometer to obtain filtered attitude data through zero-phase filtering includes: Construct a zero-phase filter, including: Design zero-phase filtering methods; Set initial conditions; Filtering attitude data is resolved based on the zero-phase filter.

6. A track detection IMU fault self-diagnosis method according to claim 5, It is characterized in that The method for designing a zero-phase filter comprises: Construct a low-pass filter h(n) for the two horizontal accelerometers a=[a x a y ] is filtered, and the filter output is y 1 (n)=h(n)*a(n) For 1 (n) Reverse the order to obtain the reversed sequence y 2 (n): y 2 (n) = y 1 (N-1-n), where N is the total length of the data; y 2 (n) is used as input and sent to the high-pass filter again to obtain: y 3 (n)=h(n)*y 2 (n) Among them, y 3 (n) is called the acceleration inverse filter output series, for y 3 (n) is reversed to obtain a sequence arranged in a forward direction of time: to f (n)=y 3 (N-1-n) Among them, a f =[a fx a fy ], a f (n) means a f The nth row of .

7. A track detection IMU fault self-diagnosis method according to claim 5, It is characterized in that Set the initial filter conditions as follows: By extending the data, the initial filtering conditions at the start or end of the data are calculated.

8. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, It is characterized in that When the processor executes the computer program, the method according to claims 1 to 7 is implemented.