CN113703019B - Fault processing method of navigation system, electronic equipment and storage medium - Google Patents

Fault processing method of navigation system, electronic equipment and storage medium Download PDF

Info

Publication number
CN113703019B
CN113703019B CN202110988512.3A CN202110988512A CN113703019B CN 113703019 B CN113703019 B CN 113703019B CN 202110988512 A CN202110988512 A CN 202110988512A CN 113703019 B CN113703019 B CN 113703019B
Authority
CN
China
Prior art keywords
accelerometer
gyroscope
gps
correction
fault
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110988512.3A
Other languages
Chinese (zh)
Other versions
CN113703019A (en
Inventor
熊小军
赵丹
李向东
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Northern Sky Long Hawk Uav Technology Co ltd
Original Assignee
Beijing Northern Sky Long Hawk Uav Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Northern Sky Long Hawk Uav Technology Co ltd filed Critical Beijing Northern Sky Long Hawk Uav Technology Co ltd
Priority to CN202110988512.3A priority Critical patent/CN113703019B/en
Publication of CN113703019A publication Critical patent/CN113703019A/en
Application granted granted Critical
Publication of CN113703019B publication Critical patent/CN113703019B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

A fault handling method for a navigation system, comprising: 1) Initializing a zone bit, and respectively setting a GPS fault zone bit, a gyro and an accelerometer correction zone bit; 2) Reading data of the gyroscope and the accelerometer and GPS output fault marks, executing the step 3) when the read GPS output fault marks cannot be positioned and the gyroscope and the accelerometer can correct the measured data, otherwise executing the step 4); 3) Correcting the gyroscope and the accelerometer; 4) Performing inertial navigation pose calculation and one-step prediction of Kalman filtering according to the read gyroscope and accelerometer data, judging whether the marker bit input by the GPS is effectively positioned, if so, executing the step 5), otherwise, executing the step 6); 5) Performing Kalman filtering, performing feedback correction of the pose, and outputting a corrected result; 6) And judging sensor correction conditions, and outputting the pose of inertial navigation solution. The fault processing method of the navigation system can not reduce the operation efficiency of the system.

Description

Fault processing method of navigation system, electronic equipment and storage medium
Technical Field
The present invention relates to the field of information technologies, and in particular, to a fault handling method for a navigation system, an electronic device, and a storage medium.
Background
At present, a SINS (strapdown inertial navigation system)/GPS (global positioning system) combined navigation method is often adopted in a navigation system of a low-altitude unmanned aerial vehicle, the method is low in cost, and accurate pose information can be provided to meet flight requirements. However, the GPS is easy to fail due to more shielding objects in the low-altitude flight process. Because unmanned aerial vehicles flying in low altitude often adopt MEMS sensors with lower precision, pose information can be rapidly dispersed in a short time when GPS fails.
The integrated navigation system consists of a laser gyro strapdown inertial navigation system, a satellite positioning system receiver, an integrated navigation computer, an odometer, an altimeter, a base station radar system and the like. The positioning precision, autonomy and the attitude measurement precision of the starlight navigation system of the star sensor of SAR+image navigation are combined, so that the autonomous flight of the unmanned plane is ensured. Unmanned aerial vehicle navigation is the correct guidance of the unmanned aerial vehicle to the destination along a predetermined route for a specified time with a required accuracy. In order for the unmanned aerial vehicle to successfully complete a predetermined navigation task, navigation parameters such as real-time position, navigation speed, heading and the like of the unmanned aerial vehicle must be known in addition to the positions of the starting point and the target. The navigation technology adopted on the unmanned aerial vehicle at present mainly comprises inertial navigation, satellite navigation, doppler navigation, terrain aided navigation, geomagnetic navigation and the like. These navigation techniques have respective advantages and disadvantages, so that in unmanned aerial vehicle navigation, it is important to select a suitable navigation positioning technique according to different tasks carried by the unmanned aerial vehicle.
The unmanned aerial vehicle adopting the three-axis gyroscope, the three-axis accelerometer and the GPS sensor navigation system is characterized in that in the operation of the integrated navigation system, the unmanned aerial vehicle flies under the condition of low-altitude shielding, the GPS can fail, and the GPS can not provide speed and position equivalent measurement information. The conventional SINS/GPS integrated navigation method cannot filter due to lack of measurement information, which results in rapid divergence of pose.
Disclosure of Invention
In order to solve the defects in the prior art, the invention aims to provide a fault processing method of a navigation system, which adopts the estimated zero offset of a three-axis gyroscope and a three-axis accelerometer in the running process of the system, corrects the real-time measured values of the three-axis gyroscope and the three-axis accelerometer, carries out pose resolving, and provides stable pose information for the normal flight of an unmanned plane in the short time of GPS failure.
In order to achieve the above object, the present invention provides a fault handling method for a navigation system, comprising the steps of:
1) Initializing a zone bit, and respectively setting a GPS fault zone bit, a gyro and an accelerometer correction zone bit;
2) Reading data of the gyroscope and the accelerometer and GPS output fault marks, executing the step 3) when the read GPS output fault marks cannot be positioned and the gyroscope and the accelerometer can correct the measured data, otherwise executing the step 4);
3) Correcting the gyroscope and the accelerometer;
4) Performing inertial navigation pose calculation and one-step prediction of Kalman filtering according to the read gyroscope and accelerometer data, judging whether the marker bit input by the GPS is effectively positioned, if so, executing the step 5), otherwise, executing the step 6);
5) Performing Kalman filtering, performing pose feedback correction, and outputting a corrected result as a system output result;
6) And judging sensor correction conditions, and outputting the pose calculated by inertial navigation as a system output result.
Further, the step 1) initializing the zone bit, respectively setting the GPS fault zone bit, the gyro and the accelerometer correction zone bit, and further comprises,
setting a GPS fault zone bit to be 1 to indicate that GPS has no positioning, and setting the GPS fault zone bit to be 0 to indicate GPS effective positioning;
setting the correction flag bit of the gyroscope and the accelerometer to be 1 indicates that correction can be performed by using the measured data of the gyroscope and the accelerometer, and setting the correction flag bit to be 0 indicates that correction cannot be performed;
and when the GPS fault flag bit is initialized, the GPS fault flag bit is assigned to 0, and the gyro and accelerometer correction flag bit is assigned to 0.
Further, the step 3) carries out the correction steps of the gyroscope and the accelerometer, and further comprises,
and correspondingly adding the recorded zero offset values of the gyroscope and the accelerometer into the angular acceleration measured by the gyroscope and the acceleration measured by the accelerometer respectively, and updating the measured values to finish correction of measured data of the gyroscope and the accelerometer.
Further, the step 4) carries out one-step prediction of inertial navigation pose calculation and Kalman filtering according to the read gyroscope and accelerometer data, and further comprises the steps of,
inertial navigation is carried out according to the angular velocity and the acceleration of the gyroscope and the accelerometer to calculate position, velocity and attitude information;
and (3) calculating the zero offset estimated value of the gyroscope and the zero offset estimated value of the accelerometer according to the calculated position, speed and gesture information.
Further, the step of performing inertial navigation to calculate position, speed and attitude information according to the angular speed and acceleration of the gyroscope and the accelerometer further comprises,
calculating quaternion according to the angular increment of the gyro data output period, and updating the gesture matrix according to the quaternion to calculate the gesture;
according to the rotation matrix of the carrier system converted to the navigation system, the rotation angular velocity of the earth generated by the carrier motion, the projection of the current velocity on the navigation system and the earth gravity acceleration, solving the velocity increment, and updating the velocity;
based on latitude, longitude, altitude, the sum of the earth's minor axis radius and the carrier altitude, and the sum of the earth's major axis radius and the carrier altitude.
Further, the step 5) of performing kalman filtering, performing feedback correction of the pose, outputting the corrected result as a system output result, further comprising,
reading GPS sensor data, calculating a Kalman filtering measurement value, and carrying out filtering estimation by a Kalman filtering method;
and carrying out feedback correction on the pose according to the calculated inertial navigation pose, and taking the corrected result as a system output result.
Further, the step 6) of judging sensor correction conditions and outputting the pose calculated by inertial navigation as a system output result, further comprises,
setting a gyro data output period threshold vector, comparing the difference value of the corresponding variable of zero offset estimated at the phase separation moment with the gyro data output period threshold, if the difference value is larger than the set threshold, the Kalman filtering can not stably estimate zero offset and can not correct the zero offset, and assigning the correction flag bits of the gyro and the accelerometer to be 0.
Still further, it also includes,
respectively judging each dimension of data of the zero offset information vector;
when any one-dimensional data meets the gyro data output period threshold value, the gyro and accelerometer correction flag bits are assigned to be 1, sensor data correction is carried out, and the step 2) is executed in a return mode.
To achieve the above object, the present invention also provides an electronic device including a memory and a processor, the memory storing thereon a computer program running on the processor, the processor executing the steps of the fault handling method of the navigation system as described above when running the computer program.
To achieve the above object, the present invention also provides a computer-readable storage medium having stored thereon a computer program which, when run, performs the steps of the fault handling method of a navigation system as described above.
The fault processing method of the navigation system has the following beneficial effects:
1) The method has the advantages that the Kalman filtering is utilized to estimate the zero offset of the three-axis gyroscope and the three-axis accelerometer, the Kalman filtering is used for compensating the original data, the use time and the use mode of the method in the integrated navigation process are designed, the precision of measured data can be effectively improved, the precision of the inertial navigation on pose calculation is indirectly improved, the method is used as a fault processing method for GPS short-time failure in a GPS navigation system, the stable pose information can be provided under the condition by the system, and the flight requirement of an unmanned plane in a short time is met;
2) Through ingenious utilization of the Kalman filtering estimated value, the fault of GPS short-time failure can be simply, conveniently and quickly processed, the cost for processing the fault is greatly reduced, and the problem of stable output of the pose of the system after the GPS failure is effectively solved.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.
Drawings
The accompanying drawings are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate the invention and together with the embodiments of the invention, and do not limit the invention. In the drawings:
FIG. 1 is a flow chart of a fault handling method for a navigation system according to the present invention;
FIG. 2 is a flow chart of an embodiment of the present invention.
Detailed Description
The preferred embodiments of the present invention will be described below with reference to the accompanying drawings, it being understood that the preferred embodiments described herein are for illustration and explanation of the present invention only, and are not intended to limit the present invention.
Example 1
Fig. 1 is a flowchart of a fault handling method of a navigation system according to the present invention, and a detailed description will be made of the fault handling method of the navigation system according to the present invention with reference to fig. 1.
First, in step 101, a flag bit is initialized.
Preferably, a GPS fault flag bit GPSErrFlag is set, wherein 1 indicates that the GPS is not positioned, and 0 indicates that the GPS can be effectively positioned; setting a gyro and accelerometer correction flag bit mendFlag, wherein a value of 1 indicates that correction can be performed by using measured data of the gyro and the accelerometer, a value of 0 indicates that correction cannot be performed, and the correction is not performed by initially setting the value to 0.
In this embodiment, the three-axis gyroscope and the three-axis accelerometer estimated by kalman filtering have zero offset B.
Initialized to 0. Wherein ε is x Zero offset epsilon of X-axis gyroscope y Zero offset epsilon of Y-axis gyro z Zero bias of Z-axis gyro>Add the zero offset of the table for the X-axis, +.>Add the zero offset of the table for Y axis, +.>The table zero offset is added to the Z axis.
In the present embodiment, the state variable is setInitialized to 0. Wherein->Is an attitude error vector, +.>Is a velocity error vector, +.>Is a position error vector, +.>Is a gyro zero offset vector->Is the accelerometer zero offset vector.
In this embodiment, the state variable covariance matrix P k Initializing an array:
wherein the method comprises the steps ofFor the initial attitude error vector, +.>For the x-axis initial posing error, < >>For the initial posing error of the y-axis,the initial attitude error of the z axis; δV (delta V) err As an initial velocity error vector δV Eerr For east initial speed error δV Nerr For north initial velocity error δV Uerr Is the initial speed error in the sky direction; />As an initial position error vector δL err As an initial latitude error, δλ err For initial longitude error δH err Is an initial height error; />Gyro initial zero offset vector under carrier coordinate system, < >>X-axis gyro initial zero offset in carrier coordinate system, < >>Y-axis gyro initial zero offset in carrier coordinate system, < >>The Z-axis gyroscope is initially zero-offset under a carrier coordinate system; />Accelerometer initial zero deflection in carrier coordinate system, < >>X-axis accelerometer initial zero bias under carrier coordinate system>Y-axis accelerometer initial zero bias in carrier coordinate system>The Z-axis accelerometer is initially zero offset in the carrier coordinate system.
In this embodiment, the gain matrix K is initialized to 0, and the corresponding observation matrix is initialized to: h k ＝[0 6×3 1 6×6 0 6×6 ]。
Preferably, the GPS fault flag gpsrrflag is assigned to 0 and the gyro and accelerometer correction flag mendFlag is assigned to 0 during initialization.
At step 102, gyroscopic, accelerometer data is read.
Preferably, reading gyroscope and accelerometer measurement data and reading GPS board card output fault marks; then judging that when the GPS board card output fault is 1 and can not be positioned, namely GPSERRFlag is 1, and when the gyroscope and the accelerometer can be corrected, namely the mendFlag value is 1, executing step 103; otherwise, step 104 is performed.
In step 103, gyroscopic and accelerometer corrections are performed.
Preferably, the recorded triaxial gyroscope and triaxial accelerometer zero offset epsilon x 、ε y 、ε z 、 Angular acceleration omega measured by adding three-axis gyroscopes respectively x 、ω y 、ω z Acceleration value a measured by triaxial accelerometer x 、a y 、a z . The updated measurement values are:
ω x ＝ω x +ε x ，ω y ＝ω y +ε y ，ω z ＝ω z +ε z ；
and after updating, finishing correction of measured data of the gyroscope and the accelerometer, setting zero offset values of the gyroscope and the accelerometer in the state variable X matrix to be 0, and executing step 104.
In step 104, inertial navigation pose solution and KF one-step prediction.
Preferably by updated angular velocity omega x 、ω y 、ω z Acceleration a x 、a y 、a z And calculating the latest position, speed and posture information. The specific inertial navigation calculation formula is as follows:
and (5) attitude updating:
wherein ω= [ ω ] x ,ω y ,ω z ] T The angular increment delta theta=omega is T, T is the output period of the gyro data, Q is the resolving quaternion,
updating the gesture matrix by the quaternion:
solving the gesture:
preferably, the solution speed increment:
wherein f b ＝[a x ,a y ,a z ] T ，C b n For the rotation matrix of the carrier system to the navigation system,rotational speed of earth>V due to rotational angular velocity of the carrier relative to the earth n Projection of current speed on navigation system g n Earth gravitational acceleration.
And (5) updating the speed: v k ＝v k-1 +f b T
Preferably, the location update:
wherein the method comprises the steps ofL is the latitude of the current moment, lambda is the longitude of the current moment, h is the altitude of the current moment, R Mh The sum of the short axis radius of the earth and the carrier height, R Nh The sum of the major axis radius of the earth and the carrier height.
The one-step prediction of the Kalman filtering is performed after the calculation, and the formula is as follows:
the zero offset estimated value of the triaxial gyroscope is calculated through the formulaZero offset estimation value of triaxial accelerometer>Assigned to the gyro at time k-1 and the zero offset vector of the accelerometer +.>And recorded. The GPS receiver determines whether the GPS is valid, assigns a GPS valid flag bit, GPSValidFlag, if it is 1, then step 105 is executed, otherwise step 106 is executed.
In step 105, kalman filtering is performed.
Preferably, the gyro, accelerometer correction flag, mendFlag, is cleared, and mendFlag is set to 0, indicating that no sensor data correction is required.
Reading GPS sensor data and calculating Kalman filtering measurement valuesThen, the filter estimation is performed by a kalman filter method. The solution formula for Kalman filter estimation is as follows:
vectorZero offset estimation value +.>Zero offset estimation value of triaxial accelerometerAssign to->And recorded.
Wherein ε is x,k Zero offset epsilon of X-axis gyro at k moment y,k Zero offset epsilon of Y-axis gyro at k moment z,k Zero offset of the Z-axis gyroscope at the moment k;zero offset of X-axis accelerometer at k moment, < >>Zero offset of Y-axis accelerometer at k moment, < >>Zero offset for the Z-axis accelerometer at time k.
Pose calculated by inertial navigation in step 104v k p k Minus->Posture error of update->Speed error->Position error->That is, feedback correction of the pose is performed, and the corrected result is used as a system output result.
In step 106, a sensor correction condition is determined.
Preferably, the pose of the inertial navigation solution is output as a system output result.
In the embodiment of the invention, firstly, the gyro zero offset and the accelerometer zero offset estimated by the Kalman filtering are not stable in the initial execution stage of the integrated navigation algorithm and the initial recovery stage after the GPS failure, and can not be used for executing the sensor error correction; secondly, the gyro zero offset and the accelerometer zero offset in different axial directions cannot be estimated at the same time due to the flight of different postures and paths, and the sensor error correction cannot be executed at the same time, so that judgment and selective correction are required.
Preferably, a threshold vector of zero offset value estimated by a gyro and an accelerometer at adjacent moments is setEstimating zero offset B for adjacent time instants k 、B k-1 And comparing the difference value of the corresponding variable with a set threshold value, if the difference value is larger than the set threshold value, indicating that the Kalman filtering can not stably estimate zero offset and can not be corrected, and assigning the correction flag bit mendFlag of the gyroscope and the accelerometer to be 0. Since the information of zero offset is recorded as a 6×1-dimensional vector, it is necessary to separately judge the data of each dimension, as shown in the following pseudo code:
caseB k (1)-B k-1 (1)<T(1)：ε x,k ＝B k (1)；
caseB k (2)-B k-1 (2)<T(2)：ε y,k ＝B k (2)；
caseB k (3)-B k-1 (3)<T(3)：ε z,k ＝B k (3)；
the pseudo code above represents: zero offset B of x-axis gyro at k moment k (1) Zero offset B of x-axis gyro at moment k-1 k-1 (1) When the difference of (C) is smaller than T (1), B is k (1) Assigning epsilon to x,k The method is used for correcting the gyroscope and the accelerometer in the next period; other variables in vector B and B k (1) The same treatment. When any data meets the threshold value, the mendFlag is assigned to be 1, so that the sensor data can be corrected, and the second step is executed.
FIG. 2 is a flow chart of an embodiment of the present invention, as shown in FIG. 2, the present invention is applicable to a combined navigation system including three sensors, three-axis gyroscope, three-axis accelerometer and GPS. In the operation of the integrated navigation system, a low-altitude shielding condition can be met, and the GPS can fail. In this case, the GPS cannot provide speed and position equivalent measurement information. The conventional SINS/GPS integrated navigation method cannot filter due to lack of measurement information, which results in rapid divergence of pose. Under the condition, the real-time measured values of the three-axis gyroscope and the three-axis accelerometer are corrected by adopting the three-axis gyroscope and the three-axis accelerometer which are estimated in the running process of the system; and then carrying out pose calculation by adopting the corrected real-time inertial measurement data. Compared with the pose calculated by the inertial data without correction, the calculated pose result diverges slowly, and stable pose information can be provided for the unmanned aerial vehicle to fly normally in a short time when the GPS fails.
Aiming at the technical defect that in the navigation system of the current low-altitude unmanned aerial vehicle, when a GPS fails, pose information can be rapidly diverged in a short time, the invention designs a fault processing method for the GPS failure in a short time in the GPS integrated navigation system, and the rapid divergence of the pose can be effectively inhibited under the condition of independently using an inertial navigation method. Other fault solutions require the introduction of new calculation methods and can effectively suppress the divergence of the attitude only when maneuvers are small. The invention does not need to introduce a more complex algorithm, and the running efficiency of the system is not reduced; the device can restrain the avionic divergence when a large maneuver exists, and can restrain the avionic divergence and the divergence of speed and position at the same time.
In one embodiment of the invention, there is also provided an electronic device comprising a memory and a processor, the memory having stored thereon a computer program running on the processor, the processor executing the steps of the fault handling method of the navigation system as described above when the computer program is run.
In one embodiment of the invention, there is also provided a computer readable storage medium having stored thereon a computer program which, when run, performs the steps of the fault handling method of a navigation system as described above.
Those of ordinary skill in the art will appreciate that: the foregoing description is only a preferred embodiment of the present invention, and the present invention is not limited thereto, but it is to be understood that modifications and equivalents of some of the technical features described in the foregoing embodiments may be made by those skilled in the art, although the present invention has been described in detail with reference to the foregoing embodiments. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (10)

1. A fault handling method for a navigation system, comprising the steps of:
1) Initializing a zone bit, and respectively setting a GPS fault zone bit GPSErrFlag and a gyro and accelerometer correction zone bit;
2) Reading data of a gyroscope and an accelerometer and outputting a GPS fault flag bit (GPSErrFlag) by a GPS board card, and executing the step 3) when the GPS fault flag bit (GPSErrFlag) output by the read GPS board card cannot be positioned and the gyroscope and the accelerometer can correct measurement data, otherwise executing the step 4);
3) Correcting the gyroscope and the accelerometer;
4) Performing inertial navigation pose resolving and one-step prediction of Kalman filtering according to the read gyroscope and accelerometer data, judging whether the GPS effective marker bit GPSValidFlag is effectively positioned, if yes, executing the step 5), otherwise, executing the step 6);
5) Performing Kalman filtering, performing pose feedback correction, and outputting a corrected result as a system output result;
6) And judging sensor correction conditions, selectively correcting sensor data, and outputting the pose calculated by inertial navigation as a system output result.
2. The method for processing fault of navigation system according to claim 1, wherein said step 1) initializes the flag bit, sets the GPS fault flag bit and gyro, accelerometer correction flag bit respectively, further comprises,
setting a GPS fault zone bit to be 1 to indicate that GPS has no positioning, and setting the GPS fault zone bit to be 0 to indicate GPS effective positioning;
setting the correction flag bit of the gyroscope and the accelerometer to be 1 indicates that correction can be performed by using the measured data of the gyroscope and the accelerometer, and setting the correction flag bit to be 0 indicates that correction cannot be performed;
and when the GPS fault flag bit is initialized, the GPS fault flag bit is assigned to 0, and the gyro and accelerometer correction flag bit is assigned to 0.
3. The method for fault handling of a navigation system of claim 1, wherein said step 3) performs gyroscopic and accelerometer corrections further comprises,
and correspondingly adding the recorded zero offset values of the gyroscope and the accelerometer into the angular acceleration measured by the gyroscope and the acceleration measured by the accelerometer respectively, and updating the measured values to finish correction of measured data of the gyroscope and the accelerometer.
4. The method of claim 1, wherein the step 4) of performing one-step prediction of inertial navigation pose calculation and kalman filtering based on the read gyro and accelerometer data, further comprises,
inertial navigation is carried out according to the angular velocity and the acceleration of the gyroscope and the accelerometer to calculate position, velocity and attitude information;
and (3) calculating the zero offset estimated value of the gyroscope and the zero offset estimated value of the accelerometer according to the calculated position, speed and gesture information.
5. The method according to claim 4, wherein the step of performing inertial navigation to calculate the position, velocity, and attitude information based on the angular velocity and acceleration of the gyro and the accelerometer, further comprises,
calculating quaternion according to the angular increment of the gyro data output period, and updating the gesture matrix according to the quaternion to calculate the gesture;
according to the rotation matrix of the carrier system converted to the navigation system, the rotation angular velocity of the earth generated by the carrier motion, the projection of the current velocity on the navigation system and the earth gravity acceleration, solving the velocity increment, and updating the velocity;
based on latitude, longitude, altitude, the sum of the earth's minor axis radius and the carrier altitude, and the sum of the earth's major axis radius and the carrier altitude.
6. The method for processing a fault of a navigation system according to claim 1, wherein said step 5) performs a kalman filter, performs a feedback correction of a pose, outputs a corrected result as a system output result, further comprises,
reading GPS sensor data, calculating a Kalman filtering measurement value, and carrying out filtering estimation by a Kalman filtering method;
and carrying out feedback correction on the pose according to the calculated inertial navigation pose, and taking the corrected result as a system output result.
7. The method for processing a fault in a navigation system according to claim 1, wherein said 6) performs a sensor correction condition determination, outputs the pose of the inertial navigation solution as a system output result, further comprises,
setting a data output period threshold vector of the gyroscope and the accelerometer, comparing the difference value of the corresponding variables of the zero offset estimated at adjacent moments with the data output period threshold of the gyroscope and the accelerometer, if the difference value is larger than the set threshold, the zero offset cannot be estimated stably by Kalman filtering, correction cannot be carried out, and the correction flag bit of the gyroscope and the accelerometer is assigned to be 0.
8. The method for fault handling of a navigation system of claim 7, further comprising,
respectively judging each dimension of data of the zero offset information vector;
when any one-dimensional data meets the output period threshold value of the gyroscope and the accelerometer data, the correction flag bit of the gyroscope and the accelerometer is assigned to be 1, sensor data correction is carried out, and the step 2) is executed in a return mode.
9. An electronic device comprising a memory and a processor, the memory having stored thereon a computer program running on the processor, the processor executing the steps of the fault handling method of the navigation system of any of claims 1 to 8 when the computer program is run.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when run, performs the steps of the fault handling method of a navigation system according to any one of claims 1 to 8.
CN202110988512.3A 2021-08-26 2021-08-26 Fault processing method of navigation system, electronic equipment and storage medium Active CN113703019B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110988512.3A CN113703019B (en) 2021-08-26 2021-08-26 Fault processing method of navigation system, electronic equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110988512.3A CN113703019B (en) 2021-08-26 2021-08-26 Fault processing method of navigation system, electronic equipment and storage medium

Publications (2)

Publication Number Publication Date
CN113703019A CN113703019A (en) 2021-11-26
CN113703019B true CN113703019B (en) 2023-12-22

Family

ID=78655246

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110988512.3A Active CN113703019B (en) 2021-08-26 2021-08-26 Fault processing method of navigation system, electronic equipment and storage medium

Country Status (1)

Country Link
CN (1) CN113703019B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114625121A (en) * 2022-01-24 2022-06-14 成都理工大学 Autonomous inspection exploration trolley system based on multi-sensor fusion and navigation method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102628691A (en) * 2012-04-09 2012-08-08 北京自动化控制设备研究所 Completely independent relative inertial navigation method
CN105300380A (en) * 2015-11-21 2016-02-03 广西南宁至简至凡科技咨询有限公司 Navigation system based on GPS/INS combination
CN106950586A (en) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN111505679A (en) * 2020-04-20 2020-08-07 中国科学院国家空间科学中心 L EO initial orbit determination method based on satellite-borne GNSS
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE10238061B4 (en) * 2002-08-20 2005-06-02 Litef Gmbh Method for determining and compensating for the scale factor error caused by wavelength change in a GPS-based INS system
US7409290B2 (en) * 2004-04-17 2008-08-05 American Gnc Corporation Positioning and navigation method and system thereof
US11747142B2 (en) * 2019-04-30 2023-09-05 Stmicroelectronics, Inc. Inertial navigation system capable of dead reckoning in vehicles

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102628691A (en) * 2012-04-09 2012-08-08 北京自动化控制设备研究所 Completely independent relative inertial navigation method
CN105300380A (en) * 2015-11-21 2016-02-03 广西南宁至简至凡科技咨询有限公司 Navigation system based on GPS/INS combination
CN106950586A (en) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 GNSS/INS/ Integrated Navigation for Land Vehicle methods for agricultural machinery working
CN111505679A (en) * 2020-04-20 2020-08-07 中国科学院国家空间科学中心 L EO initial orbit determination method based on satellite-borne GNSS
AU2020103939A4 (en) * 2020-12-08 2021-02-11 Xi'an University Of Architecture And Technology Polar Integrated Navigation Algorithm of SINS / GPS Based on Grid Framework

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
INS/GPS组合导航数据事后处理技术的研究;蒋鑫;《中国优秀硕士学位论文全文数据库 信息科技辑》(第03(2018年)期);I136-1475 *
基于安全窗口χ~2检验的捷联惯导空中修正算法;纪志农;任建辉;郑辛;王文辉;余凯;;中国惯性技术学报;第16卷(第04期);419-423 *
机载SAR用的GPS/SINS组合导航系统研究;杨大烨, 谢天怀, 胡宝余;中国惯性技术学报;第10卷(第4期);19-23 *

Also Published As

Publication number Publication date
CN113703019A (en) 2021-11-26

Similar Documents

Publication Publication Date Title
JP4782111B2 (en) System and method for estimating position, attitude and / or direction of flight of a vehicle
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
US7962255B2 (en) System and method for estimating inertial acceleration bias errors
CN108627154B (en) Polar region operation gesture and course reference system
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
JP2007232443A (en) Inertia navigation system and its error correction method
CN114858189A (en) Equivalent compensation method for gyro drift of strapdown inertial navigation system
CN111207745A (en) Inertia measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
Shen et al. A nonlinear observer for attitude estimation of vehicle-mounted satcom-on-the-move
CN113703019B (en) Fault processing method of navigation system, electronic equipment and storage medium
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN105928519B (en) Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer
Omerbashich Integrated INS/GPS navigation from a popular perspective
Yuan et al. A robust multi-state constraint optimization-based orientation estimation system for Satcom-on-the-move
CN116558511A (en) SINS/GPS integrated navigation method for improving Sage-Husa self-adaptive filtering
Avrutov Gyro north and latitude finder
Toda et al. Simulation design of thermopile and magnetometer aided INS/GPS navigation system for UAV navigation
Markov Autonomous Strapdown Attitude and Heading Reference System for a Small Agile UAV
Avrutov et al. Strapdown Gyro Latitude Finder
Stovner et al. GNSS-antenna lever arm compensation in aided inertial navigation of UAVs
Neusypin et al. Algorithmic Methods for Correcting Aircraft Astro-Inertial Systems
CN111238530B (en) Initial alignment method for air moving base of strapdown inertial navigation system
RU2776856C2 (en) Methods for determining the values of orientation angles during the movement of the aircraft and correcting the values of orientation angles
US11493344B2 (en) In-flight azimuth determination

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right

Effective date of registration: 20220113

Address after: 100190 inside 37 Xueyuan Road, Haidian District, Beijing

Applicant after: Beijing northern sky long hawk UAV Technology Co.,Ltd.

Address before: Room 201, floor 2, block C, Zhizhen building, No. 7, Zhichun Road, Haidian District, Beijing 100086

Applicant before: Beijing Yushi Hangtong Technology Co.,Ltd.

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant