MUI-TARE: Multi-Agent Cooperative Exploration with Unknown Initial Position

Autonomous exploration using either single or multiple agents has been investigated for decades from different perspectives. Single-agent exploration has been extensively studied in both 2D and 3D, and various approaches have been developed ranging from frontier-based [13, 14], receding horizon sampling-based [15], information theoretic approaches [16, 17, 18], traveling salesman-based [12], etc. Our approach is an extension of the recent hierarchical traveling salesman-based approach TARE [12] to multiple agents.

Multi-agent exploration is more challenging than single-agent and existing work has focused on communication limitation [19], sub-map merging when initial positions of agents are unknown [10], allocating the unexplored areas to agents in a cooperative manner [1], etc. In this work, we limit our focus to the problem where agents’ initial locations are unknown. Many existing multi-agent exploration works are limited to 2D environment [1, 20, 10, 21], and extending them to 3D is challenging due to the increased amount of sensory data (e.g. 3D lidar scans), a large number of frontiers (i.e., the boundary between the explored and unexplored areas), the increased risk of feature mismatch when merging the sub-maps, etc. Inspired by the literature, we compare our approach against several baselines that take similar strategies to merge sub-maps as in [10, 21] to verify the effectiveness of our approach.

Map merging problems in 3D require integrating different sub-maps together, and the map is usually represented as 3D point clouds, occupancy grids [22, 23], or 3D meshes [24]. In this work, we limit our focus to lidar-based multi-agent exploration and consider maps represented as point clouds. To merge two sub-maps accurately and robustly, a key step is to identify unique features from each of the maps and associate the detected features. The SLAM community have proposed numerous methods by leveraging distinguishable semantic objects [25, 11], bag-of-words vectors [26], learned full-image descriptors [27], and sequence-based place recognition [28, 29], to name a few. Our prior work AutoMerge [11] provides a large-scale map alignment approach for multi-agent systems by leveraging a novel spherical harmonic feature extraction method for viewpoint-invariant place recognition and an adaptive sequence alignment method for accurate loop-closure detection. This work leverages AutoMerge to merge sub-maps and estimate the relative poses between agents.

In most SLAM systems, the agents are passive, i.e., the motion of the agents cannot be controlled. In a multi-agent exploration problem, the motion of agents needs to be planned in order to achieve both high exploration efficiency and high quality in sub-map merging, which is the focus of this work.

As shown in Fig. 2, the entire system consists of multiple agents and an MUI-TARE server. During the exploration, the point clouds collected by the agents are sent to the server, and the server plans and sends the paths to the agents for execution. MUI-TARE (Fig. 2) consists of two major components: an adaptive and active map merging module, and a multi-agent sub-map exploration module. Here, the sub-map means the map that is formed by a group of agents whose relative transform is known. The map merging module merges the segments (i.e., the accumulated point cloud) of agents into sub-maps and estimates the relative pose of agents based on the merged sub-maps. Given the point clouds from different agents, our approach detects the potential overlap between the sub-maps via data association: if two series of frames (i.e. the extracted feature at a certain pose) are matched, we say a new inner connection is established between agents. Then, the server plans a path for the nearby agent to verify if this inner connection is a real overlapped area. Once the connection is confirmed, the nearest agent is further navigated to increase the overlap until this new inner connection satisfies the sub-map merge condition. Finally, the overlapped sub-maps are merged into a single sub-map, and the relative position of the agents in this sub-map can be computed.

Based on the sub-maps computed by the map merging module, multi-agent exploration is performed, which uses a hierarchical path planning approach for each sub-map. Specifically, a global coarse path is first planned for agents to visit all the sub-space to be explored. In each sub-space of an agent, a detailed local path is planned to cover all the surfaces in it. MUI-TARE runs the map merging module and the multi-agent exploration module parallel to cover the whole working space.

During the exploration process, we maintain a factor graph $G=\{V,E\}$ to represent the inner connections between the segments of agents. Let $V=\{v_{1},v_{2},...,v_{n}\}$ denote the nodes of the graph which represent the segment obtained by each robot. Then, we define edge $E=\{\omega_{i,j}\}$ as the connections between segments, where $\omega_{i,j}$ is the inner connection of node $v_{i}$ and node $v_{j}$. For the purpose of place recognition, the inner connection of two segments is determined by the overlap length (i.e., the number of consecutive matched frames) and place recognition descriptors. Thus, the inner-connection $\omega_{i,j}$ is defined as,

where $F_{i},F_{j}$ indicates the feature descriptors extracted from the overlapped area of $v_{i},v_{j}$, $L_{i,j}$ represents the overlap length between $v_{i},v_{j}$, and $C_{w},\epsilon$ are hyper-parameters introduced in AutoMerge [11].

In the merging procedure, only the segments with inner connections larger than the threshold will be clustered and merged into the same sub-map. The relative pose between agents, which is represented by a transform matrix, can be determined by the transform estimated in the map merge. However, such stable inner connections usually required long overlap distances which is hard to satisfy when agents are exploring without knowing others’ poses. Thus, using a shorter sequence of matched frames for potential overlap detection will be helpful to increase the chance an overlap can be detected. However, using a shorter sequence of matched frames may cause incorrect data association and large transform matrix errors.

Thus, the adaptive merge will plan a new path for the agent to verify the data association and enhance the inner connection(Fig. 3). As shown in Algorithm. 1, when a potential overlap was detected between agent $i$ in the sub-map $m_{1}$ and agent $j$ in the sub-map $m_{2}$, the inner connection is evaluated and added to the graph $G$. Then, a rough transform matrix $T$ can be estimated by doing the Iterative closest point (ICP) between associated point clouds. Based on eq. 1, the value of the inner connection is positively related to the overlap length and negatively related to the feature difference of the overlap. Thus, if the inner connection does not satisfy the merge condition, the planner will choose one agent $i$, whose distance to overlap is shorter, and execute the active merge to increase the inner connection. To help the path planning of active merge, an estimation of the distance that the agent needs to travel is computed:

where the $\omega^{t}_{i,j}$ is the current inner connection, and $\omega_{thresh}$ is the threshold of the inner connection to guide map merging.

As shown in Algorithm. 2, the planner will first plan a path for agent $i$ to move back to the overlapped region by doing $A^{*}$ search on the past trajectory of agent $i$. Once the agent $i$ reaches the overlap, it will be navigated to visit the key pose of agent $j$ outside of the overlap. Denote the key pose position of agent $i$ in the sub-map $m_{1}$ as $\{^{m_{1}}x_{i}^{0},...,^{m_{1}}x_{i}^{t}\}$, and denote the key pose position of agent $j$ in the sub-map $m_{2}$ as $\{^{m_{2}}x_{j}^{0},...,^{m_{2}}x_{j}^{t}\}$. The trajectory of agent $j$ can be transformed to $\{^{m_{1}}\widehat{x}_{j}^{0},...,^{m_{1}}\widehat{x}_{j}^{t}\}$ using $T$. Here, we use the greedy strategy to navigate the agent to the closest transformed key pose.

In order to measure the newly obtained overlap during the active merge, we define verification gain as:

where, $\omega^{t}_{i,j}$ is the current value of inner connection, $\omega^{t_{0}}_{i,j}$ is the value of inner connection when agent start to increase overlap, $\epsilon$ and $C_{\epsilon}$ are constant value.

During the merge process, if the verification gain is smaller than the threshold, this new connection is regarded as an incorrect connection and gets eliminated from graph $G$. Otherwise, the agent keeps active merge until it meets the sub-map merge requirement. Once the reliable inner connection between sub-maps is established, the sub-maps are merged into a single sub-map as described in [11].

In this work, we extend a state-of-the-art single-agent exploration method TARE to multiple agents. As discussed in the previous part, since the transform of agents in each sub-map is given by the adaptive sub-map merge module, we plan the paths for agents based on these sub-maps. The total number of the sub-maps is denoted as $M$, and let $m$ be the $m-th$ sub-map, where $m=1,2,\dots,M$. Initially, as each sub-map only contains one agent, $M$ equals $N$. As sub-maps get merged, $M$ decreases. Let $K_{m}$ denote the number of agents in sub-map $m$. In each sub-map, a similar sub-spaces-based representation as mentioned in TARE [12] is used. It divided the workspace ${Q}$ into even subspaces. Based on the coverage of the subspace, the status of the subspace will be set to “exploring”, “explored”, or “unexplored”. If all the surfaces in the subspace have been covered, the subspace will be labeled as “explored”. When the subspace contains both uncovered surface and covered surface, the subspace will be assigned an “exploring” label. At the same time, for the subspace that is fully uncovered, the label of the subspace will be “unexplored”.

The path is planned by combining the detailed local path from the subspace planner and the coarse global path from the global planner. For the subspace planners, this paper used the same method as the local planner used in TARE. the local planner samples a set of viewpoints that can cover the surface in the subspace. Then, a local detailed path is planned to travel through all the viewpoints with the shortest distance while satisfying smoothness constraints. For the global planner, instead of using a greedy algorithm, this planner optimized the overall path of all agents as shown in Fig. 4.

The goal of the global planner is planning a set of global path $\tau_{global}=\{\tau_{1}^{global},...\tau_{K_{m}}^{global}\}$ for $K_{m}$ agents in the sub-map $m$ to travel through all the “exploring” subspace. Since the agents travel in parallel, in order to minimize the overall travel time, the planner tries to minimize the maximum travel distance among all the agents:

Thus, this problem can be formulated as a variant of the standard multi-depot vehicle routing problem (MDVRP) as “min-max MDVRP”. Since this problem is NP-hard, heuristic methods are used to solve it [30].

As shown in the Algorithm 3, for $M$ sub-maps in the current working space, the distance between all the exploring cells and agents in each sub-map will be calculated to form the distance matrix $D^{\prime}$. This distance is obtained by doing the $A^{*}$ search on the roadmap acquired during the exploration process. Then, we use the min-max MDVRP solver to solve the minimum longest path using the distance matrix $D^{\prime}$. Given this solution, $K_{m}$ global coarse paths $\{\tau_{1}^{global},...\tau_{K_{m}}^{global}\}$ can be formed for $K_{m}$ agents in the sub-map $m$. With the local path planned by the local planner, each agent replaces the part of the global path that falls in the subspace with its own local path to create the full exploration path. Finally, these full exploration paths $\{\tau_{1},\tau_{2},\dots,\tau_{N}\}$ are sent to all the $N$ agents for execution.

We test MUI-TARE in several different simulation environments. The simulation experiment consists of multiple robots and a central server, each on a different computer. The robots are simulated in Gazebo, and each robot is equipped with a simulated 360-degree Velodyne Lidar. The collected point clouds are used for map merging and path planning for exploration. In addition, an IMU is fused with the Lidar data for state estimation. During the experiment, the maximum speed of the robot is 3 m/s. In our implementation, the simulation environment is divided evenly into sub-spaces, where each sub-space is a 10m×10m×5m cell. We use the ROS multi-master communication package [31] for data exchange between the agents and the server. As we only transfer the point clouds collected by agents, and the waypoints computed by the server. The average bandwidth needed for a single agent is 786 KB/s in our experiments, which is similar to the recent work [21]. As shown in Fig. 6, the experiments are done in two simulation environments from [32].

• •

  Campus (340m $\times$ 340m): A part of the Carnegie Mellon University campus, containing undulating terrains and convoluted environment layout.

• •

  Multi-storage Garage (140m $\times$ 130m, 5 floors): A complex 3D environment with multiple floors and sloped terrains.

This section compares our MUI-TARE with a naive extension of TARE to multiple agents. Specifically, we introduce multi-agent TARE (mTARE) as a baseline, which plans for each agent independently without planning multiple agents together after merging.