Method and System for Improving Reliability and Anti-Degradation Control of UAV Navigation System

Abstract

The present invention belongs to the technical field of unmanned aerial vehicle (UAV) flight control, and particularly relates to a method and system for improving the reliability and anti-degradation control of a UAV navigation system. The method includes: constructing a multi-modal GNSS positioning device to synchronously output RTK position information, RTK fixed status, and SPP position information; constructing a dual-channel extended Kalman filter (EKF) to fuse the position information of RTK and SPP respectively, and obtaining a first routine and a second routine of position estimation correspondingly; using a state monitor to obtain the position deviation estimation of the EKF at the current moment to calculate the current position deviation compensation amount for correcting the second routine of position estimation; performing a two-dimensional reliability quality assessment on the SPP fusion channel to obtain a two-dimensional detection flag; and selecting the first routine of position estimation or the corrected second routine of position estimation as the position information finally output by the UAV navigation system based on the RTK fixed status and the two-dimensional detection flag, so as to realize the smooth switching between the RTK and SPP positioning modes.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) flight control, and particularly relates to a method and system for improving the reliability and anti-degradation control of a UAV navigation system. Background Art

[0002] In the industrial background of the large-scale development of the low-altitude economy, the UAV flight control system has put forward higher requirements for navigation and positioning accuracy. In particular, the accuracy attenuation phenomenon of positioning signals and the system reliability problem have become technical bottlenecks that urgently need to be solved for the large-scale application of UAVs in key fields such as logistics distribution, emergency rescue, and low-altitude inspection.

[0003] The existing technology mainly adopts an integrated navigation system based on Kalman filtering, which realizes positioning by fusing the integration prediction of inertial measurement unit (IMU) data and the external correction of a GNSS module with a single positioning output mode. However, during the actual flight process, the GNSS positioning accuracy has a decisive impact on the integration accuracy of the UAV. When encountering network signal attenuation, the GNSS module with a single positioning output mode will degrade from centimeter-level accuracy real-time kinematic (RTK) positioning to meter-level accuracy pseudorange single point positioning (SPP), resulting in a serious position drift phenomenon in the navigation system and affecting the operation accuracy. In addition, for the solution to this problem, technical means such as adding external vision sensors or lidar SLAM positioning systems are usually adopted. This method effectively enhances the robustness of the system by increasing the redundancy at the system hardware level, so as to better cope with the negative impact brought by the decrease in GNSS positioning accuracy.

[0004] For example, Chinese patent document CN118483728A discloses a multi-mode positioning device, method and system for UAVs under GNSS interference conditions. By verifying the reliability of GNSS satellite signals in the environment where the UAV is located, if the reliability meets the preset standard, the UAV positioning data is obtained through GNSS combined positioning by combining the GNSS satellite signals and the UAV images. If the reliability does not meet the preset standard, the images of the scene where the UAV is located are collected to obtain the UAV positioning data through visual positioning; and during the UAV positioning and navigation, the reliability of GNSS satellites in the environment where the UAV is located is continuously monitored, and the switching between GNSS combined positioning and visual positioning is carried out according to the reliability of GNSS satellites to achieve continuous navigation and positioning during the UAV mission execution process.

[0005] Chinese Patent Document CN116753937A discloses a real-time mapping fusion method based on UAV lidar and visual SLAM, which includes the following steps: Step 1, the inertial navigation model system fusion algorithm calculates the flight Euler angle coordinate system (x, y, z); Step 2, GNSS positioning and real-time image stitching generate a 2D map; Step 3, real-time images generate a 2D map and visual pose estimation; Step 4, GPU-accelerated stereo reconstruction to visual pose estimation generates a 2.5D digital elevation model. The present invention greatly reduces the probability of the UAV causing harm to ground personnel and can even improve the accuracy of precisely landing at a designated airport location.

[0006] However, the above GNSS module adopting a single positioning output mode can only output one positioning result of real-time kinematic (RTK) positioning data or pseudorange single-point positioning (SPP) data at any given moment. When the GNSS module degrades from the RTK positioning state to the SPP positioning state, since the GNSS module needs to re-initialize the state vector and state covariance matrix and cannot inherit the historical state information of the SPP, the SPP positioning result needs to go through a convergence process and cannot immediately reach the optimal positioning accuracy, and significant positioning deviations will occur during this period. This sharp drop in positioning accuracy will have a serious impact on the UAV navigation system: if the navigation system fuses the position information with large deviations in this section, it will cause obvious position drift of the UAV; if the navigation system chooses to discard this section of positioning data, it will cause the interruption of external positioning information and make the system lose an important position reference source.

[0007] The above-mentioned scheme of adding external sensors (such as visual SLAM, lidar point cloud matching, etc.) can build a multi-level hardware redundancy system, but the scheme faces many systematic challenges in practical applications: First, the introduction of core devices such as visual sensors and lidar significantly increases the hardware cost, resulting in a substantial increase in the overall system cost; Second, the calibration process of the multi-sensor system not only involves complex spatial parameter alignment but also needs to consider the timing synchronization problem between sensors, and its complexity grows exponentially with the number of sensors; Third, the spatio-temporal synchronization problem of multi-source heterogeneous data is particularly prominent, requiring precise timestamp alignment and spatial coordinate unification, which poses a severe test to the real-time performance of the system; Finally, the parallel processing and fusion calculation of multi-sensor data will cause the computing power requirement of the airborne computing platform to increase geometrically, which not only increases the power consumption but also poses higher requirements on the computing architecture of the processor. Summary of the Invention

[0008] The present invention aims to solve the position offset problem generated when the GNSS positioning accuracy of the UAV degrades, and provides a method for improving the reliability and anti-degradation control of the UAV navigation system, realizing effective improvement of the positioning accuracy and reliability of the UAV in a weak network signal environment without significantly increasing the hardware cost and system complexity.

[0009] The present invention also discloses a system loaded with a method for improving the reliability and anti-degradation control of an unmanned aerial vehicle (UAV) navigation system.

[0010] The detailed technical solution of the present invention is as follows:

[0011] A method for improving the reliability and anti-degradation control of an unmanned aerial vehicle (UAV) navigation system, the method comprising:

[0012] S1. Construct a multi-modal GNSS positioning device that combines real-time kinematic (RTK) differential positioning and single-point positioning (SPP) pseudorange to output in parallel a dual positioning mode, and the multi-modal GNSS positioning device synchronously outputs RTK position information in the navigation coordinate system , RTK fixed status and SPP position information in the navigation coordinate system ;

[0013] S2. Construct a dual-channel extended Kalman filter (EKF) including an RTK fusion channel and an SPP fusion channel, and use the dual-channel extended Kalman filter (EKF) to fuse the RTK position information and SPP position information respectively, so as to correspondingly obtain a first routine of position estimation and a second routine of position estimation ;

[0014] S3. Use a state monitor to continuously obtain the dual-channel position deviation estimation of the dual-channel extended Kalman filter (EKF) at the current moment , so as to calculate the current position deviation compensation amount ; ;

[0015] S4. Construct a dynamic deviation compensation model for the SPP fusion channel, and use the current position deviation compensation amount to correct the second routine of position estimation , so as to obtain a target second routine of position estimation corresponding to the SPP fusion channel ;

[0016] S5. Perform a two-dimensional reliability quality assessment on the SPP fusion channel to obtain its two-dimensional detection flag, and the two-dimensional detection flag includes an SPP position measurement residual detection flag and an SPP fusion channel state variance detection flag ;

[0017] S6. Execute GNSS positioning mode degradation control, that is, based on the RTK fixed status and the two-dimensional detection flag, select the first routine of position estimation or a target position estimation second routine The position information as the final output of the UAV navigation system , realizing the smooth switching between real-time kinematic positioning RTK and single point positioning SPP of pseudorange.

[0018] According to the preference of the present invention, in the S1, the RTK position information in the navigation coordinate system is , the RTK fixed state is , the SPP position information in the navigation coordinate system is ; wherein, , , respectively represent the coordinate values of the UAV on the N-axis, E-axis, and D-axis under real-time kinematic positioning RTK; , , respectively represent the coordinate values of the UAV on the N-axis, E-axis, and D-axis under single point positioning SPP of pseudorange; T represents matrix transpose;

[0019] The origin of the navigation coordinate system is the initial position when the UAV takes off, and the N, E, and D axes are orthogonal; wherein, the N-axis points to the due north direction; the E-axis points to the east direction and is perpendicular to the N-axis; the D-axis is perpendicular to the plane where the N-axis and E-axis are located, and the direction is downward.

[0020] According to the preference of the present invention, the S2 includes using the Kalman filtering method to fuse the RTK position information output by the multi-modal GNSS positioning device , correcting the first routine inertial navigation position predicted by the UAV according to the inertial measurement unit IMU , estimating the first routine fused positioning information with centimeter-level high precision, denoted as the position estimation first routine ;

[0021] Among them, the first routine inertial navigation position is , the position estimation first routine is ;

[0022] And, using the Kalman filtering method to fuse the SPP position information output by the multi-modal GNSS positioning device , correcting the second routine inertial navigation position predicted by the UAV according to the inertial measurement unit IMU , estimating the second routine fused positioning information with meter-level precision, denoted as the position estimation second routine ;

[0023] Among them, the second routine inertial navigation position is , and the position estimation second routine is .

[0024] Preferably according to the present invention, in S3, the state monitor calculates the dual-channel position deviation estimation at the current moment based on the first routine of position estimation and the second routine of position estimation , that is:

[0025] (1);

[0026] Perform low-pass filtering on the dual-channel position deviation estimation at the current moment to calculate the current position deviation compensation amount , that is:

[0027]

[0028] (2); In formula (2): represents the filtering parameter, which is calculated from the position estimation update interval and time constant of the SPP fusion channel; represents the current moment,

[0029] represents the position deviation compensation amount at the previous moment; And, judge the RTK fixed state , if , that is, when the RTK state is a fixed solution, update the current position deviation compensation amount based on the above formulas (1) and (2);

[0030] When it is detected that the real-time kinematic differential positioning RTK fails, stop the dynamic update of the dual-channel position deviation and lock the current position deviation compensation amount .

[0031] Preferably according to the present invention, in S4, the current position deviation compensation amount is dynamically injected into the second routine of position estimation for position deviation compensation, and the compensation formula is:

[0032] (3);

[0033] In formula (3): is the smoothing gain coefficient at the current moment , which decays with time to reduce the step response during switching; its recurrence formula is expressed as:

[0034] (4);

[0035] In formula (4): is the smoothing gain coefficient at the previous moment; represents the position estimation update interval of the SPP fusion channel; represents the time constant, and dynamically adjusts the compensation intensity through the exponential decay function, where the time constant , is used to ensure that the compensation amount decays smoothly with the RTK failure time.

[0036] Preferably according to the present invention, in the step S5, obtaining the SPP position measurement residual detection flag , includes:

[0037] Based on the SPP position information output by the multi-modal GNSS positioning device and the second routine inertial navigation position predicted by the inertial measurement unit IMU , calculate the value of the second routine position information, that is, the position residual :

[0038] (5);

[0039] Based on the position residual calculate the normalized residual of the second routine elevation position :

[0040] (6);

[0041] In formula (6): represents the variance of the second routine elevation position information;

[0042] And, calculate the normalized residual of the second routine horizontal position :

[0043] (7);

[0044] In formula (7): , respectively represent the second routine N component and E component position information variances;

[0045] Based on a preset threshold for residual detection, that is, judge the normalized residual of the second routine elevation position and the normalized residual of the second routine horizontal position , to obtain the SPP position measurement residual detection flag :

[0046] (8);

[0047] In Equation (8): and respectively represent the horizontal and elevation position standardized residual detection thresholds.

[0048] According to the preference of the present invention, in the step S5, obtaining the SPP fusion channel state variance detection flag includes:[[]]

[0049] Extracting the diagonal elements of the position estimation state covariance matrix, and calculating the sum of the SPP fusion channel position estimation variances:[[]]

[0050] (9);

[0051] In Equation (9): represents the sum of the SPP fusion channel position estimation variances; and and respectively represent N component, E component and D the position state variances of the components;

[0052] Among them, the position estimation state covariance matrix is expressed as:

[0053] (10);

[0054] In Equation (10): is a vector composed of diagonal elements, that is ;

[0055] Performing state variance detection based on a preset threshold to obtain the SPP fusion channel state variance detection flag :

[0056] (11);

[0057] In Equation (11): represents the position state variance detection threshold.

[0058] According to the preference of the present invention, the step S6 includes the switching of the first routine of position estimation to the second routine, that is:

[0059] Initial state: The current position estimation first routine output by the RTK fusion channel of the UAV navigation system is used as the final output position information ;

[0060] RTK fixed state detection:

[0061] If , that is, when RTK is in a fixed state, continue to output: ;

[0062] If , that is, when RTK is in a non-fixed state, enter the reliability evaluation of the SPP fusion channel;

[0063] Reliability evaluation of the SPP channel:

[0064] If the SPP position measurement residual detection flag , that is, the measurement residual detection passes, and the SPP fusion channel state variance detection flag , that is, the state variance detection passes, then switch to the second routine, that is, output: ;

[0065] If any one of the detections fails, then keep using the first routine, that is, output .

[0066] According to the preference of the present invention, the S6 further includes the switching from the second routine of position estimation to the first routine, that is:

[0067] Initial state: The current position of the UAV navigation system using the target position estimated by the SPP fusion channel and compensated and corrected by the second routine is used as the final output position information ;

[0068] RTK fixed state detection:

[0069] If , that is, when RTK is in a non-fixed state, continue to output: ;

[0070] If , that is, when RTK is in a fixed state, then switch back to the first routine, that is, output: .

[0071] In another aspect of the present invention, a system for implementing the method of improving the reliability and anti-degradation control of the UAV navigation system is provided. The system includes:

[0072] A multi-modal GNSS positioning device, constructed based on real-time kinematic differential positioning RTK and pseudo-range single-point positioning SPP, for synchronously outputting RTK position information in the navigation coordinate system , RTK fixed state and SPP position information in the navigation coordinate system ;

[0073] A dual-channel extended Kalman filter EKF, including an RTK fusion channel and an SPP fusion channel, for respectively fusing RTK position information and SPP location information , to obtain the first routine of position estimation and the second routine for position estimation ;

[0074] State monitor, used to obtain the dual-channel extended Kalman filter EKF at the current moment in real time Dual-channel position deviation estimation , to calculate the current position deviation compensation ; and, perform GNSS positioning mode degradation control, that is, based on the RTK fixed state and dual-dimensional detection landmark selection position estimation first routine Or target position estimation second routine As the final output of the UAV navigation system , realize smooth switching between real-time dynamic differential positioning RTK and pseudo-range single point positioning SPP;

[0075] Dynamic deviation compensation module, which builds a dynamic deviation compensation model with SPP fusion channel to use the current position deviation compensation amount Second routine for position estimation Correction is performed to obtain the target position estimation second routine corresponding to the SPP fusion channel ;

[0076] A reliability quality assessment module is used to perform a two-dimensional reliability quality assessment on the SPP fusion channel to obtain its two-dimensional detection mark, which includes an SPP position measurement residual detection mark. and SPP fusion channel state variance detection flag .

[0077] Compared with the prior art, the present invention has the following beneficial effects:

[0078] (1) The present invention provides a method for improving the reliability and anti-degradation control of a UAV navigation system, which significantly improves the performance and reliability of the UAV navigation system through the coordinated optimization of core technologies such as a multi-modal GNSS positioning device, a dual-channel extended Kalman filter (EKF) architecture, a dynamic deviation compensation model, and a dual-dimensional reliability detection mechanism.

[0079] (2) The multi-modal GNSS positioning device in the present invention realizes the parallel processing and synchronous output of RTK and SPP dual modes, ensuring high-precision positioning and robustness in complex environments; the dual-channel EKF architecture effectively suppresses the position drift when RTK fails and realizes seamless mode switching through the independently operating RTK and SPP fusion channels, combined with the real-time deviation compensation and smooth switching mechanism; the state monitor and the two-dimensional reliability detection mechanism significantly reduce the probability of mis-switching through the real-time evaluation of measurement residuals and state variances, ensuring the stability of the system in weak signal environments.

[0080] (3) The present invention introduces a dynamic deviation compensation model, which dynamically adjusts the compensation intensity through an exponential decay function, further optimizing the transition process from RTK to SPP and improving the positioning accuracy and continuity.

[0081] (4) While ensuring high precision and high reliability, the overall design of the present invention avoids the high costs and high complexities of traditional redundancy schemes, significantly enhancing the adaptability and operation efficiency of drones in weak network signal environments.

[0082] (5) The present invention provides an innovative solution for the navigation of drones in the low-altitude economy field, with broad application prospects and high application value. Description of the Drawings

[0083] Figure 1 is a flowchart of the method for improving the reliability and anti-degradation control of the drone navigation system described in the present invention.

[0084] Figure 2 is an execution block diagram of the method for improving the reliability and anti-degradation control of the drone navigation system described in the present invention. Detailed Embodiments

[0085] The present invention will be further described below in conjunction with the drawings and embodiments.

[0086] It should be noted that the following detailed descriptions are all exemplary and are intended to provide further explanations of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0087] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular forms are also intended to include the plural forms. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0088] Without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0089] The present invention provides a method for improving the reliability of a navigation and positioning system based on multi-state fusion and anti-drift control when the GNSS positioning mode degrades to solve the problem of position offset of an unmanned aerial vehicle (UAV) when the GNSS positioning accuracy degrades. The method independently and parallelly estimates the RTK and SPP position information through a multi-modal GNSS positioning device and synchronously outputs dual data streams, constructing a dual-channel position information fusion architecture for the UAV navigation system. Moreover, the method introduces a dynamic deviation compensation mechanism to achieve seamless switching between the RTK and SPP positioning modes, effectively suppressing the position drift caused by the degradation of positioning accuracy. At the same time, the safety and stability of mode switching are ensured through a two-dimensional reliability detection method, ultimately achieving high-precision and high-reliability positioning of the UAV in a complex environment and meeting the stringent requirements of the navigation system in the low-altitude economy field.

[0090] The following further describes the method and system for improving the reliability of the UAV navigation system and anti-degradation control of the present invention with specific embodiments.

[0091] Embodiment 1

[0092] Refer Figure 1 and Figure 2 This embodiment provides a method for improving the reliability of a UAV navigation system and anti-degradation control, and the method includes:

[0093] S1. Construct a multi-modal GNSS positioning device with parallel output of dual positioning modes by combining real-time kinematic (RTK) positioning and single-point positioning with pseudorange (SPP). The multi-modal GNSS positioning device synchronously outputs the RTK position information in the navigation coordinate system , RTK fixed status and the SPP position information in the navigation coordinate system .

[0094] In this embodiment, the traditional GNSS positioning device with single positioning mode output is improved to a multi-modal GNSS positioning device with parallel output of dual positioning modes, that is, a parallel processing architecture is constructed, enabling the UAV navigation system to simultaneously run RTK and SPP, and synchronously output the calculation results of the dual positioning modes.

[0095] The multi-modal GNSS positioning device synchronously outputs the RTK position information in the navigation coordinate system , RTK fixed status and the SPP position information in the navigation coordinate system Among them, the origin of the navigation coordinate system is the initial position when the UAV takes off; the three axes of north (N), east (E), and down (D) are orthogonal; among them, the N axis points to the due north direction; the E axis points to the east and is perpendicular to the N axis; the D axis is perpendicular to the plane where the N axis and the E axis are located, and the direction is downward.

[0096] The RTK position information in the navigation coordinate system is , RTK fixed status is , the SPP position information in the navigation coordinate system is ; among them, , , respectively represent the coordinate values of the UAV on the N axis, E axis, and D axis under real-time kinematic differential positioning RTK; , , respectively represent the coordinate values of the UAV on the N axis, E axis, and D axis under pseudorange point positioning SPP; T represents matrix transpose.

[0097] Based on the above, by constructing a multi-modal GNSS positioning device with a parallel processing architecture, adopting a multi-threaded solution engine and a hardware-level time synchronization technology, the independent parallel operation of real-time kinematic differential positioning RTK based on carrier phase observations and single-point positioning SPP based on pseudorange observations is realized, supporting the synchronous estimation and data output of the RTK and SPP dual positioning modes, ensuring the time alignment accuracy of the dual data streams, and providing a high-precision and highly reliable positioning data basis for the UAV navigation system.

[0098] Furthermore, the multi-modal GNSS positioning device is connected to the UAV flight control through the UART serial port for data communication.

[0099] S2. Construct a dual-channel extended Kalman filter EKF including an RTK fusion channel and an SPP fusion channel, and use the dual-channel extended Kalman filter EKF to fuse the RTK position information and the SPP position information respectively to obtain a first routine of position estimation and a second routine of position estimation .

[0100] Specifically, use the Kalman filtering method to fuse the RTK position information output by the multi-modal GNSS positioning device , correct the first routine inertial navigation position predicted by the UAV according to the inertial measurement unit IMU , and estimate the first routine fusion positioning information with centimeter-level high precision, denoted as the first routine of position estimation Among them, the inertial navigation position of the first routine is , and the first routine of position estimation is .

[0101] In addition, the SPP position information output by the multi-modal GNSS positioning device is fused using the Kalman filtering method , and the inertial navigation position of the second routine predicted by the drone according to the inertial measurement unit IMU is corrected , and the fused positioning information with meter-level accuracy is estimated, denoted as the second routine of position estimation . Among them, the inertial navigation position of the second routine is , and the second routine of position estimation is .

[0102] Based on the above, by constructing a dual-channel extended Kalman filter (EKF) architecture, the independent operation and collaborative optimization of the RTK fusion channel and the SPP fusion channel are respectively realized. Among them, the RTK fusion channel adopts a high-precision data fusion algorithm to achieve centimeter-level positioning accuracy; the SPP fusion channel ensures stable positioning in complex environments through a robust data fusion algorithm.

[0103] On the other hand, the prediction of the inertial navigation position through the inertial measurement unit IMU in the above is a conventional operation without improvement in the present invention, so it will not be elaborated in detail in the present invention.

[0104] S3. Use the state monitor to continuously obtain the dual-channel position deviation estimation of the dual-channel extended Kalman filter (EKF) at the current moment to calculate the current position deviation compensation amount . .

[0105] In this step, the state monitor calculates the dual-channel position deviation estimation at the current moment based on the first routine of position estimation and the second routine of position estimation , that is: (1);

[0106] (1);

[0107] Subsequently, the dual-channel position deviation estimation at the current moment is low-pass filtered to calculate the current position deviation compensation amount , that is: (2);

[0108] (2);

[0109] In formula (2): represents the filtering parameter, which is calculated from the position estimation update interval and time constant of the SPP fusion channel; represents the current moment, represents the position deviation compensation amount at the previous moment.

[0110] Based on the above, this step further includes: judging the RTK fixed state , if , that is, when the RTK state is a fixed solution, execute the above steps to update the current position deviation compensation amount ; otherwise, when detecting the failure of real-time kinematic differential positioning (RTK), stop the dynamic update of the dual-channel position deviation and lock the current position deviation compensation amount .

[0111] The above state monitor operates as an independent module, is decoupled from the dual-channel EKF routine, and calculates the position deviation compensation amount between the RTK fusion channel and the SPP fusion channel in real time for subsequent error correction of the SPP channel. At the same time, the state monitor executes the degradation control strategy of the GNSS positioning mode in the subsequent steps to achieve seamless switching between the RTK and SPP positioning modes and suppress the position drift caused by RTK failure. This design significantly improves the positioning accuracy and system robustness of the UAV in complex environments.

[0112] On the other hand, the low-pass filtering technology is used to smooth the high-frequency noise between the two channels and output a stable and reliable deviation compensation amount to ensure the compensation accuracy. When detecting the RTK failure, the system automatically locks the current deviation compensation amount, stops the dynamic update, realizes the RTK failure protection and smooth transition, effectively suppresses the position drift, and improves the navigation stability of the UAV in the GNSS positioning degradation scenario.

[0113] S4. Construct a dynamic deviation compensation model for the SPP fusion channel, and use the current position deviation compensation amount to correct the second position estimation routine to obtain the corresponding second target position estimation routine of the SPP fusion channel .

[0114] That is, dynamically inject the current position deviation compensation amount into the second position estimation routine for position deviation compensation, and the compensation formula is:

[0115] (3);

[0116] In formula (3): is the current moment The smoothing gain coefficient, which decays over time to reduce the step response during switching; its recurrence formula is expressed as:

[0117] (4);

[0118] In formula (4): is the smoothing gain coefficient at the previous moment; represents the position estimation update interval of the SPP fusion channel; represents the time constant, and the compensation intensity is dynamically adjusted through an exponential decay function, where the time constant , ensuring that the compensation amount decays smoothly with the RTK failure time.

[0119] The above dynamic deviation compensation model provides strong compensation at the initial stage of RTK failure to suppress position jumps, and then gradually reduces the compensation intensity to achieve seamless switching from RTK to SPP, significantly improving the navigation accuracy and stability of the UAV during GNSS positioning degradation.

[0120] S5. Perform a two-dimensional reliability quality assessment on the SPP fusion channel to obtain its two-dimensional detection flag, and the two-dimensional detection flag includes the SPP position measurement residual detection flag and the SPP fusion channel state variance detection flag .

[0121] In this step, the specific process of obtaining the SPP position measurement residual detection flag is as follows:

[0122] Based on the SPP position information output by the multi-modal GNSS positioning device and the second-routine inertial navigation position predicted by the inertial measurement unit IMU , calculate the second-routine position information value, that is, the position residual :

[0123] (5);

[0124] Furthermore, calculate the second-routine elevation position normalized residual :

[0125] (6);

[0126] In formula (6): represents the second-routine elevation position information variance;

[0127] And, calculate the second-routine horizontal position normalized residual :

[0128] (7);

[0129] In formula (7): and respectively represent the second routine N component sum E and the variance of the component position information.

[0130] Based on a preset threshold, residual detection is performed, that is, it is judged whether the normalized residual of the elevation position of the second routine and the normalized residual of the horizontal position of the second routine to obtain the SPP position measurement residual detection flag :

[0131] (8);

[0132] In formula (8): and respectively represent the normalized residual detection thresholds for the horizontal and elevation positions.

[0133] Moreover, the specific process of obtaining the SPP fusion channel state variance detection flag is as follows:

[0134] Extract the diagonal elements of the position estimation state covariance matrix, and calculate the sum of the SPP fusion channel position estimation variances:

[0135] (9);

[0136] In formula (9): represents the sum of the SPP fusion channel position estimation variances; and and respectively represent N component, E component sum D and the position state variances of the

[0137] component; among them, the position estimation state covariance matrix is expressed as:

[0138] (10);

[0139] In formula (10): is a vector composed of diagonal elements, that is .

[0140] Based on a preset threshold, state variance detection is performed, and then the SPP fusion channel state variance detection flag is obtained:

[0141] (11);

[0142] In formula (11): Indicates the position state variance detection threshold.

[0143] Based on the above, through the two-dimensional evaluation mechanism of position measurement residual detection and state variance detection, the positioning quality of the SPP channel is monitored in real time. Among them, the measurement residual detection is based on the calculation of the standardized residual, and times the standard deviation ( ) is used as the threshold, which conforms to the characteristics of the Gaussian distribution; the state variance detection evaluates the positioning stability through the diagonal elements of the covariance matrix. This mechanism effectively reduces the probability of mis-switching and significantly improves the navigation reliability of the UAV in complex environments.

[0144] S6. Execute the GNSS positioning mode degradation control, that is, based on the RTK fixed state and the two-dimensional detection flag, select the first routine of position estimation or the second routine of target position estimation as the position information finally output by the UAV navigation system , to achieve smooth switching between real-time kinematic positioning RTK and single point positioning SPP.

[0145] This step specifically includes: the switching from the first routine of position estimation to the second routine, that is:

[0146] (1) Initial state: The UAV navigation system currently uses the position of the first routine of position estimation output by the RTK fusion channel as the position information finally output .

[0147] (2) RTK fixed state detection:

[0148] If , that is, RTK is in a fixed state, continue to output: ;

[0149] If , that is, RTK is in a non-fixed state, enter the reliability evaluation of the SPP fusion channel.

[0150] (3) SPP channel reliability evaluation:

[0151] If the SPP position measurement residual detection flag (measurement residual detection passes) and the SPP fusion channel state variance detection flag (state variance detection passes), then switch to the second routine (SPP fusion channel), that is, output: .

[0152] If any of the detections fails, continue to use the first routine, that is, output .

[0153] This step specifically further includes: the switching of the second routine of position estimation to the first routine, that is:

[0154] (1) Initial state: The position of the second routine of target position estimation output by the current UAV navigation system using the SPP fusion channel and compensated and corrected is used as the final output position information .

[0155] (2) RTK fixed state detection:

[0156] If , that is, RTK is in a non-fixed state, then continue to output: ;

[0157] If , that is, RTK is in a fixed state, then switch back to the first routine, that is, output: .

[0158] Based on the above, through the RTK fixed state and the two-dimensional detection flag, seamless switching between the first and second routines of position estimation is achieved, and the optimal UAV navigation system position information is dynamically selected to ensure high-reliability navigation even when RTK fails, while avoiding mis-switching.

[0159] Embodiment 2,

[0160] This embodiment provides a system for realizing a method for improving the reliability and anti-degradation control of a UAV navigation system. The system includes:

[0161] A multi-modal GNSS positioning device, constructed based on real-time kinematic differential positioning RTK and pseudorange single-point positioning SPP, for synchronously outputting RTK position information in the navigation coordinate system , RTK fixed state and SPP position information in the navigation coordinate system ;

[0162] A dual-channel extended Kalman filter EKF, including an RTK fusion channel and an SPP fusion channel, for respectively fusing RTK position information and SPP position information , to correspondingly obtain the first routine of position estimation and the second routine of position estimation ;

[0163] A state monitor, for real-time obtaining the dual-channel position deviation estimation of the dual-channel extended Kalman filter EKF at the current moment , to calculate the current position deviation compensation amount , and, performing GNSS positioning mode degradation control, that is, based on the RTK fixed state ​​ The first routine for estimating the position of the two-dimensional detection flag selection or the second routine for estimating the target position The position information as the final output of the UAV navigation system , realizing the smooth switching between real-time kinematic positioning (RTK) and single point positioning (SPP);

[0164] The dynamic deviation compensation module constructs a dynamic deviation compensation model with an SPP fusion channel, which is used to utilize the current position deviation compensation amount to correct the second routine for estimating the position to obtain the second routine for estimating the target position corresponding to the SPP fusion channel ;

[0165] The reliability quality evaluation module is used to perform two-dimensional reliability quality evaluation on the SPP fusion channel to obtain its two-dimensional detection flag, and the two-dimensional detection flag includes the SPP position measurement residual detection flag and the SPP fusion channel state variance detection flag .

[0166] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the technical solutions of the present invention, rather than limitations on the specific implementation manners of the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and principle of the claims of the present invention shall be included within the protection scope of the claims of the present invention.

Claims

1. A method for improving the reliability and anti-degradation control of an unmanned aerial vehicle navigation system, characterized in that, The method includes: S1. A multi-modal GNSS positioning device that combines real-time kinematic (RTK) positioning and single-point positioning (SPP) pseudorange to construct a dual-positioning mode for parallel output, and the multi-modal GNSS positioning device synchronously outputs RTK position information in the navigation coordinate system , RTK fixed status , and SPP position information in the navigation coordinate system ; S2. Construct a dual-channel extended Kalman filter (EKF) including an RTK fusion channel and an SPP fusion channel, and use the dual-channel extended Kalman filter (EKF) to fuse the RTK position information and the SPP position information respectively to obtain a first routine for position estimation and a second routine for position estimation ; S3. Use the state monitor to obtain the two-channel position deviation estimation of the two-channel extended Kalman filter (EKF) at the current moment for calculating the current position deviation compensation amount ; ; S4. Construct a dynamic deviation compensation model for the SPP fusion channel, and use the current position deviation compensation amount to correct the second position estimation routine to obtain the target second position estimation routine corresponding to the SPP fusion channel ; S5. Conduct a two-dimensional reliability quality assessment on the SPP fusion channel to obtain its two-dimensional detection flag, where the two-dimensional detection flag includes the SPP position measurement residual detection flag and the SPP fusion channel state variance detection flag ; S6. Execute GNSS positioning mode degradation control, that is, based on the RTK fixed state and the two-dimensional detection flag, select the first routine of position estimation or the second routine of target position estimation as the position information finally output by the UAV navigation system , and realize the smooth switching between real-time kinematic positioning RTK and single point positioning with pseudorange SPP.

2. The method for improving the reliability and anti-degradation control of the UAV navigation system according to claim 1, wherein In the above S1, the RTK position information in the navigation coordinate system is , the RTK fixed state is , and the SPP position information in the navigation coordinate system is ; where , , respectively represent the coordinate values of the UAV on the N-axis, E-axis, and D-axis under Real-Time Kinematic (RTK) positioning; , , respectively represent the coordinate values of the UAV on the N-axis, E-axis, and D-axis under Single Point Positioning (SPP) using pseudorange; T represents matrix transpose; The origin of the navigation coordinate system is the initial position when the UAV takes off, and the N, E, and D axes are orthogonal; among them, the N axis points to the due north direction; the E axis points to the east and is perpendicular to the N axis; the D axis is perpendicular to the plane where the N axis and the E axis are located, and the direction is downward.

3. The method for improving the reliability and anti-degradation control of the UAV navigation system according to claim 2, wherein, The S2 includes using a Kalman filtering method to fuse the RTK position information output by the multi-modal GNSS positioning device , and correcting the first routine inertial navigation position predicted by the drone according to the inertial measurement unit IMU , and estimating the first routine fusion positioning information with centimeter-level high precision, denoted as the first routine of position estimation ; Among them, the first routine inertial navigation position is , and the position estimation first routine is ; Moreover, the SPP position information output by the multi-modal GNSS positioning device is fused using the Kalman filtering method , and the second routine inertial navigation position predicted by the drone according to the inertial measurement unit (IMU) is corrected , and the second routine fusion positioning information with meter-level accuracy is estimated, denoted as the second routine of position estimation ; Among them, the second routine inertial navigation position is , and the position estimation second routine is .

4. The method for improving the reliability and anti-degradation control of the UAV navigation system according to claim 1, characterized in that In S3, the state monitor calculates a dual-channel position deviation estimate at the current moment based on a first position estimation routine and a second position estimation routine That is :​ （1）； For the said current moment of the dual-channel position deviation estimation perform low-pass filtering to calculate the current position deviation compensation amount , namely: （2）； In formula (2): represents the filtering parameter, which is calculated from the position estimation update interval and time constant of the SPP fusion channel; represents the current moment, represents the position deviation compensation amount at the previous moment; And, determine the RTK fixed state , if , that is, when the RTK state is a fixed solution, update the current position deviation compensation amount based on the above formulas (1) and (2) ; When real-time kinematic (RTK) positioning failure is detected, stop the dynamic update of the dual-channel position deviation and lock the current position deviation compensation amount 。 5. The method for improving the reliability and anti-degradation control of the UAV navigation system according to claim 1, characterized in that, In S4, the current position deviation compensation amount is dynamically injected into the second routine of position estimation to perform position deviation compensation. The compensation formula is: （3）； In formula (3): is the smoothing gain coefficient at the current moment which decays over time to reduce the step response during switching; Its recurrence formula is expressed as: （4）； In formula (4): is the smoothing gain coefficient at the previous moment; represents the position estimation update interval of the SPP fusion channel; represents the time constant, and dynamically adjusts the compensation intensity through the exponential decay function, where the time constant , is used to ensure that the compensation amount decays smoothly with the RTK failure time.

6. The method for improving the reliability and anti-degradation control of an unmanned aerial vehicle navigation system according to claim 3, characterized in that In S5, obtain the SPP position measurement residual detection flag , including: SPP position information output by the multi-modal GNSS positioning device and the second routine inertial navigation position predicted by the inertial measurement unit IMU , calculate the second routine position information value, that is, the position residual : （5）； Based on the position residual Calculate the normalized residual of the elevation position of the second routine : （6）； In formula (6): represents the variance of the elevation position information of the second routine; And, calculate the second routine horizontal position normalized residual : （7）； In formula (7): and respectively represent the second routine N component sum E and the variance of the component position information; Perform residual detection based on a preset threshold, that is, judge the normalized residual of the elevation position of the second routine and the normalized residual of the horizontal position of the second routine , and obtain the SPP position measurement residual detection flag : （8）； In formula (8): and respectively represent the detection thresholds of the horizontal and elevation position standardized residuals.

7. The method for improving the reliability and anti-degradation control of the UAV navigation system according to claim 6, wherein, In S5, obtain the variance detection flag of the SPP fusion channel status , including: Extract the diagonal elements of the position estimation state covariance matrix, and calculate the sum of the position estimation variances of the SPP fusion channels: （9）； In formula (9): represents the sum of the position estimation variances of the SPP fusion channels; , , respectively represent N the component, E the component and D the position state variances of the component; Among them, the position estimation state covariance matrix is expressed as: （10）； In formula (10): is a vector composed of diagonal elements, that is ; Perform state variance detection based on a preset threshold to obtain the SPP fusion channel state variance detection flag : （11）； In formula (11): represents the position state variance detection threshold value.

8. The method for improving the reliability and anti-degradation control of an unmanned aerial vehicle navigation system according to claim 7, characterized in that The S6 includes the switching of the first routine of position estimation to the second routine, that is: Initial state: The first routine of the position estimation output by the RTK fusion channel currently used by the UAV navigation system is used as the position information of the final output ; RTK fixed state detection: If , that is, if the RTK is in a fixed state, then continue to output: ; If , that is, the RTK is in a non-fixed state, and the reliability assessment of the SPP fusion channel is entered; SPP channel reliability evaluation: If the SPP position measurement residual detection flag , that is, the measurement residual detection passes, and the SPP fusion channel state variance detection flag , that is, the state variance detection passes, then switch to the second routine, that is, output: ; If any test fails, keep using the first routine, i.e., output .

9. The method for improving the reliability and anti-degradation control of an unmanned aerial vehicle navigation system according to claim 7, characterized in that The S6 also includes the switching of the second routine of position estimation to the first routine, that is: Initial state: The current target position estimation second routine output by the UAV navigation system using the SPP fusion channel and compensated and corrected is used as the final output position information ; ; RTK fixed state detection: If , that is, if the RTK is in a non-fixed state, continue to output: ; If , that is, if the RTK is in a fixed state, then switch back to the first routine, that is, output: .

10. A system for implementing a method of improving the reliability and anti-degradation control of a UAV navigation system, characterized in that, The system includes: A multi-modal GNSS positioning device, constructed based on Real-Time Kinematic (RTK) and Single Point Positioning (SPP), is used to synchronously output RTK position information in the navigation coordinate system , RTK fixed status and SPP position information in the navigation coordinate system ; Dual-channel Extended Kalman Filter (EKF), including an RTK fusion channel and an SPP fusion channel, is used to fuse RTK position information and SPP position information respectively, so as to obtain the first routine of position estimation and the second routine of position estimation ; A state monitor for obtaining the dual-channel position deviation estimation of a dual-channel Extended Kalman Filter (EKF) at the current moment in real time to calculate the current position deviation compensation amount ; and, perform GNSS positioning mode degradation control, that is, select the first routine of position estimation or the second routine of target position estimation based on the RTK fixed state and the two-dimensional detection flag as the position information finally output by the UAV navigation system to achieve smooth switching between Real-Time Kinematic (RTK) and Single Point Positioning (SPP) ; ​ The dynamic deviation compensation module constructs a dynamic deviation compensation model with an SPP fusion channel for compensating the current position deviation amount to correct the second routine of position estimation to obtain the second routine of target position estimation corresponding to the SPP fusion channel ; A reliability quality assessment module is used to perform a two-dimensional reliability quality assessment on the SPP fusion channel to obtain its two-dimensional detection flag, and the two-dimensional detection flag includes an SPP position measurement residual detection flag and an SPP fusion channel state variance detection flag .

Images