High-precision SINS air calibration method for unmanned aerial vehicle
By carrying sensors on the drone and using Kalman filters for data fusion, high-precision SINS air calibration is achieved, solving the problems of low efficiency and high cost of traditional calibration methods, improving navigation accuracy and saving calibration costs.
Patent Information
- Application Number
- CN202411368035.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-29
- Publication Date
- 2025-01-03
AI Technical Summary
In the prior art, the calibration method of airborne SINS mainly relies on the turntable method, is inefficient and costly, and causes inconvenience in disassembly and transportation, resulting in reduced navigation accuracy and inability to meet the usage needs.
The high-precision SINS air calibration method is adopted, and the sensor output data of a series is collected, and the data fusion is used to estimate the zero deviation of the inertial sensor in real time, and the navigation system is updated and corrected to complete the SINS air calibration.
High-precision SINS air calibration is achieved, navigation accuracy is improved, calibration costs are saved, disassembly and transport inconveniences in traditional methods are avoided, and calibration efficiency is improved.
Smart Images
Figure CN119245691A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of unmanned aerial vehicle navigation, and in particular to a high-precision SINS aerial calibration method for unmanned aerial vehicles. Background Art
[0002] Compared with the platform-based inertial navigation system, the strapdown inertial navigation system SINS has the advantages of small size, low cost, and high reliability, and has become the mainstream airborne inertial navigation equipment for drones and manned aircraft in my country. However, since SINS needs to be fixed to the aircraft body, as the working time increases, the navigation performance of the system will gradually decrease, and even the navigation accuracy will not meet the index requirements for use. Therefore, it is necessary to calibrate SINS to compensate for the corresponding error terms to meet subsequent use requirements. However, the calibration method for airborne SINS in my country is mainly to perform regular calibration by means of a turntable. This method has problems such as low efficiency and high cost. In order to realize the calibration of SINS, the inertial navigation system needs to be disassembled from the aircraft again, and a complex calibration process is carried out with the help of professional equipment such as a high-precision turntable. During this period, the disassembly and transportation problems will also bring many inconveniences. Summary of the invention
[0003] In view of the above technical problems, the present invention proposes a high-precision SINS aerial calibration method for unmanned aerial vehicles. The calibration method can complete the SINS aerial calibration by only carrying sensors and collecting sensor output data of one sortie. The zero position of the SINS can be estimated very accurately with high calibration accuracy.
[0004] A high-precision SINS aerial calibration method for UAVs. The calibration process is as follows:
[0005] S1. Equip the UAV with SINS and satellite navigation system;
[0006] S2. Power on the product, collect the original output data of SINS and satellite navigation system for the entire flight, and initialize and align after the output of SINS and satellite navigation system is valid;
[0007] S3, after the initialization and alignment are completed, real-time pure inertial navigation is performed, and the navigation system output is updated according to the original data output by the SINS in step S3;
[0008] S4, during the navigation process, when valid satellite navigation output data is collected, the navigation result output in step S3 and the navigation result updated in step S4 are fused through the Kalman filter, and the zero bias feedback of the inertial sensor estimated by the Kalman filter is used. When the inertial navigation is performed next time, the zero bias correction is performed to complete the combined navigation calculation of the entire sortie;
[0009] S5. Record the estimated zero position value and feed it back to SINS. Recalculate it three times according to the above process, add the three estimated zero position values together, and obtain the final SINS air calibration result.
[0010] As a preferred embodiment of the above technical solution, in step S2, the position and altitude of the satellite navigation system at the initial moment are used as the initial navigation value of the navigation system, and the symmetric process is as follows:
[0011] Assuming the latitude of the alignment point is L, the attitude matrix can be determined by the following formula:
[0012]
[0013] The attitude matrix determined by alignment is obtained, where e is the geocentric earth coordinate system, i is the geocentric inertial coordinate system, n is the navigation coordinate system, and b is the carrier coordinate system. is the base inertial coordinate system.
[0014] As a preferred embodiment of the above technical solution, in step S3, real-time inertial navigation is performed based on the gyro and table data output by SINS. The updating process of inertial navigation is divided into speed update, position update and attitude update, wherein:
[0015] The velocity update equation is:
[0016]
[0017] Where: is the specific force output of the accelerometer; v n is the velocity of the carrier in the navigation coordinate system; g n is the gravitational acceleration at the carrier’s location;
[0018] The position update equation is:
[0019]
[0020] Where: φ, λ, h are the longitude, latitude and altitude of the carrier respectively; are the velocity components of the carrier in the three directions of northeast and sky in the navigation coordinate system; R M , R N They respectively represent the meridian curvature radius and the meridian curvature radius of the carrier's location.
[0021] The attitude update equation is:
[0022]
[0023] Where: is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n; is the angular velocity of the carrier coordinate system relative to the navigation coordinate system; for The antisymmetric array.
[0024] As a preferred embodiment of the above technical solution, in step S3, the entire process of the integrated navigation calculation is as follows:
[0025] S31. Establish the state equation of the inertial satellite integrated navigation system as follows:
[0026]
[0027] Where: is the state variable of the system. The state variable of the navigation system is composed of a 15-dimensional vector consisting of attitude error, velocity error, position error, zero bias of the three-axis gyroscope and zero bias of the three-axis accelerometer, namely:
[0028]
[0029] F(t) is the system state matrix; G(t) is the system noise matrix; W(t) is the measurement noise vector; Z(t is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise vector. Among them, W(t) and V(t) are both zero-mean Gaussian white noise, independent and uncorrelated;
[0030] S32. Establish the error equation of SINS, including:
[0031] S321, the attitude error equation is:
[0032]
[0033] Where: φ n is the attitude error angle of the carrier in the navigation system; is the angular velocity output of the gyroscope; is the projection of the angular velocity output of the gyroscope in the navigation coordinate system; is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n, ε n is the projection of the gyroscope zero bias in the navigation coordinate system;
[0034] S322, speed error equation is:
[0035]
[0036] Where: δv n is the velocity error of the carrier in the navigation system; f b is the specific force output of the accelerometer; f n is the projection of the specific force output of the accelerometer in the navigation coordinate system; v nis the velocity of the carrier in the navigation coordinate system; and are the Earth's rotation rate and position rate, is the projection of the accelerometer zero bias in the navigation coordinate system;
[0037] S323, the position error equation is:
[0038]
[0039] Where: δφ, δλ, δh are the longitude, latitude and altitude errors of the carrier’s location respectively; They are the velocity components of the carrier in the east, north and sky directions in the navigation coordinate system respectively;
[0040] The gyroscope bias and accelerometer bias are modeled as white noise.
[0041] The measurement vector Z can be expressed as:
[0042] Z=[V err ,P err ]
[0043] Where: V err is the velocity error obtained by inertial navigation solution and satellite navigation measurement, P err It is the position error obtained by inertial navigation solution and satellite navigation measurement;
[0044] The measurement matrix H can be expressed as:
[0045]
[0046] Discretizing the state equation under continuous time conditions, we can get:
[0047]
[0048] Where: X k-1 , X k are the state vectors of the system at time k-1 and k respectively; Φ k,k-1 is the state transfer matrix of the system from time k-1 to time k; Γ k-1 is the noise driving matrix of the system at time k-1; W k-1 is the noise vector of the system at time k-1; Z k is the measurement vector at time k; H k is the measurement matrix at time k; V k is the measurement noise at time k;
[0049] S3. The main process of Kalman filtering includes two parts: time update and measurement update. The steps are as follows:
[0050] S31, state one-step prediction:
[0051]
[0052] Where: is the predicted value of the system state from time k-1 to time k; is the estimated value of the system state at time k-1.
[0053] S32, state one-step prediction mean square error matrix:
[0054]
[0055] Where: P k-1 is the covariance matrix of the system state at time k-1; P k,k-1 for The covariance matrix of k-1 is the system noise covariance matrix at time k-1.
[0056] S33, filter gain:
[0057]
[0058] Where: K k is the gain matrix of the filter at time k; P k,k-1 for The covariance matrix of k is the covariance matrix of the measurement noise at time k.
[0059] S34, state estimation:
[0060]
[0061] S35, state estimation mean square error matrix:
[0062]
[0063] Whenever satellite navigation observation data is obtained, the Kalman filter is used to correct the inertial navigation output results, and the estimated zero position is fed back to the SINS to correct the output results for the next inertial navigation.
[0064] The beneficial effects of the present invention are:
[0065] Compared with the traditional calibration method using a turntable, this calibration method realizes disassembly-free calibration. It can carry sensors and collect one flight of sensor output data to complete SINS aerial calibration. It has high calibration accuracy and can greatly save SINS calibration costs while improving calibration efficiency. BRIEF DESCRIPTION OF THE DRAWINGS
[0066] Figure 1 The present invention provides a SINS aerial calibration system workflow diagram. DETAILED DESCRIPTION
[0067] The technical solution of the present invention is described clearly and completely below in conjunction with the accompanying drawings of the present invention. 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.
[0068] like Figure 1 The high-precision UAV airborne SINS air calibration method can effectively and accurately estimate the constant error term of the airborne SINS by only one flight of the UAV, complete the SINS air calibration, and thus improve the reliability of the UAV airborne SINS, which has great practical significance.
[0069] The specific calibration method of this embodiment is as follows.
[0070] When the product is powered on, the output information of different sensors is collected in real time. When the outputs of SINS and the satellite navigation system are valid, initialization and alignment are started. The position and altitude of the satellite navigation system at the initial moment are used as the initial navigation value of the navigation system, and alignment is performed.
[0071] The coordinate system is defined as follows:
[0072] Geocentric Earth Coordinate System e: The origin is at the center of the earth, z e The axis is along the direction of the Earth's rotation, x e The axis lies in the prime meridian plane and is parallel to the equatorial plane, y e The axis is determined by the right-hand rule.
[0073] Geocentric inertial coordinate system i: A coordinate system assumed to be absolutely stationary, formed by solidifying the geocentric earth coordinate system at the initial moment.
[0074] Navigation coordinate system n: that is, the geographic coordinate system, the origin is located at the inertial navigation center, x e The axis points east, x n The axis points north, z n The axis points to the sky.
[0075] Carrier coordinate system b: The origin is located at the center of the inertial navigation system, x b ,y b 、z b The axes point to the right, forward and upward along the carrier, respectively.
[0076] Base inertial coordinate system The coordinate system of the carrier after inertial solidification at the initial moment.
[0077] The alignment process is as follows:
[0078] Assuming the latitude of the alignment point is L, the attitude matrix can be determined by the following formula:
[0079]
[0080] Get the pose matrix determined by the alignment.
[0081] After the alignment is completed, real-time inertial navigation is performed based on the gyro and table data output by SINS. The update process of inertial navigation is divided into velocity update, position update and attitude update.
[0082] Velocity update equation
[0083]
[0084] Where: is the specific force output of the accelerometer; v n is the velocity of the carrier in the navigation coordinate system; g n is the gravitational acceleration at the carrier's location.
[0085] Position update equation
[0086]
[0087] Where: φ, λ, h are the longitude, latitude and altitude of the carrier respectively; are the velocity components of the carrier in the three directions of northeast and sky in the navigation coordinate system; R M , R N They respectively represent the meridian curvature radius and the meridian curvature radius of the carrier's location.
[0088] Attitude update equation
[0089]
[0090] Where: is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n; is the angular velocity of the carrier coordinate system relative to the navigation coordinate system; for The antisymmetric array.
[0091] (3) When valid satellite navigation data is collected, the inertial + satellite combined navigation mode is performed. First, the state equation of the inertial satellite combined navigation system is established as:
[0092]
[0093] Where: is the state variable of the system. The state variable of the navigation system is composed of a 15-dimensional vector consisting of attitude error, velocity error, position error, zero bias of the three-axis gyroscope and zero bias of the three-axis accelerometer, namely:
[0094]
[0095] F(t) is the system state matrix; G(t) is the system noise matrix; W(t) is the measurement noise vector; Z(t) is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise vector. Among them, W(t) and V(t) are both zero-mean Gaussian white noise, independent and uncorrelated.
[0096] Then, the error equation of SINS is established:
[0097] Attitude Error Equation
[0098]
[0099] Where: φ n is the attitude error angle of the carrier in the navigation system; is the angular velocity output of the gyroscope; is the projection of the angular velocity output of the gyroscope in the navigation coordinate system; is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n, ε n is the projection of the gyroscope zero bias in the navigation coordinate system.
[0100] Speed Error Equation
[0101]
[0102] Where: δv n is the velocity error of the carrier in the navigation system; f b is the specific force output of the accelerometer; f n is the projection of the specific force output of the accelerometer in the navigation coordinate system; v n is the velocity of the carrier in the navigation coordinate system; and are the Earth's rotation rate and position rate, is the projection of the accelerometer zero bias in the navigation coordinate system.
[0103] Position Error Equation
[0104]
[0105] Where: δφ, δλ, δh are the longitude, latitude and altitude errors of the carrier’s location respectively; They are respectively the velocity components of the carrier in the east, north and sky directions in the navigation coordinate system.
[0106] The gyroscope bias and accelerometer bias are modeled as white noise.
[0107] The measurement vector Z can be expressed as:
[0108] Z=[V err ,P err ] (10)
[0109] Where: V err is the velocity error obtained by inertial navigation solution and satellite navigation measurement, P err It is the position error obtained by inertial navigation solution and satellite navigation measurement.
[0110] The measurement matrix H can be expressed as:
[0111]
[0112] Discretizing the state equation of equation (5) under continuous time conditions, we can obtain:
[0113]
[0114] Where: X k-1 , X k are the state vectors of the system at time k-1 and k respectively; Φ k,k-1 is the state transfer matrix of the system from time k-1 to time k; Γ k-1 is the noise driving matrix of the system at time k-1; W k-1 is the noise vector of the system at time k-1; Z k is the measurement vector at time k; H k is the measurement matrix at time k; V k is the measurement noise at time k.
[0115] The main process of Kalman filtering includes two parts: time update and measurement update. The steps are as follows:
[0116] State one-step prediction:
[0117]
[0118] Where: is the predicted value of the system state from time k-1 to time k; is the estimated value of the system state at time k-1.
[0119] State one-step prediction mean square error matrix:
[0120]
[0121] Where: P k-1 is the covariance matrix of the system state at time k-1; P k,k-1 for The covariance matrix of k-1 is the system noise covariance matrix at time k-1.
[0122] Filter gain:
[0123]
[0124] Where: K k is the gain matrix of the filter at time k; P k,k-1 for The covariance matrix of k is the covariance matrix of the measurement noise at time k.
[0125] State Estimation:
[0126]
[0127] State estimation mean square error matrix:
[0128]
[0129] Whenever satellite navigation observation data is obtained, the Kalman filter is used to correct the inertial navigation output results, and the estimated zero position is fed back to the SINS to correct the output results for the next inertial navigation.
[0130] (4) After solving the flight data of a flight, the final estimated SINS zero position value is recorded and fed back into the SINS measurement data of the entire flight. Alignment and combined navigation calculations are re-performed. After repeating the iterative calculation three times, the zero position values estimated each time are added together to obtain the final zero position estimation result, thus completing the SINS air calibration.
[0131] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A high-precision SINS aerial calibration method for unmanned aerial vehicles, characterized by: The calibration process is as follows: S1. Equip the UAV with SINS and satellite navigation system; S2. Power on the product, collect the original output data of SINS and satellite navigation system for the entire flight, and initialize and align after the output of SINS and satellite navigation system is valid; S3, after the initialization and alignment are completed, real-time pure inertial navigation is performed, and the navigation system output is updated according to the original data output by the SINS in step S3; S4, during the navigation process, when valid satellite navigation output data is collected, the navigation result output in step S3 and the navigation result updated in step S4 are fused through the Kalman filter, and the zero bias feedback of the inertial sensor estimated by the Kalman filter is used. When the inertial navigation is performed next time, the zero bias correction is performed to complete the combined navigation calculation of the entire sortie; S5. Record the estimated zero position value and feed it back to SINS. Recalculate it three times according to the above process, add the three estimated zero position values together, and obtain the final SINS air calibration result.
2. The high-precision SINS aerial calibration method according to claim 1 is characterized in that: In step S2, the position and altitude of the satellite navigation system at the initial moment are used as the initial navigation values of the navigation system. The symmetric process is as follows: Assuming the latitude of the alignment point is L, the attitude matrix can be determined by the following formula: The attitude matrix determined by alignment is obtained, where e is the geocentric earth coordinate system, i is the geocentric inertial coordinate system, n is the navigation coordinate system, and b is the carrier coordinate system. is the base inertial coordinate system.
3. The high-precision SINS aerial calibration method according to claim 1, characterized in that: In step S3, real-time inertial navigation is performed based on the gyro and table data output by SINS. The update process of inertial navigation is divided into speed update, position update and attitude update, among which, The velocity update equation is: Where: is the specific force output of the accelerometer; v n is the velocity of the carrier in the navigation coordinate system; g n is the gravitational acceleration at the carrier’s location; The position update equation is: Where: φ, λ, h are the longitude, latitude and altitude of the carrier respectively; are the velocity components of the carrier in the three directions of northeast and sky in the navigation coordinate system; R M , R N They respectively represent the meridian curvature radius and the meridian curvature radius of the carrier's location. The attitude update equation is: Where: is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n; is the angular velocity of the carrier coordinate system relative to the navigation coordinate system; for The antisymmetric array.
4. The high-precision SINS aerial calibration method according to claim 1, characterized in that: In step S3, the entire process of integrated navigation calculation is as follows: S31. Establish the state equation of the inertial satellite integrated navigation system as follows: Where: is the state variable of the system. The state variable of the navigation system is composed of a 15-dimensional vector consisting of attitude error, velocity error, position error, zero bias of the three-axis gyroscope and zero bias of the three-axis accelerometer, namely: F(t) is the system state matrix; G(t) is the system noise matrix; W(t) is the measurement noise vector; Z(t is the measurement vector; H(t) is the measurement matrix; V(t) is the measurement noise vector. Among them, W(t) and V(t) are both zero-mean Gaussian white noise, independent and uncorrelated; S32. Establish the error equation of SINS, including: S321, the attitude error equation is: Where: φ n is the attitude error angle of the carrier in the navigation system; is the angular velocity output of the gyroscope; is the projection of the angular velocity output of the gyroscope in the navigation coordinate system; is the attitude transformation matrix from the carrier coordinate system b to the navigation coordinate system n, ε n is the projection of the gyroscope zero bias in the navigation coordinate system; S322, speed error equation is: Where: δv n is the velocity error of the carrier in the navigation system; f b is the specific force output of the accelerometer; f n is the projection of the specific force output of the accelerometer in the navigation coordinate system; v n is the velocity of the carrier in the navigation coordinate system; and are the Earth's rotation rate and position rate, is the projection of the accelerometer zero bias in the navigation coordinate system; S323, the position error equation is: Where: δφ, δλ, δh are the longitude, latitude and altitude errors of the carrier’s location respectively; They are the velocity components of the carrier in the east, north and sky directions in the navigation coordinate system respectively; The gyroscope bias and accelerometer bias are modeled as white noise. The measurement vector Z can be expressed as: Z=[V err ,P err ] Where: V err is the velocity error obtained by inertial navigation solution and satellite navigation measurement, P err It is the position error obtained by inertial navigation solution and satellite navigation measurement; The measurement matrix H can be expressed as: Discretizing the state equation under continuous time conditions, we can get: Where: X k-1 , X k are the state vectors of the system at time k-1 and k respectively; Φ k,k-1 is the state transfer matrix of the system from time k-1 to time k; Γ k-1 is the noise driving matrix of the system at time k-1; W k-1 is the noise vector of the system at time k-1; Z k is the measurement vector at time k; H k is the measurement matrix at time k; V k is the measurement noise at time k; S3. The main process of Kalman filtering includes two parts: time update and measurement update. The steps are as follows: S31, state one-step prediction: Where: is the predicted value of the system state from time k-1 to time k; is the estimated value of the system state at time k-1. S32, state one-step prediction mean square error matrix: Where: P k-1 is the covariance matrix of the system state at time k-1; P k,k-1 for The covariance matrix of k-1 is the system noise covariance matrix at time k-1. S33, filter gain: Where: K k is the gain matrix of the filter at time k; P k,k-1 for The covariance matrix of k is the covariance matrix of the measurement noise at time k. S34, state estimation: S35, state estimation mean square error matrix: Whenever satellite navigation observation data is obtained, the Kalman filter is used to correct the inertial navigation output results, and the estimated zero position is fed back to the SINS to correct the output results for the next inertial navigation.