Unmanned aerial vehicle positioning and orienting method

Abstract

The invention relates to the technical field of unmanned aerial vehicles, in particular to an unmanned aerial vehicle positioning and orienting method, which comprises the steps of data acquisition and space-time alignment, environment feature fingerprint extraction, adaptive robust factor graph construction and dynamic covariance reweighted optimization. According to the method, the environment feature fingerprints are introduced as a prior diagnosis mechanism, GNSS non-line-of-sight propagation and visual feature depletion areas can be identified in real time, dynamic reweighting is carried out by utilizing the environment feature fingerprints on the factor graph optimization level, GNSS observation values polluted by a multipath effect or visual features with poor quality are automatically inhibited, and the accuracy and the reliability of the GNSS non-line-of-sight propagation and visual feature depletion areas are improved. Therefore, smooth seamless switching from loose coupling to tight coupling is achieved in urban canyons or indoor and outdoor transition areas, posture sudden change caused by traditional fixed threshold switching is avoided, and the positioning reliability of the system in complex electromagnetic and light environments is remarkably improved.

Description

Technical Field

[0001] This application relates to the field of unmanned aerial vehicle (UAV) technology, specifically to a UAV positioning and orientation method. Background Technology

[0002] With the increasing application of drones in non-line-of-sight scenarios such as urban security patrols, indoor emergency search and rescue, and underground space exploration, single sensors are no longer sufficient to meet the requirements of all-weather, high-precision autonomous navigation. The current mainstream technology path usually adopts a multi-source heterogeneous sensor such as GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), visual sensors, and lidar to build a combined navigation system. It uses multi-sensor data fusion algorithms to jointly estimate the position, velocity, and attitude of the aircraft, aiming to solve the problem of attitude perception and continuous tracking under conditions of limited satellite signals or complex electromagnetic environments.

[0003] However, the aforementioned fusion schemes still have limitations under certain extreme conditions. In urban canyons or densely built-up areas, GNSS signals are susceptible to multipath effects and non-line-of-sight propagation interference, resulting in non-Gaussian distribution of observation noise. The sharp decline in signal reliability often makes it difficult for filtering algorithms based on fixed noise models to converge or causes large positioning jumps. During the transition of UAVs from outdoor to indoor environments, the hard switching logic between heterogeneous sensors can easily cause unevenness in pose estimation. At the same time, in weak texture and repetitive texture scenarios such as corridors, tunnels, or white walls, feature extraction and matching of the visual front end are prone to failure, resulting in insufficient observation constraints and degradation of degrees of freedom. In addition, there is a computational delay in the real-time solution of high-dimensional state variables and the time synchronization processing of multi-source data. The accumulation of this delay will cause pose feedback to lag behind the actual motion state when UAVs are performing high-dynamic maneuvering flights, which will cause flight trajectory drift or orientation instability. Summary of the Invention

[0004] This application aims to address the technical problems of existing UAV navigation technology in complex environments such as urban canyons, rapid indoor-outdoor transitions, and weak textures, including decreased positioning accuracy, attitude divergence, and control delays caused by GNSS multipath effects, sensor data asynchrony, and limited computing power.

[0005] This application proposes a UAV positioning and orientation method, comprising: data acquisition and spatiotemporal alignment, wherein flight status data is acquired in real time through a multi-source sensor group onboard the UAV, and the heterogeneous flight status data is hard-synchronized and calibrated based on a unified time reference to establish a multi-source sensor data frame sequence; environmental feature fingerprint extraction, wherein signal quality analysis and image feature extraction are performed on the multi-source sensor data frame sequence to construct an environmental feature fingerprint reflecting the credibility of the current flight environment, wherein the environmental feature fingerprint includes the carrier-to-noise ratio distribution characteristics of the global navigation satellite system signal and the texture richness characteristics of the visual image; and adaptive robust factor graph construction, wherein a factor graph model is constructed based on the multi-source sensor data frame sequence, and the adaptive robust factor graph is constructed based on the multi-source sensor data frame sequence. The pseudorange of the Global Navigation Satellite System (GNSS), the pre-integral quantity of the Inertial Measurement Unit (INS), and the visual reprojection error are added as factor nodes to the factor graph model. Dynamic covariance reweighting optimization dynamically adjusts the robust kernel function threshold and covariance weight of each factor node according to the environmental feature fingerprint, performs nonlinear optimization calculation on the factor graph model, and outputs the optimal pose estimation result of the UAV. Specifically, the dynamic covariance reweighting optimization is as follows: when the environmental feature fingerprint indicates that the GNSS signal is in a non-line-of-sight propagation state or the visual texture features are lower than the preset richness threshold, the weight of the corresponding factor node is adaptively reduced and the robust truncation function is activated to achieve a smooth transition in urban canyon or indoor-outdoor switching scenarios.

[0006] By adopting the above technical solution and introducing environmental feature fingerprints as a priori diagnostic mechanism, it is possible to identify GNSS non-line-of-sight propagation and areas with poor visual features in real time. At the factor graph optimization level, the environmental feature fingerprints are dynamically reweighted to automatically suppress GNSS observations contaminated by multipath effects or low-quality visual features. This enables a smooth and seamless switch from loose coupling to tight coupling in urban canyons or indoor-outdoor transition zones, avoiding attitude abrupt changes caused by traditional fixed threshold switching and significantly improving the positioning reliability of the system in complex electromagnetic and lighting environments.

[0007] Optionally, the nonlinear optimization solution of the factor graph model employs a dynamic sparse Bayesian factor graph real-time fusion and refinement scheme, specifically including: constructing a hierarchical solution architecture, dividing the solution process into a high-frequency inertial recursion layer and a low-frequency graph optimization layer; in the low-frequency graph optimization layer, a sliding window mechanism is used to manage factor nodes, and the old keyframe states that slide out of the window are marginalized based on a dynamic sparse Bayesian inference strategy, compressing the old state information into a priori information matrix and transmitting it to the current window; establishing an asynchronous feedback closed loop, using the zero-bias estimate of the inertial measurement unit obtained from the low-frequency graph optimization layer to asynchronously correct the mechanical arrangement error of the high-frequency inertial recursion layer, so as to meet the real-time positioning and orientation requirements of UAVs under high-dynamic flight.

[0008] By adopting the above technical solutions, the edge processing strategy addresses the problems of computing power saturation and data lag in high-dynamic flight. While preserving historical constraint information, it limits the size of the factor graph and prevents the computation time from increasing exponentially over time. The hierarchical asynchronous architecture ensures that the control loop can acquire the high-frequency IMU status with extremely low latency (e.g., <5ms), while continuously calibrating the IMU drift using low-frequency optimization results, effectively resolving the contradiction between real-time computation and high-precision solution.

[0009] Optionally, the marginalization process specifically includes: using the Schur complement technique to linearize the oldest frame state variable in the sliding window and its associated observation constraints; constructing a sparse Hessian matrix, retaining the correlation constraints between the remaining state variables and the removed state variables in the window, forming prior factor nodes added to the new factor graph, so as to maintain the global consistency of the system and limit the growth of computing resources.

[0010] By adopting the above technical solution, constant consumption of computing resources is achieved while maintaining global system consistency, thus enabling deployment on embedded airborne platforms with limited computing power.

[0011] Optionally, the hard synchronization and extrinsic parameter calibration of the heterogeneous flight status data based on a unified time reference specifically includes: triggering the exposure time of the visual sensor using the pulse second signal of the underlying hardware, forcibly aligning the timestamp of the visual image frame to the clock domain of the inertial measurement unit; and transforming the observation data of all sensors to the inertial center in the UAV body coordinate system according to the pre-calibrated lever parameters, thereby eliminating spatial asynchrony errors caused by sensor installation position deviations.

[0012] By adopting the above technical solution, the time deviation and spatial lever effect of sensor sampling are eliminated from the hardware level, ensuring strict physical alignment of multi-source data under high-speed motion.

[0013] Optionally, the construction of the environmental feature fingerprint reflecting the credibility of the current flight environment specifically includes: calculating the geometric precision factor and multipath effect index of the currently visible satellite to generate a satellite signal confidence vector; performing gradient histogram analysis on the acquired visual image to calculate the image information entropy and feature point distribution uniformity to generate a visual observation confidence vector; and weightedly fusing the satellite signal confidence vector with the visual observation confidence vector to generate a normalized environmental feature fingerprint value, which is used to characterize the probability distribution of the current scene belonging to an open environment, a semi-occluded environment, or a weakly textured environment.

[0014] By adopting the above technical solution, the abstract degree of environmental interference is quantified into a specific mathematical probability index, providing a precise adaptive parameter basis for the robust function in factor graph optimization, enabling the system to sensitively detect environmental changes.

[0015] Optionally, it also includes a step for handling degree-of-freedom degradation in weakly textured environments, specifically: geometric feature degradation detection, performing eigenvalue decomposition on the information matrix of the optimization level in the factor graph model, and analyzing the observability of the state space in each dimension; pseudo-observation constraint construction, when the eigenvalue of a certain direction is detected to be less than a preset degradation threshold, it is determined that motion degradation has occurred in that direction, and a virtual pseudo-observation constraint factor is applied to that direction and added to the factor graph model to restore the system's degrees of freedom and suppress pose drift.

[0016] Optionally, the geometric feature degradation detection specifically includes: extracting the spatial distribution data of feature points calculated by the visual odometry front end; calculating the main direction vector of the visual feature point cloud; if the main direction vector is parallel to the direction of movement of the UAV and the number of feature points in the orthogonal direction is lower than the effective threshold, then it is determined that geometric degradation has occurred in the feed direction, corresponding to a corridor or tunnel scene.

[0017] By adopting the above technical solution, the problem of loss of degrees of freedom caused by insufficient visual feature points or degradation of lidar constraints in single geometrical scenarios such as corridors, tunnels or white walls is solved. The stability of the solution is monitored by mathematical means, and virtual constraints are introduced when necessary, which effectively suppresses pose drift in unobservable directions.

[0018] Optionally, the pseudo-observation constraint construction specifically includes: planar geometric constraints: by identifying the ground or wall normal vectors in the environment, a planar constraint factor is constructed to lock the roll and pitch degrees of freedom of the UAV, and only the yaw angle and planar displacement are updated; zero-velocity correction constraints: when the UAV is detected to be in a hovering state or the acceleration variance in the degradation direction is lower than the stationary threshold, a zero-velocity observation constraint is applied in the degradation direction.

[0019] By adopting the above technical solutions, the inherent geometric features of the environment (such as horizontal ground and vertical walls) or kinematic features (such as hovering) are used as supplementary constraints to forcibly constrain divergent degrees of freedom under extreme conditions where sensor information is lacking, thereby ensuring basic flight safety.

[0020] Optionally, it also includes an ultra-wideband opportunistic access mechanism based on degradation detection: an ultra-wideband ranging module is pre-configured on the UAV, which is in a low-power standby state by default; the ultra-wideband ranging module is activated only when motion degradation is determined to occur and the pseudo-observation constraint cannot meet the accuracy requirements; the ultra-wideband ranging data is dynamically inserted into the factor graph model as a new distance constraint factor until the degradation state is resolved.

[0021] By adopting the above technical solutions, a multi-level redundant protection system was constructed, achieving high reliability in extreme environments at the cost of low power consumption, and balancing battery life and positioning robustness through an on-demand activation strategy.

[0022] Optionally, it also includes flight control execution, specifically: mapping the optimal pose estimation result into the UAV's navigation state variables in real time, the navigation state variables including three-dimensional position, three-dimensional velocity and three-axis attitude angles; inputting the navigation state variables into the UAV flight control system, generating motor speed commands through a cascaded proportional-integral-derivative controller of the position loop and attitude loop, controlling the UAV to execute predetermined flight or hovering actions, and realizing closed-loop autonomous positioning and navigation control.

[0023] By adopting the above technical solution, the high-precision results of the perception layer are transformed into specific execution actions of the decision-making layer, realizing a complete technical closed loop from environmental perception to flight control. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the drone positioning and orientation method of this application. Detailed Implementation

[0025] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the embodiments of the present application. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present application.

[0026] like Figure 1 As shown in the figure, this application discloses a method for positioning and orientation of a drone, including the following steps.

[0027] S01: Data acquisition and spatiotemporal alignment: Flight status data is collected in real time through a multi-source sensor group on the UAV, and heterogeneous flight status data is hard-synchronized and extrinsic parameter calibrated based on a unified time reference to establish a multi-source sensor data frame sequence.

[0028] In this embodiment, an industrial-grade quadcopter UAV modification platform can be used. Its onboard computing unit employs an embedded graphics processing module, and the high-precision sensor array includes: a Global Navigation Satellite System (GNSS) receiver, selected as a high-performance module supporting multiple frequencies and constellations, including the first, second, and fifth frequency bands of the Global Positioning System (GPS), the first, second, and third frequency bands of BeiDou, and the first, second, and fifth frequency bands of Galileo. This high-performance module communicates with the main control computer via a Universal Asynchronous Transceiver (PAT) serial port at a baud rate of 460.8 kilobits per second, configured with a sampling frequency of 10 Hz, and outputs raw observation data including carrier phase, pseudorange, and Doppler shift. The nominal accuracy of the carrier phase observation is better than 2 millimeters, and the nominal accuracy of the pseudorange observation is better than 10 centimeters. The inertial measurement unit can be a tactical-grade fiber optic gyroscope, specifically a model such as TG- The 40 integrates a three-axis fiber optic gyroscope and a three-axis accelerometer. The gyroscope's zero-bias stability is better than 0.1 degrees per hour, and the accelerometer's zero-bias stability is better than 30 micro-accelerometers. The sampling frequency is set to 200 Hz to capture high-frequency vibrations and instantaneous maneuvering characteristics. Its physical interface is a four-wire serial peripheral interface, and the bus clock frequency is configured to 10 MHz to ensure low latency in data transmission. The vision sensor can be an Intel RealSense depth camera D455, configured in global shutter mode to eliminate the rolling shutter effect during high-speed drone flight. The resolution is set to 848×848 pixels, the frame rate is set to 30 frames per second, and it outputs an eight-bit depth grayscale image stream and a sixteen-bit depth map stream. In addition, a barometric altimeter can be configured for altitude hold, with an output frequency of 50 Hz and a barometric pressure resolution of 0.01 mbar.

[0029] Understandably, hard synchronization and extrinsic parameter calibration of heterogeneous flight status data based on a unified time reference specifically includes: using the pulse second signal of the underlying hardware to trigger the exposure time of the visual sensor, forcibly aligning the timestamp of the visual image frame to the clock domain of the inertial measurement unit; and transforming the observation data of all sensors to the inertial center in the UAV body coordinate system according to the pre-calibrated lever parameters, thereby eliminating spatial asynchrony errors caused by sensor installation position deviations.

[0030] Specifically, a field-programmable gate array (FPGA) can be used as the timing control center of the entire sensor system. Its general purpose input / output pins are connected to the pulse-second signal pin output by the Global Navigation Satellite System (GNSS) receiver. The rising edge accuracy of this pulse-second signal is better than 20 nanoseconds and is strictly aligned with Coordinated Universal Time (UTC). The FPGA internally maintains a 64-bit high-precision local counter, with the clock sourced from a temperature-compensated crystal oscillator at a frequency of 100 MHz. Whenever the FPGA detects the rising edge of the pulse-second signal, the hardware logic immediately resets the microsecond-level counter and calibrates the local timestamp, ensuring... The local time deviates from the satellite time by less than 1 microsecond. Simultaneously, the field-programmable gate array (FPGA) sends a high-level pulse with a pulse width of 100 microseconds to the external trigger pin of the vision sensor via hardwired connection. This forces the camera to open the shutter at a fixed delay, such as 500 microseconds, after the arrival of the pulse signal. The local timestamp of the FPGA at this exposure time is then written into the frame header of each image frame via a Universal Serial Bus (USB) metadata packet. This ensures that the time reference of the visual image is strictly locked within the clock domain of the inertial measurement unit, achieving microsecond-level time synchronization accuracy and completely solving the time jitter problem caused by soft synchronization.

[0031] Understandably, before the system leaves the factory, it is necessary to determine the rigid transformation parameters of the optical center or phase center of each sensor relative to the origin of the UAV's body coordinate system in a precision optics laboratory using a checkerboard calibration plate and a high-precision total station; the origin of the UAV's body coordinate system is defined as the geometric center of the inertial measurement unit. The Y-axis points forward of the nose, the X-axis points to the right side of the fuselage, and the Z-axis points vertically upward. The least squares method is used to calculate the translation vector (or lever arm value) of the phase center of the GNSS antenna relative to the origin of the UAV body, for example, components of 0.15 meters laterally, 0 meters longitudinally, and -0.25 meters vertically. The rotation matrix and translation vector of the camera's optical center relative to the origin of the UAV's coordinate system are also calculated. During real-time operation, the data preprocessing algorithm module reads the pre-stored lever arm parameters and uses rigid body kinematics equations to uniformly transform the position measurements received by the GNSS antenna and the coordinates of the camera's visual feature points to the center of the inertial measurement unit (IMU). In the specific calculation process, an angular velocity cross-product term is introduced as a correction to compensate for the linear velocity component of the lever arm effect caused by rapid body rotation. When the UAV's angular velocity exceeds 100 degrees per second, this correction can reach the centimeter level. Furthermore, the raw data from the IMU needs to be denoised. For example, a fourth-order Butterworth low-pass filter with a cutoff frequency set to 40 Hz can be used to filter out noise interference caused by high-frequency vibrations of the fuselage motors while preserving the true dynamic characteristics of the maneuvering flight.

[0032] Finally, a spatiotemporally aligned sequence of multi-source sensor data frames is output. This sequence is stored in a circular buffer. Each multi-source sensor data frame includes structured data with a unified and universal timestamp. The specific data structure definition includes: a 64-bit unsigned integer microsecond-level timestamp; an acceleration vector consisting of three 32-bit floating-point numbers, in meters per second squared; an angular velocity vector consisting of three 32-bit floating-point numbers, in radians per second; satellite data packets containing pseudorange observations, carrier phase observations, and Doppler observations from multiple satellites; and visual image frames after distortion correction and histogram equalization processing. This ensures strict physical consistency of the multi-source information.

[0033] S02: Environmental feature fingerprint extraction. Signal quality analysis and image feature extraction are performed on multi-source sensor data frame sequences to construct an environmental feature fingerprint that reflects the credibility of the current flight environment. The environmental feature fingerprint includes the carrier-to-noise ratio distribution characteristics of the global navigation satellite system signal and the texture richness characteristics of the visual image.

[0034] Understandably, constructing an environmental feature fingerprint that reflects the credibility of the current flight environment specifically includes: calculating the geometric precision factor and multipath effect index of the currently visible satellites to generate a satellite signal confidence vector; performing gradient histogram analysis on the acquired visual images to calculate the image information entropy and feature point distribution uniformity to generate a visual observation confidence vector; and weightedly fusing the satellite signal confidence vector and the visual observation confidence vector to generate a normalized environmental feature fingerprint value, which is used to characterize the probability distribution of the current scene belonging to an open environment, a semi-occluded environment, or a weakly textured environment.

[0035] In this embodiment, the input data for this step originates from the spatiotemporally aligned multi-source sensor data frame sequence output in step S01, including raw observations from the Global Navigation Satellite System and visual grayscale images. Specifically, a depth quality analysis is first performed on the satellite signals, traversing all visible satellite channels at the current epoch and extracting the carrier-to-noise ratio (CNR) data for each satellite, expressed in decibels (dB / Hertz). In an ideal open environment, the CNR of high-elevation satellites is typically between 40 and 50 DB / Hertz. The arithmetic mean and variance of the CNR of all visible satellites are first calculated, and further statistical analyses are performed on CNRs lower than... The proportion of satellites with a weak signal threshold (e.g., 35 dBH) to the total number of visible satellites directly reflects the degree of environmental obstruction. Simultaneously, the statistical distribution characteristics of pseudorange residuals are analyzed. By calculating the skewness and kurtosis of the pseudorange residual sequence, if the pseudorange residuals exhibit a significant non-Gaussian distribution (i.e., the absolute value of the skewness is greater than 1.0), it is determined that there is severe multipath reflection in the current environment. Combining the above indicators, a satellite signal confidence vector is constructed, which includes floating-point values ​​in four dimensions: average carrier-to-noise ratio, carrier-to-noise ratio variance, weak signal proportion, and pseudorange residual skewness.

[0036] Simultaneous visual environment analysis can employ efficient image processing techniques. The input grayscale image is used to calculate its directional gradient histogram, dividing the image into 16×16 pixel cells. The gradient direction and magnitude within each cell are statistically analyzed. If the average gradient magnitude of the overall image is lower than a preset weak texture threshold (e.g., the average gradient magnitude is less than 15), the current field of view is determined to be a weak texture environment, such as facing a white wall, reflective glass, or a road surface. Simultaneously, the image's information entropy is calculated using the Shannon entropy formula. This information entropy quantifies the richness and complexity of image texture; lower information entropy indicates less texture. Furthermore, feature points in the image are extracted using a fast corner detection algorithm, and the spatial distribution uniformity index of feature points in the image plane is calculated using the Thiessen polygon method or grid statistical method. This spatial distribution uniformity index ranges from 0 to 1; a value closer to 1 indicates a more uniform distribution of feature points, which is beneficial for pose calculation. Combining these indicators, a visual observation confidence vector is constructed, which includes four dimensions of floating-point values: average gradient magnitude, image information entropy, number of effective feature points, and feature point distribution uniformity.

[0037] Understandably, the extracted satellite signal confidence vector and visual observation confidence vector are normalized and mapped to a dimensionless range of 0 to 1 using the minimax scaling method. Then, this eight-dimensional feature vector is input into a pre-trained lightweight scene classifier. This classifier employs a support vector machine or random forest algorithm and is pre-trained on a large-scale dataset containing typical scenes such as urban canyons, open areas, indoor corridors, indoor halls, forests, and smoky environments, with a training data sample size of no less than 100,000 frames. The output of this scene classifier includes not only the category label of the current scene but also the confidence probability of that category. For example, the output might indicate an 80% probability of being in an urban canyon environment and a 20% probability of being in a forest environment.

[0038] In this embodiment, the output of this step is a normalized environmental feature fingerprint value, which is encapsulated into a data structure including detailed status identifier bits. This data structure includes: scene type enumeration value, global navigation satellite system trust score, and visual trust score. The trust score is a floating-point number between 0 and 1, which directly represents the availability of the corresponding sensor in the current environment. For example, in an urban canyon with an occlusion rate of more than 70%, the global navigation satellite system trust score may drop to 0.2, while in a textured indoor scene, the visual trust score may be as high as 0.9. This environmental feature fingerprint value will be passed to the subsequent factor graph optimization model, which directly determines the weight allocation of each sensor data by the fusion algorithm, thereby effectively preventing positioning divergence caused by blindly fusing poor data.

[0039] S03: Adaptive robust factor graph construction. A factor graph model is constructed based on multi-source sensor data frame sequences. The pseudorange of the global navigation satellite system, the pre-integration of the inertial measurement unit, and the visual reprojection error are added as factor nodes to the factor graph model.

[0040] Understandably, the input data for this step is the spatiotemporally aligned multi-source sensor data frame sequence from step S01 and the environmental feature fingerprint from step S02. First, a pre-integration operation is performed based on the high-frequency data from the inertial measurement unit (IMU). Since the sampling frequency of the IMU (e.g., 200 Hz) is much higher than the frequency of the back-end optimization (e.g., 20 Hz), directly adding all inertial measurement values ​​as factors to the factor graph model would lead to a computational dimensionality explosion. Therefore, pre-integration theory on the manifold space can be used. Between two consecutive keyframe moments, the specific force vector and angular velocity vector output by the IMU are numerically integrated to obtain the relative displacement increment, relative velocity increment, and relative rotation increment between the two frames, collectively referred to as the pre-integration quantity. Simultaneously, based on the error propagation equation, the covariance matrix of the pre-integration quantity and the Jacobian matrix of the pre-integration quantity with respect to acceleration bias and gyroscope bias are iteratively calculated. Mathematically, this pre-integration quantity is only related to the sensor bias and decoupled from the absolute pose state of the UAV. Therefore, when the linearization point changes, there is no need for re-integration, greatly saving computational resources.

[0041] Understandably, in the factor graph model, variable nodes represent the full-state vector of the UAV to be estimated. This full-state vector is fifteen-dimensional, specifically including: a three-dimensional position vector in the world coordinate system, a three-dimensional velocity vector, a quaternion representing attitude, and a three-axis accelerometer bias vector and a three-axis gyroscope bias vector in the body coordinate system. Factor nodes represent specific observation constraints, connected to related variable nodes via edges. Specifically, there are three core factors: the first is the inertial factor, which connects two state nodes at adjacent times. Its error function is defined as the residual between the pre-integrated quantity and the relative motion calculated from the current two state nodes. This residual reflects the relationship between inertial recursion and state. The first factor is the consistency of the estimation; the second is the Global Navigation Satellite System factor, which acts as a univariate factor connecting individual state nodes. Its error function is defined as the Euclidean distance between the pseudorange or carrier position observed by the satellite receiver and the calculated position after transforming the state node position to the antenna center through the lever parameters; the third is the visual factor, which acts as a binary factor connecting state nodes and landmark nodes. Its error function is defined as the reprojection error, i.e., the pixel distance between the pixel coordinates of the landmark point in the world coordinate system projected onto the current frame image plane through the camera projection model and the coordinates of the observed feature points extracted by the front-end visual algorithm; the camera projection model includes the pinhole imaging model and radial tangential distortion parameters.

[0042] Understandably, the output of this step is the completed factor graph model data structure, which is usually stored using graph structures from the C++ Standard Template Library or dedicated optimization library objects. This factor graph model data structure includes the index numbers of all variable nodes, the initial linearization points, and the connection relationships and initial covariance matrices of various factor nodes. At this point, the constructed factor graph has not yet been solved, and the weights of each factor (i.e., the inverse of the covariance matrix) are the initial values ​​set based on the sensor's factory nominal parameters, and have not yet been dynamically adjusted by environmental feature fingerprints.

[0043] S04: Dynamic Covariance Reweighted Optimization. Based on environmental feature fingerprints, the robust kernel function thresholds and covariance weights of each factor node are dynamically adjusted to perform nonlinear optimization on the factor graph model, outputting the optimal pose estimation result for the UAV. Specifically, when the environmental feature fingerprint indicates that the Global Navigation Satellite System signal is in a non-line-of-sight propagation state or the visual texture features are below a preset richness threshold, the weights of the corresponding factor nodes are adaptively reduced and the robust truncation function is activated to achieve a smooth transition in urban canyon or indoor-outdoor switching scenarios.

[0044] Specifically, firstly, a dynamic covariance reweighting strategy is implemented, traversing each GNS (Global Navigation Satellite System) factor and visual factor in the factor graph and reading environmental feature fingerprint values. For the GNS factor, if the proportion of weak signals in the environmental feature fingerprint is greater than 0.6 or the pseudorange residual skewness indicates a multipath effect, the covariance matrix of that factor will be automatically adjusted by multiplying its corresponding variance value by a penalty coefficient. This penalty coefficient ranges from 10 to 100, and its specific value is inversely proportional to the environmental confidence level, thereby significantly reducing the weight of that observation data in the optimization. Simultaneously, a dynamic covariance reweighting strategy is activated for that factor. The covariance scaling robust kernel function dynamically sets a truncation threshold based on the residual distribution. When the pseudorange residual of a satellite exceeds this truncation threshold, the kernel function suppresses the influence of the residual on the objective function from quadratic growth to linear growth or even constant truncation, thereby effectively eliminating tens of meters-level jump errors caused by non-line-of-sight signals and preventing them from skewing the overall trajectory. For visual factors, if the visual trust score displayed by the environmental feature fingerprint is below 0.5, i.e., a weak texture environment, the covariance of the visual reprojection error factor is increased simultaneously, and the kernel function threshold for feature point matching is reduced to prevent mismatched points from introducing incorrect geometric constraints.

[0045] Understandably, nonlinear optimization of the factor graph model can be achieved using a dynamic sparse Bayesian factor graph real-time fusion and refinement scheme. This involves: constructing a hierarchical solution architecture, dividing the solution process into a high-frequency inertial recursion layer and a low-frequency graph optimization layer; in the low-frequency graph optimization layer, a sliding window mechanism is used to manage factor nodes, and the old keyframe states that slide out of the window are marginalized based on a dynamic sparse Bayesian inference strategy, compressing the old state information into a priori information matrix and transmitting it to the current window; establishing an asynchronous feedback loop, using the zero-bias estimate of the inertial measurement unit obtained from the low-frequency graph optimization layer to asynchronously correct the mechanical arrangement error of the high-frequency inertial recursion layer, thus meeting the real-time positioning and orientation requirements of UAVs under high-dynamic flight conditions.

[0046] Specifically, the nonlinear optimization solution can be solved iteratively by using the Levenberg-Marquardt algorithm to solve the weighted factor graph. The optimization objective is to minimize the sum of the weighted Mahalanobis distances of all factor error terms. The solver operates within a sliding window, with the window size set to 10 to 20 keyframes to ensure real-time computation. In each iteration, the Jacobian matrix and residual vector of the objective function are calculated, an incremental equation is constructed, and the state increment is solved using sparse Cholesky decomposition until the decrease in the objective function is less than the preset convergence threshold or the maximum number of iterations is reached.

[0047] After completing the initial optimization calculation, the marginalization process begins. The marginalization process includes: using the Schur complement technique to linearize the oldest frame state variable in the sliding window and its associated observation constraints; constructing a sparse Hessian matrix to retain the correlation constraints between the remaining state variables and the removed state variables in the window, forming prior factor nodes that are added to the new factor graph to maintain the global consistency of the system and limit the growth of computational resources.

[0048] Specifically, as a resource manager for the low-frequency graph optimization layer, it is mainly used to solve the problem of computational memory overflow caused by the infinite accumulation of factor graph nodes as flight time increases; the specific implementation environment is an airborne embedded computing platform; whenever a new keyframe is inserted at the end of the window, the oldest frame and its associated visual landmarks are removed to maintain a constant window size; at this time, the state variables of the oldest frame in the sliding window and its associated observation constraints are linearized using the Schur complement technique; specifically, the system constructs a full Hessian matrix composed of all state variables in the current window, which typically has a dimension of several thousand, and the matrix elements are stored as double-precision floating-point numbers; the state variables are divided into a set of states to be removed and a set of states to be retained; the set of states to be removed includes the pose, velocity, bias, and landmarks of the oldest frame that are only observed by that frame; the set of states to be retained includes the remaining frames in the window and their associated landmarks; in terms of the order of operations, firstly... The full Hessian matrix is ​​divided into blocks, extracting the autocorrelation blocks of the states to be removed, the autocorrelation blocks of the retained states, and the cross-correlation blocks. Then, the Schur complement operation is performed to calculate the new Hessian matrix and the corresponding information vector. During this process, matrix inversion can be accelerated using Cholsky decomposition. Considering the characteristic that the Hessian matrix is ​​typically a sparse diagonal matrix, the matrix elements are rearranged using an approximate minimum degree sorting algorithm to minimize the generation of filler elements, ensuring that the computation time is controlled within 15 milliseconds. The resulting new Hessian matrix is ​​the prior information matrix, which includes all constraint information of the removed states on the remaining states. This prior information matrix is ​​encapsulated as a special prior factor node and connected to the oldest frame in the sliding window (i.e., the second oldest frame in the original window). This process is physically equivalent to compressing the flight history data from the past few minutes or even hours into a mathematical constraint, preventing the system from forgetting historical information.

[0049] Furthermore, an asynchronous feedback closed-loop mechanism is established. The zero-bias estimate of the inertial measurement unit obtained by optimizing the low-frequency map layer is used to asynchronously correct the mechanical arrangement error of the high-frequency inertial recursive layer. The high-frequency layer operates at a frequency of 20 Hz, using the latest zero-bias to correct the original acceleration and angular velocity, and then performs integral calculation to meet the flight control system's requirements for low-latency attitude data. The low-frequency layer operates at a frequency of 20 Hz and is mainly responsible for eliminating accumulated errors. The two layers exchange data through shared memory, ensuring that the positioning system has both high accuracy and high dynamic response capability.

[0050] It is understood that the embodiments of this application also include a degree-of-freedom degradation processing step for weak texture environments, specifically including the following steps.

[0051] Geometric feature degradation detection involves eigenvalue decomposition of the information matrix at the optimization level in the factor graph model to analyze the observability of the state space across various dimensions. Specifically, geometric feature degradation detection includes: extracting the spatial distribution data of feature points calculated by the visual odometry front-end; calculating the principal direction vector of the visual feature point cloud; and determining that geometric degradation has occurred in the feed direction if the principal direction vector is parallel to the UAV's motion direction and the number of feature points in the orthogonal direction is below a valid threshold, corresponding to corridor or tunnel scenarios.

[0052] In this embodiment, a preliminary screening based on geometric distribution is first performed at the front end: the spatial covariance matrix of all valid feature points (ranging from 0 to 500) in the current frame is calculated, and principal component analysis is performed on it; the three principal direction vectors of the feature point cloud and their corresponding singular values ​​are calculated; if the maximum singular value is much larger than the second and third singular values, for example, the ratio is greater than 100, and the principal direction vector is parallel to the current body axis movement direction of the UAV (e.g., the horizontal axis in front of the nose), this physically corresponds to the tunnel effect, that is, the center of the field of view is at infinity, all feature points diverge radially, resulting in the UAV's displacement in the forward direction being insignificant.

[0053] Understandably, the approximate Hessian matrix associated with the current state is extracted from the factor graph, and its eigenvalues ​​are decomposed to obtain an eigenvalue sequence and corresponding eigenvectors. Under ideal observation conditions, all eigenvalues ​​should remain within a certain order of magnitude, indicating that all dimensions of the state space are within the observation constraints. However, in degenerate scenarios, the eigenvalues ​​corresponding to unobservable degrees of freedom will drop sharply to near zero. A specific degradation threshold is set to one-thousandth. If the minimum eigenvalue detected is less than this degradation threshold, it is determined that the system has degraded in the state space direction indicated by the corresponding eigenvector. For example, when hovering in front of a textureless white wall... Vision cannot provide constraints on horizontal displacement, so the eigenvalues ​​corresponding to the horizontal position components will approach zero. When moving at a constant speed in a long straight corridor, the eigenvalues ​​corresponding to the forward speed and scale factor will decay due to scale ambiguity and texture repetition. The detection process runs in real time at a frequency of 10 Hz. Once degradation is determined, the system will immediately output a status flag indicating the degradation direction, such as horizontal translation degradation or vertical rotation degradation. This status flag directly triggers the subsequent pseudo-observation constraint construction module, thereby preventing the optimizer from generating random walks in unconstrained directions at the mathematical level and ensuring that the drone will not crash into walls due to algorithm divergence.

[0054] The pseudo-observation constraint construction involves determining motion degradation in a given direction when the eigenvalue is less than a preset degradation threshold. A virtual pseudo-observation constraint factor is then applied to this direction and added to the factor graph model to restore the system's degrees of freedom and suppress pose drift. Specifically, the pseudo-observation constraint construction includes: planar geometric constraints: By identifying ground or wall normals in the environment, planar constraint factors are constructed to lock the UAV's roll and pitch degrees of freedom, retaining only updates to yaw and planar displacement; and zero-velocity correction constraints: When the UAV is detected to be hovering or its acceleration variance in the degradation direction is lower than a rest threshold, a zero-velocity observation constraint is applied in that degradation direction.

[0055] Understandably, when the system determines that the plane geometry is degenerate, such as a flat indoor floor or a vertical wall, it first uses point cloud data from an airborne laser rangefinder or depth camera to fit the main plane of the environment. For example, in an indoor hall scene, the ground normal vector is extracted using a random sampling consensus algorithm, which is usually close to the vertical upward vector in the body coordinate system. Based on this, a plane geometry constraint factor is constructed: this plane geometry constraint factor does not constrain specific lateral or longitudinal coordinate values, but constrains the drone's altitude change rate, roll angle, and pitch angle. In terms of specific data processing, a residual equation is constructed, which involves the rotation matrix, the current position, and the reference plane point. By giving this plane geometry constraint factor a very high weight, for example, setting the covariance to one ten-thousandth, the optimizer is forced to lock the drone's attitude in a state parallel to the ground, retaining only the free update of the yaw angle, thereby effectively suppressing attitude divergence caused by insufficient feature points.

[0056] When the system determines that the zero-velocity correction constraint is applicable, such as when hovering or temporarily landing in an elevator shaft, it monitors the accelerometer and gyroscope magnitudes of the inertial measurement unit. If, for a continuous period of time, such as half a second, the variance of the acceleration magnitude is less than 0.02 meters per square second and the variance of the angular velocity magnitude is less than 0.01 radians per square second, the UAV is determined to be in a stationary or quasi-stationary state. At this time, a zero-velocity observation factor is constructed, setting the observed value of the velocity state vector at the current moment as the zero vector and setting the corresponding covariance matrix to a minimum value, such as one part per million. This constraint is dynamically inserted into the factor graph to strongly pull back the false velocity caused by integral drift, thereby eliminating the accumulated error of the inertial measurement unit under long-term absence of satellite signals.

[0057] Furthermore, to address the degradation of the feed direction in the corridor scenario, a constant velocity model constraint can be constructed. If the drone is detected to be in a uniform linear flight mode, i.e., the throttle position is constant, and the forward distance cannot be perceived visually, it is assumed that the acceleration in the body coordinate system is zero, and a virtual acceleration observation constraint is constructed. Alternatively, the velocity estimate from the previous moment can be directly reused as the observation value at the current moment to assume a constant velocity. The covariance weight is set to a moderate level, such as one percent, to allow for small velocity changes caused by real dynamics while suppressing position drift along the corridor direction. All pseudo-observation factors are subjected to chi-square tests to ensure that the virtual constraints do not seriously conflict with the current residual sensor data, avoiding system crashes due to incorrect assumptions.

[0058] In this embodiment, an ultra-wideband opportunistic access mechanism based on degradation detection may also be included: an ultra-wideband ranging module is pre-configured on the UAV, which is in a low-power standby state by default; the ultra-wideband ranging module is activated only when motion degradation is determined to occur and the pseudo-observation constraint cannot meet the accuracy requirements; the ultra-wideband ranging data is dynamically inserted into the factor graph model as a new distance constraint factor until the degradation state is resolved.

[0059] Understandably, at the hardware level, four Decawave DWM1000 series ultra-wideband ranging modules are pre-embedded around the drone's arms, and the corresponding positioning base stations are pre-positioned in the flight area, such as in the corner of a warehouse or helipad. The three-dimensional coordinates of the base stations are known and stored in the onboard database. However, when the ultra-wideband ranging module is activated for ranging, the instantaneous power consumption is as high as 150 mA or more, and it is susceptible to multipath interference in complex metallic environments. Therefore, by default, the onboard ultra-wideband ranging module is in deep sleep mode, with a power consumption of less than 5 microamps, and only the listening function of the serial peripheral interface is retained.

[0060] The triggering mechanism relies on the aforementioned degradation detection results. When the minimum eigenvalue of the factor graph optimizer is below the danger threshold (e.g., 0.01%) for three consecutive frames, and the environmental fingerprint indicates that the Global Navigation Satellite System is completely unavailable (e.g., located deep indoors, with very few visual feature points, less than 15), the main control computer sends a wake-up command to the ultra-wideband ranging module via the serial peripheral interface bus. The ultra-wideband ranging module completes crystal oscillator startup and phase-locked loop locking within two milliseconds, and then initiates bidirectional ranging requests to surrounding base stations. In the data processing flow, the ultra-wideband ranging module returns the distance measurements from the current time to each base station. The values ​​are measured in meters with a precision of ±10 centimeters. First, the raw distance data is subjected to gross error removal, and outliers caused by multipath reflection are filtered out using a median filter. Then, the valid distance measurements are used to construct a distance constraint factor and inserted into the current factor graph model. The residual function of this distance constraint factor is defined as the Euclidean distance between the UAV position to be estimated and the base station position minus the measured distance. To prevent the introduction of systematic errors by the ultra-wideband data, such as antenna phase center delay, the ultra-wideband ranging deviation can also be included as a random walk state variable in the optimization vector for online estimation.

[0061] The exit mechanism can employ a hysteresis comparator strategy to prevent the system from frequently switching in critical states. Only when the degradation detection indicators return to normal, such as the number of visual feature points rising back to more than 50 and the minimum feature value being greater than one percent, and remaining stable for more than five seconds, will the system send a command to cause the ultra-wideband ranging module to re-enter deep sleep mode. This on-demand access mechanism not only reduces the overall average power consumption of the positioning system by 40%, but also, in extreme situations where both vision and satellite fail, such as in a smoke-filled fire scene or a completely dark pipe, utilizes the absolute distance constraint provided by the ultra-wideband ranging module to limit position drift to within centimeter range, achieving all-weather, all-scenario survivability.

[0062] In this embodiment, flight control execution is also included, specifically: the optimal pose estimation result is mapped in real time to the navigation state variables of the UAV, which include three-dimensional position, three-dimensional velocity and three-axis attitude angles; the navigation state variables are input to the UAV flight control system, and motor speed commands are generated through the cascade proportional-integral-derivative controller of the position loop and attitude loop to control the UAV to execute a predetermined flight path or hovering action, thereby realizing closed-loop autonomous positioning and navigation control.

[0063] Understandably, the input data for this step is the optimal pose estimation result after factor graph optimization and degradation compensation, output from step S04, including the three-dimensional position vector, three-dimensional velocity vector, and three-axis attitude quaternion in the body coordinate system. This data is transmitted at a high frequency of 400 Hz to a high-performance flight control microcontroller via shared memory or a universal asynchronous transceiver serial port. This microcontroller can use chips such as TMS320C6748. The control algorithm adopts a cascaded proportional-integral-derivative control structure, divided into four levels: position loop, velocity loop, attitude loop, and angular velocity loop. The outermost layer is the position control loop, which takes as input the deviation between the desired waypoint position and the current estimated position, and outputs the desired velocity command. This loop is optimized for hovering accuracy, and the integral amplitude of the position error is set to ±2 meters to prevent integral saturation. The middle layer is the velocity control loop, which takes as input the deviation between the desired velocity and the current estimated velocity, and outputs the desired attitude angle. This includes roll angle, pitch angle, and total thrust commands. In this stage, the estimated acceleration bias is used to feedforward compensate for the target angle, improving the dynamic response speed. The innermost layer consists of the attitude control loop and angular velocity loop. The attitude deviation is input, and the desired torque for each axis is calculated. The algorithm is specially designed with various protection logics for positioning state jumps. For example, when the environmental fingerprint indicator switches from indoor to outdoor, that is, when the global navigation satellite system signal is relocked, the positioning result may show an overall translation of the coordinate system. If the flight control system detects a step change in the position measurement value that exceeds the safety threshold, such as 0.5 meters, it does not directly input this large error into the controller, otherwise it will cause the UAV to twitch violently. Instead, it adopts a setpoint smooth follow-up strategy to temporarily adjust the target position to match the current jump position, and then slowly return to the original planned route with a limited acceleration and deceleration rate, that is, a maximum acceleration of two meters per cubic second, to ensure a smooth transition.

[0064] Ultimately, the mixer maps the calculated triaxial torque and total thrust into pulse width modulation signals for the four motors. The pulse width ranges from 1000 microseconds to 2000 microseconds, driving the electronic speed control device to control the speed of the brushless motors, thereby changing the lift distribution of the rotor and enabling the UAV to achieve precise hovering accuracy of less than 10 centimeters and trajectory tracking in complex interference environments.

[0065] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional modules is used as an example. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. The specific working process of the system, device, and unit described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0066] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces, or indirect coupling or communication connection between apparatuses or units, and may be electrical, mechanical, or other forms.

[0067] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0068] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0069] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory, random access memory, magnetic disks, or optical disks.

[0070] The above are all preferred embodiments of this application and are not intended to limit the scope of protection of this application. Any feature disclosed in this specification (including the abstract and drawings) may be replaced by other equivalent or similar features unless specifically stated otherwise. That is, unless specifically stated otherwise, each feature is only one example of a series of equivalent or similar features.

Claims

1. A method for positioning and orientation of an unmanned aerial vehicle (UAV), characterized in that, The method comprises the following steps: Data acquisition and space-time alignment: real-time acquisition of flight state data by a multi-source sensor group on board the UAV, and hard synchronization and external parameter calibration of heterogeneous flight state data based on a unified time reference, to establish a multi-source sensor data frame sequence; Environment feature fingerprint extraction: signal quality analysis and image feature extraction on the multi-source sensor data frame sequence, to construct an environment feature fingerprint reflecting the reliability of the current flight environment, which includes the carrier-to-noise ratio distribution characteristics of the global navigation satellite system signal and the texture richness characteristics of the visual image; Adaptive robust factor graph construction: constructing a factor graph model based on the multi-source sensor data frame sequence, and adding global navigation satellite system pseudorange, inertial measurement unit pre-integration quantity and visual re-projection error as factor nodes into the factor graph model; Dynamic covariance re-weighting optimization: dynamically adjusting the robust kernel function threshold and covariance weight of each factor node according to the environment feature fingerprint, and performing nonlinear optimization calculation on the factor graph model to output the optimal pose estimation result of the UAV; The dynamic covariance re-weighting optimization specifically comprises: when the environment feature fingerprint indicates that the global navigation satellite system signal is in a non-line-of-sight propagation state or the visual texture feature is lower than a preset richness threshold, the weight of the corresponding factor node is adaptively reduced and the robust truncation function is activated, to realize smooth transition in urban canyon or indoor-outdoor switching scenarios.

2. The method of claim 1, wherein, The nonlinear optimization calculation on the factor graph model adopts a dynamic sparse Bayesian factor graph real-time fusion refinement scheme, which specifically comprises: Constructing a hierarchical calculation architecture to divide the calculation process into a high-frequency inertial recursion layer and a low-frequency graph optimization layer; In the low-frequency graph optimization layer, a sliding window mechanism is used to manage factor nodes, and a dynamic sparse Bayesian inference strategy is used to marginalize the old key frame state that slides out of the window, to compress the old state information into a prior information matrix and pass it to the current window; An asynchronous feedback closed loop is established, and the inertial measurement unit zero bias estimation value obtained by the low-frequency graph optimization layer is used to asynchronously correct the mechanical arrangement error of the high-frequency inertial recursion layer, to meet the real-time positioning and orientation requirements of the UAV under high dynamic flight.

3. The method of claim 2, wherein, The marginalization process specifically comprises: Linearization of the oldest state quantity in the sliding window and the observation constraint associated with it by using the Schur complement technique; A sparse Hessian matrix is constructed to retain the correlation constraints between the remaining state quantities and the removed state quantities within the window, to form a prior factor node and join a new factor graph, so as to maintain the global consistency of the system and limit the growth of computing resources.

4. The method of claim 1, wherein, The hard synchronization and external parameter calibration of heterogeneous flight state data based on a unified time reference specifically comprises: The exposure time of the visual sensor is triggered by the pulse second signal of the underlying hardware, and the time stamp of the visual image frame is forced to align to the clock domain of the inertial measurement unit; According to the pre-calibrated lever arm parameters, the observation data of all sensors are unified transformed to the inertia center in the body coordinate system of the UAV, to eliminate the spatial asynchronization error caused by the installation position deviation of the sensors.

5. The method of claim 1, wherein, The construction reflects the environment feature fingerprint of current flight environment credibility, specifically comprising: Calculate the geometric dilution of precision and multipath effect index of the current visible satellite, and generate a satellite signal confidence vector; Perform gradient histogram analysis on the collected visual image, calculate the image information entropy and feature point distribution uniformity, and generate a visual observation confidence vector; The satellite signal confidence vector and the visual observation confidence vector are weighted and fused to generate a normalized environment feature fingerprint value, which is used to represent the probability distribution of the current scene belonging to an open environment, a semi-occluded environment or a weak texture environment.

6. The method of claim 1, wherein, It also includes a freedom degradation processing step for weak texture environment, specifically: Geometric feature degradation detection, eigenvalue decomposition is performed on the information matrix of the optimization level in the factor graph model to analyze the observability of the state space in each dimension; Pseudo-observation constraint construction, when it is detected that the eigenvalue of a certain direction is less than a preset degradation threshold, it is determined that motion degradation occurs in that direction, and a virtual pseudo-observation constraint factor is added to the factor graph model in that direction to restore the system freedom and suppress the pose drift.

7. The method of claim 6, wherein, The geometric feature degradation detection specifically includes: Extracting feature point spatial distribution data calculated by the front end of the visual odometry; Calculate the principal direction vector of the visual feature point cloud. If the principal direction vector is parallel to the motion direction of the unmanned aerial vehicle and the number of feature points in the orthogonal direction is less than the effective threshold, it is determined that geometric degradation occurs in the feeding direction, corresponding to a long corridor or tunnel scene.

8. The method of claim 6, wherein, The pseudo-observation constraint construction specifically includes: Plane geometric constraint: by identifying the ground or wall normal vector in the environment, a plane constraint factor is constructed to lock the roll angle and pitch angle freedom of the unmanned aerial vehicle, and only the yaw angle and plane displacement update are reserved; Zero speed correction constraint: when it is detected that the unmanned aerial vehicle is in a hovering state or the acceleration variance in the degradation direction is less than a static threshold, a zero speed observation constraint is applied in the degradation direction.

9. The method of claim 6, wherein, It also includes an ultra-wideband opportunity access mechanism based on degradation detection: An ultra-wideband ranging module is pre-configured on the unmanned aerial vehicle, which is in a low-power standby state by default; Only when it is determined that motion degradation occurs and the pseudo-observation constraint cannot meet the accuracy requirement, the ultra-wideband ranging module is activated; Ultra-wideband ranging data is dynamically inserted into the factor graph model as a new distance constraint factor until the degradation state is removed.

10. The method of claim 1-9, wherein, It also includes flight control execution, specifically: Map the optimal pose estimation result to the navigation state quantity of the unmanned aerial vehicle in real time, including three-dimensional position, three-dimensional velocity and three-axis attitude angle; Input the navigation state quantity into the unmanned aerial vehicle flight control system to generate motor speed instructions through a cascade proportional-integral-derivative controller of position loop and attitude loop, control the unmanned aerial vehicle to execute the predetermined flight path or hovering action, and realize closed-loop autonomous positioning and navigation control.

Images