Real-time path planning method for fixed-wing unmanned aerial vehicle considering turning cost

Abstract

The application discloses a fixed-wing unmanned aerial vehicle real-time path planning method considering turning cost, which adopts an improved A* algorithm for path planning and proposes an improved method of adaptive step length and turning cost. Firstly, collision detection is carried out through a Bresenham straight line algorithm to obtain an adaptive step length of the next step and a neighbor node to be expanded, so as to improve the planning speed of the algorithm and the adaptability to complex environments and guarantee that a feasible path is planned within a limited time. On this basis, the turning cost is integrated into an expansion generation value function, so that the maneuver is reduced and the quality of the path is improved without destroying the optimality of the algorithm. The application comprehensively considers the planning time and the path quality when planning the path, and improves the efficiency and safety of the real-time path planning of the fixed-wing unmanned aerial vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of real-time path planning, in particular to a path planning method for fixed-wing unmanned aerial vehicles to execute effective paths within a limited time. BACKGROUND

[0002] Unmanned aerial vehicle path planning is the basis for unmanned aerial vehicles to execute tasks, which means that the unmanned aerial vehicle searches for an optimal or suboptimal path from the starting state to the target state in the task space according to the task requirements, and the purpose is to plan a collision-free feasible path from the starting point to the target point. Path planning algorithms can be divided into four categories according to the algorithm properties: graph search-based algorithms, sampling-based algorithms, bionic intelligence-based planning algorithms, and reinforcement learning-based planning algorithms. The randomness of planning time poses a safety hazard for fixed-wing unmanned aerial vehicles, and the incompleteness means there is a risk of not being able to find a path, both of which cannot be tolerated for real-time path planning of fixed-wing unmanned aerial vehicles. The last three types of path planning algorithms all have randomness, and compared to the other three types of path planning algorithms, the A* algorithm based on graph search has completeness and optimality, meets the characteristics of finding the optimal path within a certain time if there is a path, avoids the risk of planning path failure, and is widely used.

[0003] The traditional path planning method in the past takes the shortest route as an indicator to give an optimal or feasible path, and stops searching after the first path is found. However, due to the fact that the state space often grows exponentially with the complexity of the environment, a large amount of search time and computing power is required to find a practical shortest path. And when faced with obstacles in complex environments, traditional path planning algorithms are inefficient in obstacle-free areas. The path planning of fixed-wing unmanned aerial vehicles has strict conditions on time and safety, and fixed-wing unmanned aerial vehicles usually must fly smoothly and without delay, and have the characteristics of fast flight speed, limited onboard assignable computing resources, and inability to hover, etc. Compared to path quality, planning time is a more critical indicator related to the survival of unmanned aerial vehicles, and fixed-wing unmanned aerial vehicles are also subject to dynamic constraints. In order to ensure the safety of actual flight of unmanned aerial vehicles, the evaluation of path planning algorithms should consider both time and path quality. SUMMARY

[0004] In view of the above problems, the A* algorithm is improved and designed, and in order to meet the real-time requirement, the algorithm speed needs to be improved and the neighborhood search time needs to be reduced in the case of large and complex task environment scale, and methods such as adaptive step size strategy and turning cost are introduced, so that the unmanned aerial vehicle can plan a feasible path within a limited time in a known complex environment, ensuring the path quality while meeting the real-time requirement.

[0005] A fixed-wing unmanned aerial vehicle real-time path planning method considering turning cost, specifically comprising the following steps:

[0006] Step S1: using sensors to obtain environmental information to build a grid map, determine the current starting point information and end point information of the UAV;

[0007] Step S2: initialize the to-be-expanded list as an empty set, select the maximum step value of the initial adaptive step length, and set the generation value k(s start ) of the starting point to 0 and add it to the to-be-expanded list;

[0008] Step S3: if the to-be-expanded list is not an empty set, go to step S4; otherwise, go to step S6;

[0009] Step S4: pop out the node with the minimum generation value k(s) considering the turning cost from the to-be-expanded list, and go to step S6 when the end point is popped out, otherwise go to step S5;

[0010] Step S5: expand the popped node, obtain the surrounding neighbor nodes using adaptive step length, update the generation values of the neighbor nodes and the to-be-expanded list, and return to step S4;

[0011] Step S6: if the path exists, output the current path for fixed-wing UAV path following; otherwise, the path planning fails.

[0012] Further, the generation value k(s) and the turning cost turncost(s) contained therein in S4 are shown in formulas (1) and (2):

[0013]

[0014]

[0015] Wherein, the generation value k(s) is the priority of the expanded node, when multiple nodes with the same generation value k1(s) appear, the node with the smaller turning cost k2(s) is popped out first; g(s) is the path generation value of the node to the starting point, h(s) is the path estimated generation value of the node to the end point, and θ is the turning angle of the UAV.

[0016] Further, the step S5 specifically includes:

[0017] S51: expand the popped minimum node, combine the Bresenham straight line algorithm for collision detection in the grid environment, and directly obtain the surrounding feasible neighbor nodes using the adaptive step length method.

[0018] S52: calculate the generation value k(s) of each neighbor node of the popped minimum node, update the generation value of the neighbor node with a better path with it as the parent node, add the updated neighbor node to the to-be-expanded list, and return to step S4;

[0019] Further, the S51 adaptive step size obtaining surrounding neighbor node method specifically comprises:

[0020] Step S511: record the coordinates of the current node grid unit as (x g ,y g ), traverse all child nodes under the maximum step size, and record the grid coordinates of the current to-be-expanded child node as (x m ,y m ).

[0021] Step S512: connect the straight line from the current node (x g ,y g ) to the child node (x m ,y m ), calculate the absolute value |k| of the slope of the straight line, and initialize the cumulative error variable e to 0. Starting from the current node (x g ,y g ), the grid unit coordinates (x i ,y i ) are iterated along the straight line to the to-be-expanded grid unit coordinates (x m ,y m ), and the iteration direction is x dir =(x m -x g ) / |x m -x g |, y dir =(y m -y g ) / |y m -y g |. If |k|≥1, then step S513 is entered to expand along the vertical axis direction, otherwise step S514 is entered to expand along the horizontal axis direction.

[0022] Step S513: before checking each grid unit in the iteration process, e=e+1, if e≥|k| / 2, then the current grid unit coordinates are updated to (x i +x dir ,y i +y dir ), and e=e-|k|; otherwise, the coordinates are updated to (x i ,y i +y dir ); it is judged whether the coordinates are obstacle grids, if yes, then the current iteration is ended, when the grid unit coordinates of the halved step size meet the requirements, the grid unit is returned as an expanded child node, and step S515 is entered; otherwise, the grid unit coordinates are continuously updated to the current to-be-expanded child node grid unit (x m ,y m ).

[0023] Step S514: e = e + |k| before checking each grid cell in the iteration process, if e >= 1 / 2, then the current grid cell coordinates are updated as (x i +x dir ,y i +y dir ), and e = e - 1 at the same time; otherwise, the coordinates are updated as (x i +x dir ,y i ); it is judged whether the coordinates are obstacle grids, if yes, the current iteration round is ended, when the grid cell coordinates of the halving step length meet the requirements, they are returned as the expansion subnode, and step S515 is entered; otherwise, the grid cell coordinates are continuously updated to the current to-be-expanded subnode grid cell (x m ,y m ).

[0024] Step S515: it is judged whether the grid cells of all to-be-expanded subnodes of the current node are traversed, if yes, the current node expansion calculation round is ended, otherwise, step S512 is returned.

[0025] The beneficial effects of the present application are as follows:

[0026] 1. The present application adopts the Bresenham algorithm for collision detection to select adjacent nodes for expansion, which can adapt to complex environments compared with a single fixed step length, quickly find a suboptimal feasible path, and ensure the real-time requirement;

[0027] 2. The present application introduces a turning cost, the algorithm preferentially expands nodes without turning when the path length cost is the same, which can reduce unnecessary turning, reduce the node expansion of the same cost, and generate a more straight path. BRIEF DESCRIPTION OF DRAWINGS

[0028] In order to more clearly illustrate the technical details in the implementation process of the present application, the flow, mechanism, principle and simulation verification experiment results in the technical implementation process will be shown in the drawings below. Obviously, the drawings described below are only part of the embodiments in the present application.

[0029] Figure 1 An algorithm flowchart based on the real-time path planning method proposed in the present application.

[0030] Figure 2 An implementation schematic diagram of the adaptive step length mechanism under a certain maximum step length proposed in the present application.

[0031] Figure 3a , Figure 3b A schematic diagram of the embodiment of the present application Figure 1 .

[0032] Figure 4a , Figure 4bThis is an illustration of an embodiment of the present invention. Figure 2 . Detailed Implementation

[0033] The embodiments of the present invention will now be described in more detail with reference to the accompanying drawings. It should be understood that the embodiments described herein are for the purpose of better understanding the scope and implementation of the present invention and do not represent the full scope of the present invention.

[0034] Figure 1 This is a flowchart of the real-time path planning method for fixed-wing UAVs in complex environments proposed in this invention. The specific implementation steps are as follows:

[0035] Step S1: First, construct a two-dimensional grid map based on the environmental information obtained by the sensor to obtain the initial grid map, and provide the starting point and ending point information of the UAV.

[0036] Step S2: Initialize the maximum step size of the UAV's adaptive step size, initialize the cost value k(s) of the current position to 0, and add it to the list to be expanded;

[0037] Step S3: If the list to be expanded is not empty, proceed to step S4; otherwise, proceed to step S6.

[0038] Step S4: Based on the sorting in the list to be expanded, pop the node with the smallest value k(s), prioritizing sorting by path cost k1(s). When nodes with the same path cost k1(s) appear, prioritize popping the node with the smaller turning cost k2(s). As defined by k2(s), nodes with smaller k2(s) values ​​also have smaller turning angles, ensuring that paths with smaller turning angles are found first, which helps reduce the maneuverability and energy consumption of the fixed-wing UAV. If, during a certain expansion, two nodes with the same smallest k1(s) value, S4 and S5, appear in the list to be expanded, their corresponding costs... Since the algorithm of this invention takes into account the different values ​​of k2(s), it can judge the quality of two nodes and will prioritize popping point S5 for the next step of node expansion and search path; when the endpoint is popped, proceed to step S6, otherwise proceed to step S5.

[0039] Step S5: Gradually expand the neighboring nodes of the popped node, perform collision detection using the Bresenham algorithm, and retain feasible neighboring nodes, such as... Figure 2 The diagram shows a portion of the node expansion process, where grid cells S1 and S2 are the child nodes to be expanded from node S. The algorithm iterates from the current node S along line SS1 towards point S1, using the absolute value of the slope of the line. The iteration direction is x dir =-1,y dir =1, because |k|≥1, at this time, expand to the upper left along the vertical axis, and the node will move forward y along the vertical axis during each iteration update.dir And according to the condition e >= |k| / 2, the node is determined whether to advance x along the horizontal axis direction dir The node will be iterated step by step until reaching the S1 point or encountering an obstacle. Since no obstacle is encountered during the whole process, the S1 point is directly stored as a child node in the list to be expanded;

[0040] The algorithm starts from the node S and iterates step by step along the straight line SS2 to the S2 point, and the absolute value of the slope of the straight line is The iteration direction is x dir =1, y dir =1, because |k|<=1, at this time, the node is expanded to the upper right along the horizontal axis, and the node is updated along the horizontal axis direction x dir And according to the condition e >= 1 / 2, the node is determined whether to advance y along the vertical axis direction dir The node will be iterated step by step until reaching the S2 point or encountering an obstacle. Since the collision with the obstacle occurs halfway, the node S2' corresponding to the halved step of this direction is stored as a child node in the list to be expanded, and the expandability of this direction is retained. The adaptive step length is beneficial to improve the path planning efficiency and meet the real-time requirements of the fixed-wing unmanned aerial vehicle path planning. While expanding the neighboring nodes, the generation value of the neighboring node is calculated, the neighboring node with a better path is updated, and the updated neighboring node is added to the list to be expanded, and the step S4 is returned.

[0041] Step S6: If the path exists, output the current path for the fixed-wing unmanned aerial vehicle path following; otherwise, no feasible path is planned, and the path planning fails.

[0042] After the above steps, Figure 3a 、 Figure 3b 、 Figure 4a 、 Figure 4b respectively represent the implementation effect of the present application in two complex environments and the comparison data with the A* algorithm; the adaptive step length method proposed by the present application can adapt to step length expansion in any direction. In order to further improve the real-time performance, the eight directions of the neighborhood are explored in the two scenes.

[0043] Example 1

[0044] Figure 3a 、 Figure 3bThe path planned by the A* algorithm in the complex environment of the concave obstacle is mostly along the obstacle, and the path is tortuous, and multiple consecutive turning points appear, which is not conducive to the tracking of the unmanned aerial vehicle. It can be seen that the method proposed in the application has the characteristics of adaptive step length and adds the turning angle cost heuristic term, and in multiple generation value same nodes, the node with small turning cost is preferentially expanded, the planned path has fewer turns and smaller turning angles, and since a large step length is taken during initial planning, the path will not be generated in the dense island group, compared with the path planned by the A* algorithm, the path is more straight, and the unmanned aerial vehicle only needs a few turning maneuvers to reach the target point. As can be seen from the expanded nodes represented by the gray area in the figure, when the A* algorithm is planned, a large number of blank spaces around it are explored, which affects the efficiency of the algorithm, and the adaptive step length of the algorithm makes the algorithm explore the feasible area faster, and the main time and computing power are used for exploring the nodes around the obstacle and close to the target, when the algorithm runs away from the concave obstacle environment, the target point can be quickly found. The number of nodes to be expanded planned by the algorithm of the application is at a low level, reducing the occupation of running memory; from the operation time, the algorithm of the application is much faster than the A* algorithm, verifying the real-time of the algorithm of the application. As shown in Table 1.

[0045] Table 1

[0046]

[0047] Example 2

[0048] Figure 4a 、 Figure 4b The environment is complex in crossing the strait, and the infeasible area is detected, it can be seen that the algorithm of the application plans the entire path with only six turns, while the A* algorithm cannot judge the advantages and disadvantages when the path generation value is balanced, and then selects the node with small turning angle, and the planned path completely turns along the ups and downs of the obstacle, and the path is poor in followability. From the planning time, due to the adaptive step length and the expansion condition considering the turning cost, the operation time and the number of expanded nodes of the algorithm of the application are significantly reduced compared with the A* algorithm during initial planning, and the path length has reached the shortest path length in this scene. From the planning time, after the path planning of the algorithm of the application is completed, the A* algorithm has not generated a feasible path. The A* algorithm explores and expands a large number of nodes in the open area, consumes operation time, and is not conducive to timely generation of a feasible path, while the number of expanded nodes of the algorithm of the application is significantly reduced compared with the A* algorithm, the operation efficiency of path planning is high, and the real-time advantage of the algorithm of the application is further verified. As shown in Table 2.

[0049] Table 2

[0050]

[0051] The above describes the specific embodiments of the present application in combination with the drawings, but the above examples cannot be understood as limiting the scope of the present application, the protection scope of the present application is defined by the appended claims, and any modification based on the claims of the present application belongs to the protection scope of the present application.

Claims

1. A fixed-wing unmanned aerial vehicle real-time path planning method considering turning cost, characterized in that, Specifically comprising the following steps: Step S1: using a sensor to obtain environmental information to build a grid map, and determining current start point information and end point information of the UAV; Step S2: initialize the list to be expanded as an empty set, select the maximum step value of the initial adaptive step, and set the generation value k(s start ) of the starting point as 0, and add it to the list to be expanded; Step S3: if the to-be-expanded list is not empty, go to step S4; otherwise, go to step S6; Step S4: popping out a node with the minimum k(s) considering the turning cost from the to-be-expanded list, and going to step S6 when the end point is popped out, or going to step S5 otherwise; Step S5: expanding the popped-out node, obtaining surrounding neighbor nodes using an adaptive step length, updating the neighbor node k(s) and the to-be-expanded list, and returning to step S4; Step S6: if the path exists, outputting the current path for the fixed-wing UAV path following; otherwise, path planning fails; The k(s) and the turning cost turncost(s) contained in the k(s) in step S4 are shown in formulas (1) and (2): Wherein, the k(s) is the priority of the expanded node, when multiple nodes with the same k1(s) appear, the node with the smaller k2(s) is popped out first; g(s) is the path k(s) from the node to the start point, h(s) is the path estimated k(s) from the node to the end point, and θ is the turning angle of the UAV; The step S5 specifically comprises: S51: expanding the popped-out minimum node, combining the Bresenham straight line algorithm to perform collision detection in the grid environment, and directly obtaining surrounding feasible neighbor nodes using an adaptive step length method; S52: calculating the k(s) of each neighbor node of the popped-out minimum node, updating the k(s) of the neighbor node with a better path taking it as the parent node, adding the updated neighbor node to the to-be-expanded list, and returning to step S4; The adaptive step length method directly obtaining surrounding feasible neighbor nodes in step S51 specifically comprises: Step S511: record the coordinates of the current node grid unit as (x g ,y g ), traverse all child nodes under the maximum step, and record the grid coordinates of the current to-be-expanded child node as (x m ,y m ); Step S512: connect the current node (x g ,y g ) to the child node (x m ,y m ) straight line, calculate the absolute value of the slope of the straight line k = |y m -y g | / |x m -x g |, initialize the cumulative error variable e to 0; from the current node (x g ,y g ), the grid cell coordinates (x i ,y i ) along the straight line to the grid cell coordinates to be expanded (x m ,y m ) iteration, the iteration direction is x dir =(x m -x g ) / |x m -x g |, y dir =(y m -y g ) / |y m -y g |; if |k|≥1, go to step S513 to expand along the longitudinal axis direction, otherwise go to step S514 to expand along the transverse axis direction; Step S513: e = e + 1 before checking each grid cell in the iteration process, if e ≥ |k| / 2, update the current grid cell coordinates to (x i +x dir ,y i +y dir ), and e = e - |k|; otherwise, update the coordinates to (x i ,y i +y dir ); determine whether the coordinates are obstacle grids, if yes, end the iteration, return the grid cell coordinates as the extended child node when the grid cell coordinates of the halved step length meet the requirements, and enter step S515; otherwise, continue to update the grid cell coordinates to the current grid cell (x m ,y m ) to be expanded. Step S514: During the iteration process, before checking each grid cell, e = e + |k|. If e ≥ 1 / 2, then update the current grid cell coordinates to (x... i +x dir ,y i +y dir ), and e = e - 1; otherwise, the coordinates are updated to (x i +x dir ,y i ); Determine if the coordinate is an obstacle grid. If so, the current iteration ends. When the grid cell coordinates of the half-step size meet the requirements, return it as an expanded child node and proceed to step S515; otherwise, continue updating the grid cell coordinates to the current child node grid cell to be expanded (x m ,y m ); Step S515: judging whether all grid cells of all to-be-expanded child nodes of the current node are traversed, if yes, the current node expansion calculation is ended, or if not, returning to step S512.

Images