A method, device, equipment and storage medium for dynamic path planning of unmanned aerial vehicle

Abstract

The present application provides a method, apparatus, device and storage medium for dynamic path planning of a UAV. The method comprises: constructing a global environment, including attribute information of the UAV and obstacles and the starting and target points of the path. The first and second random trees are generated by the starting point and the target point, and the two-way expansion and intersection form a global optimal path. Key points are extracted to generate multiple local paths and detect dynamic obstacles. If there are no obstacles, the global path is followed; if there are obstacles, a speed sampling range is generated according to the UAV speed, acceleration and deceleration constraints and the obstacle distance, and a predicted trajectory group is generated in combination with the kinematic parameters. The trajectory with the smallest difference in direction between the end of the trajectory and the reference point is selected as the optimal trajectory and added to the global path. This method improves the flight safety, path planning robustness, obstacle avoidance and rapid response capabilities of the UAV through bidirectional expansion of random trees and local optimization.

Description

Technical Field

[0001] The present application relates to the field of drone control, and in particular to a method, device, equipment and storage medium for dynamic path planning of a drone. Background Art

[0002] With the rapid development of drone technology, UAVs have been widely used in a variety of fields, including military reconnaissance, logistics distribution, and environmental monitoring. During their missions, UAVs often face complex and changing environments, such as static and dynamic obstacles. Traditional path planning methods, such as the Dijkstra algorithm, are primarily designed for static environments and struggle to effectively address the challenges posed by dynamic obstacles. While path planning methods based on randomized rapid exploration trees (RRTs) have garnered attention for their efficiency in handling static obstacles, their real-time performance and obstacle avoidance capabilities in dynamic environments still need improvement. Summary of the Invention

[0003] The purpose of this application is to overcome the above-mentioned existing technologies and provide a method, device, equipment and storage medium for dynamic path planning of unmanned aerial vehicles.

[0004] This application provides a method for dynamic path planning of a UAV, including:

[0005] Constructing a global environment based on the drone attribute information and the obstacle attribute information, wherein the global environment includes setting the starting point and the target point of the drone's dynamic path;

[0006] Taking the starting point and the target point as root nodes, respectively generating a first random tree and a second random tree; bidirectionally extending and intersecting the first random tree and the second random tree to form a global optimal path;

[0007] Extracting key points from the global optimal path, and generating multiple local paths from the global optimal path using the key points as reference points;

[0008] detecting whether there is a dynamic obstacle within each of the local paths;

[0009] If there is no dynamic obstacle, flying according to the global optimal path;

[0010] If there is an obstacle, the speed sampling range is generated based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and the distance to the obstacle.

[0011] generating a predicted trajectory group according to the speed sampling range and the kinematic parameters of the UAV;

[0012] The trajectory with the smallest difference between the direction of the trajectory end in the predicted trajectory group and the direction of the reference point is taken as the optimal trajectory;

[0013] The optimal trajectory is added to the global optimal path to generate a dynamic path for the UAV.

[0014] Optionally, the global environment includes: the motion of the drone and dynamic obstacles;

[0015] The UAV motion is expressed as:

[0016]

[0017] Where Δt is the time interval, θ represents the yaw angle, (x, y) is the position coordinate, v is the velocity, and w is the angular velocity;

[0018] Dynamic obstacles are represented as:

[0019]

[0020] Where c is the obstacle velocity constant.

[0021] Optionally, the first random tree and the second random tree are bidirectionally extended and intersected to form a global optimal path, including:

[0022] The first random tree starts from the starting point and uses a node closest to the second random tree as a node expansion direction for expansion;

[0023] The second random tree starts from the target point and uses a node closest to the first random tree as a node expansion direction for expansion;

[0024] When the nodes of two trees intersect, all nodes are synthesized to form a global optimal path.

[0025] Optionally, when the nodes of the two trees intersect, synthesizing all nodes to form a global optimal path includes:

[0026] Use smoothing or redundant node removal to reduce the inflection points of the path.

[0027] Optionally, extract key points in the global optimal path, including:

[0028] For the first random tree and the second random tree, starting from the path starting point and the target point respectively, the angle between the two vectors represented by three consecutive nodes is determined in sequence;

[0029] If the angle is zero, the middle node of three consecutive nodes is deleted from the node set;

[0030] If the angle is not zero, solve the line segment equation and calculate the shortest distance between the line segment and the obstacle within the range;

[0031] If the shortest distance is greater than the preset safety threshold, the next node is detected;

[0032] If the shortest distance is less than or equal to a preset safety threshold, the current node is set as a key point, and the next node is detected until all nodes of the first random tree and the second random tree are detected.

[0033] Optionally, detecting whether there is a dynamic obstacle in each of the local paths includes:

[0034] This is achieved by using sensors to identify dynamic obstacles in real time and update environmental information.

[0035] Optionally, before adding the optimal trajectory to the global optimal path, the following steps are further included:

[0036] Determine the danger zone based on the distance between the drone and the obstacle, and adopt different obstacle avoidance strategies according to the type of danger zone;

[0037] The types of dangerous areas include: safe areas, controllable dangerous areas and extremely dangerous areas.

[0038] The present application also provides a UAV dynamic path planning device, comprising:

[0039] A modeling module, which processes and constructs a global environment based on the drone attribute information and obstacle attribute information, wherein the global environment includes setting the starting point and target point of the drone's dynamic path;

[0040] A random tree module, processing the starting point and the target point as root nodes, and generating a first random tree and a second random tree respectively;

[0041] a path module, processing the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path;

[0042] A local module, processing and extracting key points in the global optimal path, and generating multiple local paths in the global optimal path with the key points as reference points;

[0043] A detection module, processing detection of whether there is a dynamic obstacle in each local path;

[0044] An operation module, processing the flight according to the global optimal path if there is no dynamic obstacle;

[0045] The sampling module generates a speed sampling range based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and the distance to the obstacle if there is an obstacle.

[0046] A trajectory module, processing the speed sampling range and the UAV kinematic parameters to generate a predicted trajectory group;

[0047] a screening module for processing a trajectory having the smallest difference between the direction of the trajectory end and the direction of the reference point in the predicted trajectory group as an optimal trajectory;

[0048] An adding module processes adding the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

[0049] Optionally, the global environment includes: the motion of the drone and dynamic obstacles;

[0050] The UAV motion is expressed as:

[0051]

[0052] Where Δt is the time interval, θ represents the yaw angle, (x, y) is the position coordinate, v is the velocity, and w is the angular velocity;

[0053] Dynamic obstacles are represented as:

[0054]

[0055] Where c is the obstacle velocity constant.

[0056] Optionally, the path module processes the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path, including:

[0057] The first random tree starts from the starting point and uses a node closest to the second random tree as a node expansion direction for expansion;

[0058] The second random tree starts from the target point and uses a node closest to the first random tree as a node expansion direction for expansion;

[0059] When the nodes of two trees intersect, all nodes are synthesized to form a global optimal path.

[0060] Optionally, the path module processes the intersection of nodes of two trees and synthesizes all nodes to form a global optimal path, including:

[0061] Use smoothing or redundant node removal to reduce the inflection points of the path.

[0062] Optionally, the local module processes and extracts key points in the global optimal path, including:

[0063] For the first random tree and the second random tree, starting from the path starting point and the target point respectively, the angle between the two vectors represented by three consecutive nodes is determined in sequence;

[0064] If the angle is zero, the middle node of three consecutive nodes is deleted from the node set;

[0065] If the angle is not zero, solve the line segment equation and calculate the shortest distance between the line segment and the obstacle within the range;

[0066] If the shortest distance is greater than the preset safety threshold, the next node is detected;

[0067] If the shortest distance is less than or equal to a preset safety threshold, the current node is set as a key point, and the next node is detected until all nodes of the first random tree and the second random tree are detected.

[0068] Optionally, the detection module processes detecting whether there is a dynamic obstacle in each of the local paths, including:

[0069] This is achieved by using sensors to identify dynamic obstacles in real time and update environmental information.

[0070] Optionally, before adding the optimal trajectory to the global optimal path, the adding module further includes:

[0071] Determine the danger zone based on the distance between the drone and the obstacle, and adopt different obstacle avoidance strategies according to the type of danger zone;

[0072] The types of dangerous areas include: safe areas, controllable dangerous areas and extremely dangerous areas.

[0073] This application also provides a UAV dynamic path planning device, including:

[0074] Memory;

[0075] The processor is configured to retrieve from the memory a computer executable program for the above-mentioned method for planning a dynamic path for a drone, and execute the following steps: constructing a global environment based on drone attribute information and obstacle attribute information, the global environment including setting a starting point and a target point of the drone's dynamic path; generating a first random tree and a second random tree with the starting point and the target point as root nodes, respectively; bidirectionally extending and intersecting the first random tree and the second random tree to form a global optimal path; extracting key points from the global optimal path, and generating multiple local paths within the global optimal path with the key points as reference points; detecting whether there are dynamic obstacles within each local path; flying according to the global optimal path if there are no dynamic obstacles; if there are obstacles, generating a speed sampling range based on the drone's maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and distance to the obstacle; generating a predicted trajectory group based on the speed sampling range and the drone's kinematic parameters; determining the trajectory in the predicted trajectory group with the smallest difference between the direction of the trajectory end and the direction of the reference point as the optimal trajectory; and adding the optimal trajectory to the global optimal path to generate the drone's dynamic path.

[0076] The present application also provides a storage medium, comprising: a computer executable program stored therein, wherein the computer executable program is used to be called by a processor to execute the steps of the above-mentioned method for dynamic path planning of a drone.

[0077] The beneficial effects of this application are:

[0078] The present application provides a method for planning a dynamic path for an unmanned aerial vehicle (UAV), comprising: constructing a global environment based on UAV attribute information and obstacle attribute information, the global environment including setting a starting point and a target point of a dynamic path for the UAV; generating a first random tree and a second random tree with the starting point and the target point as root nodes, respectively; bidirectionally extending and intersecting the first random tree and the second random tree to form a global optimal path; extracting key points from the global optimal path, and generating multiple local paths within the global optimal path with the key points as reference points; detecting whether there are dynamic obstacles within each local path; flying along the global optimal path if there are no dynamic obstacles; if there are obstacles, generating a speed sampling range based on the UAV's maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and distance to the obstacle; generating a predicted trajectory group based on the speed sampling range and the UAV's kinematic parameters; determining the trajectory in the predicted trajectory group with the smallest difference between the direction of the trajectory end and the direction of the reference point as the optimal trajectory; and adding the optimal trajectory to the global optimal path to generate a dynamic path for the UAV. This application improves the flight safety, path planning robustness, obstacle avoidance performance and rapid response capability of drones through bidirectional closest point expansion and local optimization of random trees. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a schematic diagram of the UAV dynamic path planning process in this application;

[0080] Figure 2 This is a schematic diagram of dynamic path planning in this application;

[0081] Figure 3 This is a schematic diagram of the RRT-Connect algorithm node expansion process in this application;

[0082] Figure 4 This is a schematic diagram of the velocity sampling trajectory group of the DWA algorithm in this application;

[0083] Figure 5 This is a schematic diagram of the flight angle evaluation sub-function in this application;

[0084] Figure 6 This is a schematic diagram of double jeopardy range determination in this application;

[0085] Figure 7 This is a schematic diagram of the motion trajectory sampling of the drone within the prediction period in this application;

[0086] Figure 8 This is a schematic diagram of the traditional DWA algorithm in this application stopping when encountering an obstacle;

[0087] Figure 9 This is a schematic diagram of the path selected by the traditional DWA algorithm in this application to avoid obstacles;

[0088] Figure 10 This is a schematic diagram of the turning radius of the traditional DWA algorithm path in this application;

[0089] Figure 11 This is a flowchart of the integration of BGO-RRT-Connect and EA-DWA algorithms in this application. DETAILED DESCRIPTION

[0090] Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that various forms of implementation of the present disclosure are not limited to the embodiments set forth herein. Rather, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0091] Name explanation:

[0092] BGO-RRT-Connect algorithm: The BGO-RRT-Connect algorithm is based on the RRT-Connect algorithm and combines bidirectional tree search and node greedy expansion strategy.

[0093] EA-DWA algorithm: The EA-DWA algorithm is based on the DWA algorithm and improves the speed space and evaluation function.

[0094] This application proposes a UAV dynamic path planning based on the BGO-RRT-Connect algorithm and the EA-DWA algorithm, which is a key technology that enables UAVs to fly autonomously in complex and changing environments while avoiding static and dynamic obstacles.

[0095] Please refer to Figure 1 and Figure 11 As shown, the steps of the UAV dynamic path planning method include:

[0096] S101: Construct a global environment based on the drone attribute information and the obstacle attribute information, wherein the global environment includes setting a starting point and a target point of a dynamic path of the drone.

[0097] The global environment is used to reflect the drone's motion characteristics and obstacle attribute information. This obstacle attribute information includes static obstacle attribute information and dynamic obstacle attribute information. Static obstacles, because their position and volume are fixed and their parameters do not change, can be pre-set or acquired. Therefore, they can be directly modeled based on their attribute information and will not be further described here. This application focuses primarily on dynamic obstacle modeling.

[0098] The UAV motion characteristics include the linear velocity and angular velocity of the UAV. The dynamic obstacle information includes the dynamic obstacle's velocity v(t), acceleration a(t), position coordinates (x, y) in the Cartesian coordinate system, and the angle θ between the obstacle's moving velocity and the horizontal axis of the coordinate system.

[0099] Dynamic obstacle modeling:

[0100] In the drone operation area, the dynamic obstacle is moving. When the speed of the dynamic obstacle is a constant c and the acceleration is 0, then:

[0101]

[0102] The movement of the obstacle in the time interval Δt is:

[0103] p(t+Δt)=p(t)+v(t)×Δt

[0104] The expression converted to the coordinate system is:

[0105]

[0106] The previous formula is expressed in matrix form as:

[0107]

[0108] This formula represents the movement of obstacles in the working area.

[0109] Drone Modeling:

[0110] The drone is also moving, and the linear velocity v t and angular velocity w t The change of can reflect the movement of the drone, each group (v t ,w t ) represent a trajectory.

[0111] Assume that the UAV moves in a straight line at a uniform speed within the time interval Δt, then its motion differential equation is:

[0112]

[0113] According to the previous formula, the UAV motion can be obtained:

[0114]

[0115] Among them, x and y represent the coordinate position of the drone.

[0116] The global environment is constructed based on the drone, dynamic obstacles, and pre-acquired or set static obstacles. In the global environment, the drone's departure point and arrival point are set as the starting point and target point, respectively, to complete the construction of the global environment.

[0117] S102: Generate a first random tree and a second random tree respectively with the starting point and the target point as root nodes.

[0118] Please refer to Figure 2 As shown, the starting point P of the path s and the target point P g A random tree is generated for each root node, denoted as expanded tree T1 and expanded tree T2. When the nodes of the first and second random trees are expanded to intersect, synthesizing all the nodes can form a complete drone flight path. The simultaneous expansion of the two trees improves the search speed of the overall algorithm.

[0119] S103: The first random tree and the second random tree are bidirectionally extended and intersected to form a global optimal path.

[0120] like Figure 3As shown, a greedy node expansion strategy is used to expand the first and second random trees. During each iteration of generating new nodes, each random tree uses the closest node in the other tree as the node expansion direction, allowing the two trees to quickly intersect. If no obstacles are encountered during expansion in the specified direction, the algorithm will continue expanding in that direction. Otherwise, the algorithm will use randomly generated nodes for expansion to avoid falling into a local optimum.

[0121] The RRT-Connect algorithm is used to generate new nodes. The formula for generating new nodes by the RRT-Connect algorithm is as follows:

[0122]

[0123] Among them, P nearest It indicates that the expansion tree T1 takes the nearest node in the expansion tree T2 as the target point for expansion, and θ represents the node P near To node P nearest The angle between the connecting line and the horizontal line, u is the node expansion step, P new It is a node newly added to tree T1.

[0124] After the expansion of the first random tree and the second random tree, the two trees will eventually intersect. Since both trees will use the node closest to the other random tree as the expansion direction when expanding nodes, the path formed after the intersection is equivalent to the global optimal path.

[0125] S104: Extract key points in the global optimal path, and generate multiple local paths in the global optimal path using the key points as reference points.

[0126] The global path planned by two random trees can be regarded as two node sets bounded by the intersection point of the two random trees. The two node sets are {P i1 |i1=s,1,2,3,4,5,...,l} and {P i2 |i2=l,1,2,3,4,5,...,g}, Ps represents the starting point of the path, P g represents the target point of the path, P l Represents the connection node l=1, 2, 3, ..., n between two trees.

[0127] Taking the first random tree of the starting point as an example, from P s First, solve the vector consisting of three consecutive nodes and vector And determine the angle between the two vectors, which is divided into two cases:

[0128] (1) If the angle is zero, it means P s, if the three nodes P1 and P2 are on the same straight line, they are redundant collinear nodes, and the intermediate node P1 is deleted from the node set.

[0129] (2) If the angle is not zero, it means P s , the three nodes P1 and P2 are not on the same straight line, and a safety threshold d is introduced to solve the line segment equation L(P s P2), and calculate the shortest distance d between the line segment and the obstacle within the range s2 .

[0130] If d s2 >d, then continue to detect the next node P3;

[0131] If d s2 ≤d, then P s The line connecting P2 has entered the obstacle safety range, and P2 is added to the global key point set P c , then repeat the above steps with P2 as the new starting point until the connection node P l Until it is added to the global keypoint.

[0132] After the global path key point extraction of the first random tree is completed, the above process is repeated to continue the key point extraction of the second random tree until the target point P is g By adding the key point set, we can get the global key point set P that will be used in the EA-DWA algorithm. c .

[0133] The key points are the basis for generating the UAV trajectory for the local path.

[0134] Based on the key points, the global path is divided into multiple local paths, and the key points are used as reference points for optimizing the local paths.

[0135] It should be noted that the local path in the embodiment of the present application is a part of the global optimal path, and it completely overlaps with a certain part of the global optimal path.

[0136] In the above description, key points are actually the points where the drone's state changes along the optimal global path. Therefore, the period between key points, during which the drone's state remains constant, is called a motion cycle. The range of speed and angular velocity that the drone can achieve within each motion cycle is called the dynamic window.

[0137] S105: Detect whether there is a dynamic obstacle in each local path.

[0138] In order to ensure the effectiveness of system modeling, sensors for real-time identification of dynamic obstacles are added, and the dynamic obstacle information detected by the sensors is combined with the previously planned global path to formulate a dynamic local path planning strategy.

[0139] S106: If there is no dynamic obstacle, fly according to the global optimal path.

[0140] If the sensors detect no dynamic obstacles during the detection process, the drone will continue to fly along the globally optimal path. This globally optimal path is calculated before the flight mission begins using advanced path planning algorithms (such as the BGO-RRT-Connect algorithm). It takes into account multiple factors such as the drone's flight performance, environmental factors, and mission requirements, ensuring that the drone reaches its destination in the shortest time and safest way.

[0141] During this phase, the drone will fully utilize its flight control system and navigation system to maintain a stable flight attitude and speed, and move along the globally optimal path. At the same time, the system will continue to monitor the surrounding environment to respond to possible emergencies.

[0142] S107. If there is an obstacle, a speed sampling range is generated based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints of the UAV and the distance from the obstacle.

[0143] From the kinematics and trajectory simulation of the UAV, we can know that the attitude of the UAV is determined by the velocity vector (v x ,v y ,w) to infer the pose and trajectory at the sampling moment. The DWA algorithm collects the speed of the UAV and evaluates the priority of the trajectory to determine the trajectory.

[0144] When performing speed sampling, the sampling speed of the drone should be determined according to the constraints of its maximum speed and minimum speed, so the allowed speed set V a The formula is as follows:

[0145] V a ={(v,w)|v∈[0,v max ],w∈[0,w max ]}

[0146] Where v is the linear velocity and w is the angular velocity.

[0147] In order to ensure that the drone can stop before hitting an obstacle, the maximum speed allowed should be the distance Dis(v,w) between the current path point and the nearest obstacle, so the allowed speed set V s As shown in the following formula:

[0148]

[0149] Among them, v b is the linear acceleration of the drone, w b is the angular acceleration of the drone.

[0150] Considering the linear acceleration v of the drone b and angular acceleration w b There will be acceleration and deceleration constraints, v c and w c Represent the linear velocity and angular velocity at the current moment, Δt represents the time interval, then the allowed speed set V of the drone is d for:

[0151] V d ={(v,w)|v∈[v c -v b Δt,v c +v b Δt],w∈[w c -w b Δt,w c +w b Δt]}

[0152] In summary, the final dynamic window speed sampling range V of the UAV is f for:

[0153] V f =V a ∩V s ∩V d

[0154] S108: Generate a predicted trajectory group according to the speed sampling range and the kinematic parameters of the UAV.

[0155] The kinematic parameters of the drone include: initial yaw angle, initial linear velocity, initial angular velocity, maximum linear velocity, maximum angular velocity, linear acceleration, angular acceleration, linear velocity resolution, and angular velocity resolution.

[0156] For each sampling point, the drone's kinematics is used to predict its trajectory over the next period of time. The trajectory is a collection of possible positions and attitudes that the drone could reach at the current sampling speed.

[0157] The EA-DWA algorithm is obtained by improving the speed space and evaluation function of the DWA algorithm to improve the obstacle avoidance performance and environmental adaptability. Among them, the DWA dynamic window method is used to solve the problem of avoiding dynamic obstacles in path planning. The local path planning algorithm based on speed sampling is used to calculate the speed and angular velocity (v t ,w t) is sampled and the trajectory of the UAV within the expected period is calculated. The optimal trajectory is selected to enter the next motion cycle. By repeating this process, online real-time planning is achieved to avoid sudden dynamic obstacles.

[0158] like Figure 4 As shown in Figure 2, the DWA algorithm keeps the linear velocity and angular velocity unchanged within the trajectory prediction time interval Δt, and the trajectory group generated by the velocity set.

[0159] Specifically, the UAV's trajectory is predicted within a prediction period based on the motion equation and sampling speed. The prediction period is divided into multiple moments, and the interval between two adjacent moments is a motion period. Since the interval between the motion periods is short, the AUV's motion within each motion period can be simplified to linear motion, and the trajectory information is updated according to the following formula:

[0160]

[0161] Among them, v u represents the expected speed at the next moment, w u represents the expected angular velocity at the next moment, τ represents a time interval, and θ represents the yaw angle of the drone.

[0162] Through the above process, a series of predicted trajectories generated by different sampling speeds are obtained, which together constitute a trajectory group. Each trajectory in the trajectory group represents a possible motion path of the UAV at a specific speed and angular velocity.

[0163] In the EA-DWA algorithm, these trajectories are further evaluated to select the optimal trajectory as the next movement path of the drone.

[0164] The evaluation process mainly relies on an improved evaluation function that comprehensively considers multiple factors such as yaw angle, obstacle avoidance coefficient and speed to ensure that the drone can fly safely and efficiently in complex and dynamic environments.

[0165] like Figure 6 As shown in the figure, in order to improve the obstacle avoidance ability of the DWA algorithm in complex environments, especially when encountering fast-moving dynamic obstacles, in order to avoid collisions with such obstacles, a double danger range rule is proposed and different obstacle avoidance strategies are adopted according to the rule.

[0166] The distance between the drone and dynamic obstacles is divided into two categories: obstacles with a controllable dangerous distance r1 and obstacles with an extremely dangerous distance r2.

[0167] Among them, reasonable values ​​of the two distances can avoid obstacles to the greatest extent and shorten the path.

[0168] Determine the danger zone based on the distance d between the drone and the obstacle:

[0169] When r1<d, the obstacle is in a safe area and continues to move according to the original plan and speed.

[0170] When r2<d≤r1, the obstacle is located in the controllable dangerous area, and the UAV speed is reduced to 1 / 3 of the initial speed. After a period of time when the environmental information is refreshed, it is judged whether the distance between the UAV and the obstacle has met the safety area requirements. If so, the initial speed is restored. If not, it continues to wait for the environmental information to be refreshed.

[0171] When 0<d≤r2, the drone is located in an extremely dangerous area. The drone needs to stop moving immediately, replan the local path from the current location to the next sub-target, and set off along the new local path.

[0172] The traditional DWA algorithm only establishes a speed space based on the speed range that the drone can reach after the first flight cycle. The predicted trajectory at this time cannot fully reflect the drone's motion performance and there is a possibility of missing a practical and more optimal motion trajectory.

[0173] This application calculates the velocity space that the drone can reach after all motion cycles within the dynamic window, uses it for sampling and generating predicted trajectories, and the improved velocity space after this method is as follows:

[0174]

[0175] Among them, v p 、w p They are the linear velocity and angular velocity of the drone in the improved dynamic window, v0 and w0 are the initial linear velocity and angular velocity of the window, respectively. bmax 、w bmax are the maximum linear acceleration and maximum angular acceleration of the UAV, v max 、w max is the maximum linear velocity and maximum angular velocity of the UAV, and Δt is the prediction period.

[0176] After generating the velocity space, set the sampling frequency f s Sampling is performed, and the velocity state {v, w} of each sampling is used as the expected trajectory velocity.

[0177] like Figure 7 As shown, the green dotted line is the predicted trajectory. The number and range of trajectories that can be evaluated in each prediction cycle after the improved speed space are significantly better than those in the original speed space.

[0178] S109 , taking the trajectory with the smallest difference between the direction of the trajectory end in the predicted trajectory group and the direction of the reference point as the optimal trajectory.

[0179] Due to the performance limitations of the drone, some sampling speeds need to wait until several motion cycles to be achieved, so the speed state needs to be recalculated once in each motion cycle of each trajectory.

[0180] like Figure 8 As shown in the figure, when the traditional DWA algorithm performs path planning, if there is an obstacle directly in front of the current UAV's moving direction or on the global planned path, the obstacle avoidance path planned by the UAV will be very close to the obstacle, and the UAV will fly in front of the obstacle and stop or turn around and return.

[0181] According to the DWA algorithm's evaluation function, the drone will continue to move toward the obstacle until it is very close to it, which inevitably leads to a decrease in the drone's obstacle avoidance distance. As time increases, the collision detection reaction time also decreases, which is not conducive to the safety of the drone.

[0182] like Figure 9 As shown in the figure, if the drone passes through a road section with a compact obstacle area, the DWA algorithm will plan a path that circumvents the obstacle area rather than choosing a path that passes through it. As a result of this obstacle avoidance path planning, the drone will always reject routes with more obstacles in order to successfully avoid them. Choosing a longer path away from obstacles will result in a significant loss of energy consumption for the drone.

[0183] like Figure 10 As shown in the figure, when the planned obstacle avoidance path is surrounded by obstacles, even if the drone continues to fly in its current direction and passes safely, it will turn to the calculated optimal obstacle avoidance path, resulting in an increase in turning frequency and a smaller turning radius. Because the distance between the trajectory and surrounding obstacles is taken into account when evaluating the path, even if the trajectory is absolutely safe, under the same conditions, the distance score of the trajectory to the obstacle will be higher, which makes the traditional DWA algorithm tend to choose a path away from obstacles.

[0184] Since the evaluation criteria of the original evaluation function of the DWA algorithm are relatively independent, the optimal weights applicable in different environments vary greatly. Therefore, this application adds a variable angle evaluation sub-function to the evaluation function, as shown in the following formula:

[0185] Ang(v i ,w i )=180°-|θ gi -θ si |

[0186] Among them, θ gi and θ si are the end point direction of the i-th predicted trajectory and the posture direction of the robot at the starting point.

[0187] Considering that the dimensions and dimension units are inconsistent between the evaluation criteria, it is not easy to add this sub-function. Therefore, the original evaluation sub-function needs to be normalized before adding it to eliminate the dimensional influence between factors.

[0188] When selecting a new path, adding a change angle evaluation sub-function allows the drone to incorporate the change in its trajectory direction into the evaluation function. Changing the angle can simultaneously affect the direction and speed of the trajectory. By weighing the relationship between the original yaw angle, obstacle avoidance coefficient, and speed evaluation sub-functions, the improved dynamic window evaluation function of the DWA algorithm is obtained, which is expressed as follows:

[0189] After sampling the velocity space and obtaining several feasible trajectories, each trajectory needs to be evaluated and scored. The traditional DWA algorithm evaluation function is shown in the following formula:

[0190] C(v,w)=a1·Heading(v,w)+a2·Dis(v,w)+a3·Vel(v,w)+a4·Ang(v,w)such as Figure 5 As shown in the figure, Heading(v,w) is the yaw angle evaluation sub-function, which is used to evaluate the angular difference between the direction of the drone and the target point when it reaches the end of the simulated trajectory at a specified sampling speed. The evaluation method is 180°-θ, that is, the smaller θ is, the higher the evaluation. Dis(v,w) is the obstacle avoidance coefficient evaluation sub-function, which represents the distance between the current path point and the nearest obstacle. Vel(v,w) is the speed evaluation sub-function, which is used to evaluate the speed value of the current trajectory. a1, a2, and a3 are the coefficients of each evaluation sub-function. The size of a1 affects the degree to which the drone's yaw angle prioritizes toward the target point, the size of a2 affects the degree to which the drone prioritizes avoiding obstacles, and the size of a3 affects the speed of the drone.

[0191] S110: Add the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

[0192] During the dynamic path planning process of a drone, when performing a mission, the drone may encounter complex dynamic environments, including static and dynamic obstacles. In order to effectively plan a safe and efficient flight path, it is necessary to combine global path planning with local dynamic obstacle avoidance.

[0193] First, a global path planning algorithm (such as the BGO-RRT-Connect algorithm) is used to plan a globally optimal path from the starting point to the end point. This path minimizes flight distance, reduces energy consumption, and avoids collisions with known static obstacles, while taking into account the drone's performance and flight constraints. Then, as the drone flies along the global path, a local dynamic obstacle avoidance algorithm (such as the EA-DWA algorithm) is used to detect and avoid sudden dynamic obstacles in real time. This algorithm predicts the drone's future trajectory and adjusts its flight parameters (such as speed and direction) based on the position and velocity of dynamic obstacles to ensure that the drone can safely circumvent them.

[0194] During local dynamic obstacle avoidance, the EA-DWA algorithm generates multiple possible flight paths and evaluates their performance based on a pre-defined evaluation function. This evaluation function typically considers factors such as yaw angle, obstacle avoidance coefficient, and speed to determine the optimal flight path.

[0195] Finally, this optimal trajectory is added to the global optimal path to form a new dynamic path for the drone that includes local obstacle avoidance information. This path retains the optimization properties of the global path while also possessing the real-time and flexibility of local obstacle avoidance.

[0196] The present application also provides a UAV dynamic path planning device, comprising:

[0197] A modeling module, which processes and constructs a global environment based on the drone attribute information and obstacle attribute information, wherein the global environment includes setting the starting point and target point of the drone's dynamic path;

[0198] A random tree module, processing the starting point and the target point as root nodes, and generating a first random tree and a second random tree respectively;

[0199] a path module, processing the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path;

[0200] A local module, processing and extracting key points in the global optimal path, and generating multiple local paths in the global optimal path with the key points as reference points;

[0201] A detection module, processing detection of whether there is a dynamic obstacle in each local path;

[0202] An operation module, processing the flight according to the global optimal path if there is no dynamic obstacle;

[0203] The sampling module generates a speed sampling range based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and the distance to the obstacle if there is an obstacle.

[0204] A trajectory module, processing the speed sampling range and the UAV kinematic parameters to generate a predicted trajectory group;

[0205] a screening module for processing a trajectory having the smallest difference between the direction of the trajectory end and the direction of the reference point in the predicted trajectory group as an optimal trajectory;

[0206] An adding module processes adding the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

[0207] Furthermore, the global environment includes: the UAV's motion and dynamic obstacles;

[0208] The UAV motion is expressed as:

[0209]

[0210] Where Δt is the time interval, θ represents the yaw angle, (x, y) is the position coordinate, v is the velocity, and w is the angular velocity;

[0211] Dynamic obstacles are represented as:

[0212]

[0213] Where c is the obstacle velocity constant.

[0214] Furthermore, the path module processes the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path, including:

[0215] The first random tree starts from the starting point and uses a node closest to the second random tree as a node expansion direction for expansion;

[0216] The second random tree starts from the target point and uses a node closest to the first random tree as a node expansion direction for expansion;

[0217] When the nodes of two trees intersect, all nodes are synthesized to form a global optimal path.

[0218] Furthermore, the path module processes the intersection of the nodes of the two trees and synthesizes all the nodes to form a global optimal path, including:

[0219] Use smoothing or redundant node removal to reduce the inflection points of the path.

[0220] Furthermore, the local module processes and extracts key points in the global optimal path, including:

[0221] For the first random tree and the second random tree, starting from the path starting point and the target point respectively, the angle between the two vectors represented by three consecutive nodes is determined in sequence;

[0222] If the angle is zero, the middle node of three consecutive nodes is deleted from the node set;

[0223] If the angle is not zero, solve the line segment equation and calculate the shortest distance between the line segment and the obstacle within the range;

[0224] If the shortest distance is greater than the preset safety threshold, the next node is detected;

[0225] If the shortest distance is less than or equal to a preset safety threshold, the current node is set as a key point, and the next node is detected until all nodes of the first random tree and the second random tree are detected.

[0226] Furthermore, the detection module processes detecting whether there is a dynamic obstacle in each of the local paths, including:

[0227] This is achieved by using sensors to identify dynamic obstacles in real time and update environmental information.

[0228] Furthermore, before adding the optimal trajectory to the global optimal path, the adding module further includes:

[0229] Determine the danger zone based on the distance between the drone and the obstacle, and adopt different obstacle avoidance strategies according to the type of danger zone;

[0230] The types of dangerous areas include: safe areas, controllable dangerous areas and extremely dangerous areas.

[0231] This application also provides a UAV dynamic path planning device, including:

[0232] Memory;

[0233] The processor is configured to retrieve from the memory a computer executable program for the above-mentioned method for planning a dynamic path for a drone, and execute the following steps: constructing a global environment based on drone attribute information and obstacle attribute information, the global environment including setting a starting point and a target point of the drone's dynamic path; generating a first random tree and a second random tree with the starting point and the target point as root nodes, respectively; bidirectionally extending and intersecting the first random tree and the second random tree to form a global optimal path; extracting key points from the global optimal path, and generating multiple local paths within the global optimal path with the key points as reference points; detecting whether there are dynamic obstacles within each local path; flying according to the global optimal path if there are no dynamic obstacles; if there are obstacles, generating a speed sampling range based on the drone's maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and distance to the obstacle; generating a predicted trajectory group based on the speed sampling range and the drone's kinematic parameters; determining the trajectory in the predicted trajectory group with the smallest difference between the direction of the trajectory end and the direction of the reference point as the optimal trajectory; and adding the optimal trajectory to the global optimal path to generate the drone's dynamic path.

[0234] The present application also provides a storage medium, comprising: a computer executable program stored therein, wherein the computer executable program is used to be called by a processor to execute the steps of the above-mentioned method for dynamic path planning of a drone.

[0235] The above is only a preferred embodiment of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A method for dynamic path planning of an unmanned aerial vehicle, characterized in that: include: Constructing a global environment based on the drone attribute information and the obstacle attribute information, wherein the global environment includes setting the starting point and the target point of the drone's dynamic path; Taking the starting point and the target point as root nodes, respectively generate a first random tree and a second random tree; The first random tree and the second random tree are bidirectionally extended and intersected to form a global optimal path; Extracting key points from the global optimal path includes: starting from the path starting point and the target point, respectively, for the first random tree and the second random tree, sequentially determining the angle between two vectors represented by three consecutive nodes; if the angle is zero, deleting the middle node between the three consecutive nodes from the node set; if the angle is not zero, solving a line segment equation and calculating the closest distance between the line segment and an obstacle within the range; if the closest distance is greater than a preset safety threshold, detecting the next node; if the closest distance is less than or equal to the preset safety threshold, setting the current node as a key point and detecting the next node, until all nodes of the first random tree and the second random tree are detected; and generating multiple local paths from the global optimal path using the key points as reference points. detecting whether there is a dynamic obstacle within each of the local paths; If there is no dynamic obstacle, flying according to the global optimal path; If there are dynamic obstacles, the speed sampling range is generated based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and the distance to the obstacle. generating a predicted trajectory group according to the speed sampling range and the kinematic parameters of the UAV; The trajectory with the smallest difference between the direction of the trajectory end in the predicted trajectory group and the direction of the reference point is taken as the optimal trajectory; The optimal trajectory is added to the global optimal path to generate a dynamic path for the UAV.

2. The method for dynamic path planning of a UAV according to claim 1, characterized in that: The first random tree and the second random tree are bidirectionally extended and intersected to form a global optimal path, including: The first random tree starts from the starting point and uses a node closest to the second random tree as a node expansion direction for expansion; The second random tree starts from the target point and uses a node closest to the first random tree as a node expansion direction for expansion; When the nodes of two trees intersect, all nodes are synthesized to form a global optimal path.

3. The method for dynamic path planning of a UAV according to claim 2, characterized in that: When the nodes of the two trees intersect and all the nodes are synthesized to form a global optimal path, the method further includes: Use smoothing or redundant node removal to reduce the inflection points of the path.

4. The method for dynamic path planning of a UAV according to claim 1, wherein: Detecting whether there is a dynamic obstacle in each of the local paths includes: Use sensors to identify dynamic obstacles and update environmental information in real time.

5. The method for dynamic path planning of a UAV according to claim 1, wherein: Before adding the optimal trajectory to the global optimal path, the method further includes: Determine the danger zone based on the distance between the drone and the dynamic obstacle, and adopt different obstacle avoidance strategies according to the type of danger zone; The types of dangerous areas include: safe areas, controllable dangerous areas and extremely dangerous areas.

6. A dynamic path planning device for an unmanned aerial vehicle, characterized in that: include: A modeling module, which processes and constructs a global environment based on the drone attribute information and obstacle attribute information, wherein the global environment includes setting the starting point and target point of the drone's dynamic path; A random tree module, processing the starting point and the target point as root nodes, and generating a first random tree and a second random tree respectively; a path module, processing the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path; A local module processes and extracts key points from the global optimal path, including: starting from the path starting point and the target point, for the first random tree and the second random tree, respectively, sequentially determining the angle between two vectors represented by three consecutive nodes; if the angle is zero, deleting the middle node between the three consecutive nodes from the node set; if the angle is not zero, solving the line segment equation and calculating the closest distance between the line segment and an obstacle within the range; if the closest distance is greater than a preset safety threshold, detecting the next node; if the closest distance is less than or equal to the preset safety threshold, setting the current node as a key point and detecting the next node, until all nodes of the first random tree and the second random tree are detected; and generating multiple local paths in the global optimal path using the key points as reference points. A detection module, processing detection of whether there is a dynamic obstacle in each local path; An operation module, processing the flight according to the global optimal path if there is no dynamic obstacle; The sampling module generates a speed sampling range based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints, and the distance to the obstacle if there is an obstacle. A trajectory module, processing the speed sampling range and the UAV kinematic parameters to generate a predicted trajectory group; a screening module for processing a trajectory having the smallest difference between the direction of the trajectory end and the direction of the reference point in the predicted trajectory group as an optimal trajectory; An adding module processes adding the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

7. The UAV dynamic path planning device according to claim 6, characterized in that: The path module processes the first random tree and the second random tree to perform bidirectional extension intersection to form a global optimal path, including: The first random tree starts from the starting point and uses a node closest to the second random tree as a node expansion direction for expansion; The second random tree starts from the target point and uses a node closest to the first random tree as a node expansion direction for expansion; When the nodes of two trees intersect, all nodes are synthesized to form a global optimal path.

8. The UAV dynamic path planning device according to claim 7, characterized in that: The path module processes the intersection of the nodes of the two trees and synthesizes all the nodes to form a global optimal path, including: Use smoothing or redundant node removal to reduce the inflection points of the path.

9. The UAV dynamic path planning device according to claim 6, characterized in that: The detection module processes detecting whether there is a dynamic obstacle in each of the local paths, including: This is achieved by using sensors to identify dynamic obstacles in real time and update environmental information.

10. The UAV dynamic path planning device according to claim 6, characterized in that: Before adding the optimal trajectory to the global optimal path, the adding module further includes: Determine the danger zone based on the distance between the drone and the obstacle, and adopt different obstacle avoidance strategies according to the type of danger zone; The types of dangerous areas include: safe areas, controllable dangerous areas and extremely dangerous areas.

11. A dynamic path planning device for an unmanned aerial vehicle, characterized in that: include: Memory; A processor, configured to retrieve from the memory a computer executable program of the method for dynamic path planning of a drone according to any one of claims 1 to 5, and execute: constructing a global environment based on drone attribute information and obstacle attribute information, the global environment including setting a starting point and a target point of the dynamic path of the drone; and generating a first random tree and a second random tree with the starting point and the target point as root nodes, respectively; The first random tree and the second random tree are bidirectionally extended and intersected to form a global optimal path; Extracting key points in the global optimal path includes: starting from the path starting point and the target point of the first random tree and the second random tree, respectively, sequentially determining the angle between two vectors represented by three consecutive nodes; If the angle is zero, the middle node between three consecutive nodes is deleted from the node set; if the angle is not zero, the line segment equation is solved and the closest distance between the line segment and the obstacle within the range is calculated; if the closest distance is greater than a preset safety threshold, the next node is detected; if the closest distance is less than or equal to the preset safety threshold, the current node is set as a key point, and the next node is detected until all nodes of the first random tree and the second random tree are detected; multiple local paths in the global optimal path are generated using the key point as a reference point; dynamic obstacles are detected in each local path; if there are no dynamic obstacles, the global optimal path is flown; if there are obstacles, a speed sampling range is generated based on the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints of the UAV, and the distance to the obstacle; a predicted trajectory group is generated based on the speed sampling range and the kinematic parameters of the UAV; the trajectory with the smallest difference between the direction of the trajectory end in the predicted trajectory group and the direction of the reference point is determined as the optimal trajectory; and the optimal trajectory is added to the global optimal path to generate a dynamic path for the UAV.

12. A storage medium, characterized in that: include: A computer executable program is stored, which is used to be called by a processor to execute the steps of a dynamic path planning method for a drone as described in any one of claims 1 to 5.

Images