Discrete Integral Optimal Controller for Quadrotor Attitude Stabilization: Experimental Results

Abstract

The Unmanned Aerial Vehicle (UAV) attitude stabilization problem has been dealt with in many previous works through applying a vast range of philosophies of control strategies. In this paper, a discrete controller based on a Linear Quadratic Regulator (LQR) plus integral action is synthesized to stabilize the attitude and altitude of a quadrotor helicopter. This kind of control strategy allows us to reduce the energy consumption rate, and the desired UAV behavior is properly achieved. Experimental tests are conducted with external disturbances such as crosswinds deliberately added to affect the performance of the aerial vehicle. This provides experimental evidence that the integral part considered in the proposed control strategy contributes to improving the performance of the vehicle under external disturbances. In fact, a comparative analysis of potential and kinetic energy consumption is developed between the Optimal Integral Controller (OIC) and a Proportional Integral Derivative Controller (PID), allowing us to determine the level of improvement of the closed-loop system when the discrete Integral Optimal Controller is applied.

1. Introduction

In recent years, several works have been reported in the literature addressing the application of optimal control of quadrotors [1,2]. However, this issue remains interesting to the scientific community because it represents a current challenge from the point of view of control theory and engineering (mainly its real-time applications). Many techniques related to optimal control philosophy have been applied to UAVs, but this paper proposes to add an integral action to Linear Quadratic Regulator, in order to improve the performance of the closed-loop system. Moreover, both the mathematical model and synthesized control strategy are obtained in discrete time domain, ensuring a better representation to be programmed in a microcontroller such that Rabbit 4300. In this sens, recent reported works have demonstrated that it is possible to ensure the stability in discrete-time domain for multi-input-multi-output (MIMO) systems. This has been achieved through a generalization of the Letov formula, as mentioned in [3].

A typical control strategy with integral action is the Proportional-Integral-Derivative (PID) controller, which is widely used in industrial processes and devices because it can be heuristically tuned independently of the mathematical model of the system and can be used in both linear and nonlinear systems. However, there also exist many graphical and theoretical methods to tune it considering the dynamical behavior or the mathematical model of system.

It is well known that the integral action of a PID controller provides a correction for steady-state tracking error even in the presence of uncertainties [4], providing some degree of robustness to the control loop. Although this three-action controller is a powerful idea, sometimes it is not efficient or appropriate for a specific tasks. Then, it can be improved by modifying the control parameters with an adaptive scheme or by adding a nonlinear part.

This contribution uses the advantages of the integral action of the PID controller combined with modern control techniques through regulation via optimal control in order to improve the performance of a quadrotor when it executes an attitude and altitude stabilization task. Integral control applied to Unmanned Aerial Vehicles has been explored in some reported works. An integral predictive and nonlinear robust control strategy was synthesized in [5], wherein the authors solve the path-following problem for a quadrotor helicopter. The proposed control structure has a hierarchical scheme consisting of a model predictive controller to track the reference trajectory together with a nonlinear $H_{\infty }$ controller to stabilize the rotational movements of the considered vehicle; satisfactory simulation results were presented. In [6], an Integral Backstepping controller and motion planning are combined to stabilize the helicopter using point-to-point steering stabilization. Simulation results were presented to test the performance of the closed-loop system and its robustness. The same technique was presented in [7]: the goal of this work was to stabilize the attitude, altitude, and position of the vehicle. Satisfactory results for autonomous take-off, hover, landing and collision-avoidance tasks were presented, and all were validated on the OS4 simulation platform.

In addition, a comparison between PID, Linear Quadratic Regulator (LQR) and nonlinear controllers (Adaptive Integral Backstepping Controller) was exposed in [8]. A nonlinear control approach was proposed based on a recursive Lyapunov methodology using the Backstepping technique and an adaptive scheme. Satisfactory simulations and real-time experiments were conducted. In [9], LQR continuous control was used to stabilize the attitude and altitude of an Octocopter. Numerical simulations demonstrated the effectiveness of the control strategy under nominal conditions, and the authors improved the LQR by adding integral action to the altitude controller. In [10], once again an LQR methodology and integral state augmentation were adopted to achieve the desired performance of the control system. The unmeasured state variables were estimated by means of a reduced-order observer. Satisfactory simulation results for the UAV helicopter were presented. Toledo et al. [11] conducted similar work in which they used a control scheme based on Integral Backstepping with sliding modes applied to a multi-rotor vehicle. This control methodology was experimentally tested using the vision system Optitrack. Results were reported for the altitude z and displacement on the x- and y-axes. However, no analysis of energy consumption was presented, nor were real-time tests with external disturbances conducted and reported. In [12], Elkhatem published LQR and LQR-PI controllers that are applied to a quadcopter. The high performance and robustness of the Linear Quadratic Regulator controllers ensure the ability to reduce deviations in state trajectories with minimal control effort. In this work, the weighting matrices are automatically adjusted through a novel method using the full state of the flying robot. Feasibility and performance of the closed-loop system is only tested by simulation routines.

In the literature, it is possible to find documents in which the LQR technique is combined with other types of controllers, such as fuzzy controllers. For example, in [13], Malik presents the development of a longitudinal controller design for an autonomous unmanned aerial vehicle (UAV). In this work, the researchers proposed a dual-loop (inner–outer loop) control method based on intelligent algorithms. The inner feedback loop of controller uses a Linear Quadratic Regulator (LQR) to ensure adaptive stability. Meanwhile, the outer loop controller employs a Fuzzy-PID algorithm for deal with the trajectory tracking task.

Moreover, neuro-fuzzy controllers have also been recently used for the control of UAVs. In [14], Jinjun Rao et al. developed a position control approach for a quadrotor using a cascade Fuzzy Neural Network (FNN). This approach requires offline neural network training, combining the benefits of fuzzy systems and neural networks. According to authors, this fuzzy control strategy demonstrated its ability to minimize the overshoot and the settling time. This was tested by conducting flight simulations and real-time flights using a DJI Tello quadrotor UAV, showing an acceptable position control performance. In fact, one of advantages of neuro-fuzzy controllers is their ability to handle nonlinear systems, which offers better adaptability. Particularly, the controller architecture proposed in that work allows to optimize the use of the robot’s energy resources and also providing robustness to the control loop. Also, this produces a balance between adaptability and efficiency, making it particularly useful for UAV applications where energy efficiency and robust control are crucial. That type of controller can also be used in other fields such as the study of seismic structural control. In this field, the contributions reported works by Abbas and his collaborators in [15,16,17] should also be mentioned, they use neuro-fuzzy controllers and a PID controllers to tackle both stabilization and trajectory tracking task for aerial robots.

In this contribution, UAV stabilization is tackled using a controller that combines an optimal strategy (LQR) with the integral action. A difference from other reported works such as [5,9,10,12] is that this optimal synthesized controller is tested in a real-time setting, providing experimental results for the altitude stabilization problem in a four-rotor helicopter. To control the position and orientation of the vehicle, the system was subdivided into four subsystems, as was proposed in [18], and the integral action is added as an additional state variable [4] in the four subsystems. A previous exact linearization was performed on the dynamical model in order to remove the Coriolis terms and transform the rotational dynamics in the second-order differential equation depending on external torque inputs. So for control of the altitude and attitude of the UAV, four optimal controls with integral action are proposed: one for each subsystem. The controllers for rotational dynamics are synthesized using the assumption that pitch, roll and yaw are inside a bounded region around the origin, which is an equilibrium point for this aerial vehicle. The proposed optimal control strategy assumes that all state variables are available. Our proposal is experimentally tested in takeoff and altitude stabilization tasks. An Optitrack vision system is used to obtain the whole state of the vehicle, and satisfactory results are obtained when the optimal discretized controller with integral action is applied.

So the main contributions of this work are:

• Synthesis, analysis and implementation of Optimal Integral Control (OIC) tuned under the QR approach and applied to trajectory tracking of takeoff and hover flight of UAVs.
• Experimental validation of the OIC by real-time tests in the presence of induced crosswind disturbances applied during the trajectory tracking of takeoff and hover flight of a UAV, allowing the analysis of the robustness of the closed loop with the proposed control scheme.
• Comparison of the kinetic and potential energy between the PID and the Optimal Integral Control when a trajectory tracking task is executed for the take-off and hover-flight phases of the UAV in the presence of induced crosswind.

The paper is organized as follows: The nomenclature and symbols used throughout this document are presented in Section “Nomenclature”, while the introduction is reported in Section 1. Moreover, Section 2 is devoted to showing the mathematical model of the UAV together with the synthesis of the proposed control law. In Section 3, the experimental platform is shown. Real-time experimental results are displayed in Section 4, and finally, the conclusion and discussion are reported in Section 5.

2. Control Strategy

In this section, we synthesize the proposed optimal controller with integral action added. Firstly, some basic concepts about integral control are briefly recalled in order to set up a discrete time control strategy to be applied to a quadcopter.

2.1. Integral Control

Consider the nonlinear system:

$$\stackrel{.}{\overline{x}}=f(\overline{x},\overline{u})$$

$$\overline{y}=h\left(\overline{x}\right),$$

where the state $\overline{x}$∈${\mathbb{R}}^n$, and the vector control $\overline{u}$∈${\mathbb{R}}^p$. The variables f and h are continuously differentiable functions in a domain $D_{\overline{x}}$ × $D_{\overline{u}}$ ⊂${\mathbb{R}}^n$ × ${\mathbb{R}}^p$, and $\overline{y}$ ∈ ${\mathbb{R}}^p$ is the controlled output. Let ${\overline{y}}_R$∈ ${\mathbb{R}}^p$ be a constant reference; the integral control is a feedback state such that

$$\overline{y}\left(t\right)\to {\overline{y}}_R,t\to \infty$$

Assume that the controlled output $\overline{y}$ can be measured. Note that for our case $\overline{y}=\overline{x}$ because when using the Optitrack vision system, the complete state is measurable and therefore available. The regulation task will be achieved by stabilizing the system at an equilibrium point where $\overline{y}={\overline{y}}_R$. In order to maintain it in that equilibrium condition, there exists a pair $({\overline{x}}_{ss},{\overline{u}}_{ss})\in D_{\overline{x}}\times D_{\overline{u}}$ such that:

$$0=f({\overline{x}}_{ss},{\overline{u}}_{ss}),$$

$$0=h\left({\overline{x}}_{ss}\right)-{\overline{y}}_R.$$

Assume that these equations have a unique solution $({\overline{x}}_{ss},{\overline{u}}_{ss})$. Now, the integral action is included as follows: consider the tracking error $\overline{e}=$ $\overline{y}-{\overline{y}}_R$. Then, the following equivalence is defined

$$\stackrel{.}{\sigma }=\overline{e}=\overline{y}-{\overline{y}}_R,$$

So control will be obtained as a feedback function of $\overline{x}$ and $\overline{\sigma }$ such that in the closed loop there is an equilibrium point $(\overline{x},\overline{\sigma })$ with $\overline{x}=x_{ss}$. Assuming that the system is linearizable around $x_{ss},\overline{\sigma },u_{ss}$, it follows that:

$$\overline{\stackrel{.}{\xi }}=\left[\begin{array}{cc}A& 0\\ C& 0\end{array}\right]\overline{\xi }+\left[\begin{array}{c}B\\ 0\end{array}\right]v\triangleq \mathcal{A}\overline{\xi }+\mathcal{B}v,$$

(1)

with

$$\overline{\xi }=\left[\begin{array}{c}\overline{x}-x_{ss}\\ \overline{\sigma }-\overline{\sigma }\end{array}\right]$$

$v=\overline{u}-u_{ss}$ where:

$$A={\left.\frac{\partial f}{\partial \overline{x}}\right|}_{(\overline{x},\overline{u})=(x_{ss},u_{ss})},B={\left.\frac{\partial f}{\partial \overline{u}}\right|}_{(\overline{x},\overline{u})=(x_{ss},u_{ss})},$$

$$C={\left.\frac{\partial h}{\partial \overline{x}}\right|}_{\overline{x}=x_{ss}}$$

Assume that the pair ($A,B$) is controllable and

$$rank\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]=n+p.$$

Then $(\mathcal{A},\mathcal{B})$ is controllable [4]. Then, design a matrix $\mathcal{K}$ such that $\mathcal{A}+\mathcal{B}\mathcal{K}$ is Hurwitz [4]. Consider the partition for the matrix $\mathcal{K}$ as $[{\mathcal{K}}_1$ ${\mathcal{K}}_2]$. The control signal is then defined by:

$$\overline{u}={\mathcal{K}}_1(\overline{x}-x_{s}s)+{\mathcal{K}}_2(\overline{\sigma }-\overline{\sigma })+u_{ss}.$$

It is not difficult to verify that the closed-loop nonlinear system has a unique equilibrium point $(x_{ss},{\overline{\sigma }}_{ss})$ [4]. As is demonstrated in [4], the equilibrium point $(x_{ss},{\overline{\sigma }}_{ss})$ is exponentially stable, and all solutions starting close enough to this equilibrium point approach it as t tends to infinity. Then $\overline{y}\left(t\right)-{\overline{y}}_R\to 0$ as $t\to \infty$. Following these ideas, in this contribution, the matrix $\mathcal{K}$ is designed using the optimal control approach. For altitude, we take advantage of the fact that in the mathematical model for the altitude of the quadrotor, it can be stabilized by exact linearization, and then, it is not necessary to linearize the nonlinear dynamics. The integral action is used to minimize the effects of external disturbances on vehicle performance. Figure 1 shows the basic scheme for the control of integral action.

2.2. Mathematical Model of the Quadrotor

The following assumptions are considered in this paper to obtain a simplified version of the mathematical model of a vehicle [7,18]

• The quadcopter is a rigid and symmetric body.
• The center of gravity of the vehicle coincides with the origin of the body frame.
• The propellers are rigid and have a fixed pitch.
• At low velocities, aerodynamic effects can be neglected.
• The angles are restricted: $-\pi /2<\varphi <\pi /2,-\pi /2<\theta <\pi /2$ and $\psi =0$.

The dynamical model considered is that reported in [19,20] with the following structure:

$$\begin{array}{c}m\stackrel{..}{x}=-usin\theta \\ m\stackrel{..}{y}=ucos\theta sin\varphi \\ m\stackrel{..}{z}=ucos\theta cos\varphi -mg\\ \stackrel{..}{\varphi }={\tau }_{\varphi }\\ \stackrel{..}{\theta }={\tau }_{\theta }\\ \stackrel{..}{\psi }={\tau }_{\psi },\end{array}$$

(2)

where x and y are the displacements in the horizontal plane, z is the vertical position, $\psi$ is the yaw angle around the z-axis, $\theta$ is the pitch angle relative to the y-axis, and $\varphi$ is the roll angle around the x-axis. The control inputs are: u, ${\tau }_{\varphi }$, ${\tau }_{\theta }$, and ${\tau }_{\psi }$, with u the collective throttle generated by the four motors to lift the UAV; ${\tau }_{\varphi }$, ${\tau }_{\theta }$, and ${\tau }_{\psi }$ are the torques generated around the axes x, y, and z, respectively.

Here, the authors have assumed that there exists a previous controller $\tilde{\tau }=C(\eta ,\dot{\eta })\dot{\eta }+J\tau$ (see the mathematical model given by Equations (2.4) and (2.5), p. 34 of [18]), where $\eta ={(\varphi ,\theta ,\psi )}^T$ is the angular position vector, $C(\eta ,\dot{\eta })$ is the Coriolis terms matrix, and J is the inertia matrix. With this control, we arrive at the last three equations of the mathematical model given by (2), which define the resulting rotational dynamics $\tilde{\tau }={({\tau }_{\varphi },{\tau }_{\theta },{\tau }_{\psi })}^T$. Figure 2 shows a schematic representation of the positions and angles of the quadrotor.

Now, in order to obtain a discrete space state representation of the mathematical model of a flying robot, the following state variables are defined:

$$\begin{array}{ccc}\hfill x_1& =& x,x_2=\dot{x},x_3=y,x_4=\dot{y},\hfill \\ \hfill x_5& =& z,x_6=\dot{z},x_7=\varphi ,x_8=\dot{\varphi },\hfill \\ \hfill x_9& =& \theta ,x_{10}=\dot{\theta },x_{11}=\psi ,x_{12}=\dot{\psi },\hfill \end{array}$$

and discretizing the related continuous model by applying the Euler approximation considering a sampled period T, it becomes:

$$\begin{array}{c}{x}_1(k+1)=T{x}_2\left(k\right)+x_1\left(k\right)\hfill \\ x_2(k+1)=-\frac{T}{m}u\left(k\right)sin\left(x_9\left(k\right)\right)+x_2\left(k\right)\hfill \\ x_3(k+1)=T{x}_4\left(k\right)+x_3\left(k\right)\hfill \\ x_4(k+1)=\frac{T}{m}u\left(k\right)cos\left(x_9\left(k\right)\right)sin\left(x_7\left(k\right)\right)+x_4\left(k\right)-Tg\hfill \\ x_5(k+1)=T{x}_6\left(k\right)+x_5\left(k\right)\hfill \\ x_6(k+1)=\frac{T}{m}u\left(k\right)cos\left(x_9\left(k\right)\right)cos\left(x_7\left(k\right)\right)+x_6\left(k\right)-Tg\hfill \\ x_7(k+1)=T{x}_8\left(k\right)+x_7\left(k\right)\hfill \\ x_8(k+1)=T{\tau }_{\varphi }\left(k\right)+x_8\left(k\right)\hfill \\ x_9(k+1)=T{x}_{10}\left(k\right)+x_9\left(k\right)\hfill \\ x_{10}(k+1)=T{\tau }_{\theta }\left(k\right)+x_{10}\left(k\right)\hfill \\ x_{11}(k+1)=T{x}_{12}\left(k\right)+x_{11}\left(k\right)\hfill \\ x_{12}(k+1)=T{\tau }_{\psi }\left(k\right)+x_{12}\left(k\right)\hfill \end{array}$$

All of the above was performed in order to apply a digital version of a synthesized optimal controller with integral action to a quadrotor helicopter.

2.3. Discrete Model and Integral Control

As mentioned previously, the mathematical model can be subdivided into four subsystems: subsystem $z\left(k\right)$, $\psi \left(k\right)$, $x\left(k\right)$ − $\theta \left(k\right)$ and $y\left(k\right)$ − $\varphi \left(k\right)$. For subsystem $z\left(k\right)$, we have:

$$\begin{array}{ccc}\hfill x_5\left(k+1\right)& =& T{x}_6\left(k\right)+x_5\left(k\right),\hfill \\ \hfill x_6\left(k+1\right)& =& T\left[\frac{u\left(k\right)}{m}cos\left(x_9\left(k\right)\right)cos\left(x_7\left(k\right)\right)-g\right]+x_6\left(k\right)\hfill \end{array}$$

Now, we use an exact linearization as follows:

$$u\left(k\right)=m(u_1\left(k\right)+g){\left(\cos \left(x_9\left(k\right)\right)\cos \left(x_7\left(k\right)\right)\right)}^{-1},$$

with these control laws for each subsystem, which are linear and are defined as:

$$\left[\begin{array}{c}{x}_{1z}(k+1)\\ x_{2z}(k+1)\end{array}\right]=\underset{A_z}{\underbrace{\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]}}\left[\begin{array}{c}{x}_{1z}\left(k\right)\\ x_{2z}\left(k\right)\end{array}\right]+\underset{B_z}{\underbrace{\left[\begin{array}{c}0\\ T\end{array}\right]}}\left[u_1\left(k\right)\right],$$

where $x_5\left(k\right)\triangleq x_{1z}\left(k\right)$ and $x_6\left(k\right)\triangleq x_{2z}\left(k\right)$. Define $e_{1z}\left(k\right)=x_{1z}\left(k\right)-x_{1zR}\left(k\right)$, and $e_{2z}\left(k\right)\triangleq x_{2z}\left(k\right)-x_{2zR}\left(k\right)$; these are the errors for $x_{1z}\left(k\right)$ and $x_{2z}\left(k\right)$, respectively, and $x_{1zR}\left(k\right)$ and $x_{2zR}\left(k\right)$ are the references. The augmented system for the subsystem $z\left(k\right)$ is:

$${\xi }_z(k+1)=\underset{{\mathcal{A}}_z}{\underbrace{\left[\begin{array}{cc}{A}_z& 0\\ C_z& I_2\end{array}\right]}}{\xi }_z\left(k\right)+\underset{{\mathcal{B}}_z}{\underbrace{\left[\begin{array}{c}{B}_z\\ 0\end{array}\right]}}{u}_1\left(k\right)$$

where ${\xi }_z\left(k\right)={\left[x_{1z}\left(k\right)x_{2z}\left(k\right){\sigma }_{1z}\left(k\right){\sigma }_{2z}\left(k\right)\right]}^T$ and $C_z=T{I}_2$. It is not a difficult task to verify that the pair $({\mathcal{A}}_z,{\mathcal{B}}_z)$ is controllable in a finite number of steps. Define the following performance index:

$${\mathcal{J}}_z=\sum _{k=1}^{\infty }({\xi }_z^T\left(k\right){\mathcal{Q}}_z{\xi }_z\left(k\right)+u_1^2\left(k\right){\mathcal{R}}_z),$$

where ${\mathcal{Q}}_z$ is a semidefinite positive matrix of appropriate dimensions and ${\mathcal{R}}_z$ is a real positive number. As the pair $({\mathcal{A}}_z,{\mathcal{B}}_z)$ is controllable, there exists an unique solution to the Riccati equation given by:

$${\mathcal{P}}_z={\mathcal{A}}_z^T{\mathcal{P}}_z{\mathcal{A}}_z-{\mathcal{A}}_z^T{\mathcal{P}}_z{\mathcal{B}}_z{({\mathcal{R}}_z+{\mathcal{B}}_z^T{\mathcal{P}}_z{\mathcal{B}}_z)}^{-1}{\mathcal{B}}_z^T+{\mathcal{Q}}_z,$$

and the solution ${\mathcal{P}}_z$ describes the optimal control for the subsystem $z\left(k\right)$ given by:

$$u_{{}_1}^{*}\left(k\right)=-{({\mathcal{R}}_z+{\mathcal{B}}_z^T{\mathcal{P}}_z{\mathcal{B}}_z)}^{-1}{\mathcal{B}}_z^T{\mathcal{P}}_z{\mathcal{A}}_z{\xi }_{{}_z}^{*}\left(k\right).$$

The discrete model for $\psi \left(k\right)$ is given by:

$$\begin{array}{ccc}\hfill x_{11}\left(k+1\right)& =& T{x}_{12}\left(k\right)+x_{11}\left(k\right),\hfill \\ \hfill x_{12}\left(k+1\right)& =& T{\tau }_{\psi }\left(k\right)+x_{12}\left(k\right),\hfill \end{array}$$

Let $x_{11}\left(k\right)\triangleq x_{1\psi }\left(k\right)$ and $x_{12}\left(k\right)\triangleq x_{2\psi }\left(k\right)$; then, the subsystem $\psi \left(k\right)$ can be rewritten as:

$$\left[\begin{array}{c}{x}_{1\psi }(k+1)\\ x_{2\psi }(k+1)\end{array}\right]=\underset{A_{\psi }}{\underbrace{\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]}}\left[\begin{array}{c}{x}_{1\psi }\left(k\right)\\ x_{2\psi }\left(k\right)\end{array}\right]+\underset{B_{\psi }}{\underbrace{\left[\begin{array}{c}0\\ T\end{array}\right]}}{\tau }_{\psi }\left(k\right)$$

Define $e_{1\psi }\left(k\right)\triangleq x_{1\psi }\left(k\right)-x_{1\psi R}\left(k\right)$ and $e_{2\psi }\left(k\right)\triangleq x_{2\psi }\left(k\right)-x_{2\psi R}\left(k\right)$, where $x_{1\psi R}\left(k\right)$ and $x_{2\psi R}\left(k\right)$ are the given references for the variables $x_{1\psi }\left(k\right)$, $x_{2\psi }\left(k\right)$, respectively. Then, the augmented vector for the subsystem $\psi \left(k\right)$:

$${\xi }_{\psi \left(k\right)}\left(k\right)={\left[x_{1\psi }\left(k\right)x_{2\psi }\left(k\right){\sigma }_{1\psi }\left(k\right){\sigma }_{2\psi }\left(k\right)\right]}^T.$$

The space state representation of the augmented system is:

$${\xi }_{\psi \left(k\right)}(k+1)={\mathcal{A}}_{\psi }{\xi }_{\psi }\left(k\right)+{\mathcal{B}}_{\psi }{\tau }_{\psi }\left(k\right)$$

where:

$${\mathcal{A}}_{\psi }=\left[\begin{array}{cc}{A}_{\psi }& 0\\ C_{\psi }& I_2\end{array}\right],{\mathcal{B}}_{\psi }=\left[\begin{array}{c}{B}_{\psi }\\ 0\end{array}\right],C_{\psi }=T{I}_2,$$

Consider the performance index:

$${\mathcal{J}}_{\psi }=\sum _{k=1}^{\infty }(x_{\psi }^T\left(k\right){\mathcal{Q}}_{\psi }{x}_{\psi }\left(k\right)+{\tau }_{\psi }^2\left(k\right){\mathcal{R}}_{\psi })$$

with an appropriate dimension matrix ${\mathcal{Q}}_{\psi }\ge 0$ and a real $R_{\psi }>0$. As the pair $({\mathcal{A}}_{\psi },{\mathcal{B}}_{\psi })$ is controllable in a finite number of steps, the optimal control law with integral action is given by:

$${\tau }_{{}_{\psi }}^{*}\left(k\right)=-{({\mathcal{R}}_{\psi }+{\mathcal{B}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{B}}_{\psi })}^{-1}{\mathcal{B}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{A}}_{\psi }{x}_{\psi }^{*}\left(k\right),$$

where matrix ${\mathcal{P}}_{\psi }$ is the unique solution to the algebraic equation:

$${\mathcal{P}}_{\psi }={\mathcal{A}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{A}}_{\psi }-{\mathcal{A}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{B}}_{\psi }{({\mathcal{R}}_{\psi }+{\mathcal{B}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{B}}_{\psi })}^{-1}{\mathcal{B}}_{\psi }^T+{\mathcal{Q}}_{\psi }.$$

For the subsystem y-$\varphi \left(k\right)$:

$$\begin{array}{ccc}\hfill x_3\left(k+1\right)& =& T{x}_4\left(k\right)+x_3\left(k\right),\hfill \\ \hfill x_4\left(k+1\right)& =& T\left[\frac{u\left(k\right)}{m}cos\left(x_9\left(k\right)\right)sin\left(x_7\left(k\right)\right)\right]+x_4\left(k\right),\hfill \\ \hfill x_7\left(k+1\right)& =& T{x}_8\left(k\right)+x_7\left(k\right)\hfill \\ \hfill x_8\left(k+1\right)& =& T{\tau }_{\varphi }\left(k\right)+x_8\left(k\right)\hfill \end{array}$$

The control $u\left(k\right)$ was defined as an exact linearization, and the optimal control $u_1^{*}\left(k\right)$; then, the second equation is given by

$$x_4\left(k+1\right)=T(u_1^{*}\left(k\right)+g)tan{x}_7\left(k\right)+x_4\left(k\right),$$

but $u_1^{*}\left(k\right)$ tends to zero when k tends to infinity. Then, there exists $n\in {\mathbb{Z}}^{+}$ such that for all $k\ge nT,$ $\left|u_1^{*}\left(k\right)\right|$ is bounded and neglected; it follows that:

$$x_4\left(k+1\right)=gTtan{x}_7\left(k\right)+x_4\left(k\right).$$

Let there be a control law ${\tau }_{\varphi }\left(k\right)$ that guarantees that $tan{x}_7\left(k\right)\approx x_7\left(k\right)$; therefore, $x_4\left(k+1\right)=gT{x}_7\left(k\right)+x_4\left(k\right)$. Having this idea in mind, consider the following definition:

$$x_3\left(k\right)\triangleq x_{1y}\left(k\right),x_4\left(k\right)\triangleq x_{2y}\left(k\right)$$

$$x_7\left(k\right)\triangleq x_{3\varphi }\left(k\right),x_8\left(k\right)\triangleq x_{4\varphi }\left(k\right),$$

with this definition, the space state representation of the linearized system is:

$$\left[\begin{array}{c}{x}_{1y}(k+1)\\ x_{2y}(k+1)\\ x_{3\varphi }(k+1)\\ x_{4\varphi }(k+1)\end{array}\right]=\underset{A_{y\varphi }}{\underbrace{\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& gT& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{x}_{1y}\left(k\right)\\ x_{2y}\left(k\right)\\ x_{3\varphi }\left(k\right)\\ x_{4\varphi }\left(k\right)\end{array}\right]$$

$$+\underset{B_{y\varphi }}{\underbrace{\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right]}}{\tau }_{\varphi }\left(k\right)$$

In order to use the discrete integral control, define the errors:

$$e_{1y}\left(k\right)\triangleq x_{1y}\left(k\right)-x_{1yR}\left(k\right)$$

$$e_{2y}\left(k\right)\triangleq x_{2y}\left(k\right)-x_{2yR}\left(k\right)$$

$$e_{3\varphi }\left(k\right)\triangleq x_{3\varphi }\left(k\right)-x_{3\varphi R}\left(k\right)$$

$$e_{4\varphi }\left(k\right)\triangleq x_{4\varphi }\left(k\right)-x_{4\varphi R}\left(k\right)$$

where $x_{1yR}\left(k\right)$, $x_{2yR}\left(k\right)$, $x_{3\varphi R}\left(k\right)$ and $x_{4\varphi R}\left(k\right)$ are the references for the variables $x_{1y}\left(k\right)$, $x_{2y}\left(k\right)$, $x_{3\varphi }\left(k\right)$ and $x_{4\varphi }\left(k\right)$, respectively. Then, the augmented state vector for the subsystem y-$\varphi$ is given by:

$${\xi }_{y\varphi }\left(k\right)={\left[\begin{array}{c}{x}_{1y}\left(k\right)x_{2y}\left(k\right)x_{3\varphi }\left(k\right)x_{4\varphi }\left(k\right){\sigma }_{1y}\left(k\right){\sigma }_{2y}\left(k\right){\sigma }_{3\varphi }\left(k\right){\sigma }_{4\varphi }\left(k\right)\end{array}\right]}^T$$

$${\xi }_{y\varphi \left(k\right)}(k+1)=\underset{{\mathcal{A}}_{y\varphi }}{\underbrace{\left[\begin{array}{cc}{A}_{y\varphi }& 0\\ C_{y\varphi }& I_2\end{array}\right]}}{\xi }_{y\varphi \left(k\right)}\left(k\right)+\underset{{\mathcal{B}}_{y\varphi }}{\underbrace{\left[\begin{array}{c}{B}_{y\varphi }\\ 0\end{array}\right]}}{\tau }_{\varphi }\left(k\right)$$

where $C_{y\varphi }=T{I}_2$. As the pair $({\mathcal{A}}_{y\varphi },{\mathcal{B}}_{y\varphi })$ is controllable, there exists an optimal control law ${\tau }_{\varphi }^{*}\left(k\right)$

$${\tau }_{{}_{y\varphi }}^{*}\left(k\right)=-{({\mathcal{R}}_{y\varphi }+{\mathcal{B}}_{y\varphi }^T{\mathcal{P}}_{y\varphi }{\mathcal{B}}_{y\varphi })}^{-1}{\mathcal{B}}_{y\varphi }^T{\mathcal{P}}_{y\varphi }{\mathcal{A}}_{y\varphi }{x}_{y\varphi }^{*}\left(k\right)$$

which minimizes:

$${\mathcal{J}}_{y\varphi }=\sum _{k=1}^{\infty }(x_{y\varphi }^T\left(k\right){\mathcal{Q}}_{y\varphi }{x}_{y\varphi }\left(k\right)+{\tau }_{y\varphi }^2\left(k\right){\mathcal{R}}_{y\varphi })$$

where matrix ${\mathcal{Q}}_{y\varphi }\ge 0$ has appropriate dimensions and the real ${\mathcal{R}}_{y\varphi }>0$. The matrix ${\mathcal{P}}_{y\varphi }$ is the unique solution to the algebraic equation:

$${\mathcal{P}}_{\psi }={\mathcal{A}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{A}}_{\psi }-{\mathcal{A}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{B}}_{\psi }{({\mathcal{R}}_{\psi }+{\mathcal{B}}_{\psi }^T{\mathcal{P}}_{\psi }{\mathcal{B}}_{\psi })}^{-1}{\mathcal{B}}_{\psi }^T+{\mathcal{Q}}_{\psi }.$$

A similar procedure is used to obtain the optimal control ${\tau }_{{}_{x\theta }}^{*}\left(k\right)$ for the subsystem x-$\theta$:

$${\tau }_{{}_{x\theta }}^{*}\left(k\right)=-{({\mathcal{R}}_{x\theta }+{\mathcal{B}}_{x\theta }^T{\mathcal{P}}_{x\theta }{\mathcal{B}}_{x\theta })}^{-1}{\mathcal{B}}_{x\theta }^T{\mathcal{P}}_{x\theta }{\mathcal{A}}_{x\theta }{\xi }_{x\theta }^{*}\left(k\right)$$

where:

$$\begin{array}{c}\begin{array}{cc}{x}_1\left(k\right)\triangleq x_{1x}\left(k\right),& x_2\left(k\right)\triangleq x_{2x}\left(k\right),\end{array}\\ \begin{array}{cc}{x}_9\left(k\right)\triangleq x_{3\theta }\left(k\right),& x_{10}\left(k\right)\triangleq x_{4\theta }\left(k\right),\end{array}\\ \begin{array}{cc}{e}_{1x}\left(k\right)\triangleq x_{1x}\left(k\right)-x_{1xR}\left(k\right),& e_{2x}\left(k\right)\triangleq x_{2x}\left(k\right)-x_{2xR}\left(k\right),\end{array}\\ \begin{array}{cc}{e}_{3\theta }\left(k\right)\triangleq x_{3\theta }\left(k\right)-x_{3\theta R}\left(k\right),& e_{4\theta }\left(k\right)\triangleq x_{4\theta }\left(k\right)-x_{4\theta R}\left(k\right).\end{array}\end{array}$$

The augmented vector is:

$${\xi }_{x\theta }\left(k\right)={\left[\begin{array}{c}{x}_{1x}\left(k\right)x_{2x}\left(k\right)x_{3\theta }\left(k\right)x_{4\theta }\left(k\right){\sigma }_{1x}\left(k\right){\sigma }_{2x}\left(k\right){\sigma }_{3\theta }\left(k\right){\sigma }_{4\theta }\left(k\right)\end{array}\right]}^T,$$

and the following performance index is minimized:

$${\mathcal{J}}_{x\theta }=\sum _{k=1}^{\infty }({\xi }_{x\theta }^T\left(k\right){\mathcal{Q}}_{x\theta }{\xi }_{x\theta }\left(k\right)+{\tau }_{x\theta }^2\left(k\right){\mathcal{R}}_{x\theta })$$

where ${\mathcal{Q}}_{x\theta }\ge 0$, ${\mathcal{R}}_{x\theta }>0$ and ${\mathcal{P}}_{x\theta }$ is the solution to the algebraic Riccati equation:

$${\mathcal{P}}_{x\theta }={\mathcal{A}}_{x\theta }^T{\mathcal{P}}_{x\theta }{\mathcal{A}}_{x\theta }-{\mathcal{A}}_{x\theta }^T{\mathcal{P}}_{x\theta }{\mathcal{B}}_{x\theta }{({\mathcal{R}}_{x\theta }+{\mathcal{B}}_{x\theta }^T{\mathcal{P}}_{x\theta }{\mathcal{B}}_{x\theta })}^{-1}{\mathcal{B}}_{x\theta }^T+{\mathcal{Q}}_{x\theta }$$

where

$${\mathcal{A}}_{x-\theta }=\left[\begin{array}{cc}{A}_{x\theta }& 0\\ C_{x\theta }& I_2\end{array}\right],{\mathcal{B}}_{x\theta }=\left[\begin{array}{c}{B}_{x\theta }\\ 0\end{array}\right]$$

and

$$A_{x\theta }=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& -gT& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],B_{x\theta }=\left[\begin{array}{c}0\\ 0\\ 0\\ T\end{array}\right],$$

$$C_{x\theta }=T{I}_2.$$

These controllers were tested on an experimental platform described in the next section.

Remark 1. 

The OIC strategy combines two advantages of the PID and LQR control approaches: the integral part of the error and the penalization of the energy consumption and convergence of the state, respectively. In contrast to the suboptimal nonlinear discrete control approach [21], which penalizes the energy consumption and convergence of the state with state feedback plus an offset, the OIC substitutes the offset part by the integral part, which provides more robustness in a closed loop in the presence of external disturbances.

3. Experimental Platform

As shown in Figure 3, the experimental platform setup uses an Optitrack Flex 3 vision system to compute the UAV’s position and orientation. The UAV has 6 markers, which are looked at by 12 cameras; the information generated by the cameras is sent to Motive software using the USB protocol. This information is sent to Visual C++ software from Motive software using sockets; it allows to compute in C++ lenguage the integral control laws applied to stabilize the UAV at hover. The PC and UAV are communicated via RS-232 wireless protocol through two Xbee Pro S1 modems at 38,400 bits per second. Furthermore, a Futaba RF radio control is used for manual wireless control of the quadcopter, acting as emergency control if a risk situation occurs in the UAV.

Figure 3 and Figure 4 show how a Parrot’s frame was used to build the platform’s quadrotor. The UAV has an embedded Rabbit module RCM4300, one inertial measurement unit (IMU)3DM-GX3 from MicroStrain, one radio receiver and one Xbee Pro S1 modem.

The vision system obtains the position and orientation of the UAV: $x\left(k\right)$, $y\left(k\right)$, $z\left(k\right)$, $\theta \left(k\right)$, $\varphi \left(k\right)$ and $\psi \left(k\right)$. The velocity of each variable is estimated using Visual C++. Integral control laws are computed in the same way: $u_{PC}\left(k\right)$ (height control computed by the PC), ${\tau }_{\varphi PC}\left(k\right)$ (roll control computed by the PC) and ${\tau }_{\theta PC}\left(k\right)$ (pitch control computed by the PC); these control signals are sent from the PC to the Rabbit microcontroller via RS232 wireless protocol using Xbee modems. The Futaba RF radio generates calibration signals $u_c\left(k\right)$, ${\tau }_{\varphi c}\left(k\right)$, ${\tau }_{\theta c}\left(k\right)$ and ${\tau }_{\psi c}\left(k\right)$; the first one gives an offset of the $z\left(k\right)$ position to compensate for gravity; the last ones give orientation offsets of the UAV’s $\theta \left(k\right)$, $\varphi \left(k\right)$ and $\psi \left(k\right)$, respectively. In this way, the UAV’s orientation and height position are calibrated. RF radio signals are fixed and are only used to compensate for measurement errors caused by the IMU or the vision system.

The UAV radio receiver sends the calibration signals to the Rabbit microcontroller using the $I^{2}C$ protocol. The Rabbit microcontroller reads angular orientation from the IMU (${\theta }_{IMU}\left(k\right)$, ${\varphi }_{IMU}\left(k\right)$ and ${\psi }_{IMU}\left(k\right)$) by the $I^{2}C$ protocol. The information generated by the IMU produces computer orientation control signals in a direct way because noise affects the vision system, causing errors in orientation measurement to the PC, so control signals used for orientation are computed directly in the microcontroller. PC control signals, radio calibration signals and the control signals computed in the microcontroller are added to the calculation of $u\left(k\right)$, ${\tau }_{\varphi }\left(k\right)$, ${\tau }_{\theta }\left(k\right)$ and ${\tau }_{\psi }\left(k\right)$, which allow generation of the PWM signals applied to the four motors of the UAV (see Figure 5).

4. Experimental Results

Two experiments were conducted: The first considers the trajectory tracking problem during takeoff and hover flight using an OIC controller in the UAV. The second one addresses the trajectory tracking problem during takeoff and hover flight by applying the PID and OIC controllers and incorporating crosswind disturbances to the UAV.

4.1. Implementation of Optimal Integral Control Algorithm in the UAV

By combining the Linear Quadratic Regulator (LQR) controller and the Integral Controller (IC), which is the main idea presented in this research work, an Optimal Integral Controller (OIC) is generated. The aforementioned is possible by applying an exact linearization to the model of the Unmanned Aerial Vehicle (UAV) presented in [20]. The process corresponding to this step is described in detail separately in Section 2.3 from a mathematical perspective. As can be seen, a discrete representation of the model is required for each subsystem of the aerial robot.

The mathematical model of the robot described in Section 2.2 is divided into four subsystems. For each of these subsystems $z\left(k\right)$, $\psi \left(k\right)$, $x\left(k\right)$ − $\theta \left(k\right)$ and $y\left(k\right)$ − $\varphi \left(k\right)$, the integral control structure shown in Section 2.3 is applied, and the Riccati-type algebraic equation is numerically solved for every discretized subsystem. This is done considering the augmented penalty matrices $\mathcal{Q}$ and $\mathcal{R}$, which have been assigned as bellow.

For the system $z\left(k\right)$, the two matrices are defined by

$${\mathcal{Q}}_z=\left[\begin{array}{cccc}50& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],{\mathcal{R}}_z=\left[490\right]$$

while matrices used to penalize the subsystem $\psi \left(t\right)$ are chosen as:

$${\mathcal{Q}}_{\psi }=\left[\begin{array}{cccc}18& 0& 0& 0\\ 0& 171& 0& 0\\ 0& 0& 0.1& 0\\ 0& 0& 0& 1\end{array}\right],{\mathcal{R}}_{\psi }=\left[180\right]$$

For the subsystems y-$\varphi$ and x-$\theta$, the penalization matrices are:

$${\mathcal{Q}}_{y\varphi }={\mathcal{Q}}_{x\theta }=\left[\begin{array}{cccccccc}9& 0& 0& 0& 0& 0& 0& 0\\ 0& 9& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 4& 0& 0& 0\\ 0& 0& 0& 0& 0& 10& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$

$${\mathcal{R}}_{x\theta }=\left[40\right],{\mathcal{R}}_{y\varphi }=\left[31\right]$$

To test the OIC (Integral Optimal Control) algorithm, it is applied in a controlled environment to the UAV. Tracking a takeoff trajectory and maintaining hover flight with the UAV is described by the following conditions: The experiments consider a sampling period of $T=50$ ms, set points of translation positions are selected at $x_{ref}\left(t\right)$ = $y_{ref}\left(t\right)$ = 0 m, and altitude $\left|z\right(t\left)\right|$ is chosen as $z_{ref}=0.45t$ until $0.3$ m; once this altitude value is reached, the helicopter then remains there. Set points related to orientation are fixed at $\theta ref$ = $\varphi ref$ = $\psi ref$$=0$ $deg$.

The figures shown below were obtained from the real-time experimentation. Firstly, Figure 6 shows the translational position $x\left(t\right)$, $y\left(t\right)$, $z\left(t\right)$ behavior versus their respectively set points when the Integral Optimal Controller is applied to the quadrotor helicopter. Errors in those translational variables are shown in Figure 7. Moreover, the translational velocities related to each of the axes are shown in Figure 8.

The pitch, roll and yaw orientations are shown in Figure 9; angular rates for all orientation variables are shown in Figure 10.

The torque control signals are shown in Figure 11, the force control signal is shown in Figure 12, and these control signals are calculated using the integral control algorithm in the discrete quadcopter model.

The quadrotor trajectory is shown in Figure 13; this trajectory was subdivided into three trajectories for takeoff, flight and landing of the UAV. In the blue line, one can observe the UAV’s takeoff, while the hover flight of the robot is represented in dark green, and the landing phase is visualized in light green. Each set of points signifies the division of the overall trajectory into these three phases: takeoff, flight, and landing.

4.2. Robustness Test under Crosswind Conditions

Additional experiments were conducted to test the robustness of the integral control strategy. This experiment allows to evaluate how the robustness provided by the integral action affects the UAV’s flight performance. So, a fan is used to apply a crosswind to aerial platform with a velocity of 4.3 m/s (at 19 °C). This external disturbance was supplied to the system from 50 to 150 s. These conditions were used for both PID and OIC controllers. Figure 14 shows the translational position behavior of the vehicle under this crosswind condition.

Moreover, Figure 15 shows the position errors experienced by the four-rotor rotorcraft, and Figure 16 depicts the control signals applied in real time. The magnitude of the control signals shows the feasibility of integral control. Finally, Figure 17 shows the force generated by the control signal $u\left(t\right)$ (corresponding to the collective throttle) when the flying platform was subjected to wind disturbances.

4.3. Comparative Analysis of Energy Consumption between OIC and Conventional PID Controller

This experimental protocol enables a detailed evaluation of the efficacy of the integral control algorithm within an LQR control system applied to UAVs, providing empirical evidence of its performance, robustness and energy efficiency compared to a conventional PID controller. Total energy consumption was computed using potential and kinetic energies. This calculation was performed for both the tuned heuristic PID controller and the OIC optimal controller proposed in this research work. The total energy behavior for each scenario is illustrated in Figure 18. Furthermore,

Table 1. Energy saving.

Controller	Total Energy (kJ)	Saved Energy %
Proportional Integral Derivative	3.3718	0
Optimal Integral Controller	1.7888	53.0514

presents the electrical energy savings comparison between the controllers. These data were derived from a series of 30 experiments.

As evident in Table 1, the OIC (Optimal Integral Controller) improved the electrical energy consumption by 53.05% compared to a conventional PID controller. This proves that the use of a LQR (Linear Quadratic Regulator) controller in conjunction with integral action can save energy in a four-rotor aerial robot. In addition, it provides robustness to the control loop, enabling it to absorb unmodeled dynamics or external disturbances, as has been demonstrated in Section 4.2. Although these experiments were conducted in a controlled environment, it is clear that the control algorithm will work efficiently in outdoor flights, proving that the robot state approximation is accurate and the robot model is correctly parameterized.

5. Conclusions

Satisfactory experimental results using integral control are obtained. So, can be stated that the Optimal Integral Controller (OIC) improves the LQR controller behavior, because it includes an integral term. Moreover, the tuning of integral action can be optimally conducted by solving the discrete Riccati Algebraic Equation associated to LQR problem, allowing to penalize the energy consumption and the convergence rate of the state. According to optimal control theory, the exact linearization applied to the altitude and yaw subsystems allows for stability of the closed loop, and then it is guaranteed. For the subsystems y-$\varphi$ and x-$\theta$, although the optimal control obtained was synthesized using a linearized model, the experimental tests show the robustness of the controller, and the real-time results show an important energy savings rate. So using this strategy, it is possible to achieve proper UAV stabilization for both attitude and position. As future work, it is intended to apply this control strategy in external environments to verify the robustness and efficiency of the algorithm in outdoor flights. This will be carried out on a UAV with the same configuration as proposed in this investigation. Other future work includes experiments in a wind tunnel to evaluate the discharge time of the battery in order to compare the performance of the controllers more accurately.