A Model Predictive Guidance Strategy for a Multirotor Aerial Vehicle

AbstrAct: The present study faces the problem of safely controlling the position trajectory of a multirotor aerial vehicle subjected to a conic constraint on the total thrust vector and a linear convex constraint on the position vector. The problem is solved using a linear state-space model predictive control strategy, whose optimization is made handy by replacing the original conic constraint set on the thrust vector by an inscribed pyramidal space, which renders a linear set of inequalities. The proposed method is evaluated on the basis of Monte Carlo simulations taking into account a random disturbance force. The simulation results show the effectiveness of the method in tracking the commanded trajectory while respecting the constraints. They also predict the effect of both the speed command and the maximum allowed inclination angle on the system performance.


INTRODUCTION
Unmanned Aerial Vehicles (UAVs) have motivated and stimulated many researches in different fields of knowledge such as sensors fusion (Cheviron et al. 2007;Nemra and Aouf 2010;Gonçalves et al. 2013), computer vision (Saripalli et al. 2003;Xu et al. 2009;Xiao-Hong et al. 2012) and control strategies (Mian and Daobo 2008;Santos et al. 2013).A few years ago, building a low-cost miniature UAV was a challenge due to limitation imposed by equipment such as sensors, efficient motors, batteries and on-board computers.However, thanks to technological advances in actuators, small scale sensors, data processing and energy storage, the conditions improved significantly.
Among the different types of UAVs such as blimps (Elfes et al. 1998), fixed-wings (Beard et al. 2005) and rotary wings aircrafts (Bouabdallah et al. 2004), the present study focuses on the multirotor aerial vehicles (MAVs) (Gupte et al. 2012;Er et al. 2013).The interest for MAVs has increased in the last decade due to their low cost, high maneuverability, simplified mechanics, capability to perform vertical take-off and landing as well as hovering flight.These characteristics make them suitable for a wide range of applications, such as surveillance of indoor and urban environments, object delivery, building inspection, and agriculture monitoring.In all the aforementioned applications, a precise position tracking controller (or a guidance law) is crucial for autonomous operation.
Although there is a massive amount of concluded and ongoing research studies on MAVs, the design of control laws for such vehicles still has challenges to overcome.Most of those challenges are related to safety of the MAV itself and for people close to its operation.Mistler et al. (2013) and A Model Predictive Guidance Strategy for a Multirotor Aerial Vehicle Mahony et al. (2012) propose linear control laws combined with the feedback linearization technique for guiding a quadrotor through a reference trajectory.Bouabdallah and Siegwart (2005) designed 2 control laws using, respectively, the sliding mode and the backstepping methods; the authors showed by simulations that the backstepping method outperforms the sliding mode controller.Madani and Benallegue (2007) propose position control scheme combining a backstepping controller with a sliding mode observer.Castillo et al. (2014) and Hua et al. (2009) present robust control law design methods considering that the system is subjected to external disturbance and model uncertainty.It is worth mentioning that none of the aforementioned methods have considered constraints on the control force.
In fact, few MAV control methods available in the literature have dealt with constraints issues (Castillo et al. 2005;Cunha et al. 2009).In Castillo et al. (2005), the authors divided the system into smaller subsystems with 2 degrees of freedom (DOF).For each subsystem, they applied a nested saturated controller (Teel 1992) to achieve global stability while respecting a maximum constraint on the total thrust magnitude.However, the subsystems were assumed to have uncoupled dynamics, which is not true in general.In Cunha et al. (2009), to avoid an unbounded growth of the actuation commands, the authors presented an asymptotically stable controller that limits the maximum thrust magnitude by saturating the position control error.These 2 studies only considered constraints on the maximum value of the magnitude of the total thrust vector.
More recently, Santos et al. (2013) and Yan et al. (2014) tackled the problem of controlling the position of an MAV under constraints both on the inclination of the rotor plane and on the magnitude of the total thrust.Santos et al. (2013) presented a simple but effective control method derived using feedback linearization and a proportional-derivative control law.Yan et al. (2014) solved the same problem by using the retrospective cost adaptive control (RCAC) strategy.
The model predictive control (MPC) strategy appears as the most interesting choice whenever constraints are a concern.In this method, a future control sequence is obtained by minimizing a cost function on predicted values of the controlled variable along a finite horizon, typically subjected to constraints (Camacho and Bordons 1998).Applications of MPC to position control of MAVs can be found in Raffo et al. (2010), Lopes et al. (2011), Alexis et al. (2012), andChen et al. (2013).Raffo et al. (2010) proposed a control scheme consisting of an unconstrained MPC for position tracking and a non-linear H ∞ controller for attitude stabilization under aerodynamic disturbances and parametric as well as structural uncertainties.Lopes et al. (2011) designed a single MPC controller for both position control and attitude stabilization, considering constraints on both the pitch and the roll angles.Alexis et al. (2012) proposed a cascade MPC scheme, formulated over a set of piecewise affine models originated from both attitude and translation dynamics.In order to guide an MAV through a desired position trajectory, Chen et al. (2013) designed 2 separate MPCs, one for position control and the other for attitude control, the latter considering maximum constraints on the attitude angles.
In order to ensure a safe flight, it is essential to design a control law which avoids excessive accelerations and unexpected flips.The present study proposes an MPC for position control (guidance) of an MAV, constraining the total thrust vector within a conic set as well as the position vector within a parallelepiped set.

PreliminAry definitions
The motion of an MAV has 6 DOFs: 3 in translation and other 3 in rotation.However, this vehicle has only 4 independent control inputs: 3 torque components and the magnitude of the total thrust.Therefore, an MAV has an underactuated dynamics.At a first glance, it could be seem a challenge to deal with this characteristic.However, in practice, one needs to independently control only 4 DOF: the 3-dimensional position and the heading angle.
The block diagram of Fig. 1 describes a control system for controlling the 3-dimensional position r ∈ ℝ 3 of an MAV to follow a time-varying position command r c ∈ ℝ 3 .This with respect to S R ; tor represented in S R ; f d ∈ ℝ 3 is the disturbance force vector represented in S R , m is the vehicle's mass; g is the gravitational acceleration.
If, instead of a quadrotor, we had considered a hexarotor or an octo-rotor, the model in Eq. 1 would not change in any aspects, except for the origin of f that would receive contributions of either 6 or 8, instead of 4 propellers.As illustrated in Fig. 2, f is perpendicular to the rotor plane.
Defi ne the inclination angle ϕ ∈ ℝ of the rotor plane as the angle between Z B and Z R' .It can be expressed by  (2) (3) where: Defi ne the position control error r ~ ∈ ℝ 3 as where: Problem 1: ϕ max ∈ ℝ denote the maximum allowable value of ϕ, f min ∈ ℝ and f max ∈ ℝ denote, respectively, the minimum and maximum allowable values of f c , and r min ∈ ℝ 3 and r max ∈ ℝ 3 denote, respectively, the minimum and maximum allowable values of r.Th e MAV guidance problem is to fi nd a control law for f that minimizes r ~ subjected to the inclination constraint ϕ ≤ ϕ max , to the force magnitude constraint f min ≤ f c ≤ f max , and to the position constraint r min ≤ r ≤ r max .
Remark 1: the control force f of Problem 1 is not the eff ective control force undergone by the vehicle.In fact, its magnitude f c represents a command for the power electronics to drive the motors, while the inclination angle ϕ is used to compute the attitude command D c for the attitude control loop to orient the rotor plane (Fig. 1).Nevertheless, f c and D c are assumed here to be identical to the respective actual variables.Th e assumption about f c is reasonable if a precise model for the thrust force is available.On the other hand, the assumption about D c can also be approximated in practice if the controllers are tuned to allow the internal loop to have a much faster dynamics than the external one.
Remark 2: during the design of the controller, disturbance force and model uncertainty will not be considered.However, in "Computational Simulations" section, the proposed control method will be evaluated under such non-ideal conditions.
Remark 3: the position r and the velocity r . of CM are assumed to be available for feedback.In practice, these variables are provided by a navigation system (Fig. 1), which is not the focus of the present study.
In Problem 1, the parallelepiped constraint imposed on the vehicle's position r is considered so as to avoid collisions with the bounds of a box-shaped indoor environment.
On the other hand, one can visualize the corresponding constraint space on f as a conic space with an inferior and a superior spherical lid, as illustrated in Fig. 3.Note that the so-generated constraint space is non-linear and non-convex.Th e constraints on both magnitude and inclination of f are directly connected to the vertical and lateral accelerations of the vehicle.As one can see in Fig. 4, the component f z is responsible for controlling the altitude of the vehicle, while f xy produces the lateral acceleration that guides it along the X R and Y R directions, where  Using Eq. 4, the thrust magnitude constraint inequation f min ≤ f c ≤ f max can be rewritten in terms of u as Assuming that 0 ≤ ϕ max < π/2 rad, the inclination constraint ϕ ≤ ϕ max established in Problem 1 can be replaced by cos ϕ ≥ cos ϕ max .Using Eqs. 2 and 4, the last inequation can be rewritten in terms of the components of u as In order to obtain linear approximations for Eqs.18 and 19, consider the rectangular pyramid inscribed in the original conic control space depicted in Fig. 3. Th e new control space is illustrated in Fig. 5a.By inspection of this fi gure, the new constraint on f z can be expressed as Consider an arbitrary section of the pyramid depicted in Fig. 5a.Let f z denote its Z R' coordinate.Th e projection of this section on the X R' − Y R' plane consists of a square of dimensions α × α, as illustrated in Fig. 5b.From the geometry of this fi gure, one can write Finally, replacing u(k) = Δu(k) + u(k-1) into Eq.27 and taking it at M future instants starting from k, the thrust vector constraint is obtained as  3) for the internal (attitude) control loop (Fig. 1) is also computed from f, which contains information about the orientation of the rotor plane with respect to the local horizontal.In order to provide a unique 3-dimensional attitude command, it is necessary to specify a heading angle.For example, one can choose a 0 heading angle, which is just equivalent to the attitude represented by the principal Euler angle/axis (ϕ; e), where ϕ is computed from Eq. 2 and e is a unit vector given by where: f xy is the projection of f on the X R' -Y R' plane.Th e corresponding attitude command is given by (Shuster 1993) Now considering an arbitrary heading angle ψ, the attitude command D c results from 2 successive rotations.The first one is that represented by Eq. 41, while the second one is an elementary rotation D(ψ; Z B )of an angle ψ about the Z B axis, i.e.where

COMPUTATIONAL SIMULATIONS simUlAtion PArAmeters
Th e simulations are implemented in MATLAB/Simulink.Th e non-linear 6 DOF dynamics of an MAV is integrated using the Runge-Kutta-4 method with an integration step of 0.001 s.Th e vehicle's attitude is modeled using Euler angles in the rotation sequence 1-2-3.Th e vehicle's mass is m = 1 kg and the gravitational acceleration is assumed to be g = 9.81 m/s 2 .Th e vehicle's inertia matrix is Th e interior-point method is adopted to solve the MPC optimization.The control input weighting matrix Q and controlled output weighting matrix R are adjusted with ρ = 0.01 and η = 1, respectively.Th e prediction and control horizons are set to N = 80 and M = 5, respectively.Th e maximum and minimum position bounds are r max = [6 6 3] T m and r min = [0 0 0] T , respectively.Th e maximum and minimum constraints on the force magnitude are set in f max = 20 N and f min = 2 N, respectively.A non-zero minimum bound on the force magnitude f min was chosen in order to avoid loss of attitude control.On the other hand, the maximum bound f max is set sufficiently large to allow lateral accelerations without loss of lift .In order to verify the system robustness against an unknown input, a 0-mean Gaussian disturbance force with covariance Q d = 0.3× I 3 N 2 is taken into account.
The Th e following fi gure of merit is used to evaluate the position control error: (44) ( 45) Th e attitude controllers chosen for the present simulation are uncoupled proportional-derivative control laws tuned so as to make the attitude dynamics have a bandwidth signifi cantly larger than the bandwidth of the position control dynamics.

simUlAtion resUlts
Th e Monte Carlo simulation results are summarized in Table 1 in terms of e q and I l .First, one can observe that the control error increases as the speed command v is increased or as the maximum inclination ϕ max is decreased.For example, for v = 0.5 m/s, the position errors stay below 5 cm, whereas they approach 25 cm when the speed is set to v = 2.0 m/s.Regarding the violation of position constraints, no occurrence is observed with any of the 3 speed commands.Concerning the maximum inclination constraint ϕ max , for all speed commands v, the number of violations reduces as ϕ max is increased.Finally, the frequency of violations of f min and f max increases as the speed command is increased, but decreases as ϕ max is increased.
Figure 6 shows the MC realizations of the MAV position together with the corresponding mean and standard deviation curves for v = 1.0 m/s and diff erent values of ϕ max (10°; 20°; 30°).One can see that the standard deviation decreases as ϕ max increases.Th is behavior is due to the fact that a smaller value of ϕ max results in a smaller horizontal projection f xy of the total thrust on the horizontal plane, which, in turn, reduces the vehicle' s maneuverability and capability to reject horizontal disturbance forces.On the contrary, as ϕ max increases, f xy becomes larger, improving the maneuverability, which, in turn, provides a better disturbance rejection.
On the other hand, Fig. 7 shows the MC realizations of the MAV position together with the corresponding mean and standard deviation curves for ϕ max = 15° and diff erent values of v (0.5; 1.0 and 2.0 m/s).
One can see that, as the speed command increases, the standard deviation also becomes larger.The main reason is the fact that larger speed commands require better maneuverability and larger horizontal acceleration to ensure that the vehicle follows the reference trajectory with acceptable performance.
Th e worst performance observed in Table 1 occurs with ϕ max = 10° and v = 2.0 m/s.This scenario combines low maneuverability (due to a small ϕ max ) with a large speed command, which results in a large amount of control input constraint violations (Fig. 8).With a small ϕ max , the vehicle does not reach suffi cient lateral acceleration to follow a large speed command and simultaneously reject the disturbance force.It causes a large position error (Fig. 9).for q equal to x, y or z; r (i)  q (k) denotes the i th realization of r q (k).
For evaluating the frequency of constraint violation, the following fi gure of merit is adopted: (46) where: M l (i) is the number of discrete-time instants (of the i th realization) in which constraint l is violated, for l equal to x, y, z, ϕ, f min or f max .On the other hand, the best performance observed in Table 1 occurs with ϕ max = 30° and v = 0.5 m/s.This scenario combines high maneuverability (due to a large ϕ max ) with a small speed command.In this case, the vehicle does not suffer a significant influence of the disturbance forces and respects the constraint on both the inclination angle ϕ and thrust magnitude f c .For details, Fig. 10 shows ϕ and f c , while Fig. 11 shows the corresponding position control performance.

CONCLUSIONS
This study tackled the problem of controlling the position of a MAV subjected to constraints on the inclination of the rotor plane, on the total thrust magnitude, and on the position.The problem was solved using a conventional linear-quadratic state-space MPC formulation, which became possible thanks to the replacement of the original conic constraint space on the total thrust vector by an inscribed pyramid.
The method was evaluated by computational simulations considering that the vehicle was subjected to a Gaussian disturbance force.The proposed method showed able to control the vehicle's position, even under disturbance forces, while respecting the position and control constraints.However, if a large speed command is considered, it is necessary to relax the maximum inclination constraint in order to have sufficient lateral control force to overcome the disturbance forces.
The Δ-input MPC formulation used in this study appears as a good option for controlling the position of an MAV due to its ability of handling input and state constraints and, if suitably adjusted, it presents smooth responses to position commands.As the MPC has to solve an optimization problem at each update time, a drawback of this strategy is its high computational burden compared with traditional controllers such as the classic PID.

Figure 1 .
Figure 1.A typical structure of an MAV flight control system.
Newton's law, the translational dynamics of the MAV illustrated in Fig. 2 can be immediately described in S R by the following second-order diff erential equation: where: r = ∆ [r x , r y , r z ] T ∈ ℝ 3 is the CM position vector (1)

Figure 3 .
Figure 3.The original conic control space.

Figure 4 .
Figure 4. Analysis of f with respect to its constraints on inclination and magnitude.

Figure 6 .
Figure 6.Performance of position control varying the maximum inclination constraint.The solid gray lines are MC realizations; the dashed red lines are the position constraints; the solid black lines are the position command; the solid red lines are the sample means; the solid blue lines are the sample means plus/minus the standard deviation.

Figure 7 .
Figure 7. Performance of position control varying the speed command.The solid gray lines are MC realizations; the dashed red lines are the position constraints; the solid black lines are the position command; the solid red lines are the sample means; the solid blue lines are the sample means plus/minus the standard deviation.

Figure 8 .
Figure 8. Inclination angle ϕ and thrust magnitude f c in the worst case (ϕ max = 10° and v = 2.0 m/s).The gray lines are the MC realizations and the dashed red lines are the constraints.

Figure 9 .
Figure 9. Performance of position control in the worst case (ϕ max = 10° and v = 2.0 m/s) .The solid gray lines are MC realizations; the dashed red lines are the position constraints; the solid black line are the position commands; the solid red lines are the sample means; the solid blue lines are the sample means plus/minus the standard deviations.

Figure 11 .Figure 10 .
Figure 11.Performance of position control in the best case (ϕ max = 30° and v = 0.5 m/s).The solid gray lines are MC realizations; the dashed red lines are the position constraints; the solid black lines are the position commands; the solid red lines are the sample means; the solid blue lines are the sample means plus/minus the standard deviations.

Table 1 .
Monte Carlo simulation results for different values of v and ϕ max .