Unmanned aerial vehicle autonomous navigation method based on GNSS denial and multi-source information fusion

Abstract

The application discloses a GNSS denial and multi-source information fusion based unmanned aerial vehicle autonomous navigation method and belongs to the technical field of navigation measurement. The application adopts or logic to determine the GNSS environment state through multi-dimensional indexes, and realizes navigation mode jump-free switching through a weighted smoothing mechanism. When GNSS is effective, GNSS / IMU loose combination filtering is adopted, and when GNSS is denied, a factor graph optimization framework is constructed, visual inertial pre-integration, laser odometry, height constraint and heading constraint in a non-electromagnetic interference scene are uniformly accessed, and digital orthographic image and elevation model joint landscape matching correction, fault adaptive adjustment and online parameter calibration are combined. The application significantly improves the precision, stability and robustness of unmanned aerial vehicle navigation in a complex environment, effectively suppresses the cumulative drift in a long-time denial environment and is suitable for unmanned aerial vehicle autonomous flight in satellite blocking, electromagnetic interference and other scenes.

Description

Technical Field

[0001] This invention belongs to the field of navigation and measurement technology, specifically relating to an autonomous navigation method for unmanned aerial vehicles (UAVs) based on GNSS denial and multi-source information fusion. Background Technology

[0002] With the widespread application of drones in fields such as inspection, surveying, and emergency rescue, the requirements for the environmental adaptability and reliability of navigation systems are constantly increasing. Existing drones generally use the Global Navigation Satellite System (GNSS) as the core positioning method, combined with an Inertial Measurement Unit (IMU) to achieve integrated navigation. However, in urban buildings and canyons, indoor spaces, dense forests in mountains, and GNSS-denied environments with electromagnetic interference, satellite signals are prone to interruption, distortion, or complete failure. This causes traditional GNSS / IMU integrated navigation systems to be unable to output stable attitude information, seriously threatening flight safety.

[0003] To address navigation challenges in GNSS denied environments, existing technologies have proposed multi-odometer fusion schemes combining vision, laser, and inertial units. However, these schemes still suffer from the following shortcomings: First, they lack scene adaptation and switching performance. Most schemes are designed for a single navigation scenario, and hard switching between GNSS active and denied modes is often used, which can easily lead to attitude jumps during the switching process. Second, the accuracy of the fusion framework is limited. Mainstream schemes often use linear filtering frameworks such as extended Kalman filters, which are insufficient for error estimation when dealing with multi-source heterogeneous and highly nonlinear observation data. Third, the ability to suppress cumulative drift is weak. Pure relative odometry schemes can develop severe cumulative drift as flight time increases. Fourth, they lack fault tolerance and self-calibration capabilities. Single sensor failures or parameter drift can easily lead to a sharp drop in overall navigation accuracy. Summary of the Invention

[0004] The purpose of this invention is to provide an autonomous navigation method for unmanned aerial vehicles (UAVs) based on GNSS denial and multi-source information fusion, so as to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: an autonomous navigation method for unmanned aerial vehicles based on GNSS denial and multi-source information fusion, comprising the following steps: Step S1: Collect satellite signal data output by the UAV's onboard GNSS receiver in real time, calculate the signal carrier-to-noise ratio, the number of visible satellites and the confidence level of the positioning solution, determine whether the current navigation environment is a GNSS denied environment based on a preset threshold, and generate the corresponding navigation mode switching command. Step S2: Collect raw measurement data from airborne multi-source sensors, including an inertial measurement unit, a monocular vision camera, a lidar, a barometric altimeter, and a triaxial magnetometer. Perform timestamp alignment and outlier removal preprocessing on the raw measurement data from each sensor. Step S3: When the environment is determined to be a valid GNSS environment, the GNSS positioning result is used as the subjective measurement, and a loosely combined Kalman filter is performed in combination with the inertial measurement unit data to output the position, velocity and attitude navigation information of the UAV. Step S4: When the environment is determined to be GNSS denied, switch to the multi-source fusion autonomous navigation mode: obtain the image feature tracking results and IMU pre-integration increment through visual inertial front-end processing, calculate the laser odometry increment through lidar point cloud registration, extract the altitude observation value of the barometric altimeter, and extract the heading observation value of the three-axis magnetometer as a constraint value in the GNSS denied scenario caused by non-electromagnetic interference. Step S5: Construct a factor graph optimization framework, integrate the visual-inertial pre-integration factor, laser odometry factor, altitude constraint factor, and available heading constraint factor into the factor graph, and obtain the optimal estimate of the UAV's six-degree-of-freedom pose through nonlinear optimization. Step S6: Output the optimal pose estimate obtained in step S5 to the UAV flight control system as the navigation input for autonomous flight control of the UAV.

[0006] As a preferred implementation, the specific method for determining the GNSS denial environment in step S1 is as follows: preset carrier-to-noise ratio threshold, visible satellite number threshold, and positioning accuracy factor threshold. When any of the following conditions are met within N consecutive sampling periods: the satellite signal carrier-to-noise ratio is lower than the carrier-to-noise ratio threshold, the number of visible satellites is less than the visible satellite number threshold, or the positioning accuracy factor is greater than the positioning accuracy factor threshold, it is determined that the environment has entered the GNSS denial environment, where N is an integer between 3 and 10.

[0007] As a preferred implementation, the timestamp alignment in step S2 adopts linear interpolation. Taking the sampling time of the inertial measurement unit as the reference time axis, the asynchronous sampling data of the monocular vision camera, lidar, barometric altimeter and triaxial magnetometer are uniformly registered to the reference time axis through interpolation operation to achieve time synchronization of multi-source data.

[0008] In a preferred embodiment, in step S4, the visual inertial front-end processing uses optical flow to track the corner features of consecutive frame images, calculates the pixel displacement between adjacent frames, and combines the angular velocity and acceleration pre-integration of the inertial measurement unit to generate visual inertial pre-integration observation factors, which are then directly input into the factor graph framework.

[0009] In a preferred implementation, the laser radar point cloud registration in step S4 adopts a normal distribution transformation algorithm to register the current frame laser point cloud with the local point cloud map of the previous time step, solve the six-degree-of-freedom transformation matrix as the laser odometry increment, and mark and remove frames with registration scores lower than a set threshold.

[0010] As a preferred implementation, the factor graph optimization framework in step S5 adopts an incremental smooth graph building algorithm, where each sensor measurement corresponds to a factor node and the UAV pose state corresponds to a variable node, and incremental updates are achieved through a Bayesian tree structure. Each time a new observation is added, only the affected variable nodes are relinearized and optimized.

[0011] As a preferred implementation, a landscape matching correction step is also performed in GNSS denied environments: real-time ground images acquired by an airborne monocular vision camera are matched with pre-stored digital orthophoto maps and corresponding regional digital elevation model datasets, and the three-dimensional geographic coordinates of matching feature points are obtained by combining the digital elevation model. The absolute geographic coordinates of the UAV are calculated by the perspective n-point algorithm, and the absolute coordinates are added to the factor map as global position constraint factors to periodically correct navigation cumulative drift.

[0012] As a preferred implementation, it also includes a sensor fault adaptive adjustment step: calculating the residual statistics of each sensor measurement in real time; when the residual of a certain sensor continuously exceeds the chi-square test threshold, it is determined that the sensor has failed, and the information weight of the corresponding factor of the sensor in the factor graph is automatically reduced until it is temporarily removed from the fusion system. When it is determined that the GNSS rejection is caused by electromagnetic interference, the heading constraint factor corresponding to the three-axis magnetometer is automatically removed from the fusion system and does not participate in the pose optimization solution.

[0013] As a preferred implementation, the method also includes an online calibration step: during navigation, the residual information optimized by the factor graph is used to estimate the zero bias error of the inertial measurement unit, the external parameter deviation between the camera and the inertial measurement unit, and the installation angle deviation of the lidar online, and the estimated deviation is fed back to the preprocessing stage in real time for compensation and correction.

[0014] As a preferred implementation, a weighted smooth transition mechanism is adopted when switching navigation modes in step S1: when the GNSS signal is detected to change from valid to rejected, the GNSS observation weight is gradually reduced within M sampling periods, while the multi-source fusion weight is gradually increased. When the GNSS signal recovers from rejection to validity, a reverse weighted transition is performed, where M is an integer between 5 and 20.

[0015] Compared with the prior art, the beneficial effects of the present invention are: 1. Achieve smooth and seamless switching in GNSS scenarios. By jointly determining the GNSS status through multi-dimensional indicators and combining a weighted smooth transition mechanism, the pose jump problem caused by traditional hard switching is avoided, and the stability and continuity of flight in complex environments are improved. Compared with single threshold determination and hard switching schemes, it has a significant performance improvement.

[0016] 2. A unified factor graph fusion framework for multi-source heterogeneous sensors is constructed, which integrates four types of heterogeneous observations—visual inertial, laser odometry, altitude constraint, and heading constraint—into a factor graph optimization system. Combined with an incremental Bayesian tree optimization algorithm, this approach fully leverages the complementary advantages of each sensor while ensuring the real-time computing capabilities of the airborne platform. The navigation accuracy in nonlinear scenarios is significantly better than that of traditional Kalman filtering schemes.

[0017] 3. Multi-dimensional suppression of cumulative drift and extension of endurance in denied environments: Based on relative odometry, global position constraints for landscape matching, absolute constraints for barometric altitude and absolute constraints for magnetometer heading are introduced to correct cumulative drift under long-term GNSS denial from three dimensions. This can significantly improve navigation accuracy in satellite-free environments without the need for additional high-precision hardware.

[0018] 4. It has fault tolerance and online self-calibration capabilities, and the system is highly robust. It deeply integrates fault adaptive weight adjustment and multi-sensor online calibration into the factor graph framework, forming a complete closed loop of fault detection, dynamic weight adjustment and real-time parameter compensation. It can effectively deal with sensor faults and parameter drift problems, and significantly improve the reliability and environmental adaptability of the navigation system under complex working conditions. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the overall process of the method of the present invention. Detailed Implementation

[0020] The present invention will be further described below with reference to embodiments.

[0021] The following embodiments are used to illustrate the present invention, but should not be used to limit the scope of protection of the present invention. The conditions in the embodiments can be further adjusted according to specific conditions, and simple improvements to the method of the present invention under the premise of the concept of the present invention are all within the scope of protection claimed by the present invention.

[0022] This embodiment is applied to a small quadcopter drone. The airborne navigation sensors include a GNSS receiver, a six-axis MEMS inertial measurement unit (sampling frequency 200Hz), a global shutter monocular vision camera (sampling frequency 20Hz), a single-line lidar (sampling frequency 10Hz), a MEMS barometric altimeter (sampling frequency 50Hz), and a three-axis magnetoresistive magnetometer (sampling frequency 50Hz).

[0023] Please see Figure 1 This invention provides an autonomous navigation method for unmanned aerial vehicles (UAVs) based on GNSS denial and multi-source information fusion, comprising the following steps: S1, GNSS Status Determination and Mode Switching The system collects satellite signal data output from the UAV's onboard GNSS receiver in real time, calculates the signal-to-noise ratio, the number of visible satellites, and the confidence level of the positioning solution, determines whether the current navigation environment is a GNSS denied environment based on a preset threshold, and generates corresponding navigation mode switching commands.

[0024] In this step, the preset carrier-to-noise ratio threshold is 35dB-Hz, the visible satellite number threshold is 4, and the positioning accuracy factor (PDOP) threshold is 6.0. A continuous judgment period N is set. When any of the following conditions are met within N consecutive GNSS sampling periods: the average carrier-to-noise ratio of satellites is lower than 35dB-Hz, the number of visible satellites is less than 4, or the PDOP value is greater than 6.0, it is determined that the current environment has entered the GNSS denial environment, and a mode switching command is generated.

[0025] The continuous determination period N ranges from 3 to 10. The suitable scenarios and implementation methods for each integer value are as follows: N=3: Suitable for scenarios with frequent GNSS state switching, such as high dynamic penetration and rapid traversal of buildings and canyons. It has the shortest judgment delay, can quickly respond to signal interruption and recovery, and is compatible with GNSS receivers with a sampling frequency ≥10Hz. N=4: Suitable for medium and high speed inspection and low-altitude flight in mountainous areas, balancing response speed and anti-misjudgment ability, and balancing switching sensitivity and navigation stability. N=5: This is the preferred value for typical scenarios in this embodiment, applicable to most inspection and surveying operations, achieving the optimal balance between anti-misjudgment capability and response speed; N=6: Suitable for urban-rural transition areas and scenarios with intermittent satellite signal obstruction. It can filter out brief signal fluctuations and reduce the number of invalid mode switching. N=7: Suitable for flight in forest areas and scenarios with scattered tree canopy obstruction, suppressing short-term signal attenuation misjudgment caused by swaying branches and leaves; N=8: Suitable for stable cruise and mild electromagnetic fluctuation scenarios, further enhancing anti-interference capabilities and avoiding frequent mode switching from affecting flight control stability; N=9: Suitable for long-endurance, stable flight scenarios with high signal stability, and will only trigger switching when the signal continues to deteriorate; N=10: Suitable for high-altitude cruise and wide-area mapping scenarios, with the strongest anti-misjudgment capability, and can completely filter out false triggers caused by instantaneous signal flicker and multipath interference.

[0026] Those skilled in the art can directly select the corresponding integer value within the range of 3 to 10 based on the UAV's flight conditions, GNSS sampling frequency, and anti-misjudgment requirements.

[0027] The navigation mode switching adopts a weighted smooth transition mechanism, setting a transition period M. Seamless switching is achieved through a linear weighting gradient. The specific implementation is as follows: Let the total number of transition periods be M, and the current period be the k-th transition period (k=1,2,…,M). The weights are calculated according to a linear rule: The effective-to-rejection process of GNSS results: The weight of GNSS results gradually decreases, while the weight of multi-source fusion results gradually increases. w_gnss=1-k / M, w_fusion=k / M GNSS denial to validity process: weight change reverse execution w_gnss=k / M, w_fusion=1-k / M The final navigation output is obtained by weighted fusion of the two results, using the following formula: P_out=w_gnss×P_gnss+w_fusion×P_fusion In the formula: P_out: The final output navigation pose result; w_gnss: Weights of the GNSS / IMU loosely integrated navigation results; P_gnss: Pose result output by GNSS / IMU loose combination; w_fusion: Weights of the multi-source factor graph fusion result; P_fusion: The pose result output by multi-source factor graph fusion.

[0028] The transition period M ranges from 5 to 20, and the suitable scenarios and implementation methods for each integer value are as follows: M=5: Suitable for strong interference scenarios where GNSS signals deteriorate rapidly, can quickly switch to autonomous navigation mode, has short transition time, and is suitable for high dynamic flight conditions; M=6: Suitable for building canyon entrance scenarios with rapid signal attenuation, balancing switching speed and pose smoothness; M=7: Suitable for scenarios where signals disappear rapidly, such as sudden entry into mountainous areas or tunnel entrances; M=8: Suitable for typical building occlusion scenarios, balancing switching response speed and navigation output smoothness; M=9: Suitable for general urban-rural transition areas, adaptable to scenarios where the signal slowly attenuates; M=10: This is the preferred value for typical scenarios in this embodiment, which is applicable to most inspection and surveying operation scenarios and achieves the optimal balance between switching speed and pose smoothness. M=11: Suitable for low-altitude flight scenarios in urban areas with frequent signal fluctuations; M=12: Suitable for densely populated areas, suppressing frequent weight fluctuations caused by intermittent signal transmission; M=13: Suitable for intermittent obstruction scenarios in forest areas, filtering out short-term signal fluctuations caused by tree canopy obstruction; M=14: Suitable for urban canyon scenarios with significant multipath interference, improving navigation output stability; M=15: Suitable for industrial areas with complex electromagnetic environments, providing a smooth transition and reducing the impact of interference on flight control; M=16: Suitable for long-endurance, stable flight and wide-area scenarios with slow signal changes; M=17: Suitable for wide-area surveying and mapping, and high-altitude patrol operations. M=18: Suitable for aerial surveying and mapping scenarios where the smoothness of navigation output is extremely important; M=19: Suitable for high-precision surveying operations, maximizing pose smoothness during transitions; M=20: Suitable for work scenarios with stringent requirements for attitude stability. Mode switching has no perceptible impact on posture, and navigation output is extremely smooth.

[0029] Those skilled in the art can directly select the corresponding integer value within the range of 5 to 20 based on the complexity of the flight environment and the requirements for smoothness of navigation output.

[0030] This step improves the accuracy and sensitivity of GNSS state identification by using multi-dimensional indicators or logical joint judgment, avoids missed judgments when a single indicator deteriorates, and avoids the attitude jump problem caused by traditional hard handover by combining a weighted smooth transition mechanism, effectively improving the stability of flight and the continuity of navigation output in complex environments.

[0031] S2, Multi-source sensor data preprocessing Raw measurement data from airborne multi-source sensors, including an inertial measurement unit, a monocular vision camera, a lidar, a barometric altimeter, and a triaxial magnetometer, are collected. The raw measurement data from each sensor are preprocessed by timestamp alignment and outlier removal.

[0032] In this step, timestamp alignment uses the sampling time of the 200Hz inertial measurement unit as the reference time axis. First-order linear interpolation is used for the 20Hz image data, 10Hz laser point cloud data, and 50Hz barometer and magnetometer data to register the measurement values ​​of each sensor to each sampling time of the inertial measurement unit, thereby achieving time synchronization of all sensor data. Outlier removal adopts the 3σ criterion. The mean and standard deviation of the measurement values ​​of the most recent 100 sampling points of each sensor are statistically calculated. Data points that deviate from the mean by more than three times the standard deviation are identified as outliers and removed. Interpolation of adjacent valid data is used to complete the data.

[0033] This step enables time synchronization and data cleaning of multi-source heterogeneous sensors, providing a reliable input with a unified time reference for subsequent multi-source fusion, and effectively eliminating fusion errors caused by asynchronous sampling and abnormal data.

[0034] Loose-combined Kalman filtering in S3 and GNSS-enabled environments When the environment is determined to be a valid GNSS environment, the GNSS positioning result is used as a subjective measurement, and a loosely combined Kalman filter is performed in combination with the inertial measurement unit data to output the UAV's position, velocity, and attitude navigation information.

[0035] In this step, based on the strapdown inertial navigation mechanics arrangement of the inertial measurement unit, a 15-dimensional state vector containing position error, velocity error, attitude error, and IMU zero bias error is constructed; using the three-dimensional position and three-dimensional velocity output by GNSS as observation vectors, Kalman filter state equations and observation equations are constructed; through a prediction-update filtering loop, various errors of the inertial navigation system are estimated in real time and feedback corrections are made, outputting the three-dimensional position, three-dimensional velocity, and three-dimensional attitude angle navigation information of the UAV.

[0036] The specific construction and execution process of the pine-combined Kalman filter is as follows: 1. Definition of State Vector Select a 15-dimensional error state vector: X=[δr,δv,δφ,∇,ε]^T In the formula: δr is the three-dimensional position error, δv is the three-dimensional velocity error, and δφ is the three-dimensional attitude error angle. ε represents the zero bias error of the three-dimensional accelerometer, and ε represents the zero bias error of the three-dimensional gyroscope.

[0037] 2. State equations (discrete form) Describe the propagation relationship of state variables over time: X_k=Φ·X_{k-1}+Γ·W_{k-1} In the formula: X_k and X_{k-1} are the state vectors at time k and time k-1, respectively. Φ: The state transition matrix from time k-1 to time k; Γ: Noise driving matrix; W_{k-1}: System noise vector, containing random noise from the accelerometer and gyroscope.

[0038] 3. Observation Equations The difference between GNSS observations and inertial navigation system (INS) calculations is used as the observation input: Z_k = H·X_k + V_k In the formula: Z_k: Observation vector, obtained by subtracting the position and velocity calculated by the inertial navigation system from the position and velocity observed by the GNSS, i.e., Z_k=[r_imu-r_gnss,v_imu-v_gnss]^T; H: Observation matrix, which represents the mapping relationship between state error and observation difference; V_k: GNSS observation noise vector.

[0039] 4. Prediction-Update Filter Loop (1) Prediction steps: Calculate the prior state at the current time based on the state at the previous time step. Prior state estimation:

[0040] Prior covariance matrix: P_k⁻=Φ·P_{k-1}·Φ^T+Γ·Q·Γ^T In the formula: Here, P is the state estimate, Q is the system noise covariance matrix, and the superscript - indicates the prior value.

[0041] (2) Update step: Combine the current observation correction state to obtain the posterior estimate. Kalman gain:

[0042] Posterior state estimation:

[0043] Posterior covariance matrix:

[0044] In the formula: R is the observation noise covariance matrix, and I is the identity matrix.

[0045] The error estimate obtained from filtering is fed back to the strapdown inertial navigation system in real time to correct the position, velocity, attitude and sensor bias, and output the final navigation information.

[0046] This step, under normal GNSS signal conditions, fully utilizes the absolute positioning advantage of GNSS combined with the high dynamic response characteristics of inertial navigation to ensure the accuracy and real-time performance of navigation output, adapting to the routine flight requirements in open environments.

[0047] Multi-source measurement preprocessing in S4 and GNSS denied environments When the environment is determined to be GNSS denied, the multi-source fusion autonomous navigation mode is switched on: the image feature tracking results and IMU pre-integration increment are obtained through visual inertial front-end processing, the laser odometry increment is calculated through lidar point cloud registration, and the altitude observation value of the barometric altimeter is extracted. In GNSS denial scenarios not caused by electromagnetic interference, the heading observation value of the triaxial magnetometer is extracted as a constraint. If the GNSS denial is determined to be caused by electromagnetic interference, the triaxial magnetometer data is shielded and not included in the fusion observation.

[0048] The determination that "GNSS denial is caused by electromagnetic interference" adopts a three-level determination method, as follows: 1. Initial judgment of signal characteristics: When the carrier-to-noise ratio of GNSS full-band signal drops by more than 10dB-Hz in one sampling period, the number of visible satellites drops to zero in a short time and there is no gradual recovery trend, and the UAV is currently in an open and unobstructed airspace, it is initially judged to be an electromagnetic interference scenario. 2. Electromagnetic monitoring confirmation: If the airborne system is equipped with an electromagnetic monitoring module, it will be confirmed by combining L-band electromagnetic field strength data. When the L-band (1164MHz~1615MHz) electromagnetic field strength exceeds the preset interference threshold, it will be finally determined that the GNSS denial is caused by electromagnetic interference. 3. Elimination method as a fallback: When there is no airborne electromagnetic monitoring module, the elimination method is used to determine the situation. If, after eliminating physical obstruction factors such as terrain obstruction, building obstruction, and drone movement obstruction, the GNSS is still in a denial state and the visual, laser, and barometric pressure sensors are working normally, it is determined to be an electromagnetic interference scenario.

[0049] Once an electromagnetic interference scenario is identified, the triaxial magnetometer data is immediately shielded and excluded from the fusion observation.

[0050] In this step, the visual-inertial front-end processing uses the KLT optical flow method to track Harris corner features in adjacent image frames and calculate the pixel displacement of feature points. Simultaneously, it pre-integrates the IMU data between adjacent image frames to obtain incremental observations of angular velocity and acceleration, generating visual-inertial pre-integration factors that are directly input into the subsequent factor map optimization framework. This avoids separate sliding window optimization, preventing architectural conflicts and consistency errors with the back-end factor map optimization. The lidar point cloud registration uses the Normal Distribution Transform (NDT) algorithm. After transforming the current frame's lidar point cloud to the vehicle coordinate system, it is registered with the local point cloud map constructed in the previous time step. The six-degree-of-freedom transformation matrix between the two frame point clouds is iteratively optimized and used as the lidar odometry increment. A registration score threshold of 0.3 is set. When the registration score is below 0.3, the current frame is considered to have failed registration, is marked and removed, and does not participate in subsequent fusion. Meanwhile, the altitude value output by the barometric altimeter is used as the absolute altitude observation after temperature compensation. In non-electromagnetic interference scenarios, the geomagnetic vector output by the triaxial magnetometer is used to calculate the heading angle after tilt compensation, and is used as the absolute heading observation.

[0051] This step utilizes complementary observations from multiple sensors, including vision, laser, barometer, and magnetometer, to form multi-dimensional pose estimation and constraints. This avoids navigation interruption caused by the failure of a single sensor and provides rich observation inputs for subsequent fusion optimization. Meanwhile, the front end only performs observation preprocessing, unifying all optimization solutions to the back end factor graph, ensuring the consistency of the system architecture and the accuracy of estimation.

[0052] S5. Factor plot optimization and drift correction A factor graph optimization framework is constructed, which integrates visual-inertial pre-integration factors, laser odometry factors, altitude constraint factors, and available heading constraint factors into the factor graph. The optimal estimate of the UAV's six-degree-of-freedom pose is obtained through nonlinear optimization.

[0053] In this step, the factor graph optimization framework adopts the incremental smooth graph construction (iSAM2) algorithm, and the specific construction process is as follows: 1. Define variable nodes: The six-degree-of-freedom pose of the UAV at each keyframe time T_i=(p_i,R_i) (p_i is the three-dimensional position vector and R_i is the three-dimensional attitude rotation matrix) is used as the core variable node; the subsequently augmented sensor parameters are used as auxiliary variable nodes.

[0054] 2. Define factor nodes and residuals: Each type of sensor observation corresponds to a factor node, and each factor corresponds to a residual term, representing the deviation between the observed value and the state estimate. Visual-inertial pre-integration factor: connects the poses of adjacent keyframes, and the residual is the error between the pre-integrated observation and the pose transformation. Laser odometry factor: Connecting the poses of adjacent laser frames, the residual is the error between the pose transformation amount and the pose difference in NDT registration; Altitude constraint factor: corresponding to a single-time position node, the residual is the error between the barometric altitude observation value and the pose altitude component; Heading constraint factor: corresponding to the attitude node at a single moment, the residual is the error between the magnetometer heading angle and the attitude heading component.

[0055] 3. Construct the optimization objective function: The overall optimization objective is to minimize the weighted sum of squared residuals of all factors, as shown in the formula: minΣ||r_i||²_{Ω_i} In the formula: r_i is the residual vector of the i-th factor, and Ω_i is the information matrix of the i-th factor (the inverse of the observation covariance), which represents the confidence level of the observation.

[0056] 4. Incremental solution: Based on the iSAM2 algorithm, the factor graph is transformed into a Bayesian tree structure. Each time a new observation factor is added, only the affected subtree is relinearized and solved, without the need for full graph re-optimization, thus ensuring airborne real-time performance.

[0057] Meanwhile, in GNSS denied environments, terrain matching correction is performed every 5 seconds: real-time ground downward-looking images acquired by a monocular camera are compared with pre-stored digital orthophoto maps of the operational area and corresponding digital elevation models (DEM) datasets using ORB feature extraction and matching. Based on the coordinates of the matched image feature points, the corresponding elevation values ​​are read from the DEM to generate the three-dimensional geographic coordinates of the feature points. The absolute geographic coordinates of the UAV are then calculated using the perspective n-point (PnP) algorithm.

[0058] The specific process of adding absolute geographic coordinates as a global location constraint factor to the factor graph is as follows: After calculating the absolute geographic coordinates p_global and corresponding covariance of the UAV using the PnP algorithm, a global position prior factor is constructed, corresponding to the pose variable node at the matching time. The residual of this factor is r_pos = p_k - p_global (where p_k is the position component of the pose node at that time in the factor graph). This residual and its information matrix are added to the overall optimization objective function as a global absolute position constraint to participate in this round of factor graph optimization, correcting the previously accumulated horizontal and altitude drift. After matching, this factor is retained in the factor graph as a global anchor point for subsequent optimization. The matching interval can be dynamically adjusted according to the onboard computing power margin and flight speed to reduce the computational load while ensuring drift suppression.

[0059] During navigation, the residuals between the observations of each sensor and the pose estimates from the factor map are calculated in real time. A chi-square test with a significance level of 0.05 is used to verify the residuals. If the residual of a sensor exceeds the chi-square test threshold three times consecutively, the sensor is considered faulty. The information weight of the corresponding factor for that sensor is automatically reduced by a step size of 0.2. When the weight drops to 0, the sensor is temporarily removed from the fusion system, while the remaining sensors continue to maintain factor map optimization for navigation. When the residuals recover to the threshold range, their weights are gradually restored to normal values. When GNSS signal characteristics and electromagnetic environment detection indicate a strong electromagnetic interference scenario, the heading constraint factor weight corresponding to the triaxial magnetometer is directly reset to 0, temporarily removing it from the fusion system to prevent interference with heading data from introducing navigation errors.

[0060] In addition, by utilizing the residual information optimized by the factor graph, the accelerometer zero bias, gyroscope zero bias, external parameters between the camera and the inertial measurement unit, and installation angle deviation between the lidar and the inertial measurement unit are taken as augmented state variables to be estimated and integrated into the factor graph for joint optimization. The calibration results are output every 5 optimization cycles, and the deviation is fed back to the data preprocessing stage in real time for compensation and correction.

[0061] The specific process of online joint optimization of multi-sensor parameters is as follows: 1. Augmented state definition: The parameters to be calibrated are added to the factor graph as augmented variable nodes, including: IMU accelerometer zero bias, gyroscope zero bias, camera-IMU extrinsic parameters (rotation + translation), and lidar-IMU mounting angle deviation.

[0062] 2. Constructing parameter-correlated residuals: Binding the residuals of the calibration parameters to the corresponding observation factors, so that the residuals are differentiated with respect to both pose and parameters: IMU zero-biased correlation is applied to the visual-inertial pre-integration factor, and the pre-integration calculation introduces a zero-biased variable; Camera extrinsic parameters are correlated with visual feature reprojection residuals, and coordinate transformation introduces extrinsic parameter variables. The laser mounting angle is associated with the laser odometry factor, and the mounting angle variable is introduced into the point cloud registration.

[0063] 3. Joint optimization solution: The augmented variable nodes and associated residuals are incorporated into the overall optimization objective function and nonlinear optimization is performed together with the pose variables; after every 5 optimization cycles, the updated calibration parameter values ​​are output.

[0064] 4. Parameter feedback compensation: The latest estimated zero bias value is fed back to the IMU pre-integration stage, the camera extrinsic parameters are fed back to the visual front-end coordinate transformation stage, and the laser mounting angle deviation is fed back to the laser point cloud preprocessing stage to correct the transformation deviation in real time, forming a calibration compensation closed loop.

[0065] This step unifies and integrates multi-source heterogeneous nonlinear observations through factor graphs, significantly improving navigation accuracy compared to traditional Kalman filtering schemes. Incremental Bayesian tree optimization ensures the real-time computing power of the airborne platform. The landscape matching scheme combining digital orthophotos and elevation models solves the problem that pure planar images cannot calculate complete absolute pose. At the same time, optimizing the matching frequency reduces computing power consumption, effectively suppresses cumulative drift in long-term denial environments, and extends the accurate navigation time. Fault adaptive adjustment and online calibration mechanisms improve the system's robustness and environmental adaptability, and can compensate for sensor parameter drift and installation deviations in real time. The newly added electromagnetic interference scenario adaptation mechanism further expands the system's applicable scenarios.

[0066] S6. Navigation Result Output The optimal pose estimate obtained from S5 is output to the UAV flight control system as a navigation input for autonomous flight control of the UAV.

[0067] In this step, the optimal estimate of the UAV's six-degree-of-freedom pose obtained by factor graph optimization is output to the UAV flight control system via serial communication, providing a stable navigation reference for attitude control and trajectory tracking.

[0068] This step provides the UAV flight control system with continuous and high-precision attitude information, ensuring the safety and controllability of autonomous flight in GNSS-denied environments.

[0069] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An autonomous navigation method for unmanned aerial vehicles (UAVs) based on GNSS denial and multi-source information fusion, characterized by the following steps: Step S1: Collect satellite signal data output by the UAV's onboard GNSS receiver in real time, calculate the signal carrier-to-noise ratio, the number of visible satellites and the confidence level of the positioning solution, determine whether the current navigation environment is a GNSS denied environment based on a preset threshold, and generate the corresponding navigation mode switching command. Step S2: Collect raw measurement data from airborne multi-source sensors, including an inertial measurement unit, a monocular vision camera, a lidar, a barometric altimeter, and a triaxial magnetometer. Perform timestamp alignment and outlier removal preprocessing on the raw measurement data from each sensor. Step S3: When the environment is determined to be a valid GNSS environment, the GNSS positioning result is used as the subjective measurement, and a loosely combined Kalman filter is performed in combination with the inertial measurement unit data to output the position, velocity and attitude navigation information of the UAV. Step S4: When the environment is determined to be GNSS denied, switch to the multi-source fusion autonomous navigation mode: obtain the image feature tracking results and IMU pre-integration increment through visual inertial front-end processing, calculate the laser odometry increment through lidar point cloud registration, extract the altitude observation value of the barometric altimeter, and extract the heading observation value of the three-axis magnetometer as a constraint value in the GNSS denied scenario caused by non-electromagnetic interference. Step S5: Construct a factor graph optimization framework, integrate the visual-inertial pre-integration factor, laser odometry factor, altitude constraint factor, and available heading constraint factor into the factor graph, and obtain the optimal estimate of the UAV's six-degree-of-freedom pose through nonlinear optimization. Step S6: Output the optimal pose estimate obtained in step S5 to the UAV flight control system as the navigation input for autonomous flight control of the UAV.

2. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, The specific method for determining the GNSS denial environment in step S1 is as follows: preset carrier-to-noise ratio threshold, visible satellite number threshold, and positioning accuracy factor threshold. When any of the following conditions are met within N consecutive sampling periods: the satellite signal carrier-to-noise ratio is lower than the carrier-to-noise ratio threshold, the number of visible satellites is less than the visible satellite number threshold, or the positioning accuracy factor is greater than the positioning accuracy factor threshold, it is determined that the environment has entered the GNSS denial environment, where N is an integer between 3 and 10.

3. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In step S2, the timestamp alignment adopts the linear interpolation method. Taking the sampling time of the inertial measurement unit as the reference time axis, the asynchronous sampling data of the monocular vision camera, lidar, barometric altimeter and triaxial magnetometer are uniformly registered to the reference time axis through interpolation operation to achieve time synchronization of multi-source data.

4. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In step S4, the visual inertial front-end processing uses optical flow to track the corner features of consecutive frame images, calculates the pixel displacement between adjacent frames, and combines the angular velocity and acceleration pre-integration of the inertial measurement unit to generate visual inertial pre-integration observation factors, which are then directly input into the factor map framework.

5. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In step S4, the laser radar point cloud registration adopts the normal distribution transformation algorithm to register the current frame laser point cloud with the local point cloud map of the previous time. The six-degree-of-freedom transformation matrix is ​​solved as the laser odometry increment, and frames with registration scores lower than the set threshold are marked and removed.

6. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In step S5, the factor graph optimization framework adopts an incremental smooth graph building algorithm. Each sensor measurement corresponds to a factor node, and the UAV pose state corresponds to a variable node. Incremental updates are achieved through a Bayesian tree structure. Each time a new observation is added, only the affected variable nodes are relinearized and optimized.

7. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In GNSS denied environments, a landscape matching and correction step is also performed: real-time ground images acquired by the airborne monocular vision camera are matched with pre-stored digital orthophoto maps and corresponding regional digital elevation model datasets. The three-dimensional geographic coordinates of the matching feature points are obtained by combining the digital elevation model. The absolute geographic coordinates of the UAV are calculated by the perspective n-point algorithm. The absolute coordinates are added to the factor map as global position constraint factors to periodically correct navigation cumulative drift.

8. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, It also includes a sensor fault adaptive adjustment step: real-time calculation of the residual statistics of each sensor measurement; when the residual of a certain sensor continuously exceeds the chi-square test threshold, the sensor is determined to be faulty, and the information weight of the corresponding factor of the sensor in the factor graph is automatically reduced until it is temporarily removed from the fusion system. When it is determined that the GNSS rejection is caused by electromagnetic interference, the heading constraint factor corresponding to the three-axis magnetometer is automatically removed from the fusion system and does not participate in the pose optimization solution.

9. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, It also includes an online calibration step: during the navigation process, the residual information optimized by the factor map is used to estimate the zero bias error of the inertial measurement unit, the external parameter deviation between the camera and the inertial measurement unit, and the installation angle deviation of the lidar online, and the estimated deviation is fed back to the preprocessing stage in real time for compensation and correction.

10. The UAV autonomous navigation method based on GNSS denial and multi-source information fusion according to claim 1, characterized in that, In step S1, a weighted smooth transition mechanism is used when switching navigation modes: when the GNSS signal is detected to change from valid to rejected, the GNSS observation weight is gradually reduced within M sampling periods, while the multi-source fusion weight is gradually increased. When the GNSS signal recovers from rejection to validity, a reverse weighted transition is performed, where M is an integer between 5 and 20.

Images