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

Junyoung Kim; Hanwen Ren; Lei Zhang; Ahmed H. Qureshi

Source-linked AI summary

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

Junyoung Kim, Hanwen Ren, Lei Zhang, Ahmed H. Qureshi

arXiv:2608.27726v1Aug 27, 2026cs.RO

TL;DR

Multi-agent planning for high-DoF manipulators remains difficult because centralized methods scale poorly, decentralized methods face coordination and safety challenges, and articulated multi-arm ILQ applications are limited. The paper introduces ILQ-Arm, which models arms as interacting agents, solves iterative local LQ games, and integrates differentiable collision penalties. Across challenging scenarios, the framework produces smooth, dynamically consistent, collision-aware trajectories with efficient, balanced performance.

Problem
High-DoF multi-arm planning in shared workspaces requires coordination and collision avoidance, while existing ILQ applications have not addressed articulated manipulators’ kinematic and collision complexity.
Method
ILQ-Arm models each manipulator as an interacting optimization agent and computes feedback Nash strategies through iterative local LQ games with differentiable self- and inter-arm collision penalties.
Results
The full formulation achieves zero collisions while maintaining high success rates, competitive planning times, and short path lengths across test scenarios.
Takeaways & Limitations
Differential-game formulations can support coordinated, safe, smooth, and dynamically consistent motion generation for multi-arm manipulation without fixed priority rules.

Abstract

from arXiv · show

Multi-agent motion planning for high-degree-of-freedom robotics manipulators in shared workspaces remains a fundamental yet challenging problem. Centralized planners often suffer from poor scalability, while decentralized approaches face robustness and safety concerns. Game-theoretic formulations offer a promising approach for modeling agent interactions, potentially overcoming these limitations. However, their application to articulated multi-arm systems remains limited. This paper presents an iterative Linear Quadratic (LQ) game framework for multi-manipulator motion planning, where each manipulator is modeled as an independent agent optimizing its own objective while interacting with other agents based on shared global states and collision constraints. The method solves a series of local LQ games by linearizing the dynamics and approximating the cost around a nominal trajectory, with Riccati backward recursions yielding feedback Nash strategies. To address the challenges of articulated systems, we incorporate differentiable penalties for self-collision and inter-arm collision into the optimization pipeline, enabling coordinated, collision-aware trajectory generation. Experiments demonstrate that our framework produces smooth, safe, and efficient trajectories in high-dimensional settings, outperforming traditional methods. This highlights the effectiveness of differential game formulations for multi-robot manipulation.

I. INTRODUCTION

Multi-agent planning for high-DoF manipulators requires coordination and collision avoidance, but centralized and decentralized methods face scalability, coordination, robustness, or safety limitations. ILQ-Arm extends differential ILQ games to articulated multi-arm systems with collision penalties and cost-design analysis.

High-DoF manipulators must coordinate in shared workspaces while avoiding collisions across collaborative, warehouse, surgical, and human-robot applications.
Centralized planners provide globally consistent solutions but struggle with scalability and real-time performance in multi-manipulator settings.
Decentralized planners improve computational efficiency but face coordination, robustness, and safety challenges when predicting other agents’ actions.
ILQ games model each manipulator as an agent minimizing its own cost while producing feedback Nash strategies through local linear-quadratic approximations and Riccati recursions.
ILQ-Arm adds differentiable self-collision and inter-arm collision penalties to a centralized solver that computes decentralized per-agent feedback strategies.
Quadratic control regularization, goal costs, and extensive multi-arm validation are used to assess convergence, safety, trajectory quality, and performance across agent counts and environmental constraints.

II. RELATED WORK

Prior work spans centralized, decentralized, learning-based, trajectory-optimization, and game-theoretic approaches to multi-agent motion planning. ILQ-Arm bridges trajectory optimization and differential games for coordinated manipulation without fixed priorities or passive-obstacle assumptions.

Centralized planning offers global coordination but scales poorly as agent numbers and system dimensionality increase.
Learning-based methods enable fast deployment after offline training, while safety-aware variants add shielding, barrier-function, or reachability constraints.
Trajectory optimization handles nonlinear dynamics and safety constraints, but many methods treat other agents as passive dynamic obstacles.
Differential, LQ, and ILQ games represent coupled decisions and compute feedback Nash equilibria through Riccati recursions and iterative local approximations.
ILQ-Arm combines differential-game coordination with quadratic control, goal, and self-collision costs for high-dimensional articulated systems.

A. Problem Definition

The problem is formulated as a centralized multi-agent dynamical system whose agents are articulated robot arms with coupled state and control variables. The ILQ solver repeatedly linearizes dynamics and costs, solves local games by backward recursions, and updates trajectories until convergence.

A. Problem Definition: The global state contains all agents, while each m-DOF arm contributes joint configuration, velocity, acceleration, and control variables.
A. Problem Definition: Each agent minimizes a cost comprising running terms over the horizon and a terminal term evaluated at the final state.
B. Background: The solver seeks time-varying state-feedback strategies forming a global Nash equilibrium, although nonlinear global equilibria are generally intractable.
B. Background: Agents observe the full system state but not other agents’ control policies, and the procedure begins from an initial state and feedback strategies.
B. Background: At each iteration, the method simulates nonlinear dynamics, linearizes them around the current trajectory, and forms quadratic cost approximations.
B. Background: Coupled Riccati equations solve each local general-sum LQ subgame backward in time, returning affine feedback strategies that update the trajectory.
B. Background: A step size η ∈ (0, 1] moderates strategy updates because local linear-quadratic approximations may diverge outside the current trajectory’s neighborhood.

C. Our method

The method design integrates collision avoidance, running costs, and terminal costs into a pipeline for multi-agent motion planning.

The design combines collision avoidance, running-cost formulation, and termination-cost design within the final multi-agent planning pipeline.

1) Collision Penalty:

The framework uses differentiable penalties to model self-collision, inter-agent collision, and environmental obstacles while preserving feasible motion planning.

1) Collision Penalty:: Self-collision avoidance enforces a minimum distance between all non-adjacent manipulator links using a safety margin.The penalty includes an associated weighting coefficient.
1) Collision Penalty:: Inter-agent collision avoidance represents each agent’s links as a capsule volume and requires pairwise distances to exceed the safety margin.
1) Collision Penalty:: Environmental collision penalties use a Signed Distance Function to measure manipulator distance from workspace obstacles.The resulting distance measure supports feasible planning in dynamic environments.
1) Collision Penalty:: A quadratic control penalty is included alongside collision penalties to promote smooth and physically feasible trajectories.It penalizes excessive actuation through the squared L2-norm of the control input.

2) Control Cost:

The quadratic control cost discourages impulsive commands and improves the smoothness, physical feasibility, and numerical stability of the optimized trajectories.

2) Control Cost:: The quadratic control cost distributes effort across the horizon instead of producing impulsive, high-amplitude commands.
2) Control Cost:: Strict convexity penalizes large control magnitudes disproportionately, suppressing abrupt input variations and smoothing state evolution.For manipulators, this supports fluid joint motions, reduced excitation of unmodeled dynamics, and improved mechanical stability.
2) Control Cost:: As an optimization regularizer, the quadratic control term improves control-Hessian definiteness and numerical conditioning for reliable backward-pass updates.It also helps prevent ill-posed updates caused by poorly conditioned curvature.

3) Running Cost:

The running cost combines collision penalties with control regularization, while a terminal penalty supports reaching the desired configuration within a finite horizon.

3) Running Cost:: The running cost combines self-collision, inter-agent collision, environmental obstacle, and quadratic control penalties.
3) Running Cost:: The termination cost penalizes final-state deviation to encourage goal reachability within a finite-time horizon.It minimizes error between the terminal state and the desired configuration.

4) Termination Cost:

The terminal cost prioritizes reaching target joint configurations while allowing transient detours for collision avoidance and dynamic feasibility. Evaluation uses diverse multi-agent scenarios and compares success, time, and path length against baselines.

4) Termination Cost:: The target xg denotes agent i’s desired joint configuration, while β weights goal attainment relative to other cost components.
4) Termination Cost:: A terminal-only goal penalty lets the planner prioritize collision avoidance and dynamic feasibility during transients, including detours around obstacles.
4) Termination Cost:: The coupling of terminal and running costs encourages braking near xg, balancing goal achievement with motion quality and dynamic consistency.
4) Termination Cost:: The evaluation includes representative scenarios with four agents, obstacles, and NeHMO benchmarks involving two and three agents.
4) Termination Cost:: Experiments use six scenario types with 250 randomly generated instances each, a 5s horizon, and Δt = 0.1s.All cases are selected so direct motion toward targets would cause collisions.
4) Termination Cost:: Scenarios vary agent counts from two to four and include ground-plane obstacles, with one scenario adding multiple static obstacles in narrow scenes.
4) Termination Cost:: Scenarios 5 and 6 enable direct comparison with NeHMO, producing 1,500 total multi-agent path-planning test cases.
4) Termination Cost:: The reported metrics are success rate, planning time, and path length, measuring valid collision-free completion, computation time, and cumulative travel distance.

B. Simulation Results

ILQ-Arm is evaluated against centralized and decentralized baselines across multi-agent scenarios, with comparisons covering success, efficiency, and path quality. The ablation study examines how self-collision penalties, control costs, and goal-cost placement affect planning.

Baseline comparison: ILQ-Arm is compared with Centralized CHOMP, Best Response CHOMP, Centralized RRT*, and NeHMO across multiple test scenarios.Table I averages metrics over successful instances except success rate.
Baseline comparison: ILQ-Arm outperforms Centralized CHOMP in efficiency and path quality, although Centralized CHOMP has slightly higher success rates in static-obstacle cases.The centralized baseline avoids constrained collisions more often but produces longer paths and requires substantially more planning time.
Baseline comparison: Decentralized CHOMP has poor reliability and computational efficiency compared with ILQ-Arm, especially as agent numbers increase.Its sequential best-response procedure struggles across random and complex environments and produces inefficient trajectories.
Baseline comparison: NeHMO achieves relatively high success in its evaluated scenarios but requires more planning time, generates longer paths, cannot handle static obstacles, and degrades with more agents.Its applicability is restricted in cluttered or confined environments because it does not account for static obstacles.
Ablation setup: The full ILQ-Arm formulation achieves the best balance of success rate, collision avoidance, planning time, and path length.The ablation table identifies the control cost, terminal goal cost, and self-collision penalty as important components.

2) Ablation Studies:

Ablations show that explicit self-collision modeling and the full cost design are central to reliable multi-agent planning. Quadratic control and terminal goal costs preserve performance better than the tested alternatives as coordination becomes more difficult.

Self-collision penalty: Removing the self-collision penalty reduces planning time but causes frequent collisions, making the planner unsafe and unreliable.The simplified objective is computationally cheaper but loses collision-free behavior.
Control cost: As agent numbers increase, quadratic control consistently achieves higher success rates and more stable convergence than Pseudo-Huber control.The stronger curvature of the quadratic penalty is reported to improve numerical conditioning and stabilize the backward pass.
Goal cost: A running goal cost substantially degrades multi-agent performance, reducing success rates while increasing planning time and path length.The reported explanation is that uniform goal pressure interferes with coordinated negotiation and reduces intermediate-motion flexibility.
Full formulation: The complete ILQ-Arm formulation achieves zero collisions while maintaining high success rates, competitive planning times, and short paths.The results support complementary roles for the cost components in robust motion planning.

C. Real Robot Experiments

ILQ-Arm is deployed on two UR5e robots in inter-robot and static-obstacle avoidance tasks. It solves all reported cases while coordinating collision avoidance with each robot’s own objective.

Experimental setup: The real-robot system uses two UR5e robots placed 0.8 m apart for five collision-avoidance tasks.Four tasks involve inter-robot collision avoidance and one includes static obstacles.
Results: ILQ-Arm succeeds in all real-world cases with an average planning time of 0.475 seconds and an average execution time of 64 seconds.The experiments include narrow environments with static obstacles and inter-robot collisions.
Method in deployment: The framework computes feedback Nash strategies and integrates differentiable self-collision and inter-arm penalties for smooth, dynamically consistent, collision-aware motion.The game formulation captures strategic coupling without fixed priority rules.