Unmanned aerial vehicle path planning method, system, equipment and medium

Abstract

The invention discloses an unmanned aerial vehicle path planning method, system and device and a medium, and belongs to the field of electric power systems.The method comprises the steps that initial point cloud data are obtained to be subjected to multi-coordinate conversion and filtering processing, and a three-dimensional voxel grid map is constructed through voxel grid mapping; identifying whether each voxel grid in the map is an obstacle or not according to the occupancy threshold, generating an obstacle set, generating a KDtree structure based on the center coordinates of the occupied grids, calculating the Euclidean distance from any voxel grid to the nearest occupied grid by using K nearest neighbor query, and obtaining a target grid map; a composite heuristic function combining a distance field penalty term, a direction deviation penalty term and a curvature penalty term is adopted, an initial path is obtained through a target heuristic path search algorithm, smooth fitting is conducted through a B spline curve, optimization is conducted through a minimum Snap trajectory model, and a target path is obtained. And the obstacle avoidance safety and the flight stability of the unmanned aerial vehicle can be effectively guaranteed.

Description

Technical Field

[0001] This invention relates to the field of power systems, and more particularly to a method, system, device, and medium for unmanned aerial vehicle (UAV) path planning. Background Technology

[0002] With the rapid development of drone technology, its applications in logistics delivery, agricultural plant protection, and power line inspection are becoming increasingly widespread. One of the core technologies for achieving autonomous drone flight is three-dimensional path planning, which aims to plan a safe and efficient flight path from start to finish in complex three-dimensional obstacle environments.

[0003] Currently, mainstream UAV 3D path planning methods mainly include graph search-based planning methods and sampling-based planning methods. Graph search-based planning methods primarily employ the A* algorithm. This method first discretizes the continuous 3D space into a voxel grid map or topological map, constructing a graph network. Nodes in the graph represent passable locations, and edges represent movement paths and their costs between nodes. Subsequently, heuristic search algorithms such as A* are used to search for the optimal path on this graph network. However, the path generated by this method is composed of a series of discrete grid center points, and the path turns often exhibit acute or right angles, resulting in extremely poor smoothness. This does not conform to the dynamic model of UAVs and makes direct flight impossible. Furthermore, in pursuit of the shortest path, the algorithm often generates paths that infinitely approach obstacle boundaries, exhibiting severe "edge-hugging" phenomena, insufficient safety margins, and a high risk of collisions in actual flight. Sampling-based planning methods primarily employ the Rapid Expanding Random Tree (RRT) algorithm. Its path is composed of randomly sampled points, and the randomness leads to a large number of unnecessary redundant turns and jitters in the path, resulting in poor smoothness and optimality. Therefore, the biggest drawback of existing path planning methods is the low safety and smoothness of the paths, resulting in poor path quality. Summary of the Invention

[0004] This invention provides a method, system, device, and medium for unmanned aerial vehicle (UAV) path planning, which can solve the problem of how to improve path safety and smoothness.

[0005] This invention provides a method for unmanned aerial vehicle (UAV) path planning, comprising:

[0006] Acquire initial 3D point cloud data, perform multi-coordinate system transformation and filtering on the initial 3D point cloud data to obtain 3D point cloud data, and perform voxel raster mapping on the 3D point cloud data to obtain a 3D voxel raster map.

[0007] Obstacles are identified in each voxel grid in the 3D voxel grid map based on the occupancy threshold to obtain an obstacle set. A KDtree structure is generated based on the 3D center coordinates of each occupied grid. The Euclidean distance from any voxel grid in the 3D voxel grid map to the occupied grid with the shortest distance is calculated through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles.

[0008] A target heuristic path search algorithm based on a composite heuristic function is used for UAV path planning to obtain an initial path. The initial path is then smoothed by B-spline curves, and the smoothed path is optimized based on the minimization of the Snap trajectory optimization model to obtain the target path. The composite heuristic function is a heuristic function that combines a range field penalty term, a direction deviation penalty term, and a curvature penalty term.

[0009] This invention achieves high-precision, structured environmental modeling by constructing a 3D voxel grid map, providing a reliable environmental constraint foundation for subsequent accurate distance field calculation and path search. By introducing a distance field accelerated by KDtree, environmental information is upgraded from binary (obstacle / non-obstacle) to continuous distance information, providing a data foundation for the algorithm to "perceive" safe distances and serving as a prerequisite for solving the "edge-hugging" phenomenon. Through a composite heuristic function (fusing distance field, direction, and curvature penalty), a high-quality initial path is generated in the front-end path search stage, which has already initially avoided obstacles, tends towards a straight line, and has smooth turns, rather than simply pursuing the shortest path, greatly reducing the optimization burden on the back-end. B-spline fitting ensures the geometric continuity of the path (G... 2 (Above), then through Minimum Snap optimization to ensure the dynamic sub-slipperiness of the trajectory (continuity of higher-order derivatives), finally generating an optimal trajectory that the UAV can directly, smoothly and efficiently track.

[0010] Furthermore, the UAV path planning is performed using a target heuristic path search algorithm based on composite heuristic functions to obtain an initial path, wherein the initial path includes multiple path nodes, specifically:

[0011] Determine the starting and target nodes in the path planning;

[0012] The voxel grid corresponding to the starting node is taken as the current node of the first round of path planning. In each round of path planning, the composite heuristic value between the current node and multiple reachable nodes of the current node is calculated according to the composite heuristic function. The path node of the current iteration is obtained according to the reachable node corresponding to the minimum value of the composite heuristic value. The reachable node refers to the node among the adjacent nodes of the current node that meets the preset node conditions. The preset node conditions are that the adjacent node does not belong to the occupied grid or the adjacent node does not belong to the path node determined in each round of path planning.

[0013] The current node is iteratively updated based on the path nodes until the current node is the voxel grid corresponding to the target node, at which point the iteration stops and the initial path is obtained.

[0014] This clarifies the decision-making mechanism of the search algorithm, which always selects the node with the smallest composite heuristic value for expansion. This ensures that each step of path planning tends to select the path with the lowest overall cost (i.e., considering distance, safety, direction, and smoothness simultaneously), which is the key execution step to generate a high-quality initial path in one go.

[0015] Furthermore, the step of calculating the composite heuristic value between the current node and multiple reachable nodes of the current node according to the composite heuristic function specifically involves:

[0016] For each reachable node, calculate the first-generation value between the reachable node and the starting node, and calculate the second-generation value between the reachable node and the target node. Determine the initial heuristic value based on the first-generation value and the second-generation value.

[0017] Obtain a safe distance threshold and a distance cost weight. Based on the target grid map, when the shortest obstacle distance corresponding to the reachable node is greater than the safe distance threshold, calculate the distance difference between the shortest obstacle distance and the safe distance threshold. Calculate the distance difference by weighting it according to the distance cost weight to obtain the distance field penalty term corresponding to the reachable node.

[0018] Obtain the deviation cost weight, determine the first vector and the second vector, calculate the first magnitude corresponding to the cross product of the first vector and the second vector, and calculate the second magnitude of the first vector. Combine the deviation cost weight, the first magnitude and the second magnitude to obtain the direction deviation penalty term corresponding to the reachable node. The first vector is used to represent the vector from the starting node to the target node, and the second vector table is used to represent the vector from the starting node to the current node.

[0019] Obtain curvature cost weights, determine the first direction vector corresponding to the current node and the second direction vector corresponding to the reachable node, calculate the cosine value of the angle between the current node and the reachable node based on the first direction vector and the second direction vector, and combine the curvature cost weights and the cosine value of the angle to obtain the curvature penalty term corresponding to the reachable node.

[0020] The initial heuristic value is adjusted by combining the distance field penalty term, the direction deviation penalty term, and the curvature penalty term to obtain the composite heuristic value.

[0021] This approach quantifies the safe distance through a distance field penalty term. When a node is too close to an obstacle, the cost of the distance field penalty term increases dramatically, forcing the path planning process to avoid that node and thus proactively planning a safe path. The straightness of the path is quantified through a direction deviation penalty term. By calculating the degree to which a node deviates from the desired direction (the line connecting the start and end points) using vector cross products, it guides the selection of paths closer to a straight line, reducing unnecessary detours and shortening the path length. The smoothness of the path is quantified through a curvature penalty term. By calculating the cosine value of the turning angle, it penalizes sharp turns (small angle, small cosine value, large penalty), encouraging the selection of gentler turns, making the initial path inherently smoother. Combining these three penalties with the first and second generation values ​​(G(n) + H(n)) forms a comprehensive evaluation index, ultimately achieving an optimal trade-off between multiple objectives such as safety, short distance, and smoothness.

[0022] Further, the step of identifying obstacles in each voxel grid in the three-dimensional voxel grid map based on an occupancy threshold to obtain an obstacle set specifically involves:

[0023] When the occupancy threshold is greater than or equal to the number of point clouds in each voxel grid, the voxel grid is marked as an obstacle, and an initial obstacle set is obtained;

[0024] Based on the initial obstacle set, a dilation operation is performed using cube structuring elements. The resulting grid is then merged with the initial obstacle set to obtain the obstacle set.

[0025] This morphological dilation operation constructs a "safety buffer" or "virtual shell" around the originally identified obstacles. This forces subsequent planning algorithms to maintain a safe distance from the original obstacles, eliminating the possibility of "grazing" at the environmental representation level and greatly improving flight safety.

[0026] Further, the process of acquiring initial 3D point cloud data, performing multi-coordinate system transformation and filtering on the initial 3D point cloud data to obtain 3D point cloud data specifically involves:

[0027] The initial three-dimensional point cloud data is obtained by collecting the lidar of the UAV. The installation tilt angle of the lidar is obtained based on the pose information of the UAV. A compensation rotation matrix is ​​determined according to the installation tilt angle. The coordinates of each point cloud in the initial three-dimensional point cloud data are transformed according to the compensation rotation matrix to obtain the second point cloud data, so as to transform the initial three-dimensional point cloud data in the sensor coordinate system to the second point cloud data in the body coordinate system.

[0028] Based on the pose information of the UAV, the pitch angle, roll angle and yaw angle of the UAV are obtained, and the pitch matrix, roll matrix and yaw matrix are determined according to the pitch angle, roll angle and yaw angle respectively.

[0029] The pitch matrix, roll matrix, and yaw matrix are multiplied together to obtain a composite rotation matrix. The coordinates of each point cloud in the second point cloud data are then transformed according to the composite rotation matrix to obtain the third point cloud data, thereby transforming the second point cloud data in the body coordinate system to the third point cloud data in the world coordinate system.

[0030] A height range threshold is obtained, and ground point cloud data and noise data in the third point cloud data are filtered according to the height threshold to obtain the fourth point cloud data. The fourth point cloud data is then processed using a statistical filtering algorithm to obtain the three-dimensional point cloud data.

[0031] By performing two coordinate transformations (sensor frame -> machine frame -> world frame), the influence of sensor installation deviations and UAV attitude changes on point cloud positions is eliminated, ensuring all point cloud data are unified under a fixed world coordinate system, thus guaranteeing the accuracy and stability of the environment model. Through height threshold filtering and statistical filtering, useless ground points, measurement noise, and outliers are effectively removed, purifying the point cloud data and preventing false obstacles from appearing during subsequent map construction, thereby improving the reliability of the environment model.

[0032] Furthermore, the smooth fitting of the initial path using a B-spline curve specifically involves:

[0033] Each path node in the initial path is used as a fitting data point for the B-spline curve, and the chord length parameterization method is used to determine the corresponding parameter value for each fitting data point.

[0034] Based on each of the fitted data points and the corresponding parameter values, a system of linear equations about the control points of the B-spline curve is constructed using the least squares method.

[0035] The linear equations are solved by combining regularization terms to obtain multiple control points of the B-spline curve, and a smoothed initial path is generated based on each control point.

[0036] By employing chord length parameterization, the distribution of data points in the actual space can be better reflected, resulting in a more accurate B-spline curve shape and avoiding unnecessary distortion. The least squares method ensures that the fitted curve closely approximates the original path points, guaranteeing path accuracy; while the regularization term effectively prevents overfitting, controls the overall smoothness of the curve, and achieves an optimal balance between accuracy and smoothness.

[0037] Furthermore, the smoothed path is optimized based on the minimized Snap trajectory optimization model to obtain the target path, specifically as follows:

[0038] The smoothed initial path is divided into multiple trajectory segments, and the position function of each trajectory segment is determined;

[0039] Construct an optimization function with the objective of minimizing the sum of squared integrals of Snap for all trajectory segments, where Snap is the fourth derivative of the position function;

[0040] Based on the boundary conditions and continuity constraints of the UAV's motion, the solution of the optimization function is transformed into a quadratic programming problem;

[0041] Solve the quadratic programming problem to obtain the polynomial coefficients of each trajectory segment, and generate the target path based on the polynomial coefficients.

[0042] This method minimizes the sum of squared integrals of Snap across all trajectory segments. Since Snap is the derivative of acceleration, minimizing Snap is equivalent to minimizing the rate of change of jerk, resulting in a very smooth acceleration variation in the generated trajectory. This significantly reduces motor losses and energy consumption of the UAV, and improves flight stability and passenger comfort (if carrying cargo). The complex trajectory optimization problem is transformed into a mathematically standard and efficient quadratic programming (QP) problem, ensuring the computational feasibility and real-time performance of the optimization step. Based on the boundary conditions and continuity constraints of the UAV's motion, it is ensured that the generated trajectory matches the UAV's actual physical capabilities (such as maximum speed and acceleration), and that the transitions between trajectory segments are smooth, guaranteeing the physical feasibility and continuity of the trajectory.

[0043] Another embodiment of the present invention provides an unmanned aerial vehicle (UAV) path planning system, including: a point cloud processing module, an obstacle module, and a path planning module;

[0044] The point cloud processing module is used to acquire initial three-dimensional point cloud data, perform multi-coordinate system transformation and filtering on the initial three-dimensional point cloud data to obtain three-dimensional point cloud data, and perform voxel raster mapping on the three-dimensional point cloud data to obtain a three-dimensional voxel raster map.

[0045] The obstacle module is used to identify obstacles in each voxel grid in the three-dimensional voxel grid map according to an occupancy threshold, obtain an obstacle set, generate a KDtree structure according to the three-dimensional center coordinates of each occupied grid, and calculate the Euclidean distance from any voxel grid in the three-dimensional voxel grid map to the occupied grid with the shortest distance through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles.

[0046] The path planning module is used to perform UAV path planning using a target heuristic path search algorithm based on a composite heuristic function to obtain an initial path, to smoothly fit the initial path using a B-spline curve, and to optimize the smoothed path based on a minimized Snap trajectory optimization model to obtain a target path. The composite heuristic function is a heuristic function that combines a distance field penalty term, a direction deviation penalty term, and a curvature penalty term.

[0047] This invention achieves high-precision, structured environmental modeling by constructing a 3D voxel grid map, providing a reliable environmental constraint foundation for subsequent accurate distance field calculation and path search. By introducing a distance field accelerated by KDtree, environmental information is upgraded from binary (obstacle / non-obstacle) to continuous distance information, providing a data foundation for the algorithm to "perceive" safe distances and serving as a prerequisite for solving the "edge-hugging" phenomenon. Through a composite heuristic function (fusing distance field, direction, and curvature penalty), a high-quality initial path is generated in the front-end path search stage, which has already initially avoided obstacles, tends towards a straight line, and has smooth turns, rather than simply pursuing the shortest path, greatly reducing the optimization burden on the back-end. B-spline fitting ensures the geometric continuity of the path (G... 2 (Above), then through Minimum Snap optimization to ensure the dynamic sub-slipperiness of the trajectory (continuity of higher-order derivatives), finally generating an optimal trajectory that the UAV can directly, smoothly and efficiently track.

[0048] Another embodiment of the present invention provides a terminal device, including: a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, it implements the steps of the UAV path planning method of the present invention.

[0049] Another embodiment of the present invention also provides a computer-readable storage medium item, including: a stored computer program, which, when the computer program is running, controls the device where the computer-readable storage medium is located to perform steps such as the UAV path planning method of the present invention. Attached Figure Description

[0050] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0051] Figure 1 This is a flowchart illustrating a UAV path planning method provided in an embodiment of the present invention;

[0052] Figure 2 This is a flowchart illustrating a UAV path planning method provided in an embodiment of the present invention;

[0053] Figure 3 This is a side view of the three-dimensional path planning result of the A* algorithm before improvement in a three-dimensional map provided by an embodiment of the present invention;

[0054] Figure 4 This is a top view of the three-dimensional path planning result of the A* algorithm before improvement in a three-dimensional map provided by an embodiment of the present invention;

[0055] Figure 5 This is a side view of the 3D path planning result of an improved A* algorithm in a 3D map provided in an embodiment of the present invention;

[0056] Figure 6 This is a top view of the 3D path planning result of an improved A* algorithm in a 3D map provided in an embodiment of the present invention;

[0057] Figure 7 This is a schematic diagram of the structure of a drone path planning system provided in an embodiment of the present invention. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of this application clearer, the technical solutions of this application will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0059] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains; the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the application; the terms “comprising” and “having”, and any variations thereof, in the specification, claims, and foregoing description of the drawings are intended to cover non-exclusive inclusion.

[0060] In the description of the embodiments of this application, technical terms such as "first" and "second" are used only to distinguish different objects and should not be construed as indicating or implying relative importance or implicitly specifying the number, specific order, or primary and secondary relationship of the indicated technical features. In the description of the embodiments of this application, "multiple" means two or more, unless otherwise explicitly defined.

[0061] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0062] In the description of the embodiments in this application, the term "and / or" is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this document generally indicates that the preceding and following related objects have an "or" relationship.

[0063] In the description of the embodiments of this application, the term "multiple" refers to two or more (including two), similarly, "multiple sets" refers to two or more (including two sets), and "multiple pieces" refers to two or more (including two pieces).

[0064] In the description of the embodiments of this application, unless otherwise expressly specified and limited, technical terms such as "installation," "connection," "joining," and "fixing" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. For those skilled in the art, the specific meaning of the above terms in the embodiments of this application can be understood according to the specific circumstances.

[0065] See Figure 1 To address the issue of improving path safety and smoothness in existing technologies, an embodiment of the present invention provides a UAV path planning method, including steps S101-S103, specifically including:

[0066] Step S101: Obtain initial 3D point cloud data, perform multi-coordinate system transformation and filtering on the initial 3D point cloud data to obtain 3D point cloud data, and perform voxel raster mapping on the 3D point cloud data to obtain a 3D voxel raster map.

[0067] In this embodiment, initial 3D point cloud data is acquired using LiDAR, and multi-coordinate system transformation is performed by combining the pose information of the UAV. Then, point cloud filtering is performed using a height threshold and filtering algorithm to obtain 3D point cloud data p. f By defining the spatial extent and voxel size of the 3D voxel raster map, and using floor operations to determine the number of graticles in each dimension of the 3D map, and then calculating the graticle index corresponding to each point cloud based on the filtered point cloud set, the 3D voxel raster mapping is completed, resulting in the 3D voxel raster map. The mapping process is as follows: Let the spatial extent of the voxel raster map be: x∈[x... min ,x max ],y∈[y min ,y max ],z∈[z min ,z max If the voxel dimensions are Δx, Δy, Δz, then the number of raster cells in each dimension is:

[0068]

[0069] Where, N x N y N z This represents the number of grid cells in three dimensions. The floor function is used to round down, indicating that the largest integer not greater than the value within the parentheses is taken. For 3D point cloud data p... f =[x,y,z] T Its corresponding raster index (i,j,k) is:

[0070]

[0071] As an example of an embodiment of the present invention, the step of obtaining initial three-dimensional point cloud data, and performing multi-coordinate system transformation and filtering processing on the initial three-dimensional point cloud data to obtain three-dimensional point cloud data specifically includes:

[0072] The initial 3D point cloud data is acquired by the lidar of the UAV. Based on the UAV's pose information, the installation tilt angle of the lidar is obtained. A compensation rotation matrix is ​​determined based on the installation tilt angle, and the coordinates of each point in the initial 3D point cloud data are transformed according to the compensation rotation matrix to obtain second point cloud data. This transforms the initial 3D point cloud data in the sensor coordinate system to the second point cloud data in the aircraft coordinate system. Based on the UAV's pose information, the pitch angle, roll angle, and yaw angle of the UAV are obtained, and the pitch angle is determined based on these angles. The system generates a pitch matrix, a roll matrix, and a yaw matrix. These matrices are multiplied to obtain a composite rotation matrix. The coordinates of each point cloud in the second point cloud data are then transformed using this composite rotation matrix to obtain third point cloud data. This transforms the second point cloud data in the body coordinate system to the third point cloud data in the world coordinate system. A height range threshold is obtained, and ground point cloud data and noise data in the third point cloud data are filtered according to this threshold to obtain fourth point cloud data. A statistical filtering algorithm is then used to process the fourth point cloud data to obtain the three-dimensional point cloud data.

[0073] In this embodiment, for the initial 3D point cloud data acquired by the lidar, the sensor's own attitude deviation is corrected by installing a tilt compensation matrix. The coordinates of the initial 3D point cloud data of the lidar in the sensor coordinate system are p. s =[x s ,y s ,z s ] T s represents the sensor coordinate system. If the preset sensor installation angle is the tilt angle θ around the y-axis... y Then, based on the installation tilt angle θ y Determine the compensation rotation matrix R y for:

[0074]

[0075] Then, the coordinates of each point in the initial 3D point cloud data are transformed according to the compensation rotation matrix to obtain the second point cloud data, and the point coordinates p in the compensated body coordinate system b. b for:

[0076]

[0077] Then, using the rotation matrix from the body coordinate system to the world coordinate system (based on the ZYX sequence rotation of Euler angles), the point cloud is transformed from the sensor local coordinate system to the global coordinate system. Let the UAV attitude angles be pitch angle γ, roll angle θ, and yaw angle α, and the corresponding rotation matrix R α ,R γ ,Rθ for:

[0078]

[0079] Based on the rotation matrices, the composite rotation matrix from the body coordinate system to the world coordinate system w is obtained. Then, the coordinates of each point in the second point cloud data are transformed according to the composite rotation matrix to obtain the third point cloud data, and the point coordinates in the world coordinate system w are...

[0080] Ground points and redundant noise in the third point cloud data are removed by height threshold filtering (retaining points within a specific Z-axis range). Let p be the fourth point cloud data that retains the retained points after filtering. f The expression for filtering ground and noise using a height threshold is as follows:

[0081] p f ={p w =[x,y,z] T |z min ≤z≤z max};

[0082] Where x, y, z are the three-dimensional coordinates of the point cloud before filtering, and z min ,z max This is a preset height range threshold.

[0083] Statistical filtering algorithms are used to remove obvious outliers from the dense point cloud data in the fourth point cloud dataset. Since laser point clouds typically produce point cloud datasets with uneven density, and measurement errors also generate sparse outliers, filtering is necessary for the fourth point cloud data p. f Statistical analysis is performed on the neighborhood of each point. The distances between all points in the point cloud are assumed to follow a Gaussian distribution, the shape of which is determined by the mean μ and standard deviation σ. Let p be the coordinates of the nth point in the point cloud. n =(x n ,y n ,z n ), from this point to any point p m The distance D is:

[0084]

[0085] The mean μ and standard deviation σ of the distances from each point to any other point are calculated as follows:

[0086]

[0087] Where n and i represent the nth point cloud, respectively.

[0088] The filtering process assumes the tolerable factor of the standard deviation is β, calculates the distance d from each point to its neighborhood k, and determines the distance when the average distance of a point's k neighboring points is within the standard range. The point is retained; if it is not within the range, it is considered an outlier and needs to be removed, resulting in statistically filtered 3D point cloud data p. f .

[0089] Step S102: Based on the occupancy threshold, obstacles are identified in each voxel grid in the three-dimensional voxel grid map to obtain an obstacle set. A KDtree structure is generated based on the three-dimensional center coordinates of each occupied grid. The Euclidean distance from any voxel grid in the three-dimensional voxel grid map to the occupied grid with the shortest distance is calculated through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles.

[0090] In this embodiment, the number of point clouds n(i,j,k) within each grid cell is counted, and this number of point clouds is compared with a preset occupancy threshold T. occ The comparison process is as follows: if the number of points in a voxel grid is greater than or equal to the occupancy threshold, the voxel grid is determined to be an occupied grid with obstacles, and all such occupied grids together constitute the obstacle set; if the number of points in a voxel grid is less than the occupancy threshold, the voxel grid is determined to be a free grid. When generating the KDtree structure, the 3D center coordinates of each occupied grid in the obstacle set are extracted (i.e., the geometric center coordinates of the voxel grid in 3D space, calculated from the grid index combined with the voxel size). These 3D center coordinates are input as sample points into the KDtree construction function. By spatially partitioning the sample points (based on recursive segmentation of the median of each dimension's coordinates), a KDtree data structure for fast nearest neighbor lookup is generated. When calculating the Euclidean distance from any voxel grid in a 3D voxel grid map to the shortest occupied grid, for each voxel grid (including occupied and free grids), its 3D center coordinates are extracted as a query point. The K-nearest neighbor query interface of KDtree (with K set to 1) is called to retrieve the 3D center coordinates of the occupied grid closest to the query point. Then, the distance between the two is calculated using the Euclidean distance formula (i.e., the square root of the sum of the squares of the differences between the coordinates of two points in 3D space). This distance is the distance from the voxel grid to the shortest obstacle. The shortest obstacle distance of each voxel grid is associated and stored with the corresponding grid information to form a target grid map containing the shortest obstacle distances of each voxel grid.

[0091] It should be noted that in this embodiment, KDtree can be used to achieve real-time fusion of multi-frame point clouds. The most recent multi-frame LiDAR data is retained by sliding time windows, and newly added obstacle points are quickly integrated through KDtree insertion operations. Outdated points are automatically removed from the map to ensure the timeliness of environmental representation.

[0092] As an example of an embodiment of the present invention, the step of identifying obstacles in each voxel grid in the three-dimensional voxel grid map according to an occupancy threshold to obtain an obstacle set specifically involves:

[0093] When the occupancy threshold is greater than or equal to the number of point clouds in each voxel grid, the voxel grid is marked as an obstacle to obtain an initial obstacle set; based on the initial obstacle set, a dilation operation is performed using cube structuring elements, and the grid obtained by the dilation operation is merged with the initial obstacle set to obtain the obstacle set.

[0094] In this embodiment, when performing obstacle identification on each voxel grid in the 3D voxel grid map according to the occupancy threshold to obtain the obstacle set, firstly, the number of points in the filtered 3D point cloud data of each voxel grid in the 3D voxel grid map is counted. This number of points is compared with a preset occupancy threshold. When the number of points in a voxel grid is less than or equal to the occupancy threshold, the voxel grid is marked as an occupied grid with an obstacle. All occupied grids together constitute the initial obstacle set. The occupied grid O(i,j,k) is represented as:

[0095]

[0096] To prevent edge-grabbing, a safety margin is introduced during the 3D grid map construction process. The grid map undergoes 3D morphological dilation. Specifically, a cube-shaped structural element is selected as the structural element for the dilation operation. The size of this cube-shaped structural element is determined based on the safe distance required for UAV flight and the size of the voxel grid (for example, if the safe distance is k times the voxel size, the side length of the cube-shaped structural element is set to 2k+1 voxel sizes to ensure the dilation range covers the space corresponding to the safe distance). For each voxel grid in the initial obstacle set, the dilation operation is performed using the aforementioned cube-shaped structural element. This involves traversing all adjacent voxel grids in 3D space corresponding to the cube-shaped structural element and marking these adjacent voxel grids as newly added obstacle grids after dilation. The occupied state of the dilated grid is as follows:

[0097]

[0098] in, For the expansion operation, S is a cube structuring element with radius r and side length 2r+1. For a three-dimensional space centered at (0,0,0), this cube structuring element S... cube Defined as:

[0099]

[0100] Where i, j, k are the relative offsets of the raster indexes, with values ​​ranging from [-r, r]. Through the above process, 3D point cloud data can be transformed into a 3D voxel raster map containing obstacle information and safety margins, providing environmental constraints for subsequent 3D path planning.

[0101] After completing the expansion operation on all voxel grids in the initial obstacle set, the newly added obstacle grids obtained from the expansion are merged with the grids in the initial obstacle set, and duplicate grids are removed. Finally, an obstacle set containing the initial obstacles and the grids within the expanded safety boundary range is obtained. This set can more comprehensively reflect the areas that the UAV needs to avoid, ensuring flight safety.

[0102] It should be noted that in this embodiment, a three-dimensional morphological dilation algorithm is used to add safety boundaries to obstacles. This algorithm uses the physical dimensions of the drone in the database as a benchmark and uniformly expands the obstacle grid in all directions. This ensures that the actual distance between the planned path and the obstacle is not less than a set threshold, thus avoiding collision risks at the source.

[0103] Step S103: Use a target heuristic path search algorithm based on a composite heuristic function to plan the path of the UAV, obtain an initial path, smooth the initial path by using a B-spline curve, and optimize the smoothed path based on the minimization of the Snap trajectory optimization model to obtain the target path. The composite heuristic function is a heuristic function that combines a distance field penalty term, a direction deviation penalty term, and a curvature penalty term.

[0104] In this embodiment, the A* algorithm based on a composite heuristic function is used for UAV path planning. The A* algorithm is the core front-end search method for 3D path planning. Its core is to guide the search for the optimal path from the starting point to the target point through a cost function. For UAV obstacle avoidance scenarios, the algorithm is improved by combining the KDtree distance field under a 3D obstacle expansion map and adding two penalty terms: directional deviation penalty and dynamic curvature penalty. These two penalty mechanisms greatly improve the search efficiency and path smoothness of the A* algorithm in 3D space, avoiding edge-grabbing phenomena.

[0105] When smoothly fitting the initial path using B-spline curves, a set of points is constructed by extracting key data points of the initial path. The parameter values ​​of each point are determined by chord length parameterization. A node vector containing repeated nodes at both ends and uniformly interpolated nodes in the middle is constructed by combining the preset B-spline degree. The B-spline basis function is defined based on the recursive rule, and the curve equation is established. The least squares optimization problem is constructed with the goal of minimizing the sum of squared distances between the curve and the data points, and it is transformed into a system of linear equations. A regularization term is introduced to balance the smoothness of the curve and the fit of the points. The control points are obtained by solving the system of equations, and finally a smooth curve is generated to achieve smooth fitting of the initial path.

[0106] When optimizing a smoothed path based on the Minimum Snap Trajectory Optimization Model, the smoothed curve is used as the initial trajectory. The optimization objective is to minimize the integral of the square of the fourth derivative (Snap) of the trajectory in the time domain. At the same time, UAV kinematic constraints (such as maximum speed, acceleration, jerk value) and path constraints (such as avoiding occupied grids) are introduced. The optimization problem is solved by quadratic programming to obtain the target path that satisfies the constraints and has better motion smoothness.

[0107] As an example of an embodiment of the present invention, the smooth fitting of the initial path using a B-spline curve specifically involves:

[0108] Each path node in the initial path is used as a fitting data point for the B-spline curve. The chord length parameterization method is used to determine the corresponding parameter value for each fitting data point. Based on each fitting data point and the corresponding parameter value, a system of linear equations about the control points of the B-spline curve is constructed using the least squares method. The system of linear equations is solved by combining a regularization term to obtain multiple control points of the B-spline curve. A smoothed initial path is generated based on each control point.

[0109] In this embodiment, n+1 control points P0, P1, ..., P are given. n And a node vector U = [u0, u1, ..., u n+k+1 Then, the k-th degree B-spline curve is defined as C(u), expressed as:

[0110]

[0111] Where, N i,k (u) is a k-th degree B-spline basis function, u∈[u k ,u k+1 The symbol ] represents the parameter variable. The basis functions are defined recursively, calculated progressively from order 0 to order k. When k = 0, the zero-order basis function is represented as follows:

[0112]

[0113] Here, the zeroth-order basis function is an interval indicator function, indicating whether the parameter u falls within the node interval [u...]. i ,u i+1 Within ) . When k≥1, the higher-order basis functions are represented as follows:

[0114]

[0115] This formula decomposes a k-th degree basis function into a weighted sum of two (k-1)-th degree basis functions.

[0116] When processing the initial path using B-spline curves, assume that the initial path has a total of m data points, represented by the set S. q ={Q0,Q1,...,Q m The optimization problem is constructed using the principle of least squares fitting, to find n+1 control points P. i Make:

[0117]

[0118] Among them, u j These are the parameter values ​​corresponding to the data points, calculated through chord length parameterization:

[0119]

[0120] Then, substituting the B-spline curve equation into the least squares problem, we obtain a system of linear equations:

[0121]

[0122] Where A is an m×(n+1) coefficient matrix, and the elements A j,i =N i,k (u j P is the control point vector [P0, P1, ..., P2]. n ] T Q is the data point vector [Q0, Q1, ..., Q...]. m ] T .

[0123] To avoid overfitting and control curve smoothness, a regularization term is introduced:

[0124]

[0125] Here, λ is the regularization parameter, which controls the trade-off between smoothness and fitting accuracy.

[0126] B-spline fitting transforms the piecewise linear paths generated by the A* algorithm with improved KDtree distance fields into smooth curves through mathematical optimization, significantly enhancing the feasibility of path execution. The control points are solved using the least squares method, and regularization is combined to control smoothness, balancing path quality and computational efficiency.

[0127] As an example of an embodiment of the present invention, the optimization of the smoothed path based on the minimization of the Snap trajectory optimization model to obtain the target path is specifically as follows:

[0128] The smoothed initial path is divided into multiple trajectory segments, and the position function of each trajectory segment is determined. An optimization function is constructed with the objective of minimizing the sum of the squared integrals of Snap for all trajectory segments, where Snap is the fourth derivative of the position function. Based on the boundary conditions and continuity constraints of the UAV motion, the solution of the optimization function is transformed into a quadratic programming problem. The quadratic programming problem is solved to obtain the polynomial coefficients of each trajectory segment, and the target path is generated based on the polynomial coefficients.

[0129] In this embodiment, the Minimum Snap algorithm optimizes smoothness by minimizing the fourth derivative of the trajectory. Assuming the smoothed initial path is divided into M segments, each described by a polynomial of degree h, the optimization objective is to minimize the sum of the square integrals of the Snap values ​​for each segment. Then the... Segment trajectory (time interval) The position function is a polynomial of degree h:

[0130]

[0131] in, It is a three-dimensional position vector. For the first The polynomial coefficients of the segment (i = 0, 1, ..., h).

[0132] The derivatives of the position function polynomial (i.e., velocity, acceleration, jerk, and snap) are:

[0133]

[0134] The objective function is to minimize Snap, which is the sum of the squares of the Snap segments over the time interval, J, expressed as:

[0135]

[0136] Substituting into the Snap expression, it can be transformed into a quadratic form:

[0137]

[0138] Where α=[a 1,0 ,a 1,1 ,...,a 1,h ,...,a M,0 ,...,a M,h ]T Let Q be the coefficient vector of all segments, and let Q be a symmetric positive definite matrix, i.e., a block diagonal structure. Block corresponding to the first The Snap integral coefficients of the segment, whose elements are the product of polynomial coefficients and the time interval. Calculated Represented as:

[0139]

[0140] Furthermore, the trajectory must satisfy continuity constraints for position, velocity, acceleration, and jerk, as well as boundary conditions for the start / end point and position constraints for intermediate points, uniformly represented as:

[0141] Aα = b;

[0142] The starting point (t=t1), its position p1, velocity v1, acceleration a1, and jerk j1 must match the initial values, that is:

[0143]

[0144] And the endpoint (t=t) M+1 There are also similar constraints, namely:

[0145]

[0146] For intermediate path points Corresponding time The following relationship must be satisfied:

[0147]

[0148] At the connection point of two adjacent trajectory segments Position, velocity, acceleration, and jerk must be continuous, that is:

[0149]

[0150] The constraint matrix A and vector b are obtained by expanding the constraints mentioned above. Each row corresponds to a constraint equation, and the elements are the coefficients of a polynomial. The elements of b are the constraint objective values. Combining the objective function and the constraints, the trajectory optimization problem is transformed into a standard QP problem:

[0151]

[0152] By solving the QP equation, the polynomial coefficients α are obtained, and a smooth three-dimensional trajectory is reconstructed. After obtaining the coefficients, the position, velocity, acceleration, and jerk of each segment of the trajectory are calculated, and finally merged into a continuous three-dimensional trajectory.

[0153] As an example of an embodiment of the present invention, the UAV path planning is performed using a target heuristic path search algorithm based on composite heuristic functions to obtain an initial path, wherein the initial path includes multiple path nodes, specifically:

[0154] The starting node and target node in the path planning are determined. The voxel grid corresponding to the starting node is taken as the current node in the first round of path planning. In each round of path planning, the composite heuristic value between the current node and multiple reachable nodes of the current node is calculated according to the composite heuristic function. The path node of the current iteration is obtained according to the reachable node corresponding to the minimum value of the composite heuristic value. The reachable node refers to the node among the adjacent nodes of the current node that meets the preset node conditions. The preset node conditions are that the adjacent node does not belong to the occupied grid or does not belong to the path node determined in each round of path planning. The current node is iteratively updated according to the path node until the current node is the voxel grid corresponding to the target node. The iteration stops when the current node is the voxel grid corresponding to the target node, and the initial path is obtained.

[0155] In this embodiment, as Figure 2 The flowchart of the method steps shown first involves locating the voxel grids corresponding to the UAV's takeoff position and the mission endpoint position in the constructed target grid map (including the shortest obstacle distance for each voxel grid). The voxel grid corresponding to the takeoff position is set as the starting node, and the voxel grid corresponding to the mission endpoint position is set as the target node. At the same time, the three-dimensional center coordinates of the two nodes are recorded. The three-dimensional center coordinates are calculated from the voxel grid index and voxel size. For example, if the grid index is (x, y, z) and the voxel size is a, then the center coordinates are (x*a+a / 2, y*a+a / 2, z*a+a / 2).

[0156] The voxel grid corresponding to the starting node is used as the current node for the first round of path planning, and two node lists are initialized: open list S. openlist (used to store candidate nodes for calculating composite heuristic values) and the closed list S closedlist (Used to store the path nodes determined in each round), initially the current node is stored in the open list, and the closed list is empty.

[0157] In each round of path planning, the current node is first retrieved from the open list. Centered on the current node, the voxel grids in 26 adjacent directions (including 6 positive directions: front, back, left, right, up, and down, and 18 diagonal directions) in the 3D space are traversed. These adjacent grids are used as potential reachable nodes to be screened. Then, the potential reachable nodes are screened according to the preset node conditions. If a potential reachable node belongs to an occupied grid in the target grid map (including the original obstacle grid and the expanded safety boundary grid), or if the potential reachable node already exists in the closed list, the node is removed. The remaining nodes that meet the conditions are the reachable nodes of the current node.

[0158] The composite heuristic value from the current node to each reachable node is calculated based on the composite heuristic function. This composite heuristic function needs to be designed in conjunction with the distance field penalty term, the direction deviation penalty term, and the curvature penalty term. Specifically, the actual path cost g(n) from the starting point to the reachable node is determined first (which is the sum of the cumulative path cost from the starting node to the current node and the Euclidean distance from the current node to the reachable node, and the Euclidean distance is calculated from the three-dimensional center coordinates of the two nodes), and the heuristic cost h(n) from the reachable node to the target node is determined (which is the Euclidean distance between the three-dimensional center coordinates of the reachable node and the target node). Then, the distance field penalty term D(n) is calculated (if the shortest obstacle distance of the reachable node is less than the preset safety threshold, then D(n) is the difference between the safety threshold and the shortest obstacle distance multiplied by the distance penalty coefficient; otherwise, D(n) is 0, and the shortest obstacle distance is obtained from the target grid map). The deviation penalty term O(n) is calculated by multiplying the magnitude of the cross product of the vector from the starting point to the target node and the vector from the starting point to the reachable node by the deviation penalty coefficient. The vector is calculated from the three-dimensional center coordinates of the corresponding node. The curvature penalty term C(n) is calculated by first determining the movement direction vector from the last node in the closed list to the current node, and then calculating the movement direction vector from the current node to the reachable node. The angle θ between the two vectors is calculated using the vector dot product formula. C(n) is (1-cosθ) multiplied by the curvature penalty coefficient. If the closed list is empty, then C(n) is 0. Finally, g(n), h(n), and each penalty term are superimposed according to preset weights (e.g., f(n) = g(n) + h(n) + ω1D(n) + ω2O(n) + ω3C(n), where ω1, ω2, and ω3 are weight coefficients) to obtain the composite heuristic value for each reachable node.

[0159] The minimum value is selected from the composite heuristic values ​​of all reachable nodes. The reachable node corresponding to the minimum value is the path node of the current iteration. The path node is added to the closed list and removed from the open list. At the same time, the path node is stored in the open list as a candidate node for the next round of calculation.

[0160] Update the current node based on the path node determined in the current iteration (i.e., set the current node of the next round as the path node), and repeat the above operations of "traversing adjacent nodes → filtering reachable nodes → calculating composite heuristic values ​​→ determining path nodes → updating the current node" until the current node after a certain round of updates is the voxel raster corresponding to the target node. Stop the iteration, connect the nodes in the closed list in the order of iteration to form a continuous node sequence from the starting node to the target node. The three-dimensional spatial connection corresponding to this node sequence is the initial path.

[0161] As an example of an embodiment of the present invention, the step of calculating the composite heuristic value between the current node and multiple reachable nodes of the current node according to the composite heuristic function specifically involves:

[0162] For each reachable node, calculate the first-generation value between the reachable node and the starting node, and calculate the second-generation value between the reachable node and the target node. Determine an initial heuristic value based on the first-generation value and the second-generation value. Obtain a safe distance threshold and a distance cost weight. Based on the target grid map, when the shortest obstacle distance corresponding to the reachable node is greater than the safe distance threshold, calculate the distance difference between the shortest obstacle distance and the safe distance threshold. Weight the distance difference according to the distance cost weight to obtain the distance field penalty term corresponding to the reachable node. Obtain a deviation cost weight, determine a first vector and a second vector, calculate the first modulus corresponding to the cross product of the first vector and the second vector, and calculate the second modulus of the first vector. Combine this with the... The deviation cost weight, the first modulus, and the second modulus are used to obtain the direction deviation penalty term corresponding to the reachable node. The first vector represents the vector from the starting node to the target node, and the second vector represents the vector from the starting node to the current node. The curvature cost weight is obtained, and the first direction vector corresponding to the current node and the second direction vector corresponding to the reachable node are determined. The cosine value of the angle between the current node and the reachable node is calculated based on the first and second direction vectors. The curvature penalty term corresponding to the reachable node is obtained by combining the curvature cost weight and the cosine value. The initial heuristic value is adjusted by combining the distance field penalty term, the direction deviation penalty term, and the curvature penalty term to obtain the composite heuristic value.

[0163] In this embodiment, the first step is that the search priority of the A* algorithm is determined by the cost function f(n) = g(n) + h(n), where g(n) is the actual path cost from the starting point to the current node n, and h(n) is the heuristically estimated cost from the current node n to the target point. The algorithm achieves efficient optimal path search by prioritizing the expansion of the node with the smallest f(n).

[0164] The second step is to determine the grid occupancy state O after the expansion in step two. inflated (i,j,k) is combined with the KDtree to dynamically update the distance field set D(q) from any grid q in the space to the nearest grid occupied by an obstacle, defined as:

[0165]

[0166] in, For Euclidean distance, o∈O inflated (i,j,k) represents the set of occupied grid cells dynamically maintained by the KDtree.

[0167] Based on the distance field D(q), the node evaluation function of A* will be extended as follows:

[0168]

[0169] Where g(q) is the first-generation value, representing the actual cost from the starting point to the current node q, h(q) is the second-generation value, representing the heuristic cost from node q to the target point, and ω... d r represents the distance field cost weight. safe As a safety distance threshold, nodes with a penalty distance field smaller than the safety threshold are used to guide the search to shift towards a safe area.

[0170] The third step is to add a penalty for directional deviation. This penalty is essentially the penalty for the current node q. c To the starting point q start -Target point q goal The perpendicular distance between the lines can guide the planned path to prioritize paths that are closer to a straight line, reducing redundant searches. The directional deviation penalty is calculated using the cross product of vectors to determine the degree to which the current node deviates from the line connecting the start and target points, expressed as a deviation penalty cost J. d express:

[0171]

[0172] Among them, v sg Represents the starting point q start to target point q goal The vector, v cs Represents the starting point q start To the current node q c The vector, × represents the cross product operation, k d This indicates the deviation penalty coefficient.

[0173] The fourth step is to add a dynamic curvature penalty. Without a curvature penalty, the path will contain sharp turns, which is unsuitable for moving vehicles such as drones. After adding a dynamic curvature penalty, the generated path will be smoother and conform to dynamic constraints. The dynamic curvature penalty is based on the cosine of the angle between the current movement direction and the previous movement direction, and the curvature penalty cost J is used. c express:

[0174]

[0175] Among them, v prev Indicates the direction of movement from the previous sampling time, v curr k represents the movement time at the current sampling time. c This represents the curvature penalty coefficient, which controls the impact of curvature on path planning.

[0176] The complete A* algorithm for the KDtree distance field improvement can now be expressed as:

[0177]

[0178] By adjusting the weight parameters, a balance can be achieved between obstacle avoidance safety, search efficiency, and path smoothness, resulting in a better 3D path planning route.

[0179] Compared with existing technologies, this method has the following advantages:

[0180] (1) The environment is more accurately represented and the obstacle modeling is more robust. The three-dimensional voxel grid map accurately describes the spatial distribution of obstacles by quantizing the grid occupancy state, and reduces the impact of sensor noise by combining multi-frame point cloud fusion; the traditional graph search relies on simplified two-dimensional grids or topological maps, which are prone to losing three-dimensional information; the sampling method represents the environment by random points, which is difficult to accurately capture obstacle details and is prone to generating false free space.

[0181] (2) Good controllability of safety margin. The three-dimensional voxel grid map can precisely control the safety margin through morphological dilation operation, and the dilation process expands uniformly in three-dimensional space, ensuring that the UAV maintains a safe distance in the xyz directions and avoiding the risk of collision in the vertical direction due to two-dimensional dilation.

[0182] (3) Fast planning speed and high efficiency. The 3D voxel grid map combined with KDtree can realize real-time updating of obstacles: when the lidar detects a new obstacle, it is only necessary to update the occupancy state of the local voxels through the insertion operation of KDtree, without reconstructing the entire map. The improved A* algorithm can quickly trigger local replanning based on the updated grid (such as only re-searching the path segments affected by the obstacle), with low computational burden and high real-time performance.

[0183] (4) Strong environmental adaptability. The improved A* algorithm makes the heuristic function more goal-oriented by integrating directional deviation penalty (path straightness guidance based on vector cross product) and distance field information (obstacle distance penalty based on KDtree). In particular, when the UAV approaches an obstacle, the distance field penalty term significantly increases the evaluation cost of nodes in that area, guiding the algorithm to prioritize exploring paths away from obstacles and towards the target, making path planning safer.

[0184] As an example of an embodiment of the present invention, firstly, some obstacles with different lengths, widths, and heights are randomly generated, such as... Figure 3 The blue block-shaped area in the image represents the obstacle portion for the UAV to perform 3D obstacle avoidance. The starting point is set as the green sphere in the image, and the ending point is set as the pink pentagram in the image. The mission objective is for the UAV to avoid obstacles from the starting point and reach the ending point. By comparing the A* algorithm before and after the improvement in the same environment, the resulting flight path is analyzed.

[0185] Figure 3 and Figure 4 The figures show the side and top views of the 3D path planning results of the un-improved A* algorithm on a 3D map. As can be seen from the figures, the un-improved 3D path planning results have significant defects. The 3D trajectory exhibits obvious oscillation characteristics, with frequent right-angle and acute-angle turns during direction adjustments. Such non-smooth trajectories fundamentally conflict with the physical motion characteristics of the UAV. The UAV's power system and attitude control struggle to respond to high-frequency, drastic directional changes, easily leading to decreased flight stability and even fuselage vibration, severely restricting its ability to safely traverse obstacles and accurately reach the target point. Further observation from the top view reveals that even if the path eventually reaches the target point, there is a severe "edge-hugging phenomenon," with the distance between the trajectory and the obstacle boundary often less than the safety threshold. This not only unnecessarily increases energy consumption and time costs due to path redundancy but also amplifies the collision risk due to system positioning errors or external disturbances, significantly reducing the reliability of mission execution.

[0186] Figure 5 and Figure 6The figures show the side and top views of the improved A* algorithm's 3D path planning results on a 3D map. As can be seen, the improved 3D path planning results achieve a performance leap in multiple dimensions. The oscillation amplitude of the 3D trajectory is effectively suppressed, the overall smoothness is significantly improved, sharp angle deflections are eliminated during directional transitions, and the turning angle is always controlled within the physical range where the UAV can respond smoothly, highly adapting to the aircraft's dynamic constraints. Most importantly, by introducing a morphological dilation algorithm to expand the safety boundary of the obstacle area, a stable safety buffer zone is formed between the trajectory and obstacles, solving the problem of edge-hugging. This optimization not only simplifies the path length and reduces resource consumption but also avoids potential collision risks from a spatial layout perspective, making the planning results more closely match the actual flight needs of the UAV and providing higher safety and efficiency guarantees for autonomous navigation in complex environments.

[0187] like Figure 7 As shown, based on the above method embodiments, corresponding system embodiments are provided;

[0188] One embodiment of the present invention provides an unmanned aerial vehicle (UAV) path planning system 700, including: a point cloud processing module 701, an obstacle module 702, and a path planning module 703;

[0189] The point cloud processing module 701 is used to acquire initial three-dimensional point cloud data, perform multi-coordinate system transformation and filtering on the initial three-dimensional point cloud data to obtain three-dimensional point cloud data, and perform voxel raster mapping on the three-dimensional point cloud data to obtain a three-dimensional voxel raster map.

[0190] The obstacle module 702 is used to identify obstacles in each voxel grid in the three-dimensional voxel grid map according to the occupancy threshold, obtain an obstacle set, generate a KDtree structure according to the three-dimensional center coordinates of each occupied grid, and calculate the Euclidean distance from any voxel grid in the three-dimensional voxel grid map to the occupied grid with the shortest distance through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles.

[0191] The path planning module 703 is used to perform UAV path planning using a target heuristic path search algorithm based on a composite heuristic function to obtain an initial path, to smoothly fit the initial path using a B-spline curve, and to optimize the smoothed path based on a minimized Snap trajectory optimization model to obtain a target path. The composite heuristic function is a heuristic function that combines a distance field penalty term, a direction deviation penalty term, and a curvature penalty term.

[0192] It is understood that the above system item embodiments correspond to the method item embodiments of the present invention, and can implement the UAV path planning X method provided by any of the above method item embodiments of the present invention.

[0193] It should be noted that the system embodiments described above are merely illustrative, and some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Furthermore, in the accompanying drawings of the system embodiments provided by this invention, the connection relationships between modules indicate that they have communication connections, which can be specifically implemented as one or more communication buses or signal lines. Those skilled in the art can understand and implement this without any creative effort.

[0194] For ease of description and brevity, the system embodiments of the present invention include all the implementation methods described in the above embodiments of the UAV-based information recognition method, and will not be repeated here.

[0195] Based on the above embodiments of the UAV path planning method, another embodiment of the present invention provides a terminal device, which includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements the UAV path planning method of any embodiment of the present invention.

[0196] For example, in this embodiment, the computer program can be divided into one or more modules, which are stored in the memory and executed by the processor to complete the present invention. The one or more modules may be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the computer program in the terminal device.

[0197] The terminal device may be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and a memory.

[0198] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal device, connecting all parts of the terminal device via various interfaces and lines.

[0199] Based on the above-described method embodiments, another embodiment of the present invention provides a computer-readable storage medium including a stored computer program, wherein, when the computer program is executed, it controls the device where the computer-readable storage medium is located to execute the UAV path planning method described in any of the above-described method embodiments of the present invention.

[0200] The modules / units integrated in the device / terminal equipment, if implemented as software functional units and sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc.

[0201] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A method for unmanned aerial vehicle (UAV) path planning, characterized in that, include: Acquire initial 3D point cloud data, perform multi-coordinate system transformation and filtering on the initial 3D point cloud data to obtain 3D point cloud data, and perform voxel raster mapping on the 3D point cloud data to obtain a 3D voxel raster map. Obstacles are identified in each voxel grid in the 3D voxel grid map based on the occupancy threshold to obtain an obstacle set. A KDtree structure is generated based on the 3D center coordinates of each occupied grid. The Euclidean distance from any voxel grid in the 3D voxel grid map to the occupied grid with the shortest distance is calculated through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles. A target heuristic path search algorithm based on a composite heuristic function is used for UAV path planning to obtain an initial path. The initial path is then smoothed by B-spline curves, and the smoothed path is optimized based on the minimization of the Snap trajectory optimization model to obtain the target path. The composite heuristic function is a heuristic function that combines a range field penalty term, a direction deviation penalty term, and a curvature penalty term.

2. The UAV path planning method as described in claim 1, characterized in that, The method employs a target heuristic path search algorithm based on composite heuristic functions for UAV path planning to obtain an initial path. This initial path includes multiple path nodes, specifically: Determine the starting and target nodes in the path planning; The voxel grid corresponding to the starting node is taken as the current node of the first round of path planning. In each round of path planning, the composite heuristic value between the current node and multiple reachable nodes of the current node is calculated according to the composite heuristic function. The path node of the current iteration is obtained according to the reachable node corresponding to the minimum value of the composite heuristic value. The reachable node refers to the node among the adjacent nodes of the current node that meets the preset node conditions. The preset node conditions are that the adjacent node does not belong to the occupied grid or the adjacent node does not belong to the path node determined in each round of path planning. The current node is iteratively updated based on the path nodes until the current node is the voxel grid corresponding to the target node, at which point the iteration stops, and the initial path is obtained.

3. The UAV path planning method as described in claim 2, characterized in that, The step of calculating the composite heuristic value between the current node and multiple reachable nodes of the current node according to the composite heuristic function is specifically as follows: For each reachable node, calculate the first-generation value between the reachable node and the starting node, and calculate the second-generation value between the reachable node and the target node. Determine the initial heuristic value based on the first-generation value and the second-generation value. Obtain a safe distance threshold and a distance cost weight. Based on the target grid map, when the shortest obstacle distance corresponding to the reachable node is greater than the safe distance threshold, calculate the distance difference between the shortest obstacle distance and the safe distance threshold. Perform a weighted calculation on the distance difference according to the distance cost weight to obtain the distance field penalty term corresponding to the reachable node. Obtain the deviation cost weight, determine the first vector and the second vector, calculate the first magnitude corresponding to the cross product of the first vector and the second vector, and calculate the second magnitude of the first vector. Combine the deviation cost weight, the first magnitude and the second magnitude to obtain the direction deviation penalty term corresponding to the reachable node. The first vector is used to represent the vector from the starting node to the target node, and the second vector table is used to represent the vector from the starting node to the current node. Obtain curvature cost weights, determine the first direction vector corresponding to the current node and the second direction vector corresponding to the reachable node, calculate the cosine value of the angle between the current node and the reachable node based on the first direction vector and the second direction vector, and combine the curvature cost weights and the cosine value of the angle to obtain the curvature penalty term corresponding to the reachable node. The initial heuristic value is adjusted by combining the distance field penalty term, the direction deviation penalty term, and the curvature penalty term to obtain the composite heuristic value.

4. The UAV path planning method as described in claim 1, characterized in that, The step of identifying obstacles in each voxel grid in the three-dimensional voxel grid map based on an occupancy threshold to obtain an obstacle set is specifically as follows: When the occupancy threshold is greater than or equal to the number of point clouds in each voxel grid, the voxel grid is marked as an obstacle, and an initial obstacle set is obtained; Based on the initial obstacle set, a dilation operation is performed using cube structuring elements. The resulting grid is then merged with the initial obstacle set to obtain the obstacle set.

5. The UAV path planning method as described in claim 1, characterized in that, The process of acquiring initial 3D point cloud data, and performing multi-coordinate system transformation and filtering on the initial 3D point cloud data to obtain 3D point cloud data specifically involves: The initial three-dimensional point cloud data is obtained by collecting the lidar of the UAV. The installation tilt angle of the lidar is obtained based on the pose information of the UAV. A compensation rotation matrix is ​​determined according to the installation tilt angle. The coordinates of each point cloud in the initial three-dimensional point cloud data are transformed according to the compensation rotation matrix to obtain the second point cloud data, so as to transform the initial three-dimensional point cloud data in the sensor coordinate system to the second point cloud data in the body coordinate system. Based on the pose information of the UAV, the pitch angle, roll angle and yaw angle of the UAV are obtained, and the pitch matrix, roll matrix and yaw matrix are determined according to the pitch angle, roll angle and yaw angle respectively. The pitch matrix, roll matrix, and yaw matrix are multiplied together to obtain a composite rotation matrix. The coordinates of each point cloud in the second point cloud data are then transformed according to the composite rotation matrix to obtain the third point cloud data, thereby transforming the second point cloud data in the body coordinate system to the third point cloud data in the world coordinate system. A height range threshold is obtained, and ground point cloud data and noise data in the third point cloud data are filtered according to the height threshold to obtain the fourth point cloud data. The fourth point cloud data is then processed using a statistical filtering algorithm to obtain the three-dimensional point cloud data.

6. The UAV path planning method as described in claim 1, characterized in that, The smoothing and fitting of the initial path using a B-spline curve specifically involves: Each path node in the initial path is used as a fitting data point for the B-spline curve, and the chord length parameterization method is used to determine the corresponding parameter value for each fitting data point. Based on each of the fitted data points and the corresponding parameter values, a system of linear equations about the control points of the B-spline curve is constructed using the least squares method. The linear equations are solved by combining regularization terms to obtain multiple control points of the B-spline curve, and a smoothed initial path is generated based on each control point.

7. The UAV path planning method as described in claim 6, characterized in that, The optimization of the smoothed path based on the minimized Snap trajectory optimization model yields the target path, specifically as follows: The smoothed initial path is divided into multiple trajectory segments, and the position function of each trajectory segment is determined; Construct an optimization function with the objective of minimizing the sum of squared integrals of Snap for all trajectory segments, where Snap is the fourth derivative of the position function; Based on the boundary conditions and continuity constraints of the UAV's motion, the solution of the optimization function is transformed into a quadratic programming problem; Solve the quadratic programming problem to obtain the polynomial coefficients of each trajectory segment, and generate the target path based on the polynomial coefficients.

8. A UAV path planning system, characterized in that, include: Point cloud processing module, obstacle module, and path planning module; The point cloud processing module is used to acquire initial three-dimensional point cloud data, perform multi-coordinate system transformation and filtering on the initial three-dimensional point cloud data to obtain three-dimensional point cloud data, and perform voxel raster mapping on the three-dimensional point cloud data to obtain a three-dimensional voxel raster map. The obstacle module is used to identify obstacles in each voxel grid in the three-dimensional voxel grid map according to an occupancy threshold, obtain an obstacle set, generate a KDtree structure according to the three-dimensional center coordinates of each occupied grid, and calculate the Euclidean distance from any voxel grid in the three-dimensional voxel grid map to the occupied grid with the shortest distance through the K-nearest neighbor query of the KDtree to obtain a target grid map. The target grid map includes the shortest obstacle distance of each voxel grid, and the obstacle set includes multiple occupied grids with obstacles. The path planning module is used to perform UAV path planning using a target heuristic path search algorithm based on a composite heuristic function to obtain an initial path, to smoothly fit the initial path using a B-spline curve, and to optimize the smoothed path based on a minimized Snap trajectory optimization model to obtain a target path. The composite heuristic function is a heuristic function that combines a distance field penalty term, a direction deviation penalty term, and a curvature penalty term.

9. A terminal device, characterized in that, The device includes a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein when the processor executes the computer program, it implements the UAV path planning method as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, include: A stored computer program, wherein, when the computer program is executed, it controls the device containing the computer-readable storage medium to perform the UAV path planning method as described in any one of claims 1-7.

Images