Abstract
This paper presents a solution to stability and trajectory tracking of a quadrotor system using a model predictive controller designed using a type of orthonormal functions called Laguerre functions. A linear model of the quadrotor is derived and used. To check the performance of the controller we compare it with a linear quadratic regulator and a more traditional linear state space MPC. Simulations for trajectory tracking and stability are performed in MATLAB and results provided in this paper.
1. Introduction
A quadrotor is a helicopter which has four equally spaced rotors, usually arranged at the corners of a square body. When these rotors spin, they push air downwards and in the process create a thrust force that keeps the quadrotor aloft. Unlike the conventional helicopter (with two rotors) which requires a swashplate mechanism in order to have more degrees of freedom, such mechanism is not needed in quadrotor systems since the two additional rotors provide the same level of control as that with conventional helicopters fixed with swashplate mechanism.
The task to control a quadrotor is a fundamentally difficult and interesting problem. With six degrees of freedom (three translational and three rotational) and only four independent inputs (rotor speeds), quadrotors are severely underactuated. In order to achieve six degrees of freedom, rotational and translational motion are coupled. The resulting dynamics are highly nonlinear, especially after accounting for the complicated aerodynamic effects. Finally, unlike ground vehicles, quadrotors have very little friction to prevent their motion, so they must provide their own damping in order to stop moving and remain stable. Put together, these factors create a very challenging control problem.
2. Literature Review
Amongst the many control techniques used in quadrotor control PID, LQR and recently MPC have been widely used.
While PID is the most popular choice for controlling several types of processes, it turns out that tuning becomes a big challenge especially for MIMO systems like the quadrotor. Several techniques have been used to divide the quadrotor control problem to several SISO systems. While this has worked in some cases it has to be pointed out that this takes away the most natural way of controlling the system and designing many SISO controllers may make maintenance more difficult.
LQR is a relatively modern control method that is very powerful yet limited to applications where linear system models are available. To use this method to control nonlinear systems a linear model has to be obtained from the corresponding nonlinear model. To evaluate the performance of the LQR controller we need to compare it with results obtained from other control techniques. This means designing the same system using another control technique which could be cumbersome. Because of this, LQR is only very popular to use in inherently linear systems or where results of control obtained using other control techniques are available for comparison.
On the other hand, MPC can handle both linear and nonlinear systems [1]. Comparisons between the nonlinear system and its corresponding linear model can easily be made with very little modification. Compared to PID, tuning MPC is easier even for complex MIMO systems.
However, in trying to track the reference trajectory in an optimal way and at the same time obey the constraints imposed MPC makes much more calculations than either PID or LQR. This computation burden makes MPC less suitable for “fast” processes. It requires very powerful processors to be used in hardware implementation. Not surprisingly a lot of effort is being put in to reduce the computations so that MPC can be faster and easily be implemented even on low cost processors.
In [2] Wang proposed a method of designing MPC using orthonormal functions. This method makes less computations than the traditional MPC. With reduced number of computations this technique can be used where rapid system dynamics are required. We will use this method in our task to control a quadrotor using MPC.
Other MPC applications to quadrotor can be found in [3, 4]. In [4] the control structure consists of a controller based on MPC to track the reference trajectory and a second one based on a nonlinear control technique to stabilize the rotational movements. Similarly in [3] an MPC controller is designed to control position, and a second feedforward controller is used to perform stabilization of the quadrotor system.
In both [3, 4] the control loop uses two controllers: the controller based on MPC which is used for tracking the position reference trajectory and a second controller (which is designed using other techniques) that is used for stabilization of the rotational movements. In this paper, MPC is used to control both the position and rotational movements. This is good for uniformity and ease of maintenance.
3. Kinematics and Dynamics of a Quadrotor
A quadrotor is completely described by twelve states. The first six states describe translational motion of the quadrotor. These states are , and which represent position in inertial frame and , and which denote velocity vectors along the body frame. Similarly, the remaining six variables describe rotational motions. These are , , and commonly known as Euler angles and , , and which denote angular velocities.
A free body diagram of a quadrotor is shown in Figure 1. The force and moment (where , and ) produced by the rotors can be described by their angular speed as and , respectively. Similarly, torques , and and the thrust force () are related to angular speed of rotors as where and is the length of arm of the quadrotor.
(a)
(b)
Equations describing the kinematics of the quadrotor are given by where , stands for , stand for , and stands for . Similarly, dynamic equations can be derived using Newtons laws of motion which are listed in where , , and are the respective moment of inertia about , , and axes of the quadrotor. For the simulation of the controller of quadrotor we have considered , , , and as the inputs to the system.
3.1. Linearization
In order to design a linear controller it is vital that (2) to (3) be simplified to linear ones. To do that roll, pitch, and heading angles are assumed to operate within very small angles [5] which leads to the following:
4. Controller Design
In order to control attitude (, , and ), altitude (), and position (, ), we propose a control architecture given in Figure 2 where we have utilized MPC to design the three controllers. The attitude controller generates , , and actuator signals whereas the altitude controller generates required thrust for the system. The position controller controls position in the and directions and generates control signals and which, when combined with the signal, act as reference signals to the attitude controller.
In the next section we look at the design of each of the three controllers.
4.1. Discretization
In this section, discrete-time state-space equations necessary for altitude, position, and attitude control are derived.
4.1.1. Altitude Controller
The linear equations responsible for altitude control are These two equations can be combined to give the following second order ODE: This second order ODE can be simplified to where or . Equation (7) can be written in state-space form as in the following equation: Using the forward Euler method and choosing a sampling interval , we can express (8) in discrete form as in The states and represent the position and velocity, respectively, and we could choose a proper output matrix depending on which state we are going to measure. In this paper we are interested in the position and, therefore, we will use the output matrix .
4.1.2. Position Controller
The equations responsible for position and velocity control in direction are These two equations can be combined to give the second order ODE: Similarly the equations responsible for position and velocity control in direction are which give rise to the second order ODE: Equations (11) and (13) can be combined and form a state-space equation such as As stated before, we can use the forward Euler method to express (14) in discrete form. Consider The states and represent the position while and represent velocity. Since we are interested in the position, we will use the output matrix but we could have used a different output matrix if we were interested in the velocity. But it is important to note that we can not use a or as there is no guarantee that we will be able to control each of the measured outputs independently with zero steady state errors. This is generally the case for a system with inputs, outputs, and [6].
4.1.3. Attitude Controller
In attitude control we are interested in controlling the roll , pitch , and yaw (heading) such that we generate appropriate torque signals responsible for steering the quadrotor in the desired direction with the required attitude. The equations responsible for roll control are These two equations can be combined to give the second order ODE: Similarly the equations responsible for pitch are given by which give rise to the second order ODE: Also the equations responsible for heading are expressed as which, like in roll and pitch, give rise to the second order ODE: Equations (17), (19), and (21) can be combined and written in state-space form as in where , , and are the torques required to give the required roll, pitch, and yaw, respectively.
Again, using the forward Euler method we express (22) in discrete form as Since we are interested in measuring , , and , we will use the output matrix .
So far we have formulated the necessary equations used to build the three controllers. Figure 2 shows, in block diagram form, what we have done so far. But that is not the whole story. In the next section we will look at exactly how model predictive control is applied to the quadrotor system using the discrete state-space equations formulated in this section.
5. Model Predictive Control
One of the many tenets deployed in control of complex systems like the quadrotor using MPC is to use models that reduce the computation burden which is a necessity when it comes to online implementation. It is also agreed upon that linear models perform better in this respect than the nonlinear models they represent. In this and subsequent sections the performance of a qudrotor controller designed using Laguerre-based MPC (LMPC) is compared with that designed using the more common linear state-space approach presented in [7]. We will call the linear state-space method SS-MPC.
5.1. Linear State-Space MPC (SS-MPC)
Linear state-space MPC is modeled using a linear state-space relation and plant constraints are modeled using linear equalities and inequalities. When combined with convex quadratic cost function this implies that the desired control action can be obtained, at each sample interval, via the solution of a corresponding quadratic program. This is attractive because the quadratic programs can be solved efficiently online [7]. The general block diagram representing SSMPC is shown in Figure 3.
5.1.1. Plant
The plant is modeled by using the following state-space system: where is the state noise and is the measurement noise. Both and are assumed to be Gaussian distributed with zero mean, respective covariances of and , and cross covariance . This is represented mathematically as
5.1.2. Observer
Using Gaussian assumptions stated above it is possible to make optimal predictions of state and output using the Kalman filter as The closed loop gain is found by solving the discrete-time algebraic Riccati equation (DARE):
5.1.3. Optimal Estimation of State and Output
The general equation representing the optimal estimate of state at instant and written in terms of the initial state and future control inputs is given by Similarly the output equation is given by The vector containing all output vectors can be written as while that containing all control actions can be written as Therefore, we can write as where
5.1.4. Cost Function
The prime goal in MPC is to reject disturbances whilst tracking a reference signal. But at each instant it has to ensure that the control signal is within reasonable range that is practical for actuation. An objective function that combines these two tasks is where both and are assumed to be symmetric and positive definite.
5.2. Laguerre-Based MPC (LMPC)
Model predictive control designed using Laguerre functions is developed and summarized in [2]. The author presented this design technique for both single-variable and multivariable systems. Fortunately, the derivations for the single-variable case and the multivariable case are very similar and knowing one would give enough insight into the derivation of the other. For brevity only the single-variable case is presented and it is hoped that this is enough to set forth the LMPC design technique.
Consider a plant with inputs, outputs, and states as described by (35) where the subscript stands for model where is the input variable, is the process output, and is the state vector. is the input disturbance and is assumed to be a sequence of integrated white noise.
The plant described by (35) can be expressed in augmented state-space form [2, 6] as where , , , and the state . is a zero mean white noise sequence related to the disturbance by the difference equation [6].
5.2.1. Design Framework
In designing MPC using Laguerre functions the control trajectory is expressed using a set of orthonormal functions called Laguerre functions. Since the state and output vectors can also be described in terms of , it follows that they too can be expressed using Laguerre functions.
At sampling instant , the state variable is available through measurement. The future control trajectory is given by where is the control horizon.
Laguerre functions can approximate the incremental terms contained in . The -transfroms of the discrete-time Laguerre functions are written as In the discrete-time Laguerre network of (38), is the pole of the discrete-time Laguerre network and for stability of the network. The parameter is called the scaling factor and is the number of Laguerre terms used in the network. Note that with the first term as .
Let be the inverse -transform of the th term in the discrete Laguerre network and the vector containing all inverse -transform terms. Then taking advantage of (39), successive inverse -transform vectors are obtained through the difference equation: The matrix has size and is a function of parameters and . The initial condition is given by In the case where , matrix and the initial condition are respectively.
At sampling instant , the control trajectory is described using Laguerre functions as in where with as the prediction horizon, terms used in the expansion and is the inverse -transform of the th term in the discrete Laguerre network.
Equation (44) can be rewritten as where , , and the coefficients are obtained from system data.
At an arbitrary future instant , the state is described using Laguerre functions as Similarly, the output is described as
5.2.2. Cost Function
The cost function is used to choose the optimal control trajectory to bring the predicted output as close as possible to the set-point. The cost function minimizes the error between the set-point signal and the output in the shortest possible time by carefully tuning the weighting matrices and .
5.2.3. Cost Function Minimization
The state variable in (46) can be rewritten as where .
By substituting (49) into (48) and performing the partial derivative , the Laguerre coefficients vector is found to be with and .
5.2.4. Receding Horizon Control
In receding horizon control (RHC) only the first term in , that is, , is implemented at instant . The rest of the sequence is ignored. Only the most recent measurement is taken to form the state vector for calculation of the control signal. This procedure is repeated in real-time to give the receding horizon control law [6, 8].
5.2.5. Stability
In stability analysis we make use of the technique of exponential data weighting originated by Anderson and Moore [9] and applied to MPC in [6]. More specifically we will concentrate on the discrete exponential factor where is the sampling interval and the discrete weights form a geometric sequence .
The proposed cost function is similar to the one used in linear quadratic regulator (LQR) systems but with the inclusion of discrete weights For , the exponential weights , , put more emphasis on the current state and less emphasis on subsequent future states.
The exponentially weighted cost function can be expressed more compactly as with state equation and and .
With , , and , minimizing the cost function is equivalent to the DLQR problem which is solved using algebraic Riccati equation (54). The pair is assumed to be controllable and observable with . Then there is a stabilizing state feedback control gain matrix , that gives a stable closed loop system with all its eigenvalues inside the unit circle and the the closed loop system being described by From (55), the transformed system has all its eigenvalues inside the unit circle by taking . So which gives Thus by choosing there is a great chance that the system will be stable. Several simulations on the quadrotor system indicate that a choice of slightly greater than unity makes the system stable almost all the time.
6. Simulation Parameters and Results
In order to illustrate the controller’s stability we will check if the eigenvalues of each of the three individual controllers appear inside a unit circle. And we will use a trajectory in a 3D plane to check the overall controller’s ability to track a desired trajectory.
Table 1 shows the parameters of the quadrotor used in simulation.
6.1. Stability Analysis
In this section we look at the location of eigenvalues of LMPC and check if they fall within the unit circle for stability of the system. We also compare them to those obtained from the optimal DLQR system. To ensure stability as explained in Section 5.2.5 we use .
By looking at Figure 4 we note that all the eigenvalues appear inside the unit circle as required. Also the eigenvalues obtained from LMPC closely match with those of the optimal DLQR. This not only shows the system is stable but also shows the controllers perform optimally.
6.2. Trajectory Tracking
In this section we look at the performance of LMPC when used to track a 3D trajectory. Also LMPC is compared to SS-MPC and individual state trajectories are observed. The starting point of the trajectory is and final point is . Parameter while . See Figure 5.
Clearly both LMPC and SS-MPC track the desired trajectory very well. See Figures 5, 6 and 7. However, LMPC performs better than SS-MPC in that it requires only five parameters () to capture the control trajectory compared to SS-MPC’s minimum of 25 () parameters (see Table 2 for relationship of and with ).
(a)
(b)
(a)
(b)
6.3. Control Horizon in Relation to and
The control horizon () is related to the parameters and [2] by
6.3.1. Constant
Using (58) and choosing a constant parameter we get Table 2.
Thus we can increase the control horizon by increasing the parameter without necessarily changing the order .
6.3.2. Constant Control Horizon
Assuming we want to achieve a control horizon of 30, we choose one parameter usually because it is an integer and directly defines the order of the discrete Laguerre network. Therefater, we simply use (58) to find the corresponding scaling factor as shown in Table 3.
Figures 8, 9, 10, 11, 12, and 13 compare two LMPC designs both of which yield a control horizon of 30. One design uses and the other . Thus we can choose a Laguerre network with lower number of terms (that gives lower computation burden) but with a larger scaling factor to achieve the same control horizon. The computational cost is lower if a smaller number of parameters are used. For , only one Laguerre term is used to capture the control trajectory while requires nine Laguerre terms to capture the same control trajectory.
7. Conclusion
This paper has presented a solution to stability and trajectory tracking of quadrotor systems using a model predictive controller designed by using a special type of orthonormal functions called Laguerre functions. A quadratic cost function similar to that used in linear quadratic regulator (LQR) has been used. In stability analysis LMPC has been compared to the optimal DLQR system whereas in trajectory tracking LMPC has been compared to a popular linear state-space MPC desgin technique in which plant constraints are modeled using linear equalities and inequalities. The results from the simulations indicate that the controller performs very well and is considered feasible. With this perceived feasibility LMPC offers the added advantage that it can handle systems where rapid sampling and more complicated process dynamics are required [5, 10]. By selecting appropriate values of and LMPC reduces the number of parameters required for accurate prediction when using the traditional (MPC) approach. This is a big advantage in that, with the reduced number of parameters, online implementation might be possible where the traditional MPC would have failed.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgment
This work was supported by the National Research Foundation of Korea (NRF) grant funded by the Korean government (MEST) (no. 2013R1A2A2A01068127 and no. 2013R1A1A2A10009458).