Terminal Matters

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

Zhuoyun Zhong Seyedali Golestaneh Constantinos Chamzas

Worcester Polytechnic Institute · ELPIS Lab

Under review at IEEE Transactions on Robotics (T-RO)

Car parking with terminal cost

Terminal cost enables continuous optimization after finding a feasible solution.

Planar pushing with learned dynamics

Planning with learned dynamics and uncertainty to push an object reliably toward the goal.

Reaching the goal is not the whole objective

Two pushing trajectories trading running cost against terminal uncertainty — A shorter pushing trajectory can finish with greater uncertainty, while a longer trajectory can reach the goal more reliably.

A car choosing between two feasible parking goals with different preferences — Multiple goal regions may be feasible, but a terminal cost can encode which final state is preferable.

Sampling-based kinodynamic planners usually optimize costs accumulated along a trajectory and treat goal arrival as a feasibility check. Terminal-state quality can matter just as much: a goal may be preferable, closer to its center, or substantially more reliable under uncertainty. KiTe augments AO-RRT with an explicit terminal cost so the planner optimizes both how it moves and where its trajectory ends.

KiTe: Kinodynamic planning with a Terminal cost. The formulation preserves asymptotic optimality while supporting deterministic state space, belief space, goal preference, and goal-reaching reliability.

Add terminal quality to the planning objective

State-space objective combining running cost and terminal cost — In state space, KiTe combines trajectory running cost with a cost evaluated at the terminal state.

Belief-space objective combining running cost and terminal belief cost — In belief space, the terminal term evaluates the final state distribution rather than only its mean.

AO-RRT with a terminal cost

KiTe searches in the augmented state-cost space and updates the best cost using both accumulated running cost and terminal-state cost. The paper proves that AO-RRT remains asymptotically optimal under this augmented objective.

Belief-space extension

The same construction applies when each planning state is a belief distribution. KiTe propagates mean and covariance and preserves asymptotic optimality with respect to the belief-space objective.

A principled objective for goal-reaching reliability

Lower bound on goal-reaching probability expressed using Wasserstein distance — Reducing the 2-Wasserstein distance between the terminal belief and the goal Dirac measure improves a lower bound on the probability of reaching the goal region.

Instead of imposing only a hard terminal probability threshold, KiTe directly improves terminal reliability. The Wasserstein terminal cost jointly captures distance to the goal and state uncertainty, encouraging solutions whose final belief is concentrated inside the goal region.

Flappy Bird

Car parking

Gaussian Belief Tree baseline

KiTe

The baseline tends to select the nearest feasible parking region and stops refining after satisfying the goal constraint. KiTe selects the preferred open-space goal and continues improving the terminal belief.

Car parking trajectories and terminal belief distributions — KiTe accounts for both goal preference and terminal uncertainty in belief-space car parking.

Learning belief dynamics from interactions

Chef can data collection

Mustard bottle data collection

For contact-rich systems without analytical uncertainty models, interaction data provides the transitions needed to learn belief dynamics.

Neural belief dynamics model with mean and uncertainty prediction heads — A neural network trained with negative log-likelihood predicts both the mean local transition and process uncertainty for belief propagation during planning.

Planar pushing in simulation

Chef can

Mustard bottle

Real-world planar pushing

Cracker box trial 1

Cracker box trial 2

Trash truck trial 1

Trash truck trial 2

Across simulated and real-world objects, KiTe uses learned belief dynamics to favor trajectories with smaller terminal uncertainty and stronger goal-reaching reliability.