💻 computer science

Navigating the Clutter: Waypoint-Based Bi-Level Planning for Multi-Robot Systems

This paper proposes a hybrid multi-robot control framework that jointly optimizes task and motion planning in cluttered environments by introducing waypoints for trajectory representation and a curriculum-based RLVR training strategy to propagate motion feasibility feedback, demonstrating superior task success on the BoxNet3D-OBS benchmark compared to existing baselines.

Original authors: Jiabao Ji, Yongchao Chen, Yang Zhang, Ramana Rao Kompella, Chuchu Fan, Gaowen Liu, Shiyu Chang

Published 2026-04-24

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Jiabao Ji, Yongchao Chen, Yang Zhang, Ramana Rao Kompella, Chuchu Fan, Gaowen Liu, Shiyu Chang

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: The "Warehouse Chaos" Problem

Imagine a busy warehouse where nine robots are trying to move boxes from one side of the room to another. The room is packed with pillars, other boxes, and the robots themselves.

The problem is that robots are bad at two things simultaneously:

The Big Picture (Task Planning): Deciding who should move which box and in what order.
The Details (Motion Planning): Actually driving the robot without bumping into a pillar or another robot.

If the "Big Picture" boss tells a robot to go to a spot that is physically blocked, the robot crashes. If the "Big Picture" boss gives a good order, but the robot's "driver" is too clumsy to navigate the tight squeeze, it also crashes.

Most old systems tried to solve these problems separately. They had a boss who gave orders and a driver who just tried to follow them. If the driver crashed, the boss didn't know why, or the boss kept giving impossible orders because they didn't understand the driver's limitations.

The Solution: WAYPLAN

The authors created a new system called WAYPLAN. Think of it as a team of two smart managers who talk to each other constantly to make sure the plan is both smart and physically possible.

Here are the three main "magic tricks" they used:

1. The "Yellow Star" Map (Waypoints)

Usually, asking a robot to move is like asking a human to walk a path by describing every single muscle movement (lift foot, move foot 2 inches, place foot). This is hard for AI to learn.

Instead, WAYPLAN uses Waypoints.

The Analogy: Imagine you are giving directions to a friend in a crowded city. You don't say, "Turn left 30 degrees, step forward 0.5 meters." Instead, you say, "Go to the Yellow Star on the map, then go to the Blue Star, then the Green Star."
Why it works: The AI only needs to pick these "Stars" (key points in space) to get around obstacles. A separate, reliable, old-school computer algorithm (like a GPS) then fills in the tiny details of how to drive between the stars. This makes the AI's job much easier and less prone to errors.

2. The "Training Camp" (Curriculum Learning)

You can't just throw a new robot team into a chaotic warehouse and expect them to work perfectly immediately. They need training.

The authors used a Curriculum Strategy (like a school):

Stage 1 (School): They taught the "Boss" (Task Planner) and the "Driver" (Motion Planner) separately using textbooks (Supervised Fine-Tuning). The Boss learned how to assign tasks, and the Driver learned how to pick "Yellow Stars."
Stage 2 (Drills): They let them practice alone with rewards for doing well. The Boss got points for good orders; the Driver got points for smooth driving.
Stage 3 (Live Fire Exercise): This is the big innovation. They put them together. Now, if the Driver crashes because the Boss gave a bad order, the Boss gets a "penalty." If the Boss gives a good order but the Driver fails, the Driver gets a penalty.
- The Result: The Boss learns, "Oh, I can't send Robot A there because the Driver can't get through that gap." The Driver learns, "I need to be better at navigating these tight spots." They learn to cooperate rather than blame each other.

3. The "Feedback Loop" (Credit Assignment)

In many AI systems, if a plan fails, it's hard to know who is to blame. Was it the strategy or the execution?

WAYPLAN solves this by making the "Driver" talk back to the "Boss."

The Analogy: Imagine a general giving orders to a soldier. If the soldier gets stuck in a swamp, the general needs to know why so they don't send another soldier there next time.
In WAYPLAN, the motion planner explicitly tells the task planner: "I can't reach that spot." The task planner then uses this information to change the strategy. This removes the confusion about who made the mistake.

The Results: Small Brain, Big Success

The team tested this on a new, super-hard simulation called BoxNet3D-OBS (a 3D grid with up to 9 robots and lots of obstacles).

The Surprise: They used a relatively small AI model (4 Billion parameters) for their "Boss."
The Comparison: They compared it to massive, famous AI models (like GPT-5) that are much bigger but don't understand robot physics.
The Winner: Their small, specialized team (WAYPLAN) crushed the giant models.
- The giant models kept sending robots into walls because they didn't understand the "Yellow Star" navigation.
- The small team, because it was trained to understand the physical limits of the robots, succeeded 62% of the time, while the giants struggled to get above 15%.

Summary

This paper is about teaching robots to work together in messy, crowded rooms. Instead of trying to make one super-AI do everything (which is hard and error-prone), they split the job into a Strategist and a Navigator. They use Waypoints (simple map markers) to make navigation easier, and they train them together so the Strategist learns the Navigator's limits.

The result? A system that is smarter, safer, and more efficient than using giant, generic AI models, proving that sometimes a specialized, well-coordinated team beats a lone genius.

1. Problem Statement

The paper addresses the challenge of multi-robot control in cluttered environments. As the number of robots and obstacles increases, coordinating their actions becomes difficult due to complex physical constraints:

Kinematic Feasibility: Robots must adhere to their physical movement limits.
Collision Avoidance: Robots must avoid colliding with static obstacles and each other.
Task-Motion Coupling: A high-level task plan (e.g., "Robot A moves Object X to Target Y") may be logically sound but physically infeasible due to obstacles or robot interference. Conversely, a valid motion plan might fail if the high-level task assignment was flawed (e.g., assigning a robot to a region it cannot reach).

Existing approaches often treat task planning and motion planning separately. Task planners (often using LLMs or symbolic logic) ignore low-level motion feasibility, while motion planners (like VLA models) struggle with high-dimensional, long-horizon optimization and unstable reinforcement learning (RL) in multi-robot settings. This leads to a "mismatch" where high-level plans fail because the low-level execution cannot realize them.

2. Methodology: WAYPLAN Framework

The authors propose WAYPLAN, a hybrid control framework that jointly optimizes task and motion planning using two coupled Large Language Model (LLM) planners.

A. Bi-Level Planning Architecture

Task Planner ( $M_\theta$ ): Operates at a high level. It generates a finite-horizon plan assigning abstract actions to robots (e.g., "Robot 1 moves to position P while carrying Object O"). It does not predict continuous trajectories but rather discrete goals and states.
Motion Planner ( $M_\phi$ ): Operates at a low level. Instead of predicting dense joint configurations (as in Vision-Language-Action models), it generates waypoints (a sequence of 3D coordinates).
- Waypoint Abstraction: The motion planner outputs a sequence of intermediate 3D points.
- Execution: These waypoints are fed into a classical, reliable motion planner (e.g., Rapidly-exploring Random Trees, RRT) which converts them into continuous, kinematically feasible trajectories. This decouples the learning of where to go from the complex physics of how to get there.

B. Curriculum Training Strategy

To solve the difficulty of joint optimization, the authors employ a three-stage curriculum:

Stage 1: Supervised Fine-Tuning (SFT) Warmup:
- Task Planner: Trained on data generated by an A* search, enhanced with reflection-aware prompting. The model learns to reason about failures (e.g., "Why did this path fail? Because of a collision") rather than just imitating success.
- Motion Planner: Trained on collision-free waypoint sequences generated via Breadth-First Search (BFS) and RRT.
Stage 2: Separate RLVR (Reinforcement Learning with Verifiable Rewards):
- The planners are trained independently.
- Task Planner: Optimized for task success and efficiency, ignoring motion feasibility penalties to prevent early instability.
- Motion Planner: Optimized for generating feasible waypoints in single-robot scenarios.
Stage 3: Joint RLVR with Modified Credit Assignment:
- Joint Optimization: The task planner is optimized with a motion-feasibility penalty. If a task plan leads to infeasible motion steps (detected by the motion planner or a search algorithm), the task planner receives a negative reward.
- Feedback Propagation: The motion planner is trained on motion queries induced by the current task planner. This creates a tight coupling where the motion planner learns to solve the specific problems the task planner generates, and the task planner learns to avoid generating impossible tasks.
- Algorithm: The authors use GSPO (Group Sequence Policy Optimization) instead of standard GRPO, finding it more stable for long-horizon reasoning tasks.

3. Key Contributions

BoxNet3D-OBS Benchmark: A new, challenging multi-robot benchmark featuring dense obstacles, up to 9 robots, and complex motion constraints, extending previous BoxNet environments.
WAYPLAN Framework: A novel bi-level architecture that explicitly couples task decisions with motion feasibility using waypoint-based motion parameterization. This reduces the dimensionality of the motion learning problem compared to direct VLA control.
Curriculum Training & Modified RLVR: A training strategy that resolves the credit assignment ambiguity between task and motion levels. By propagating motion failure feedback to the task planner, the system learns to avoid infeasible high-level plans.
Waypoint vs. VLA: Demonstration that waypoint-based motion planning significantly outperforms Vision-Language-Action (VLA) models under similar data budgets, particularly in avoiding "invalid" trajectories.

4. Experimental Results

Experiments were conducted on the BoxNet3D-OBS benchmark using various LLMs (including GPT-5, Gemini, and Qwen3 up to 32B parameters).

Performance: WAYPLAN consistently outperformed motion-agnostic baselines and VLA-based approaches.
- A 4B parameter model (Qwen3-4B) trained with WAYPLAN achieved a 0.62 success rate when paired with the waypoint motion planner.
- This significantly outperformed much larger models (e.g., GPT-5, Gemini3-Pro) which lacked motion awareness, achieving success rates of only ~0.14–0.19.
Replanning: The NC-REPLAN (New-Context Replan) mode, which discards long execution history and replans from scratch upon failure, outperformed in-context replanning, likely due to better context management for smaller models.
Motion Planner Impact: The choice of motion planner drastically affected success. The waypoint planner achieved the highest success, followed by VLA, and then fixed RRT.
Error Analysis:
- With VLA motion planners, the primary failure mode was motion execution failure (invalid trajectories).
- With waypoint planners, the primary failure mode shifted to unreachable motion steps (task-level infeasibility), indicating that the motion planner became robust enough that the bottleneck moved back to high-level planning.
Efficiency: LLM-based planning with waypoint motion was significantly faster (under 60s total runtime) than traditional search-based A* methods (580s), as it avoids repeated simulation during the planning phase.

5. Significance

Bridging the Gap: The work effectively bridges the gap between high-level semantic reasoning (LLMs) and low-level physical constraints (robotics), a critical step for deploying multi-robot systems in real-world, cluttered warehouses.
Scalability: By using waypoints and classical planners for the low-level execution, the approach scales better than VLA models, which struggle with the high-dimensional output space of multi-robot joint configurations.
Model Efficiency: It demonstrates that smaller, specialized models (4B parameters) trained with the right curriculum and architecture can outperform massive general-purpose models in specific robotic control tasks.
Training Stability: The proposed curriculum and GSPO-based RLVR offer a stable pathway for training complex, coupled task-motion policies, addressing the instability often seen in RL for robotics.

In conclusion, WAYPLAN provides a robust, scalable, and efficient solution for multi-robot coordination in cluttered environments by rethinking how motion is parameterized (waypoints) and how task-motion feedback is integrated (curriculum RLVR).