Obstacle avoidance and path planning method and device for unmanned aerial vehicle

Abstract

The present disclosure provides an unmanned aerial vehicle obstacle avoidance and path planning method and device. The technical scheme of the present disclosure reduces the blind selection of nodes in the target optimal search through a greedy heuristic strategy, and improves the local convergence speed. The distance information of the nodes and the obstacles is used to dynamically determine the gravitational component and the expansion step, so that the gravitational component can be used to accelerate the expansion towards the terminal point when far away from the obstacles, and the randomness is increased to effectively avoid obstacles when close to the obstacles, thereby ensuring the completeness and efficiency of the route search. In addition, the present disclosure removes redundant nodes through smoothness optimization, significantly improving the smoothness and overall quality of the path.

Description

Technical Field

[0001] This disclosure relates to the field of autonomous control of unmanned aerial vehicles (UAVs), and in particular to a method and apparatus for UAV obstacle avoidance and trajectory planning. Background Technology

[0002] With the rapid development of drone technology, its application scenarios have expanded from simple open environments to complex urban and jungle environments. In these complex environments, drones need to have the ability to perform autonomous remote sensing operations at all times and in all areas, among which autonomous obstacle avoidance and trajectory planning are key technologies to achieve this capability.

[0003] Currently, trajectory planning algorithms suffer from strong randomness in node selection, resulting in a large number of redundant nodes in complex task spaces, which slows down the convergence speed of trajectory planning. At the same time, the generated trajectories are often tortuous and have poor smoothness, making them difficult to use directly for actual flight control of UAVs.

[0004] Although some improvement strategies have been proposed in existing studies, there is still room for improvement in how to effectively balance search speed, obstacle avoidance safety and trajectory smoothness while ensuring the completeness of search probabilities. Summary of the Invention

[0005] This disclosure provides at least one method and apparatus for obstacle avoidance and trajectory planning of unmanned aerial vehicles (UAVs) to solve at least one of the above-mentioned technical problems.

[0006] According to one aspect of this disclosure, a method for obstacle avoidance and trajectory planning of an unmanned aerial vehicle (UAV) is provided, comprising:

[0007] Acquire point cloud data of the flight environment of the drone, and extract obstacle point cloud from the point cloud data;

[0008] Based on the obstacle point cloud, a convex surface model of the obstacle is constructed, and the flight safety zone is determined based on the convex surface model.

[0009] Within the flight safety area, the following steps are performed iteratively to generate a random tree until a newly generated node is located in the endpoint region:

[0010] A greedy heuristic strategy is adopted to determine random target points based on a preset probability threshold;

[0011] Based on the distance information between nodes and obstacles, a first gravitational component in the direction of the endpoint and a second gravitational component in the direction of the random target point are determined; based on the first gravitational component and the second gravitational component, a first target expansion step size in the direction of the endpoint and a second target expansion step size in the direction of the random target point are determined; combining the node in the random tree closest to the random target point, the first target expansion step size, and the second target expansion step size, candidate new nodes are determined;

[0012] Based on the convex model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; if there is no obstacle threat, the candidate new node is treated as a new node and added to the random tree;

[0013] An initial trajectory is generated using the nodes in the random tree, and the initial trajectory is smoothed to obtain the target planned trajectory.

[0014] In one possible implementation, the step of employing a greedy heuristic strategy to determine random target points based on a preset probability threshold includes:

[0015] Set a preset probability threshold;

[0016] Generate a random number in the interval [0, 1].

[0017] If the random number is less than the preset probability threshold, the random target point is set as the endpoint;

[0018] If the random number is greater than or equal to the preset probability threshold, a point is randomly selected within the task space as the random target point.

[0019] In one possible implementation, determining candidate new nodes by combining the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size includes:

[0020] Candidate new nodes are determined using the following formula:

[0021]

[0022] in, As a candidate new node, The node in the random tree that is closest to the random target point; The step size for expanding towards the random target point. The step size is the extension step size towards the endpoint; The second gravitational component, This is the first gravitational component; To extend the step size towards the second target in the direction of the random target point; To extend the step size towards the first target in the direction of the endpoint.

[0023] In one possible implementation, the second gravitational component Calculate using the following formula:

[0024]

[0025] In the formula, Let M be the coordinates of the center of the i-th obstacle, and M be the number of obstacles. , eff Let the radius be the obstacle. , where (x, y, z) is the adjustment factor, and (x, y, z) are the coordinates of the node in the random tree closest to the random target point.

[0026] In one possible implementation, the first target expansion step size satisfies the following constraints:

[0027] If the first target expansion step size is less than the minimum step size, then the first target expansion step size is set to the minimum step size;

[0028] If the first target expansion step size is greater than the maximum step size, then the first target expansion step size is set to the maximum step size;

[0029] The expansion step size of the second objective satisfies the following constraints:

[0030] If the second target expansion step size is less than the minimum step size, then the second target expansion step size is set to the minimum step size;

[0031] If the second target expansion step size is greater than the maximum step size, then the second target expansion step size is set to the maximum step size.

[0032] In one possible implementation, constructing a convex surface model of the obstacle based on the obstacle point cloud includes:

[0033] Using the obstacle point cloud, the obstacle is fitted as at least one convex surface model among a hemisphere, cylinder, cone, or frustum;

[0034] The approximate expression of the convex solid model is as follows:

[0035]

[0036] in, Let be any point on the convex model. The coordinates of the center of the convex solid model are: These are preset parameters.

[0037] In one possible implementation, determining whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree, based on the convex model and the nodes in the random tree, includes:

[0038] Based on the convex body model, the obstacle threat area is determined;

[0039] The previous node of the candidate new node is determined based on the nodes in the random tree;

[0040] If the flight path between the previous node and the candidate new node is at least partially located within the obstacle threat area, then it is determined that there is an obstacle threat on the path between the candidate new node and the parent node of the random tree.

[0041] In one possible implementation, smoothing the initial trajectory to obtain the target planned trajectory includes:

[0042] Iterate through the nodes in the initial track and perform the following operations:

[0043] Determine if there are any obstacles or threats in the connection between the current node and the node two after next;

[0044] If there is no threat, discard the next node of the current node and directly connect the current node to the node after that.

[0045] If a threat exists, retain the next node of the current node, or take the midpoint between the current node and the node after that as the new current node, and return to the step of determining whether there is an obstacle threat in the connection between the current node and the node after that.

[0046] In one possible implementation, extracting obstacle point clouds from the point cloud data includes:

[0047] The point cloud data is resampled using the farthest point sampling method, and the center point of the point cloud is determined based on the sampling results.

[0048] The point cloud data is grouped according to the center point of the point cloud to obtain multiple local neighborhoods;

[0049] By using a miniature PointNet network to extract features from each neighborhood, local feature vectors representing the overall shape of each neighborhood are obtained.

[0050] Using an encoder-decoder structure, based on the extracted local feature vectors, the ground, vegetation, buildings, and power lines contained in the point cloud of the grouped local neighborhood are classified and segmented.

[0051] According to another aspect of this disclosure, a drone obstacle avoidance and trajectory planning device is provided, comprising:

[0052] The point cloud preliminary processing module is used to acquire point cloud data of the flight environment of the UAV and extract obstacle point clouds from the point cloud data;

[0053] The region segmentation module is used to construct a convex surface model of the obstacle based on the obstacle point cloud, and to determine the flight safety area based on the convex surface model.

[0054] The random tree generation module is used to iteratively perform the following steps within the flight safety area to generate a random tree until a newly generated node is located in the endpoint region:

[0055] A greedy heuristic strategy is adopted to determine random target points based on a preset probability threshold;

[0056] Based on the distance information between nodes and obstacles, a first gravitational component in the direction of the endpoint and a second gravitational component in the direction of the random target point are determined; based on the first gravitational component and the second gravitational component, a first target expansion step size in the direction of the endpoint and a second target expansion step size in the direction of the random target point are determined; combining the node in the random tree closest to the random target point, the first target expansion step size, and the second target expansion step size, candidate new nodes are determined;

[0057] Based on the convex model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; if there is no obstacle threat, the candidate new node is treated as a new node and added to the random tree;

[0058] The trajectory generation module is used to generate an initial trajectory using the nodes in the random tree, and to smooth the initial trajectory to obtain the target planned trajectory.

[0059] The UAV obstacle avoidance and trajectory planning method and apparatus disclosed herein first acquires point cloud data of the UAV's flight environment and extracts obstacle point cloud data from the point cloud data; then, based on the obstacle point cloud data, a convex surface model of the obstacle is constructed, and a flight safety zone is determined based on the convex surface model; then, within the flight safety zone, the following steps are iteratively executed to generate a random tree until a newly generated node is located in the endpoint region: a greedy heuristic strategy is used to determine a random target point according to a preset probability threshold; based on the distance information between the node closest to the random target point in the random tree and the obstacle, a first gravitational component in the endpoint direction and a second gravitational component in the direction of the random target point are determined; based on the first gravitational component... Based on the quantity and the second gravitational component, determine the first target expansion step size towards the endpoint and the second target expansion step size towards the random target point; combine the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size to determine candidate new nodes; based on the convex body model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; if there is no obstacle threat, add the candidate new node as a new node to the random tree; finally, use the nodes in the random tree to generate an initial trajectory and smooth the initial trajectory to obtain the target planned trajectory. The technical solution disclosed herein reduces the blind selection of random target points during the optimal search by employing a greedy heuristic strategy, thereby improving the local convergence speed. By dynamically determining the gravitational component and expansion step size based on the distance information between nodes and obstacles, it is possible to accelerate the expansion towards the endpoint by utilizing the gravitational component when moving away from obstacles, and to increase randomness to effectively avoid obstacles when approaching them, thus ensuring the completeness and efficiency of the route search. In addition, this disclosure removes redundant nodes through smoothness optimization, significantly improving the smoothness and overall quality of the trajectory.

[0060] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of this disclosure, nor is it intended to limit the scope of this disclosure. Other features of this disclosure will become readily apparent from the following description. Attached Figure Description

[0061] The accompanying drawings are provided to better understand this solution and do not constitute a limitation of this disclosure. Wherein:

[0062] Figure 1 This is a flowchart of a drone obstacle avoidance and trajectory planning method in one embodiment of this disclosure;

[0063] Figure 2 This is a schematic diagram of the structure of an unmanned aerial vehicle obstacle avoidance and trajectory planning device in one embodiment of the present disclosure. Detailed Implementation

[0064] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of the embodiments to aid understanding, and should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0065] This disclosure addresses the shortcomings of UAV path search, such as low search speed, poor obstacle avoidance safety, and insufficient trajectory smoothness, by proposing a UAV obstacle avoidance and trajectory planning method and apparatus. The technical solution of this disclosure reduces the blind selection of random target points during optimal target search through a greedy heuristic strategy, thereby improving local convergence speed. By dynamically determining the gravitational component and expansion step size based on the distance information between nodes and obstacles, it enables accelerated expansion towards the endpoint using the gravitational component when moving away from obstacles, and increases randomness to effectively avoid obstacles when approaching them, ensuring the completeness and efficiency of the path search. Furthermore, this disclosure removes redundant nodes through smoothness optimization, significantly improving the smoothness and overall quality of the trajectory.

[0066] The technical solution of this disclosure will be described below through specific embodiments.

[0067] like Figure 1 The diagram shown is a flowchart of the UAV obstacle avoidance and trajectory planning method of this embodiment. The execution subject of this embodiment is a computing device, component, or platform with data processing capabilities. Specifically, the method of this embodiment may include the following steps:

[0068] S110. Obtain point cloud data of the flight environment of the UAV, and extract obstacle point cloud from the point cloud data.

[0069] Specifically, airborne lidar (such as the XT32M multi-line lidar) can be used to collect point cloud data.

[0070] The extraction of obstacle point clouds from the aforementioned point cloud data specifically includes:

[0071] The point cloud data is resampled using the farthest point sampling method, and the center point of the point cloud is determined based on the sampling results. The point cloud data is then grouped according to the center point to obtain multiple local neighborhoods. Feature extraction is performed on each local neighborhood using a mini PointNet network to obtain local feature vectors representing the overall shape of each neighborhood. Using an encoder-decoder structure, the ground, vegetation, buildings, and power lines contained in the point cloud of the grouped local neighborhoods are classified and segmented based on the extracted local feature vectors.

[0072] The above steps can be implemented using the PointNet++ network architecture. Since the original point cloud is disordered and sparse, this embodiment uses the PointNet++ deep learning network to segment the point cloud. PointNet++ obtains the center points of the point cloud through farthest point sampling (FPS), and performs grouping and feature extraction, solving the problem of missing local features and accurately classifying the point cloud into ground, vegetation, buildings, etc.

[0073] S120. Based on the obstacle point cloud, construct a convex surface model of the obstacle, and determine the flight safety area based on the convex surface model.

[0074] For detected obstacles, to simplify calculations, they are fitted as convex body models (such as hemispheres, cylinders, cones, etc.). Constructing the convex body model of the obstacle specifically includes: using the obstacle point cloud, fitting the obstacle as at least one convex body model selected from hemispheres, cylinders, cones, or frustums.

[0075] The approximate expression of the convex solid model is as follows:

[0076]

[0077] in, Let be any point on the convex model. The coordinates of the center of the convex solid model are: The parameters r, k, and T are preset parameters that determine the shape and size of the obstacle. When r < 1, T = 1, and T > 1, it represents the obstacle being inside, on the surface, and outside the model, respectively. The fitted convex body model can be one or more.

[0078] S130. Within the flight safety area, iteratively execute the step of generating new nodes to generate a random tree until the generated new node is located in the endpoint region:

[0079] S1301. A greedy heuristic strategy is adopted to determine random target points based on a preset probability threshold.

[0080] Specifically, the steps include: setting a preset probability threshold; generating a random number in the interval [0, 1]; setting a random target point as the endpoint if the random number is less than the preset probability threshold; and randomly selecting a point in the task space as the random target point if the random number is greater than or equal to the preset probability threshold. The task space is defined as follows:

[0081] The mission space refers to the three-dimensional geometric space in which the UAV performs trajectory planning, denoted as This space includes the starting point and ending point of the drone's flight, the threat area occupied by all known or perceived obstacles, and the safe zone for flight without obstacles; wherein, the obstacle threat area is denoted as... The safe flight zone is recorded as And satisfy:

[0082]

[0083] The mission space consists of both the obstacle threat zone and the safe flight zone, and the objective of trajectory planning is to fly within the safe flight zone. Search for a collision-free feasible path from the origin to the destination.

[0084] This strategy incorporates a greedy approach in the direction of the endpoint, causing the random tree to grow as far towards the endpoint as possible until it encounters an obstacle. The preset probability threshold can be determined by the average planning time and number of nodes in flight tests.

[0085] Regarding the preset probability threshold The specific method for determining the parameters is as follows: This disclosure provides a parameter calibration mechanism based on a multi-objective cost function, which includes the following steps:

[0086] Step 1: Construct the test sample set and parameter sequence.

[0087] Set up multiple simulated or actual flight test scenarios for the UAV in typical complex environments; set probability thresholds. The test sequence, let Increment from 0.05 to 0.95, with a step size of 0.05.

[0088] Step 2: Perform the Monte Carlo test and collect indicator data.

[0089] For each of the test sequences Values ​​are retrieved within a defined scenario. Sub-independent trajectory planning test (e.g.) ), and record the planning time for each test. and the total number of generated random tree nodes .

[0090] Calculation in the current Average planning time With average number of nodes :

[0091]

[0092]

[0093] Step 3: Dimensionless normalization of evaluation indicators.

[0094] Due to average planning time (Unit: seconds) and average number of nodes The units (in terms of quantity and order of magnitude) are different, so they need to be normalized to their minimum and maximum values.

[0095]

[0096] in, and Each for all The maximum and minimum values ​​of time and number of nodes in the test group.

[0097] Step 4: Calculate the comprehensive cost function and select the optimal threshold.

[0098] Construct about probability thresholds Comprehensive evaluation cost function :

[0099]

[0100] in, and For the weighting coefficients, satisfying The weighting coefficients are assigned values ​​based on the emphasis placed on computational real-time performance and memory consumption by the UAV mission (for example, for airborne platforms with limited computing power, a specific weighting coefficient can be set). ).

[0101] Finally, compare all Comprehensive cost function under the given value Select to make Get the minimum value This serves as a preset probability threshold for final application in actual flight.

[0102] S1302. Based on the distance information between the node closest to the random target point in the random tree and the obstacle, determine the first gravitational component in the direction of the destination and the second gravitational component in the direction of the random target point; based on the first gravitational component and the second gravitational component, determine the first target expansion step size in the direction of the destination and the second target expansion step size in the direction of the random target point; combine the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size to determine candidate new nodes.

[0103] The above-mentioned method of determining candidate new nodes by combining the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size includes:

[0104] Candidate new nodes are determined using the following formula:

[0105]

[0106] in, As a candidate new node, The node in the random tree that is closest to the random target point; The step size for expanding towards the random target point. The step size is the extension step size towards the endpoint; For the second gravitational component, 1- This is the first gravitational component; To extend the step size towards the second target in the direction of the random target point; To extend the step size towards the first target in the direction of the endpoint.

[0107] It is a vector whose direction is from point to The length is usually the preset base step size. . It is also a vector, and its direction is from point to The length is also usually .

[0108] and The composite vector obtained by adding these two vectors is What needs to be constrained is the length of this composite vector (that is, the distance of this actual flight step of the drone): Must between)

[0109] Second gravitational component Calculate using the following formula:

[0110]

[0111] In the formula, Let M be the coordinates of the center of the i-th obstacle, and M be the number of obstacles. , eff Let the radius be the obstacle. , where (x, y, z) is the adjustment factor, and (x, y, z) are the coordinates of the node in the random tree closest to the random target point.

[0112] The first target expansion step size satisfies the following constraints: if the first target expansion step size is less than the minimum step size, then the first target expansion step size is set to the minimum step size; if the first target expansion step size is greater than the maximum step size, then the first target expansion step size is set to the maximum step size.

[0113] The second target expansion step size satisfies the following constraints: if the second target expansion step size is less than the minimum step size, then the second target expansion step size is set to the minimum step size; if the second target expansion step size is greater than the maximum step size, then the second target expansion step size is set to the maximum step size.

[0114] The above steps, when the node is far away from the obstacle, The new nodes are relatively small, and they are mainly oriented towards the endpoint. Expanding to improve convergence speed; when a node approaches an obstacle, Larger, increasing the direction towards random target nodes The extended component is used to enhance obstacle avoidance capabilities. Simultaneously, the target extension step size is constrained to ensure that the step size is within... Between these steps, it is important to avoid excessively long step sizes that could lead to collision detection failure, or excessively short step sizes that could result in slow growth. For the minimum step size, This represents the maximum step size.

[0115] S1303. Based on the convex model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; if there is no obstacle threat, treat the candidate new node as a new node and add it to the random tree.

[0116] The above determination of whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree specifically includes:

[0117] Based on the convex model, an obstacle threat region is determined; based on the nodes in the random tree, the previous node of the candidate new node is determined; if the flight path between the previous node and the candidate new node is at least partially located within the obstacle threat region, then an obstacle threat is determined to exist on the path between the candidate new node and the parent node of the random tree. The flight path between the previous node and the candidate new node can be a line connecting the previous node and the candidate new node.

[0118] The aforementioned endpoint area is the area where the mission to be performed by the drone ends. This area can be a spherical area centered on the endpoint, with the radius of the spherical area being a preset value.

[0119] S140. Generate an initial trajectory using the nodes in the random tree, and smooth the initial trajectory to obtain the target planned trajectory.

[0120] The initial trajectory can be generated as follows: Instead of connecting nodes in the chronological order of their generation, a reverse backtracking strategy based on node hierarchical relationships is employed. Specifically, during the random tree expansion, whenever a new collision-free node is successfully generated using the dynamic gravity step size strategy and added to the random tree, the system simultaneously records the nearest expansion source node as the new node's parent node, thus establishing a unique topological dependency relationship between nodes. The trajectory search ends when a node in the random tree successfully extends to the preset endpoint region. Subsequently, the algorithm starts from the node in the endpoint region, reads the current node's parent node index, and sequentially extracts the parent nodes in reverse order until it backtracks to the trajectory's starting node. Finally, by reversing the sequence of backtracked nodes, an initial collision-free flight trajectory connecting the starting point to the endpoint is obtained.

[0121] The specific implementation process for determining the obstacle threat zone and the safe flight zone based on the aforementioned convex body model includes the following steps:

[0122] Step 1: Introducing a safety expansion process for the UAV's physical dimensions and flight errors. In actual flight, to avoid collisions between the UAV's edges and obstacles, this disclosure not only considers the physical boundaries of the obstacles themselves but also treats the UAV as a rigid body with a certain volume, performing a safety expansion operation based on a convex body model. The equivalent physical radius of the UAV is set as... The sum of the system's maximum positioning error and the preset safety margin is For the fitted obstacle convex body model, its boundary is expanded outward along the outward normal direction by a distance. ,in Taking the standard hemispherical model as an example, the equivalent radius of the expanded threat region... Substituting the expanded parameters into a unified approximation expression yields the equation for the convex body model after safe expansion, thus transforming the "physical obstacle model" into an "equivalent threat model in flight configuration space (C-space)".

[0123] Step 2: Three-dimensional discretization and voxelization representation of the mission space. Due to the UAV's flight mission space (set) The task space is mathematically continuous. To facilitate rapid collision detection by the computer, this disclosure uses an octree data structure or a three-dimensional voxel grid to discretize the entire task space. The entire task space is divided into tiny cubes (i.e., voxels) with a preset resolution and side length. The resolution is dynamically adjusted according to the size of the UAV and the computing power of the onboard computer.

[0124] Step 3: Voxel State Determination Based on Convex Solid Equations. Traverse all voxel nodes within the task space (or the local space where the obstacle bounding box is located). Extract the center point coordinates of each voxel. Substitute this into the equation of the convex body after the safety expansion process in step 1: Let the decision function be... ; like Then, it is determined that the voxel node is located inside an obstacle or on an expanded surface, and its status is marked as "Occupied" or "High Threat". If Then, the voxel node is determined to be in an absolutely safe region, and its state is marked as "Free".

[0125] (x0, y0, z0) represents the geometric center coordinates of the convex surface model of the obstacle. 膨胀 This new threshold is calculated by adding the drone's radius and positioning error to the original size of the obstacle. Taking a simple sphere (r=1, k=1) as an example: the original physical boundary is determined by the obstacle's radius... The decision was made at this time. The expanded boundary radius became Then the corresponding inflation threshold becomes .

[0126] Step 4: Construct and output the obstacle threat region set. Gather all voxel nodes whose state is marked as "occupied" to form the final obstacle threat region set. This identifies the obstacle threat area. Collecting all voxel nodes marked as "Free" constitutes the flight safety area. During the subsequent execution of the integrated improved RRT algorithm, when the system needs to verify new nodes... When determining the safety of a generated flight path segment, instead of recalculating complex nonlinear equations each time, the voxel status at the corresponding coordinates can be directly queried in the octree or voxel map. If the query finds a match... The voxels in the data, i.e., those that indicate a collision threat, render the node expansion invalid, thus greatly improving the collision detection speed in 3D trajectory planning.

[0127] The above-mentioned smoothing process for the initial trajectory to obtain the target planned trajectory specifically includes:

[0128] Iterate through the nodes in the initial path (i.e., the nodes in the random tree) and perform the following operations: determine whether there is an obstacle threat in the connection between the current node and the next next node; if there is no threat, discard the next node of the current node and directly connect the current node and the next next node; if there is a threat, keep the next node of the current node, or take the midpoint between the current node and the next next node as the new current node, and return to the step of determining whether there is an obstacle threat in the connection between the current node and the next next node.

[0129] To determine whether an obstacle threat exists, we can determine whether the line connecting two nodes is at least partially located within the obstacle threat area. Specifically, if it is at least partially located within the obstacle threat area, then an obstacle threat exists; otherwise, there is no obstacle threat.

[0130] Step S130 above can be considered an improved algorithm based on the RRT algorithm. This disclosure uses a deep learning network to detect and segment obstacles, and constructs a convex model of the obstacles. During trajectory planning, a comprehensive improved RRT algorithm is adopted: a greedy heuristic strategy is introduced to reduce the randomness of node search through probability guidance; a dynamic gravity step size strategy is introduced to dynamically adjust the expansion step size and direction based on the distance between nodes and obstacles, balancing search speed and obstacle avoidance safety; finally, a smoothness optimization method is used to process the planned trajectory. Compared with existing technologies, this disclosure overcomes the problems of slow convergence speed, tortuous paths, and low search efficiency in complex environments of traditional RRT algorithms, achieving fast, smooth, and safe autonomous obstacle avoidance flight for UAVs in complex three-dimensional environments.

[0131] Based on the same inventive concept, this disclosure provides a drone obstacle avoidance and trajectory planning device. The steps performed by the components of this device are the same as or similar to those described above, therefore, similar details will not be repeated. Figure 2 As shown, the drone obstacle avoidance and trajectory planning device in this embodiment includes:

[0132] The point cloud preliminary processing module 210 is used to acquire point cloud data of the flight environment of the UAV and extract obstacle point clouds from the point cloud data.

[0133] The region segmentation module 220 is used to construct a convex surface model of the obstacle based on the obstacle point cloud, and to determine the flight safety area based on the convex surface model.

[0134] The random tree generation module 230 is used to iteratively execute the following steps within the flight safety area to generate a random tree until a newly generated node is located in the endpoint region: using a greedy heuristic strategy, a random target point is determined based on a preset probability threshold; based on the distance information between the node and obstacles, a first gravitational component in the endpoint direction and a second gravitational component in the random target point direction are determined; based on the first and second gravitational components, a first target expansion step size in the endpoint direction and a second target expansion step size in the random target point direction are determined; combining the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size, a candidate new node is determined; based on the convex body model and the nodes in the random tree, it is determined whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; if there is no obstacle threat, the candidate new node is taken as a new node and added to the random tree.

[0135] The trajectory generation module 240 is used to generate an initial trajectory using the nodes in the random tree and to smooth the initial trajectory to obtain the target planned trajectory.

[0136] The various embodiments of the techniques described above can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include: implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0137] The program code used to implement the methods of this disclosure may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing apparatus, such that when executed by the processor or controller, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0138] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0139] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0140] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as a data server), or computing systems that include middleware components (e.g., an application server), or computing systems that include frontend components (e.g., a user computer with a graphical user interface or web browser through which a user can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., a communication network). Examples of communication networks include local area networks (LANs), wide area networks (WANs), and the Internet.

[0141] Computer systems can include clients and servers. Clients and servers are generally located far apart and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. Servers can be cloud servers, servers in distributed systems, or servers incorporating blockchain technology.

[0142] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this disclosure can be achieved, and this is not limited herein.

[0143] The specific embodiments described above do not constitute a limitation on the scope of protection of this disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A method for obstacle avoidance and trajectory planning of unmanned aerial vehicles (UAVs), characterized in that, include: Acquire point cloud data of the flight environment of the drone, and extract obstacle point cloud from the point cloud data; Based on the obstacle point cloud, a convex surface model of the obstacle is constructed, and the flight safety zone is determined based on the convex surface model. Within the flight safety area, the following steps are performed iteratively to generate a random tree until a newly generated node is located in the endpoint region: A greedy heuristic strategy is adopted to determine random target points based on a preset probability threshold; Based on the distance information between the node closest to the random target point in the random tree and the obstacle, the first gravitational component of the destination direction and the second gravitational component of the random target point direction are determined; Based on the first gravitational component and the second gravitational component, determine the first target expansion step size towards the endpoint and the second target expansion step size towards the random target point; combine the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size to determine candidate new nodes; Based on the convex solid model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; In the absence of any obstacle threat, the candidate new node is adopted as a new node and added to the random tree; An initial trajectory is generated using the nodes in the random tree, and the initial trajectory is smoothed to obtain the target planned trajectory.

2. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, The method of employing a greedy heuristic strategy to determine random target points based on a preset probability threshold includes: Set a preset probability threshold; Generate a random number in the interval [0, 1]. If the random number is less than the preset probability threshold, the random target point is set as the endpoint; If the random number is greater than or equal to the preset probability threshold, a point is randomly selected within the task space as the random target point; wherein, the task space includes the obstacle threat area occupied by obstacles and the flight safety area.

3. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, The step of determining candidate new nodes by combining the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size includes: Candidate new nodes are determined using the following formula: ； in, As a candidate new node, The node in the random tree that is closest to the random target point; The step size for expanding towards the random target point. The step size is the extension step size towards the endpoint; For the second gravitational component, 1- This is the first gravitational component; To extend the step size towards the second target in the direction of the random target point; To extend the step size towards the first target in the direction of the endpoint.

4. The UAV obstacle avoidance and trajectory planning method according to claim 3, characterized in that, The second gravitational component Calculate using the following formula: ； In the formula, Let M be the coordinates of the center of the i-th obstacle, and M be the number of obstacles. Where is the radius of the obstacle. , where (x, y, z) is the adjustment factor, and (x, y, z) are the coordinates of the node in the random tree closest to the random target point.

5. The UAV obstacle avoidance and trajectory planning method according to claim 4, characterized in that, The expansion step size of the first target all satisfy the following constraints: If the first target expansion step size is less than the minimum step size, then the first target expansion step size is set to the minimum step size; If the first target expansion step size is greater than the maximum step size, then the first target expansion step size is set to the maximum step size; The expansion step size of the second objective satisfies the following constraints: If the second target expansion step size is less than the minimum step size, then the second target expansion step size is set to the minimum step size; If the second target expansion step size is greater than the maximum step size, then the second target expansion step size is set to the maximum step size.

6. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, The construction of a convex surface model of the obstacle based on the obstacle point cloud includes: Using the obstacle point cloud, the obstacle is fitted as at least one convex surface model among a hemisphere, cylinder, cone, or frustum; The approximate expression of the convex solid model is as follows: ； in, Let be any point on the convex model. The coordinates of the center of the convex solid model are: These are preset parameters.

7. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, The step of determining whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree based on the convex model and the nodes in the random tree includes: Based on the convex body model, the obstacle threat area is determined; The previous node of the candidate new node is determined based on the nodes in the random tree; If the flight path between the previous node and the candidate new node is at least partially located within the obstacle threat area, then it is determined that there is an obstacle threat on the path between the candidate new node and the parent node of the random tree.

8. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, The process of smoothing the initial trajectory to obtain the target planned trajectory includes: Iterate through the nodes in the initial track and perform the following operations: Determine if there are any obstacles or threats in the connection between the current node and the node two after next; If there is no threat, discard the next node of the current node and directly connect the current node to the node after that. If a threat exists, retain the next node of the current node, or take the midpoint between the current node and the node after that as the new current node, and return to the step of determining whether there is an obstacle threat in the connection between the current node and the node after that.

9. The UAV obstacle avoidance and trajectory planning method according to claim 1, characterized in that, Extracting obstacle point clouds from the point cloud data includes: The point cloud data is resampled using the farthest point sampling method, and the center point of the point cloud is determined based on the sampling results. The point cloud data is grouped according to the center point of the point cloud to obtain multiple local neighborhoods; The mini PointNet network is used to extract features from the neighborhoods to obtain local feature vectors representing the overall shape of each neighborhood. Using an encoder-decoder structure, based on the extracted local feature vectors, the ground, vegetation, buildings, and power lines contained in the point cloud of the grouped local neighborhood are classified and segmented.

10. A device for obstacle avoidance and trajectory planning of unmanned aerial vehicles (UAVs), characterized in that, include: The point cloud preliminary processing module is used to acquire point cloud data of the flight environment of the UAV and extract obstacle point clouds from the point cloud data; The region segmentation module is used to construct a convex surface model of the obstacle based on the obstacle point cloud, and to determine the flight safety area based on the convex surface model. The random tree generation module is used to iteratively perform the following steps within the flight safety area to generate a random tree until a newly generated node is located in the endpoint region: A greedy heuristic strategy is adopted to determine random target points based on a preset probability threshold; Based on the distance information between the node closest to the random target point in the random tree and the obstacle, the first gravitational component of the destination direction and the second gravitational component of the random target point direction are determined; Based on the first gravitational component and the second gravitational component, determine the first target expansion step size towards the endpoint and the second target expansion step size towards the random target point; combine the node closest to the random target point in the random tree, the first target expansion step size, and the second target expansion step size to determine candidate new nodes; Based on the convex solid model and the nodes in the random tree, determine whether there is an obstacle threat on the path between the candidate new node and the parent node of the random tree; In the absence of any obstacle threat, the candidate new node is adopted as a new node and added to the random tree; The trajectory generation module is used to generate an initial trajectory using the nodes in the random tree, and to smooth the initial trajectory to obtain the target planned trajectory.

Images