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.

KEYWORDS:
Multirotor aerial vehicle; Model predictive control; Position control; Guidance

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 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), and Chen 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.

PROBLEM STATEMENT

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 ∈ ℝ³ of an MAV to follow a time-varying position command r _c ∈ ℝ³. This system is structured into 2 control loops: an internal loop for attitude control and an external one for position control. The Navigation System is generally responsible for estimating the vehicle's attitude D ∈ SO(3), angular velocity ω ∈ ℝ³, position r, and linear velocity ṙ ∈ ℝ³ . The Attitude Controller receives an attitude command D ∈ SO(3) and produces the control torque τ_c ∈ ℝ³ necessary to rotate the rotor plane with respect to the local horizontal plane. The Position Controller has the function of computing a command f_c ∈ ℝ³ for the total thrust magnitude and a command D_c ∈ SO(3) for the attitude. These commands are computed in such a way to produce suitable lateral acceleration on the vehicle. The present study is concerned with the design of a position controller.

Figure 1
A typical structure of an MAV flight control system.

POSITION CONTROL PROBLEM

Consider the MAV and the 3 Cartesian coordinate systems (CCS) illustrated in Fig. 2. Assume that the vehicle has a rigid structure. The body CCS S_B$\stackrel{\Delta }{=}$ {X_B, Y_B, Z_B} is fixed to the structure and its origin coincides with the vehicle's center of mass (CM). The reference CCS S_R$\stackrel{\Delta }{=}$ {X_R, Y_R, Z_R} is Earth-fixed and its' origin is at a known point O. Finally, a second reference CCS S_R'$\stackrel{\Delta }{=}$ {X_R', Y_R', Z_R} is defined to be parallel to S_R, but with origin at CM. Assume that S_R is an inertial frame.

Figure 2
A multirotor aerial vehicle and 3 Cartesian coordinate systems.

Invoking the second 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 differential equation:

(1) $\ddot{r}=\frac{1}{m}\left(f+f_d\right)+\left[\begin{array}{c}0\hfill \\ 0\hfill \\ -g\hfill \end{array}\right],$

where: r $\stackrel{\Delta }{=}$ [r_x, r_y, r_z]^T ∈ ℝ³ is the CM position vector with respect to S_R; f $\stackrel{\Delta }{=}$ [f_x, f_y, f_z]^T ∈ ℝ³ is the total thrust vector represented in S_R; f_d ∈ ℝ³ 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.

Define the inclination angle ϕ ∈ ℝof the rotor plane as the angle between Z_B and Z_R'. It can be expressed by

(2) $\mathit{\phi }\triangleq {\cos }^{-1}\frac{f_z}{f_c},$

where: f_c$\stackrel{\Delta }{=}$ ||f||.

Define the position control error r̃ ∈ ℝ³ as

(3) $\tilde{r}\triangleq r-r_c,$

where: r_c$\stackrel{\Delta }{=}$ [r_c,x r_c,y r_c,z]^T is a position command.

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 ∈ ℝ³ and r_max ∈ ℝ³ denote, respectively, the minimum and maximum allowable values of r. The MAV guidance problem is to find 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 effective 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. The 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 ṙ 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. The 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_Rdirections, where f_xy$\stackrel{\Delta }{=}$ [f_x f_y ] ∈ ℝ² denotes the horizontal projection of f. As ϕ increases, the lateral acceleration of the vehicle also increases. If the constraint f_max is not sufficiently high, the vehicle could suffer a loss of lift. Furthermore, it is interesting to choose a ϕ_max that avoids unexpected flips as well as large lateral accelerations.

Figure 3
The original conic control space.

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

The choice of suitable values for the constraints on f and r can enhance the flight safety, since it avoids abrupt motion and delimits the flight region, respectively. Such characteristics are quite desirable, for example, in MAVs used to support people in indoor and urban environments.

PROBLEM SOLUTION

STATE-SPACE MODEL FOR TRANSLATION

Define the state vector x $\stackrel{\Delta }{=}$ [r_x ṙ_x r_y ṙ_y r_z ṙ_z]^T ∈ ℝ⁶ and the control input vector u $\stackrel{\Delta }{=}$ [u_x u_y u_z]^T ∈ ℝ³,

(4) $u\triangleq \frac{1}{m}f-\left[\begin{array}{c}0\hfill \\ 0\hfill \\ g\hfill \end{array}\right].$

Using Eq. 4, Eq. 1 can be immediately rewritten as a continuous-time linear state equation of the form ẋ= Ax + Bu. Similarly, by defining the controlled output vector to be the MAV position y $\stackrel{\Delta }{=}$ [r_x r_y r_z]^T ∈ ℝ³, one can obtain a continuous-time output equation of the form y = Cx. Let x(k) ∈ ℝ⁶, u(k) ∈ ℝ³ and y(k) ∈ ℝ³ denote, respectively, the state vector, the control input vector and the controlled output vector, all in the discrete-time domain. Using the ZOH method with a sampling time of T_s = 20 ms, a discrete-time version of the above state-space model is obtained as

(5) $x\left(k+1\right)=A_{d}x\left(k\right)+B_{d}u\left(k\right),$

(6) $y\left(k\right)=\mathrm{Cdx}\left(k\right),$

where

(7) $A_d=\left(\begin{array}{cccccc}1& 0.02& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0.02& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0.02\\ 0& 0& 0& 0& 0& 1\end{array}\right)\in {ℝ}^{6\times 6},$

(8) $B_d=\left[\begin{array}{ccc}2\times {10}^{-4}& 0& 0\\ 0.02& 0& 0\\ 0& 2\times {10}^{-4}& 0\\ 0& 0.02& 0\\ 0& 0& 2\times {10}^{-4}\\ 0& 0& 0.02\end{array}\right]\in {ℝ}^{6\times 3},$

(9) $C_d\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\in R^{}.$

Consider the discrete-time state-space model of Eqs. 5 and 6. It can be rewritten in the incremental-input form as (Maciejowski 2002)

(10) $\begin{array}{c}\mathit{\xi }\left(k+1\right)=\tilde{A}\mathit{\xi }\left(k\right)+\tilde{B}\Delta u\left(k\right),\hfill \\ y\left(k\right)=\tilde{C}\mathit{\xi }\left(k\right)\hfill \end{array}$

with

(11) $\mathit{\xi }\left(k\right)=\left[\begin{array}{c}\Delta x\left(k\right)\hfill \\ y\left(k\right)\hfill \end{array}\right]\in {ℝ}^9,$

(12) $\tilde{A}=\left[\begin{array}{cc}{A}_d& 0_{6\times 3}\\ C_d{A}_d& I_3\end{array}\right]\in R^{9\times 9},$

(13) $\tilde{B}=\left[\begin{array}{c}{B}_d\hfill \\ C_d{B}_d\hfill \end{array}\right]\in R^{9\times 3},$

(14) $\tilde{C}=\left[0_{3\times 6}{I}_3\right]\in R^{3\times 9},$

where: Δx(k) $\stackrel{\Delta }{=}$x(k) -x(k -1) ∈ℝ⁶ denotes the incremental state vector; Δu(k) $\stackrel{\Delta }{=}$u(k) -u(k -1) ∈ ℝ³ is the incremental control input vector; I₃ is the identity matrix with dimensions 3 × 3; 0_{3 ×6} is a zero matrix with dimensions 3 × 6.

PREDICTION MODEL

Using Eq. 10, the prediction model can be obtained as (see Maciejowski 2002, p. 50)

(15) ${𝒴}_N=𝒢\Delta {𝒰}_M+ℱ,$

where: γ_N ∈ ℝ^{3 N×1} stacks the controlled outputs along a prediction horizon of length N; Δu_M ∈ ℝ^{3 M×1} stacks the incremental control inputs along a control horizon of length M,

(16) $𝒢\triangleq \left[\begin{array}{cccc}\tilde{C}\tilde{B}& 0_{3\times 3}& \dots & 0_{3\times 3}\\ \tilde{C}\tilde{A}\tilde{B}& \tilde{C}\tilde{B}& \dots & 0_{3\times 3}\\ ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{M-1}\tilde{B}& \tilde{C}{\tilde{A}}^{M-2}\tilde{B}& \dots & \tilde{C}\tilde{B}\\ ⋮& ⋮& & ⋮\\ \tilde{C}{\tilde{A}}^{N-1}\tilde{B}& \tilde{C}{\tilde{A}}^{N-2}\tilde{B}& \dots & \tilde{C}{\tilde{A}}^{N-M}\tilde{B}\end{array}\right]∊{ℝ}^{3N\times 3M}$

and

(17) $ℱ\triangleq \left[\begin{array}{c}\tilde{C}\tilde{A}\hfill \\ \tilde{C}{\tilde{A}}^2\hfill \\ ⋮\hfill \\ \tilde{C}{\tilde{A}}^N\hfill \end{array}\right]\mathit{\xi }\left(k\right)\in {ℝ}^{3N\times 1}.$

THRUST VECTOR CONSTRAINTS

Using Eq. 4, the thrust magnitude constraint inequation f_min ≤ f_c ≤ f_max can be rewritten in terms of u as

(18) $f_{\mathrm{mim}}\le m\sqrt{u_x^2+u_y^2+{\left(u_z+g\right)}^2\le f_{\max }.}$

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

(19) $\frac{u_z+g}{\sqrt{u_x^2+u_y^2+{\left(u_z+g\right)}^2}}\ge \cos \mathit{\phi }\max .$

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. The new control space is illustrated in Fig. 5a. By inspection of this figure, the new constraint on f_z can be expressed as

(20) $f_{\min }\le f_z\le f_{\max }\cos {\mathit{\phi }}_{\max }.$

Figure 5
Pyramidal control space.

Consider an arbitrary section of the pyramid depicted in Fig. 5a. Let f_z denote its Z_R, coordinate. The 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 figure, one can write

(21) $\mathit{\alpha }=\sqrt{2}{f}_z\tan {\mathit{\phi }}_{\max }.$

By inspection of Fig. 5b, it can be seen that the implication of f to be bounded inside the pyramidal space is that f_x ∈ [-α/2, α/2] and f_y ∈ [-α/2, α/2]. By substituting Eq. 21 into these intervals, one can obtain

(22) $-\frac{\sqrt{2}}{2}{f}_z\tan {\mathit{\phi }}_{\max }\le f_x\le \frac{\sqrt{2}}{2}{f}_z\tan {\mathit{\phi }}_{\max }$

(23) $-\frac{\sqrt{2}}{2}{f}_z\tan {\mathit{\phi }}_{\max }\le fy\le \frac{\sqrt{2}}{2}{f}_z\tan {\mathit{\phi }}_{\max }.$

Rewriting Eqs. 20, 22 and 23 in matrix form, one yields

(24) $\Lambda f\le ⋋,$

where

(25) $\Lambda \triangleq \left[\begin{array}{ccc}-1& 0& -\frac{\sqrt{2}}{2}\tan {\mathit{\phi }}_{\max }\\ 1& 0& -\frac{\sqrt{2}}{2}\tan {\mathit{\phi }}_{\max }\\ 0& -1& -\frac{\sqrt{2}}{2}\tan {\mathit{\phi }}_{\max }\\ 0& 1& -\frac{\sqrt{2}}{2}\tan {\mathit{\phi }}_{\max }\\ 0& 0& -1\\ 0& 0& 1\end{array}\right]$

and

(26) $⋋\triangleq \left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ -f_{\min }\\ f_{\max }\cos {\mathit{\phi }}_{\max }\end{array}\right]$

Now using Eq. 4, Eq. 24 can be rewritten in terms of u as

(27) $\overline{\Lambda }u\le \overline{⋋},$

where Λ $\stackrel{\Delta }{=}$mΛ and

(28) $\overline{⋋}\triangleq ⋋-m\Lambda \left[\begin{array}{c}0\\ 0\\ g\end{array}\right].$

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

(29) $\mathit{\tau }\Delta {𝒰}_M\le \mathbf{\rho },$

where 𝒯 = [λ]_M - diag(Λ)[u(k-1)]_M and

(30) $𝒯=\left[\begin{array}{cccc}\overline{\Lambda }& 0_{6\times 3}& \cdots & 0_{6\times 3}\\ \overline{\Lambda }& \overline{\Lambda }& \cdots & 0_{6\times 3}\\ ⋮& ⋮& \ddots & ⋮\\ \overline{\Lambda }& \overline{\Lambda }& \cdots & \overline{\Lambda }\end{array}\right]\in {ℝ}^{6M\times 3M}.$

The matrix 𝒯 is lower block-triangular, [•]_M is an operator that stacks M copies of a column vector in an augmented vector, where and u(k-1) ∈ ℝ³ is the control input at instant k − 1.

POSITION VECTOR CONSTRAINTS

Now consider the constraints on the MAV position r_min ≤ r ≤ r_max . Taking them along the prediction horizon N, an augmented set of constraints is obtained as

(31) ${\left[r_{\min }\right]}_N\le Y_N\le {\left[r_{\max }\right]}_N,$

which can be rewritten in terms of Δu_M by means of Eq. 15, resulting in

(32) $A\Delta {𝒰}_M\le \mathit{\gamma },$

where

(33) $A\triangleq \left[\begin{array}{c}𝒢\\ -𝒢\end{array}\right]\in {ℝ}^{6N\times 3M}$

and

(34) $\mathit{\gamma }\triangleq \left[\begin{array}{c}{\left[r_{\max }\right]}_N-ℱ\\ ℱ-{\left[r_{\min }\right]}_N\end{array}\right]\in {ℝ}^{\mathrm{RN}}.$

MODEL PREDICTIVE CONTROLLER

The optimal control vector u*(k) ∈ ℝ³ computed at the discrete-time instant k is given by u*(k) = Δu*(k) + u*(k-1), where Δu*(k) ∈ ℝ³ is the first control vector in Δu* _M, which, in turn, is obtained by minimizing the following quadratic cost function:

(35) $J\left(\Delta {𝒰}_M\right)=\frac{1}{2}\Delta {𝒰}_M^T\mathscr{H}\Delta {𝒰}_M+{ℳ}^T\Delta {𝒰}_M+c,$

subjected to

(36) $𝒮\Delta {𝒰}_M\le \vartheta ,$

where: 1/2H = 𝒢^T Q𝒢 + R, M^T = 2(𝓕 - [r_c]_N)^T Q𝒢, c = (𝓕 - [r_c]_N)^T Q (𝓕 - [r_c]_N),

(37) $𝒮\triangleq \left[\begin{array}{c}𝒯\\ A\end{array}\right]∊{ℝ}^{\left(6M+6N\right)\times 3M},$

and

(38) $\vartheta \triangleq \left[\begin{array}{c}\mathit{\rho }\\ \mathit{\gamma }\end{array}\right]\in {ℝ}^{\left(6M+6N\right)}.$

In this study, the controlled output weighting matrix is set to Q = η × I_3N and the control input weighting matrix is set to R = ρ × I_3M. The above optimization problem is in the conventional quadratic programming form, for which there are efficient numerical solution methods (Rossiter 2003). The Δ-input MPC formulation presented here has an intrinsic integral action, which allows to track a constant set point with 0 steady-state error and reject a constant disturbance input (Maciejowski 2002).

THRUST MAGNITUDE AND ATTITUDE COMMANDS

Now we need to compute the total thrust magnitude command f_c and the attitude command D_c from the control input u(k) provided by the MPC. The first command is obtained where by simply taking the Euclidian norm of f. Using Eq. 4, f_c is obtained as

(39) $f_c=m\sqrt{u_x^2+u_y^2+{\left(u_z+g\right)}^2}.$

The attitude command D_c ∈ SO(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

(40) $e=\frac{f\times f_{\mathrm{xy}}}{\parallel f\times f_{\mathrm{xy}}\parallel },$

where: f_xy is the projection of f on the X_R' - Y_R' plane.

The corresponding attitude command is given by (Shuster 1993)

(41) $D\left(\mathit{\phi };e\right)=\cos \mathit{\phi }{I}_3+\left(1-\cos \mathit{\phi }\right){\mathrm{ee}}^T-\sin \mathit{\phi }\left[e\times \right].$

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.

(42) $D_c=D\left(\mathit{\psi }{;Z}_B\right)D\left(\mathit{\phi };e\right),$

where

(43) $D\left(\mathit{\psi }{,Z}_B\right)=\left[\begin{array}{ccc}\cos \mathit{\psi }& \sin \mathit{\psi }& 0\\ -\sin \mathit{\psi }& \cos \mathit{\psi }& 0\\ 0& 0& 1\end{array}\right].$

COMPUTATIONAL SIMULATIONS

SIMULATION PARAMETERS

The simulations are implemented in MATLAB/Simulink. The non-linear 6 DOF dynamics of an MAV is integrated using the Runge-Kutta-4 method with an integration step of 0.001 s. The vehicle's attitude is modeled using Euler angles in the rotation sequence 1-2-3. The vehicle's mass is m = 1 kg and the gravitational acceleration is assumed to be g = 9.81 m/s². The vehicle's inertia matrix is

(44) $J=\left[\begin{array}{ccc}0.0171& 0& 0\\ 0& 0.0171& 0\\ 0& 0& 0.0286\end{array}\right]{\mathrm{kmg}}^2.$

The 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. The prediction and control horizons are set to N = 80 and M = 5, respectively. The maximum and minimum position bounds are r_max = [6 6 3]^T m and r_min= [0 0 0]^T, respectively. The 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₃ N² is taken into account.

The position commands consist of a sequence of line segments from the initial position r_i = [1 1 0]^T m to the final one r_f = [5 5 1]^T m passing by the waypoints w₁ = [1 1 1]^T m; w₂ = [2 2 2]^T m; w₃ = [4 2 2]^Tm; w₄ = [5 3 2]^T m and w₅ = [5 5 2]^T m. A total of 15 MC simulations with 300 realizations are carried out considering all the combinations of 3 different speed command v values (0.5; 1.0 and 2.0 m/s) and 5 different maximum inclination ϕ_max values (10°; 15°; 20°; 25° and 30°).

The following figure of merit is used to evaluate the position control error:

(45) $e_q\triangleq \frac{1}{N_r\sqrt{k_f}}\sum _{i=1}^{N_r}\sqrt{\sum _{k=1}^{k_f}{\left(r_{d,q}\left(k\right)-r_q^i\left(k\right)\right)}^2},$

for q equal to x, y or z; r⁽ⁱ⁾ _q(k) denotes the i^th realization of r_q(k).

For evaluating the frequency of constraint violation, the following figure of merit is adopted:

(46) $I_l\triangleq \frac{1}{N_r{k}_f}{\sum _{i=1}^{N_r}{M}_l}^{(i)},$

where: M_l ⁽ⁱ⁾ 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.

The attitude controllers chosen for the present simulation are uncoupled proportional-derivative control laws tuned so as to make the attitude dynamics have a bandwidth significantly larger than the bandwidth of the position control dynamics.

SIMULATION RESULTS

The 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.

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

v (m/s)	ϕmax (deg)	ex (m)	ey (m)	ez (m)	Ix (%)	Iy (%)	Iz (%)	Iϕ (%)	Ifmax (%)	Ifmin (%)
0.5	10	0.028	0.037	0.021	0.00	0.00	0.00	0.58	0.00	0.00
	15	0.010	0.014	0.014	0.00	0.00	0.00	0.00	0.00	0.00
	20	0.009	0.013	0.014	0.00	0.00	0.00	0.00	0.00	0.00
	25	0.009	0.012	0.013	0.00	0.00	0.00	0.00	0.00	0.00
	30	0.009	0.012	0.013	0.00	0.00	0.00	0.00	0.00	0.00
1.0	10	0.057	0.071	0.060	0.00	0.00	0.00	1.54	0.00	0.00
	15	0.021	0.032	0.039	0.00	0.00	0.00	0.00	0.00	0.00
	20	0.021	0.031	0.039	0.00	0.00	0.00	0.00	0.00	0.00
	25	0.020	0.030	0.039	0.00	0.00	0.00	0.00	0.00	0.00
	30	0.020	0.030	0.039	0.00	0.00	0.00	0.00	0.00	0.00
2.0	10	0.211	0.258	0.216	0.00	0.00	0.00	15.6	0.37	0.36
	15	0.052	0.109	0.097	0.00	0.00	0.00	3.38	0.00	0.00
	20	0.042	0.097	0.095	0.00	0.00	0.00	0.00	0.00	0.00
	25	0.042	0.093	0.093	0.00	0.00	0.00	0.00	0.00	0.00
	30	0.042	0.088	0.092	0.00	0.00	0.00	0.00	0.00	0.00

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 different values of ϕ_max (10°; 20°; 30°). One can see that the standard deviation decreases as ϕ_max increases. This 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.

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.

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 different values of v (0.5; 1.0 and 2.0 m/s).

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.

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.

The 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 sufficient lateral acceleration to follow a large speed command and simultaneously reject the disturbance force. It causes a large position error (Fig. 9).

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
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.

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 thrustmagnitude f_c. For details, Fig. 10 shows ϕ and f_c, while Fig. 11 shows the corresponding position control performance.

Figure 10
Inclination angle ϕ and thrust magnitude f_c in the best case (ϕ_max = 30° and v = 0.5 m/s). The gray lines are the MC realizations; the dashed red lines are the constraints.

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.

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.

ACKNOWLEDGMENTS

The first author would like to thank Fundação de Amparo à Pesquisa do Estado do Amazonas for providing his master scholarship during the project. Furthermore, we are grateful to Conselho Nacional de Desenvolvimento Científico e Tecnológico for supporting the project under the grant 475251/2013.

REFERENCES

• Alexis K, Nikolakopoulos G, Tzes A (2012) Model predictive quadrotor control: attitude, altitude and position experimental studies. IET Control Theory Appl 6(12):1812-1827. doi: 10.1049/iet-cta.2011.0348
  » https://doi.org/10.1049/iet-cta.2011.0348
• Beard R, Kingston D, Quigley M, Snyder D, Christiansen R, Johnson W, McLain T, Goodrich MA (2005) Autonomous vehicle technologies for small fixed-wing UAVS. J Aero Comput Inform Comm 2(1):92-108. doi: 10.2514/1.8371
  » https://doi.org/10.2514/1.8371
• Bouabdallah S, Murrieri P, Siegwart R (2004) Design and control of an indoor micro quadrotor. Proceedings of the IEEE International Conference on Robotics and Automation; New Orleans, USA.
• Bouabdallah S, Siegwart R (2005) Backstepping and sliding-mode techniques applied to an indoor micro quadrotor. Proceedings of the IEEE International Conference on Robotics and Automation; Barcelona, USA.
• Camacho EF, Bordons C (1998) Model predictive control. London: Springer.
• Castillo P, Lozano R, Dzul A (2005) Stabilization of a mini rotorcraft with four rotors. IEEE Contr Syst Mag 25(6):45-55. doi: 10.1109/MCS.2005.1550152
  » https://doi.org/10.1109/MCS.2005.1550152
• Castillo P, Munoz LE, Santos O (2014) Robust control algorithm for a rotorcraft disturbed by crosswind. IEEE Trans Aero Electron Syst 50(1):756-763. doi: 10.1109/TAES.2013.110136
  » https://doi.org/10.1109/TAES.2013.110136
• Chen X, Wang L, Xung J (2013) Cascaded model predictive control of a quadrotor UAV. Proceedings of the 3th Australian Control Conference; Fremantle, Australia.
• Cheviron T, Hamel T, Mahony R, Baldwin G (2007) Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV. Proceedings of the IEEE International Conference on Robotics and Automation; Rome, Italy.
• Cunha R, Cabecinhas D, Silvestre C (2009) Nonlinear trajectory tracking control of a quadrotor vehicle. Proceedings of the 7th European Control Conference; Budapest, Hungary.
• Elfes A, Bueno SS, Bergerman M, Ramos JG (1998) A semiautonomous robotic airship for environmental monitoring missions. Proceedings of the IEEE International Conference on Robotics and Automation; Leuven, Belgium.
• Er MJ, Yuan S, Wang N (2013) Development control and navigation of octocopter. Proceedings of the 10th International Conference on Control and Automation; Hangzhou, China.
• Gonçalves PFSM, Brusnicki R, Santos DA (2013) Attitude determination of multirotors using camera vector measurements. Proceedings of the 22nd International Congress of Mechanical Engineering; Ribeirão Preto, Brazil.
• Gupte S, Mohandas PIT, Conrad JM (2012) A survey of quadrotor unmanned aerial vehicles. Proceedings of the IEEE SoutheastCon; Orlando, USA.
• Hua MD, Hamel T, Morin P, Samson C (2009) A control approach for thrust-propelled underactuated vehicles and its application to VTOL drones. IEEE Trans Automat Contr 54(8):1837-1853. doi: 10.1109/TAC.2009.2024569
  » https://doi.org/10.1109/TAC.2009.2024569
• Lopes RV, Santana PHRQA, Borges GA, Ishihara JY (2011) Model predictive control applied to tracking and attitude stabilization of a VTOL quadrotor aircraft. Proceedings of the 21st International Congress of Mechanical Engineering; Natal, Brazil.
• Maciejowski JM (2002) Predictive control with constraints. Harlow; New York: Prentice-Hall.
• Madani T, Benallegue A (2007) Sliding mode observer and backstepping control for a quadrotor unmanned aerial vehicles. Proceedings of the American Control Conference; New York, USA.
• Mahony R, Kumar V, Corke P (2012) Multirotor aerial vehicles: modeling, estimation, and control of quadrotor. IEEE Robot Autom Mag 19(3):20-32. doi: 10.1109/MRA.2012.2206474
  » https://doi.org/10.1109/MRA.2012.2206474
• Mian AA, Daobo W (2008) Nonlinear flight control strategy for an underactuated quadrotor aerial robot. Proceedings of the International Conference on Networking, Sensing and Control; Sanya, China.
• Mistler V, Benallegue A, M'Sirdi N (2013) Exact linearization and noninteracting control of a 4 rotors helicopter via dynamic feedback. Proceedings of the 10th IEEE International Workshop on Robot and Human Interactive Communication; Bordeaux, France.
• Nemra A, Aouf N (2010) Robust INS/GPS sensor fusion for UAV localization using SDRE nonlinear filtering. IEEE Sensor J 10(4):789-798. doi: 10.1109/JSEN.2009.2034730
  » https://doi.org/10.1109/JSEN.2009.2034730
• Raffo GV, Ortega MG, Rubio FR (2010) An integral predictive/nonlinear H _∞ control structure for a quadrotor helicopter. Automatica 46(1):29-39. doi: 10.1016/j.automatica.2009.10.018
  » https://doi.org/10.1016/j.automatica.2009.10.018
• Rossiter JA (2003) Model-based predictive control: a practical approach. Boca Raton: CRC Press.
• Santos DA, Saotome O, Cela A (2013) Trajectory control of multirotor helicopters with thrust vector constraints. Proceedings of the 21st Mediterranean Conference on Control and Automation; Chania, Greece.
• Saripalli S, Montgomery JF, Sukhatme GS (2003) Visually-guided landing of an unmanned aerial vehicle. IEEE Trans Robot Autom 19(3):371-380. doi: 10.1109/TRA.2003.810239
  » https://doi.org/10.1109/TRA.2003.810239
• Shuster MD (1993) A survey of attitude representations. J Astronaut Sci 41(4):439-517.
• Teel AR (1992) Global stabilization and restricted tracking for multiple integrators with bounded controls. Syst Contr Lett 18(3):165-171. doi: 10.1016/0167-6911(92)90001-9
  » https://doi.org/10.1016/0167-6911(92)90001-9
• Xiao-Hong W, Gui-Li X, Yu-Peng T, Biao W, Jing-Dong W (2012) UAV's automatic landing in all weather based on the cooperative object and computer vision. Proceedings of the 2nd International Conference on Instrumentation, Measurement, Computer, Communication and Control; Harbin, China.
• Xu G, Zhang Y, Ji S, Cheng Y, Tian Y (2009) Research on computer vision-based for UAV autonomous landing on a ship. Pattern Recogn Lett 30(6):600-605. doi: 10.1016/j.patrec.2008.12.011
  » https://doi.org/10.1016/j.patrec.2008.12.011
• Yan J, Santos DA, Bernstein DS (2014) Adaptive control with convex and saturation constraints. IET Control Theory Appl 8(12):1096-1104. doi: 10.1115/DSCC2013-3877
  » https://doi.org/10.1115/DSCC2013-3877

Publication Dates

• Publication in this collection
  Jan-Mar 2017

History

• Received
  03 Feb 2016
• Accepted
  22 Aug 2016