An adaptive centroid boundary exploration method for UAV based on laser SLAM

Abstract

The present invention discloses a method for adaptive centroid boundary exploration of unmanned aerial vehicles based on laser slam, including setting regional boundaries, loading environmental configuration parameters; constructing a grid map; using a weighted centroid method to detect and identify boundary points, and summarizing the unknown areas surrounded by the explored areas as the explored areas; finding the boundary points with the highest current benefit value as the exploration points of the unmanned aerial vehicles by weighting energy efficiency; adaptively adjusting the perception radius of the unmanned aerial vehicles; and optimizing the path planning by obstacle cost. The present invention introduces weighted centroid in boundary identification, takes the centroid point of the unknown area as the boundary exploration point and infers it, effectively increasing the efficiency of exploration and avoiding the waste of computing power. In terms of boundary exploration, the method of comprehensive distance, energy consumption efficiency and dynamic perception adjustment is adopted, and the optimized path planning with obstacle cost is considered at the same time, which can accurately reflect the exploration and mapping challenges in the real world, and provide robots with a safer, more efficient and adaptable solution.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle mapping and exploration, and in particular relates to an unmanned aerial vehicle adaptive centroid point boundary exploration method based on laser SLAM. Background Art

[0002] Traditionally, drone positioning, navigation and map building are two independent problems, where positioning usually relies on pre-built maps, while map building requires accurate positioning information. However, in many real-world scenarios, the environment is unknown or dynamically changing, so traditional methods often cannot meet the requirements. The emergence of SLAM technology solves this problem, allowing drones to simultaneously locate and build maps in unknown environments. By fusing sensor data such as lidar, cameras, etc. and motion models, the SLAM system can estimate the position of the drone in real time and build a map of the environment.

[0003] However, in unknown environments, maps are usually incomplete or inaccurate, and pre-planned paths may not be able to adapt to changes in the environment. At the same time, existing algorithms do not make sufficient use of the amount of information in the environment. During the boundary exploration of drones, the edge midpoint of the location area is often used as the exploration point, which makes it impossible to effectively determine the next moving target and obtain more information gain, resulting in low exploration efficiency. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a laser slam-based UAV adaptive centroid point boundary exploration method in view of the deficiencies of the above-mentioned prior art. The concept of weighted centroid is introduced in boundary recognition, and the centroid point of the unknown area is used as the boundary exploration point, and reasonable inference is made, which effectively increases the efficiency of exploration and avoids wasting computing power. In terms of boundary exploration, a method that integrates distance, energy consumption efficiency and dynamic perception adjustment is adopted, and an optimized path planning model with obstacle cost is considered at the same time. The optimized path with obstacle cost is considered, which can more accurately reflect the exploration and mapping challenges in the real world, and provide a safer, more efficient and adaptable solution for robots, realize adaptive centroid point boundary exploration in unknown environments, and make improvements in the positioning of centroid points, so that UAVs can better adapt to complex and changeable environmental conditions, and improve the exploration efficiency and accuracy of UAVs in unknown environments.

[0005] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0006] A laser SLAM-based adaptive centroid boundary exploration method for unmanned aerial vehicles, characterized by comprising:

[0007] Step 1: Set the boundaries of the map exploration area and load the autonomous mapping configuration parameters of the corresponding environment;

[0008] Step 2: Build a grid map based on the laser SLAM mapping algorithm;

[0009] Step 3: Define the prior rate and the posterior rate in constructing the grid map, obtain the boundary points of the unexplored area, and determine whether the two sides of the unexplored area are surrounded by two obstacles. If so, select the midpoint of the boundary between the two obstacles as the identification point, otherwise use the weighted centroid point of the unexplored area as the identification point of the boundary, and count the identification point into the two-dimensional linked list centroids of the spare exploration points;

[0010] Step 4: Traverse all the alternative exploration points in centroids, and find the boundary point with the highest current benefit value through energy efficiency weighted optimization based on the prior rate and the posterior rate, as the exploration point that the drone is going to;

[0011] Step 5: After determining the exploration point that the drone is going to, adaptively adjust the drone’s perception radius;

[0012] Step 6: If there are obstacles on the path that the drone is exploring to the exploration point it wants to go to, when the drone approaches the obstacle, the obstacle cost optimization path planning model optimizes the drone trajectory to help the drone bypass the obstacle until it reaches the exploration point it wants to go to, updates the map, and returns to step 3.

[0013] To optimize the above technical solutions, the specific measures taken also include:

[0014] The above step 2 includes:

[0015] The LaserScan data of the LiDAR, the odom data of the odometer, and the IMU data are used to infer the robot's current position and posture. After scanning and matching the surrounding map environment, the LaserScan data of the LiDAR are constructed into submaps, and a grid map is constructed through submaps.

[0016] The above step 3 includes the following sub-steps:

[0017] Step 31: Get the two-dimensional array occupancy_grid_map stored in the grid map;

[0018] Step 32: traverse the occupancy_grid_map, for each grid cell, use its coordinates to obtain its status information, and calculate the position information of the grid cell in combination with the origin coordinates and resolution of the map;

[0019] Step 33: Define two-dimensional arrays prior_probabilities and posterior_probabilities to store the prior and posterior rates of each grid; the prior rate is obtained by making an initial estimate of the state of each location in the map before sensor observation;

[0020] According to the sensor data sensor_data, the state of the updated map grid is obtained. Considering the information of sensor observation and drone movement, the posterior rate of the map is updated. Specifically:

[0021] The posterior rate of the grid in the explored area is marked as 0, the posterior rate of the obstacle area is marked as the number of occupied grids, and the posterior rate of the grid in the unexplored area is marked as 0.5;

[0022] Step 34: After the grid state is updated by the sensor, the area where the centroid point is calculated is judged, and the unknown area surrounded by the explored area is classified as the explored area, and no longer included in the explorable point; for the unexplored area, if it is surrounded by two obstacles on both sides, the midpoint of the boundary between the two obstacles is selected as the identification point, otherwise its weighted centroid is used as the identification point of the boundary;

[0023] Step 35: Store the coordinate information of the identified points into the two-dimensional linked list centroids of candidate exploration points.

[0024] The weighted centroid mentioned above is calculated by the following formula:

[0025]

[0026]

[0027] Where N is the total number of boundary points detected by the drone through sensors; x i With y i are the horizontal and vertical coordinates of the i-th detected boundary point; w i is the weight assigned to the i-th point, which is determined according to the importance of the point or its contribution to the exploration task; C x With C y They are respectively the horizontal and vertical coordinates of the weighted centroid finally calculated.

[0028] The optimization goal of the energy efficiency weighted optimization in step 4 above is:

[0029]

[0030] Where F represents the set of all available alternative exploration points, f represents a single exploration point in the set, r represents the current position of the drone, E(f,r) represents the estimated energy consumption from the robot to the exploration point f, G(f,r) represents the information gain that the robot can obtain from the exploration point f, and θ1 and θ2 are weight factors.

[0031] The specific formula of E(f,r) above is:

[0032]

[0033] Where E(f,r) is the estimated energy consumption from the robot to the exploration point f, d1 and d2 are the distances moved by the drone at the beginning and end, and R(v) is the energy consumption rate of the drone at speed v.

[0034] The specific formula of G(f,r) above is:

[0035] G(f,r)=H prior -H posterior

[0036] Among them, H prior is the prior uncertainty before the sensor observation, and the specific formula is H prior =-∑ i p i log2(p i ), where p i Represents the prior rate of the i-th cell or feature in the location area;

[0037] H posterior It is the posterior uncertainty obtained by updating the map based on the results of simulated observations after sensor observation. The specific formula is H posterior =-∑ i p i ′ log2(p i ′ ), where p i ′ It represents the posterior rate of the i-th cell or feature in the map after the sensor observation.

[0038] The above step 5 includes the following sub-steps:

[0039] Step 51: Traverse all current exploration point location information from the two-dimensional linked list centroids, extract the location information of the exploration point to be visited into the target array, and read the current location information P of the drone at the same time robot ;

[0040] Step 52: Set the Gaussian kernel function And a bandwidth parameter h, get the drone position P robot The density of the exploration points at Among them, P i represents the exploration points around the drone, n is the number of exploration points, and then the exploration point density term frontier_density(P robot )=f(P robot );

[0041] Step 53: According to the change in the distance between the drone and the selected exploration point, the drone proximity term is obtained:

[0042]

[0043] Where ∈ is a positive number;

[0044] Step 54: According to the exploration point density term and the drone proximity term, the adaptive adjustment value of the drone perception radius is obtained:

[0045] R(P robot ,P target ) = frontier_density(P robot )+robot_proximity(P robot )

[0046] Step 55: After each drone reaches the target point, the map is updated based on the sensor information, the new location area centroid is recalculated, and a new exploration point is selected as the next target point.

[0047] The above step 6 includes the following sub-steps:

[0048] Step 61: After the laser radar scans, the location information of the obstacle is identified, and the obstacle information is stored in the list obstacles from the callback function laser_callback;

[0049] Step 62: After determining the target point P to be reached target Then, the straight line path between the drone and the point is obtained through the path callback function path_callback;

[0050] Step 63: traverse each point on the straight path, and determine whether there is an obstacle or is close to an obstacle on the straight path according to the data information in the radar callback function laser_callback;

[0051] Step 64: If there is an obstacle or the obstacle is within the set range of the drone, the path planning result is optimized according to the following optimization model:

[0052]

[0053] Among them, P is the path from the current position to the target position, that is, the target to be optimized, C(P) represents the cost of path P, which is proportional to the path length, G(P) represents the information gain that can be obtained during the exploration of path P, μ1, μ2, μ3 are weight coefficients used to balance the importance of cost, information gain and obstacle cost, O(P) represents the cost required to pass through obstacles on path P, and its specific expression is:

[0054]

[0055] Where O is the set of all obstacles on the path; cost(o) is the cost of bypassing the obstacle, which is determined by the type and size of the obstacle.

[0056] The present invention has the following beneficial effects:

[0057] The present invention proposes an adaptive boundary recognition method based on centroid points, introduces the concept of weighted centroids to identify boundaries, and uses centroid points in unknown areas as boundary exploration points for drones in location areas, i.e., the targets of boundary exploration, which greatly increases the information gain that can be obtained during the exploration process. And on this basis, weighted processing is performed based on reasonable inferences, and the concept of weighted centroids is introduced, which not only introduces innovative improvements to boundary identification and selection, but also provides a more flexible and more practical method for drone exploration, which is particularly suitable for scenarios that require fine management of exploration focus. By adjusting the weight distribution, drones can give priority to areas that are more critical to completing the mission, thereby effectively improving exploration efficiency and safety and avoiding waste of resources.

[0058] The present invention proposes a method for comprehensively considering energy consumption and information gain in boundary selection, and proposes a method for adjusting the perception radius for the effectiveness of UAV exploration. By balancing the UAV's choice of energy consumption information gain, it aims to reduce the number of information gains obtained by the UAV while also taking into account the effective use of energy. For the adjustment of the perception radius, by adjusting the UAV approach item and the exploration point density item, the UAV can gradually increase the exploration radius in the process of approaching the exploration point, and is more inclined to go to areas with a higher density of exploration points, so as to obtain environmental information more concentratedly in the area. At the same time, this adjustment can also reduce overlap in areas with dense UAVs and improve exploration efficiency.

[0059] For exploration paths with obstacles, the present invention proposes an obstacle cost path planning model. In complex or unknown environments, the obstacle cost can be reflected as the additional energy or time required for the robot to bypass or pass through obstacles in the process of reaching the target point. The obstacle cost is introduced to quantify the additional cost caused by encountering obstacles in the path, which can include additional energy consumption, time delay, or potential damage risk to the drone. Through this multi-objective optimization model, not only the minimization of path cost and risk and the maximization of information gain are considered, but also the minimization of the cost of crossing obstacles, which makes path planning more comprehensive and practical, especially in environments with dense obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 This is a flow chart of the adaptive centroid boundary exploration method of unmanned aerial vehicle based on laser SLAM of the present invention;

[0061] Figure 2 A schematic diagram of the exploration points for introducing weighted centroids in the present invention;

[0062] Figure 3 The exploration effect diagram in a complex maze environment is selected for the present invention;

[0063] Figure 4 The exploration effect diagram in the indoor multi-room environment is selected for the present invention;

[0064] Figure 5 An exploration effect diagram in an indoor environment with dense obstacles is selected for the present invention. DETAILED DESCRIPTION

[0065] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0066] Although the steps in the present invention are arranged with numbers, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It is understood that the term "and / or" used in this article involves and covers any and all possible combinations of one or more of the associated listed items.

[0067] like Figure 1 As shown, a laser SLAM-based adaptive centroid boundary exploration method for unmanned aerial vehicles of the present invention comprises the following steps:

[0068] Step 1: Set the boundaries of the map exploration area and load the autonomous mapping configuration parameters of the corresponding environment, including the scanning frequency f of the lidar, the angle range (θ min ,θ max ), resolution Δθ, the motion model parameters of the drone, including the speed limit v max , acceleration limita max , as well as the map resolution (Δx, Δy) and size settings (L x ,L y ); the scanning frequency, angle range and resolution related to the laser radar are used for laser radar measurement, and the relevant parameters of the drone and map are used for map generation;

[0069] Step 2: Build a grid map based on the laser SLAM mapping algorithm;

[0070] Step 2: Use the lidar sensor to obtain the distance information of the surrounding environment through laser, and build a grid map through the existing SLAM mapping algorithm;

[0071] Step 3: Define the prior rate and the posterior rate in constructing the grid map, obtain the boundary points of the unexplored area, and determine whether the two sides of the unexplored area are surrounded by two obstacles. If so, select the midpoint of the boundary between the two obstacles as the boundary identification point, otherwise use the weighted centroid point of the unexplored area as the boundary identification point, and count the identification point into the two-dimensional linked list centroids of the spare exploration points;

[0072] In the obtained grid map, the boundary points of the unexplored areas are explored by the prior rate and posterior rate methods. The weighted centroid points of these areas are detected and calculated and classified as backup exploration points. The backup exploration points surrounded by known areas are regarded as explored areas.

[0073] Step 4: Traverse all the alternative exploration points, and find the boundary point with the highest current benefit value through the energy efficiency weighted method based on the prior rate and the posterior rate, as the exploration point that the drone is going to;

[0074] Step 5: After determining the exploration point that the drone is going to, adaptively adjust the drone’s perception radius;

[0075] Step 6: If there are obstacles on the path that the drone is exploring to the exploration point, when the drone approaches the obstacle, the obstacle cost optimization path planning model optimizes the trajectory of the drone to help the drone bypass the obstacle until it reaches the exploration point to be explored, updates the map, and returns to step 3;

[0076] Step 7: Add termination conditions and the exploration ends.

[0077] In the embodiment, the process of constructing the grid map in step 2 includes:

[0078] The Laserscan data of the LiDAR is a set of measurement values ​​describing the LiDAR sensor during the scanning process, including distance data, angle data, perception radius, angle range (θ min ,θ max ), angular resolution Δθ, odom data of the odometer, i.e., information used to describe the position and posture changes of the robot in its environment over time, and IMU data, i.e., information about the motion state of the object collected by the IMU sensor, including accelerometer and gyroscope. The data includes linear acceleration, angular velocity, and timestamp to record the time point of data collection, which is convenient for data synchronization and fusion. The position and posture of the robot at the current time are inferred through these data. After scanning and matching the surrounding map environment, the Laserscan data of the lidar are constructed into submaps (local submaps), and the grid map map is constructed through submaps.

[0079] Step 3: In the obtained grid map, the boundary points of the unexplored areas are explored by the prior rate and the posterior rate method. The weighted centroid points of these areas are detected and calculated and classified as spare exploration points. The spare exploration points surrounded by the known areas are regarded as explored areas. The specific sub-steps are as follows:

[0080] Step 31: Get the two-dimensional array occupancy_grid_map stored in the grid map;

[0081] Step 32: Traverse the occupancy_grid_map array, for each grid cell, use its coordinates to obtain its status information, and calculate the location information of the grid cell in combination with the origin coordinates and resolution of the map;

[0082] Furthermore, step 32 involves calculating the position information of the grid cells. According to the origin coordinates and resolution of the map, the index of the grid cell is converted into its position coordinates in the map coordinate system. Assuming that the origin coordinates of the map are x ,origin y ), resolution (Δx, Δy), grid cell index is (i, j), then the position coordinates of the grid cell are:

[0083] (x,y)=(origin x +i·Δx,origin y +j·Δy)

[0084] Step 33: Use the two-dimensional arrays prior_probabilities and posterior_probabilities to store the prior and posterior rates of each grid. The prior and posterior rates here are used to adaptively adjust the drone's perception radius in step 4;

[0085] The prior rate (prior probability) is an initial estimate of the state (e.g., occupied or idle) of each location in the map before any sensor observation is made. It is assumed here that each grid cell in the map has the same initial occupancy probability of 0.5.

[0086] According to the sensor data sensor_data, the state of the updated map grid can be obtained. With the help of methods such as Bayesian filtering, the information of sensor observation and drone movement can be considered to update the posterior probability of the map.

[0087] According to the prior and posterior rates stipulated in informatics, the posterior rate of the grid in the known area is marked as 0, the posterior rate of the obstacle area is the number of grids occupied, and the posterior rate of the grid in the unknown area is marked as 0.5. Methods such as Bayesian filtering are used to comprehensively consider the information of sensor observations and drone movements, thereby updating the posterior probability of the map;

[0088] Step 34: Figure 2 As shown in the figure, after the grid state is updated by the sensor, the area where the centroid point is calculated is judged, and the unknown area surrounded by the explored area is classified as the explored area, and no longer included in the explorable point; in the unexplored area, if its edge is not blocked by obstacles, the centroid is regarded as the feature point of the boundary; and when the area is surrounded by obstacles on both sides, the midpoint of the boundary between the two obstacles is selected as the identification point;

[0089] The concept of weighted centroid is introduced to judge the area where the centroid point is calculated, and the unknown area surrounded by the explored area is classified as the explored area, and no longer included in the explorable points;

[0090] The center of mass calculation formula of a specific robot exploration scene is optimized, and the concept of weighted center of mass is introduced. The improved weighted center of mass (C x ,C y ) can be calculated by the following formula:

[0091]

[0092] Where N is the total number of boundary points detected by the drone through the sensor, x i With y i are the horizontal and vertical coordinates of the i-th detected boundary point, w iis the weight assigned to the i-th point, which will be determined according to the importance of the point or its contribution to the exploration task. x With C y They are respectively the horizontal and vertical coordinates of the weighted centroid finally calculated.

[0093] Furthermore, first, initialize the weights for each location point, and set the weights to the same initial value. Each time the weighted centroid is calculated, adjust the weights according to the accumulation of counts. Use the value of count as an adjustment factor so that location points with larger counts have higher weights, thereby affecting the location of the centroid more. Calculate the location of the weighted centroid according to the adjusted weights. The centroid can be calculated using a weighted average or weighted sum to ensure that location points with larger counts have a greater impact on the centroid. Each time the centroid is updated, the weights are adjusted repeatedly according to the count until the number of iterations reaches a preset value.

[0094] Unlike traditional centroids that treat all points equally, weighted centroids assign different weights to each point, allowing some points to have a greater influence on the centroid position. Such a method can be adjusted according to actual conditions to reflect the actual importance of different points.

[0095] Weight w i It can be determined based on a variety of factors, such as the distance from the point to the nearest obstacle, the difficulty of exploring the area where the point is located, or the richness of the environmental information. For example, points close to obstacles can be assigned higher weights because these points may point to new exploration directions or critical paths.

[0096] Furthermore, this weighted centroid calculation method is particularly suitable for scenarios that require fine management of exploration focus. By adjusting the weight distribution, the drone can prioritize those areas that are more critical to completing the mission, thereby improving exploration efficiency and safety. The successful implementation of this method will rely on properly defining and calculating the weight of each point, which may need to be adjusted and optimized according to the specific mission and environmental characteristics.

[0097] Step 35: Finally, the calculated centroid coordinate information is stored in the two-dimensional linked list centroids of the candidate exploration points.

[0098] Weighted centroid allows certain points to have a greater influence on the centroid position by assigning different weights to each point. Such a method can be adjusted according to actual conditions to reflect the actual importance of different points. i It can be determined based on a variety of factors, such as the distance from the point to the nearest obstacle, the difficulty of exploring the area where the point is located, or the richness of the environmental information.

[0099] In the embodiment, step 4 traverses all the spare exploration points, and finds the boundary point with the highest current benefit value by using the energy efficiency weighted method as the exploration point to be headed by the drone, and includes the following sub-steps:

[0100] Step 41: traverse and read the location information of the candidate exploration points from the two-dimensional linked list centroids in sequence;

[0101] Step 42: Use a more energy-efficient exploration point selection, using the following optimization objective:

[0102]

[0103] Where F represents the set of all available exploration points, f represents a single exploration point in the set, r represents the current position of the drone, E(f,r) represents the estimated energy consumption from the robot to the exploration point f, G(f,r) represents the information gain that can be obtained from the robot to the exploration point f, θ1 and θ2 are weight factors used to balance the importance of distance and energy consumption in frontier selection. The optimal exploration point is selected by weighing information gain and energy consumption.

[0104] Furthermore, the specific energy consumption formula is:

[0105]

[0106] Where E(f,r) is the estimated energy consumption from the robot to the exploration point f, d1 and d2 are the distances moved by the drone at the beginning and end, and R(v) is the energy consumption rate of the drone at speed v.

[0107] Furthermore, the specific formula of information gain G(f,r) is:

[0108] G(f,r)=H prior -H posterior

[0109] Among them, H prior It is the prior uncertainty before the sensor observation, that is, the prior entropy of the map, that is, the amount of unknown information in the map. The specific formula is H prior =-∑ i p i log2(p i ), where p i It represents the prior rate of the ith cell or feature in a certain location area. By summing all possible states in a certain area of ​​the map, the prior entropy of the area can be obtained. posterior It is the posterior uncertainty obtained by updating the map according to the results of simulated observations after sensor observation, that is, the posterior entropy of the map. The specific formula is H posterior =-∑ ip i ′ log2(p i ′ ), where p i ′ It represents the posterior value of the i-th cell or feature in the map after being observed by the sensor. By summing all possible states of a certain area in the map, the posterior entropy of the area on the map can be obtained.

[0110] The development process of prior entropy and posterior entropy is usually realized in the iterative optimization of SLAM algorithm. In the SLAM process, by constantly observing the environment and updating the map, the prior entropy gradually decreases, while the posterior entropy gradually increases. By comparing the changes in prior entropy and posterior entropy, the effect of sensor observation and the quality of map update can be evaluated, thereby guiding the subsequent exploration and mapping process.

[0111] In the embodiment, after determining the exploration point to be traveled, step 5 adaptively adjusts the drone's perception radius, and includes the following sub-steps:

[0112] Step 51: Traverse all current exploration point location information from the two-dimensional linked list centroids, extract the location information of the exploration point to be visited into the target array, and read the current location information P of the drone at the same time robot ;

[0113] Step 52: Set the Gaussian kernel function And a bandwidth parameter h, through this kernel density estimation method, the drone position P can be obtained robot The density of the exploration points at Among them, P i represents the exploration points around the drone, n is the number of exploration points, so we can get the exploration point density term frontier_density(P robot ),Right now:

[0114] The Gaussian kernel density function is a commonly used probability density estimation method used to smooth and model data sets. This function is based on the Gaussian distribution, also known as the normal distribution. Its characteristics are that it can smoothly model data and can flexibly adapt to different data distribution situations.

[0115] Step 53: According to the change in the distance between the drone and the selected exploration point, the drone proximity term is obtained:

[0116]

[0117] where ∈ is a small positive number used to avoid division by zero errors.

[0118] Step 54: According to the exploration point density term and the drone proximity term, the adaptive adjustment value of the drone perception radius can be obtained, that is, the drone perception radius is controlled by the combination of the exploration point density term and the drone proximity term:

[0119] R(P robot ,P target ) = frontier_density(P robot )+robot_proximity(P robot )

[0120] R(P robot ,P target ) is the final radius value obtained after joint adjustment.

[0121] Step 55: After each drone reaches the target point, the map is updated based on the sensor information, the new location area centroid is recalculated, and a new exploration point is selected as the next target point.

[0122] In the embodiment, when the UAV approaches an obstacle, step 6 introduces an obstacle cost optimization path planning model to help the UAV bypass the obstacle, and includes the following sub-steps:

[0123] Step 61: After the laser radar scans, identify the location information of the obstacles and store the obstacle information in the list obstacles from the callback function laser_callback;

[0124] Step 62: After determining the target point P to be reached target Then, the straight line path between the drone and the point is obtained through the path callback function path_callback;

[0125] Step 63: traverse each point on the path, and determine whether there is an obstacle or is close to an obstacle on the straight path according to the data information in the radar callback function laser_callback;

[0126] Step 64: If there is an obstacle or when the obstacle is within a certain range of the drone, the path planning result is optimized according to the following optimization model:

[0127]

[0128] Among them, P is the path from the current position to the target position, that is, the target to be optimized, C(P) represents the cost of path P, which is proportional to the path length, G(P) represents the information gain that can be obtained during the exploration of path P, μ1, μ2, μ3 are weight coefficients used to balance the importance of cost, information gain and obstacle cost, O(P) represents the cost required to pass through obstacles on path P, and its specific expression is:

[0129]

[0130] Where O is the set of all obstacles on the path, and cost(o) is the cost of bypassing these obstacles, which is determined by the type and size of the obstacles. The larger the size and the more complex the structure of the obstacle, the greater the cost, and the smaller the size and the simpler the structure of the obstacle, the smaller the cost.

[0131] The obstacle cost is introduced to quantify the additional cost of encountering obstacles in the path, which can include additional energy consumption, time delay, or potential risk of damage to the robot. By adjusting the weight coefficients μ1, μ2, and μ3, the importance of each factor can be balanced according to the requirements of the specific task and environment to find the optimal path planning strategy. This path planning model takes into account the existence of obstacles, thereby more accurately simulating the navigation environment in the real world. It provides a safer, more efficient, and more adaptable path planning solution for robots.

[0132] Example

[0133] This embodiment provides a simulation environment for the algorithm, such as Figure 3 As shown, the size of the entire map is 770px×730px, the scanning frequency of the UAV laser radar is 10Hz, and the angle range of the laser radar is -135°~+135°. In such a complex maze environment, it is necessary to detect as many location areas as possible, and the road needs to cross obstacles to detect, so the weight of the weighted centroid is accumulated in the form of count, the closer to the obstacle, the greater the weight, and the closer to the middle position, the smaller the weight; in the energy efficiency weighting method, in order to make the UAV detect less back and forth in this map, θ1=1.2, θ2=0.8 are set, aiming to increase the UAV's consideration of path energy consumption; in the adjustment of the UAV's perception radius, the bandwidth function of the Gaussian kernel function The standard deviation σ=0.1; in the obstacle cost path planning optimization model, since the obstacles are complex but relatively regular, the weights μ1=1.2, μ2=0.8, and μ3=0.5 are set to encourage the drone to explore completely in one direction.

[0134] like Figure 4 The size of the entire map shown is 400px×300px. In such an indoor multi-room environment, the weights of the weighted centroid are accumulated in the form of count, and the closer to the middle position, the greater the weight. In the energy efficiency weighting method, in order to make the drone explore more completely in this map, θ1=0.75 and θ2=1.5 are set to increase the drone's consideration of information gain. In the adjustment of the drone's perception radius, the bandwidth function of the Gaussian kernel function The standard deviation σ=0.05; in the obstacle cost path planning optimization model, since the obstacles are relatively regular, the weights μ1=0.6, μ2=1.6, and μ3=0.8 are set to encourage the drone to go to areas with more information gain.

[0135] like Figure 5 The size of the entire map shown is 775px×745px. In such an indoor multi-room environment, the weight of the weighted centroid is accumulated in the form of count, and the closer to the obstacle position, the greater the weight. In the energy efficiency weighting method, in order to make the drone explore more completely in this map, θ1=0.6 and θ2=1.8 are set. In the adjustment of the drone's perception radius, the bandwidth function of the Gaussian kernel function The standard deviation σ=0.1; in the obstacle cost path planning optimization model, since the obstacles are extremely irregular, the weights μ1=0.8, μ2=1.2, and μ3=1.6 are set to encourage the drone to obtain a safer path when the obstacles are cluttered and complex.

[0136] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0137] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A laser slam-based UAV adaptive centroid boundary exploration method, characterized in that: include Step 1: Set the boundaries of the map exploration area and load the autonomous mapping configuration parameters of the corresponding environment; Step 2: Build a grid map based on the laser SLAM mapping algorithm; Step 3: Define the prior rate and the posterior rate in constructing the grid map, obtain the boundary points of the unexplored area, and determine whether the two sides of the unexplored area are surrounded by two obstacles. If so, select the midpoint of the boundary between the two obstacles as the identification point, otherwise use the weighted centroid point of the unexplored area as the identification point of the boundary, and count the identification point into the two-dimensional linked list centroids of the spare exploration points; Step 4: Traverse all the alternative exploration points in centroids, and find the boundary point with the highest current benefit value through energy efficiency weighted optimization based on the prior rate and the posterior rate, as the exploration point that the drone is going to; Step 5: After determining the exploration point that the drone is going to, adaptively adjust the drone’s perception radius; Step 6: If there are obstacles on the path that the drone is exploring to the exploration point, when the drone approaches the obstacle, the obstacle cost optimization path planning model optimizes the trajectory of the drone to help the drone bypass the obstacle until it reaches the exploration point to be explored, updates the map, and returns to step 3; The optimization goal of the energy efficiency weighted optimization in step 4 is: Where F represents the set of all available alternative exploration points, f represents a single exploration point in the set, r represents the current position of the drone, E(f,r) represents the estimated energy consumption from the drone to the exploration point f, G(f,r) represents the information gain that can be obtained from the drone to the exploration point f, and θ1 and θ2 are weight factors; The specific formula of G(f,r) is: G(f,r)=H prior -H posterior Among them, H prior is the prior uncertainty before the sensor observation, and the specific formula is H prior =-∑ i p i log2(p i ), where p i Represents the prior rate of the i-th cell or feature in the location area; H posterior It is the posterior uncertainty obtained by updating the map based on the results of simulated observations after sensor observation. The specific formula is H posterior =-∑ i p i ′ log2(p i ′ ), where p i ′ represents the posterior rate of the i-th cell or feature in the map after being observed by the sensor; The step 5 comprises the following sub-steps: Step 51: Traverse all current exploration point location information from the two-dimensional linked list centroids, extract the location information of the exploration point to be visited into the target array, and read the current location information P of the drone at the same time robot ; Step 52: Set the Gaussian kernel function And a bandwidth parameter h, get the drone position P robot The density of the exploration points at Among them, P i represents the exploration points around the drone, n is the number of exploration points, and then the exploration point density term frontier_density(P robot )=f(P robot ); Step 53: According to the change in the distance between the drone and the selected exploration point, the drone proximity term is obtained: Where ∈ is a positive number, P target is the location information of the target point; Step 54: According to the exploration point density term and the drone proximity term, the adaptive adjustment value of the drone perception radius is obtained: R(P robot ,P target )=frontier_density(P robot )+robot_proximity(P robot ) Step 55: After each drone reaches the target point, the map is updated based on the sensor information, the new location area centroid is recalculated, and a new exploration point is selected as the next target point.

2. According to the laser SLAM-based adaptive centroid boundary exploration method for unmanned aerial vehicles of claim 1, it is characterized in that: The step 2 comprises: The Laserscan data of the LiDAR, the odom data of the odometer, and the IMU data are used to infer the position and posture of the drone at the current time. After scanning and matching the surrounding map environment, the Laserscan data of the LiDAR are constructed into submaps, and a grid map is constructed through submaps.

3. The method for adaptive centroid boundary exploration of unmanned aerial vehicle based on laser SLAM according to claim 1 is characterized in that: The step 3 includes the following sub-steps: Step 31: Get the two-dimensional array occupancy_grid_map stored in the grid map; Step 32: traverse the occupancy_grid_map, for each grid cell, use its coordinates to obtain its status information, and calculate the position information of the grid cell in combination with the origin coordinates and resolution of the map; Step 33: Define two-dimensional arrays prior_probabilities and posterior_probabilities to store the prior and posterior rates of each grid; the prior rate is obtained by making an initial estimate of the state of each location in the map before sensor observation; According to the sensor data sensor_data, the state of the updated map grid is obtained. Considering the information of sensor observation and drone movement, the posterior rate of the map is updated. Specifically: The posterior rate of the grid in the explored area is marked as 0, the posterior rate of the obstacle area is marked as the number of occupied grids, and the posterior rate of the grid in the unexplored area is marked as 0.5; Step 34: After the grid state is updated by the sensor, the area where the centroid point is calculated is judged, and the unknown area surrounded by the explored area is classified as the explored area, and no longer included in the explorable point; for the unexplored area, if it is surrounded by two obstacles on both sides, the midpoint of the boundary between the two obstacles is selected as the identification point, otherwise its weighted centroid is used as the identification point of the boundary; Step 35: Store the coordinate information of the identified points into the two-dimensional linked list centroids of candidate exploration points.

4. The method for adaptive centroid boundary exploration of unmanned aerial vehicle based on laser SLAM according to claim 1 is characterized in that: The weighted centroid is calculated by the following formula: Where N is the total number of boundary points detected by the drone through sensors; x i With y i are the horizontal and vertical coordinates of the i-th detected boundary point respectively; w i is the weight assigned to the i-th point, which is determined according to the importance of the point or its contribution to the exploration task; C x With C y They are respectively the horizontal and vertical coordinates of the weighted centroid finally calculated.

5. The laser SLAM-based adaptive centroid boundary exploration method for unmanned aerial vehicles according to claim 1 is characterized in that: The specific formula of E(f,r) is: Where E(f,r) is the estimated energy consumption from the UAV to the exploration point f, d1 and d2 are the distances moved by the UAV at the beginning and end, and R(v) is the energy consumption rate of the UAV at speed v.

6. The laser SLAM-based adaptive centroid boundary exploration method for unmanned aerial vehicles according to claim 1 is characterized in that: The step 6 comprises the following sub-steps: Step 61: After the laser radar scans, the location information of the obstacle is identified, and the obstacle information is stored in the list obstacles from the radar callback function laser_callback; Step 62: After determining the target point P to be reached target Then, the straight line path between the drone and the point is obtained through the path callback function path_callback; Step 63: traverse each point on the straight path, and determine whether there is an obstacle or is close to an obstacle on the straight path according to the data information in the radar callback function laser_callback; Step 64: If there is an obstacle or the obstacle is within the set range of the drone, the path planning result is optimized according to the following optimization model: Among them, P is the path from the current position to the target position, that is, the target to be optimized, C(P) represents the cost of path P, which is proportional to the path length, G(P) represents the information gain that can be obtained during the exploration of path P, μ1, μ2, μ3 are weight coefficients used to balance the importance of cost, information gain and obstacle cost, O(P) represents the cost required to pass through obstacles on path P, and its specific expression is: Where O is the set of all obstacles on the path; cost(o) is the cost of bypassing the obstacle, which is determined by the type and size of the obstacle.

Images