Multi-sensor fusion inertial device calculation result degradation degree analysis method

Abstract

The invention relates to the technical field of inertial device calculation, and discloses a multi-sensor fusion inertial device calculation result degradation degree analysis method, which comprises the following steps: obtaining inertial sensor calibration data, establishing an error state model of an inertial navigation system in discrete time, and obtaining a system error state transition equation; establishing a multi-sensor combined system state solving model according to state variables selected in the system error state transition equation; constructing a local observable matrix based on an error state transition equation; inversely deducing an initial nominal state of the inertial navigation system based on the observable matrix; calculating the degradation degree of the inertial system based on the initial nominal state and the initial prediction state of the inertial navigation system; according to the method, the accuracy of the state estimation result of the inertial sensor can be reflected in real time, the calculation error of the inertial sensor can be accurately reflected, and the weight of the inertial sensor can be improved or reduced in the multi-sensor fusion process, so that the overall state estimation precision and robustness of the system are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial device solution, and in particular to a method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion. Background Art

[0002] Inertial sensors are widely used in the field of navigation, but there are many errors in inertial devices themselves, such as gyroscope drift and accelerometer bias. These error factors will cause the navigation and positioning errors of the inertial navigation system to accumulate over time, resulting in a decrease in the accuracy and reliability of the long-term operation of the inertial navigation system. Therefore, in actual use, multi-sensor fusion technology is often used to achieve combined navigation by multiple sensor systems to compensate for the errors of the inertial navigation system. However, it is inevitable that the inertial device will still reduce its own measurement accuracy in sports scenes such as curves and bumps, as well as its own system errors. Therefore, in the process of multi-sensor fusion, identifying the real-time operating status of the inertial sensor can better enable the system to reduce the noise impact caused by the degradation of the inertial sensor in the joint estimation stage.

[0003] At present, the research on the degradation state of inertial devices mainly focuses on its own system error and random error. Among them, random error has a greater impact on the results of positioning solution, so more and more multi-sensor fusion algorithms consider the impact of random errors of inertial devices on the system. Relevant research literature 1 (Forster C, Carlone L, Dellaert F, et al. On-manifold preintegration for real-time visual--inertial odometry [J]. IEEE Transactions on Robotics, 2016, 33 (1): 1-21.) proposed to add the random error of the inertial device to the state quantity of the fusion system, and correct the random error in combination with the inertial sensor pre-integration algorithm. This method can calculate the degradation state of the inertial device to a certain extent, but it does not take into account the external interference such as the severe vibration of the carrier and other factors that will cause deviations in the measurement results of the inertial device.

[0004] In addition, literature 2 (Shen K, Xia Y, Wang M, et al. Quantifying observability and analysis in integrated navigation [J]. Navigation: Journal of The Institute of Navigation, 2018, 65 (2): 169-181.) and other works have conducted in-depth research on observability theory and derived the real-time observability of the satellite-inertial navigation integrated navigation system to evaluate the degradation state of inertial devices. Based on this method, when the satellite signal is good, the random error of the inertial device and the error caused by external interference will be quantified. However, when the satellite status is poor, the real-time status of the inertial device will be estimated inaccurately. Summary of the invention

[0005] In view of the shortcomings of the prior art, an object of the present invention is to provide a method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion.

[0006] In order to achieve the above object, the present invention provides the following technical solutions:

[0007] The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion includes the following steps:

[0008] S1. Obtain inertial sensor calibration data, establish an error state model of the inertial navigation system under discrete time, and obtain the error state transfer equation of the inertial navigation system;

[0009] S2. Establishing a multi-sensor combined inertial navigation system state solution model according to the state variables selected from the inertial navigation system error state transfer equation;

[0010] S3, constructing a local observable matrix based on the error state transfer equation;

[0011] S4, inversely infer the initial nominal state of the inertial navigation system based on the observable matrix;

[0012] S5. Calculate the degradation degree of the inertial navigation system based on the initial nominal state and the initial predicted state of the inertial navigation system, and obtain the real-time degradation degree of the i-th variable of the inertial navigation system at time k in the form of:

[0013]

[0014] In the above formula, x i is the i-th component of the error state variable at time j, y i is the i-th component of the nominal error state at time j derived from system observations, is the variance of the measurement noise at time k, is the measurement noise variance at time k+n-1, and the two satisfy

[0015] In the present invention, preferably, in step S1, the error state transfer equation of the inertial navigation system at time k+1 is:

[0016]

[0017] in is the error state at time k, the I matrix is ​​the unit matrix, F is the state coefficient matrix, G is the process noise distribution matrix, Δt is the interval between two sampling actions of the inertial sensor, and w k The matrix is ​​the random error.

[0018] In the present invention, preferably, in step S2, according to the state variables selected in the error state transfer equation of the inertial navigation system, the auxiliary observation sensor B is selected, the inertial navigation system residual is designed, and a multi-sensor joint system state solution model is established:

[0019]

[0020] The first of these Observation for auxiliary sensor B Used to solve the inertial navigation system state X, where B represents a type of auxiliary sensor, which can also be other auxiliary sensors, and the second item is based on the inertial sensor IMU observation Solve for the inertial navigation system state X, r B represents the auxiliary sensor residual vector, r IMU Represents the residual vector of the inertial sensor IMU;

[0021] The solution result of this model is the inertial navigation system state X, and the corresponding inertial navigation solution result is x k Recursion is x k+n-1 , and the inertial navigation system state X satisfies the equation X=Hx k+n-1 +v k , where v k is the error introduced by the observation noise at time k, and the H matrix is ​​the observation matrix.

[0022] In the present invention, preferably, in step S3, the time interval T for the multi-sensor joint solution is set, then X=O k x k +V k ,

[0023]

[0024] Among them, kis the observable matrix of the inertial navigation recursive process starting at time k, where the first row is k

[0025] The time observation matrix H k , the remaining rows have a recursive relationship with the previous row F k , that is, the state coefficient matrix;

[0026]

[0027] Where V k is the total error from time k to time k+n-1. k is the error introduced by the observation noise at time k. The random error H is introduced from time k to time k+1 k+1 G k w k , and the random error will be recursively converted to H in the next iteration. k+ 1F k+1,k+2 G k w k .

[0028] In the present invention, preferably, in step S4, the steps are included:

[0029] S41. According to the pseudo-inverse transformation, we can get: is the pseudo-inverse of the observable matrix, V k is the total error from time k to time k+n-1. The nominal error state derived from system observations You can get y k The components of each row in are:

[0030]

[0031] where z k is the component of each column in the inertial navigation system state X, for The weight of each row;

[0032] S42. Similarly, the observation noise is: is the pseudo-inverse of the observable matrix, V k is the total error from time k to time k+n-1, ζ k The components of each row are:

[0033]

[0034] From the above formula, it can be deduced that the inertial navigation system observation noise The variance of satisfies the relationship:

[0035]

[0036] is the noise variance of the initial observation in the time interval T.

[0037] In the present invention, preferably, the error state of the inertial navigation system Contains position error, velocity error, attitude error, as well as accelerometer bias and gyroscope drift.

[0038] In the present invention, preferably, in step S1, the error variables of each inertial device of the inertial navigation system are selected as state variables, and then the continuous-time error model is discretized to obtain a discrete state space model:

[0039] x k+1 =(I+F·Δt)x k +G·Δt·w k ,

[0040] in is the error state at time k+1 is the error state at time k, the I matrix is ​​the unit matrix, F is the state coefficient matrix, G is the process noise distribution matrix, Δt is the interval between two sampling actions of the inertial sensor, and w k The matrix is ​​the random error.

[0041] In the present invention, preferably, in step S1, inertial sensor calibration data is acquired, including sensor measurement noise w and process noise distribution matrix G.

[0042] In the present invention, preferably, in step S2, the auxiliary observation sensor satisfies that the state variables have one or more of the states of the inertial navigation system.

[0043] In the present invention, preferably, in step S4, the calculation process of the pseudo inverse matrix can be expressed as:

[0044]

[0045] Compared with the prior art, the present invention has the following beneficial effects:

[0046] The inertial sensor degradation degree detection method provided by the method of the present invention can reflect the accuracy of the inertial sensor state estimation result in real time, can accurately reflect the inertial sensor calculation error, and is helpful to increase or decrease the inertial sensor weight in the multi-sensor fusion process, thereby improving the overall state estimation accuracy and robustness of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 The figure is a flow chart of the method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to the present invention.

[0048] Figure 2 The motion route map and motion scene map of the inertial sensor according to the embodiment of the present invention.

[0049] Figure 3 Schematic diagram of the process of solving state quantities by multi-sensor joint optimization according to the present invention.

[0050] Figure 4 It is the position error obtained by comparing the inertial sensor calculation result obtained by the degradation degree analysis method of the inertial device solution result of multi-sensor fusion described in the present invention with the high-precision RTK acquisition result.

[0051] Figure 5 It is the degradation state of the inertial sensor obtained by the degradation degree analysis method of the inertial device solution result of multi-sensor fusion described in the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions 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. 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.

[0053] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more related listed items.

[0054] See also Figure 1 A preferred embodiment of the present invention provides a method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion, comprising the steps of:

[0055] , obtain the calibration data of the inertial sensor, establish the error state model of the inertial navigation system under discrete time, and obtain the system error state transfer equation;

[0056] S2. Establishing a state solution model of a multi-sensor combined inertial navigation system according to state variables selected from the system error state transfer equation;

[0057] S3, constructing a local observable matrix based on the error state transfer equation;

[0058] S4, inversely infer the initial nominal state of the inertial navigation system based on the observable matrix;

[0059] S5. Calculate the degradation degree of the inertial system based on the initial nominal state and the initial predicted state of the inertial navigation system, and obtain the real-time degradation degree of the i-th variable of the inertial navigation system at time k in the form of:

[0060]

[0061] In the above formula, x i is the i-th component of the error state variable at time j, y i It is the i-th component of the nominal error state at time j derived from system observations. is the variance of the measurement noise at time k, is the measurement noise variance at time k+n-1, and the two satisfy

[0062] In this embodiment, the vehicle positioning state is estimated by combining the visual camera and the inertial sensor. The positioning coordinate system is selected as the northeast coordinate system. Assuming that the vehicle motion trajectory is as follows Figure 2 As shown in the figure, the route includes scenes such as vehicle turning, accelerating, decelerating, and stopping. The inertial sensor data undergoes significant changes. This method can be used for analysis. The specific steps are as follows:

[0063] Step S1, obtain the calibration data of the inertial sensor, that is, obtain the initial wk random error matrix through the calibration method, and establish the error state model of the inertial navigation system in discrete time as follows:

[0064] x 15×1 =[δL 3×1 δV 3×1 δθ 3×1 b a 3×1 b g 3×1 ] T The internal parameters are position error, velocity error, attitude error, accelerometer bias and gyroscope drift, which are all 0 at the beginning. As Δt increases, they are calculated by the state transfer equation (1).

[0065] The error state transfer equation of the inertial navigation system is:

[0066]

[0067] in is the error state at time k, the I matrix is ​​the unit matrix, F is the state coefficient matrix, G is the process noise distribution matrix, Δt is the interval between two sampling actions of the inertial sensor, and the Wk matrix is ​​the random error;

[0068] The state coefficient matrix of the inertial sensor is:

[0069]

[0070] The neutron matrix in the above formula The state vector can be transformed from the body coordinate system to the navigation coordinate system. G is the process noise distribution matrix, which contains the variance related to the state vector.

[0071]

[0072] The error model in discrete form can be expressed as:

[0073] x k+1 =(I+F·Δt)x k +G·Δt·w k

[0074] Δt is the interval between two sampling actions of the inertial sensor. So we can get:

[0075]

[0076] The observation model in discrete form can be expressed as:

[0077] z k+1 =H k+1 x k+1 +v k+1 (2)

[0078] is the process noise vector, where n v With n θ is the process residual noise n v With n θ is the sensor measurement noise, which is usually modeled as Gaussian white noise with mean 0. The dynamic coefficient matrix F is the error model of the inertial navigation system state vector x.

[0079] Step S2: Based on the state variables of the selected inertial sensor and visual camera, the inertial navigation system residual is designed, and a multi-sensor combined inertial navigation system state solution model is established:

[0080]

[0081] The first one is the auxiliary sensor, namely the visual camera (camera) observation Solve the state variable X of the inertial navigation system. The second term is based on the observation of the inertial sensor IMU Solve the inertial navigation system state variable X, r represents the residual vector;

[0082] The solution of this model is the inertial navigation system state X. The corresponding inertial navigation solution is obtained by recursively deducing x k For x k+n-1, and satisfy the equation X=Hx k+n-1 +v k , where v k The error introduced by the observation noise.

[0083] Step S3: The joint solution process of camera observation and inertial navigation system solution is as follows: Figure 3 As shown, the time interval T for the multi-sensor joint solution is set, and the system state during this period and the predicted value of the inertial sensor system satisfy the equation: X = O k x k +V k ,

[0084]

[0085] Among them, k is the observable matrix of the inertial navigation recursive process starting at time k, where the first row is the observation matrix H at time k k , the remaining rows have a recursive relationship with the previous row F k , which is the state coefficient matrix.

[0086]

[0087] Where V k is the total error from time k to time k+n-1. k is the error introduced by the observation noise at time k. The random error H is introduced from time k to time k+1 k+1 G k w k , and the random error will be recursively converted to H in the next iteration. k+ 1F k+1,k+2 G k w k .

[0088] Step S4: According to the pseudo-inverse transformation, we can obtain: in, For O k The inverse operation of the matrix is ​​obtained, V k is the total error from time k to time k+n-1, and the nominal error state derived from system observation You can get y k The components of each row in are:

[0089]

[0090] where z k is the component of each column in the inertial navigation system state matrix X, for The weight of each row;

[0091] Step S5: Similarly, the observed noise is: ζ k The components of each row are:

[0092]

[0093] From the above formula, it can be deduced that the inertial navigation system observation noise The variance of satisfies the following relationship:

[0094]

[0095] is the noise variance of the initial observation in the time interval T;

[0096] Step S6: The real-time degradation degree of the inertial navigation system observation result can be finally obtained as:

[0097]

[0098] In the above formula, x i is the component of the error state variable of the inertial navigation system, y i It is the nominal result of the error state variable derived from the inertial navigation system observation. The calculation results of the inertial sensor are compared with the results obtained by high-precision RTK observation, and the position error calculated by the inertial sensor is obtained. Four areas (attached Figure 2 ) to verify whether the degradation model of the present invention successfully reflects the area where the inertial sensor calculation error is large.

[0099] Figure 4 represents the position error calculated by the inertial sensor, Figure 5 Represents the calculated degree of degradation of the inertial sensor. The road surface in area 1 is rough and uneven, and a large vibration will occur when the vehicle passes by. The position error calculated by the inertial sensor is relatively high in this area, and the degradation model of the present invention can reflect the high level of degradation here. Area 2 is a curve with a larger radius. When the vehicle passes by at a higher speed, a very large lateral acceleration change occurs, so the degradation degree of this area rises rapidly. When the vehicle stops at area 3 and waits for the traffic light, the degradation degree drops to a relatively low level. Area 4 is also a curve scene. Compared with area 2, the turning radius is smaller but the passing speed is slower, so the degradation degree and position error here are slightly lower than those in area 2. The degradation model of the present invention successfully reflects the phenomenon that the solution results of the inertial sensor in the degraded scene are prone to drift.

[0100] In some other preferred embodiments of the present invention, a computer-readable storage medium is provided, storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the method described in the above embodiment.

[0101] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc., which can store program codes.

[0102] The above description is a detailed description of the preferred feasible embodiments of the present invention, but the embodiments are not intended to limit the scope of the patent application of the present invention. All equivalent changes or modified changes completed under the technical spirit suggested by the present invention should fall within the patent scope covered by the present invention.

Claims

1. A method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion, characterized in that: Includes steps: S1. Obtain inertial sensor calibration data, establish an error state model of the inertial navigation system in discrete time, and obtain the system error state transfer equation; S2. Establishing a state solution model of a multi-sensor combined inertial navigation system according to state variables selected from the system error state transfer equation; S3, constructing a local observable matrix based on the error state transfer equation; S4, inversely infer the initial nominal state of the inertial navigation system based on the observable matrix; S5. Calculate the degradation degree of the inertial system based on the initial nominal state and the initial predicted state of the inertial navigation system, and obtain the real-time degradation degree D of the i-th variable of the inertial navigation system at time k. k i The form is: In the above formula, x i is the i-th component of the error state variable at time j, y i is the i-th component of the nominal error state at time j derived from system observations, is the variance of the measurement noise at time k, is the measurement noise variance at time k+n-1, and the two satisfy 2. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 1 is characterized in that: In step S1, the system error state transfer equation at time k+1 is: in is the error state at time k, I matrix is ​​the unit matrix, F is the state coefficient matrix, G is the process noise distribution matrix, Δt is the interval between two sampling actions of the inertial sensor, and w k The matrix is ​​the random error.

3. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 2 is characterized in that: In step S2, according to the state variables selected in the system error state transfer equation, the auxiliary observation sensor is selected, the inertial navigation system residual is designed, and a multi-sensor joint inertial navigation system state solution model is established: The first of these The auxiliary sensor observes Solve for the inertial navigation system state X, the second term Based on the inertial sensor observation Solve for the inertial navigation system state X, r B represents the auxiliary sensor residual vector, r IMU Represents the residual vector of the inertial sensor IMU; The solution result of the state solution model is the inertial navigation system state X. The corresponding inertial navigation solution result is recursively x k For x k+n-1 , and the inertial navigation system state X satisfies the equation X=Hx k+n-1 +v k , where v k is the error introduced by the observation noise at time k, and the H matrix is ​​the observation matrix.

4. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 1 is characterized in that: In step S3, the time interval T for the multi-sensor joint solution is set, then X=O k x k +V k , Among them, k is the observable matrix of the inertial navigation recursive process starting at time k, where the first row is the observation matrix H at time k k , the remaining rows have a recursive relationship with the previous row F k , that is, the state coefficient matrix; Where V k is the total error from time k to time k+n-1, where v k is the error introduced by the observation noise at time k. The random error H will be introduced from time k to time k+1. k+1 G k w k , and the random error will be recursively converted to H in the next iteration. k+ 1F k+1,k+2 G k w k .

5. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 1, characterized in that: In step S4, the steps are included: S41. According to the pseudo-inverse transformation, we can get: is the pseudo-inverse of the observable matrix, V k is the total error from time k to time k+n-1, and the nominal error state derived from system observation You can get y k The components of each row in are: ， where z k is the component of each column in the inertial navigation system state X, for The weight of each row; S42, observation noise is: is the pseudo-inverse of the observable matrix, V k is the total error from time k to time k+n-1, ζ k The components of each row are: From the above formula, it can be deduced that the inertial navigation system observation noise The variance of satisfies the relationship: is the noise variance of the initial observation in the time interval T.

6. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 2 is characterized in that: Error states of inertial navigation systems Contains position error, velocity error, attitude error, as well as accelerometer bias and gyroscope drift.

7. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 2 is characterized in that: In step S1, the error variables of each inertial device of the inertial navigation system are selected as state variables, and then the continuous-time error model is discretized to obtain a discrete state space model: x k+1 =(I+F·Δt)x k +G·Δt·w k , in is the error state at time k+1 is the error state at time k, the I matrix is ​​the unit matrix, F is the state coefficient matrix, G is the process noise distribution matrix, Δt is the interval between two sampling actions of the inertial sensor, and w k The matrix is ​​the random error.

8. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 1, characterized in that: In step S1, inertial sensor calibration data is obtained, including sensor measurement noise w and process noise distribution matrix G.

9. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 3 is characterized in that: In step S2, the auxiliary observation sensor satisfies one or more of the inertial navigation system states in the solution result.

10. The method for analyzing the degradation degree of inertial device solution results of multi-sensor fusion according to claim 5, characterized in that: In step S4, the calculation process of the pseudo-inverse matrix can be expressed as:

Images