A-satellite unmanned aerial vehicle path planning method and system based on adaptive weight and layering

By adopting adaptive weighting and layered A-star algorithm in drone path planning, the problem of low path planning efficiency in complex indoor environments is solved, and a more efficient and safer drone flight path is achieved.

CN118938962APending Publication Date: 2024-11-12WUHAN UNIV
View PDF 0 Cites 3 Cited by

Patent Information

Application Number
CN202411173540.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-26
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

In complex indoor environments, UAV path planning technology has problems such as high computational complexity, slow search speed and low path planning efficiency. Especially in the absence of GPS, it is difficult to achieve efficient obstacle avoidance and real-time path adjustment.

Method used

Adopting the A-star path planning method based on adaptive weights and hierarchy, obstacle map is constructed through lidar point cloud data, obstacle probability is processed layer by layer, and path planning is used with the A-star algorithm with adaptive weights, and cost function is dynamically adjusted To improve path quality and efficiency.

Benefits of technology

It effectively reduces the computational complexity, improves search efficiency, generates better quality and smoother paths, and improves the flight safety of drones in complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938962A_ABST
    Figure CN118938962A_ABST
Patent Text Reader

Abstract

The invention discloses an A-satellite unmanned aerial vehicle path planning method and system based on adaptive weight and layering, and the method comprises the steps: carrying out the positioning of an unmanned aerial vehicle in a GPS-free environment, and obtaining the three-dimensional coordinates and attitude information of the unmanned aerial vehicle; establishing an obstacle map by using an octree algorithm based on the laser radar point cloud data; layering the obstacle map according to the elevation information of the point cloud, and constructing sub-maps; and performing unmanned aerial vehicle path planning on the sub-map by using a hierarchical A star algorithm based on adaptive weight. According to the method provided by the invention, by layering the obstacle map, the search efficiency is effectively improved, and by dynamically adjusting the weight of the cost function, the search direction can be more flexibly changed, and unnecessary node expansion is avoided, so that a higher-quality and smoother path is generated.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a path planning method and system for an A-star unmanned aerial vehicle based on adaptive weight and stratification. Background Art

[0002] Unmanned Aerial Vehicle (UAV) technology has developed rapidly in recent years and has been widely used in many fields. According to different application scenarios, UAV technology can be roughly divided into two categories: military and civilian. Military UAVs are mainly used for reconnaissance, surveillance and attack missions, while civilian UAVs have shown great application potential in agriculture, logistics, transportation, land and resources management, environmental monitoring, disaster relief, film and television shooting and other fields. Compared with traditional ground robots, UAVs have the advantages of wide coverage, strong mobility, efficient detection and response, etc.

[0003] Path planning technology is a hot development direction in the current drone field, aiming to provide stable and reliable flight routes for drone flights. Obstacle avoidance path planning needs to consider seeking the optimal path or suboptimal path based on path length and energy consumption issues while ensuring safety, so as to ensure the efficiency and results of the task. There is no GPS in the indoor environment, and the environment is complex and changeable, with various types of obstacles and high density. Drones need to have efficient environmental perception capabilities and obstacle avoidance technology to ensure safe flight. At present, although some advanced drones are equipped with advanced obstacle avoidance systems, the reliability and effectiveness of these systems still need to be further improved in complex indoor environments.

[0004] At present, the path planning technology of UAV is mainly divided into two aspects: global path planning and local path planning. Global path planning refers to the technology of UAV path planning under the condition of understanding global environmental information. In this regard, the Dijkstra algorithm is a typical global shortest path search algorithm. Although it can find the best solution, its search speed is slow. The rapid search random tree (RRT) algorithm avoids spatial modeling and can generate a set of collision-free paths, thereby effectively solving the path planning problem of high-dimensional space and complex constraints. In addition, some intelligent algorithms such as genetic algorithms are often used to effectively find the global optimal solution. For example, an evolutionary search strategy based on genetic algorithms can find the optimal path on a dynamic grid. Local path planning refers to the technology of the intelligent agent perceiving and planning the local environment during movement to avoid collisions and ensure the stability and safety of the path. In the field of UAVs, local path planning is particularly important because UAVs may be affected by external factors such as dynamic obstacles and wind, and need to adjust the path in real time to adapt to environmental changes.

[0005] Global path planning usually requires calculating the optimal path from the starting point to the end point, which requires a lot of computing resources and time in large-scale environments, so it performs poorly in terms of real-time performance. Local path planning mainly relies on the current environmental information provided by sensors and lacks a global view of the overall environment, which may lead to the inability to find the global optimal path. Summary of the invention

[0006] In view of the problems existing in the prior art, the present invention proposes an A-star UAV path planning method based on adaptive weight and layering, which includes the following steps:

[0007] Step 1: Position the UAV in a GPS-free environment to obtain the UAV's three-dimensional coordinates and attitude information;

[0008] Step 2: Based on the lidar point cloud data, an obstacle map is established using the octree algorithm;

[0009] Step 3: Layer the obstacle map according to the elevation information of the point cloud, calculate the total obstacle probability of each layer in the obstacle map, select the layer with the smallest total obstacle probability and construct a sub-map together with the obstacle map layer where the drone and the set target point are located;

[0010] Step 4: Use the hierarchical A-star algorithm based on adaptive weights to perform UAV path planning on the sub-map obtained in step 3.

[0011] Furthermore, in step 1, the Cartographer pure positioning mode algorithm is used to locate the UAV in a GPS-free environment. First, environmental data is obtained from the lidar, IMU, and odometer, including point cloud data of the entire area obtained by the lidar, the attitude and acceleration of the UAV obtained by the IMU, and the flight distance of the UAV obtained by the odometer. The environmental data of multiple sensors are synchronized and denoised, and the initial posture of the UAV is estimated using the IMU data and the motion model; then, a map previously established for the environment is used as an occupancy grid map, the current lidar scanning data is matched with the occupancy grid map, the current posture of the UAV is accurately estimated, and the current accurate posture is added to the posture graph for global optimization to reduce cumulative errors; then, the occupancy grid map is updated according to the optimized posture, the closed loop is detected and the error is corrected, and the map accuracy is further improved; finally, the constructed map and the posture results of the UAV are published and stored.

[0012] Furthermore, in the step 2, the relevant parameters in the obstacle map are first defined, including the map origin, the actual map size, the map resolution, the map expansion distance and the occupancy map size, and then the point cloud data of the entire area obtained by the laser radar scanning is expanded according to the map expansion distance, and then the map is constructed using the octree; the specific process of using the octree to construct the obstacle map is as follows: ① Initialize the octree, define the boundary of the entire space, initialize the root node, and the root node is the highest level node of the octree; ② Divide the space of each node into eight subspaces of equal size, and create a child node for each subspace; ③ Continue to perform the same division as step ② on each child node until a predetermined number of subdivision layers is reached or the density of obstacles in the space reaches a set threshold; ④ Insert each point in the point cloud data into the corresponding leaf node in the octree, and mark the leaf node as an occupied node, an idle node or a partially occupied node according to application requirements. When the node space is not occupied by an obstacle, it is an idle node, when the node space is completely occupied by an obstacle, it is an occupied node, and when only part of the node space is occupied by an obstacle, it is a partially occupied node, and finally an obstacle map is formed.

[0013] Furthermore, in step 3, the obstacle map is layered according to the z coordinate of the point cloud. Assuming that the range of the z coordinate is N1 to N2, and the layering is performed at intervals of △h, the layer corresponding to the height N1 is 0, the layer corresponding to the height N1+△h is 1, and so on to obtain the layers corresponding to all heights; read the environmental data of the lidar, IMU and odometer and the obstacle map constructed in step 2, take the position of the drone obtained in step 1 as the starting point, manually set the position of the target point, obtain the obstacle map layer where the starting point and the target point are located according to the z coordinates of the starting point and the target point, and calculate the total obstacle probability of each layer in the obstacle map. The calculation formula is as follows:

[0014] P z =∑p(x p ,y q ,z) (1)

[0015] Where Pz represents the total obstacle probability of the zth layer; p(x p ,y q ,z) means that at the z layer (x p ,y q ) is there an obstacle. If there is an obstacle, the value is 1, otherwise the value is 0. p and q are the row and column numbers on the zth layer of the obstacle map.

[0016] Select the layer with the smallest total obstacle probability and construct a submap together with the layer where the starting point is located and the layer where the set target point is located.

[0017] Furthermore, the cost function expression of the adaptive weighted hierarchical A-star algorithm in step 4 is:

[0018] F(n)=g(n)+w×h(n) (2)

[0019]

[0020] h(n)=|x n -x target |+y n -y target |+|z n -z target | (5)

[0021] Where n is the current location, n≥2, that is, a point on the path, and the starting point is the first location; F(n) represents the total cost of location n; g(n) represents the actual cost from the starting point to location n; cost i represents the cost from position i-1 to position i; (x i-1 ,y i-1 ,z i-1 ) is the position of point i-1; (x i ,y i ,z i ) is the coordinate of position point i; h(n) represents the heuristic estimated cost from position point n to the target point; (x n ,y n ,z n ) is the coordinate of the position point n, (xtarget, ytarget, ztarget) is the coordinate of the target point; w is the adaptive weight, whose value is determined by the distance from the target point, and is calculated as follows:

[0022]

[0023] In the formula, is the Manhattan distance from the current location n to the target point, B m The length of the longest side of the submap.

[0024] Use the adaptive weight-based hierarchical A-star algorithm for drone path planning, define the OPEN list and CLOSED list, initialize them to empty lists, and then perform the following operations:

[0025] ① Add the starting point and its spatial neighboring points in the submap to the OPEN list. If its spatial neighboring points are obstacle points in the submap, they will not be added to the OPEN list. When the added spatial neighboring points are not in the layer where the initial point is located but in other layers, it is necessary to judge whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points will not be added to the OPEN list.

[0026] ② Calculate the total cost of each point in the OPEN list according to formulas (2)-(6), select the point with the minimum total cost as the parent node and add it to the CLOSED list, and add the spatial neighboring points of the parent node to the OPEN list. If its spatial neighboring points are obstacle points in the submap, they will not be added to the OPEN list. When the added spatial neighboring points are not in the layer where the parent node is located but in other layers, it is necessary to judge whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points will not be added to the OPEN list.

[0027] ③ When the spatial neighboring point of the parent node is already in the OPEN list, it is necessary to calculate the actual cost from the starting point to the parent node and then to the spatial neighboring point. If this actual cost is less than the actual cost from the starting point to the spatial neighboring point, the parent node is used as the starting point of the neighborhood spatial point;

[0028] ④ Check whether the target point is included in the OPEN list. If it is included, the search ends and the path from the starting point to the target point is reconstructed by backtracking through the parent node chain; otherwise, execute steps ②-④.

[0029] The present invention also provides an A-star UAV path planning system based on adaptive weights and layers, which is used to implement the A-star UAV path planning method based on adaptive weights and layers as described above.

[0030] Moreover, it includes a processor and a memory, the memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the A-star UAV path planning method based on adaptive weights and stratification as described above.

[0031] Alternatively, it includes a readable storage medium, on which a computer program is stored, and when the computer program is executed, the path planning method for an A-star UAV based on adaptive weights and stratification as described above is implemented.

[0032] Compared with the prior art, the present invention has the following advantages:

[0033] 1) The method proposed in the present invention divides a large-scale environment into multiple levels by layering the obstacle map, so that path search can be performed independently at each level, which effectively reduces the search space, reduces the computational complexity, and improves the search efficiency.

[0034] 2) The traditional A-star algorithm may lead to low path planning efficiency or poor path quality due to unreasonable cost function design. The method proposed in the present invention can more flexibly change the search direction and avoid unnecessary node expansion by dynamically adjusting the weight of the cost function (i.e., adaptive weight), thereby generating a better and smoother path.

[0035] 3) In complex indoor environments, obstacles are diverse and dense, which puts higher requirements on the path planning of drones. The method proposed in the present invention can improve the safety of drones during flight to a certain extent by layering the map and selecting the map layer with the least obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0037] Figure 1 The present invention is a flowchart of an A-star UAV path planning method based on adaptive weights and stratification according to an embodiment of the present invention.

[0038] Figure 2 The present invention is a flowchart of an embodiment of the present invention using the Cartographer algorithm to perform drone positioning.

[0039] Figure 3 A schematic diagram of constructing an obstacle map using an octree algorithm according to an embodiment of the present invention.

[0040] Figure 4 A flowchart of sub-map construction according to an embodiment of the present invention.

[0041] Figure 5 The figure is a schematic diagram of path planning using the method proposed in the present invention.

[0042] Figure 6 The following is a UAV planning path map generated by the method proposed in the present invention.

[0043] Figure 7 This is a performance comparison chart between the method proposed in the present invention and the traditional A-star algorithm. DETAILED DESCRIPTION

[0044] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in combination with the drawings and embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0045] Example 1

[0046] like Figure 1As shown, the embodiment of the present invention provides an A-star UAV path planning method based on adaptive weight and layering, comprising the following steps:

[0047] Step 1: Position the UAV in a GPS-free environment to obtain the UAV's three-dimensional coordinates and attitude information.

[0048] Cartographer’s pure positioning mode algorithm is used to locate drones in GPS-free environments. Cartographer is based on the idea of ​​graph optimization. It abstracts the poses of drones at different times into points (pose), and abstracts the constraints generated by observations of drones at different positions into edges between points, that is, constraints (constraint). The algorithm flow chart of Cartographer is as follows: Figure 2 As shown in the figure, the specific operations are as follows: first, environmental data is obtained from sensors such as lidar, IMU, and odometer, including point cloud data of the entire area obtained by lidar, the attitude and acceleration of the drone obtained by IMU, and the flight distance of the drone obtained by odometer. The environmental data of multiple sensors are synchronized and denoised, and the initial posture of the drone is estimated using IMU data and motion model; the map previously established for the environment is used as the occupancy grid map, the current lidar scanning data is matched with the occupancy grid map, the current posture of the drone is accurately estimated, and the current accurate posture is added to the posture graph for global optimization to reduce cumulative errors; then the occupancy grid map is updated according to the optimized posture, the closed loop is detected and the error is corrected to further improve the map accuracy; finally, the constructed map and the posture results of the drone are published and stored.

[0049] Step 2: Create an obstacle map.

[0050] First, define the relevant parameters in the obstacle map, including map origin, map actual size, map resolution, map expansion distance, and occupancy map size. In this embodiment, the occupancy map size is equal to the actual map size / map resolution. At the same time, define an occupancy container to query whether the map node is occupied. In order to ensure the safety of the drone during flight, the point cloud data of the entire area obtained by the laser radar scan is first expanded according to the map expansion distance, and then the map is constructed using the octree. Figure 3As shown, the specific process of using the octree to construct an obstacle map in this embodiment is as follows: ① Initialize the octree, define the boundaries of the entire space, and initialize the root node, which is the highest-level node of the octree. ② Divide the space of each node into eight subspaces of equal size, and create a child node for each subspace. ③ Continue to perform the same division as step ② on each child node until the predetermined number of subdivision levels is reached or the density of obstacles in the space reaches the set threshold. ④ Insert each point in the point cloud data into the corresponding leaf node in the octree, and mark the leaf node as an occupied node, an idle node, or a partially occupied node according to application requirements. When the node's space is not occupied by an obstacle, it is an idle node, when the node's space is completely occupied by an obstacle, it is an occupied node, and when only part of the node's space is occupied by an obstacle, it is a partially occupied node, and finally an obstacle map is formed. Figure 3 The squares in the middle represent leaf nodes, the transparent squares represent free nodes, and the black squares represent occupied nodes. The lower the level of the leaf node, the smaller the volume it occupies, and the higher the accuracy of the obstacle map created.

[0051] Step 3: Layer the obstacle map to obtain sub-maps.

[0052] The obstacle map is layered according to the z coordinate of the point cloud. Assuming that the range of the z coordinate is N1 to N2, the layers are layered at intervals of △h. The layer corresponding to height N1 is 0, the layer corresponding to height N1+△h is 1, and so on to obtain the layers corresponding to all heights. The sub-map construction method is as follows Figure 4 As shown in the figure, read the environmental data of the lidar, IMU and odometer and the obstacle map constructed in step 2, take the position of the drone obtained in step 1 as the starting point, manually set the position of the target point, and obtain the obstacle map level where the starting point and the target point are located according to the z coordinates of the starting point and the target point. Calculate the total obstacle probability of each layer in the obstacle map. The calculation formula is as follows:

[0053] P z =∑p(x p ,y q ,z) (1)

[0054] Where Pz represents the total obstacle probability of the zth layer; p(x p ,y q ,z) means that (x p ,y q ) is there an obstacle. If there is an obstacle, the value is 1, otherwise the value is 0. p and q are the row and column numbers on the zth layer of the obstacle map.

[0055] Select the layer with the smallest total obstacle probability and construct a submap together with the layer where the starting point is located and the layer where the set target point is located.

[0056] Step 4: Use the adaptive weight-based hierarchical A-star algorithm to plan the UAV path.

[0057] The traditional A-star algorithm has high requirements for the design of the cost function. If the cost function is not designed properly, it will lead to low path planning efficiency or poor path quality. Based on the submap obtained in step 3, the present invention uses the improved A-star algorithm to calculate the optimal path from the starting position to the target position. The cost function expression of the improved A-star algorithm is:

[0058] F(n)=g(n)+w×h(n) (2)

[0059]

[0060] h(n)=|x n -x target |+y n -y target |+|z n -z target | (5)

[0061] In the formula, n is the current location, that is, a point on the path; F(n) represents the total cost of location n; g(n) represents the actual cost from the starting point to location n; cost i represents the cost from position i-1 to position i; (x i-1 ,y i-1 ,z i-1 ) is the position of point i-1; (x i ,y i ,z i ) is the coordinate of position point i; h(n) represents the heuristic estimated cost from position point n to the target point; (x n ,y n ,z n ) is the coordinate of the position point n, (x target ,y target ,z target ) is the coordinate of the target point; w is the adaptive weight, whose value is determined by the distance from the target point and is calculated as follows:

[0062]

[0063] In the formula, is the Manhattan distance from the current location n to the target point, B m The length of the longest side of the submap.

[0064] By adjusting the weight w of the cost function in real time based on the distance, the search direction can be changed more flexibly during the search process, avoiding unnecessary node expansion, thereby improving the search efficiency.

[0065] Use the adaptive weight-based hierarchical A-star algorithm for drone path planning, define the OPEN list and CLOSED list, initialize them to empty lists, and then perform the following operations:

[0066] ① Add the starting point and its spatial neighboring points in the submap to the OPEN list. If its spatial neighboring points are obstacle points in the submap, they will not be added to the OPEN list. When the added spatial neighboring points are not in the layer where the initial point is located but in other layers, it is necessary to determine whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points will not be added to the OPEN list. Figure 5 The green dot is the starting point, the yellow dot is the target point, and the brown dot is the obstacle point. The spatial neighborhood points of the starting point in the submap are points 1-7 marked in the figure. Points 1-7 are not obstacle points in the submap and there are no obstacles between layers, so points 1-7 are added to the OPEN list.

[0067] ② Calculate the total cost value of each point in the OPEN list according to formulas (2)-(6), select the point with the minimum total cost value as the parent node and add it to the CLOSED list, and add the spatial neighboring points of the parent node to the OPEN list. If its spatial neighboring points are obstacle points in the submap, they will not be added to the OPEN list; when the added spatial neighboring points are not in the layer where the parent node is located but in other layers, it is necessary to judge whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points will not be added to the OPEN list. In this embodiment, point 2 has the minimum cost function value.

[0068] ③ When the spatial neighboring points of the parent node are already in the OPEN list (such as Figure 5 1, 3, 4-7 in the figure), at this time, it is necessary to calculate the actual cost value from the starting point to the parent node and then to the spatial neighboring point. If this actual cost value is less than the actual cost value from the starting point to the spatial neighboring point, the parent node of the neighboring spatial point is changed. If the actual cost value from the starting point to point 2 and then to point 1 is g S-2-1 (1) than the original g from the starting point to point 1 S-1 (1) If the parent node of point 1 is smaller, the parent node of point 1 is changed, that is, from the starting point to point 2.

[0069] ④ Check whether the target point is included in the OPEN list. If it is included, the search ends and the path from the starting point to the target point is reconstructed by backtracking through the parent node chain; otherwise, execute steps ②-④.

[0070] The UAV planning path generated in this embodiment is as follows: Figure 6As shown, the white line represents the planned drone path. In order to better illustrate the effect of the method proposed in the present invention, the hierarchical A-star algorithm based on adaptive weights proposed in the present invention is compared with the traditional A-star algorithm in terms of time performance. Figure 7 As shown in the figure, the horizontal axis refers to each frame of the drone operation, the vertical axis represents the time consumed by the drone to run the algorithm, the red line represents the traditional A-star algorithm, and the green line represents the hierarchical A-star algorithm based on adaptive weights proposed in the present invention. It can be clearly seen from the figure that the time consumed by the hierarchical A-star algorithm based on adaptive weights proposed in the present invention is significantly less than that of the traditional A-star algorithm.

[0071] Example 2

[0072] Based on the same inventive concept, the present invention also provides an A-star UAV path planning system based on adaptive weights and layers, including a processor and a memory, the memory being used to store program instructions, and the processor being used to call the program instructions in the memory to execute the A-star UAV path planning method based on adaptive weights and layers as described above.

[0073] Example 3

[0074] Based on the same inventive concept, the present invention also provides an A-star UAV path planning system based on adaptive weights and layers, including a readable storage medium, on which a computer program is stored. When the computer program is executed, an A-star UAV path planning method based on adaptive weights and layers as described above is implemented.

[0075] In specific implementation, the method proposed in the technical solution of the present invention can be implemented by technical personnel in this field using computer software technology to realize automatic operation process. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of the present invention and computer equipment running the corresponding computer program, should also be within the scope of protection of the present invention.

[0076] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific implementation cases described, or replace them with similar methods, without departing from the spirit of the present invention or exceeding the scope defined by the appended claims.

Claims

1. A method for A-star UAV path planning based on adaptive weight and layering, characterized in that: The following steps are involved: Step 1: Position the UAV in a GPS-free environment to obtain the UAV's three-dimensional coordinates and attitude information; Step 2: Based on the lidar point cloud data, an obstacle map is established using the octree algorithm; Step 3: Layer the obstacle map according to the elevation information of the point cloud, calculate the total obstacle probability of each layer in the obstacle map, select the layer with the smallest total obstacle probability and construct a sub-map together with the obstacle map layer where the drone and the set target point are located; Step 4: Use the hierarchical A-star algorithm based on adaptive weights to perform UAV path planning on the sub-map obtained in step 3.

2. The method for A-star UAV path planning based on adaptive weight and layering as claimed in claim 1, characterized in that: In step 1, the Cartographer pure positioning mode algorithm is used to locate the UAV in a GPS-free environment. First, environmental data is obtained from the lidar, IMU, and odometer, including the point cloud data of the entire area obtained by the lidar, the attitude and acceleration of the UAV obtained by the IMU, and the flight distance of the UAV obtained by the odometer. The environmental data of multiple sensors are synchronized and denoised, and the initial pose of the UAV is estimated using the IMU data and motion model. Then, the map previously established for the environment is used as the occupancy grid map, and the current lidar scan data is matched with the occupancy grid map to accurately estimate the current pose of the UAV, and the current accurate pose is added to the pose graph for global optimization to reduce the cumulative error. Then, the occupancy grid map is updated according to the optimized pose, the closed loop is detected and the error is corrected to further improve the map accuracy. Finally, the constructed map and the pose results of the UAV are published and stored.

3. The method for A-star UAV path planning based on adaptive weight and layering as claimed in claim 1, characterized in that: In step 2, the relevant parameters in the obstacle map are first defined, including the map origin, the actual map size, the map resolution, the map expansion distance and the occupancy map size. Then, the point cloud data of the entire area obtained by the laser radar scanning is expanded according to the map expansion distance, and then the map is constructed using the octree. The specific process of using the octree to construct the obstacle map is as follows: ① Initialize the octree, define the boundary of the entire space, and initialize the root node, which is the highest level node of the octree; ② Divide the space of each node into eight subspaces of equal size, and create a child node for each subspace; ③ Continue to divide each child node in the same way as step ② until the predetermined number of subdivision levels is reached or the density of obstacles in the space reaches the set threshold; ④ Insert each point in the point cloud data into the corresponding leaf node in the octree. According to the application requirements, the leaf node is marked as an occupied node, an idle node or a partially occupied node. When the node space is not occupied by an obstacle, it is an idle node; when the node space is completely occupied by an obstacle, it is an occupied node; when only part of the node space is occupied by an obstacle, it is a partially occupied node, and finally an obstacle map is formed.

4. The method for A-star UAV path planning based on adaptive weight and layering as claimed in claim 1, characterized in that: In step 3, the obstacle map is layered according to the z coordinate of the point cloud. Assuming that the range of the z coordinate is N1~N2, and the layering is performed at intervals of △h, the layer corresponding to the height N1 is 0, and the layer corresponding to the height N1+△h is 1. The layers corresponding to all heights are obtained by analogy. The environmental data of the lidar, IMU and odometer and the obstacle map constructed in step 2 are read. The position of the drone obtained in step 1 is used as the starting point, and the position of the target point is manually set. The obstacle map layer where the starting point and the target point are located is obtained according to the z coordinates of the starting point and the target point. The total obstacle probability of each layer in the obstacle map is calculated. The calculation formula is as follows: P z =Σp(x p ,y q ,z) (1) Where Pz represents the total obstacle probability of the zth layer; p(x p ,y q ,z) means that at the z layer (x p ,y q ) is there an obstacle, if there is an obstacle, the value is 1, otherwise the value is 0, p and q are the row number and column number on the zth layer of the obstacle map; Select the layer with the smallest total obstacle probability and construct a submap together with the layer where the starting point is located and the layer where the set target point is located.

5. The method for A-star UAV path planning based on adaptive weight and layering as claimed in claim 1, characterized in that: The cost function expression of the adaptive weight-based hierarchical A-star algorithm in step 4 is: F(n)=g(n)+w×h(n) (2) h(n)=|x n -x target |+y n -y target |+|z n -z target | (5) Where n is the current location, n≥2, that is, a point on the path, and the starting point is the first location; F(n) represents the total cost of location n; g(n) represents the actual cost from the starting point to location n; cost i represents the cost from position i-1 to position i; (x i-1 ,y i-1 ,z i-1 ) is the position of point i-1; (x i ,y i ,z i ) is the coordinate of position point i; h(n) represents the heuristic estimated cost from position point n to the target point; (x n ,y n ,z n ) is the coordinate of the position point n, (xtarget, ytarget, ztarget) is the coordinate of the target point; w is the adaptive weight, whose value is determined by the distance from the target point and is calculated as: In the formula, is the Manhattan distance from the current location n to the target point, B m The length of the longest side of the submap.

6. The method for A-star UAV path planning based on adaptive weight and layering as claimed in claim 5, characterized in that: In step 4, the hierarchical A-star algorithm based on adaptive weights is used for UAV path planning. The OPEN list and the CLOSED list are defined and initialized as empty lists. Then, the following operations are performed: ① The starting point and its spatial neighboring points in the sub-map are added to the OPEN list. If its spatial neighboring points are obstacle points in the sub-map, they are not added to the OPEN list. When the added spatial neighboring points are not in the layer where the initial point is located but in other layers, it is necessary to judge whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points are not added to the OPEN list. ② According to formulas (2)-(6), the total cost value of each point in the OPEN list is calculated, and the point with the minimum total cost value is selected as the parent node and added to the CLOSED list. The spatial neighboring points of the parent node are added to the OPEN list. If its spatial neighboring points are obstacle points in the sub-map, they are not added to the OPEN list. When the added spatial neighboring points are not in the layer where the parent node is located but in other layers, it is necessary to judge whether there are obstacles between the layers. If there are obstacles between the layers, the spatial neighboring points are not added to the OPEN list. ③ When the spatial neighboring point of the parent node is already in the OPEN list, it is necessary to calculate the actual cost from the starting point to the parent node and then to the spatial neighboring point. If this actual cost is less than the actual cost from the starting point to the spatial neighboring point, the parent node is used as the starting point of the neighborhood spatial point; ④ Check whether the target point is included in the OPEN list. If the target point is included, the search ends and the path from the starting point to the target point is reconstructed by backtracking the parent node chain; Otherwise, execute steps ②-④.

7. A path planning system for A-star UAV based on adaptive weight and stratification, characterized in that: It includes a processor and a memory, the memory is used to store program instructions, and the processor is used to call the program instructions in the memory to execute an A-star UAV path planning method based on adaptive weights and stratification as described in any one of claims 1-6.

8. An A-star UAV path planning system based on adaptive weight and stratification, characterized in that: It includes a readable storage medium, on which a computer program is stored. When the computer program is executed, an A-star UAV path planning method based on adaptive weights and stratification as described in any one of claims 1 to 6 is implemented.

Citation Information

Cited By

  • Multi-map path planning method and device

    CN119803502A

  • Low-altitude path planning method and system based on dynamic adaptive graph neural network

    CN121544810A

  • A Low-Altitude Path Planning Method and System Based on Dynamic Adaptive Graph Neural Network

    CN121544810B