Adaptive control method and system for correcting UAV flight trajectory

Abstract

The present invention discloses an adaptively controlled UAV flight trajectory correction method and system, relating to the field of UAV flight control technology. The method comprises: collecting flight environment data through a sensor group carried by the UAV, constructing a three-dimensional environment model and dividing it into spatial units, determining the real-time posture of the UAV based on point cloud matching and posture estimation technology, predicting the trajectory deviation according to a preset flight path and generating flight trajectory deviation parameters, analyzing the trend of the deviation parameters to derive a three-axis compensation strategy for horizontal, vertical, and lateral deviations, and finally dynamically compensating and correcting the UAV flight trajectory based on the three-axis adjustment amount. The method solves the technical problem of low efficiency in real-time and accurate correction of the flight trajectory of existing UAVs in complex environments. By utilizing industrial automatic control system device manufacturing technology and intelligent monitoring devices, the method achieves the technical effect of shortening the dynamic compensation response time of the UAV flight trajectory deviation.

Description

Technical Field

[0001] The present application relates to the field of UAV flight control technology, and in particular to a method and system for adaptively controlling UAV flight trajectory correction. Background Art

[0002] Existing drones (UAVs) generally rely on GPS and inertial navigation systems for positioning and path tracking when flying in complex environments (such as dense obstacles and dynamic interference). When performing tasks such as terrain mapping, logistics distribution, and agricultural monitoring, UAVs often need to fly autonomously in complex and changing environments. Environmental uncertainties, such as obstacles and fluctuating wind direction, can cause them to deviate from their pre-set flight paths. Traditional trajectory correction methods, which rely heavily on GPS signals, are significantly less effective in areas with limited GPS signals, such as urban canyons and forests. Furthermore, current positioning methods that rely on GPS and inertial navigation systems are susceptible to signal obstruction, multipath effects, and accumulated sensor errors, making real-time correction of trajectory deviation difficult. Traditional methods suffer from response lag and a single compensation strategy in multi-sensor data fusion and dynamic obstacle avoidance, making them difficult to achieve high-precision adaptive trajectory correction. Improving the real-time trajectory correction capabilities and collaborative control efficiency of UAVs in complex environments, without relying on external navigation signals, and enabling autonomous adaptation to environmental changes and precise trajectory correction, has become an urgent technical challenge.

[0003] At present, relevant technologies still face the technical problem of low efficiency in real-time and accurate correction of UAV flight trajectories in complex environments. Summary of the Invention

[0004] This application solves the technical problem of low efficiency of real-time and accurate correction of flight trajectories of existing drones in complex environments by providing a method and system for correcting the flight trajectory of drones with adaptive control.

[0005] This application provides a method for adaptively controlling the flight trajectory of a UAV, including:

[0006] A flight environment data set is collected and acquired by a sensor device group carried by a target UAV, and three-dimensional modeling and unit division are performed based on the flight environment data set to obtain a flight environment space unit model; point cloud matching and posture estimation are performed on the current flight environment data based on the flight environment space unit model to determine the real-time posture of the UAV; a preset flight path is planned and determined, and a trajectory deviation prediction is performed on the real-time posture of the UAV according to the preset flight path to obtain a flight trajectory deviation parameter; trend judgment and strategy analysis are performed on the flight trajectory deviation parameter to obtain a three-axis compensation flight strategy, and the three-axis compensation flight strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy and a lateral offset adjustment strategy; the three-axis compensation flight strategy is used to analyze the real-time posture of the UAV and the flight trajectory deviation parameters to determine the three-axis adjustment amount of the flight trajectory, and the flight trajectory compensation correction of the target UAV is performed based on the three-axis adjustment amount of the flight trajectory.

[0007] This application provides an adaptively controlled UAV flight trajectory correction system, including:

[0008] A flight environment space unit model acquisition module, the flight environment space unit model acquisition module is used to collect and acquire a flight environment data set through a sensor device group carried by a target UAV, and perform three-dimensional modeling and unit division based on the flight environment data set to obtain a flight environment space unit model; a UAV real-time posture determination module, the UAV real-time posture determination module is used to perform point cloud matching and posture estimation on the current flight environment data based on the flight environment space unit model to determine the UAV real-time posture; a flight trajectory deviation parameter acquisition module, the flight trajectory deviation parameter acquisition module is used to plan and determine a preset flight path, and perform trajectory analysis on the UAV real-time posture according to the preset flight path. The invention also provides a three-axis compensation flight strategy acquisition module, which is used to perform trend judgment and strategy analysis on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, wherein the three-axis compensation flight strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy and a lateral offset adjustment strategy; a flight trajectory three-axis adjustment amount determination module, which is used to analyze the real-time posture of the UAV and the flight trajectory deviation parameters using the three-axis compensation flight strategy, determine the flight trajectory three-axis adjustment amount, and perform flight trajectory compensation correction on the target UAV based on the flight trajectory three-axis adjustment amount.

[0009] The adaptive control UAV flight trajectory correction method and system proposed in this application first collects flight environment data through the sensor group carried by the UAV, constructs a three-dimensional environment model and divides it into spatial units, determines the real-time posture of the UAV based on point cloud matching and posture estimation technology, predicts the trajectory deviation according to the preset flight path and generates the flight trajectory deviation parameters, analyzes the trend of the deviation parameters to parse the three-axis compensation strategy of the horizontal plane, vertical direction and lateral deviation, and finally performs dynamic compensation correction on the UAV flight trajectory according to the three-axis adjustment amount. Through the manufacturing technology of industrial automatic control system devices (such as dynamic analysis of the three-axis compensation strategy) and intelligent monitoring devices

[0010] (such as trajectory deviation detection by multi-sensor fusion), achieving the technical effect of shortening the dynamic compensation response time of UAV flight trajectory deviation. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] To more clearly illustrate the technical solutions of the embodiments of the present disclosure, the accompanying drawings of the embodiments of the present disclosure are briefly introduced below. Flowcharts are used in this application to illustrate the operations performed by the systems according to the embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed in precise order. Instead, various steps may be processed in reverse order or simultaneously as needed. Furthermore, other operations may be added to these processes, or one or more operations may be removed from these processes.

[0012] Figure 1 A schematic diagram of a flow chart of a method for correcting the flight trajectory of a UAV using adaptive control provided in an embodiment of the present application;

[0013] Figure 2 A schematic diagram of the structure of the adaptively controlled drone flight trajectory correction system provided in an embodiment of the present application.

[0014] Explanation of the reference numerals: flight environment space unit model acquisition module 10, UAV real-time posture determination module 20, flight trajectory deviation parameter acquisition module 30, three-axis compensation flight strategy acquisition module 40, flight trajectory three-axis adjustment amount determination module 50. DETAILED DESCRIPTION

[0015] The above description is only an overview of the technical solution of the present application. In order to more clearly understand the technical means of the present application, it can be implemented in accordance with the contents of the specification. In order to make the above and other purposes, features and advantages of the present application more obvious and easy to understand, the specific implementation methods of the present application are listed below.

[0016] In order to make the purpose, technical solutions and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings. The described embodiments should not be regarded as limiting this application. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0017] In the following description, reference is made to “some embodiments”, which describes a subset of all possible embodiments, but it will be understood that “some embodiments” may be the same subset or different subsets of all possible embodiments and may be combined with each other without conflict, and the terms “first\second” involved are merely used to distinguish similar objects and do not represent a specific ordering of the objects. The terms “including” and “having” and any variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or server that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or modules that are not clearly listed or that are inherent to these processes, methods, products or devices. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which this application belongs. The terms used herein are for the purpose of describing the embodiments of this application only.

[0018] The present application provides a method for correcting the flight trajectory of a UAV with adaptive control, such as Figure 1 As shown, the method includes:

[0019] Step S100 involves collecting a flight environment dataset using the sensor suite onboard the target UAV. Three-dimensional modeling and cell division are then performed based on this dataset to produce a spatial unit model of the flight environment. Specifically, while the target UAV is in flight, its onboard sensor suite, consisting of a lidar, visual sensor, ultrasonic sensor, and IMU, continuously collects flight environment data at a specific sampling frequency and through a time synchronization mechanism. This data is then transmitted in real time to a data processing unit or ground control station. The collected data undergoes preprocessing, integrating multi-sensor information through data fusion, applying filtering algorithms for noise reduction, and performing data registration to align the data from different sensors to the same coordinate system. Subsequently, the lidar point cloud data undergoes sequential filtering, segmentation, and feature extraction. A three-dimensional model is constructed using algorithms such as triangulation, voxelization, and surface fitting. This model is then optimized through smoothing, repair, and simplification. Next, the three-dimensional model is divided into cells using a selection of partitioning strategies, such as regular grid, adaptive, or object-based, based on the flight environment and mission requirements, taking into account factors such as cell size, shape, and location. Finally, the divided units are combined into a flight environment space unit model and stored using data structures such as octree and quadtree to provide support for autonomous flight and path planning of UAVs.

[0020] In one possible implementation, a flight environment dataset is collected by a sensor group onboard a target UAV. Three-dimensional modeling and unit division are performed based on the flight environment dataset to obtain a flight environment spatial unit model. Step S100 further includes step S110, where the flight environment dataset is classified and extracted according to the sensor group, thereby obtaining flight environment point cloud data and flight environment image data. While the UAV is in flight, the sensor group continuously collects data to form a dataset. The classification and extraction process is as follows: The sensor group typically includes a laser radar (LIDAR) and various cameras. The LIDAR uses the time-of-flight measurement principle to obtain distance information from surrounding objects, forming point cloud data. This data comes in various formats, such as binary or ASCII text. The system first identifies the format and then parses each packet according to the transmission protocol. By determining the scanning angle, distance value, and timestamp, it selects flight environment point cloud data that meets the requirements. The camera captures a two-dimensional image of the flight environment. Common image data formats include JPEG and PNG, each with a unique file header identifier. The system first identifies the format and then verifies the data integrity according to format specifications, such as by checking the quantization table of JPEG and the data block information of PNG. Only data that passes verification is extracted from the dataset as valid flight environment image data.

[0021] Step S120 triangulates and reconstructs Poisson surfaces on the flight environment point cloud data to generate an initial three-dimensional model of the flight environment. Specifically, to convert the flight environment point cloud data into the initial three-dimensional model, triangulation and Poisson surface reconstruction must be performed in sequence. The original point cloud data is discrete and unstructured, so it is first preprocessed using statistical filtering to remove noise and voxelization to reduce redundant points. Then, using the Delaunay triangulation algorithm, three points are selected to create an initial triangle. The remaining points are inserted point by point according to the empty circumcircle criterion. If the new point is within the triangle circumcircle, the triangle is reconstructed. This process is repeated until all points are included in the mesh, forming a triangular patch mesh structure. However, the surface of the triangulated mesh is not smooth and has holes. Therefore, the Poisson surface reconstruction stage is entered. The point cloud normal vector is first calculated using the K-nearest neighbor algorithm combined with principal component analysis, and the direction of the normal vector is unified using the propagation algorithm. The triangulated mesh and normal vector are then used as input to construct the Poisson equation, which is solved using numerical methods such as the multigrid method. Parameters are adjusted based on the point cloud data distribution and normal vector information to fill in the holes and smooth the surface. Finally, a continuous, smooth, and realistic three-dimensional model of the initial flight environment is generated.

[0022] Step S130 , mapping the flight environment image data to the initial flight environment three-dimensional model for enhancement and optimization to obtain a target flight environment three-dimensional model. Specifically, to obtain the target flight environment 3D model, the flight environment image data must be matched and mapped to the initial flight environment 3D model and enhanced and optimized. First, the scale-invariant feature transform (SIFT) algorithm is applied to the flight environment image data. By constructing a scale space, using the difference of Gaussian (DoG) operator to detect extreme points and calculate the gradient direction histogram, feature points and descriptors are obtained. At the same time, the geometric and topological structural features of the triangular patches are extracted on the initial 3D model. Then, using metrics such as Euclidean distance and a KD tree to perform a fast nearest neighbor search, the feature descriptions of the two are compared to achieve feature matching. Then, based on the matching results, the connection between the image and the 3D model is established. The transformation matrix is ​​obtained by calculating the coordinate transformation relationship. The image points are then mapped to the 3D model surface. Bilinear interpolation is used to obtain texture color values ​​from the image and fill them into the triangular patches for texture mapping. Finally, the mapping parameters are adjusted, and image data from different angles are fused using image fusion technology. The Laplace smoothing algorithm is used for smoothing. The high-frequency details of the image are analyzed to enhance the model details and highlight the detailed features, so that the target flight environment 3D model more realistically and accurately reflects the actual flight environment.

[0023] Step S140 determines the unit division type and density based on the flight trajectory analysis requirements. Specifically, the flight trajectory analysis objectives and data characteristics are clarified, and analysis purposes such as monitoring activity hotspots and assessing flight stability are sorted out. The trajectory data's location, timestamp, flight status parameters, time span, and accuracy are understood. The unit division type is then determined. Spatial divisions include rectangular, hexagonal, and irregular polygonal (used for special areas, generated by GIS based on regional boundaries and using raycasting to determine aircraft position). Temporal divisions include fixed time intervals (dividing by fixed duration facilitates analysis of flight activity dynamics and statistical analysis to generate time series graphs) and event-driven (dividing by specific flight events to specifically study trajectory characteristics at key stages). Finally, the unit division density is determined, taking into account the analysis accuracy requirements (higher accuracy corresponds to higher division density), the trade-off between data volume and computing resources (which can be achieved through methods such as data sampling or distributed computing), and reference to actual application scenarios (different scenarios have different requirements). Through comprehensive analysis and repeated trials and adjustments, a unit division type and density that balances these factors is determined.

[0024] Step S150, divide the three-dimensional model of the target flight environment into spatial units according to the unit division type and unit division density to obtain the flight environment spatial unit model. Specifically, after obtaining the three-dimensional model of the target flight environment, the flight environment spatial unit model is constructed according to the established unit division type and density. First, clarify the basis for division, review the unit division type and density details, and at the same time, load the target flight environment three-dimensional model data into the analysis system in its entirety. Then, operate according to different division types: when dividing a rectangular grid, a grid is constructed in three-dimensional space within the model geographic coordinate range according to the set side length, and the ownership is determined by comparing the coordinates of the model vertices, triangles and other elements with the grid boundary coordinates; when dividing a hexagonal grid, the layout must be designed first, and hexagons are constructed with specific starting points and side lengths and arranged in a honeycomb shape. Geometric algorithms such as the vector method are used to determine whether the model elements are within the hexagon; when dividing an irregular polygon, pre-defined boundary data must be imported, and spatial analysis algorithms such as the ray method are used to filter out the model elements within the polygon. Finally, after the ownership determination is completed, a data structure containing corresponding model elements and attribute information is constructed for each spatial unit, integrated to form a flight environment spatial unit model framework, and verified through visualization and other means. If there are any problems, the parameters or algorithms are adjusted in time to ensure the accuracy and completeness of the model.

[0025] Step S200: Based on the flight environment spatial unit model, point cloud matching and pose estimation are performed on the current flight environment data to determine the real-time pose of the UAV. Specifically, to determine the real-time pose of the UAV based on the flight environment spatial unit model, data preparation is first performed, the flight environment spatial unit model is processed, the data integrity is ensured, and spatial index structures such as KD trees are constructed. At the same time, the current flight environment data is collected and pre-processed by denoising, downsampling (for lidar point clouds), image enhancement, and distortion correction (for visual sensor data). Then, point cloud matching is performed to extract local surface features such as normal direction and curvature from the current and model point clouds. The iterative closest point (ICP) algorithm is used to find the nearest neighbor points in the model point cloud using some points of the current point cloud as the initial set. The two sets of point clouds are aligned by continuously iteratively calculating the transformation matrix until the mean square error reaches a threshold. Finally, the pose estimation is performed, and the real-time pose parameters of the UAV are parsed based on the transformation matrix obtained from point cloud matching. The extended Kalman filter (EKF) algorithm is then used to fuse the inertial measurement unit (IMU) data with the point cloud matching pose information, and continuous corrections are made to obtain a more accurate and stable real-time pose of the UAV.

[0026] In one possible implementation, point cloud matching and pose estimation are performed on the current flight environment data based on the flight environment space unit model to determine the real-time pose of the UAV. Step S200 further includes step S210, in which key feature points are extracted from the flight environment space unit model and the current flight environment data, respectively, to obtain a space model key point set and a current flight environment key point set. Specifically, the space model key point set and the current flight environment key point set are obtained, and key feature points are extracted from the flight environment space unit model and the current flight environment data, respectively. For the flight environment space unit model, its data structure and characteristics are first understood, and noise and missing data are preprocessed. Then, the Harris corner detection algorithm (judged by calculating the eigenvalues ​​of the local autocorrelation matrix of the point cloud) or the curvature-based method (screened by calculating the curvature based on the local fitting plane) can be selected to traverse the point cloud data and collect the detected key feature points to form a space model key point set. For the current flight environment data, the lidar point cloud data needs to be denoised and voxel filtered, and the image data needs to be preprocessed such as image enhancement. Then the image data can use the SIFT algorithm (constructing a scale space, using the DoG operator to detect extreme points and generate feature descriptors), and the lidar point cloud data uses an algorithm based on local geometric features (such as calculating the normal direction, etc.) to organize the extracted key feature points to form the current flight environment key point set.

[0027] Step S220 sequentially performs multidimensional feature descriptions on the spatial model key point set and the current flight environment key point set to obtain a spatial model point cloud feature description subset and a current flight environment point cloud feature description subset. Specifically, multidimensional feature descriptions are performed on the spatial model key point set and the current flight environment key point set to obtain corresponding subsets. The specific process is as follows: For the spatial model key point set, the geometric features of each key point are first calculated, such as by using PCA to determine the normal direction within the neighborhood and calculating the curvature based on the plane fitting of the neighborhood points; then, the topological relationship with the neighboring points is analyzed, such as the position within the unit and the connection status with other feature points. The features such as the normal direction, curvature, and topological relationship are then integrated into a multidimensional vector to form the spatial model point cloud feature description subset. For the current flight environment keypoint set, if image data exists, the SIFT algorithm is applied to the image keypoints. A window is created within their neighborhood, and the pixel gradient magnitude and direction are calculated. The subregion gradient direction histogram is then statistically analyzed to generate a 128-dimensional feature vector. For lidar point cloud keypoints, in addition to the algorithm's line direction and curvature, the distance from the point to the radar origin and the uniformity of the neighborhood point distribution are also considered. Finally, the feature vectors of the image and point cloud keypoints are integrated. If both are present, they are appropriately merged to construct a subset of the current flight environment point cloud feature description.

[0028] Step S230, using the ICP algorithm to perform point cloud matching calculations on the spatial model key point set and the current flight environment key point set according to the spatial model point cloud feature description subset and the current flight environment point cloud feature description subset, to obtain a set of matching spatial feature points. Specifically, the ICP algorithm is used to perform point cloud matching calculations on the spatial model key point set and the current flight environment key point set to obtain a set of matching spatial feature points. The process is as follows: first, unify the data format and set the initial transformation matrix T0 as the unit matrix. Then, calculate the similarity based on the feature description subset to determine the potential corresponding point pairs, and use the distance threshold to filter out the reliable corresponding point pair set P. Then define the error function E(T) to measure the matching error, and use the least squares method to solve the transformation matrix T that minimizes E(T). opt Then use T opt Transform the current flight environment key point set to determine whether the iteration termination condition is met (the error is less than E thresh Or reach the maximum number of iterations N max If the condition is not met, the operations of searching for corresponding point pairs and calculating the transformation matrix are repeated. After the termination condition is met, the final corresponding point pairs are determined and combined to form a set of matching spatial feature points, which provides a data basis for subsequent pose estimation.

[0029] Step S240 performs Kalman filtering and pose estimation based on the set of matched spatial feature points to determine the real-time pose of the drone. Specifically, the real-time pose of the drone is determined using the set of matched spatial feature points. Kalman filtering is first performed. The drone's pose is set as the system state, comprising three-dimensional position and attitude angles. A state transition model is constructed based on the drone's uniform motion characteristics to describe its state changes from one moment to the next, while accounting for the effects of process noise. An observation model is established based on the set of matched spatial feature points, and the obtained relative position and attitude information of the drone is used as the observation value, accounting for observation noise. Through iterative calculations of prediction and update, the current state and covariance are first predicted based on the previous moment's state and the state transition model. The Kalman gain is then calculated using the observation value, and the state estimate and covariance are then updated to optimize the drone's pose estimate. After filtering, the position and attitude information are extracted from the final state estimate. This information represents the real-time pose of the drone and is output to flight control, navigation, and other systems for adjusting flight attitude, planning paths, and ensuring stable and safe flight of the drone.

[0030] In one possible implementation, Kalman filtering and pose estimation are performed based on the set of matching spatial feature points to determine the real-time pose of the drone. Step S240 further includes step S241, initializing the Kalman filter based on the initial flight state of the target drone and the noise characteristic information of the sensor device group. Specifically, when initializing the Kalman filter, the initial flight state of the target drone is first determined. Accurate position information such as longitude, latitude, and altitude is obtained through a high-precision GPS module and a barometric altimeter. The gyroscope and magnetometer in the inertial measurement unit (IMU) are used to determine attitude information such as heading angle, pitch angle, and roll angle. This data serves as the input for the filter's initial state estimation. At the same time, the noise characteristics of the sensor device group are analyzed. The mean and standard deviation of the static data collected by the accelerometer are calculated and the power spectral density is analyzed. The gyroscope is subjected to spectrum analysis and Allan variance analysis to construct a noise covariance matrix. Finally, the initial flight state is set as the filter's initial state vector. The process noise and observation noise covariance matrices are reasonably configured according to the noise covariance matrix to complete the initialization.

[0031] Step S242: Fitting the UAV's flight state based on the flight pose state data in the flight environment dataset is performed to construct a UAV dynamics model and obtain flight state sequence estimates. The pose state data in the flight environment dataset is first processed. Data is collected from sensors such as the GPS and IMU and organized sequentially. Gaussian filtering is used to remove noise and threshold filtering is used to eliminate outliers. A spline curve is then used to fit the flight state, and parameters are determined using the least squares method. The mean square error is evaluated and adjustments are made if the results are poor. Based on dynamic principles, differential equations are constructed using Newton's second law and the law of angular momentum, taking into account forces and torques such as gravity and air resistance. Parameters such as mass and moment of inertia are determined through manual analysis, experiments, and optimization algorithms. Finally, using the initial pose of the fitted data as the model's initial condition, the fourth-order Runge-Kutta method is used to solve the equations according to the time step to obtain flight state sequence estimates. These estimates are then compared with the actual values, and the Kalman filter parameters are optimized using the difference.

[0032] Step S243, based on the flight posture state data and the flight state sequence estimation value, the Kalman filter is subjected to covariance prediction and iterative update to obtain a Kalman optimization filter. Specifically, the Kalman filter is optimized based on the flight posture state data and the flight state sequence estimation value. First, the UAV state transition model x is determined. k =F k x k-1 +B k u k +w k , estimate x' based on the state at time k-1 k-1k-1 Values ​​and covariance matrix P k-1k-1 , calculate the estimated value x' of the predicted state at time k kk-1 With the covariance matrix Pkk-1 Then establish the observation model Z k =H k x k +u k , based on which the Kalman gain k is calculated k , and then update the state estimate x' at time k kk With the covariance matrix P kk By repeating the above process with the new result as the initial value, the performance of the Kalman filter is improved after multiple iterations, and the Kalman optimization filter is finally obtained, which provides accurate state estimation for UAV flight control and navigation.

[0033] In step S244, the Kalman optimization filter is used to estimate the pose state of the matching spatial feature point set to determine the real-time pose of the UAV. Specifically, when using the Kalman optimization filter to determine the real-time pose of the UAV, the matching spatial feature point set is first input as observation data. The filter predicts the current pose prior value based on the state transition equation and the pose estimate value at the previous moment, and at the same time converts the feature point set into an observation value through the observation model. The Kalman gain is then calculated based on the prediction covariance and observation covariance matrix to fuse the predicted value and the observed value, correct the prior estimate value, and obtain an accurate real-time pose. This process is continuously cycled, and the pose estimate is dynamically adjusted with the help of the new feature point set to provide key and accurate data for flight control, etc.

[0034] In one possible implementation, the Kalman filter is subjected to covariance prediction and iterative updates based on the flight posture state data and the flight state sequence estimate to obtain a Kalman optimization filter. Step S243 further includes step S2431, obtaining a flight state sequence observation value based on the flight posture state data. Specifically, the drone relies on onboard sensors such as GPS and IMU to obtain flight posture state data. GPS provides location information such as longitude, latitude, and altitude, with an accuracy of meters or even sub-meters under ideal conditions, collecting data multiple times per second. The IMU measures attitude information such as heading, pitch, and roll angles using a gyroscope and accelerometer. The gyroscope can accurately measure angular rate changes of ±2000° / s per second, with an acquisition frequency of several hundred hertz. Because different sensors may have different acquisition times, time synchronization is first performed, and then the calibrated data is organized in chronological order. The GPS position data at each moment and the IMU attitude data at the same moment are combined into a single data point. These chronologically arranged data points constitute the flight state sequence observation value, which is an important foundation for subsequent data analysis and state estimation.

[0035] Step S2432, based on the error between the flight state sequence observation value and the flight state sequence estimation value, covariance prediction is performed to generate an observation error covariance matrix and a prediction error covariance matrix respectively. Specifically, the flight state sequence observation value is derived from the integration of drone sensor data, and the flight state sequence estimation value is calculated through a dynamic model. When calculating the error, it is necessary to calculate the difference between the observation value and the estimation value in the position (longitude, latitude, altitude) and attitude (heading angle, pitch angle, roll angle) dimensions at each time point, and these differences constitute the error data. When generating the observation error covariance matrix, taking the position error as an example, the longitude, latitude, and altitude error data at different times are collected, and the covariance between the two is calculated using statistical methods to form the position-related part in the matrix. The same is true for the attitude error. This matrix reflects the correlation and discreteness of the errors in each dimension of the observation data. When generating the prediction error covariance matrix, based on the state transition model, combined with the state covariance matrix of the previous moment, the matrix of the current moment is calculated through the state transition matrix transformation and considering the process noise covariance. It reflects the degree of uncertainty in the prediction process. The larger the value, the higher the prediction uncertainty, and the need to optimize the model or parameters.

[0036] Step S2433, based on the Kalman filter, the difference gain calculation is performed on the observation error covariance matrix and the prediction error covariance matrix to obtain Kalman gain information. Specifically, the Kalman filter takes the observation error covariance matrix that reflects the correlation and dispersion of the observation data errors and the prediction error covariance matrix that reflects the uncertainty of the prediction based on the state transition model as input, and uses the matrix operation rules to perform the difference gain calculation according to its algorithm rules. )(H k is the observation matrix, P kk-1 is the forecast error covariance matrix, R k is the observation noise covariance matrix), and This matrix multiplication operation, which comprehensively considers multiple factors, ultimately yields the Kalman gain information. This gain is a weight coefficient matrix that dynamically adjusts the ratio of observed to predicted values ​​when updating the state estimate based on the two covariance matrices. When the observation error covariance matrix is ​​small, it relies more on the observation value, while when the prediction error covariance matrix is ​​small, it tends to favor the predicted value, thereby achieving a more accurate estimate of the drone's state.

[0037] Step S2434: Iteratively optimize and update the Kalman filter according to the Kalman gain information to obtain the Kalman optimized filter. Specifically, after obtaining the Kalman gain information, the Kalman filter is iteratively optimized and updated. The Kalman gain is used to fuse the flight state sequence observations and estimates. Specifically, the Kalman gain is multiplied by the difference between the observation and the value based on the predicted state estimate after transformation by the observation matrix, and then added to the predicted state estimate, thereby obtaining a more accurate state estimate. Simultaneously, the filter's internal state covariance matrix is ​​updated by subtracting the product of the Kalman gain and the observation matrix from the identity matrix, and then multiplying the result by the prediction error covariance matrix. This ensures that the state covariance matrix more accurately reflects the uncertainty of the current state estimate. This iterative optimization and update is a continuous cycle. With each new flight state sequence observation, the covariance prediction, difference gain calculation, and iterative optimization and update must be repeated. As the number of iterations increases, the Kalman filter continuously adjusts itself, gradually improving the accuracy and reliability of the drone's flight state estimation. After multiple iterations, the performance of the Kalman filter is greatly improved, and it can better adapt to the complex flight environment of the UAV and provide more accurate and reliable state estimation data for flight control, navigation, etc. The filter at this time is the Kalman optimization filter.

[0038] Step S300 plans and determines a preset flight path. The real-time position of the UAV is then predicted for trajectory deviation according to the preset flight path to obtain flight trajectory deviation parameters. Specifically, by clarifying the UAV's mission requirements (e.g., surveying requires consideration of regional scope and accuracy, while logistics distribution requires consideration of starting and delivery points and time constraints), geographic information of the flight area is collected. Digital elevation and surface models are generated using GIS data and satellite imagery. An appropriate algorithm, such as Dijkstra, A*, or a rapidly exploring random tree, is then selected. The starting and target positions and obstacle constraints are input to produce a preset flight path consisting of a series of waypoints containing latitude, longitude, and altitude. Next, the UAV uses sensors such as GPS and IMU to frequently collect real-time position information. A motion model with external interference terms is established based on Newtonian mechanics and dynamic characteristics. Model-based prediction is used, using the real-time position as the initial condition to input into the model to calculate the difference between the real-time position and the preset path. Alternatively, a data-driven approach is employed, using a large amount of historical data from similar flight conditions to train the model using algorithms such as neural networks. The real-time information is then input into the model to predict trajectory deviation. Finally, the flight trajectory deviation parameters, including position (longitude, latitude, and altitude deviation) and attitude (heading angle, pitch angle, and roll angle deviation), are defined and calculated, and output to the flight control system for adjusting power and attitude to return the drone to the preset path. These parameters are also used to record and analyze flight performance, providing a basis for subsequent optimization and improvement.

[0039] In one possible implementation, a preset flight path is planned and determined, and a trajectory deviation prediction is performed on the real-time posture of the UAV according to the preset flight path to obtain a flight trajectory deviation parameter. Step S300 further includes step S310, in which a flight state prediction is performed on the real-time posture of the UAV based on the Kalman optimization filter to obtain a flight posture prediction sequence value at multiple preset moments. Specifically, a Kalman optimization filter is used to predict the real-time flight state of a drone. To obtain a sequence of predicted flight pose values ​​at multiple preset moments, the filter must first be initialized. High-precision GPS and IMUs are used to determine the initial state vector, which includes the initial position (longitude, latitude, and altitude) and initial attitude (heading, pitch, and roll). Based on long-term monitoring and analysis of sensor performance and the flight environment, a process noise covariance matrix, reflecting uncontrollable flight factors, and an observation noise covariance matrix, reflecting sensor measurement errors, are then set. After initialization, real-time pose data, collected several times per second by the GPS and several hundred hertz by the IMU, is continuously fed into the filter. The filter iteratively operates using an internal algorithm, first predicting the current state based on the previous state estimate and a process model constructed based on the drone's dynamic characteristics. The filter then fuses the sensor observations and predictions with the Kalman gain, whose weights are dynamically adjusted based on the covariance matrix of the observation and prediction errors. Through continuous iteration, the drone's position and attitude at user-preset moments in the future, such as 1 second, 2 seconds, and 3 seconds, are calculated and arranged in sequence to form a sequence of flight pose predictions, providing a key data foundation for subsequent analysis.

[0040] Step S320, generates flight prediction trajectory information based on the flight posture prediction sequence value. Specifically, after obtaining the flight posture prediction sequence value, first extract the position coordinates such as longitude, latitude and altitude corresponding to each preset time from it. These coordinates are the basis for constructing the trajectory. Because different application scenarios have different requirements for the coordinate system, coordinate conversion may be required, such as using map projection to convert geographic coordinates into a plane rectangular coordinate system to adapt to local area applications. Afterwards, the extracted and converted position coordinates are connected in chronological order to preliminarily generate a flight prediction trajectory. However, due to sensor noise and prediction uncertainty, the trajectory will fluctuate, so a common smoothing algorithm such as the moving average method is used to average the coordinates of several points before and after each point on the trajectory and replace the original coordinates to reduce fluctuations, thereby generating smoother and more accurate flight prediction trajectory information.

[0041] Step S330, based on the preset flight path, the vector difference calculation of the flight prediction trajectory information is performed to obtain the flight trajectory deviation parameter. Specifically, based on the preset flight path, the vector difference calculation of the flight prediction trajectory information is performed to obtain the flight trajectory deviation parameter. First, the preset flight path (composed of a series of waypoints containing latitude, longitude, and altitude arranged in order) and the flight prediction trajectory (composed of position points at different times predicted by the Kalman optimization filter) are discretized into line segments between adjacent points, and correspond one to one according to the time or spatial position relationship, such as P1, P2, P3 of the preset path and Q1, Q2, Q3 of the predicted trajectory. Then construct a vector, the adjacent points P on the preset path i With P i+1 Composition vector Its modulus is calculated using the distance formula between two points in three-dimensional space, and the corresponding adjacent point Q on the flight prediction trajectory is i With Q i+1 Composition vector The modulus is calculated in the same way. Then the directional deviation is calculated, and the vector dot product formula is used to calculate the cosine value of the angle between the preset path vector and the predicted trajectory vector. The angle is then obtained through the inverse trigonometric function. This angle is the directional deviation, with a positive value indicating a clockwise deviation and a negative value indicating a counterclockwise deviation. The specific deviation value is then calculated, and the difference in the coordinates of the corresponding vector endpoint in the x, y, and z axes is calculated. The Euclidean distance formula is then used to calculate the comprehensive deviation value, which intuitively reflects the size of the position deviation. Finally, the directional deviation and specific deviation values ​​of each pair of corresponding vectors are summarized and sorted by flight time or path point order to form a flight trajectory deviation parameter set, which can be used for real-time adjustment of drone flight control, flight performance evaluation, and path planning optimization.

[0042] Step S400 involves trend analysis and strategy analysis of the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy. This strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral deviation adjustment strategy. Specifically, after obtaining the flight trajectory deviation parameters, they are first organized by time or path point sequence, smoothed using a moving average method, and then analyzed by linear regression to determine trends. This identifies trends in the directional deviation and the specific horizontal, vertical, and lateral deviation values, such as whether the directional deviation consistently increases or decreases, or whether the vertical deviation increases over time. Strategy analysis is then performed to formulate a three-axis compensation flight strategy: the horizontal plane adjustment strategy adjusts the heading angle and horizontal velocity vector components based on the directional and horizontal position deviation trends; the vertical direction adjustment strategy adjusts the ascent or descent rate and fine-tunes the attitude based on the vertical position deviation trend; and the lateral deviation adjustment strategy is implemented by controlling mechanisms such as the ailerons based on the lateral deviation trend. Finally, these three strategies are integrated and verified using computer simulations to verify their effectiveness in correcting deviations under different flight conditions. Strategy parameters are optimized based on the simulation results to ensure that the strategies are coordinated and can stably and accurately return the drone to the preset path.

[0043] In one possible implementation, trend determination and strategy analysis are performed on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, which includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy. Step S400 further includes step S410, determining the changing trend of the flight trajectory deviation parameters according to a time series to determine trajectory deviation trend information. Specifically, to determine the changing trend of the flight trajectory deviation parameters according to a time series and determine trajectory deviation trend information, it is necessary to first arrange the parameters covering the specific deviation values ​​of the directional deviation, horizontal, vertical, and lateral offsets in chronological order. Because sensors are susceptible to noise interference during flight, resulting in erroneous data, it is necessary to eliminate extremely abnormal data points, such as those that suddenly appear in the vertical direction, and to standardize parameters of different units for easier comparison. Next, trend analysis methods are applied, such as the moving average method (using horizontal deviation as an example, a five-time window is set, and the horizontal deviation values ​​of the current and previous four time points are accumulated and divided by 5 to smooth the data) to highlight long-term trends. For deviations with complex trends (such as vertical deviation), polynomial fitting is used (using the least squares method, assuming a quadratic polynomial relationship with time, and analyzing the rate of change of the deviation and the changes reflected by the function coefficients). Finally, the trends of the deviation parameters in each direction are summarized, such as the possibility that the directional deviation may continue to increase clockwise, the horizontal deviation will first increase slowly and then stabilize, and the vertical deviation will first increase and then decrease towards the preset value after takeoff. This will form complete trajectory deviation trend information, providing a basis for subsequent strategy formulation.

[0044] Step S420 analyzes the three-axis adjustment strategies based on the trajectory deviation trend information to obtain horizontal, vertical, and lateral offset adjustment strategies. Specifically, after obtaining the trajectory deviation trend information, the three-axis adjustment strategy analysis is performed. For the horizontal adjustment strategy, if the directional deviation continues to increase clockwise, the drone is instructed to rotate counterclockwise by a corresponding angle based on the severity of the deviation. For example, if the clockwise deviation increases by 1 degree per second over the past 10 seconds, the drone may be instructed to rotate counterclockwise by 5 degrees. Furthermore, if the horizontal position deviation gradually increases in a certain direction (e.g., eastward), the component of the velocity vector in the horizontal plane is adjusted, changing the original speed of 5 meters per second eastward and 0 meters per second westward to 3 meters per second eastward and 1 meter per second westward, thereby keeping the drone closer to the preset path in the horizontal plane. Regarding the vertical adjustment strategy, if the vertical deviation continues to increase (i.e., the actual altitude exceeds the preset altitude and the difference becomes larger), the ascent rate may be reduced or the descent rate may be increased. The pitch angle is adjusted to assist in control. When the deviation is large, the pitch angle is increased to slightly downward, while when the deviation is negative, the pitch angle is decreased to slightly upward. In terms of lateral offset adjustment strategy, if the lateral deviation continues to increase to the left, the right aileron deflection angle is appropriately increased to generate a rightward force to push the drone back on track. Based on the size and speed of the lateral deviation, the aileron is adjusted quickly and significantly when the deviation is rapid and large, and slowly and slightly when the deviation is slow, so as to achieve precise lateral offset adjustment.

[0045] Step S430 determines the three-axis compensation flight strategy based on the horizontal plane adjustment strategy, the vertical direction adjustment strategy, and the lateral offset adjustment strategy. Specifically, to determine the three-axis compensation flight strategy, it is necessary to first integrate the horizontal plane, vertical direction, and lateral offset adjustment strategies, and construct a comprehensive control model to coordinate the operation of each strategy. For example, when adjusting the heading angle and velocity vector components in the horizontal plane, it does not affect the vertical and lateral adjustments. When adjusting the vertical direction adjustment rate and the pitch angle and the lateral aileron deflection angle, they also cooperate with each other to avoid conflicts. The model simultaneously outputs adjustment instructions for the three directions to form a strategy framework. Then, before actual application, with the help of computer simulation, different initial deviations, wind speeds, and flight environments are set. The results of the drone flying according to this strategy are observed, and the effect, stability, and flight performance of its return to the preset path are evaluated. Then, based on the simulation results, the strategy parameters are optimized, such as adjusting the excessive heading angle adjustment amplitude and unreasonable vertical direction adjustment rate. After repeated simulation and optimization, the strategy is adapted to complex scenarios and deviations, ensuring that the drone flies accurately and stably along the preset path.

[0046] Step S500: Analyze the real-time posture of the UAV and the flight trajectory deviation parameters using the three-axis compensation flight strategy to determine a three-axis flight trajectory adjustment, and then perform flight trajectory compensation correction on the target UAV based on the three-axis flight trajectory adjustment. Specifically, when analyzing the real-time posture of the UAV and the flight trajectory deviation parameters using the three-axis compensation flight strategy, each sub-strategy in the strategy is first associated with the real-time posture (position and attitude information) of the UAV obtained by sensors such as GPS and IMU. For example, the horizontal plane adjustment strategy combines the real-time heading angle to determine the target heading angle, while also combining the flight trajectory deviation parameters. For example, the vertical direction adjustment strategy determines the ascent or descent rate adjustment value based on the vertical position deviation, and the lateral deviation adjustment strategy determines the aileron deflection angle adjustment value based on the lateral deviation. Then, the three-axis flight trajectory adjustment values ​​are determined. In the horizontal plane, the heading angle adjustment value and the longitude and latitude velocity component adjustments are calculated. In the vertical plane, the ascent or descent rate adjustment value and the pitch angle adjustment value are determined. In the lateral deviation direction, the aileron deflection angle adjustment value and the roll angle adjustment value are calculated. Finally, based on these adjustment values, flight trajectory compensation correction is performed, and the adjustment values ​​are converted into adjustment instructions for the power and attitude control systems. These instructions are transmitted to the actuator through the internal communication link of the flight control system. The actuator adjusts the flight status of the drone in real time accordingly. During the process, the sensors monitor and feedback information in real time, and the flight controller continuously adjusts the adjustment values ​​accordingly to ensure that the drone flies accurately and stably along the preset path.

[0047] In one possible implementation, the three-axis compensation flight strategy is used to analyze the real-time posture of the UAV and the flight trajectory deviation parameters to determine a three-axis flight trajectory adjustment. Flight trajectory compensation correction is then performed on the target UAV based on the three-axis flight trajectory adjustment. Step S500 further includes step S510, where the horizontal plane adjustment strategy is used to perform deviation compensation and dynamic obstacle avoidance analysis on the real-time posture of the UAV and the flight trajectory deviation parameters to determine the horizontal plane trajectory adjustment. Specifically, when determining the horizontal plane trajectory adjustment, the UAV's GPS and IMU sensors first acquire real-time posture information, such as longitude, latitude, and heading angle. Flight trajectory deviation parameters, such as directional deviation and horizontal position deviation, are also acquired and correlated to assess the difference between the actual and preset states. When performing the deviation compensation analysis, for directional deviation, if the deviation is clockwise, the strategy determines a counterclockwise rotation angle based on multiple factors, and the heading is adjusted by controlling the motor speed difference. For horizontal position deviation, a velocity component adjustment is calculated based on the longitude and latitude deviations, and the motor output power is controlled to change the movement speed to compensate. When implementing dynamic obstacle avoidance analysis, obstacles are detected and identified with the help of visual sensors and millimeter-wave radars, and the position, speed and other information they provide are integrated. Combined with the strategy, if the obstacle is on the right side in front, the drone is instructed to slightly adjust the heading angle to the left and reduce speed to avoid the obstacle. During obstacle avoidance, continuous monitoring is carried out and the strategy is adjusted based on real-time information. Finally, the results of deviation compensation and dynamic obstacle avoidance are combined to determine the horizontal plane trajectory adjustment plan including adjustment quantities such as heading angle and horizontal velocity component.

[0048] Step S520 analyzes the drone's real-time posture and the flight trajectory deviation parameters using pressure data and time-of-flight (TOF) sensor data, and performs altitude lock compensation based on the vertical adjustment strategy to determine a vertical trajectory adjustment amount. Specifically, to determine the vertical trajectory adjustment amount, the drone uses sensors to collect altitude, vertical position deviation, as well as pressure and time-of-flight (TOF) sensor data in real time. The drone analyzes its vertical flight status according to the vertical adjustment strategy. For example, at a given moment, the drone can determine the relevant data and preliminarily determine whether the flight status is normal and the degree of deviation. Because pressure sensors are susceptible to fluctuations in temperature and pressure, and TOF sensors can exhibit deviations in complex environments, the two data types must be compared and fused by setting accuracy weights to determine a calibration altitude, providing a reliable basis for altitude lock compensation. If the vertical deviation indicates an actual altitude exceeding a preset altitude, the ascent rate is reduced or the descent rate is increased according to the strategy. For example, the current ascent rate is subtracted from the product of a coefficient and the deviation to determine the adjustment rate. Simultaneously, an algorithm is used to calculate the pitch angle adjustment. When the altitude exceeds the preset altitude, the pitch angle is increased, and when it exceeds the preset altitude, the pitch angle is decreased. This achieves precise control and correction of the vertical flight trajectory, ultimately determining a vertical trajectory adjustment plan that includes both the rate and pitch angle adjustments.

[0049] In step S530, the lateral offset adjustment strategy is used to perform PID control compensation on the real-time position and trajectory deviation parameters of the drone, thereby obtaining a lateral trajectory adjustment. Specifically, the drone uses sensors such as GPS and IMU to obtain real-time position and posture. The GPS longitude and latitude are combined with a map and a preset path to clearly define the horizontal position. The IMU measures attitude data to assist in analyzing lateral offset and simultaneously collects lateral offset deviations, such as the distance of east-west deviation when a north-south path is preset. Because sensor data is susceptible to interference, it requires algorithmic processing, such as using Kalman filtering to denoise GPS data, calibrating and filtering IMU data, and integrating them to obtain accurate data for PID control compensation. The PID control algorithm calculates the control variable based on the deviation between the drone's lateral position and its ideal position perpendicular to the preset path. The proportional component generates a proportional control signal based on the magnitude of the deviation. A positive deviation forces the drone to steer leftward, and the proportional coefficient influences response speed and stability. The integral component accumulates past deviations to eliminate steady-state errors, allowing the drone to accurately return to the preset path. The integral coefficient determines the speed and degree of error elimination. The differential component adjusts the control variable in advance based on the predicted rate of change of the deviation, ensuring a smooth system response. The differential coefficient determines the sensitivity to the rate of change. The real-time lateral deviation is input into the PID controller. The total control variable calculated through various components is used to adjust the aileron deflection angle. If the control variable is positive, the right aileron deflection angle is increased, shifting the drone leftward and minimizing the deviation. After the drone adjusts, sensors monitor the new position and deviation, feeding them back to the PID controller. Based on this feedback, the control variable is reduced to prevent overshoot if the deviation decreases, while it is increased to accelerate adjustment if the deviation increases. Through continuous feedback optimization, the appropriate lateral trajectory adjustment is determined to ensure accurate and stable flight along the preset path and eliminate lateral deviation.

[0050] Step S540, based on the horizontal plane trajectory adjustment amount, the vertical trajectory adjustment amount and the lateral trajectory adjustment amount, the three-axis adjustment amount of the flight trajectory is determined. Specifically, to determine the three-axis adjustment amount of the flight trajectory, it is necessary to first integrate and analyze the adjustment amounts of each dimension. Among them, the horizontal plane trajectory adjustment amount includes the heading angle adjustment amount, which corrects the flight direction deviation by controlling the motor speed difference; and the horizontal speed component adjustment amount, which adjusts the horizontal movement speed according to the position deviation in the longitude and latitude directions. The vertical trajectory adjustment amount covers the ascent or descent rate adjustment amount, which adjusts the rate according to the vertical position deviation, and the pitch angle adjustment amount to assist in altitude control. The lateral trajectory adjustment amount uses the PID control algorithm to obtain the aileron deflection angle adjustment amount based on the lateral offset deviation through comprehensive calculations of proportional, integral and differential links. After integrating these adjustments and analyzing their comprehensive impact and mutual relationship on the flight trajectory of the drone, collaborative optimization is carried out. For example, when adjusting the heading angle, the impact on the vertical and lateral directions is considered. If the amplitude is large, the vertical and lateral adjustment amounts are fed back to reduce the amplitude or adjust them simultaneously to maintain stability. When adjusting the vertical velocity, if the horizontal and lateral directions are affected, the horizontal velocity and aileron deflection angle are fine-tuned according to the corresponding adjustment amount. When adjusting the aileron deflection angle laterally, both horizontal and vertical directions are taken into account and optimized reasonably according to needs. Ultimately, through collaborative optimization, the three-axis adjustment amount of the flight trajectory is comprehensively determined, including the precise heading angle, horizontal velocity component, ascent or descent rate, pitch angle, and aileron deflection angle adjustment values. This allows the drone to fly stably along the preset trajectory, compensate for deviations, and complete the flight mission.

[0051] In one possible implementation, the lateral offset adjustment strategy is used to perform PID control compensation on the real-time posture of the UAV and the flight trajectory deviation parameters to obtain a lateral trajectory adjustment amount. Step S530 further includes step S531, initializing the PID controller according to the lateral offset adjustment strategy, and the parameter information of the PID controller includes a proportional control item, an integral control item, and a differential control item. Specifically, the lateral offset adjustment strategy relies on the PID controller to correct the deviation of the UAV from the vertical direction of the preset path. When initializing the PID controller, its function of receiving the real-time posture and flight trajectory deviation parameters and outputting a lateral motion control signal is clarified. Set the proportional control item and determine the proportional coefficient K according to the characteristics of the UAV and the flight environment. p , such as high-speed maneuverable drones adapted to the K p To quickly respond to deviations, use a small K for slow and steady flight. p The integral control term eliminates the steady-state error and the integral coefficient K is set according to the interference situation. i , more interference can increase K i , but be careful of oscillation. The differential control item predicts and adjusts the control quantity in advance according to the deviation change rate, and determines the differential coefficient K according to the flight scenario. d In fast response tasks, when the system is sensitive to deviation changes, a large K can be set. d, while also taking care to avoid noise interference. Based on a PID controller for closed-loop compensation calculations, the drone uses sensors such as GPS and IMUs to acquire position and lateral offset data. Because this data is susceptible to interference, it is preprocessed, such as using a Kalman filter to process GPS data and calibrate and compensate the IMU data. This preprocessed data is then fed into the PID controller. The proportional term generates a control signal based on the deviation, aligning the drone with the path. The integral term accumulates the deviation to eliminate steady-state errors. The differential term adjusts the control variable in advance based on the rate of change of the deviation. The three terms are combined to determine the final control variable. If the control variable is positive, increasing the right aileron deflection angle generates a leftward lateral force to reduce the deviation. After the drone executes the command, the sensors feed the new position and deviation back to the PID controller. If the deviation decreases, the control variable is appropriately reduced to prevent overshoot. If the deviation increases or decreases, the control variable is increased to accelerate adjustment. Through continuous feedback optimization, the appropriate lateral trajectory adjustment is determined, ensuring that the drone accurately follows the preset path and eliminates lateral offset.

[0052] Step S532, based on the PID controller, a closed-loop compensation calculation is performed on the real-time posture of the UAV and the flight trajectory deviation parameters to obtain the lateral trajectory adjustment amount. Specifically, the UAV uses GPS and IMU to collect real-time posture and synchronously obtains the lateral offset deviation. Because the sensor data is susceptible to interference, the GPS data is denoised using Kalman filtering, and the IMU data is calibrated to eliminate drift errors. After preprocessing, the lateral offset deviation is input into the PID controller. The proportional control term generates a proportional control signal according to the deviation size, and the proportional coefficient K p Determines response speed and stability, Big K p Fast response but easy to overshoot, small K p The adjustment is slow but stable. The integral control term accumulates the deviation to eliminate the steady-state error. The integral coefficient K i Determines the elimination speed and oscillation degree, big K i Eliminate errors quickly but easily oscillate, small K i The process is slow. The differential control item adjusts the control amount in advance according to the deviation change rate, and the differential coefficient K d Determines the sensitivity to the rate of change, large K d Responsive but easy to amplify noise, small K d The PID controller integrates three signals to determine the final control variable, which is used to adjust the aileron deflection angle. A positive control variable increases the right aileron deflection angle, shifting the drone to the left and reducing deviation. After the drone executes the command, the sensor feeds the new position and deviation back to the PID controller. If the deviation decreases, the control variable is reduced to prevent overshoot. If the deviation is large or increasing, the control variable is increased to speed up the adjustment. Through repeated feedback optimization, the appropriate lateral trajectory adjustment amount is determined, allowing the drone to accurately fly along the preset path and eliminate lateral deviation.

[0053] In the above, refer to Figure 1The adaptive control method for correcting the flight trajectory of a UAV according to an embodiment of the present invention is described in detail. Figure 2 The following describes an adaptively controlled UAV flight trajectory correction system according to an embodiment of the present invention.

[0054] The adaptively controlled UAV flight trajectory correction system according to an embodiment of the present invention addresses the low efficiency of real-time, accurate flight trajectory correction for existing UAVs in complex environments. By utilizing industrial automatic control system manufacturing technology (such as dynamic analysis of three-axis compensation strategies) and intelligent monitoring devices (such as multi-sensor fusion trajectory deviation detection), the system achieves the technical effect of shortening the response time for dynamic compensation of UAV flight trajectory deviations. The adaptively controlled UAV flight trajectory correction system includes a flight environment spatial unit model acquisition module 10, a UAV real-time posture determination module 20, a flight trajectory deviation parameter acquisition module 30, a three-axis compensation flight strategy acquisition module 40, and a flight trajectory three-axis adjustment amount determination module 50.

[0055] The flight environment space unit model acquisition module 10 is used to collect and acquire a flight environment data set through a sensor device group carried by a target UAV, perform three-dimensional modeling and unit division based on the flight environment data set, and obtain a flight environment space unit model.

[0056] The UAV real-time posture determination module 20 is used to perform point cloud matching and posture estimation on the current flight environment data based on the flight environment space unit model to determine the UAV real-time posture.

[0057] The flight trajectory deviation parameter acquisition module 30 is used to plan and determine a preset flight path, perform trajectory deviation prediction on the real-time posture of the UAV according to the preset flight path, and obtain flight trajectory deviation parameters.

[0058] The three-axis compensation flight strategy acquisition module 40 is used to perform trend judgment and strategy analysis on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, which includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy.

[0059] The flight trajectory three-axis adjustment amount determination module 50 is used to adopt the three-axis compensation flight strategy to analyze the real-time posture of the UAV and the flight trajectory deviation parameters, determine the flight trajectory three-axis adjustment amount, and perform flight trajectory compensation correction on the target UAV based on the flight trajectory three-axis adjustment amount.

[0060] Next, the specific configuration of the flight environment space unit model acquisition module 10 will be described in detail. As described above, a flight environment data set is collected and acquired by a sensor device group carried by a target UAV, and three-dimensional modeling and unit division are performed based on the flight environment data set to obtain a flight environment space unit model. The flight environment space unit model acquisition module 10 further includes: a classification extraction unit, the classification extraction unit is used to classify and extract the flight environment data set according to the sensor device group to obtain flight environment point cloud data and flight environment image data; an initial flight environment three-dimensional model generation unit, the initial flight environment three-dimensional model generation unit is used to triangulate and reconstruct the flight environment point cloud data to generate an initial flight environment three-dimensional model; an enhancement optimization unit, the enhancement optimization unit is used to match and map the flight environment image data to the initial flight environment three-dimensional model for enhancement optimization to obtain a target flight environment three-dimensional model; a flight trajectory analysis requirement unit, the flight trajectory analysis requirement unit is used to determine the unit division type and unit division density according to the flight trajectory analysis requirement; a spatial unit division unit, the spatial unit division unit is used to perform spatial unit division on the target flight environment three-dimensional model according to the unit division type and unit division density to obtain the flight environment space unit model.

[0061] The specific configuration of the drone real-time posture determination module 20 will be described in detail below. As described above, based on the flight environment space unit model, point cloud matching and posture estimation are performed on the current flight environment data to determine the real-time posture of the drone. The drone real-time posture determination module 20 further includes: a key point set acquisition unit, which is used to extract key feature points from the flight environment space unit model and the current flight environment data respectively to obtain a space model key point set and a current flight environment key point set; a multidimensional feature description unit, which is used to perform multidimensional feature description on the space model key point set and the current flight environment key point set in turn. feature description, obtaining a spatial model point cloud feature description subset and a current flight environment point cloud feature description subset; a point cloud matching calculation unit, the point cloud matching calculation unit is used to use the ICP algorithm to perform point cloud matching calculation on the spatial model key point set and the current flight environment key point set according to the spatial model point cloud feature description subset and the current flight environment point cloud feature description subset, to obtain a set of matching spatial feature points; a UAV real-time posture determination unit, the UAV real-time posture determination unit is used to perform Kalman filtering processing and posture estimation based on the matching spatial feature point set to determine the real-time posture of the UAV.

[0062] Among them, Kalman filtering processing and posture estimation are performed based on the matching spatial feature point set to determine the real-time posture of the drone, and the drone real-time posture determination unit further includes: a Kalman filter initialization subunit, the Kalman filter initialization subunit is used to initialize the Kalman filter according to the initial flight state of the target drone and the noise characteristic information of the sensor device group; a drone flight state fitting subunit, the drone flight state fitting subunit is used to perform drone flight state fitting based on the flight posture state data in the flight environment data set, construct a drone dynamics model, and obtain a flight state sequence estimation value based on the drone dynamics model prediction; a Kalman optimization filter acquisition subunit, the Kalman optimization filter acquisition subunit is used to perform covariance prediction and iterative update on the Kalman filter based on the flight posture state data and the flight state sequence estimation value to obtain a Kalman optimization filter; a posture state estimation subunit, the posture state estimation subunit is used to use the Kalman optimization filter to perform posture state estimation on the matching spatial feature point set to determine the real-time posture of the drone.

[0063] Among them, the Kalman filter is covariance predicted and iteratively updated based on the flight posture state data and the flight state sequence estimation value to obtain a Kalman optimization filter, and the Kalman optimization filter acquisition subunit further includes: a flight state sequence observation value acquisition micro unit, the flight state sequence observation value acquisition micro unit is used to obtain the flight state sequence observation value according to the flight posture state data; a covariance prediction micro unit, the covariance prediction micro unit is used to perform covariance prediction based on the error of the flight state sequence observation value and the flight state sequence estimation value, and generate an observation error covariance matrix and a prediction error covariance matrix respectively; a difference gain calculation micro unit, the difference gain calculation micro unit is used to perform difference gain calculation on the observation error covariance matrix and the prediction error covariance matrix based on the Kalman filter to obtain Kalman gain information; an iterative optimization update micro unit, the iterative optimization update micro unit is used to iteratively optimize and update the Kalman filter according to the Kalman gain information to obtain the Kalman optimization filter.

[0064] The specific configuration of the flight trajectory deviation parameter acquisition module 30 will be described in detail below. As described above, a preset flight path is planned and determined, and the trajectory deviation of the real-time posture of the UAV is predicted according to the preset flight path to obtain the flight trajectory deviation parameter. The flight trajectory deviation parameter acquisition module 30 further includes: a flight posture prediction sequence value acquisition unit, which is used to predict the flight state of the real-time posture of the UAV based on the Kalman optimization filter to obtain the flight posture prediction sequence values ​​at multiple preset moments; a flight prediction trajectory information generation unit, which is used to generate flight prediction trajectory information based on the flight posture prediction sequence value; and a vector difference calculation unit, which is used to perform vector difference calculation on the flight prediction trajectory information based on the preset flight path to obtain the flight trajectory deviation parameter.

[0065] The specific configuration of the three-axis compensation flight strategy acquisition module 40 will be described in detail below. As described above, the flight trajectory deviation parameters are subjected to trend judgment and strategy analysis to obtain a three-axis compensation flight strategy, which includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy. The three-axis compensation flight strategy acquisition module 40 further includes: a change trend judgment unit, which is used to perform change trend judgment on the flight trajectory deviation parameters according to a time series to determine trajectory deviation trend information; a three-axis adjustment strategy analysis unit, which is used to perform three-axis adjustment strategy analysis based on the trajectory deviation trend information to obtain a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy; and a three-axis compensation flight strategy determination unit, which is used to determine the three-axis compensation flight strategy based on the horizontal plane adjustment strategy, the vertical direction adjustment strategy, and the lateral offset adjustment strategy.

[0066] The specific configuration of the flight trajectory three-axis adjustment amount determination module 50 will be described in detail below. As described above, the three-axis compensation flight strategy is used to analyze the real-time posture of the UAV and the flight trajectory deviation parameters, determine the flight trajectory three-axis adjustment amount, and perform flight trajectory compensation correction on the target UAV based on the flight trajectory three-axis adjustment amount. The flight trajectory three-axis adjustment amount determination module 50 further includes: a horizontal plane trajectory adjustment amount determination unit, the horizontal plane trajectory adjustment amount determination unit is used to use the horizontal plane adjustment strategy to perform deviation compensation and dynamic obstacle avoidance analysis on the real-time posture of the UAV and the flight trajectory deviation parameters, and determine the horizontal plane trajectory adjustment amount; a vertical trajectory adjustment amount determination unit, the vertical trajectory adjustment amount determination unit is used to Based on the vertical direction adjustment strategy, the real-time posture of the UAV and the flight trajectory deviation parameters are analyzed by air pressure data and TOF sensor data and altitude lock compensation to determine the vertical trajectory adjustment amount; a lateral trajectory adjustment amount acquisition unit is used to adopt the lateral offset adjustment strategy to perform PID control compensation on the real-time posture of the UAV and the flight trajectory deviation parameters to obtain the lateral trajectory adjustment amount; a flight trajectory three-axis adjustment amount determination unit is used to determine the flight trajectory three-axis adjustment amount based on the horizontal plane trajectory adjustment amount, the vertical trajectory adjustment amount and the lateral trajectory adjustment amount.

[0067] Among them, the lateral offset adjustment strategy is adopted to perform PID control compensation on the real-time posture of the UAV and the flight trajectory deviation parameters to obtain a lateral trajectory adjustment amount. The lateral trajectory adjustment amount acquisition unit further includes: a controller parameter information composition subunit, the controller parameter information composition subunit is used to initialize the PID controller according to the lateral offset adjustment strategy, and the parameter information of the PID controller includes a proportional control item, an integral control item and a differential control item; a compensation closed-loop calculation subunit, the compensation closed-loop calculation subunit is used to perform compensation closed-loop calculation on the real-time posture of the UAV and the flight trajectory deviation parameters based on the PID controller to obtain the lateral trajectory adjustment amount.

[0068] The adaptively controlled UAV flight trajectory correction system provided in the embodiment of the present invention can execute the adaptively controlled UAV flight trajectory correction method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0069] Although the present application makes various references to certain modules in the system according to the embodiments of the present application, any number of different modules may be used and run on the user terminal and / or server, and the various units and modules included are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of the functional units are only for the convenience of distinguishing each other and are not used to limit the scope of protection of the present invention.

[0070] The above specific embodiments do not constitute a limitation on the scope of protection of this application. Those skilled in the art should understand that various modifications, combinations, and substitutions may be made based on design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application shall be included within the scope of protection of this application.

Claims

1. The adaptive control UAV flight trajectory correction method is characterized by: The method comprises: A flight environment data set is acquired by collecting a sensor device group carried by the target UAV, and three-dimensional modeling and unit division are performed based on the flight environment data set to obtain a flight environment space unit model; Performing point cloud matching and pose estimation on the current flight environment data based on the flight environment space unit model to determine the real-time pose of the UAV; Planning and determining a preset flight path, performing trajectory deviation prediction on the real-time posture of the UAV according to the preset flight path, and obtaining a flight trajectory deviation parameter; Performing trend judgment and strategy analysis on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, wherein the three-axis compensation flight strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy; The three-axis compensation flight strategy is used to analyze the real-time posture of the UAV and the flight trajectory deviation parameters, determine the three-axis adjustment amount of the flight trajectory, and perform flight trajectory compensation correction on the target UAV based on the three-axis adjustment amount of the flight trajectory; The three-axis compensation flight strategy is obtained, including: Determine the change trend of the flight trajectory deviation parameter according to the time series to determine the trajectory deviation trend information; Based on the trajectory deviation trend information, three-axis adjustment strategy analysis is performed to obtain a horizontal adjustment strategy, a vertical adjustment strategy, and a lateral offset adjustment strategy; Determining the three-axis compensation flight strategy based on the horizontal plane adjustment strategy, the vertical direction adjustment strategy, and the lateral offset adjustment strategy; Determining the three-axis adjustment amount of the flight trajectory includes: The horizontal plane adjustment strategy is used to perform deviation compensation and dynamic obstacle avoidance analysis on the real-time posture of the UAV and the flight trajectory deviation parameters to determine the horizontal plane trajectory adjustment amount; Based on the vertical direction adjustment strategy, the real-time posture of the UAV and the flight trajectory deviation parameters are analyzed using air pressure data, TOF sensor data, and altitude lock compensation to determine the vertical trajectory adjustment amount; The lateral offset adjustment strategy is used to perform PID control compensation on the real-time posture of the UAV and the flight trajectory deviation parameter to obtain a lateral trajectory adjustment amount; The three-axis flight trajectory adjustment amount is determined based on the horizontal trajectory adjustment amount, the vertical trajectory adjustment amount, and the lateral trajectory adjustment amount.

2. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 1, wherein: The obtaining of the flight environment space unit model comprises: Classify and extract the flight environment data set according to the sensor device group to obtain flight environment point cloud data and flight environment image data; Performing triangulation and Poisson surface reconstruction on the flight environment point cloud data to generate an initial flight environment three-dimensional model; Matching and mapping the flight environment image data to the initial flight environment three-dimensional model for enhancement and optimization to obtain a target flight environment three-dimensional model; Determine the unit division type and unit division density based on flight trajectory analysis requirements; The target flight environment three-dimensional model is spatially divided into units according to the unit division type and unit division density to obtain the flight environment spatial unit model.

3. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 1, wherein: Determining the real-time posture of the drone includes: Extracting key feature points from the flight environment spatial unit model and the current flight environment data respectively to obtain a spatial model key point set and a current flight environment key point set; Performing multi-dimensional feature description on the spatial model key point set and the current flight environment key point set in sequence to obtain a spatial model point cloud feature description subset and a current flight environment point cloud feature description subset; Performing point cloud matching calculation on the spatial model key point set and the current flight environment key point set according to the spatial model point cloud feature description subset and the current flight environment point cloud feature description subset using an ICP algorithm to obtain a matching spatial feature point set; Kalman filtering and posture estimation are performed based on the set of matching spatial feature points to determine the real-time posture of the UAV.

4. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 3, wherein: Determining the real-time posture of the drone includes: Initializing a Kalman filter according to the initial flight state of the target UAV and the noise characteristic information of the sensor device group; Performing UAV flight state fitting based on the flight posture state data in the flight environment dataset, constructing a UAV dynamics model, and predicting a flight state sequence estimation value based on the UAV dynamics model; Performing covariance prediction and iterative updating on the Kalman filter based on the flight posture state data and the flight state sequence estimation value to obtain a Kalman optimization filter; The Kalman optimization filter is used to estimate the posture state of the matching spatial feature point set to determine the real-time posture of the UAV.

5. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 4, wherein: The Kalman optimization filter is obtained, comprising: Obtaining a flight state sequence observation value according to the flight posture state data; Performing covariance prediction based on the errors between the flight state sequence observation values ​​and the flight state sequence estimation values, and generating an observation error covariance matrix and a prediction error covariance matrix respectively; Performing difference gain calculation on the observation error covariance matrix and the prediction error covariance matrix based on the Kalman filter to obtain Kalman gain information; The Kalman filter is iteratively optimized and updated according to the Kalman gain information to obtain the Kalman optimization filter.

6. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 4, wherein: The obtaining of the flight trajectory deviation parameter includes: Perform flight state prediction on the real-time posture of the UAV based on the Kalman optimization filter to obtain flight posture prediction sequence values ​​at multiple preset moments; Generating flight prediction trajectory information according to the flight posture prediction sequence value; A vector difference calculation is performed on the flight prediction trajectory information based on the preset flight path to obtain the flight trajectory deviation parameter.

7. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to claim 1, wherein: Obtaining the lateral trajectory adjustment amount includes: Initializing a PID controller according to the lateral offset adjustment strategy, wherein parameter information of the PID controller includes a proportional control item, an integral control item, and a differential control item; Based on the PID controller, a compensation closed-loop calculation is performed on the real-time posture of the UAV and the flight trajectory deviation parameter to obtain the lateral trajectory adjustment amount.

8. The adaptive control UAV flight trajectory correction system is characterized by: The system is applied to the adaptive control UAV flight trajectory correction method according to any one of claims 1 to 7, and the system comprises: A flight environment space unit model acquisition module is used to acquire a flight environment data set through a sensor device group carried by the target UAV, perform three-dimensional modeling and unit division based on the flight environment data set, and obtain a flight environment space unit model; A UAV real-time posture determination module is used to perform point cloud matching and posture estimation on the current flight environment data based on the flight environment space unit model to determine the real-time posture of the UAV; A flight trajectory deviation parameter acquisition module is used to plan and determine a preset flight path, perform trajectory deviation prediction on the real-time posture of the UAV according to the preset flight path, and obtain flight trajectory deviation parameters; a three-axis compensation flight strategy acquisition module, which is used to perform trend judgment and strategy analysis on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, which includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy; A flight trajectory three-axis adjustment amount determination module is used to adopt the three-axis compensation flight strategy to analyze the real-time posture of the UAV and the flight trajectory deviation parameters, determine the flight trajectory three-axis adjustment amount, and perform flight trajectory compensation correction on the target UAV based on the flight trajectory three-axis adjustment amount.

Images