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

2026-05-09

AI Summary

This work addresses the limitation of existing motion planning methods that often neglect terminal state quality and struggle to reliably achieve goals under uncertainty. The authors propose a unified motion planning framework that explicitly integrates terminal cost with cumulative trajectory cost, introducing terminal cost for the first time into asymptotically optimal kinodynamic planning (AO-RRT) while preserving its theoretical optimality guarantees. The approach is further extended to belief space by formulating goal achievement as the optimization of a lower bound on success probability, derived using the Wasserstein distance. Leveraging data-driven models of dynamics and process uncertainty, the method demonstrates significantly improved goal achievement rates in both simulation and real-world experiments across diverse tasks—including Flappy Bird, autonomous parking, and planar pushing—under uncertain conditions.

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.

Problem

• kinodynamic planning
• terminal cost
• uncertainty
• goal-reaching reliability
• belief space

Innovation

• terminal cost
• kinodynamic planning
• belief space
• learned uncertainty
• Wasserstein distance