Sequential Kalman filtering degradation integrated navigation method

Abstract

The invention discloses a sequential Kalman filtering degradation integrated navigation method. The method comprises the following steps: resolving and outputting an attitude angle by a micro electro mechanical inertial navigation system; performing sequential Kalman filtering information fusion by using position and speed information output by the satellite navigation module; a speed measurement residual error is monitored in real time, and if a fault occurs when the speed measurement residual error exceeds a threshold value, the speed measurement residual error is degraded into position measurement Kalman filtering; and after the fault is recovered, speed and position Kalman filtering is restored. Compared with Kalman filtering in a traditional fixed measurement mode, the technical scheme provided by the invention has the advantages that the adaptability to speed measurement errors is higher, the algorithm complexity is lower, the possibility of divergent collapse of the whole integrated navigation system is reduced, and the reliability of the navigation system is improved.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, and in particular relates to a sequential Kalman filtering degradation combined navigation method. Background Art

[0002] The application of inertial navigation / satellite combined navigation in long-endurance aircraft has important research value. Inertial navigation systems (INS) have short output cycles and strong autonomy, but poor long-term accuracy. Satellite navigation systems, on the other hand, have errors that do not diverge over time, but have long output cycles and are susceptible to interference.

[0003] When the integrated navigation system is working, the speed and position information of the carrier output by the satellite navigation system is combined with the attitude angle, speed and position information output by the inertial navigation system through the Kalman filter algorithm to obtain the attitude angle, position and speed after the integrated navigation solution.

[0004] The sequential Kalman filter separates the velocity measurement matrix from the position measurement matrix and updates the velocity measurement and position measurement in sequence, which can reduce the dimension of the Kalman filter matrix and reduce the calculation time.

[0005] The hardware part of the micro inertial navigation / satellite combined navigation device involves micro-electromechanical gyroscope, micro-electromechanical accelerometer, satellite navigation module, and satellite navigation antenna.

[0006] When a micro-insulator / satellite navigation system is operating, due to the poor pure inertial accuracy of the micro-insulator, the satellite navigation velocity and position must be incorporated into Kalman filtering measurements to correct the micro-insulator's attitude, velocity, and position. Since satellite navigation velocity noise is high and can exceed noise specifications when subjected to interference, if the integrated navigation system continues to operate with the original Kalman filter parameters, velocity errors will affect the estimated attitude error, thereby affecting flight stability.

[0007] The doctoral dissertation "Research on Inertial / Satellite / Vision Integrated Navigation Technology for Small UAVs" (Qu Yunjie, Harbin Institute of Technology, June 2020, pp. 46-59) designed an observation noise adaptive UKF algorithm, which can count the observation noise as a residual matrix with a window width. However, the algorithm needs to know the statistical characteristics of the observation noise of the satellite navigation system and the inertial navigation system in advance. Its effectiveness is questionable when the satellite navigation speed fails and the speed error is large (such as greater than 3m / s) or the inertial navigation system fails and the attitude error is large; and the calculation is large and time-consuming.

[0008] The master's thesis "Research on GNSS / INS Integrated Navigation Performance Improvement Technology" (Zhou Xianlin, National Time Service Center, Chinese Academy of Sciences, September 2020, pp. 39-41) proposed a measurement noise variance matrix detection method, which reduces the impact of measurement failures by adjusting the value of the R matrix. However, when the measurement failure value is extremely large, the abnormal velocity error will still enter the Kalman filter of the integrated navigation, causing a certain impact. Summary of the Invention

[0009] To overcome the shortcomings of existing technologies, the present invention provides a sequential Kalman filter downgraded integrated navigation method, including a micro-electromechanical inertial navigation system (MEMS) solution that outputs attitude angles; using the position and velocity information output by the satellite navigation module for sequential Kalman filter information fusion; real-time monitoring of velocity measurement residuals. If a fault occurs when the residual exceeds a threshold, downgrade to a position measurement Kalman filter; and after fault recovery, restore to a velocity and position Kalman filter. Compared to traditional fixed-measurement Kalman filters, the technical solution of the present invention is more adaptable to velocity measurement errors, has a lower algorithmic complexity, reduces the possibility of divergence and collapse of the entire integrated navigation system, and increases the reliability of the navigation system.

[0010] The technical solutions adopted by the present invention to solve the technical problems are as follows:

[0011] Step 1: The satellite navigation system successfully locates and outputs speed and position; the micro inertial navigation system performs pure inertial solution;

[0012] Step 2: After the carrier is launched, the speed and position sequence is used to conduct Kalman filter combined navigation;

[0013] Step 3: Detect velocity measurement residual dZv:

[0014]

[0015] in, are the north celestial east speed of the integrated navigation system, They are the north celestial east speed of the satellite navigation system;

[0016] Step 4: When the velocity measurement residual dZv exceeds the threshold VRerr, the navigation system is downgraded to the position measurement Kalman filter integrated navigation;

[0017] Step 5: The velocity measurement residual dZv does not exceed the threshold VRerr, and the navigation system reverts to the velocity and position measurement Kalman filter combined navigation;

[0018] Step 6: Repeat steps 4 and 5.

[0019] Preferably, the step 1 is specifically:

[0020] The navigation device is powered on when it is stationary on the ground. The outputs of the micro-electromechanical gyroscope and accelerometer are converted into angular velocity and acceleration, and inertial navigation solution is performed for a certain period. After the satellite navigation system locates, the initial position and velocity are bound to obtain the projection of the rotational angular velocity of the navigation system relative to the inertial system under the carrier system.

[0021] Assume that the specific force under the load system corresponding to the accelerometer is f b , the angular velocity of the carrier system corresponding to the gyroscope is The transformation matrix from the carrier system to the navigation system is obtained by initial alignment Corresponding to the attitude quaternion Q; leading to the specific force and angular velocity f in the navigation system n 、 By f n Subtract the gravitational acceleration g to get the acceleration a in the navigation system n , by a n Integrate over time to get the velocity in the navigation system, and integrate again to get the position in the navigation system; and have to The inertial navigation solution cycle is DT1, which is determined by and DT1 to obtain the angular increment during the solution time Then we can get the attitude error angle correction quaternion △Θ, which is multiplied by the original attitude quaternion to get the updated attitude quaternion and

[0022] A computer program enables a computer to execute the above-mentioned sequential Kalman filtering degradation integrated navigation method.

[0023] An electronic device comprises: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device performs the above-mentioned sequential Kalman filtering degraded combined navigation method.

[0024] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the above-mentioned sequential Kalman filtering degraded integrated navigation method.

[0025] A chip includes: a processor for calling and running a computer program from a memory, so that a device equipped with the chip executes the above-mentioned sequential Kalman filtering degradation combined navigation method.

[0026] A computer program product includes a computer storage medium storing a computer program, wherein the computer program includes instructions that can be executed by at least one processor, and when the instructions are executed by the at least one processor, the above-mentioned sequential Kalman filter degradation integrated navigation method is implemented.

[0027] The beneficial effects of the present invention are as follows:

[0028] Compared to existing technologies, the technical solution of the present invention can achieve the following beneficial effects: it detects the velocity error of the satellite navigation system. If the error exceeds a threshold, the velocity measurement is deemed to have failed and the velocity measurement is disconnected. Compared to traditional sequential velocity and position integrated navigation systems, even in the event of a satellite navigation system velocity failure or a temporary abnormal output from the inertial navigation system that causes a velocity output failure, the position measurement can still be protected, ensuring stable operation of the integrated navigation system and overcoming the failure. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0030] The present invention will be further described below with reference to the accompanying drawings and examples.

[0031] When using micro-insulator / satellite navigation combined navigation, the velocity error of the satellite navigation system may occasionally increase abnormally beyond the specification range. The micro-insulator system may also experience temporary failure, resulting in a large velocity error, which in turn causes the attitude error of the entire navigation system to fluctuate greatly or even filter divergence.

[0032] The present invention is to determine whether to perform speed measurement by judging the measurement residual between the speed of the satellite navigation system and the speed of the integrated navigation system, and then to eliminate the erroneous speed input when a fault occurs as much as possible while ensuring the accuracy of the integrated navigation speed.

[0033] (2) Technical solution

[0034] To solve the above technical problems, the present invention provides a sequential Kalman filter degradation integrated navigation method. The method comprises the following steps:

[0035] Step 1: The satellite navigation system successfully locates and outputs speed and position; the micro inertial navigation system performs pure inertial solution.

[0036] Step 2: After the carrier is launched, the speed and position sequence is used to conduct Kalman filter combined navigation;

[0037] Step 3: Detect the velocity measurement residual dZv.

[0038]

[0039] in, are the north celestial east speed of the integrated navigation system, They are the north celestial east speed of the satellite navigation system, and the unit is m / s.

[0040] Step 4: When the velocity measurement residual dZv exceeds the threshold VRerr, the navigation system is downgraded to the position measurement Kalman filter integrated navigation.

[0041] Step 5: The velocity measurement residual dZv does not exceed the threshold VRerr, and the navigation system reverts to the velocity and position measurement Kalman filter combined navigation.

[0042] Step 6: Repeat steps 4 and 5.

[0043] Example:

[0044] This embodiment specifically describes a method for initial heading binding for inertial / satellite integrated navigation proposed by the present invention, which is applied to a navigation device in a miniaturized unmanned aerial vehicle. The navigation device includes a micro-electromechanical system gyroscope, an accelerometer, a satellite navigation module, a microprocessor, an acquisition circuit, and the like.

[0045] Step 1: The navigation device is powered on while stationary on the ground. The outputs of the MEMS gyroscope and accelerometer are converted into angular velocity and acceleration, and inertial navigation is calculated for a certain period. After the satellite navigation system locates the device, the initial position and velocity are bound. The projection of the angular velocity of the navigation system relative to the inertial system under the carrier system is obtained.

[0046] Assume that the specific force under the load system corresponding to the accelerometer is f b , the angular velocity of the carrier system corresponding to the gyroscope is The transformation matrix from the carrier system to the navigation system is obtained by initial alignment (corresponding to the attitude quaternion Q). From the above, we can get the specific force and angular velocity f in the navigation system n 、 By f n Deducting the gravitational acceleration g, we can get the acceleration a in the navigation system n By a n By integrating over time, we can get the velocity in the navigation system, and integrating again, we can get the position in the navigation system. and Available The inertial navigation solution cycle is DT1. and DT1 can be used to obtain the angular increment within the solution time Then we can get the attitude error angle correction quaternion △Θ, which can be multiplied by the original attitude quaternion to get the updated attitude quaternion and

[0047] Step 2: The satellite navigation module outputs position and speed. The output period is DT2.

[0048] Step 3: Perform sequential Kalman filtering on the attitude angle obtained by the inertial navigation solution in step 1 and the speed and position obtained in step 2 to output the combined attitude (including attitude quaternion), position, and speed.

[0049] Step 4: The vehicle takes off / is launched, and the integrated navigation system starts calculating the velocity measurement residual dZv.

[0050] Step 5: Detect that the velocity residual dZv is 3.5 m / s, disconnect the velocity measurement, and perform only position measurement.

[0051] Step 6: Detect the velocity residual dZv to be 0.8 m / s, resume velocity measurement, and perform sequential velocity and position measurements.

[0052] The present invention utilizes the characteristic that the speed measurement residual between the satellite navigation speed and the integrated navigation speed increases sharply when the satellite navigation speed output fails or the inertial navigation speed output fails, determines the speed measurement failure, and downgrades the integrated navigation system to a Kalman filter that only measures position, thereby reducing the risk of instability or even divergence of the entire navigation system due to speed measurement failure.

Claims

1. A sequential Kalman filter degradation integrated navigation method, characterized in that: The steps include: Step 1: The satellite navigation system successfully locates and outputs speed and position; the micro inertial navigation system performs pure inertial solution; Step 2: After the carrier is launched, the speed and position sequence is used to conduct Kalman filter combined navigation; Step 3: Detect velocity measurement residual dZv: in, are the north celestial east speed of the integrated navigation system, They are the north celestial east speed of the satellite navigation system; Step 4: When the velocity measurement residual dZv exceeds the threshold VRerr, the navigation system is downgraded to the position measurement Kalman filter integrated navigation; Step 5: The velocity measurement residual dZv does not exceed the threshold VRerr, and the navigation system reverts to the velocity and position measurement Kalman filter combined navigation; Step 6: Repeat steps 4 and 5.

2. A sequential Kalman filter degradation integrated navigation method according to claim 1, characterized in that: The step 1 is specifically as follows: The navigation device is powered on when it is stationary on the ground. The outputs of the micro-electromechanical gyroscope and accelerometer are converted into angular velocity and acceleration, and inertial navigation solution is performed for a certain period. After the satellite navigation system locates, the initial position and velocity are bound to obtain the projection of the rotational angular velocity of the navigation system relative to the inertial system under the carrier system. Assume that the specific force under the load system corresponding to the accelerometer is f b , the angular velocity of the carrier system corresponding to the gyroscope is The transformation matrix from the carrier system to the navigation system is obtained by initial alignment Corresponding to the attitude quaternion Q; leading to the specific force and angular velocity f in the navigation system n 、 By f n Subtract the gravitational acceleration g to get the acceleration a in the navigation system n , by a n Integrate over time to get the velocity in the navigation system, and integrate again to get the position in the navigation system; and have to The inertial navigation solution cycle is DT1, which is determined by and DT1 to obtain the angular increment during the solution time Then we can get the attitude error angle correction quaternion △Θ, which is multiplied by the original attitude quaternion to get the updated attitude quaternion and 3. A computer program, characterized in that The computer program enables a computer to execute the method according to any one of claims 1 to 2.

4. An electronic device, characterized in that: include: processor and memory; The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device performs the method according to any one of claims 1 to 2.

5. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 2 is implemented.

6. A chip, characterized in that: include: A processor, configured to call and run a computer program from a memory, so that a device equipped with the chip executes the method according to any one of claims 1 to 2.

7. A computer program product, characterized in that The computer program product comprises a computer storage medium storing a computer program, wherein the computer program comprises instructions executable by at least one processor, and when the instructions are executed by the at least one processor, the method according to any one of claims 1 to 2 is implemented.

Images