Autonomous exploration method combined with unknown environment contour information

Abstract

The invention discloses an autonomous exploration method combined with unknown environment contour information, which takes a depth camera as a main sensor for constructing an environment map, utilizes a laser radar to extract important environment contour information so as to guide an exploration process, and improves the overall space exploration efficiency by reducing the repeated access rate of an unmanned aerial vehicle to the same area. According to the method, the path information gain optimization strategy is utilized, optimization of the azimuth trajectory of the robot is additionally considered while the path is planned, the unmanned aerial vehicle can effectively avoid obstacles in the flight process, and the safety of a space exploration task is guaranteed.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotics, and particularly relates to an autonomous exploration method combined with unknown environment contour information. Background Art

[0002] With the rapid development of artificial intelligence and automation technologies, as well as the strategic investment of the country in the independent research and development of intelligent equipment, intelligent robot technology has become a key indicator for measuring a country's scientific and technological innovation strength. In the field of intelligent robots, multi-rotor unmanned aerial vehicles (UAVs) play an increasingly important role in fields such as emergency rescue, environmental monitoring, agricultural management, and military reconnaissance due to their high maneuverability, wide adaptability, and low cost. To achieve autonomous and efficient operation of UAVs in complex environments, a high-precision environmental map is an essential basic support. The environmental map can not only help UAVs identify obstacles and plan paths, but also provide a basis for formulating reasonable task execution strategies. However, traditional environmental map surveying mainly relies on manual operations and has the following significant defects: First, manual surveying is costly, time-consuming, and inefficient; second, repeated surveying is required when the environment changes, resulting in poor flexibility; finally, in high-risk scenarios such as post-disaster rescue or hostile areas, manual surveying may endanger the lives of operating personnel. Therefore, developing UAV real-time perception and adaptive mapping technologies to improve their autonomous decision-making ability and spatial exploration ability in unknown environments has important practical significance.

[0003] Currently, UAV autonomous exploration algorithms mainly use visual sensors or lidar for environmental perception. Although exploration algorithms based on visual sensors can obtain rich environmental texture information, their perception range is relatively limited and they are easily affected by lighting conditions, making it difficult to meet the high-efficiency perception requirements in complex environments. 2D lidar has a 360-degree horizontal scanning ability and a long measurement distance, and can quickly obtain large-scale environmental contour information, but there are problems such as the lack of vertical direction information and difficulty in extracting environmental features. In addition, there is a large representational difference between the sparse point cloud data generated by lidar and the actual environmental features, which increases the complexity of subsequent data processing and affects the real-time performance of the system. Given the inherent limitations of a single sensor, multi-sensor fusion has gradually become an important research direction for improving UAV environmental perception ability. Summary of the Invention

[0004] To overcome the deficiencies of the prior art, the present invention provides an autonomous exploration method that combines unknown environmental contour information. Using a depth camera as the main sensor for constructing an environmental map, lidar is utilized to extract important environmental contour information to guide the exploration process, and the overall spatial exploration efficiency is improved by reducing the repeated access rate of the drone to the same area. The present invention uses a path information gain optimization strategy, and additionally considers the optimization of the robot's azimuth trajectory while planning the path, enabling the drone to effectively avoid obstacles during flight and ensuring the safety of the spatial exploration task.

[0005] The technical solution adopted by the present invention to solve its technical problems is as follows:

[0006] Step 1: Based on the pose of the mobile robot and the depth image data of the depth camera, a high-resolution and detailed three-dimensional environmental map is constructed in real time; based on the detailed three-dimensional environmental map, the detection and clustering of the front units are carried out, and multiple candidate viewpoints are sampled and generated for each front cluster, and the one with the largest number of observable front units for the current front cluster is selected as the best viewpoint;

[0007] Step 2: Based on the pose of the mobile robot, the two-dimensional lidar point cloud, and the depth map data of part of the depth camera, a low-resolution and rough three-dimensional environmental map is constructed in real time;

[0008] Step 3: Using the detailed three-dimensional map and the rough three-dimensional map, a two-dimensional hybrid grid map that can represent the environmental contour information near the current position of the robot is constructed;

[0009] Step 4: Based on the two-dimensional hybrid grid map, small front clusters and isolated front cluster regions are detected and marked in real time;

[0010] Step 5: A visit cost matrix is constructed, and the visit order of sequentially visiting the best viewpoints of each front cluster from the current position with the lowest overall visit cost is obtained by solving; for the global front cluster visit order, a refined viewpoint visit path within the local area centered on the current position of the robot is obtained through local path optimization;

[0011] Step 6: Based on the path information gain optimization strategy, the robot's azimuth and path trajectory that takes into account the safety on the way to the next target path and the coverage ability of the front units are planned;

[0012] Step 7: The controller obtains the planned trajectory and calculates the control amount that enables the robot to move along the trajectory; at the same time, the on-board camera senses the environmental information in real time and gradually constructs the environmental map;

[0013] Step 8: Repeat the above steps 1 to 7 until there are not enough frontiers in the map, which is regarded as the completion of the environmental perception task.

[0014] Further, step 3 is specifically as follows:

[0015] Step 3-1: Taking the current position of the robot as the center and the maximum sensing range of the 2D lidar as the radius, project the low-resolution 3D occupancy grid map within the range along the Z-axis direction, and perform morphological processing on it to obtain a 2D occupancy grid map centered on the robot;

[0016] Step 3-2: For each grid cell in the 2D occupancy grid map, query the cell state at its corresponding position in the high-resolution 3D occupancy grid map, thereby fusing the environmental perception situations of the two sensors, the 2D lidar and the depth camera, to generate a 2D hybrid grid map;

[0017] For the generated 2D hybrid grid map, its cells include three types of states: unknown state cells, known free state cells, and occupied cells; among them, unknown state cells represent spaces not observed by the 2D lidar and the depth camera, known free state cells are spaces observed by the 2D lidar as known spaces but not observed by the depth camera, and the remaining spaces are marked as occupied state cells.

[0018] Further, step 4 is specifically as follows:

[0019] Step 4-1: For each frontier cluster to be visited, taking its best viewing point position as the starting point and the positions of a series of frontier cells as the intermediate points, calculate a series of unit rays; subsequently, taking the positions of the series of frontier cells as the starting points, along the corresponding unit ray directions, calculate the size of the potentially exploitable area behind the frontier cluster in combination with the information of the 2D hybrid grid map. If the size of the exploitable area is less than the set threshold, mark this frontier cluster area as a small frontier cluster area;

[0020] Step 4-2: Binarize the 2D hybrid grid map, where the grid cells sensed by the lidar but not sensed by the depth camera are assigned a value of 1, and the rest are assigned a value of 0; then, use morphological processing means to calculate a series of connected regions with a state of 1, and the connected regions with an area within the threshold range are marked as isolated regions; then, the frontier cluster areas that have an intersection with the isolated regions are marked as isolated frontier cluster areas.

[0021] Further, step 5 is specifically as follows:

[0022] Step 5-1: Construct a cost function that comprehensively considers special frontier regions, access costs, and motion continuity, where a higher access priority weight is assigned to special frontier regions;

[0023] Step 5-2: According to the cost function, assign values to the cost matrix, and use the traveling salesman problem solver to obtain the global access order of sequentially visiting a series of frontier clusters from the current position;

[0024] Step 5-3: Intercept the global access order within a preset radius centered on the current robot, and use a series of viewpoints in the involved frontier clusters for local path optimization, and finally solve to obtain the next access target.

[0025] Further, the specific steps of Step 6 are as follows:

[0026] Step 6-1: After determining the next exploration target, calculate the path trajectory time from the current position to the target;

[0027] Step 6-2: Traverse the best viewpoints of the frontier clusters near the current position of the robot, and select the azimuth angle of the best viewpoint that satisfies the trajectory time constraint as the intermediate azimuth angle, and preferentially select the intermediate azimuth angle from the special frontier cluster area;

[0028] Step 6-3: If the intermediate azimuth angle is successfully selected in Step 6-2, perform azimuth angle trajectory planning from the current azimuth angle to the intermediate azimuth angle and from the intermediate azimuth angle to the next target azimuth angle; if not selected, directly perform trajectory planning from the current to the next target azimuth angle.

[0029] An autonomous exploration system combining unknown environment contour information, including a perception module, a map construction module, a candidate target generation module, and a motion planning module;

[0030] The perception module can configure the parameters of the sensors on the mobile robot, support dynamic adjustment of the sensor perception range to adapt to the real-time perception requirements in different complex environments.

[0031] The map construction module receives the sensor data input provided by the perception module, and uses the Fiesta map structure to maintain a high-resolution 3D map in real time, and uses the Octomap structure to maintain a low-resolution 3D map in real time;

[0032] The candidate target generation module performs frontier detection clustering and viewpoint sampling, small frontier and isolated frontier cluster area detection, and solves the traveling salesman problem for the next access target based on the map construction module;

[0033] The motion planning module obtains the next access target solved by the candidate target generation module, performs azimuth angle trajectory and path trajectory planning, and generates a smooth motion trajectory of the robot.

[0034] The beneficial effects of the present invention are as follows:

[0035] 1. The present invention utilizes a path information gain optimization strategy, and additionally considers the optimization of the robot's azimuth angle trajectory while planning the path, enabling the UAV to effectively avoid obstacles during flight and ensuring the safety of space exploration missions.

[0036] 2. The present invention selects a low-cost, low-weight, and low-power two-dimensional lidar sensor. By fusing the sensor data of the depth image and the two-dimensional lidar, a hybrid map that can intuitively represent the contour information of the robot's surrounding environment is effectively constructed. Based on this, small front clustering and isolated front clustering regions can be detected in a timely manner, effectively reducing the reciprocating motion during the robot's exploration. The overall exploration time of the proposed algorithm is improved by 15%-25% compared with existing advanced algorithms. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 is a block diagram of the method of the present invention;

[0038] Figure 2 is a flowchart for constructing a hybrid map;

[0039] Figure 3 is the effect of removing noise points and morphological processing of a two-dimensional grid map, (a) mapping with pure lidar point cloud, (b) mapping with lidar + depth map point cloud, (c) adding a point cloud radius filter, (d) performing morphological processing;

[0040] Figure 4 is a schematic diagram of the generation of a two-dimensional hybrid map;

[0041] Figure 5 is a schematic diagram of the process of detecting small front clustering based on a two-dimensional hybrid map;

[0042] Figure 6 is a schematic diagram of the process of detecting isolated front clustering based on a two-dimensional hybrid map;

[0043] Figure 7 is a schematic diagram of the problems existing in the azimuth angle planning of existing exploration algorithms, (a) the true structure diagram of the explored scene, (b) the grid map constructed during the exploration process, (c) a simplified representation of sub-diagram (b);

[0044] Figure 8 is a schematic diagram of a physical flight platform;

[0045] Figure 9 is a schematic diagram of two real scenarios of an embodiment of the present invention, (a) an indoor exploration experiment scenario, (b) an outdoor exploration experiment scenario;

[0046] Figure 10 is a schematic diagram of the grid maps constructed after space exploration in two real scenarios, (a) the grid map and the UAV flight trajectory in the indoor experiment scenario, (b) the grid map and the UAV flight trajectory in the outdoor experiment scenario;

[0047] Figure 11 Coverage curves for spatial exploration in two real scenarios, (a) coverage curve in an indoor scenario, (b) coverage curve in an outdoor scenario. Detailed implementation manners

[0048] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0049] The present invention uses a depth camera as the main sensor for constructing an environmental map, extracts important environmental contour information by using a lidar to guide the exploration process, and improves the overall spatial exploration efficiency by reducing the repeated access rate of the drone to the same area.

[0050] The purpose of the present invention is to provide an autonomous exploration method and system combined with unknown environmental contour information. The proposed method and system can be deployed on mobile robot platforms such as drones and unmanned vehicles to solve problems existing in existing exploration algorithms based on depth cameras, such as low exploration efficiency and easy collision of the planned path with obstacles.

[0051] The purpose of the present invention is achieved through the following steps:

[0052] S1. Based on the pose of the mobile robot and the depth image data of the depth camera, a fine three-dimensional environmental map is constructed in real time; based on the fine three-dimensional map, the detection and clustering of the frontier cells are carried out, and many candidate viewpoints are sampled and generated for each frontier cluster, and the one with the best observation ability for the current frontier cluster is selected as the best viewpoint;

[0053] S2. Based on the pose of the mobile robot, the two-dimensional lidar point cloud and the depth map data of part of the depth camera, a rough three-dimensional environmental map is constructed in real time;

[0054] S3. Using the fine three-dimensional map and the rough three-dimensional map, a two-dimensional hybrid grid map capable of characterizing the environmental contour information near the current position of the robot is constructed;

[0055] (1) Taking the current position of the robot as the center and the maximum sensing range of the two-dimensional lidar as the radius, the low-resolution three-dimensional occupancy grid map within the range is projected along the Z-axis direction and subjected to morphological processing to obtain a two-dimensional occupancy grid map centered on the robot;

[0056] (2) For each grid cell in the two-dimensional occupancy grid map obtained in the above step, query the cell state at its corresponding position in the high-resolution three-dimensional occupancy grid map, and thus fuse the environmental perception situations of the two sensors of the two-dimensional lidar and the depth camera to generate a two-dimensional hybrid grid map.

[0057] (3) For the generated two-dimensional hybrid grid map, its cells include three types of states: unknown, known free, and occupied. Among them, the unknown state cells represent the space not observed by the two-dimensional lidar and depth camera. The known free state cells are the space observed by the two-dimensional lidar as known space but not observed by the depth camera. The remaining space is marked as occupied state cells.

[0058] S4. Based on the two-dimensional hybrid grid map, real-time detect and mark the small frontier clusters and isolated frontier cluster regions that are often missed by existing algorithms during the exploration process;

[0059] (1) For each frontier cluster to be visited, starting from its best viewpoint position, with the positions of a series of frontier cells as intermediate points, a series of unit rays are calculated. Subsequently, starting from the positions of the series of frontier cells, along the corresponding unit ray directions, combined with the information of the two-dimensional hybrid grid map, calculate the size of the potentially explorable area behind the frontier cluster. If the size of the explorable area is less than the set threshold, mark this frontier cluster region as a small frontier cluster region;

[0060] (2) Binarize the two-dimensional hybrid grid map, where the grid cells perceived by the lidar but not by the depth camera are assigned a value of 1, and the rest are assigned a value of 0. Then, use morphological processing means to calculate a series of connected regions with a state of 1. Among them, the connected regions with an area within the threshold range are marked as isolated regions. Subsequently, the frontier cluster regions that have an intersection with the isolated regions will be marked as isolated frontier cluster regions.

[0061] S5. Construct an access cost matrix, and solve for the access order that visits the best viewpoints of each frontier cluster in turn with the lowest overall access cost from the current position; for the global frontier cluster access order, obtain a refined viewpoint access path within the local area centered on the current position of the robot through local path optimization;

[0062] (1) Construct a cost function that comprehensively considers special frontier regions, access costs, and motion continuity, where a higher access priority weight will be assigned to special frontier regions;

[0063] (2) According to the cost function, assign values to the cost matrix, and use a traveling salesman problem solver to obtain the global access order that visits the best viewpoints of a series of frontier clusters in turn from the current position;

[0064] (3) Intercept the global access order within a preset radius centered on the current robot, and use a series of viewpoints in the involved frontier clusters for local path optimization, and finally solve for the next access target.

[0065] S6. Based on the path information gain optimization strategy, plan the azimuth angle and path trajectory of the robot that takes into account the safety during the path to the next target and the coverage ability of the frontier cells.

[0066] (1) After determining the next exploration target, calculate the path trajectory time from the current position to the target.

[0067] (2) Traverse the best viewpoints of the frontier clusters near the current position of the robot, and select the azimuth angle of the best viewpoint that meets the trajectory time constraint as the intermediate azimuth angle, where the intermediate azimuth angle is preferentially selected from the special frontier cluster areas.

[0068] (3) If there is a suitable intermediate azimuth angle, perform the azimuth angle trajectory planning from the current to the intermediate azimuth angle and from the intermediate azimuth angle to the next target azimuth angle; if not, directly perform the trajectory planning from the current to the next target azimuth angle.

[0069] S7. The controller obtains the planned trajectory and calculates the control quantity to make the robot move along the trajectory; at the same time, the on-board camera perceives the environmental information in real time and gradually constructs an environmental map.

[0070] S8. Repeat the above steps until there are not enough frontiers in the map, which is regarded as the completion of the environmental perception task.

[0071] An autonomous exploration system combining unknown environmental contour information, including a perception module, a map construction module, a frontier area detection module, and a target selection module, is used to drive the robot to achieve the autonomous exploration ability of the unknown environment.

[0072] The map construction module uses the Fiesta map structure to maintain a high-resolution 3D map in real time and uses the Octomap structure to maintain a low-resolution 3D map in real time.

[0073] The perception module supports dynamically adjusting the sensor perception range to meet the real-time perception requirements in different complex environments.

[0074] Embodiment:

[0075] Figure 1 It is a schematic diagram of the framework process of the autonomous exploration method for integrating unknown environmental contour information of the present invention. The software algorithm framework of the autonomous exploration method and system includes the following steps:

[0076] Step 1, the mobile robot needs to provide depth image data, robot self-pose estimation data, and two-dimensional lidar point cloud data for the algorithm system, and at the same time provide a controller interface for receiving external control instructions.

[0077] Step 2: The map construction module will respectively and real-time maintain a fine 3D map, a rough 3D map, and a 2D hybrid grid map. The fine 3D map is constructed from depth images and pose estimation data, and is used to characterize the perception of environmental information by the robot using a depth camera; the rough 3D map is constructed from 2D lidar point clouds, partial depth images, and pose estimation data, and is used to characterize the perception of environmental contour information by the robot using a 2D lidar; the 2D hybrid map is constructed by fusing the fine 3D map and the rough 3D map, and is a map centered on the current position of the robot with half of the maximum perception range of the 2D lidar as the side length, and is used to characterize the difference in the perception information of the same environment by the two sensors.

[0078] Step 3: The candidate target generation module will use the map information maintained by the map construction module to generate candidate targets and solve for the optimal order of the current series of candidate targets to be visited. First, using the fine 3D map, detect and cluster the frontier cells, and sample a series of candidate viewpoints for each frontier cluster. If the number of frontier cells in the map is not sufficient at this time, it is considered that the exploration task is over, otherwise the subsequent program is executed. After that, use the 2D hybrid grid map to detect small frontier clusters and isolated frontier clusters, and construct a cost matrix to solve the traveling salesman problem to obtain the next target to be visited.

[0079] Step 4: The motion planning module will generate a path and azimuth trajectory from the current position to the next target based on the path information gain optimization strategy, and transmit the trajectory to the mobile robot to control the robot's movement and further perceive environmental information.

[0080] Figure 2 The following is the specific construction process of the 2D hybrid grid map in the map construction module involved in the present invention. This construction process includes the following sub-steps:

[0081] (1) Filter the data at a given height in the depth image and the 2D lidar point cloud data to remove unreasonable outliers, and then construct a rough 3D map.

[0082] (2) Given the current position of the robot and the maximum perception range of the 2D lidar, project the corresponding area of the rough 3D map along the Z-axis direction, and combine morphological processing techniques (such as removing small connected regions, dilation and erosion) to obtain a 2D grid map used to characterize the perception of the surrounding environment by the 2D lidar.

[0083] Figure 3 The following is a schematic diagram of the effect of constructing a 2D grid map based on steps (1) and (2) of the present invention.

[0084] (3) To ensure the real-time performance of the exploration algorithm, the present invention crops a local two-dimensional grid map centered on the current position of the robot based on the current position of the robot and the maximum sensing range of the two-dimensional lidar. Each cell in the local two-dimensional grid map belongs to three states {-1, 1, 100}: -1 represents an unknown cell, 1 represents a known free cell, and 100 represents a cell with an obstacle. For this local grid map, the state of each cell on the query map in the fine three-dimensional grid map will be queried, and a hybrid map capable of characterizing the contour information of the unexplored environment near the current position of the robot will be generated as follows:

[0085]

[0086] Wherein, is the state of each cell in the local two-dimensional grid map, is the state of a series of grid cells within a certain height range in the corresponding position of the fine three-dimensional map. If there is an occupied cell within the current height range, the state of the corresponding cell in the hybrid map is updated to 100; if there is no occupancy but there is an unknown cell, the current state is maintained; otherwise, the state of the corresponding cell is updated to 80. It should be noted that in the cell with a state value of 80 represents the free space that has been explored and no longer has exploration value in and will be classified as an occupied cell in subsequent processing. After the above operations, a hybrid map capable of characterizing the contour information of the unexplored environment near the robot can be obtained. The unknown and known cells in the hybrid map are all potential areas to be explored, while the occupied cells are areas with no exploration value.

[0087] Figure 4 FIG.

[0088] Figure 5 shows the effect of constructing a two-dimensional hybrid grid map based on the foregoing step (3) of the present invention.

[0089] (1) Data preprocessing: First, downsampling is performed on the original front clustering cells, and a series of front cells obtained by downsampling are used as the starting points C lidarstart of the light rays. This process mainly considers the front cells close to the current height of the robot and projects them onto the two-dimensional plane. The purpose of downsampling is to reduce the computational amount while maintaining the representativeness of their spatial distribution. Subsequently, based on the size of the downsampled point set, the number N ray of ray projections to be performed is determined.

[0090] (2) Determination of the end point of the light ray: For each downsampled front unit c i , in combination with the maximum effective sensing range R of the lidar max and the normalized direction pointing from the viewpoint position to the unit c i , determine the end point position of the ray projection. At the same time, calculate the average ray direction R for calculating the lidar gain dir , which is used to assist in calculating the subsequent isolated front clustering area.

[0091] (3) Ray projection and counting: For each ray with a determined starting point, perform a light ray projection operation on the hybrid map. Specifically, starting from the starting point, check the status of the grid map units along the ray direction. This process continues until an occupied grid unit is encountered or the preset end point position is reached. During the ray projection process, count the number C of potentially explorable units lidar , which directly reflects the number of unknown units to be explored behind this front clustering. Thus, the size G of the potentially explorable area at each front clustering set can be calculated lidar (unit: m) as follows:

[0092]

[0093] where M hybrid .res() is the resolution of the two-dimensional hybrid grid map. For the front area where the size of the explorable area is less than the set threshold thr small , it will be marked as a small front clustering and given a higher weight for priority access. The cost function c k of the small area front clustering ftr s is as follows:

[0094]

[0095] where k s is a constant value greater than thr small , and the smaller the G libar of the front clustering, the higher the access priority. At the same time, to avoid unnecessary back-and-forth maneuvers caused by over-focusing on the small front clustering area, only consider the small front clustering areas whose average position is within the range of F small from the current position when solving the next target each time.

[0096] Figure 6 This is a schematic diagram of the effect of the present invention for detecting isolated clustering areas based on the constructed two-dimensional hybrid grid map. The detection process of this small front clustering area includes the following sub-steps:

[0097] (1) First, divide the cell states of the two-dimensional hybrid grid map into two categories {0, 1} to generate a binary image, where the occupied or explored cell states are marked as 1, and the known free or unknown cell states are marked as 0.

[0098] (2) Perform connected region detection on the binary image to obtain a series of connected regions located in the inner layer with a state of 0. If the size of the connected region meets the set minimum area threshold, it is regarded as an isolated connected region with exploration value. Then, solve the minimum external rectangle enclosing each connected region and record its four vertices as the outer contour.

[0099] (3) Determine whether the average position of the front cluster near the isolated connected region is located within the region enclosed by the four recorded vertices. For the front cluster located within any isolated connected region, mark it as an isolated front cluster region. And calculate the isolated region cost c k of the front cluster ftr iso (k) as follows:

[0100]

[0101] where k iso is an empirical weight used to adjust the access priority to the isolated front cluster region. Considering that there are usually multiple isolated front cluster regions within a connected region. Therefore, maintaining the cost difference between these isolated front cluster regions is beneficial for planning a smoother coverage path. For this reason, different from the small front cluster, the present invention will assign the same priority access weight coefficient to all isolated front clusters. Once an isolated front cluster region is detected, a higher access priority will prompt the robot to preferentially cover these regions to avoid inefficient exploration caused by reciprocating motion.

[0102] After the present invention completes Figure 5 and Figure 6 the detection of the special front cluster regions shown, the problem of sequentially accessing a series of candidate targets from the current position is described as an asymmetric traveling salesman problem, that is, only the order of sequentially accessing the candidate targets from the current position needs to be solved, without caring about which candidate target to return to the current position. The present invention constructs a cost matrix M (n+1)(n+1) , where n is the number of the front cluster set, and the index of the current position is 0. The cost from the candidate target to the current position is set to 0 because the present invention does not care about the order of returning to the current position.

[0103] M tsp (k, 0) = 0 (5)

[0104] In addition, the access cost between the front cluster sets mainly considers the travel time constraint, as shown in the following formula:

[0105] M tsp (i, j) = M tsp (j, i) = t lb (V i , V j ), i, j ∈ {1, 2,..., n} (6)

[0106]

[0107] Wherein, V i represents the best viewing point at the leading cluster fit i p i is the position of the viewing point, θ i is the azimuth angle of the viewing point, v max is the maximum linear velocity constraint of the robot, is the maximum angular velocity constraint.

[0108] For the cost of accessing a series of leading clusters at the current position, the travel time, motion continuity, special leading cluster constraints, and boundary constraints will be comprehensively considered, and the calculation method is as follows:

[0109] M tsp (0, k) = t lb (V0, V k ) + w c * c c (V k ) - w s * c s (k) - w iso * c iso (k) (8)

[0110] Wherein, w c is the motion continuity penalty factor, w s is the reward weight coefficient for small leading clusters, w iso is the reward weight coefficient for isolated leading clusters. c c (V k ) will penalize the leading clusters with a large change in the current motion direction. The boundary cost c b (k) is the minimum distance from the average position p k of the candidate viewing point to the map boundary, which is used to provide a guiding constraint for the driving direction of the robot. Given the cost matrix constructed above, a TSP solver can be used to obtain an access order that starts from the current position and sequentially accesses each leading cluster.

[0111] Figure 7For the existing exploration algorithm to directly perform the azimuth trajectory planning from the current azimuth angle to the target observation perspective, it is shown that when the UAV executes the position trajectory, it cannot observe the environmental obstacle information in the moving direction, resulting in a collision between the UAV and the obstacle. To ensure the safety and coverage efficiency of the current position to the next target, this paper proposes a path information gain optimization strategy to increase the amount of environmental information that the UAV can obtain during the flight to the next target. Before each azimuth trajectory planning, the present invention calculates the angle between the moving direction of the current path trajectory and the target yaw angle θ next in the direction. If the angle is greater than the preset safety angle θ safe , the angle θ temp corresponding to the moving direction of the current path trajectory will be calculated and used as the temporary target yaw for azimuth trajectory planning to ensure that the UAV can safely reach the next access target. In the experiment, the safety angle θ safe is set to half of the horizontal field of view angle of the on-board depth camera.

[0112] In addition, if the angle is less than the preset safety angle, in order to improve the coverage ability of the UAV for the adjacent unknown environment during the flight to the next target, the present invention will first select a suitable intermediate yaw angle target from the adjacent small front or isolated front clustering area, and generate an information-rich azimuth trajectory by considering time constraints and other situations. This process includes the following sub-steps:

[0113] (1) First, obtain the set V now of the best viewpoints of all front clusterings within the radius R near of the current position p local , and calculate the time T lb required to go from the current position to the next target.

[0114] (2) Then, traverse each viewpoint element in the local viewpoint set V local . Estimate the time T1 required to turn from the current azimuth angle to the azimuth angle at the viewpoint, and the time T2 required to turn from the azimuth angle at the viewpoint to the target azimuth angle. If the total time T min required for the two yaw movements is less than the path time T lb and the azimuth angle at the viewpoint is not the same as the initial azimuth or the target azimuth angle, record the current azimuth angle as the selectable intermediate azimuth angle. The process will iteratively select the intermediate azimuth angle that satisfies the time constraint and has the longest azimuth movement time to fully observe the nearby environmental information.

[0115] (3) Finally, if an intermediate azimuth angle is selected after traversing the local viewpoint set V local , plan the azimuth trajectory from the current azimuth to the intermediate azimuth angle and then to the target azimuth angle in sequence. Otherwise, normally plan the azimuth trajectory from the current to the target.

[0116] The present invention has been implemented in a real - machine deployment on a four - rotor UAV platform, and experimental verifications have been carried out in the two real sites shown below. Figure 9 The experimental verifications have been carried out in the two real sites shown below.

[0117] Figure 8 The four - rotor platform for the real - machine deployment of the present invention has a diameter of 380 mm between the motor shafts of two opposite corners, and is equipped with an RGB - D camera, a 2D lidar, and an Nvidia in - vehicle computer. In addition, the researchers also used an Intel T265 camera to estimate the position of the UAV. In the outdoor scenario, the motion constraints of the UAV are set as: the maximum speed is v max = 0.75 m / s, the maximum acceleration is a max = 0.75 m / s 2 and the maximum yaw angular velocity is θ max = 0.75 rad / s. In the indoor scenario, the maximum speed is v max = 0.5 m / s, and the maximum acceleration is a max = 0.5 m / s 2 .

[0118] The present invention was first tested in an outdoor courtyard environment. The size of the exploration map is 27×16×2.1 cubic meters, and the test results are as shown in Figure 10 and Figure 11 . Since there are branches and weeds in the scene, the detection threshold thr small of a small area is set to 1.0 m. The total time of the entire exploration process is 190 s, and the total length of the flight path is 120 m. During the exploration process, the multi - sensor - based hybrid map quickly detected and covered the special frontier areas, ensuring the stable growth trend of the coverage curve in Figure 11 . Subsequently, the present invention was also tested in an indoor environment. The map size is 22×10×1.8 cubic meters, the exploration time is 100 s, and the flight path length is 42 m. The test results show that the algorithm proposed by the present invention can quickly cover the special frontier areas detected in different scenarios, demonstrating the ability to efficiently explore unknown environments. It should be noted that the minimum height preset for the exploration area by the present invention is - 0.1 m. Due to the uneven terrain of the courtyard, the existence of large - area depressions, and visual positioning drift, Figure 10 there are many visual blank areas (no point clouds) in

Claims

1. An autonomous exploration method combining unknown environment contour information, characterized in that: The steps include: Step 1: Based on the position and posture of the mobile robot and the depth image data of the depth camera, a high-resolution and detailed 3D environment map is constructed in real time; Detect and cluster frontier units based on a detailed 3D environment map, generate multiple candidate viewpoints for each frontier cluster sampling, and select the viewpoint with the largest number of observable frontier units for the current frontier cluster as the best viewpoint; Step 2: Based on the position and posture of the mobile robot, the 2D LiDAR point cloud and the depth map data of some depth cameras, a low-resolution rough 3D environment map is constructed in real time; Step 3: Using the fine 3D map and the coarse 3D map, a 2D hybrid grid map is constructed that can represent the environmental contour information near the current position of the robot; Step 4: Based on the two-dimensional hybrid grid map, small frontier clusters and isolated frontier cluster areas are detected and marked in real time; Step 5: Construct an access cost matrix and solve the access sequence of the best viewpoints of each frontier cluster from the current position with the lowest overall access cost; for the global frontier cluster access sequence, obtain the refined viewpoint access path in the local area centered on the current position of the robot through local path optimization; Step 6: Based on the path information gain optimization strategy, the robot's azimuth and path trajectory are planned to take into account both the safety and frontier unit coverage capabilities on the way to the next target. Step 7: The controller obtains the planned trajectory and solves the control amount that makes the robot move along the trajectory; at the same time, the onboard camera perceives the environmental information in real time and gradually builds the environmental map; Step 8: Repeat steps 1 to 7 above until there are no sufficient frontiers in the map, which is considered to complete the environment perception task.

2. The autonomous exploration method according to claim 1, characterized in that: The step 3 is specifically as follows: Step 3-1: With the current position of the robot as the center and the maximum sensing range of the two-dimensional laser radar as the radius, the low-resolution three-dimensional occupancy grid map within the range is projected along the Z-axis direction, and morphologically processed to obtain a two-dimensional occupancy grid map centered on the robot; Step 3-2: For each grid cell in the 2D occupancy grid map, query the cell status of its corresponding position in the high-resolution 3D occupancy grid map, thereby integrating the environmental perception of the two sensors, the 2D LiDAR and the depth camera, to generate a 2D hybrid grid map; Step 3-3: For the generated two-dimensional hybrid grid map, its cells include three states: unknown state cells, known idle state cells and occupied state cells; among them, unknown state cells are represented as spaces that are not observed by the two-dimensional lidar and the depth camera, known idle state cells are spaces that are observed as known spaces by the two-dimensional lidar but not observed by the depth camera, and the remaining spaces are marked as occupied state cells.

3. The autonomous exploration method according to claim 2, characterized in that: The step 4 is specifically as follows: Step 4-1: For each frontier cluster to be visited, take its best viewpoint as the starting point and the position of the series frontier unit as the middle point to calculate a series of unit rays; then, take the position of the series frontier unit as the starting point, follow the corresponding unit ray direction, and calculate the size of the potential explorable area behind the frontier cluster in combination with the information of the two-dimensional hybrid grid map. If the size of the explorable area is less than the set threshold, mark the frontier cluster area as a small frontier cluster area; Step 4-2: Binarize the two-dimensional hybrid grid map, where the grid cells sensed by the lidar but not by the depth camera are assigned a value of 1, and the rest are assigned a value of 0; then, a series of connected areas with a state of 1 are calculated using morphological processing methods, where the connected areas with an area within the threshold range are marked as isolated areas; then, the frontier cluster areas that intersect with the isolated areas are marked as isolated frontier cluster areas.

4. The autonomous exploration method according to claim 3, characterized in that: The step 5 is specifically as follows: Step 5-1: Construct a cost function that comprehensively considers special frontier areas, access costs, and motion continuity, in which a higher access priority weight is given to special frontier areas; Step 5-2: According to the cost function, the cost matrix is ​​assigned, and the global visit order of the best viewpoints of the series frontier clusters is obtained by using the traveling salesman problem solver from the current position; Step 5-3: Take the current robot as the center, preset the global access order within the radius, and use the series of viewpoints in the frontier cluster involved to perform local path optimization, and finally solve the next access target.

5. The autonomous exploration method according to claim 4, characterized in that: The step 6 is specifically as follows: Step 6-1: After determining the next exploration target, calculate the path trajectory time from the current position to the target; Step 6-2: Traverse the best viewpoints of the frontier clusters near the current position of the robot, and select the azimuth of the best viewpoint that meets the trajectory time constraint as the intermediate azimuth, wherein the intermediate azimuth is selected preferentially from the special frontier cluster area; Step 6-3: If the intermediate azimuth is successfully selected in step 6-2, azimuth trajectory planning is performed from the current azimuth to the intermediate azimuth and from the intermediate azimuth to the next target azimuth; if it is not selected, trajectory planning is performed directly from the current azimuth to the next target azimuth.

6. A system using the autonomous exploration method as claimed in claim 1, characterized in that: It includes perception module, map building module, candidate target generation module and motion planning module; The perception module can configure the parameters of the sensors on the mobile robot and support dynamic adjustment of the sensor perception range to meet the real-time perception needs in different complex environments; The map building module receives sensor data input provided by the perception module, uses the Fiesta map structure to maintain a high-resolution 3D map in real time, and uses the Octomap structure to maintain a low-resolution 3D map in real time; The candidate target generation module performs frontier detection clustering and viewpoint sampling, small frontier and isolated frontier clustering area detection and traveling salesman problem solving for the next visited target based on the map construction module; The motion planning module obtains the next visited target solved by the candidate target generation module, plans the azimuth trajectory and the path trajectory, and generates a smooth motion trajectory of the robot.

Images