Model predictive cooperative localization control of multiple UAVs using potential function sensor constraints

The global localization of multiple mobile robots can be achieved cost efficiently by localizing one robot globally and the others in relation to it using local sensor data. However, the drawback of this cooperative localization is the requirement of continuous sensor information. Due to a limited sensor perception space, the tracking task to continuously maintain this sensor information is challenging. To address this problem, this contribution is presenting a model predictive control (MPC) approach for such cooperative localization scenarios. In particular, the present work shows a novel workflow to describe sensor limitations with the help of potential functions. In addition, a compact motion model for multi-rotor drones is introduced to achieve MPC real-time capability. The effectiveness of the presented approach is demonstrated in a numerical simulation, an experimental indoor scenario with two quadrotors as well as multiple indoor scenarios of a quadrotor obstacle evasion maneuver.


Introduction
During the last years, there has been a dramatic increase in the use of Unmanned Aerial Vehicles (UAVs) for all kind of applications such as surveillance, aerial photography, transport, etc. However, the fast dynamics of these systems and the extended operational space makes their autonomous piloting a challenging task. The recent development (Wingfield 2016) of applications for such systems targets not only autonomous flying of single UAVs, but also the coordinated interaction of multiple UAVs or of UAVs and ground robots. One essential task herein is the precise localization of these UAVs and robots in their environment.
The precise global navigation of UAVs though typically requires expensive specialized equipment such as differen-1 SnT -Interdisciplinary Centre for Security, Reliability and Trust, University of Luxembourg, 6, rue Richard Coudenhove-Kalergi, 1359 Luxembourg, Luxembourg Fig. 1 Cooperative localization scenario tial G P S in outdoor applications or laser-/RF-based global positioning systems in indoor applications. For cooperative mobile robot scenarios, one idea is therefore to deploy a reduced number of mobile robots which are equipped with the costly global localization system. The broadcasting of their global position allows all other UAVs to determine their own global position based on the relative position to these UAVs. The determination of the relative position can be achieved by less expensive onboard sensors such as optical sensors as shown in Fig. 1 in a UAV scenario. The major problem of this approach is the necessity to continuously detect and track the globally localized robots with sufficient accuracy. This is further exacerbated by very dynamical UAV scenarios and the onboard sensor limitations. Accordingly, the UAV motion has to be controlled in order to keep the globally localized UAVs within the perception space of the onboard sensors. The major focus of this work is therefore to provide a central control strategy for such cooperative localization scenarios. To limit the scope of this paper, the estimation and localization itself are not addressed and a stable communication channel is assumed. Yet, it should be mentioned that all robot systems considered here do have internal controllers. A communication failure would thus only lead to missing localization data, but not to a complete system failure.
One method to handle such complex control scenarios is model predictive control. MPC allows defining the control objective by means of an optimization problem. This so-called optimal control problem (OCP) is minimizing a given objective function subject to constraints. In general, the computational burden to solve an OCP is high. Hence, the efficiency of the applied solver is limiting the complexity of the controlled real-time scenarios. Nevertheless, a central MPC is well suited to control a small amount of robots. Such a central control simplifies the implementation of safety features and allows the computation of a global OCP solution without considering additional consensus techniques. However, the presented methods here are also applicable for distributed controllers which will be addressed in future work.
One essential factor for fast MPC is a compact description of the system's behavior. To tackle this problem, the first contribution of this work is a novel compact motion model for multi-rotor systems which is described in Sect. 4. This simplistic motion model is based on a semi-nonlinear model for multi-rotor UAVs from previous work (Dentler et al. 2016a). Due to its angle discontinuity problem, as shown experimentally in Sect. 4.1, this previous model is not suitable for sensor tracking as considered within this work. To address this issue, Sect. 4.2 is presenting a model, where the typical orientation description with a single yaw ψ angle is replaced by a direction vector description. For validation, Sect. 4.3 is showing the MPC of a real AR.Drone 2.0 quadrotor based on the derived direction vector model. The same modeling approach can also be adapted to other velocity controlled mobile robot systems.
Another difficulty of MPC is the translation of the considered control scenario into an OCP. For this purpose, the second major contribution of this paper is a workflow that allows to represent sensor limitations in the form of potential functions, as presented in Sect. 5. The considered use case is a sensor tracking scenario which is introduced in Sect. 5.1. In this scenario, a quadrotor is controlled to keep an object within the cone-shaped perception space of the attached sensor. In this context, the sensor limitations are described as inequality constraints. These are subsequently transformed into weakened constraints that just appear in the OCP cost function, as shown in Sect. 5.2. This transformation is executed with unit steps and is recommended to maintain a low complexity which facilitates the mathematical and graphical validation of the resulting potential function. To improve the properties of the potential function for gradient and Newton based OCP-solvers, Sect. 5.3 is dedicated to the introduction of artificial gradients in undesired regions of the potential function. As last step, Sect. 5.4 is showing how the potential function is finally transformed into an analytical function by approximating all unit step functions by sigmoids.
For the experimental validation in the laboratory, additional safety constraints are introduced in Sect. 6, based on the described workflow. This includes further potential functions to avoid collision in Sect. 6.1 and to limit the operational space in Sect. 6.2. Section 7 is finally presenting the validation of the deduced potential functions in the use case scenario and the chosen control parameters. The numerical validation is described in Sect. 7.1, using the simulation environment V -REP. Section 7.2 is subsequently showing the results of the proposed control approach on real AR.Drone 2.0 quadrotors. In the following Sect. 7.3, the influence of obstacles, utilized constraints and different initial conditions is discussed on the basis of a set of collision avoidance scenarios in which a UAV is evading an obstacle while tracking a target.
Finally, conclusion and future perspective are given in Sect. 8.

Related work
In order to solve a cooperative sensor based robot localization, the problem can be divided into a self-localization and a position tracking problem. The present work is focusing on providing a control strategy for the position tracking problem.
To be able to localize a robot by another robot while executing tasks, both robots have to be controlled in a cooperative manner. There is extensive literature regarding such cooperative control scenarios. A comprehensive overview of the subject is given by Gazi and Passino (2011) and (Zhang and Mehrjerdi 2013). Gazi and Passino (2011) is discussing the theory of swarm mechanics and interaction constraints while Zhang and Mehrjerdi (2013) is providing a survey on formation control and coordination of multiple robots. According to Zhang and Mehrjerdi (2013) the coordination and control algorithms can be classified in leader-follower, behavioral based, virtual structure, graph based and potential field based approaches. The leader-follower principle is a well-established approach for non-holonomic mobile robots (Consolini et al. 2008;Cui et al. 2009), particularly regarding decentralized controllers in order to maintain the flexibility of a distributed system. These decentralized controllers are typically based on feedback linearization (Ge and Cui 2002;Desai et al. 1998) or backstepping and can be adapted to different tasks by switching the control law (Das et al. 2002). The same control approaches can also be found in behavioral (Lawton et al. 2003), virtual structure (Beard et al. 2001;Mehrjerdi et al. 2011), graph theory based (Fax and Murray 2004;Cai and Queiroz 2015;Dong and Farrell 2008) and artificial potential (Nascimento et al. 2014(Nascimento et al. , 2013 based control approaches. A more generic tool for multi-robot control is MPC which is based on formulating the control scenario as optimization problem. One typical example of MPC for UAVs is trajectory tracking in formation flight while considering collision avoidance constraints. Examples of a centralized MPC of cooperative control scenarios is given in Shin and Kim (2009) and Alrifaee et al. (2014). In Shin and Kim (2009) a leaderfollower mode is used to perform airplane formation flight with collision avoidance using nonlinear MPC. For this purpose Shin and Kim (2009) compares centralized, sequential decentralized and fully decentralized methods of nonlinear MPC. Alrifaee et al. (2014) is presenting a non-convex MPC for cooperative control. Here, the first objective is to tackle collision avoidance while the secondary performance objective is to deal with the quality of the collision-free trajectory. Examples of the use of decentralized MPC is given by Shin andKim (2009), Fukushima et al. (2013), Turpin et al. (2012) and Bemporad and Rocchi (2011). The considered scenario in Fukushima et al. (2013) is formation control of a multi-vehicle system with collision avoidance and input constraints. For this purpose a feedback linearization controller is integrated with MPC. The application of Turpin et al. (2012) is trajectory tracking of aerial robots under formation constraints using decentralized MPC. In Bemporad and Rocchi (2011) a decentralized linear time-varying MPC is used for formation control of multiple UAVs using a leader-follower approach.
The generality and the high control performance of MPC come with a high computational burden. Hence, the real-time capability is a crucial aspect of MPC which has led to a variety of fast optimization algorithms to minimize the related computational effort. A theoretically well-established and widely used fast MPC algorithm is sequential quadratic programming (SQP) in combination with Newton-type solvers with e.g. Gauß-Newton or Broyden-Fletcher-Goldfarb-Shannon (BFGS) Hessian approximation. A comprehensive framework with a wide variety of related algorithms is ACADO ). The computational efficiency and realtime feasibility for fast mobile robot systems have been validated experimentally, as for example in collision avoidance scenarios with an aerial manipulator (Geisert and Mansard 2016) under use of BFGS. A computationally efficient non-SQP variation of a Newton-type method is the continuation generalized minimal residual (CGMRES) method as presented in Ohtsuka (2004). Its underlying concept is introduced in Sect. 3. A compact version in C++ code is freely available under (Ohtsuka 2015). The low computational burden of CGMRES makes it particularly suitable to control fast systems, such as e.g. gasoline engines (Kang et al. 2014), hover crafts (Seguchi and Ohtsuka 2003) and Eco cruise control scenarios (Sajadi-Alamdari et al. 2016). To increase the numerical stability, the condensed multiple shooting derivative CMSCGMRES has been developed in Shimizu et al. (2009Shimizu et al. ( , 2006. In the previous publication (Dentler et al. 2016a), CMSCGMRES has been successfully implemented to control a commercial quadrotor. The low computation time and real-time capability of CMSCGMRES has been confirmed experimentally for the given scenario. For this reason, this contribution is also based on CMSCGM-RES. To reduce the implementational effort of additional inter-robot communication and consensus mechanisms and to compute a globally optimal solution, this contribution is computing the MPC centrally. Yet, in order to maintain the modularity of the distributed system in the central MPC scheme, the modularization scheme from previous work (Dentler et al. 2016b Within this work the cooperative control for a localization scenario with limited sensor perception is discussed. In the context of formation control the problem of a limited sensor perception space is critical along with collision avoidance and trajectory tracking. One approach for vision sensors is visual servoing based on optical flow or features, as shown in de Ruiter and Benhabib (2008), Kendoul et al. (2009) and Erkent and Işıl Bozma (2012). For traditional backstepping controllers, sensor perception limits are addressed by switching the robot's formation control according to the compliance with the sensor constraints (Wang et al. 2015). Another approach is to compute the optimal boundary trajectories to satisfy the sensor constraint and track these, as shown for non-holonomic robots in Bhattacharya and Hutchinson (2006) or for visual servoing in Bhattacharya et al. (2007) and López-Nicolás et al. (2010). Nevertheless, the task dependency of the control laws makes it challenging to formulate control laws for complex scenarios with constraints. One way to avoid this loss of generality is to use MPC which allows defining tasks and constraints as optimization problem in a generic way. An example for a cooperative MPC using barrier function constraint handling with sensor and collision avoidance constraint is given in Ding et al. (2016). In Seo et al. (2017) an aerial manipulator is presented with a camera attached to the end-effector. The camera is controlled using a stochastic MPC method for visual servoing in order to keep the target in the field of view. Avanzini et al. (2015) is presenting the direct implementation of sensor constraints in MPC for holonomic mobile manipulators. The handling of constraints in MPC itself is a wide field of research. The major difficulty in MPC is how constraints are handled within the MPC solver. An overview and benchmark of computationally efficient inequality constraint handling techniques with CGMRES is given in Huang et al. (2015). The disadvantage of these simplistic constraint handling techniques, as for example auxiliary variable and logarithmic barrier method, is that a violation leads to an infeasible OCP and accordingly to a crash of the MPC solver. This is particularly problematic for fast MPC solvers, as they do not consider invalid values (inf, nan) within the prediction horizon and do therefore not automatically recover from an infeasible state. If a small constraint violation can be accepted, one way to avoid this problem is the use of weakened constraints. A weakened constraint is approximating the inequality constraint switching behavior by an analytical function [e.g. sigmoid (Lau and Lim 2017), tanh] which leads to a potential function. A comprehensive study on such weakened constraints in combination with MPC for multi UAV control strategies is given in Bertrand et al. (2014). The provided examples are collision avoidance, area exploration and formation flying. The same approach has been successfully implemented for collision avoidance of a MPC controlled quadrotor system in previous work (Dentler et al. 2016a). To conclude, in order to solve the position tracking problem of cooperative localization scenarios, this work is presenting a workflow to formulate sensor constraints as potential functions. To be more flexible than feedback linearization and backstepping control approaches in task and constraint description, a MPC approach is applied. In order to achieve real-time capability the MPC problem is solved by a central CMSCGMRES approach. To maintain the modularity of the MPC with respect to the distributed problem, this approach is combined with the MPC modularization technique, presented in Dentler et al. (2016b). The prediction model for the MPC is based on the quadrotor model from the previous contribution (Dentler et al. 2016a). An introduction of the utilized MPC approach is given in the following section.

Model predictive control principle and modularization
The idea of MPC is to compute optimal controls for a given system objective within a receding horizon as shown in Fig. 2. For this purpose, the current system state is measured at each control update time instance (t k , t k+1 , . . .). From each of these measured states, the future systems behavior can be predicted by means of a system model within a given horizon (0 ≤ τ ≤ T ). The controls for this horizon are then determined to minimize the error of the future system behavior to a given target behavior. After determining the optimal controls, u (t k ) = u (τ 0 ) is applied to the system and the horizon is shifted by Δt. The control loop is closed by a new measurement and prediction at time instance t k+1 (Δt later). The computation of the optimal controls within the prediction horizon can be formulated as optimization problem, a so called optimal control problem. In this work, OCPs of the form are considered. The desired behavior is defined by the cost function J which is consisting of an integral cost term. It is subjected to the prediction model dynamics f (x, u, τ ). The initial state condition x 0 is representative for the measurement at each control update interval. A common way to solve OCPs in real-time applications is to derive the Hamiltonian for (1) with the Lagrange multipliers λ. λ can then be computed via backward integration from 0 over the horizon usinġ The controls are then typically determined by solving the first order optimality condition There is a wide variety of methods to solve (4), whereas for real-time MPC common approaches are gradient [e.g. GRAMPC (Graichen and Käpernick 2012)] or Newton-type [e.g. Gauß-Newton in ACADO )] line search methods.
Within this work, a continuation generalized minimal residual method CGMRES derivative is used which has been already successfully applied in previous work (Dentler et al. 2016a, b). The continuation idea is to look at the continuous closed loop dynamics of the stabilized systeṁ which can then be used to approximateu u can be computed efficiently with a G M R E S method under use of a forward difference approximation of H −1 u . The controls u are finally gained by integrating u from the previous time step withu More detailed information about the CGMRES algorithm can be found in Ohtsuka (2004). By making the continuation assumption (5) also for the states x, a multiple shooting method can be derived to increase numerical stability. This increases the problem dimension, as the corresponding equation to (5) for the inputs u has also to be solved for the predicted system states within the horizon. An additional condensing addresses this issue by reducing the problem dimension again. A detailed overview of the CMSCGMRES method is given in Shimizu et al. (2006). The major advantage of using the continuation approach is the low computation time of the CGMRES method which makes it particularly interesting for real-time MPC, as already discussed in previous work (Dentler et al. 2016a).
Within this work, a modularization of MPC for cooperative control scenarios is applied, as presented in Dentler et al. (2016b). For cooperative scenarios with two entities (agent0 : x 0 , u 0 and agent1 : x 1 , u 1 ) a cost function can have the form where l 0 and l 1 represent tracking cost functions, while l c is defining some interaction costs. Under this assumption the optimality condition 6 yields to With the optimality condition (9) l c will influence u 0 through The modularization allows to switch off the influence of l c on e.g. u 1 by neglecting ∂l c (x 0 ,u 0 ,x 1 ,u 1 ) ∂u 1 in the optimality condition (9). This is useful, if one central MPC is used to control the complete cooperation scenario, but some interaction tasks shall just influence one drone (e.g. the use case scenario of this work). The further advantage of this centralized approach is its high performance, as not just the measured, but also the predicted future system states are taken into consideration. In the OCP the exclusion of cost derivatives (e.g. with respect to u 0 ) will be marked by an index \{u 0 }, e.g.
To finally apply a MPC in a mobile robot scenario, a prediction model is required. For real-time applications the complexity of this prediction model is crucial for the per-

Direction vector prediction model and validation
Most mobile robots are designed to move in a planar space. Their position is defined in a x y-plane and the height of this plane (z). Accordingly, their attitude is defined by the rotation angle Ψ around the plane normal vector. This description does not only fit for most wheeled ground robots, but can also be applied to multi-rotor unmanned aerial vehicles.
Multi-rotor UAVs are typically operated around their static equilibrium. By assuming that rotations around x (roll) and y (pitch) are so small to be be neglected, the quadrotor attitude can be linearized. This yields to a hover controller (Corke 2013), as implemented in most commercial multi-rotor UAVs. As a result, the pose can be described by a single yaw angle Ψ , representing the rotation around the z axis. To address the modeling of such systems, this section is introducing a modeling method for hover controlled multi-rotor systems. This model has furthermore the advantage to contain all elements from which omnidirectional and unidirectional ground robot motion models can also be derived.
To derive the generalized multi-rotor model, the coordinate frames, given in Fig. 3 are considered. A standard approach is to model the system's behavior in the state-space and to give the system function as an Ordinary Differential Equation (O DE). The state vector x of such a system is composing x G , y G , z G position in the global frame G, the Ψ G rotation around z G and the forwardẋ V and sidewardẏ V velocity in the vehicle frame V. Under use of the forward, sideward, upward and heading velocity as input the system dynamics f (x (t) , u (t)) can be approximated by a semi-nonlinear state-space model (Dentler et al. 2016a) For low-cost quadrotors the velocity inputs are typically just related to an attitude displacement, e.g. increase pitch angle to increase forward velocity. Accordingly the inputs are not expressing the exact velocity in m s −1 . The identification of the linear model parameters helps to describe this attitude control behavior and to accommodate such unnormed system inputs. Representatively for such low-cost mobile robotic systems, the quadrotor AR.Drone 2.0 is applied as example within this work.

Angle discontinuity effect on UAV control
The attitude of mobile robots which are moving in a planar plane is typically defined by a single angle Ψ on the interval For a full rotation of the mobile robot, Ψ is changing the sign between ±π . By artificially limiting the angle on the interval (15) a discontinuity is introduced into the model of the angle. This is problematic, if the attitude is controlled by means of velocity or acceleration. To show the problematic behavior, a model predictive controller is applied to an AR.Drone 2.0 quadrotor under use of the prediction model (11) The resulting trajectory is given in Fig. 4. To control the quadrotor attitude, traditionally the error e Ψ = Ψ des − Ψ to the desired yaw angle Ψ des is minimized. The yaw angle plot in Fig. 4 shows a step in the desired angle from Ψ des (0 s) = π 2 to Ψ des (2.8 s) = π . To minimize e Ψ , the controller is applying a positive angular velocity to converge asymptotically towards Ψ des = π . As real mobile robotic systems are exposed to disturbance, the robot is likely to overshoot Ψ des = π which leads to a change of sign to Ψ = −π . This can be seen in Fig. 4 at t ≈ 5.4 s. Hence, the quadrotor will again try to reach the desired value from the new angle Ψ des = −π . As a result, positions close to Ψ des = ±π cannot be stabilized which leads to a repetitive rotational movement. The main use of angle Ψ is the mathematical description for the mapping of the vehicle coordinate frame V to the global coordinate frame G. This is typically done by means of a rotation matrix, as shown in (14). For the planar case the rotation matrix yields To overcome the described angle discontinuity in the transformation, a continuous way to describe the robot attitude has to be used. One way to do so is to use a direction vector. For 3D-space this is well established in the form of quaternions q ∈ R 4 which consist of a real part q 0 and three imaginary parts q 1 , q 2 , q 3 to describe rotations q =< q 0 , q 1 , q 2 , q 3 >.
In state space models these four elements are treated as separate states. For real-time MPC the related increase in the OCP dimension is significant. For the considered robots, the pose control is only based on a rotation in the 2D xyplane. Accordingly, a direction vector approach is sufficient to describe the attitude, as discussed in the next Sect. 4.2.

Direction vector approach
As previously discussed, the direction of a vector can be used to describe a robot attitude. Each position vector of the points on the unit circle can accordingly be used which is directly related to complex number theory. These vectors are uniquely defined by their projections onto the coordinate axis x and y as shown in Fig. 5. In comparison to a single angle description with yaw Ψ , this transformation is bijective. In the context of this work, the combination of these two projections (d x and d x ) is called direction vector d. The direction vector d can be written in vector form As the direction vector is expressing the projection from the unit circle, d x and d y are fulfilling the circle constraint To receive the state space description,ḋ yields under use of (18) tȯ This is intuitive, as the derivativeḋ has to be orthogonal to d to force d to stay on the unit circle.
The system input u Ψ is expressing the angular velocitẏ Ψ in (20) with the constant factor b ψ . Hence, the derivative yields tȯ Transformation matrix (17) is transformed with (18) to To transform the system dynamics (14) into a direction vector model, Ψ is substituted by the direction vector in the state vector Using the direction vector (18), the derivative (21) with u Ψ and the coordinate transformation (22), the system dynamics (14) finally leads tȯ To track attitude d, a simple quadratic penalty can be used under the assumption, that numerical errors of the MPC solver avoid the singular problem of opposing attitudes, e.g.

Experimental validation of direction vector approach
To validate the direction vector quadrotor model (24), the desired drone attitude is rotated anti-clock-wise in steps of Ψ = π 2 . The Ψ plot in Fig. 6 shows the desired and actual attitude of the system. In contrast to the previous instability at Ψ = ±π (Fig. 4), Fig. 6 is showing the desired asymptotic approaching of the desired trajectory. The oscillations This includes airflow disturbance, modeling errors, numerical errors and the trade-off between energy optimality and position tracking. Hence, the direction vector approach is resolving the angle discontinuity problem stated in Sect. 4.1. The proposed approach is a trade-off between the continuous attitude description, the computational effort, regarding the quaternion approach (4 states) and the standard angle description (1 state). Regarding the generality of the direction vector model, the same approach can be easily applied also to other planar robots with single angle attitude description. For example ground robots can be modeled by neglecting the z component of (24) with u z = 0. For unidirectional robots, the sideward movement can be neglected by setting u s = 0. Based on the resulting direction vector based system dynamics, the next sections describe the application of the model in a sensor constrained model predictive control scenario.

Sensor based control with potential functions
One major difficulty to control complex tasks autonomously is the mathematical formulation of such tasks. A generic way to do so are inequality constraints. MPC can take such constraints into consideration. In this work, they are considered in the MPC as weakened constraints. This refers to the substitution of hard inequality constraints by a cost function, that imposes a repulsive behavior from a violation of the constraint. This implementation in the cost function equals to a potential function. In this section, a generic procedure to create such potential functions is presented. The formulation of a sensor constraint serves as example. In the example scenario an object is tracked with a sensor attached to a quadrotor. The sensor perception space is thereby shaped like a cone (e.g. ultrasonic distance sensor). How to describe this sensor limitation is shown in the following section.

Sensor constraints
The considered use case is an AR.Drone 2.0 quadrotor with the presented dynamics (24). With the example of a coneformed sensor perception space, the robot/sensor system can be illustrated as shown in Fig. 7. The first step to implement the sensor based control is, to formulate the perception space limitation in terms of constraints. For this purpose, a target object is defined in the sensor frame with the position vector p S ∈ S p S = x pS y pS z pS ∈ R 3 .
Regarding the position of the trackable object in the sensor frame p S , the field of view of the sensor can be expressed by the constraints where (27) is representing a double cone. To receive a single cone perception space, constraint (28) is limiting the cone to the positive half plane of x pS . In contrast to just pointing the quadrotor into the target's direction, the formulation of the perception area offers more flexibility to the MPC to optimize the energy consumption and to adapt the scenario to other constraints e.g. obstacles. The question of how to consider the sensor constraint (27) and (28) in an optimal control problem is described in the next section.

Potential function constraint handling
Considering the constraints (27) and (28) as hard constraints is problematic, because their violation would lead to an infeasible optimization problem. In the sensor based tracking scenario this would be the case, if the object is outside the sensor perception space. Such a violation is possible due to disturbance or an infeasible initial pose. For this reason, the sensor constraints are designed as weakened constraints by imposing a repulsive behavior from constraint violation. One way to accomplished this is to add an additional penalty term to the OCP's integral costs l. Due to the fact that OCPs are typically defined as minimization problems with the optimum l = 0, a constraint violation has to be penalized with a higher cost. An intuitive approach to translate the constraint c ≤ 0 to the weakened constraint l c is therefore to penalize the compliant area with l = 0 and the constraint violation area with l = 1. This can be described by using a unit step , such that For the cone constraint (27) and (28) this leads to c 2 is used to distinguish between the negative and positive half-space of x pS : To initially use the unit step approximation (29) for the constraint transformation has proven to be particularly helpful regarding potential functions developed from nested constraints. More complex approximation functions lead to

Addressing vanishing gradient
The resulting cost function for the x y-plane is shown in the left plot of Fig. 9. As expected, the figure shows that the gradient satisfies ∇l c (p S , α) = 0 ∀c 1 (p S , α) = 0 As most approaches for searching the minimum of the defined cost function l c are based on a gradient descent, this property becomes problematic, as the convergence of the search algorithm cannot be guaranteed. To address this issue a cost slope can be added around the cone as shown in the right illustration of Fig. 9. To impose a gradient, a simple quadratic penalty can be added in the undesired regions. Hence, (30) can be extended to l c (p S , α, κ G ) Fig. 10 Sigmoid approximation of unit step with corresponding derivatives whereby parameter κ G is controlling the steepness of the gradient derivative. As a remark, gradient-based solvers typically require convex OCP problems to ensure convergence. Naturally, most sensor perception spaces are convex. However this should be kept in mind for development of more complex constraints or the combination of multiple constraints.

Addressing differentiability of the potential function
The second problematic property of the proposed unit step constraint translation (29) is that the derivative of the unit step is not an analytical function. Yet, most fast optimal control problem solvers require the differentiability of the cost function. To address this problem, the switching behavior of the unit step, with respect to the constraints, can be approximated by means of a sigmoid function. This has been already validated in previous work for a collision avoidance constraint (Dentler et al. 2016a) and is similar to the tanh approximation, as introduced in Bertrand et al. (2014). Accordingly, the unit step can be approximated with where κ A is a design parameter. The derivative of sig (c, κ A ) can be determined analogously Figure 10 is showing the behavior of the sigmoid function and it's derivative for a variation of κ A . For increasing κ A , sig (c, κ A ) is converging towards a step function (c). Its derivative is accordingly converging towards a δ impulse Factor κ A is therefore determining the trade-off between quality of the approximation and condition number of the  (35), finally yields to which is shown in Fig. 11 for κ A = 10. To be able to track an object with known position in the global coordinate system, p S in (30) has to be determined by its counterpart p G in the global coordinate system. The required coordinate transformation is explained in the following section.

Coordinate transformation
The coordinate transformation from the global coordinate frame to the sensor frame can be described as a sequential transformation with homogeneous transformation matrices. For a matrix that transforms a point p from the global coordinates G into the sensor frame coordinates S we use the following nomenclature This transformation matrix can consist of the position displacement of coordinate systems the rotational displacement expressed by an angle β e.g. around y and a direction vector e.g. around z With the robot position p r G =(x r G , y r G , y r G ) and robot orientation the transformation matrix from the global frame G into the vehicle frame V and then to the sensor frame S leads to the transformation matrix The global coordinates in the sensor frame is accordingly given by (40) and leads to Finally the target position in the sensor coordinate frame can be expressed in global coordinates by (48) and applied in l c (p S , α, κ G , κ A ) (39). Due to its complexity, the resulting equation is not given here. Figure 12 is showing the resulting costs in the x y-plane for a quadrotor at position p r G = (−1, −1, 0) and orientation d r G = (0.71, 0.71) ≡ Ψ = 45 • . The triangular base form of the cone is oriented as expected from the UAV origin in p r G = (−1, −1, 0) .

Safety constraints
To validate the derived potential function experimentally, additional safety measures are necessary. The most important safety constraint is treating collision avoidance and ensures that a safety distance d min between object and drone is not violated. Second, the maximum distance d max of the sensor has to be considered. This can be accomplished by implementing a cohesion constraint which introduces a repulsive behavior from large distances between object and robot. Both constraints are derived according to the workflow presented for the sensor constraint in Sect. 5.

Collision avoidance constraint
Considering that p G could also represent an obstacle to avoid, a potential function for obstacle avoidance can be formulated as penalty of a distance below a limit d min : The norm can be reformulated by means of quadrature which then is transformed into a potential function with unit steps To improve the convergence properties, (51) is extended by a quadratic penalty term Here, κ H is defining the maximum height and κ G the decent of the gradient of the potential function. Finally, (52) can be transformed to an analytical function with the help of the sigmoid function (36) κ A is describing the quality of the unit step approximation as before. The final result of the potential function is shown in Fig. 13 which shows a high penalty for the area with distance d min around the origin. The form of the convex top  (x, y, z) with robot placed at p r G = (−1, −1, 0) . As desired any violation of d min (49) is addressed with a high penalty. This yields a repulsive behavior between robot and object.

Cohesion constraint
Cohesion can be seen as inversion of the collision avoidance problem (49), where distances bigger than d max to an object should be avoided. The constraint can therefore be formulated as As before, the norm can be expressed with a the help of a quadrature and transformed into a cost function with a unit step function To manipulate the curvature of the given penalty function, κ H is introduced to define the maximum height. κ G is describing the ascent of the gradient of the potential function The final analytical cost function is gained from an approximation of the unit step with the sigmoid function (36). Which yields Figure 15 is showing the desired inverse behavior as for the collision avoidance behavior in Fig. 13. The cohesion constraint leads to a high penalty for distances greater than d max around the origin. Figure 16 is showing penalty values l max D > 0.5 for an obstacle in position p G = (x, y, z) with the robot placed at p r G = (−1, −1, 0) . The figure validates that the whole area of p G − p r G ≥ d max is highly penalized which leads to an attracting behavior between object and robot.
With the developed safety constraints (53) and (59), the cone constraint has been validated as discussed in the following section.

Validation of the sensor based control with potential functions
The validation scenario is a visual quadrotor tracking scenario, where one quadrotor is equipped with a camera and tracking a target quadrotor. The task is to keep the target quadrotor in the camera frame as shown in Fig. 17. The control of both quadrotors is accomplished with a central MPC controller (Dentler 2016) and extended by the The index \{u 1 } of the cost functions l c , l max D and l min D reflects, that the influence of the cost functions on UAV 1 is neglected, as explained in (10). This means that the cone, collision avoidance and cohesion constraint is only affecting UAV 0 . The advantage of this modular approach of a central control for such a scenario is, that future states of both systems are considered for the computation of the optimal controls, while the effect of the tracking costs can be limited to one quadrotor. The parameters of (60) for the experimental validation are given as Q 0 = Diag 0, 0, 0, 0, 0, 0.7, 0.7 (61) Q 1 = Diag 1.5, 1.5, 1.6, 1, 1, 0, 0 (63) The parameters p cgmres are referring to the CMSCGMRES solver and are given in a coherent notation to the previous work (Dentler et al. 2016a). To examine the dynamic behavior of the proposed control solution, the target position of UAV 1 is moving in a circular trajectory. For UAV 0 , the choice of Q 0 leads to a tracking of zero forward velocitẏ x uav0V (t) = 0 and sideward velocityẏ uav0V (t) = 0 which yields to the desired states x * 1 = Diag 1 2 cos(0.3t), 1 2 sin(0.3t), 1, 0, 0, 0.0, 0.0 .
Based on the given parameters and target trajectories, the scenario is validated in the following sections.

Numerical validation
For the numerical validation, two AR.Drone 2.0 models have been implemented in the simulation environment V -REP (Fig. 18). The position information of quadrotors and target trajectories is shared via a Robot Operating System (ROS) interface. Figure 19 is showing the trajectories of both UAVs. UAV 1 is following the circular moving target which is reflected by sinusoidal position trajectories. UAV 0 is tracking UAV 1 with the developed sensor constraints which is indicated by likewise sinusoidal position trajectories of UAV 0 . The form and position of the resulting trajectory is depending on the initial UAV positions. As the position of UAV 0 is just dependent on the applied constraints, UAV 0 can rotate freely around UAV 1 . This explains the drift of the sinusoidal position trajectory of UAV 0 . The control trajectories of UAV 1 and UAV 0 are showing that the input limits are respected. The distance d stays in the defined limits d min ≤ d ≤ d max with one exception at the initial phase of the simulation. Here the repulsive Fig. 19 Numerical simulation: Trajectories of use case scenario behavior of the collision avoidance constraint can be seen, where the controller increases the distance to fulfill d ≥ d min . On one hand, the disadvantage of the weakened constraint is, that d can violate the given constraint depending on the parametrization of the potential function and the smoothness of the unit step approximation by the sigmoid (35). On the other hand, the advantage is, that a constraint violation does not lead to an infeasible OCP. To conclude, the distance trajectory is validating the active collision avoidance and cohesion constraint.
To validate that the tracked UAV 1 stays within the sensor beam width angle α, Fig.19 is therefore also showing the absolute tracking angle α t which is describing the angle between the UAV 0 sensor orientation vector and the distance vector to UAV 1 in the sensor frame. The resulting α t plot in Fig. 19 is validating, that the cone constraint keeps the tracking angle α t smaller than the sensor beam width angle α t ≤ α = 0.5 rad. It can be seen that the tracking angle α t is in fact much smaller. This is caused by the smooth approximation of the unit steps by sigmoids (35) which leads to a convergence towards the center of the cone. Especially for undisturbed systems, this yields to a smaller tracking angle α t than the beam width angle α.
To reduce this effect κ A can be increased. A more intuitive access to the UAV behavior is gained by plotting the UAV positions and the orientation of UAV 0 by means of a vector as shown in Fig. 20. To be able to associate both UAV positions, time-related UAV positions are connected with a line. It is visible that UAV 1 is following the desired circular trajectory, while UAV 0 is tracking UAV 1 in an ellipsoidal movement. The orientation vectors are displayed at each Δt ≈ 1.68 s for means of visualization.
To resume, Figs. 19 and 20 validate the desired behavior of the proposed sensor constrained MPC controller in simulation. The next section is discussing the extension of this numerical validation to the real scenario.

Experimental validation
The use case scenario with real AR.Drone 2.0 quadrotors, as shown in Fig. 21, is subject to a variety of disturbance which is not considered in the numerical validation. Light-weight UAVs like an AR.Drone 2.0 quadrotor are particularly responsive to airflow disturbance. In addition, their flight dynamics are very volatile, as their body consist of deformable Styrofoam. These and more influences (communication latency, prediction model errors, measurement uncertainty) are not considered in the MPC model and simulation environment. For this reason an experimental val- idation is necessary to assess the robustness of the proposed control approach. The position data of the real AR.Drone 2.0 quadrotors is hereby measured by a motion capture system. The resulting system trajectories are given in Fig. 22. In contrast to the numerical simulation, the real AR.Drone 2.0 trajectories are subject to significant disturbance. Especially the mutual airflow disturbance is causing low-frequency oscillations ≈ 0.5Hz which are visible in the y-position of UAV 0 and UAV 1 . As the airflow disturbance error is propagating from the UAV 1 position to the UAV 0 response, the resulting oscillations are particularly visible in the controls of UAV 0 .
To evaluate the control performance, the distance d and absolute tracking angle α t are given in Fig. 22. The distance plot is stating that UAV 1 is tracked within the given distance limitations. At t ≈ 55 s the behavior of the weakened constraint is visible which is allowing a minor violation of d min in return for avoiding infeasible solutions and ease of implementation. The absolute tracking angle (71) in Fig. 22 is measured between the UAV 0 orientation vector and the distance vector to UAV 1 . Due to the initial conditions, α t is violating the applied cone constraint at the beginning, but is tracked within the given beam width angle α t ≤ α = 0.5 rad for t ≥ 6 s. Hence, the trajectories in Fig. 22 confirm the desired tracking behavior. Figure 23 is showing the resulting x y-trajectory with samples of the UAV 0 orientation vector. For means of visualization, the orientation sample time is reduced to Δt ≈ 1.68 s. Due to different initial conditions (positions, velocities, etc.), the x y-trajectory is not directly comparable with the numerical simulation. However, it visualizes the closedloop UAV position response to mutual airflow disturbance in the form of small oscillation. In comparison to the simulation trajectory, the rejection of these oscillations (input costs) dominate the minor gradient within the compliant area of the sensor constraint. As a result the absolute tracking angle α t and distance d plots in Fig. 22 go closer to their constraint limits and the x y-plot does not follow the ellipsoidal pattern of UAV 0 in Fig. 20. In order to evaluate the tracking without the mutual airflow disturbance and under different initial conditions, a further analysis of the robustness is shown in the experimental discussion (Sect. 7.3).
For the evaluation of the computational efficiency of the proposed approach, Fig. 24 is giving the MPC computation time on a standard computer (Dell Latitude E5440). The peaks in the computation time of t comp ≈ 20ms are not directly related to the solver, but are caused by CPU interrupts by other processes. The resulting average computation time t av,comp = 2.5ms is very low in comparison to the control update interval of Δt = 10ms and states the real-time feasibility of the MPC.
To conclude, the experimental results are validating the computational efficiency and effectiveness of the MPC control based on potential functions.

Experimental discussion
In order to discuss the robustness to disturbance and initial conditions, the previous experiment of Sect. 7.2 is altered by substituting UAV 1 by a target with constant position p uav0G → p tG = [0.04, −0.05, 0.96] m . The disturbance is introduced manually by means of an obstacle with the position p OG and a related collision avoidance constraint (52) l min D p OG , p uav0G , d O,min , κ H 3, κ G 3, κ A 3 . In order to make the evasive behavior visible in the x y-plane, the z-axis action of UAV 0 is reduced by increasing the input penalty for u z . The resulting OCP (72)-(78) does consider the obstacle and target position within the prediction horizon as constant with The experimental outline is shown in Fig. 25. As in Sect. 7.2, the pose of UAV 0 is measured with a motion capture system. The constant target position is indicated as green diamond in the center of the pictures. In its initial pose, UAV 0 is not necessarily fulfilling the underlying inequality constraints of the sensor cone constraint (39). The activation of the controller therefore initially leads to a convergence towards a compliant pose. As a next step, an obstacle (red star) is introduced manually by means of the motion capture system. UAV 0 (blue circle) is evading the approaching obstacle by moving in the opposite direction. This evasion maneuver shows the form of an arc due to the active target tracking constraints. Following UAV 0 with the obstacle consequently leads to a circular trajectory around the tracked target point p tG . The corresponding UAV 0 orientation is indicated by a blue arrow.
To cope different initial conditions and obstacle patterns, a set of 10 experiments is conducted. Figure 26 is showing the resulting x y-trajectory of UAV 0 (blue line), obstacle (dashed red line) and the fixed target position (green diamond). To visualize the system at different time instances, the position of the obstacle (red star) and pose of UAV 0 (blue circle with arrow) is marked every Δt = 4.2 s. The distances between UAV 0 and target is signalized as light grey line at each of these time instances. Accordingly, the distance between UAV 0 and the obstacle is indicated as light red line. The resulting circular patters validates the target tracking during the evasion maneuver. Furthermore, the direction of the drone is pointing to the center which is indicating the tracking of the target. In this context, in plot 7 center left and plot 9 center up the drone orientations which are not pointing towards the target are representing initial conditions. These initial poses of UAV 0 have been chosen arbitrarily within the spacial limitations of the laboratory and its exact values can be exerted from the UAV 0 pose plots in Fig. 27. In this initial phase a typical increase in the altitude (z) can be observed which is caused by the inclination angle β of the sensor cone constraint. The steps in the Ψ values in Fig. 27 are based on the limited Ψ -angle interval. The sinusoidal trajectories in x and y evidence the circular evasion maneuver of UAV 0 . Figure 28 is showing measurements of the corresponding obstacle position. Also here the sinusoidal trajectory, in order to follow the UAV 0 and provoke an evasion, is visible. The steps in the obstacle position at the beginning are caused by entering the detection zone of the motion capture system.
To analyze the influence of the obstacle collision avoidance constraint l min D (53), Fig. 29 is showing the distance between UAV 0 and obstacle. The plots show how the obstacle is moved close to d Omin to provoke an evasion maneuver of UAV 0 . The measured minimal distances are given in Table 1.  As the collision avoidance design is based on weakened constraints, violations are feasible. For all 10 experiments the highest violation of the obstacle distance d O appears in run 5 with min(d O ) = 0.601m. The violation generally depends on UAV 0 and obstacle speed as well as cost gradient design of the collision avoidance constraint. For the here considered UAV and obstacle speeds, the cost gradient is chosen less steep (77) in order to show a smooth repulsive behavior while showing the desired evasion. In reverse conclusion higher violations are accepted. For higher system velocities this cost gradient has to be chosen steeper. Its repulsive behavior can be observed as oscillation around the constraint border in Fig. 29 run 6, 7 and 9.   The influence of the target collision avoidance and cohesion constraints can be evaluated using the Euclidean distance of UAV 0 to the target, as shown in Fig. 30. Both constraints restrict the distance to d min = 0.7m ≤ d ≤ 2.0m = d max . The measured distance d lies in the interval 0.941m ≤ d ≤ 1.966 and therefore complies to the target distance constraints for all 10 experiments. The peak distance values are given in Table 1. The minimum tracking distance min(d) = 0.941m appears in run 2 and its maximum value max(d) = 1.966m in run 10.
Finally, the behavior of the developed sensor cone constraint (39) is validated. For this purpose the absolute tracking angle α t (71) is shown in Fig. 31. The sensor constraint (39) is restricting the absolute tracking angle to α t ≤ α t,max = 0.5. For the set of experiments, the initial conditions do typically not satisfy this constraint due to a low initial UAV 0 altitude z and incompliant orientation ψ. This is directly shown by the controller counteraction as shown in Fig. 32. In the initial phase experiment 1-5 show a direct reaction in the altitude by |u z | >> 0, while a significant adjustment in the ψ-axis by |u ψ | >> 0 is dominating in experiment 5-10. The resulting convergence towards the constraint compliance α t → α t,max can be seen in all plots of Fig. 30 and is followed by a period of low action, as all constraints are satisfied and the obstacle is not considered yet. With the approaching obstacle d O → d O,min in Fig. 29, the control action in Fig. 32 is increased due to the evasion maneuver. As a result, also the tracking angle α t in Fig. 31 is disturbed. To measure the constraint violation the maximal absolute tracking angle in the nominal state max(α t,ns ) is given in Table 1. For this purpose, max(α t,ns ) takes into consideration the absolute tracking angle peak values after the initial convergence phase. α t ≤ α t,max = 0.5 rad holds after the initial phase for all the experiments. The measured maximum appears in run 2 with max(α t,ns ) = 0.438 rad.
The experimental results validate the desired behavior of sensor based tracking also under the influence of disturbance introduced as obstacle. The senor field of view limitations are respected and collisions are avoided. Furthermore, the set of experiments shows the robust convergence towards a compliant state under differing initial conditions and constraint violations.

Conclusion and perspective
The present paper focuses on the presentation of a workflow for the generation of potential functions for sensor constrained MPC. The workflow is tested in simulation and a real-world implementation of a sensor constraint tracking scenario with quadrotors.
For this purpose a compact motion model for multi-rotor UAVs has been developed. To be able to control the robot's   yaw angle orientation in 360 • , the problem of the discontinuously defined yaw angle −π < Ψ ≤ π has been addressed. The MPC of the resulting direction vector model has been validated experimentally with an AR.Drone 2 quadrotor.
Based on the developed system dynamic description, the workflow for a sensor constrained MPC control has been presented. The AR.Drone 2 quadrotor with attached sensor has served as platform for the sensor based tracking scenario. The first step in the workflow is to formulate the sensor perception space within a sensor coordinate frame by means of inequality constraints. For the use case, the cone shape of the sensor perception space has been described by a combination of two inequality constraints. As second step, these constraints have been transformed into a potential function with the help of unit steps. The idea is to introduce a repulsive behavior for a violation of the constraints which leads to a weakening of the constraints. This allows a small violation to maintaining feasibility of the OCP. The transformation into a weakened constraint with the help of unit steps helps to verify the cost functions without having to deal with the increased complexity of sigmoids. In addition, it allows a simple graphical verification of the potential function with visualization tools. To improve the solvability of the problem for gradient based solvers, the next step has been the introduction of a gradient in the undesired ares of the potential function which is pointing away from the target region. Subsequently, the previously introduced unit steps in the potential function are approximated with sigmoids which leads to a continuous gradient around the constraint borders. As a result the potential function becomes analytical and therefore solvable by standard real-time MPC solvers.
In order to validate the derived potential function for the cone constraint, additional safety measures have been necessary. Therefore, a collision avoidance and a cohesion constraint have been developed, using the previously described workflow. In the validation scenario an AR.Drone 2.0 is used to track another AR.Drone 2.0 quadrotor which is following a circular trajectory. The tracking by means of the sensor constraint has been tested experimentally in simulation and a real-world implementation. To further discuss the influence of obstacles, utilized constraints and different initial conditions, a series of real-world collision avoidance scenarios has been presented. In the presented scenarios an AR.Drone 2.0 is tracking a fixed target using the developed constraints for the cooperative control scenario while disturbance is introduced in form of an obstacle. The results show the desired collision evasion maneuver while maintaining sensor tracking for different initial conditions. The results have validated the developed multi-rotor prediction model as well as the sensor constraint potential function. Future work will focus on the solution of the localization problem with cameras. A further development will be the analysis of the energy efficiency of the proposed sensor constraint, in contrast to a simple orientation tracking. Particularly interesting would be a statistical analysis of large numbers of real-world experiments. Another direction of future studies is to distribute the presented central MPC and to explore the applicability of cloud computing. In addition, the application of the tracking scenario in heterogeneous systems e.g. ground robots and UAVs, will also be addressed in future work.