High-frequency odometer positioning method and system for quad-rotor unmanned aerial vehicle adaptive adjustment under disturbed environment

Abstract

This invention relates to the field of UAV positioning technology, providing a high-frequency odometry positioning method and system for quadrotor UAVs under perturbed environments. The method includes: performing IMU integration and motion prediction based on the nominal state vector; constructing an error state Kalman filter framework; and updating the pre-integrated error covariance matrix. It also involves reverse correction of point cloud motion distortion using high-frequency predicted point clouds, extracting planar feature points, and downsampling for optimization. The matching search radius is adaptively adjusted based on the covariance matrix, and feature matching, plane fitting, and outlier removal are performed using a voxel map. Dual evaluation of geometric and perturbation confidence levels is conducted, and the time threshold is dynamically adjusted and the observation covariance matrix is ​​expanded. The optimized covariance matrix is ​​integrated into the filter operation to calculate the error increment, correct the pose, and simultaneously update the voxel map and output the positioning result. This scheme is adaptable to complex perturbed scenarios, balances computational load, and effectively prevents instantaneous overload of onboard computing power and system lag.

Description

Technical Field

[0001] This invention belongs to the field of UAV positioning technology, and particularly relates to a high-frequency odometry positioning method and system for adaptive adjustment of quadrotor UAVs under disturbed environments. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Quadrotor drones, due to their compact structure and high maneuverability, have been widely used in complex scenarios such as industrial inspection, disaster relief, and underground space exploration. In these practical tasks, drones often need to deal with uncontrollable wind disturbances, complex airflow effects in confined spaces, and violent maneuvers triggered to avoid obstacles (such as rapid acceleration and deceleration, and rapid rotation at large angles). In long corridors or open areas, the planar features extracted by radar are mostly parallel and lack cross constraints, leading to underdetermined equations for solving the pose and causing geometric degradation. In order to maintain the stability of the fuselage and the accurate execution of the flight trajectory under the above-mentioned disturbances, the drone's underlying flight controller relies heavily on low latency, high bandwidth (high output frequency), and robust self-pose estimation.

[0004] Currently, a single sensor is insufficient to meet the stringent requirements of autonomous flight. Inertial Measurement Units (IMUs) can output angular velocity and acceleration at extremely high frequencies (typically 200Hz to 500Hz), but due to the inherent zero-bias walk and measurement white noise of sensors, relying solely on the IMU for integration and calculation will generate divergent cumulative displacement errors in a very short time. On the other hand, three-dimensional lidar (LiDAR) can provide high-precision geometry of the surrounding environment, but its data output frequency is relatively low (typically 10Hz to 20Hz), and when the UAV is disturbed and undergoes violent maneuvers, severe motion distortion will occur within a single frame scan period of tens to hundreds of milliseconds.

[0005] To combine the advantages of both, laser-inertial tightly coupled odometry has become the mainstream solution in both academia and industry. However, existing mainstream tightly coupled Lidar-Inertial Odometry (LIO) systems have the following two main implementation methods and inherent limitations when dealing with highly disturbed environments: One approach is to update the state based on "radar frames." This type of approach typically uses iterative error state Kalman filtering or factor graph optimization, accumulating a full frame of point cloud data before performing low-frequency correction (approximately 10Hz) on the IMU integral. Since it relies entirely on the IMU's open-loop integration within the "blind zone" between two adjacent observations, the IMU's integration error will rapidly diverge upon encountering external disturbances such as strong winds, causing the flight control system to receive incorrect feedback. Simultaneously, when the divergent IMU state is used for backpropagation to remove radar point cloud distortion, it can lead to local map blurring, further reducing the final alignment accuracy.

[0006] Secondly, there is the scheme based on "radar points" for state updates. This type of scheme no longer waits to accumulate a whole frame of point cloud (100ms), but updates the state on a per-radar-point basis. Although this makes the state update frequency soar to the radar's packet transmission frequency, it requires the airborne computing platform to have extremely high computing power; more critically, a single radar point can only provide one-dimensional range constraints, lacks a global geometric perspective, is extremely sensitive to environmental noise, and is prone to state estimation degradation.

[0007] In summary, existing technologies exhibit significant limitations under strong disturbance environments: firstly, the contradiction between computing power bottlenecks and odometer output frequency is difficult to reconcile; secondly, motion distortion compensation is lagging, and the system lacks dynamic perception and weight adjustment mechanisms for the degree of environmental disturbance, resulting in insufficient overall robustness. Summary of the Invention

[0008] To address the technical problems mentioned above, this invention provides a high-frequency odometry positioning method and system for quadrotor UAVs under disturbed environments, employing IMU high-frequency data as a benchmark. It innovatively integrates a time-adaptive subframe dynamic truncation threshold, a spatial adaptive matching dynamic search radius threshold, a voxel map, and an error state iterative Kalman filter. For the time-adaptive subframe dynamic truncation threshold, the truncation time is determined by calculating the macroscopic geometric degradation degree and the physical disturbance degree. The subsequent judgment process is a "discrete four-level control law," dynamically adjusting the subframe cumulative duration and observation covariance weight. This ensures both distortion resistance and strong geometric constraints while achieving computational power "peak shaving and valley filling," effectively avoiding instantaneous overload of airborne computing power and system lag. For the spatial adaptive matching dynamic search threshold, the maximum eigenvalue is calculated using the pre-integrated error covariance matrix of the current subframe; the variance from the lidar hardware ranging is utilized, along with the uniformly distributed quantized variance resulting from the discretization of the OctVox (compact eight-voxel) map grid; and statistical methods are combined. The (Three Sigma) criterion achieves extremely high robustness in uncontrollable wind or violent maneuvering environments. Simultaneously, in degraded scenarios with few features, the system's temporal and spatial mechanisms accumulate more laser points over time and expand the spatial matching range spatially, effectively resisting environmental disturbances to the positioning effect in degraded scenarios.

[0009] To achieve the above objectives, the present invention adopts the following technical solution: The first aspect of this invention provides a time-adaptive subframe dynamic truncation threshold method for adaptive adjustment under perturbation conditions, comprising: The first aspect of the present invention provides a high-frequency odometry positioning method for adaptive adjustment of a quadcopter unmanned aerial vehicle under disturbed conditions, comprising: The data output from the IMU and the point cloud output from the solid-state lidar are acquired. The point cloud subframes within the interval and the IMU data are packaged into an independent processing package based on the target time span threshold. Maintain the nominal state vector, integrate using IMU data, calculate new states including high-frequency predicted pose and predicted velocity through motion prediction, simultaneously construct the error state Kalman filter framework, calculate the state error transfer matrix, and recursively update the pre-integrated error covariance matrix. For each point cloud subframe in the package, the high-frequency predicted pose corresponding to the subframe scanning time period is retrieved, the motion distortion of the point cloud is eliminated by backpropagation, the local curvature of the point cloud is calculated and points below the threshold are selected, and the set of planar feature points is obtained by voxel filtering and downsampling. Based on the pre-integrated error covariance matrix, the spatial adaptive matching search radius is calculated. Using a voxel map, nearest neighbor search and local plane fitting are performed on the set of planar feature points to dynamically remove outliers. The confidence of geometric degradation and physical perturbation is evaluated to update the target time span threshold. The observation covariance matrix is ​​adaptively expanded by a comprehensive degradation factor. The expanded observation covariance matrix is ​​injected into the gain calculation of the iterative error state Kalman filter to solve for the optimal error state increment, which is then compensated to the nominal state vector to correct the pose, maintain the voxel map, and output the odometry positioning result.

[0010] Furthermore, the updated target time span threshold is specifically as follows: if the geometric degradation confidence is lower than the preset safety threshold, a veto mechanism is triggered to set the target time span threshold to the maximum preset value; if it is higher than the safety threshold, the target time span threshold is dynamically calculated by calculating the nonlinear inverse proportional mapping function of the macroscopic geometric degradation confidence and the physical disturbance confidence. The geometric degradation confidence is evaluated by the minimum eigenvalue of the information matrix corresponding to the set of planar feature points; The confidence level of the physical disturbance is quantified by the variance of linear acceleration and angular velocity in the IMU data.

[0011] Furthermore, the minimum eigenvalue of the information matrix is: the geometric residual selected by the threshold of the adaptive matching search radius in the computational space. Regarding pose error state Jacobian matrix Construct an approximate Hessian information matrix Extract the minimum eigenvalue of the information matrix As a geometric degeneration confidence level This refers to eigenvalue decomposition of a matrix; The confidence level of the physical disturbance is: ;in, and These represent the three-axis acceleration and three-axis angular velocity of the UAV measured by the IMU during this short time window, respectively. and This refers to the variance of linear acceleration and the variance of angular velocity; and These are weighting coefficients; The target time span threshold The discrete four-level control law is as follows: ; in, To achieve a safe degradation threshold, and The calculated value is the physical disturbance threshold. The shutdown that drives the next round of data flow.

[0012] Furthermore, the elimination of point cloud motion distortion through backpropagation includes: for any point cloud subframe within the package, retrieving its high-frequency predicted pose at the sampling time and the reference time, calculating the relative rotation matrix and relative translation vector between the two times, and mapping the point cloud subframe to the reference time coordinate system through spatial transformation, wherein the reference time is set as the end timestamp of the point cloud subframe.

[0013] Furthermore, the local curvature is: Where S is the set of consecutive adjacent points within the same scan line or spatial neighborhood. and The coordinates of the point cloud are in the reference time coordinate system.

[0014] Furthermore, the formula for calculating the spatial adaptive matching search radius is as follows: ;in, The largest eigenvalue of the three-dimensional covariance submatrix of position error in the pre-integration error covariance matrix is... For the hardware ranging variance of the lidar, The variance of the grid discretization inherent in the voxel map. For voxel resolution.

[0015] Furthermore, the nearest neighbor search is implemented as follows: the planar feature points are projected onto the global coordinate system, the corresponding query sub-voxels are located through bit operations, a heuristic traversal list based on spatial distance priority is retrieved, and the search is performed from nearest to farthest in each group. The adaptive matching search radius is used as the maximum physical boundary, and a fixed-length max-heap is maintained. When the max-heap is full and the distance of the farthest point in the heap is less than the physical lower bound distance of the next group, an early termination mechanism is triggered.

[0016] Furthermore, the objective cost function of the iterative error state Kalman filter is: ;in, For the error state increment, The pre-integration error covariance matrix is... Let be the Jacobian matrix of the geometric residual with respect to the pose error state. The geometric residual from the point cloud to the local plane, This is the observation covariance matrix after adaptive dilation.

[0017] A second aspect of the present invention provides a high-frequency odometry positioning system for a quadcopter unmanned aerial vehicle that adapts to disturbances, comprising: The data packaging and fusion module is configured to: acquire the data output by the IMU and the point cloud output by the solid-state lidar, and package the point cloud subframes and IMU data within the interval into an independent processing package based on the target time span threshold; The prior state continuous estimation module is configured to: maintain the nominal state vector, integrate using IMU data, calculate new states including high-frequency predicted pose and predicted velocity through motion prediction, simultaneously construct the error state Kalman filter framework, calculate the state error transfer matrix, and recursively update the pre-integrated error covariance matrix. The feature extraction module is configured to: for each point cloud subframe in the package, retrieve the high-frequency predicted pose corresponding to the subframe scanning time period, eliminate point cloud motion distortion through backpropagation, calculate the local curvature of the point cloud and filter points below the threshold, and obtain a set of planar feature points through voxel filtering downsampling. The environmental perception module is configured to: calculate the spatial adaptive matching search radius based on the pre-integrated error covariance matrix; perform nearest neighbor search and local plane fitting on the set of planar feature points using a voxel map; dynamically remove outliers; evaluate the confidence of geometric degradation and the confidence of physical perturbation to update the target time span threshold; and adaptively expand the observation covariance matrix through a comprehensive degradation factor. The map maintenance module is configured to: inject the expanded observation covariance matrix into the gain calculation of the iterative error state Kalman filter, solve for the optimal error state increment, compensate to the nominal state vector to correct the pose, maintain the voxel map, and output the odometry positioning results.

[0018] A third aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described above.

[0019] A fourth aspect of the present invention provides a computer device including a computer-readable storage medium, a processor, and a computer program stored on the computer-readable storage medium and executable on the processor, wherein the processor executes the program to implement the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described above.

[0020] Compared with the prior art, the beneficial effects of the present invention are: This invention uses high-frequency IMU data as a benchmark and innovatively integrates a state-driven streaming point cloud dynamic truncation mechanism, voxel mapping, adaptive intervention of environmental perception observation weights, and error state iterative Kalman filtering. By real-time evaluation of geometric degradation confidence and physical perturbation confidence, it dynamically adjusts the subframe accumulation time and observation covariance weights. This achieves peak-shaving and valley-filling of computing power while ensuring anti-distortion and strong geometric constraints, effectively avoiding instantaneous overload of airborne computing power and system lag. Specifically, the time-adaptive subframe dynamic truncation threshold dynamically determines the accumulated laser points for matching based on the external environment of the UAV flight. The spatial adaptive matching dynamic search threshold determines the matching range. For example, in high-maneuver scenarios with external wind disturbances, a 20ms subframe is used to accelerate the IMU correction frequency, while a low search radius is used to speed up the search and eliminate exaggerated outliers. The two adaptive strategies are mutually corresponding and integrated.

[0021] This invention innovatively proposes a spatial adaptive mechanism that comprehensively considers the IMU integral pose covariance, map discretization error, and lidar hardware ranging noise, and calculates the dynamic search radius in real time based on the 3σ criterion. This mechanism endows the system with strong adaptability to highly dynamic external environments: it expands the search radius under high maneuver conditions and shrinks the search radius under stable low-speed operation. While ensuring extremely high matching accuracy, it can strictly eliminate interference noise in the steady state and effectively accommodate the drastic shift of the point cloud under strong disturbances, significantly reducing the overall computational cost and the risk of system collapse due to target point loss. Attached Figure Description

[0022] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0023] Figure 1 This is a flowchart of the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance environment according to Embodiment 1 of the present invention; Figure 2 The spatial adaptive matching dynamic search threshold of Embodiment 1 of the present invention A schematic diagram of the solution process; Figure 3 This is a flowchart of the method for calculating the time-adaptive subframe dynamic truncation threshold ΔT in Embodiment 1 of the present invention; Figure 4 This is a framework diagram of the high-frequency odometry positioning system for adaptive adjustment of a quadcopter UAV under disturbance environment according to Embodiment 2 of the present invention. Figure 5 This is a schematic diagram of the structure of a computer device according to Embodiment 4 of the present invention. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.

[0025] It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0026] Example 1 This embodiment provides a high-frequency odometry positioning method for quadrotor UAVs that adapts to disturbance environments.

[0027] The high-frequency odometry positioning method for quadrotor UAVs with adaptive adjustment under disturbed environments provided in this embodiment is applied to quadrotor UAVs under disturbed environments.

[0028] Designing a laser-inertial tightly coupled odometry system that can effectively overcome point cloud distortion caused by strong disturbances and get rid of the burden of heavy nonlinear iterative optimization, thereby achieving both high robustness and high output frequency (reaching the native frequency of the IMU) under the condition of limited airborne computing power, is a key technical problem that urgently needs to be solved in the field of high dynamic autonomous navigation of quadcopter UAVs.

[0029] To address the problems of severe point cloud motion distortion, high computational cost of nonlinear iterative optimization, high odometry output latency, and easy filtering divergence caused by local noise points in existing technologies for quadrotor UAVs facing dynamic scenarios such as strong wind disturbances and agile obstacle avoidance maneuvers, this embodiment provides a high-frequency odometry positioning method for quadrotor UAVs that is adaptively adjusted under disturbance conditions. It uses the high-frequency output of the inertial measurement unit (IMU) as the absolute physical reference and innovatively integrates the time adaptive subframe dynamic truncation threshold and the spatial adaptive matching dynamic search radius threshold, OctVox map and error state iterative Kalman filter (IESKF). This achieves low-latency, distortion-resistant and all-terrain robust high-frequency six-degree-of-freedom pose estimation under limited airborne computing power.

[0030] This embodiment provides a high-frequency odometry positioning method for quadrotor UAVs that adapts to disturbance environments, such as... Figure 1 As shown, it includes the following steps: S1: Enables synchronous acquisition and alignment of multi-source sensor data.

[0031] S1.1: Continuously listen to and receive the ultra-high frequency data stream (usually 200Hz) output by the airborne inertial measurement unit (IMU), and extract the instantaneous three-axis acceleration and three-axis angular velocity of the UAV body contained in the data stream.

[0032] Defined in At any given moment, the instantaneous three-axis acceleration measurement values ​​of the extracted UAV body are recorded as follows: The triaxial angular velocity measurement value is recorded as .

[0033] S1.2: The discrete data sent by the high-frequency laser points of the solid-state lidar is directly monitored at the underlying network interface; for each data packet, a very small amount of point cloud and hardware timestamp are parsed and immediately pushed into a high-frequency sliding buffer maintained in memory.

[0034] S1.3: The global absolute time reference is the IMU hardware timestamp, which has a higher frequency and lower latency. The IMU and Lidar points will have their own timestamps during the acquisition process. The IMU timestamps will be used as the reference to align and package the timestamps between the IMU and Lidar.

[0035] Let the sampling timestamps of two adjacent frames of LiDAR data be respectively and The timestamp of the current discrete IMU data obtained from parsing is .

[0036] S1.4 to 1.6: Target time span thresholds for receiving feedback from stage S4. Real-time monitoring of the sliding cache area; once the latest timestamp is detected... With the oldest timestamp satisfy This immediately triggers a truncation. The interval... The extracted point cloud subframes and their corresponding IMU data are packaged and defined as the first... Each package is handled independently. .

[0037] S1.4: During the IMU and LiDAR packet processing, based on the "geometric degradation confidence" and "physical perturbation confidence" fed back from the previous time step S4, a two-layer dynamic threshold decision strategy is executed to set the target time span threshold. If the geometric degradation confidence level in the previous time step is lower than the preset safety threshold, a veto mechanism is triggered, forcing the change to be reversed. The maximum preset value is set to maximize the point cloud accumulation to ensure spatial constraints; if the geometric degradation confidence level of the previous time step is higher than the preset safety threshold, the physical perturbation confidence level of the previous time step is used to dynamically calculate the value through a nonlinear inverse proportional mapping function. The more intense the physical maneuver, the smaller the calculated ΔT. This is based on the final settings of the strategy. This will guide subsequent truncation operations.

[0038] S1.5: Real-time monitoring of the high-frequency sliding buffer constructed in step S1.2. Once the timestamp difference between the latest and oldest points in the buffer reaches the set target threshold, the point cloud within this time period is instantly packaged and extracted into an independent short-time subframe, and the buffer segment is cleared to receive subsequent water flow, thereby realizing the division of one frame of LiDAR data into N subframes.

[0039] S1.6: Based on the initial and end timestamps of the subframe, the IMU data within this timestamp and the point cloud data of the Lidar subframe are packaged into a single package. The point cloud will then be processed based on these packages.

[0040] S1.7: Call the pre-calibrated external parameter matrix (including translation vector and rotation matrix) during initialization to perform a three-dimensional spatial transformation on the time-aligned laser point cloud, mapping the coordinates of all laser points from the "radar self-coordinate system" to the "IMU local body coordinate system".

[0041] Let the radar itself be in a coordinate system ( The packaged single-frame point cloud is ,in accordance with IMU data inside Backpropagation of the IMU is performed to obtain a single-frame point cloud with distortion removed. Its discrete point coordinates Call the pre-calibrated external translation vector of the radar to the IMU. With rotation matrix By mapping all laser points to the IMU's local body coordinate system (I-frame) using the spatial transformation formula, the aligned coordinates are obtained. : (1); S2: Continuous extrapolation of prior states based on high-frequency inertial data.

[0042] S2.1: Allocate and maintain a multi-dimensional "nominal state vector" in memory to record the UAV's current three-dimensional position, three-dimensional velocity, attitude quaternion, and the accelerometer and gyroscope zero bias of the IMU sensor in real time.

[0043] Allocate and maintain a continuous-time nominal state vector in memory. Its definition is as follows: (2); in, This is the rotation matrix from the IMU coordinate system to the global coordinate system. These represent the position and velocity of the IMU in the global coordinate system; These are the zero bias values ​​for the accelerometer and gyroscope, respectively. It is a special orthogonal group of dimension 3.

[0044] S2.2: During the gap ΔT while waiting for the sliding buffer to reach the target cutoff threshold, the high-frequency IMU data obtained in S1.1 is substituted into the rigorous rigid body kinematics equations to perform high-frequency continuous integration calculations.

[0045] S2.3: Through the above integral calculation, the prior high-frequency predicted pose and predicted velocity of the UAV at the current moment are derived.

[0046] Waiting for the target cutoff threshold During the interval, IMU measurements extracted in the S1 stage are used. and Substituting into the INS (Inertial Navigation System) mechanization model, given the measurement values ​​read by the accelerometer and gyroscope in the robot's main frame at time k... and The new state is calculated through motion prediction, and the new state predicted by INS is written as: (3); in, It is the integration interval. and These are the current position estimate and the previous position estimate, respectively. and These are the linear velocity estimates at the current time and the linear velocity estimates at the previous time, respectively. Exp is the linear velocity estimate up to the last time step. Group exponential mapping, These are the pose estimation at the current moment and the pose estimation at the previous moment, respectively. and These are the linear accelerations at the current moment and the linear accelerations at the previous moment, respectively. The system zero bias is defined for the accelerometer and gyroscope, respectively. It is the gravity vector in the global coordinate system.

[0047] S2.4: Simultaneously construct the mathematical framework for Error State Kalman Filtering (ESKF). Based on the noise model, calculate the transfer matrix of the state error and recursively update the "pre-integration error covariance matrix". This matrix mathematically quantifies how the "lack of confidence" in the current estimated position gradually increases as the integration time increases. This error covariance matrix will serve as the core physical prior input for subsequent spatial adaptive data association and dynamic search radius calculation.

[0048] Use an error-state Kalman filter to process the robot's state. Make an estimate: (4); in, , and These represent the position, attitude, and velocity errors, respectively. and This indicates the bias error of the gyroscope and accelerometer.

[0049] The discrete model of the IMU can then be written as the following linearized error state transition equation: (5); in, For the current moment The error state vector, which is the 15-dimensional vector in formula (4) that includes position error, attitude error, velocity error and gyroscope and accelerometer bias (zero bias) error; Previous moment The error state vector; The discrete state transition Jacobian matrix determines the error of the previous second. This is the input noise vector, which is the built-in noise of the IMU sensor, including Gaussian white noise from the accelerometer and gyroscope. and zero-bias random walk noise ; The Jacobian matrix is ​​driven by noise because of the underlying physical noise of the sensor. It cannot be directly equated to position or velocity error, therefore it needs to be... This matrix maps the underlying physical noise onto the 15-dimensional state error.

[0050] in, Gaussian white noise from accelerometers and gyroscopes and zero-biased random walk noise The input noise vector is composed of the first-order Taylor expansion and Euler integral based on rigid body kinematics (integration interval is...). The discrete state transition Jacobian matrix can be specifically derived. With noise-driven Jacobian matrix .

[0051] After obtaining the state transition matrix, the error covariance matrix is ​​recursively updated. : (6); in, This is the diagonal covariance matrix of the noise. The uncertainty of the system state distribution is mathematically quantified and will be directly used as the core physical prior input for spatial adaptive data association and dynamic search radius calculation in the S4.1 stage.

[0052] S3: High-frequency distortion correction and feature extraction of sub-point clouds using IMU.

[0053] S3.1: For each Lidar subframe in the package, send a request to the time axis to retrieve the high-frequency IMU predicted pose calculated by S2.3 within the scanning time period of that subframe.

[0054] For the extraction of the first stage in S1 Each subframe is processed independently, and the reference alignment time for this subframe is set to... (Usually, the end timestamp of the subframe is taken) For any number of items within the package... A discrete laser point, with a sampling timestamp of . (satisfy ).

[0055] Send a request to the timeline to retrieve the data derived from continuous integration in stage S2.2, and... IMU high-frequency predicted pose at time 1 and base time Corresponding predicted pose: (7); S3.2: Using the retrieved pose data for a very short time period, perform backpropagation of IMU data for all laser points within the independent subframe to eliminate motion distortion caused by the high-speed flight of the UAV.

[0056] Using the retrieved pose data over a very short time period, calculate from Time to base time The relative spatial rigid body transformation yields the relative rotation matrix. With relative translation vector : (8); (9); Let the coordinates of the discrete point S1.7 mapped to the IMU local coordinate system be... The IMU data of all laser points within the subframe is backpropagated and projected to transform them uniformly to the reference time. In a coordinate system, obtain the true observed coordinates after distortion correction. : (10); S3.3: Considering the physical characteristics of sparse edge points and susceptibility to noise interference in solid-state radar, local curvature calculation is performed on the distortion-free subframe point cloud, and edge point extraction is actively abandoned. Downsampling is performed through spatial voxel filtering, selecting and retaining only the "set of planar feature points" with minimal curvature that represent stable walls and ground surfaces in the environment, providing the purest geometric material for subsequent matching.

[0057] Considering the physical characteristics of solid-state lidar, local geometric curvature calculations are performed on the distortion-corrected subframe point cloud. For points... Select those within the same scan line or spatial neighborhood consecutive adjacent points ( ), define its local curvature for: (11); Set curvature rejection threshold ; Voluntarily give up the right to withdraw High curvature edge points and noise, only retaining The point; subsequently, a spatial voxel filter mesh (mesh side length set to) is introduced. The remaining point cloud is uniformly downsampled. Within each voxel, only the single laser point closest to the geometric center of the voxel is selected and retained, ultimately constructing a set of planar feature points representing stable walls and ground in the environment. This provides geometric material for the subsequent OctVox map hash matching in the S4 stage.

[0058] S4: A strategy based on spatial adaptive matching dynamic search radius threshold and temporal adaptive subframe dynamic truncation threshold using voxel hashing.

[0059] S4.1: Obtain the pre-integration error covariance matrix of the current subframe output from stage S2.4. Based on the unscented transform and the maximum range ranging model of the lidar, the covariance matrix is ​​mapped from the pose space to the point-to-point distance displacement variance in the three-dimensional physical space. Then, by fusing the inherent grid discretization variance of the OctVox map and the ranging variance of the lidar hardware, and based on the statistical 3σ criterion, the spatial adaptive matching search radius of the current subframe is calculated in real time.

[0060] according to Figure 2 The spatial adaptive mechanism process involves obtaining the pre-integration error covariance matrix of the current subframe output in stage S2.4. Extract the three-dimensional covariance submatrix corresponding to the positional errors. Calculate the largest eigenvalue Combining the inherent hardware ranging variance of lidar And the uniform distribution quantization variance brought about by the OctVox map grid discretization. (in (for voxel resolution), based on statistical 3 Criteria for calculating the adaptive matching search threshold of the current subframe in 3D space. : (12); in, This dynamic threshold represents the largest eigenvalue of the matrix. This will strictly limit the physical boundaries of subsequent feature matching.

[0061] S4.2: Invoke the internally maintained compact octovox map based on hash index. Project the independent sub-frame planar feature points extracted in stage S3 onto the global coordinate system. Use bitwise operations to quickly locate the query sub-vox to which it belongs, and retrieve the heuristic traversal list based on spatial distance priority that was built offline beforehand.

[0062] For the distortion-removed planar feature points of the independent subframes extracted in stage S3 The reference time prior rotation matrix calculated using the S2 stage Translation vector Projecting it onto the global coordinate system yields : (13); in, It refers to the set of all distortion-corrected planar feature points.

[0063] Calculate the voxel index of the point in the OctVox map using bitwise operations. With daughter voxel index And retrieve a heuristic traversal list based on spatial distance and physical priority. in, This represents a complete traversal order list, which specifies the order in which the drone searches for points in the voxel map. Represents the distance equivalent group, It is the "voxel grid" closest to the current point. It's the ring of squares right next to the outside. These are the outermost grids. (The concatenation operator) indicates sequential arrangement. The physical meaning of this formula is: it forces the nearest string to be placed first. Search within the circle, and then go to... The circles expand outwards sequentially, never skipping levels.

[0064] S4.3: Based on the heuristic traversal list, perform nearest neighbor search on the map from nearest to farthest in groups of equivalent distance. During the search, strictly use the dynamic search radius calculated in stage S4.1 as the maximum physical boundary of the spatial search, and maintain a fixed-length max-heap. When the max-heap is full and the distance to the farthest point in the heap is strictly less than the lower bound of the physical distance of the next group, trigger an early termination mechanism to avoid redundant global traversal while ensuring accuracy.

[0065] By group The order is to traverse the surrounding sub-voxels from nearest to farthest, and maintain a maximum capacity of A max-heap. In this process, all spatial Euclidean distances are ignored. The representative point. Early termination condition: when the max-heap is full and the distance of the farthest point in the heap is... satisfy (in When the physical lower bound distance of the next group to be traversed is reached, the search is terminated immediately.

[0066] S4.4: Fit the local plane using the searched precise nearest neighbor points and calculate the primary geometric residual from each feature point to the corresponding plane, analyzing the distribution of the primary geometric residual for each point. At this point, replace the traditional empirical static threshold with the dynamic search radius to dynamically eliminate outliers with excessive geometric residuals, strictly retaining the set of static interior points within the dynamic high-confidence interval, thus avoiding local noise causing subsequent filtering divergence.

[0067] Using the search results Fit a local plane using precise nearest neighbor points to obtain the plane normal vector. and a little bit on the surface Calculate the first Geometric observation residuals of each feature point : (14); The system uses a dynamic search radius threshold Set a rejection threshold range for the baseline. If the absolute value of the residual satisfies... ( If the preset safety tolerance coefficient is used, then the point is determined to be a local dynamic noise point or a mismatch point, and it is removed from the static internal point set.

[0068] S4.5: Extract the information matrix corresponding to the current static interior point set, calculate its minimum eigenvalue to assess the confidence level of macroscopic geometric degradation; simultaneously calculate the variance of linear acceleration and angular velocity of IMU measurement data within a short time window to quantify the confidence level of physical disturbance. Based on the dual-reset confidence level, execute a discrete four-level adaptive truncation mechanism to output the target time threshold. First, a geometry-priority decision is performed: in a highly degraded scenario, a maximum threshold of 150ms is forced to maximize the collection of spatial constraints; if the scenario is not degraded, a maneuver-adaptive decision is performed based on the physical disturbance confidence level: 75ms for normal stable flight, 50ms for moderate wind disturbance and high maneuverability, and a minimum threshold of 20ms for strong wind disturbance and extreme maneuverability to combat motion distortion. This threshold result is fed back to the front end in real time to guide the truncation of the underlying data stream. This threshold result is also fed back to step S1.4 of the front end in real time to guide the temporal truncation of the underlying data stream.

[0069] Calculate the geometric residuals for the retained static interior points. Regarding pose error state Jacobian matrix Construct an approximate Hessian information matrix. : (15); according to Figure 3 The process of time-adaptive subframe dynamic segmentation thresholding involves eigenvalue decomposition to extract the minimum eigenvalue. As a macroscopic geometric degradation degree, among which This refers to eigenvalue decomposition of a matrix, while It refers to the smallest extracted feature value. Target time span threshold. Simultaneously retrieve the weighted variance of IMU measurement data within the corresponding time window of stage S1. As a physical perturbation degree, among which... and These represent the three-axis acceleration and three-axis angular velocity of the UAV measured by the IMU during this short time window, respectively. and This refers to the variance of linear acceleration and the variance of angular velocity; and These are weighting coefficients, empirical parameters obtained through pre-calibration and debugging. The system sets a safety degradation threshold. and physical disturbance threshold and These are also empirical parameters obtained through pre-calibration and debugging. Target time span threshold. The discrete four-level control law is as follows: (16); The solution The data is fed back to the front-end S1.4 step in real time, driving the next round of data flow to be cut off.

[0070] S4.6: Based on the above evaluation results, a comprehensive degradation factor is calculated using a preset joint evaluation function (this factor is amplified synchronously based on the accumulated error variance when the 150ms equal-length threshold is triggered). Before entering the back-end optimization, this comprehensive degradation factor is used to perform an adaptive expansion operation on the covariance matrix of the radar observation in the current subframe.

[0071] The comprehensive degradation and expansion factor was calculated based on the results of the dual evaluation. : (17); in, Characterizes the comprehensive degradation expansion factor, and limits its value range to 1. This factor is used to dynamically adjust the system's trust weight for the current radar observation data; A geometric degradation penalty term is represented. A maximum function is used to limit its lower bound to zero, and a positive penalty incentive is generated only when the geometric eigenvalue is below a safe threshold. If the environmental features are sufficient ( This value is 0. If features are scarce (e.g., in a long corridor), the larger the difference, the more the system suspects radar. This parameter characterizes the weighted physical perturbation (variance) of IMU measurement data within a short time window. It is used to quantify the degree of instantaneous high-dynamic maneuverability or wind disturbance experienced by the UAV. This is an IMU drift penalty. This refers to the "trace" of the matrix (the sum of the elements on the main diagonal). Here, it represents the total positional uncertainty of the IMU pre-integration in the X, Y, and Z directions. If the IMU integration time is too long, resulting in a large error, it will also affect the confidence in radar observations; and , and These are empirical parameters obtained through pre-calibration and debugging.

[0072] Let the original radar observation noise covariance matrix be... Use this factor to perform observational interventions: (18); in, It is the dilated observation covariance matrix, which is calculated using... The original error is directly "magnified many times over".

[0073] S5: Joint optimization of error states, closed-loop feedback and map maintenance.

[0074] S5.1: Combine the IMU pre-integration residual calculated in stage S2 with the geometric residual constructed in stage S4 to construct a tightly coupled nonlinear objective cost function; at the same time, force the expanded observation covariance matrix in stage S4 into the gain calculation module of the Iterative Error State Kalman Filter (IESKF).

[0075] The pre-integral prior matrix derived in the joint S2 stage The observed covariance matrix after the S4 stage intervention Construct the following IESKF objective cost function. : (19); in, The target cost function representing the system to be optimized; It is the variable to be determined that characterizes the system, namely the error state increment. Characterizes the prior residual terms; among which, The pre-integration error covariance matrix is... Its inverse matrix serves as the information matrix for the residual term; The representation involves summing all static valid planar feature points selected and retained within the current subframe; Characterizing the geometric observation residuals; The Jacobian matrix representing the geometric residual of the i-th feature point in relation to the pose error state. The primary geometric residual characterizing the i-th point cloud feature point to the fitted local plane; The inverse of the observation covariance matrix after adaptive expansion is used as the information matrix of the observation residual term.

[0076] Once the iterative solver is started, the high-frequency truncation mechanism (especially at the 20m or 50ms level) provides extremely close initial prediction values. After distortion correction, the system typically only needs 1-2 Gauss-Newton iterations to quickly obtain the optimal error state increment for the aforementioned cost function. .

[0077] S5.2: Start the Iterative Error State Kalman Filter (IESKF) solver. Thanks to the extremely high accuracy of the initial prediction provided by the discrete five-level truncation mechanism under high-speed maneuvers (such as 20ms or 50ms), it can quickly converge with only a few iterations and solve for the optimal error state increment.

[0078] S5.3: The obtained optimal error state increment is compensated into the nominal state vector to complete the absolute correction of the UAV's true pose; and the inherent zero bias estimate of the IMU is updated simultaneously to suppress long-endurance integral drift.

[0079] S5.4: Combines the corrected high-precision pose with the real-time high-frequency integration of the IMU to continuously send 6-DoF (six degrees of freedom) odometry positioning results to the UAV's underlying flight controller, ensuring the UAV's flight under strong disturbances.

[0080] S5.5: Using the corrected latest absolute pose, the feature points of the current sub-frame are projected onto the global coordinate system and mapped to the corresponding voxels and their internal eight-level molecular voxels using a hash function. Each sub-voxel is strictly limited to retaining at most one representative point. If a historical representative point already exists for the target sub-voxel, an unbiased incremental mean update is performed based on the number of historical points, recalculating the coordinates and number of representative points. This achieves constant memory space downsampling with constant time complexity and online progressive denoising.

[0081] Among them, the addition operation of generalized manifolds is used. The obtained error increment Compensation to nominal state vector Complete absolute pose correction in the middle: (20); Synchronous extraction The error values ​​are updated to reflect the zero bias of the accelerometer and gyroscope. Combined with the updated absolute pose and IMU data, high-frequency 6-DoF odometry is continuously transmitted to the underlying drone system.

[0082] Using the corrected high-precision pose, the points within the current subframe are projected onto the global coordinate system to obtain the final mapping points. A hash function is used to map it to the corresponding OctVox map sub-voxel.

[0083] Extract the coordinates of the historical representative points maintained in the memory of this sub-voxel. With the accumulator of the number of occupied point clouds If the target sub-voxel has not yet been initialized, then let and If a historical representative point already exists and the maximum capacity threshold has not been exceeded, then an unbiased incremental mean update is performed. (twenty one); (twenty two); This mechanism ensures that only one continuously refined statistical representative point is retained within each sub-voxel, eliminating the possibility of memory overflow from the physical level and achieving online constant-time noise reduction of point cloud maps.

[0084] In summary, to address the challenges of severe point cloud distortion, the contradiction between computing power and output frequency, and geometric degradation faced by quadrotor UAVs in disturbed environments, this invention proposes an adaptive adjustment-based laser-inertial high-frequency odometry positioning method. This method uses IMU high-frequency data as a benchmark and innovatively integrates a state-driven streaming point cloud dynamic truncation mechanism, OctVox map, adaptive intervention of environmental perception observation weights, and error state iterative Kalman filtering (IESKF). By real-time evaluation of geometric degradation confidence and physical disturbance confidence, it dynamically adjusts the subframe accumulation time (20-150 ms) and observation covariance weights, achieving computational power "peak shaving" while ensuring anti-distortion and strong geometric constraints. After IMU prior integration, backpropagation distortion correction, adaptive radius nearest neighbor matching, and tight coupling joint optimization, it continuously outputs a highly robust, low-latency six-degree-of-freedom pose estimate to the flight control system, balancing rapid response under high-dynamic maneuvers and stable positioning in feature-sparse scenarios.

[0085] This embodiment provides an adaptive high-frequency odometry positioning method for quadrotor UAVs under disturbed environments. It innovatively introduces a state-driven streaming point cloud truncation mechanism and OctVox map, combined with an adaptive intervention mechanism for environmental perception observation weights and error state iterative Kalman filtering (IESKF), achieving low-latency, distortion-resistant, and all-terrain robust high-frequency six-DOF pose estimation under limited onboard computing power. To address the extreme scenarios where UAVs face high-dynamic maneuvers due to wind disturbances and limited features in long corridors or open fields, this invention establishes an adaptive mechanism combining feedforward and feedback. During mission execution, the UAV may spend 90% of its time in stable cruising, requiring only 10% of the time for emergency obstacle avoidance, sharp cornering, or acceleration. It always maintains a minimal... For example, point matching during smooth flight is a waste of computing power and may even lead to feature degradation due to excessively fragmented frames and narrow single-frame fields of view. Based on "geometric eigenvalues" and "physical perturbation variance" as feedforward perception of the external environment, a dynamic subframe adaptive truncation mechanism dynamically scales the subframe truncation threshold. (S3.1) Adapt to the external environment with effective dynamic feedback, and realize the need for high frequency control in the high dynamic maneuvering environment of wind disturbance with 20ms and 50ms ΔT subframe mode. In the environment of long corridor or open space with scarce features, the geometric features are accumulated by forcibly using 150ms ΔT subframe mode to deal with the scenario of degradation disturbance.

[0086] Unlike traditional methods that rely on waiting for fixed-time collection of full-frame point clouds or radar-based methods, the high-frequency odometry positioning method for quadrotor UAVs in disturbed environments provided in this embodiment extracts short-time subframes during the UAV's highly dynamic maneuvering state amidst wind disturbances through underlying UDP packet streaming reception (S1.2) and a state-driven dynamic truncation mechanism (S3.1 and S3.2). This not only reduces odometry observation latency but also distributes the enormous point cloud processing pressure evenly across multiple time slices, effectively avoiding instantaneous overload of onboard computing power and system lag.

[0087] This embodiment provides a high-frequency odometry positioning method for quadrotor UAVs under disturbed environments that adapts to localization. Based on a hash-indexed OctVox map, it significantly improves the candidate point selection speed using heuristic voxel hash nearest neighbor search. Furthermore, this invention innovatively proposes a spatial adaptive mechanism that comprehensively considers IMU integral pose covariance, map discretization error, and LiDAR hardware ranging noise, and is based on 3D... The criterion calculates the dynamic search radius in real time. This mechanism endows the system with strong adaptability to highly dynamic external environments: it expands the search radius under high maneuvering conditions and shrinks it under stable, low-speed operation. While ensuring extremely high matching accuracy, it can strictly eliminate interference noise in steady-state conditions and effectively accommodate drastic point cloud shifts under strong disturbances, significantly reducing overall computational overhead and the risk of system crash due to target point loss.

[0088] Example 2 The high-frequency odometry positioning system for quadcopter UAVs that adapts to disturbance environments provided in this embodiment, such as Figure 4 As shown, it includes: The data packaging and fusion module is configured to: acquire the data output by the IMU and the point cloud output by the solid-state lidar, and package the point cloud subframes and IMU data within the interval into an independent processing package based on the target time span threshold; The prior state continuous estimation module is configured to: maintain the nominal state vector, integrate using IMU data, calculate new states including high-frequency predicted pose and predicted velocity through motion prediction, simultaneously construct the error state Kalman filter framework, calculate the state error transfer matrix, and recursively update the pre-integrated error covariance matrix. The feature extraction module is configured to: for each point cloud subframe in the package, retrieve the high-frequency predicted pose corresponding to the subframe scanning time period, eliminate point cloud motion distortion through backpropagation, calculate the local curvature of the point cloud and filter points below the threshold, and obtain a set of planar feature points through voxel filtering downsampling. The environmental perception module is configured to: calculate the spatial adaptive matching search radius based on the pre-integrated error covariance matrix; perform nearest neighbor search and local plane fitting on the set of planar feature points using a voxel map; dynamically remove outliers; evaluate the confidence of geometric degradation and the confidence of physical perturbation to update the target time span threshold; and adaptively expand the observation covariance matrix through a comprehensive degradation factor. The map maintenance module is configured to: inject the expanded observation covariance matrix into the gain calculation of the iterative error state Kalman filter, solve for the optimal error state increment, compensate to the nominal state vector to correct the pose, maintain the voxel map, and output the odometry positioning results.

[0089] It should be noted that each module in this embodiment corresponds one-to-one with each step in Embodiment 1, and their specific implementation processes are the same, so they will not be repeated here.

[0090] Example 3 This embodiment provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described in Embodiment 1 above.

[0091] Example 4 This embodiment provides a computer device, such as... Figure 5 As shown, the system includes a computer-readable storage medium 1003, a processor 1001, a communication interface 1002, and a computer program stored on the computer-readable storage medium 1003 and executable on the processor 1001. The processor 1001, communication interface 1002, and computer-readable storage medium 1003 can be connected via a bus or other means. The communication interface 1002 is used to receive and transmit data. When the processor 1001 executes the program, it implements the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described in Embodiment 1 above.

[0092] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed conditions, characterized in that, include: The data output from the IMU and the point cloud output from the solid-state lidar are acquired. The point cloud subframes within the interval and the IMU data are packaged into an independent processing package based on the target time span threshold. Maintain the nominal state vector, integrate using IMU data, calculate new states including high-frequency predicted pose and predicted velocity through motion prediction, simultaneously construct the error state Kalman filter framework, calculate the state error transfer matrix, and recursively update the pre-integrated error covariance matrix. For each point cloud subframe in the package, the high-frequency predicted pose corresponding to the subframe scanning time period is retrieved, the motion distortion of the point cloud is eliminated by backpropagation, the local curvature of the point cloud is calculated and points below the threshold are selected, and the set of planar feature points is obtained by voxel filtering and downsampling. Based on the pre-integrated error covariance matrix, the spatial adaptive matching search radius is calculated. Using a voxel map, nearest neighbor search and local plane fitting are performed on the set of planar feature points to dynamically remove outliers. The confidence of geometric degradation and physical perturbation is evaluated to update the target time span threshold. The observation covariance matrix is ​​adaptively expanded by a comprehensive degradation factor. The expanded observation covariance matrix is ​​injected into the gain calculation of the iterative error state Kalman filter to solve for the optimal error state increment, which is then compensated to the nominal state vector to correct the pose, maintain the voxel map, and output the odometry positioning result.

2. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The target time span threshold The discrete four-level control law is as follows: ； in, To achieve a safe degradation threshold, and The calculated value is the physical disturbance threshold. The shutdown that drives the next round of data flow is cut off; The minimum eigenvalue of the information matrix is: the geometric residual selected by the adaptive matching search radius threshold in the computational space. Regarding pose error state Jacobian matrix Construct an approximate Hessian information matrix Extract the minimum eigenvalue of the information matrix As a geometric degeneration confidence level This refers to eigenvalue decomposition of a matrix; The confidence level of the physical disturbance is: ;in, and These represent the three-axis acceleration and three-axis angular velocity of the UAV measured by the IMU during this short time window, respectively. and This refers to the variance of linear acceleration and the variance of angular velocity; and These are weighting coefficients.

3. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The method of eliminating point cloud motion distortion through backpropagation includes: for any point cloud subframe within the package, retrieving its high-frequency predicted pose at the sampling time and the reference time, calculating the relative rotation matrix and relative translation vector between the two times, and mapping the point cloud subframe to the reference time coordinate system through spatial transformation, wherein the reference time is set as the end timestamp of the point cloud subframe.

4. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The local curvature is: Where S is the set of consecutive adjacent points within the same scan line or spatial neighborhood. and The coordinates of the point cloud are in the reference time coordinate system.

5. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The formula for calculating the spatial adaptive matching search radius is: ;in, The largest eigenvalue of the three-dimensional covariance submatrix of position error in the pre-integration error covariance matrix is... For the hardware ranging variance of the lidar, The variance of the grid discretization inherent in the voxel map. For voxel resolution.

6. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The nearest neighbor search is implemented as follows: the planar feature points are projected onto the global coordinate system, the query sub-voxels are located through bit operations, a heuristic traversal list based on spatial distance priority is retrieved, and the search is performed from nearest to farthest in each group. The adaptive matching search radius is used as the maximum physical boundary, and a fixed-length max-heap is maintained. When the max-heap is full and the distance of the farthest point in the heap is less than the physical lower bound distance of the next group, an early termination mechanism is triggered.

7. The high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbed environment as described in claim 1, characterized in that, The objective cost function of the iterative error state Kalman filter is: ;in, For the error state increment, The pre-integration error covariance matrix is... Let be the Jacobian matrix of the geometric residual with respect to the pose error state. The geometric residual from the point cloud to the local plane, This is the observation covariance matrix after adaptive dilation.

8. A high-frequency odometry positioning system for a quadcopter UAV that adapts to disturbances, characterized in that, include: The data packaging and fusion module is configured to: acquire the data output by the IMU and the point cloud output by the solid-state lidar, and package the point cloud subframes and IMU data within the interval into an independent processing package based on the target time span threshold; The prior state continuous estimation module is configured to: maintain the nominal state vector, integrate using IMU data, calculate new states including high-frequency predicted pose and predicted velocity through motion prediction, simultaneously construct the error state Kalman filter framework, calculate the state error transfer matrix, and recursively update the pre-integrated error covariance matrix. The feature extraction module is configured to: for each point cloud subframe in the package, retrieve the high-frequency predicted pose corresponding to the subframe scanning time period, eliminate point cloud motion distortion through backpropagation, calculate the local curvature of the point cloud and filter points below the threshold, and obtain a set of planar feature points through voxel filtering downsampling. The environmental perception module is configured to: calculate the spatial adaptive matching search radius based on the pre-integrated error covariance matrix; perform nearest neighbor search and local plane fitting on the set of planar feature points using a voxel map; dynamically remove outliers; evaluate the confidence of geometric degradation and the confidence of physical perturbation to update the target time span threshold; and adaptively expand the observation covariance matrix through a comprehensive degradation factor. The map maintenance module is configured to: inject the expanded observation covariance matrix into the gain calculation of the iterative error state Kalman filter, solve for the optimal error state increment, compensate to the nominal state vector to correct the pose, maintain the voxel map, and output the odometry positioning results.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described in any one of claims 1-7.

10. A computer device comprising a computer-readable storage medium, a processor, and a computer program stored on the computer-readable storage medium and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the high-frequency odometry positioning method for adaptive adjustment of a quadcopter UAV under disturbance conditions as described in any one of claims 1-7.

Images