HEHA: Bounded Suboptimal Planning for Constrained Routing and Its Use in Heterogeneous Multi-Robot Exploration of Unknown Environments

Longrui Yang; Yiyu Wang; Jingfan Tang; Yunpeng Lyu; Shizhe Zhao; Chao Cao; Zhongqiang Ren

cs.ROOct 5, 2025

HEHA: Bounded Suboptimal Planning for Constrained Routing and Its Use in Heterogeneous Multi-Robot Exploration of Unknown Environments

Authors: Longrui Yang, Yiyu Wang, Jingfan Tang, Yunpeng Lyu, Shizhe Zhao, Chao Cao, Zhongqiang Ren

Organizations: Shanghai Jiao Tong University in China · Carnegie Mellon University, PA, 15213

Abstract

This paper considers the path planning problem for autonomous exploration of an unknown environment using multiple heterogeneous robots such as drones, wheeled, and legged robots, with different capabilities to traverse complex terrains. A key challenge there is to intelligently allocate the robots to the unknown areas to be explored and determine the visiting order of those areas subject to traversability constraints, which leads to a large-scale constrained multiple hamiltonian path problem (mHPP) that needs to be quickly and iteratively solved every time when new space is explored. To address the hallenge, we propose HEHA (Hierarchical Exploration with Heterogeneous Agents) using a recent hierarchical method that decomposes the exploration into global planning and local planning. The major contribution in HEHA is its global planning, where we propose a new routing algorithm RCH (Routing with Constraints and Heterogeneity) that can quickly find bounded sub-optimal solutions for mHPP to minimize the maximum path length (i.e., makespan) among the agents subject to traversability constraints. Additionally, the local planner in HEHA also considers heterogeneity to avoid repeated and duplicated exploration among the robots. The experimental results show, in the presence of heterogeneous traversability, our RCH can solve mHPP with up to 47% smaller makespan than several baselines, and HEHA can reduce the exploration time by up to 30% compared with existing approaches.

Figures & tables

Fig. 1: Exploration using a ground vehicle (GV) and a legged vehicle (LV). (a) shows the results after exploration where the orange and red lines show the robots’ trajectories. (b-d) are images of the real world for the areas (i-iii) in (a). The stairs in area (i) as shown in (b) are first found by GV but is only accessible by the LV. Our planner considers such traversability constraints during path planning. Our video attachment shows the exploration process.

Fig. 2: HEHA System Overview. The red boxed modules, namely global and local planning, are the innovative part of our approach. Other components are similar to the existing hierarchical exploration system [ 9 ] . Different types of robots collect point clouds, and IMU measurements through onboard sensors. Then each robot transmits them back to the master (e.g. a laptop) via the Wi-Fi Mesh network. The system first performs ICP-based registration and combines the results with terrain analysis to construct a grid map. Finally, the planning layer conducts global task allocation and local path planning.

Fig. 3: (a) An example of global planning, where a path in the workspace for a robot corresponds to an edge in the target graph in (b), and the edge cost in (b) is same as the corresponding path length in (a). The color of a target node indicates the assignment constraints. The objective is to find a joint path in the mHPP graph, as in (b), for all robots that minimizes the maximum path cost. (b) The corresponding complete target graph, where $v_{s}^{i}$ , $v_{g}^{i}$ are the start and goal nodes of robots $i=1,2$ and $v_{1},v_{2},v_{3}$ are the target nodes to be visited. (c) Visualization of a search label, which represents a partial solution and its possible successors during expansion. Here, to expand this label, the next moves of robot $2$ are considered as shown by the dashed lines, which correspond to a feasible target or goal node. (d) The proposed MST-based heuristic estimates a lower bound of the remaining cost to visit all unvisited targets, where current robot positions are connected by zero-cost virtual edges.

Fig. 4: A visualization of the terrain analysis process in HEHA. Multiple occupancy grids are generated for different types of robots, and viewpoints are sampled for different robots with different traversability, which are then used for the global planning.

Fig. 5: An illustration of two tricks in Local Planning. (a) shows the set of local viewpoints and the baseline path generated without specialized planning constraints; a specific node $v$ in heterogeneous region (painted as violet) is identified within the overlapping local areas of both robots. (b) shows that $v$ is assigned to the AV due to the high priority of AV. (c) details the initial phase of local planning, where the TSP cost matrix is adjusted to incorporate hetero-frontier cost, encouraging an optimal approach to the viewpoints. (d) displays the final optimized local plan resulting from the cost-adjusted TSP.

Fig. 6: Five grid maps of size $512\times 512$ from a public dataset, which is used in our experiments to generate mHPP instances.

Fig. 7: Makespan (i.e., maximum length of all robots’ path) and Sum of costs (i.e., total length of all robots’ path) comparison under Setting A and B in five maps. Colors of algorithms (B1–B6 and OURS) are consistent across both settings.

Fig. 8: Visualization the solution paths of baseline B2, B3, B4 and OURS on an instance with Setting A. The boxed number is the makespan. Our RCH can distribute the nodes in a more balanced fashion among the robots and thus achieves smaller makespan.

Fig. 9: Time to get final solution under Setting A and B in five maps. Colors of algorithms (B1–B6 and OURS) are consistent across both settings.

Fig. 10: Focal weight $\epsilon$ when RCH terminates under Setting A and B in five maps, which provides a sub-optimality bound of the returned solutions.

Fig. 11: Average solving time on Battleground under varying problem sizes and robot configurations.

Map and Metric	Nearest	NBVP	Ours	Margin
Garden (51m $\times$ 51m)
Time Avg (s)	255.2	284.0	249.4	2.3%
Length Avg (m)	480.5	494.1	309.1	35.7%
Village (81m $\times$ 121m)
Time Avg (s)	1189.4	1024.6	715.0	30.2%
Length Avg (m)	2095.8	1766.4	1204.5	31.8%

TABLE I: Results of exploration in simulation in three maps shown in Fig. 12 . The last column Margin shows the percentage of reduction in exploration time and path length of our HEHA over the best results among the baselines (Nearest, NBVP).

Fig. 12: Exploration in three environments: Garden (A), Village (B), Forest (C). (i) shows the overview of simulation environment. (ii) shows the point cloud map after exploration. (iii) shows the trajectories of the robots using Nearest , NBVP and Ours. (iv) shows the exploration rates over time.

Fig. 13: Exploration in a four-floored environment.

Fig. 14: (a) shows the terrain analysis result with varing surface_tol . (b) shows the exploration efficiency with different terrain analysis result.

Fig. 15: Exploration efficiency with different noise in terrain analysis module.

Village (81m $\times$ 121m)	Time (s)	Length (m)	Reduction of Time
$\alpha=0$	843.7	1209.8	0
$\alpha=0.2$	821.3	1288.0	2.7%
$\alpha=0.4$	743.3	1189.4	11.9%
$\alpha=0.6$	704.3	1189.7	16.5%
$\alpha=0.8$	727.0	1185.2	13.8%
$\alpha=1$	781.3	1222.1	7.4%

TABLE II: Results with varying

\alpha

in hetero-frontier costs.

Map and Metric	NoPr	NoHe	NoLo	Full
Village (81m $\times$ 121m)
Time Avg (s)	758.0	843.7	896.7	704.3
Length Avg (m)	1398.6	1209.8	1405.3	1189.7

TABLE III: Ablation Study of Local Planning. NoPr means no priority assignment. NoHe means no Hetero-frontier cost. NoLo means neither of the two tricks is included. Full means using both.

Fig. 16: Real-world exploration using HEHA. The green and yellow lines show the executed trajectories of the ground vehicle (GV) and the legged vehicle (LV) respectively. The red and blue lines represent the local path of the GV and LV respectively. The red and green boxes indicate the local planning range of the GV and LV respectively. The violet line denotes the global path of the LV. Due to the size of the space, GV only explores locally and there is no global path for the GV. The white points represent the robots’ local viewpoints, the orange points represent global heterogeneous points, and the green points represent global normal viewpoints. The hetero-regions are the areas with stairs where only LV can traverse.

Multi-Robot Exploration

Multi-robot exploration coordinates robot teams to map unknown environments, find targets, and gather information efficiently and safely.

See all 54 papers

Decentralized Heterogeneous Multi-Robot Collaborative Exploration for Indoor and Outdoor 3D Environments
Apr 26, 2026Yuxiang Li, Kun Chen, Jiancheng Wang +3Multi-Robot Motion Planning Multi-Robot Systems
College of Artificial Intelligence, Harbin Institute of Technology (Shenzhen), Shenzhen 518055, China · Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, Hongkong 999077, China
Min-Max Regret Task Allocation and Planning of Heterogeneous Multi-Robot System in Partially Known Environments
Jul 15, 2026Xinkai Liang, Huixuan Chan, Ying Liu +2Minimax Regret Multi-Robot Exploration
Collaborative Task and Path Planning for Heterogeneous Robotic Teams using Multi-Agent PPO
Apr 1, 2026Matthias Rubio, Julia Richter, Hendrik Kolvenbach +1Multi-Robot Task Allocation Multi-Robot Motion Planning
Robotic Systems Lab (RSL), ETH Zürich, Zürich, Switzerland

Apr 26, 2026J/K move · Enter open · S save

Decentralized Heterogeneous Multi-Robot Collaborative Exploration for Indoor and Outdoor 3D Environments

Yuxiang Li, Kun Chen, Jiancheng Wang, Shihao Fang+2

College of Artificial Intelligence, Harbin Institute of Technology (Shenzhen), Shenzhen 518055, China · Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, Hongkong 999077, China

Multi-Robot Motion Planning Multi-Robot Systems Multi-Robot Exploration Multi-Robot Coordination Multi-Robot Task Allocation

Open paper AskPDF arXiv

Heterogeneous multi-robot systems feature significant adaptability for complex environments. However, effective collaboration that fully exploits the robots' potential remains a core challenge. This paper proposes a decentralized collaborative framework for heterogeneous multi-robot systems to autonomously explore indoor and outdoor 3D environments. First, a basic perception map that integrates terrain and observation metrics is designed. Improved supervoxel segmentation is developed to simplify the map structure and form a high-level representation that supports lightweight communication. Second, the traversal and observation capabilities of heterogeneous robots are modeled to evaluate the requirements of task views derived from incomplete supervoxels. These task views are grouped by requirements and clustered to streamline assignment. Subsequently, the view-cluster assignment is formulated as a heterogeneous multi-depot multi-traveling salesman problem (HMDMTSP) that incorporates constraints between view-cluster requirements and robot capabilities. An improved genetic algorithm is developed to efficiently solve this problem while ensuring global consistency. Based on the assignments, redundant views within clusters are eliminated to refine exploration routes. Finally, conflicts between robots' motion paths are resolved. Simulations and field experiments in cluttered indoor and outdoor environments demonstrate that our approach effectively coordinates exploration tasks among heterogeneous robots, achieving superior exploration efficiency and communication savings compared to state-of-the-art approaches.

Abstract

Multi-Robot Exploration

Explore similar work

Decentralized Heterogeneous Multi-Robot Collaborative Exploration for Indoor and Outdoor 3D Environments

Min-Max Regret Task Allocation and Planning of Heterogeneous Multi-Robot System in Partially Known Environments

Collaborative Task and Path Planning for Heterogeneous Robotic Teams using Multi-Agent PPO