PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Shashank A. Deshpande, Jonathan P. How

arXiv:2608.26314v1cs.ROmath.OC

TL;DR

Steering-based kinodynamic planners can require difficult boundary-value solves, while forward-propagation planners lack finite-sample guarantees. This paper develops dispersive forward search with deterministic guarantees for differentially flat systems, then combines it with pruning and platform-specific samplers. The resulting DFT* and WWDFT* implementations support competitive offline planning and real-time dynamic-environment operation at embedded-tier compute budgets.

  • Problem

    State-to-state boundary-value problems are often difficult for nonlinear systems, while forward-propagation planners lack finite-sample near-optimality analysis.

  • Method

    The paper uses differential flatness to build locally dispersive command sets and forward trees with deterministic finite-sample near-optimality guarantees.

  • Results

    DFT* retains near-optimality through cost-conditioned dominance pruning, reducing required tree size from exponential to polynomial in the horizon.

  • Takeaways & Limitations

    The approach supports competitive solution quality and real-time dynamic-environment planning at embedded-tier compute budgets.

  • Takeaways & Limitations

    Complete dispersive trees remain exponentially large, and system-agnostic sampling can waste computation through low admissibility acceptance rates.

Abstract

from arXiv · show

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.

1 Introduction

The paper addresses missing finite-sample guarantees for forward-propagation kinodynamic planners, especially when nonlinear systems make state-to-state steering difficult. It develops deterministic theory, dispersive sampling, pruning, and implementations for practical planning.

  • Motivation: Forward-propagation planners avoid boundary-value steering, but their finite-sample convergence rates and near-optimality guarantees remain absent.The unresolved challenge is quantifying how finite command samples cover dynamically feasible trajectories.
  • Theory: Differential flatness enables locally dispersive command sets whose rollouts cover reference trajectories without point-to-point steering.The resulting tree supports deterministic near-optimality bounds for sufficiently regular costs and robust trajectory classes.
  • Theory: The paper establishes deterministic, finite-sample near-optimality guarantees for forward-propagation kinodynamic search on differentially flat nonlinear systems.These guarantees are measured against optimal costs over smooth trajectories robust to constraints and control erosion.
  • Dispersive sampling: It constructs dispersive command samplers for control-affine systems and provides efficient designs for the unicycle, trailer car, and quadrotor.These samplers are necessary for implementing the theoretically prescribed search.
  • Algorithm and evaluation: DFT* uses dominance pruning to control forward-tree growth, while WWDFT* adapts the search for real-time planning in dynamic environments.The evaluation includes offline comparisons with state-of-the-art kinodynamic planners and real-time embedded-tier deployment.

2 Related Work

Related work divides kinodynamic planners into steering-based configuration-space methods and forward-propagation control-space methods. Existing guarantees are strongest for steering-based methods, while forward-propagation methods have weaker finite-sample theory and practical trade-offs.

  • Configuration-space sampling and BVP steering: Configuration-space planners connect sampled states by solving boundary-value problems and inherit strong convergence guarantees when optimal steering is available.PRM*, RRT*, and FMT* provide asymptotic or finite-sample guarantees under relevant steering assumptions.
  • Guarantee taxonomy: Table 1 organizes sampling-based kinodynamic planners by probabilistic versus deterministic guarantees and their convergence properties.Its notation includes returned cost, optimal costs under robustness or control erosion, sample dispersion, and command-set dispersion.
  • Forward propagation: Forward-propagation planners sample controls and numerically propagate dynamics, avoiding steering but historically offering weaker convergence properties.The original kinodynamic RRT has probabilistic completeness with asymptotic discovery of a suboptimal solution.
  • Motion primitives: Motion-primitive libraries provide an alternative when nonlinear boundary-value problems are expensive, using precomputed or regenerated primitives with resolution-dependent guarantees.These approaches relax or structure primitive matching to support graph search.
  • Optimization and hardware: Trajectory-optimization methods can produce high-quality solutions but remain expensive and sensitive to initialization, motivating lighter online formulations and parallel sampling.GPU, SIMD, and batched expansion strategies accelerate propagation, kinematics, collision checks, or frontier processing.

3 Preliminaries

The preliminaries introduce differential flatness, dynamic feedback linearization, and dispersion-based covering concepts. These definitions provide the representation and metric-entropy tools used to analyze finite command sets and trajectory coverage.

  • Differential flatness: Differential flatness represents system states and inputs through a flat output and finitely many of its derivatives.For regular points, it is equivalent to dynamic feedback linearizability through an endogenous feedback transformation.
  • Dynamic feedback linearization: In Brunovský form, the flat input w drives chains of integrators indexed by R, with d denoting the flat dimension.The flat input components and derivative orders organize the transformed system coordinates.
  • State-determined output: A state-determined flat output can be obtained by extending the system so output derivatives become coordinates of a diffeomorphic representation.The paper then writes z=h(x) without loss of generality when the extended state is used.
  • Dispersion: Dispersion is the radius of the largest empty ball centered in the domain, measuring how well a finite set covers that domain.Covering numbers minimize the size of a set at a target dispersion, while packing numbers maximize separated subsets.
  • Metric entropy: Metric entropy log Ns(Z) quantifies covering complexity at scale s, and Lipschitz-class entropy bounds support the paper’s later complexity results.Theorem 3.8 supplies the relevant entropy estimate for Lipschitz functions under the supremum metric.

4 Problem Formulation

The paper formulates a finite-horizon optimal-control problem and rewrites it in flat-output coordinates. Under regularity, flatness, compact inputs, and jet observability, admissible state trajectories correspond to suitable output curves.

  • 4 Problem Formulation: The generic finite-horizon OCP assigns a cost and state-trajectory predicates to controls evolving under nonlinear dynamics.This structure includes kinodynamic motion-planning problems.
  • 4 Problem Formulation: Kinodynamic planning seeks a control signal that stays in time-varying free space, reaches a goal region, and minimizes arrival time.Obstacle clearance is expressed through signed distance, while goal proximity uses distance to the goal set.
  • 4.1 Assumptions: The theory assumes unique trajectories, differential flatness on a fixed regular domain, a compact input set, and jet observability.Jet observability may require extending the state so output derivatives can be recovered from state information.
  • 4.2 Flat Reformulation: Flat reformulation converts the OCP from state trajectories to output traces with bounded derivatives and controls induced by flatness maps.Admissible output curves must start at the initial jet and induce controls lying in U almost everywhere.
  • 4.2 Flat Reformulation: Jet observability and flatness make the state a function of the output jet, so costs and predicates become functionals of that jet curve.Along each trajectory, output derivatives assemble the jet used to evaluate the original OCP quantities.

5 Dispersive Tree Search: Coverage and Complexity

Dispersive forward trees provide deterministic coverage and near-optimality guarantees, but complete coverage of the full trajectory class requires exponential horizon growth. Cost-conditioned dominance pruning preserves near-optimality while reducing tree size and compute to polynomial order under stated OCP structure.

  • Dispersive forward trees: A dispersive forward tree recursively applies every command from a finite unit-duration set, with each local rollout within deviation δ of a command rollout.This construction avoids state-to-state steering while defining the search tree used throughout the theory.
  • Coverage and optimality: The tree covers the smooth trajectory class generated by interior controls at deviation O(δ), uniformly over tree depth, and achieves closure to the robust eroded optimum.The coverage argument uses correctors and spare control authority to track tubes around eroded references.
  • Coverage complexity: Finer local dispersion requires exponentially many commands, and complete-tree coverage of the full smooth class remains exponentially large in the horizon.Packing-number estimates establish that this exponential growth is necessary, not merely an artifact of naive tree expansion.
  • Dominance pruning: For a broad class of costs and predicates, spatio-temporal dominance pruning reduces the required tree size and deployment compute to polynomial order while retaining closure to optimal trajectories.The pruning procedure operates on near-optimal trajectory subclasses and incurs an increment from δA to δA + s in the certified gap.
  • Empirical implications: The optimality-gap rate O(δA) is tight, while double-integrator experiments show terminal error e6 ≈ 0.65 δA and complete trees reaching approximately 10^11 trajectories.The complete-tree scale motivates pruning as a prerequisite for tractable deployment.
  • Implementation: Dominance-pruned search requires compute iterations with substantial parallelization across depths, enabling implementation on embedded-tier parallel processors.DFT* uses breadth-first expansion of the dispersive tree and carries the near-optimality certificate.

6 Dispersive Tree Search: Algorithm and Experiments

DFT* performs breadth-first forward search over dispersive command trees, using depth-synchronous pruning and parallel expansion. Experiments apply system-specific command sets across several platforms and show strong solution quality and computational scaling.

  • Algorithm and implementation: DFT* expands a dispersive forward tree breadth-first, with independently propagated state-command pairs parallelized before pruning.The implementation uses fused CUDA kernels, leaving tree depth as the only sequential search dimension.
  • Algorithmic variants: Cost-conditioned dominance pruning preserves near-optimality while reducing the necessary tree size from exponential to polynomial order in the horizon.A* beam ordering and static pruning are supported specializations for structured problems.
  • Command-set construction: System-specific dispersive command sets are constructed through a pushforward mechanism for the unicycle, trailer car, and quadrotor platforms.The resulting dispersion estimates vanish as the underlying admissible control grid is refined.
  • Offline evaluation: Static-problem search variants reduce planning wall time by up to 44× at the same solution costs.The evaluated Dynobench instances permit both A* beam ordering and static dominance pruning.
  • Offline evaluation: DFT* finds solutions close to optimum within embedded-tier budgets, with wall times shrinking by up to 15× on an RTX 4090.The comparison summarizes results across the offline evaluation tables.
  • Online evaluation: WWDFT* demonstrates safety-assured real-time flight across all 30 runs, while its certified tree can still empty and declare failure.The online implementation does not guarantee recursively feasible planning for unbounded horizons with unknown obstacle dynamics.

7 Conclusion

The paper develops deterministic finite-sample near-optimal kinodynamic planning for differentially flat nonlinear systems and implements it through DFT* and WWDFT*. It reports improved offline and online performance, while identifying command-set efficiency and CPU deployment as limitations.

  • Contributions: The planner provides deterministic finite-sample near-optimality guarantees for differentially flat nonlinear systems.A dispersive forward tree contains a near-optimal trajectory with convergence certified by local dispersion.
  • Contributions: DFT* prunes the forward tree during growth to avoid naive exponential cost while maintaining near-optimality.The paper also develops WWDFT* for receding-horizon online navigation.
  • Results: Offline experiments across multiple platforms report improved performance relative to other state-of-the-art algorithms.The evaluated platforms include unicycles, a trailer car, and quadrotors.
  • Limitations: Theorem 5.8’s general dispersive-sampling mechanism is inefficient at low sample counts because control, state, and flat-input relationships are nonlinear.Efficient deployment therefore requires exploiting additional control-system structure to design command sets.
  • Limitations: DFT*’s breadth-first structure suits parallel processors such as GPUs but is unsuitable for naive CPU deployment without additional low-level programming.The paper identifies vectorization and fine-grained parallelism as plausible CPU implementation avenues.

A Proofs

The appendix develops proof tools for relating flat-trajectory dispersion to the trajectory-space metric. It introduces a top-order reduction based on shared initial jets and integral representations of lower derivatives.

  • Proof tools: Top-order reduction bounds canonical trajectory-space dispersion using supremum-norm dispersion of the highest-order flat derivatives.The lemma assumes two flat trajectories share their initial jet.

A.1 Proof of Proposition 5.6 (Existence and Complexity)

The existence and complexity proof constructs commands by covering admissible top-order flat inputs and integrating the corresponding flat dynamics from the initial jet. The construction yields accepted commands that approximate every admissible trajectory within the target dispersion.

  • Cover construction: The proof covers the admissible top-order derivative tuples using a product of d classes of M-Lipschitz curves.Jet observability supplies the componentwise bounds needed for the covering-number argument.
  • Command construction: For each cover element intersecting the admissible trajectory set, the proof selects a representative tuple and assembles a command set.The resulting commands are built from selected tuples near the cover centers.
  • Command construction: Each selected command is integrated through the flat-system integrator chain from the initial state jet to produce its rollout.The endpoint trajectory and command are thereby obtained from the chosen flat-input tuple.
  • Coverage guarantee: The constructed commands are accepted by construction, and every admissible control trajectory lies within the target dispersion of a selected rollout.The final approximation uses the triangle inequality after matching initial jets.
  • Complexity: The covering-number estimate absorbs a factor of 2 into the constant in the logarithmic bound.This completes the stated complexity estimate.
  • Admissibility: Control-affine dynamics and the flatness maps relate the flat input to an admissible original control through a left inverse of the input matrix.The proof equates the state derivative expressions and solves for the control almost everywhere.

A.3 Proof of Theorem 5.8 (Dispersion Realization)

The proof constructs locally dispersive commands that approximate an admissible control’s flat input while preserving admissibility and bounded jet error. An induction then shows accepted commands can track a reference trajectory across the horizon.

  • The acceptance test retains commands approximating the flat input of an eroded admissible control.This lemma supports the subsequent realization proof.
  • Piecewise-constant commands formed from grid centers approximate subwindow averages of the reference flat input.The command set uses a grid with per-axis spacing 2M/n and cardinality bounded through |A| ≤ |G|^j.
  • Convexity and Jensen’s inequality show the constructed command remains admissible almost everywhere.The resulting control lies in Uε + [−ε, ε]^m ⊆ U.
  • The approximation gives a uniform jet deviation at most δ, sufficient for the margin-transfer lemma when ε ≥ 2LΨδ.The rollouts share their initial jet, and the top-order gap is the flat-input error.
  • Inductively selecting accepted commands preserves bounded jet error and extends the reference-tracking invariant through the full horizon.At each step, a corrector control supplies an accepted command whose next state remains close to the reference.

A.6 Proof of Theorem 5.11 (Eroded Optimality)

The proof establishes eroded optimality by approximating a near-optimal admissible control with finite accepted command sequences. Finite-tree recurrence then yields a feasible trajectory whose cost approaches the optimum.

  • A margin condition γ ≥ LG E, with E := CBδA + δA, supplies the robustness required by the construction.The proof begins from a control in the nonempty class U_M,γ,ε.
  • Theorem 5.10 produces accepted command sequences whose trajectories satisfy the stated approximation bound over horizon T.The construction is applied to increasingly accurate controls from the admissible class.
  • Because the accepted sequence set is finite, one sequence recurs infinitely often and defines a feasible trajectory.Passing to the limit along the recurrence establishes the cost bound.
  • The constructed perturbed trajectories remain in ZK because their induced controls stay within the ε-eroded admissible set.The control deviation is at most η, with u0 + [−η, η]^m ⊆ Uε.
  • Sign-pattern bump perturbations generate 2^Kn pairwise separated feasible trajectories, forcing the corresponding tree to contain distinct branches.This establishes the coverage-complexity lower bound.

A.8 Proof of Theorem 5.17 (Sparse Tree Optimality)

The sparse-tree proof combines cost-conditioned dominance pruning with inductive tracking of an eroded reference trajectory. It preserves near-optimality while bounding the pruned tree polynomially in the horizon.

  • The pruned tree contains at most K (8KM/s)^d (8M/s)^{|R|−d} + 1 nodes, including the root.The bound follows from coordinate ranges and occupancy of the s-grid.
  • The induction maintains a retained node whose jet and summary remain close to those of the reference trajectory.The construction starts at the initial node and extends one accepted command per window.
  • Windowed Lipschitz bounds preserve predicate margins during tracking, yielding nonnegative predicate values for every g ∈ G.The proof uses γ ≥ LG(CB + 1)ρ to show the margin remains sufficient.
  • Dominance pruning retains at most one node per occupied grid cell while preserving a node satisfying the induction invariants.The surviving node has no worse summary cost than the candidate it replaces.
  • Every committed window remains separated from moving obstacles by radius r under the stated exact-execution and displacement assumptions.The proof propagates safety inductively across commits.

B Command Set Dispersion for the Platform Catalog

The platform-specific constructions instantiate dispersive command sets for unicycles, trailer cars, and quadrotors using bounded grids and smoothness assumptions. Their approximation errors vanish as grid spacings and the time window decrease.

  • Each platform samples command channels from explicit bounded grids satisfying the uniform input bound required by Theorem 5.17.The grid boxes must lie within [−2M, 2M]^d.
  • 1. Unicycle, first order: For the first-order unicycle, gridded acceleration and angular-rate channels approximate averaged reference values under bounded derivatives and speeds.The construction uses nearest grid values on each subwindow.
  • 2. Unicycle, second order: d(πref, π*; [0, Δ]) ≤ c(h0 + h1 + h2 + hs + (M + Ms)Δ) for the second-order unicycle.The constant depends on gains, thrust bounds, body-rate bounds, the envelope constant, and the window.
  • 3. Car with trailer: For the trailer car, the dispersion constant depends on speed, wheelbase, hitch length, and window duration, and vanishes with grid refinement and subdivision.The construction imposes mild additional restrictions on the input signal.
  • 4. Quadrotor: The quadrotor construction grids thrust, thrust rate, higher derivatives, and body-rate quantities while assuming thrust interiority and geometric separation.The flat jet depends on thrust and its rate in addition to the state.

C Search Variants on the Dynobench Instances

The search variants are evaluated on Dynobench instances across five runs at the Orin Nano compute tier, using DFT* as the headline comparison algorithm. Static variants use a pruning radius lowered by 0.01 to avoid starving the frontier on some runs.

  • Table 4 compares the Section 6.1.1 search variants on the Section 6.2 Dynobench instances against DFT*.Results are averaged over 5 runs at the Orin Nano compute tier.
  • Static variants lower the pruning radius by 0.01 for the reported comparisons.Without this adjustment, their cross-depth claims can starve the frontier short of the goal on some runs.
  • The media extensions include a WWDFT* hard dynamic episode and a DFT* Dynobench-solutions video.
Loading 2608.26314v1…