Unmanned aerial vehicle tracking control system and method based on three-dimensional model
Through a drone tracking control system based on three-dimensional model, point cloud data is used to predict target moving paths and environmental changes, and the drone flight path is optimized, which solves the problem of tracking path in complex environments and improves image data clarity and Track the effect.
Patent Information
- Application Number
- CN202410968127.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-18
- Publication Date
- 2024-11-08
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In complex environments, the drone tracking path cannot adapt to the target moving path and environmental changes in real time, resulting in the image data being not clear enough, affecting the tracking effect.
Adopting a drone tracking and control system based on three-dimensional models, a point cloud three-dimensional model is established through point cloud data, target movement paths and environmental changes are predicted, the drone flight paths are optimized, and the image data is improved.
It significantly improves the clarity and quality of image data, improves the overall effect of drones in tracking mobile targets, and provides more accurate environmental perception and dynamic obstacle avoidance capabilities.
Smart Images

Figure CN118915782A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned aerial vehicle tracking control, and in particular to a three-dimensional model-based unmanned aerial vehicle tracking control system and method. Background Art
[0002] In the process of continuous tracking of moving targets by UAVs, the flight path of the UAV is mainly controlled by image processing and navigation algorithms. These technologies can achieve good tracking when the target is relatively stationary or the motion trajectory is simple. When dealing with fast-moving targets, tracking loss and image blur are prone to problems, and continuous and clear image data collection cannot be guaranteed.
[0003] Traditional drone tracking systems usually rely on a single visual sensor and lack the ability to fully perceive the surrounding environment, which makes it difficult to effectively avoid obstacles and maintain continuous tracking of the target in environments with many obstacles. For example, in complex environments such as cities or forests, the target may be blocked by obstructions, making it difficult for the drone's tracking path to remain optimal. Existing path planning algorithms lack real-time performance and accuracy in complex environments, and are unable to cope with the rapid movement of targets and dynamic changes in the environment, resulting in lagging and unreasonable tracking paths. Existing systems lack effective target movement path prediction capabilities, making it difficult to plan and adjust the drone's flight path in advance, resulting in inefficiency in actual tracking.
[0004] When a target moves quickly in a complex environment, the UAV's tracking path cannot adapt to the target's moving path and changes in the surrounding environment in real time, which often causes the images captured by the UAV camera to appear blurred and the collected image data to be unclear, affecting the clarity and effectiveness of the image data. Summary of the invention
[0005] In order to solve the problem that the tracking path of the UAV cannot adapt to the changes in the target's moving path and the surrounding environment in real time, resulting in the collected image data not being clear enough, the present invention proposes a UAV tracking control system and method based on a three-dimensional model, which establishes a point cloud three-dimensional model through point cloud data, predicts the moving path of the tracked target according to the surrounding environment data, coordinates the UAV flight tracking path according to the surrounding obstructions and the predicted target moving path, and significantly improves the clarity and quality of the image data by optimizing the UAV's flight tracking path, so that the collected image data achieves the best collection effect, thereby improving the overall effect of the UAV tracking the moving target.
[0006] In order to achieve the above object, the present invention is implemented by the following technical solutions:
[0007] A three-dimensional model-based unmanned aerial vehicle tracking and control system, the system comprising:
[0008] The data acquisition module is used to scan the tracked target and the surrounding environment in real time through the point cloud data acquisition equipment and camera device carried by the drone, and collect three-dimensional point cloud data in real time;
[0009] A data processing module is used to process the three-dimensional point cloud data, divide the three-dimensional point cloud data into target point cloud data, static environment point cloud data and dynamic environment point cloud data, and respectively construct a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model, and fuse the static environment three-dimensional model and the dynamic environment three-dimensional model to form a comprehensive environment model;
[0010] A path prediction module, used to predict a target moving path based on the target three-dimensional model and the comprehensive environment model;
[0011] The flight control module is used to optimize the flight tracking path of the UAV according to the predicted target movement path and the comprehensive environment model.
[0012] As a preferred solution of the present invention, the data processing module includes:
[0013] A data preprocessing unit, used for denoising, downsampling and coordinate transformation of the collected 3D point cloud data;
[0014] A feature extraction unit is used to perform multi-level feature extraction on the converted 3D point cloud data using the PointNet++ model;
[0015] The point cloud segmentation unit is used to segment the three-dimensional point cloud data after feature extraction using the DBSCAN algorithm, and identify target point cloud data, static environment point cloud data, and dynamic environment point cloud data;
[0016] A model building unit, used to build a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model respectively based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data;
[0017] The environment model fusion unit is used to fuse the static environment three-dimensional model and the dynamic environment three-dimensional model to form a comprehensive environment model.
[0018] As a preferred solution of the present invention, the implementation method of the data preprocessing unit includes:
[0019] Use statistical filters to denoise: The collected 3D point cloud data is represented as a set P = {p1, p2, ..., p n}, each point p in the set P i Expressed as three-dimensional coordinates (x i ,y i ,z i ), n is the total amount of 3D point cloud data;
[0020] Select the k nearest neighbor points of each point and calculate the p of each point i The mean distance d to the k nearest neighbors i , the formula is:
[0021]
[0022] Where j represents the current neighbor point index, ranging from 1 to k; p ij Represents point p i The coordinates of the jth nearest neighbor;
[0023] Calculate the mean distance d of all points in set P mean and standard deviation σ d , the formula is:
[0024]
[0025] Set the mean distance threshold to d meand ±σ·σ d , σ is the standard deviation threshold, and the points whose mean distance is within the mean distance threshold are retained as the denoised three-dimensional point cloud data P denoised ;
[0026] Downsampling using voxel grid filtering: define the size v of each voxel size , the denoised three-dimensional point cloud data P denoised Divide into voxel grids, take the average value of the points in each voxel as the representative point of the voxel, and output the downsampled 3D point cloud data P downsampled ;
[0027] The downsampled 3D point cloud data P downsampled Perform coordinate transformation to convert the 3D point cloud data from the drone coordinate system to the global coordinate system. The conversion formula is:
[0028] p i ′=R·p i +t;
[0029] In the formula, p i ′ is the coordinate of the point after transformation, p i is the coordinate of the original point, R is the rotation matrix, and t is the translation vector;
[0030] Output the converted 3D point cloud data P transformed .
[0031] As a preferred solution of the present invention, in the feature extraction unit, the method for performing multi-level feature extraction includes:
[0032] Input the converted 3D point cloud data Ptransformed , set the hyperparameters of the PointNet++ model, including the number of layers, the number of sampling points per layer, and the neighborhood size;
[0033] The farthest point sampling algorithm is used to extract the converted 3D point cloud data P transformed A fixed number of points are sampled in the spherical neighborhood. For each sampling point, a spherical neighborhood is defined. Let the neighborhood radius be r. The neighborhood radius r includes a fixed number of neighborhood points.
[0034] For the neighborhood points of each sampling point, feature transformation is performed through a shared multi-layer perceptron, and the transformed neighborhood point features are subjected to maximum pooling operation to generate the local feature vector of the sampling point;
[0035] Repeat multiple grouping layers, extract features layer by layer, use the output of each layer as the input of the next layer, and in the last layer, use global maximum pooling to pool the features of all points to generate a global feature vector;
[0036] Output point cloud feature F after feature extraction features , including local features and global feature vectors.
[0037] As a preferred solution of the present invention, in the point cloud segmentation unit, the point cloud feature F after feature extraction is features The segmentation is performed using the DBSCAN algorithm, which includes the following steps:
[0038] Define parameters: define the neighborhood radius ∈ and the minimum number of points MinPts;
[0039] Initialization: mark all points as unvisited in preparation for traversal and clustering;
[0040] Traverse each point: point cloud feature F features Traverse each point in, if the point has not been visited, then perform the following steps:
[0041] Determine the core point: Calculate the number of points in the ∈-neighborhood of the current point. If the minPts condition is met, mark it as a core point and start expanding the cluster; otherwise, mark it as a noise point;
[0042] Expand the cluster: traverse the neighborhood of the core point, add the points that meet the minPts condition to the cluster, and continue to expand until the cluster stops growing;
[0043] Repeat the above steps for all unvisited points until all points have been visited;
[0044] According to the feature extraction results and the spatial position of the cluster, different types of point cloud features are identified and the 3D point cloud data is segmented into target point cloud data P target , static environment point cloud data P staticand dynamic environment point cloud data P dynamic .
[0045] As a preferred solution of the present invention, in the model building unit, the method for building the target three-dimensional model is:
[0046] Input target point cloud data P target And divide it into voxel grids and set the size of each voxel;
[0047] Calculate the density value of the points within each voxel;
[0048] For each voxel, the Marching Cubes algorithm is used to extract the isosurface from the voxel data;
[0049] According to the density value of the points in the voxel, the vertex position of the isosurface is determined, and the triangular mesh of the target 3D model is generated using the predefined lookup table, and the target 3D model M is output. target ;
[0050] The construction method of the static environment 3D model is:
[0051] Use octree to analyze static environment point cloud data P static Perform hierarchical partitioning and recursively divide the space into eight subspaces until the maximum depth d is reached, and store the corresponding static environment point cloud data in each leaf node;
[0052] Based on the octree structure, Poisson's equation is used to calculate the static environment point cloud data P static Perform surface reconstruction to generate a static environment 3D model M static ;
[0053] The reconstructed 3D model is post-processed using the QEM algorithm;
[0054] The iSAM method is used to construct a dynamic environment 3D model, including the following steps:
[0055] Set the initial posture X0 of the drone and create a factor graph G to store all posture nodes and observation factors;
[0056] For the input dynamic environment point cloud data P dynamic Perform preprocessing operations such as denoising and downsampling;
[0057] Add a pose node X to the factor graph for each time t t , create the odometer factor, use the UAV's motion model, and connect the continuous pose nodes through the odometer factor. The formula is:
[0058] f odom (X t ,X t+1 )=||Xt+1 -X t -u t || 2 ;
[0059] Where, X t is the pose node at time t, X t+1 is the pose node at time t+1; u t is the motion control input at time t; f odom (X t ,X t+1 ) represents the odometer factor;
[0060] The preprocessed dynamic environment point cloud data is added as observation to the factor graph, and an observation factor is added to each observation point. The formula is:
[0061] f obs (X t ,q m )=||h(X t )-q m || 2 ;
[0062] In the formula, q m represents the mth observation point in the dynamic environment point cloud data; h(X t ) is based on the pose X t Calculated observational predictions; f obs (X t ,q m ) is the observation factor;
[0063] Use the iSAM method to incrementally optimize the factor graph and update the pose estimation and factor graph;
[0064] According to the updated position estimation and factor graph, all observation points are transformed into the global coordinate system to construct a 3D model M of the dynamic environment. dynamic .
[0065] As a preferred solution of the present invention, in the environment model fusion unit, the static environment 3D model and the dynamic environment 3D model are fused using the ICP algorithm, which specifically includes:
[0066] Input static environment 3D model M static and dynamic environment 3D model M dynamic , and set the maximum number of iterations N iter and convergence tolerance δ;
[0067] The dynamic environment 3D model M dynamic With the static environment 3D model M static Perform initial alignment;
[0068] Repeat the following steps until the maximum number of iterations N is reached. iter Or the difference between the models before and after the transformation is less than the convergence tolerance δ:
[0069] Closest point matching: Find the dynamic environment 3D model M dynamic Each point in the static environment 3D model M static The closest point pair in
[0070] Compute transformation: Use the singular value decomposition algorithm to compute the rigid body transformation that aligns the closest points;
[0071] Apply transformation: Apply the calculated rigid body transformation to the dynamic environment 3D model M dynamic Update the dynamic environment 3D model M dynamic The position of each point in
[0072] Output the aligned dynamic environment 3D model M dynamic_aligned ;
[0073] The aligned dynamic environment 3D model M dynamic_aligned and static environment 3D model M static The corresponding points in are respectively corresponding, and the weighted average method is used to fuse the corresponding points to form a comprehensive environment model M fused .
[0074] As a preferred solution of the present invention, in the path prediction module, an LSTM network is used to predict the target moving path, specifically including:
[0075] Extract a time series data set D from the target three-dimensional model and the comprehensive environment model, where the time series data set D includes an input feature vector at time t and a target position at time t;
[0076] Building an LSTM network: Setting the input feature dimension n features , the number of hidden layer units n hidden , LSTM layer number n layers and output dimension n output (i.e. the dimension of the target position), construct a layers LSTM network of layers, each layer contains n hidden Units, add a fully connected layer after the LSTM layer to map the output of LSTM to the target location;
[0077] Train the LSTM network:
[0078] Input the time series dataset D and the constructed LSTM network, set the batch size, learning rate and number of training rounds;
[0079] Initialize the mean square error loss function and Adam optimizer;
[0080] Input the time series data set D into the LSTM network in batches for forward propagation and calculate the predicted value;
[0081] Calculate the mean square error loss function and update the network weights through the back-propagation algorithm;
[0082] Repeat the above process until all training rounds are completed to form a trained LSTM model;
[0083] The current input feature vector is input into the trained LSTM model for forward propagation, and the predicted target position is output through the LSTM network.
[0084] As a preferred solution of the present invention, the flight control module includes:
[0085] The trajectory optimization unit is used to preliminarily plan the flight tracking path of the drone using the A* algorithm. The methods include:
[0086] Initialize the open list and closed list. The open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. Add the drone starting point to the open list.
[0087] Select the node with the lowest cost function from the open list, the cost function includes the actual cost from the starting point to the node k and the heuristic estimated cost from the node k to the end point, and remove the node with the lowest cost function from the open list and add it to the closed list;
[0088] Evaluate each adjacent node of the current node and calculate the new cost from the starting point to the adjacent node. If the adjacent node is not in the open list or the new cost is lower, update the information of the adjacent node and add it to the open list.
[0089] Repeat the above steps and continuously select the best node from the open list for expansion until the end point is found or the open list is empty; if the end point is found, backtrack to the parent node and output the path from the start point to the end point as the preliminary planned path;
[0090] A path smoothing unit, used to smooth the path using a B-spline interpolation method after finding the initially planned path through the A* algorithm;
[0091] The dynamic obstacle avoidance unit is used to dynamically optimize the smoothed path using a dynamic window method, the method comprising:
[0092] Generate all possible combinations of velocity and acceleration within a dynamic window;
[0093] For each combination, simulate the trajectory of the drone within the time window and predict the future position of the drone;
[0094] Calculate the cost function of each motion trajectory, including path tracking error, collision risk and energy consumption cost;
[0095] The motion trajectory with the minimum cost function is selected as the optimized flight tracking path.
[0096] A three-dimensional model-based unmanned aerial vehicle tracking control method, the method comprising:
[0097] The point cloud data acquisition equipment and camera device carried by the drone scans the tracked target and the surrounding environment in real time, and collects three-dimensional point cloud data in real time;
[0098] De-noising, down-sampling and coordinate transformation of the collected 3D point cloud data;
[0099] Use the PointNet++ model to perform multi-level feature extraction on the converted 3D point cloud data;
[0100] Use the DBSCAN algorithm to segment the 3D point cloud data after feature extraction, and identify the target point cloud data, static environment point cloud data, and dynamic environment point cloud data;
[0101] Based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data, respectively construct a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model;
[0102] The static environment three-dimensional model and the dynamic environment three-dimensional model are integrated to form a comprehensive environment model;
[0103] Based on the target three-dimensional model and the comprehensive environment model, an LSTM network is used to predict the target movement path;
[0104] The flight tracking path of the UAV is optimized according to the predicted target movement path and the comprehensive environment model.
[0105] The beneficial effects of the present invention are as follows: a PointNet++ model is used to perform multi-level feature extraction of three-dimensional point cloud data, and local and global feature vectors are generated by performing feature transformation and maximum pooling operations on the neighborhood points of each sampling point. Using these feature vectors, the system can accurately identify target point cloud data, static environment point cloud data and dynamic environment point cloud data in the environment; the dynamic environment model is incrementally optimized based on the iSAM method, and the static environment model and the dynamic environment model are integrated through the ICP algorithm to form a comprehensive environment model, providing more accurate environmental perception capabilities; an LSTM network is used to predict the moving path of the target, and the LSTM network is trained by a time series data set, which can accurately predict the future position of the target and improve the tracking capability of the drone; after preliminary path planning, the system smoothes the path through the B-spline interpolation method, making the path more natural and smooth, which is conducive to the smooth flight of the drone, and the dynamic obstacle avoidance module uses a dynamic window method to optimize the smoothed path in real time, ensuring that the drone can dynamically avoid obstacles during flight and improve safety. BRIEF DESCRIPTION OF THE DRAWINGS
[0106] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative labor.
[0107] in:
[0108] Figure 1 It is a system modular structure diagram of the present invention;
[0109] Figure 2 4 is a flow chart of a method in an embodiment of the present invention. DETAILED DESCRIPTION
[0110] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all of the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.
[0111] like Figure 1 As shown, it is an embodiment of the present invention, which provides a UAV tracking control system based on a three-dimensional model, including a data acquisition module, a data processing module, a path prediction module and a flight control module.
[0112] (1) The data acquisition module is used to scan the tracked target and the surrounding environment in real time through the point cloud data acquisition equipment and camera device carried by the UAV, and collect three-dimensional point cloud data in real time;
[0113] The drone is equipped with a variety of sensors, including laser radar, depth camera, etc., which scan the tracked target and the surrounding environment in real time to obtain comprehensive three-dimensional point cloud data;
[0114] (2) The data processing module is used to process the 3D point cloud data, segment the 3D point cloud data into target point cloud data, static environment point cloud data and dynamic environment point cloud data, and respectively construct the target 3D model, the static environment 3D model and the dynamic environment 3D model, and fuse the static environment 3D model and the dynamic environment 3D model to form a comprehensive environment model;
[0115] Target point cloud data refers to the three-dimensional coordinate data of the tracked target, which is used to accurately determine the position, shape, motion state, etc. of the tracked target; static environment point cloud data refers to the fixed parts of the drone's surrounding environment, including buildings, trees, road signs, etc.; dynamic environment point cloud data refers to the dynamically changing parts of the drone's surrounding environment, such as moving vehicles, pedestrians, etc. The dynamic environment point cloud data is monitored and updated in real time through multi-sensor fusion technology, and is also converted into three-dimensional point cloud data for real-time updating and adjustment of the dynamic environment three-dimensional model.
[0116] In one of the embodiments, the data processing module includes a data preprocessing unit, a feature extraction unit, a point cloud segmentation unit, a model building unit and an environment model fusion unit.
[0117] The data preprocessing unit is used to perform preprocessing such as denoising, downsampling, and coordinate transformation on the collected 3D point cloud data to ensure the accuracy and validity of the data;
[0118] Preferably, a statistical filter is used for denoising: the collected three-dimensional point cloud data is represented as a set P = {p1, p2, ..., p n}, each point p in the set P i Expressed as three-dimensional coordinates (x i ,y i ,z i ), n is the total amount of 3D point cloud data;
[0119] Select the k nearest neighbors of each point (usually k = 50) and calculate the p i The mean distance d to the k nearest neighbors i , the formula is:
[0120]
[0121] Where j represents the current nearest neighbor index, ranging from 1 to k; p ij Represents point p i The coordinates of the jth nearest neighbor;
[0122] Calculate the mean distance d of all points in set P mean and standard deviation σ d , the formula is:
[0123]
[0124] Set the mean distance threshold to d mean ±σ·σ d , σ is the standard deviation threshold (usually σ = 1.0), retaining the mean distance d i Points within the mean distance threshold are taken as the denoised 3D point cloud data P denoised ;
[0125] Downsampling using voxel grid filtering: define the size v of each voxel size , the denoised three-dimensional point cloud data P denoised Divide into voxel grids, take the average value of the points in each voxel as the representative point of the voxel, and output the downsampled 3D point cloud data P downsampled ;
[0126] The downsampled 3D point cloud data P downsampled Perform coordinate transformation to convert the 3D point cloud data from the drone coordinate system to the global coordinate system. The conversion formula is:
[0127] p i ′=R·p i +t;
[0128] In the formula, p i ′ is the coordinate of the point after transformation, p i is the coordinate of the original point, R is the rotation matrix, and t is the translation vector;
[0129] Output the converted 3D point cloud data P transformed .
[0130] The feature extraction unit is used to perform multi-level feature extraction on the converted three-dimensional point cloud data using the PointNet++ model;
[0131] Preferably, the method for performing multi-level feature extraction includes:
[0132] Input the converted 3D point cloud data P transformed , set the hyperparameters of the PointNet++ model, including the number of layers, the number of sampling points per layer, and the neighborhood size;
[0133] The farthest point sampling algorithm (FPS) is used to extract the converted 3D point cloud data P transformed A fixed number of points are sampled in , and for each sampling point, a spherical neighborhood is defined, and the neighborhood radius is set to r, and the neighborhood radius r includes a fixed number of neighborhood points;
[0134] For the neighborhood points of each sampling point, the shared multi-layer perceptron (MLP) is used to perform feature transformation, and the transformed neighborhood point features are subjected to the maximum pooling operation to generate the local feature vector of the sampling point.
[0135] Repeat multiple grouping layers, extract features layer by layer, use the output of each layer as the input of the next layer, and in the last layer, use global maximum pooling to pool the features of all points to generate a global feature vector;
[0136] Output point cloud feature F after feature extraction features , including local features and global feature vectors.
[0137] The point cloud segmentation unit is used to segment the three-dimensional point cloud data after feature extraction using the DBSCAN algorithm, and identify target point cloud data, static environment point cloud data, and dynamic environment point cloud data;
[0138] DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a density-based clustering algorithm that can identify clusters of any shape and find outliers in the data. Clusters are defined by density connectivity, that is, an area containing a certain number of points within a specified radius is considered to be density-reachable, thus forming a cluster.
[0139] Preferably, the point cloud feature F after feature extraction features The segmentation is performed using the DBSCAN algorithm, which includes the following steps:
[0140] Define parameters: define the neighborhood radius ∈ and the minimum number of points MinPts, usually setting appropriate values through data analysis or experience;
[0141] Initialization: mark all points as unvisited in preparation for traversal and clustering;
[0142] Traverse each point: point cloud feature F features Traverse each point in, if the point has not been visited, then perform the following steps:
[0143] Determine the core point: Calculate the number of points in the ∈-neighborhood of the current point. If the minPts condition is met, mark it as a core point and start expanding the cluster; otherwise, mark it as a noise point, which may be included in other clusters later.
[0144] Expand the cluster: traverse the neighborhood of the core point, add the points that meet the minPts condition to the cluster, and continue to expand until the cluster stops growing;
[0145] Repeat the above steps for all unvisited points until all points have been visited;
[0146] According to the feature extraction results and the spatial position of the cluster, different types of point cloud features are identified and the 3D point cloud data is segmented into target point cloud data P target , static environment point cloud data P static and dynamic environment point cloud data P dynamic Typically, target point cloud data form one or more tight clusters, while static environment point cloud data and dynamic environment point cloud data can be distinguished by analyzing their relative positions and change characteristics.
[0147] The model building unit is used to build a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model respectively based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data;
[0148] The target three-dimensional model is constructed as follows:
[0149] Input target point cloud data P target And divide it into voxel grids and set the size of each voxel;
[0150] Calculate the density value of the points within each voxel;
[0151] For each voxel, the Marching Cubes algorithm is used to extract the isosurface from the voxel data;
[0152] According to the density value of the points in the voxel, the vertex position of the isosurface is determined, and the triangular mesh of the target 3D model is generated using the predefined lookup table, and the target 3D model M is output. target .
[0153] The construction method of the static environment 3D model is:
[0154] Use octree to analyze static environment point cloud data P static Perform hierarchical partitioning and recursively divide the space into eight subspaces until the maximum depth d is reached, and store the corresponding static environment point cloud data in each leaf node;
[0155] Based on the octree structure, Poisson's equation is used to calculate the static environment point cloud data P staticPerform surface reconstruction to generate a static environment 3D model M static ;
[0156] Use mesh simplification algorithms (such as QEM, Quadric Error Metrics) to reduce the number of triangles and post-process the reconstructed 3D model to improve model quality.
[0157] The iSAM method (Incremental Smoothing And Mapping) is used to construct a dynamic environment 3D model, including the following steps:
[0158] Set the initial posture X0 of the drone and create a factor graph G to store all posture nodes and observation factors;
[0159] For the input dynamic environment point cloud data P dynamic Perform preprocessing operations such as denoising and downsampling to reduce noise and computational complexity;
[0160] Add a pose node X to the factor graph for each time t t , create the odometer factor, use the UAV's motion model, and connect the continuous pose nodes through the odometer factor. The formula is:
[0161] f odom (X t ,X t+1 )=||X t+1 -X t -u t || 2 ;
[0162] Where, X t is the pose node at time t, X t+1 is the pose node at time t+1; u t is the motion control input at time t, usually including linear velocity and angular velocity; f odom (X t ,X t+1 ) represents the odometry factor, corresponding to the constraints between consecutive pose nodes;
[0163] The preprocessed dynamic environment point cloud data is added as observation to the factor graph. An observation factor is added to each observation point and connected to the corresponding pose node. The formula is:
[0164] f obs (X t ,q m )=||h(X t )-q m || 2 ;
[0165] In the formula, q m represents the mth observation point in the dynamic environment point cloud data; h(X t ) is based on the pose X t Computed observational predictions, usually obtained through sensor models; f obs (X t ,q m ) is the observation factor, corresponding to the constraint between the pose node and the observation point;
[0166] The iSAM method is used to incrementally optimize the factor graph G to minimize the errors of all factors, thereby obtaining the optimal pose estimation and factor graph. The formula is:
[0167]
[0168] In the formula, X represents the set of all pose nodes;
[0169] Through incremental optimization, the pose nodes X in the factor graph are updated. These pose nodes and the observation factors together form the optimized factor graph, which includes more accurate pose estimation and fusion of observation data.
[0170] According to the updated position estimation and factor graph, all observation points are transformed into the global coordinate system to construct a 3D model M of the dynamic environment. dynamic .
[0171] The environment model fusion unit is used to fuse the static environment 3D model and the dynamic environment 3D model to form a comprehensive environment model;
[0172] Furthermore, the static environment 3D model and the dynamic environment 3D model are fused using the ICP algorithm (Iterative Closest Point, iterative closest point algorithm), including:
[0173] Input static environment 3D model M static and dynamic environment 3D model M dynamic , and set the maximum number of iterations N iter and convergence tolerance δ;
[0174] The dynamic environment 3D model M dynamic With the static environment 3D model M static Perform initial alignment;
[0175] Repeat the following steps until the maximum number of iterations N is reached. iter Or the difference between the models before and after the transformation is less than the convergence tolerance δ:
[0176] Closest point matching: Find the dynamic environment 3D model Mdynamic Each point in the static environment 3D model M static The closest point pair in
[0177] Compute transformation: Use the singular value decomposition algorithm to compute the rigid body transformation that aligns the closest points;
[0178] Apply transformation: Apply the calculated rigid body transformation to the dynamic environment 3D model M dynamic Update the dynamic environment 3D model M dynamic The position of each point in
[0179] Output the aligned dynamic environment 3D model M dynamic_aligned ;
[0180] The aligned dynamic environment 3D model M dynamic_aligned and static environment 3D model M static The corresponding points in are respectively corresponding, and the weighted average method is used to fuse the corresponding points to form a comprehensive environment model M fused .
[0181] (3) The path prediction module is used to predict the target movement path based on the target three-dimensional model and the comprehensive environment model;
[0182] In one embodiment, an LSTM network is used to predict the target moving path, specifically including:
[0183] Extract a time series data set D from the target three-dimensional model and the comprehensive environment model, where the time series data set D includes an input feature vector at time t and a target position at time t;
[0184] Build the LSTM network:
[0185] Set the input feature dimension n features , the number of hidden layer units n hidden , LSTM layer number n layers and output dimension n output (i.e. the dimension of the target location);
[0186] Construct a file containing n layers LSTM network of layers, each layer contains n hidden Units, add a fully connected layer after the LSTM layer to map the output of LSTM to the target location;
[0187] Train the LSTM network:
[0188] Input the time series dataset D and the constructed LSTM network, set the batch size, learning rate and number of training rounds;
[0189] Initialize the mean square error loss function and Adam optimizer;
[0190] Input the time series data set D into the LSTM network in batches for forward propagation and calculate the predicted value;
[0191] Calculate the mean square error loss function and update the network weights through the back-propagation algorithm;
[0192] Repeat the above process until all training rounds are completed to form a trained LSTM model;
[0193] The current input feature vector is input into the trained LSTM model for forward propagation, and the predicted target position is output through the LSTM network.
[0194] (4) The flight control module is used to optimize the flight tracking path of the UAV based on the predicted target movement path and the comprehensive environment model;
[0195] In one embodiment, the flight control module includes:
[0196] The trajectory optimization unit is used to preliminarily plan the flight tracking path of the drone using the A* algorithm. The methods include:
[0197] Initialize the open list and closed list. The open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. Add the starting point (current location) of the drone to the open list.
[0198] Select the node with the lowest cost function from the open list, the cost function includes the actual cost from the starting point to the node k and the heuristic estimated cost from the node k to the end point, and remove the node with the lowest cost function from the open list and add it to the closed list;
[0199] Evaluate each adjacent node of the current node and calculate the new cost from the starting point to the adjacent node. If the adjacent node is not in the open list or the new cost is lower, update the information of the adjacent node and add it to the open list.
[0200] Repeat the above steps and continuously select the best node from the open list for expansion until the end point (target position) is found or the open list is empty (indicating that there is no path to reach it);
[0201] If the end point is found, backtrack to the parent node and output the path from the start point to the end point as the preliminary planned path;
[0202] A path smoothing unit is used to smooth the path using the B-spline interpolation method after finding the preliminary planned path through the A* algorithm;
[0203] The dynamic obstacle avoidance unit is used to dynamically optimize the smoothed path using a dynamic window method (DWA), the method comprising:
[0204] Generate all possible combinations of velocity and acceleration within a dynamic window;
[0205] For each combination, simulate the trajectory of the drone within the time window and predict the future position of the drone;
[0206] Calculate the cost function of each motion trajectory, including path tracking error, collision risk and energy consumption cost;
[0207] The motion trajectory with the minimum cost function is selected as the optimized flight tracking path.
[0208] like Figure 2 FIG. 1 is another embodiment of the present invention, which provides a tracking control method for a UAV based on a three-dimensional model, comprising the following steps:
[0209] S1: The point cloud data acquisition equipment and camera device carried by the drone scan the tracked target and the surrounding environment in real time to collect three-dimensional point cloud data in real time;
[0210] S2: De-noising, down-sampling and coordinate transformation of the collected 3D point cloud data;
[0211] S3: Use the PointNet++ model to perform multi-level feature extraction on the converted 3D point cloud data;
[0212] S4: Use the DBSCAN algorithm to segment the 3D point cloud data after feature extraction, and identify the target point cloud data, static environment point cloud data, and dynamic environment point cloud data;
[0213] S5: Based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data, a target 3D model, a static environment 3D model and a dynamic environment 3D model are constructed respectively;
[0214] S6: Fusing the static environment 3D model and the dynamic environment 3D model to form a comprehensive environment model;
[0215] S7: Based on the target three-dimensional model and the comprehensive environment model, using an LSTM network to predict the target movement path;
[0216] S8: Optimizing the flight tracking path of the UAV according to the predicted target movement path and the comprehensive environment model.
[0217] In summary, the present invention adopts the PointNet++ model to perform multi-level feature extraction of three-dimensional point cloud data, and generates local and global feature vectors by performing feature transformation and maximum pooling operations on the neighborhood points of each sampling point. Using these feature vectors, the system can accurately identify the target point cloud data, static environment point cloud data and dynamic environment point cloud data in the environment; the dynamic environment model is incrementally optimized based on the iSAM method, and the static environment model and the dynamic environment model are integrated through the ICP algorithm to form a comprehensive environment model, providing more accurate environmental perception capabilities; the LSTM network is used to predict the moving path of the target, and the LSTM network is trained by a time series data set, which can accurately predict the future position of the target and improve the tracking capability of the drone; after the preliminary path planning, the system smoothes the path through the B-spline interpolation method, making the path more natural and smooth, which is conducive to the smooth flight of the drone, and the dynamic obstacle avoidance module uses the dynamic window method to optimize the smoothed path in real time to ensure that the drone can dynamically avoid obstacles during flight and improve safety.
[0218] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or other arbitrary combinations. When implemented using software, it can be implemented in whole or in part in the form of a computer program product, which includes one or more computer instructions. When loading and executing computer program instructions on a computer, the process or function according to the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium.
[0219] In addition, each functional unit in each embodiment of the present application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into one module. The above-mentioned integrated module can be implemented in the form of hardware or in the form of a software functional module. If the above-mentioned integrated module is implemented in the form of a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. The storage medium can be a read-only memory, a disk or an optical disk, etc.
[0220] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of various changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.
Claims
1. A UAV tracking control system based on a three-dimensional model, characterized in that: The system comprises: The data acquisition module is used to scan the tracked target and the surrounding environment in real time through the point cloud data acquisition equipment and camera device carried by the drone, and collect three-dimensional point cloud data in real time; A data processing module is used to process the three-dimensional point cloud data, divide the three-dimensional point cloud data into target point cloud data, static environment point cloud data and dynamic environment point cloud data, and respectively construct a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model, and fuse the static environment three-dimensional model and the dynamic environment three-dimensional model to form a comprehensive environment model; A path prediction module, used to predict a target moving path based on the target three-dimensional model and the comprehensive environment model; The flight control module is used to optimize the flight tracking path of the UAV according to the predicted target movement path and the comprehensive environment model.
2. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 1, characterized in that: The data processing module comprises: A data preprocessing unit, used for denoising, downsampling and coordinate transformation of the collected 3D point cloud data; A feature extraction unit is used to perform multi-level feature extraction on the converted 3D point cloud data using the PointNet++ model; The point cloud segmentation unit is used to segment the three-dimensional point cloud data after feature extraction using the DBSCAN algorithm, and identify target point cloud data, static environment point cloud data, and dynamic environment point cloud data; A model building unit, used to build a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model respectively based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data; The environment model fusion unit is used to fuse the static environment three-dimensional model and the dynamic environment three-dimensional model to form a comprehensive environment model.
3. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 2, characterized in that: The implementation method of the data preprocessing unit includes: Use statistical filters for denoising: The collected 3D point cloud data is represented as a set P{p1,p2,...,p n }, each point p in the set P i Expressed as three-dimensional coordinates (x i ,y i ,z i ), n is the total amount of 3D point cloud data; Select the k nearest neighbor points of each point and calculate the p of each point i The mean distance d to the k nearest neighbors i , the formula is: Where j represents the current neighbor point index, ranging from 1 to k; p ij Represents point p i The coordinates of the jth nearest neighbor; Calculate the mean distance d of all points in set P mean and standard deviation d , the formula is: Set the mean distance threshold to d meand ±σ·σ d , σ is the standard deviation threshold, and the points whose mean distance is within the mean distance threshold are retained as the denoised three-dimensional point cloud data P denoised ; Downsampling using voxel grid filtering: define the size v of each voxel size , the denoised three-dimensional point cloud data P denoised Divide into voxel grids, take the average value of the points in each voxel as the representative point of the voxel, and output the downsampled 3D point cloud data P downsampled ; The downsampled 3D point cloud data P downsampled Perform coordinate transformation to convert the 3D point cloud data from the drone coordinate system to the global coordinate system. The conversion formula is: p i ′=R·p i +t; In the formula, p i ′ is the coordinate of the point after transformation, p i is the coordinate of the original point, R is the rotation matrix, and t is the translation vector; Output the converted 3D point cloud data P transformed .
4. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 2, characterized in that: In the feature extraction unit, the method for performing multi-level feature extraction includes: Input the converted 3D point cloud data P transformed , set the hyperparameters of the PointNet++ model, including the number of layers, the number of sampling points per layer, and the neighborhood size; The farthest point sampling algorithm is used to extract the converted 3D point cloud data P transformed A fixed number of points are sampled in the spherical neighborhood. For each sampling point, a spherical neighborhood is defined. Let the neighborhood radius be r. The neighborhood radius r includes a fixed number of neighborhood points. For the neighborhood points of each sampling point, feature transformation is performed through a shared multi-layer perceptron, and the transformed neighborhood point features are subjected to maximum pooling operation to generate the local feature vector of the sampling point; Repeat multiple grouping layers, extract features layer by layer, use the output of each layer as the input of the next layer, and in the last layer, use global maximum pooling to pool the features of all points to generate a global feature vector; Output point cloud feature F after feature extraction features , including local features and global feature vectors.
5. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 2, characterized in that: In the point cloud segmentation unit, the point cloud feature F after feature extraction is features Use DBSCAN algorithm for segmentation. The following processes are included: define parameters: define the neighborhood radius and the minimum number of points MinPts; Initialization: mark all points as unvisited in preparation for traversal and clustering; Traverse each point: point cloud feature F features Traverse each point in, if the point has not been visited, then perform the following steps: Determine the core point: calculate the number of points in the ∈-neighborhood of the current point, if it meets the minPts condition, mark it as a core point and start expanding the cluster; otherwise , marked as noise points; Expand the cluster: traverse the neighborhood of the core point, add the points that meet the minPts condition to the cluster, and continue to expand until the cluster stops growing; Repeat the above steps for all unvisited points until all points have been visited; According to the feature extraction results and the spatial position of the cluster, different types of point cloud features are identified and the 3D point cloud data is segmented into target point cloud data P target , static environment point cloud data P static and dynamic environment point cloud data P dynamic .
6. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 2, characterized in that: In the model construction unit, the target three-dimensional model is constructed by: Input target point cloud data P target And divide it into voxel grids and set the size of each voxel; Calculate the density value of the points within each voxel; For each voxel, the Marching Cubes algorithm is used to extract the isosurface from the voxel data; According to the density value of the points in the voxel, the vertex position of the isosurface is determined, and the triangular mesh of the target 3D model is generated using the predefined lookup table, and the target 3D model M is output. target ; The construction method of the static environment 3D model is: Use octree to analyze static environment point cloud data P static Perform hierarchical partitioning and recursively divide the space into eight subspaces until the maximum depth d is reached, and store the corresponding static environment point cloud data in each leaf node; Based on the octree structure, Poisson's equation is used to calculate the static environment point cloud data P static Perform surface reconstruction to generate a static environment 3D model M static ; The reconstructed 3D model is post-processed using the QEM algorithm; The iSAM method is used to construct a dynamic environment 3D model, including the following steps: Set the initial posture X0 of the drone and create a factor graph G to store all posture nodes and observation factors; For the input dynamic environment point cloud data P dynamic Perform preprocessing operations such as denoising and downsampling; Add a pose node X to the factor graph for each time t t , create the odometer factor, use the UAV's motion model, and connect the continuous pose nodes through the odometer factor. The formula is: f odom (X t ,X t1 )=||X t+1 -X t -u t || 2 Where, X t is the pose node at time t, X t+1 is the pose node at time t+1; u t is the motion control input at time t; f odom (X t ,X t+1 ) represents the odometer factor; The preprocessed dynamic environment point cloud data is added as observation to the factor graph, and an observation factor is added to each observation point. The formula is: f obs (X t ,q m )=||h(X t )-q m || 2 ; In the formula, q m represents the mth observation point in the dynamic environment point cloud data; h(X t ) is based on the pose X t Calculated observational predictions; f obs (X t ,q m ) is the observation factor; Use the iSAM method to incrementally optimize the factor graph and update the pose estimation and factor graph; According to the updated position estimation and factor graph, all observation points are transformed into the global coordinate system to construct a 3D model M of the dynamic environment. dynamic .
7. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 2, characterized in that: In the environment model fusion unit, the static environment 3D model and the dynamic environment 3D model are fused using the ICP algorithm, which specifically includes: Input static environment 3D model M static and dynamic environment 3D model M dynamic , and set the maximum number of iterations N iter and convergence tolerance δ; The dynamic environment 3D model M dynamic With the static environment 3D model M static Perform initial alignment; Repeat the following steps until the maximum number of iterations N is reached. iter Or the difference between the models before and after the transformation is less than the convergence tolerance δ: Closest point matching: Find the dynamic environment 3D model M dynamic Each point in the static environment 3D model M static The closest point pair in Compute transformation: Use the singular value decomposition algorithm to compute the rigid body transformation that aligns the closest points; Apply transformation: Apply the calculated rigid body transformation to the dynamic environment 3D model M dynamic Update the dynamic environment 3D model M dynamic The position of each point in Output the aligned dynamic environment 3D model M dynamic_aligned ; The aligned dynamic environment 3D model M dynamic_aligned and static environment 3D model M static The corresponding points in are respectively corresponding, and the weighted average method is used to fuse the corresponding points to form a comprehensive environment model M fused .
8. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 1, characterized in that: In the path prediction module, an LSTM network is used to predict the target moving path, specifically including: Extract a time series data set D from the target three-dimensional model and the comprehensive environment model, where the time series data set D includes an input feature vector at time t and a target position at time t; Building an LSTM network: Setting the input feature dimension n features , the number of hidden layer units n hidden , LSTM layer number n layers and output dimension n output (i.e. the dimension of the target position), construct a layers LSTM network of layers, each layer contains n hidden Units, add a fully connected layer after the LSTM layer to map the output of LSTM to the target location; Train the LSTM network: Input the time series dataset D and the constructed LSTM network, set the batch size, learning rate and number of training rounds; Initialize the mean square error loss function and Adam optimizer; Input the time series data set D into the LSTM network in batches for forward propagation and calculate the predicted value; Calculate the mean square error loss function and update the network weights through the back-propagation algorithm; Repeat the above process until all training rounds are completed to form a trained LSTM model; The current input feature vector is input into the trained LSTM model for forward propagation, and the predicted target position is output through the LSTM network.
9. The three-dimensional model-based unmanned aerial vehicle tracking control system according to claim 1, characterized in that: The flight control module comprises: The trajectory optimization unit is used to preliminarily plan the flight tracking path of the drone using the A* algorithm. The methods include: Initialize the open list and closed list. The open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. Add the drone starting point to the open list. Select the node with the lowest cost function from the open list, the cost function includes the actual cost from the starting point to the node k and the heuristic estimated cost from the node k to the end point, and remove the node with the lowest cost function from the open list and add it to the closed list; Evaluate each adjacent node of the current node and calculate the new cost from the starting point to the adjacent node. If the adjacent node is not in the open list or the new cost is lower, update the information of the adjacent node and add it to the open list. Repeat the above steps and continuously select the best node from the open list for expansion until the end point is found or the open list is empty; if the end point is found, backtrack to the parent node and output the path from the start point to the end point as the preliminary planned path; A path smoothing unit, used to smooth the path using a B-spline interpolation method after finding the initially planned path through the A* algorithm; The dynamic obstacle avoidance unit is used to dynamically optimize the smoothed path using a dynamic window method, the method comprising: Generate all possible combinations of velocity and acceleration within a dynamic window; For each combination, simulate the trajectory of the drone within the time window and predict the future position of the drone; Calculate the cost function of each motion trajectory, including path tracking error, collision risk and energy consumption cost; The motion trajectory with the minimum cost function is selected as the optimized flight tracking path.
10. A tracking control method for a UAV tracking control system based on a three-dimensional model according to any one of claims 1 to 9, characterized in that: The method comprises: The point cloud data acquisition equipment and camera device carried by the drone scans the tracked target and the surrounding environment in real time, and collects three-dimensional point cloud data in real time; De-noising, down-sampling and coordinate transformation of the collected 3D point cloud data; Use the PointNet++ model to perform multi-level feature extraction on the converted 3D point cloud data; Use the DBSCAN algorithm to segment the 3D point cloud data after feature extraction, and identify the target point cloud data, static environment point cloud data, and dynamic environment point cloud data; Based on the target point cloud data, the static environment point cloud data and the dynamic environment point cloud data, respectively construct a target three-dimensional model, a static environment three-dimensional model and a dynamic environment three-dimensional model; The static environment three-dimensional model and the dynamic environment three-dimensional model are integrated to form a comprehensive environment model; Based on the target three-dimensional model and the comprehensive environment model, an LSTM network is used to predict the target movement path; The flight tracking path of the UAV is optimized according to the predicted target movement path and the comprehensive environment model.
Citation Information
Patent Citations
LiDAR point cloud and optical image priori coupling classification method based on deep learning
CN111950658A
Road surface obstacle detection method and system based on three-dimensional point cloud
CN115359453A
Aircraft dynamic target tracking navigation method and device and readable medium
CN115657718A
Intelligent obstacle avoidance control method and system for drilling robot
CN116449851A
Unmanned aerial vehicle target chasing trajectory generation method, electronic equipment and unmanned aerial vehicle
CN116661486A
Cited By
Environment monitoring method and system based on digital twinborn technology
CN119147048A
Traffic safety management method and system combined with unmanned aerial vehicle operation risk control
CN119169876A
Traffic safety management method and system combined with drone operation risk control
CN119169876B
Three-dimensional measurement and simulation analysis system and method based on multi-sensor fusion
CN119741579A
General aviation monitoring and early warning method and system based on artificial intelligence
CN120259730A
