💻 computer science

Coordinated Motion Planning for Multi-Arm Systems via Iterative LQ Games

This paper proposes an iterative Linear Quadratic (LQ) game framework that enables coordinated, collision-aware motion planning for high-degree-of-freedom multi-arm robotic systems by modeling agents as independent optimizers solving local games with differentiable collision penalties, resulting in smooth and efficient trajectories that outperform traditional methods.

Original authors: Junyoung Kim, Hanwen Ren, Lei Zhang, Ahmed H. Qureshi

Published 2026-08-31

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Junyoung Kim, Hanwen Ren, Lei Zhang, Ahmed H. Qureshi

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

In the bustling world of modern robotics, a persistent challenge lies in getting multiple machines to work together without bumping into one another. Imagine a warehouse where dozens of robotic arms must move parts from one shelf to another, or a surgical suite where several instruments operate in the same tight space. The difficulty is not just in moving a single arm from point A to point B, but in coordinating many arms simultaneously so they reach their destinations safely and efficiently. Traditional methods often struggle here. Some approaches try to control every arm from a single central brain, which becomes too slow and complex as the number of robots grows. Others let each robot plan its own path independently, but this often leads to confusion and collisions because the robots cannot anticipate each other's moves. To solve this, scientists have turned to a concept borrowed from economics and strategy: game theory. In this framework, each robot is treated as a player in a game, trying to achieve its own goal while constantly reacting to the moves of the others. The aim is to find a state of balance where no robot can improve its outcome by changing its plan alone, a state known as a Nash equilibrium.

A team of researchers at Purdue University has taken this concept and applied it to a new, difficult frontier: high-precision robotic arms with many moving joints. In their recent work, they developed a system called ILQ-Arm, designed to coordinate multiple complex manipulators in shared spaces. Unlike previous attempts that simplified robots into basic shapes or ignored the risk of an arm hitting itself, this system treats each robot as a sophisticated, articulated agent. The researchers modeled the interaction between these arms as a series of strategic games. In this setup, every arm calculates its own best path while simultaneously considering the positions and intended movements of all other arms in the workspace. The system does not rely on a fixed rule where one robot always has the right of way; instead, the robots negotiate their paths through a continuous, mathematical optimization process that happens in real time.

The core of the method involves breaking down the complex motion of the robots into small, manageable steps. The computer starts with a rough guess of how the robots might move and then refines this guess over and over. In each step, it simplifies the physics of the situation just enough to solve it quickly, then uses that solution to update the plan. This process repeats until the paths settle into a smooth, collision-free trajectory. A key innovation in this work is how the system handles safety. The researchers programmed the robots to understand not just the danger of hitting another robot, but also the danger of an arm hitting its own body or the static obstacles in the room, like walls or tables. They achieved this by adding specific penalties to the robots' decision-making process whenever a path brought them too close to a collision. These penalties are designed so that the robots naturally steer away from danger, much like a person instinctively pulling their hand back from a hot surface, but calculated with extreme precision.

When the researchers tested this system in simulations, the results were striking. They created scenarios with up to four robotic arms working in cluttered environments filled with obstacles. In these tests, the new method successfully planned safe paths for the robots in less than two seconds, even in the most crowded setups. By comparison, other established methods took significantly longer, sometimes over a minute, and often failed to find a solution at all as the number of robots increased. The paths generated by the new system were also shorter and smoother, meaning the robots wasted less energy and time. The researchers also tested the system on real physical robots, two UR5e arms placed 0.8 meters apart. In these real-world trials, the system successfully guided the robots through narrow spaces, avoiding both each other and static obstacles, with an average planning time of just 0.475 seconds per task. The robots moved in a synchronized, fluid manner, reaching their goals without any collisions.

The study also explored what happens when certain parts of the system are removed, revealing why each component is vital. When the researchers removed the penalty for an arm hitting itself, the system became much faster to compute, but the robots frequently crashed into their own bodies, proving that this specific safety check is non-negotiable for complex machines. Similarly, when they changed the way the robots were encouraged to reach their final destination, the system became less reliable and took longer to find a solution. These findings suggest that the specific combination of costs and penalties the team designed is essential for balancing speed, safety, and efficiency. The work demonstrates that by viewing multi-robot coordination as a strategic game where every player adapts to the others, it is possible to create systems that are both safe and highly efficient. This approach offers a promising path forward for deploying fleets of complex robots in dynamic, shared environments, from automated factories to future surgical suites, where the ability to move together without conflict is paramount.

Technical Summary: Coordinated Motion Planning for Multi-Arm Systems via Iterative LQ Games

Problem Definition

The paper addresses the challenge of Multi-Agent Motion Planning (MAMP) for multiple high-degree-of-freedom (DoF) robotic manipulators operating within shared workspaces. The core difficulty lies in coordinating articulated arms to achieve specific tasks while simultaneously avoiding collisions with static obstacles, other agents, and themselves (self-collision).

Existing approaches face significant trade-offs:

Centralized planners (optimizing joint state space) offer global consistency but suffer from poor scalability and real-time performance as the number of agents and system dimensionality increases.
Decentralized planners improve computational efficiency but often lack robustness and safety guarantees, relying on simplified assumptions about other agents' behaviors.
Game-theoretic methods (specifically Iterative Linear-Quadratic or ILQ games) have shown promise in low-dimensional domains (e.g., autonomous driving) but have remained unexplored for high-DoF articulated systems due to the complexity of modeling collision penalties and the numerical conditioning required for second-order optimization.

Methodology: ILQ-Arm

The authors propose ILQ-Arm, a motion planning framework that formulates multi-manipulator coordination as a differential game. The method treats each manipulator as an independent agent optimizing its own objective while interacting with others through shared global states and collision constraints.

1. Game-Theoretic Formulation

The system models $N$ agents, where each agent $i$ seeks to minimize a cost functional $J_i$ consisting of a running cost $\ell_i$ and a termination cost $\phi_i$ . The goal is to find a set of time-varying state-feedback strategies $\{\gamma^*_i\}$ that constitute a Nash equilibrium, where no agent can unilaterally reduce its cost by deviating from the strategy.

Since solving for a global Nash equilibrium in nonlinear dynamics is intractable, the method employs an Iterative Linear-Quadratic (ILQ) approach:

Linearization: The nonlinear dynamics and cost functions are linearized and approximated quadratically around a nominal trajectory $\xi_k$ .
Backward Recursion: The algorithm solves a series of local LQ games using coupled Riccati backward recursions to compute feedback Nash strategies.
Forward Update: The strategies are updated with a step size $\eta$ to generate a new trajectory, iterating until convergence.

2. Integration of Collision Penalties

A key technical contribution is the explicit integration of differentiable geometric collision penalties into the optimization pipeline, tailored for complex manipulator kinematics:

Self-Collision: Enforces a minimum distance between all non-adjacent link pairs of a single arm.
Inter-Agent Collision: Models agents as capsule volumes and enforces minimum distances between them.
Environmental Obstacles: Utilizes Signed Distance Functions (SDF) to penalize proximity to static obstacles.

These penalties are formulated as differentiable terms within the running cost, allowing the second-order optimizer to handle safety constraints directly without fixed priority rules.

3. Cost Function Design

The running cost $\ell_i$ combines:

Collision Penalties: For self, inter-agent, and environmental obstacles.
Control Regularization: A quadratic control cost ( $\|u\|^2_2$ ) to promote smooth, physically feasible trajectories and improve numerical conditioning.
Termination Cost: A quadratic penalty on the final state deviation from the goal, applied only at the terminal time step. This design allows the planner flexibility to navigate detours during the transient phase without incurring high intermediate costs, while ensuring goal convergence.

Key Contributions

Differential Game Formulation for Multi-Manipulators: The paper presents a novel framework modeling each arm as an optimization agent, computing feedback Nash strategies via an ILQ game procedure.
Differentiable Collision Integration: It integrates differentiable geometric collision penalties (including self-collision) directly into the ILQ backward-forward optimization pipeline, a capability previously unexplored for high-DoF systems.
Systematic Cost Analysis: The authors provide a systematic analysis of cost design, demonstrating how quadratic control regularization and terminal-only goal costs impact convergence, safety, and trajectory quality.
Scalable Validation: The framework is validated on challenging scenarios involving varying numbers of agents (up to 4) and diverse environmental constraints, outperforming traditional centralized and decentralized baselines.

Experimental Results

The authors evaluated ILQ-Arm against Centralized CHOMP, Best Response CHOMP, Centralized RRT*, and the learning-based NeHMO planner across 1,500 test instances.

Performance Metrics: The evaluation focused on Success Rate (SR), Planning Time (PT), and Path Length (PL).
Comparative Results:
- ILQ-Arm consistently achieved the highest success rates (e.g., 96.8% for 2 agents, 86.0% for 4 agents) with the lowest planning times (e.g., 0.121s for 2 agents).
- Centralized CHOMP showed reasonable success rates but suffered from significantly higher planning times and longer path lengths, failing to scale effectively as agent count increased.
- Decentralized CHOMP (Best Response) struggled with reliability and efficiency, particularly in complex environments.
- NeHMO achieved moderate success in specific scenarios but produced less efficient paths and could not handle static obstacles.
Ablation Studies: Removing self-collision penalties drastically reduced computation time but resulted in frequent collisions, confirming the necessity of explicit safety modeling. Replacing quadratic control with Pseudo-Huber loss reduced success rates in multi-agent scenarios, highlighting the importance of quadratic curvature for numerical stability. Using a running goal cost (instead of terminal) degraded performance by interfering with coordinated negotiation.
Real-World Deployment: The method was successfully deployed on two UR5e robots, solving collision avoidance tasks with an average planning time of 0.475 seconds and executing safely in dynamic environments.

Significance and Claims

The paper claims that ILQ-Arm provides a principled and scalable solution for safe multi-agent manipulation in shared workspaces. By extending ILQ games to high-dimensional articulated systems, the framework:

Naturally models interaction: Coordinated behavior emerges from the optimization process without relying on fixed priority rules or treating agents as passive obstacles.
Ensures Safety and Smoothness: It guarantees collision-free trajectories (including self-collision) while maintaining dynamic consistency through quadratic control regularization.
Offers Computational Efficiency: It achieves a balance between the global consistency of centralized methods and the efficiency of decentralized approaches, making it suitable for real-time, high-DoF multi-robot applications.

The authors conclude that differential game formulations are highly effective for multi-robot manipulation, offering a robust alternative to traditional planning paradigms in complex, shared environments.