💻 computer science

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

This paper introduces KiTe, a kinodynamic planner that optimizes terminal-state quality via a learned terminal cost and Wasserstein distance in belief space to improve goal-reaching reliability under uncertainty, while maintaining asymptotic optimality and supporting data-driven uncertainty models.

Original authors: Zhuoyun Zhong, Seyedali Golestaneh, Constantinos Chamzas

Published 2026-05-15

📖 6 min read🧠 Deep dive

CC BY 4.0

Original authors: Zhuoyun Zhong, Seyedali Golestaneh, Constantinos Chamzas

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

The Big Picture: It's Not Just About Getting There; It's About How You Arrive

Imagine you are a robot trying to navigate a complex world. Most traditional robot planners are like a GPS that only cares about one thing: getting you from Point A to Point B as quickly and cheaply as possible.

If a GPS sees two routes to your destination, it picks the one with the shortest distance or least fuel. It doesn't care if you arrive at a dead-end alley (where you can't leave) or if you arrive at a busy intersection where you might get hit by a car. It just checks, "Did you get there? Yes? Good job."

This paper introduces a new planning method called KiTe (Kinodynamic planning with a Terminal cost). KiTe changes the rulebook. It doesn't just ask, "Did you get there?" It asks, "How well did you get there?"

The Core Problem: The "Good Enough" Trap

The authors argue that in the real world, simply reaching a goal isn't enough. You need to reach it in a state that is safe and ready for the next step.

The "Flappy Bird" Analogy:
Imagine playing Flappy Bird.

Old Planners: They find a path that gets the bird through the gap between the pipes. If the bird flies through the very top corner of the gap, it counts as a success. But if it hits the top pipe on the way out, it crashes.
KiTe: KiTe knows that flying through the center of the gap is better. Even if the center path is slightly longer or uses a bit more energy, KiTe chooses it because arriving in the center gives the bird a much better chance of surviving the next gap.

The "Parking" Analogy:
Imagine you are parking a car.

Old Planners: They might park you in the spot closest to the exit, even if it's a tight squeeze where you can't open your door easily.
KiTe: KiTe might choose a spot slightly further away that is wide open. It accepts a slightly longer drive to get there because it ensures you can actually get out of the car without hitting the neighbor's vehicle.

The Secret Sauce: "Belief" and Uncertainty

Robots aren't perfect. They slip, their sensors are fuzzy, and the world is unpredictable. This paper treats the robot's location not as a single dot on a map, but as a cloud of uncertainty (a "belief").

Think of this cloud like a foggy flashlight beam.

A narrow beam means the robot is very sure where it is.
A wide, fuzzy beam means the robot is confused and could be anywhere in that area.

Most planners try to keep the robot moving while the beam stays somewhat narrow. But KiTe adds a special rule: It wants the beam to be tight and centered exactly on the target when the robot stops.

If a path leads to the goal but leaves the robot with a huge, fuzzy cloud of uncertainty (meaning the robot might actually be 5 feet to the left of where it thinks it is), KiTe rejects that path. It will choose a longer, more careful path that ensures the robot arrives with a sharp, focused beam right in the middle of the target.

The "Terminal Cost": The Final Grade

The paper introduces a mathematical concept called a Terminal Cost.

Running Cost: The "grade" you get for the journey (how much energy you used, how long you took).
Terminal Cost: The "grade" you get for the final moment (how close you are to the center, how sure you are of your position).

KiTe combines these. It's like a teacher who grades a student not just on how fast they ran the race, but also on how neatly they crossed the finish line. If you run fast but trip at the finish, you get a lower grade. KiTe optimizes for the perfect finish.

Learning from Mistakes (The "Black Box" Problem)

Real robots often don't have perfect math models of how they move. Pushing a heavy box might make it slide differently depending on the floor texture, which is hard to write down in a formula.

To solve this, KiTe uses a Neural Network (a type of AI) to learn the robot's movement directly from data.

The Analogy: Imagine teaching a child to push a toy truck. You don't give them a physics textbook. Instead, you let them push it 1,000 times. You watch where it goes and how much it wobbles.
The Innovation: KiTe doesn't just learn where the truck goes; it learns how much the truck wobbles (the uncertainty). It builds a "mental model" that says, "If I push hard here, the truck usually goes there, but sometimes it slips a little." It uses this learned model to plan paths that avoid the slippery, unpredictable moves.

The Results: Does It Work?

The authors tested KiTe in three scenarios:

Flappy Bird: The robot learned to fly through the center of gaps, not just the edges, leading to fewer crashes.
Car Parking: The robot chose the "easier" parking spot (the one with more room to maneuver) even if it was slightly further away, resulting in a higher success rate.
Pushing Objects: In a real-world test with a robot arm pushing boxes and trucks, KiTe successfully pushed objects into target zones more often than other methods. It did this by choosing paths that minimized the "wobble" (uncertainty) at the end.

Summary

KiTe is a smarter way for robots to plan their moves. Instead of just rushing to the finish line, it plans a route that ensures the robot arrives safely, precisely, and ready for the next task. It does this by:

Adding a "penalty" for arriving in a messy or uncertain state.
Using AI to learn exactly how much the robot might slip or slide.
Proving mathematically that this smarter approach still finds the best possible path over time.

In short: Don't just get there. Get there right.

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

Problem Statement

Robotic systems operating in real-world environments must generate dynamically feasible motions that satisfy kinodynamic constraints while accounting for execution uncertainty arising from modeling errors, perception noise, and complex contact interactions. While existing sampling-based kinodynamic planners (e.g., AO-RRT, SST) offer probabilistic completeness and asymptotic optimality, they typically optimize only the running cost (accumulated along the trajectory, such as path length or energy) and treat goal-reaching merely as a feasibility check.

This approach presents two critical limitations:

Terminal State Quality: Many tasks depend on the quality of the final state, not just the path taken. For instance, in planar pushing, a shorter path might result in high terminal uncertainty, reducing the probability of successfully reaching the goal. In multi-goal scenarios (e.g., car parking), planners may select a geometrically closer goal that is less desirable or harder to depart from, ignoring preferences for specific terminal regions.
Uncertainty Propagation: Planning in belief space (where the state is a probability distribution) is essential for robustness. However, existing methods often rely on hard probability thresholds or trajectory-wise surrogates rather than explicitly optimizing the terminal belief state. Furthermore, complex systems (like contact-rich pushing) often lack analytical uncertainty models, necessitating data-driven approaches that are rarely integrated tightly with belief-space planners.

Methodology

The authors propose KiTe (Kinodynamic planning with a Terminal cost), a framework that augments the Asymptotically Optimal Rapidly-exploring Random Tree (AO-RRT) with an explicit terminal cost objective. The methodology operates in three main layers:

1. KiTe with Terminal Cost in State Space

The core algorithm reformulates the optimal kinodynamic planning problem in an augmented state-cost space $\mathcal{Y} = \mathcal{X} \times \mathbb{R}^+$ .

Augmented Dynamics: The state is extended from $x$ to $y = (x, c)$ , where $c$ accumulates the running cost $\ell(x, u)$ . The dynamics are extended to include the cost rate: $\dot{y} = (f(x, u), \ell(x, u))$ .
Terminal Cost Integration: Unlike previous formulations where the terminal cost is ignored during the search, KiTe evaluates the total cost $C_T = \int \ell dt + \phi(x(T))$ only upon reaching the goal region.
Search Strategy: The algorithm maintains a cost bound $c_{best}$ . When a solution is found, the bound is updated to $c_{new} + \phi(x_{new})$ . The sampling strategy then focuses on the sublevel set defined by this bound, pruning branches that cannot improve the total cost (running + terminal).
Theoretical Guarantee: The authors provide a rigorous proof that AO-RRT with a terminal cost preserves asymptotic optimality, provided the system dynamics, running cost, and terminal cost satisfy Lipschitz continuity conditions.

2. Extension to Belief Space

The formulation is extended to the belief state-cost space $\mathcal{Y}_b = \mathcal{B} \times \mathbb{R}^+$ , where $\mathcal{B}$ is the space of probability distributions over the state.

Metric: The 2-Wasserstein distance is adopted as the metric for the belief space, as it naturally captures changes in both the mean and covariance of Gaussian beliefs.
Goal-Reaching Lower Bound: A key theoretical contribution is the proof that the probability of reaching a goal region $G$ (defined as a ball of radius $r$ ) is lower-bounded by the Wasserstein distance between the terminal belief $b(T)$ and the Dirac measure at the goal $\nu_g$ :
$\text{Pr}(x(T) \in G) \geq 1 - \frac{W_2^2(b(T), \nu_g)}{r^2}$
This allows the planner to use the Wasserstein distance as a terminal cost $\psi(b(T))$ , directly optimizing for a lower bound on goal-reaching success without requiring hard, manually tuned probability thresholds.
Lipschitz Continuity: The authors prove that under mild assumptions on the stochastic dynamics (specifically regarding the Lipschitz continuity of process uncertainty and noise injection), the belief dynamics and cost functions remain Lipschitz continuous with respect to the Wasserstein metric. This ensures the asymptotic optimality of KiTe extends to the belief space.

3. Learning Belief Dynamics

To address systems where analytical uncertainty models are unavailable (e.g., contact-rich pushing), KiTe integrates a learned belief dynamics model.

Model Architecture: A neural network (MLP) is trained to predict the local stochastic transition. Given a state $x_i$ and control $u_i$ , the network outputs a mean transition $\Delta x_i$ and a process covariance parameterized by a variance vector $\sigma_i^2$ .
Training Objective: The model is trained using Negative Log-Likelihood (NLL) loss on interaction data. This allows the network to learn both the mean dynamics and the aleatoric uncertainty (process noise) directly from data.
Propagation: The learned mean and covariance are used to propagate the belief state (mean and covariance) forward in the planner, supporting both Euclidean and Lie-group (e.g., SE(2)) state spaces.

Key Contributions

Algorithmic Extension & Proof: Implementation of AO-RRT with an explicit terminal cost, accompanied by a rigorous proof that asymptotic optimality is preserved in both state and belief spaces.
Theoretical Link to Success Probability: Proof that minimizing the Wasserstein distance between the terminal belief and the goal induces a lower bound on the probability of goal-reaching success, providing a principled terminal objective for reliability.
Data-Driven Belief Propagation: Demonstration of how to learn belief dynamics models (mean and uncertainty) directly from interaction data using NLL loss and integrate them into the KiTe framework for systems lacking analytical models.
Empirical Validation: Comprehensive evaluation across three diverse tasks (Flappy Bird, Car Parking, Planar Pushing) in both simulation and the real world, comparing KiTe against state-of-the-art baselines (AO-RRT, SST, Gaussian Belief Tree).

Experimental Results

The paper validates KiTe across three tasks:

Flappy Bird (Deterministic): Demonstrates that KiTe can encode goal preferences (e.g., centering the bird in the gap) by optimizing terminal cost, whereas baselines stop refining once the goal region is reached. KiTe achieves trajectories closer to the goal center with only a modest increase in running cost.
Car Parking (SE(2) with Uncertainty): In a task with multiple feasible parking spots, KiTe successfully prioritizes a "preferred" goal region (front spot) and improves goal-reaching success rates compared to baselines that only optimize path length or use chance constraints. Belief-space planning with terminal cost yielded the highest execution success rates.
Planar Pushing (Contact-Rich, Real World): Using learned dynamics for objects like a Trash Truck and Cracker Box, KiTe outperformed baselines (including Active Planning with model uncertainty) in execution success rates. The planner selected longer geometric paths that resulted in lower terminal uncertainty and higher success probabilities, demonstrating the value of optimizing terminal belief quality.

Significance and Claims

The paper claims that KiTe addresses a fundamental gap in kinodynamic planning: the separation of trajectory optimization from terminal state quality. By explicitly optimizing the terminal state, the method:

Improves Reliability: Directly increases the probability of reaching the goal under uncertainty by minimizing the Wasserstein distance to the goal.
Encodes Preferences: Allows planners to distinguish between multiple feasible goals based on desirability (e.g., ease of departure, stability) rather than just geometric proximity.
Bridges Learning and Planning: Provides a framework where learned uncertainty models are not just used to bias sampling but are tightly integrated into the belief propagation and optimization loop.

The authors conclude that while the method introduces design parameters (terminal weights) and relies on learned models, it offers a principled, theoretically grounded approach to generating motions that are not only dynamically feasible but also reliable and preference-aware in uncertain environments.