An error estimation method based on motion-assisted inertial navigation
Through the motion-assisted inertial navigation error estimation method, combined with zero speed and incompleteness constraint information, the Kalman filter equation is used to detect the vehicle's motion state, which solves the navigation error accumulation of the strap-inner inertial navigation system in complex environments and the interference of satellite navigation in satellite navigation in complex environments. The problem of this achieves high-precision autonomous navigation of vehicles in complex environments.
Patent Information
- Application Number
- CN202210896854.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-07-28
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2042-07-28
AI Technical Summary
In complex electromagnetic environments, the navigation error of the flash-connected inertial navigation system accumulates over time, and the navigation accuracy in the dynamic environment diverges. The satellite navigation system is affected by signal interference, occlusion and deception attacks, and cannot work effectively, resulting in the on-board equipment in a complex environment. The autonomous navigation capability is limited.
Using the inertial navigation error estimation method based on motion assistance, by establishing a state space vector and autonomous navigation model, combining zero-speed constraints and incompleteness constraints, using the Kalman filtering equation to perform error estimation, detect the vehicle's motion state, eliminate abnormal interference, and improve The stability of the navigation system.
It effectively avoids side slip and jumping caused by car maneuvering, improves the stability and navigation accuracy of the navigation system, ensures the vehicle's autonomous navigation ability in complex environments, and improves the reliability of the system.
Smart Images

Figure CN115290082B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the technical field of inertial navigation and combined navigation, and relates to an error estimation method based on motion-assisted inertial navigation. Background Art
[0002] Since the beginning of the 21st century, the war environment has gradually entered the information age, and mastering accurate information has become the key to information warfare. As an information assurance equipment for navigation and positioning, the Strapdown Inertial Navigation System (SINS) has the characteristics of high autonomy, concealment and stability, and has the advantages of providing all-round navigation information and good short-term accuracy. However, its navigation error accumulates over time, and in a dynamic environment, it will stimulate various errors and cause the navigation accuracy to diverge rapidly. Under normal circumstances, the combination of the strapdown inertial navigation system and the satellite navigation system can achieve the characteristics of complementing each other's strengths and weaknesses, and can significantly improve the positioning capability of the navigation system. However, in a complex electromagnetic environment, the various working performances of the satellite navigation system will be severely weakened, and its navigation performance will be subject to factors such as signal interference, building obstruction, and deception attacks, making it unable to work. Summary of the invention
[0003] In order to meet the use requirements of vehicle-mounted positioning and orientation equipment, the present invention regards the zero speed constraint (TZV, Tri-axis Zero Velocity) information when the vehicle is parked and the non-holonomic constraint (NHC, Non-Holonomic Constraints) information when the vehicle is driving as auxiliary motion constraint information, and proposes an error estimation method based on motion-assisted inertial navigation, which is of great significance to improving the autonomous navigation capability of land-based vehicle-mounted equipment in complex environments and has high engineering practical value.
[0004] The technical solution adopted by the present invention is:
[0005] An error estimation method based on motion-assisted inertial navigation comprises the following steps:
[0006] Step 1: Establish the state space vector according to the motion-assisted navigation principle, and establish the SINS / TZV / NHC autonomous navigation state space model according to the motion-assisted navigation principle, including the state equation and measurement equation. The measurement equation includes the zero-speed constraint measurement equation and the non-holonomic constraint measurement equation;
[0007] Step 2: Based on the original information output by the inertial component and the navigation information output by the inertial navigation system, a detection method for the zero-speed motion state is established; wherein the detection method for the original information is a threshold analysis of the original data results of the gyro accelerometer within the timing period, and the detection method for the navigation information is a threshold analysis of the heading, speed or position data of the navigation results within the timing period;
[0008] Step 3: In view of the jumping phenomenon of the vehicle under large maneuvering conditions and the side sliding phenomenon of the vehicle under fast azimuth maneuvering conditions, a detection method for non-holonomic motion constraints is established based on the threshold setting of the celestial accelerometer information and celestial gyroscope information output by the inertial component;
[0009] Step 4: According to the detection method of the zero-speed motion state established in step 2 and the detection method of the incomplete motion constraint established in step 3, determine the motion state of the system, select the corresponding measurement equation according to the motion state, establish a complete Kalman filter equation, and obtain the error value in the state space vector.
[0010] Furthermore, step 1 specifically includes the following steps:
[0011] Step 1.1: Establish the state space vector X according to the motion-assisted navigation principle TH , including the attitude error φ n , speed error Position error δp, gyro bias error ε b , Add zero bias error Installation error δα θ and δα ψ And space arm
[0012] Step 1.2: Establish the state equation based on the established state space vector and system error transmission relationship;
[0013] Step 1.3: Considering that the zero-speed constraint information is valid when the vehicle is stationary, the difference between the east, north, and sky speeds calculated by SINS in real time and the zero-speed constraint information is used as the measurement, and the zero-speed constraint measurement equation is established using the state space vector, system observation matrix, and observation noise matrix;
[0014] Step 1.4: Considering that the non-integral motion constraint information is valid when the vehicle has no sideslip, jump, or rapid azimuth maneuver, the x-axis and z-axis velocities of the carrier solved by SINS in real time are calculated in O H The difference between the projection of the point and the lateral and vertical zero-speed constraint information of the vehicle is taken as the measurement, and the non-holonomic constraint measurement equation is established using the state space vector, the non-holonomic motion constraint observation matrix and the observation noise matrix.
[0015] Furthermore, step 2 specifically includes the following steps:
[0016] Step 2.1: Introduce sliding window [t j-1 ,t j ] and a reasonable threshold as the zero speed criterion, the heading constraint criterion includes two sub-criteria, as follows:
[0017]
[0018] In the formula, and They represent the sliding window [t j-1 ,t j ] The maximum and minimum values of the heading angle are taken as t j -t j-1 is the measurement period T of the SINS / TZV autonomous navigation system M =t j -t j-1 , Δψ Th is the sliding window [t j-1 ,t j ]; where C12 is the critical state of the heading angle definition domain. Before each measurement update, if one of the conditions of C11 and C12 is met, it is considered that the SINS meets the heading constraint criterion. The heading constraint criterion establishment condition is abbreviated as C11||C12;
[0019] Step 2.2: Note the sliding window [t j-1 ,t j ] i The analog of the gyro angular velocity information at the moment is The analog of the accelerometer force information is
[0020] The sliding window [t j-1 ,t j ] The maximum and average values of the gyro and accelerometer outputs and a reasonable threshold are used as the zero-speed criterion. The inertial information constraint criterion includes four sub-criteria, as follows:
[0021]
[0022] In the formula, and They represent the sliding window [t j-1 ,t j ]The maximum and average modulus of the internal gyro output; ω Th,max and ω Th,mean They represent the sliding window [t j-1 ,t j ] set within the maximum angular velocity modulus threshold and the average angular velocity modulus threshold; and They represent the sliding window [t j-1 ,t j ] The maximum and average modulus values of the accelerometer output; f Th,max and f Th,mean They represent the sliding window [t j-1 ,t j]; if C21, C22, C23 and C24 are met at the same time, it is considered that SINS meets the inertial information constraint criterion. The inertial information constraint criterion is abbreviated as C21&C22&C23&C24;
[0023] Step 2.3: The conclusion of zero-speed detection is determined by using the two criteria of step 2.1 and step 2.2 to be established separately or by using the two criteria to be established simultaneously.
[0024] Furthermore, the specific process of step 3 is as follows:
[0025] In view of the jumping phenomenon of the vehicle under large maneuvering conditions and the side sliding phenomenon of the vehicle under fast azimuth maneuvering conditions, an effective criterion for the vehicle's non-integrity constraint information is established as follows:
[0026]
[0027] In the formula, Indicates t i The measured specific force of the z-axis accelerometer at the sampling moment; and They represent the minimum threshold and maximum threshold of the specific force measured by the z-axis accelerometer at each sampling moment respectively; Indicates t i The absolute value of the angular velocity measured by the z-axis gyroscope at the sampling moment; It represents the maximum threshold of the z-axis gyro measurement angular velocity setting at each sampling moment; before each measurement update, if C31 and C32 are both met, it is considered that SINS meets the non-integrity constraint condition detection, and the detection condition is abbreviated as C31&C32.
[0028] The advantages of the method of the present invention compared with the prior art are:
[0029] (1) The present invention realizes quantitative analysis of the vehicle's motion state, which can effectively avoid interference to the system caused by side slipping, jumping and other phenomena caused by violent vehicle maneuvers, thereby improving the stability of the system.
[0030] (2) Based on the mutual exclusivity principle of the detection of zero speed and non-integrity constraints and the uniqueness principle of the motion state, the present invention realizes the accurate detection of the vehicle motion state and ensures the effectiveness of the vehicle motion state detection. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] Figure 1 is a flow chart of the method of the present invention.
[0032] Figure 2 It is a schematic diagram of the installation of the device of the present invention. DETAILED DESCRIPTION
[0033] like Figure 1 As shown, an error estimation method based on motion-assisted inertial navigation of the present invention comprises the following steps:
[0034] Step 1: Establish the state space vector according to the motion-assisted navigation principle, and establish the SINS / TZV / NHC autonomous navigation state space model according to the motion-assisted navigation principle, including the state equation and measurement equation. The measurement equation includes the zero-speed constraint measurement equation and the non-holonomic constraint measurement equation;
[0035] The step 1 specifically comprises the following steps:
[0036] Step 1.1: Establish the state space vector X according to the motion-assisted navigation principle TH , mainly due to the attitude error φ n , speed error Position error δp, gyro bias error ε b , Add zero bias error Considering the pitch angle and heading angle installation error δα θ ,δα ψ and space arm State space vector X TH as follows:
[0037]
[0038] Step 1.2: Establish the state equation based on the established state space vector and system error transmission relationship
[0039]
[0040]
[0041]
[0042] where F TH is the system state transfer matrix, F 15×15 is the non-zero part of the system state transfer matrix, G TH is the system noise distribution matrix, W TH is the system noise, is the attitude matrix.
[0043] Step 1.3: Considering that the zero-speed constraint information is valid when the vehicle is stationary, the east, north, and sky speeds calculated by SINS in real time are The difference between the information of zero speed constraint and the information of zero speed constraint is taken as the measurement, and the measurement equation of zero speed constraint is established;
[0044] Z T =H T X TH +VT
[0045]
[0046] H T =[0 3×3 I 3×3 0 3×14 ]
[0047] Among them, Z T is the observed quantity, H T is the system observation matrix, V T is the observation noise matrix, I 3×3 is the identity matrix.
[0048] Step 1.4: Figure 2 As shown in the figure, the non-holonomic motion constraint information is valid when the vehicle has no sideslip, jump, or rapid azimuth maneuvering. H Projection of a point Zero speed constraint information for the vehicle lateral and vertical directions The difference is used as the measurement to establish the non-holonomic constraint measurement equation;
[0049] Z H =H H X TH +V H
[0050]
[0051]
[0052] Among them, H H is the non-holonomic motion constraint observation matrix, V H is the observation noise matrix, and each component of M is the relationship matrix corresponding to the state quantity parameters. is the angular velocity of the carrier system relative to the Earth coordinate system, is the transfer matrix between the carrier system and the odometer coordinate system, and is the component of the space arm in the carrier coordinate system.
[0053] Step 2: Based on the original information output by the inertial component and the navigation information output by the inertial navigation system, a zero-speed motion state detection method is established;
[0054] The step 2 specifically includes the following steps:
[0055] Step 2.1: When the vehicle is stationary, the horizontal attitude of the strapdown inertial navigation system is easily affected by the external environment and people getting on and off the vehicle, and is not suitable as a zero-speed judgment criterion; the heading keeping accuracy of the medium- and high-precision strapdown inertial navigation system is very high, and a sliding window [t j-1 ,t j ] and a reasonable threshold as the zero speed criterion, namely the heading constraint criterion (HCC). Considering the influence of vehicle idling and external disturbance, the heading constraint threshold should not exceed 0.1mil in the application of medium and high precision strapdown inertial navigation system. The heading constraint criterion includes two sub-criteria, as follows:
[0056]
[0057] In the formula, and They represent the sliding window [t j-1 ,t j ] The maximum and minimum values of the heading angle can be taken as t j -t j-1 is the measurement period of the SINS / TZV autonomous navigation system. This scheme adopts T M =t j -t j-1 =1s; Δψ Th is the sliding window [t j-1 ,t j ] is the heading change threshold set in . It should be pointed out that C12 is aimed at the critical state of the heading angle definition domain. Before each measurement update, if one of the conditions of C11 and C12 is met, it can be considered that the strapdown inertial navigation meets the heading constraint criterion. The heading constraint criterion is abbreviated as C11||C12.
[0058] Step 2.2: At the same time, when the vehicle is stationary, the gyroscope and accelerometer are mainly sensitive to the earth's rotation angular velocity information and the supporting force, where the supporting force is equal to the gravity modulus and opposite in direction; in addition, when the vehicle is idling, the gyroscope and accelerometer are sensitive to the vibration angular velocity and acceleration information respectively. j-1 ,t j ] i The analog of the gyro angular velocity information at the moment is The analog of the accelerometer force information is
[0059] In actual engineering, the sampling information of the gyroscope and accelerometer at a certain moment is the angular increment information and the velocity increment information. Dividing by the sampling time, the angular velocity information (unit: rad / s) and the specific force information (unit: m / s) can be obtained. 2 ).
[0060] The sliding window [t j-1 ,t j The maximum and average values of the outputs of the gyro and accelerometer and a reasonable threshold are used as the zero-speed criterion, namely the inertial information constraint criterion (IMCC). The inertial information constraint threshold can be calculated based on the inertial information collected during the initial alignment phase of the strapdown INS as a reference, and can be adjusted based on the actual vehicle dynamic environment. The inertial information constraint criterion includes 4 sub-criteria, as shown in the formula:
[0061]
[0062] In the formula, and They represent the sliding window [t j-1 ,t j ]The maximum and average modulus of the internal gyro output; ω Th,max and ω Th,mean They represent the sliding window [t j-1 ,t j ] set within the maximum angular velocity modulus threshold and the average angular velocity modulus threshold; and They represent the sliding window [t j-1 ,t j ] The maximum and average modulus values of the accelerometer output; f Th,max and f Th,mean They represent the sliding window [t j-1 ,t j ] set in the maximum acceleration modulus threshold and average acceleration modulus threshold. Before each measurement update, if C21, C22, C23 and C24 are established at the same time, it can be considered that the strapdown inertial navigation meets the inertial information constraint criterion. The inertial information constraint criterion is abbreviated as C21&C22&C23&C24.
[0063] Step 2.3: The conclusion of zero speed detection is determined by using the two criteria of step 2.1 and step 2.2 to be established separately or by using the two criteria to be established at the same time. In this scheme, the scheme of both criteria being established at the same time is adopted.
[0064] Step 3: The incomplete motion constraint detection method is as follows:
[0065] In the land-based field, the horizontal posture of vehicles is relatively small. When the vehicle jumps under large maneuvers, the z-axis accelerometer of the strapdown inertial navigation system measures the relative There will be major changes, which can be targeted The judgment is made by setting high and low thresholds; the vehicle side slip phenomenon is easy to occur when the vehicle is in a fast azimuth maneuver. At this time, the z-axis gyroscope measures the absolute value of the angular velocity Larger, can be targeted The maximum angular velocity threshold is set for judgment. According to the above two situations, the vehicle non-holonomic constraint criterion (NHCC, Non-Holonomic Constraints Criterion) is as follows:
[0066]
[0067] In the formula, Indicates t i The measured specific force of the z-axis accelerometer at the sampling time (usually a positive value in actual situations); and They represent the minimum threshold and maximum threshold of the specific force measured by the z-axis accelerometer at each sampling moment respectively; Indicates t i The absolute value of the angular velocity measured by the z-axis gyroscope at the sampling moment; Indicates the maximum threshold of the z-axis gyro measurement angular velocity setting at each sampling moment. Before each measurement update, if C31 and C32 are both met, it can be considered as the non-integrity constraint criterion of SINS, which is abbreviated as C31&C32.
[0068] Step 4: According to the detection method of the zero-speed motion state established in step 2 and the detection method of the incomplete motion constraint established in step 3, determine the motion state of the system, select the corresponding measurement equation according to the motion state, establish a complete Kalman filter equation, that is, the SINS / TZV or SINS / NHC autonomous navigation equation, complete the time update and measurement update (ZUPT or DZUPT) according to the corresponding equations and nodes, and finally obtain the error value in the state space vector, and realize the correction of navigation error through closed-loop feedback.
[0069] The contents not described in detail in this specification belong to the prior art known to the professionals in this field.
[0070] At this point, the complete workflow is clearly explained.
[0071] The error estimation method of motion-assisted inertial navigation described in the present invention is a perfection and supplement to the inertial autonomous navigation algorithm, which can quickly detect the vehicle motion state, accurately eliminate abnormal states, improve the accuracy of system state error estimation, and thus improve the reliability and stability of the navigation system. The present invention can solve the problem of divergence of system positioning results caused by unstable system motion state, improve the navigation accuracy of the autonomous navigation algorithm, and has high engineering practical value.
Claims
1. An error estimation method based on motion-assisted inertial navigation, characterized in that: The steps include: Step 1: Establish the state space vector according to the motion-assisted navigation principle, and establish the SINS / TZV / NHC autonomous navigation state space model according to the motion-assisted navigation principle, including the state equation and measurement equation. The measurement equation includes the zero-speed constraint measurement equation and the non-holonomic constraint measurement equation; Step 2: Based on the original information output by the inertial component and the navigation information output by the inertial navigation system, a detection method for the zero-speed motion state is established; wherein the detection method for the original information is a threshold analysis of the original data results of the gyro accelerometer within the timing period, and the detection method for the navigation information is a threshold analysis of the heading, speed or position data of the navigation results within the timing period; Step 3: In view of the jumping phenomenon of the vehicle under large maneuvering conditions and the side sliding phenomenon of the vehicle under fast azimuth maneuvering conditions, a detection method for non-holonomic motion constraints is established based on the threshold setting of the celestial accelerometer information and celestial gyroscope information output by the inertial component; Step 4: According to the detection method of the zero-speed motion state established in step 2 and the detection method of the non-integral motion constraint established in step 3, determine the motion state of the system, select the corresponding measurement equation according to the motion state, establish the complete Kalman filter equation, and obtain the error value in the state space vector; Wherein, step 2 specifically includes the following steps: Step 2.1: Introduce sliding window [t j-1 ,t j ] and a reasonable threshold as the zero speed criterion, the heading constraint criterion includes two sub-criteria, as follows: In the formula, and They represent the sliding window [t j-1 ,t j ] The maximum and minimum values of the heading angle are taken as t j -t j-1 is the measurement period T of the SINS / TZV autonomous navigation system M =t j -t j-1 , Δψ Th is the sliding window [t j-1 ,t j ]; C12 is the critical state of the heading angle definition domain. Before each measurement update, if one of the conditions of C11 and C12 is met, it is considered that the SINS meets the heading constraint criterion. The heading constraint criterion establishment condition is abbreviated as C11 C12; Step 2.2: Note the sliding window [t j-1 ,t j ] i The analog of the gyro angular velocity information at the moment is The analog of the accelerometer force information is The sliding window [t j-1 ,t j ] The maximum and average values of the gyro and accelerometer outputs and a reasonable threshold are used as the zero-speed criterion. The inertial information constraint criterion includes four sub-criteria, as follows: In the formula, and They represent the sliding window [t j-1 ,t j ]The maximum and average modulus of the internal gyro output; ω Th,max and ω Th,mean They represent the sliding window [t j-1 ,t j ] set within the maximum angular velocity modulus threshold and the average angular velocity modulus threshold; and They represent the sliding window [t j-1 ,t j ] The maximum and average modulus values of the accelerometer output; f Th,max and f Th,mean They represent the sliding window [t j-1 ,t j ]; if C21, C22, C23 and C24 are met at the same time, it is considered that SINS meets the inertial information constraint criterion. The inertial information constraint criterion is abbreviated as C21&C22&C23&C24; Step 2.3: The conclusion of zero-speed detection is determined by using the two criteria of step 2.1 and step 2.2 to be established separately or by using the two criteria to be established simultaneously.
2. The error estimation method based on motion-assisted inertial navigation according to claim 1, characterized in that: Step 1 specifically includes the following steps: Step 1.1: Establish the state space vector X according to the motion-assisted navigation principle TH , including the attitude error φ n , speed error Position error δp, gyro bias error ε b , Add zero bias error Installation error δα θ and δα ψ And space arm Step 1.2: Establish the state equation based on the established state space vector and system error transmission relationship; Step 1.3: Considering that the zero-speed constraint information is valid when the vehicle is stationary, the difference between the east, north, and sky speeds calculated by SINS in real time and the zero-speed constraint information is used as the measurement, and the zero-speed constraint measurement equation is established using the state space vector, system observation matrix, and observation noise matrix; Step 1.4: Considering that the non-integral motion constraint information is valid when the vehicle has no sideslip, jump, or rapid azimuth maneuver, the x-axis and z-axis velocities of the carrier solved by SINS in real time are calculated in O H The difference between the projection of the point and the lateral and vertical zero-speed constraint information of the vehicle is taken as the measurement, and the non-holonomic constraint measurement equation is established using the state space vector, the non-holonomic motion constraint observation matrix and the observation noise matrix.
3. The error estimation method based on motion-assisted inertial navigation according to claim 1, characterized in that: The specific process of step 3 is as follows: In view of the jumping phenomenon of the vehicle under large maneuvering conditions and the side sliding phenomenon of the vehicle under fast azimuth maneuvering conditions, an effective criterion for the vehicle's non-integrity constraint information is established as follows: In the formula, Indicates t i The measured specific force of the z-axis accelerometer at the sampling moment; and They represent the minimum threshold and maximum threshold of the specific force measured by the z-axis accelerometer at each sampling moment respectively; Indicates t i The absolute value of the angular velocity measured by the z-axis gyroscope at the sampling moment; It represents the maximum threshold of the z-axis gyro measurement angular velocity setting at each sampling moment; before each measurement update, if C31 and C32 are both met, it is considered that SINS meets the non-integrity constraint condition detection, and the detection condition is abbreviated as C31&C32.
