Automation and Robots for Disaster Response

Topological Map-Based Autonomous Exploration in Large-Scale Scenes for Unmanned Vehicles

By inergency On Mar 27, 2024

[ad_1]

1. Introduction

Autonomous robot exploration technology requires robots to collect data within a given region and construct corresponding environmental maps. As a critical technology that reveals robotic autonomy, relevant research in robotics has garnered significant attention, driving widespread applications in geological exploration, 3D reconstruction, post-disaster rescue, and other fields.

Numerous autonomous exploration methods have been proposed in recent years and are divided into sampling-based and frontier-based categories. Sampling-based methods originated from the NBV (Next Best View) algorithm in the field of 3D reconstruction. RH-NBV (recurrent hybrid neural-based visual) first introduced the NBV algorithm into the autonomous exploration field [1], which consisted of the robot randomly sampling viewpoints in explored free space, constructing a rapidly exploring random tree (RRT), and evaluating the utility of each branch on the RRT. Finally, the robot focuses on the branch with the highest information reward and selects the first node of this branch as the local target. After that, numerous researchers have extended and improved the RH-NBV to meet the requirements of various application scenarios [2,3,4,5,6]. However, sampling-based autonomous exploration methods have lower exploration efficiency and lead to the robot being trapped. Ref. [7] first introduced the frontier-based exploration method, which groups free voxels adjacent to unknown voxels as frontier clusters and then drives the robot towards these frontier clusters to move to explore unknown areas. Since then, many frontier-based exploration methods have been proposed to meet various application requirements [8,9,10]. Ref. [11] proposed to select a viewpoint with minimal speed changes as the next goal within the Field Of View (FOV) of sensors, aiming to maintain the high movement speed of unmanned aerial vehicles (UAVs) and achieve efficient exploration. Fast UAV expLoration (FUEL) proposes the incremental frontier information structure (FIS) to address the problem of high computation of frontier extraction and low decision frequency of the path planner [12]. Based on FIS, UAVs can quickly and incrementally extract environmental information that the planner needs and promptly plan the exploration path.

However, most autonomous exploration methods tend to greedily guide the robot to exploration scenes with immediate rewards and neglect some targets with long-term rewards, resulting in lower efficiency in global exploration [12]. Although some methods plan paths from the global exploration standpoint, the robot inevitably overlooks some scenarios during exploration because of the limited perception range of sensors and the unpredictability of unknown environments [12,13]. To thoroughly explore a given region, the robot must revisit areas containing those missed scenarios, resulting in a waste of resources. Furthermore, when exploring large-scale scenes, the more information the robot collects with exploration, the more the path planner computes, which poses a significant challenge to onboard computers.

In order to solve the above problems, the work [14] proposed that supplying robots with prior information about a given region can aid them in making decisions that align with long-term benefits. Ref. [15] proposed a probabilistic information gain map as the prior knowledge to guide exploration. Ref. [16] introduced a general information theory framework to control multiple robots to search and rescue, wherein the prior knowledge of people is modeled to capture target positions and dynamics. Ref. [17] employs hand-drawn sketches as prior information, enabling the robot to explore even when the metric description of the environment is incomplete.

As the concision of a topological map, many researchers use them as prior information to guide robots in autonomous exploration. Ref. [18] proposed a novel autonomous exploration method based on a prior topometric graph, which verifies that prior information could aid the robot in swiftly completing the exploration of unknown environments. Ref. [19] proposed a path planning method based on topology information for 3D reconstruction, in which the multi-view stereo path planning is decomposed into a collection of overlapped viewing optimization problems that can be processed in parallel. In [14], the prior topometric map is employed to improve exploration efficiency and guide the robot to trigger a loop close, improving the localization accuracy of the Simultaneous Localization And Mapping (SLAM) system. Finally, the environmental information collected will be used to refine prior information.

Furthermore, some researchers focus on the generation of topological maps. Ref. [20] proposed a framework called “topomap” to provide robots with customized maps to simplify robot navigation tasks, transforming the sparse feature-based map from visual SLAM into a three-dimensional topological map. Ref. [21] proposed an efficient and flexible algorithm that generates a trajectory-independent 3D sparse topological skeleton graph captured from the spatial structure of free space.

Inspired by the abovementioned research, we select the topological map as the prior information to guide in robot exploration and employ the frontier-based exploration method suitable for exploring large-scale scenes to plan exploration paths. As a form of map representation, the topological map briefly provides relative position and connectivity between critical places in complex scenes, which could guide the robot in planning paths that follow long-term benefits [20]. In practical cases, many methods can easily acquire the skeleton structure of the environment as the topological map [20,21,22,23].

To fully take advantage of the guiding function of prior topological maps, we propose an autonomous exploration method based on topological maps. The proposed method employs a hierarchical path planning framework, integrates frontier information with prior topological maps, and plans the exploration path with long-term benefits. It first plans a global exploration path by solving the constructed Priority Constrained Asymmetric Traveling Salesman Problem (PCATSP). The global exploration path would follow optimal or customized global exploration strategies to guide the robot to cover frontiers but prioritizes exploring scenes outside the prior information, thus preventing the robot from revisiting previously visited areas. Then, the exploration path is refined from the global exploration path by quickly evaluating the rewards and costs of the candidate viewpoint for each frontier.

Because of the one-pass exploration process, our method will maintain the frontier at a small number, preventing excessive computational burden on the solver during exploration. The above properties make our method more suitable for autonomous exploration in large-scale scenes, and the contributions of this paper are as follows:

(1) An autonomous exploration method based on prior topological maps. The robot follows an optimal or customized strategy to explore a given region autonomously but prioritizes exploring scenes outside prior information, preventing the robot from revisiting the explored areas.

(2) A path planning method integrates information between frontiers and prior topological maps, which makes the topological map deeply involved in the path planning of robot exploration.

(3) A local path planning method, which quickly evaluates the rewards and costs of each candidate viewpoint to optimize the global exploration path, enhances the stability of exploration efficiency.

2. Design Objectives

Give the robot a region to autonomously explore, and provide it with a topological map of the region to be explored. The topological map should reflect the fundamental layout of the region but may not represent all of its spaces. The objectives we address are as follows:

Objective 1: The robot completes a comprehensive exploration of the given region. When there are no frontier clusters extractable within the given region, it indicates the completion of an information gathering task.

Objective 2: The robot utilizes real-time collected scene information and prior topological maps to plan exploration paths. When the robot encounters scenes that are not included in the priori information, the exploration path will guide the robot to prioritize exploring scenes beyond a priori information.

Objective 3: The exploration path enables the robot to complete an exploration of the visited area in a one-pass manner, preventing the robot from repeatedly visiting the areas that have been explored.

3. Methods

We define the topological map as follows.

G (S, E)

consist of the global targets set

S = {s_{1}, s_{2}, \dots, s_{n}}

and the undirected edges set

E = {e_{1}, e_{2}, \dots, e_{m}}

. s denotes a global target corresponding to a corner or intersection in the environment.

e_{k} = (s_{i}, s_{j})

is an undirected edge, connecting

s_{i}

and

s_{j}

, representing a straight-line scene, such as a road or corridor. With the support of a prior topological map, we can obtain a global exploration strategy

O = {\bar{o_{0}}, \bar{o_{1}}, \bar{o_{2}}, \dots, \bar{o_{h}}}

for the given region by customizing or solving the Chinese Postman Problem (CPP) [24], which is a priority queue.

\bar{o_{k}} = (s_{i}, s_{j})

denotes a directed line segment from

s_{i}

s_{j}

, corresponding to the exploration guidance. Define f as the frontier cluster, and

F = {f^{1}, f^{2}, \dots, f^{n}}

as a set of remaining frontier clusters in a scene. We adopt the incrementally frontier information structure (FIS) proposed by FUEL to update frontier clusters efficiently [12]. Viewpoint sequential queue

{V P}^{k} = {{v p}_{1}^{k}, {v p}_{2}^{k}, \dots, {v p}_{m}^{k}}

of frontier cluster

f^{k}

is extracted by random sampling, where

{v p}_{1}^{k}

is a viewpoint with the largest reward of

f^{k}

and will replace

f^{k}

in constructing PCATSP.

Figure 1 shows an overview of the proposed method, which operates upon a voxel grid map. We employ a hierarchical architecture to plan the exploration path, which consists of global path planning (Section 3.1) and local path planning (Section 3.4). The global path planning module takes a prior topological map, global exploration strategy, and frontier clusters as input to plan the global exploration path based on the Priority Constrained Asymmetric Traveling Salesman Problem (PCATSP). Nodes with priority in PCATSP will be extracted (Section 3.2), and the movement cost of some frontier clusters will also be updated (Section 3.3). Then, the global exploration path is given to the local path planning module, which refines the input path based on rewards and costs of each viewpoint candidate to improve the stability of exploration efficiency. Finally, the exploration path will output to the trajectory generation module. The exploration task will be completed when no frontier clusters can be extracted from environment.

3.1. PCATSP-Based Global Path Planning

PCATSP is a variation of the classic Traveling Salesman Problem (TSP), which aims to find a minimum-cost Hamiltonian circuit, with the constraint that some nodes must be visited before others. If splitting the start and end points of PCATSP into two nodes, PCATSP is equivalent to finding a path between the start and end points that satisfy priority constraints, which is also considered a Sequential Ordering Problem (SOP) [25]. To address Objective 3, our basic idea of global path planning is to solve PCATSP with frontiers and priority-constrained global targets. It is equivalent to inserting frontiers into a sequential queue of global targets, utilizing the global targets to influence the covered sequence of some frontier clusters.

However, the construction of PCATSP faces the following challenges: First, constrained by the perception range of sensor and obstacle obstruction, the robot cannot accurately calculate the movement distance between global targets in unknown space and viewpoints inside the free space. Second, for TSP, the farther the metric distance between frontier cluster and global target, the less influence the global target can exert. So, we need to enable global targets to influence specific frontier clusters.

To address the above challenges, we define the following: if a global target

s_{i}

is inside the free space, and the other global target

s_{j}

connected to

s_{i}

is in unknown space, then the shortest path

P_{p_{k}, s_{j}}

between any node

p_{k}

in free space and global target

s_{j}

in unknown space is given by

$P_{p_{k}, s_{j}} = P_{p_{k}, a} + P_{a, s_{j}},$

(1)

where a is an intersection point of frontier and undirected edge $e_{k}$ that connects $s_{i}$ and $s_{j}$ , as shown in Figure 2a. $P_{a, s_{j}}$ is a portion of $e_{k}$ in unknown space, and $P_{p_{k}, a}$ is a search path from $p_{k}$ to a.

Based on the above definition and by incorporating the solving property of TSP, if all elements in a certain row or column of cost matrix D of TSP are subtracted by the same value, a new cost matrix

D^{*}

for the TSP will be obtained. However, D and

D^{*}

correspond to the same TSP solution [26]. Thus, we can subtract

P_{a, s_{j}}

from all

P_{p, s_{j}}

, and leave the TSP solution unchanged. It is equivalent to setting an agent point for the global target on the frontier. For constructing PCATSP, the intersection point

a^{j}

could be extracted on a frontier cluster as an agent for global target

s_{j}

and the priority can be set to

a^{j}

based on global exploration strategy O.

Hence, based on the above theory, we define agent

a^{j}

as the intersection point of a frontier cluster

f^{k}

and an undirected edge

e_{k} = (s_{i}, s_{j})

, which is associated with a global target

s_{j}

in unknown space and possesses access priority. Then, the path between any point inside free space and an agent on frontier could be found by a path-searching algorithm, and then path length could also be accurately calculated. The method of agent extraction and priority assignment will be elaborated in Section 3.2.

Finally, we can naturally combine the prior topological map with real-time updated scene information in path planning based on PCATSP. We can then solve the PCATSP with the extracted agent set $A = {a^{i}, a^{j}, \dots, a^{l}}$ and frontier cluster set F, where agents with priority will influence the visited order of nearby frontier clusters.

In this section, we suppose that the movement cost

c (p_{i}, p_{j})

between any two nodes

p_{i}

and

p_{j}

is calculated as follows:

$c (p_{i}, p_{j}) = \frac{L (P_{p_{i}, p_{j}})}{v_{max}},$

(2)

where $P_{p_{i}, p_{j}}$ is the shortest search path between nodes $p_{i}$ and $p_{j}$ ; $L ()$ denotes the length of search path; and $v_{m a x}$ is the maximum velocity of the robot.

PCATSP solver provides a path

t o u r

composed of input nodes. By removing A from

t o u r

, we obtain a global exploration path

t o u r_{e x p l o r e} = {{v p}_{1}^{i}, {v p}_{1}^{j}, \dots, {v p}_{1}^{l}}

that satisfies global exploration strategy O, as shown in Figure 2b [27]. However,

t o u r_{e x p l o r e}

cannot guarantee that the robot with the priority will explore areas outside prior information, i.e., Objective 2. To address this objective, frontier clusters that guide the robot towards global targets will be recognized, and movement cost between these frontier clusters and the robot will be increased. Details are discussed in Section 3.3.

Figure 3 is a schematic diagram of the robot exploration process. The blue arrows represent the global exploration strategy, and green arrows are basic exert programs for global exploration paths. Based on our method, the robot explores the given region according to the global exploration strategy but prioritizes exploring scenarios outside prior information. Finally, following the global exploration strategy, the robot actively loops close and completes the exploration.

3.2. Agent Extraction and Priority Assignment

When the position of a global target $s_{i}$ is explored by the robot, other global targets $s_{j}$ connected to $s_{i}$ will be searched on a topological map. Then, the directed line segment $\bar{e_{k}} = (s_{i}, s_{j})$ is constructed based on $e_{k} = (s_{i}, s_{j})$ , indicating from $s_{i}$ to $s_{j}$ . During implementation, we set up some satellite points for each global target to prevent the robot from missing them. The satellite points are evenly distributed at a set distance around the global target point.

The Oriented Bounding Box (OBB) of each new frontier cluster is extracted by principal component analysis (PCA), which is used to extract agents. As shown in Figure 4a, if

f^{k}

is crossed by

\bar{e_{k}}

, OBB of

f^{k}

must be crossed, and the following condition is met:

$\begin{matrix} {()}_{(s_{j} - s_{i}) \times (o b b_{p} - s_{i}) z} {()}_{(s_{j} - s_{i}) \times (o b b_{q} - s_{i}) z} < 0, \\ p, q \in {0, 1, 2, 3}, p \neq q, \end{matrix}$

(3)

where $o b b$ are vertices of OBB. For the new frontier clusters that meet the condition, proceed with the following secondary evaluation to identify which global target the forthcoming agent will belong to:

$\begin{matrix} π > {cos}^{- 1} ((R - f_{a v e}^{k}) \cdot) (s_{j} - f_{a v e}^{k}) \geq χ, \\ 0 < {cos}^{- 1} ((R - f_{a v e}^{k}) \cdot) (s_{i} - f_{a v e}^{k}) \leq ψ, \end{matrix}$

(4)

where R is the position of the robot, and $f_{a v e}^{k}$ is the average position of $f^{k}$ , as shown in Figure 4b.

If the above conditions are met with the newly extracted frontier cluster $f^{k}$ , the cell closest to $f_{a v e}^{k}$ in $f^{k}$ will be defined as an agent $a^{j}$ for global target $s_{j}$ . An agent of the global target is independent of the frontier cluster but is associated with it; if a frontier cluster is deleted or updated, the corresponding agent will be deleted.

The movement cost between an agent

a^{j}

and other nodes of PCATSP are calculated as follows:

$\begin{matrix} c (R, a^{j}) = C, \\ c (a^{j}, R) = 0, \\ c (a^{j}, x) = c (x, a^{j}) = \frac{L (P_{a^{j}, x})}{v_{max}}, x = v p_{1}^{k}, a^{i}, \end{matrix}$

(5)

where C is a large value, ensuring that the robot prioritizes exploring scenes outside prior information.

The priority

{p r y}_{a^{j}}

of agent

a^{j}

is assigned as follows:

${p r y}_{a^{j}} = \{\begin{matrix} u, \bar{e_{k}} = \bar{o_{u}} \\ s i z e (O) + 1, o t h e r s \end{matrix},$

(6)

which is determined by the sequential position of its $\bar{e_{k}}$ in O. $\bar{o_{u}}$ is a directed line segment in O, where u represents its ordinal position in O. The higher the sequential position of $\bar{e_{k}}$ , the lower the priority of the agent extracted from $\bar{e_{k}}$ . If $\bar{e_{k}}$ is not found in O, the priority of the agent equals $s i z e (O) + 1$ , and these agents with the lowest priority will not be actively explored, as shown in Figure 3. Additionally, during exploration, not only will one agent be generated by a directed line segment, but all agents will point towards the same global target. In such cases, we define the agent that is further away from the global target to have a higher priority, ensuring the robot does not miss the scene during exploration.

3.3. Update of Movement Cost of Frontier Clusters

As mentioned earlier, to ensure that the robot prioritizes exploring scenes outside prior information, the system needs to recognize frontier clusters that would guide the robot towards a global target and set a higher cost between them and the robot. To achieve this, we classify frontier clusters into three categories:

The first class includes frontier clusters with agents and other frontier clusters that are adjacent to these frontier clusters. They will guide the robot towards global targets.

The second class includes frontier clusters adjacent to the first class frontier clusters. These frontier clusters may guide the robot towards global targets. We utilize density-based spatial clustering of applications with noise (DBSCAN) algorithm to recognize these frontier clusters and rely on the following methods to identify whether they could guide the robot to global targets [28]:

$\begin{matrix} e^{- r_{s_{j}, f^{k}} / r_{s_{j}, s_{i}}} e^{cos α^{k} - 1} cos β^{k} \geq ε, \\ r_{s_{j}, f^{k}} < r_{s_{j}, R} \leq r_{s_{j}, s_{i}} + γ, \end{matrix}$

(7)

as shown in Figure 5a, $α^{k}$ is the angle between vector $\vec{f_{a v e}^{k} s_{j}}$ and $\vec{s_{i} s_{j}}$ ; $β^{k}$ is the angle between vector $\vec{f_{a v e}^{k} s_{j}}$ and $\vec{R f_{a v e}^{k}}$ ; and r is the Euclidean distance between two points.

However, as the robot explores, the relative position between frontier clusters and robot changes, and frontier clusters cannot always satisfy Equation (7), as shown in Figure 5b,c. Thus, to keep the consistency of determination for these frontier clusters during exploration, we employ the Dynamic Time Warping (DTW) algorithm to evaluate similarities between path

P_{R, a^{j}}

and all paths of

P_{R, F} = {P_{R, f^{1}}, P_{R, f^{2}}, \dots, P_{R, f^{n}}}

, as shown in Figure 5d [29]. If the similarity ranking of

f^{k}

satisfies the following condition,

f^{k}

is still believed to guide the robot towards global target

s_{j}

$r a n k (d t w) (P_{R, a^{j}}, P_{R, f^{k}}) \leq φ .$

(8)

The third class consists of the remaining frontier clusters. They will guide the robot to explore scenes outside prior information.

For the frontier clusters

f k^{*}

that guide the robot towards global targets, the movement cost from the robot to them is set as follows:

$c (R, v p_{1}^{k^{*}}) = C .$

(9)

The movement cost from robot to other frontier clusters

f^{k}

is computed as follows:

$\begin{matrix} c (R, v p_{1}^{k}) = max \{\frac{L (P_{R, v p_{1}^{k}})}{v_{max}}, min () |ξ_{1}^{k} - ξ_{R}|, 2 π - |ξ_{1}^{k} - ξ_{R}|\} ω_{max} + λ {cos}^{- 1} \frac{(v p_{1}^{k} - R) \cdot v_{R}}{∥v p_{1}^{k} - R∥ ∥v_{R}∥} \end{matrix},$

(10)

which considers the path length, yaw change, and motion consistency, where $p_{1}^{k}$ and $ξ_{1}^{k}$ are coordinates, and the yaw angle of viewpoint ${v p}_{1}^{k}$ , $ω_{m a x}$ is the maximum angular change rate; $ξ_{R}$ is the yaw angle of the robot; and $v_{R}$ is the current velocity.

c ({v p}_{1}^{k^{*}}, R)

and

c ({v p}_{1}^{k}, R)

do not impact the solution results of open-loop path planning, we set them to 0. The movement cost between all frontier clusters are calculated as follows:

$\begin{matrix} c (v p_{1}^{i}, v p_{1}^{j}) = c (v p_{1}^{j}, v p_{1}^{i}) = max \{\frac{L (P_{v p_{1}^{i}, v p_{1}^{j}})}{v_{max}}, min () |ξ_{1}^{i} - ξ_{1}^{j}|, 2 π - |ξ_{1}^{i} - ξ_{1}^{j}|\} ω_{max} \end{matrix} .$

(11)

3.4. Local Path Planning

The global path planning module aims to assist the robot in making decisions at a global standpoint for efficient exploration. The local path planning module aims to find the best viewpoints to make the robot to follow. Many previous works refine a path by evaluating the cost and reward for efficient exploration, but they consume significant computational resources in information evaluation [1,30]. Thus, we define the potential reward of a candidate viewpoint as a volume of unknown space within its a Field of View (FoV) and propose a simple and fast reward evaluation method based on incremental frontier information structure (FIS). Finally, the local path is refined by synthesizing the reward and movement cost of each viewpoint candidate.

Cells of a frontier cluster are stored in FIS [12]. We use them to evaluate the volume of unknown space in FOV. As shown in Figure 6, each truncated pyramid is constructed based on cells that the candidate viewpoint could cover, and its volume is calculated as follows:

$V = (h_{r w} {(\frac{h_{r w} e_{c e l l}}{h_{c e l l}})}^{2} - h_{c e l l} {(e_{c e l l})}^{2}) / 3,$

(12)

where $e_{c e l l}$ is width of cell, $h_{c e l l}$ is distance between cell and candidate viewpoint. $h_{r w}$ is effective distance to calculate reward and computed by:

$h_{r w} = min (h_{max}, h_{c e l l} + δ),$

(13)

$h_{m a x}$ is maximum range of FOV, and $δ$ is used to control the depth of truncated pyramid to balance movement cost and expected rewards.

The expected reward

{r w}_{i}^{k}

of a candidate viewpoint

{v p}_{i}^{k}

is evaluated by accumulating V:

${r w}_{i}^{k} = \sum_{x = 1}^{n} V_{x}^{k} + η \sum_{y = 1}^{m} V_{y}^{l},$

(14)

where $V_{x}^{k}$ is taken from current frontier cluster $f^{k}$ , and $V_{y}^{l}$ is taken from next adjacent frontier cluster $f^{l}$ to be visited. m and n, respectively, represent the number of cells that ${v p}_{i}^{k}$ could cover. $η$ is a weight coefficient.

We formulate local path planning as a graph search problem and refine an optimal path from the global exploration path by balancing expected reward and movement cost, where viewpoints of each frontier cluster serve as candidate points. Suppose that the optimal exploration path

p a t h = {{v p}_{i}^{1}, {v p}_{k}^{2}, \dots, {v p}_{j}^{N_{r f}}, {v p}_{1}^{N_{r f} + 1}}

provided by the Dijkstra algorithm will minimize the cost/reward ratio:

$\begin{matrix} c (P_{R, {v p}_{1}^{N_{r f} + 1}}) = \frac{c (R, v p_{i}^{1})}{W_{i}^{1}} + \frac{c (v p_{j}^{N_{r f}}, v p_{1}^{N_{r f} + 1})}{W_{1}^{N_{r f} + 1}} + \sum_{n = 1}^{N_{r f} - 1} \frac{c (v p_{k}^{n}, v p_{k}^{n + 1})}{W_{k}^{n + 1}} \end{matrix},$

(15)

where $N_{r f} + 1$ is the size of the frontier clusters to be optimized, and

$W_{k}^{n} = \frac{{r w}_{k}^{n}}{V_{F O V}},$

(16)

$V_{F O V}$ is the volume of FOV.

Finally, the exploration path $p a t h$ is output to the trajectory generation module.

[ad_2]