Online Trajectory Generation with Distributed Model Predictive Control for Multi-Robot Motion Planning

Carlos E. Luis; Marijan Vukosavljev; Angela P. Schoellig

Source-linked AI summary

Online Trajectory Generation with Distributed Model Predictive Control for Multi-Robot Motion Planning

Carlos E. Luis, Marijan Vukosavljev, Angela P. Schoellig

arXiv:1909.05150v2Sep 11, 2019cs.RO

TL;DR

Multi-robot trajectory generation must produce collision-free, goal-oriented motion in shared and changing environments. The paper proposes a DMPC framework with on-demand collision avoidance and event-triggered replanning, achieving higher transition-task success and lower travel times than BVC, including dense quadrotor simulations and real-time experiments.

Problem
Real-time multi-robot planning requires collision-free, goal-oriented trajectories while accounting for environmental and neighboring-agent states.
Method
The paper combines distributed model predictive control, on-demand collision avoidance, and event-triggered replanning for real-time trajectory generation.
Results
More than 90% success rate was achieved with 30 palm-sized quadrotors in an 18 m^3 arena, while experiments sent trajectories at 20 Hz to 20 quadrotors.
Takeaways & Limitations
The method provides real-time multi-robot planning with higher transition-task success and lower travel times than the Buffered Voronoi Cells method.

Abstract

from arXiv · show

We present a distributed model predictive control (DMPC) algorithm to generate trajectories in real-time for multiple robots. We adopted the \textit{on-demand collision avoidance} method presented in previous work to efficiently compute non-colliding trajectories in transition tasks. An event-triggered replanning strategy is proposed to account for disturbances. Our simulation results show that the proposed collision avoidance method can reduce, on average, around 50% of the travel time required to complete a multi-agent point-to-point transition when compared to the well-studied Buffered Voronoi Cells (BVC) approach. Additionally, it shows a higher success rate in transition tasks with a high density of agents, with more than 90% success rate with 30 palm-sized quadrotor agents in a 18 m^3 arena. The approach was experimentally validated with a swarm of up to 20 drones flying in close proximity.

I. INTRODUCTION

The paper targets real-time, collision-free trajectory generation for multi-robot tasks in dynamic or unknown environments. It proposes a DMPC framework with event-triggered replanning and on-demand collision avoidance to support less conservative, faster transitions.

Multi-robot planners must generate collision-free, goal-oriented trajectories while accounting for environmental and neighboring-agent states.
The framework is presented as a real-time multi-vehicle planner evaluated for transition-task success in agent-dense environments and drone swarms.
The framework uses distributed model predictive control for real-time trajectory generation, state feedback, and online replanning.
The method is purely optimization-based and formulates planning as an efficiently solvable Quadratic Program.
On-demand collision avoidance replaces BVC free-space partitioning, producing less conservative movement and faster transition times.

II. PROBLEM STATEMENT

The problem is to compute inputs for multiple agents with known linear dynamics so they avoid collisions and obstacles, remain within the workspace, and reach their desired positions. The formulation assumes position-tracking controllers and parameterizes reference trajectories with smooth Bézier curves.

Given known linear dynamics, the planner computes inputs that avoid inter-agent and obstacle collisions, remain in the workspace, and approach desired positions after a finite time.
A. The Agents: Each agent is assumed to have a position trajectory-tracking controller, with the input serving as a position reference.
A. The Agents: The quadrotor model uses position and velocity states, while the framework also permits more complex linear-system models.
Trajectory Parameterization: Receding-horizon control recomputes finite-horizon input sequences at each discrete planning step.
A. Trajectory Parameterization: Continuous inputs are represented as concatenated Bézier curves whose control points are optimized to generate trajectories.
A. Trajectory Parameterization: Samples of Bézier curves and their derivatives are linear combinations of control points, enabling convex optimization.

B. The Agent Prediction Model

The prediction model propagates each agent’s state over a finite horizon and optimizes Bézier-curve control points as decision variables. Equality and inequality constraints enforce smoothness, dynamic feasibility, workspace limits, and collision-avoidance requirements while reducing conservativeness.

B. The Agent Prediction Model: The notation ˆ(·)[k|kt] denotes the predicted value at future step kt+k using information available at kt.
B. The Agent Prediction Model: The stacked predicted state sequence is constructed from the measured state and stacked input sequence over the prediction horizon.
B. The Agent Prediction Model: The optimization decision vector consists of the control points for the concatenated Bézier curves.
C. Input Continuity: Equality constraints enforce initial conditions and continuity between successive Bézier curves, including selected derivatives.
Dynamic Feasibility: Linear inequality constraints encode actuator and workspace limits using selected samples of the input and its derivatives.
Dynamic Feasibility: The sampling-based constraint method avoids convex-hull conservativeness and can use platform-specific acceleration limits for quadrotors.

E. Optimization-Based Collision Avoidance

The framework compares BVC and on-demand collision avoidance within a distributed predictive-planning formulation. On-demand avoidance constrains only predicted collision samples, producing less conservative maneuvers than BVC.

Both methods impose hyperplane constraints that restrict the free space available for optimizing future inputs.
Buffered Voronoi Cells: BVC keeps each agent within its Buffered Voronoi Cell for a specified horizon interval.The cell constraint becomes p + 1 constraints on the first Bézier curve's control points.
Buffered Voronoi Cells: BVC constraints produce collision-free updates in the two-agent example.
On-demand collision avoidance: On-demand avoidance uses predicted future actions and can impose constraints in either state space or input space.State-space constraints apply to predicted states, while input-space constraints apply directly to inputs and produce collision-free reference positions over the horizon.
On-demand collision avoidance: On-demand avoidance constrains a specific curve sample at the first predicted collision, whereas BVC constrains a complete curve segment.The resulting trajectories are described as less conservative for on-demand avoidance.
Constraint implementation: Collision constraints are added to the optimization as an inequality tuple AcollUi ≤ bcoll.Slack variables εij < 0 relax the state-space collision constraint.

F. Cost Function

The cost function is formed by summing multiple terms, with tuning parameters omitted from the notation even though agents may use different values.

The optimization cost is the sum of various terms.
Tuning-parameter subscripts are omitted from the notation, although each agent may have different values.

1) Error to goal:

The formulation combines goal-tracking, smoothness, and collision-violation penalties in a quadratic-programming framework. These terms respectively encourage reaching the goal, penalize input derivatives, and soften collision constraints.

Error to goal:: The goal-error term minimizes weighted position errors over the last κ < K horizon steps relative to each agent's target pd,i.The weights qk are positive, and the term can be written as a quadratic form in the inputs and measured state.
Energy: The energy term minimizes a weighted sum of squared input derivatives through the rth derivative.Its coefficient αc > 0 weights the derivatives, and the term has a quadratic form in Ui.
Constraint violation: Soft collision avoidance adds a penalty term to limit constraint relaxation, using both quadratic and linear penalty costs.
The complete optimization is a standard QP composed of Ji,error, Ji,energy, and Ji,violation.

IV. EVENT-TRIGGERED REPLANNING

The event-triggered replanning strategy resets the reference only when detected disturbances indicate that the agent has been perturbed. This avoids the jitter associated with continuously matching the reference to the measured state during normal operation.

Continuous replanning can create reference-signal discontinuities that cause jittering in the agent state.The stated limitation is especially relevant with imperfect tracking or slow dynamics.
Event-triggered replanning resets the input to match the agent state only when perturbation is detected.
The activation function uses trajectory-tracking error and a small velocity-sign term ε ≪ 1 to avoid singularities.The formulation assumes |vi,n[kt]| > 0, which the paper associates with noise in state estimation.
The activation function detects disturbances from tracking-error and velocity behavior rather than relying only on a fixed error threshold.It targets cases where error is growing while velocity is small or points opposite the error.
Normal operation is defined by fmin < fn[kt] < fmax for every component of fn[kt].The threshold extrema are extracted under normal operation; otherwise the Bézier curve's initial position and velocity match the vehicle state, with higher derivatives set to zero.
In a quadrotor experiment, replanning repeatedly reset the reference during human-applied disturbances and allowed task continuation after disturbance removal.The task was to reach y = -1 m.
Under normal operation, replanning is not required, yielding a smooth reference signal; undetected perturbations are assumed rejectable by the underlying controller.The authors report experimental validation of this assumption.

V. THE ALGORITHM

The algorithm updates each agent’s trajectory inputs from measured states, desired locations, and other agents’ latest predictions. Although designed for distributed execution, it is implemented centrally here and run in parallel for all agents.

Inputs and execution: Each agent’s update uses its measured state, desired location, and two time bases for MPC planning and refined execution.The algorithm is designed for agents with communication capabilities and uses a coarse planning step h and refined step Ts.
Distributed update: Agents receive the latest predictions of other agents and update their input sequences by solving a quadratic program.The latest predictions are exchanged through an ideal communication channel assumed to have no delays or packet drops.
Collision-avoidance information: On-demand collision avoidance uses other agents’ predictions, whereas BVC uses their measured states instead of predictions.The two methods therefore rely on different inter-agent information during collision-avoidance updates.
Implementation: The centralized implementation executes the update algorithm in parallel for all agents because input updates have no inter-agent data dependency.This implementation compensates for the physical platform’s lack of inter-agent communication capabilities.

VI. SIMULATION RESULTS

The simulations compare BVC and on-demand collision avoidance in increasingly dense 18 m3 transition scenarios. Input-space on-demand avoidance performs best at high density and reduces transition times, while performance declines beyond 30 agents.

Compared methods: Four optimization-based methods were compared: standard BVC, soft-constraint BVC, state-space on-demand avoidance, and input-space on-demand avoidance.Discrete planning was removed for the comparison, although it can improve the performance of any method.
Simulation setup: The experiments used randomly generated initial and final locations in an obstacle-free 18 m3 workspace, with swarm sizes from 10 to 60 agents.Each swarm size was evaluated using 50 random test cases, and success required collision-free goal completion within 20 seconds.
Success rate: More than 90% success rate was achieved by input-space on-demand collision avoidance for swarm sizes up to 30 agents.The method showed better performance than the alternatives especially in high-agent-density workspaces.
Density limitation: After 30 agents, performance declined significantly for all tested methods, and increasing constraint relaxation can result in collisions.The authors identify this density-dependent decline as a weakness of their approach.
Transition time: Around 50% transition-time reductions were obtained on average with on-demand collision avoidance compared with BVC methods.The comparison reports faster transition times across the tested methods’ random transition scenarios.
Travelled distance: All methods produced trajectories of almost the same length, while soft BVC trajectories were around 3% shorter on average.Travelled distance was analyzed as a measure of optimality.

B. Runtime Benchmark

The runtime benchmark measures per-agent input-update computation for on-demand avoidance and BVC across swarm sizes. The empirical comparison focuses on the associated quadratic-program solve time and the implementation used for experiments.

Benchmark scope: The benchmark compares the average runtime per agent required to update inputs with on-demand collision avoidance and BVC.Results are averaged over 50 randomly generated tests for each considered swarm size.
Measured quantity: The reported runtime specifically measures the average time per agent to solve the associated quadratic-program problem.This is the computation used to update each agent’s input sequence.
Scaling analysis: Both methods’ inequality-constraint counts scale with O(N_i,kt), where N_i,kt is the number of nearby neighbours considered for agent i.Soft BVC additionally introduces (p + 1) × N_i,kt slack variables, while on-demand methods add N_i,kt decision variables.
Runtime result: Soft BVC had the slowest empirical runtime because of its added slack variables and overall problem formulation.The runtime comparison is presented in Fig. 6 for the considered swarm sizes.
Experimental implementation: The experimental system computed all inputs on a single computer and broadcast them to the drone swarm through a radiolink.The implementation used C++, ROS, and qpOASES, with estimated individual positions supplied by motion capture.

A. Obstacle-Free Transitions

The DMPC framework supports real-time multi-drone transitions by scaling computation across agents, while maintaining collision-free motion under dense conditions and disturbances. Experiments include obstacle-mediated hula-hoop transitions, showing progress toward goals despite detours and tuning-dependent completion times.

Obstacle-Free Transitions: 2–20 drones were tested in 3 × 3 × 2 m3 with rmin = 0.35 m across repeated 30-second flights.Each swarm size used three independent flights with randomly generated transitions.
Obstacle-Free Transitions: Pseudo-linear runtime scaling was observed as agents were added, despite all computations running on a single computer.The approach parallelizes computation through its distributed structure.
Obstacle-Free Transitions: Dense flights reduced minimum inter-agent distance and sometimes violated rmin = 0.35 m, yet agents remained collision-free when the safety distance was sufficiently large.The optimizer may violate the collision constraint when required, creating a safety-performance trade-off.
Obstacle-Free Transitions: In the 10-drone hula-hoop task, transition success was declared within a 6 cm target radius, with completion at Tf = 28 s.The target-distance envelope tracked minimum and maximum distance over time.
Obstacle-Free Transitions: Completion time varied from 20.1 to 48.4 seconds across 18 tuning trials, while penalties ξ < −2 × 103 sometimes caused oscillations and deadlocks.State-space collision avoidance had a much lower success rate than input-space avoidance; non-convex obstacles may require discrete-planner waypoints.
Obstacle-Free Transitions: 20 quadrotors received trajectories in real time at 20 Hz, while the approach replanned robustly under unmodeled disturbances.The hula-hoop transition also produced satisfactory experimental results.