Self-adaptive control unmanned aerial vehicle flight path correction method and system

Abstract

The invention discloses a self-adaptive control unmanned aerial vehicle flight path correction method and system, and relates to the technical field of unmanned aerial vehicle flight control, and the method comprises the steps: collecting flight environment data through a sensor group carried by an unmanned aerial vehicle, constructing a three-dimensional environment model, and dividing the three-dimensional environment model into space units, the real-time pose of the unmanned aerial vehicle is determined based on point cloud matching and pose estimation technologies, trajectory offset is predicted according to a preset flight path, flight trajectory deviation parameters are generated, and a triaxial compensation strategy of horizontal plane, vertical direction and lateral offset is analyzed through trend analysis of the deviation parameters. And finally performing dynamic compensation correction on the flight path of the unmanned aerial vehicle according to the three-axis adjustment amount. The technical problem that the real-time accurate correction efficiency of the flight path of an existing unmanned aerial vehicle in a complex environment is low is solved, and the technical effect of shortening the dynamic compensation response time of the flight path deviation of the unmanned aerial vehicle is achieved through an industrial automatic control system device manufacturing technology and an intelligent monitoring device.

Description

Technical Field

[0001] This application relates to the technical field of UAV flight control, and particularly to a method and system for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control. Background Art

[0002] When existing UAVs fly in complex environments (such as dense obstacles and dynamic interference), they generally rely on GPS and inertial navigation systems for positioning and path tracking. When performing tasks such as terrain mapping, logistics distribution, and agricultural monitoring, UAVs often need to fly autonomously in complex and changing environments. The uncertainties of the environment, such as obstacles and wind direction changes, may cause them to deviate from the preset flight trajectory. Traditional trajectory correction methods mostly rely on GPS signals. In areas where GPS signals are limited, such as urban canyons and forests, their effectiveness is greatly reduced. At the same time, the current positioning method relying on GPS and inertial navigation systems is susceptible to signal occlusion, multipath effects, and sensor cumulative errors, resulting in difficult real-time correction of trajectory deviation. Traditional methods have problems such as response lag and single compensation strategy in multi-sensor data fusion and dynamic obstacle avoidance, making it difficult to achieve high-precision adaptive trajectory correction. How to improve the real-time trajectory correction ability and cooperative control efficiency of UAVs in complex environments without relying on external navigation signals, and achieve autonomous adaptation to environmental changes and precise trajectory correction has become a technical problem to be solved urgently.

[0003] In the current related technologies, there is a technical problem of low efficiency in real-time and precise correction of the flight trajectory of UAVs in complex environments. Summary of the Invention

[0004] This application provides a method and system for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control, and solves the technical problem of low efficiency in real-time and precise correction of the flight trajectory of existing UAVs in complex environments.

[0005] This application provides a method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control, including:

[0006] Collect and obtain a flight environment data set through a sensor device group carried on a target unmanned aerial vehicle (UAV). Based on the flight environment data set, perform three-dimensional modeling and cell division to obtain a flight environment space cell model. Based on the flight environment space cell model, perform point cloud matching and pose estimation on the current flight environment data to determine the real-time pose of the UAV. Plan and determine a preset flight path, and perform trajectory offset prediction on the real-time pose of the UAV according to the preset flight path to obtain flight trajectory deviation parameters. Perform trend judgment and strategy analysis on the flight trajectory deviation parameters to obtain a three-axis compensation flight strategy, where the three-axis compensation flight strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy. Use the three-axis compensation flight strategy to analyze the real-time pose of the UAV and the flight trajectory deviation parameters, determine the three-axis adjustment amount of the flight trajectory, and perform flight trajectory compensation and correction on the target UAV based on the three-axis adjustment amount of the flight trajectory.

[0007] This application provides a UAV flight trajectory correction system with adaptive control, including:

[0008] A flight environment space cell model acquisition module, which is used to collect and obtain a flight environment data set through a sensor device group carried on a target UAV, and perform three-dimensional modeling and cell division based on the flight environment data set to obtain a flight environment space cell model; a UAV real-time pose determination module, which is used to perform point cloud matching and pose estimation on the current flight environment data based on the flight environment space cell model to determine the real-time pose of the UAV; a flight trajectory deviation parameter acquisition module, which is used to plan and determine a preset flight path, and perform trajectory offset prediction on the real-time pose of the UAV according to the preset flight path to 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, where the three-axis compensation flight strategy includes a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy; a three-axis adjustment amount determination module of the flight trajectory, which is used to analyze the real-time pose of the UAV and the flight trajectory deviation parameters by using the three-axis compensation flight strategy, determine the three-axis adjustment amount of the flight trajectory, and perform flight trajectory compensation and correction on the target UAV based on the three-axis adjustment amount of the flight trajectory.

[0009] The method and system for correcting the flight trajectory of an unmanned aerial vehicle (UAV) with adaptive control proposed in this application first collect flight environment data through a sensor group carried by the UAV, construct a three-dimensional environment model and divide it into spatial units, determine the real-time pose of the UAV based on point cloud matching and pose estimation techniques, predict trajectory deviation according to a preset flight path and generate flight trajectory deviation parameters, analyze the trend of the deviation parameters to resolve the three-axis compensation strategies for horizontal plane, vertical direction and lateral deviation, and finally dynamically compensate and correct the flight trajectory of the UAV according to the three-axis adjustment amounts. Through industrial automatic control system devices manufacturing technologies (such as the dynamic analysis of three-axis compensation strategies) and intelligent monitoring devices

[0010] (such as the detection of trajectory deviation by multi-sensor fusion), the technical effect of shortening the dynamic compensation response time of the UAV flight trajectory deviation is achieved. Description of the Drawings

[0011] To more clearly illustrate the technical solutions of the embodiments of the present disclosure, the drawings of the embodiments of the present disclosure will be briefly introduced below. Flowcharts are used in this application to illustrate the operations performed by the systems according to the embodiments of the application. It should be understood that the operations described above or below do not necessarily need to be executed precisely in order. Instead, various steps can be processed in reverse order or simultaneously as needed. At the same time, other operations can also be added to these processes, or one or several operations can be removed from these processes.

[0012] Figure 1 It is a schematic flowchart of the method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control provided by an embodiment of the present application;

[0013] Figure 2 It is a schematic structural diagram of the system for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control provided by an embodiment of the present application.

[0014] Description of the reference numerals: Flight environment spatial unit model acquisition module 10, UAV real-time pose 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 Embodiments

[0015] The above description is only an overview of the technical solutions of the present application. In order to be able to understand the technical means of the present application more clearly, it can be implemented according to the content of the specification. And in order to make the above and other purposes, features and advantages of the present application more obvious and understandable, the following specifically gives the detailed embodiments of the present application.

[0016] To make the objectives, technical solutions, and advantages of this application clearer, the following will further describe this application in detail with reference to the accompanying drawings. The described embodiments should not be construed as limitations on this application. All other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of this application.

[0017] In the following description, reference is made to "some embodiments", which describe a subset of all possible embodiments. However, it can be understood that "some embodiments" can be the same subset or different subsets of all possible embodiments and can be combined with each other without conflict. The terms "first / second" involved are only used to distinguish similar objects and do not represent a specific order for the objects. The terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or server that comprises a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or modules that are not clearly listed or 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 technical field to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application.

[0018] The embodiments of this application provide an adaptive control method for correcting the flight trajectory of an unmanned aerial vehicle (UAV), as Figure 1 shown, the method includes:

[0019] Step S100, acquiring a flight environment data set through a sensor device group carried on the target UAV, and performing three-dimensional modeling and cell division based on the flight environment data set to obtain a flight environment space cell model. Specifically, when the target UAV is flying, the sensor device group composed of lidar, vision sensors, ultrasonic sensors, IMU, etc. carried on it continuously acquires flight environment data at a specific sampling frequency and through a time synchronization mechanism, and transmits it to the data processing unit or the ground control station in real time. The acquired data needs to be preprocessed. By data fusion, multi-sensor information is integrated, a filtering algorithm is used to denoise, and data registration is performed to make the data of different sensors in the same coordinate system. Subsequently, the lidar point cloud data is processed successively by filtering, segmentation, feature extraction, etc., and three-dimensional models are constructed using algorithms such as triangulation, voxel, and surface fitting, and then optimized through operations such as smoothing, repair, and simplification. Then, according to the flight environment and mission requirements, division strategies such as regular grid, adaptive, and object-based are selected to divide the three-dimensional model, taking into account factors such as cell size, shape, and position. Finally, the divided cells are combined into a flight environment space cell model and stored using data structures such as octree and quadtree to provide support for the autonomous flight and path planning of the UAV.

[0020] In a possible implementation, a flight environment data set is collected through a sensor device group carried on a target unmanned aerial vehicle (UAV). Based on the flight environment data set, three-dimensional modeling and cell division are performed to obtain a flight environment space cell model. Step S100 further includes step S110, where the flight environment data set is classified and extracted according to the sensor device group, and flight environment point cloud data and flight environment image data can be obtained. When the UAV is flying, the sensor device group continuously collects data to form a data set. The classification and extraction process is as follows: The sensor device group usually includes a lidar and various cameras. The lidar obtains the distance information of surrounding objects based on the principle of time-of-flight measurement of light to form point cloud data. Its data format is diverse, such as binary or ASCII text format. The system first identifies the format and then parses each packet according to the transmission protocol. By judging the scanning angle, distance value and using the time stamp, etc., the flight environment point cloud data that meets the characteristics is filtered out. The camera captures a two-dimensional image of the flight environment. Common image data formats include JPEG, PNG, etc., each with a unique file header identifier. The system first identifies the format and then verifies the data integrity according to the format specification, such as checking the quantization table of JPEG, the data block information of PNG, etc. The data that passes the verification is extracted from the data set as valid flight environment image data.

[0021] Step S120, perform triangulation and Poisson surface reconstruction 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 an initial three-dimensional model, triangulation and Poisson surface reconstruction need to be performed in sequence. The original point cloud data is discrete and unstructured. First, preprocess it by using statistical filtering to remove noise points and voxel downsampling to reduce redundant points. Then use the Delaunay triangulation algorithm. Select three points to build an initial triangle, and insert the remaining points one by one according to the empty circumcircle criterion. If the new point is inside the circumcircle of the triangle, the triangle is reconstructed. Iterate until all points are included in the network to form a triangular patch grid structure. However, the surface of the triangulated grid is not smooth and has holes. Enter the Poisson surface reconstruction stage. First, use the K-nearest neighbor algorithm combined with the principal component analysis to calculate the normal vector of the point cloud, and use the propagation algorithm to unify the direction of the normal vector. Then, use the triangulated grid and the normal vector as inputs to construct a Poisson equation, and use numerical methods such as the multigrid method to solve it. Adjust the parameters according to the distribution of the point cloud data and the normal vector information to fill the holes and smooth the surface, and finally generate a continuous, smooth and practical initial three-dimensional model of the flight environment.

[0022] Step S130: Match and map the flight environment image data to the initial three-dimensional model of the flight environment for enhancement and optimization to obtain the target three-dimensional model of the flight environment. Specifically, to obtain the target three-dimensional model of the flight environment, it is necessary to match and map the flight environment image data to the initial three-dimensional model of the flight environment and perform enhancement and optimization. First, apply the Scale-Invariant Feature Transform (SIFT) algorithm 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, extract the geometric features and topological structure features of the triangular patches on the initial three-dimensional model. Then, through measurement methods such as Euclidean distance, use a KD tree for fast nearest neighbor search to compare the feature descriptions of the two to achieve feature matching. Next, based on the matching results, establish the connection between the image and the three-dimensional model. Obtain the transformation matrix by calculating the coordinate transformation relationship, and then map the image points to the surface of the three-dimensional model. Use bilinear interpolation to obtain the texture color value from the image and fill it into the triangular patches for texture mapping. Finally, adjust the mapping parameters, use image fusion technology to fuse image data from different angles, perform smoothing processing using the Laplacian smoothing algorithm, analyze the high-frequency details of the image to enhance the details of the model, highlight the detail features, and make the target three-dimensional model of the flight environment more realistically and accurately reflect the actual flight environment.

[0023] Step S140: Determine the unit division type and unit division density according to the flight trajectory analysis requirements. Specifically, clarify the flight trajectory analysis objectives and data characteristics, sort out the analysis purposes such as monitoring activity hotspots and evaluating flight stability, and understand the characteristics of the trajectory data such as location, timestamp, flight state parameters, as well as time span and accuracy. Subsequently, determine the unit division type. For spatial division, there are rectangles, hexagons, and irregular polygons (used for special areas, generated according to the regional boundary with the help of GIS, and use the ray method to judge the position of the aircraft); for time division, there are fixed time intervals (divided by a fixed duration, which is conducive to analyzing the dynamic changes of flight activities, and draw a time series graph through statistical analysis), and event-driven (divided according to specific flight events, which can specifically study the trajectory characteristics of the key stages of the aircraft). Finally, determine the unit division density. It is necessary to consider the analysis accuracy requirements (high accuracy corresponds to high division density), weigh the data volume and computing resources (can use means such as data sampling or distributed computing), and refer to the actual application scenario (different scenarios have different requirements). Through comprehensive analysis and repeated experimental adjustment, obtain the unit division type and density that balance various factors.

[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, construct the flight environment spatial unit model based on the established unit division type and density. First, clarify the division basis, review the details of the unit division type and density, and at the same time fully load the data of the three-dimensional model of the target flight environment into the analysis system. Subsequently, operate according to different division types: When performing rectangular grid division, construct a grid in the three-dimensional space within the geographical coordinate range of the model according to the set side length, and determine the attribution by comparing the coordinates of elements such as model vertices and triangular patches with the grid boundary coordinates; For hexagonal grid division, first design the layout, construct hexagons with a specific starting point and side length and arrange them in a honeycomb pattern, and use geometric algorithms such as the vector method to judge whether the model elements are within the hexagons; For irregular polygon division, import the pre-defined boundary data, and use spatial analysis algorithms such as the ray method to screen out the model elements located within the polygon. Finally, after completing the attribution determination, construct a data structure containing the corresponding model elements and attribute information for each spatial unit, integrate it to form the framework of the flight environment spatial unit model, and verify it through visualization and other means. If there are problems, adjust the parameters or algorithms in a timely manner to ensure the accuracy and integrity of the model.

[0025] Step S200: Based on the flight environment spatial unit model, perform point cloud matching and pose estimation 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, first perform data preparation, process the flight environment spatial unit model to ensure the integrity of the data and construct spatial index structures such as KD trees. At the same time, after collecting the current flight environment data, perform preprocessing such as denoising, downsampling (for lidar point clouds), image enhancement, and distortion correction (for visual sensor data). Then carry out point cloud matching, extract local surface features such as normal directions and curvatures from the current and model point clouds, and use the iterative closest point (ICP) algorithm. Take some points in the current point cloud as the initial set, find the nearest neighbor points in the model point cloud, and continuously iterate to calculate the transformation matrix to align the two groups of point clouds until the mean square error reaches the threshold. Finally, perform pose estimation, analyze the real-time pose parameters of the UAV based on the transformation matrix obtained from point cloud matching, and then use the extended Kalman filter (EKF) algorithm to fuse the inertial measurement unit (IMU) data with the pose information of point cloud matching, and continuously correct it to obtain a more accurate and stable real-time pose of the UAV.

[0026] In a possible implementation, based on the flight environment space 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. Step S200 further includes step S210 of respectively extracting key feature points from the flight environment space unit model and the current flight environment data to obtain a spatial model key point set and a current flight environment key point set. Specifically, the spatial model key point set and the current flight environment key point set are obtained, and key feature points are respectively extracted from the flight environment space unit model and the current flight environment data. For the flight environment space unit model, first understand its data structure and characteristics, preprocess noise and missing data, and 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 (filtered by calculating the curvature based on the locally fitted plane) can be selected. Traverse the point cloud data, and collect the detected key feature points to form the spatial 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 SIFT algorithm (construct a scale space, detect extreme points with the DoG operator and generate feature descriptors) can be used for the image data, and the local geometric feature-based algorithm (such as calculating the normal direction, etc.) can be used for the lidar point cloud data. Organize the extracted key feature points to form the current flight environment key point set.

[0027] Step S220, sequentially perform multi-dimensional feature description 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, multi-dimensional feature description is performed on the spatial model key point set and the current flight environment key point set to obtain the corresponding subsets. The specific process is as follows: For the spatial model key point set, first calculate the geometric features of each key point, such as calculating the normal direction in the neighborhood through PCA and calculating the curvature based on the plane fitted by the neighborhood points; then analyze its topological relationship with the neighborhood points, such as its position within the unit and its connection with other feature points. Then, features such as the normal direction, curvature, and topological relationship are integrated into a multi-dimensional vector, and collected to form the spatial model point cloud feature description subset. For the current flight environment key point set, if there is image data, the SIFT algorithm is used for the image key points, a window is divided in its neighborhood, the pixel gradient amplitude and direction are calculated, and the gradient direction histogram of the sub-region is statistically obtained to get a 128-dimensional feature vector; for the lidar point cloud key points, in addition to the algorithm line direction and curvature, the distance from the point to the radar origin and the uniformity of the distribution of neighborhood points are also considered. Finally, the feature vectors of the image and point cloud key points are integrated. If both are available, they are reasonably combined to finally construct the current flight environment point cloud feature description subset.

[0028] Step S230: Use the ICP algorithm to perform point cloud matching calculations on the key point set of the spatial model and the key point set of the current flight environment according to the subset of the spatial model point cloud feature description and the subset of the current flight environment point cloud feature description, and obtain a set of matching spatial feature points. Specifically, use the ICP algorithm to perform point cloud matching calculations on the key point set of the spatial model and the key point set of the current flight environment 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 T 0 as the identity matrix. Then, calculate the similarity based on the subset of feature descriptions to determine potential corresponding point pairs, and use a distance threshold to filter out a set of reliable corresponding point pairs P. Next, define an 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 . After that, use T opt to transform the key point set of the current flight environment, and determine whether the iteration termination condition is satisfied (the error is less than E thresh or the maximum number of iterations N max is reached. If not, repeat operations such as finding corresponding point pairs and calculating the transformation matrix. After the termination condition is satisfied, determine the final corresponding point pairs, combine them to form a set of matching spatial feature points, and provide a data basis for subsequent pose estimation.

[0029] Step S240: Perform Kalman filtering processing and pose estimation based on the set of matching spatial feature points to determine the real-time pose of the UAV. Specifically, use the set of matching spatial feature points to determine the real-time pose of the UAV. First, perform Kalman filtering processing. Set the UAV pose as the system state, including three-dimensional position and attitude angles. Construct a state transition model based on the uniform motion characteristics of the UAV to describe its state change from the current moment to the next moment, and consider the influence of process noise at the same time. Establish an observation model based on the set of matching spatial feature points, use the obtained relative position and attitude information of the UAV as the observation value, and consider the observation noise. Through iterative calculations of prediction and update, first predict the current state and covariance according to the state at the previous moment and the state transition model, then calculate the Kalman gain using the observation value, and further update the state estimate value and covariance, so as to optimize the estimation of the UAV pose. After the filtering is completed, extract the position and attitude information from the final state estimate value. This information is the real-time pose of the UAV, which is output to flight control, navigation and other systems to adjust the flight attitude and plan the path to ensure the stable and safe flight of the UAV.

[0030] In a possible implementation, Kalman filtering processing and pose estimation are performed based on the set of matched spatial feature points to determine the real-time pose of the UAV. Step S240 further includes step S241 of initializing a Kalman filter according to the initial flight state of the target UAV and the noise characteristic information of the sensor device group. Specifically, when initializing the Kalman filter, first clarify the initial flight state of the target UAV, obtain accurate position information such as longitude, latitude, and altitude through a high-precision GPS module and a barometric altimeter, and use the gyroscope and magnetometer in the inertial measurement unit (IMU) to determine attitude information such as heading angle, pitch angle, and roll angle. These data are used as the input for the first state estimation of the filter. At the same time, analyze the noise characteristics of the sensor device group, calculate the mean, standard deviation, and analyze the power spectral density of the static data collected by the accelerometer, and perform spectral analysis and Allan variance analysis on the gyroscope, and then construct a noise covariance matrix. Finally, set the initial flight state as the initial state vector of the filter, and reasonably configure the process noise and observation noise covariance matrices according to the noise covariance matrix to complete the initialization.

[0031] Step S242, perform UAV flight state fitting based on the flight pose state data in the flight environment dataset, construct a UAV dynamics model and obtain an estimated value of the flight state sequence. First, process the pose state data in the flight environment dataset. Collect data from sensors such as GPS and IMU and sort them in order, and remove outliers through Gaussian filtering denoising and threshold screening. Then use spline curves to fit the flight state, determine the parameters by the least squares method, and evaluate according to the mean square error. If it is not good, adjust it. Then, based on the principles of dynamics, consider forces and torques such as gravity and air resistance, establish differential equations according to Newton's second law and the angular momentum theorem, and determine parameters such as mass and moment of inertia through manuals, experiments, and optimization algorithms. Finally, use the initial pose of the fitted data as the initial condition of the model, and use the fourth-order Runge-Kutta method to solve the equation according to the time step to obtain the estimated value of the flight state sequence, compare it with the actual value, and use the difference to optimize the parameters of the Kalman filter.

[0032] Step S243, perform covariance prediction and iterative update on the Kalman filter based on the flight pose state data and the estimated value of the flight state sequence to obtain an optimized Kalman filter. Specifically, optimize the Kalman filter based on the flight pose state data and the estimated value of the flight state sequence. First, clarify the UAV state transition model x k = F k x k-1 + B k u k + w k , according to the state estimation x' k-1k-1 value and covariance matrix P k-1k-1 at time k-1, calculate the predicted state estimation value x' kk-1 and covariance matrix Pkk-1 。Then, establish the observation model Z k = H k x k + u k , and calculate the Kalman gain k based on this k , and then update the state estimate value x' at time k kk and the covariance matrix P kk . Continuously use the new result as the initial value to repeat the above process. After multiple iterations, the performance of the Kalman filter is improved, and finally the Kalman optimized filter is obtained, which provides accurate state estimation for the flight control and navigation of the UAV.

[0033] Step S244: Use the Kalman optimized filter to perform pose state estimation on the set of matched spatial feature points to determine the real-time pose of the UAV. Specifically, when using the Kalman optimized filter to determine the real-time pose of the UAV, first input the set of matched spatial feature points as the observation data. The filter predicts the prior value of the current pose according to the state transition equation, combined with the pose estimate value of the previous moment, and at the same time converts the set of feature points into an observation value through the observation model. Subsequently, calculate the Kalman gain based on the predicted covariance and the observation covariance matrix, and use this to fuse the predicted value and the observation value to correct the prior estimate value to obtain the accurate real-time pose. This process loops continuously, dynamically adjusting the pose estimation with the new set of feature points, providing key accurate data for flight control, etc.

[0034] In a possible implementation manner, perform covariance prediction and iterative update on the Kalman filter based on the flight pose state data and the estimated value of the flight state sequence to obtain the Kalman optimized filter. Step S243 further includes step S2431: Obtain the observed value of the flight state sequence according to the flight pose state data. Specifically, the UAV relies on sensors such as GPS and IMU carried on it to obtain flight pose state data. GPS provides position information such as longitude, latitude, and altitude, and the accuracy can reach the meter level or even the sub-meter level in an ideal environment, collecting data multiple times per second. IMU measures attitude angles such as heading angle, pitch angle, and roll angle through gyroscopes and accelerometers. The gyroscope can accurately measure the angular rate change of ±2000° / s per second, and the acquisition frequency reaches several hundred Hertz. Since the acquisition times of different sensors may vary, first perform time synchronization, and then sort and calibrate the data in chronological order. Combine the position data of GPS at each moment with the attitude data of IMU at the same moment into a data point. Numerous data points arranged in chronological order constitute the observed value of the flight state sequence, which is an important basis for subsequent data analysis and state estimation.

[0035] Step S2432: Perform covariance prediction based on the error between the observed flight state sequence values and the estimated flight state sequence values, and generate an observation error covariance matrix and a prediction error covariance matrix respectively. Specifically, the observed flight state sequence values are derived from the integration of UAV sensor data, and the estimated flight state sequence values are calculated through a dynamic model. When calculating the error, at each time point, the differences between the observed values and the estimated values are calculated separately in the dimensions of position (longitude, latitude, altitude) and attitude (heading angle, pitch angle, roll angle), and these differences form 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 each pair is calculated using statistical methods to form the position-related part in the matrix. The same applies to the attitude error. This matrix reflects the correlation and dispersion degree of the errors in each dimension of the observed data. When generating the prediction error covariance matrix, based on the state transition model, combined with the state covariance matrix at the previous moment, through the transformation of the state transition matrix and considering the process noise covariance, the matrix at the current moment is calculated. It reflects the degree of uncertainty in the prediction process. The larger the value, the higher the prediction uncertainty, indicating that the model or parameters need to be optimized.

[0036] Step S2433: Calculate the difference gain based on the Kalman filter for 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 degree of the observation data errors, and the prediction error covariance matrix that reflects the prediction uncertainty based on the state transition model as inputs. According to its algorithm rules, matrix operation rules are used to calculate the difference gain. This involves the inverse operation of )(H k is the observation matrix, P kk-1 is the prediction error covariance matrix, R k is the observation noise covariance matrix), and matrix multiplication operations that comprehensively consider multiple factors such as to finally obtain the Kalman gain information. This gain is a weight coefficient matrix that can dynamically adjust the proportion of the observed value and the predicted value when updating the state estimate according to the situations of the two covariance matrices. When the observation error covariance matrix is relatively small, it relies more on the observed value, and when the prediction error covariance matrix is relatively small, it tends more to the predicted value to achieve a more accurate estimation of the UAV state.

[0037] Step S2434: Iteratively optimize and update the Kalman filter according to the Kalman gain information to obtain the optimized Kalman filter. Specifically, after obtaining the Kalman gain information, the Kalman filter is iteratively optimized and updated. The Kalman gain is used to fuse the observed value and the estimated value of the flight state sequence. Specifically, it is the Kalman gain multiplied by the difference between the observed value and the value obtained by transforming the estimated value based on the predicted state through the observation matrix, and then adding the predicted state estimated value, so as to obtain a more accurate state estimated value. At the same time, the internal state covariance matrix of the filter is updated. Subtract the product of the Kalman gain and the observation matrix from the identity matrix, and then multiply by the predicted error covariance matrix, so that the state covariance matrix can more accurately reflect the uncertainty of the current state estimate. This iterative optimization and update is continuously looped. Every time there is a new observed value of the flight state sequence, covariance prediction, difference gain calculation, and iterative optimization and update need to be performed again. As the number of iterations increases, the Kalman filter continuously adjusts itself, and the accuracy and reliability of the UAV flight state estimation are gradually improved. 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, providing more accurate and reliable state estimation data for flight control, navigation, etc. The filter at this time is the optimized Kalman filter.

[0038] Step S300: Plan and determine a preset flight path, and predict the trajectory deviation of the real-time pose of the UAV according to the preset flight path to obtain flight trajectory deviation parameters. Specifically, by clarifying the mission requirements of the UAV, such as considering the area range and accuracy for mapping, and considering the starting and delivery points and time limits for logistics distribution, etc., collect the geographical information of the flight area, generate digital elevation and surface models with the help of geographical information system data, satellite images, etc., and then select appropriate algorithms such as Dijkstra, A*, or rapidly exploring random tree, input the starting and target positions and obstacle constraints, and obtain a preset flight path composed of a series of waypoints including longitude, latitude, and altitude. Then, the UAV relies on sensors such as GPS and IMU to collect real-time pose information at high frequency, establish a motion model with external disturbance terms based on Newtonian mechanics and dynamic characteristics, use model prediction, input the real-time pose as the initial condition into the model to calculate the difference from the preset path, or use data-driven methods, train the model with a large amount of historical data under similar flight conditions through algorithms such as neural networks, and input real-time information to predict the trajectory deviation. Finally, define and calculate the flight trajectory deviation parameters including position (longitude, latitude, altitude deviation) and attitude (heading angle, pitch angle, roll angle deviation), and output them to the flight control system to adjust the power and attitude, so that the UAV returns to the preset path. At the same time, these parameters are also used to record and analyze the flight performance, providing a basis for subsequent optimization and improvement.

[0039] In a possible implementation, a preset flight path is planned and determined, and the trajectory offset of the real-time pose of the UAV is predicted according to the preset flight path to obtain a flight trajectory deviation parameter. Step S300 further includes step S310 of predicting the flight state of the real-time pose of the UAV based on the Kalman optimized filter to obtain a flight pose prediction sequence value at multiple preset times. Specifically, when using the Kalman optimized filter to predict the flight state of the real-time pose of the UAV and obtaining multiple preset-time flight pose prediction sequence values, the filter needs to be initialized first. The initial state vector including the initial position (longitude, latitude, altitude) and the initial attitude (heading angle, pitch angle, roll angle) is determined by a high-precision GPS and IMU. According to the long-term monitoring and analysis of the sensor performance and the flight environment, the process noise covariance matrix reflecting the uncontrollable factors of flight and the observation noise covariance matrix reflecting the measurement error of the sensor are set. After initialization, the real-time pose data collected by the GPS several times per second and the IMU at a high frequency of several hundred Hertz are continuously input into the filter. The filter uses an internal algorithm for iterative calculation. First, it predicts the current state based on the state estimate value of the previous moment and the process model constructed based on the dynamic characteristics of the UAV, and then combines the Kalman gain that dynamically adjusts the weight according to the covariance matrix of the observation error and the prediction error to fuse the sensor observation value and the prediction value. After continuous iteration, according to the preset future times such as 1 second, 2 seconds, 3 seconds, etc. by the user, the position and attitude of the UAV at the corresponding times are calculated and arranged in order to form a flight pose prediction sequence value, providing a key data basis for subsequent analysis.

[0040] Step S320, generating flight prediction trajectory information according to the flight pose prediction sequence value. Specifically, after obtaining the flight pose 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. Since different application scenarios have different requirements for the coordinate system, coordinate conversion may be required. For example, map projection is used to convert geographic coordinates into a plane rectangular coordinate system to adapt to local area applications. Then, connect the extracted and converted position coordinates in chronological order to initially generate a flight prediction trajectory. However, due to sensor noise and prediction uncertainty, the trajectory will have fluctuations. Therefore, common smoothing algorithms such as the moving average method are 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 a smoother and more accurate flight prediction trajectory information.

[0041] Step S330: Calculate the vector difference of the flight prediction trajectory information based on the preset flight path to obtain the flight trajectory deviation parameter. Specifically, to calculate the flight trajectory deviation parameter by calculating the vector difference of the flight prediction trajectory information based on the preset flight path, first discretize the preset flight path (composed of a series of waypoints including longitude, latitude, and altitude arranged in sequence) and the flight prediction trajectory (constituted by the position points at different times predicted by the Kalman optimized filter) into line segments between adjacent points, and correspond them one by one according to the time or spatial position relationship. For example, P 1 、P 2 、P 3 on the preset path correspond to Q 1 、Q 2 、Q 3 on the prediction trajectory. Then construct vectors. The adjacent points P i and P i+1 on the preset path form a vector whose modulus length is calculated by the three-dimensional space two-point distance formula. The corresponding adjacent points Q i and Q i+1 on the flight prediction trajectory form a vector with the same modulus length calculation method. Then calculate the direction deviation. Use the vector dot product formula to calculate the cosine value of the angle between the preset path vector and the prediction trajectory vector, and then obtain the angle through the inverse trigonometric function. This angle is the direction deviation, positive for clockwise deviation and negative for counterclockwise deviation. Then calculate the specific deviation value. Calculate the differences in the x, y, and z-axis directions of the coordinates of the corresponding vector endpoints, and use the Euclidean distance formula to calculate the comprehensive deviation value, which intuitively reflects the magnitude of the position deviation. Finally, summarize the direction deviations and specific deviation values of each pair of corresponding vectors, and organize them in the order of flight time or path points to form a flight trajectory deviation parameter set. This set can be used for real-time adjustment of UAV flight control, flight performance evaluation, and path planning optimization.

[0042] Step S400, trend judgment 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. Specifically, after obtaining the flight trajectory deviation parameters, they are first sorted in order of time or path points, and the moving average method is used to smooth the data and the linear regression analysis is used to determine the trend, so as to identify the trend of the direction deviation and the horizontal, vertical, and lateral specific deviation values, such as whether the direction deviation continues to increase or decrease, whether the vertical deviation increases over time, etc. Then, the strategy analysis is performed to formulate a three-axis compensation flight strategy: the horizontal plane adjustment strategy adjusts the heading angle and the horizontal velocity vector component according to the direction and horizontal position deviation trend; the vertical direction adjustment strategy adjusts the ascent or descent rate and fine-tunes the attitude according to the vertical position deviation trend; the lateral offset adjustment strategy is implemented by controlling the aileron and other mechanisms according to the lateral deviation trend. Finally, the three strategies are integrated, and the effect of correcting the deviation under different flight conditions is verified by computer simulation. The strategy parameters are optimized according to the simulation results to ensure that each strategy is coordinated and can enable the drone to return to the preset path stably and accurately.

[0043] In a possible implementation, the flight trajectory deviation parameter is 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. Step S400 further includes step S410, in which the flight trajectory deviation parameter is subjected to change trend judgment according to a time series to determine trajectory deviation trend information. Specifically, to judge the change trend of the flight trajectory deviation parameter 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. Since the sensor is susceptible to noise interference during flight, resulting in erroneous data, it is necessary to exclude extremely abnormal data points that suddenly appear in the vertical direction, and to standardize the parameters of different units for easy comparison. Then, trend analysis methods are used, such as the moving average method (taking the horizontal deviation as an example, a 5-time window is set, and the horizontal deviation values ​​of the current and previous 4 time points are accumulated and divided by 5 to smooth the data) to highlight the long-term trend, and polynomial fitting is used for the deviation of complex trends (such as vertical deviation) (using the least squares method to assume that it has a quadratic polynomial relationship with time, and analyze the deviation change rate and change reflected by the function coefficients). Finally, the trends of the deviation parameters in each direction are summarized, such as the direction deviation may continue to increase clockwise, the horizontal deviation first increases slowly and then stabilizes, and the vertical deviation first rises and then falls after takeoff to approach the preset value, etc., to form complete trajectory deviation trend information, providing a basis for subsequent strategy formulation.

[0044] Step S420: Based on the trajectory deviation trend information, perform three-axis adjustment strategy analysis respectively to obtain a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy. Specifically, after obtaining the trajectory deviation trend information, perform three-axis adjustment strategy analysis. For the horizontal plane adjustment strategy, if the direction deviation continuously increases clockwise, instruct the UAV to rotate counterclockwise by a corresponding angle according to the severity of the deviation. For example, if it increases clockwise by 1 degree per second in the past 10 seconds, it can be instructed to rotate counterclockwise by 5 degrees. At the same time, if the horizontal position deviation gradually increases in a certain direction (such as eastward), by adjusting the component of the velocity vector on the horizontal plane, change the original velocity of 5 m / s eastward and 0 m / s westward to 3 m / s eastward and 1 m / s westward, so that the UAV approaches the preset path on the horizontal plane. In terms of the vertical direction adjustment strategy, if the vertical direction deviation continuously increases, that is, the actual height is higher than the preset height and the gap becomes larger, the rising rate can be reduced or the falling rate can be increased, and the pitch angle is adjusted to assist in control. When the deviation is large, increase the pitch angle to make the head slightly downward, and when the deviation is negative, decrease the pitch angle to make the head slightly upward. In the lateral offset adjustment strategy, if the lateral deviation continuously increases to the left, appropriately increase the deflection angle of the right aileron to generate a lateral force to the right to push the UAV back on track. According to the magnitude and change speed of the lateral deviation, when the deviation is rapid and large, quickly and significantly adjust the aileron, and when the deviation is slow and small, slowly and slightly adjust it to achieve precise lateral offset adjustment.

[0045] Step S430: Based on the horizontal plane adjustment strategy, the vertical direction adjustment strategy, and the lateral offset adjustment strategy, determine the three-axis compensation flight 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 to construct a comprehensive control model to make each strategy operate in coordination. For example, when adjusting the heading angle and the velocity vector component on the horizontal plane, it does not affect the vertical and lateral adjustments. When adjusting the rate and pitch angle in the vertical direction and the aileron deflection angle in the lateral adjustment, they also cooperate with each other to avoid conflicts. The adjustment instructions for the three directions are output simultaneously by the model to form a strategy framework. Subsequently, before actual application, with the help of computer simulation, set different initial deviations, wind speeds, and flight environments, observe the results of the UAV flying according to this strategy, evaluate its effect of returning to the preset path, stability, and flight performance, and then optimize the strategy parameters according to the simulation results, such as adjusting the excessive heading angle adjustment amplitude, unreasonable vertical direction adjustment rate, etc. After repeated simulation optimization, make the strategy adapt to complex scenarios and deviations to ensure that the UAV accurately and stably flies along the preset path.

[0046] Step S500: Analyze the real-time pose of the UAV and the flight trajectory deviation parameters by using the three-axis compensation flight strategy, determine the three-axis adjustment amounts of the flight trajectory, and perform flight trajectory compensation and correction on the target UAV based on the three-axis adjustment amounts of the flight trajectory. Specifically, when analyzing the real-time pose of the UAV and the flight trajectory deviation parameters by using the three-axis compensation flight strategy, first associate each sub-strategy in the strategy with the real-time pose (position and attitude information) of the UAV obtained by sensors such as GPS and IMU. For example, determine the target heading angle by combining the horizontal plane adjustment strategy with the real-time heading angle, and at the same time, combine the flight trajectory deviation parameters. For instance, the vertical direction adjustment strategy determines the upward or downward speed adjustment value according to the vertical position deviation, and the lateral offset adjustment strategy determines the aileron deflection angle adjustment value according to the lateral offset deviation. Then determine the three-axis adjustment amounts of the flight trajectory. Calculate the heading angle adjustment value and the speed component adjustment amounts in the longitude and latitude directions on the horizontal plane, determine the upward or downward speed adjustment amount and the pitch angle adjustment amount in the vertical direction, and calculate the aileron deflection angle adjustment amount and the roll angle adjustment amount in the lateral offset direction. Finally, perform flight trajectory compensation and correction based on these adjustment amounts, convert the adjustment amounts into adjustment instructions for the power and attitude control systems, transmit them to the actuator through the internal communication link of the flight control system, and the actuator adjusts the flight state of the UAV in real time accordingly. During the process, the sensors monitor and feedback information in real time, and the flight controller continuously adjusts the adjustment amounts accordingly to ensure that the UAV flies accurately and stably along the preset path.

[0047] In a possible implementation, the real-time pose of the UAV and the flight trajectory deviation parameters are analyzed by using the three-axis compensation flight strategy to determine the three-axis adjustment amounts of the flight trajectory, and the flight trajectory of the target UAV is compensated and corrected based on the three-axis adjustment amounts of the flight trajectory. Step S500 further includes step S510, in which the deviation compensation and dynamic obstacle avoidance analysis are performed on the real-time pose of the UAV and the flight trajectory deviation parameters by using the horizontal plane adjustment strategy to determine the horizontal plane trajectory adjustment amounts. Specifically, when determining the horizontal plane trajectory adjustment amounts, the pose information such as longitude, latitude, and heading angle is first obtained in real time by sensors such as the GPS and IMU of the UAV, and at the same time, the flight trajectory deviation parameters such as the direction deviation and the horizontal position deviation are obtained. The two are integrated and correlated for evaluation to understand the difference between the actual and the preset states. When performing the deviation compensation analysis, for the direction deviation, if it deviates clockwise, the counterclockwise rotation angle is determined by comprehensively considering multiple factors according to the strategy, and the heading is adjusted by controlling the motor speed difference; for the horizontal position deviation, the speed component adjustment amounts are calculated according to the longitude and latitude direction deviations, and the motor output power is controlled to change the moving speed for compensation. When realizing the dynamic obstacle avoidance analysis, the visual sensor and the millimeter wave radar are used to detect and identify obstacles, and the position, speed and other information provided by them are fused. According to the strategy, if the obstacle is on the right front, the UAV is instructed to slightly adjust the heading angle to the left and reduce the speed to avoid the obstacle. During the obstacle avoidance process, it is continuously monitored and the strategy is adjusted according to the real-time information. Finally, based on the results of the deviation compensation and the dynamic obstacle avoidance, a horizontal plane trajectory adjustment plan including adjustment amounts such as the heading angle and the horizontal speed component is determined.

[0048] Step S520, based on the vertical direction adjustment strategy, perform air pressure data, TOF sensor data analysis and altitude locking compensation on the real-time pose of the UAV and the flight trajectory deviation parameters to determine the vertical trajectory adjustment amounts. Specifically, to determine the vertical trajectory adjustment amounts, the UAV uses sensors to collect the altitude, vertical position deviation, as well as the air pressure and TOF sensor data in real time, and analyzes the vertical flight state according to the vertical direction adjustment strategy. For example, at a certain moment, all relevant data can be known and the flight state can be initially judged whether it is normal and the degree of deviation. Since the air pressure sensor is easily interfered by temperature and air pressure fluctuations, and the TOF sensor has deviations in complex environments, it is necessary to compare the data of the two, fuse the data by setting the accuracy weights to obtain the calibrated altitude, which provides a reliable basis for the altitude locking compensation. When the vertical direction deviation shows that the actual altitude is higher than the preset altitude, according to the strategy, the rising rate is reduced or the falling rate is increased. For example, the adjusted rate is obtained by subtracting the product of the coefficient and the deviation from the current rising rate, and at the same time, the pitch angle adjustment amount is calculated by combining the algorithm. The pitch angle is increased when it is higher than the preset altitude, and vice versa, so as to achieve the precise control and correction of the vertical direction flight trajectory. Finally, a vertical trajectory adjustment plan including the rate and pitch angle adjustment amounts is determined.

[0049] Step S530, perform PID control compensation on the real-time pose of the UAV and the flight trajectory deviation parameters by using the lateral offset adjustment strategy to obtain the lateral trajectory adjustment amount. Specifically, the UAV obtains the real-time pose through sensors such as GPS and IMU. The latitude and longitude of GPS, combined with the map and the preset path, determine the horizontal position. The attitude data measured by IMU assists in analyzing the lateral offset. At the same time, the lateral offset deviation is collected. For example, when setting a north-south path, it reflects the deviation distance in the east-west direction. Since the sensor data is easily interfered, algorithms need to be used for processing. For example, Kalman filtering is used to denoise the GPS data, calibrate and filter the IMU data, and integrate to obtain accurate data for PID control compensation. The PID control algorithm calculates the control amount based on the deviation between the lateral position of the UAV and the ideal position in the vertical direction of the preset path. The proportional link generates a proportional control signal according to the size of the deviation. If the deviation is positive, it drives the UAV to adjust to the left, and the proportional coefficient affects the response speed and stability. The integral link accumulates the past deviations to eliminate the steady-state error, enabling the UAV to accurately return to the preset path. The integral coefficient determines the speed and oscillation degree of error elimination. The derivative link predicts the trend according to the change speed of the deviation and adjusts the control amount in advance to make the system response stable. The derivative coefficient determines the sensitivity to the change speed. Input the real-time lateral offset deviation into the PID controller. After calculation by each link, the total control amount is obtained, which is used to adjust the aileron deflection angle. If the control amount is positive, increase the right aileron deflection angle to make the UAV move left to reduce the deviation. After the UAV is adjusted, the sensor monitors the new pose and deviation and feeds them back to the PID controller. According to the feedback, if the deviation becomes smaller, reduce the control amount to prevent over-adjustment. If the deviation is large or increasing, increase the control amount to accelerate the adjustment. Through continuous feedback optimization, the appropriate lateral trajectory adjustment amount is determined to ensure that the UAV flies accurately and stably along the preset path and eliminates the lateral offset deviation.

[0050] Step S540: Determine the three-axis adjustment amount of the flight trajectory based on the horizontal plane trajectory adjustment amount, the vertical trajectory adjustment amount, and the lateral trajectory adjustment amount. Specifically, to determine the three-axis adjustment amount of the flight trajectory, it is necessary to first integrate and analyze the adjustment amounts in 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 for assisting altitude control. The lateral trajectory adjustment amount uses the PID control algorithm to obtain the aileron deflection angle adjustment amount through comprehensive operations of the proportional, integral, and differential links based on the lateral offset deviation. After integrating these adjustment amounts and analyzing their comprehensive effects and mutual relationships on the UAV flight trajectory, collaborative optimization is carried out. For example, when adjusting the heading angle, consider its impact on the vertical and lateral directions. If the amplitude is large, then according to the feedback of the vertical and lateral adjustment amounts, reduce the amplitude or make synchronous adjustments to maintain stability; when adjusting the vertical direction rate, if it affects the horizontal and lateral directions, fine-tune the horizontal speed and aileron deflection angle according to the corresponding adjustment amounts; when laterally adjusting the aileron deflection angle, take into account the horizontal and vertical directions and reasonably optimize according to requirements. Finally, through collaborative optimization, comprehensively determine the three-axis adjustment amount of the flight trajectory, including accurate adjustment values such as the heading angle, horizontal speed component, ascent or descent rate, pitch angle, and aileron deflection angle, so that the UAV can fly stably according to the preset trajectory, compensate for the deviation, and complete the flight mission.

[0051] In a possible implementation manner, the PID control compensation is performed on the real-time pose of the UAV and the flight trajectory deviation parameters by using the lateral offset adjustment strategy to obtain the lateral trajectory adjustment amount. Step S530 further includes step S531: Initialize the PID controller according to the lateral offset adjustment strategy. The parameter information of the PID controller includes the proportional control term, the integral control term, and the differential control term. Specifically, the lateral offset adjustment strategy relies on the PID controller to correct the deviation in the vertical direction between the UAV and the preset path. When initializing the PID controller, clarify its role of receiving the real-time pose and flight trajectory deviation parameters and outputting the lateral motion control signal. Set the proportional control term, and determine the proportional coefficient K according to the UAV characteristics and the flight environment p , such as a high-speed maneuvering UAV is adapted to a large K p to quickly respond to the deviation, while a low-speed stable flight one uses a small K p . The integral control term eliminates the steady-state error, and set the integral coefficient K according to the interference situation i , if there is more interference, K can be increased i , but oscillation should be prevented. The differential control term predicts and adjusts the control amount in advance according to the deviation change rate, and determines the differential coefficient K according to the flight scenario d , in a fast-response mission, when being sensitive to the deviation change, a large K can be set d, while paying attention to avoiding noise interference. Based on the PID controller for compensation closed-loop calculation, the drone obtains pose and lateral offset deviation data through sensors such as GPS and IMU. Since the data is vulnerable to interference, it is preprocessed first. For example, Kalman filtering is used to process GPS data and calibrate and compensate IMU data. The preprocessed data is input into the PID controller. The proportional term generates a control signal according to the deviation to make the drone approach the path. The integral term accumulates the deviation to eliminate the steady-state error. The derivative term adjusts the control quantity in advance according to the deviation change rate. The final control quantity is obtained by synthesizing the three terms. If the control quantity is positive, the deflection angle of the right aileron is increased to generate a lateral force to the left to reduce the deviation. After the drone executes the command, the sensor feeds back the new pose and deviation to the PID controller. When the deviation decreases, the control quantity is appropriately reduced to prevent overshoot. When the deviation is large or increasing, the control quantity is increased to speed up the adjustment. Through continuous feedback optimization, the appropriate lateral trajectory adjustment quantity is determined, so that the drone can fly precisely along the preset path and eliminate the lateral offset deviation.

[0052] Step S532, based on the PID controller, perform compensation closed-loop calculation on the real-time pose of the drone and the flight trajectory deviation parameters to obtain the lateral trajectory adjustment quantity. Specifically, the drone collects the real-time pose through GPS and IMU, and synchronously obtains the lateral offset deviation. Since the sensor data is vulnerable to interference, Kalman filtering is used to reduce the noise of GPS data, and the IMU data is calibrated to eliminate the drift error. 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 magnitude, and the proportional coefficient K p determines the response speed and stability. A large K p has a fast response but is prone to overshoot. A small K p has a slow adjustment but is stable. The integral control term accumulates the deviation to eliminate the steady-state error, and the integral coefficient K i determines the elimination speed and oscillation degree. A large K i eliminates the error quickly but is prone to oscillation. A small K i has a slow process. The derivative control term adjusts the control quantity in advance according to the deviation change rate, and the derivative coefficient K d determines the sensitivity to the change rate. A large K d is sensitive but prone to amplifying noise. A small K d has a slow response. The PID controller synthesizes the three signals to obtain the final control quantity, which is used to adjust the aileron deflection angle. When the control quantity is positive, the deflection angle of the right aileron is increased to make the drone move leftward to reduce the deviation. After the drone executes the command, the sensor feeds back the new pose and deviation to the PID controller. When the deviation decreases, the control quantity is reduced to prevent overshoot. When the deviation is large or increasing, the control quantity is increased to speed up the adjustment. Through repeated feedback optimization, the appropriate lateral trajectory adjustment quantity is determined, so that the drone can fly precisely along the preset path and eliminate the lateral offset deviation.

[0053] In the above text, refer to Figure 1A method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control according to an embodiment of the present invention is described in detail. Next, reference will be made to Figure 2 Describe an unmanned aerial vehicle flight trajectory correction system with adaptive control according to an embodiment of the present invention.

[0054] The unmanned aerial vehicle flight trajectory correction system with adaptive control according to an embodiment of the present invention is used to solve the technical problem of low efficiency in real-time accurate correction of the flight trajectory of existing unmanned aerial vehicles in complex environments. Through industrial automatic control system device manufacturing technologies (such as dynamic analysis of the three-axis compensation strategy) and intelligent monitoring devices (such as trajectory deviation detection by multi-sensor fusion), the technical effect of shortening the dynamic compensation response time of the unmanned aerial vehicle flight trajectory deviation is achieved. The unmanned aerial vehicle flight trajectory correction system with adaptive control includes: a flight environment space unit model acquisition module 10, a real-time pose determination module 20 of the unmanned aerial vehicle, 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 obtain a flight environment data set through a sensor device group carried on the target unmanned aerial vehicle, and perform three-dimensional modeling and unit division based on the flight environment data set to obtain a flight environment space unit model.

[0056] The real-time pose determination module 20 of the unmanned aerial vehicle is used to perform 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 unmanned aerial vehicle.

[0057] The flight trajectory deviation parameter acquisition module 30 is used to plan and determine a preset flight path, and perform trajectory offset prediction on the real-time pose of the unmanned aerial vehicle according to the preset flight path to 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, and the three-axis compensation flight strategy 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 analyze the real-time pose of the unmanned aerial vehicle and the flight trajectory deviation parameters by using the three-axis compensation flight strategy to determine the flight trajectory three-axis adjustment amount, and perform flight trajectory compensation correction on the target unmanned aerial vehicle 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 by the sensor device group carried on the target unmanned aerial vehicle (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 configured 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 configured to perform triangulation and Poisson surface reconstruction on the flight environment point cloud data to generate an initial flight environment three-dimensional model; an enhancement and optimization unit configured to match and map the flight environment image data into the initial flight environment three-dimensional model for enhancement and optimization to obtain a target flight environment three-dimensional model; a flight trajectory analysis requirement unit configured to determine a unit division type and a unit division density according to flight trajectory analysis requirements; and a space unit division unit configured to perform space unit division on the target flight environment three-dimensional model according to the unit division type and the unit division density to obtain the flight environment space unit model.

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

[0062] Among them, Kalman filtering processing and pose estimation are performed based on the set of matched spatial feature points to determine the real-time pose of the UAV. The real-time UAV pose determination unit further includes: a Kalman filter initialization subunit, which is used to initialize the Kalman filter according to the initial flight state of the target UAV and the noise characteristic information of the sensor device group; a UAV flight state fitting subunit, which is used to fit the UAV flight state based on the flight pose state data in the flight environment dataset, construct a UAV dynamics model, and predict the estimated value of the flight state sequence according to the UAV dynamics model; a Kalman optimized filter acquisition subunit, which is used to perform covariance prediction and iterative update on the Kalman filter based on the flight pose state data and the estimated value of the flight state sequence to obtain a Kalman optimized filter; a pose state estimation subunit, which is used to perform pose state estimation on the set of matched spatial feature points by using the Kalman optimized filter to determine the real-time pose of the UAV.

[0063] Among them, covariance prediction and iterative update are performed on the Kalman filter based on the flight pose state data and the estimated value of the flight state sequence to obtain a Kalman optimized filter. The Kalman optimized filter acquisition subunit further includes: a flight state sequence observation value acquisition micro-unit, which is used to obtain the flight state sequence observation value according to the flight pose state data; a covariance prediction micro-unit, which is used to perform covariance prediction based on the error between the flight state sequence observation value and the estimated value of the flight state sequence, and generate an observation error covariance matrix and a prediction error covariance matrix respectively; a difference gain calculation micro-unit, which is used to perform difference gain calculation on the observation error covariance matrix and the prediction error covariance matrix by using the Kalman filter to obtain Kalman gain information; an iterative optimization update micro-unit, which is used to perform iterative optimization update on the Kalman filter according to the Kalman gain information to obtain the Kalman optimized filter.

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

[0065] Next, the specific configuration of the three-axis compensation flight strategy acquisition module 40 will be described in detail. As described above, the trend of the flight trajectory deviation parameters is judged and the strategy is analyzed 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 judge the change trend of the flight trajectory deviation parameters according to the time series to determine the trajectory deviation trend information; a three-axis adjustment strategy analysis unit, which is used to analyze the three-axis adjustment strategies respectively 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; 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] Next, the specific configuration of the flight trajectory three-axis adjustment amount determination module 50 will be described in detail. As described above, the real-time pose of the UAV and the flight trajectory deviation parameters are analyzed by using the three-axis compensation flight strategy to determine the three-axis adjustment amount of the flight trajectory, and the flight trajectory of the target UAV is compensated and corrected based on the three-axis adjustment amount of the flight trajectory. The flight trajectory three-axis adjustment amount determination module 50 further includes: a horizontal plane trajectory adjustment amount determination unit, which is used to perform deviation compensation and dynamic obstacle avoidance analysis on the real-time pose of the UAV and the flight trajectory deviation parameters by using the horizontal plane adjustment strategy to determine the horizontal plane trajectory adjustment amount; a vertical trajectory adjustment amount determination unit, which is used to perform air pressure data, TOF sensor data analysis and altitude locking compensation on the real-time pose of the UAV and the flight trajectory deviation parameters based on the vertical direction adjustment strategy to determine the vertical trajectory adjustment amount; a lateral trajectory adjustment amount acquisition unit, which is used to perform PID control compensation on the real-time pose of the UAV and the flight trajectory deviation parameters by using the lateral offset adjustment strategy to obtain the lateral trajectory adjustment amount; a flight trajectory three-axis adjustment amount determination unit, which is used to determine the three-axis adjustment amount of the flight trajectory based on the horizontal plane trajectory adjustment amount, the vertical trajectory adjustment amount and the lateral trajectory adjustment amount.

[0067] Among them, when performing PID control compensation on the real-time pose of the UAV and the flight trajectory deviation parameters by using the lateral offset adjustment strategy to obtain the lateral trajectory adjustment amount, the lateral trajectory adjustment amount acquisition unit further includes: a controller parameter information composition subunit, which 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 term, an integral control term and a derivative control term; a compensation closed-loop calculation subunit, which is used to perform compensation closed-loop calculation on the real-time pose of the UAV and the flight trajectory deviation parameters based on the PID controller to obtain the lateral trajectory adjustment amount.

[0068] The UAV flight trajectory correction system with adaptive control provided by the embodiments of the present invention can execute the UAV flight trajectory correction method with adaptive control provided by any embodiment of the present invention, and has the corresponding functional modules and beneficial effects for executing the method.

[0069] Although the present application makes various references to certain modules in the system according to embodiments of the present application, however, any number of different modules can be used and run on the user terminal and / or the server. 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 mutual distinction and do not limit the protection scope of the present invention.

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

Claims

1. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control is characterized in that: 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; 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 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.

2. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control as claimed in claim 1, characterized in that: 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 according to the flight trajectory analysis requirements; The three-dimensional model of the target flight environment is spatially divided into units according to the unit division type and the 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 as claimed in claim 1, characterized in that: Determining the real-time position and posture of the drone includes: Extracting 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; Performing multi-dimensional feature description on the key point set of the space model and the key point set of the current flight environment in turn to obtain a feature description subset of a point cloud of the space model and a feature description subset of a point cloud of the current flight environment; Using the ICP algorithm to perform point cloud matching calculation on the key point set of the spatial model and the key point set of the current flight environment according to the point cloud feature description subset of the spatial model and the point cloud feature description subset of the current flight environment, to obtain a set of matching spatial feature points; Kalman filtering and posture estimation are performed based on the set of matching spatial feature points to determine the real-time posture of the drone.

4. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control as claimed in claim 3, characterized in that: Determining the real-time position and 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; Fitting the flight state of the UAV based on the flight posture state data in the flight environment data set, building a UAV dynamics model, and predicting the flight state sequence estimation value according to the UAV dynamics model; Performing 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; The Kalman optimization filter is used to estimate the posture state of the matching space 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 as claimed in claim 4, characterized in that: 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 as claimed in claim 4, characterized in that: The obtaining of the flight trajectory deviation parameter comprises: Based on the Kalman optimization filter, the real-time posture of the UAV is predicted to perform flight state prediction to obtain flight posture prediction sequence values ​​at multiple preset moments; Generate flight prediction trajectory information according to the flight posture prediction sequence value; A vector difference calculation is performed on the predicted flight 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 as claimed in claim 1, characterized in that: The three-axis compensation flight strategy is obtained, including: Judging the change trend of the flight trajectory deviation parameter according to the time series, and determining trajectory deviation trend information; Based on the trajectory deviation trend information, three-axis adjustment strategy analysis is performed to obtain a horizontal plane adjustment strategy, a vertical direction adjustment strategy, and a lateral offset adjustment strategy; The three-axis compensation flight strategy is determined based on the horizontal plane adjustment strategy, the vertical direction adjustment strategy and the lateral offset adjustment strategy.

8. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control as claimed in claim 1, characterized in that: 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 by 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 adjustment amount of the flight trajectory is determined based on the horizontal trajectory adjustment amount, the vertical trajectory adjustment amount and the lateral trajectory adjustment amount.

9. The method for correcting the flight trajectory of an unmanned aerial vehicle with adaptive control as claimed in claim 8, characterized in that: The obtaining of the lateral trajectory adjustment amount comprises: 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.

10. The adaptive control UAV flight trajectory correction system is characterized by: The system is applied to the adaptively controlled UAV flight trajectory correction method according to claim 1, and the system comprises: 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, perform three-dimensional modeling and unit division based on the flight environment data set, and obtain a flight environment space unit model; A drone real-time posture determination module, the drone 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 drone real-time posture; A flight trajectory deviation parameter acquisition module, wherein the 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 a flight trajectory deviation parameter; 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