💻 computer science

Multi-Objective Kinodynamic Motion Planning with Asymptotic Pareto Optimality

This paper proposes a unified algorithmic framework based on the Stable Sparse-RRT (SST) that extends multi-objective motion planning to systems with kinodynamic constraints by replacing single representative nodes with locally Pareto-optimal sets, thereby providing theoretically guaranteed solutions for lexicographic, constrained, and Pareto-front optimization problems.

Original authors: Yusif Razzaq, Anne Theurkauf, Nisar Ahmed, Morteza Lahijanian

Published 2026-07-20

📖 7 min read🧠 Deep dive

CC BY 4.0

Original authors: Yusif Razzaq, Anne Theurkauf, Nisar Ahmed, Morteza Lahijanian

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

Imagine you are programming a robot to navigate a maze. In the old days, engineers gave the robot a single goal: "Get to the exit as fast as possible." The robot would calculate the shortest path, ignoring everything else. But real life is messy. A self-driving car doesn't just want to be fast; it also wants to be safe, comfortable, and energy-efficient. A delivery drone might need to balance speed against battery life and the risk of hitting a bird. When a robot has to juggle multiple, often conflicting goals, it can't just pick one "best" path. Instead, it has to find a whole menu of "best compromises." This is the world of multi-objective motion planning.

To understand the challenge, think of a robot's path as a line drawn on a map. The robot has rules it must follow, like not driving through walls (obstacles) and obeying the laws of physics (it can't turn on a dime if it's moving too fast). These rules are called "kinodynamic constraints." When you add multiple goals—like "minimize time" and "maximize safety"—you aren't looking for a single winner anymore. You are looking for a "Pareto front," which is a fancy way of saying a collection of paths where you can't improve one goal without making the other worse. It's like a menu where every dish is a perfect balance of spicy and sweet; you can't make it spicier without losing some sweetness.

This paper tackles the problem of how to help robots find these perfect balances when they are moving in the real, continuous world, not just on a grid. The authors, Yusif Razzaq and his team from the University of Colorado Boulder, argue that the old tricks used to solve these problems don't work well for robots with complex physics. They propose a new, unified way to help robots explore all the possible "best compromises" at once, rather than guessing and checking.

The Problem with "Mixing" Goals

For a long time, when engineers faced a robot with two goals (like speed and safety), they used a trick called "scalarization." Imagine you have a bag of apples (speed) and oranges (safety). To decide which bag is better, you might say, "One orange is worth two apples," and then just count the total number of "fruit points." This turns two goals into one. The robot then just tries to get the highest score.

The authors of this paper show that this "mixing" trick has a fatal flaw. They prove mathematically that you cannot simply mix costs together to solve certain types of problems, especially when the goals have a strict order of importance. For example, if a robot must first avoid crashing (safety) and then be fast, no amount of "fruit point" math can guarantee it will prioritize safety correctly. If you try to mix them, the robot might take a slightly faster route that is dangerously close to a wall, because the math says the "points" are higher. The paper explicitly rules out the idea that simple weighted sums (mixing goals) can solve these problems with the same reliability as their new method.

The New Approach: A Team of Explorers

The authors' solution is built on an existing algorithm called SST (Stable Sparse-RRT), which is like a robot that throws darts at a map to find a path. Usually, SST keeps only one "best" path in each small area of the map. If a new path is slightly better, it replaces the old one.

The authors realized that for multiple goals, keeping just one path is like trying to find the best compromise by only looking at one dish on the menu. Instead, they changed the algorithm to keep a team of paths in each area. In their new framework, every time the robot explores a neighborhood, it doesn't just pick the single winner; it keeps a small group of "locally Pareto-optimal" paths. These are paths that are so good that you can't improve one without hurting another.

This single change allows them to build three different specialized robots, all based on the same core idea:

LEXSST (The Strict Boss): This robot handles situations where goals have a strict priority list (e.g., "Safety first, speed second"). The authors found that you can't just use a math formula to enforce this order in a continuous world. So, LEXSST uses a clever "fuzzy" rule. It finds the safest paths, but allows them to be almost as safe as the absolute best (within a tiny, user-defined tolerance). Then, among those "almost perfect" safe paths, it picks the fastest one. This ensures the robot respects the priority order without getting stuck trying to find a mathematically impossible "perfect" tie.
COSST (The Rule Follower): This robot handles situations where you have hard limits (e.g., "Speed must be under 50 mph, but minimize fuel"). The paper shows that the old SST method often fails here because it might pick a path that is fast but barely breaks the speed limit, leaving no room to maneuver around a sudden obstacle. COSST keeps all paths that stay within the rules, ensuring the robot doesn't accidentally get trapped in a dead end just because it was too focused on being fast.
POSST (The Menu Maker): This is the most ambitious robot. Its job is to find the entire menu of best compromises. Instead of picking one winner, it maps out the whole "Pareto front." It shows the robot (and the human designer) every possible trade-off: "Here is a path that is very fast but risky, here is one that is very safe but slow, and here are all the perfect balances in between."

What They Found

The team tested these new algorithms in various simulated environments, from simple open fields to cluttered mazes with narrow passages. They compared their methods against the old "mixing" techniques (scalarization).

The results were clear. In the "Strict Boss" scenario, the old methods produced paths that were either too risky or too slow, depending on how the engineers tuned the math. LEXSST consistently found the paths that perfectly respected the priority order. In the "Rule Follower" scenario, the old method failed to find a solution in 93% of the runs in a tricky narrow-passage test, while COSST succeeded 100% of the time. This happened because the old method was too greedy, picking a path that looked good initially but couldn't finish the job, whereas COSST kept enough options open to find a way through.

Perhaps most impressively, when it came to mapping the entire menu of trade-offs (POSST), the new method was vastly more efficient. To get a similar variety of solutions using the old "mixing" method, the computer had to run the planning algorithm 101 times with different settings. POSST found a better, more diverse set of solutions in a single run.

The Bottom Line

This paper doesn't just suggest a tweak; it provides a new way of thinking about how robots make decisions when they have multiple, competing goals. By proving that simple math mixing fails for certain problems and by introducing a method that keeps a "team" of good options rather than a single "winner," the authors have created a toolkit that is more reliable and efficient.

Their work is backed by mathematical proofs that guarantee the robots will find solutions if they exist (completeness) and that the solutions will be very close to the best possible ones (near-optimality). While the paper notes that some challenges remain—like how to handle more than two goals in the "Strict Boss" scenario—their new algorithms, LEXSST, COSST, and POSST, offer a robust foundation for the next generation of intelligent, multi-goal robots. They show that sometimes, to find the best path, you have to stop looking for a single winner and start appreciating the whole team.

Technical Summary: Multi-Objective Kinodynamic Motion Planning with Asymptotic Pareto Optimality

Problem Formulation
This paper addresses the challenge of multi-objective motion planning for robotic systems operating under kinodynamic constraints (continuous state spaces with complex dynamics). The authors identify three distinct problem classes that arise when multiple competing cost objectives are present:

Lexicographic Optimization: Minimizing objectives according to a strict priority ordering (e.g., minimize $c_1$ , then $c_2$ among solutions with optimal $c_1$ ).
Constrained Optimization: Minimizing a primary objective subject to upper-bound constraints on remaining costs.
Pareto Front Optimization: Approximating the full set of optimal trade-offs (the Pareto front) where no single trajectory optimizes all objectives simultaneously.

The paper argues that existing approaches, which typically rely on cost scalarization (combining multiple costs into a single weighted sum or maximum), are fundamentally insufficient for these problems in continuous domains. Specifically, scalarization fails to recover non-convex regions of the Pareto front, cannot guarantee the recovery of lexicographic optima due to the lack of a utility function representing lexicographic dominance over continuous spaces, and often requires multiple independent planning runs to approximate a diverse set of trade-offs.

Methodology
The proposed solution is a unified algorithmic framework built upon the Stable Sparse-RRT (SST) algorithm. The core innovation is replacing the single representative node maintained in each witness neighborhood (as done in standard SST) with a representative set of locally Pareto-optimal nodes. This structure allows the algorithm to simultaneously track and refine non-dominated subtrajectories within a single search tree.

Based on this framework, the authors propose three specific algorithms:

LEXSST (Lexicographic SST): Designed for lexicographic minimization. It introduces the concept of $\epsilon$ -equivalence to handle the continuous nature of costs, treating costs within a user-defined tolerance $\epsilon$ as equivalent. This allows the algorithm to refine secondary objectives without losing the primary optimum. The algorithm maintains a set of candidates within an $\epsilon$ -tolerance of the best-known primary cost.
COSST (Constrained SST): Designed for constrained optimization. It maintains a representative set of nodes that satisfy user-defined cost constraints ( $\bar{c}_i$ ). Unlike standard SST, which greedily prunes based on a single cost, COSST retains multiple constraint-admissible subtrajectories per witness to ensure completeness, preventing the premature pruning of paths that might satisfy constraints later in the trajectory.
POSST (Pareto SST): Designed for approximating the full Pareto front. It retains all non-dominated candidates in each neighborhood (subject to a sparsity parameter $\vec{\epsilon}$ in the objective space) without imposing priority or constraints, thereby exploring the full extent of trade-offs.

Theoretical Guarantees
The paper provides rigorous theoretical analysis establishing that these algorithms are:

Probabilistically $\delta$ -robustly complete: They will find a solution (or the full Pareto set) with probability 1 if a $\delta$ -robust solution exists.
Asymptotically near-optimal: As the planning time approaches infinity and sparsity parameters ( $\delta$ , $\vec{\epsilon}$ ) approach zero, the solutions converge to the true optimal or Pareto-optimal sets with bounded sub-optimality.

The authors prove that no scalar utility function can correctly represent lexicographic dominance over continuous cost spaces, establishing a fundamental limitation of scalarization-based approaches for Problem 1. They further demonstrate that vanilla SST is structurally incomplete for constrained optimization (Problem 2) because it cannot maintain the diversity of paths required to satisfy downstream constraints.

Experimental Results
The algorithms were evaluated across eight case studies using a 2D double integrator and a 4D bicycle model in various workspaces (simple, multi-homotopy, and cluttered). Key findings include:

Lexicographic Performance: LEXSST successfully refines secondary objectives while respecting primary priorities, whereas standard SST (and weighted-sum SST) produces wide variances in secondary costs or fails to find the true lexicographic optimum regardless of weight tuning.
Constrained Completeness: In narrow-passage scenarios, standard SST failed to find feasible solutions in 93% of runs because it greedily optimized the primary cost and pruned necessary paths. COSST achieved a 100% success rate by maintaining multiple constraint-admissible branches.
Pareto Front Coverage: POSST successfully recovered non-convex Pareto fronts that weighted-sum scalarization (WS-SST) missed entirely. In one experiment, a single 30-second run of POSST achieved better coverage of the Pareto front than 101 separate WS-SST runs (totaling 3030 seconds of computation).
Sparsity Trade-offs: The experiments demonstrated that the objective-space sparsity parameter $\vec{\epsilon}$ effectively controls the trade-off between tree size (computational efficiency) and the tightness of the Pareto front approximation.

Significance
The paper claims to provide the first probabilistically complete and asymptotically near-optimal motion planners for lexicographic and constrained kinodynamic planning, as well as the first for full Pareto-front approximation in continuous domains. By moving away from scalarization, the proposed framework enables efficient exploration of the multi-objective landscape, offering formal guarantees of correctness and completeness that were previously unattainable for these problem classes. The work establishes that maintaining a set of locally Pareto-optimal nodes, rather than a single representative, is essential for solving multi-objective kinodynamic planning problems with rigorous guarantees.

The Problem with "Mixing" Goals

The New Approach: A Team of Explorers

What They Found

The Bottom Line

More like this