CN113703019A - 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
CN113703019A
CN113703019A CN202110988512.3A CN202110988512A CN113703019A CN 113703019 A CN113703019 A CN 113703019A CN 202110988512 A CN202110988512 A CN 202110988512A CN 113703019 A CN113703019 A CN 113703019A
Authority
CN
China
Prior art keywords
accelerometer
gyroscope
gps
fault
correction
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.)
Granted
Application number
CN202110988512.3A
Other languages
Chinese (zh)
Other versions
CN113703019B (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 Yushi Hangtong 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 Yushi Hangtong Technology Co ltd filed Critical Beijing Yushi Hangtong 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

Images

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

Abstract

A fault handling method of a navigation system includes: 1) initializing a zone bit, and respectively setting a GPS fault zone bit and a gyro and accelerometer correction zone bit; 2) reading data of a gyroscope, an accelerometer and a GPS output fault mark, and executing the step 3) when the read GPS output fault mark cannot be positioned and the gyroscope and the accelerometer can correct measurement data, or executing the step 4); 3) correcting a gyroscope and an accelerometer; 4) performing inertial navigation pose resolving and Kalman filtering one-step prediction 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), and otherwise, executing the step 6); 5) performing Kalman filtering, performing pose feedback correction, and outputting a corrected result; 6) and judging the correction condition of the sensor, and outputting the pose resolved by the inertial navigation. The fault processing method of the navigation system does 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, an SINS (strapdown inertial navigation system)/GPS combined navigation method is commonly adopted in a low-altitude unmanned navigation system, the method is low in cost, and accurate pose information can be provided to meet flight requirements. However, the GPS is prone to failure due to the large number of obstructions during low-altitude flight. Because the unmanned aerial vehicle flying at low altitude mostly adopts MEMS sensors with lower precision, when the GPS fails, the pose information can be rapidly dispersed in a short time.
The integrated navigation system consists of a laser gyro strapdown inertial navigation system, a satellite positioning system receiver, an integrated navigation computer, a milemeter, an altitude meter, a base station radar system and the like. The positioning precision and the autonomy of SAR + image navigation and the attitude determination precision of a star light navigation system of a star sensor are combined, so that the autonomous flight of the unmanned aircraft is ensured. Drone navigation is the correct guidance of a drone to a destination along a predetermined route at a specified time, with the required accuracy. In order for the unmanned aerial vehicle to successfully complete a predetermined navigation task, navigation parameters such as a real-time position, a navigation speed, a course and the like of the unmanned aerial vehicle must be known besides 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-assisted navigation, geomagnetic navigation and the like. These navigation technologies have their own advantages and disadvantages, and therefore, in the navigation of the unmanned aerial vehicle, it is important to select an appropriate navigation positioning technology according to different tasks that the unmanned aerial vehicle is responsible for.
Adopt three kinds of sensors of triaxial gyroscope, triaxial accelerometer, GPS to make up navigation's unmanned aerial vehicle, make up navigation in service, unmanned aerial vehicle's flight can meet the condition that the low latitude sheltered from, and GPS can become invalid, and GPS can't provide speed, position equivalent survey information. The traditional SINS/GPS integrated navigation method can not carry out filtering because of lack of measurement information, and can cause rapid divergence of the 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 corrects the real-time measurement values of a triaxial gyroscope and a triaxial accelerometer and carries out pose calculation by adopting the estimated triaxial gyroscope and triaxial accelerometer zero offset in the running process of the system, and provides stable pose information for the normal flight of an unmanned aerial vehicle 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 following steps:
1) initializing a zone bit, and respectively setting a GPS fault zone bit and a gyro and accelerometer correction zone bit;
2) reading data of a gyroscope, an accelerometer and a GPS output fault mark, and executing the step 3) when the read GPS output fault mark cannot be positioned and the gyroscope and the accelerometer can correct measurement data, or executing the step 4);
3) correcting a gyroscope and an accelerometer;
4) performing inertial navigation pose resolving and Kalman filtering one-step prediction 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), and 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 the correction condition of the sensor, and outputting the pose resolved by the inertial navigation as a system output result.
Further, the step 1) of initializing the zone bit and respectively setting the GPS fault zone bit and the gyro and accelerometer correction zone bit further comprises,
setting the GPS fault flag bit as 1 to indicate that the GPS has no positioning, and setting the GPS fault flag bit as 0 to indicate that the GPS has effective positioning;
setting the correction flag bit of the gyroscope and the accelerometer to be 1 to indicate that the measurement data of the gyroscope and the accelerometer can be corrected, and setting the correction flag bit of the gyroscope and the accelerometer to be 0 to indicate that the correction cannot be performed;
during initialization, the GPS fault flag bit is assigned to 0, and the gyro and accelerometer correction flags are assigned to 0.
Further, the step 3) of correcting the gyroscope and the accelerometer further comprises the steps of,
and (4) 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 the correction of the measured data of the gyroscope and the accelerometer.
Further, the step 4) of performing inertial navigation pose solution and further prediction of Kalman filtering according to the read gyroscope and accelerometer data further comprises,
performing inertial navigation to calculate position, speed and attitude information according to the angular velocity and acceleration of the gyroscope and the accelerometer;
and solving the zero offset estimation value of the gyroscope and the zero offset estimation value of the accelerometer according to the solved position, speed and attitude information.
Further, the step of performing inertial navigation to calculate the position, the speed and the attitude information according to the angular velocity and the acceleration of the gyroscope and the accelerometer also comprises the steps of,
resolving quaternion according to the angle increment of the gyro data output period, and updating an attitude matrix according to the quaternion to resolve attitude;
solving a speed increment according to a rotation matrix converted from a carrier system to a navigation system, the rotational angular velocity of the earth, the rotational angular velocity relative to the earth generated by the motion of the carrier, the projection of the current speed on the navigation system and the gravity acceleration of the earth, and updating the speed;
and resolving the position according to the latitude, the longitude, the altitude, the sum of the short axis radius of the earth and the altitude of the carrier and the sum of the long axis radius of the earth and the altitude of the carrier.
Further, the step 5) of performing kalman filtering, performing feedback correction of the pose, and outputting the corrected result as the system output result further comprises,
reading GPS sensor data, calculating a Kalman filtering measurement value, and performing filtering estimation by a Kalman filtering method;
and performing feedback correction of 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 the sensor correction condition and outputting the pose resolved by the inertial navigation as the system output result further comprises,
setting a gyro data output period threshold vector, comparing the difference value of variables corresponding to zero offset estimated at different moments with a gyro data output period threshold, if the difference value is greater than a set threshold, Kalman filtering fails to stably estimate zero offset, correction cannot be performed, and correction flag bits of a gyro and an accelerometer are assigned to be 0.
Furthermore, the method also comprises the following steps of,
respectively judging data of each dimension of the recorded zero offset information vector;
and when any dimension of data meets the gyro data output period threshold, assigning the correction flag bits of the gyro and the accelerometer to be 1, correcting the sensor data, and returning to execute the step 2).
In order to achieve the above object, the present invention further provides an electronic device, which includes a memory and a processor, wherein the memory stores a computer program running on the processor, and the processor executes the steps of the fault handling method of the navigation system when executing 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 executed, performs the steps of the fault handling method of the navigation system as described above.
The fault processing method of the navigation system has the following beneficial effects:
1) the Kalman filtering is utilized to estimate the zero offset of the three-axis gyroscope and the three-axis accelerometer, the estimation 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 inertial navigation alignment attitude calculation is indirectly improved, and the method can be used as a fault processing method for short-time failure of a GPS in a GPS navigation system, so that the system can provide stable attitude information under the condition and meet the flight requirement of an unmanned aerial vehicle in a short time;
2) by skillfully utilizing the Kalman filtering estimation value, the fault of the 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 pose output 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 embodiments of the invention and together with the description serve to explain the principles of the invention and not to limit the invention. In the drawings:
FIG. 1 is a flow chart of a method of fault handling for a navigation system according to the present invention;
FIG. 2 is a flow chart according to an embodiment of the invention.
Detailed Description
The preferred embodiments of the present invention will be described in conjunction with the accompanying drawings, and it will be understood that they are described herein for the purpose of illustration and explanation and not limitation.
Example 1
Fig. 1 is a flowchart of a fault handling method of a navigation system according to the present invention, and the fault handling method of the navigation system of the present invention will be described in detail with reference to fig. 1.
First, in step 101, a flag bit is initialized.
Preferably, a GPS fault flag GPSErrFlag is set, wherein 1 indicates that the GPS has no positioning, and 0 indicates that the GPS can effectively position; setting a correction flag mendFlag of the gyroscope and the accelerometer, wherein the correction flag is 1, which indicates that the data measured by the gyroscope and the accelerometer can be corrected, and 0 indicates that the data cannot be corrected, and the correction flag is initially set to 0 and is not corrected.
In this embodiment, the three-axis gyroscope and the three-axis accelerometer estimated by kalman filtering have a zero offset value B.
Figure BDA0003231643780000051
The initialization is 0. Wherein epsilonxIs zero offset, epsilon, of X-axis gyroyZero-offset, epsilon, of a Y-axis gyrozIs zero offset of the Z-axis gyroscope,
Figure BDA0003231643780000052
adding zero offset of the table to the X axis,
Figure BDA0003231643780000053
adding zero offset of the table to the Y axis,
Figure BDA0003231643780000054
zero offset is added to the Z axis.
In this embodiment, a state variable is set
Figure BDA0003231643780000055
The initialization is 0. Wherein
Figure BDA0003231643780000056
In order to be the attitude error vector,
Figure BDA0003231643780000057
in order to be the velocity error vector,
Figure BDA0003231643780000058
in order to be a position error vector,
Figure BDA0003231643780000059
is a zero-offset vector of the gyroscope,
Figure BDA00032316437800000510
is the accelerometer zero offset vector.
In this embodiment, the state variable covariance matrix PkThe initialization of the array is:
Figure BDA00032316437800000511
Figure BDA00032316437800000512
Figure BDA00032316437800000513
Figure BDA00032316437800000514
Figure BDA00032316437800000515
Figure BDA00032316437800000516
wherein
Figure BDA00032316437800000517
In order to be the initial attitude error vector,
Figure BDA00032316437800000518
for the x-axis initial attitude error,
Figure BDA00032316437800000519
for the initial attitude error of the y-axis,
Figure BDA00032316437800000520
is the z-axis initial attitude error; delta VerrFor the initial velocity error vector, δ VEerrFor east initial velocity error, δ VNerrFor north initial velocity error, δ VUerrIs the initial speed error in the direction of the sky;
Figure BDA00032316437800000521
as initial position error vector, δ LerrTo initial latitude error, δ λerrFor initial longitude error, δ HerrIs the initial height error;
Figure BDA00032316437800000522
the initial zero offset vector of the gyroscope in the carrier coordinate system,
Figure BDA00032316437800000523
the initial zero offset of the X-axis gyroscope in the carrier coordinate system,
Figure BDA00032316437800000524
the initial zero offset of the Y-axis gyroscope in the carrier coordinate system,
Figure BDA00032316437800000525
the initial zero offset of a Z-axis gyroscope under a carrier coordinate system;
Figure BDA0003231643780000061
an accelerometer initial zero offset vector under a carrier coordinate system,
Figure BDA0003231643780000062
the initial zero offset of the accelerometer of the X axis under the carrier coordinate system,
Figure BDA0003231643780000063
the initial zero offset of the Y-axis accelerometer in the carrier coordinate system,
Figure BDA0003231643780000064
and (3) carrying out initial zero offset on the Z-axis accelerometer in a carrier coordinate system.
In this embodiment, the gain matrix K is initialized to 0, and the corresponding observation matrix is initialized to: hk＝[06×3 16×606×6]。
Preferably, during initialization, the GPS fault flag GPSErrFlag is assigned to 0, and the gyro and accelerometer correction flag mendFlag is assigned to 0.
At step 102, gyro and accelerometer data are read.
Preferably, reading the measurement data of the gyroscope and the accelerometer and reading the fault mark output by the GPS board card; then, when the output fault of the GPS board card is 1, the GPS board card cannot be positioned, namely the GPS error flag is 1, and the gyro and the accelerometer can be corrected, namely the mendFlag value is 1, the step 103 is executed; otherwise, step 104 is performed.
In step 103, gyro and accelerometer corrections are performed.
Preferably, the zero offset value epsilon of the three-axis gyroscope and the three-axis accelerometer to be recordedx、εy、εz、
Figure BDA0003231643780000065
Figure BDA0003231643780000066
Respectively and correspondingly adding angular acceleration omega measured by a three-axis gyroscopex、ωy、ωzAcceleration value a measured by a triaxial accelerometerx、ay、az. The updated measurements are:
ωx＝ωx+εx，ωy＝ωy+εy，ωz＝ωz+εz；
Figure BDA0003231643780000067
and after updating, finishing correction of the measurement data of the gyroscope and the accelerometer, setting the zero offset values of the gyroscope and the accelerometer in the state variable X matrix to be 0, and executing the step 104.
In step 104, inertial navigation pose calculation and KF one-step prediction are carried out.
Preferably by means of an updated angular velocity ωx、ωy、ωzAcceleration ax、ay、azAnd solving the latest position, speed and attitude information. The specific inertial navigation solution formula is as follows:
and (3) posture updating:
Figure BDA0003231643780000068
wherein ω is [ ω ═ ω [ [ ω ]x,ωy,ωz]TThe angular increment delta theta is omega T, T is the gyro data output period, Q is the resolving quaternion,
Figure BDA0003231643780000071
updating the attitude matrix by quaternion:
Figure BDA0003231643780000072
resolving the attitude:
Figure BDA0003231643780000073
preferably, solving for the speed increment:
Figure BDA0003231643780000074
wherein f isb＝[ax,ay,az]T，Cb nFor the carrier system to be converted to the rotation matrix of the navigation system,
Figure BDA0003231643780000075
the rotational angular velocity of the earth is measured,
Figure BDA0003231643780000076
angular velocity of rotation, v, relative to the earth due to carrier motionnProjection of the current velocity on the navigation system, gnAcceleration of earth gravity.
And (3) updating the speed: v. ofk＝vk-1+fbT
Preferably, the location update:
Figure BDA0003231643780000077
wherein
Figure BDA0003231643780000078
L is the latitude at the current moment, lambda is the longitude at the current moment, h is the altitude at the current moment, RMhSum of the minor axis radius of the earth and the height of the carrier, RNhThe sum of the radius of the earth's long axis and the height of the carrier.
Performing one-step prediction of Kalman filtering after resolving, wherein the formula is as follows:
Figure BDA0003231643780000081
Figure BDA0003231643780000082
the zero offset estimation value of the three-axis gyroscope is solved by the formula
Figure BDA0003231643780000083
Zero offset estimation value of triaxial accelerometer
Figure BDA0003231643780000084
Assigning zero offset vector to gyroscope and accelerometer at the moment of k-1
Figure BDA0003231643780000085
And recorded. The GPS receiver judges whether the GPS is effective or not and assigns a GPS effective flag bit GPSValidFlag, if so, step 105 is executed, otherwise, step 106 is executed.
In step 105, kalman filtering is performed.
Preferably, the gyro and accelerometer correction flag bits mendFlag are cleared, and mendFlag is set to 0, indicating that no sensor data correction is required.
Reading GPS sensor data and calculating Kalman filtering measurement value
Figure BDA0003231643780000086
Then, the filter estimation is performed by the kalman filter method. The solution formula for kalman filter estimation is as follows:
Figure BDA0003231643780000087
will vector
Figure BDA0003231643780000088
Zero offset estimation value of medium-updated k-time three-axis gyroscope
Figure BDA0003231643780000089
Zero offset estimation value of triaxial accelerometer
Figure BDA00032316437800000810
Is assigned to
Figure BDA00032316437800000811
And recorded.
Wherein epsilonx,kZero offset, epsilon, of X-axis gyro at time ky,kZero offset, epsilon, of Y-axis gyro at time kz,kZero offset of the Z-axis gyroscope at the time k;
Figure BDA00032316437800000812
for the time k the X-axis accelerometer is zero offset,
Figure BDA00032316437800000813
the Y-axis accelerometer is zero offset for time k,
Figure BDA0003231643780000091
the Z-axis accelerometer has zero offset for time k.
Resolving the pose of step 104 inertial navigation
Figure BDA0003231643780000092
vkpkMinus
Figure BDA0003231643780000093
In the updated attitude error
Figure BDA0003231643780000094
Error in velocity
Figure BDA0003231643780000095
Position error
Figure BDA0003231643780000096
Namely, feedback correction of the pose is executed, and the corrected result is madeAnd outputting the result for the system.
In step 106, the sensor corrects the condition determination.
Preferably, the pose of the inertial navigation solution is output as the system output result.
In the embodiment of the invention, firstly, in the initial execution stage of the integrated navigation algorithm and the initial recovery stage after the GPS fails, the zero offset of the gyroscope and the zero offset of the accelerometer estimated by Kalman filtering are unstable and cannot be used for executing the error correction of the sensor; secondly, the zero offset of the gyroscope and the zero offset of the accelerometer in different axial directions cannot be estimated at the same time due to the flight of different attitudes and paths, and the error correction of the sensor cannot be executed at the same time, so that the judgment and selective correction are needed.
Preferably, threshold vectors of zero deviation values estimated by the gyroscope and the accelerometer at adjacent moments are set
Figure BDA0003231643780000097
Estimating zero offset B for adjacent timek、Bk-1And comparing the difference value of the corresponding variable with a set threshold value, and if the difference value is greater than the set threshold value, indicating that the Kalman filtering cannot stably estimate zero offset and cannot correct, and assigning the correction flag bits mendFlag of the gyroscope and the accelerometer to be 0. Since the information of recording zero offset is a 6 × 1 dimensional vector, the data of each dimension needs to be separately determined, as shown by the following pseudo code:
caseBk(1)-Bk-1(1)<T(1)：εx,k＝Bk(1)；
caseBk(2)-Bk-1(2)<T(2)：εy,k＝Bk(2)；
caseBk(3)-Bk-1(3)<T(3)：εz,k＝Bk(3)；
Figure BDA0003231643780000098
Figure BDA0003231643780000099
Figure BDA00032316437800000910
the above pseudo code represents: zero offset B of x-axis gyroscope at time kk(1) Zero offset B of x-axis gyroscope at time k-1k-1(1) When the difference of (A) is less than T (1), B is addedk(1) Assign to epsilonx,kThe correction of the gyroscope and the accelerometer in the next period; other variables in vector B with Bk(1) The same is done. And when any data meets the threshold value, assigning the mendFlag to be 1, namely correcting the sensor data, and returning to execute the second step.
Fig. 2 is a schematic flow chart of an embodiment of the present invention, and as shown in fig. 2, the present invention is applicable to a combined navigation system including three sensors, namely, a three-axis gyroscope, a three-axis accelerometer, and a GPS. In the operation of the integrated navigation system, the condition of low altitude occlusion can be met, and the GPS can be disabled. In this case, the GPS cannot provide measurement information such as speed and position. The traditional SINS/GPS integrated navigation method can not carry out filtering because of lack of measurement information, and can cause rapid divergence of the pose. Under the condition, the real-time measurement values of the three-axis gyroscope and the three-axis accelerometer are corrected by adopting the zero offset of the three-axis gyroscope and the three-axis accelerometer which are estimated in the running process of the system; and then, adopting the corrected real-time inertial measurement data to carry out pose calculation. Compared with the pose resolved by the inertial data which is not corrected, the resolved pose result is dispersed and slowed down, and stable pose information can be provided for the unmanned aerial vehicle to fly normally in the short time of GPS failure.
The invention designs a fault processing method for short-time failure of a GPS in a GPS integrated navigation system aiming at the technical defect that pose information can be quickly dispersed in a short time when the GPS fails in the conventional low-altitude unmanned navigation system, and can effectively inhibit quick dispersion of the pose under the condition of independently using an inertial navigation method. Other failure solutions need to introduce a new calculation method, and the divergence of the attitude can be effectively inhibited only when the movement is small. The invention does not need to introduce a more complex algorithm, and can not reduce the operating efficiency of the system; the device can inhibit the divergence of the attitude and the divergence of the speed and the position while inhibiting the divergence of the attitude and the divergence of the speed and the position when the device has larger maneuvering.
In an embodiment of the present invention, there is also provided an electronic device, including a memory and a processor, where the memory stores a computer program running on the processor, and the processor executes the steps of the fault handling method of the navigation system as described above when running the computer program.
In an embodiment of the present invention, there is also provided a computer-readable storage medium having stored thereon a computer program which, when running, performs the steps of the fault handling method of a navigation system as described above.
Those of ordinary skill in the art will understand that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that changes may be made in the embodiments and/or equivalents thereof without departing from the spirit and scope of the invention. Any modification, equivalent replacement, or improvement 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 of a navigation system is characterized by comprising the following steps:
1) initializing a zone bit, and respectively setting a GPS fault zone bit and a gyro and accelerometer correction zone bit;
2) reading data of a gyroscope, an accelerometer and a GPS output fault mark, and executing the step 3) when the read GPS output fault mark cannot be positioned and the gyroscope and the accelerometer can correct measurement data, or executing the step 4);
3) correcting a gyroscope and an accelerometer;
4) performing inertial navigation pose resolving and Kalman filtering one-step prediction 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), and 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 the correction condition of the sensor, and outputting the pose resolved by the inertial navigation as a system output result.
2. The method for handling the fault of the navigation system according to claim 1, wherein the step 1) of initializing a flag bit, setting a GPS fault flag bit and a gyro and accelerometer correction flag bit, respectively, further comprises,
setting the GPS fault flag bit as 1 to indicate that the GPS has no positioning, and setting the GPS fault flag bit as 0 to indicate that the GPS has effective positioning;
setting the correction flag bit of the gyroscope and the accelerometer to be 1 to indicate that the measurement data of the gyroscope and the accelerometer can be corrected, and setting the correction flag bit of the gyroscope and the accelerometer to be 0 to indicate that the correction cannot be performed;
during initialization, the GPS fault flag bit is assigned to 0, and the gyro and accelerometer correction flags are assigned to 0.
3. The method for processing the fault of the navigation system according to claim 1, wherein the step 3) of performing gyro and accelerometer correction further comprises,
and (4) 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 the correction of the measured data of the gyroscope and the accelerometer.
4. The method for processing the fault of the navigation system according to claim 1, wherein the step 4) of performing one-step prediction of inertial navigation pose solution and Kalman filtering according to the read gyro and accelerometer data further comprises,
performing inertial navigation to calculate position, speed and attitude information according to the angular velocity and acceleration of the gyroscope and the accelerometer;
and solving the zero offset estimation value of the gyroscope and the zero offset estimation value of the accelerometer according to the solved position, speed and attitude information.
5. The method of processing a failure in a navigation system according to claim 4, wherein the step of performing inertial navigation to calculate position, velocity, and attitude information based on angular velocities and accelerations of a gyro and an accelerometer further comprises,
resolving quaternion according to the angle increment of the gyro data output period, and updating an attitude matrix according to the quaternion to resolve attitude;
solving a speed increment according to a rotation matrix converted from a carrier system to a navigation system, the rotational angular velocity of the earth, the rotational angular velocity relative to the earth generated by the motion of the carrier, the projection of the current speed on the navigation system and the gravity acceleration of the earth, and updating the speed;
and resolving the position according to the latitude, the longitude, the altitude, the sum of the short axis radius of the earth and the altitude of the carrier and the sum of the long axis radius of the earth and the altitude of the carrier.
6. The method for processing a failure in a navigation system according to claim 1, wherein the step 5) of performing Kalman filtering, performing feedback correction of the pose, and outputting the corrected result as a system output result further comprises,
reading GPS sensor data, calculating a Kalman filtering measurement value, and performing filtering estimation by a Kalman filtering method;
and performing feedback correction of the pose according to the calculated inertial navigation pose, and taking the corrected result as a system output result.
7. The method for processing the fault of the navigation system according to claim 1, wherein the 6) step of performing the judgment of the sensor correction condition and outputting the pose of the inertial navigation solution as the system output result further comprises,
setting a gyro data output period threshold vector, comparing the difference value of variables corresponding to zero offset estimated at different moments with a gyro data output period threshold, if the difference value is greater than a set threshold, Kalman filtering fails to stably estimate zero offset, correction cannot be performed, and correction flag bits of a gyro and an accelerometer are assigned to be 0.
8. The method of fault handling for a navigation system of claim 7, further comprising,
respectively judging data of each dimension of the recorded zero offset information vector;
and when any dimension of data meets the gyro data output period threshold, assigning the correction flag bits of the gyro and the accelerometer to be 1, correcting the sensor data, and returning to execute the step 2).
9. An electronic device, comprising a memory and a processor, wherein the memory stores a computer program running on the processor, and the processor executes the steps of the fault handling method of the navigation system according to any one of claims 1 to 8 when executing the computer program.
10. A computer-readable storage medium, on which a computer program is stored, characterized in that the computer program, when running, performs the steps of the fault handling method of the navigation system of 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 true CN113703019A (en) 2021-11-26
CN113703019B 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)

Cited By (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 (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
US20060149473A1 (en) * 2002-08-20 2006-07-06 Manfred Krings Method for determining and compensating the scale factor error cause by a wavelength change in a gps-based inertial navigation system
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
US20200348137A1 (en) * 2019-04-30 2020-11-05 Stmicroelectronics, Inc. Inertial navigation system capable of dead reckoning in vehicles
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

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060149473A1 (en) * 2002-08-20 2006-07-06 Manfred Krings Method for determining and compensating the scale factor error cause by a wavelength change in a gps-based inertial navigation system
US20050234644A1 (en) * 2004-04-17 2005-10-20 Ching-Fang Lin Positioning and navigation method and system thereof
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
US20200348137A1 (en) * 2019-04-30 2020-11-05 Stmicroelectronics, Inc. Inertial navigation system capable of dead reckoning in vehicles
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
杨大烨, 谢天怀, 胡宝余: "机载SAR用的GPS/SINS组合导航系统研究", 中国惯性技术学报, vol. 10, no. 4, pages 19 - 23 *
纪志农;任建辉;郑辛;王文辉;余凯;: "基于安全窗口χ~2检验的捷联惯导空中修正算法", 中国惯性技术学报, vol. 16, no. 04, pages 419 - 423 *
蒋鑫: "INS/GPS组合导航数据事后处理技术的研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 03, pages 136 - 1475 *

Cited By (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

Also Published As

Publication number Publication date
CN113703019B (en) 2023-12-22

Similar Documents

Publication Publication Date Title
CN109813311B (en) Unmanned aerial vehicle formation collaborative navigation method
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
JP2005283586A (en) Error correction of inertia navigation system
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
JP2007232443A (en) Inertia navigation system and its error correction method
CN110296719B (en) On-orbit calibration method
CN113503894B (en) Inertial navigation system error calibration method based on gyro reference coordinate system
CN111207745A (en) Inertia measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
CN108627154B (en) Polar region operation gesture and course reference system
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN114858189A (en) Equivalent compensation method for gyro drift of strapdown inertial navigation system
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
US9217639B1 (en) North-finding using inertial navigation system
CN111141285B (en) Aviation gravity measuring device
CN113932803B (en) Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft
CN116222551A (en) Underwater navigation method and device integrating multiple data
Avrutov Gyro north and latitude finder
Avrutov et al. Strapdown Gyro Latitude Finder
Yuan et al. A robust multi-state constraint optimization-based orientation estimation system for Satcom-on-the-move
CN111964671A (en) Inertial astronomical integrated navigation system and method based on double-axis rotation modulation
CN111238530B (en) Initial alignment method for air moving base of strapdown inertial navigation system
Stovner et al. GNSS-antenna lever arm compensation in aided inertial navigation of UAVs

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
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.

GR01 Patent grant
GR01 Patent grant