Multi Agent Motion Planning

project image

People

Roland Siegwart - Autonomous Systems Lab, ETH Zurich
Paul Beardsley - Disney Research Zurich
Prof. Javier Alonso-Mora - Autonomous Systems Lab, ETH Zurich

Funding

This project was funded by Disney Research Zurich

About the Project

Multi-robot systems are designed to achieve tasks by collaboration. A key requirement for their efficient operation is good coordination and collision avoidance. Various methods have previously been proposed for single robot cases but when applied to multi-robot systems they often are computationally expensive. This project proposes novel collision avoidance strategies for a group of non-holonomic robots such as non-holonomic optimal reciprocal collision avoidance(NH-ORCA). A set of planners are developed for aerial vehicles as well that have low-computational complexity.

NH-ORCA works by controlling robots to stay within a maximum tracking error of the ideal trajectory. This builds on an approach called ORCA which worked well for holonomic robots. NH-ORCA is a fast and distributed method that can deal with crowded situations as well and helps robotic systems achieve smooth and visually appealing trajectories. Several experiments are conducted and recorded with a number of non-holonomic puck robots which help demonstrate the effectiveness of this method.

Another contribution of this project was the development of methods based on Velocity Obstacles(VO) to provide real-time motion planning for quadrotors. This provides a local motion planner which considers motion constraints, static obstacles and other agents. The approaches are either centralised or distributed and have been applied to varying sizes of quadrotor agent groups. Succesful performance was shown in these experiments with up to four quadrotors in close proximity and including humans.

Project Demonstrations

Funding & Partners

This project was performed at ETH Zurich and funded by Disney Research Zurich.


Cooperative Collision Avoidance for Nonholonomic Robots
J. Alonso-Mora, P. Beardsley, R. Siegwart. In IEEE Transactions on Robotics, vol. 34, no. 2, pp. 404-420, 2018.

In this paper, we present a method, namely € CCA, for collision avoidance in dynamic environments among interacting agents, such as other robots or humans. Given a preferred motion by a global planner or driver, the method computes a collision-free local motion for a short time horizon, which respects the actuator constraints and allows for smooth and safe control. The method builds on the concept of reciprocal velocity obstacles and extends it to respect the kinodynamic constraints of the robot and account for a grid-based map representation of the environment. The method is best suited for large multirobot settings, including heterogeneous teams of robots, in which computational complexity is of paramount importance and the robots interact with one another. In particular, we consider a set of motion primitives for the robot and solve an optimization in the space of control velocities with additional constraints. Additionally, we propose a cooperative approach to compute safe velocity partitions in the distributed case. We describe several instances of the method for distributed and centralized operation and formulated both as convex and nonconvex optimizations. We compare the different variants and describe the benefits and tradeoffs both theoretically and in extensive experiments with various robotic platforms: robotic wheelchairs, robotic boats, humanoid robots, small unicycle robots, and simulated cars.

Collision Avoidance for Aerial Vehicles in Multi-Agent Scenarios
J. Alonso-Mora, T. Naegeli, R. Siegwart, P. Beardsley. In , Autonomous Robots, vol. 39, no. 1, pp. 101–121, 2015.

This article describes an investigation of local motion planning, or collision avoidance, for a set of decision-making agents navigating in 3D space. The method is applicable to agents which are heterogeneous in size, dynamics and aggressiveness. It builds on the concept of velocity obstacles (VO), which characterizes the set of trajectories that lead to a collision between interacting agents. Motion continuity constraints are satisfied by using a trajectory tracking controller and constraining the set of available local trajectories in an optimization. Collision-free motion is obtained by selecting a feasible trajectory from the VO’s complement, where reciprocity can also be encoded. Three algorithms for local motion planning are presented—(1) a centralized convex optimization in which a joint quadratic cost function is minimized subject to linear and quadratic constraints, (2) a distributed convex optimization derived from (1), and (3) a centralized non-convex optimization with binary variables in which the global optimum can be found, albeit at higher computational cost. A complete system integration is described and results are presented in experiments with up to four physical quadrotors flying in close proximity, and in experiments with two quadrotors avoiding a human.

Towards Estimation and Correction of Wind Effects on a Quadrotor UAV
F. Schiano, J. Alonso-Mora, K. Rudin, P. Beardsley, R. Siegwart, B. Siciliano. In Proc. of the Int. Micro Air Vehicle Conference and Competition, 2014.

In this paper the problem of estimation and correction of wind effects on a quadrotor UAV, without using wind sensors, is discussed. A large body of research addresses the effects of wind on a quadrotor, however most of them consider it only as a disturbance in their control loop and, for this reason, they solved the problem compensating for the wind with a powerful controller. The main part of this paper is related to the modeling of wind on a quadrotor and to the wind tunnel tests performed at the IFD wind tunnel of ETH Zurich. The approach presented can be used as a starting point for future works. The results obtained in the wind tunnel are really promising for the formulation of a complete aerodynamic model of the quadrotor that has been missing until now.

Shared Control of Autonomous Vehicles based on Velocity Space Optimization
J. Alonso-Mora, P. Gohl, S. Watson, R. Siegwart, P. Beardsley. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA), 2014.

This paper presents a method for shared control of a vehicle. The driver commands a preferred velocity which is transformed into a collision-free local motion that respects the actuator constraints and allows for smooth and safe control. Collision-free local motions are achieved with an extension of velocity obstacles that takes into account dynamic constraints and a grid-based map representation. To limit the freedom of the driver, a global guidance trajectory can be included, which specifies the areas where the vehicle is allowed to drive in each time instance. The low computational complexity of the method makes it well suited for multi-agent settings and high update rates and both a centralized and a distributed algorithm are provided that allow for real-time control of tens of vehicles. Extensive experimental results with real robotic wheelchairs at relatively high speeds in tight scenarios are presented.

Collaborative Motion Planning for Multi-Agent Systems
J. Alonso-Mora, R. Siegwart, D. Rus. In Workshop The future of multiple-robot research and its multiple identities at the RSJ/IEEE Int. Conf. on Robotics and Intelligent Systems (IROS), 2014.

Reciprocal Collision Avoidance with Motion Continuity Constraints
M. Rufli, J. Alonso-Mora, R. Siegwart. In IEEE Transactions on Robotics, vol. 29, no. 4, pp. 899-912, 2013.

This paper addresses decentralized motion planning among a homogeneous set of feedback-controlled, decision-making agents. It introduces the continuous control obstacle ( C n -CO), which describes the set of C n -continuous control sequences (and thus trajectories) that lead to a collision between interacting agents. By selecting a feasible trajectory from C n -CO's complement, a collision-free motion is obtained. The approach represents an extension to the reciprocal velocity obstacle (RVO, ORCA) collision-avoidance methods so that trajectory segments verify C n continuity rather than piecewise linearity. This allows the large class of robots capable of tracking C n -continuous trajectories to employ it for partial motion planning directly-rather than as a mere tool for collision checking. This paper further establishes that both the original velocity obstacle method and several of its recently developed reciprocal extensions (which treat specific robot physiologies only) correspond to particular instances of C n -CO. In addition to the described extension in trajectory continuity, C n -CO thus represents a unification of existing RVO theory. Finally, the presented method is validated in simulation-and a parameter study reveals under which environmental and control conditions C n -CO with admits significantly improved navigation performance compared with inflated approaches based on ORCA.

A message-passing algorithm for multi-agent trajectory planning
J. Bento, N. Derbinsky, J. Alonso-Mora, J. Yedidia. In , In Advances in Neural Information Processing Systems (NIPS), 2013.

We describe a novel approach for computing collision-free \emph{global} trajectories for p agents with specified initial and final configurations, based on an improved version of the alternating direction method of multipliers (ADMM). Compared with existing methods, our approach is naturally parallelizable and allows for incorporating different cost functionals with only minor adjustments. We apply our method to classical challenging instances and observe that its computational requirements scale well with p for several cost functionals. We also show that a specialization of our algorithm can be used for {\em local} motion planning by solving the problem of joint optimization in velocity space.

Collision Avoidance for Multiple Agents with Joint Utility Maximization
J. Alonso-Mora, M. Rufli, R. Siegwart, P. Beardsley. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA), 2013.

In this paper a centralized method for collision avoidance among multiple agents is presented. It builds on the velocity obstacle (VO) concept and its extensions to arbitrary kino-dynamics and is applicable to heterogeneous groups of agents (with respect to size, kino-dynamics and aggressiveness) moving in 2D and 3D spaces. In addition, both static and dynamic obstacles can be considered in the framework. The method maximizes a joint utility function and is formulated as a mixed-integer quadratic program, where online computation can be achieved as a trade-off with solution optimality. In experiments with groups of two to 50 agents the benefits of the joint utility optimization are shown. By construction, it's suboptimal variant is at least as good as comparable decentralized methods, while retaining online capability for small groups of agents. In its optimal variant, the proposed algorithm can provide a benchmark for distributed collision avoidance methods, in particular for those based on the VO concept that take interaction into account.

Limited benefit of Sharing Information in Multi-Agent Iterative Learning Control
A. Schoellig, J. Alonso-Mora, R. D'Andrea. In Asian Journal of Control, vol. 14, no. 3, pp. 613-623, 2012.

This paper studies iterative learning control (ILC) in a multi-agent framework, wherein a group of agents simultaneously and repeatedly perform the same task. Assuming similarity between the agents, we investigate whether exchanging information between the agents improves an individual's learning performance. That is, does an individual agent benefit from the experience of the other agents? We consider the multi-agent iterative learning problem as a two-step process of: first, estimating the repetitive disturbance of each agent; and second, correcting for it. We present a comparison of an agent's disturbance estimate in the case of (I) independent estimation, where each agent has access only to its own measurement, and (II) joint estimation, where information of all agents is globally accessible. When the agents are identical and noise comes from measurement only, joint estimation yields a noticeable improvement in performance. However, when process noise is encountered or when the agents have an individual disturbance component, the benefit of joint estimation is negligible.

Reciprocal Collision Avoidance for Multiple Car-like Robots
J. Alonso-Mora, A. Breitenmoser, P. Beardsley, R. Siegwart. In Proc. of the IEEE Int. Conf. on Robotics and Automation (ICRA), 2012.

In this paper a method for distributed reciprocal collision avoidance among multiple non-holonomic robots with bike kinematics is presented. The proposed algorithm, bicycle reciprocal collision avoidance (B-ORCA), builds on the concept of optimal reciprocal collision avoidance (ORCA) for holonomic robots but furthermore guarantees collision-free motions under the kinematic constraints of car-like vehicles. The underlying principle of the B-ORCA algorithm applies more generally to other kinematic models, as it combines velocity obstacles with generic tracking control. The theoretical results on collision avoidance are validated by several simulation experiments between multiple car-like robots.

Independent vs. Joint Estimation in Multi-Agent Iterative Learning Control
A. Schoellig, J. Alonso-Mora, R. D'Andrea. In Proc. of the Conf. on Decision and Control (CDC), 2010.

This paper studies iterative learning control (ILC) in a multi-agent framework, wherein a group of agents simultaneously and repeatedly perform the same task. The agents improve their performance by using the knowledge gained from previous executions. Assuming similarity between the agents, we investigate whether exchanging information between the agents improves an individual's learning performance. That is, does an individual agent benefit from the experience of the other agents? We consider the multi-agent iterative learning problem as a two-step process of: first, estimating the repetitive disturbance of each agent; and second, correcting for it. We present a comparison of an agent's disturbance estimate in the case of (I) independent estimation, where each agent has access only to its own measurement, and (II) joint estimation, where information of all agents is globally accessible. We analytically derive an upper bound of the performance improvement due to joint estimation. Results are obtained for two limiting cases: (i) pure process noise, and (ii) pure measurement noise. The benefits of information sharing are negligible in (i). For (ii), a performance improvement is observed when a high similarity between the agents is guaranteed.

Optimal Reciprocal Collision Avoidance for Multiple Non-Holonomic Robots
J. Alonso-Mora, A. Breitenmoser, M. Rufli, P. Beardsley, R. Siegwart. In Proc. of the Int. Symp. on Distributed Autonomous Robotics Systems (DARS), 2010. Nominated Best Student Paper Award.

IIn this paper an optimalmethod for distributed collision avoidance among multiple non-holonomic robots is presented in theory and experiments. Non-holonomic optimal reciprocal collision avoidance (NH-ORCA) builds on the concepts introduced in [2], but further guarantees smooth and collision-free motions under non-holonomic constraints. Optimal control inputs and constraints in velocity space are formally derived for the non-holonomic robots. The theoretical results are validated in several collision avoidance experiments with up to fourteen e-puck robots set on collision course. Even in scenarios with very crowded situations, NH-ORCA showed to be collision-free for all times.