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

Apr 13, 2026

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

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.

Yi Wang, Bingxian Mu, Shahab Shokouhi +1

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

Ask PDF

Mar 17, 2026

Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

Motion planning under dynamics constraints, i.e, kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-DOF robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics, both of which cause computational bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops FLASK, a fast parallelized sampling-based kinodynamic motion planning framework for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP problem can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our framework is fast, exact, and compatible with any sampling-based motion planner, while offering theoretical guarantees on probabilistic exhaustivity and asymptotic optimality based on the closed-form BVP solutions. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.

Thai Duong, Clayton W. Ramsey, Zachary Kingston +2

Department of Computer Science, Rice University, Houston, TX, 77005, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Differential Flatness Sampling-Based Motion Planning

Ask PDF

Jun 25, 2026

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

This paper presents BOWConnect, a bidirectional parallel kinodynamic motion planner that addresses three fundamental limitations of existing sampling-based methods: sample inefficiency in high-dimensional state spaces, unreliable cost heuristics under dynamic constraints, and poor performance in narrow passage environments. Unlike classical planners that rely on random control sampling and geometric distance heuristics, BOWConnect integrates Bayesian Optimization over Windows (BOW) as a learning-based steering function within a parallel tree-based exploration framework, enabling each worker to learn local cost maps and constraints to guide sampling toward dynamically feasible and collision-free controls. A bidirectional architecture simultaneously grows forward and backward trees from the start and goal regions in parallel threads, with a spatial hashing mechanism enabling fast connection queries and a boundary value problem solver generating kinodynamically consistent bridge trajectories. Extensive evaluations across ten benchmark environments demonstrate that BOWConnect achieves 100% success while delivering the fastest or near-fastest planning time in complex scenarios, including narrow passages and non-convex spaces where state-of-the-art planners fail or degrade substantially. Real-world deployment on a ground vehicle and a quadrotor confirms real-time planning with no collisions. Videos of real-world and simulated experiments, high-resolution versions of the figures, and the open-source code are available at https://bow-connect.github.io/.

Sourav Raxit, Abdullah Al Redwan Newaz, Jose Fuentes +1

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

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

Ask PDF

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