Estimated Informed Anytime Search for Sampling-Based Planning via Adaptive Sampler

TL;DR

This paper introduces MIT*, an advanced sampling-based path planner that uses adaptive informed sets and sampling strategies to significantly improve convergence speed and efficiency in high-dimensional robotic path planning tasks.

Contribution

MIT* is a novel planner that constructs estimated informed sets from prior solution costs and employs an adaptive sampler, enhancing efficiency over existing methods.

Findings

MIT* outperforms existing planners in R^4 to R^16 problems.

It achieves faster initial convergence and higher success rates.

Demonstrated effectiveness in real-world robot manipulation tasks.

Abstract

Path planning in robotics often involves solving continuously valued, high-dimensional problems. Popular informed approaches include graph-based searches, such as A*, and sampling-based methods, such as Informed RRT*, which utilize informed set and anytime strategies to expedite path optimization incrementally. Informed sampling-based planners define informed sets as subsets of the problem domain based on the current best solution cost. However, when no solution is found, these planners re-sample and explore the entire configuration space, which is time-consuming and computationally expensive. This article introduces Multi-Informed Trees (MIT*), a novel planner that constructs estimated informed sets based on prior admissible solution costs before finding the initial solution, thereby accelerating the initial convergence rate. Moreover, MIT* employs an adaptive sampler that dynamically adjusts the sampling strategy based on the exploration process. Furthermore, MIT* utilizes length-related adaptive sparse collision checks to guide lazy reverse search. These features enhance path cost efficiency and computation times while ensuring high success rates in confined scenarios. Through a series of simulations and real-world experiments, it is confirmed that MIT* outperforms existing single-query, sampling-based planners for problems in R^4 to R^16 and has been successfully applied to real-world robot manipulation tasks. A video showcasing our experimental results is available at: https://youtu.be/30RsBIdexTU

Tables

Table 1. TABLE I : Benchmarks evaluation comparison (unit seconds) (Fig.  5 )

	$𝒞$-space	EIT* (SOTA)			MIT* (ours)			$t_{\text{init}}^{\text{med}}\Uparrow$ (%)
	time $\left({10}^{-4}\right)$	$t_{\text{init}}^{\text{med}}$	$c_{\text{init}}^{\text{med}}$	$c_{\text{final}}^{\text{med}}$	$t_{\text{init}}^{\text{med}}$	$c_{\text{init}}^{\text{med}}$	$c_{\text{final}}^{\text{med}}$
$\text{FG}-{ℝ}^4$	1.9514	0.0068	1.7640	0.7350	0.0053	0.5227	0.7158	22.05
$\text{FG}-{ℝ}^8$	2.0918	0.0085	2.7520	1.7887	0.0065	2.4283	1.6745	23.52
$\text{FG}-{ℝ}^{16}$	2.2267	0.0129	4.4062	3.1841	0.0083	4.1485	3.1171	35.65
$\text{RR}-{ℝ}^4$	1.6610	0.0423	1.4779	1.1062	0.0303	1.3375	1.0807	28.36
$\text{RR}-{ℝ}^8$	1.6589	0.1671	4.2433	2.7571	0.1213	3.9080	2.7217	27.41
$\text{RR}-{ℝ}^{16}$	2.0987	0.2094	4.6672	3.5319	0.1423	4.3738	3.3797	32.04
$\text{DW}-{ℝ}^4$	1.9932	0.0268	2.3135	1.3952	0.0192	2.0259	1.3339	28.73
$\text{DW}-{ℝ}^8$	2.0128	0.0350	3.8081	2.3013	0.0233	3.4246	2.2455	33.42
$\text{DW}-{ℝ}^{16}$	2.3486	0.0661	6.2456	3.8538	0.0389	5.7522	3.7675	41.15
$\text{GE}-{ℝ}^2$	1.0055	0.0065	1.1004	0.8688	0.0051	0.9743	0.8685	21.54
$\text{GE}-{ℝ}^4$	2.0734	0.0075	1.3792	0.9203	0.0052	1.2006	0.9036	30.67
$\text{GE}-{ℝ}^8$	2.2283	0.1203	1.5726	1.3775	0.0157	1.5686	1.3676	86.95

Equations

$$\begin{array}{rl}{\sigma }^{\ast }& =\arg \underset{\sigma \in \Sigma }{\min }\{c(\sigma )|\sigma (0)={\mathbf{x}}_{\text{start}},\sigma (1)\in {\mathbf{x}}_{\text{goal}},\\ & \forall t\in [0,1],\sigma (t)\in X_{\text{free}}\}.\end{array}$$

$$g({\mathbf{x}}_{}):=\underset{\sigma \in \Sigma }{\min }\{c(\sigma )\mid \sigma (0)={\mathbf{x}}_{\text{start}},\sigma (1)={\mathbf{x}}_{}\},$$

$$h({\mathbf{x}}_{}):=\underset{\sigma \in \Sigma }{\min }\{c(\sigma )\mid \sigma (0)={\mathbf{x}}_{},\sigma (1)\in X_{\text{goal}}\},$$

$$X_{\text{subset}}:=\{{\mathbf{x}}_{}\in X_{\text{free}}\mid f({\mathbf{x}}_{})<c_i\}.$$

$${\Phi }_{}({\mathbf{x}}_{}):=\{\begin{array}{ll}1& \text{if }{\mathbf{x}}_{}\in X_{\text{obs}}\\ 0& \text{if }{\mathbf{x}}_{}\notin X_{\text{obs}}\end{array}\text{with}\text{Vol}(X_{\text{obs}})=1,$$

$${\Phi }_{}({\mathbf{x}}_{\text{temp}}|{\mathbf{x}}_{\text{pre}}):={\rho }_{\text{gau}}({\mathbf{x}}_{\text{temp}})\mathcal{B}({\mathbf{x}}_{\text{temp}})/{\Psi }_{\text{const}},$$

$${\Psi }_{\text{const}}:={\int }_X{\rho }_{\text{gau}}({\mathbf{x}}_{\text{temp}})\mathcal{B}({\mathbf{x}}_{\text{temp}})d{\mathbf{x}}_{\text{temp}},$$

$${\pi }_{\text{adapt}}({\mathbf{x}}_{\text{crit}}):={\int }_X{\Phi }_{}({\mathbf{x}}_{\text{temp}}|{\mathbf{x}}_{\text{pre}}){\Phi }_{}({\mathbf{x}}_{\text{pre}})d{\mathbf{x}}_{\text{pre}}.$$

$${\mathbf{x}}_{\text{temp}}:={\xi }_{}\cdot {\mathbf{x}}_{\text{crit}}-{\mathbf{x}}_{\text{pre}},$$

$$\begin{gathered} {\pi }_{\text{adapt}}({\mathbf{x}}_{\text{crit}}):= \\ {\int }_{X\cap X_{\text{obs}}}\frac{{\rho }_{\text{gau}}({\xi }_{}{\mathbf{x}}_{\text{crit}}-{\mathbf{x}}_{\text{pre}})\mathcal{B}({\xi }_{}{\mathbf{x}}_{\text{crit}}-{\mathbf{x}}_{\text{pre}})}{{\Psi }_{\text{const}}}d{\mathbf{x}}_{\text{pre}}. \end{gathered}$$

$${\hat{s}}_{\text{adms}}:=g_{\mathcal{F}}({\mathbf{x}}_s)+\hat{c}({\mathbf{x}}_s,{\mathbf{x}}_t)+\hat{h}({\mathbf{x}}_t),$$

$$\gamma :=\frac{g_{\mathcal{F}}({\mathbf{x}}_s)}{g_{\mathcal{F}}({\mathbf{x}}_s)+\hat{c}({\mathbf{x}}_s,{\mathbf{x}}_t)+\hat{h}({\mathbf{x}}_t)}=\frac{g_{\mathcal{F}}({\mathbf{x}}_s)}{{\hat{s}}_{\text{adms}}}.$$

$$e_{\gamma }:=\sqrt{1+(1-\gamma {)}^2},$$

$${\tilde{s}}_{\text{est}}:={\hat{s}}_{\text{adms}}\cdot e_{\gamma },$$

$$X_{\tilde{\sigma }}:=\{\mathbf{x}\in X_{\text{free}}\mid \tilde{f}(\mathbf{x})<{\tilde{s}}_{\text{est}}\},$$

$$s_{\text{min}}:=\Vert {\mathbf{x}}_{\text{goal}}-{\mathbf{x}}_{\text{start}}{\Vert }_2,$$

$$X_{\tilde{\sigma }}=X_{\text{free}}\cap X_{\text{HES}},$$

$$X_{\text{HES}}:=\{{\mathbf{x}}_{}\in {\mathbb{R}}^n\mid \Vert {\mathbf{x}}_{}-{\mathbf{x}}_{\text{start}}{\Vert }_2+\Vert {\mathbf{x}}_{\text{goal}}-{\mathbf{x}}_{}{\Vert }_2<{\tilde{s}}_{\text{est}}\}.$$

$$X_{\tilde{\sigma }}:=\{{\mathbf{x}}_{}\in X_{\text{free}}\mid \Vert {\mathbf{x}}_{}-{\mathbf{x}}_{\text{start}}{\Vert }_2+\Vert {\mathbf{x}}_{\text{goal}}-{\mathbf{x}}_{}{\Vert }_2<{\tilde{s}}_{\text{est}}\},$$

$$X_{\tilde{\sigma }}=\bigcup _{o=1}^m{X}_{\tilde{\sigma },o},$$

$$X_{\tilde{\sigma },o}:=\{{\mathbf{x}}_{}\in X_{\text{free}}\mid \Vert {\mathbf{x}}_{}-{\mathbf{x}}_{\text{start}}{\Vert }_2+\Vert {\mathbf{x}}_{\text{goal,o}}-{\mathbf{x}}_{}{\Vert }_2<{\tilde{s}}_{\text{est,o}}\},$$

$$X_{\text{HES}}:=\{{\mathbf{x}}_{}\in {\mathbb{R}}^n\mid ({\mathbf{x}}_{}-{\mathbf{x}}_{\text{center}}{)}^T{\mathbf{R}}_{}{\mathbf{P}}_{}^{-1}{\mathbf{R}}_{}^T({\mathbf{x}}_{}-{\mathbf{x}}_{\text{center}})<1\},$$

$${\mathbf{x}}_{\text{center}}:=\frac{{\mathbf{x}}_{\text{start}}+{\mathbf{x}}_{\text{goal}}}{2},$$

$${\mathbf{P}}_{}:=diag\left(\frac{{\tilde{s}}_{\text{est}}^2}{4},\frac{{\tilde{s}}_{\text{est}}^2-s_{\text{min}}^2}{4},...,\frac{{\tilde{s}}_{\text{est}}^2-s_{\text{min}}^2}{4}\right),$$

$$J_{}({\mathbf{R}}_{}):=\Vert {\mathbf{R}}_{}{\mathbf{k}}_{\text{1}}-{\mathbf{I}}_{\text{1}}{\Vert }^2,$$

$${\mathbf{R}}_{}:={\mathbf{U}}_{}{\mathbf{\Lambda }}_{}{\mathbf{V}}_{}^T,$$

$${\mathbf{\Lambda }}_{}:=\{\begin{array}{ll}{\mathbf{I}}_{}& \text{if }det({\mathbf{U}}_{}{\mathbf{V}}_{}^T)=1,\\ diag(1,1,...,-1)& \text{if }det({\mathbf{U}}_{}{\mathbf{V}}_{}^T)=-1.\end{array},$$

$${\mathbf{B}}_{}:={\mathbf{k}}_{\text{1}}{\mathbf{I}}_{\text{1}}^T=(({\mathbf{x}}_{\text{goal}}-{\mathbf{x}}_{\text{start}})/s_{\text{min}}){\mathbf{I}}_{\text{1}}^T.$$

$$X_{\text{ball}}:=\{{\mathbf{x}}_{}\in {\mathbb{R}}^n\mid \Vert {\mathbf{x}}_{}{\Vert }_2<1\},$$

$${\mathbf{x}}_{\text{hes}}={\mathbf{R}}_{}{\mathbf{L}}_{}{\mathbf{x}}_{\text{ball}}+{\mathbf{x}}_{\text{center}},$$

Full text

Note to Practitioners— The motivation for this work stems from the challenges faced by existing informed path planners in high-dimensional, continuously valued environments, particularly when an initial feasible solution is difficult to find. Traditional asymmetric bidirectional planners rely on the best current solution to define problem subsets. When a lazy path has been found through lazy reverse search, these planners tend to re-sample and explore the entire problem space, which could hinder the path planning process. Our proposed MIT* algorithm addresses this issue by constructing an estimated informed set based on prior admissible solution costs before finding the initial solution. This estimated set helps to narrow the search area, thereby accelerating the initial convergence rate. MIT* also integrates an adaptive sampling strategy that dynamically adjusts based on the ongoing exploration process, enhancing the planner’s ability to efficiently navigate through challenging spaces. Furthermore, MIT* employs adaptive sparse collision checks, which guide the lazy reverse search that balances computational efficiency with accuracy in pathfinding. The proposed algorithm can be applied to industrial robots, humanoid robots, or service robots to achieve efficient path planning.

I Introduction

Trajectory planning is a fundamental challenge in robotics, aiming to devise a feasible path for a robot while avoiding collisions. This task can be framed as finding collision-free paths from a start to a goal state in state space while avoiding obstacles [1, 2]. In practical applications, path planning for robot manipulators remains computationally intensive, particularly in high-dimensional spaces. The most resource-demanding aspect of this process is often the collision checking step. To address these challenges, a variety of methods have been developed, each designed to enhance the efficiency and effectiveness of robot path planning.

Popular graph-based algorithms, such as Dijkstra’s [3], find the shortest path by exploring all routes in a discrete graph, while the A* algorithm [4] improves efficiency with heuristic search. The Anytime Repairing A* (ARA*) [5] provides progressively optimal solutions, ensuring feasible paths at anytime. However, applying these methods to continuous spaces requires discretization, which is either sparse (yielding suboptimal paths) or dense (incurring high computational costs due to the curse of dimensionality [6]). To tackle this trade-off, sampling-based planners such as Rapidly-exploring Random Trees (RRT) [7], Expansive Space Trees (EST) [8], and Probabilistic Roadmaps (PRM) [9] operate directly in the configuration space ($\mathcal{C}$-space). By sampling configurations and connecting them via local planners with collision checking, these methods efficiently solve high-dimensional planning problems without prior discretization of $\mathcal{C}$-space [10].

RRT-Connect [11] extends RRT to efficiently find a path between two points in $\mathcal{C}$-space by utilizing two RRTs to connect start and goal states [12]. RRT* [13] builds on RRT by incrementally rewiring the tree to ensure asymptotic optimality. In the context of lazy collision checking [14], Lazy PRM [15] and Lazy PRM* [16] assume all edges are initially valid and delay the expensive collision checks until after a path is found. This postponement of collision checks accelerates the planning process. To narrow the search domain, Informed RRT* [17] refines RRT* by employing elliptical informed sampling rely on the current best solution cost (i.e., the informed set) [17, 18], which simplifies the search and sampling set, thereby enhancing the convergence rate. The greedy informed set [19] further improves convergence to the optimal solution by basing the informed set on the maximum admissible estimated cost of the current path. Guided Incremental Local Densification (GuILD) [20] enhances path optimization by defining local subsets of problem domain via beacon selector. However, these sets can only be constructed when an initial solution has been found [21]. When no solution is available, the planners inefficiently re-sample and explore the entire $\mathcal{C}$-space [18].

In this article, we propose a novel sampling-based planner, Multi-Informed Trees (MIT*), which is multi-informed by the prior admissible cost during the initial pathfinding and the current best cost during the optimization. The prior admissible cost is calculated from the lazy reverse search to construct an estimated informed set (Fig. 1). By combining prior costs with a reliability expansion factor, MIT* explores within an ellipsoidal subset of the planning domain, thereby guiding the search while no initial solution has been established yet. Furthermore, MIT* employs an adaptive sampler that adjusts its strategy based on the exploration process, which densifies distribution around obstacles. MIT* improves reverse search by adjusting resolution based on edge length to enhance computational efficiency while maintaining search effectiveness.

MIT* outperforms state-of-the-art (SOTA) methods in initial solution time, initial solution cost (e.g., path length), and final solution cost (i.e., optimized path) across generalized simulation benchmarks and various real-world experiments.

The contributions of this work are summarized as follows:

Estimated informed set: Constructs a set before finding the initial path, using prior costs and expansion factors to effectively guide the exploration and sampling process. 2. 2.

Adaptive sampling strategy: Integrates dynamic sampling adjustments to improve distribution around obstacles and in narrow corridors based on the exploration process. 3. 3.

Real-world application in robotic manipulation: MIT* was evaluated in versatile real-world tasks and achieved the highest success rate across multiple trials.

The rest of this article is organized as follows. Section III introduces the problem definition. In Section IV, the MIT* algorithm with the proof of asymptotic optimality, is presented in detail. Simulation and experimental results are discussed in Section V. Finally, Section VI concludes this article.

II Related Work

In sampling-based motion planning, a common drawback is the slow convergence to an optimal solution. This section introduces popular methods to expedite path optimization.

II-A Informed Optimal Path Planning

Recently, researchers have proposed various heuristics to improve the convergence of sampling-based algorithms to an optimal path [22, 23, 24]. Batch Informed Trees (BIT*) [25] builds on Informed RRT* by constructing an implicit random geometric graph (RGG) [26] and performing a step-wise search similar to Lifelong Planning A* (LPA*) [27]. BIT* focuses on almost-surely asymptotic optimality by refining the RGG through batch processing [28], aggregating multiple state batches to form a denser graph over time, which reduces computational overhead while improving solution quality. Advanced BIT* (ABIT*) [29] introduces inflation and truncation factors to balance exploration and exploitation in the denser RGG approximation, enhancing its effectiveness in complex environments. Energy-efficient BIT* for reconfigurable robots (EBITR*) [19] utilizes a greedy informed set, based solely on the maximum heuristic cost of the current solution’s state. Adaptively Informed Trees (AIT*) [14, 30] use adaptive heuristics and an asymmetrical search strategy, incorporating sparse collision checks during the reverse search phase to improve precision by continuously updating the heuristic based on the evolving approximation. BiAIT* [31] employs symmetrical bidirectional search for both heuristic and space searching. Effort Informed Trees (EIT*) [14], a SOTA planner, utilize admissible cost (i.e., the lower bound on the true value) and effort heuristics (e.g., the number of collision checks) to optimize objectives with obstacle clearance [32].

Despite these advancements, these planners typically rely on uniform sampling strategies, which may struggle with critical zones such as narrow passages. Uniform sampling often has a lower probability of sampling in such challenging regions, potentially impacting the efficiency and effectiveness of pathfinding in complex, high-dimensional spaces.

II-B Non-uniform Sampling Methods

Improved sampling strategies are crucial for navigating complex environments. Learning-based sampling [33] distribution from demonstrations using a conditional variational autoencoder, Neural RRT* [34] trains a neural network to perform nonuniform sampling, leading to more efficient tree expansion. Techniques like Obstacle-based PRM (OBPRM) [35] enhance sampling distribution near obstacles by generating samples close to obstacle surfaces through perturbation, which improves path discovery in constrained regions. Gaussian sampling [36] biases samples toward obstacle boundaries, increasing the likelihood of finding paths through tight spaces by placing one sample inside and another outside an obstacle’s influence. Bridge test sampling [37] connects pairs of samples with a bridge to detect narrow passages, concentrating sampling efforts on critical pathways in cluttered environments. However, OBPRM may result in uneven sample distribution, Gaussian sampling assumes that data follows a normal distribution, and Bridge Test samplers focus only on the midpoint, potentially missing key samples along the edge.

In contrast to previous work, we utilize prior knowledge from lazy search attempts (i.e., prior admissible costs) to define the estimated informed set. Moreover, we implement an adaptive sampler that adjusts its sampling strategy dynamically and increases sampling density in critical zones.

III Preliminaries

In this section, we first formulate the optimal path planning and then provide notations for the MIT* algorithm (Alg. 1).

III-A Problem Formulation

Definition 1 (Optimal Planning): Define a planning problem with the state space $X\subseteq\mathbb{R}^{n}$. Let $X_{\textnormal{obs}}\subset X$ represent states in collision with obstacles, and $X_{\textnormal{free}}=\textnormal{cl}(X\setminus X_{\textnormal{obs}})$ denote the resulting permissible states, where $\textnormal{cl}(\cdot)$ represents the closure of a set. The initial state is denoted by $\mathbf{x}_{\textnormal{start}}\in X_{\textnormal{free}}$, and the set of desired final states is $X_{\textnormal{goal}}\subset X_{\textnormal{free}}$. A sequence of states $\sigma:[0,1]\mapsto X$ forms a continuous map (i.e., a collision-free path), and $\Sigma$ represents the set of all nontrivial paths [13].

The optimal solution, represented as $\sigma^{*}$, corresponds to the path that minimizes a selected cost function $c:\Sigma\mapsto\mathbb{R}_{\geq 0}$. This path connects the initial state $\mathbf{x}_{\textnormal{start}}$ to any goal state $\mathbf{x}_{\textnormal{goal}}\in X_{\textnormal{goal}}$ through the free space:

[TABLE]

where $\mathbb{R}_{\geq 0}$ denotes the non-negative real numbers. The cost of the optimal path is $c^{*}$, and $t$ represents the continuous time parameter over the path $\sigma$. Considering set of states, $X_{\textnormal{samples}}\subset X$, as a graph where a transition function determines edges, we can describe its properties using a probabilistic model implicit in dense RGGs when these states are randomly sampled, i.e., $X_{\textnormal{samples}}=\{\mathbf{x}\sim X_{\textnormal{free}}\}$, as discussed in [26].

Definition 2 (Potential subsets of problem domain): Let $g(\mathbf{x}_{\textnormal{}})$ denote the cost of the optimal path from the start to a state $\mathbf{x}_{\textnormal{}}\in X_{\textnormal{free}}$, where the optimal cost-to-come is defined as:

[TABLE]

Let $h(\mathbf{x}_{\textnormal{}})$ denote the cost of the optimal path from $\mathbf{x}_{\textnormal{}}$ to the goal region, where the optimal cost-to-go is defined as:

[TABLE]

The cost of the optimal path from $\mathbf{x}_{\textnormal{start}}$ to $X_{\textnormal{goal}}$, constrained to pass through $\mathbf{x}_{\textnormal{}}$, is given by $f(\mathbf{x}_{\textnormal{}}):=g(\mathbf{x}_{\textnormal{}})+h(\mathbf{x}_{\textnormal{}})$. This defines the states within the subsets that can potentially yield a solution better than the current solution $c_{i}$ as:

[TABLE]

The defined set ${X}_{\textnormal{subset}}\subseteq X$ represents a subset of the problem domain and depends on the cost (Fig. 1), as discussed in [18].

III-B Notation

The state space is defined as $X\subseteq\mathbb{R}^{n}$ with the initial state $\mathbf{x}_{\textnormal{init}}\in X$ and target states $X_{\textnormal{goal}}\subset X$. Sampled states are noted as $X_{\textnormal{sampled}}$. Forward and reverse trees are represented as $\mathcal{T_{F}}=(V_{\mathcal{F}},E_{\mathcal{F}})$ and $\mathcal{T_{R}}=(V_{\mathcal{R}},E_{\mathcal{R}})$, respectively. The nodes $V_{\mathcal{F}}$ and $V_{\mathcal{R}}$ correspond to valid states. Edges $E_{\mathcal{F}}\subset V_{\mathcal{F}}\times V_{\mathcal{F}}$ in the forward tree link validly connected states, while edges $E_{\mathcal{R}}\subset V_{\mathcal{R}}\times V_{\mathcal{R}}$ in the reverse tree may traverse invalid areas. Each edge $(\mathbf{x}_{s},\mathbf{x}_{t})$ connects source state $\mathbf{x}_{s}$ to target state $\mathbf{x}_{t}$.

The function $c:X\times X\rightarrow[0,\infty)$ denotes the actual cost (path length) of connecting two states, and $\hat{c}:X\times X\rightarrow[0,\infty)$ provides an approximation ensuring $\hat{c}(\mathbf{x}_{i},\mathbf{x}_{j})\leq c(\mathbf{x}_{i},\mathbf{x}_{j})$. The estimated costs from the initial state to any state are given by $\hat{g}(\mathbf{x}):=\hat{c}(\mathbf{x}_{\textnormal{start}},\mathbf{x})$. The admissible cost heuristics to reach the goal are denoted by $\hat{h}(\mathbf{x}):=\min_{\mathbf{x}_{\textnormal{goal}}\in X_{\textnormal{goal}}}\hat{c}(\mathbf{x},\mathbf{x}_{\textnormal{goal}})$. The forward tree cost from the initial state to a given state is $g_{\mathcal{F}}:X\rightarrow[0,\infty)$. The path cost from start to goal via a state is $\hat{f}(\mathbf{x}):=\hat{g}(\mathbf{x})+\hat{h}(\mathbf{x})$. The informed set $X_{\hat{f}}:=\{\mathbf{x}\in X|\hat{f}(\mathbf{x})<c_{\textnormal{curr}}\}$ contains states that can improve the cost of the current solution, denotes as $c_{\textnormal{curr}}$.

Let $A$ be a set and $B,C$ be subsets of $A$. The notation $B\stackrel{{\scriptstyle+}}{{\leftarrow}}C$ denotes $B\leftarrow B\cup C$ and $B\stackrel{{\scriptstyle-}}{{\leftarrow}}C$ denotes $B\leftarrow B\setminus C$.

MIT specific notation:* The admissible initial solution is denoted by ${\hat{\sigma}_{\textnormal{adms}}}$ and its cost by $\hat{s}_{\textnormal{adms}}:X\rightarrow[0,\infty)$. The estimated initial solution is denoted by ${\tilde{\sigma}_{\textnormal{est}}}$ and its cost by $\tilde{s}_{\textnormal{est}}$. The estimated informed set is denoted as $X_{\tilde{\sigma}}$. The reliability parameter of $\hat{s}_{\textnormal{adms}}$ is $\gamma\in(0,1)$. The expand factor of $X_{\tilde{\sigma}}$ is $e_{\gamma}$. The number of sparse checks per edge in iteration $k$ is \Theta_{\textnormal{sparse,k}}. The resolution of sparse checks for edges is \Delta_{\textnormal{sparse,k}}.

IV Multi-Informed Trees (MIT*)

In this section, we first introduce the concept of obstacle-based adaptive sampling. Next, we illustrate the estimated informed set. Then, we propose a sparse collision check based on the edge length. Finally, we analyze the probabilistic completeness and asymptotic optimality of MIT*.

IV-A Obstacle-Based Adaptive Sampler

The sampling strategy of the adaptive sampler is categorized into distinct scenarios. During sampling, the uniform sampler (Alg. 2, line 3-10) is employed to generate the preliminary sampled state, denoted as $\mathbf{x}_{\textnormal{pre}}$, and for conducting the initial validity assessment. The Gaussian distribution-generated temporary state (Alg. 2, line 8) is denoted as $\mathbf{x}_{\textnormal{temp}}$. The state within the critical zone is denoted as $\mathbf{x}_{\textnormal{crit}}$. The adaptive sampling process for valid and invalid states with obstacles-based sampled states distribution is detailed in Fig. 3.

• •

${\mathbf{x}_{\textnormal{pre,val}}}$: Upon validation of preliminary samples ${\mathbf{x}_{\textnormal{pre}}}$, it is incorporated into the resultant sample set ${X}_{\textnormal{sampled}}$.

• •

${\mathbf{x}_{\textnormal{pre,inv}}}$: For an invalid ${\mathbf{x}_{\textnormal{pre}}}$, a Gaussian distribution is utilized to procure a temporary sampled state ${\mathbf{x}_{\textnormal{temp}}}$, positioned at a $\delta$ distance from ${\mathbf{x}_{\textnormal{pre,inv}}}$.

• •

${\mathbf{x}_{\textnormal{temp,val}}}$: If ${\mathbf{x}_{\textnormal{temp}}}$ is valid, it is added to set ${X}_{\textnormal{sampled}}$ to increase the density of samples close to obstacles.

• •

${\mathbf{x}_{\textnormal{temp,inv}}}$: When ${\mathbf{x}_{\textnormal{temp}}}$ is invalid, a binary search is conducted to locate a state ${\mathbf{x}_{\textnormal{crit}}}$ situated between ${\mathbf{x}_{\textnormal{pre}}}$ and ${\mathbf{x}_{\textnormal{temp}}}$, essentially on the edge (${\mathbf{x}_{\textnormal{pre}}}$, ${\mathbf{x}_{\textnormal{temp}}}$).

• •

${\mathbf{x}_{\textnormal{crit,val}}}$: Upon validation of ${\mathbf{x}_{\textnormal{crit}}}$, the first valid ${\mathbf{x}_{\textnormal{crit}}}$ is appended to set ${X}_{\textnormal{sampled}}$ to enhance the density of states in critical zones (e.g., narrow corridors, wall gaps).

• •

${\mathbf{x}_{\textnormal{crit,inv}}}$: If no viable area exists between ${\mathbf{x}_{\textnormal{pre}}}$ and ${\mathbf{x}_{\textnormal{temp}}}$, all ${\mathbf{x}_{\textnormal{crit}}}$ are invalid, the re-sampling process is recommenced.

For the analysis of sample distributions in confined settings. We define the sampling distribution in the critical zone as a weighted mixture of probability functions $\pi_{\textnormal{adapt}}$. Let ${\Phi}_{\textnormal{}}(\cdot)$ be the probability density of any state $\mathbf{x}_{\textnormal{}}\in X_{\textnormal{}}$, thus:

[TABLE]

here, $\texttt{Vol}(\cdot)$ is the $\mathcal{C}$-space volume function. The conditional probability density function of ${\mathbf{x}_{\textnormal{temp}}}$ given ${\mathbf{x}_{\textnormal{pre}}}$ is defined as:

[TABLE]

where $\rho_{\textnormal{gau}}(\mathbf{x}_{\textnormal{}})$ is the density function of Gaussian distribution around ${\mathbf{x}_{\textnormal{pre}}}$, and $\mathcal{B}(\mathbf{x}_{\textnormal{}})$ is a binary function that $\mathcal{B}(\mathbf{x}_{\textnormal{}})=1$ when $\mathbf{x}_{\textnormal{}}\in X_{\textnormal{obs}}$, and 0 otherwise.

[TABLE]

where $\Psi_{\textnormal{const}}\in\mathbb{R}$ is a normalizing constant. Thus we have the probability $\pi_{\textnormal{adapt}}$ created by the adaptive sampler as:

[TABLE]

since ${\mathbf{x}_{\textnormal{crit}}}$ represents the sample points between line segment $\overline{\mathbf{x}_{\textnormal{pre}}\mathbf{x}_{\textnormal{temp}}}$, therefore the ${\mathbf{x}_{\textnormal{temp}}}$ can be defined as:

[TABLE]

where $\xi\in\mathbb{R}^{+}$ is defined as the position of the ${\mathbf{x}_{\textnormal{crit}}}$, with its maximum value equal to the resolution of the state space. By substituting ${\Phi}_{\textnormal{}}(\mathbf{x}_{\textnormal{temp}}|\mathbf{x}_{\textnormal{pre}})$, ${\Phi}_{\textnormal{}}(\mathbf{x}_{\textnormal{pre}})$ and $\mathbf{x}_{\textnormal{temp}}$, we have:

[TABLE]

where ${\Phi}_{\textnormal{}}(\mathbf{x}_{\textnormal{pre}})=1$ (preliminary invalid samples). The $\rho_{\textnormal{gau}}(\mathbf{x}_{\textnormal{}})$ is large if ${\mathbf{x}_{\textnormal{temp}}}$ lies close to ${\mathbf{x}_{\textnormal{pre}}}$, and the integrand in Eq. 8 is non-zero only if ${\mathbf{x}_{\textnormal{temp}}}$ is invalid. Therefore, for a point ${\mathbf{x}_{\textnormal{crit}}}$ in critical zones, the probability density $\pi_{\textnormal{adapt}}$ is higher.

IV-B Estimated Informed Set

To expedite the search process, MIT* introduces the concept of the admissible initial solution (${\hat{\sigma}_{\textnormal{adms}}}$), which is constructed based on the $\mathcal{T_{F}}$ that have undergone a full collision check and failed (Alg. 1, line 9). In this case, the forward tree is connected with the reverse tree by the edge that failed to pass the full collision check. Although this is a failed search attempt, its cost, denoted as ${\hat{s}_{\textnormal{adms}}}$, can still serve as a valuable heuristic for our path planning problem. A comprehensive definition of the cost of ${\hat{\sigma}_{\textnormal{adms}}}$ is provided as follows:

[TABLE]

where $g_{\mathcal{F}}$ is the actual cost to come, $\hat{c}(\mathbf{x}_{s},\mathbf{x}_{t})$ is the edge cost, and $\hat{h}(\mathbf{x}_{t})$ is the admissible cost to reach the goal. Traditional informed set construction delays sampling by requiring the exact solution cost. The ${\hat{\sigma}_{\textnormal{adms}}}$ approach enables an estimated informed set to guide sampling before the initial solution is found. However, directly using ${\hat{\sigma}_{\textnormal{adms}}}$ can overly restrict the estimated informed set, risking the omission of the exact solution. We define the estimated initial solution ${\tilde{\sigma}_{\textnormal{est}}}$, which is calculated using an expansion factor $e_{\gamma}>1$ to expand the sampling range, and a reliability parameter $\gamma$ to determine ${\hat{s}_{\textnormal{adms}}}$, where:

[TABLE]

The expansion factor is then computed as:

[TABLE]

which serves to adjust the extent of the sampling range based on the reliability of the initial estimate. The optimal of $e_{\gamma}$ could be a potential direction for future work.

This yields a hypothetical path ${\tilde{\sigma}_{\textnormal{est}}}$, whose cost is denoted as ${\tilde{s}_{\textnormal{est}}}$ and can be calculated as follows:

[TABLE]

with ${\tilde{s}_{\textnormal{est}}}$, we can formulate the estimated informed set with the detailed definition provided in Fig. 2 and Alg. 3.

An estimated informed set, $X_{\tilde{\sigma}}$, represents all states within ${{X}_{\textnormal{free}}}$, where any state $\mathbf{x}\in X_{\tilde{\sigma}}$ can potentially be part of a path that has a lower cost than the cost of the current ${\tilde{\sigma}_{\textnormal{est}}}$. This set can be mathematically described by the equation:

[TABLE]

where $\tilde{f}(\mathbf{x}):=\tilde{g}(\mathbf{x})+\tilde{h}(\mathbf{x})$ combines the cost to reach state $\mathbf{x}$ from the start $\tilde{g}(\mathbf{x})$ and the cost from state $\mathbf{x}$ to the goal $\tilde{h}(\mathbf{x})$. For problems where the objective is to minimize path length, the estimated informed set typically takes the shape of an $n$-dimensional prolate hyperspheroid or an ellipsoid that is elongated along one axis, which encompasses all points that can result in a path length less than ${\tilde{s}_{\textnormal{est}}}$. The $n$-dimensional hyperellipsoid is defined by the initial position ${\mathbf{x}_{\textnormal{start}}}$ and terminal position ${\mathbf{x}_{\textnormal{goal}}}$, alongside two critical metrics: the cost of the ${\tilde{\sigma}_{\textnormal{est}}}$ so far, ${\tilde{s}_{\textnormal{est}}}$ and conjugate diameters of $\sqrt{\tilde{s}_{\textnormal{est}}^{2}-s_{\textnormal{min}}^{2}}$, where the minimal possible cost ${s_{\textnormal{min}}}$ comes from:

[TABLE]

here, the eccentricity of the ellipse is given by ratio $s_{\textnormal{min}}/\tilde{s}_{\textnormal{est}}$. The estimated informed set is the intersection of the free space, ${{X}_{\textnormal{free}}}$, and an $n$-dimensional Hyper-Ellipsoid (${{X}_{\textnormal{HES}}}$) which symmetric about its transverse axis:

[TABLE]

where

[TABLE]

The estimated informed set ${X_{\tilde{\sigma}}}$ in $\mathbb{R}^{2}$ can be directly defined using the $L^{2}$ norm (i.e., Euclidean distance):

[TABLE]

To extend EIS into multiple goals scenarios based on informed set [18], goal region $X_{\textnormal{goal}}:=\left\{\mathbf{x}_{\textnormal{goal,o}}\right\}_{o=1}^{m}$ set is the union of the individual estimated informed sets:

[TABLE]

where $m$ is the number of goals in the problem domain, therefore the $o$-th EIS $X_{\tilde{\sigma},o}$ can be described as:

[TABLE]

The direct method to generate distributed samples via adaptive sampler in the estimated informed set is defined from direct sampling in subset [17]. We define the $n$-dimensional hyper ellipsoid that contains an estimated informed set as:

[TABLE]

where

[TABLE]

[TABLE]

where $\mathbf{P}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ is the positive symmetric matrix, and the rotation matrix ${\mathbf{R}_{\textnormal{}}}$ can be obtained by minimizing a cost function that measures the alignment error between the rotated hyperellipsoid’s major axis and the first axis of the world frame. The cost function is typically defined as:

[TABLE]

where $\mathbf{k}_{\textnormal{1}}\in\mathbb{R}^{n\crossproduct 1}$ (Alg. 4, line 6) is the major axis of the rotated hyperellipsoid and $\mathbf{I}_{\textnormal{1}}\in\mathbb{R}^{n\crossproduct 1}$ is the first column of the identity matrix, which represents the first axis of the world frame. Thus we can retrieve rotation matrix ${\mathbf{R}_{\textnormal{}}}$:

[TABLE]

where diagonal matrix $\mathbf{\Lambda}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ is:

[TABLE]

where $det(\cdot)$ is the matrix determinant. The unitary matrices $\mathbf{U}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ and $\mathbf{V}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ are defined by the singular value decomposition $\texttt{SVD}(\cdot)$, such as $\mathbf{U}_{\textnormal{}}\mathbf{\Sigma}_{\textnormal{}}\mathbf{V}_{\textnormal{}}^{T}\equiv\mathbf{B}_{\textnormal{}}$, and $\mathbf{B}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ is calculated by the outer product of axes:

[TABLE]

with predefined matrices, we can generate samples directly by transforming samples $\mathbf{x}_{\textnormal{ball}}\in{X}_{\textnormal{ball}}$ into a $\mathbf{x}_{\textnormal{hes}}\in{X}_{\textnormal{HES}}$. The ${{X}_{\textnormal{ball}}}$ is defined as:

[TABLE]

and the transform is given by:

[TABLE]

where $\mathbf{L}_{\textnormal{}}\in\mathbb{R}^{n\crossproduct n}$ is the lower-triangular Cholesky decomposition of the ${\mathbf{P}_{\textnormal{}}}$ such that:

[TABLE]

the ${\mathbf{L}_{\textnormal{}}}$ can be written as:

[TABLE]

This section defines the ${X_{\tilde{\sigma}}}$ which re-projects $\mathcal{C}$-space set to the estimated hyperellipsoid set, and the pseudocode of direct sampling within the estimated informed set is given in Alg. 4.

IV-C Length-related Adaptive Sparse Collision Check

Traditional sparse collision-checking methods are inefficient due to the fixed number of checks, \Theta_{\textnormal{sparse,k}}\in\mathbb{N}^{+}, to all edges. These methods eventually adapt, but their initial check level is not robust (i.e., only one check per edge), leading to inaccuracies. In contrast, MIT* introduces a length-related adaptive sparse collision-checking strategy. MIT* starts with an initial resolution, $\Delta_{\textnormal{sparse,ini}}$ (e.g., $5\times 10^{-6}$ in simulation benchmark), and adjusts it according to the edge length, adapting the resolution for sparse collision checks as follows:

[TABLE]

here, $\lfloor\cdot\rfloor$ is the floor function for integers, \Delta_{\textnormal{sparse,k}} is the next sparse-check resolution, $\Omega\in(0,1)$ is the tuning parameter. The number of edge collision checks $\Theta_{\textnormal{sparse,k}}$ in $\mathcal{T_{R}}$ increases when the edge length is longer. This approach enables MIT* to perform effective initial collision checks and adjust resolution, making the reverse search more reliable and problem-specific.

IV-D Probabilistic Completeness and Asymptotic Optimality

Most sampling-based path planning algorithms have been proven to be probabilistically complete and asymptotically optimal, and MIT* can also guarantee these two properties. As the number of iterations $k$ approaches infinity, the entire space will be explored, satisfying the following condition:

[TABLE]

which means that if there is a feasible path, it must be found by the MIT*. Therefore, the probabilistic completeness of MIT* is guaranteed.

The MIT* implements the same Choose Parent and Rewire strategies as the EIT*. It means that if the rewiring radius $r(q)$ in Choose Parent and Rewire processes satisfies:

[TABLE]

here, $q$ denotes the number of sampled states in the sets, $\eta>1$ is a tuning parameter, $n$ is the dimensionality of the workspace. The term $\lambda(X_{\tilde{\sigma}}\cup X_{\hat{f}})$ represents the Lebesgue measure of the union of the estimated informed set ${X_{\tilde{\sigma}}}$, and the informed set $X_{\hat{f}}$, where ${X_{\tilde{\sigma}}}$ is used in initial pathfinding phase and $X_{\hat{f}}$ is used for path optimization phase. $\zeta_{n}$ is the volume of the unit ball in the current workspace. In reference to Lemma 56, 71, and 72 in [13], the following equation holds:

[TABLE]

where $q$ is the number of samples, $\Sigma_{q}\subset\Sigma$ is the set of valid paths from the start to the goal found by the planner from those samples, $c:\Sigma\rightarrow[0,\infty)$ is the cost function, and $c^{*}$ is the optimal solution cost. It indicates that the MIT* can find an optimal path, if it exists, as the number of iterations go to infinity. Therefore, the asymptotic optimality is guaranteed.

V Experimental Results

In this article, we utilize the Planner-Arena benchmark database [38], the Planner Developer Tools (PDT) [39], and MoveIt [40] to benchmark proposed motion planner behaviors. MIT* was tested against SOTA algorithms in both simulated random scenarios (Fig. 4, resolution $5\times 10^{-6}$) and real-world manipulation problems (Fig. 6, resolution $5\times 10^{-3}$). The comparison involved several versions of RRT-Connect, Informed RRT*, BIT*, ABIT*, AIT*, and EIT* sourced from the Open Motion Planning Library (OMPL) [41]. The construction time costs of the $\mathcal{C}$-space (Table I) across different dimensions are evaluated using the space information constructor of OMPL. The experimental results demonstrate that the $\mathcal{C}$-space construction time remains a minor component of the overall planning process and does not constitute a computational bottleneck. The evaluations are implemented on a computer with an Intel i7 3.90 GHz processor and 32GB of LPDDR3 3200 MHz memory. These comparisons were carried out in simulated environments ranging from $\mathbb{R}^{2}$ to $\mathbb{R}^{16}$. The primary objective for the planners was to minimize path length (cost). The RGG constant $\eta$ was uniformly set to 1.001, and the rewire factor was set to 1.2 for all planners. Gaussian standard deviation $\delta$ is default set to 10% of the maximum extent of $\mathcal{C}$-space.

For RRT-based algorithms, a 5% goal bias was used, with maximum edge lengths of 0.3, 0.5, 1.25, and 3.0 in $\mathbb{R}^{2}$, $\mathbb{R}^{4}$, $\mathbb{R}^{8}$, $\mathbb{R}^{16}$. All batch-sorted planners sampled 100 states per batch, and informed planners defined the informed set $X_{\hat{f}}$ using the current best costs. MIT* leveraged prior admissible costs to guide the search using an estimated informed set $X_{\tilde{\sigma}}$ before discovering the initial solution. It then used an adaptive sampler to target critical areas in confined spaces. The implementation of MIT* planner into OMPL framework is available at: https://github.com/Liding-Zhang/ompl_release.

V-A Simulation Experimental Tasks

The planners were tested across four distinct benchmarks in four domains: $\mathbb{R}^{2}$, $\mathbb{R}^{4}$, $\mathbb{R}^{8}$, and $\mathbb{R}^{16}$. In the first scenario, a constrained flanking gap (FG) environment with a narrow gap was simulated, offering one general direction for optimal path (Fig. 4a). Each planner ran 100 times with varying random seeds, and the computation times for optimal planners are labeled. Figs. 5a, 5b and 5c show that MIT* quickly finds the initial solution in different dimensions with minimal time.

In the second scenario, random widths were assigned to axis-aligned hyperrectangles, creating non-convex regions within the $\mathcal{C}$-space (Fig. 4b). Ten random rectangle (RR) problems were generated for each $\mathcal{C}$-space dimension, with 100 trials per planner. Figs. 5d, 5e and 5f show that MIT* achieved the highest success rates within a limited time.

The third test simulated a constrained dividing wall (DW) environment with multiple narrow gaps, allowing all planners to search optimal paths in various non-trivial directions (Fig. 4c). Each planner ran 100 times with different random seeds, and maximal computation times for optimal planners are shown in the labels. Figs. 5g, 5h and 5i indicate that MIT* outperforms the SOTA planner in both finding the initial solution and converging to the optimal solution.

The last test problem featured a hollow, non-convex C-shaped obstacle surrounding the goal state. In this configuration, even in higher dimensions, the goal could only be accessed through the face of the hyper-obstacle farthest from the start state (Fig. 4d). This setup presents a challenge for sampling-based planners, as many invalid edges near the reverse search tree often require repair. Effective coverage of narrow channels is crucial, and strategies such as biased or adaptive sampling may be necessary to improve efficiency. A direct path planner would result in collisions, resulting in the planner rerouting around obstacles. As shown in Figs. 5j, 5k and 5l, MIT* outperforms SOTA planners by quickly finding an initial solution and converging to the optimal one.

As shown in Table I, MIT* consistently outperforms SOTA in initial median time ($t^{\textit{med}}_{\textit{init}}$) across various scenarios. For instance, in the $\textnormal{FG}-\mathbb{R}^{16}$ scenario, MIT* achieves a $t^{\textit{med}}_{\textit{init}}$ of 0.0083s, which is 35.65% faster than EIT*. Similarly, in the $\textnormal{RR}-\mathbb{R}^{16}$ scenario, MIT* reduces $t^{\textit{med}}_{\textit{init}}$ by 32.04%. In more confined settings, such as $\textnormal{DW}-\mathbb{R}^{16}$ and $\textnormal{GE}-\mathbb{R}^{8}$, the level of improvement is even more substantial, with MIT* achieving a 41.15% and 86.95% reduction, respectively.

Overall, the table highlights the superior efficiency of MIT* in reducing initial median times and enhancing the effectiveness of path planning algorithms.

V-B Real-world Path Planning Tasks

We compare MIT* with SOTA optimal path planners via the base-manipulator robot (DARKO) to evaluate performance in converging to the optimal solution and success rate over 30 runs (Fig. 6). The Beer Barrel-ENV includes simple cup holder obstacles, while the Shelf-ENV and Kitchen-ENV feature navigating through cluttered, narrow spaces. Each scenario requires finding a collision-free path from start to goal.

V-B1 Beer barrel cup placement task

Fig. 6a illustrates the start and goal configurations for the cup placement task. In this scenario, a single robotic manipulator is used to grab a beer cup and position it under the beer tap of a keg, all while avoiding obstacles. All planners were given 1.0 seconds to address the beer barrel cup placement problem. Over the course of 30 trials, MIT* achieved a 93.33% success rate with a median solution cost of 13.2741. EIT* had a success rate of 76.67% with a median solution cost of 16.8917. AIT* was 63.33% successful, with a median solution cost of 18.6277.

V-B2 Industry shelf container rearrangement task

The initial and final configurations for the shelf task are shown in Fig. 6b. The robot grasps an industry-standard (tolerance $\leq$ 5mm) container from a lower position and positions it on the upper layer between two larger containers. The challenge lies in precisely inserting the container into narrow spaces, making the planning of a collision-free path particularly difficult. Each planner was allocated 1.5 seconds to solve the constrained lift-up and insertion problem within a limited space. Across 30 trials, MIT* achieved an 80% success rate with a median solution cost of 9.7145. EIT* had a 60% success rate with a median solution cost of 11.1054. AIT* managed a 53.33% success rate with a median solution cost of 12.7851.

V-B3 Kitchen model pan cooking task

The DARKO robot was positioned in front of a kitchen model for the third task. The start and goal configurations are shown in Fig. 6c. This task is challenging because the manipulator must maneuver the pan within a cluttered oven while avoiding collisions with both the base robot and the kitchen shelves. Each planner was given 5.0 seconds to solve the kitchen pan reallocation problem. Over 30 trials, MIT* achieved a 36.67% success rate with a median solution cost of 15.9380. EIT* had a success rate of 26.67% and a median solution cost of 21.2667. AIT* recorded a 20% success rate with a median solution cost of 23.5611.

In short, compared with the AIT* and the EIT*, the MIT* achieves the best performance in finding the initial solution and converging to the optimal solution.

V-C Dynamic Experimental Re-planning Tasks

We evaluate MIT* with SOTA optimal path planners using the robot arm to benchmark its performance in terms of the optimal solution costs and success rate over 30 runs (Fig. 7). The Horizontal-movement-ENV scenario involves manipulating blocks while avoiding a single dynamic obstacle, whereas the Vertical-movement-ENV scenario features navigation through two vertically moving dynamic obstacles traveling in opposite directions. Each scenario requires computing a collision-free path from the start to the goal configuration.

V-C1 Horizontal-movement block task

Fig. 7a illustrates the start and goal configurations for the horizontal movable block task. In this scenario, a single robotic manipulator is used to grab a table block and navigate between the start and goal positions five times, all while avoiding obstacles. All planners were allocated 0.1 seconds to solve the problem. Across 30 trials, for forward planning from the start to the goal configuration, AIT* achieved an average cost of 8.8167 with a 93.33% success rate, while EIT* achieved 6.7306 with a 90% success rate. Our proposed MIT* attained an average cost of 5.5329 with a 96.67% success rate, representing a 17.78% cost improvement over the best-performing baseline planner.

V-C2 Two vertical-movement blocks task

As illustrated in Fig. 7b, the task starts from the right part of the table and ends at the left. Two dynamic obstacles move repeatedly, one from top to bottom and the other from bottom to top. The robot arm needs to pick up a block from the start and place it at the goal, repeating this process five times. Avoiding collisions with two opposite moving obstacles, constrained narrow space, and limited planning time make the planning problem challenging. Each planner was allocated 0.1 seconds of re-planning time to solve the back-and-forth problem within a limited space. Across 30 trials, AIT* achieved an 86.67% success rate with a median solution cost of 10.2138, while EIT* reached a 90% success rate with a median cost of 9.6934. Our proposed MIT* attained a 93.33% success rate and a median cost of 7.8718, resulting overall in an 18.79% cost improvement.

In summary, MIT* quickly re-planning and converges to high-quality paths, it effectively supports robotic task execution in constrained and dynamic environments.

V-D Discussion

V-D1 Comparison with SOTA Planners

To evaluate the performance of MIT*, we compared it with SOTA planners using success rate and solution cost metrics across three real-world and nine simulation tasks. MIT* demonstrated superior performance in all scenarios. In real-world tasks, MIT* achieved notable improvements: in the Beer Barrel-ENV, it reduced solution cost by 21.55% and improved success rate by 16.67% over EIT*. Compared to AIT*, MIT* lowered cost by 28.73% and raised success rate by 30%. In the Shelf-ENV, MIT* reduced cost by 12.55% and improved success rate by 20% compared to EIT*, and reduced cost by 24.02% with a 26.67% higher success rate than AIT*. In the Kitchen-ENV, MIT* outperformed EIT* by reducing cost by 25% and increasing success rate by 10%, and compared to AIT*, MIT* lowered cost by 32.74% while increasing success rate by 16.67%.

Through simulations and real-world experiments, MIT* is shown to explore and converge more efficiently under the guidance of EIS and an adaptive sampler for dynamic settings, enabling the rapid discovery of feasible solutions.

V-D2 Ablation Study for Contributions

To evaluate the individual contributions of the proposed planner, we performed an ablation study focusing on the adaptive sampler (MIT*-AS), estimated informed set (MIT*-EIS), and length-related sparse collision checking (MIT*-SC) in two representative scenarios (Fig. 8): goal enclosure (GE)-$\mathbb{R}^{2}$ and dividing walls (DW)-$\mathbb{R}^{4}$. In the GE scenario, which lacks narrow passages, the adaptive sampler was less effective as the bridge sampling component was underutilized, resulting in inferior performance compared to MIT*-EIS, which could initially prune the search space. Sparse collision checking offered limited improvement here, slightly reducing path cost but increasing initial solution time due to additional collision checks. In contrast, in the DW scenario, which features numerous narrow passages, the adaptive sampler demonstrated certain advantages, as both the bridge test and Gaussian sampling were effective, outperforming MIT*-EIS. Sparse collision checking showed benefits in this scenario by improving reverse search precision and reducing the need for costly restarts. Overall, MIT*, integrating all contributions, delivered better performance, highlighting the synergistic nature of the proposed methods.

V-D3 Limitations and Future Work

While MIT* demonstrates superior performance, it also has some limitations. The proposed EIS may be overly conservative in environments with irregular cost landscapes or poorly aligned heuristics. In such cases, the repeated enlargement of the EIS could introduce computational overhead, potentially negating the method’s advantages. Although we utilized a dynamic adjustment mechanism to iteratively refine the EIS, its effectiveness relies on the complexity of the problem and the quality of the initial solution cost. Furthermore, the current objective of our approach is to optimize path costs in Euclidean space. This limits the EIS’s applicability to other cost metrics and non-Euclidean spaces, where the EIS may not effectively constrain the search space. Expanding the EIS to include broader optimal objectives (e.g., obstacle clearance, energy efficiency, etc.) and dynamic scenarios (i.e., via local motion planners) is crucial for future works to enhance robustness. In future work, we will integrate our planner with an RGB-D/LiDAR perception stack that continuously refreshes an occupancy or signed-distance map, enabling closed-loop planning around non-convex and dynamic obstacles.

VI Conclusion

In this article, we proposed Multi-Informed Trees (MIT*), a sampling-based planner that is multi-informed by prior admissible cost (i.e., estimated informed set) and current solution cost (i.e., informed set). The novel estimated informed set can improve early-stage exploration, the adaptive sampling strategy can refine distribution in critical regions, and length-related adaptive sparse collision checks for edges can optimize lazy reverse search. Then, the probabilistic completeness and asymptotic optimality were guaranteed. Through simulation across dimensions and real-world experiments, we demonstrated that our algorithm shows faster initial path convergence and shorter path length over the SOTA algorithms in a robust manner. Moreover, real-world tasks can further demonstrate the effectiveness of our method in practical applications.