Three-dimensional path planning method and system for safe flight path of unmanned aerial vehicle

Abstract

The invention provides a three-dimensional path planning method and system for a safe track of an unmanned aerial vehicle, and relates to the technical field of intelligent control, and the method comprises the steps: generating a new expansion node coordinate containing wind field compensation through combining a sampling node direction vector, a current adaptive step length and a wind drift compensation amount; the method comprises the following steps: performing three-dimensional spherical collision inspection on a new expansion node according to an unmanned aerial vehicle envelope radius; if yes, performing voxelization ray collision detection on a line segment from the nearest node to the new extension node to obtain a collision detection result; and periodically counting collision detection results, dynamically adjusting the step length by adopting a PID optimization algorithm based on a historical collision rate, inputting the collision rate into a PID controller, outputting a step length adjustment coefficient, multiplying the current step length by the adjustment coefficient to obtain a new step length, and constraining the new step length in a preset proportion interval of the heuristic initial step length. According to the invention, intelligent path planning can be realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent control, in particular to a three-dimensional path planning method and system for safe flight path of unmanned aerial vehicle. BACKGROUND

[0002] In recent years, unmanned aerial vehicles (UAVs) are increasingly widely used in complex scenarios such as logistics inspection and disaster rescue, and their autonomous flight relies on efficient and reliable three-dimensional path planning algorithms. As the mainstream planning algorithm based on random sampling, RRT (Rapidly-exploring Random Tree) and its optimized version RRT* are integrated into open motion planning library (OMPL) and other open source frameworks due to their advantages of probabilistic completeness and no need for prior modeling of the environment, and are widely deployed in academic and industrial fields. However, the existing public RRT* implementation has the following inherent defects: For example, the traditional RRT* uses a constant step size for node expansion, which reduces search efficiency due to frequent collisions in obstacle-dense areas, and leads to slow convergence due to too small step size in open areas. Especially in three-dimensional narrow channels (such as urban canyons and power line corridors), the number of iterations of the fixed step size strategy increases by an average of more than 40%.

[0003] For example, the goal sampling probability (goal bias) is usually set empirically, which is too high to cause the tree structure to converge to a local minimum too early, and too low to slow down the target approach speed. In three-dimensional non-convex terrain, inappropriate bias can increase the path length by an average of 22%. SUMMARY

[0004] The technical problem to be solved by the present application is to provide a three-dimensional path planning method and system for safe flight path of unmanned aerial vehicle, which reduces the number of iterations of path search.

[0005] To solve the above technical problems, the technical solutions of the present application are as follows: In a first aspect, a three-dimensional path planning method for safe flight path of unmanned aerial vehicle, comprising: Step 1: generating a sampling node direction vector pointing to a target point according to a preset target bias probability in a three-dimensional space; Step 2: calculating a three-dimensional wind field vector according to real-time wind speed and azimuth angle, and generating a wind drift compensation amount based on current step size and ground speed of the unmanned aerial vehicle to calculate single-step flight time; Step 3: generating new expansion node coordinates containing wind field compensation by combining the sampling node direction vector of step 1, the current adaptive step size, and the wind drift compensation amount of step 2; Step 4, first perform a three-dimensional spherical collision detection on the new extended node with the envelope radius of the UAV; if passed, perform voxelized ray collision detection on the line segment from the nearest node to the new extended node to obtain the collision detection result; Step 5, periodically count the collision detection result of step 4, dynamically adjust the step length using a PID optimization algorithm based on historical collision rate, input the collision rate into the PID controller, output the step length adjustment coefficient, multiply the current step length by the adjustment coefficient to obtain a new step length, and constrain the new step length within a preset proportion interval of the heuristic initial step length and input it into step 3; Step 6, repeat steps 1 to 5 until the new extended node enters the target neighborhood, and backtrack the nodes to construct the final flight path.

[0006] The second aspect is a three-dimensional path planning system for safe flight of a UAV, comprising: A generation module for generating a sampling node direction vector pointing to a target point according to a preset target bias probability in a three-dimensional space; A calculation module for calculating a three-dimensional wind field vector according to real-time wind speed and azimuth, and generating a wind drift compensation amount based on the current step length and ground speed of the UAV; A fusion module for generating a new extended node coordinate containing wind field compensation by combining the sampling node direction vector, the current adaptive step length, and the wind drift compensation amount; A detection module for first performing a three-dimensional spherical collision detection on the new extended node with the envelope radius of the UAV; if passed, performing voxelized ray collision detection on the line segment from the nearest node to the new extended node to obtain the collision detection result; A constraint module for periodically counting the collision detection result of step 4, dynamically adjusting the step length using a PID optimization algorithm based on historical collision rate, inputting the collision rate into the PID controller, outputting the step length adjustment coefficient, multiplying the current step length by the adjustment coefficient to obtain a new step length, and constraining the new step length within a preset proportion interval of the heuristic initial step length; A processing module for determining that the new extended node enters the target neighborhood, and backtracking the nodes to construct the final flight path.

[0007] The above-mentioned scheme of the present application at least includes the following beneficial effects: Through the synergistic effect of dynamic target bias sampling and PID step length adjustment, the number of iterations of path search is significantly reduced; the lightweight wind field compensation model and the efficient collision detection mechanism cooperate to reduce the algorithm calculation load.

[0008] The "point-line" double-layer collision detection mechanism effectively solves the path penetration risk of traditional single-point detection, and the wind field vector compensation model fundamentally suppresses the trajectory deviation caused by crosswind, ensuring the path executability.

[0009] The coupling of dynamic step size and target bias significantly shortens the average and worst-case path length, and substantially improves the path passing rate in narrow channels and non-convex obstacle scenarios. The PID step size adjustment dynamically responds to environmental mutations based on historical collision rates, avoiding step size oscillation, and the heuristic step size constraint mechanism ensures high convergence success rate in extreme scenarios with sparse / dense obstacle distribution. BRIEF DESCRIPTION OF DRAWINGS

[0010] Figure 1 FIG. 1 is a flow diagram of a three-dimensional path planning method for a safe flight path of a UAV according to an embodiment of the present application.

[0011] Figure 2 FIG. 2 is a schematic diagram of a three-dimensional path planning system for a safe flight path of a UAV according to an embodiment of the present application. DETAILED DESCRIPTION

[0012] Exemplary embodiments of the present disclosure will be described in greater detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the drawings, it is understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure can be more thoroughly understood and the scope of the present disclosure can be accurately conveyed to those skilled in the art.

[0013] As shown in Figure 1 FIG. 1, an embodiment of the present application proposes a three-dimensional path planning method for a safe flight path of a UAV, which includes: Step 1: generating a sampling node direction vector pointing to a target point according to a preset target bias probability in a three-dimensional space; Step 2: calculating a three-dimensional wind field vector according to a real-time wind speed and azimuth angle, and generating a wind drift compensation amount based on a current step size and ground speed of the UAV to calculate a single-step flight time; Step 3: generating new extended node coordinates containing wind field compensation in combination with the sampling node direction vector of Step 1, the current adaptive step size, and the wind drift compensation amount of Step 2; Step 4: first performing a three-dimensional spherical collision detection on the new extended node with a UAV envelope radius; if passed, performing a voxelized ray collision detection on a line segment from the nearest node to the new extended node to obtain a collision detection result; Step 5: periodically counting the collision detection result of Step 4, dynamically adjusting the step size using a PID optimization algorithm based on historical collision rates, inputting the collision rate into a PID controller to output a step size adjustment coefficient, multiplying the current step size by the adjustment coefficient to obtain a new step size, and constraining the new step size within a preset proportion interval of the heuristic initial step size and inputting it into Step 3; Step 6: repeating Steps 1 to 5 until the new extended node enters the target neighborhood, and backtracking the nodes to construct a final flight path.

[0014] In the embodiments of the present application, by calculating the three-dimensional wind field vector in real time, the wind drift compensation amount is generated, effectively offsetting the interference of wind speed and direction on the flight trajectory, ensuring that the unmanned aerial vehicle can still fly stably along the planned path under complex weather conditions, and avoiding target deviation. At the same time, the preset target bias probability is used to generate a sampling direction vector to guide the search process to preferentially expand towards the target area, reduce invalid sampling, and greatly improve the path planning efficiency, so that the unmanned aerial vehicle can find a feasible path more quickly. The multi-level collision detection mechanism builds a strong defense line for the safe flight of the unmanned aerial vehicle. First, a three-dimensional spherical collision test is performed on the envelope radius of the unmanned aerial vehicle to quickly filter out obviously dangerous nodes and reduce the calculation complexity; then, a voxelized ray collision detection is performed on the path segment for fine detection, which accurately identifies complex obstacles and avoids collision detection. The PID optimization algorithm based on the historical collision rate dynamically adjusts the step size, automatically reduces the step size in the dense obstacle area to improve the path resolution, and increases the step size in the open area to improve the planning efficiency, balancing the calculation efficiency while ensuring safety. The closed-loop optimization mechanism formed by the PID adjustment based on the historical collision rate enables the algorithm to automatically adjust the parameters according to the environmental changes, avoids falling into local optimization, and enhances the autonomous exploration ability in unknown complex environments. The whole process is based on three-dimensional space modeling, which can effectively handle terrain undulations and vertical obstacles, meet the needs of complex three-dimensional scenes such as urban three-dimensional traffic and forest inspection, and expand the coverage range of unmanned aerial vehicle tasks.

[0015] In a preferred embodiment of the present application, step 1, generating a sampling node direction vector pointing to the target point in the three-dimensional space according to the preset target bias probability, comprises: Step 11, generating a random number uniformly distributed in the interval [0, 1], if the random number is less than the preset target bias probability, setting the final target point coordinates as the current sampling point; otherwise, randomly generating a free sampling point coordinate outside the three-dimensional space obstacle range; Step 12, calculating the three-dimensional space vector from the nearest node in the current path tree to the sampling point obtained in step 11 to obtain the original direction vector; Step 13, performing a length normalization calculation on the original direction vector generated in step 12 to output a unit direction vector.

[0016] In the embodiments of the present application, the above steps can be implemented by the following steps: The above step 11 randomly generates a value in the interval [0, 1], which is like the random result when flipping a coin. Then, the random number is compared with the pre-set target bias probability. If the random number is less than the preset probability, the position of the final target point is directly determined as the current sampling point; if the random number is greater than the preset probability, a point in the three-dimensional space without obstacles is randomly determined as the sampling point.

[0017] In this embodiment of the present invention, this approach cleverly balances the pursuit of the goal and exploration of the global environment during path planning. By presetting probabilities to determine whether to sample directly toward the goal, the algorithm can more efficiently find a path to the goal. Random sampling outside obstacles ensures that other possible paths are not missed, avoiding the dilemma of falling into a local optimal solution.

[0018] In step 12 above, find the node closest to the sampling point determined in step 11 in the constructed path tree. Then, use this closest node as the starting point and the sampling point as the endpoint to determine a line segment with direction and length in 3D space. The vector corresponding to this line segment is the original direction vector, which indicates the direction and approximate distance from the current path tree node to the sampling point.

[0019] In this embodiment of the present invention, by determining the directional relationship between the path tree nodes and the sampling points, a clear guide is provided for subsequent path expansion. This allows the drone to know which direction to fly, reduces blind exploration, and makes path planning more directional and purposeful.

[0020] In step 13 above, based on the original direction vector obtained in step 12, its length (modulus) is processed. Regardless of the original length of the vector, it is scaled to a vector with a length of 1. This ensures that the vector only retains the direction information while removing the original length effect, ultimately resulting in a unit direction vector.

[0021] In an embodiment of the present invention, the direction vector is standardized so that it can be more conveniently and accurately combined with data such as the step size and the wind drift compensation amount when the coordinates of the new extended node are subsequently calculated.

[0022] In a preferred embodiment of the present invention, step 2, calculating the three-dimensional wind field vector based on the real-time wind speed and azimuth, and estimating the single-step flight time based on the current step length and ground speed of the UAV to generate the wind drift compensation, includes: Step 21: Calculate the single-step flight time based on the movement direction corresponding to the unit direction vector and the current ground speed of the UAV. Step 22: convert the scalar wind speed value and azimuth angle input by the meteorological sensor into a wind field vector in a three-dimensional rectangular coordinate system by decomposing the horizontal wind field component and compensating the vertical wind field component; Step 23: Perform vector multiplication on the single-step flight time and the wind field vector to generate a three-dimensional wind drift compensation amount.

[0023] In the embodiment of the present invention, the above steps can be implemented by the following steps: The step 21 determines the direction of the UAV movement indicated by the unit direction vector, which represents the direction of the UAV's next flight. Combined with the current ground speed of the UAV, which represents the actual speed of the UAV in the current state, the time required for the UAV to fly one step along the unit direction vector can be calculated by dividing the distance to be flown (i.e. the current step length) by the ground speed.

[0024] The present application accurately calculates the single-step flight time, providing a time reference for subsequent calculations considering the impact of wind on the UAV. Only by knowing the time length of each step of the UAV flight, the distance that the wind will blow the UAV away from the original route in this time can be calculated more accurately, thereby effectively improving the accuracy of path planning in the time dimension.

[0025] The step 22 of the above-mentioned meteorological sensor returns the scalar wind speed value and azimuth angle in real time. The scalar wind speed is first decomposed into two perpendicular components in the horizontal direction according to the indication of the azimuth angle, so that the force of the wind in different directions in the horizontal plane can be known. At the same time, considering that the wind may also have an impact on the UAV in the vertical direction, the wind field component in the vertical direction is determined through certain compensation calculation. Finally, the horizontal and vertical wind field components are integrated to form a complete wind field vector in the three-dimensional rectangular coordinate system, clearly presenting the direction and intensity of the wind in space.

[0026] The present application converts the wind speed and azimuth angle into a three-dimensional wind field vector, fully capturing the force of the wind in all directions in space on the UAV. This enables the path planning to fully consider the actual impact of the wind in complex environments, whether it is a horizontal crosswind or a vertical upward or downward airflow, which enhances the adaptability of the UAV in dynamic wind field environments.

[0027] The step 23 of the above-mentioned step 21 calculates the single-step flight time, and the three-dimensional wind field vector obtained in step 22 is operated. The vector multiplication operation here is to multiply the single-step flight time by each component of the wind field vector to obtain the displacement amount of the UAV caused by the wind in three directions in three-dimensional space within this flight time. Combining these displacement amounts, a three-dimensional wind drift compensation amount is obtained.

[0028] The present application quantifies the impact of the wind on the UAV as specific displacement compensation data by calculating the wind drift compensation amount. In subsequent path planning, the flight route of the UAV can be adjusted according to this compensation amount to offset the interference of the wind, ensuring the stable flight of the UAV along the planned path, greatly improving the safety and reliability of the path planning in dynamic wind fields.

[0029] In a preferred embodiment of the present invention, step 3 combines the sampling node direction vector of step 1, the current adaptive step size, and the wind drift compensation amount of step 2 to generate new extended node coordinates including wind field compensation, including: Step 31, multiplying the unit direction vector output in step 13 by the current adaptive step size to generate a basic displacement vector under no wind conditions; Step 32, performing a three-dimensional vector addition operation on the wind drift compensation amount generated in step 23 and the basic displacement vector in step 31 to obtain a composite displacement vector; Step 33 , taking the coordinates of the nearest node in the current tree as a reference, superimposes the synthetic displacement vector obtained in step 32 , and outputs the final three-dimensional coordinates of the new expanded node.

[0030] In the embodiment of the present invention, the above steps can be implemented by the following steps: In step 31 above, first extract the unit direction vector obtained in step 13. This vector represents only the direction and has a fixed length of 1. Combined with the current adaptive step size, which represents the distance the drone moves at each step, each directional component of the unit direction vector is multiplied by the step size to obtain a new vector. This new vector represents the displacement of the drone for one step along the unit direction vector, ideally without wind interference. This is also known as the base displacement vector.

[0031] In an embodiment of the present invention, determining the basic displacement vector under no-wind conditions provides a reference benchmark for subsequent comprehensive consideration of the impact of the wind field. It clearly defines the original movement trend of the drone, allowing path planning to further superimpose the changes brought about by the wind field on the basic movement direction, making the path calculation more organized and accurate.

[0032] In step 32, the wind drift compensation calculated in step 23 is combined with the base displacement vector obtained in step 31. In three-dimensional space, the wind drift compensation components in each of the three dimensions are added to the corresponding components of the base displacement vector to obtain three new component values. This new vector, composed of these three new components, is the composite displacement vector that combines the effects of wind with the drone's original motion.

[0033] In this embodiment of the present invention, the wind's interference with the drone's motion is integrated with the drone's own motion trends through vector addition. This allows path planning to fully account for the potential wind impacts during actual flight, preventing the drone from deviating from the planned path due to ignoring the wind field, and enhancing the reliability of path planning in dynamic environments.

[0034] The step 33 finds the nearest node to the new node in the current constructed path tree, and obtains the three-dimensional coordinates of the nearest node. Then, the components of the resultant displacement vector obtained in the step 32 are added to the corresponding dimensions of the coordinates of the nearest node, respectively. For example, the X component of the resultant displacement vector is added to the X value of the coordinates of the nearest node, and the like. Finally, the new coordinates obtained are the three-dimensional coordinates of the new extended node containing the wind field compensation.

[0035] In the embodiment of the application, the new extended node coordinates are generated based on the current path tree node, so that the new node can be reasonably integrated into the planned path, and a complete flight path is gradually constructed. Meanwhile, the path planning fully reflects the comprehensive influence of the wind field and the motion of the unmanned aerial vehicle by superimposing the resultant displacement vector, so that a safer path that is more suitable for the actual flight environment of the unmanned aerial vehicle is planned.

[0036] In a preferred embodiment of the application, in the step 4, first, the three-dimensional spherical collision detection is performed on the new extended node with the envelope radius of the unmanned aerial vehicle; if the detection passes, the voxelized ray collision detection is performed on the line segment from the nearest node to the new extended node to obtain the collision detection result, including: In the step 41, the new extended node coordinates output in the step 3 are taken as the center of a sphere, and the envelope radius of the unmanned aerial vehicle is taken as the radius of the sphere, so as to detect the geometric interference between the sphere and the three-dimensional obstacle model: If there is interference, the collision flag is output and the detection is terminated, and if there is no interference, the step 42 is executed. In the step 42, the coordinates of the nearest node in the current tree and the coordinates of the new extended node are obtained, and the line connecting the two points is discretized into an equal-interval path point sequence, and the interval is less than or equal to 1 / 2 of the envelope radius of the unmanned aerial vehicle. In the step 43, the path points generated in the step 42 are read in sequence, and the spatial relationship between the path points and the obstacles is detected point by point: If the distance between any point and the obstacle is less than or equal to the envelope radius of the unmanned aerial vehicle, the collision flag is output, and if all the points pass the detection, the success flag is output. In the step 44, the collision flag or the success flag in the step 41 or the step 43 is taken as the detection result.

[0037] In the embodiment of the application, the above steps can be implemented by the following steps: In the step 41, a three-dimensional sphere is constructed with the new extended node as the center and the envelope radius determined according to the size of the unmanned aerial vehicle as the radius, and it is checked whether the sphere overlaps or contacts the known three-dimensional obstacle model in space. If there is overlap or contact, it indicates that the new extended node is in a dangerous area, and is marked as collision, and the subsequent detection is not performed; if the sphere does not interfere with the obstacle, the next step is continued.

[0038] The application can quickly screen out obviously infeasible nodes through three-dimensional spherical collision detection, avoid further calculation of paths that will certainly collide, greatly reduce the calculation amount of subsequent complex detection, and improve the algorithm efficiency.

[0039] The step 42 determines the node closest to the newly expanded node in the current path tree, connects the two nodes to form a line segment, discretizes the line segment into a series of equidistant path points according to certain rules, and forms a dense path point sequence, wherein the distance between adjacent path points is not more than half of the UAV envelope radius.

[0040] The application discretizes the continuous path line segment into dense path points, provides enough sampling points for subsequent fine collision detection, ensures that narrow channels or small obstacles that may exist in the path can be detected, and improves the accuracy of collision detection.

[0041] The step 43 reads the discretely generated path points in sequence, calculates the distance between each path point and the nearest obstacle, compares the distance between each point and the obstacle with the UAV envelope radius, and if the distance between any path point and the obstacle is less than or equal to the envelope radius, it indicates that the path has a collision risk at this point, and is marked as collision; only when the distance between all path points and the obstacle is greater than the envelope radius, it is marked as successfully passing the detection.

[0042] The point-by-point detection ensures that each critical position on the path is checked for safety, can find obstacles that may be missed in spherical collision detection, especially for complex-shaped obstacles or narrow channels, provides more fine collision detection, and ensures the safety of the path.

[0043] The step 44 summarizes the detection results of the steps 41 and 43. If a collision is found in the step 41, the collision mark is directly output as the final result; if the step 41 passes the detection, the collision mark or the success mark is output as the final collision detection result according to the detection of all path points in the step 43.

[0044] The application integrates the results of the two collision detection methods to form a comprehensive collision detection conclusion, which utilizes the rapidity of spherical collision detection and the fineness of voxelized ray detection, improves the detection efficiency while ensuring the detection accuracy.

[0045] In a preferred embodiment of the application, step 5, periodically statistics the collision detection results of step 4, adopts a PID optimization algorithm based on historical collision rate to dynamically adjust the step size, inputs the collision rate into the PID controller, outputs the step size adjustment coefficient, multiplies the current step size by the adjustment coefficient to obtain a new step size, and inputs the new step size into step 3, including: Periodically read the collision detection result output by step 4, count the number of collisions and the number of successful expansions in the last N expansions, and calculate the current collision rate; Input the collision rate into a proportional-integral-derivative controller to generate a first adjustment component proportional to the difference between the current collision rate and the expected collision rate, a second adjustment component proportional to the cumulative value of the historical collision rate error, and a third adjustment component proportional to the current collision rate change rate; Output the first adjustment component, the second adjustment component, and the third adjustment component to the step length adjustment coefficient K; Get the current adaptive step length value, multiply the current step length by the adjustment coefficient K to obtain a preliminary updated step length; Constrain the preliminary updated step length to obtain a final step length, and input the final step length as the new current adaptive step length to step 3.

[0046] In the embodiments of the present application, the above steps can be implemented by the following steps: Set a statistical period, and periodically collect the collision detection result output by step 4. Count the number of successful obstacle avoidance and the number of collisions in the last N path expansion attempts. Divide the number of collisions by the total number of expansions to obtain the current collision rate.

[0047] The present application periodically counts the collision rate, which reflects the obstacle density and complexity in the path planning process in real time, so that the algorithm can be dynamically adjusted according to the environment changes.

[0048] The above step 52 compares the currently calculated collision rate with the pre-set expected collision rate to obtain the difference between the two. Based on this difference, a first adjustment component proportional to the difference is generated, which directly responds to the current error. At the same time, the cumulative error of the collision rate and the expected collision rate calculated in the history is generated, and a second adjustment component proportional to the cumulative error value is generated to eliminate the steady-state error of the system. In addition, the change rate of the current collision rate is calculated to generate a third adjustment component proportional to the change rate, which is used to predict the trend of the collision rate and adjust in advance.

[0049] The three adjustment components of the PID controller of the present application cooperate with each other, the proportional term quickly responds to the current error, the integral term eliminates the long-term deviation, and the differential term predicts the future trend, so that the step length adjustment is more accurate and stable, and can adapt to different complexity environments.

[0050] The above step 53 weights and combines the first adjustment component, the second adjustment component, and the third adjustment component generated above to obtain a comprehensive adjustment coefficient K. This coefficient reflects the amplitude and direction of adjusting the step length according to the current collision situation.

[0051] The application generates a comprehensive adjustment coefficient by reasonably combining three adjustment components, can make timely response to the current collision situation, can consider historical cumulative error and future trend, makes the step length adjustment more smooth and stable, and avoids excessive adjustment or insufficient adjustment.

[0052] The step 54 obtains the current adaptive step length value, multiplies the step length value with the adjustment coefficient K to obtain a preliminary updated step length, and the preliminary updated step length reflects the new step length size adjusted according to the collision rate.

[0053] The application directly associates the collision rate with the step length, and realizes the dynamic adjustment of the step length through multiplication operation. When the collision rate is high, the adjustment coefficient K is less than 1, the step length is reduced, and the fineness of path planning is increased. When the collision rate is low, the adjustment coefficient K is greater than 1, the step length is increased, and the efficiency of path planning is improved, and the adaptive adjustment of the step length is realized.

[0054] The step 55 performs constraint processing on the preliminary updated step length, ensures that the preliminary updated step length is in a certain proportion interval of the pre-set heuristic initial step length, limits the preliminary updated step length to the boundary value of the interval if the preliminary updated step length exceeds the interval, keeps the preliminary updated step length unchanged if the preliminary updated step length is in the interval, and obtains a final step length value. The final step length is used as a new current adaptive step length and is fed back to the step 3 for subsequent path expansion calculation.

[0055] The application avoids the situation that the step length is too large or too small in the step length adjustment process by performing constraint processing on the step length, and ensures that the step length changes in a reasonable range. This prevents the situation that the step length is too large in a complex environment, which increases the collision risk, and avoids the situation that the step length is too small in a simple environment, which reduces the calculation efficiency, and balances the safety and efficiency of path planning.

[0056] The step 6 repeatedly performs the steps 1 to 5 until a new expansion node enters a target neighborhood, and backtracks the nodes to construct a final flight path, including: The steps 1 to 5 are repeatedly performed in a loop, a new expansion node is generated in each iteration through target bias sampling, wind field compensation, collision detection and step length adjustment, and it is judged whether the node enters a pre-set target neighborhood (i.e. the distance from the target point is less than a specific threshold). When the new node meets the condition of entering the target neighborhood, the loop is terminated, the path tree is backtracked from the target neighborhood node, the complete node sequence from the starting point to the target point is extracted by reverse traversal according to the node connection relationship, and finally a three-dimensional flight path containing wind field compensation and obstacle avoidance information is constructed.

[0057] The present invention realizes autonomous exploration of the entire process from the starting point to the target point through cyclic iteration, without human intervention, and adapts to the real-time path planning needs in dynamic environments; the backtracking mechanism ensures the continuity of the path from the starting point to the target point, and combines target bias sampling and adaptive step size adjustment to shorten the path length as much as possible while ensuring safety, thereby improving flight efficiency; termination judgment based on the target neighborhood prevents the algorithm from falling into an infinite loop, and allows the drone to automatically complete the path construction when approaching the target.

[0058] like Figure 2 As shown in the figure, the three-dimensional path planning system for the safe trajectory of the UAV includes: A generation module is used to generate a sampling node direction vector pointing to the target point according to a preset target bias probability in three-dimensional space; The calculation module is used to calculate the three-dimensional wind field vector based on the real-time wind speed and azimuth, and to estimate the single-step flight time based on the current step length and ground speed of the UAV to generate the wind drift compensation; The fusion module is used to combine the sampling node direction vector, the current adaptive step size and the wind drift compensation to generate the new extended node coordinates including wind field compensation; The detection module is used to first perform a three-dimensional spherical collision test on the newly expanded node using the drone's envelope radius. If the collision passes, a voxelized ray collision test is then performed on the line segment from the nearest node to the newly expanded node to obtain a collision test result. The constraint module is used to periodically collect statistics on the collision detection results of step 4, dynamically adjust the step size using a PID optimization algorithm based on the historical collision rate, input the collision rate into the PID controller, output the step size adjustment coefficient, multiply the current step size by the adjustment coefficient to obtain a new step size, and constrain the new step size to be within a preset proportional range of the heuristic initial step size; The processing module is used to determine the new expansion node entering the target neighborhood and backtrack the nodes to construct the final flight path.

[0059] The above descriptions are merely some preferred embodiments of the present disclosure and illustrate the underlying technical principles. Those skilled in the art should understand that the scope of the invention encompassed by the embodiments of the present disclosure is not limited to technical solutions formed by specific combinations of the aforementioned technical features. It also encompasses other technical solutions formed by any combination of the aforementioned technical features or their equivalents, without departing from the aforementioned inventive concept. For example, a technical solution formed by replacing the aforementioned features with (but not limited to) technical features with similar functions disclosed in the embodiments of the present disclosure.

Claims

1. A three-dimensional path planning method for a safe UAV trajectory, characterized by: The method comprises: Step 1: Generate a sampling node direction vector pointing to the target point in three-dimensional space according to the preset target bias probability; Step 2: Calculate the three-dimensional wind field vector based on the real-time wind speed and azimuth, and calculate the single-step flight time based on the current step length and ground speed of the UAV to generate the wind drift compensation; Step 3: Combine the sampling node direction vector of step 1, the current adaptive step size and the wind drift compensation amount of step 2 to generate new extended node coordinates including wind field compensation; Step 4: First, perform a 3D spherical collision check on the newly expanded node using the drone's envelope radius. If the collision passes, perform a voxelized ray collision check on the line segment from the nearest node to the newly expanded node to obtain the collision test result. Step 5: Periodically collect statistics on the collision detection results of step 4, dynamically adjust the step size using a PID optimization algorithm based on the historical collision rate, input the collision rate into the PID controller, output the step size adjustment coefficient, multiply the current step size by the adjustment coefficient to obtain a new step size, constrain the new step size to be within a preset proportional range of the heuristic initial step size, and input it into step 3; In step 6, steps 1 to 5 are repeated until the newly expanded node enters the target neighborhood, and the nodes are backtracked to construct the final flight path.

2. The three-dimensional path planning method for a safe trajectory of an unmanned aerial vehicle according to claim 1, characterized in that: Step 1: Generate a sampling node direction vector pointing to the target point in three-dimensional space according to the preset target bias probability, including: Step 11: Generate a random number uniformly distributed in the interval [0, 1]. If the random number is less than the preset target bias probability, set the final target point coordinates as the current sampling point; otherwise, randomly generate free sampling point coordinates outside the obstacle range in the three-dimensional space. Step 12, calculating the three-dimensional space vector from the nearest node in the current path tree to the sampling point obtained in step 11, and obtaining the original direction vector; Step 13: Perform modulus normalization calculation on the original direction vector generated in step 12 to output a unit direction vector.

3. The three-dimensional path planning method for a safe trajectory of an unmanned aerial vehicle according to claim 2, characterized in that: Step 2: Calculate the three-dimensional wind field vector based on the real-time wind speed and azimuth, and calculate the single-step flight time based on the current step length and ground speed of the drone to generate the wind drift compensation, including: Step 21: Calculate the single-step flight time based on the movement direction corresponding to the unit direction vector and the current ground speed of the UAV. Step 22: convert the scalar wind speed value and azimuth angle input by the meteorological sensor into a wind field vector in a three-dimensional rectangular coordinate system by decomposing the horizontal wind field component and compensating the vertical wind field component; Step 23: Perform vector multiplication on the single-step flight time and the wind field vector to generate a three-dimensional wind drift compensation amount.

4. The three-dimensional path planning method for a safe trajectory of an unmanned aerial vehicle according to claim 3, characterized in that: Step 3: Combine the sampling node direction vector from step 1, the current adaptive step size, and the wind drift compensation from step 2 to generate new extended node coordinates including wind field compensation, including: Step 31, multiplying the unit direction vector output in step 13 by the current adaptive step size to generate a basic displacement vector under no wind conditions; Step 32, performing a three-dimensional vector addition operation on the wind drift compensation amount generated in step 23 and the basic displacement vector in step 31 to obtain a composite displacement vector; Step 33 , taking the coordinates of the nearest node in the current tree as a reference, superimposes the synthetic displacement vector obtained in step 32 , and outputs the final three-dimensional coordinates of the new expanded node.

5. The three-dimensional path planning method for a safe trajectory of an unmanned aerial vehicle according to claim 4, characterized in that: Step 4: First, perform a three-dimensional spherical collision test on the newly expanded node using the drone envelope radius; If it passes, voxelized ray collision detection is performed on the line segment from the nearest node to the newly expanded node to obtain the collision detection results, including: In step 41, the coordinates of the newly extended node output in step 3 are used as the center of the sphere and the envelope radius of the drone is used as the radius of the sphere to detect the geometric interference between the sphere and the three-dimensional obstacle model: If there is interference, a collision mark is output and the detection is terminated. If there is no interference, step 42 is executed; Step 42: Get the coordinates of the nearest node and the newly expanded node in the current tree, and discretize the line connecting the two points into a sequence of equally spaced path points, with a spacing of ≤ 1 / 2 of the drone envelope radius; Step 43, sequentially read the path points generated in step 42, and detect the spatial relationship between the point and the obstacle point by point: If the distance between any point and the obstacle is less than or equal to the drone envelope radius, a collision mark is output; if all points pass the test, a success mark is output; Step 44: Use the collision mark or success mark of step 41 or step 43 as the detection result.

6. The three-dimensional path planning method for a safe trajectory of an unmanned aerial vehicle according to claim 5, characterized in that: Step 5: Periodically collect statistics on the collision detection results of step 4, dynamically adjust the step size using a PID optimization algorithm based on the historical collision rate, input the collision rate into the PID controller, output the step size adjustment coefficient, multiply the current step size by the adjustment coefficient to obtain a new step size, constrain the new step size to be within a preset proportional range of the heuristic initial step size, and input it into step 3, including: Periodically read the collision detection results output in step 4, count the number of collisions and successful extensions in the last N expansions, and calculate the current collision rate; Inputting the collision rate into a proportional-integral-derivative controller, generating a first adjustment component proportional to the difference between the current collision rate and the expected collision rate, generating a second adjustment component proportional to the accumulated value of the historical collision rate error, and generating a third adjustment component proportional to the rate of change of the current collision rate; Output the step adjustment coefficient K of the first adjustment component, the second adjustment component and the third adjustment component; Get the current adaptive step size value, multiply the current step size by the adjustment coefficient K to obtain the initial update step size; The initial update step size constraint is processed to obtain the final step size, which is used as the new current adaptive step size and input into step 3.

7. A three-dimensional path planning system for safe UAV flight paths, characterized by: The system is used to perform the method according to any one of claims 1 to 6, comprising: A generation module is used to generate a sampling node direction vector pointing to the target point according to a preset target bias probability in three-dimensional space; The calculation module is used to calculate the three-dimensional wind field vector based on the real-time wind speed and azimuth, and to estimate the single-step flight time based on the current step length and ground speed of the UAV to generate the wind drift compensation; The fusion module is used to combine the sampling node direction vector, the current adaptive step size and the wind drift compensation to generate the new extended node coordinates including wind field compensation; The detection module is used to first perform a three-dimensional spherical collision test on the newly expanded node using the drone's envelope radius. If the collision passes, a voxelized ray collision test is then performed on the line segment from the nearest node to the newly expanded node to obtain a collision detection result. The constraint module is used to periodically collect statistics on the collision detection results of step 4, dynamically adjust the step size using a PID optimization algorithm based on the historical collision rate, input the collision rate into the PID controller, output the step size adjustment coefficient, multiply the current step size by the adjustment coefficient to obtain a new step size, and constrain the new step size to be within a preset proportional range of the heuristic initial step size; The processing module is used to determine the new expansion node entering the target neighborhood and backtrack the nodes to construct the final flight path.

Images