🔢 mathematics

Optimized and kinematically feasible multi-agent motion planning

This paper proposes a two-step framework for optimized and kinematically feasible multi-agent motion planning that combines an initial feasible solution from algorithms like Conflict-Based Search with a subsequent multi-phase optimal control improvement step, demonstrating its effectiveness on tractor-trailer systems where CBS outperforms PBS and lattice-based planners surpass safe interval path planning.

Original authors: Anja Hellander, Kristoffer Bergman, Daniel Axehill

Published 2026-05-05

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Anja Hellander, Kristoffer Bergman, Daniel Axehill

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 the traffic controller for a busy parking lot filled with giant, articulated trucks (like a tractor pulling a long trailer). Your job is to tell each truck exactly how to move from its starting spot to its destination without crashing into the walls or into each other.

This is a hard problem because these trucks don't move like simple dots on a grid; they have complex physics. They can't stop instantly, they can't turn on a dime, and if the trailer hits a wall, the whole truck is stuck.

The authors of this paper propose a two-step "Plan and Polish" strategy to solve this problem efficiently.

Step 1: The Rough Draft (The "Sketch")

First, the computer needs a quick, safe plan. It can't solve the perfect physics equation immediately because that takes too long. Instead, it uses a "discretized" approach.

Think of this like a board game. Instead of allowing the trucks to move smoothly in any direction, the computer forces them to move only along specific, pre-calculated "moves" (like a knight in chess).

The Tool: They use a "Lattice-based planner." Imagine a grid of invisible stepping stones. The computer finds a path by hopping from stone to stone.
The Conflict: When multiple trucks are on the board, they might try to step on the same stone at the same time. To fix this, the paper compares two methods for deciding who goes first:
- CBS (Conflict-Based Search): Like a referee who watches the game, spots a collision, and says, "You two can't be here at the same time; one of you must wait or take a different path." It keeps doing this until everyone is safe.
- PBS (Priority-Based Search): Like a line at a coffee shop. The computer picks a priority order (Truck A goes first, then Truck B). The later trucks treat the earlier ones as moving obstacles and plan around them.

The Surprise Finding:
The authors expected a more complex algorithm called SIPP-IP (which handles time in "safe intervals") to be the best. However, for these big trucks, the simple Lattice-based planner actually worked better.

Why? SIPP-IP is overly cautious. It's like a security guard who says, "If any part of your truck might touch the wall, you can't go." The Lattice planner is slightly more relaxed, checking if the truck actually overlaps with the wall, allowing for smoother, faster paths.

Step 2: The Polish (The "Smoothie")

The "Rough Draft" from Step 1 is safe, but it looks jerky. It's like a robot moving in a series of sharp, 90-degree turns because it was forced to hop on grid stones.

Now, the computer takes that rough path and runs it through a mathematical optimizer (an Optimal Control Problem solver).

The Analogy: Imagine you have a rough sketch of a road drawn with a jagged crayon. Step 2 takes that sketch and uses a high-tech smoothing tool to turn it into a perfect, flowing highway.
The Trick: The computer uses the rough sketch as a "warm start." It doesn't start from scratch; it just tweaks the existing path to make it smoother, faster, and more fuel-efficient while ensuring the trucks still obey the laws of physics.

The "Time-Sync" Secret Sauce

To make Step 1 work well, the authors had to invent a new way to create those "stepping stones" (motion primitives).

Normally, one move might take 1.2 seconds and another 1.7 seconds. This makes it hard to check if two trucks will crash.
The authors forced all moves to be time-synchronized. Every move is a multiple of a tiny, fixed time slice (like 0.1 seconds).
Analogy: Imagine a marching band. Instead of everyone marching at their own speed, everyone steps exactly on the beat. This makes it incredibly easy to see if two band members are about to bump into each other.

What They Found

They tested this on a computer simulation with 2 to 5 tractor-trailer systems in a 200x200 meter area.

The Planner: The simple "Lattice" planner was faster and found more successful paths than the complex "SIPP-IP" method, especially when obstacles were present.
The Conflict Solver:
- In an empty room, the "Priority" method (PBS) solved more problems than the "Referee" method (CBS).
- In a room full of obstacles, the "Referee" method (CBS) was faster and more successful.
The Result: After the "Polish" step, both methods produced paths of very similar quality. The rough draft didn't matter as much as the final smoothing step.

Summary

The paper presents a system that first finds a safe, rough path using a grid-based game approach (which works better than expected for big trucks) and then smooths it out using advanced math. It's like hiring a quick sketch artist to draw a route, and then hiring a master sculptor to refine that sketch into a perfect, collision-free trajectory.

Technical Summary: Optimized and Kinematically Feasible Multi-Agent Motion Planning

Problem Formulation

The paper addresses the Multi-Agent Motion Planning (MAMP) problem for autonomous systems with complex kinematics, specifically focusing on tractor-trailer systems. Unlike standard Multi-Agent Pathfinding (MAPF) which often assumes point agents on discrete grids with simplified dynamics, this work considers agents as nonlinear dynamical systems described by $\dot{x}_i(t) = f_i(x_i(t), u_i(t))$ . The objective is to find a set of feasible, collision-free, and optimized trajectories for $k$ agents moving from initial states $x_{i,0}$ to final states $x_{i,f}$ within a workspace $W$ containing obstacles $O$ .

The optimization problem seeks to minimize the sum of individual cost functions $J_i = \int_{t_0}^{t_f} l(x_i(t), u_i(t)) dt$ , subject to:

System dynamics constraints.
State constraints (avoiding obstacles and workspace boundaries).
Control constraints.
Inter-agent collision avoidance ( $R_i(x_i) \cap R_j(x_j) = \emptyset$ ).

The authors note that finding a globally optimal solution is NP-complete due to the combinatorial nature of route selection and the nonlinearity of the system, compounded by the priority conflicts between agents.

Methodology

The proposed framework is a two-step approach designed to balance computational tractability with solution quality and kinematic feasibility.

Step 1: Initial Feasible Solution

The first step generates an initial feasible, collision-free solution using discrete search methods.

Multi-Agent Search: The framework utilizes either Conflict-Based Search (CBS) or Priority-Based Search (PBS). These methods decompose the MAMP problem into single-agent planning problems, resolving conflicts by adding constraints (CBS) or exploring priority orders (PBS).
Single-Agent Planner: The authors evaluate two single-agent planners:
1. Extended SIPP-IP (Safe Interval Path Planning with Interval Projection): Modified to handle general cost functions and larger agents (multiple grid cells). It uses time intervals to represent safe states.
2. Lattice-Based Planner: Uses a graph search (A*) on a discretized state space augmented with time.
Motion Primitives: A critical component is the generation of time-synchronized motion primitives. The authors propose a method to generate optimal kinematically feasible primitives where the duration of every primitive is a multiple of a fixed sample time $t_s$ . This synchronization is required for efficient collision checking in SIPP-IP and lattice planners.

Step 2: Continuous Improvement

The second step refines the discrete solution using numerical optimal control.

Multi-Phase OCP: The initial discrete solution is used to "warm-start" a numerical solver for a Multi-Phase Optimal Control Problem (OCP).
Phase Structure: To handle agents with different arrival times ( $t_{i,f}$ ), the problem is divided into $M=k$ phases. Phase $m$ covers the time interval between the goal achievement of agent $m-1$ and agent $m$ . This structure allows for a consistent time discretization within each phase for collision checking while permitting different total durations for different agents.
Optimization: The solver optimizes the continuous trajectories and control inputs, removing discretization artifacts and ensuring strict kinematic feasibility.

Key Contributions

Two-Step MAMP Framework: A hybrid approach combining discrete search (CBS/PBS) for global feasibility and continuous optimal control (OCP) for local optimization and kinematic smoothing.
Time-Synchronized Primitive Generation: A novel algorithm (Algorithm 1) to generate optimal motion primitives with durations constrained to be integer multiples of a common sample time, facilitating synchronized collision checking.
Extensions to SIPP-IP: Modifications to the SIPP-IP algorithm to support general cost functions and agents with complex geometries (occupying multiple grid cells), rather than just single points.
Multi-Phase OCP Formulation: A specific formulation of the MAMP improvement step that handles heterogeneous agent arrival times by partitioning the problem into sequential time phases.

Experimental Results

The framework was evaluated on tractor-trailer systems (2-trailer configurations) in both obstacle-free and obstacle-rich environments.

Single-Agent Performance:
- In obstacle-free environments, the lattice-based planner was significantly faster (0.008s vs 0.12s) and produced slightly lower costs than the extended SIPP-IP.
- In environments with obstacles, the lattice-based planner remained faster and produced solutions with approximately 14% lower cost. The authors attribute SIPP-IP's higher cost and lower success rate to its "conservative" collision checking (grid-cell based), which discards feasible paths that the lattice planner (using circle-to-circle checks) accepts.
Multi-Agent Performance (CBS vs. PBS):
- Success Rate: In obstacle-free environments, PBS solved a higher number of problems than CBS. However, in environments with obstacles, CBS achieved a higher success rate and lower average runtime.
- Solution Quality: Both planners produced solutions of similar quality after the improvement step. While CBS initially found solutions with slightly lower costs (due to its optimality guarantees), the continuous improvement step often equalized or slightly reversed this advantage.
- Scalability: As the number of agents increased, the number of successfully solved problems decreased for both planners, but CBS maintained a performance edge in complex, obstacle-filled scenarios.

Significance and Claims

The paper claims that for complex kinematic systems like tractor-trailers, a lattice-based planner combined with CBS or PBS outperforms the more theoretically robust SIPP-IP approach, primarily due to less conservative collision checking. The proposed two-step framework successfully bridges the gap between finding a feasible solution in a reasonable time and achieving a kinematically feasible, optimized trajectory.

The authors modestly note that while PBS can solve more problems in open spaces, CBS is superior in cluttered environments. They conclude that the initial discrete solution quality does not strictly dictate the final optimized cost, as the continuous improvement step can significantly alter the outcome. Future work is suggested to focus on improving the computational efficiency of the improvement step via distributed optimization and incorporating traffic rules.