Unmanned aerial vehicle autonomous flight strategy based on fusion of improved A* algorithm and DWA algorithm

Abstract

The invention discloses an unmanned aerial vehicle autonomous flight strategy based on fusion of an improved A * algorithm and a DWA algorithm. The unmanned aerial vehicle autonomous flight strategy comprises the following steps: S1, setting a take-off point of an unmanned aerial vehicle; s2, environment modeling is carried out on the motion scene of the unmanned aerial vehicle and the dynamic target, and flight environment information data is collected and converted into grid map representation; s3, planning a global optimal path of the unmanned aerial vehicle in combination with an A * algorithm of a heuristic function value and a path cost value; and fusing a DWA algorithm to perform avoidance optimization on the local path of the unmanned aerial vehicle. An A * algorithm is improved and a DWA (Dynamic Window Method) algorithm is fused. The improved A * algorithm can quickly generate a global optimal or suboptimal path, and the improved DWA algorithm can quickly respond and avoid obstacles in a local range. Real-time obstacle avoidance can be carried out in a local range according to the speed and acceleration limitation of the unmanned aerial vehicle evaluated in real time and the position of the dynamic obstacle, so that the route of the unmanned aerial vehicle tends to be perfect.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle flight control, and specifically to an unmanned aerial vehicle autonomous flight strategy based on the fusion of improved A* algorithm and DWA algorithm. Background Art

[0002] As UAV technology matures, it can be widely used in military reconnaissance, power inspection, cargo transportation, logistics distribution and other fields. With the rapid development of automatic control technology, sensor technology, navigation technology and other aspects, the functions of UAVs are becoming more and more perfect, and the application fields are constantly expanding. Therefore, it is of great significance to study the path planning problem of UAVs.

[0003] Traditional UAV path planning algorithms include DijkstrA* algorithm, artificial potential field method and traditional A* algorithm. The DijkstrA* algorithm has the problem of large computational complexity and slow convergence, the artificial potential field method is prone to falling into the local optimal point, and the traditional A* algorithm has the problem that the path line is not smooth and in a dynamic environment, when the area changes or obstacles appear temporarily, the A* algorithm may not be able to complete the path update in time.

[0004] Therefore, by improving the A* algorithm and integrating the DWA (dynamic window method) algorithm, according to the real-time evaluation of the speed and acceleration limits of the drone, as well as the position of dynamic obstacles, real-time obstacle avoidance can be performed in a local range, making the drone's route perfect. This will be a beneficial embodiment of the current technical advantages in this field. Summary of the invention

[0005] This paper proposes an autonomous flight strategy for UAVs based on the fusion of improved A* algorithm and DWA algorithm. It aims to combine the global path planning capability of A* algorithm and the local dynamic obstacle avoidance capability of DWA algorithm to provide a more efficient, safe and flexible path planning solution for UAVs in complex environments.

[0006] Technical solution of the present invention

[0007] An autonomous flight strategy for a UAV based on the fusion of improved A* algorithm and DWA algorithm includes the following steps:

[0008] S1. Set the take-off point of the drone;

[0009] S2, conduct environmental modeling for the motion scenes of UAVs and dynamic targets, collect flight environment information data, and convert it into a raster map representation;

[0010] S3. Combine the A* algorithm of the heuristic function value and the path cost value to plan the global optimal path of the UAV; integrate the DWA algorithm to avoid and optimize the local path of the UAV.

[0011] Preferably, step S2 comprises:

[0012] S21. According to the overall size of the target environment, define the size of the grid and generate a two-dimensional array or matrix, where each array element represents a grid;

[0013] S22, collecting flight environment information data, and converting the data into a coordinate system on a grid map. After the traversal is completed, the status of all grids is updated to one of an idle state or an occupied state, and a grid map is generated.

[0014] Preferably, the step S21 generates a two-dimensional matrix, and each matrix element represents a grid.

[0015] Preferably, step S3 comprises:

[0016] S31, initialize the starting point and the end point, set the starting point and the end point, and their coordinates are (x s ,y s ) and (x g ,y g ), the open list means that it is initialized to an empty list containing the starting point and stores the nodes to be explored; the closed list means that it is initialized to an empty list and stores the nodes that have been explored;

[0017] S32. Using the Euclidean distance as the heuristic function, calculate the estimated distance from node i to the target node:

[0018]

[0019] Where (x i ,y i ) is the coordinate of node i, (x g ,y g ) are the coordinates of the target node;

[0020] S33, select an initial node, set the starting node s as the current node, add the starting node to the open list, and set its initial cost g(s) to 0;

[0021] S34, selecting a target node from the open list;

[0022] S35. Output the found optimal path and its total path length L:

[0023]

[0024] Among them, P i is a node on the path, d(P i , P i+1 ) is the distance between adjacent nodes;

[0025] S36: If a new obstacle node is found in the local path, the DWA algorithm is used to avoid it;

[0026] S37. Select the best trajectory of the drone, and score each trajectory using an evaluation function.

[0027] Preferably, the step S34 includes:

[0028] S341. Select the current node from the open list:

[0029] Select the node with the lowest f(i) value in the open list as the current node i, calculated as:

[0030] f(i)=g(i)+h(i)

[0031] Among them, g(i) is the actual path cost from the starting point to node i, h(i) is the Euclidean distance from node i to the target node g;

[0032] S342, remove the current node i from the open list and add it to the closed list;

[0033] S343. Check whether the current node is the target node:

[0034] If the current node is the target node i, the path is found and the search ends;

[0035] S344. Expand neighbor nodes of the current node:

[0036] For each neighbor node i+1 of the current node i, perform the following operations: If the neighbor node is already in the closed list, skip it. Calculate the actual path cost g of the neighbor node:

[0037] g(i+1)=g(i)+d(i,i+1)

[0038]

[0039] Among them, d(i,i+1) is the distance from the current node i to the neighbor node i+1;

[0040] In a two-dimensional grid, use the Euclidean distance to calculate the h value of neighbor nodes:

[0041]

[0042] Calculate the f value of neighbor nodes:

[0043] f(i+1)=g(i+1)+h(i+1)

[0044] Update the open list. If the neighbor node is not in the open list, or the new path has a smaller g value, update the neighbor node's g value and add it to the open list or update its position. When the target node's g value is found, backtrack the path through each target node's parent node, from g to s, to get the optimal path.

[0045] Preferably, the coordinates of the newly added obstacle node are defined as (x d ,y d ), the step S36 comprises:

[0046] S361. Give the UAV linear velocity, angular velocity, and acceleration limit ranges. The maximum linear velocity, minimum linear velocity, maximum angular velocity, and minimum angular velocity of the UAV are v respectively. max 、v min 、w max and w min , and the acceleration limit of the drone is α max and α min ;

[0047] v min =max(v min ,v-α max ·Δt)

[0048] v max =min(v max ,v+α max ·Δt)

[0049] Angular velocity range of the drone:

[0050] w min =max(w min ,w-α max ·Δt)

[0051] w max =min(w max ,w+α max ·Δt)

[0052] Where Δt is the time step of the prediction;

[0053] S362, trajectory prediction, for a given linear velocity v d and the angular velocity w d , calculate the position of the drone in the future. Assuming the prediction time is T, the position prediction formula is:

[0054]

[0055] θ t =θ 0 +w d ·T

[0056] Among them, (x 0 ,y 0 ,θ 0 ) is the current state of the drone, (x t ,y t ) is the position at time t, θ t is the heading angle at time T;

[0057] S363, referring to the geometric shape of the obstacle and the shape of the drone, detect the current position of the drone (x i ,y i ) and whether each point on the predicted trajectory is in contact with an obstacle (x d ,y d ) overlap, if overlapped, collision may occur;

[0058] S364, objective function evaluation, using the angle between the target direction and the drone's orientation to measure, comparing the drone's current orientation and the target direction; the distance to the nearest obstacle can also be calculated, usually the safety distance threshold is used to determine whether it is safe.

[0059] Preferably, the method of selecting the best trajectory in step S37 includes:

[0060] Evaluation function G of the distance between the drone and the target head :

[0061]

[0062] Among them, θ head is the angle between the drone and the target point;

[0063] Evaluation function G of the obstacle avoidance distance of UAV d :

[0064]

[0065] Where d is the distance to the nearest obstacle on the trajectory, and r is the safety distance threshold;

[0066] UAV speed evaluation function G v :

[0067]

[0068] Among them, v t is the velocity at time T, v max is the maximum speed;

[0069] The final comprehensive evaluation function G is the weighted sum of the above items:

[0070] G=α·G head +β·Gd +γ·G v

[0071] Among them, α, β, and γ are weight coefficients of each part, α+β+γ=1 and α≥β≥γ.

[0072] Preferably, the method for collecting flight environment information data is to use one or more of laser radar, ultrasonic sensor, infrared sensor, and camera.

[0073] Technical Effects of the Invention

[0074] The advantages of the UAV autonomous flight strategy based on the fusion of the improved A* algorithm and the DWA algorithm are as follows: (1) high efficiency: the improved A* algorithm can quickly generate the global optimal or suboptimal path, while the improved DWA algorithm can quickly respond and avoid obstacles in the local range; (2) safety: through real-time obstacle avoidance and dynamic adjustment, the UAV can be ensured to fly safely in complex environments; (3) flexibility: combining global path planning and local dynamic obstacle avoidance, the UAV can adapt to various complex environments and dynamic changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 This is a structural block diagram of the autonomous flight strategy of the UAV of the present invention.

[0076] Figure 2 It is a binary grid constructed according to the environment in the present invention.

[0077] Figure 3 This is the A* algorithm path diagram before the improvement of the present invention.

[0078] Figure 4 This is the improved A* algorithm path diagram of the present invention.

[0079] Figure 5 This is the autonomous flight path diagram of the UAV that integrates the improved A* algorithm and DWA algorithm of the present invention. Specific implementation methods

[0081] According to the present invention, a UAV autonomous flight strategy based on the fusion of improved A* algorithm and DWA algorithm is provided. The overall flow chart is as follows: Figure 1 As shown, including:

[0082] Step 1: Set the take-off point of the drone, (x s ,y s ) are the coordinates of the starting node.

[0083] Step 2: Model the environment for the motion scenes of drones and dynamic targets, use lidar, ultrasonic sensors, infrared sensors or cameras to obtain data or preset no-fly airspaces, obtain the environment information of drone operation, and convert it into a grid map representation;

[0084] Step 2.1: Determine the overall size of the environment and define the size of the grid, such as 20*20, that is, the size of the actual physical space represented by each grid; generate a two-dimensional array (matrix) based on the size of the environment and the grid size, where each array element represents a grid.

[0085] Step 2.2: Use laser radar, ultrasonic sensor, infrared sensor or camera to collect environmental data or preset no-fly airspace, and convert the data into the coordinate system on the grid map. After the traversal is completed, the status of all grids will be updated to 0 (free) or 1 (occupied), and a binary grid map will be generated. The simulation diagram is as follows: Figure 2 As shown. Obstacle coordinates: (5:8):2, 7:(5:8), (8:10):(13:15), (10:15):15;

[0086] Step 3: Complete global path planning through the improved A* algorithm, including heuristic function, path cost, and comprehensive evaluation function.

[0087] Step 3.1: Initialize the starting point and end point, set the starting point and end point, and their coordinates are (x s ,y s ) and (x g ,y g ). An open list is an empty list that is initialized to include the starting point and stores the nodes to be explored; a closed list is an empty list that is initialized to store the nodes that have been explored.

[0088] Step 3.2: Using Euclidean distance as a heuristic function, calculate the estimated distance from node i to the target node:

[0089]

[0090] Where (x n ,y n ) is the coordinate of node n, (x g ,y g ) are the coordinates of the target node.

[0091] Step 3.3: Select the initial node, set the starting node s as the current node, add the starting node to the open list, and set its initial cost g(s) to 0.

[0092] Step 3.4: Search process, repeat the following steps until the target node is found or the open list is empty:

[0093] Step 3.4.1: Select a node from the open list:

[0094] Select the node with the lowest f(i) value in the open list as the current node i, calculated as:

[0095] f(i)=g(i)+h(i)

[0096] Among them, g(i) is the actual path cost from the starting point to node i, and h(i) is the Euclidean distance from node i to the target node g.

[0097] Step 3.4.2: Remove the current node from the open list and add it to the closed list:

[0098] Remove the current node i from the open list and add it to the closed list.

[0099] Step 3.4.3: Check whether the current node is the target node:

[0100] If the current node is the target node i, then the path is found and the search ends. The simulation is as follows Figure 3 shown.

[0101] Step 3.4.4: Expand the neighbor nodes of the current node:

[0102] For each neighbor node i+1 of the current node i, perform the following operations: If the neighbor node is already in the closed list, skip it. Calculate the actual path cost g of the neighbor node: g(i+1)=g(i)+d(i,i+1), where d(i,i+1) is the distance from the current node i to the neighbor node i+1.

[0103] In a two-dimensional grid, use the Euclidean distance to calculate the h value of neighbor nodes:

[0104]

[0105] Calculate the f value of neighbor nodes:

[0106] f(i+1)=g(i+1)+h(i+1)

[0107] Update the open list. If the neighbor node is not in the open list, or the new path has a smaller g value, update the neighbor node's g value and add it to the open list or update its position. When the target node g is found, trace back the path through the parent node of each node, from g to s, to get the optimal path.

[0108] Step 3.5: Improved A* algorithm path output:

[0109] Output the optimal path found and its total path length L:

[0110]

[0111] Among them, P i is a node on the path, d(P i , P i+1 ) is the distance between adjacent nodes, such as Figure 4 .

[0112] Step 3.6: When a new obstacle node is found, (x d ,y d ) is the coordinate of the newly added obstacle node, which is avoided by the DWA algorithm.

[0113] Step 3.6.1: Assume that the current speed of the drone is v (linear speed) and w (angular speed), the maximum linear speed, minimum linear speed, maximum angular speed and minimum angular speed of the drone are v and w respectively. max 、v min 、w max and w min , and the acceleration limit of the drone is α max and α min , therefore, the linear velocity range of the dynamic window is: v min =max(v min ,v-α max ·Δt)

[0114] v max =min(v max ,v+α max ·Δt)

[0115] Angular velocity range of dynamic window:

[0116] w min =max(w min ,w-α max ·Δt)

[0117] w max =min(w max ,w+α max ·Δt)

[0118] Where Δt is the time step of the prediction.

[0119] Step 3.6.2: Trajectory prediction

[0120] For a given linear velocity v d and angular velocity w d , the position of the drone in the future can be calculated. Assuming the prediction time is T, the trajectory prediction formula and position prediction formula are:

[0121]

[0122] θ t =θ 0 +w d ·T

[0123] Among them, (x 0 ,y 0 ,θ 0 ) is the current state of the drone, (x t ,y t ) is the position at time t, θ t is the heading angle at time T.

[0124] Step 3.6.3: Collision Detection

[0125] For each trajectory, check whether the drone collides with an obstacle. The general approach is to check whether the drone's current position and each point on the predicted trajectory overlap with an obstacle. The specific collision detection method depends on the geometry of the obstacle and the shape of the drone.

[0126] Step 3.6.4: Objective Function Evaluation

[0127] Distance to target:

[0128]

[0129] Among them, (x g ,y g ) is the position of the target, (x t ,y t ) is the position of the trajectory at the prediction time T. Heading: It is usually evaluated by comparing the current direction of the drone with the target direction.

[0130] It can be measured by the angle between the target direction and the drone's direction. Safety: The distance to the nearest obstacle can be calculated, and a safety distance threshold is usually used to determine whether it is safe.

[0131] Step 3.7: Select the best trajectory

[0132] Each trajectory needs to be scored by an evaluation function, which usually consists of the following parts:

[0133] The drone wants to move towards the target as much as possible, and the evaluation function G of the distance to the target head for:

[0134]

[0135] Among them, θ headIt is the angle between the direction of the drone and the target point.

[0136] The drone needs to stay away from obstacles as much as possible, and the evaluation function G of the obstacle avoidance distance d for:

[0137]

[0138] Among them, d is the distance to the nearest obstacle on the trajectory, and r is the safety distance threshold.

[0139] To ensure efficient movement, the speed evaluation function G v factor:

[0140]

[0141] Among them, v t is the velocity at time T, v max is the maximum speed.

[0142] The final comprehensive evaluation function is usually the weighted sum of the above items:

[0143] G=α·G head +β·G d +γ·G v

[0144] Among them, α, β, and γ are the weight coefficients of each part for adjustment.

[0145] Select the best trajectory: Select the trajectory that maximizes or minimizes the evaluation function G as the optimal trajectory. Through these calculation steps and formulas, the improved DWA algorithm can plan a suitable obstacle avoidance path based on dynamic constraints. Through the above steps and formulas, combined with the A* algorithm and the Euclidean distance heuristic, efficient and accurate path planning can be achieved, such as Figure 5 shown.

[0146] Although the above is a description of the illustrative specific embodiments of the present invention to facilitate the understanding of the present invention by those skilled in the art, it should be clear that the present invention is not limited to the scope of the specific embodiments. Where equivalent substitution or equivalent substitution is adopted, these changes are obvious, and all inventions and creations using the concept of the present invention are protected.

Claims

1. A UAV autonomous flight strategy based on the fusion of improved A* algorithm and DWA algorithm, characterized in that: The following steps are involved: S1. Set the take-off point of the drone; S2, conduct environmental modeling for the motion scenes of UAVs and dynamic targets, collect flight environment information data, and convert it into a raster map representation; S3. Combine the A* algorithm of the heuristic function value and the path cost value to plan the global optimal path of the UAV; integrate the DWA algorithm to avoid and optimize the local path of the UAV.

2. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 1, characterized in that: The step S2 comprises: S21. According to the overall size of the target environment, define the size of the grid and generate a two-dimensional array or matrix, where each array element represents a grid; S22, collecting flight environment information data, and converting the data into a coordinate system on a grid map. After the traversal is completed, the status of all grids is updated to one of an idle state or an occupied state, and a grid map is generated.

3. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 2, characterized in that: The step S21 generates a two-dimensional matrix, and each matrix element represents a grid.

4. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 1, characterized in that: The step S3 comprises: S31, initialize the starting point and the end point, set the starting point and the end point, and their coordinates are (x s ,y s ) and (x g ,y g ), the open list means that it is initialized to an empty list containing the starting point and stores the nodes to be explored; the closed list means that it is initialized to an empty list and stores the nodes that have been explored; S32. Using the Euclidean distance as the heuristic function, calculate the estimated distance from node i to the target node: Where (x i ,y i ) is the coordinate of node i, (x g ,y g ) are the coordinates of the target node; S33, select an initial node, set the starting node s as the current node, add the starting node to the open list, and set its initial cost g(s) to 0; S34, selecting a target node from the open list; S35. Output the found optimal path and its total path length L: Among them, P i is a node on the path, d(P i , P i+1 ) is the distance between adjacent nodes; S36: If a new obstacle node is found in the local path, the DWA algorithm is used to avoid it; S37. Select the best trajectory of the drone, and score each trajectory using an evaluation function.

5. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 4, characterized in that: The step S34 comprises: S341. Select the current node from the open list: Select the node with the lowest f(i) value in the open list as the current node i, calculated as: f(i)=g(i)+h(i) Among them, g(i) is the actual path cost from the starting point to node i, h(i) is the Euclidean distance from node i to the target node g; S342, remove the current node i from the open list and add it to the closed list; S343. Check whether the current node is the target node: If the current node is the target node i, the path is found and the search ends; S344, expand the neighbor nodes of the current node: For each neighbor node i+1 of the current node i, perform the following operations: If the neighbor node is already in the closed list, skip it. Calculate the actual path cost g of the neighbor node: g(i+1)=g(i)+d(i,i+1) Among them, d(i,i+1) is the distance from the current node i to the neighbor node i+1; In a two-dimensional grid, use the Euclidean distance to calculate the h value of neighbor nodes: Calculate the f value of neighbor nodes: f(i+1)=g(i+1)+h(i+1) Update the open list. If the neighbor node is not in the open list, or the new path has a smaller g value, update the neighbor node's g value and add it to the open list or update its position. When the target node's g value is found, backtrack the path through each target node's parent node, from g to s, to get the optimal path.

6. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 4, characterized in that: Define the coordinates of the newly added obstacle node as (x d ,y d ), the step S36 comprises: S361. Give the UAV linear velocity, angular velocity, and acceleration limit ranges. The maximum linear velocity, minimum linear velocity, maximum angular velocity, and minimum angular velocity of the UAV are v respectively. max 、v min 、w max and w min , and the acceleration limit of the drone is α max and α min ; v min =max(v min ,v-α max ·Δt) v max =min(v max ,v+α max ·Δt) Angular velocity range of the drone: w min =max(w min ,w-α max ·Δt) w max =min(w max ,w+a max ·Δt) Where Δt is the time step of the prediction; S362, trajectory prediction, for a given linear velocity v d and angular velocity w d , calculate the position of the drone in the future. Assuming the prediction time is T, the position prediction formula is: i t =θ0+w d ·T Among them, (x0, y0, θ0) is the current state of the drone, (x t ,y t ) is the position at time t, θ t is the heading angle at time T; S363, referring to the geometric shape of the obstacle and the shape of the drone, detect the current position of the drone (x i ,y i ) and whether each point on the predicted trajectory is in contact with an obstacle (x d ,y d ) overlap, if overlapped, collision may occur; S364, objective function evaluation, using the angle between the target direction and the drone's orientation to measure, comparing the drone's current orientation and the target direction; the distance to the nearest obstacle can also be calculated, usually the safety distance threshold is used to determine whether it is safe.

7. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 4, characterized in that: The method of selecting the best trajectory in step S37 includes: Evaluation function G of the distance between the drone and the target head : Among them, θ head is the angle between the drone and the target point; Evaluation function G of the obstacle avoidance distance of UAV d : Where d is the distance to the nearest obstacle on the trajectory, and r is the safety distance threshold; UAV speed evaluation function G v : Among them, v t is the velocity at time T, v max is the maximum speed; The final comprehensive evaluation function G is the weighted sum of the above items: G=α·G head +β·G d +γ·G v Among them, α, β, and γ are weight coefficients of each part, α+β+γ=1 and α≥β≥γ.

8. The autonomous flight strategy of a UAV based on the fusion of improved A* algorithm and DWA algorithm as claimed in claim 1, characterized in that: The method for collecting flight environment information data is to use one or more of laser radar, ultrasonic sensor, infrared sensor, and camera.

Images