Unmanned aerial vehicle path planning method and system based on adaptive Hybrid A* algorithm

Abstract

The invention discloses an unmanned aerial vehicle path planning method and system based on an adaptive Hybrid A * algorithm, and the method comprises the steps: S1, initializing system parameters: constructing an environment three-dimensional occupation grid map, determining an initial state, and setting a target position of path planning; s2, generating an initial path of the unmanned aerial vehicle by using a self-adaptive Hybrid A * algorithm, inputting an environment three-dimensional occupied grid map, an initial state and a target position of path planning by using the self-adaptive Hybrid A * algorithm, adaptively determining the sampling duration of an expansion process according to the number of collision-free child nodes of a father node, and outputting a safe collision-free path conforming to the dynamics of the unmanned aerial vehicle; and S3, fusing the polynomial curve and the Floyd algorithm to smooth the initial path. According to the invention, a self-adaptive sampling duration strategy for subsequent node expansion is designed, so that the improved Hybrid A * algorithm not only can improve the node expansion efficiency in an open scene, but also can meet the obstacle avoidance requirement in a complex environment. Meanwhile, a path smoothing module is designed by fusing a polynomial curve and a Floyd algorithm, so that the smoothness of a planned path can be effectively improved, and the acceleration input continuity of the unmanned aerial vehicle is enhanced.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot path planning, and particularly relates to a UAV path planning method based on an adaptive Hybrid A* algorithm. Background Art

[0002] In recent years, the key technologies such as perception, positioning, planning, and control of intelligent systems have developed rapidly, and UAVs are widely used in the fields of logistics transportation, post-disaster rescue, security inspection, etc. due to their vertical takeoff and landing and fixed-point hovering functions. As the core algorithm of UAV autonomous navigation, path planning inputs the surrounding environment information and its own pose estimation, calculates the collision-free optimal path connecting the starting point and the target point, and outputs it to the control module for tracking. The planning process needs to comprehensively consider the initial and end boundary constraints, dynamic constraints, obstacle constraints, etc. How to quickly plan a safe and smooth trajectory that meets specific constraint conditions in a complex scenario is a difficult problem that the UAV industry urgently needs to solve.

[0003] The mainstream path planning methods are divided into search-based algorithms, sampling-based algorithms, and optimization-based algorithms. The sampling-based algorithm constructs a feasible road network through free space sampling, and then uses the Dijkstra algorithm to complete the optimal path search. However, the algorithm consistency is poor and it is difficult to handle narrow channels. The optimization-based algorithm transforms the path planning problem into an optimal control problem, and obtains the UAV trajectory by solving the maximum and minimum values of the objective function that meets the constraint conditions. The quality of the trajectory solved by the algorithm is relatively high, but the equivalent processing of obstacles is difficult and the optimization process depends on a good initial value input. The search-based algorithms are divided into two steps: environmental map construction and path search, mainly including the A* algorithm and the Dijkstra algorithm, which can realize the shortest path search of the grid map. However, the path has many turns and is not conducive to tracking control. The Hybrid A* algorithm integrates the non-holonomic constraints of the robot on the basis of the traditional A* algorithm, and can plan a continuous feasible path that meets the initial and end pose conditions. However, the collision detection module has a long operation time, the expansion efficiency in the open space scene is low, and the final path may have unnecessary input mutations.

[0004] The Chinese patent application with the publication number CN115755908A discloses a mobile robot path planning method based on JPS-guided Hybrid A*. On the basis of the traditional Hybrid A* method, the JPS algorithm is used to replace the A* algorithm for calculating the h collision_avoidancel heuristic value of the two-dimensional grid, and a deviation from the guiding path penalty term is added to the actual cost. This method can improve the node traversal speed and shorten the path length to a certain extent, but there are still unnecessary turning actions in the final path, and the tracking burden on the control module is relatively heavy.

[0005] A Chinese patent application with the publication number CN119024859A proposes a UAV path planning method based on a multi-strategy improved ant colony algorithm, including introducing an angle function into the state transition probability, adopting an inward expansion and concentration limit strategy for pheromone update, and adaptively dynamically adjusting the evaporation factor. This method can effectively alleviate the problems of complex parameter adjustment and easy entrapment in local optima of the traditional ant colony algorithm. However, it has many path turning times and a slow algorithm convergence speed, making it difficult to meet the real-time requirements of the system. Summary of the Invention

[0006] The technical problem to be solved by the present invention is how to shorten the algorithm convergence time while robustly planning a smooth path for the UAV.

[0007] The present invention solves the above technical problems through the following technical means: A UAV path planning method based on an adaptive Hybrid A* algorithm, including:

[0008] S1: Initialize system parameters, including: constructing a three-dimensional occupancy grid map of the environment, determining the starting state, and setting the target position for path planning;

[0009] S2: Use the adaptive Hybrid A* algorithm to generate the initial path of the UAV. The adaptive Hybrid A* algorithm inputs the three-dimensional occupancy grid map of the environment, the starting state, and the target position for path planning, adaptively determines the sampling duration of the expansion process according to the number of collision-free child nodes of the parent node, and outputs a safe collision-free path that conforms to the dynamics of the UAV;

[0010] S3: Smooth the initial path by fusing polynomial curves and the Floyd algorithm.

[0011] As a further optimized technical solution, step S1 includes:

[0012] Global environment map construction: Construct a three-dimensional occupancy grid map of the environment through PCD point cloud file conversion or direct loading, and determine the origin position and attitude of the map coordinate system;

[0013] Starting pose acquisition: Determine the starting state of UAV path planning based on a vision positioning scheme or a laser positioning scheme. The starting state includes the starting position and the starting speed;

[0014] Target position setting: Set the target position for UAV path planning through ROS terminal commands, Rviz, or Launch file methods.

[0015] As a further optimized technical solution, step S2 includes:

[0016] Step 21 initializes the priority queue Open list and Close list, and pushes the starting state into the Open list;

[0017] In step 22, it is judged whether the Open list is empty. If it is empty, the current path search fails; otherwise, go to step 23.

[0018] In step 23, read the node n with the smallest total cost value f in the Open list, and move this node from the Open list to the Close list.

[0019] In step 24, it is judged whether the current node n is the target node. If so, go to step 28; otherwise, continue to execute step 25.

[0020] In step 25, sample the acceleration control quantity, determine the adaptive sampling duration, and generate the set of successor node states.

[0021] In step 26, traverse the set of successor nodes of node n: eliminate the invalid nodes, update the states of the feasible successor nodes and add them to the Open list.

[0022] In step 27, according to the collision situation of the successor nodes, update the number of collision-free successor nodes of the current node n, and return to step 22.

[0023] In step 28, node n continuously traces back to the starting node, and after reversing the path, the initial path between the start and end states of the UAV is obtained, and the program ends.

[0024] As a further optimized technical solution, step S25 specifically includes:

[0025] Generate a set of successor nodes with the current node n as the parent node. First, sample the acceleration control quantity of the UAV. Assume that the value range of the acceleration in a certain dimension is [-a max , a max , and perform 2k + 1 equal-segment sampling on this dimension to obtain the acceleration at each sampling point. Then, there are a total of (2k + 1) 3 kinds of UAV acceleration sampling combinations in the x, y, and z dimensions;

[0026] Then, determine the adaptive sampling duration of the expansion process according to the number of collision-free child nodes of the parent node of node n. If the number of collision-free child nodes of the parent node of node n is m, then the adaptive sampling duration t s of its successor node expansion is:

[0027]

[0028] Among them, t f represents the fixed sampling duration. It is easy to know that the more open the environment, the larger the adaptive sampling duration and the faster the node expansion speed;

[0029] Next, according to the adaptive sampling duration t sAnd the acceleration sampling combination generates the successor node state set of the current node n: the state s of node n n Is a 6×1 column vector composed of three-dimensional position and three-dimensional velocity

[0030] [p nx , p ny , p nz , v nx , v ny , v nz , the current acceleration sampling input c n Is a 3×1 column vector [a nx , a ny , a nz , I 3 Represents a 3×3 identity matrix, then the current successor node state s s The column vector [p sx , p sy , p sz , v sx , v sy , v sz is:

[0031]

[0032] As a further optimized technical solution, step 26 specifically includes: traversing the successor node state set of the current node n: if the successor node is in the Close list, in the same grid as the parent node, the node velocity is greater than the maximum velocity limit, or the successor path collides with an obstacle, it is an invalid node and this invalid node is skipped; otherwise, it is a feasible successor node, and the actual cost g s And the heuristic cost h s of this feasible successor node are calculated. If the feasible successor node is not in the Open list, this feasible successor node is pushed into the Open list; if the feasible successor node is already in the Open list and the newly calculated actual cost g value of the feasible successor node is smaller, then the original node in the Open list is replaced with the current feasible successor node. Among them, the actual cost g s value of the feasible successor node is:

[0033] g s = g n + l n_s + p a_change

[0034] g n represents the actual cost value of the parent node n of the current feasible successor node, l n_s represents the path length from the parent node to this feasible successor node, p a_change penalizes the change in the acceleration input amount between the parent and child nodes,

[0035]

[0036] The above formula is the heuristic cost h of the feasible successor node s Calculation formula, β represents the heuristic cost weight coefficient, h obstacle is the length of the JPS path connecting the current feasible successor node and the target node considering the distribution of environmental obstacles, h smooth (t h ) is the length of the quintic polynomial curve that ignores the obstacle avoidance requirement but satisfies the position, speed, and acceleration boundary constraints of the feasible successor node and the target node. The time interval t between the start and end points of the curve h is the straight-line distance l between two points s_g and half of the maximum speed v of the UAV max ratio.

[0037] As a further optimized technical solution, step S3 includes:

[0038] Step 31 sequentially extracts and stores all the nodes between the start node and the target node of the Hybrid A* initial path as the vertex set of the Floyd smoothing algorithm. The starting point of the curve is initialized as the first element in the vertex set, and the ending point of the curve is assigned as the last element in the vertex set;

[0039] Step 32 If the index of the starting point of the curve in the vertex set is less than the index of the ending point of the curve in the vertex set, then determine whether the polynomial path between the starting and ending points of the curve satisfies the obstacle avoidance, speed, and acceleration dynamics constraints. If it satisfies, remove the redundant vertices between the starting and ending points of the curve in the vertex set and go to step 33 to continue execution; if it does not satisfy, subtract 1 from the index of the ending point of the curve and continue to execute this step 32; if the index of the starting point of the curve in the vertex set is greater than or equal to the index of the ending point of the curve in the vertex set, also execute step 33;

[0040] Step 33 If the starting point of the curve does not point to the last element of the updated vertex set, then add 1 to the index of the starting point of the curve in the vertex set, the ending point of the curve points to the last element of the updated vertex set, and go to step 32 to continue execution; if the starting point of the curve points to the last element of the updated vertex set, then the path smoothing process ends, and the collision-free safe path after smoothing is output.

[0041] As a further optimized technical solution, in step S32, the polynomial trajectory between the starting and ending points of the curve is solved by minimizing the cost function J, aiming to ensure the optimal energy input and time consumption during the operation of the UAV:

[0042]

[0043] a(t) represents the acceleration input, and ρ is the weight coefficient of the time term of the cost function. If the starting point position of the curve is p uc , the starting point speed is v uc; The end position of the curve is p ug , and the end velocity is v ug , then the analytical expression of the polynomial trajectory is:

[0044]

[0045] The present invention also discloses a UAV path planning system based on the adaptive Hybrid A* algorithm corresponding to the method of any of the above solutions, including:

[0046] A system parameter initialization module, which is used to construct a three-dimensional occupancy grid map of the environment, determine the starting state, and set the target position of path planning;

[0047] An adaptive Hybrid A* algorithm module, which is used to generate the initial path of the UAV. The adaptive Hybrid A* algorithm inputs the three-dimensional occupancy grid map of the environment, the starting state, and the target position of path planning, adaptively determines the sampling duration of the expansion process according to the number of collision-free child nodes of the parent node, and outputs a safe collision-free path that conforms to the dynamics of the UAV;

[0048] A smooth initial path module, which is used to fuse the polynomial curve and the Floyd algorithm to smooth the initial path.

[0049] As a further optimized technical solution, the adaptive Hybrid A* algorithm module includes:

[0050] An initialization list unit, which is used to initialize the priority queue Open list and Close list, and push the starting state into the Open list;

[0051] A first judgment unit, which is used to judge whether the Open list is empty. If it is empty, the current path search fails; otherwise, it enters the reading unit;

[0052] A reading unit, which is used to read the node n with the smallest total cost value f in the Open list, and move this node from the Open list to the Close list;

[0053] A second judgment unit, which is used to judge whether the current node n is the target node. If it is, it turns to the backtracking unit; otherwise, it continues to execute the sampling unit;

[0054] A sampling unit, which is used to sample the acceleration control quantity, determine the adaptive sampling duration, and generate a set of successor node states;

[0055] A first update unit, which is used to traverse the set of successor nodes of node n: eliminate invalid nodes, update and add the feasible successor node states to the Open list;

[0056] A second update unit, configured to update the number of collision-free successor nodes of the current node n according to the collision situation of the successor nodes, and return to the first judgment unit;

[0057] A backtracking unit, configured to continuously backtrack the node n to the starting node, and after the path is reversed, obtain the initial path between the start and end states of the UAV, and the program ends.

[0058] As a further optimized technical solution, the smooth initial path module includes:

[0059] An extraction unit, configured to sequentially extract and store all nodes between the starting node and the target node of the Hybrid A* initial path as the vertex set of the Floyd smoothing algorithm, initialize the starting point of the curve as the first element in the vertex set, and assign the end point of the curve as the last element in the vertex set;

[0060] A comparison unit, configured to perform the following operations: if the index of the starting point of the curve in the vertex set is less than the index of the end point of the curve in the vertex set, determine whether the polynomial path between the starting and ending points of the curve meets the obstacle avoidance, speed, and acceleration dynamics constraints. If it meets, remove the redundant vertices between the starting and ending points of the curve in the vertex set, and go to the output unit to continue execution; if it does not meet, subtract 1 from the index of the end point of the curve and continue to execute this unit; if the index of the starting point of the curve in the vertex set is greater than or equal to the index of the end point of the curve in the vertex set, also execute the output unit;

[0061] An output unit, configured to perform the following operations: if the starting point of the curve does not point to the last element of the updated vertex set, add 1 to the index of the starting point of the curve in the vertex set, the end point of the curve points to the last element of the updated vertex set, and go to the comparison unit to continue execution; if the starting point of the curve points to the last element of the updated vertex set, the path smoothing process ends, and the collision-free safe path after smoothing is output.

[0062] The advantages of the present invention are as follows:

[0063] 1. The present invention proposes an adaptive Hybrid A* algorithm that dynamically determines the sampling duration according to the number of collision-free child nodes of the parent node, significantly improves the node expansion efficiency, shortens the algorithm convergence time, and provides an important guarantee for the stable operation of the path smoothing module.

[0064] 2. The present invention designs a path smoothing scheme based on the Floyd algorithm and polynomial curves, effectively alleviates the problem of sudden acceleration input of the initial path, improves the continuity and smoothness of the output path, and enhances the stability of the path tracking of the control module. Description of the Drawings

[0065] Figure 1 It is a schematic flowchart of the UAV path planning method based on the adaptive Hybrid A* algorithm in the embodiment of the present invention;

[0066] Figure 2 is the flowchart of the UAV initial path generation algorithm based on the adaptive Hybrid A* in the embodiments of the present invention;

[0067] Figure 3 is the schematic diagram of the collision situation of the successor nodes in the embodiments of the present invention;

[0068] Figure 4 is the schematic diagram of the UAV initial path generation based on the adaptive Hybrid A* algorithm in the embodiments of the present invention;

[0069] Figure 5 is the schematic diagram of path smoothing integrating polynomial curve and Floyd algorithm in the embodiments of the present invention. Specific Embodiments

[0070] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0071] The present invention proposes a UAV path planning method based on the adaptive Hybrid A* algorithm, which mainly includes program initialization, initial path generation, and smooth path steps, and can realize the rapid planning of high-quality smooth trajectories. The schematic diagram of the method flow is shown in Figure 1 , and specifically includes the following steps:

[0072] S1: Initialize system parameters such as the map, positioning, and target position, specifically including:

[0073] 11 Global environment map construction: Construct a three-dimensional occupancy grid map of the environment by converting PCD point cloud files or directly loading, etc., and determine the origin position and attitude of the map coordinate system.

[0074] 12 Starting pose acquisition: Determine the starting state of the UAV path planning based on visual positioning schemes such as VINS-Fusion and ORB-SLAM, or laser positioning schemes such as FAST-LIO and LIO-SAM. The starting state includes the starting position (p ix , p iy , p iz ) and the starting speed (v ix , v iy , v iz ).

[0075] 13 Target position setting: Set the target position for the UAV path planning through ROS terminal commands, Rviz, or Launch files (p ex ,p ey ,p ez ).

[0076] S2: Generate the initial UAV path using the adaptive Hybrid A* algorithm

[0077] The adaptive Hybrid A* algorithm takes as input the three-dimensional occupancy grid map of the environment, the starting state, and the target position for the path planning (p ex ,p ey ,p ez ), adaptively determines the sampling duration during the expansion process according to the number of collision-free child nodes of the parent node, and can quickly output a safe collision-free path that conforms to the UAV dynamics. The schematic diagram of its process is shown in Figure 2 , and specifically includes the following steps:

[0078] 21 Initialize the priority queue Open list and Close list. A large number of nodes will be generated during the UAV path planning process using hybrid A*. The node states include: unvisited, visited but waiting to be expanded, and expanded nodes (must have been visited). When the system is initialized, all nodes are in the "unvisited state". The Open list is used to store the nodes that have been visited but are waiting to be expanded, and the Close list stores the information of the expanded nodes, and the starting state is pushed into the Open list.

[0079] 22 Judge whether the Open list is empty. If it is empty, it means that there is no safe initial path connecting the start and end states, and the current path search fails; otherwise, go to step 23;

[0080] 23 Read the node n with the smallest total cost f in the Open list, and move this node from the Open list to the Close list. The calculation formula for the total cost f of the node is:

[0081] f(n) = g(n) + h(n)

[0082] where g represents the actual cost from the starting node to the current node, and h is the heuristic cost from the current node to the target node.

[0083] 24 Judge whether the current node n is the target node. If it is, go to step 28; otherwise, continue to execute step 25.

[0084] 25 Sample the acceleration control quantity, determine the adaptive sampling duration, and generate the set of successor node states, specifically including:

[0085] Generate a set of successor nodes with the current node n as the parent node. First, sample the acceleration control quantity of the UAV. Assume that the value range of the acceleration in a certain dimension is [-a max , a max . Perform 2k + 1 equal-segment sampling on this dimension to obtain the acceleration at each sampling point Then, there are (2k + 1) 3 kinds of UAV acceleration sampling combinations in the three dimensions of x, y, and z.

[0086] Then, determine the adaptive sampling duration of the expansion process according to the number of collision-free child nodes of the parent node of node n. Figure 3 It can be seen that when the local environment obstacles are sparse, the number of collision-free successor nodes is relatively large, while when the surrounding environment obstacles are dense, the number of collision-free child nodes is relatively small. That is, the number of collision-free successor nodes reflects the obstacle distribution in the local environment where the parent node is located. If the number of collision-free child nodes of the parent node of node n is m, then the adaptive sampling duration t s of its successor node expansion is:

[0087]

[0088] where t f represents the fixed sampling duration. It is easy to know that the more open the environment, the larger the adaptive sampling duration and the faster the node expansion speed. Among them, the selection of the parameters 0.5 and 1.5 is only to make the adaptive sampling duration t s linearly vary within the range of 0.5 times t f and 2 times t f according to the obstacle distribution, that is, the adaptive sampling duration t s is 0.5 times t f when the obstacles are very dense, and 2 times t f in a relatively open scene. Those of ordinary skill in the art can know that the range of change of the adaptive sampling duration (i.e., the multiple of the fixed sampling duration t f ) or the function (linear function, quadratic function, etc.) can be selected according to their own needs.

[0089] Next, generate the set of successor node states of the current node n according to the adaptive sampling duration t s and the acceleration sampling combination: The state s n of node n is a 6*1 column vector composed of three-dimensional position and three-dimensional velocity

[0090] [p nx , p ny , p nz , v nx , v ny , v nz , and the current acceleration sampling input c n is a 3*1 column vector [a nx, a ny , a nz , I 3 represents the 3*3 identity matrix, then the current successor node state s s column vector [p sx , p sy , p sz , v sx , v sy , v sz is:

[0091]

[0092] 26 Traverse the successor node set of node n: Eliminate invalid nodes, update the feasible successor node states and add them to the Open list, specifically including:

[0093] Traverse the successor node state set of the current node n: If the successor node is in the Close list, in the same grid as the parent node, the node speed is greater than the maximum speed limit, or the successor path collides with an obstacle, it is an invalid node and skip this invalid node. Otherwise, it is a feasible successor node, calculate the actual cost g s and the heuristic cost h s , if the feasible successor node is not in the Open list, push this feasible successor node into the Open list; if the feasible successor node is already in the Open list and the newly calculated actual cost g value of the feasible successor node is smaller, then replace the original node in the Open list with the current feasible successor node. Among them, the actual cost g s value is:

[0094] g s = g n + l n_s + p a_change

[0095] g n represents the actual cost value of the parent node n of the current feasible successor node, l n_s represents the path length from the parent node to this feasible successor node, p a_change penalizes the change in the acceleration input between the parent and child nodes.

[0096]

[0097] The above formula is the calculation formula for the heuristic cost h s of the feasible successor node, β represents the heuristic cost weight coefficient, h obstacle is the JPS path length connecting the current feasible successor node and the target node considering the distribution of environmental obstacles. h smooth (t h)The length of the fifth - order polynomial curve that ignores the obstacle - avoidance requirement but satisfies the boundary constraints of the positions, velocities, and accelerations of the feasible successor node and the target node, and the time interval \(t\) between the start and end points of the curve h is the straight - line distance \(l\) between two points s_g and half of the maximum speed \(v\) of the UAV max ratio.

[0098] 27 Update the number of collision - free successor nodes of the current node \(n\) according to the collision situation of the successor nodes, and return to step 22.

[0099] 28 The node \(n\) continuously backtracks to the starting node, and after reversing the path, the initial path between the start and end states of the UAV is obtained, and the program ends.

[0100] S3 Smooth the initial path by integrating the polynomial curve and the Floyd algorithm

[0101] The adaptive Hybrid A* algorithm can plan a safe initial path that conforms to the dynamics of the UAV at a high frequency, but there are unnecessary mutations in its acceleration input, and the overall smoothness and continuity of the path are insufficient, as Figure 4 shown. Based on the Floyd algorithm, the polynomial curve is used to perform the smoothing operation of the initial path, and a high - order continuous and smooth path is output for the control module to accurately track. The schematic diagram is shown in Figure 5 .

[0102] 31 Extract and store all the nodes between the starting node and the target node of the Hybrid A* initial path in sequence as the vertex set of the Floyd smoothing algorithm. The starting point of the curve is initialized as the first element in the vertex set, that is Figure 5 vertex 1 in; the end point of the curve is assigned as the last element in the vertex set, that is Figure 5 vertex 7 in.

[0103] 32 If the index of the starting point of the curve in the vertex set is less than the index of the end point of the curve in the vertex set, then judge whether the polynomial path between the start and end points of the curve satisfies the obstacle - avoidance, speed, and acceleration dynamics constraints. If it satisfies, remove the redundant vertices between the start and end points of the curve in the vertex set, and go to step 33 to continue execution; if it does not satisfy, then subtract 1 from the index of the end point of the curve and continue to execute this step 32. If the index of the starting point of the curve in the vertex set is greater than or equal to the index of the end point of the curve in the vertex set, step 33 is also executed.

[0104] Among them, the polynomial trajectory between the start and end points of the curve is solved by minimizing the cost function \(J\), aiming to ensure the optimal energy input and time consumption during the operation of the UAV:

[0105]

[0106] \(a(t)\) represents the acceleration input, and \(\rho\) is the weight coefficient of the time term of the cost function. If the starting position of the curve is \(p\)uc , the starting speed is v uc ; the end position of the curve is p ug , the end speed is v ug , then the polynomial trajectory analysis expression is:

[0107]

[0108] 33 If the starting point of the curve does not point to the tail element of the updated vertex set, then the index of the starting point of the curve in the vertex set is incremented by 1, the end point of the curve points to the tail element of the updated vertex set, and go to step 32 to continue execution. If the starting point of the curve points to the tail element of the updated vertex set, the path smoothing process ends, and the collision-free safe path after smoothing is output.

[0109] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A UAV path planning method based on an adaptive Hybrid A* algorithm, characterized by: include: S1: System parameter initialization, including: building a three-dimensional occupancy grid map of the environment, determining the starting state, and setting the target position for path planning; S2: Adaptive Hybrid A* algorithm is used to generate the initial path of the UAV. The adaptive Hybrid A* algorithm inputs the three-dimensional occupancy grid map of the environment, the starting state, and the target position of the path planning. The sampling time of the expansion process is adaptively determined according to the number of collision-free child nodes of the parent node, and the output is a safe collision-free path that conforms to the dynamics of the UAV. S3 combines polynomial curves and Floyd's algorithm to smooth the initial path.

2. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 1, characterized in that: Step S1 includes: Global environment map construction: Construct the environment 3D occupancy grid map by converting or directly loading PCD point cloud files, and determine the origin position and posture of the map coordinate system; Initial posture acquisition: Determine the initial state of the drone path planning based on the visual positioning solution or the laser positioning solution. The initial state includes the initial position and initial speed. Target position setting: Set the target position of the drone path planning through ROS terminal commands, Rviz or Launch files.

3. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 1, characterized in that: Step S2 includes: Step 21 initializes the Open list and Close list of the priority queue, and pushes the start state into the Open list; Step 22 determines whether the Open list is empty. If it is empty, the path search fails. Otherwise, go to step 23. Step 23 reads the node n with the smallest total cost value f in the Open list, and moves the node from the Open list to the Close list; Step 24 determines whether the current node n is the target node, if so, go to step 28; otherwise, continue to step 25; Step 25: sampling the acceleration control amount, determining the adaptive sampling duration, and generating a set of successor node states; Step 26 traverses the set of successor nodes of node n: removes invalid nodes, updates the status of feasible successor nodes and adds them to the Open list; Step 27 updates the number of collision-free successor nodes of the current node n according to the collision status of the successor nodes, and returns to step 22; Step 28: Node n continuously traces back to the starting node, and after the path is reversed, the initial path between the start and end states of the drone is obtained, and the program ends.

4. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 3, characterized in that: Step S25 specifically includes: Take the current node n as the parent node to generate the set of successor nodes. First, sample the acceleration control value of the drone. Assume that the value range of the acceleration of a certain dimension is [-a max ,a max ], perform 2k+1 equal sampling on this dimension to obtain the acceleration of each sampling point Then the three dimensions xyz exist cumulatively (2k+1) 3 A combination of UAV acceleration sampling; Then, the adaptive sampling duration of the expansion process is determined according to the number of collision-free child nodes of the parent node of node n. If the number of collision-free child nodes of the parent node of node n is m, the adaptive sampling duration of the expansion of its successor node is t s for: Among them, t f Indicates the fixed sampling duration. It is easy to know that the more spacious the environment is, the longer the adaptive sampling duration is, and the faster the node expansion speed is; Then, according to the adaptive sampling time t s and acceleration sampling to generate the state set of successor nodes of the current node n: node n state s n is a 6*1 column vector consisting of three-dimensional position and three-dimensional velocity [p nx ,p ny ,p nz ,v nx ,v ny ,v nz ], current acceleration sampling input c n is a 3*1 column vector [a nx ,a ny ,a nz ], I3 represents the 3*3 identity matrix, then the current successor node state s s Column vector [p sx ,p sy ,p sz ,v sx ,v sy ,v sz ]for:

5. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 1, characterized in that: Step 26 specifically includes: traversing the state set of successor nodes of the current node n: if the successor node is in the Close list, is in the same grid as the parent node, the node speed is greater than the maximum speed limit, or the successor path collides with an obstacle, it is an invalid node and the invalid node is skipped; otherwise, it is a feasible successor node and the actual cost g of the feasible successor node is calculated. s and the heuristic cost h s If the successor node is not in the Open list, the node is pushed into the Open list; if the feasible successor node is already in the Open list, and the actual cost g of the newly calculated feasible successor node is smaller, the original node in the Open list is replaced with the current feasible successor node, where the actual cost g of the feasible successor node is s The values ​​are: g s =g n +l n_s +p a_change g n Indicates the actual cost value of the parent node n of the current feasible successor node, l n_s represents the path length from the parent node to the feasible successor node, p a_change Penalize the change of acceleration input between parent and child nodes, The above formula is the heuristic cost h of the feasible successor node s Calculation formula, β represents the heuristic cost weight coefficient, h obstacle h is the JPS path length connecting the current feasible successor node and the target node under the distribution of environmental obstacles. smooth (t h ) is the length of the quintic polynomial curve that ignores the obstacle avoidance requirement but satisfies the position, velocity, and acceleration boundary constraints of the feasible successor node and the target node. The time interval t between the start and end points of the curve h is the straight-line distance between two points l s_g With the maximum speed of the drone v max Half the ratio.

6. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 1, characterized in that: Step S3 include: Step 31 extracts and stores all nodes from the starting node to the target node of the Hybrid A* initial path in sequence as the vertex set of the Floyd smoothing algorithm, the starting point of the curve is initialized to the first element in the vertex set, and the end point of the curve is assigned to the last element in the vertex set; In step 32, if the index of the starting point of the curve in the vertex set is less than the index of the end point of the curve in the vertex set, determine whether the polynomial path between the starting and ending points of the curve meets the obstacle avoidance, speed and acceleration dynamics constraints. If so, remove the redundant vertices between the starting and ending points of the curve in the vertex set, and go to step 33 to continue execution; if not, reduce the index of the end point of the curve by 1, and continue to execute this step 32; if the index of the starting point of the curve in the vertex set is greater than or equal to the index of the end point of the curve in the vertex set, also execute step 33; In step 33, if the starting point of the curve does not point to the last element of the updated vertex set, the index of the starting point of the curve in the vertex set is increased by 1, and the end point of the curve points to the last element of the updated vertex set, and the process goes to step 32 to continue execution; if the starting point of the curve points to the last element of the updated vertex set, the path smoothing process ends, and the smoothed collision-free safe path is output.

7. The UAV path planning method based on the adaptive Hybrid A* algorithm as claimed in claim 1, characterized in that: In step S32, the polynomial trajectory between the start and end points of the curve is solved to minimize the cost function J, aiming to ensure the optimal energy input and time consumption during the operation of the drone: a(t) represents the acceleration input, and ρ is the weight coefficient of the time term of the cost function. If the starting position of the curve is p uc , the starting speed is v uc ; The end point of the curve is p ug , the terminal velocity is v ug , then the polynomial trajectory analytical expression is:

8. A UAV path planning system based on an adaptive Hybrid A* algorithm, characterized by: include: System parameter initialization module, used to build a three-dimensional occupancy grid map of the environment, determine the starting state, and set the target position for path planning; Adaptive Hybrid A* algorithm module, used for generating the initial path of the UAV. The adaptive Hybrid A* algorithm inputs the three-dimensional occupancy grid map of the environment, the starting state, and the target position of the path planning. It adaptively determines the sampling time of the expansion process according to the number of collision-free child nodes of the parent node, and outputs a safe collision-free path that conforms to the dynamics of the UAV. The smooth initial path module is used to fuse the polynomial curve and the Floyd algorithm to smooth the initial path.

9. The UAV path planning system based on the adaptive Hybrid A* algorithm as claimed in claim 8, characterized in that: The adaptive Hybrid A* algorithm module includes: Initialize the list unit, which is used to initialize the Open list and Close list of the priority queue, and push the starting state into the Open list; The first judgment unit is used to judge whether the Open list is empty. If it is empty, the path search fails. Otherwise, it enters the reading unit. A reading unit is used to read the node n with the smallest total cost value f in the Open list, and move the node from the Open list to the Close list; The second judgment unit is used to judge whether the current node n is the target node, and if so, it turns to the backtracking unit; otherwise, it continues to execute the sampling unit; A sampling unit, used to sample the acceleration control amount, determine the adaptive sampling duration, and generate a set of successor node states; The first updating unit is used to traverse the set of successor nodes of node n: remove invalid nodes, update the status of feasible successor nodes and add them to the Open list; A second updating unit, used for updating the number of collision-free successor nodes of the current node n according to the collision status of the successor nodes, and returning to the first judging unit; The backtracking unit is used for node n to continuously trace back to the starting node. After the path is reversed, the initial path between the initial and final states of the drone is obtained, and the program ends.

10. The UAV path planning system based on the adaptive Hybrid A* algorithm as claimed in claim 8, characterized in that: The smooth initial path module includes: An extraction unit is used to sequentially extract and store all nodes between the starting node and the target node of the Hybrid A* initial path as the vertex set of the Floyd smoothing algorithm. The starting point of the curve is initialized as the first element in the vertex set, and the end point of the curve is assigned to the last element in the vertex set. The comparison unit is used to perform the following operations: if the index of the starting point of the curve in the vertex set is less than the index of the end point of the curve in the vertex set, then determine whether the polynomial path between the starting and ending points of the curve meets the obstacle avoidance, speed and acceleration dynamics constraints. If so, remove the redundant vertices between the starting and ending points of the curve in the vertex set, and transfer to the output unit for further execution; if not, reduce the index of the end point of the curve by 1, and continue to execute this unit; if the index of the starting point of the curve in the vertex set is greater than or equal to the index of the end point of the curve in the vertex set, the output unit is also executed; The output unit is used to perform the following operations: if the starting point of the curve does not point to the last element of the updated vertex set, the index of the starting point of the curve in the vertex set is increased by 1, and the end point of the curve points to the last element of the updated vertex set, and the comparison unit is turned to continue execution; if the starting point of the curve points to the last element of the updated vertex set, the path smoothing process ends, and the smoothed collision-free safe path is output.

Images