An improved method for trajectory planning of agricultural quadrotor UAV

Abstract

The present invention discloses an improved method for trajectory planning of agricultural quad-rotor UAVs. The method aims to solve the problem that the path planned by the traditional A* algorithm is not smooth and has many turning points, which ultimately makes it unsuitable for direct flight of UAVs. The method proposes to expand the search space of the A* algorithm, converting the traditional 8-domain search into a 24-domain search. By expanding the search domain, the algorithm is no longer limited to the integer multiples of π / 4 in the expansion direction of the next node, and finally makes the planned path suitable for UAV flight.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and in particular relates to an improved method for trajectory planning of an agricultural quad-rotor UAV. Background Art

[0002] When a drone is spraying pesticides, it first needs to deliver the pesticides to the designated area. Planning the flight path of the drone during the pesticide delivery process can effectively avoid obstacles in the farmland, and on the other hand, the shorter planned flight path can increase the endurance of the drone in the later stage of spraying pesticides.

[0003] Traditionally, the A* algorithm is used to complete the route planning. The A* algorithm was proposed in 1969. Its operation efficiency is higher than the mainstream Dijkstra algorithm and has received widespread attention. Researchers applied it to path planning and found that its efficiency and results are much higher than other algorithms. The A* algorithm is the most effective direct search algorithm for solving the shortest path in a static road network and is a commonly used heuristic search algorithm. However, the path obtained by the A* algorithm is usually not smooth, and the path needs to be smoothed later. Summary of the invention

[0004] To solve the above problems, the present invention discloses an improved method for trajectory planning of agricultural quadrotor UAVs, which expands the search area of ​​nodes in the grid graph to 24 areas, obtains a path that is smoother than the traditional A* algorithm, reduces the number of turning points, and is more suitable for UAV flight.

[0005] To achieve the above object, the technical solution of the present invention is as follows:

[0006] An improved method for trajectory planning of an agricultural quadrotor drone comprises the following steps:

[0007] S1, various environmental, map and other factors are abstracted into computer models that can be understood by computers, and the grid method is often used for modeling. First, the map is preprocessed, and then the image is binarized. The purpose of binarization is to generate a binary map based on the map, where the black part represents obstacles and the white part represents the navigable area of ​​the drone. Then the grid method is used to generate environmental modeling. When selecting the grid particle size, the actual modeling environment needs to be considered. The smaller the grid size, the more accurate the environmental model, but the lower the computing efficiency. On the contrary, the larger the grid size, the rougher the environmental model and the higher the computing efficiency.

[0008] S2, create two lists openlist and closelist, and obtain the starting node and target node of the path to be planned.

[0009] S3, starting from the starting point s, puts s into the openlist as a square waiting to be checked.

[0010] S4, find the reachable squares around the starting point s (up to 24), put them into the openlist, and set their parent square to s.

[0011] S5, delete the starting point s from the openlist and put s into the closelist.

[0012] S6, calculate the F value of the surrounding squares. f(n) = g(n) + h(n), g(n) represents the movement cost from the starting point to s to the specified square, set the cost of moving one square horizontally to 10, and the cost of moving one square diagonally to 14, H represents the estimated cost of moving from the specified square to the target point E, and the calculation method of H is to ignore obstacles and only perform vertical and horizontal calculations.

[0013] S7, select the square a with the smallest F value from the openlist, delete it from the openlist, and put it into the closelist.

[0014] S8, check all adjacent and reachable squares of a, which can be divided into the following three cases:

[0015] 1) Obstacles and squares in the closed list are not considered

[0016] 2) If these squares are not in the openlist yet, add them to the openlist, calculate the F value of these squares, and set the parent square to a.

[0017] 3) If the adjacent square c is already in the openlist, calculate the new path from s to square c (the path through a), and determine whether the G value is lower. If the new G value is lower, change the parent square to a and recalculate the F value. If the new G value is higher, the F value does not change.

[0018] S9, continue to find blocks with smaller F values ​​from the openlist, delete them from the openlist, add them to the closelist, and continue to find blocks that can be reached around them, and repeat this cycle.

[0019] S10, end judgment: when there is an end point in the openlist, it means that the path has been found; when there is no data in the openlist, it means that there is no suitable path.

[0020] Furthermore, in the present invention, in step S1, max is the long side length of the largest obstacle, l min is the short side length of the smallest obstacle, S sumIndicates the size of the final generated graph of the environmental modeling model, S obs Represents the area of ​​all obstacles and non-passable areas. Assuming the grid length is l, then

[0021]

[0022] Furthermore, in the present invention, in step S2, openlist means recording all grid sets considered for finding the shortest path, and closelist means recording grid sets that will no longer be considered.

[0023] Further, in the present invention, in step S6, f(n)=g(n)+h(n), h(n) is selected as follows:

[0024] 1) If h(n) = 0, then only g(n) is actually useful. In this case, the A* algorithm, also known as the Dijkstra algorithm, is guaranteed to find an optimal path.

[0025] 2) If h(n) is always less than (or equal to) the number of steps from node n to the target node, then the A* algorithm can definitely find the optimal path. The smaller h(n), the more nodes A* expands, making the A* algorithm slower.

[0026] 3) If h(n) is exactly equal to the number of steps from node n to the target node, all nodes expanded by the A* algorithm are on the optimal path, and it will not expand any other irrelevant nodes. At this time, the speed of the A* algorithm is very fast.

[0027] 4) If the information given by h(n) is sometimes greater than the number of steps from node n to the goal node, then the A* algorithm will not be guaranteed to find the optimal path, but it will run faster.

[0028] 5) In the other extreme case, if h(n) is very close to g(n), then only h(n) will work, and the A* algorithm actually becomes a breadth-first search.

[0029] Considering the above situation, h(n) = |x1-x2| + |y1-y2| is set, where (x1, y1) represents the coordinates of the starting node, and (x2, y2) represents the coordinates of the target node. h(n) represents the cost to the end point, and g(n) represents the cost from the starting point to the specified point. The cost of horizontal or vertical movement is 10, and the cost of diagonal movement is 14.

[0030] Compared with the prior art, the beneficial effects of the present invention are:

[0031] The improved method for trajectory planning of an agricultural quad-rotor UAV provided by the present invention adds more search directions and reduces the number of turning points compared to the traditional A* algorithm, making the trajectory of the UAV smoother and more suitable for UAV flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 is a flow chart of the method of the present invention;

[0033] Figure 2 It is an 8-field search map;

[0034] Figure 3 It is a 24-area search map. DETAILED DESCRIPTION

[0035] The present invention will be further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0036] like Figure 1 The figure shows the flow chart of the improved algorithm for trajectory planning of pesticide delivery by quadrotor drones. The specific implementation steps are as follows:

[0037] S1, various environmental, map and other factors are abstracted into computer models that can be understood by computers, and the grid method is often used for modeling. First, the map is preprocessed, and then the image is binarized. The purpose of binarization is to generate a binary map based on the map, where the black part represents obstacles and the white part represents the navigable area of ​​the drone. Then the grid method is used to generate environmental modeling. When selecting the grid particle size, the actual modeling environment needs to be considered. The smaller the grid size, the more accurate the environmental model, but the lower the computing efficiency. On the contrary, the larger the grid size, the rougher the environmental model and the higher the computing efficiency.

[0038] Assume l max is the long side length of the largest obstacle, l min is the short side length of the smallest obstacle, S sum Indicates the size of the final generated graph of the environmental modeling model, S obs Represents the area of ​​all obstacles and non-passable areas. Assuming the grid length is l, then

[0039]

[0040] S2, create two lists openlist and closelist, and obtain the starting node and target node of the path to be planned. Openlist means recording all the grid sets considered to find the shortest path, and closelist means recording the grid sets that will not be considered again.

[0041] S3, starting from the starting point s, puts s into the openlist as a square waiting to be checked.

[0042] S4, find the reachable squares around the starting point s (up to 24), put them into the openlist, and set their parent square to s.

[0043] S5, delete the starting point s from the openlist and put s into the closelist.

[0044] S6, calculate the F value of the surrounding grid. f(n) = g(n) + h(n), g(n) represents the cost of moving from the starting point to s to the specified grid, set the cost of moving one grid horizontally to 10, and the cost of moving one grid diagonally to 14, H represents the estimated cost of moving from the specified grid to the target point E, and the calculation method of H is to ignore obstacles and only perform vertical and horizontal calculations. Let (x1, y1) represent the coordinates of the starting node, and (x2, y2) represent the coordinates of the target node, then

[0045] h(n)=|x1-x2|+|y1-y2|

[0046] g(n) represents the cost from the starting point to the specified point. The cost of horizontal or vertical movement is specified as 10, and the cost of diagonal movement is specified as 14.

[0047] S7, select the square a with the smallest F value from the openlist, delete it from the openlist, and put it into the closelist.

[0048] S8, check all adjacent and reachable squares of a

[0049] 1) Obstacles and squares in the closed list are not considered

[0050] 2) If these squares are not in the openlist yet, add them to the openlist, calculate the F value of these squares, and set the parent square to a.

[0051] 3) If the adjacent square c is already in the openlist, calculate the new path from s to square c (the path through a), and determine whether the G value is lower. If the new G value is lower, change the parent square to a and recalculate the F value. If the new G value is higher, the F value does not change.

[0052] S9, continue to find blocks with smaller F values ​​from the openlist, delete them from the openlist, add them to the closelist, and continue to find blocks that can be reached around them, and repeat this cycle.

[0053] S10, end judgment: when there is an end point in the openlist, it means that the path has been found; when there is no data in the openlist, it means that there is no suitable path.

[0054] It should be noted that the above content only illustrates the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications all fall within the protection scope of the claims of the present invention.

Claims

1. An improved method for trajectory planning of agricultural quadrotor drones, characterized by: The following steps are involved: S1, abstract various environmental and map factors into computer models that can be understood by computers, often using the grid method to model; First, the map is preprocessed, and then the image is binarized. The purpose of binarization is to generate a binary map based on the map, in which the black part represents obstacles and the white part represents the navigable area of ​​the drone; then the grid method is used to generate environmental modeling. When selecting the grid particle size, the actual modeling environment needs to be considered. The smaller the grid size, the more accurate the environmental model, but the lower the computing efficiency. On the contrary, the larger the grid size, the rougher the environmental model and the higher the computing efficiency. S2, create two lists openlist and closelist, and obtain the starting node and target node of the path to be planned; S3, starting from the starting point s, put s into the openlist as a square to be checked; S4, find the reachable squares around the starting point s, up to 24 squares, put them into the openlist, and set their parent square to s; S5, delete the starting point s from the openlist and put s into the closelist; S6, calculate the F value of the surrounding squares; f(n) = g(n) + h(n), g(n) represents the movement cost from the starting point to s to the specified square, the cost of moving one square horizontally is set to 10, and the cost of moving one square diagonally is set to 14, H represents the estimated cost of moving from the specified square to the target point E, and the calculation method of H is to ignore obstacles and only perform vertical and horizontal calculations; S7, select the square a with the smallest F value from the openlist, delete it from the openlist, and put it into the closelist; S8, check all adjacent and reachable squares of a, which can be divided into the following three cases: 1) The squares in the obstacle and closed lists are not considered; 2) If these squares are not in the openlist yet, add them to the openlist, calculate the F value of these squares, and set the parent square to a; 3) If the adjacent square c is already in the openlist, calculate a new path from s through a to reach c, and determine whether the G value is lower. If the new G value is lower, change the parent square to a and recalculate the F value. If the new G value is higher, the F value remains unchanged. S9, continue to find blocks with smaller F values ​​from the openlist, delete them from the openlist, add them to the closelist, and continue to find blocks that can be reached around them, and repeat this cycle; S10, end judgment: when there is an end point in the openlist, it means that the path has been found; when there is no data in the openlist, it means that there is no suitable path.

2. The improved method for trajectory planning of an agricultural quadrotor drone according to claim 1, characterized in that: In the step S1, max is the long side length of the largest obstacle, l min is the short side length of the smallest obstacle, S sum Indicates the size of the final generated graph of the environmental modeling model, S obs Represents the area of ​​all obstacles and non-passable areas. Assuming the grid length is l, then 3. The improved method for trajectory planning of an agricultural quad-rotor UAV according to claim 1 is characterized in that: In step S2, openlist means recording all grid sets considered for finding the shortest path, and closelist means recording grid sets that will no longer be considered.

4. The improved method for trajectory planning of an agricultural quad-rotor UAV according to claim 1 is characterized in that: In step S6, h(n)=|x1-x2|+|y1-y2| is set, where (x1, y1) represents the coordinates of the starting node, and (x2, y2) represents the coordinates of the target node; h(n) represents the consumption to the end point, and g(n) represents the consumption from the starting point to the specified point. The consumption for horizontal or vertical movement is specified to be 10, and the consumption for diagonal movement is specified to be 14.

Images