Terminal Matters: Kinodynamic Planning with a Terminal Cost and Learned Uncertainty in Belief State-Cost Space

Abstract: In many real-world robotic tasks, robots must generate dynamically feasible motions that reliably reach desired goals even under uncertainty. Yet existing sampling-based kinodynamic planners typically optimize accumulated trajectory costs and treat goal reaching as a feasibility check, rather than explicitly optimizing terminal-state quality, such as goal preference or goal-reaching reliability. In this work, we introduce a terminal-cost formulation for kinodynamic planning that allows terminal-state quality to be optimized alongside accumulated trajectory cost. We prove that AO-RRT, an asymptotically optimal kinodynamic planner, preserves its asymptotic optimality under this augmented objective. We further extend the formulation to belief space and prove that minimizing the Wasserstein distance between the terminal belief and the goal improves a lower bound on the probability of reaching the goal region. The resulting planner, KiTe, uses this terminal-cost objective to encode goal preferences and improve reliability under uncertainty. To support systems without analytical uncertainty models, we learn dynamics and process uncertainty directly from data and integrate the learned belief dynamics into planning. Experiments on Flappy Bird, Car Parking, and Planar Pushing show that KiTe consistently improves goal-reaching success under uncertainty. Real-world Planar Pushing experiments further demonstrate that KiTe can plan effectively with learned dynamics and uncertainty. Source code is available at https://github.com/elpis-lab/KiTe.

• The paper introduces KiTe, a framework that incorporates a terminal cost in AO-RRT to directly optimize terminal state quality under uncertainty.
• It extends planning into belief state-cost space using the Wasserstein metric to establish a lower bound on goal-reaching probability.
• Empirical results in tasks like Flappy Bird, car parking, and planar pushing demonstrate improved goal alignment and performance via learned neural uncertainty.

Kinodynamic Planning with a Terminal Cost and Learned Uncertainty: The KiTe Framework

Introduction

The paper "Terminal Matters: Kinodynamic Planning with a Terminal Cost and Learned Uncertainty in Belief State-Cost Space" (2605.09046) addresses a critical gap in sampling-based kinodynamic motion planning: the failure to directly optimize the quality of terminal states under uncertainty. The authors introduce KiTe, a kinodynamic planning framework that augments Asymptotically Optimal RRT (AO-RRT) with a terminal-cost objective, extending planning from state-cost to belief state-cost space, and propose both theoretical and practical advancements. The framework incorporates learned process uncertainty through neural belief dynamics and provides novel theoretical foundations, including a formal lower bound on goal-reaching probability via the Wasserstein metric.

Problem Formulation and Theoretical Contributions

Traditional kinodynamic motion planners prioritize minimizing cumulative (running) cost over a trajectory (e.g., path length, energy) but treat goal attainment as a binary feasibility check. This leads to solutions that may be suboptimal in terms of terminal state desirability or reliability, especially under uncertainty. The authors generalize the optimality criterion by introducing an explicit terminal cost into the objective:

$$C_T(\mathbf{x}, \mathbf{u}) = \int_0^T \ell(\mathbf{x}(t), \mathbf{u}(t))\,dt + \phi(\mathbf{x}(T))$$

where $\ell$ is the running cost and $\phi$ is the terminal cost function.

KiTe extends AO-RRT to optimize this joint cost while rigorously proving that the inclusion of a terminal cost preserves asymptotic optimality under suitable Lipschitz continuity assumptions for the dynamics, running, and terminal costs. The analysis is extended to belief space, representing state uncertainty as Gaussian distributions and using the 2-Wasserstein distance as a belief-space metric. Crucially, the authors provide new theoretical results:

• Asymptotic Optimality with Terminal Costs: A formal proof (Theorem 1) shows AO-RRT remains asymptotically optimal when objective functions include terminal costs satisfying regularity conditions.
• Belief Space Extension: Under reasonable regularity (e.g., stochastic dynamics with Gaussian process noise continuous in the Wasserstein sense), all theoretical guarantees extend to belief state-cost space.
• Goal-Reaching Probability Lower Bound: A novel lower bound is established for the probability that a terminal belief (modeled as a Gaussian) lands in the goal region, expressed as $1 - W_2^2(b, \delta_g)/r^2$, where $W_2$ is the Wasserstein distance to a Dirac at the goal and $r$ is the goal region radius. This justifies directly penalizing terminal Wasserstein distance to maximize goal-reaching reliability.

Practical Algorithmic Developments

Incorporating Terminal Costs into Sampling-Based AO Planners

AO-RRT is reformulated to work in an augmented state-cost space while integrating the terminal cost only upon reaching the goal, preserving the decomposability necessary for sampling-based search. The method operates by iteratively tightening cost bounds and focusing exploration, with the augmented Euclidean metric ensuring correct nearest-neighbor queries.

Extension to Belief Space

Belief propagation is modeled for both Euclidean and Lie group state spaces. For common systems, belief means and covariances are propagated under the learned stochastic dynamics, using either Kalman-style updates (for linear systems) or tangent-space propagation for rigid-body systems. The Wasserstein metric facilitates propagation of uncertainty and the definition of running and terminal costs in belief space.

Learning Belief Dynamics and Uncertainty

For systems lacking analytical uncertainty models (contact-rich domains), the belief dynamics—including transition mean and process covariance—are learned directly from data via a neural network trained with negative log-likelihood loss. This enables the planner to operate in settings where only empirical data is available, integrating learned aleatoric uncertainty into belief propagation.

Empirical Evaluation

Three benchmark tasks evaluate the framework:

• Flappy Bird: In deterministic planning, the terminal cost causes the planner to generate trajectories with better goal state centering, at a minor increase in path length, highlighting the ability to encode goal preferences.
• Car Parking (SE(2)): Both deterministic and belief-space variants are tested. KiTe achieves a significantly higher rate of parking in the preferred region and better empirical goal-reaching success under uncertainty. Notably, increasing the terminal cost weight improves goal-reaching probability but must be balanced against search efficiency.
• Planar Pushing: In both simulation and real-world experiments with YCB objects, KiTe outperforms prior approaches including active planning driven by model uncertainty and Gaussian Belief Trees. It achieves the highest success rate and the lowest terminal Wasserstein cost, demonstrating the efficacy of integrating learned uncertainty with belief-space terminal optimization.

Implications and Future Directions

The explicit inclusion of a terminal-cost objective in sampling-based kinodynamic planners addresses a significant shortcoming of prior methods, permitting the direct optimization of terminal state reliability and goal preference. The belief-space extension, with a learned uncertainty model, robustly accommodates stochastic and analytically intractable dynamics. The lower bound linking Wasserstein distance to goal-reaching probability provides a rigorous and actionable design tool for shaping planner objectives.

Practically, this formulation enables:

• Specification and optimization of task-relevant goal-state metrics (e.g., preferred docking locations, reduced final uncertainty) alongside feasibility.
• Integration of data-driven uncertainty models in real-world robotics, addressing environments where process uncertainty is empirical or complex.
• Trade-off tuning in multi-objective robotic tasks, balancing path efficiency and goal outcome reliability as a function of task specification and resource constraints.

The authors identify several directions to extend this work: (1) closed-loop execution and on-line observation updates for robust real-world performance, (2) multi-objective formulations for automated cost-weight selection, (3) learning richer, possibly correlated or hybrid (residual-physics) uncertainty models, and (4) empirical scalability to complex, high-dimensional dynamical systems.

Conclusion

This work presents a theoretically rigorous and empirically validated framework for kinodynamic motion planning that incorporates a terminal cost and learned belief dynamics. By combining asymptotically optimal sampling-based planning, Wasserstein-metric-based belief space reasoning, and neural uncertainty learning, KiTe enables explicit optimization of terminal state quality under uncertainty and demonstrates enhanced goal-reaching reliability in both simulation and physical robotic tasks (2605.09046). This work substantiates a shift toward explicitly multi-objective, uncertainty-aware motion planning in robotics and autonomous systems, with robust theoretical foundations and practical impact.