Autonomous Exploration Method and System for Unmanned Vehicles in Unknown Environments Based on Frontier Search Trees

Abstract

The present invention discloses a method and system for autonomous exploration of an unknown environment by an unmanned vehicle based on a frontier search tree. The method includes: establishing and updating a voxel grid map according to the real-time environmental data and pose data of the unmanned vehicle, and extracting effective ground grids; extracting frontier grids from the effective ground grids to form a frontier information set; constructing a frontier search tree based on the frontier information set; traversing the frontier search tree based on a depth-first search strategy to select an optimal target; performing trajectory planning on the path between the unmanned vehicle and the optimal target; making the unmanned vehicle travel to the optimal target along the planned trajectory route according to the real-time pose data; repeating the above process until there are no frontier grids in the voxel grid map, and completing the exploration of the unknown environment. In the present invention, the structure of the frontier search tree effectively represents the topological structure of the 2.5D environment, and traversing the frontier search tree based on the depth-first search strategy reduces the unnecessary reciprocating movement of the unmanned vehicle and speeds up the decision-making and exploration process.

Description

Technical Field

[0001] The present invention relates to the technical field of unknown environment exploration of unmanned vehicles, and particularly to an autonomous exploration method and system for unmanned vehicles in unknown environments based on a frontier search tree. Background Art

[0002] Unmanned vehicles have the advantages of strong carrying capacity, long endurance time, safety and reliability, etc., and play an increasingly important role in autonomous environment exploration tasks, such as inspections, geographical explorations, search and rescue. Among them, traversing unknown environments and collecting information are basic components. Considering exploration in dangerous environments where GPS and wireless communication cannot be used and battery limitations, it has become crucial to develop algorithms that can fully run on unmanned vehicles and execute efficiently. One challenge of this task is that there are multi-layer structures in unknown environments, that is, there are multiple floors at the same location, and such an environment is called a 2.5D environment. Traditional ground unmanned vehicle planning algorithms mainly focus on 2D planes, and the 3D maps constructed during the exploration process cannot be converted into 2D maps by downward projection. This limits the application of unmanned vehicles in complex environment exploration tasks.

[0003] In recent years, various autonomous exploration methods have been proposed. Some methods calculate indexes such as information gain and motion performance of each area in the known area according to certain evaluation criteria, continuously select the best target area and navigate to that area for exploration. Some methods search for the boundaries between known and unknown areas and generate the best path connecting multiple boundaries to improve the continuity of motion. However, these methods only consider using the latest information to make decisions without considering the continuity of the results. The known environment is continuously updated, and the optimal target of each decision may not be in the same unexplored area. This will cause the unmanned vehicle to move back and forth between several areas. In addition, the computational overhead of some methods is very high, resulting in the unmanned vehicle remaining stationary for a long time waiting for the decision result, and it is not real-time. This delay seriously hinders the overall progress of the unmanned vehicle in the exploration task. Summary of the Invention

[0004] In view of the above problems, the present invention proposes an autonomous exploration method and system for unmanned vehicles in unknown environments based on a frontier search tree.

[0005] According to one aspect of the present invention, an autonomous exploration method for unmanned vehicles in unknown environments based on a frontier search tree is proposed, and the method includes the following steps:

[0006] Obtain the real-time environment data and real-time pose data of the unmanned vehicle;

[0007] Establish and update a voxel grid map according to the real-time environment data and real-time pose data, and extract effective ground grids;

[0008] Extract the front grid in the valid ground grids to form a front information set; wherein, the front grid is a valid ground grid adjacent to an unknown grid.

[0009] Construct a front search tree with real-time incremental updates based on the front information set.

[0010] Traverse the front search tree based on the depth-first search strategy to select the best target.

[0011] Perform trajectory planning for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory route.

[0012] Make the unmanned vehicle walk to the best target along the planned trajectory route according to the real-time pose data of the unmanned vehicle.

[0013] Repeat the above process until there are no front grids in the voxel grid map, and the unmanned vehicle completes the exploration of the unknown environment.

[0014] Further, the real-time environment data includes point cloud data collected by a radar or depth image data collected by a camera.

[0015] Further, the process of extracting the valid ground grids includes: the state of each voxel grid in the voxel grid map includes unknown, occupied or free, where the initial state of all voxel grids in the map is unknown, the state of the ground grid is occupied and the state of the grid directly above it is free; perform principal component analysis on the neighboring grids with the state of occupied around each ground grid to remove the invalid ground grids that are too inclined; expand the grids with the state of occupied other than the ground grids by a certain radius and delete the ground grids within the expanded layer; the remaining ground grids are the valid ground grids.

[0016] Further, the process of extracting the front grids in the valid ground grids to form a front information set includes: clustering the front grids to generate a front set, and calculating a front information structure for each front, where the front set and the front information structure form the front information set; wherein, the front information structure includes the front grids included in the front, the axis-aligned bounding box and the center coordinates of each front grid.

[0017] Further, the process of constructing a real-time incremental updated frontier search tree based on the frontier information set includes: each node data of the frontier search tree includes the parent node, child nodes, the information structure of the represented frontier, whether the node is fully explored, and whether there is a path connected to the node; at the initial moment, the root node of the frontier search tree is initialized to the current position of the unmanned vehicle, and the child nodes of the root node are initialized by the frontier information set of the first frame; when the unmanned vehicle moves, check whether the newly detected frontier overlaps with the leaf nodes of the frontier search tree; each frontier selects the nearest leaf node that overlaps with it; merge all the matching frontiers of each leaf node, where all the frontier grids are re-clustered to update the leaf node information or split into several child nodes.

[0018] Further, the process of traversing the frontier search tree based on the depth-first search strategy to select the best target includes: starting a depth-first search process at the root node of the frontier search tree, setting the currently explored node to the root node, and selecting one of the child nodes of the root node as the next node to be explored; the update of the next node to be explored is by delving into a certain branch of the frontier search tree until reaching the deepest leaf node of that branch; when a leaf node is explored and no child nodes are generated, return to the previous node to explore other unexplored branches, and sort the leaf nodes of other unexplored branches according to the score h(i), and select the leaf node with the highest score as the next node to be explored; repeat the above process until there are no unexplored nodes in the entire frontier search tree, and each time select the center of the frontier of the node to be explored as the target point for trajectory planning.

[0019] Further, the score h(i) is calculated according to the following formula:

[0020]

[0021] where P(p r ,n i ) represents a collision-free path searched on the ground grid connecting the unmanned vehicle p r and the i-th node n i , and length(P(p r ,n i )) represents the path length; FIS represents the information structure of the frontier represented by the node, Cells represents the frontier grids included in the frontier; size(n i .FIS.Cells) represents the number of grids in Cells; w s and w l represent the weights of each score term; l max and s max represent the maximum path length and the maximum frontier size among the candidates, respectively, which are scales for normalizing the size and length.

[0022] Further, perform trajectory planning for the path between the driverless vehicle and the best target in a 2.5D environment. The process of obtaining the planned trajectory includes: using the A* algorithm to perform path planning for the driverless vehicle and the best target in a 2.5D environment on a voxel grid map, and using the path planning result as the initial solution of the unconstrained nonlinear optimization problem corresponding to the trajectory planning. Among them, the objective function of the unconstrained nonlinear optimization problem corresponding to the trajectory planning is:

[0023]

[0024] Among them, J d represents the kinematic constraint penalty term, J g represents the ground safety constraint penalty, J e represents the elastic band penalty of the z-axis, J f represents the feasibility constraint penalty, w d , w g , w e , w f are the weight coefficients of each penalty term respectively.

[0025] Further, the kinematic penalty term is expressed as:

[0026]

[0027] In the formula, the number of states in the trajectory is n + 1, f(s i , u i , Δt) represents the planar kinematic model of the driverless vehicle, s i+1 represents the next state of recursion; u i represents the planar acceleration control quantity; Δt represents the discrete time interval;

[0028] The ground safety constraint penalty is expressed as:

[0029]

[0030] In the formula, d i represents the minimum distance from the position of the trajectory state point to the effective ground grid; s f represents the preset safety distance; L(d i , s f ) is a second-order continuously differentiable function,

[0031]

[0032] Among them, c j is the demarcation point between the quadratic term and the cubic term; c = d i - s f ;

[0033] The elastic band penalty of the z-axis is expressed as:

[0034]

[0035] In the formula, p z,i+1 represents the component on the z-axis of the position of the trajectory state point;

[0036] The feasibility constraint penalty is expressed as:

[0037]

[0038] In the formula, v max and a max represent the limits of speed and acceleration, w v , w a , w j are the weights of each penalty term; v i represents the speed of the trajectory state point.

[0039] According to another aspect of the present invention, an autonomous exploration system for an unmanned vehicle in an unknown environment based on a frontier search tree is proposed. The system includes:

[0040] A data acquisition module configured to acquire real-time environmental data and real-time pose data of the unmanned vehicle;

[0041] A frontier information extraction module configured to establish and update a voxel grid map according to real-time environmental data and extract valid ground grids; extract the frontier grids in the valid ground grids to form a frontier information set; wherein, the frontier grid is a valid ground grid adjacent to an unknown grid;

[0042] A search tree update module configured to construct a real-time incremental update frontier search tree based on the frontier information set;

[0043] A target selection module configured to traverse the frontier search tree based on a depth-first search strategy to select an optimal target;

[0044] A trajectory planning module configured to perform trajectory planning for the path between the unmanned vehicle and the optimal target in a 2.5D environment to obtain a planned trajectory route;

[0045] A control module configured to make the unmanned vehicle walk along the planned trajectory route to the optimal target according to the real-time pose data of the unmanned vehicle.

[0046] The beneficial technical effects of the present invention are:

[0047] The present invention proposes an autonomous exploration method and system for an unmanned vehicle in an unknown environment based on a frontier search tree. First, a voxel grid map is established and updated according to the real-time environment data and real-time pose data of the unmanned vehicle, and effective ground grids are extracted; then, frontier grids in the effective ground grids are extracted to form a frontier information set; a real-time incrementally updated frontier search tree is constructed based on the frontier information set; the frontier search tree is traversed based on a depth-first search strategy to select the best target; trajectory planning is performed for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory route; the unmanned vehicle is made to walk along the planned trajectory route to the best target according to the real-time pose data of the unmanned vehicle; until there are no frontier grids in the voxel grid map, the unmanned vehicle completes the exploration of the unknown environment. In the present invention, a real-time incrementally updated frontier search tree structure is proposed, which effectively represents the topological structure of a 2.5D environment; and a depth-first search strategy is used to traverse the entire frontier search tree, which reduces unnecessary reciprocating motion of the unmanned vehicle and speeds up the decision-making and exploration process. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The present invention can be better understood by referring to the following description in conjunction with the accompanying drawings, which are included in this specification and form a part of this specification together with the following detailed description, and are used to further illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0049] Figure 1 is a framework diagram of an autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to an embodiment of the present invention.

[0050] Figure 2 is a flowchart of an autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to an embodiment of the present invention.

[0051] Figure 3 is a schematic diagram of the search tree update process and the robot exploration process in an embodiment of the present invention.

[0052] Figure 4 is an example diagram of a maze in Gazebo simulation in an embodiment of the present invention.

[0053] Figure 5 is an example diagram of the update process of the search tree in an embodiment of the present invention.

[0054] Figure 6 is an example diagram of effective ground grids (red) and a frontier search tree (green line) in an embodiment of the present invention.

[0055] Figure 7 is a comparison result diagram of the greedy method and the method of the present invention in an embodiment of the present invention.

[0056] Figure 8 It is a diagram showing the physical experiment site in the embodiments of the present invention.

[0057] Figure 9 It is a diagram showing the hardware settings of the Mecanum wheel robot in the embodiments of the present invention.

[0058] Figure 10 It is an example diagram of the search tree (green), ground (red), obstacles and their inflated layers (white), and exploration path (purple) in the physical scene in the embodiments of the present invention. Specific embodiments

[0059] In order to enable those skilled in the art to better understand the solution of the present invention, the exemplary embodiments or examples of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described embodiments or examples are only part of the embodiments or examples of the present invention, rather than all of them. All other embodiments or examples obtained by those of ordinary skill in the art based on the embodiments or examples in the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0060] In order to effectively navigate an unmanned vehicle to complete autonomous exploration tasks in a 2.5D environment, the present invention proposes an exploration framework based on a frontier search tree, as Figure 1 shown, which can support the exploration of unmanned vehicles in a 2.5D environment. This framework is based on a voxel grid map, extracts effective ground grids, and clusters the frontier grids in the ground grids to obtain the boundaries of known and unknown regions, called frontiers. In order to effectively represent the topology of the known environment, the present invention introduces an incrementally updated frontier search tree, where leaf nodes represent the latest frontiers; for the exploration planning task, a depth-first search strategy is used to effectively traverse the frontier exploration tree. A scoring metric is designed to evaluate each frontier, and in each decision, focus on exploring the branch with the highest score, and switch to other branches after completely exploring this branch. This method effectively reduces unnecessary reciprocating motion. The motion planning module receives the target area from the decision-making module, generates a ground-conforming global path on the ground grid using the A* algorithm, and generates a discrete-time 2.5D trajectory based on an optimization method to achieve the motion of the unmanned vehicle in a 2.5D environment.

[0061] The embodiments of the present invention propose an autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree, as Figure 2 shown, and this method includes the following steps:

[0062] Step 1: Obtain the real-time environment data and real-time pose data of the unmanned vehicle;

[0063] Step 2: Establish and update a voxel grid map according to the real-time environment data and real-time pose data, and extract effective ground grids;

[0064] Step 3: Extract the front grid cells from the valid ground grid cells to form a front information set. Among them, the front grid cells are valid ground grid cells adjacent to unknown grid cells.

[0065] Step 4: Build a front search tree with real-time incremental updates based on the front information set.

[0066] Step 5: Traverse the front search tree based on the depth-first search strategy to select the best target.

[0067] Step 6: Perform trajectory planning for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory route.

[0068] Step 7: According to the real-time pose data of the unmanned vehicle, make the unmanned vehicle travel to the best target along the planned trajectory route.

[0069] Step 8: Repeat the above process until there are no front grid cells in the voxel grid map, and the unmanned vehicle completes the exploration of the unknown environment.

[0070] The method starts from Step 1. In Step 1, first, obtain the real-time environment data and real-time pose data of the unmanned vehicle. Among them, the real-time environment data includes point cloud data collected by a radar or depth image data collected by a camera. The real-time pose data includes pose data collected and processed by an odometer.

[0071] Then execute Step 2. In Step 2, establish an updated voxel grid map according to the real-time environment data and real-time pose data, and extract valid ground grid cells.

[0072] According to the embodiments of the present invention, in order to ensure safe movement, since the unmanned vehicle physically moves on the ground, it is crucial to extract effective ground information. To simplify the calculation, the extraction in this embodiment is based on the voxel grid map, where the state of each voxel grid is "unknown", "occupied", or "idle". At the beginning, all voxel grid cells in the map are initially set to "unknown". During the entire exploration process, the unmanned vehicle obtains depth measurement values from on-board sensors and measurement values from internal pose estimation algorithms. Then, these measurement results are used to update the state of the grid using ray casting technology.

[0073] The ground grid must be an "occupied" grid, and the grid directly above it is "idle". However, invalid ground grids where the unmanned vehicle cannot be placed, such as the top of a low wall, may also meet this condition. Therefore, principal component analysis is performed on all the "occupied" grids in the neighborhood of each ground grid to remove overly inclined invalid ground grids. In addition, the "occupied" grids other than the ground grids are dilated by a certain radius, and the valid ground grids within the dilation layer are deleted. The remaining ground grids are valid ground grids. In this way, collisions can be avoided when the unmanned vehicle moves on the valid ground.

[0074] Then step three is executed. In step three, the leading edge grids in the valid ground grids are extracted to form a leading edge information set; among them, the leading edge grids are valid ground grids adjacent to unknown grids.

[0075] According to an embodiment of the present invention, the leading edge grids are clustered in the valid ground grids. According to the definition of the leading edge, the leading edge grids are defined as valid ground grids adjacent to unknown grids. Then DBSCAN clustering is performed on these leading edge grids to generate a leading edge set, and a leading edge information structure FIS is calculated for each leading edge. The leading edge set and the leading edge information structure constitute the leading edge information set; among them, the leading edge information structure includes the leading edge grids included in the leading edge, the axis-aligned bounding box and the center coordinates of each leading edge grid. To accelerate collision checking, the axis-aligned bounding box (AABB) of the leading edge is calculated. The data stored in the leading edge information structure is listed in Table 1. The set of leading edge information structures is represented by F = {FIS1, FIS2,...}.

[0076] Table 1 Data included in the leading edge information structure (FIS)

[0077]

[0078] Then step four is executed. In step four, a leading edge search tree with real-time incremental update is constructed based on the leading edge information set.

[0079] According to an embodiment of the present invention, in order to establish a connection between all current and past frontier information, the present invention constructs a search tree ST, and the data stored in the search tree node STN is described in Table 2. Set Check to mark whether the branch has been explored, and set Reachable to mark whether there is a path linking to this node. At the initial moment, the root node of the tree is initialized to the current position of the unmanned vehicle, and the child nodes of the root node will be initialized by the frontier set of the first frame. When the unmanned vehicle moves, check whether there is a collision between the newly detected frontier and the leaf node of the search tree, that is, whether the Boxes of the two overlap. Then each frontier selects the nearest leaf node that overlaps with it. This ensures that each frontier is matched with a leaf node, and a leaf node can be matched with multiple frontiers. Finally, all the matching frontiers of each leaf node are merged, where all the frontier grids are re-clustered to update the leaf node information or split into several child nodes, as described in Algorithm 1. Figure 3 Intuitively demonstrates this process.

[0080] Table 2 Data contained in each node (STN) of the search tree

[0081]

[0082]

[0083] Then step five is executed. In step five, the frontier search tree is traversed based on the depth-first search strategy to select the best target.

[0084] According to an embodiment of the present invention, in actual exploration, the movement of the unmanned vehicle is restricted to a branch road, which can be divided into several branches. The unmanned vehicle can only choose one branch to continue moving forward. This characteristic corresponds to the structure of the search tree, where each node represents a section of the road, and the child nodes represent the reachable branches, as Figure 3 shown.

[0085] Depth-First Search (DFS) is an efficient graph traversal algorithm, usually used to explore nodes in a tree data structure. The algorithm first starts the DFS process at the root node of the search tree. The current node being explored (NCE) is set to the root node, and the next node to be explored (N2E) is selected from one of the child nodes of the root node. The update of N2E is achieved by delving as deep as possible into a certain branch of the search tree until reaching the deepest leaf node of that branch. Once reaching the deepest leaf node and completing the exploration, the algorithm backtracks to the previous node to explore other unexplored branches, as shown in Algorithm 2. This process is repeated until there are no unexplored nodes in the entire tree, representing the end of the entire exploration task. In the motion planning module, the center of the frontier belonging to the current node being explored is selected as the target point.

[0086] Whenever N2E is about to be updated, the child node with the highest score will become the target. The score h(i) is calculated using a combination of the path length required to reach the STN and the size of the STN's front.

[0087]

[0088] where P(p r ,n i ) represents the collision-free path searched on the ground grid connecting the unmanned vehicle p r and the i-th node n i . The length(P(p r ,n i )) represents the path length; FIS represents the information structure of the front represented by this node, and Cells represents the front grids included in the front; size(n i .FIS.Cells) represents the number of grids in Cells; w s and w l represent the weights of each score term; l max and S max represent the maximum path length and the maximum front size among the candidates, respectively, which are used to normalize the scales of the size and length.

[0089]

[0090]

[0091] Then, step six is executed. In step six, trajectory planning in a 2.5D environment is performed on the path between the unmanned vehicle and the best target to obtain the planned trajectory route.

[0092] According to the embodiments of the present invention, the A* algorithm is used to search for a path on the voxel grid map. Since the movement of the unmanned vehicle is restricted to the ground, only the effective ground grids need to be considered, which reduces the number of grids to be expanded in the A* algorithm. The 2.5D trajectory planning is based on a linear kinematic model. Taking an omnidirectional unmanned vehicle as an example, let x = [p, v] T , u = [a x , a y T represent the state vector and the control input vector, where p = [p x , p y , p z T , v = [v x , v y T . Note that all variables are defined in the global coordinate system. The discrete-time motion equations on the xy axes are given by:

[0093] p​​​xy,i+1 = p xy,i + v i Δt + 0.5u i Δt 2

[0094] v i+1 = v i + u i Δt

[0095] where p xy = [p x , p y T . The dynamics of the ground vehicle on the z-axis are difficult to calculate online because they are related to the slope of the terrain at the wheel contact points. They will be treated as elastic band penalties later.

[0096] The trajectory contains n + 1 states and n control inputs. The optimization problem can be formulated as:

[0097]

[0098] where J d is the kinematic constraint penalty term, J g is the ground safety constraint penalty, J e is the elastic band penalty on the z-axis, J f is the feasibility constraint penalty, and w d , w g , w e , w f are the weight coefficients of each penalty term.

[0099] 1) Dynamics penalty term: Let s = [p xy , v] T and abbreviate the motion equation as:

[0100] s i+1 = f(s i , u i , Δt)

[0101] Then, the dynamics penalty term can be constructed with a quadratic term:

[0102]

[0103] where f(s i , u i , Δt) represents the planar kinematic model of the unmanned vehicle, s i+1 represents the next state in the recursion; u i represents the planar acceleration control quantity; Δt represents the discrete time interval.

[0104] ​2) Ground safety constraint penalty: After removing all invalid ground grids, the remaining valid ground grids form a safety space. The trajectory is constrained within the safety space to ensure safety. We set a safety distance s f and penalize the minimum distance d between the position of each state variable in the trajectory and the safety space i , where the safety distance s f is generally slightly larger than the radius of the circumscribed circle of the projection of the driverless vehicle on the xoy plane. The safety constraint penalty is constructed as:

[0105]

[0106] where L(d i , s f ) is a second-order continuously differentiable function:

[0107]

[0108] c = d i - s f

[0109] where c j is the demarcation point between the quadratic term and the cubic term.

[0110] 3) Elastic band penalty: The problem is simplified by ignoring the dynamics of the z-axis, that is, using the elastic band penalty to constrain p z to ensure smoothness, and its formula is

[0111]

[0112] where p z,i+1 represents the component of the position of the trajectory state point on the z-axis.

[0113] 4) Feasibility constraint penalty: Feasibility represents the physical limitations of the driverless vehicle, which is ensured by restricting the trajectory state. In addition, our goal is to make the trajectory as smooth as possible. Therefore, the penalty function can be expressed as

[0114]

[0115] where v max and a max represent the speed and acceleration limits, w v , w a , w j are the weights of each penalty term; v i represents the speed of the trajectory state point.

[0116] Then perform step seven, in which, according to the real-time pose data of the driverless vehicle, the driverless vehicle walks along the planned trajectory route to the optimal target.

[0117] According to an embodiment of the present invention, the control input of the underlying controller is the speed in the unmanned vehicle coordinate system. First, the speed feedforward method is used to calculate the speed input in the global coordinate system:

[0118]

[0119] where p xy (t) and v(t) are the trajectory states at time t, p r,xy is the xy component of the unmanned vehicle position, and α is the weight of the feedforward quantity. Since the trajectory consists of discrete points, Hermite interpolation is used to estimate the state at the intermediate time between two states of the trajectory. Then, the speed command in the global coordinate system is converted to the unmanned vehicle coordinate system according to the global pose of the unmanned vehicle.

[0120] Repeat the above process until there are no frontier voxels in the voxel grid map, and the unmanned vehicle completes the exploration of the unknown environment.

[0121] Furthermore, simulation experiments and physical experiments are conducted to verify the technical effects of the present invention.

[0122] Set the weights of the penalty function as follows: w d = 10, w g = 20, w e = 0.5, w f = 1, w v = 0.9, w a = 0.9, w j = 0.9. The weight α of the trajectory tracking feedforward quantity is 0.8. In the simulation experiment, the maximum speed, acceleration, and discrete time interval of the unmanned vehicle are set to 0.5 m / s, 0.5 m / s 2 and 0.22 s respectively, and in the physical experiment, they are set to 0.7 m / s, 0.5 m / s 2 and 0.36 s respectively. The LBFGS algorithm is used to solve the unconstrained optimization problem. The resolution of the voxel grid map is set to 0.1 m in the simulation experiment and 0.15 m in the real experiment. In the simulation, a laptop computer equipped with an AMD R7-5800H CPU and an Nvidia GeForce RTX3060 GPU is used. To verify the effectiveness of the method of the present invention in a 2.5D environment, a double-layer maze is constructed based on Gazebo, as Figure 4 shown, to simulate an omnidirectional ground unmanned vehicle equipped with a lidar. The field of view of the radar is [360×59] degrees, and the maximum range is 3 m.

[0123] 1) Search tree generation: The height of the lidar relative to the ground is low, resulting in sparse point clouds far from the center of the unmanned vehicle. This sparsity hinders the extraction of effective ground grids and fronts. To solve this problem, a range is set, and the operations are limited to the point clouds within this range. Figure 5 The update process of the search tree is shown, where the orange squares represent effective ground grids. The green voxels represent front grids, and the green hollow rectangles are the axisymmetric bounding boxes of the fronts. The green lines represent branch connections. The red dots represent lidar points, covering the surrounding ground, enabling the extraction of ground information. As the unmanned vehicle moves, the front on the right splits into two fronts after encountering a corner, which also represents the generation of two branches. After each leaf of the ST is explored, the entire effective ground accessible to the ground unmanned vehicle is explored, as Figure 6 shown.

[0124] 2) 2.5D environment exploration: To prove that using the DFS-based strategy can reduce back-and-forth movement, the method of the present invention is compared with the greedy method of selecting the largest size from all fronts as the target. Accordingly, the scoring weights are set as w s = 1 and w l = 0. The statistics and exploration progress of the two methods are shown in Table 3 and Figure 7 shown. Obviously, the exploration path of the greedy method (largest size) is longer than that of the proposed method (w s = 1 and w l = 0). This is because the optimal front selected by the greedy method may not be on the same branch, resulting in back-and-forth movement. In contrast, the search tree structure proposed in the present invention prunes the fronts of other branches, and the DFS-based strategy ensures the continuity of movement, achieving efficient exploration.

[0125] Then, the greedy method is modified to select the shortest path between the unmanned vehicle and the front. The results are shown in Table 3 and Table 4. Interestingly, the lengths of the exploration paths between the two methods are similar. This shows that selecting the shortest path can also reduce back-and-forth movement. However, compared with the proposed method (w s = 0 and w l = 1), the calculation time of the greedy method (shortest path) is significantly longer. The reason for this difference is that the greedy method (shortest path) uses the A* algorithm multiple times to select the target front, and the search time of the A* algorithm may be relatively long in a large dense grid map. In contrast, the proposed search tree structure uses nodes and branches to represent the topology of the map, making it unaffected by the map size. In addition, the number of nodes in a dense grid map is much smaller than the number of grids, which makes the DFS search faster. In addition, the method only makes decisions when there are forks in the road and directly continues to move forward when there are no forks, further reducing the computational complexity of decision-making.

[0126] Figure 6 It is the effective ground grid (red) and the frontier search tree (green line). Figure 7 It is the comparison between the greedy method (maximum size and shortest path) and the method of the present invention (w s = 1 and w l = 0, w s = 0 and w l = 1). The overall exploration paths are shown in purple, brown, green, and red.

[0127] Table 3 Exploration statistical results in the maze

[0128]

[0129] Table 4 Decision-making calculation time of different methods

[0130]

[0131]

[0132] To further verify the application of the proposed method in the real world, a single-layer maze was conducted in the real world, as Figure 8 shown. The omnidirectional-wheel unmanned vehicle ( Figure 9 ) was used as the platform to implement the algorithm. All calculations were performed on the on-board NVIDIA Jetson NX computer. To achieve reliable lidar-based positioning, Livox MID360 was used and the Point-Lio algorithm was utilized. The lidar was installed upside down so that its field of view could cover the surrounding ground, thereby enabling effective ground extraction. In the underlying controller composed of STM32 MCUs, the speed command was converted into the rotational speed commands of the four omnidirectional wheels. The size of the space to be explored was 7×4.5 m 2 . The exploration results are as Figure 10 shown. The length of the exploration path was 34.08 m, and the total time used was 48 s.

[0133] In summary, the present invention proposes a frontier-based exploration framework that can support the exploration of unmanned vehicles in a 2.5D environment. This framework utilizes a real-time updated voxel grid map to extract effective ground grids and frontiers. In addition, an incrementally updated frontier search tree is constructed, and an efficient target selection and lower calculation time are achieved through a DFS strategy that reduces reciprocating motion. To enable a ground unmanned vehicle to navigate and explore in a 2.5D environment, a discrete-time trajectory optimization method is adopted to generate a 2.5D trajectory that conforms to the ground. Simulations and physical experiments have proven the effectiveness of this framework.

[0134] Another embodiment of the present invention proposes an unmanned vehicle unknown environment autonomous exploration system based on a frontier search tree, and this system includes:

[0135] A data acquisition module configured to acquire real-time environmental data and real-time pose data of the unmanned vehicle;

[0136] A front information extraction module configured to establish and update a voxel grid map based on the real-time environmental data and extract valid ground grids; extract the front grids in the valid ground grids to form a front information set; wherein, the front grids are valid ground grids adjacent to unknown grids;

[0137] A search tree update module configured to construct a real-time incremental updated front search tree based on the front information set;

[0138] A target selection module configured to traverse the front search tree based on a depth-first search strategy to select an optimal target;

[0139] A trajectory planning module configured to perform trajectory planning for the path between the unmanned vehicle and the optimal target in a 2.5D environment to obtain a planned trajectory route;

[0140] A control module configured to make the unmanned vehicle travel to the optimal target along the planned trajectory route according to the real-time pose data of the unmanned vehicle.

[0141] The functions of an unmanned vehicle unknown environment autonomous exploration system based on a front search tree in an embodiment of the present invention can be illustrated by the aforementioned unmanned vehicle unknown environment autonomous exploration method based on a front search tree. Therefore, for the parts not detailed in the system embodiment, reference can be made to the above method embodiment, which will not be elaborated here.

[0142] Although the present invention is described based on a limited number of embodiments, those skilled in the art in this technical field understand that other embodiments can be envisioned within the scope of the present invention thus described. For the scope of the present invention, the disclosure of the present invention is illustrative rather than restrictive, and the scope of the present invention is defined by the appended claims.

Claims

1. An autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree, characterized in that, Including the following steps: Obtain the real-time environmental data and real-time pose data of the unmanned vehicle; Establish and update the voxel grid map according to the real-time environmental data and real-time pose data, and extract the effective ground grid; Extract the front grids in the effective ground grids to form a front information set; wherein, the front grids are effective ground grids adjacent to the unknown grids; Construct a front search tree with real-time incremental updates based on the front information set; Traverse the front search tree based on the depth-first search strategy to select the best target, including: starting the depth-first search process at the root node of the front search tree, setting the currently explored node as the root node, and selecting one of the child nodes of the root node as the next node to be explored; the update of the next node to be explored is by delving into a certain branch of the front search tree until reaching the deepest leaf node of that branch; when a leaf node has been explored and no child nodes are generated, return to the previous node to explore other unexplored branches, and sort the leaf nodes of other unexplored branches according to the score h(i), and select the leaf node with the highest score as the next node to be explored; repeat the above process until there are no unexplored nodes in the entire front search tree, and each time select the center of the front of the node to be explored as the target point for trajectory planning; the score h(i) is calculated according to the following formula: Among them, P(p r , n i ) represents the collision-free path searched on the ground grid connecting the unmanned vehicle p r and the i-th node n i . length(P(p r , n i )) represents the path length; FIS represents the information structure of the front represented by this node, and Cells represents the front grids included in the front; size(n i .FIS.Cells) represents the number of grids in Cells; w s and w l represent the weights of each fractional term; l max and s max respectively represent the maximum path length and the maximum front size among the candidates, which are used as the scales for normalizing the size and length; Perform trajectory planning for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory route; Make the unmanned vehicle travel to the best target along the planned trajectory route according to the real-time pose data of the unmanned vehicle; Repeat the above process until there are no front grids in the voxel grid map and the unmanned vehicle completes the exploration of the unknown environment.

2. The autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to claim 1, wherein The real-time environmental data includes point cloud data collected by radar or depth image data collected by a camera.

3. The autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to claim 1, characterized in that, The process of extracting the effective ground grids includes: the state of each voxel grid in the voxel grid map includes unknown, occupied or free, wherein the initial state of all voxel grids in the map is unknown, the state of the ground grid is occupied and the state of the grid directly above it is free; perform principal component analysis on the neighboring grids with the state of occupied around each ground grid to remove the ineffective ground grids that are too inclined; expand the grids with the state of occupied other than the ground grids by a certain radius, and delete the ground grids within the expansion layer; the remaining ground grids are the effective ground grids.

4. The method for autonomous exploration of an unmanned vehicle in an unknown environment based on a frontier search tree according to claim 1, characterized in that The process of extracting the front grids in the effective ground grids to form a front information set includes: clustering the front grids to generate a front set, and calculating a front information structure for each front, and the front set and the front information structure form a front information set; wherein, the front information structure includes the front grids included in the front, the axis-aligned bounding box and the center coordinates of each front grid.

5. The autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to claim 1, characterized in that The process of constructing a real-time incremental updated frontier search tree based on the frontier information set includes: the data of each node in the frontier search tree includes the parent node, child nodes, the information structure of the represented frontier, whether the node is fully explored, and whether there is a path connected to the node; at the initial moment, the root node of the frontier search tree is initialized to the current position of the unmanned vehicle, and the child nodes of the root node are initialized by the frontier information set of the first frame; when the unmanned vehicle moves, check whether the newly detected frontier overlaps with the leaf nodes of the frontier search tree; each frontier selects the nearest leaf node that overlaps with it; merge all the matching frontiers of each leaf node, where all the frontier grids are re-clustered to update the leaf node information or split into several child nodes.

6. The autonomous exploration method for an unmanned vehicle in an unknown environment based on a frontier search tree according to claim 1, characterized in that The process of performing trajectory planning for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory includes: using the A* algorithm to perform path planning between the unmanned vehicle and the best target on the voxel grid map in a 2.5D environment, and the path planning result is used as the initial solution of the unconstrained nonlinear optimization problem corresponding to the trajectory planning. Among them, the objective function of the unconstrained nonlinear optimization problem corresponding to the trajectory planning is: Among them, J d represents the kinematic constraint penalty term, J g represents the ground safety constraint penalty, J e represents the elastic band penalty of the z-axis, J f represents the feasibility constraint penalty, w d , w g , w e , w f are the weight coefficients of each penalty term respectively.

7. The method for autonomous exploration of an unknown environment by an unmanned vehicle based on a frontier search tree according to claim 6, wherein The kinematic constraint penalty term is expressed as: where the number of states in the trajectory is n + 1, f(s i , u i , Δt) represents the planar kinematic model of the unmanned vehicle, s i+1 represents the next state of the recursion; u i represents the planar acceleration control quantity; Δt represents the discrete time interval; The ground safety constraint penalty is expressed as: where d i represents the minimum distance from the trajectory state point position to the effective ground grid; s f represents the preset safety distance; L(d i , s f ) is a second-order continuously differentiable function Among them, c j is the demarcation point between the quadratic term and the cubic term; c = d i - s f ; The elastic band penalty of the z-axis is expressed as: where p z,i+1 represents the component on the z-axis of the trajectory state point position; The feasibility constraint penalty is expressed as: where, v max and a max represent the limits of speed and acceleration, w v , w a , w j are the weights of each penalty term; v i represents the speed of the trajectory state point.

8. An autonomous exploration system for an unmanned vehicle in an unknown environment based on a frontier search tree, characterized in that, including: A data acquisition module configured to acquire real-time environmental data and real-time pose data of the unmanned vehicle; A frontier information extraction module configured to establish an updated voxel grid map based on the real-time environmental data and extract valid ground grids; extract the frontier grids in the valid ground grids to form a frontier information set; wherein, the frontier grid is a valid ground grid adjacent to the unknown grid; A search tree update module configured to construct a real-time incremental updated frontier search tree based on the frontier information set; A target selection module configured to traverse the frontier search tree based on the depth-first search strategy to select the best target, including: starting the depth-first search process at the root node of the frontier search tree, setting the currently explored node as the root node, and selecting one of the child nodes of the root node as the next node to be explored; the update of the next node to be explored is achieved by delving into a certain branch of the frontier search tree until reaching the deepest leaf node of the branch; when a leaf node is explored and no child nodes are generated, return to the previous node to explore other unexplored branches, and sort the leaf nodes of other unexplored branches according to the score h(i), and select the leaf node with the highest score as the next node to be explored; repeat the above process until there are no unexplored nodes in the entire frontier search tree, and each time select the center of the frontier of the node to be explored as the target point of the trajectory planning; the score h(i) is calculated according to the following formula: Among them, P(p r ,n i ) represents the collision-free path searched on the ground grid connecting the unmanned vehicle p r and the i-th node n i . length(P(p r ,n i )) represents the path length; FIS represents the information structure of the front represented by this node, and Cells represents the front grids included in the front; size(n i .FIS.Cells) represents the number of grids in Cells; w s and w l represent the weights of each fractional term; l max and s max respectively represent the maximum path length and the maximum front size among the candidates, which are used as the scales for normalizing the size and length; A trajectory planning module configured to perform trajectory planning for the path between the unmanned vehicle and the best target in a 2.5D environment to obtain the planned trajectory route; A control module, configured to cause the driverless vehicle to travel to the optimal target along the planned trajectory route according to the real-time pose data of the driverless vehicle.

Images