Odometer fault isolation method and isolation device of inertial integrated positioning system

Abstract

The invention provides an odometer fault isolation method and isolation device of an inertial integrated positioning system, and solves the technical problem that the fault of an odometer cannot be found and corrected in time due to poor fault-tolerant capability of the existing inertial integrated positioning system. The method comprises the following steps: establishing a Kalman filter observation vector; acquiring sensor distance data of observation vector availability judgment; and judging the availability of the observation vector according to the error of the distance data, and determining a settlement process according to the availability. And the settlement process is improved. And judging the rationality of an observation vector by using an error of distance data, forming fault isolation of a speedometer signal, and forming switching of whether a settlement process needs to be intervened by a filter or not. And the output failure of the inertial integrated positioning system caused by rapid divergence of the positioning error is effectively prevented.

Description

Technical Field

[0001] The invention relates to the technical field of inertial combined positioning technology, and in particular to an odometer fault isolation method and an isolation device of an inertial combined positioning system. Background Art

[0002] In the prior art, an inertial combined positioning system is Figure 1 As shown. Figure 1 In the process, the gyro and accelerometer signals output by the inertial unit (IMU) are solved by pure strapdown to obtain the carrier position, velocity and attitude. At the same time, the gyro, accelerometer signals and the mileage pulse signal output by the odometer are input into the Kalman filter for solution to obtain the estimated values ​​of the position, velocity and attitude errors. The above error estimates are output to the pure strapdown solution device to correct the position, velocity and attitude.

[0003] The filtering algorithm deployed by the Kalman filter is divided into two parts: time update and measurement update during the application process. In the odometer / inertial group inertial combined positioning system, the Kalman filter uses the difference between the odometer mileage pulse and the inertial group accumulated displacement as the observation vector for measurement update.

[0004] When the odometer component fails to output mileage pulses, incorrect mileage information will be substituted into the filter measurement update equation during driving, which will affect the state estimation result of the entire filter and ultimately cause the positioning error to diverge rapidly. Summary of the invention

[0005] In view of the above problems, an embodiment of the present invention provides an odometer fault isolation method and an isolation device for an inertial combined positioning system, which solves the technical problem that the existing inertial combined positioning has poor fault tolerance and cannot timely detect and correct odometer faults.

[0006] The odometer fault isolation method of the inertial combined positioning system according to the embodiment of the present invention includes:

[0007] Establish Kalman filter observation vector;

[0008] Collect sensor distance data for observation vector availability judgment;

[0009] The availability of the observation vector is determined based on the error of the distance data, and the settlement process is determined based on the availability.

[0010] In one embodiment of the present invention, establishing a Kalman filter observation vector includes:

[0011] According to the displacement components within a time period, the observation vector of the Kalman filter in the odometer / IMU combined positioning system is established for the odometer and IMU outputs.

[0012] In one embodiment of the present invention, the observation vector is:

[0013] Z k =[D e -S e ,D n -S n ] T

[0014] Among them, T is the calculation period, and the eastward component of the odometer cumulative displacement is S e and the north component is S n , the accumulated displacement of the inertial group accelerometer in the east direction is D e The cumulative displacement to the north is D n .

[0015] In one embodiment of the present invention, the sensor distance data collected for determining the availability of the observation vector includes:

[0016] The mileage is calculated as follows:

[0017]

[0018] Where T is the calculation period, S is the accumulated mileage of the odometer, △t is the sampling period of the odometer pulse signal, N is the accumulated number of times, N = T / △t; k is the number of odometer pulses at the kth sampling moment;

[0019] The accumulated mileage of the IMU accelerometer is calculated as follows:

[0020]

[0021] Where T is the calculation period, D is the accumulated mileage of the IMU accelerometer, △t is assumed to be the strapdown calculation period of the IMU, N is the accumulated number of times, N = T / △t;

[0022] V ek is the eastward velocity of the inertial group at the kth sampling moment;

[0023] V nk is the northward velocity of the inertial group at the kth sampling moment.

[0024] In one embodiment of the present invention, judging the availability of the observation vector according to the error of the distance data and determining the settlement process according to the availability includes:

[0025] Set a safety threshold ε to measure the cumulative mileage error;

[0026] The availability of the observation vector is determined according to the following judgment rules:

[0027] |DS|<ε

[0028] Among them, S is the accumulated mileage of the odometer, and D is the accumulated mileage of the IMU accelerometer;

[0029] When it is determined that the observation vector is unavailable, the Kalman filter filtering solution is not performed, and the positioning solution is switched from the combined solution process to the pure inertial group solution process.

[0030] The odometer fault isolation device of the inertial combined positioning system according to the embodiment of the present invention comprises:

[0031] A memory, used to store program codes corresponding to the processing process of the odometer fault isolation method of the inertial combined positioning system;

[0032] A processor is used to execute the program code.

[0033] The odometer fault isolation device of the inertial combined positioning system according to the embodiment of the present invention comprises:

[0034] Index setting module, used to establish Kalman filter observation vector;

[0035] A data acquisition module, used to collect sensor distance data for observation vector availability judgment;

[0036] The indicator judgment module is used to judge the availability of the observation vector according to the error of the distance data and determine the settlement process according to the availability.

[0037] In one embodiment of the present invention, the indicator setting module includes:

[0038] The observation vector establishment unit is used to establish the observation vector output by the Kalman filter for the odometer and the inertial group IMU in the odometer / inertial group inertial combined positioning system according to the displacement component in the time period.

[0039] In one embodiment of the present invention, the data acquisition module includes:

[0040] The odometer data collection unit is used to calculate the accumulated mileage of the odometer in the following manner:

[0041]

[0042] Where T is the calculation period, S is the accumulated mileage of the odometer, △t is the sampling period of the odometer pulse signal, N is the accumulated number of times, N = T / △t; k is the number of odometer pulses at the kth sampling moment;

[0043] The accelerometer data acquisition unit is used to calculate the cumulative mileage of the IMU accelerometer in the following way:

[0044]

[0045] Where T is the calculation period, D is the accumulated mileage of the IMU accelerometer, △t is assumed to be the strapdown calculation period of the IMU, N is the accumulated number of times, N = T / △t;

[0046] V ek is the eastward velocity of the inertial group at the kth sampling moment;

[0047] V nk is the northward velocity of the inertial group at the kth sampling moment.

[0048] In one embodiment of the present invention, the indicator judgment module includes:

[0049] A threshold setting unit, used to set a safety threshold ε for measuring the cumulative mileage error;

[0050] The error measurement unit is used to determine the availability of the observation vector according to the following judgment rules:

[0051] |DS|<ε

[0052] Among them, S is the accumulated mileage of the odometer, and D is the accumulated mileage of the IMU accelerometer.

[0053] The solution switching unit is used to switch the positioning solution from the combined solution process to the pure inertial group solution process when it is determined that the observation vector is unavailable and not to perform the Kalman filter solution.

[0054] The odometer fault isolation method and isolation device of the inertial combined positioning system of the embodiment of the present invention improves the settlement process. The error of the distance data is used to judge the rationality of the observation vector and form the fault isolation of the odometer signal, and at the same time form a switch to determine whether the settlement process requires filter intervention. Effectively prevent the output failure of the inertial combined positioning system caused by the rapid divergence of the positioning error. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 The figure shows a schematic diagram of the architecture of an inertial combined positioning system of an inertial group in the prior art.

[0056] Figure 2 FIG. 1 is a flow chart of an odometer fault isolation method of an inertial combined positioning system according to an embodiment of the present invention.

[0057] Figure 3 FIG. 1 is a schematic diagram of signal processing in an odometer fault isolation method of an inertial combined positioning system according to an embodiment of the present invention.

[0058] Figure 4FIG. 1 is a schematic diagram of the architecture of an odometer fault isolation device of an inertial combined positioning system according to an embodiment of the present invention. FIG. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solution and advantages of the present invention clearer and more understandable, the present invention is further described below in conjunction with the accompanying drawings and specific implementation methods. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of 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.

[0060] A method for isolating odometer faults in an inertial combined positioning system according to an embodiment of the present invention is as follows: Figure 2 As shown. Figure 2 In this embodiment, the present invention comprises:

[0061] Step 100: Establish a Kalman filter observation vector.

[0062] According to the position and velocity signal outputs of different types of positioning sensor devices used in the inertial combined positioning system, an error estimate is formed as an observation vector of the combined signal. The observation vector is used as the error data input of the Kalman filter to obtain a reliable estimate of the type signal error. In one embodiment of the present invention, the positioning sensor device uses an odometer and a strapdown inertial group.

[0063] Step 200: Collect sensor distance data for observation vector availability judgment.

[0064] Those skilled in the art will understand that if the error reflected by the observation vector is distorted, the state estimation result of the entire filter will be distorted, causing the error feedback obtained by the strapdown inertial group to be distorted. If it cannot be quickly identified, the positioning error will quickly diverge, and the inertial combination positioning system will fail to output misalignment. The corresponding distance data is formed according to the characteristics of the sensor to reflect the working status of the sensor.

[0065] Step 300: Determine the availability of the observation vector based on the error of the distance data, and determine the settlement process based on the availability.

[0066] The parallel availability of the sensor equipment is judged based on the error judgment, and the settlement data and settlement process are switched based on the judgment result and the reliability priority of the sensor equipment.

[0067] The odometer fault isolation method of the inertial combined positioning system of the embodiment of the present invention improves the settlement process of the existing combined positioning system. The error of the distance data is used to judge the rationality of the observation vector and form the fault isolation of the odometer signal, and at the same time, a switch is formed to determine whether the settlement process requires filter intervention. It effectively prevents the output failure of the inertial combined positioning system caused by the rapid divergence of the positioning error.

[0068] like Figure 2 As shown, in one embodiment of the present invention, step 100 includes:

[0069] Step 110: Establish an observation vector of the Kalman filter in the odometer / IMU combined positioning system for the odometer and IMU outputs according to the displacement component in the time period.

[0070] In one embodiment of the present invention, the Kalman filter observation vector Z k Defined as:

[0071] Z k =[D e -S e ,D n -S n ] T

[0072] Among them, T (unit: s, generally T=1 can be selected, and it can also be adjusted according to the situation of each system) is used as the calculation period, and the eastward component S of the odometer cumulative displacement is calculated respectively. e (unit: m) and the north component S n (Unit: m), the accumulated displacement D of the inertial group accelerometer in the east direction e (unit: m) and the cumulative displacement to the north D n (Unit: m).

[0073] The odometer fault isolation method of the inertial combined positioning system of the embodiment of the present invention forms an observation vector in a specific coordinate system according to the odometer and inertial group output in the system, providing a means of measuring small errors in the solution process.

[0074] like Figure 2 As shown, in one embodiment of the present invention, step 200 includes:

[0075] Step 210: Calculate the accumulated mileage of the odometer in the following manner:

[0076]

[0077] Where T (unit s) is the calculation period, S is the accumulated mileage of the odometer (unit m), △t (unit s) is the sampling period of the odometer pulse signal, N is the cumulative number of times, N = T / △t; kis the number of odometer pulses at the kth sampling moment (in m).

[0078] Step 220: Calculate the accumulated mileage of the IMU accelerometer in the following manner:

[0079]

[0080] Wherein, T (unit s) is the calculation period, D is the cumulative mileage of the IMU accelerometer (unit m), △t (unit s) is assumed to be the strapdown solution period of the IMU, N is the cumulative number of times, N = T / △t;

[0081] V ek is the eastward velocity of the inertial group at the kth sampling moment (in m / s);

[0082] V nk is the northward velocity of the inertial group at the kth sampling moment (in m / s).

[0083] The odometer fault isolation method of the inertial combined positioning system of the embodiment of the present invention obtains sensor data of different mileage accumulation paths in real time, thereby ensuring the synchronization of distance data collection.

[0084] like Figure 2 As shown, in one embodiment of the present invention, step 300 includes:

[0085] Step 310: Set a safety threshold for measuring the cumulative mileage error.

[0086] The safety threshold ε is used to measure the quantitative deviation between the dynamic mileage formed by different sensors. The sensitivity of error judgment is adjusted by the safety threshold.

[0087] Step 320: Determine the availability of the observation vector according to the following judgment rule:

[0088] |DS|<ε.

[0089] The judgment rule is used to reflect the availability of the observation vector through the accumulated mileage error.

[0090] Step 330: When it is determined that the observation vector is unavailable, the Kalman filter filtering solution is not performed, and the positioning solution is switched from the combined solution process to the pure inertial group solution process.

[0091] The odometer fault isolation method of the inertial combined positioning system of the embodiment of the present invention forms the basis for selecting the solution process, so that the fault isolation of the odometer signal can trigger in real time the switching of the positioning solution from the combined solution process to the pure inertial group solution process, thereby preventing the positioning error from diverging rapidly.

[0092] The signal processing process in the odometer fault isolation method of the inertial combined positioning system according to an embodiment of the present invention is as follows: Figure 3 As shown. Combined Figure 2 As shown, when judging the observation vector Z k When available, the Kalman filter is first used for filtering and solving, and then the error estimate of the filtering solution is input into the pure strapdown solver for pure strapdown inertial settlement. k When it is unavailable, the odometer mileage signal is first isolated according to the reliability of the inertial group IMU, and then the pure strapdown solver is directly used for pure strapdown inertial settlement. While isolating the fault of the odometer pulse signal immediately, the positioning system is switched from the combined positioning mode to the pure inertial positioning mode.

[0093] An odometer fault isolation device for an inertial combined positioning system according to an embodiment of the present invention comprises:

[0094] A memory, used to store program codes corresponding to the processing process of the odometer fault isolation method of the inertial combined positioning system of the above embodiment;

[0095] The processor is used to execute the program code corresponding to the processing process of the odometer fault isolation method of the inertial combined positioning system in the above embodiment.

[0096] The processor may be a DSP (Digital Signal Processor) digital signal processor, an FPGA (Field-Programmable Gate Array) field programmable gate array, an MCU (Microcontroller Unit) system board, a SoC (system on a chip) system board, or a PLC (Programmable Logic Controller) minimum system including I / O.

[0097] An odometer fault isolation device for an inertial combined positioning system according to an embodiment of the present invention is as follows Figure 4 As shown. Figure 4 In this embodiment, the present invention comprises:

[0098] An indicator setting module 10 is used to establish a Kalman filter observation vector;

[0099] A data acquisition module 20, for acquiring sensor distance data for observing vector availability judgment;

[0100] The indicator judgment module 30 is used to judge the availability of the observation vector according to the error of the distance data, and determine the settlement process according to the availability.

[0101] like Figure 4 As shown, in one embodiment of the present invention, the indicator setting module 10 includes:

[0102] The observation vector establishing unit 11 is used to establish the observation vector output by the Kalman filter for the odometer and the IMU in the odometer / IMU combined positioning system according to the displacement component in the time period.

[0103] like Figure 4 As shown, in one embodiment of the present invention, the data acquisition module 20 includes:

[0104] The odometer data collection unit 21 is used to calculate the accumulated mileage of the odometer in the following manner:

[0105]

[0106] Where T (unit s) is the calculation period, S is the accumulated mileage of the odometer (unit m), △t (unit s) is the sampling period of the odometer pulse signal, N is the cumulative number of times, N = T / △t; k is the number of odometer pulses at the kth sampling moment (in m);

[0107] The accelerometer data acquisition unit 22 is used to calculate the accumulated mileage of the IMU accelerometer in the following manner:

[0108]

[0109] Wherein, T (unit s) is the calculation period, D is the cumulative mileage of the IMU accelerometer (unit m), △t (unit s) is assumed to be the strapdown solution period of the IMU, N is the cumulative number of times, N = T / △t;

[0110] V ek is the eastward velocity of the inertial group at the kth sampling moment (in m / s);

[0111] V nk is the northward velocity of the inertial group at the kth sampling moment (in m / s).

[0112] like Figure 4 As shown, in one embodiment of the present invention, the indicator judgment module 30 includes:

[0113] A threshold setting unit 31, used to set a safety threshold for measuring the cumulative mileage error;

[0114] The error measurement unit 32 is used to determine the availability of the observation vector according to the following judgment rule:

[0115] |DS|<ε.

[0116] The solution switching unit 33 is used to not perform the filtering solution of the Kalman filter when it is determined that the observation vector is unavailable, and switch the positioning solution from the combined solution process to the pure inertial group solution process.

[0117] 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 changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A method for isolating odometer faults of an inertial combined positioning system, characterized in that: include: Establish Kalman filter observation vector; Collect sensor distance data for observation vector availability judgment; The availability of the observation vector is determined based on the error of the distance data, and the settlement process is determined based on the availability.

2. The odometer fault isolation method according to claim 1, characterized in that: The establishment of the Kalman filter observation vector comprises: According to the displacement components within a time period, the observation vector of the Kalman filter in the odometer / IMU combined positioning system is established for the odometer and IMU outputs.

3. The odometer fault isolation method according to claim 2, characterized in that: The observation vector is: Z k =[D e -S e ,D n -S n ] T Among them, T is the calculation period, and the eastward component of the odometer cumulative displacement is S e and the north component is S n , the accumulated displacement of the inertial group accelerometer in the east direction is D e The cumulative displacement to the north is D n .

4. The odometer fault isolation method according to claim 1, characterized in that: The sensor distance data collected to determine the availability of the observation vector includes: The mileage is calculated as follows: Where T is the calculation period, S is the accumulated mileage of the odometer, △t is the sampling period of the odometer pulse signal, N is the accumulated number of times, N = T / △t; k is the number of odometer pulses at the kth sampling moment; The accumulated mileage of the IMU accelerometer is calculated as follows: Where T is the calculation period, D is the accumulated mileage of the IMU accelerometer, △t is assumed to be the strapdown calculation period of the IMU, N is the accumulated number of times, N = T / △t; V ek is the eastward velocity of the inertial group at the kth sampling moment; V nk is the northward velocity of the inertial group at the kth sampling moment.

5. The odometer fault isolation method according to claim 1, characterized in that: The method of judging the availability of the observation vector according to the error of the distance data and determining the settlement process according to the availability includes: Set a safety threshold ε to measure the cumulative mileage error; The availability of the observation vector is determined according to the following judgment rules: |DS|<ε Among them, S is the accumulated mileage of the odometer, and D is the accumulated mileage of the IMU accelerometer; When it is determined that the observation vector is unavailable, the Kalman filter filtering solution is not performed, and the positioning solution is switched from the combined solution process to the pure inertial group solution process.

6. An odometer fault isolation device for an inertial combined positioning system, characterized in that: include: A memory, used to store a program code corresponding to a processing process of an odometer fault isolation method of an inertial combined positioning system as claimed in any one of claims 1 to 5; A processor is used to execute the program code.

7. An odometer fault isolation device for an inertial combined positioning system, characterized in that: include: Index setting module, used to establish Kalman filter observation vector; A data acquisition module, used to collect sensor distance data for observation vector availability judgment; The indicator judgment module is used to judge the availability of the observation vector according to the error of the distance data and determine the settlement process according to the availability.

8. The odometer fault isolation device of the inertial combined positioning system according to claim 7, characterized in that: The indicator setting module includes: The observation vector establishment unit is used to establish the observation vector output by the Kalman filter for the odometer and the inertial group IMU in the odometer / inertial group inertial combined positioning system according to the displacement component in the time period.

9. The odometer fault isolation device of the inertial combined positioning system according to claim 7, characterized in that: The data acquisition module comprises: The odometer data collection unit is used to calculate the accumulated mileage of the odometer in the following manner: Where T is the calculation period, S is the accumulated mileage of the odometer, △t is the sampling period of the odometer pulse signal, N is the accumulated number of times, N = T / △t; k is the number of odometer pulses at the kth sampling moment; The accelerometer data acquisition unit is used to calculate the cumulative mileage of the IMU accelerometer in the following way: Where T is the calculation period, D is the accumulated mileage of the IMU accelerometer, △t is assumed to be the strapdown calculation period of the IMU, N is the accumulated number of times, N = T / △t; V ek is the eastward velocity of the inertial group at the kth sampling moment; V nk is the northward velocity of the inertial group at the kth sampling moment.

10. The odometer fault isolation device of the inertial combined positioning system according to claim 7, characterized in that: The indicator judgment module includes: A threshold setting unit, used to set a safety threshold ε for measuring the cumulative mileage error; The error measurement unit is used to determine the availability of the observation vector according to the following judgment rules: |DS|<ε Among them, S is the accumulated mileage of the odometer, and D is the accumulated mileage of the IMU accelerometer. The solution switching unit is used to switch the positioning solution from the combined solution process to the pure inertial group solution process when it is determined that the observation vector is unavailable and not to perform the Kalman filter solution.

Images