Unmanned aerial vehicle dynamic path planning method, device and equipment and storage medium

Abstract

The invention provides an unmanned aerial vehicle dynamic path planning method and device, equipment and a storage medium, and the method comprises the steps: constructing a global environment which comprises the attribute information of an unmanned aerial vehicle and an obstacle, and path starting and target points; a first random tree and a second random tree are generated through a starting point and a target point, and a global optimal path is formed through bidirectional expansion intersection. Key points are extracted to generate a plurality of local paths, and dynamic obstacles are detected. If no obstacle exists, running according to a global path; if yes, generating a speed sampling range according to the unmanned aerial vehicle speed, acceleration and deceleration constraints and the obstacle distance, and generating a prediction trajectory group in combination with kinematics parameters; and selecting the trajectory with the minimum difference value between the trajectory tail end and the reference point direction as the optimal trajectory, and adding the optimal trajectory into a global path. According to the method, through random tree bidirectional expansion and local optimization, the flight safety, path planning robustness, obstacle avoidance and quick response capability of the unmanned aerial vehicle are improved.

Description

Technical Field

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

[0002] With the rapid development of UAV technology, UAVs have been widely used in military reconnaissance, logistics distribution, environmental monitoring and other fields. UAVs often face complex and changing environments during missions, such as static obstacles and dynamic obstacles. Traditional path planning methods, such as the Dijkstra algorithm, are mainly designed for static environments and are difficult to effectively deal with the challenges brought by dynamic obstacles. In addition, the path planning method based on random rapid exploration tree (RRT) has attracted attention due to its high efficiency in dealing with static obstacles, but its real-time performance and obstacle avoidance capabilities in dynamic environments still need to be improved. Summary of the invention

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

[0004] The present application provides a method for dynamic path planning of a UAV, comprising:

[0005] Constructing 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;

[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 in the global optimal path, and generating multiple local paths in the global optimal path with the key points as reference points;

[0008] Detecting whether there is a dynamic obstacle in each of the local paths;

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

[0010] If there are obstacles, the speed sampling range is generated according to the maximum linear speed, maximum angular speed, minimum linear speed, minimum angular speed, acceleration and deceleration constraints and the distance from the obstacle.

[0011] Generate 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 end of the trajectory 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] Among them, Δ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 a starting point and expands using a node closest to the second random tree as a node expansion direction;

[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, all nodes are synthesized to form a global optimal path, including:

[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 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 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.

[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, processing and constructing a global environment according to 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;

[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 of the local paths;

[0044] An operation module, processing flying 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 from the obstacle if there is an obstacle.

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

[0047] A screening module, processing a 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;

[0048] An adding module is used to process and add 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] Among them, Δ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 a starting point and expands using a node closest to the second random tree as a node expansion direction;

[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 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 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.

[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] The present application also provides a UAV dynamic path planning device, comprising:

[0074] Memory;

[0075] A processor is used to retrieve a computer executable program of the above-mentioned unmanned aerial vehicle dynamic path planning method from the memory, and execute: constructing a global environment according to the unmanned aerial vehicle attribute information and the obstacle attribute information, the global environment including setting the starting point and the target point of the unmanned aerial vehicle dynamic path; 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; 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; detecting whether there are dynamic obstacles in each of the local paths; if there are no dynamic obstacles, flying according to the global optimal path; if there are obstacles, generating a speed sampling range according to the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints and the distance from the obstacle of the unmanned aerial vehicle; generating a predicted trajectory group according to the speed sampling range and the kinematic parameters of the unmanned aerial vehicle; 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; adding the optimal trajectory to the global optimal path to generate the unmanned aerial vehicle dynamic path.

[0076] The present application also provides a storage medium, including: 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 unmanned aerial vehicle dynamic path planning method.

[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, comprising: constructing a global environment according to unmanned aerial vehicle attribute information and obstacle attribute information, wherein the global environment comprises setting a starting point and a target point of a dynamic path for the unmanned aerial vehicle; taking the starting point and the target point as root nodes, respectively generating a first random tree and a second random tree; performing bidirectional extension and intersection of the first random tree and the second random tree to form a global optimal path; extracting key points in the global optimal path, and generating a plurality of local paths in the global optimal path with the key points as reference points; detecting whether there are dynamic obstacles in each of the local paths; if there are no dynamic obstacles, flying according to the global optimal path; if there are obstacles, generating a speed sampling range according to the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints and the distance from the obstacle of the unmanned aerial vehicle; generating a predicted trajectory group according to the speed sampling range and the kinematic parameters of the unmanned aerial vehicle; 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; adding the optimal trajectory to the global optimal path to generate a dynamic path for the unmanned aerial vehicle. This application improves the flight safety, path planning robustness, obstacle avoidance performance and rapid response capability of the UAV 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 It is a schematic diagram of dynamic path planning in this application;

[0081] Figure 3 It is a schematic diagram of the node expansion process of the RRT-Connect algorithm 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 It is a schematic diagram of the flight angle evaluation sub-function in this application;

[0084] Figure 6 It is a schematic diagram of double hazard 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] Fig. 9 This is a schematic diagram of the path selected by the traditional DWA algorithm in this application to stay away from obstacles;

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

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

[0090] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that various forms of implementing the present disclosure should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided in order 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 the UAV to fly autonomously in complex and changing environments while avoiding static and dynamic obstacles.

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

[0096] S101. Construct a global environment according to 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 motion characteristics and obstacle attribute information of the drone. The obstacle attribute information includes static obstacle attribute information and dynamic obstacle attribute information. Among them, static obstacles, because their position and volume are fixed, the parameters will not change, and can be pre-set or obtained, so they can be directly modeled according to their attribute information, which will not be repeated here. The focus of this application is mainly on dynamic obstacle modeling.

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

[0099] Dynamic Obstacle Modeling:

[0100] In the UAV 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 can be 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] Assuming that the UAV moves in a straight line at a uniform speed within the time interval Δt, its motion differential equation is:

[0112]

[0113] According to the above 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 above-mentioned drone, dynamic obstacles and pre-acquired or set static obstacles. In the global environment, the departure point and arrival point of the drone are set as the starting point and the target point respectively, and the construction of the global environment is completed.

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

[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, which is recorded as extended tree T1 and extended tree T2. When the nodes of the first random tree and the second random tree are extended to intersect, synthesizing all nodes can form a complete UAV operation path, and the search speed of the overall algorithm is improved by simultaneously extending the two trees.

[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, the node greedy expansion strategy is used to expand the first random tree and the second random tree. In each iteration of generating new nodes, each random tree uses the closest node of another tree as the node expansion direction, so that the two trees can intersect quickly. If no obstacles are encountered when expanding in the expansion direction, the algorithm will continue to expand nodes in this direction, otherwise the algorithm will use randomly generated nodes for expansion to avoid the algorithm falling into the dilemma of local optimality.

[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 represents the target point for expansion of the expansion tree T1 with the nearest node in the expansion tree T2 as the expansion direction, 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 the node newly added to the tree T1.

[0124] After the expansion of the first random tree and the second random tree, the two trees will eventually intersect. Since the two trees take 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 the two random trees can be regarded as two node sets bounded by the intersection of the two random trees. 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 connecting node l=1, 2, 3, ..., n of 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 can be 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 between P2 and P2 has entered the obstacle safety range, and P2 is added to the global key point set P c Then, P2 is used as the new starting point and the above steps are repeated 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 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 UAV trajectories for local paths.

[0134] Based on the key points, the global path is divided into a plurality of 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 part of the global optimal path.

[0136] In the above content, the key point is actually the state change point of the drone in the optimal global path. Therefore, between two key points, the drone state will not change, which is called a motion cycle. The speed and angular velocity range that the drone can reach in each motion cycle is called a dynamic window.

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

[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 along the global optimal path.

[0140] If the sensor does not detect any dynamic obstacles during the detection process, the drone will continue to fly along the global optimal path. The global optimal path is calculated by advanced path planning algorithms (such as the BGO-RRT-Connect algorithm) before the flight mission begins. It takes into account the flight performance of the drone, environmental factors, and mission requirements, ensuring that the drone can reach its destination in the shortest time and the safest path.

[0141] During this phase, the drone will make full use of its flight control system and navigation system to maintain a stable flight attitude and speed and move along the global 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 according to 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 drone, we can know that the attitude of the drone is determined by the velocity vector (v x ,v y ,w) to infer the pose and trajectory at the sampling time. 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 its own maximum speed and minimum speed constraints, 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] Among them, 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 allowed speed 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 below:

[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, respectively, Δt represents the time interval, then the allowed speed set V of the drone 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 drone 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 kinematics of the drone is used to predict the motion trajectory of the drone in the future. The motion trajectory is a set of possible positions and postures that the drone may reach at the current sampling speed.

[0157] The speed space and evaluation function of the DWA algorithm are improved to obtain the EA-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 calculates 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 is generated by the velocity set.

[0159] Specifically, the motion trajectory of the UAV in the prediction period is predicted based on the motion equation and the 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 motion of the AUV in 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 drone 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 takes into account multiple factors such as yaw angle, obstacle avoidance coefficient and speed to ensure that the UAV 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 collision 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 speed of the drone is reduced to 1 / 3 of the initial speed. After a period of time when the environmental information is refreshed, it is determined whether the distance between the drone and the obstacle meets 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, re-plan 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 the speed space according to the speed range that the UAV can reach after the first flight cycle. At this time, the predicted trajectory cannot fully reflect the motion performance of the UAV, and there is a possibility of losing the actual feasible and better motion trajectory.

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

[0174]

[0175] Among them, v p 、w p 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, v bmax 、w bmax are the maximum linear acceleration and maximum angular acceleration of the drone, 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 speed state {v, w} of each sampling is used as the expected trajectory speed.

[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 can only be achieved after several motion cycles, 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 to return.

[0181] According to the evaluation function of the DWA algorithm, the drone will keep moving towards the obstacle until it is very close to the obstacle, which will inevitably lead to a decrease in the obstacle avoidance distance of the drone. As time goes by, the reaction time of collision detection is also shortened, which is not conducive to the safety of the drone.

[0182] like Fig. 9 As shown in the figure, if the drone passes through a section of a compact obstacle area, the DWA algorithm will plan a path that bypasses the obstacle area instead of choosing a path that passes through the obstacle area. The result of this obstacle avoidance path planning is that in any case, the drone will reject the route with more obstacles in order to successfully complete the obstacle avoidance. Choosing a longer path away from obstacles will cause a large consumption of the drone's energy.

[0183] like Fig.10 As shown in the figure, when the planned obstacle avoidance path is surrounded by obstacles, even if the drone continues to fly in the current direction and passes safely, it will turn to the calculated optimal obstacle avoidance path, resulting in an increase in the turning frequency and a smaller path turning radius. Because the distance between the trajectory and the surrounding obstacles is taken into account when evaluating the path, even if the trajectory is absolutely safe, under the same conditions, the distance score between the trajectory and 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 change 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 They are respectively 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 the factors.

[0188] When a new path is selected, adding a change angle evaluation sub-function enables the drone to include the change in its forward direction in the trajectory into the evaluation function. Changing the angle can affect both 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 dynamic window evaluation function of the improved DWA algorithm is obtained, which is expressed as follows:

[0189] After sampling the velocity space to obtain 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, Heading(v,w) is the yaw angle evaluation sub-function, which is used to evaluate the angle difference between the direction of the drone and the target point when it reaches the end of the simulated trajectory at the specified sampling speed. The evaluation method is 180°-θ, that is, the smaller θ is, the higher the evaluation is. 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 is preferentially oriented toward the target point, the size of a2 affects the degree to which the drone preferentially avoids 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] In the process of UAV dynamic path planning, when the UAV is performing a mission, it may encounter complex dynamic environments, including static obstacles 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 global optimal path from the starting point to the end point. This path minimizes the flight distance, reduces energy consumption, and avoids collisions with known static obstacles while taking into account the performance and flight constraints of the drone. 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. The algorithm predicts the future trajectory of the drone and adjusts the flight parameters of the drone (such as speed, direction, etc.) according to the position and speed of the dynamic obstacles to ensure that the drone can safely bypass the obstacles.

[0194] During the local dynamic obstacle avoidance process, the EA-DWA algorithm generates multiple possible flight trajectories and evaluates the pros and cons of these trajectories based on a preset evaluation function. The evaluation function usually considers multiple factors such as yaw angle, obstacle avoidance coefficient, and speed to derive the optimal flight trajectory.

[0195] Finally, this optimal trajectory is added to the global optimal path to form a new UAV dynamic path that contains local obstacle avoidance information. This path not only retains the optimization characteristics of the global path, but also has 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, processing and constructing a global environment according to 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;

[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 of the local paths;

[0202] An operation module, processing flying 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 from the obstacle if there is an obstacle.

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

[0205] A screening module, processing a 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;

[0206] An adding module is used to process and add the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

[0207] Furthermore, the global environment includes: the movement of the drone and dynamic obstacles;

[0208] The UAV motion is expressed as:

[0209]

[0210] Among them, Δ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 a starting point and expands using a node closest to the second random tree as a node expansion direction;

[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 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 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.

[0226] Furthermore, the detection module processes the detection of 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 the adding module adds 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] The present application also provides a UAV dynamic path planning device, comprising:

[0232] Memory;

[0233] A processor is used to retrieve a computer executable program of the above-mentioned unmanned aerial vehicle dynamic path planning method from the memory, and execute: constructing a global environment according to the unmanned aerial vehicle attribute information and the obstacle attribute information, the global environment including setting the starting point and the target point of the unmanned aerial vehicle dynamic path; 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; 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; detecting whether there are dynamic obstacles in each of the local paths; if there are no dynamic obstacles, flying according to the global optimal path; if there are obstacles, generating a speed sampling range according to the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints and the distance from the obstacle of the unmanned aerial vehicle; generating a predicted trajectory group according to the speed sampling range and the kinematic parameters of the unmanned aerial vehicle; 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; adding the optimal trajectory to the global optimal path to generate the unmanned aerial vehicle dynamic path.

[0234] The present application also provides a storage medium, including: 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 unmanned aerial vehicle dynamic path planning method.

[0235] The above is only a preferred implementation 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 a starting point and a target point of a dynamic path of the drone; 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 in the global optimal path, and generating multiple local paths in the global optimal path with the key points as reference points; Detecting whether there is a dynamic obstacle in 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 according to the maximum linear speed, maximum angular speed, minimum linear speed, minimum angular speed, acceleration and deceleration constraints and the distance from the obstacle. Generate 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 end of the trajectory 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 an unmanned aerial vehicle according to claim 1, characterized in that: The global environment includes: drone motion and dynamic obstacles; The UAV motion is expressed as: Among them, Δ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; Dynamic obstacles are represented as: Where c is the obstacle velocity constant.

3. The method for dynamic path planning of an unmanned aerial vehicle 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 a starting point and expands using a node closest to the second random tree as a node expansion direction; 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.

4. The method for dynamic path planning of an unmanned aerial vehicle according to claim 3, characterized in that: When the nodes of the two trees intersect, 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.

5. The method for dynamic path planning of an unmanned aerial vehicle according to claim 1, characterized in that: Extracting key points in the global optimal path includes: For the first random tree and the second random tree, starting from the path starting point and the target point respectively, the angle between two vectors represented by three consecutive nodes is determined in sequence; If the angle is zero, the middle node of three consecutive nodes is deleted from the node set; 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; If the shortest distance is greater than the preset safety threshold, the next node is detected; If the shortest 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.

6. The method for dynamic path planning of an unmanned aerial vehicle according to claim 1, characterized in that: Detecting whether there is a dynamic obstacle in each of the local paths includes: The sensors can identify dynamic obstacles and update the environment information in real time.

7. The method for dynamic path planning of an unmanned aerial vehicle according to claim 1, characterized in that: 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.

8. A dynamic path planning device for an unmanned aerial vehicle, characterized in that: include: A modeling module, processing and constructing a global environment according to 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; 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, 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; A detection module, processing detection of whether there is a dynamic obstacle in each of the local paths; An operation module, processing flying 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 from the obstacle if there is an obstacle. A trajectory module, processing the speed sampling range and the kinematic parameters of the UAV to generate a predicted trajectory group; A screening module, processing a 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; An adding module is used to process and add the optimal trajectory to the global optimal path to generate a dynamic path for the UAV.

9. The unmanned aerial vehicle dynamic path planning device according to claim 8, characterized in that: The global environment includes: the movement of the drone and dynamic obstacles; The UAV motion is expressed as: Among them, Δ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; Dynamic obstacles are represented as: Where c is the obstacle velocity constant.

10. The unmanned aerial vehicle dynamic path planning device according to claim 8, 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 a starting point and expands using a node closest to the second random tree as a node expansion direction; 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.

11. The unmanned aerial vehicle dynamic path planning device according to claim 10, 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.

12. The unmanned aerial vehicle dynamic path planning device according to claim 8, characterized in that: The local module processes and extracts key points in the global optimal path, including: For the first random tree and the second random tree, starting from the path starting point and the target point respectively, the angle between two vectors represented by three consecutive nodes is determined in sequence; If the angle is zero, the middle node of three consecutive nodes is deleted from the node set; 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; If the shortest distance is greater than the preset safety threshold, the next node is detected; If the shortest 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.

13. The unmanned aerial vehicle dynamic path planning device according to claim 8, 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.

14. The unmanned aerial vehicle dynamic path planning device according to claim 8, characterized in that: The adding module processing further includes, before adding the optimal trajectory to the global optimal path: 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.

15. 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 a method for dynamic path planning of a drone according to any one of claims 1 to 7, and execute: constructing a global environment according to drone attribute information and obstacle attribute information, wherein the global environment includes setting a starting point and a target point of a dynamic path of the drone; taking the starting point and the target point as root nodes, respectively generating a first random tree and a second random tree; performing bidirectional extension and intersection of the first random tree and the second random tree to form a global optimal path; Extract key points in the global optimal path, and generate multiple local paths in the global optimal path with the key points as reference points; detect whether there are dynamic obstacles in each local path; if there are no dynamic obstacles, fly according to the global optimal path; if there are obstacles, generate a speed sampling range according to the maximum linear velocity, maximum angular velocity, minimum linear velocity, minimum angular velocity, acceleration and deceleration constraints and the distance from the obstacle of the UAV; generate a predicted trajectory group according to the speed sampling range and the kinematic parameters of the UAV; take 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; add the optimal trajectory to the global optimal path to generate a dynamic path of the UAV.

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

Images