Managing Conflicting Tasks in Heterogeneous Multi-Robot Systems Through Hierarchical Optimization

Approach

Every requirement placed on the fleet — the robots’ own dynamics included — is written as a task, a set of linear equality and inequality constraints on the stacked states and inputs of all $n_\mathrm{r}$ robots:

where the slack variable $\mu_p$ keeps the problem feasible when a task cannot be satisfied. The tasks are then solved in a strict priority order: the robots’ dynamics take the highest priority, the input limits follow, and the remaining tasks are ordered according to the requirements of the application. The priority ordering is the only thing the designer sets — no weight tuning is required.

Hierarchical optimization

Each task $\mathcal{T}_p$ is solved as a quadratic program that also carries the inequality constraints of all higher-priority tasks, giving a partial solution $x_p^*$. The global solution, which also satisfies the equality constraints of the higher-priority tasks, is obtained by projecting the partial solution into their null space:

with the null-space projector computed recursively from the singular value decomposition of $A_p N_{p-1}$:

Since $N_p$ loses columns as each task constrains the solution space, lower-priority tasks optimize over a progressively smaller space and cannot undo what a more important task has achieved. The solution of the whole set of tasks is $\bar{x}_m^*$, obtained after solving $m$ subproblems.

Hierarchical constraint-based model predictive control

The hierarchy is embedded in a model predictive controller covering a horizon of $n_\mathrm{c} \Delta t$. The optimization uses direct multiple shooting, which jointly optimizes the states and the inputs of the fleet and offers better convergence than single shooting at the cost of more variables. Each robot’s dynamics are discretized and linearized around a nominal trajectory rather than a single point, which significantly reduces linearization errors. Heterogeneity is handled naturally, because every robot contributes its own dynamics and its own input and input-rate limits to the same problem.

Tasks

The paper formalizes a set of multi-robot tasks within the framework. Since several tasks may share the same priority level and be traded off by relative weights, the approach is a superset of weighted optimization.

Results

Prioritized versus weighted optimization

Two single-robot scenarios separate the proposed approach from the weighted one, where relative importance is expressed by a scalar $\kappa$ that must be tuned.

In the first, a unicycle is given three conflicting go-to-goal tasks, with $\mathcal{T}_\mathrm{a}$ more important than $\mathcal{T}_\mathrm{b}$ and $\mathcal{T}_\mathrm{c}$. The prioritized controller satisfies $\mathcal{T}_\mathrm{a}$ without compromise while moving smoothly and within the robot’s dynamics. The weighted controller only matches it once $\kappa$ is raised to 1000.

In the second scenario the same robot is given five partially conflicting tasks of decreasing importance — $\mathcal{T}_1: x = 5$, $\mathcal{T}_2: x = -12$, $\mathcal{T}_3: y = 10$, $\mathcal{T}_4: y = -4$, and $\mathcal{T}_5: \theta = \pi/4$ — whose optimal final configuration is $x = 5$, $y = 10$, $\theta = \pi/4$. The prioritized controller reaches it without tuning: it first moves horizontally to satisfy $\mathcal{T}_1$ with minimal delay, only then moves vertically to complete $\mathcal{T}_3$, and finally rotates to accomplish $\mathcal{T}_5$. No weight value reproduces that behaviour. With $\kappa = 100$ the robot completes a single task even though the others are non-conflicting, with $\kappa = 5$ it accomplishes none, and the best intermediate value leaves feasible tasks uncompleted.

Coverage and battery charging

A fleet of six mobile robots must cover a square area while each robot keeps a sufficient battery charge, recharged by reaching a charging station. Coverage is prioritized while the battery is full; below a threshold, recharging is assigned a higher priority for that robot. The remaining robots keep covering the area and adapt to the movement of the one that leaves. The scenario is run with six unicycles and repeated with a heterogeneous fleet of four unicycles and two omnidirectional robots, which the framework manages by carrying each robot’s dynamics and constraints in the same optimization problem.

Collision avoidance, formation control, and centroid velocity

Five omnidirectional robots must hold a formation and follow a centroid velocity reference while avoiding an obstacle and each other, keeping at least 50 cm between robots. Collision avoidance always has the highest priority; swapping the order of the other two tasks decides which one is sacrificed. With formation control above the velocity reference, the formation is preserved through the evasive manoeuvre and the centroid velocity error peaks at 0.7 m/s. With the order reversed, the velocity error stays at 0 m/s at all times and the formation is temporarily broken instead.

In both cases the safety distance is never violated: the highest-priority task is achieved without compromise, and prioritization is enforced by null-space projection rather than by weights that would have to be tuned against safety.

Computation time

The framework is implemented in Python 3.10, with the tasks defined and linearized through CasADi and solved with quadprog, on a 12th Gen Intel Core i7-12700H. The weighted approach solves one large problem, the hierarchical one solves $m$ smaller subproblems, so which is faster depends on how the inequality constraints are distributed. The weighted approach is roughly four times faster in the two single-robot scenarios and 50% faster on coverage, whereas the hierarchical approach is 40% faster on the collision-avoidance-heavy scenario, where its subproblems are considerably smaller.

The computational cost scales linearly with the prediction horizon $n_\mathrm{c}$ and quadratically with the number of robots $n_\mathrm{r}$. At a control frequency of 20 Hz, the approach remains real-time capable with fleets of more than 40 robots for short prediction horizons.

Conclusion

By leveraging hierarchical optimization within a model predictive control structure, the proposed method handles task prioritization efficiently, ensuring strict task ordering and execution while simultaneously accounting for the robots’ dynamics and constraints. The framework maintains task priority without the extensive parameter tuning that weighted approaches require, suits heterogeneous robot systems and complex environments, and manages concurrent tasks such as collision avoidance, coverage, formation control, and battery charging. Future work will explore decentralized optimization and validation under more dynamic conditions.

BibTeX citation

@article{debenedittis2025managing,
  author  = {De Benedittis, Davide and Garabini, Manolo and Pallottino, Lucia},
  journal = {IEEE Robotics and Automation Letters},
  title   = {Managing Conflicting Tasks in Heterogeneous Multi-Robot Systems Through Hierarchical Optimization},
  year    = {2025},
  volume  = {10},
  number  = {6},
  pages   = {5305--5312},
  doi     = {10.1109/LRA.2025.3559843}
}