Unmanned aerial vehicle layered exploration method based on gravity mechanism viewpoint generation

Abstract

The invention discloses an unmanned aerial vehicle layered exploration method based on gravity mechanism viewpoint generation. According to the method, a layering mode is used, and different gravity mechanisms are provided for a fine scene and a wide scene: in the wide scene, the unmanned aerial vehicle uses an interactive lateral gravity mechanism to update a viewpoint, so that the viewpoint is closer to a vehicle body of the unmanned aerial vehicle, and unnecessary long-distance maneuver caused by the fact that the viewpoint is far away from the unmanned aerial vehicle is reduced; in a fine scene, a narrow-channel gravity mechanism is used for sampling and generating a viewpoint, so that the generating radius of the viewpoint is smaller, the unmanned aerial vehicle has a longer flight path plan, low-efficiency maneuvering of advancing-retreating is reduced, and finally the unmanned aerial vehicle completes an exploration task in a high-precision and high-efficiency mode.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous unmanned exploration of unmanned aerial vehicles in unknown environments, and in particular relates to a layered exploration method of unmanned aerial vehicles based on viewpoint generation of a gravity mechanism. Background Art

[0002] Autonomous exploration by drones has shown great potential in emergency rescue, ground-air coordination, and other aspects, and its related progress has also attracted the attention of many researchers in recent years. Generally speaking, autonomous exploration by drones means that within a designated area, drones can autonomously explore and scan the environment in the area by sequentially executing processes such as environmental perception data acquisition, incremental mapping and environmental representation, track problem modeling, track generation, and track action execution, and then restore the scene in the designated area by mapping for use in other tasks such as drone and unmanned vehicle path planning.

[0003] However, existing methods still have limitations in multi-scale environments and cannot adapt well to changes in environmental size, which greatly reduces the corresponding exploration efficiency. Specifically, existing methods generally limit the autonomous exploration environment of drones to relatively wide areas, and the efficiency and safety of drones in performing these exploration tasks can be better guaranteed. However, in multi-scale scenes with both wide and narrow scenes, existing methods face conflicts in efficiency and precision: when the obstacle expansion coefficient is high, the drone exploration efficiency is relatively high and the planned trajectory is smooth, but narrow scenes cannot be entered; when the expansion coefficient is low, the drone exploration accuracy is guaranteed and narrow areas can be entered smoothly, but there are more wandering and reciprocating phenomena in maneuvers. Summary of the invention

[0004] In view of this, the present invention provides a UAV hierarchical exploration method based on gravity mechanism viewpoint generation, so that the UAV uses an innovative gravity mechanism viewpoint generation mechanism in an unknown multi-scale environment to complete the exploration task in a high-precision and high-efficiency manner. The method acquires the sensor pose and depth image based on the forward-looking depth camera of the UAV at the current moment, and incrementally constructs a global map; uses an overlapping area judgment mechanism to update all frontiers in the global map; judges whether the scene scale is a fine scene / wide scene; uses a narrow channel gravity mechanism to sample and generate viewpoints / uses an interactive lateral gravity mechanism to update viewpoints according to the scene scale being fine / wide; generates an ATSP cost matrix using the current architecture viewpoint, and solves the matrix to obtain a UAV waypoint sequence; calculates the number of remaining frontiers, and continues the cycle if it is not zero, and ends the exploration if it is zero.

[0005] A UAV hierarchical exploration method based on gravity mechanism viewpoint generation, including:

[0006] Step 1: The drone uses the forward-looking depth camera to obtain the sensor pose T and depth image Idepth , incrementally build the global map M globe ;

[0007] Step 2: Use the overlapping area judgment mechanism to update all frontiers in the global map at the current time t, and then obtain the frontier set F t ;

[0008] Step 3: Calculate the frontier set F t The number of remaining frontiers in the loop. If the return value is not zero, the next step is executed. If the return value is zero, the end point is reached and the loop ends.

[0009] Step 4: Determine the scene scale and confirm whether the current scene is a detailed scene or a wide scene;

[0010] Step 5: If it is a fine scene, the narrow-channel gravity mechanism is used to sample and generate the viewpoint. If it is a wide scene, the interactive lateral gravity mechanism is used to update the viewpoint.

[0011] Step 6: Generate the ATSP cost matrix Matrix using the viewpoints generated in step 5 cost , solve the matrix to get the UAV waypoint sequence P waypoints , return to step 1.

[0012] Preferably, in step 1, after obtaining the sensor posture T and depth image I depth After that, the depth image ID epth Each of them has pixel coordinates (u n , v n ), the depth value is Z n The following operations are performed on the pixels:

[0013]

[0014] Among them, P w for That is, the coordinates of the pixel in the world coordinate system, T is the current position of the sensor obtained, and K is the intrinsic parameter matrix of the camera;

[0015] Repeat the above steps for each depth image, convert each pixel into a point in the world coordinate system, and accumulate these point clouds to form a complete three-dimensional point cloud M at the current time t t ;

[0016] Next, the global map M is incrementally constructed by using the complete 3D point cloud at the current moment and the 3D point clouds at all past moments through the point cloud registration method. globe .

[0017] Preferably, in step 2, the method for updating all frontiers in the global map at the current time t includes:

[0018] First, the complete three-dimensional point cloud M at the current time t t Convert to voxel map V t , and then classify the voxel states into occupied, unknown or free according to the occupancy state; using the growing algorithm, cluster the voxels in the same area that are adjacent to the unknown voxels into the frontier f k ; Among them, f k It contains all the constituent voxel cells, the cluster average position mean_position and the cluster bounding box coordinates (x, y, z).

[0019] Preferably, in step 2, the frontier set F is obtained t The methods include:

[0020] The voxel map V detected at the current moment t Boundary and previous moment set F t-1 All the frontier bounding box coordinates (x, y, z) in the voxel map V are used for overlapping area judgment. If (x, y, z) is included in the voxel map V t In the boundary, delete this frontier; and so on, calculate the voxel map V at the current moment t The frontier in k , add it to the previous moment set F that has just been processed t-1 In the current moment, we get the frontier set F t .

[0021] Preferably, in step 4, the method for determining the scene scale includes:

[0022] All frontiers deleted at the current time t form a set AF t , calculate its cluster average position mean_position and cluster bounding box coordinates (x, y, z); use the three sets of abandoned boundary AF that have recently entered the dormant state t , AF t-1 and AF t-2 As a judgment sequence, determine whether the current scene is a detailed scene or a broad scene:

[0023] The first step is to determine the three groups of abandoned boundaries AF t , AF t-1 and AF t-2 Whether all sizes meet the narrow requirement. If so, proceed to step 2 to determine whether it is a narrow scene. If one set does not meet the requirement, it is determined to be a wide scene and step 5 is executed.

[0024] The second step is to determine the abandoned boundary AF in three consecutive updates t , AF t-1 and AF t-2 Arrangement characteristics:

[0025]

[0026] in:

[0027] m t =AF t .mean_position

[0028] Judgment mechanism:

[0029]

[0030] This determines the scale of the scene.

[0031] Preferably, in step 4, whether the stenosis requirement is met is: discarding the boundary AF t , AF t-1 and AF t-2 The 3D coordinates of any voxel in AF t Whether the distance of the average position of the cluster is less than the set threshold.

[0032] Preferably, in step 5, according to the judgment of the fine scene and the wide scene, the viewpoint generation method is determined, including:

[0033] If it is a fine scene, the narrow channel gravity mechanism is used to sample and generate the viewpoint F t .f k .viewpoints, these viewpoints are composed of n sampling points p i Composition; sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean_position:

[0034]

[0035] The polar diameter r i The distribution conforms to:

[0036]

[0037] The polar angle The distribution conforms to:

[0038]

[0039] If the sampling point p i If it coincides with occupied state voxels or unknown state voxels, it will be resampled;

[0040] For wide scenes, use an interactive lateral gravity mechanism to update the viewpoint F t .f k.viewpoints, these viewpoints are composed of n sampling points p i Composition; sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean_position:

[0041]

[0042] The polar diameter r i The distribution conforms to:

[0043]

[0044] The polar angle The distribution conforms to:

[0045]

[0046] Where θ is the current frontier average position F t .f k .mean_position points to the drone's own coordinate p ego , and the current frontier average position F t .f k .mean_position points to the sampling point p i The angle between them; the above r are all set fixed values.

[0047] Preferably, in step 6, the ATSP cost matrix Matrix is ​​generated cost The methods include:

[0048] Using the current set F t Generate ATSP cost matrix Matrix cost , where the elements of Matrix cost (k1, k2) means the cost from the k1th frontier to the k2th frontier, Matrix cost (0, k1) means the cost from the drone position to the k1th frontier, Matrix cost (k1, 0) means the cost from the k1th frontier to the drone position, which is generated as follows:

[0049] The elements of the first column are:

[0050] Matrix cost (k, 0) = 0, 0 ≤ k ≤ n

[0051] The elements of the first row are:

[0052]

[0053] Where n is the number of viewpoints, p ego is the coordinate of the drone itself, p k F t The average position F of the kth frontier f viewpoints in t .f k .viewpoints_meanposition, is the UAV velocity vector, length(p ego , p k ) represents the path length from the UAV to the kth frontier;

[0054] In addition, the ATSP cost matrix Matrix cost Except for the first row and the first column, the other n×n square matrix elements are:

[0055]

[0056] Preferably, in step 6, the matrix is ​​solved using the LKH solver to obtain the frontier f access sequence, which is used as the drone waypoint sequence P waypoints and execute.

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

[0058] Aiming at the problem of efficiency / precision conflict faced by unmanned aerial vehicles in autonomous exploration and performing tasks in multi-scale scenes, the present invention designs a drone hierarchical exploration method based on viewpoint generation using a gravity mechanism; the method uses a hierarchical approach and proposes different gravity mechanisms for fine scenes and wide scenes: in wide scenes, the drone uses an interactive lateral gravity mechanism to update the viewpoint, so that the viewpoint is generated closer to the drone's own body, reducing unnecessary long-distance maneuvers due to the long distance between the viewpoint and the drone; in fine scenes, a narrow-channel gravity mechanism is used to sample and generate viewpoints, so that the viewpoint generation radius is smaller, thereby enabling the drone to have a longer track planning, reducing the generation of inefficient forward-backward maneuvers, and ultimately enabling the drone to complete the exploration task in a high-precision and high-efficiency manner. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a flowchart of the drone hierarchical exploration method based on gravity mechanism viewpoint generation of the present invention;

[0060] Figure 2 A schematic diagram of a viewpoint generation algorithm for sampling a narrow channel gravity mechanism according to the present invention;

[0061] Figure 3 Schematic diagram of the viewpoint updating algorithm of the interactive lateral gravity mechanism of the present invention. DETAILED DESCRIPTION

[0062] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0063] Step 1: The drone uses the forward-looking depth camera to obtain the sensor pose T and depth image I depth , incrementally build the global map M globe ;

[0064] Step 2: Use the overlapping area judgment mechanism to update all frontiers F in the global map at the current time t t ;

[0065] Step 3: Calculate the number of remaining frontiers F t .size(), if the return value is not zero, continue to the next step, if the return value is zero, it is considered to have reached the end point and the loop ends.

[0066] Step 4: Discard AF based on the last three moments t , AF t-1 and AF t-2 Determine the scene scale and confirm whether the current scene is a detailed scene or a broad scene;

[0067] Step 5: If it is a fine scene, use the narrow channel gravity mechanism to sample and generate the viewpoint F t .f k .viewpoints, if it is a wide scene, use the interactive lateral gravity mechanism to update the viewpoint F t .f k .viewpoints;

[0068] Step 6: Generate ATSP cost matrix Matrix using the current architecture viewpoint cost , solve the matrix to get the UAV waypoint sequence P waypoints , then proceed to step 1;

[0069] Furthermore, in step 1, after obtaining the sensor pose T and depth image I depth After that, the depth image I depth Each of them has pixel coordinates (u n , v n ), the depth value is z n The following operations are performed on the pixels:

[0070]

[0071] Among them, P w for That is, the coordinates of the pixel in the world coordinate system, T is the current pose of the sensor obtained, and K is the intrinsic parameter matrix of the camera.

[0072] Repeat the above steps for each depth image, convert each pixel into a point in the world coordinate system, and accumulate these point clouds to form a complete three-dimensional point cloud M at the current time t t .

[0073] Next, the global map M is incrementally constructed by using the complete 3D point cloud at the current moment and the 3D point clouds at all past moments through the point cloud registration method. globe .

[0074] In step 2, firstly, the complete three-dimensional point cloud M at the current time t is t Convert to voxel map V t , and then divide the voxel state into occupied (with obstacles), unknown and free (known to have no obstacles) according to the occupancy state, and detect all the frontiers f at the current moment k Stored in F t Medium, F t is called the set of all frontiers in the global map at the current time t.

[0075] Frontier k Detection method: Use a growing algorithm to cluster voxels in the same area that are adjacent to the unknown voxel into frontiers f k , f k It contains all the constituent voxel cells, the cluster average position mean_position and the cluster bounding box coordinates (x, y, z).

[0076] Frontier Set F t Detection method: The voxel map V detected at the current moment t Boundary and previous moment F t-1 All F t-1 .f k The bounding box coordinates (x, y, z) are used to determine the overlapping area. If (x, y, z) is contained in the voxel map V t In the boundary, delete this F t-1 .f k ; According to the frontier detection method, calculate the current voxel map V t The frontier in k , add it to the just processed F t-1 In the current moment, we get the frontier set F t . Above F t-1 .f k is defined as the frontier set F at time t-1 t-1 The kth frontier f k .

[0077] All F deleted at the current time t t-1 .f k .cells make up the set AFt , calculate its cluster mean position mean_position and cluster bounding box coordinates (x, y, z).

[0078] In step 4, the three groups of abandoned boundary AFs that have recently entered the dormant state are used. t , AF t- 1 and AF t-2 As a judgment sequence, it is determined whether the current scene is a detailed scene or a broad scene.

[0079] The division of the exploration scene into fine scene / wide scene is the main idea of ​​the hierarchical exploration method. The UAV exploration always runs the wide scene exploration mode after initialization, during which the viewpoints of the boundary are generated using the interactive lateral gravity mechanism. When the boundary between the fine scene and the wide scene, i.e. the switching boundary we proposed, is detected, it will enter the narrow area exploration mode.

[0080] The switching boundary at the junction of fine scene and wide scene has unique morphological characteristics and sequence characteristics. The algorithm uses three groups of abandoned boundary AF that have recently entered the dormant state. t , AF t-1 and AF t-2 Make a judgment:

[0081] The first step is to determine the three groups of abandoned boundaries AF t , AF t-1 and AF t-2 Whether the dimensions meet the narrow requirements, AF t For example:

[0082] ||AF t .cell-AF t .mean_position|| <r narrow

[0083] AF t .cell is AF t The 3D coordinates of any voxel in AF t .mean_position is AF t The average position of the cluster.

[0084] Three groups of abandoned borders AF t , AF t-1 and AF t-2 If the sizes meet the narrow requirement, the process proceeds to the second step to determine whether it is a narrow scene; otherwise, it is determined to be a wide scene and the second step is not performed.

[0085] The second step is to determine the abandoned boundary AF in three consecutive updates t , AF t-1 and AF t-2 Arrangement characteristics:

[0086]

[0087] in:

[0088] m t =AF t .mean_position

[0089] Judgment mechanism:

[0090]

[0091] This can be used to determine the scale of the scene.

[0092] Furthermore, in step 5, the viewpoint generation method is determined according to the judgment of the fine scene / broad scene.

[0093] Viewpoint is the frontier t .f k The way nearby locations for drones to reach are generated plays a crucial role in the efficiency of drone exploration.

[0094] If it is a detailed scene, such as Figure 2 As shown, the narrow channel gravity mechanism is used to sample and generate the viewpoint F t .f k .viewpoints, these viewpoints are composed of n sampling points p i Sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean-position:

[0095]

[0096] The polar diameter r i The distribution conforms to:

[0097]

[0098] The polar angle The distribution conforms to:

[0099]

[0100] If the sampling point p i If it coincides with occupied state voxels or unknown state voxels, it will be resampled.

[0101] If it is a wide scene, such as Figure 3 As shown, the interactive lateral gravity mechanism is used to update the viewpoint F t .f k.viewpoints, these viewpoints are composed of n sampling points p i Sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean-position:

[0102]

[0103] The polar diameter r i The distribution conforms to:

[0104]

[0105] The polar angle The distribution conforms to:

[0106]

[0107] Where θ is the current frontier average position F t .f k .mean-position points to the drone's own coordinate p ego , and the current frontier average position F t .f k .mean_position points to the sampling point p i The angle between them. The above r are all fixed values.

[0108] After sampling, calculate all viewpoints F on this front t .f k The average value of .viewpoints position is F t .f x .viewpoints_meanposition.

[0109] Furthermore, in step 6, using the current architecture F t (Contains the average position of n frontier viewpoints) Generate ATSP cost matrix Matrix cost , where the elements of Matrix cost (k1, k2) means the cost from the k1th frontier to the k2th frontier, Matrix cost (0, k1) means the cost from the drone position to the k1th frontier, Matrix cost (k1, 0) means the cost from the k1th frontier to the drone position, which is generated as follows:

[0110] The elements of the first column are:

[0111] Matrix cost(k,0)=0,0≤k≤n

[0112] The elements of the first row are:

[0113]

[0114] Where n is the number of viewpoints, p ego is the coordinate of the drone itself, p k F t The average position F of the kth frontier f viewpoints in t .f k .viewpoints_meanposition, is the UAV velocity vector, length(p ego ,p k ) represents the path length from the UAV to the kth frontier;

[0115] In addition, the ATSP cost matrix Matrix cost Except for the first row and the first column, the other n×n square matrix elements are:

[0116]

[0117] All of the above length(), i.e. the length calculation between two points, are completed using the A* algorithm.

[0118] Next, use the LKH solver to solve the matrix to obtain the frontier f visit sequence, which is used as the drone waypoint sequence P waypoints and execute.

[0119] After the execution is completed, continue to execute the steps from step 1.

[0120] A multi-scale field of 50×50m was selected to test the improved algorithm, and the maximum flight speed v was set. max =2(m / s), maximum acceleration a=2(m / s), maximum angular velocity α=0.9(rad / s). In actual tests, when the drone flies in a narrow channel, it will fly a longer distance towards the next boundary. The intuitive effect is that the boundary sequence in the narrow channel exerts a greater gravitational force on the drone, greatly reducing the phenomenon of the drone flying back and forth in the narrow channel; in a wide scene, each time the boundary is updated, the algorithm generates a viewpoint of the boundary within a certain radius centered on the drone in a lateral viewpoint optimization manner, so that the generated viewpoint is convenient for the drone to scan with the minimum maneuvering cost at the current position and speed.

[0121] The final test results show that this method has obvious advantages over existing methods in key indicators.

[0122] algorithm Total path length (m) Total exploration time(s) The present invention 794.3 622.1 Existing methods 883.7 713.9

[0123] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A drone hierarchical exploration method based on gravity mechanism viewpoint generation, characterized in that: include: Step 1: The drone uses the forward-looking depth camera to obtain the sensor pose T and depth image I depth , incrementally build the global map M globe ; Step 2: Use the overlapping area judgment mechanism to update all frontiers in the global map at the current time t, and then obtain the frontier set F t ; Step 3: Calculate the frontier set F t The number of remaining frontiers in the loop. If the return value is not zero, the next step is executed. If the return value is zero, the end point is reached and the loop ends. Step 4: Determine the scene scale and confirm whether the current scene is a detailed scene or a wide scene; Step 5: If it is a fine scene, the narrow-channel gravity mechanism is used to sample and generate the viewpoint. If it is a wide scene, the interactive lateral gravity mechanism is used to update the viewpoint. Step 6: Generate the ATSP cost matrix Matrix using the viewpoints generated in step 5 cost , solve the matrix to get the UAV waypoint sequence P waypoints , return to step 1.

2. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 1, characterized in that: In step 1, after obtaining the sensor pose T and depth image I depth After that, the depth image I depth Each of them has pixel coordinates (u n ,v n ), the depth value is Z n The following operations are performed on the pixels: Among them, P w for That is, the coordinates of the pixel in the world coordinate system, T is the current position of the sensor obtained, and K is the intrinsic parameter matrix of the camera; Repeat the above steps for each depth image, convert each pixel into a point in the world coordinate system, and accumulate these point clouds to form a complete three-dimensional point cloud M at the current time t t ; Next, the global map M is incrementally constructed by using the complete 3D point cloud at the current moment and the 3D point clouds at all past moments through the point cloud registration method. globe .

3. A drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 1 or 2, characterized in that: In step 2, the method for updating all frontiers in the global map at the current time t includes: First, the complete three-dimensional point cloud M at the current time t t Convert to voxel map V t , and then classify the voxel states into occupied, unknown or free according to the occupancy state; using the growing algorithm, cluster the voxels in the same area that are adjacent to the unknown voxels into the frontier f k ; Among them, f k It contains all the constituent voxel cells, the cluster average position mean_position and the cluster bounding box coordinates (x, y, z).

4. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 3, characterized in that: In step 2, the frontier set F is obtained. t The methods include: The voxel map V detected at the current moment t Boundary and previous moment set F t-1 All the frontier bounding box coordinates (x, y, z) in the voxel map V are used for overlapping area judgment. If (x, y, z) is included in the voxel map V t In the boundary, delete this frontier; and so on, calculate the voxel map V at the current moment t The frontier in k , add it to the previous moment set F that has just been processed t-1 In the current moment, we get the frontier set F t .

5. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 4, characterized in that: In step 4, the method for determining the scene scale includes: All frontiers deleted at the current time t form a set AF t , calculate its cluster average position mean_position and cluster bounding box coordinates (x, y, z); use the three sets of abandoned boundary AF that have recently entered the dormant state t , AF t-1 and AF t-2 As a judgment sequence, determine whether the current scene is a detailed scene or a broad scene: The first step is to determine the three groups of abandoned boundaries AF t , AF t-1 and AF t-2 Whether all sizes meet the narrow requirement. If so, proceed to step 2 to determine whether it is a narrow scene. If one set does not meet the requirement, it is determined to be a wide scene and step 5 is executed. The second step is to determine the abandoned boundary AF in three consecutive updates t , AF t-1 and AF t-2 Arrangement characteristics: in: m t =AF t .mean_position Judgment mechanism: This determines the scale of the scene.

6. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 5, characterized in that: In step 4, whether the stenosis requirement is met is: discarding the boundary AF t , AF t-1 and AF t-2 The 3D coordinates of any voxel in AF t Whether the distance of the average position of the cluster is less than the set threshold.

7. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 1, characterized in that: In step 5, according to the judgment of the fine scene and the wide scene, the viewpoint generation method is determined, including: If it is a fine scene, the narrow channel gravity mechanism is used to sample and generate the viewpoint F t .f k .viewpoints, these viewpoints are composed of n sampling points p i Composition; sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean_position: The polar diameter r i The distribution conforms to: The polar angle The distribution conforms to: If the sampling point p i If it coincides with occupied state voxels or unknown state voxels, it will be resampled; For wide scenes, use an interactive lateral gravity mechanism to update the viewpoint F t .f k .viewpoints, these viewpoints are composed of n sampling points p i Composition; sampling point p i Expressed in polar coordinates, the origin of the polar coordinates is the average position of the frontier F t .f k .mean_position: The polar diameter r i The distribution conforms to: The polar angle The distribution conforms to: Where θ is the current frontier average position F t .f k .mean_position points to the drone's own coordinate p ego , and the current frontier average position F t .f k .mean_position points to the sampling point p i The angle between them; the above r are all set fixed values.

8. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 7, characterized in that: In step 6, the ATSP cost matrix Matrix is ​​generated cost The methods include: Using the current set F t Generate ATSP cost matrix Matrix cost , where the elements of Matrix cost (k1,k2) means the cost from the k1th frontier to the k2th frontier, Matrix cost (0,k1) means the cost from the drone position to the k1th frontier, Matrix cost (k1,0) means the cost from the k1th frontier to the drone position, which is generated as follows: The elements of the first column are: Matrix cost (k,0)=0,0≤k≤n The elements of the first row are: Where n represents the number of viewpoints, p ego is the coordinate of the drone itself, p k F t The average position F of the kth frontier f viewpoints in t .f k .viewpoints_meanposition, is the UAV velocity vector, length(p ego ,p k ) represents the path length from the UAV to the kth frontier; In addition, the ATSP cost matrix Matrix cost Except for the first row and the first column, the other n×n square matrix elements are:

9. The drone layered exploration method based on gravity mechanism viewpoint generation as claimed in claim 8, characterized in that: In step 6, the LKH solver is used to solve the matrix to obtain the frontier f access sequence, which is used as the drone waypoint sequence P waypoints and execute.

Images