Nonlinear MPC for Collision Avoidance and Controlof UAVs With Dynamic Obstacles

This article proposes a Novel Nonlinear Model Predictive Control (NMPC) for navigation and obstacle avoidance of an Unmanned Aerial Vehicle (UAV). The proposed NMPC formulation allows for a fully parametric obstacle trajectory, while in this article we apply a classification scheme to differentiate between different kinds of trajectories to predict future obstacle positions. The trajectory calculation is done from an initial condition, and fed to the NMPC as an additional input. The solver used is the nonlinear, non-convex solver Proximal Averaged Newton for Optimal Control (PANOC) and its associated software OpEn (Optimization Engine), in which we apply a penalty method to properly consider the obstacles and other constraints during navigation. The proposed NMPC scheme allows for real-time solutions using a sampling time of 50 ms and a two second prediction of both the obstacle trajectory and the NMPC problem, which implies that the scheme can be considered as a local path-planner. This paper will present the NMPC cost function and constraint formulation, as well as the methodology of dealing with the dynamic obstacles. We include multiple laboratory experiments to demonstrate the efficacy of the proposed control architecture, and to show that the proposed method delivers fast and computationally stable solutions to the dynamic obstacle avoidance scenarios.



There are no comments yet.


page 1


A Fast Computational Optimization for Control and Trajectory Planning for Obstacle Avoidance between Polytopes

Obstacle avoidance between polytopes is a challenging topic for optimal ...

Trajectory Planning for Autonomous Parking in Complex Environments: A Tunnel-based Optimal Control Approach

This paper proposes a fast and accurate trajectory planning algorithm fo...

On the Dual Implementation of Collision-Avoidance Constraints in Path-Following MPC for Underactuated Surface Vessels

A path-following collision-avoidance model predictive control (MPC) meth...

Joint Vision-Based Navigation, Control and Obstacle Avoidance for UAVs in Dynamic Environments

This work addresses the problem of coupling vision-based navigation syst...

Moving Obstacle Avoidance: a Data-Driven Risk-Aware Approach

This paper proposes a new structured method for a moving agent to predic...

Optimization-based Trajectory Tracking Approach for Multi-rotor Aerial Vehicles in Unknown Environments

The goal of this paper is to develop a continuous optimization-based ref...

Online Weight-adaptive Nonlinear Model Predictive Control

Nonlinear Model Predictive Control (NMPC) is a powerful and widely used ...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Nowadays, Unmanned Aerial Vehicles (UAVs) demonstrate their promising capabilities in numerous application domains, such as infrastructure inspection [14], underground mine navigation [16], search and rescue mission [24], delivering first-aid or defibrillators in case of an accident [25], etc. In most of these scenarios, the interaction of the UAV with the environment is not considered and it is assumed that the surrounding obstacles remain static. However, to guarantee the overall safety and success of the mission and for demonstrating the capabilities of UAVs in close interaction with humans and in field trials, extending the classic notion of obstacle avoidance into the case of moving obstacles should be considered.

To be more specific, to deploy the UAVs in complex missions, such as urban environments, unstructured and unknown or constantly varying application scenarios with e.g. the existence of multiple moving obstacles and other robotic agents, it is essential to guarantee safe operations and zero incidents with inhabitants. In the urban environment, the UAV may interact with other aerial and ground vehicles, as well as inhabitants, which are all constantly moving. Thus, this article proposes a novel collision avoidance approach for dynamic obstacles, included in the control layer, in order to guarantee a robust and online collision avoidance agile capability. As it will be presented, in the novel proposed framework, the predicted trajectory of the obstacle is fed to the Nonlinear Model Predictive Control (NMPC) for guaranteeing for a collision free path.

I-a Background and Motivation

In the related scientific literature the majority of research has been done for the case of dynamic / moving obstacles, mainly in the context of an autonomous car  [6], since it is required to ensure collision-free paths in the urban environment. These methods use a wide array of stochastic prediction models [1] or hypothesis based models [5] for the consideration of moving obstacles. For the case of an UAV, there have been few works focusing in the area of dynamic obstacle avoidance. The general consensus is to consider the obstacles / environment static and plan a path around it. For this approach, there is a myriad of different control approaches e.g. [13]. Reactive avoidance schemes include the widely used potential field methods [3, 12] and on-line graph-search methods, such as *ADA [7].

The NMPC approach to path-planning has started to gain more traction in the field of UAVs as well [22, 4], once the issues of computation time was solved. These methods are often used in conjunction with other forms of obstacle avoidance, such as the potential fields [15], but there exist also examples of obstacle avoidance schemes using a pure NMPC structure [20]. In [10] a NMPC was developed that considered a linear prediction model to ensure collision-free paths between agents in a collaborative scheme, but it is also paired with a potential field. The advantage of the NMPC scheme, over other obstacle avoidance schemes, is the ability to generate a collision-free trajectory based purely on the nonlinear kinematics of the UAV, where each point in the trajectory is described by a series of control inputs. In this specific application scenario, the main issue for the NMPC approach for UAVs, is the solver time.

The adopted solver used in this article is the Proximal Averaged Newton for Optimal Control (PANOC) [19, 23]

and the associated open-source software OpEn (Optimization Engine)

[21]. As the name implies OpEn uses a Newton-type method and is specifically designed for optimal control problems, while it uses the same oracle as the projected gradient method [17], has a very low memory and computational footprint, and relies only on simple algebraic operations. OpEn also employs a penalty method [8] for the consideration of equality constraints. The practicality and speed of OpEN makes it the prefect candidate for real-time applications of an NMPC scheme.

I-B Contributions

Based on the aforementioned state of the art, the first contribution of this article is in the coupling of the dynamic collision avoidance with the control layer, a concept that has never been presented before, to the authors best knowledge. In this novel approach, the NMPC is implemented for performing set-point tracking, while the nonlinear dynamic of the UAV is considered and proper constraint formulation allows the consideration of the dynamic obstacles. The overall proposed framework can be used as a baseline controller for guaranteeing collision avoidance and enables a larger application use for the UAVs. The second contribution is in defining the trajectory of the dynamic obstacles by classification and predicting the trajectory of an obstacle based on a discrete dynamics, feeding it directly to the optimizer as a parameter. In this way, the changes in the motion of the obstacle are considered in the prediction horizon of the NMPC formulation. The third contribution stems from the multiple laboratory experiments that display the efficacy of the proposed method for different scenarios of dynamic obstacle avoidance. This demonstration has significant novelty and impact as an enabler for a continuation of research efforts towards the real-life application of UAVs in dynamic environments.

I-C Outline

The rest of the article is structured as follows. Initially, the kinematic model of the UAV is presented in Section II-A, followed by the presentation of the corresponding objective function and the formulation of the obstacle constraints and trajectories in Sections II-B and II-C respectively. A brief description of the optimization framework is presented in Section II-D, and the trajectory classification scheme is presented in II-E. The experimental set-up and full control architecture is described in III-A, while multiple experimental scenarios with related results and discussion that show the efficiency of the proposed framework are presented in Section III-B. Finally, Section IV concludes the article by summarizing the findings and offering directions for future research.

Ii Methodology

Ii-a UAV Kinematics

The UAV coordinate systems are depicted in Figure 1, where denote the body-fixed coordinate system, while

denote the global coordinate system. In this article the states are defined in a yaw-compensated global frame of reference. The six degrees of freedom (DoF) for the UAV are defined by the set of equations (

1), while the full derivation of the adopted model can be found in [11].

Fig. 1: Utilized coordinate frames, where and denote the world and body coordinate frames respectively.

where is the position and is the linear velocity in the global frame of reference. and are the roll and pitch angles along the and axes respectively. Moreover, is a rotation matrix that describes the attitude in Euler form. , and

are the reference inputs to the system in roll, pitch and the total thrust. The above model assumes that the acceleration depends only on the magnitude and angle of the thrust vector, produced by the motors, as well as the linear damping terms

and the gravitational acceleration . The attitude terms are modeled as a first-order system between the attitude (roll/pitch) and the references , , with gains and time constants . These terms model the closed-loop behavior of a low-level controller, which also implies that the UAV is equipped with a lower-level attitude controller that takes thrust, roll and pitch commands and provides motor commands for the UAV.

Ii-B Cost Function

We denote the state vector by , and the corresponding control actions as . The system dynamics of the UAV is discretized with a sampling time of using the forward Euler method to obtain . This discrete model is used as the prediction model of the NMPC. The prediction is done with a receding horizon e.g., the prediction considers specified number of steps into the future. We denote this as the prediction horizon, , of the NMPC. By associating a cost to a configuration of states and inputs at the current time and in the prediction, a nonlinear optimizer is tasked with finding an optimal set of control actions, defined by the cost minimum of this cost function. Let denote the predicted state at time step , produced at the time step . The corresponding control actions are denoted by . Let us also denote and the full predicted states and corresponding control inputs along the prediction horizon correspondingly. The goal of the controller is to make the states reach the prescribed set points and deliver smooth control inputs. Thus we formulate the cost function as:


where are positive definite weight matrices for the states, inputs and input rates respectively. In (2), the first term denotes the state cost, which penalizes deviating from a certain state reference . The second term denotes the input cost that penalizes a deviation from the steady-state input i.e. the inputs that describe hovering. Finally, to enforce smooth control actions, a third term is added that penalizes changes in successive inputs. Note that the first such penalty, , depends on the previous control action.

Ii-C Obstacle Definition and Constraints

Following the constraint formulation structure for OpEn used in [20], while also keeping the constraints fully parametric so that their positions and size is part of the input fed to the NMPC scheme, we use the function . This allows us to formulate the constraints as equality expressions such that implies that the constraint is satisfied. We can use this formulation to express a constrained area by choosing h as an expression that is positive inside of the constrained area and negative outside of it. For the experimental scenarios described in III-B a simple spherical obstacle fits very well to the needs. Moreover, to guarantee bounds on changes in control actions, a constraint on the control input variations will be also established. Finally, it should be highlighted that all the underlying constraints are considered in the full control horizon N to account for the constraints at all the predicted future time steps.

Ii-C1 Spherical Obstacle

The spherical obstacle represents the dynamic obstacle that is thrown at/approaching the UAV, with an included safety distance. For now the obstacle is defined with an arbitrary trajectory, i.e. the position of the sphere at each time step in the prediction is set as an input to the solver. Taking inspiration from the collision avoidance scheme in [10] we increase the radius of the obstacle along the prediction horizon. This is needed due to the fact that with imperfect measurements and prediction models, the further the prediction is, the larger the errors in the obstacle trajectory will be, which is unavoidable. The obstacle constraint is defined as:


where . The obstacle positions are the world-frame coordinates of the center of the sphere, is the radius of the obstacle and is an extra safety radius. This implies that as long as the UAV position lies outside the sphere (3) is zero and can thus be stated as an equality constraint. To extend this notion to a dynamic obstacle, we form a separate constraint for each predicted time step. Let denote the vectors of size that describe the obstacle trajectory, denote a linearly increasing safety radius along , and denote the full predicted UAV positions at time step . Then the vector of obstacle constraints can be formulated as:


where , such that the constraints are satisfied if the UAV does not enter the spherical obstacle, as described by the obstacle trajectory, at any of the predicted UAV positions. As such, the trajectory of the obstacle is fully described and parametrized by . The NMPC is not set for a specific trajectory of the obstacle, and for the identification and prediction of the dynamic obstacle’s trajectories the constraint-formulation is agnostic to the method of trajectory prediction.

Ii-C2 Control Input Rate

Rapidly moving out of the trajectory of an incoming obstacle it can result in an aggressive or oscillatory behavior of the control inputs and thus we impose a constraint on the successive differences of control actions. The purpose of this constraint is to set a bound on the magnitude of the change in control inputs and , which is done by an upper and a lower bound. This can be written as an equality constraints as:


Additionally we also form the same constraint for , with and as the maximum change in input per time step.

Ii-C3 Input constraints

Finally, we also directly apply constraints on the control inputs. Since the NMPC is to be used with a real UAV, hard bounds on reference angles must be considered, as a low-level controller will only be able to stabilize the attitude within a certain range. Since the thrust of a UAV is limited, such hard bounds must also be applied to the thrust input, T. Thus we define bounds on inputs as:


Ii-D Optimization

The NMPC problem is solved by PANOC [19, 20, 21], while a penalty method is applied for the consideration of equality constraints. OpEn solves general parametric optimization problems on the form:

subject to: (7b)

where is a Lipschitz-differentiable function and is a vector-valued mapping so that is a Lipschitz-differentiable function. The decision variable and parameter are denoted by and respectively. Based on the cost function and constraints outlined in II-B and II-C we can formulate the NMPC problem with spherical obstacles with parameterized trajectories, as:

s. t.:

This can be fit into the OpEn framework by performing a single-shooting of the cost function via decision variable and define by the input constraints (6). We also define to cast the equality constraints (3) and (5). The parameter is set to include initial conditions, references and the obstacle trajectory. For the consideration of the equality constraints, a quadratic penalty method is applied. By formulating the problem as , where is a positive penalty parameter, the PANOC algorithm can be applied to the problem. In the penalty method an optimization problem, where the constraints are mapped to the cost-domain, is re-solved multiple times with an increasing penalty parameter associated to the constraints, while using the previous solution as the initial guess. This method gradually moves the cost-minima until non of the constraints are violated, or rather until a specified tolerance is met. In III-B a maximum of four penalty method iterations are applied.

Ii-E Trajectory Classification

While many different types of trajectories of obstacles can be encountered in the urban environment, and the NMPC formulation allows for trajectories of arbitrary shape, in this article we will limit the trajectories to three categories, namely: linear motion, projectile motion, or static obstacles. As such, a direct prediction model to predict the future positions of the obstacle could be utilized in case that it is possible to compute its trajectory class. General movements, such as pedestrians or cars in the urban environment, are often moving from starting point to destination, i.e. linear movement, while objects thrown or rock-falls follow the projectile motion equations. Additionally we include a static obstacle class, without any movement (). Linear motion is described by:


where are the velocities of the obstacle and no forces are acting on the obstacle. The projectile-motion trajectory is defined by:


where, much like in (1), are linear aerodynamic damping terms. The buoyancy force of the obstacle is considered small enough to ignore. Equations (10), (9) are then discretized by forward Euler using the same sampling time as the controller/prediction model, . For the obstacle states , we thus have the discrete-predictive form


where denoted the discrete prediction model of the trajectory, and can be iterated indefinitely. Similarly for the backwards prediction of . In the discrete prediction model of the projectile-motion we also include a condition for bouncing, with a much-simplified collision interaction with a coefficient of restitution applied on the velocities (assuming the ground is flat) as the sphere hits the ground to model the energy-loss in the collision.

The classification is done by comparing the M last measured position and velocity terms to a backwards prediction based on (9) and (10) iterating from the current measured state, . Using the same notation as for the prediction of the NMPC, as denotes the current measurement let denote the predicted obstacle state steps back in time. Also denote the vector of previous measurements in position and velocity of the obstacle as and respectively. We measure the error, , as:


Equation (12) is evaluated for the three different classes of trajectories and the class that generates the lowest error is chosen for the trajectory prediction. This is run for every new measurement of the obstacle and thus the trajectory of a single obstacle is allowed to change during the movement, and the predicted trajectory only depends on the current measurement. We can then form the full obstacle parameter by computing from by iterating the discrete prediction model in (11) to produce . The radii are, in this paper, provided by the operator.

Iii Results

Iii-a Experimental Set-up

For all the following experiments we use a Vicon Motion-Capture System to track the UAV and the obstacle. All the state data used in the NMPC, namely the UAV and the obstacle

, are provided by Vicon using a complementary filter to estimate the velocities. The chosen platform is the Crazyflie 2.0 Nano Quadcopter (and is also seen in Figure

1), which is a small and lightweight quadcopter. This type of smaller platform was chosen for safety reasons due to the nature of the experiments where obstacles are thrown at the UAV. The Crazyflie has no on-board computer and thus all the computation is done on a remote laptop, namely a Lenovo 430 Thinkpad with a 3rd gen Core i5 2.60 GHz and 4GB of RAM. To allow communication and low-level attitude control of the Crazyflie we use the open source Robot Operating System (ROS) [18] package developed for the Crazyflie [9]. As can be seen in Figure 2 the trajectory is generated outside of the NMPC module and fed to the optimizer as an additional input, together with other user-specified parameters. We also perform a basic thrust-mapping, since [9] accepts an input , where we assume has a quadratic relation to the thrust. The low-level controller also accepts a yaw-rate command, and as seen in Figure 2 we use a basic P-controller with gain to keep the yaw, , at zero. Despite forcing the yaw to zero we also covert the control inputs from global to local frame.

Fig. 2: Proposed Control Architecture. The Vicon motion capture system (and a median filter for velocity estimation from the position data) provides state data for the UAV and the obstacle

. The obstacle data is used to classify the trajectory and generate a predicted trajectory based on the measured initial condition. The UAV state data and the obstacle trajectory is fed to the NMPC module as the solver parameter which also include

. The NMPC generates control inputs and , which after the relevant mapping are fed to the low-level attitude controller.

Iii-B Laboratory Experiments

For the presented experiments, the corresponding model parameters that are described as in (1) are chosen as , , in order to approximately match the response of a low-level controller acting on an UAV, while damping terms are set to . Additionally, g is set to , the control horizon is set to and (the same as in [20]), which implies a prediction of two seconds. The selection of horizon is based on the trade-off between computational complexity and considering the obstacle in time to avoid collision. The prediction horizon (in combination with ) is the limiting factor to how early the system can react to incoming obstacles. The weights in (2) are chosen as , , , while the constraints on control inputs are (in SI-units) as and .

Additionally the constraints on change in input described in (5) are chosen as and . The safety radius is set to 0.0 m at and is increased linearly to 0.2 m at , while the obstacle radius is specified for each experiment. The classification is done with the number of backwards measurements = 5.

Due to the nature of the following experiments, with multiple simultaneously moving objects, we recommend also watching the video summary of the experiments to get a more clear view of the set-up and results. The video includes a brief comparison with other obstacle avoidance methods, and the four experimental scenarios discussed in the sequel. It can be found at: Each experiment using the proposed method was performed 5-10 times without collisions, and the data presented in this section and in the video is based purely on the best-looking trajectories.

Iii-B1 Position hold while avoiding projectile

The task of the UAV is to hold position while avoiding any incoming obstacles, which is a projectile thrown at the UAV. We will use this scenario to evaluate how popular methods, that do not include a trajectory prediction, performs at avoiding an incoming projectile. While many methods such as the artificial potential fields have been evaluated for dynamic obstacles [2], it is under the assumption that the UAV is fast enough to respond to the moving obstacle while still considering it static (but updating its position on-line). Since we are only interested in avoiding one point-like incoming projectile we form the repulsive force as , where , is the radius of influence of the potential field, and is the repulsive constant which is chosen very large for an aggressive response, while the attractive force is the hold-position position reference. We also compare with the same NMPC-constraint method presented in this paper (being similar to [20], using a spherical obstacle), but considering the obstacle static. To assist these methods we choose the radius of influence and obstacle radius respectively to , which is much larger than the needed safety distance to avoid a static obstacle, while tuning the reference-following controller of the potential field to be as aggressive as possible, also relaxing input-rate constraints (5) for the NMPC for a faster response.

While maintaining distance to a slowly moving obstacle (also shown in the video), both these methods fail at avoiding collisions with the projectile-motion obstacle as shown in Figure 8. There is simply not enough time from the obstacle entering the area of influence of these controllers until collision with the UAV, to initiate avoidance in time, nor do the controllers have a notion of where the obstacle’s future position are and as such their avoidance maneuver might move them along the trajectory of the obstacle.

Using the predictive method and trajectory classification described in this paper and setting the obstacle radius to 0.4 m, the approximate distance required to safely not result in a collision, we test the proposed controller in the same scenario. Figure 3 shows the paths of the projectile and UAV. The obstacle is thrown at approximately 0.7 s, as seen by the relative distances in Figure 8, and the avoidance maneuver starts at approximately 0.8 s in Figure 4. Figure 3

also displays the predicted trajectory at the moment the avoidance maneuver starts (note: the bounce condition is not applied here, see


The prediction error of the obstacle, used for trajectory classification, can be found in Figure 10, as well as the point where the predicted trajectory is determined to be on a collision course with the UAV (within the prediction horizon), which is also when the controller initiates the avoidance. We can see that the NMPC computes control inputs to avoid the obstacle as soon as the trajectory is classed as projectile-motion. The classification successfully identifies the projectile-motion trajectory and the obstacle avoidance is initiated in time to successfully avoid the incoming projectile. Figure 9 displays the solver time, which peaks at 40 ms. The minimum distance in this experiment was 0.51 m (due to ). Although not a perfect hit, the minimum distance of the initial position of the UAV and the projectile was around 0.17 m which would result in a collision since the distance is calculated from the center of the obstacle to the center of the UAV.

At exactly one second into the experiment the trajectory classification fails for one instant, as seen in Figure 10, and classifies the trajectory as linear, and can be seen in Figure 8 where the penalty method iterations are not applied and such the solver time drastically decreases, since the obstacle prediction is no longer on a collision course. In Figure 4 a problem regarding remotely controlling the UAV is also shown, and will persist in the upcoming experiments as well. There is a delay of approximately 0.1 - 0.15 s between the control input and the state changing, which is the nature of a remotely controlled UAV and can not be avoided.

Fig. 3: Path of UAV and dynamic obstacle during projectile-motion experiment.
Fig. 4: Control inputs during projectile-motion experiment.

Iii-B2 Position hold while avoiding pedestrian

The task is to hold a position, while avoiding approaching obstacles. In this experiment a ”pedestrian” is walking towards the UAV on a direct collision course. The obstacle radius is set to 0.6 m, due to the larger size of the incoming obstacle. The path of the UAV and obstacle is shown in Figure 5, as well as the predicted trajectory starting from the time step that the trajectory is identified and within the prediction horizon, and the avoidance maneuver starts. As in the previous case, looking at Figure 8 and Figure 10 the pedestrian starts to move at 0.3 s into the experiment, while the controller reacts to initiate the avoidance maneuver at around 0.4 s. Figure 8 shows that the minimum distance is 0.59 m, which implies that the UAV cleanly avoids the collision. The solver time is also found in Figure 9 and peak to a maximum of 29 ms.

Fig. 5: Path of UAV and dynamic obstacle during linear motion.

Iii-B3 Bounce condition

The bouncing ball is often seen as an interesting hybrid system. Thus to show the power of this type of NMPC structure, where the trajectory of the obstacle is provided by an external source, we include an experiment of the avoidance of a bouncing ball. As in the previous two cases the task of the UAV is to hold position, while avoiding incoming obstacles, and the obstacle radius is set to 0.4 m. In this experiment the obstacle is thrown on a trajectory such that it would impact the UAV after the first bounce (approximately 2.1 s the experiment in Figure 8). The obstacle is thrown at approximately 0.25 s, while the controller reacts at 0.35 s, also seen in Figure 10. This displays that even a simplified trajectory model still results in a good enough prediction of the obstacle path, especially with the inclusion of an increasing safety radius along the prediction. The predicted trajectory of the obstacle, based on the initial condition at the time when the avoidance maneuver starts, is displayed in Figure 6, together with the measured path of the obstacle and UAV. The UAV successfully avoids the obstacle with a minimum distance of 0.38 m which can be seen in Figure 8, while the solver time peaks to 33 ms. Such a small constraint violation is expected due to solver tolerances and imperfect measurements.

Fig. 6: Path of UAV and dynamic obstacle during experiment with bouncing ball.

Iii-B4 Multiple Obstacles

Finally, we evaluate the proposed method in terms of avoiding multiple dynamic obstacles. The experimental set-up is such that a separate UAV is set on a collision course with the avoiding UAV, while a projectile is simultaneously thrown at it, both set with . The trajectory classification and prediction scheme is applied on separate measurements of the two obstacles, but is otherwise used identically as in the single obstacle cases. The trajectories of the two UAVs and the projectile are shown in Figure 7, while the distances between the avoiding UAV and the approaching UAV and obstacle are shown in Figure 8. The safety distances are maintained, with a minimum distance of 0.45 m and 0.42 m respectively for the two incoming obstacles. It should be noticed also how the avoiding UAV keeps the safety distance for a prolonged time to the approaching UAV while it is also avoiding the incoming projectile. In the video results, the avoidance maneuver can also be seen to start as soon as the obstacle-UAV starts its motion. The solver time in Figure 9 peaks at 35 ms, which is similar to the single-obstacle case.

Fig. 7: Paths of (avoiding) UAV, projectile-motion obstacle, and UAV obstacle.
Fig. 8: Distance measurements between the UAV and obstacles for the comparisons and the four described obstacle avoidance scenarios.
Fig. 9: Solver time of the NMPC for the four obstacle avoidance scenarios.
Fig. 10: The prediction error, for the dynamic obstacle, during relevant time samples for the first three three experiments. The class with the lowest error is chosen for the prediction of the obstacle trajectory.

Iv Conclusions

In this paper we proposed a novel path-planning and obstacle avoidance scheme that has the ability to handle dynamic obstacles using a NMPC architecture. To supplement this we also include a simple trajectory classification scheme to better display the applicability of the controller in a more general approach, so that the same scheme can be used in all experimental scenarios. The proposed scheme of NMPC and trajectory classification successfully provides collision-free paths in all the considered cases. The online optimization problem is solved within the required real-time restrictions of 50 ms without violation of the established obstacle or input constraints. The limitations of this method are: a) the fact that the overall performance is based on the reliance on classification of trajectories, and b) using an explicit prediction of future obstacle positions. If the predictive scheme fails or involve too large errors, the UAV might completely ignore an obstacle on a collision-course and even for our limited study of trajectories our scheme momentarily classifies the trajectory incorrectly.

This paper displays a novel and interesting approach to obstacle avoidance of UAVs, that can be much expanded on by further research. This future work includes more general trajectory identification such as curve-fitting or estimating the forces acting on the obstacle, and of course removing the reliance of a motion-capture system for obstacle detection. The last part would include extracting the position and velocity of obstacles using, for example, stereo-cameras or 3D lidars. Although the trajectory classification scheme never failed in a way that resulted in collisions in the around 40 performed experiments, further statistical analysis on how such a scheme performs in more difficult scenarios should also be considered.

Additionally, further analysis on how the complexity of the NMPC problem scales with more obstacles and how that relates to the solver time, to see at what point it is more appropriate to not solve for obstacle avoidance directly in the control layer. This type of control structure is, in the authors opinion, an interesting path towards using UAVs in urban dynamic environments or any environment where collision avoidance is of great importance to ensure the safety of persons and vehicles.


  • [1] M. Althoff, O. Stursberg, and M. Buss (2009) Model-based probabilistic collision detection in autonomous driving. IEEE Transactions on Intelligent Transportation Systems 10 (2), pp. 299–310. Cited by: §I-A.
  • [2] A. Budiyanto, A. Cahyadi, T. B. Adji, and O. Wahyunggoro (2015) UAV obstacle avoidance using potential field under dynamic environment. In 2015 International Conference on Control, Electronics, Renewable Energy and Communications (ICCEREC), pp. 187–192. Cited by: §III-B1.
  • [3] D. Droeschel, M. Nieuwenhuisen, M. Beul, D. Holz, J. Stückler, and S. Behnke (2016) Multilayered mapping and navigation for autonomous micro aerial vehicles. Journal of Field Robotics 33 (4), pp. 451–475. Cited by: §I-A.
  • [4] I. K. Erunsal, A. Martinoli, and R. Ventura (2019) Decentralized nonlinear model predictive control for 3d formation of multirotor micro aerial vehicles with relative sensing and estimation. In 2019 International Symposium on Multi-Robot and Multi-Agent Systems (MRS), pp. 176–178. Cited by: §I-A.
  • [5] D. Ferguson, M. Darms, C. Urmson, and S. Kolski (2008) Detection, prediction, and avoidance of dynamic obstacles in urban environments. In 2008 IEEE Intelligent Vehicles Symposium, pp. 1149–1154. Cited by: §I-A.
  • [6] T. Fraichard and A. Scheuer (1994) Car-like robots and moving obstacles. In Proceedings of the 1994 IEEE International Conference on Robotics and Automation, pp. 64–69. Cited by: §I-A.
  • [7] L. Heng, L. Meier, P. Tanskanen, F. Fraundorfer, and M. Pollefeys (2011) Autonomous obstacle avoidance and maneuvering on a vision-guided mav using on-board processing. In 2011 IEEE International Conference on Robotics and Automation, pp. 2472–2477. Cited by: §I-A.
  • [8] B. Hermans, P. Patrinos, and G. Pipeleers (2018) A penalty method based approach for autonomous navigation using nonlinear model predictive control. IFAC-PapersOnLine 51 (20), pp. 234 – 240. Cited by: §I-A.
  • [9] W. Hönig and N. Ayanian (2017) Flying multiple uavs using ros. In Robot Operating System (ROS): The Complete Reference (Volume 2), A. Koubaa (Ed.), pp. 83–118. External Links: ISBN 978-3-319-54927-9, Document, Link Cited by: §III-A.
  • [10] M. Kamel, J. Alonso-Mora, R. Siegwart, and J. Nieto (2017) Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 236–243. Cited by: §I-A, §II-C1.
  • [11] M. Kamel, T. Stastny, K. Alexis, and R. Siegwart (2017) Model predictive control for trajectory tracking of unmanned aerial vehicles using robot operating system. In Robot operating system (ROS), pp. 3–39. Cited by: §II-A.
  • [12] C. Kanellakis, S. S. Mansouri, G. Georgoulas, and G. Nikolakopoulos (2018) Towards autonomous surveying of underground mine using mavs. In International Conference on Robotics in Alpe-Adria Danube Region, pp. 173–180. Cited by: §I-A.
  • [13] S. M. LaValle (2006) Planning algorithms. Cambridge university press. Cited by: §I-A.
  • [14] S. S. Mansouri, C. Kanellakis, E. Fresk, D. Kominiak, and G. Nikolakopoulos (2018) Cooperative coverage path planning for visual inspection. Control Engineering Practice 74, pp. 118–131. Cited by: §I.
  • [15] S. S. Mansouri, P. Karvelis, C. Kanellakis, D. Kominiak, and G. Nikolakopoulos (2019)

    Vision-based mav navigation in underground mine using convolutional neural network

    In IEEE Industrial Electronics Society, Cited by: §I-A.
  • [16] S. S. Mansouri, P. Karvelis, C. Kanellakis, D. Kominiak, and G. Nikolakopoulos (2019) Vision-based mav navigation in underground mine using convolutional neural network. In IECON 2019-45th Annual Conference of the IEEE Industrial Electronics Society, Vol. 1, pp. 750–755. Cited by: §I.
  • [17] Y. Nesterov (2018) Lectures on convex optimization. Vol. 137, Springer. Cited by: §I-A.
  • [18] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng (2009) ROS: an open-source robot operating system. In ICRA workshop on open source software, Vol. 3, pp. 5. Cited by: §III-A.
  • [19] A. Sathya, P. Sopasakis, R. Van Parys, A. Themelis, G. Pipeleers, and P. Patrinos (2018) Embedded nonlinear model predictive control for obstacle avoidance using panoc. In 2018 European Control Conference (ECC), pp. 1523–1528. Cited by: §I-A, §II-D.
  • [20] E. Small, P. Sopasakis, E. Fresk, P. Patrinos, and G. Nikolakopoulos (2019) Aerial navigation in obstructed environments with embedded nonlinear model predictive control. In 2019 18th European Control Conference (ECC), pp. 3556–3563. Cited by: §I-A, §II-C, §II-D, §III-B1, §III-B.
  • [21] P. Sopasakis, E. Fresk, and P. Patrinos (2019) Optimization Engine. External Links: Link Cited by: §I-A, §II-D.
  • [22] T. J. Stastny, A. Dash, and R. Siegwart (2017) Nonlinear mpc for fixed-wing uav trajectory tracking: implementation and flight experiments. In AIAA Guidance, Navigation, and Control Conference, pp. 1512. Cited by: §I-A.
  • [23] L. Stella, A. Themelis, P. Sopasakis, and P. Patrinos (2017) A simple and efficient algorithm for nonlinear model predictive control. In 2017 IEEE 56th Annual Conference on Decision and Control (CDC), pp. 1939–1944. Cited by: §I-A.
  • [24] T. Tomic, K. Schmid, P. Lutz, A. Domel, M. Kassecker, E. Mair, I. L. Grixa, F. Ruess, M. Suppa, and D. Burschka (2012) Toward a fully autonomous uav: research platform for indoor and outdoor urban search and rescue. IEEE robotics & automation magazine 19 (3), pp. 46–56. Cited by: §I.
  • [25] J. K. Zègre-Hemsey, B. Bogle, C. J. Cunningham, K. Snyder, and W. Rosamond (2018) Delivery of automated external defibrillators (aed) by drones: implications for emergency cardiac care. Current cardiovascular risk reports 12 (11), pp. 25. Cited by: §I.