An unmanned aerial vehicle high-precision height estimation method based on optical flow divergence constraint

Abstract

The application provides a kind of unmanned aerial vehicle high-precision height estimation method based on light flow divergence constraint, belongs to unmanned aerial vehicle integrated navigation technical field.The method includes: extracting the observation light flow divergence of pure translation from the onboard camera image and IMU data;Degradation detection is carried out on the visual observation generated by the onboard camera image, and a visual adaptive degradation weight is generated;A factor graph architecture containing IMU pre-integration factor, GNSS observation factor, adaptive vision factor and dynamic constraint factor is constructed, and multi-source information fusion is realized through nonlinear geometric coupling;Asymmetric weight configuration is carried out for the GNSS observation factor, and on the basis of the visual adaptive degradation weight, the information matrix of the adaptive vision factor is constructed;Based on the factor graph architecture and the information matrix, a global nonlinear objective function is constructed, and through the manifold-based iterative optimization strategy, the optimal height estimation value of the unmanned aerial vehicle is obtained, so as to guarantee the continuity and robustness of height estimation in the visual degradation environment.

Description

Technical Field

[0001] This invention belongs to the field of UAV integrated navigation technology, and particularly relates to a high-precision altitude estimation method for UAVs based on optical flow divergence constraints. Background Technology

[0002] In recent years, with the rapid development of sensor technology, microelectronics technology, artificial intelligence, and automated control technology, unmanned aerial vehicles (UAVs) have been increasingly widely used in logistics transportation, power line inspection, agricultural plant protection, and disaster relief. To adapt to complex operating environments and complete high-precision flight missions, UAVs must possess all-weather, highly reliable autonomous navigation and positioning capabilities. During vertical takeoff and landing, UAVs are close to the ground and experience severe environmental interference; therefore, precise altitude control and environmental awareness are crucial for ensuring safe landing and preventing crashes.

[0003] Due to the limitations of individual sensor performance, which makes it difficult to provide reliable data in all scenarios, modern drones typically employ multi-sensor fusion technology. Common low-cost sensor combinations include Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), and visual sensors. However, existing fusion positioning methods still face significant challenges during actual drone takeoff and landing. Firstly, there is the issue of visual degradation. When drones take off or land in sandy areas, grassy areas, or under poor lighting conditions such as at night or in strong backlight, feature extraction and matching by visual sensors can produce huge errors or even fail completely, leading to catastrophic drift in altitude estimation based on visual odometers (VO). Secondly, there is the issue of altitude perception accuracy. Low-cost drones often lack lidar or high-precision radar altimeters, and relying solely on GNSS vertical position data cannot meet the centimeter-level control accuracy required for soft landing.

[0004] Current mainstream solutions mostly employ extended Kalman filters (EKF) for loosely or tightly coupled fusion. While EKF has low computational cost and strong real-time performance, it is based on the first-order linearization assumption, which introduces linearization errors when dealing with nonlinear visual optical flow models. Moreover, traditional filtering frameworks struggle to effectively handle outliers caused by sensor degradation and often lack dynamic evaluation mechanisms for visual observation quality. Summary of the Invention

[0005] To address the high reliability requirements of UAV takeoff and landing in complex environments, this invention proposes a high-precision altitude estimation method for UAVs based on optical flow divergence constraints, overcoming the limitations of traditional EKF algorithms and the shortcomings of single sensors. This invention utilizes GNSS / IMU to construct a reference motion model for online detection of visual degradation and introduces an optical flow divergence factor to construct nonlinear constraints. Within the Factor Graph Optimization (FGO) framework, robust kernel functions are used to adaptively adjust the weights of each sensor. This method aims to solve the problem of accurate altitude holding under conditions without barometers / lidar, ensuring that UAVs can still achieve safe and stable takeoff and landing by utilizing complementary multi-source information in visually limited environments. Addressing the shortcomings of traditional barometers, which are susceptible to airflow interference, GNSS, which have low vertical position accuracy and are prone to drift, and visual sensors, which are prone to degradation in weak texture or dusty environments leading to altitude estimation failures, this invention further proposes a factor graph optimization algorithm that integrates spherical optical flow divergence, dynamic constraints, and asymmetric observation weights, building upon traditional multi-sensor fusion algorithms.

[0006] The objective of this invention is mainly achieved through the following technical solutions:

[0007] 1. Based on the traditional planar optical flow algorithm, a spherical optical flow model is introduced, and a spherical projection transformation and rotation decoupling module is designed. The visual data is decoupled using IMU angular velocity. While eliminating lens distortion, the induced optical flow caused by the body rotation is effectively eliminated, and the pure translational optical flow related only to vertical translational motion is extracted.

[0008] 2. Design an optical flow divergence extraction method based on field of view integration. Perform weighted integration and linear projection on the translational optical flow within the field of view window centered on the optical axis, and solve the divergence observation value to characterize the geometric relationship between the vertical velocity and absolute altitude of the UAV.

[0009] 3. Construct a visual quality assessment system based on kinematic cross-validation. Utilize drift-free GNSS Doppler velocimetry or IMU pre-integrated velocity to construct a theoretical reference divergence independent of the visual system. By calculating the consistency residual between visual observations and theoretical reference values, real-time monitoring of the reliability of visual observations can be achieved.

[0010] 4. Design an adaptive visual degradation weight generation mechanism, introduce the Sigmoid inverse function to construct a nonlinear mapping model, transform the consistency residual into optimized weights, automatically suppress abnormal visual data in degradation scenarios such as drastic changes in illumination or missing textures, and generate a visual degradation weight factor that can be dynamically adjusted.

[0011] 5. Construct a factor graph optimization framework based on a sliding window, define a state vector containing position, velocity and IMU zero bias, design the topology of IMU pre-integration factor, GNSS observation factor, adaptive vision factor and dynamic constraint factor, and realize tight coupling of multi-source information under a unified framework;

[0012] 6. Considering the forces acting on the UAV under violent maneuvers, an aerodynamic constraint model is designed. Differential equations including gravity, thrust, and air resistance are introduced into the factor diagram. High-frequency noise is smoothed using physical laws. In the event of sensor failure, velocity prediction is provided based on the thrust model, thus preventing state divergence.

[0013] 7. Design an asymmetric weighting strategy based on the characteristics of GNSS sensors, assigning extremely high weight to high-precision GNSS vertical velocity and low weight to low-precision GNSS vertical altitude.

[0014] 8. Construct a nonlinear geometrically coupled residual based on the principle of optical flow geometry. Utilize the scale uncertainty and kinematic constraints of monocular vision to establish equations for optical flow divergence, vertical velocity, and relative altitude. This forces the altitude state to converge based on high-precision velocity observations during the optimization process, thereby suppressing GNSS altitude noise.

[0015] 9. Design an adaptive degradation information matrix mapping mechanism to map the visual degradation weight factors to the information matrix of the factor graph, so as to realize the smooth and robust switching between the visually reliable high-precision height-fixing mode and the visual degradation inertial / dynamic calculation mode.

[0016] 10. Construct a global joint optimization and closed-loop correction solution strategy based on manifolds, utilize marginalization prior techniques and Schur complement to retain historical information, and use the Gauss-Newton algorithm to iteratively solve the full state vector to obtain the optimal height and velocity estimates while ensuring real-time performance.

[0017] The specific technical solution is as follows:

[0018] A high-precision altitude estimation method for UAVs based on optical flow divergence constraints includes the following steps:

[0019] Step 1: Based on the spherical projection model and rotation decoupling, extract the pure translational observation optical flow divergence from airborne camera images and IMU data;

[0020] Step 2: Construct a visual theoretical reference divergence, perform degradation detection on visual observations generated by airborne camera images, and generate adaptive visual degradation weights;

[0021] Step 3: Construct a factor graph architecture that includes IMU pre-integration factor, GNSS observation factor, adaptive vision factor and dynamic constraint factor, wherein the adaptive vision factor includes the observation optical flow divergence, and multi-source information fusion is achieved through nonlinear geometric coupling;

[0022] Step 4: Perform asymmetric weighting for GNSS observation factors, and construct an information matrix of adaptive visual factors based on the visual adaptive degradation weights.

[0023] Step 5: Based on the factor graph architecture and information matrix, construct a global nonlinear objective function that includes prior factors, IMU pre-integration factors, GNSS observation factors, adaptive vision factors, and dynamic constraint factors. Through a manifold-based iterative optimization strategy, obtain the optimal altitude estimate of the UAV.

[0024] The beneficial effects of this invention are as follows:

[0025] 1. This invention does not rely on expensive lidar or barometers that are susceptible to airflow interference. By utilizing the geometric coupling relationship between optical flow divergence and GNSS Doppler velocity measurement, it effectively corrects the meter-level jumps and drifts common in GNSS altitude signals, achieving centimeter-level relative altitude accuracy.

[0026] 2. This invention addresses the complex environments that UAVs may encounter during takeoff and landing, such as dust, water surface reflection, or texture loss, by proposing a degradation detection method based on kinematic cross-validation. By adaptively adjusting visual weights, it automatically and smoothly transitions to IMU pre-integration and a dynamic model when visual errors occur, avoiding the risk of divergence in harsh environments and ensuring flight safety.

[0027] 3. This invention incorporates aerodynamic constraints into the traditional kinematic optimization framework, which makes up for the shortcomings of pure kinematic models under high-frequency vibration of UAVs. It uses physical laws to smooth state estimation and provides state predictions that conform to physical characteristics when sensors fail in the short term. Attached Figure Description

[0028] Figure 1 This is a topological structure diagram of the multi-source fusion factor graph;

[0029] Figure 2 The flowchart shows the overall process of a high-precision altitude estimation method for UAVs based on optical flow divergence constraints.

[0030] Figure 3 This is a structural diagram of the functional modules of an unmanned aerial vehicle (UAV) altitude estimation system. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other. To achieve the above objectives, this invention adopts the following technical solution.

[0032] like Figure 1 As shown, within the sliding window, the GNSS factor and the adaptive vision factor connect a single state node, the IMU pre-integration factor and the dynamic constraint factor connect two adjacent state nodes, the oldest state node and its associated factors are marginalized as the prior factors for the next time step, and the state node has state variables.

[0033] The high-precision altitude estimation method for UAVs of the present invention utilizes a sliding window factor graph optimization framework, such as... Figure 2 As shown, the height estimation process is completed in five steps. The specific process is as follows:

[0034] Step 1: Based on the spherical projection model and rotation decoupling, extract the pure translational observation optical flow divergence from airborne camera images and IMU data;

[0035] Step 2: Construct a visual theoretical reference divergence, perform degradation detection on visual observations generated by airborne camera images, and generate adaptive visual degradation weights;

[0036] Step 3: Construct a factor graph architecture that includes IMU pre-integration factor, GNSS observation factor, adaptive vision factor and dynamic constraint factor. The adaptive vision factor includes the observation optical flow divergence constructed in Step 1, and multi-source information fusion is achieved through nonlinear geometric coupling.

[0037] Step 4: Perform asymmetric weighting for GNSS observation factors, and construct an information matrix of adaptive visual factors based on the visual adaptive degradation weights.

[0038] Step 5: Based on the factor graph architecture and information matrix, construct a global nonlinear objective function that includes prior factors, IMU pre-integration factors, GNSS observation factors, adaptive vision factors, and dynamic constraint factors. Through a manifold-based iterative optimization strategy, obtain the optimal altitude estimate of the UAV.

[0039] This invention provides specific methods for front-end signal processing, mid-end quality assessment, and back-end joint optimization. In actual operation, the system can adaptively adjust the weight allocation between vision and GNSS / IMU according to changes in environmental texture and illumination. When vision is good, geometric constraints are used to correct GNSS altitude noise. When vision degrades, the system automatically switches to dynamic estimation mode to maintain the robustness of altitude estimation.

[0040] Step 1: Based on a spherical projection model and rotation decoupling technology, extract the pure translational optical flow divergence from airborne camera images and IMU data. This includes addressing the issue that traditional planar optical flow algorithms struggle to distinguish between translational and rotational optical flow during UAV takeoff and landing due to potential significant attitude tilt and high-frequency vibration interference. Therefore, this step introduces a spherical optical flow model and uses IMU angular velocity to decouple the visual data, extracting only the optical flow divergence related to vertical translational motion. This includes:

[0041] Step 1.1, Spherical Projection:

[0042] To simplify optical flow calculations and unify the field of view model, the planar image data acquired by the airborne camera is first mapped onto a unit sphere.

[0043] Coordinate system definition: The origin of the camera coordinate system is assumed to coincide with the center of mass of the UAV, and the camera optical axis points to the ground along the Z-axis.

[0044] Projection transformation: Let the coordinates of any pixel on the image plane acquired at time t be... ,in These are the pixel coordinates. The x-coordinate of the pixel is The vertical coordinate of the pixel is , and the camera focal length is . Projecting this point onto a unit sphere centered at the camera's optical center yields the spherical coordinate vector. According to the principle of perspective projection, the mapping relationship is as follows:

[0045] (1)

[0046] in, This represents the three-dimensional coordinates of a point in the camera coordinate system. This step transforms the planar image into a spherical image, eliminating the effects of different lens distortions and facilitating subsequent motion analysis.

[0047] Step 1.2, Rotational Decoupling:

[0048] In a spherical model, the velocity of image feature points is a linear superposition of translation and rotation components. To obtain information reflecting changes in height, the rotation component must be removed.

[0049] According to the principles of rigid body kinematics and spherical geometry, the equation of motion for a characteristic point on a unit sphere can be expressed as:

[0050] (2)

[0051] in, The velocity of a feature point on a unit sphere. For the body's angular velocity, The translational speed of the machine body. The projection matrix of the tangent plane. This refers to the induced optical flow generated by the rotation of the organism.

[0052] The total observed optical flow on the sphere is calculated using the Lucas-Kanade (LK) optical flow method. The body angular velocity is synchronously acquired using a high-frequency IMU. As The observed values. Based on the above dynamic equations, the translational optical flow is calculated by reverse compensation of the rotation term. :

[0053] (3)

[0054] Step 1.3, Observe and extract optical flow divergence:

[0055] This step aims to extract a scalar index characterizing the vertical motion state from the translational optical flow field: optical flow divergence.

[0056] Considering the measurement noise inherent in single-point optical flow, this invention employs the field-of-view integration method. Within the field-of-view window centered on the optical axis... Inside, for translational optical flow By performing weighted integration, the integral vector of the translated optical flow field is obtained. :

[0057] (4)

[0058] In vertical takeoff and landing scenarios, the ground can be considered an approximate plane, and the main direction of motion is perpendicular to the ground. This is achieved by integrating the vector... Perform linear projection and calculate the scalar form of the observed optical flow divergence, specifically including:

[0059] Based on the principles of pinhole imaging and rigid body kinematics, feature points on the image plane Translational optical flow Its radial distance modulus from the center of the image It satisfies the linear expansion relation, that is:

[0060] (5)

[0061] in, The relative velocity of the drone perpendicular to the ground. This is the absolute height of the camera's optical center relative to the ground.

[0062] Since the integration process occurs at the same sampling time, the physical state quantities corresponding to all pixels in the field are... Since it is a constant, we can obtain:

[0063] (6)

[0064] Define the normalization benchmark operator as follows:

[0065] (7)

[0066] Through the performed linear transformation The observed optical flow divergence was obtained:

[0067] (8)

[0068] The observed optical flow divergence This will serve as the direct observation input for the adaptive visual factors in subsequent factor graph optimization, used to construct the geometric constraints for velocity and height.

[0069] Step 2: A reference benchmark is constructed using GNSS / IMU kinematic consistency. Degradation detection is performed on visual observations, and adaptive visual weights are generated. Addressing the issue of optical flow algorithms easily generating erroneous observations under weak textures, repetitive textures, or environmental interference, a visual quality assessment system based on kinematic cross-validation is constructed. A robust reference motion model is built using high-frequency IMU integration and drift-free GNSS velocity to monitor the reliability of visual observations in real time and generate adaptive weights. This includes:

[0070] Step 2.1: Construct a visual theoretical reference divergence:

[0071] To assess the observed optical flow divergence of visual measurements at the current moment. To determine its reliability, a theoretical reference value independent of the vision system is constructed. Utilizing the kinematic relationships of the UAV, optical flow divergence is physically equivalent to the ratio of vertical velocity to altitude. A reference value is then constructed using data from other sensors in the navigation system. :

[0072] (9)

[0073] in, It is vertical velocity. It is a priori altitude; Doppler velocities are unaffected by ionospheric delays, possess extremely high accuracy, and have no cumulative drift, making them an ideal velocity reference. Therefore... GNSS Doppler velocity data should be used preferentially. If poor GNSS signal quality is detected (due to a small number of satellites), the system will automatically switch to the vertical velocity calculated by IMU pre-integration. This is to ensure the continuity of the reference value. Due to the precise altitude at the current moment... Not yet solved; using the posterior height estimate optimized by the factor graph at the previous time step. As a priori metric, considering that the control frequency of the UAV is much higher than its flight dynamics, and the relative altitude change is extremely small over a short period of time, therefore... The assumption is true.

[0074] Step 2.2: Calculate the consistency residuals:

[0075] If the vision sensor is working properly, its observed optical flow divergence It should be consistent with the reference divergence value calculated by kinematics. Maintain a high degree of consistency. Define the deviation between the two as the consistency residual. :

[0076] (10)

[0077] Low residuals indicate that visual observations conform to the laws of physical motion, and the data is reliable. High residuals indicate that visual observations have anomalies.

[0078] Step 2.3, Adaptive Visual Degradation Weights:

[0079] To achieve automatic suppression of visually anomalous data, a nonlinear mapping model is constructed using the Sigmoid inverse function to transform residual values ​​into optimization weights. The calculation formula is as follows:

[0080] (11)

[0081] in, The output visual degradation weight factor has a value range of 1. ; The preset residual threshold is used to define the critical point at which visual observations have significantly degraded; This is a sensitivity coefficient used to adjust the decay rate of the weight as the residual changes.

[0082] Step 3: Construct a factor graph architecture including IMU pre-integration factors, GNSS observation factors, adaptive visual factors, and dynamic constraint factors. The adaptive visual factors include the observation optical flow divergence constructed in Step 1, and multi-source information fusion is achieved through nonlinear geometric coupling; including:

[0083] Step 3.1, Definition of Reference Coordinate System:

[0084] Earth-centered and Earth-fixed coordinate system:

[0085] World Frame As an inertial fixed system, the origin is usually defined as the UAV's takeoff position, with the Z-axis pointing vertically upwards, following the northeast-northeast criterion. This invention focuses on the height h, which is the Z-axis component in this coordinate system.

[0086] Body Frame ): Fixed to the fuselage of the UAV, with its origin located at the IMU measurement center, the X-axis pointing forward of the nose, the Z-axis perpendicular to the fuselage and upward, and the Y-axis conforming to the right-hand rule.

[0087] Step 3.2, Define state variables:

[0088] The system is maintained within a sliding window. The state node corresponding to each keyframe. At any moment ( The state vector of a given state is defined as follows:

[0089] (12)

[0090] The physical meaning of each state vector component is as follows:

[0091] (1) express The three-dimensional position vector of the origin of the machine's coordinate system in the world coordinate system at any given time. It is a three-dimensional position vector Height to be optimized.

[0092] (2) express The three-dimensional velocity vector of the origin of the machine's coordinate system in the world coordinate system at any given time. Three-dimensional velocity vector Vertical velocity to be optimized.

[0093] (3) They represent The three-axis zero bias of the accelerometer and gyroscope is constantly monitored.

[0094] (4) express The attitude of the origin of the machine body coordinate system in the world coordinate system at any given time.

[0095] Step 3.3, Construction of the dynamic constraint model:

[0096] To compensate for the shortcomings of pure kinematics under intense maneuvering or sensor degradation, this invention introduces aerodynamic equations into the factor diagram.

[0097] In the world coordinate system, the translational motion of the UAV follows Newton's second law. Considering gravity, thrust, and air resistance, the differential equation is defined as follows:

[0098] (13)

[0099] in, The acceleration of the drone in the world coordinate system. Let be a rotation matrix. To normalize the total thrust acceleration, The direction of the thrust of the aircraft. It is the acceleration due to gravity. Here is a linear drag coefficient matrix, where , , They are respectively axis, shaft and The drag coefficient in the axial direction, The velocity of the drone in the world coordinate system.

[0100] Integrating the above equations, we construct the connection state nodes. With the next state node dynamic residuals That is, the dynamic constraint factor:

[0101] (14)

[0102] in, for The corresponding speed, for The corresponding speed, for The corresponding time, for The corresponding time, For integration time variable, This factor, which is the unit vector of the z-axis, serves as a "soft constraint." It utilizes the air resistance model to smooth high-frequency noise and provides a physically consistent velocity prediction based on the thrust model in the event of GNSS / visual failure, thus preventing state divergence.

[0103] Step 3.4: Constructing the factor graph topology:

[0104] The factor graph consists of variable nodes (corresponding to the state vectors). It consists of a factor node and factor nodes (corresponding to observation constraints). The factor nodes constructed in this step specifically include:

[0105] (1) IMU pre-integration factor: connects adjacent state nodes and By pre-integrating the high-frequency IMU data, the relative pose and velocity changes between two frames are constrained;

[0106] (2) GNSS observation factor: connecting a single state node This constrains the absolute position and velocity at that moment;

[0107] (3) Adaptive visual factor: connects adjacent state nodes and The geometric relationship between constraint height and vertical velocity;

[0108] (4) Dynamic constraint factor: connects adjacent state nodes and Physical model constraints are introduced.

[0109] Step 4: Perform asymmetric weighting for GNSS observation factors, and construct an adaptive visual factor information matrix based on the visual adaptive degradation weights; including:

[0110] Step 4.1, Asymmetric weighting of GNSS observation factors based on sensor characteristics:

[0111] GNSS observation factors include GNSS velocity constraints and GNSS position constraints, where GNSS position constraints include GNSS horizontal position constraints and vertical height constraints.

[0112] (1) GNSS velocity weighting configuration:

[0113] GNSS receiver velocity measurements primarily utilize the Doppler frequency shift. This measurement is insensitive to satellite geometry and possesses extremely high instantaneous accuracy and zero-bias characteristic. In the factor diagram, GNSS velocity observation factors are... noise covariance It is set to a minimum value. The extremely high weight forces the vertical velocity estimate in the state vector to be locked near the true value, providing a stable velocity benchmark for altitude estimation.

[0114] (2) GNSS vertical height weighting configuration:

[0115] GNSS positioning calculations are limited by the spatial geometry of satellites, and their vertical accuracy is typically only 1 / 2 to 1 / 3 of their horizontal accuracy. Furthermore, they are susceptible to ionospheric delay and ground multipath effects, often exhibiting meter-level drift or jumps. (The last sentence appears to be incomplete and possibly refers to a different topic.) noise covariance Set to a relatively large value. The extremely low weight prevents the altitude calculation from diverging unbounded over long periods, but allows the optimized altitude to freely deviate from the GNSS observations within a short time window, thus reserving mathematical space for visual geometric corrections.

[0116] Step 4.2, Construction of Adaptive Visual Factors:

[0117] (1) Construction of nonlinear geometrically coupled residuals:

[0118] Utilizing the scale uncertainty and kinematic constraints inherent in monocular vision, a nonlinear residual function connecting optical flow observations and system states is constructed. Based on the pinhole imaging model, and assuming the UAV primarily performs translational motion or has already completed rotational decoupling, the optical flow divergence is... Vertical velocity relative altitude Satisfies the geometric equation:

[0119] (15)

[0120] Define the coupling residual function at time step :

[0121] (16)

[0122] in, The observation divergence calculated for the visual front end; The vertical velocity component in the state variable; This represents the altitude component in the state variables. Through this mechanism, the system combines millimeter-precision optical flow observations with centimeter-precision velocity observations, thereby indirectly obtaining centimeter-precision relative altitude information and effectively suppressing meter-level noise in the raw GNSS altitude.

[0123] Step 4.3: Construction of the information matrix of adaptive visual factors:

[0124] To improve the system's robustness in extreme environments such as dust storms, drastic changes in lighting, or solid-color ground, the degradation weighting factor calculated in step 2.3 is used. An optimization framework is introduced to construct an adaptive information matrix.

[0125] Definition of the first Information matrix of time-adaptive visual factors :

[0126] (17)

[0127] in, This represents the nominal noise variance of the visual measurement. This weighting mechanism produces the following binary state switching effect:

[0128] (1) Visually reliable state: rich texture, good lighting, no occlusion. The information matrix remains large, and the optimizer must strictly satisfy the geometric relationship. At this time, the height estimation is mainly determined by vision and speed, and the accuracy is extremely high.

[0129] (2) Visual degradation state: Dust during take-off and landing leads to increased optical flow noise, water surface reflection, and loss of texture. The information matrix approaches 0. The optimizer automatically smooths back off, relying only on IMU pre-integration to maintain short-term pose estimation, and using the dynamic model to limit velocity drift until visual quality is restored.

[0130] Step 5: Based on the factor graph architecture and information matrix, construct a global nonlinear objective function that includes prior factors, IMU pre-integration factors, GNSS observation factors, adaptive vision factors, and dynamic constraint factors. Through a manifold-based iterative optimization strategy, obtain the optimal altitude estimate of the UAV.

[0131] All state nodes within the sliding window are treated as variables to be optimized, and a nonlinear least-squares objective function is constructed, incorporating marginalization prior factors, IMU pre-integration factors, GNSS observation factors, adaptive vision factors, and dynamic constraint factors. The optimal state estimate is obtained through iterative solution using the Gauss-Newton algorithm, and a feedback loop is constructed to correct the front-end processing.

[0132] Step 5.1: Constructing the global nonlinear objective function:

[0133] The system transforms the state estimation problem into a nonlinear least squares problem based on the maximum a posteriori probability estimation criterion. The full state vector within the sliding window is defined. Construct a global nonlinear objective function for:

[0134] (18)

[0135] Among them, marginalized priors Using the Schur complement technique, the state information of the oldest frame that slides out of the window is preserved in the prior matrix. and residual vector In this approach, while conserving computational resources by maintaining a fixed window size, the precision loss caused by sparsity is avoided. Marginalized prior covariance; IMU pre-integration term IMU constraints were constructed between two adjacent frames, where For the IMU pre-integration term residual, The values ​​are the IMU observations from time k to time k+1. Covariance of IMU pre-integration term; dynamic constraint term Based on IMU pre-integration, aerodynamic model constraints are superimposed. For dynamic constraint residuals, This is the covariance of the dynamic constraint term. When IMU data generates noise due to high-frequency vibration, this term utilizes the physical balance between thrust and drag to smooth velocity estimation, ensuring that the state trajectory conforms to Newtonian mechanics; GNSS observation term. An asymmetric weighting strategy is employed, assigning extremely high weight to vertical velocity and extremely low weight to height, thus providing a benchmark for visual geometric correction. For GNSS robust kernel functions, For GNSS observation residuals, For GNSS observations, For GNSS observation term covariance; adaptive vision coupling term middle, For visual robust kernel functions, For the adaptive visual coupling term residual, For visual observation, Visual covariance is the information matrix from step 4.2. The reverse.

[0136] Step 5.2, Manifold-based iterative optimization strategy:

[0137] Due to the quaternion attitude in the state vector Since the Lie group belongs to a non-Euclidean space, the "parameterization on the manifold" strategy is adopted:

[0138] Define the error state in the tangent space. For the attitude part, Lie algebras are used for perturbation.

[0139] (19)

[0140] in, The attitude before the perturbation. The attitude after the perturbation. This is quaternion multiplication. This is the angle increment.

[0141] Calculate the residual relative to the error state Jacobian matrix Construct incremental equations:

[0142] (20)

[0143] in, Let covariance matrix be the variance matrix. As a regulating factor, It is the identity matrix. This is the residual.

[0144] Using the optimization strategy described above, the global nonlinear objective function in step 5.1 is optimized. The optimal height estimate is obtained through processing. .

[0145] like Figure 3As shown, the hierarchical processing logic from multi-source sensor input to flight status output is illustrated: the bottom-level sensor input module integrates visual, inertial measurement, and global navigation satellite system data, providing the perception foundation for the system; the core processing system consists of three functional modules working together, where the visual front-end processing module transforms images into geometric features through spherical projection, rotation decoupling, and divergence calculation; the adaptive weight generation module dynamically evaluates data reliability by constructing a reference benchmark, performing consistency detection, and nonlinear mapping; and the multi-source fusion optimization module combines dynamic constraints and asymmetric weight configuration, uses a factor graph solving unit to jointly solve multi-dimensional residuals, and outputs high-precision flight status estimation results in real time.

Claims

1. A high-precision altitude estimation method for unmanned aerial vehicles (UAVs) based on optical flow divergence constraints, characterized in that, Includes the following steps: Step 1: Based on the spherical projection model and rotation decoupling, extract the pure translational observation optical flow divergence from airborne camera images and IMU data; Step 2: Construct a visual theoretical reference divergence, perform degradation detection on visual observations generated by airborne camera images, and generate adaptive visual degradation weights; Step 3: Construct a factor graph architecture that includes IMU pre-integration factor, GNSS observation factor, adaptive vision factor and dynamic constraint factor, wherein the adaptive vision factor includes the observation optical flow divergence, and multi-source information fusion is achieved through nonlinear geometric coupling; Step 4: Perform asymmetric weighting for GNSS observation factors, and construct an information matrix of adaptive visual factors based on the visual adaptive degradation weights. Step 5: Based on the factor graph architecture and information matrix, construct a global nonlinear objective function that includes prior factors, IMU pre-integration factors, GNSS observation factors, adaptive vision factors, and dynamic constraint factors. Through a manifold-based iterative optimization strategy, obtain the optimal altitude estimate of the UAV.

2. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The method for obtaining the observed optical flow divergence is as follows: the planar image acquired by the camera is mapped onto a unit sphere to obtain a spherical coordinate vector; the total observed optical flow on the sphere is calculated, and the translational optical flow is calculated by using the IMU angular velocity as the observed body angular velocity to compensate for the rotation term based on the rigid body kinematic equations; the translational optical flow is weighted and integrated within the field of view window centered on the optical axis to obtain the integral vector, and the scalar as the observed optical flow divergence is calculated by linearly projecting the integral vector.

3. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The visual theoretical reference divergence is constructed as follows: GNSS Doppler velocity data is used as the vertical velocity reference, and when the GNSS signal quality is lower than the threshold, the vertical velocity is switched to the vertical velocity calculated by IMU pre-integration; the posterior height estimate optimized by the factor map at the previous moment is used as the prior height scale; and the ratio of the vertical velocity reference to the prior height scale is used as the visual theoretical reference divergence.

4. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The adaptive visual degradation weights are generated as follows: the absolute value of the difference between the observed optical flow divergence and the theoretical reference divergence is defined as the consistency residual, and the Sigmoid inverse function is used to construct a nonlinear mapping relationship from the consistency residual to the optimized weights to obtain the adaptive visual degradation weights.

5. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The dynamic constraint factor is constructed as follows: a differential equation of Newton's second law containing gravity, thrust, and air resistance is established in the world coordinate system, where the air resistance term is represented by a linear drag coefficient matrix; the differential equation is integrated between adjacent state nodes to construct the dynamic residual connecting the velocity at the previous moment and the velocity at the next moment, thus obtaining the dynamic constraint factor.

6. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The GNSS observation factors adopt an asymmetric weight configuration: the GNSS Doppler velocity observations are given a very small noise covariance, so that the vertical velocity estimation is forced to be locked near the GNSS velocity value. A larger noise covariance is assigned to GNSS vertical height observations to prevent unbounded divergence in height calculation, while reserving optimization space for visual geometric correction. The larger noise covariance is greater than the smaller noise covariance.

7. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The adaptive visual factor information matrix is ​​constructed as follows: the adaptive visual degradation weight is multiplied by the reciprocal of the nominal noise variance of the visual measurement to form the adaptive information matrix; when the vision is in a reliable state, the information matrix maintains a large value, and the optimizer satisfies the geometric relationship between optical flow divergence, vertical velocity and height; when the vision is in a degradation state, the information matrix approaches zero, and the optimizer automatically switches to the calculation mode that relies on IMU pre-integration and dynamic model, wherein the large value of the information matrix is ​​greater than the value of the information matrix approaching zero.

8. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 4, characterized in that, In the calculation of the consistency residual between the observed optical flow divergence and the theoretical reference divergence, when the consistency residual is lower than a preset threshold, the visual observation is deemed reliable, and the visual factor information matrix in the factor graph optimization maintains a high weight so that the height estimation depends on the geometric constraints of vision and velocity; when the consistency residual is higher than the preset threshold, the vision is deemed to have degraded, the visual factor information matrix is ​​adaptively decayed, and the optimizer automatically and smoothly transitions to the IMU pre-integration and dynamic constraint factor estimation mode until the consistency residual falls back below the preset threshold to restore the visual constraints.

9. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The global nonlinear objective function includes: a marginalized prior term that retains the oldest state information that slides out of the sliding window; an IMU pre-integration term that connects adjacent state nodes; and a dynamic constraint term that connects adjacent state nodes. The GNSS observation term connects individual state nodes, where the GNSS position and velocity constraints use asymmetric weights; the visual coupling term connects adjacent state nodes and carries adaptive visual degradation weights, where the visual coupling term residual is defined as the difference between the observed optical flow divergence and the vertical velocity in the state variables divided by the height.

10. The high-precision altitude estimation method for UAVs based on optical flow divergence constraints according to claim 1, characterized in that, The manifold-based iterative optimization strategy is as follows: taking advantage of the characteristic that the quaternion attitude in the state vector belongs to a non-Euclidean space Lie group, an error state is defined in the tangent space, and the attitude part is perturbed using Lie algebras; the Jacobian matrix of the residual relative to the error state is calculated and an incremental equation is constructed; the Gauss-Newton algorithm is used to iteratively solve the objective function to obtain the optimal state estimate.

Images