Dispersive Forward Tree Search for Optimal Control: Coverage, Complexity, and Computation

Shashank A. Deshpande; Jonathan P. How

cs.ROAug 26, 2026

Dispersive Forward Tree Search for Optimal Control: Coverage, Complexity, and Computation

Authors: Shashank A. Deshpande, Jonathan P. How

Organizations: Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, Cambridge, MA, USA

Abstract

Steering-based planners require solutions to state-to-state boundary value problems, which can be inaccessible for nonlinear platforms. Forward propagation evades the steering requirement, but the finite-sample behavior of the associated planners remains uncharacterized and their implementations underperform in practice. This paper develops a propagation-based kinodynamic planner with deterministic finite-sample near-optimality guarantees. We work within the large class of differentially flat nonlinear systems and show that a forward tree of locally dispersive control commands contains a near-optimal trajectory at a certified tree size. We provide a general mechanism to construct dispersive command sets for control-affine systems, which are necessary to implement the search algorithm prescribed by the theory. We show that covering the certified trajectory class irrespective of cost provably demands a tree exponentially sized in the problem horizon, and present a cost-conditioned dominance pruning procedure that retains near-optimality at a tree size polynomial in the horizon. We implement the resulting search algorithm, Dispersive Forward Tree search (DFT*), as breadth-first expansion of the forward tree, which maps naturally onto parallel hardware. We design efficient dispersive samplers for the unicycle, the trailer car, and the quadrotor and evaluate challenging planning tasks for these platforms. DFT* delivers consistently competitive and often substantially better solution quality than state-of-the-art kinodynamic planners at comparable solution times on embedded-tier processors, accelerating further as parallel compute is scaled. We also implement DFT* in a receding-horizon loop to demonstrate real-time planning in dynamic environments at embedded-tier compute budgets.

Figures & tables

Figure 1: Dispersive forward tree search for online navigation in dynamic environments. The forward tree of dispersive commands is grown subject to safety predicates from the static and moving obstacles. The colored segmentation records tree depth, and the concentric rings mark the worst-case reachable sets of nearby moving obstacles at the matching depths.

Class	Planning algorithm	Mechanism	Convergence property
Probabilistic, asymptotic	RRT * /PRM * ( Perez et al., 2012 ; Webb and van den Berg, 2013 ; Schmerling et al., 2015 )	Optimal steering	$\Pr\bigl(\lim_{n}c_{n}=c^{*}\bigr)=1$
	Kinodynamic RRT ( LaValle and Kuffner, 2001 ; Kleinbort et al., 2019 )	Forward propagation	$\Pr\bigl(c_{n}<\infty\bigr)\to 1$
	SST ( Li et al., 2016 )	Forward propagation	$\Pr\bigl(\lim_{n}c_{n}\leq(1+\alpha(\delta))\,c^{*}_{\delta}\bigr)=1$
Probabilistic, finite-sample	FMT * ( Janson et al., 2015 )	Geometric connections	$\Pr\bigl(c_{n}\leq(1+\epsilon)\,c^{*}\bigr)\geq 1-\rho(n)$
Probabilistic, finite-sample	FLASK ( Duong et al., 2026 )	Flat LQMT steering	$\Pr\bigl(c_{n}\leq(1+\epsilon)\,c^{*}\bigr)\geq 1-\rho(n)$
Deterministic, asymptotic	gPRM ( Janson et al., 2018 )	Geometric connections	$\lim_{n}c_{n}=c^{*}$

Table 1: Convergence guarantees in sampling-based kinodynamic planning. Notation:

c_{n}

— returned cost at

n

samples;

c^{*}

— optimal cost;

c^{*}_{\delta}

— optimum over

\delta

-robust solutions;

c^{*}_{\varepsilon}

— optimum under

\varepsilon

-eroded control authority;

D_{n}

— sample dispersion;

\delta_{A}(n)

— command-set dispersion of an

n

-node tree. Remaining symbols are schematic.

Figure 2: Corrective tracking of an eroded reference

\pi_{u}

by a dispersive command trajectory, certified by correctors

\nu

Panel of Figure 3 — Figure 3: Coverage on the double integrator ( $\varepsilon=0.25$ ). (a) Coverage error $e_{k}=\min_{\pi\in\mathcal{A}^{k}}d\bigl(\pi,\pi_{u};[0,k]\bigr)$ for $(j,n)=(1,8)$ , four eroded references (band) and the extremal reference. (b) $e_{6}$ vs the local dispersion $\delta_{\mathcal{A}}$ over $\mathcal{U}^{\varepsilon/2}$ , one point per family.

Figure 4: Dominance pruning illustrated for the 2D single integrator

\dot{p}=u

. At each depth

k

, one representative is retained per occupied state-space cell.

Panel of Figure 6 — Algorithm 1 DFT * — breadth-first forward dispersive search

Figure 5: Depth-two forward trees under the constructed command sets. Gray trajectories represent those pruned off.

(a)		DFT * (ours)		iDb-A *		SST *			RRT * +TO
system	task	cost (s)	wall (ms)	cost (s)	wall (ms)	cost (s)	wall (ms)	cost@60s	cost (s)	wall (ms)
Unicycle 1st order	bugtrap	$\mathbf{21.1}$	$\mathbf{202}$ ( $32$ )	$22.5$	$269$	$72.7$	$325$	$24.4$	$27.8$	$5171$
	kink	$\mathbf{14.0}$	$\mathbf{61}$ ( $18$ )	$21.4$	$147$	$49.7$	$106$	$15.7$	$24.5$	$418$
	parallelpark	$\mathbf{3.5}$	$\mathbf{4}$ ( $5$ )	$3.9$	$5$	$12.8$	$17$	$3.2$	$3.7$	$6$
Unicycle 2nd order	bugtrap	$\mathbf{24.5}$	$3218$ ( $310$ )	$25.3$	$\mathbf{687}$	$65.9$	$5696$	$58.5$	$40.7$	$8156$
	kink	$18.9$	$1116$ ( $107$ )	$\mathbf{18.1}$	$\mathbf{235}$	$95.6$	$6856$	$88.2$	$29.1$	$1399$

Table 2: Offline planning in static environments. (a) First solutions on the Dynobench instances, averaged over

5

runs for each of the evaluated planners and DFT * . DFT * wall times reported use the Orin Nano equivalent compute tier (

8

SMs), with the bracketed blue walls scaling the same computation to the full desktop RTX 4090; the CPU-based planners run on an Intel i9-14900KF. (b) Post-optimization, which both improves the trajectory and repairs its terminal discontinuity, closing the goal tolerance from

\delta_{0}

{\sim}0

. For iDb-A * the optimizer additionally repairs the discontinuities its search leaves between motion primitives, so its first solution is already an optimizer output and we report that cost alone. (c) DFT * against the results reported by FLASK ( Duong et al., 2026 ) , the blue walls again the full RTX 4090, our two rows averaged over

5

runs as in (a). The reported cost is the path length but we continue to solve for minimum time, consistent with FLASK’s LQMT connections. The search variants of Section 6.1.1 are evaluated on the same instances as presented in Table 4 of Appendix C . The cost therein remains the same but the computation times improve due to the narrower search trees.

Figure 7: WWDFT * mid-flight in the hard dynamic environment ( $n_{\mathrm{st}}=70$ , $n_{\mathrm{dyn}}=130$ ). The quadrotor flies from the start to the goal (green, right) through static cuboids and moving spheres, trailing its executed trajectory in black.

			$n_{\mathrm{st}}{=}18$ , $n_{\mathrm{dyn}}{=}32$		$n_{\mathrm{st}}{=}35$ , $n_{\mathrm{dyn}}{=}65$		$n_{\mathrm{st}}{=}70$ , $n_{\mathrm{dyn}}{=}130$		Max violation (%)
Method	Platform	Constraints	Succ.	$T_{\mathrm{trav}}$ (s)	Succ.	$T_{\mathrm{trav}}$ (s)	Succ.	$T_{\mathrm{trav}}$ (s)	$v$	$a$	$j$
FAPP	Triple integrator	$\\|v\\|_{2}{\leq}8$ , $\\|a\\|_{2}{\leq}12.2$ , $\\|j\\|_{2}{\leq}60$	$60\%$	21.06	$60\%$	22.47	$40\%$	20.02	39.3	17.0	2.1
MIGHTY	Triple integrator	$\\|v\\|_{2}{\leq}8$ , $\\|a\\|_{2}{\leq}12.2$ , $\\|j\\|_{2}{\leq}60$	$\mathbf{100\%}$	18.15	$\mathbf{100\%}$	18.61	$\mathbf{100\%}$	19.31	0	0	0.03
WWDFT *	Triple integrator	$\\|v\\|_{2}{\leq}8$ , $\\|a\\|_{2}{\leq}12.2$ , $\\|j\\|_{2}{\leq}60$	$\mathbf{100\%}$	$\mathbf{15.10}$	$\mathbf{100\%}$	$\mathbf{15.35}$	$\mathbf{100\%}$	$\mathbf{15.20}$	0	0	0
WWDFT *	3D quadrotor	$\\|v\\|_{2}{\leq}8$ , $\|\omega\|{\leq}\omega_{\max}$ , $\theta{\leq}45^{\circ}$	$\mathbf{100\%}$	$\mathbf{14.25}$	$\mathbf{100\%}$	$\mathbf{14.50}$	$\mathbf{100\%}$	$\mathbf{14.60}$	0	–	–

Table 3: Real-time planning in dynamic environments. Collision-free success rates and travel times are reported as mean over 5 runs. Relative worst-case violation of each of the imposed

v

a

j

constraints is also reported, structurally absent for WWDFT * .

Panel of Figure 12 — Algorithm 2 WWDFT * — receding-horizon DFT *

Appendix figures & tables1 asset

Supplementary material from the paper’s appendix.

Appendix

		DFT *		DFT * -Static		DFT-A *		DFT-A * -Static
system	task	cost (s)	wall (ms)	cost (s)	wall (ms)	cost (s)	wall (ms)	cost (s)	wall (ms)
Unicycle 1st order	bugtrap	$21.4$	$169$	$\mathbf{21.2}$	$\mathbf{18}$	$21.3$	$82$	$21.5$	$24$
	kink	$14.1$	$50$	$13.6$	$8$	$14.1$	$9$	$\mathbf{13.4}$	$\mathbf{6}$
	parallelpark	$3.5$	$2$	$3.2$	$\mathbf{1}$	$3.4$	$2$	$\mathbf{3.1}$	$2$
Unicycle 2nd order	bugtrap	$24.5$	$3220$	$24.2$	$388$	$24.4$	$1117$	$\mathbf{23.7}$	$\mathbf{333}$
	kink	$18.8$	$1100$	$18.0$	$176$	$19.2$	$176$	$\mathbf{17.6}$	$\mathbf{76}$

Appendix

Table 4: The search variants of Section 6.1.1 on the Dynobench instances of Section 6.2 , averaged over

5

runs at the Orin Nano compute tier and reported against the headline algorithm DFT * . The static variants run at the pruning radius lowered by

0.01

, without which their cross-depth claims can starve the frontier short of the goal on some runs.

Differential Flatness

See all 8 papers

Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition
Apr 13, 2026Yi Wang, Bingxian Mu, Shahab Shokouhi +1Motion Planning Kinodynamic Motion Planning
Department of Mechanical Engineering, University of New Hampshire, Durham, NH, 03824, USA. · Faculty of Sustainable Design Engineering, University of Prince Edward Island, Charlottetown, PE C1A 4P3, Canada
Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness
Mar 17, 2026Thai Duong, Clayton W. Ramsey, Zachary Kingston +2Constrained Motion Planning Kinodynamic Motion Planning
Department of Computer Science, Rice University, Houston, TX, 77005, USA
BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning
Jun 25, 2026Sourav Raxit, Abdullah Al Redwan Newaz, Jose Fuentes +1Constrained Motion Planning Kinodynamic Motion Planning
Department of Computer Science, Louisiana State University New Orleans, New Orleans, LA 70148, USA · Knight Foundation School of Computing and Information Sciences, Florida International University, Miami, FL 33199, USA

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

Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition

Yi Wang, Bingxian Mu, Shahab Shokouhi, May-Win Thein

Department of Mechanical Engineering, University of New Hampshire, Durham, NH, 03824, USA. · Faculty of Sustainable Design Engineering, University of Prince Edward Island, Charlottetown, PE C1A 4P3, Canada

Motion Planning Kinodynamic Motion Planning Sampling-Based Motion Planning Tree Search

Open paper AskPDF arXiv

This paper introduces Bidirectional Tight Informed Trees (BTIT*), an asymptotically optimal kinodynamic sampling-based motion planning algorithm that integrates an anytime bidirectional heuristic search (Bi-HS) and ensures the meet-in-the-middle property (MMP) and optimality (MM-optimality). BTIT* is the first anytime MEET-style algorithm to utilize termination conditions that are efficient to evaluate and enable early termination on-the-fly in batch-wise sampling-based motion planning. Experiments show that BTIT* achieves strongly faster time-to-first-solution and improved convergence than representative non-lazy informed batch planners on two kinodynamic benchmarks: a 4D double-integrator model and a 10D linearized Quadrotor. The source code is available here.

Abstract

Differential Flatness

Explore similar work

Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition

Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning