If you’re into robotics, AI, or just curious about how robots navigate without bumping into each other, this post on MAPF is for you. Recently, I wrapped up a project that’s been on my mind for a while: a prioritized planner for finding collision-free paths for multiple car-like robots, which, of course, cannot drive sideways.
In this post, I’ll walk you through the why and how of this project. We’ll start with the basics of Multi-Agent Path Finding (MAPF), dive into the extra headaches that come with nonholonomic robots (think cars that can’t slide sideways), and then get into my approach using a prioritized Hybrid A* algorithm. I’ll share some engineering nitty-gritty from the code, like how I handled time reservations and collision checks. By the end, you’ll see why this setup prioritizes speed and scalability over perfect optimality. Let’s jump in.
If you are interested in learning more about robotics as your future career, you might also find other postings below interesting and helpful.
Table of Contents
The World of Multi-Agent Path Finding: Why It’s a Big Deal
Picture this: You’ve got a warehouse full of autonomous robots zipping around, picking up packages, or maybe a fleet of self-driving cars navigating a busy intersection. The core problem here is Multi-Agent Path Finding—figuring out paths for multiple agents from start to goal without them colliding. It’s a staple in robotics and AI, with applications in everything from video games to logistics.
MAPF algorithms generally fall into a few categories:
- MAPF algorithms generally fall into a few categories:
- Optimal Solvers: These guarantee the shortest or most efficient paths for everyone. Think CBS (Conflict-Based Search) or its variants. They’re great for small groups but can explode in computation time as agents increase.
- Suboptimal but Scalable: Prioritized planning, where you plan paths one agent at a time and treat earlier paths as obstacles. It’s faster and works well for larger teams, even if it’s not always optimal.
- Decoupled vs. Coupled: Decoupled methods plan independently and resolve conflicts later; coupled ones consider everyone at once, which is more thorough but heavier.
The big challenges? Avoiding deadlocks (where robots block each other forever), handling dynamic environments, and scaling to dozens or hundreds of agents. In real life, things get messier with obstacles, time constraints, and robot physics.
Two Fundamental Works of My Choice
Of so many existing works on MAPF by many great researchers, I find these two works introduce the most important fundamental ideas in MAPF:
- Conflict-Based Search (CBS)
- Cooperative A*
Picture this: You’ve got a warehouse full of autonomous robots zipping around, picking up packages, or maybe a fleet of self-driving cars navigating a busy intersection. The core problem here is Multi-Agent Path Finding—figuring out paths for multiple agents from start to goal without them colliding. It’s a staple in robotics and AI, with applications in everything from video games to logistics.
MAPF problems come in a few flavors. There’s the optimal variety, where you want the shortest total paths or minimal makespan (time for all agents to finish). Algorithms like Conflict-Based Search (CBS) [Sharon et al., 2015] shine here, but they can be computationally heavy for large groups. Then there are suboptimal or bounded-suboptimal methods that trade perfection for speed, like Enhanced CBS or even greedy approaches.
Another way to classify them is by planning strategy: coupled vs. decoupled. Coupled planners treat all agents together, solving one big joint problem—great for optimality but scales poorly. Decoupled ones plan for each agent separately, often prioritizing them to resolve conflicts. That’s where my project fits in: it’s a prioritized, decoupled method inspired by classics like Cooperative A* [Silver, 2005], where agents plan one by one, reserving space-time slots to avoid collisions.
The big challenge in MAPF? Conflicts. Agents might want the same spot at the same time, leading to deadlocks or endless replanning. In grid-based worlds with holonomic agents (ones that can move in any direction, like drones or omnidirectional robots), it’s tricky but manageable. But throw in nonholonomic constraints—like cars that have to turn realistically—and things get spicy.
Nonholonomic Robots: Why They’re Trickier Than You Think
Most MAPF research assumes holonomic agents. These guys can pivot on a dime, slide sideways, or rotate in place. Planning for them often boils down to A* on a grid, with heuristics like Manhattan distance working like a charm.
Nonholonomic robots, on the other hand, are more like real cars or bikes. They have constraints from their kinematics—limited steering angles, minimum turning radii, and the inability to move laterally. This means their paths aren’t just straight lines; they involve curves, reverses, and careful maneuvering. Standard A* doesn’t cut it because it ignores these dynamics.
Why is this harder? For one, the state space explodes. You need to track not just position (x, y) but also orientation (yaw) and even velocity in some cases. Collision checks become more complex too—it’s not just overlapping grids; you have to verify if rotated rectangles (representing the robot’s footprint) intersect over time.
In my experience, adapting MAPF for nonholonomics often leads to hybrid approaches. You might use Reeds-Shepp curves [Reeds and Shepp, 1990] for smooth paths or Dubins paths for forward-only motion. These analytic solvers generate feasible curves quickly, but integrating them into a multi-agent setup requires handling dynamic obstacles (other robots) and time dimensions.
Scalability is another pain point. Optimal solvers for nonholonomics, like those extending CBS with continuous-time checks, can handle maybe a dozen agents before choking. That’s why I went prioritized: plan high-priority agents first, let lower ones adapt. It’s not always optimal—lower-priority bots might take detours—but it’s fast and works for denser scenarios.
My Approach: Prioritized Hybrid A* for car-like robots
I wanted something practical: Fast enough for real-time use, scalable to 10+ robots, and handling nonholonomic kinematics. Enter Prioritized Hybrid A* (PHA*), a mashup of prioritized planning and Hybrid A*. Github
High-level flow
- Prioritize Agents: Plan paths sequentially based on some order. For each agent, treat previous agents’ paths as dynamic obstacles. Just like Cooperative A*, an agent with higher priority is considered as a dynamic obstacle.
- Hybrid Search: Use a modified A* that discretizes space (x, y, yaw, time) but incorporates continuous primitives like straight drives, turns, reverses, and waits. This will be one of the two heuristics of the Hybrid A* planner.
- TimeTable for Reservations: Inspired by Cooperative A*, I introduce a TimeTable to reserve space-time volumes. It interpolates poses over time for collision checks. There are other works that uses the same concept with different name, so nothing surprising.
- Analytic Expansion to get to the exact goal: Near the goal, switch to Reeds-Shepp curves for quick, smooth connections if they’re collision-free. Reeds-Shepp is also the second heuristic of Hybrid A* planner.
The conventional Hybrid A* utilizes two path planning results as heuristics: A* with constrained primitives and Reeds-Sheep curves. A good introduction on Hybrid A* can be found here:
What makes mine different? Most nonholonomic MAPF is optimal but slow (e.g., using SMT solvers). Mine sacrifices optimality for speed; prioritized planning finds solutions in seconds for small maps, even with curves. It’s not optimal (might miss paths due to priority order), but generally works good enough for most cases.
The pipeline is very straightforward. First, assign priorities to each agent. It could be random, or it could be determined by task allocations. If it doesn’t matter, we could even shuffle and find the best.
Planning is very quick for not-so-dense environment. For the demo above (gif), where each robot has two goals to visit in order, each path planning took roughly 0.003 second, 0.012 seconds in total. This is fast enough for online planning use.
But, is it really ready for real applications where online planning is crucial? Hard to say that. The reason is that the planning time tends to grow a lot when i) the environment is dense that the planner faces many collisions to avoid, and ii) many robots are in motion that it make it harder to find collision-free path.
Limitations
While it works in many cases, it still poses some limitations. First, it is not optimal solver like CBS, meaning, the total travel length of the agents is probably not the shortest possible. This is because the Hybrid A*, unlike conventional A*, is not an optimal solver, and prioritized method like cooperative A* is generally not optimal. Inherently, my method is also not optimal.
Well, to be frank, humans are not optimal in traveling in multi-agent settings. At an airport, for example, people just want to go to their gates without an accident rather than focusing on taking the shortest path possible. Even when people drive, they generally don’t care as long as they are accident-free and not taking crazy inefficient paths.
The real problem to solve is that there is no guarantee that this is deadlock free, meaning there is a chance that the planner end up not finding a solution because robots get stuck by each other and obstacles. It happens less in free spaces where there are no/little obstacle, but it happens. There are research works tackling the deadlock problems, so I’ll have to study the topic to figure out what to do with my method.