## Abstract

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.

This is a preview of subscription content, log in to check access.

## Access options

### Buy single article

Instant unlimited access to the full article PDF.

US$ 39.95

Price **includes VAT** for USA

### Subscribe to journal

Immediate online access to all issues from 2019. Subscription will auto renew annually.

US$ 99

This is the **net price**. Taxes to be calculated in checkout.

## Notes

- 1.
Estimated states with a Kalman filter, including time delay compensation, are used in our implementation.

- 2.
In this formulation arbitrary object shapes can be considered, but with the assumption that they do not rotate during the local planning horizon (typically a few seconds).

- 3.
In previous works we have referred to it as holonomic trajectory and reference trajectory but, to hopefully increase clarity we now adopt the latter.

- 4.
To account for the downwash effect that does not allow for close operation in the vertical direction. The results readily extend to robots of arbitrary shape with the assumption of constant orientation for \(t\in [0,\tau ]\).

- 5.
We use IBM ILOG CPLEX.

- 6.
We use IBM ILOG CPLEX.

- 7.
Although the local planning is designed for on-board performance, this is left as future work.

- 8.
Position control and the local trajectory interpreter could be on-board given access to position sensing.

- 9.
Alternatively, any other controller with arbitrary constraints, or an LQR controller can be employed.

- 10.
- 11.
- 12.
- 13.
- 14.
This might not always be the case, mostly in scenarios with fast dynamic obstacles, and if a collision can not be avoided. In that case, the quadrotor stops to guarantee passive safety.

- 15.
If they are linearized following a given strategy such as avoid to the right, coordination is always achieved, but the solutions can be suboptimal.

- 16.
For example both agents try to avoid each other on the same side.

## References

Alonso-Mora, J., Breitenmoser, A., Beardsley, P., & Siegwart, R. (2012a). Reciprocal collision avoidance for multiple car-like robots. In

*2012 IEEE International conference on robotics and automation (ICRA)*(pp. 360–366).Alonso-Mora, J., Schoch, M., Breitenmoser, A., Siegwart, R., & Beardsley, P. (2012b). Object and animation display with multiple aerial vehicles. In

*2012 IEEE/RSJ international conference on intelligent robots and systems (IROS)*(pp. 1078–1083).Alonso-Mora, J., Rufli, M., Siegwart, R., & Beardsley, P. (2013). Collision avoidance for multiple agents with joint utility maximization.

*ICRA*,*2013*, 1–6.Augugliaro, F., Schoellig, A. P., & D’Andrea, R. (2012). Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In

*IEEE/RSJ international conference on intelligent robots and systems*(pp. 1–6).Bareiss, D., & van den Berg, J. (2013). Reciprocal collision avoidance for robots with linear dynamics using LQR-obstacles. In

*IEEE international conference robotics and automation*.Fiorini, P., & Shillert, Z. (1998). Motion planning in dynamic environments using velocity obstacles.

*International Journal of Robotics Research*,*17*(7), 760–772.Fox, D., Burgard, W., & Thrun, S. (1997). The dynamic window approach to collision avoidance.

*IEEE Robotics & Automation Magazine*,*4*(1), 23–33.Frazzoli, E., Dahleh, M. A., & Feron, E. (2002). Real-time motion planning for agile autonomous vehicles.

*Journal of Guidance, Control, and Dynamics*,*25*(1), 116–129.Guy, S. J., Chhugani, J., Curtis, S., Dubey, P., Lin, M., & Manocha, D. (2010). Pledestrians: A least-effort approach to crowd simulation. In

*Proceedings of the 2010 ACM SIGGRAPH/Eurographics symposium on computer animation*(pp. 119–128).Hoffmann, G. M., & Tomlin, C. J. (2008). Decentralized cooperative collision avoidance for acceleration constrained vehicles. In

*47th IEEE conference on decision and control (CDC)*(pp. 4357–4363).Hoffmann, G. M., Waslander, S. L., & Tomlin, C. J. (2008). Quadrotor helicopter trajectory tracking control. In

*AIAA guidance, navigation and control conference and exhibit*(pp. 1–14).Knepper, R. A., & Mason, M. T. (2012). Real-time informed path sampling for motion planning search.

*The International Journal of Robotics Research*,*31*, 1231–1250.Kumar, V., & Michael, N. (2012). Opportunities and challenges with autonomous micro aerial vehicles.

*The International Journal of Robotics Research*,*31*, 1279–1291.Kushleyev, A., Mellinger, D., & Kumar, V. (2012). Towards a swarm of agile micro quadrotors. In

*Robotics: Science and systems (RSS)*.Kuwata, Y., & How, J. P. (2007). Robust cooperative decentralized trajectory optimization using receding horizon MILP. In

*Proceedings of the 2007 American control conference*(pp. 11–13).Lee, T., Leoky, M., & McClamroch, N. H. (2010). Geometric tracking control of a quadrotor UAV on SE (3). In

*49th IEEE conference on decision and control (CDC), 2010*(pp. 5420–5425).Lupashin, S., Schöllig, A., Hehn, M., & D’Andrea, R. (2011). The Flying Machine Arena as of 2010. In:

*2011 IEEE international conference on robotics and automation (ICRA)*(pp. 2970–2971).Mahony, R., Kumar, V., & Corke, P. (2012). Multirotor aerial vehicles: modeling, estimation, and control of quadrotor.

*IEEE Robotics & Automation Magazine*,*19*(3), 20–32.Mcfadyen, A., Corke, P., & Mejias, L. (2012). Rotorcraft collision avoidance using spherical image-based visual servoing and single point features. In:

*2012 IEEE/RSJ international conference on intelligent robots and systems (IROS)*, IEEE (pp. 1199–1205).Mellinger, D., & Kumar, V. (2011). Minimum snap trajectory generation and control for quadrotors. In

*2011 IEEE international conference on robotics and automation (ICRA)*(pp. 2520–2525).Mellinger, D., Kushleyev, A., & Kumar, V. (2012). Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams. In:

*2012 IEEE international conference on robotics and automation (ICRA)*(pp. 477–483).Michael, N., Mellinger, D., Lindsey, Q., & Kumar, V. (2010). The GRASP multiple micro-UAV testbed.

*IEEE Robotics & Automation Magazine*,*17*(3), 56–65.Ogren, P., Fiorelli, E., & Leonard, N. E. (2004). Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment.

*IEEE Transactions on Automatic Control*,*49*(8), 1292–1302.Pivtoraiko, M., & Kelly, A. (2005). Generating near minimal spanning control sets for constrained motion planning in discrete state spaces. In

*2005 IEEE/RSJ international conference on intelligent robots and systems, 2005 (IROS 2005)*(pp. 3231–3237).Raghunathan, A. U., Gopal, V., Subramanian, D., Biegler, L. T., & Samad, T. (2004). Dynamic optimization strategies for three-dimensional conflict resolution of multiple aircraft.

*Journal of Guidance, Control, and Dynamics*,*27*(4), 586–594.Rufli, M., Alonso-Mora, J., & Siegwart, R. (2013). Reciprocal collision avoidance with motion continuity constraints.

*IEEE Transactions on Robotics*,*29*, 899–912.Schwager, M., Julian, B. J., Angermann, M., & Rus, D. (2011). Eyes in the sky: Decentralized control for the deployment of robotic camera networks.

*Proceedings of the IEEE*,*99*(9), 1541–1561.Shim, D. H., Kim, H. J., & Sastry, S. (2003). Decentralized nonlinear model predictive control of multiple flying robots. In

*Proceedings of the 42nd IEEE conference on decision and control, 2003*(pp. 3621–3626).van den Berg, J., Guy, S. J., Lin, M., & Manocha, D. (2009). Reciprocal n-body collision avoidance. In

*International symposium on robotics research (ISRR)*.Van Nieuwstadt, M. J., & Murray, R. M. (1997). Real time trajectory generation for differentially flat systems.

*International Journal of Robust and Nonlinear Control*,*8*, 995–1020.

## Author information

## Electronic supplementary material

Below is the link to the electronic supplementary material.

Supplementary material 1 (mp4 14075 KB)

## Appendices

### Appendix 1: Extension to homogenous group of agents

This appendix describes an extension of the local motion planning method of Sect. 4 towards an homogeneous group of agents (having the same control parameters).

With the assumption that all agents have the same control parameters (\(\omega _0, \omega _1\)) for the local trajectories (Sec. 7.4), the \(\varepsilon \) enlargement of the agents is not required. This is achieved by substituting the \(VO\) constraint (Constraint 3 of Sec. 4.5) by a 3D extension of the control obstacle \(C^M-CO\) introduced by Rufli et al. (2013). The \(C^M-CO\) characterizes the (\(n\)-differentiable) control trajectories in collision and is computed by formulating in relative candidate reference velocity space the full trajectories (Eq. (22) for \(M=2\)). Linearization of the constraint is still required and is done with respect to the current velocity. The algorithms described in this paper can be applied thereafter.

Relying on the concept of differential flatness for a quadrotor vehicle (Mellinger and Kumar 2011), if \(M=5\) is used, the quadrator would, in theory, be able to perfectly track the control trajectory. In this case the full state (up to the fifth derivative) shall be known for all agents.

### Appendix 2: Equations repulsive velocity

For the repulsive velocity field of Fig. 4, left, the repulsive velocity for agent \(i\in {\mathcal {A}}\) is given by

where \(V_r\) is the maximum repulsive force and \(D_r\), \(D_h\) the preferred minimal inter-agent distance in the X–Y plane and in the Z component respectively.

### Appendix 3: Equations linearization of VO

Denote \(\bar{h}_{ij} = \bar{h}_i + \bar{h}_j\) and \(\bar{r}_{ij} = \bar{r}_i + \bar{r}_j\).

The non-convex constraint \({\mathbb {R}}^3 \setminus {\mathcal {VO}}_{ij}^{\tau }\) is linearized to obtain a convex problem. For an approximation with five linear constraints, as in Fig. 7, the linear constraints are given by

where \(H_{ij}^1\) and \(H_{ij}^2\) represent avoidance to the right / left, \(H_{ij}^3\) and \(H_{ij}^4\) above / below and \(H_{ij}^5\) represents a head-on maneuver, which remains collision-free up to \(t=\tau \).

## Rights and permissions

## About this article

### Cite this article

Alonso-Mora, J., Naegeli, T., Siegwart, R. *et al.* Collision avoidance for aerial vehicles in multi-agent scenarios.
*Auton Robot* **39, **101–121 (2015). https://doi.org/10.1007/s10514-015-9429-0

Received:

Accepted:

Published:

Issue Date:

### Keywords

- Collision avoidance
- Reciprocal
- Aerial vehicle
- Quadrotor
- Multi-robot
- Multi-agent
- Motion planning
- Dynamic environment