Comparative Study of Optimal Multivariable LQR and MPC Controllers for Unmanned Combat Air Systems in Trajectory Tracking

Abstract

Guidance, navigation, and control system design is, undoubtedly, one of the most relevant issues in any type of unmanned aerial vehicle, especially in the case of military missions. This task needs to be performed in the most efficient way possible, which involves trying to satisfy a set of requirements that are sometimes in opposition. The purpose of this article was to compare two different control strategies in conjunction with a path-planning and guidance system with the objective of completing military missions in the most satisfactory way. For this purpose, a novel dynamic trajectory-planning algorithm is employed, which can obtain an appropriate trajectory by analyzing the environment as a discrete 3D adaptive mesh and performs a softening process a posteriori. Moreover, two multivariable control techniques are proposed, i.e., the linear quadratic regulator and the model predictive control, which were designed to offer optimal responses in terms of stability and robustness.

1. Introduction

On the basis of several forecasts and predictions, the global market for unmanned aerial vehicles (UAVs) is predicted to grow quickly in future years. For instance, the integration of UAVs into the aerospace market of the United States is projected to represent an economic impact of USD 89.1 billion between 2015 and 2025 [1].

A wide variety of actions can be completed by UAVs in different scenarios [2], offering a huge versatility, while artificial intelligence (AI) is more and more frequently employed to enhance their autonomy [3]. Missions incorporate civilian (photography, environmental monitoring, extinguishing wildfires, etc.) and military (reconnaissance, surveillance, target acquisition, etc.) purposes [4,5,6,7].

One example of a military mission could be dropping bombs over an objective from an initial location, which is the purpose of the UAV proposed in this article. In this case, the selected aircraft is the F-86 Sabre [8,9], which was hypothetically flown without a pilot for the whole mission. Obviously, other unmanned combat air systems (UCASs) would be more suitable for this mission, such as the X-47B [10,11]. However, it is not easy to find precise information about the aerodynamic characteristics of these combat aircraft, which would allow the implementation of a dynamic nonlinear model for accurate simulations. In contrast, there is detailed information available regarding Sabre, which facilitates the elaboration of those precise nonlinear dynamical simulation models.

To accomplish any autonomous 3D flight, path planning and guidance, navigation, and control methodologies are employed. Path planning determines the flight trajectory that the UAV should follow to move from an initial point to a final one, avoiding obstacles during the route. By employing sensors like a global positioning system (GPS), inertial units, magnetometers, etc, as well as estimation algorithms like the Extended Kalman Filter [12], the navigation system calculates the actual position and velocity. The proposed trajectory along with the navigation information is computed by the guidance law to determine the main reference signals for the control algorithms. Finally, to ensure the desired motion of a UAV, appropriate control techniques that can determine the actuator signals at each time step are required.

The purpose of this work was to compare two optimal multivariable controllers—in particular, a linear quadratic regulator (LQR) and model predictive control (MPC)—for trajectory tracking in combat missions in which obstacles are averted and a blitz is performed. Hence, both control techniques’ behaviors were studied based on flight autonomy, reference following, obstacle avoidance, and bomb impact accuracy. The main goal is to discuss which is the most suitable alternative for this kind of mission using a realistic simulation environment. Additionally, in this comparative study, no landing or takeoff procedures were studied, since the objective consisted of evaluating the flying system’s performance during demanding maneuvers.

Figure 1 summarizes the global scheme implemented in this article for the comparative study of the MPC vs. LQR control techniques.

The autonomous flying system implemented in this article is composed of two fundamental blocks: path-planning algorithms and a guidance, navigation, and control (GNC) system.

1.1. Path Planning

There is a high diversity of options for implementing successful 3D path planning. Some algorithms are based on sampling fundamentals, with a deep analysis from a 2D perspective [13], but not in a 3D environment; other options are node-based algorithms, which are mathematical structures utilized to model pairwise relationships (structures with vertices and edges), and they have a final purpose of analyzing the cost of examining nodes to find the optimal path. Moreover, bio-inspired algorithms and mathematical models are widely employed, among other techniques [14].

The node-based approach was utilized herein. This type of algorithm presents various methodologies, such as Dijkstra’s algorithm [15], A* [16], D* [17], or Theta* [18]. Recently, this technique has been enhanced with modern learning machine methods [14].

Moreover, different planning possibilities have been presented: obstacle avoidance (static and/or dynamic), real-time processing (on- or offline), and optimization parameters (total distance, flight parameters, etc.). The flying system must be able to analyze any obstacle online, although, in order to simplify the problem, an offline static obstacle approach, which attempts to achieve the least demanding trajectory for a UAV, is typically selected.

Many obstacle analysis strategies have been implemented over the years, and have mainly utilized accurate sensors [19] or image processing [20,21]. Nevertheless, in this article, a more simple approach was created, since the obstacles in this approach are defined computationally from the beginning.

Adaptive cell decomposition (ACD) is a strong method for solving physical systems by means of partial differential equations, enabling refined complex 3D Cartesian geometry reconstructions. Consequently, an ACD is proposed in this article, in which the 3D space is decomposed and explored through recursive rewarding cost functions. Afterward, a 3D smooth path-planning algorithm is applied after the ACD in order to obtain a more achievable dynamic trajectory for the UAV during the mission through the introduction of gyration radii in each waypoint from the ACD route. In addition, a reference velocity profile is defined.

1.2. Guidance, Navigation, and Control

As mentioned before, GNC systems are applied to ensure appropriate management of any vehicle’s movement. First, the guidance function determines the commanded attitude based on the reference trajectory and the navigation information. Then, the control block applies the control actions needed by actuators to fulfill such commands, while stability is guaranteed [22].

In relation to these systems, there are different aspects that play a key role in ensuring the aircraft’s proper motion, such as position detection techniques [23] or hardware implementation [24], but only theoretical investigation is proposed in this article. Furthermore, the navigation problem has been simplified by assuming that the state vector of the aircraft (positions, velocities, attitude, etc.) is completely known; in other words, it was defined as completely measurable. Therefore, the navigation system has not been implemented or simulated.

On the other hand, the stability of the aircraft depends on the quality of the control strategy, but also on an adequate performance of the guidance law. In this case, the notable nonlinearity of the system makes the selection of an appropriate methodology a hard task.

1.2.1. Control

Nowadays, there is a considerable variety of control methods. The most essential control method is the PID, which enables one to control a system in a closed loop through a proportional, integral, and derivative action [25]. Nevertheless, it is only applicable to single-input/single-output (SISO) systems, so more complex methods are needed to proceed with multiple-input/multiple-output (MIMO) systems. Hence, full state feedback (FSF), in which the dynamics of the process can be changed by modifying the poles of the closed loop system, constitutes a realistic option [26].

The LQR is a suitable control design option that calculates the most adequate feedback gain by diminishing a cost function, which measures changes in the state and control variables [27,28].

Other MIMO control strategies, such as MPC and robust control, can also be applied. The former utilizes the system model and the prediction and control horizon times to predict its future behavior by solving the online optimization algorithm to select the best control action. The MPC can be linear (time-invariant, adaptive, or gain-scheduled) or nonlinear, depending on the recalculation of the plant model and the variation in states and constraints [29,30]. Meanwhile, the latter combines frequency design techniques, such as Bode or Nyquist, and LQR control, and was designed to deal with multiple possible sources of uncertainties, noises, and disturbances [31].

In this article, the LQR and linear time-invariant MPC control methods were selected.

1.2.2. Guidance

In guidance laws, two main groups can be differentiated: proportional and those based on predictive control [32]. Proportional guidance was born as an improvement of pure chasing laws [33]; it creates a velocity vector from the aircraft that points to the target (the next desired point on the route), obtaining excessive acceleration. Proportional guidance laws achieve proportional acceleration in accordance with the rotation of the position vector between the target and the UAV.

However, proportional laws present several problems, such as the generation of acceleration instead of velocity, the lack of future prediction, and the inability to introduce constraints or consider perturbations. Thus, optimal control algorithms, such as LQR and MPC, have been explored in recent investigations and implementations to generate predictive control guidance laws, resulting in a more stable tracking of the reference, but also in an increase in the complexity [32,34,35,36,37].

Thus, a proportional guidance law, together with the control techniques mentioned above, was chosen to evaluate and study the stability, tracking, robustness against perturbations, and complexity. Additionally, this law contained an improvement: Circular trajectories for the aircraft were imposed so as to avoid the issues of classical proportional laws; hence, it is referred to as a nonlinear guidance law (proportional guidance based on forces) from this point forth.

2. Problem Definition

The F-86 Sabre was tasked with a mission to reach a final destination $q_f$ from an initial point $q_i$ at a certain velocity and precision so as to accomplish a successful blitz on the objective. The weapons chosen for this purpose were two M117 demolition bombs [38]. Furthermore, several obstacles had to be avoided during the mission.

The aircraft, which actually acted as a UAV, had to conduct autonomous tracking of the reference “dynamic trajectory”, defined as a combination of the path in the 3D Cartesian coordinates $(x,y,z)$ and the velocity profile that the aircraft must follow, while actuators modified the deflection of the control surfaces and the activation of the throttle lever on the basis of sensor measurements. Only the rudder, ailerons, and elevators were taken into account; hence, the horizontal stabilizer was assumed to be fixed. All of the deflection and throttle lever amplitudes and rates, together with structural limitations and other parameters, were based on [39,40].

The governing equations of the UAV’s motion were given by a typical flight mechanics model and appropriate thrust and fuel consumption equations [41,42,43,44], although several fundamental aspects were adapted to this case, such as the time variation in the mass, mass center, and inertia matrix, as well as a non-null atmosphere motion. The UAV model is fully detailed in [45]. A simplified normally distributed wind model was supposed based on [46,47,48], and average wind velocity and direction values in the operation area were employed (see

Table A1. The wind parameters employed to build the wind model: $\mu$ (mean), $\sigma$ (standard deviation), and ${{\rm Y}}_{max}$ (maximum) and ${{\rm Y}}_{min}$ (minimum) values.

Wind Parameters
Parameter	Modulus ${\mathit{V}}_{\mathit{wind}}$ (m/s)	Direction Angle $\mathbf{\kappa }$ (°)	Elevation Angle $\mathbf{\omega }$ (°)
$\mu$	15	180	0
$\sigma$	1	5	1
${{\rm Y}}_{max}$	18	195	3
${{\rm Y}}_{min}$	12	165	−3

and Equations (A1) and (A2)) [49,50]. The atmospheric parameters were provided by the International Standard Atmosphere (ISA) [43,51,52].

2.1. Dynamic Trajectory

In order to accomplish the mission, a 3D representation of the complete environment had to be conducted, in which restrictions were introduced. The flying system had to ensure the construction of a realistic dynamic trajectory for the UAV while avoiding demanding turning rates and excessive accelerations or decelerations as much as possible, since the control and guidance systems were not able to track the reference. To prove the performance of the flying system, a hypothetical military mission is proposed.

2.1.1. Three-Dimensional Space Creation

The whole space in the $XY$ plane was defined by the initial $q_i$ and final $q_f$ points. Regarding the z coordinates, the range of values was determined by the sea level altitude and a maximum reference altitude.

Each pair of latitude and longitude coordinates were translated into a couple $(x,y)$ by means of the great circle distance concept, which defines the shortest length between two points across the globe.

2.1.2. Obstacles

In a real military mission, static and dynamic obstacles can be found; however, only static ones were considered in this case, since no enemy patrols were expected to be encountered. The following elements were used to define the environment of action:

1.

Danger area: Zone in which the aircraft was not allowed to enter, generated as a cylinder from the sea level altitude to the maximum altitude.

2.

Radar area: Similarly to the danger area, the radar area approximated the scope of an enemy radar. It was defined as a cylinder, with its bottom altitude determined by the minimum altitude at which the UAV could be detected.

3.

Dropping slope: This is not a real obstacle, but instead was employed to ensure a correct descent of the aircraft when it approached the objective, generated as an inverted cone whose vertex was $q_f$.

4.

Ground elevation: Through mapping based on a digital elevation model (DEM) [53] called the National Elevation Dataset (NED) [54], provided by the United States Geological Survey [55], the mountains of the environment were properly defined. The resolution provided by these data was between 30 and 60 m. First, a planar mesh defined by the latitude and longitude coordinates was created in Keyhole Markup Language (KML) [56]; then, it was converted into GPS exchange format (GPX) [57,58], and the elevation data were introduced.

2.1.3. Mission Specifications

The F-86 Sabre, which began acting as a flight bomber during the Korean War (1950–1953) [59,60], was hypothetically modernized in order to enhance its military functionality. Nevertheless, this new technology has not yet been tested in this aircraft in autonomous bombing operations; therefore, the United States Air Force (USAF) has decided to conduct a mock blitz in the Center Eastern region of Alaska. The mission will consist of bombarding an uninhabited area close to the border with Canada at a minimum vertical distance of 200 m, starting from the Eielson Air Force Base at an altitude of 1200 m. The Sabre will have to face a radar with a minimum detection altitude of 900 m and two danger areas, with a maximum dropping slope of 2${}^{\circ }$.

Figure 2 represents a sketch of the operation environment from Google Earth and the required restrictions, which are shown in the 3D space.

3. Dynamic Trajectory Planning

3.1. Adaptive Cell Decomposition

A typical 3D ACD algorithm attempts to conduct a discretization of the environment in a tree data structure known as Octree. The algorithm verifies a possible collision of each rectangloid with the obstacles at each decomposition level; afterwards, independently of whether a potential collision is identified, all subspaces are divided into eight new children. When the final decomposition level is attained, all children identified as potentially colliding with obstacles are discarded, taking only the collision-free rectangloids. Furthermore, in this case, regardless of whether a subspace is fully included in an obstacle, that subspace is rejected and not divided into new ones at the current decomposition level.

Algorithm 1 shows the pseudo-code utilized in the discretization process. Once the decomposition level (n), obstacles, and initial space are defined, the decomposition starts from the first level to the nth level. At each step, all subspaces of the same level are analyzed (from $p_0$ to p), provided that they are not fully included in any obstacle (complete collision). The term $rectangloid$ refers to a 3D matrix that contains all subspaces generated by the algorithm, so that each rectangloid is represented by a position and a group of eight vertices. Then, the vertices of the children are calculated by the $new\_vertices$ function and the collision object of each rectangloid is generated by the $collisionBox$ function. When the final decomposition is completed, the selection of collision-free rectangloids is accomplished, obtaining a list with their positions in the $rectangloid$ matrix.

Algorithm 1 Adaptive cell decomposition (ACD)

1: $n\to decomposition\_level$ 2: define Obstacles 3: $[rectangloid\_size,collision\_rectangloid(1\left)\right]=initial\_rectangloid$ 4: $p_0=p=1$ 5: $free\_rectangloids=[ ]$ 6: for$i=1:n$do 7:   for$s=p_0:p$do 8:    if$s==p_0$then 9:    $p_0=p+1;$ 10:    end if  11:   $[-,complete\_collision]=collision\_box\_objects(collision\_rectangloid(s),obstacles)$; 12:    if$complete\_collision==0$then 13:    $[new\_vertices]$ = vertix_generation(rectangloids(:,:,s)); 14:     for j=1:8 do 15:     $rectangloids(:,:,p+j)=new\_vertices\left(j\right);$ 16:     $[collision\_rectangloid(p+j\left)\right]=collisionBox(i,rectangloids(:,:,p+j),rectangloid\_size);$ 17:     end for  18:    $p=p+8;$ 19:    end if 20:   end for 21: end for 22: for$s=p_0:p$do 23:  $[collision,-]=collision\_box\_objects(collision\_rectangloids(s),obstacles);$ 24:   if collision==0 then 25:   $free\_rectangloids=\left[free\_rectangloids,s\right];$ 26:   end if 27: end for 28: $output\to planner$

This list, together with the vertices of the collision-free subspaces, was employed in the recursive rewarding adaptive cell decomposition (RRACD) algorithm to determine the next possible waypoint on the route.

This methodology is not the most efficient in the computational sense, as it would be more adequate to divide only rectangloids with verified collisions, as is done by the modified ACD algorithm in [14]. However, the basic ACD methodology was preferred, since it facilitates waypoint selection by offering a wider range of possibilities.

In Figure 3, an example of the application of the ACD algorithm to the complete space of study is shown. Moreover, the tree scheme used for the Octree decomposition is represented: The verified and non-verified collisions (black and white circles, respectively) were divided into eight new rectangloids, while the inclusions in the obstacles (black squares) were discarded. This scheme is based on [61].

Furthermore, to diminish the danger of extreme closeness to the terrain, a security distance equivalent to three times the aircraft’s wingspan was introduced in the elevations of the ground collision mesh object, allowing for a more secure flight.

3.2. Recursive Rewarding Adaptive Cell Decomposition Algorithm

An approach to a node-based algorithm was created herein, although the centers of the rectangloids were used instead of edges and vertices, and the optimization parameters comprised the distance and the course and flight path angles. The RRACD algorithm defines a final path $\rho$ by means of the initial subspaces, so that a starting decomposition of the complete environment is made, assigning to all rectangloids a collision-free grade. The initial level selected for this case was $i=5$ ($n_1$), giving, as a result, a number of starting rectangloids of $n_i=8^5$.

Afterward, beginning at the initial point ($q_i$) as a free subspace ($s_k$), the algorithm searches for neighboring rectangloids ($S_{k+1}$) from the initial decomposition and divides them into new ones ($s_{k+1}$) by utilizing the ACD code at a level of $n_2$, which is equal to 3 for this operation. It is important to mention that the complete decomposition $n=n_1+n_2$ is limited, since the final rectangloid dimensions must be greater than the planar accuracy of the ground elevation mesh. The most adequate neighboring option is found by the sum of ${\mathcal{D}}_1$ and ${\mathcal{D}}_2$, two rewarding functions that measure the cost of selecting a future destination. This procedure is repeated until the final point ($q_f$) is attained.

${\mathcal{D}}_1$ is obtained from the path analysis of $s_k$ to $s_{k+1}$ ($s_k\to s_{k+1}\Rightarrow {\mathcal{D}}_1$) and ${\mathcal{D}}_2$ from $s_{k+1}$ to $q_f$ ($s_{k+1}\to q_f\Rightarrow {\mathcal{D}}_2$). Each one is defined by a set of $R_{m,n}$ partial functions in combination with a Gaussian function $g\left(R_{m,n}\right)$ and the weight parameters ${\eta }_1$ (for ${\mathcal{D}}_1$) and ${\eta }_2$ (for ${\mathcal{D}}_2$).

The set $R_{m,n}$ is a group of $m=1\dots N,n=1\dots N$ partial functions that involve 3D flying characteristics. In the algorithm, a total of $N=3$ functions are defined (position n) and further divided into types 1 and 2 (position m) in relation to the subpaths $s_k\to s_{k+1}$ and $s_{k+1}\to q_f$, respectively. Type 1 functions take into account previous course and flight path angles to measure their variations, while type 2 functions only consider the angles of the subpath.

1.

Distance: Given by the expressions below, where $M_{distance}(s_i\to s_j)$ is the Euclidean distance between the chosen subspaces $s_i$ and $s_j$, and $M_{direct}$ is the straight line between $q_i$ and $q_f$.

$$R_{11}(i,j)=R_{21}(i,j)=\frac{M_{distance}(s_i\to s_j)}{M_{direct}}\in \mathbb{R}: \left[0,1\right]$$

(1)

$$M_{distance}(s_i\to s_j)=\sqrt{{(s_{i_x}-s_{j_x})}^2+{(s_{i_y}-s_{j_y})}^2+{(s_{i_z}-s_{j_z})}^2}$$

(2)

2.

Flight path angle: Where ${\gamma }_0$ is the flight path angle of the previous branch of the path and $\gamma$ is one of the vectors given by $s_i\to s_j$. The functions must provide a value between $-1$ and 1, so that in cases in which $|\gamma -{\gamma }_0|,|\gamma |>{45}^{\circ }$, the output will be 1.

$$R_{12}(i,j)=tan(\gamma -{\gamma }_0),R_{22}(i,j)=tan\left(\gamma \right)\in \mathbb{R}: \left[-1,1\right]$$

(3)

$$\gamma =arctan\left(\frac{s_{j_z}-s_{i_z}}{\sqrt{{(s_{j_x}-s_{i_x})}^2+{(s_{j_y}-s_{i_y})}^2}}\right)$$

(4)

3.

Course angle: ${\mathrm{\Psi }}_0$ is the course angle of the previous branch of the path and $\mathrm{\Psi }$ is one of the vectors given by $s_i\to s_j$. As before, when $|\mathrm{\Psi }-{\mathrm{\Psi }}_0|,|\mathrm{\Psi }|>{45}^{\circ }$, the output in the code will be 1.

$$R_{13}(i,j)=tan(\mathrm{\Psi }-{\mathrm{\Psi }}_0),R_{23}(i,j)=tan\left(\mathrm{\Psi }\right)\in \mathbb{R}: \left[-1,1\right]$$

(5)

$$\mathrm{\Psi }=arctan\left(\frac{s_{j_y}-s_{i_y}}{s_{j_x}-s_{i_x}}\right)$$

(6)

Regarding the Gaussian function $g\left(R_{m,n}\right)$ utilized to analyze the reward in executing a possible action, it is defined by the expression (7), in which the transition cost values ($s_k\to s_{k+1},s_{k+1}\to q_f$) have been normalized within the boundaries $\left[0,1\right]$.

$$g\left(R_{m,n}\right)=\frac{sin(\pi ·R_{m,n}+\pi /2)+1}{2}$$

(7)

It is important to note that the higher the effort $R_{m,n}$, the smaller the reward $g\left(R_{m,n}\right)$, and vice versa. All of these rewards are collected in the vector $G_m(i,j)$:

$$G_m(i,j)=\left[g\left(R_{m,1}(i,j)\right),g\left(R_{m,2}(i,j)\right),\dots ,g\left(R_{m,N}(i,j)\right)\right].$$

(8)

Finally, $G_m(i,j)$, the priority parameters (${\eta }_1,{\eta }_2$), and a new parameter $\xi$ are employed to build the a final rewarding function $\mathcal{D}$, which is composed of ${\mathcal{D}}_1$ and ${\mathcal{D}}_2$. The neighbor with the greatest reward is selected as the next waypoint on the route. The reward of selecting a subspace $s_j$ is given by the following expressions:

$${\mathcal{D}}_1={\eta }_1·G_1(s_k,s_j)+{\xi }_{s_j},{\eta }_1=\left[{\eta }_{1D},{\eta }_{1\gamma },{\eta }_{1\mathrm{\Psi }}\right]$$

(9)

$${\mathcal{D}}_2={\eta }_2·G_2(s_j,q_f),{\eta }_2=\left[{\eta }_{2D},{\eta }_{2\gamma },{\eta }_{2\mathrm{\Psi }}\right]$$

(10)

$$\mathcal{D}={\mathcal{D}}_1+{\mathcal{D}}_2.$$

(11)

The parameter $\xi$ is a variable reward value that is assigned to the neighbors $s_{k+1}$ that are considered as very problematic or extremely necessary given the aircraft position and the obstacle locations. For instance, before entering into the radar area, the aircraft must decrease its altitude significantly to cross it successfully, so that only the neighbors that are located in a lower position at each step will be scored with a positive $\xi$.

Regarding the neighborhood choice, the block subspaces $s_{k+1}$ located on the top, bottom, sides, and in front (gray) of the subspace $s_k$ are evaluated as neighbors (see Figure 4). Furthermore, three diagonal rectangloids (blue) are permanently available neighbors for improving the algorithm’s capabilities in the path-building process, while the other three on the bottom front, diagonal, and right sides (yellow) are introduced when the aircraft is close to the radar’s perimeter in order to facilitate the descent. If the UAV has already been to one of these possible subspaces, it is automatically eliminated from the list of destinations. Once the neighborhood has been obtained, the ACD algorithm is applied at each subspace, and afterward, the centers of all new rectangloids for which the rewarding function is used are calculated so as to choose the next most appropriate destination.

In addition, dynamic restrictions are applied after all centers have been obtained. The centers that suppose a change in the course angle greater than 90${}^{\circ }$ or imply the entrance of the UAV into a fully forbidden zone (see Figure 4) are discarded. Moreover, if the selection of a center entails the UAV navigating a trajectory with a flight path angle $\left|\gamma \right|>5^{\circ }$, this center is also rejected.

In Algorithm 2, the RRACD process is shown. First, the initial and local decomposition levels are defined, together with the rest of the variables. Then, the subspaces that have already been visited are discarded and the neighborhood is calculated. The ACD code is applied to each rectangloid and the available centers are calculated by the function $center\_selection$; in the case that there are no centers, a new process with a higher level of decomposition is performed. The rewards of the centers are obtained, selecting the point with the greatest reward. $M_{distance}$ is calculated every time, so that if the last waypoint is fairly close to $q_f$, the algorithm takes it directly.

The final reference trajectory, which is shown in Figure 5, offers acceptable flight conditions and avoids all imposed obstacles. However, this is not the best possible solution, and different combinations of weighting parameters should be selected to enhance the results.

Algorithm 2 Recursive rewarding adaptive cell decomposition (RRACD)

1: define $q_i,q_f,obstacles,{\eta }_1,{\eta }_2,d_{min},{\xi }_{areas}$ 2: $n_1\to$initial decomposition level 3: $n_2\to$local decomposition level 4: $[\mathit{rectangloids},free\_rectangloids]=ACD(initial\_rectangloid,n_1,[\left]\right)$ 5: $wp=q_i$ 6: $M_{distance}=Euclidean\_distance(q_i,q_f)$ 7: $\left[s_k\right]$ =$\left[s_{k_i}\right]$ = $cube\_finding(q_i,rectangloids,free\_rectangloids)$ 8: $\left[\mathrm{\Psi }\right]$ = $initial\_angle(q_i,q_f)$ 9: while$M_{distance}>d_{min}$do 10:   if$s_k==s_{k_i}$then 11:   $free\_rectangloids\left(find(free\_rectangloids==s_k)\right)=[ ];$ 12:   else 13:    for$j=1:length\left(s_{k+1}\right)$do 14:    $free\_rectangloids\left(find(free\_rectangloids==s_{k+1}\left(j\right))\right)=[ ];$ 15:    end for 16:   end if 17:  $s_{k+1}=neighborhood(s_k,rectangloids,free\_rectangloids,wp(end,:),{\overrightarrow{r}}_{radar},R_{radar});$ 18:  $M_{distance}=Euclidean\_distance(q_i,q_f);$ 19:  $local\_divisions=n_2;$ 20:  $\left[centers\right]=[ ];$ 21:   while length(centers)==0 do 22:    for$i=s_{k+1 }$do 23:    $[rectangloid{s}^{\prime },free\_rectangloid{s}^{\prime }]$=$ACD(rectangloids(:,:,s_{k+1}),local\_divisions,obstacles);$ 24:    $\left[centers\right]=center\_selection(rectangloid{s}^{\prime },free\_rectangloid{s}^{\prime });$ 25:    end for 26:   $local\_divisions=local\_divisions+1;$ 27:   end while 28:  $[center\_chosen,s_k,\mathrm{\Psi }]=rewards(wp(end,:),centers,q_f,{\xi }_{areas},\mathrm{\Psi }\left(end\right));$ 29:  $wp=[wp;center\_chosen];$ 30: end while 31: $wp=[wp;q_f]$ 32: return$Final\_path\_waypoints\to wp$

3.3. Trajectory Smoothing

Although the path obtained by the RRACD algorithm is appropriate for UAV guidance, sudden changes in direction when passing a waypoint are impossible for an aircraft to achieve, which could thus provoke excessive maneuvers by the UAV in the guidance process; therefore, the radii of gyration are applied to obtain a smooth trajectory, which is easier to track than the previous one. However, this smooth path could collide with obstacles; hence, the possibility of collisions is analyzed to ensure that this does not happen, and if it does occur, the radii of gyration are diminished in the conflict regions. Thus, the smooth 3D path-planning algorithm finds the optimal trajectory in terms of low angle variation rates and obstacle avoidance based on some concepts from [62]. Moreover, a velocity profile is designed by the algorithm.

The algorithm is applied to each non-aligned group of three waypoints. In Figure 6, an example of the application of the radii of gyration is represented, in which points $s_1$ and $s_2$ are the tangential points among the arc and both lines, vectors ${\overrightarrow{v}}_1$, ${\overrightarrow{v}}_2$, ${\overrightarrow{r}}_1$, and ${\overrightarrow{r}}_2$ are the tools utilized to define points $s_1$ and $s_2$, as well the arc center (O), and R is the radius. The directions of vectors ${\overrightarrow{r}}_1$ and ${\overrightarrow{r}}_2$ are the result of the following vectorial product: ${\overrightarrow{r}}_i={\overrightarrow{v}}_i\times ({\overrightarrow{v}}_1\times {\overrightarrow{v}}_2)$.

The generation of the final smooth trajectory is made by means of two steps at each iteration: first, the path and velocity profile using Algorithm 3, and second, analysis of the errors in the trajectory building and collision of the new path with the obstacles, which are conducted using Algorithm 4.

Algorithm 3 Smooth 3D path simple design (S-3D-PSD)

1: define $path,\gamma ,\mathrm{\Psi },V_{min},V_{max},V_0,\frac{\mathrm{d}V}{\mathrm{d}S},ds,n_0,n_{max},factors$ 2: $pathref=\left[\mathit{path}\right(\mathit{1},:)$V0]; 3: $S=\left[0\right];$ 4: $s_v=[ ]$ 5: $i=1;$ 6: $flag=0;$ 7: while not$(i==size(path,1\left)\right)$do 8:   if not$\left(\right(\mathrm{\Psi }(i+1)-\mathrm{\Psi }\left(i\right))==0)$ or not $\left(\right(\gamma (i+1)-\gamma \left(i\right))==0)$then 9:   $[\overrightarrow{v_1},\overrightarrow{v_2}]=vector\_v\left(path(i:i+2,:)\right);$ 10:   $\overrightarrow{v_{12}}=\overrightarrow{v_1}\times \overrightarrow{v_2};$ 11:   $\left[\alpha \right]=angle(\overrightarrow{v_1},\overrightarrow{v_2});$ 12:   $n=load\_factor(n_0,factors\left(i\right),\mathrm{\Psi });$ 13:   $R=max({\left(pathref(end,4)\right)}^2/(9.81·\sqrt{n^2-1}),{\left(pathref(end,4)\right)}^2/(9.81·\sqrt{n_{max}^2-1});$ 14:   $\left[s\right]=s\_distance(R,\alpha )$ 15:   $s_v=[s_v s;i];$ 16:   $\overrightarrow{r_1}=\overrightarrow{v_{12}}\times \overrightarrow{v_1};$ 17:   $\overrightarrow{r_2}=\overrightarrow{v_2}\times \overrightarrow{v_{12}};$ 18:   $[s_1,s_2]=s\_point(\overrightarrow{v_1},\overrightarrow{v_2},s,path,\mathrm{\Psi });$ 19:   $O=center(s_1,s_2,r_1,r_2,v_1,v_2);$ 20:   $pc=circle\_points(v_{12},s_1,s_2,O,R,ds);$ 21:   $[{points}_{ref},S_{tot},i_{val},fla{g}_{val}]=build\_path1(V_{min},V_{max},\frac{\mathrm{d}V}{\mathrm{d}S},pathref(end,:),$ $path(i:i+2,:),\mathrm{\Psi }(i:i+3),\gamma (i:i+3),v_1,v_2,s_1,s_2,ds,flag,pc);$ 22:   else 23:   $\left[\overrightarrow{v_1}\right]=vector\_v\left(path(i:i+1,:)\right);$ 24:   $[{points}_{ref},S_{tot},i_{val},{flag}_{val}]=build\_path2(V_{min},V_{max},\frac{\mathrm{d}V}{\mathrm{d}S},pathref(end,:),$ $path(i:i+2,:),\mathrm{\Psi }(i:i+3),\gamma (i:i+3),v_1,ds,flag);$ 25:   end if  26:  $pathref =$ [pathref;pointsref]; 27:  $S=[\mathit{S}$ Stot]; 28:  $flag={flag}_{val};$ 29:  $i=i_{val};$ 30: end while

Algorithm 4 Smooth 3D path planning (S-3D-PP)

1: define $path,\gamma ,\mathrm{\Psi },V_{min},V_{max},V_0,\frac{\mathrm{d}V}{\mathrm{d}S},ds,n_0,n_{max},precision,obstacles$   2: $factors=zeros\left(size\right(path,1)-1)$ 3: $solution=0$ 4: repeat 5:  $[pathref,S,s_v]=S\_3D\_PSD(path,\gamma ,\mathrm{\Psi },V_{min},V_{max},V_0,\frac{\mathrm{d}V}{\mathrm{d}S},ds,n_0,n_{max},factors);$ 6:  $\left[error,{locations}_{error}\right]=error\_finding(pathref,s_v);$ 7:  $\left[collision,{locations}_{collision}\right]=collision\_finding(pathref,obstacles);$ 8:   if error ==true then 9:   $\left[{factors}_{new}\right]=modification\_factors(factors,{locations}_{error},precision);$ 10:   else if collision ==true then 11:   $\left[{factors}_{new}\right]=modification\_factors(factors,{locations}_{collision},precision);$ 12:   else if (error and collision) ==false then 13:   $solution==1$ 14:   end if  15:   $factors={factors}_{new};$ 16: until$solution==1$   17: $[{\mathrm{\Psi }}_{new},{\gamma }_{new},\frac{\mathrm{d}\mathrm{\Psi }}{\mathrm{d}t}]=new\_angles\left(pathref\right);$ 18: return$Final\_trajectory\_parameters$$\to wp=[pathref {\mathrm{\Psi }}_{new} {\gamma }_{new} \frac{\mathrm{d}\mathrm{\Psi }}{\mathrm{d}t}]$

Regarding Algorithm 3, the first step consists of initializing the required variables: The path, yaw, and flight path angles obtained from the RRACD code; the maximum, minimum, and initial speeds in the velocity profile; the maximum positive velocity variation per meter ($\frac{\mathrm{d}V}{\mathrm{d}S}$); the precision of the trajectory building ($ds$); the minimum load factor used at each curve ($n_0$); the maximum allowed load factor ($n_{max}=5.5$) [40]; the vector $factors$. The vector $factors$ contains the gradient of the load factor with respect to the yaw angle change ($\frac{\mathrm{d}n}{\mathrm{d}\mathrm{\Psi }}$), so that when a change in $\mathrm{\Psi }$ takes place, an additional load factor is applied. Therefore, the formula for the load factor in the case of i is given by the expression:

$$n_i=n_0+factors\left(i\right)·|{\mathrm{\Psi }}_{i+1}-{\mathrm{\Psi }}_i|/{90}^{\circ }.$$

(12)

Afterward, the code examines the complete reference path by applying the radii of gyration when any change in the course or flight path angles occurs, which can be approximated by $R=V^2/(g·\sqrt{n^2-1})$ [43]. Vectors ${\overrightarrow{v}}_1$ and ${\overrightarrow{v}}_2$ and their angle $\alpha$ are obtained; then, the load factor is obtained from the expression (12) to calculate the radii of gyration, which are restricted to the radius corresponding to $n_{max}$. After that, the distance $|s_i-P_{i+1}|$ and the vectors ${\overrightarrow{r}}_1$ and ${\overrightarrow{r}}_2$ are calculated, from which points $s_1$ and $s_2$ and the center O are found.

Finally, the points from $P_i$ to $P_{i+2}$, which contain the arc, are obtained by the $build\_path1$ function. These points also include all of the velocities as a fourth element, except the 3D coordinates. The $flag$ variable can be 0 or 1 and determines whether a radius of gyration has been applied in the previous group of waypoints in order for the path to be built properly. In the case of an aligned group of waypoints, the $build\_path2$ function generates a straight line.

Once a first dynamic trajectory is generated, Algorithm 4 attempts to find possible errors in the construction due to overlapping arcs and studies whether the new trajectory collides or not. At the beginning, the load factors are set at low values to create big radii of gyration in order for the algorithm to be able to find the optimal solution by means of optimizing the vector $factors$. Therefore, the code analyzes which waypoints are problematic and adds to their respective $factors$ parameters and $precision$ values, increasing the load factor in the points that have excessive radii of gyration (these positions are defined by the vector $locations$) and trying to prevent the arcs from overlapping or colliding with obstacles. Every time a building error or collision is found, the whole code generates a new trajectory with the improved ${factors}_{new}$ vector; hence, only when the new path is perfectly well defined can the algorithm end and the solution be obtained.

Finally, a new trajectory course and its time derivative and flight path angles are calculated with the $new\_angles$ function.

In Figure 7, the final results of the algorithms are shown. The parameters utilized in this case are:

$$\left\{\begin{array}{c}{V}_{min}=190\frac{\mathrm{m}}{\mathrm{s}}\hfill \\ V_{max}=240\frac{\mathrm{m}}{\mathrm{s}}\hfill \end{array}\right.\left\{\begin{array}{c}{V}_0=200\frac{\mathrm{m}}{\mathrm{s}}\hfill \\ \frac{\mathrm{d}V}{\mathrm{d}S}=4\frac{\mathrm{m}/\mathrm{s}}{\mathrm{km}}\hfill \end{array}\right.\left\{\begin{array}{c}ds=20\mathrm{m}\hfill \\ n_0=1.075\hfill \end{array}\right.\left\{\begin{array}{c}{n}_{max}=5.50\hfill \\ precision=1\times {10}^{-2}\hfill \end{array}\right..$$

(13)

The velocity profile is created with the purpose of reducing the speed during curves and descents, while it is increased for straight lines in order to minimize the flight time and to make the dynamic trajectory easier to follow. In addition, the desired velocity of a bomb dropping at $V=170$ m/s is selected to accurately impact the objective; hence, the UAV must progressively diminish its speed when it is close to said objective.

4. Guidance Law

A nonlinear guidance law induces an acceleration in an aircraft to incorporate it into the reference trajectory. In this case, the acceleration is only in the horizontal plane, seeing that the control is conducted on the altitude state variable and not over the flight path angle.

This force is applied perpendicularly to the velocity vector ($\overrightarrow{V}$) according to the expression (14). N is a proportional gain that must be adjusted to achieve greater or lower accelerations depending on the case because an excessive value can produce an unstable flight with many oscillations, while a low value generates poor adaptive guidance when the UAV faces changes in the reference. $\overrightarrow{r}$ is the relative position vector and $\eta$ is the angle between $\overrightarrow{V}$ and $\overrightarrow{r}$.

$$a_{horizontal}=N\frac{V^2}{r}sin\left(\eta \right)$$

(14)

The desired trajectory of interception is circular, so that the length of $\overrightarrow{r}$ is defined by the radius R of such a circular motion. Both the target in the reference path and the instantaneous aircraft location are points of this circumference. In Figure 8, this process can be observed.

In addition, in Figure 8, a simplification of the GNC system scheme is shown. The code composed of Algorithms 1–4 sends position and velocity data to the guidance system. The guidance law creates an utter reference in altitude, attitude (roll and yaw angles only), and velocity based on the navigation information. These new references are analyzed by the control system, which generates the required control action signals. Note that, as mentioned in Section 1.2, the navigation problem has been simplified by assuming that the state vector of the aircraft is completely known.

The attitude control cannot accept accelerations; consequently, the horizontal force is translated into a desired course angle ($\mathrm{\Psi }$), from which the required yaw angle ($\psi$) that the aircraft must achieve is obtained. At each sampling time ($T_s$), a command of the course angle change ($\Delta \mathrm{\Psi }$) is given. The expression of this angle change is as follows:

$$\Delta \mathrm{\Psi }\approx \frac{a_{horizontal}}{V}{T}_s=N\frac{V}{r}sin\left(\eta \right)T_s.$$

(15)

The velocity and bank angle ($\eta$) are assumed as constants and the flight path angle ($\gamma$) as null, and a symmetrical flight is also supposed, resulting in the bank and roll angles ($\varphi$) being equal [32]. These hypotheses, defined by the expressions (16) of uniform circular motion in the horizontal plane, are no longer true if the sideslip and flight path angles are very different from zero, but this is the best approximation for achieving a set of equations that allows one to obtain a manageable solution of the reference roll angle.

$$T=D,\frac{W}{g}\frac{V^2}{R}=Lsin\left(\eta \right)=Lsin\left(\varphi \right),W=Lcos\left(\eta \right)=Lcos\left(\varphi \right)$$

(16)

Hence, the new horizontal acceleration is $\frac{V^2}{R}$. These conditions are translated into expressions for the load factor and the roll angle of the aircraft, defined by the following equations:

$$W=Lcos\left(\varphi \right)\Rightarrow n=\frac{L}{W}=\frac{1}{cos\left(\varphi \right)}\Rightarrow \varphi =arccos\left(\frac{1}{n}\right)$$

(17)

$$\frac{W}{g}\frac{V^2}{R}=Lsin\left(\varphi \right)\Rightarrow n=\sqrt{{\left(\frac{V^2}{gR}\right)}^2+1}\Rightarrow n=\sqrt{{\left(\frac{2V^{2}sin\left(\eta \right)}{gr}\right)}^2+1}.$$

(18)

In addition, the load factor must be between boundaries in order to guarantee stall avoidance and exceeding the structural limits; therefore, the load factor value must ensure that:

$$n\le min(n_{max}=5.5,\frac{\rho V^2{S}_{w}C{L}_{max}}{2W}).$$

(19)

Once the fundamentals of this guidance law are detailed, Algorithm 5 is implemented, which attempts to find the desired target point at each time step and defines the reference in the velocity, altitude, and roll and yaw angles.

Algorithm 5 Nonlinear guidance law

1: define $waypoints,T_s,x,y,z,V,\mathrm{\Psi },u,v,w,\varphi ,\theta ,n_{max},S_w,m,{CL}_{max},threshold$ 2: $R=assign\_radius\left(V\right)$ 3: $\left[{index}_1\right]=next\_point(waypoints,R,x,y)$ 4: $x_{ref}=waypoints({index}_1,1)$ 5: $y_{ref}=waypoints({index}_1,2)$ 6: $\left[{index}_2\right]=closest\_point(waypoints,x,y)$ 7: $z_{ref}=waypoints({index}_2,3)$ 8: $V_{ref}=waypoints({index}_2,4)$ 9: ${\overrightarrow{V}}_0=[Vcos\left(\mathrm{\Psi }\right)Vsin\left(\mathrm{\Psi }\right)]$ 10: $\overrightarrow{r}=[x_{ref}-x{y}_{ref}-y]$ 11: $\left[\eta \right]=angle({\overrightarrow{V}}_0,\overrightarrow{r})$ 12: if$\eta ==0$then 13:  $\Delta \mathrm{\Psi }=0$ 14: else 15:  $\Delta \mathrm{\Psi }=sign\left(\eta \right)(2V{T}_s/|\overrightarrow{r}|)$ 16: end if 17: $\mathrm{\Psi }=\mathrm{\Psi }+\Delta \mathrm{\Psi }$ 18: $n=min(n_{max},\sqrt{((2V^{2}sin\left(\eta \right))/\left(g\right|\overrightarrow{r}{\left|\right))}^2+1},\left(\rho V^2{S}_{w}C{L}_{max}\right)/\left(2W\right))$ 19: ${\varphi }_{ref}=sign\left(\eta \right)arccos(1/n)$ 20: ${[}^{\mathrm{\Psi }}R{}_B]=rotation\_matrix(\varphi ,\theta )$ 21: $\overrightarrow{V}=\left[u;v;w\right]$ 22: ${\overrightarrow{V}}_{\mathrm{\Psi }}{=}^{\mathrm{\Psi }}R{}_B·\overrightarrow{V}$ 23: $\psi =\mathrm{\Psi }-arctan({\overrightarrow{V}}_{\mathrm{\Psi }}(2,1)/{\overrightarrow{V}}_{\mathrm{\Psi }}(1,1))$ 24: if$\left|\right[x,y,z]-waypoints(end,1:3\left)\right|<threshold$then 25:  $stop=1$ 26: else 27:  $stop=0$ 28: end if

First, the current flight parameters are defined, together with the aircraft parameters ($S_w$, m, and ${CL}_{max}$) and the threshold, which is the length that specifies whether the simulation stops when the distance between the aircraft and the final destination is lower. $waypoints$ is provided by the dynamic trajectory-planning process and includes reference 3D coordinates and velocities.

The radius R is calculated from the velocity, taking greater values for high velocities and lower values if the speed is minimum. Afterward, the coordinates $(x,y)$ of the intersection point between the circle, whose radius is R and whose center is the aircraft position, and the trajectory are obtained. There are two intersections, so the one with a higher $waypoints$ index is chosen. Second, repeating the same procedure but without the radius, the closest point of the reference path to the aircraft position is selected to specify the altitude and velocity to be tracked.

Afterward, the relative position and velocity vectors in the $XY$ plane are obtained, so that the changes in the course angle ($\Delta \mathrm{\Psi }$), the load factor (n), and the reference roll angle ($\varphi$) can be calculated by means of the previously explained equations. Then, the reference yaw angle is achieved by means of the rotation matrix ${}^{\mathrm{\Psi }}R{}_B$, which translates the velocity in body axes into $XY$ plane motion with the aircraft’s direction, as well as the course angle. Finally, the distance from the aircraft to the final destination is analyzed, so that if it is lower than the threshold, the simulation ends.

5. Control Strategies

In order to develop a control system, the mathematical model must be linearized beforehand with respect to an equilibrium point. The equilibrium conditions correspond to the trimmed point of the aircraft in a vertical planar motion characterized by the given velocity and altitude values. In this case, $V_{trim}=215$ m/s and $z_{trim}=848.4$ m are utilized, which are the mean values of the dynamic trajectory obtained in the previous chapter.

If the system of equations obtained from the linearization process is rearranged, a linearized state–space model can be achieved, given by the expression (20), in which $\mathfrak{x}$ represents the state variables, $\mathfrak{u}$ the control variables, and d the modeled perturbations. A is the system matrix and B is the control matrix, while $A_d$ is the disturbance matrix. All variables are given in increments with respect to their corresponding equilibrium position values. Attending to the Equations (A3) and (A4) of these matrices, all variables are interrelated; hence, the longitudinal and lateral direction motions are coupled.

$$\dot{\mathfrak{x}}=A\mathfrak{x}+B\mathfrak{u}+A_{d}d\to \dot{\Delta \mathfrak{x}}=A·\Delta \mathfrak{x}+B·\Delta \mathfrak{u}+A_d·d=A·\Delta \mathfrak{x}+\overline{B}\left[\begin{array}{cc}\Delta \mathfrak{u}& d\end{array}\right]$$

(20)

The chosen state variables are the body velocities (u, v, and w) and the angular body velocities (p, q, and r), the Euler angles ($\varphi$, $\theta$, and $\psi$), the altitude (z), and the total body velocity (V). The control variables are the throttle lever activation (${\delta }_P$) and elevator (${\delta }_E$), the ailerons (${\delta }_A$), and the rudder deflections (${\delta }_R$). To facilitate the control system design, the perturbation variables are also added to the control variables, i.e., the wind velocities in the fixed axes ($V_{x_w}$, $V_{y_w}$, and $V_{z_w}$) and the drag compressibility coefficient ($C{D}_{wave}$), since they are zero when the Mach number is below the divergence number ($M_{div}=0.85$) [43]. The implemented linearized state–space model is as follows:

$$\Delta \mathfrak{x}={\left[\begin{array}{ccccccccccc}\Delta z& \Delta u& \Delta v& \Delta w& \Delta \varphi & \Delta \theta & \Delta \psi & \Delta p& \Delta q& \Delta r& \Delta V\end{array}\right]}^T$$

(21)

$$\overline{\Delta \mathfrak{u}}={\left[\begin{array}{cccccccc}\Delta {\delta }_P& \Delta {\delta }_E& \Delta {\delta }_A& \Delta {\delta }_R& V_{x_w}& V_{y_w}& V_{z_w}& C{D}_{wave}\end{array}\right]}^T$$

(22)

$$\left\{\begin{array}{c}{\dot{\Delta \mathfrak{x}}}_{11\times 1}=A_{11\times 11}·\Delta {\mathfrak{x}}_{11\times 1}+{\overline{B}}_{11\times 8}·{\overline{\Delta \mathfrak{u}}}_{8\times 1}\hfill \\ y_{11\times 1}=C_{11\times 11}·\Delta {\mathfrak{x}}_{11\times 1}+D_{11\times 8}·{\overline{\Delta \mathfrak{u}}}_{8\times 1}\hfill \end{array}\right.,\left\{\begin{array}{c}{C}_{11\times 11}=I_{11\times 11}\hfill \\ D_{11\times 8}={\left[0\right]}_{11\times 8}\hfill \end{array}\right..$$

(23)

In addition, regarding the sample time, it must fulfill the Nyquist theorem [63], in which this parameter must be less than the settling time of the system to allow an acceptable control. In this case, the settling time of the system is related to the sixth eigenvalue of the system matrix. The selected sample time is $T_s=0.021$ s, so that it satisfies the requirement:

$$T_s=0.021\mathrm{s}<\frac{1}{2·Re\left({\lambda }_6\right)}=0.18\mathrm{s}.$$

(24)

5.1. Linear Quadratic Regulator

An LQR is an optimal control technique that provides the best possible performance with respect to some given performance index. The LQR design problem involves the design of a state feedback controller K such that the objective function $\mathcal{J}$ is minimized [64]. In this technique, a feedback gain matrix is designed that minimizes the objective function in order to achieve some compromise between the use of control effort, the magnitude, and the speed of response that guarantee a stable system. The $\mathcal{J}$ function proposed in this work is the well-known cost index described in (25) [27]; although the $N_{\mathfrak{x}\mathfrak{u}}$ weighting matrix is typically considered null, the cost function becomes a quadratic one.

$$\mathcal{J}={\int }_0^{\infty }({\mathfrak{x}}^{T}Q\mathfrak{x}+{\mathfrak{u}}^{T}R\mathfrak{u}+2{\mathfrak{x}}^T{N}_{\mathfrak{x}\mathfrak{u}}\mathfrak{u})dt\to \mathcal{J}={\int }_0^{\infty }({\mathfrak{x}}^{T}Q\mathfrak{x}+{\mathfrak{u}}^{T}R\mathfrak{u})dt$$

(25)

Q and R are the weighting matrices for the state and control signals, respectively, and are defined by the designer such that $Q\ge 0$ and $R>0$. The higher the weighting value, the lower the state or control action associated with that value. Regarding the calculation of the matrix K, it is obtained from the Riccati equation [28], where S is the Riccati solution to the equation:

$$A^{T}S+SA-SB{R}^{-1}{B}^{T}S+Q=0\to K=R^{-1}{B}^{T}S.$$

(26)

Since the longitudinal and lateral direction motions are coupled during flight, it is necessary to separate them in order to develop an appropriate LQR controller. Hence, four different subcontrollers are created so as to fulfill the requirements, so that four feedback gains can be obtained: $K_{1z}$, related to the altitude; $K_{1V}$, related to the speed; $K_{21}$ and $K_{22}$, related to the tracking of the roll and yaw angles. The reference following $\varphi$ and $\psi$ is complex; thus, a unique feedback gain does not provide an adequate response, resulting in a requirement to use two feedback gains in a cascade configuration.

In this way, $K_{1z}$ and $K_{1V}$ assign the required elevator deflection and throttle lever action to follow the reference in altitude and velocity from the guidance algorithm, respectively. In relation to the roll and yaw angles, $K_{21}$ creates a reference in angular velocities p and r on the basis of the error in these angles with respect to the reference; however, this error is not integrated because a very aggressive and fast action is demanded. Afterward, the feedback gain $K_{22}$ is applied to this error. The state and control variables and cost functions corresponding to each feedback gain matrix are given by the following expression:

$${\overline{\mathfrak{x}}}_{1z}={\left(\begin{array}{cccccc}\Delta z& \Delta u& \Delta w& \Delta \theta & \Delta q& {ϵ}_z\end{array}\right)}^T,{\mathfrak{u}}_{1z}=\left(\Delta {\delta }_E\right)\Rightarrow {\mathcal{J}}_1={\int }_0^{\infty }({\overline{\mathfrak{x}}}_{1z}^T{Q}_{1z}{\overline{\mathfrak{x}}}_{1z}+{\mathfrak{u}}_{1z}^T{R}_{1z}{\mathfrak{u}}_{1z})dt$$

(27)

$${\overline{\mathfrak{x}}}_{1V}={\left(\begin{array}{cc}\Delta V& {ϵ}_V\end{array}\right)}^T,{\mathfrak{u}}_{1V}=\left(\Delta {\delta }_E\right)\Rightarrow {\mathcal{J}}_2={\int }_0^{\infty }({\overline{\mathfrak{x}}}_{1V}^T{Q}_{1V}{\overline{\mathfrak{x}}}_{1V}+{\mathfrak{u}}_{1V}^T{R}_{1V}{\mathfrak{u}}_{1V})dt$$

(28)

$${\overline{\mathfrak{x}}}_{21}={\left(\begin{array}{cccc}\Delta \varphi & \Delta \psi & {ϵ}_{\varphi }& {ϵ}_{\psi }\end{array}\right)}^T,{\mathfrak{u}}_{21}={\left(\begin{array}{cc}\Delta p& \Delta r\end{array}\right)}^T\Rightarrow {\mathcal{J}}_3={\int }_0^{\infty }({\overline{\mathfrak{x}}}_{21}^T{Q}_{21}{\overline{\mathfrak{x}}}_{21}+{\mathfrak{u}}_{21}^T{R}_{21}{\mathfrak{u}}_{21})dt$$

(29)

$${\overline{\mathfrak{x}}}_{22}={\left(\begin{array}{cccc}\Delta p& \Delta r& {ϵ}_p& {ϵ}_r\end{array}\right)}^T,{\mathfrak{u}}_{22}={\left(\begin{array}{cc}\Delta {\delta }_A& \Delta {\delta }_R\end{array}\right)}^T\Rightarrow {\mathcal{J}}_4={\int }_0^{\infty }({\overline{\mathfrak{x}}}_{22}^T{Q}_{22}{\overline{\mathfrak{x}}}_{22}+{\mathfrak{u}}_{22}^T{R}_{22}{\mathfrak{u}}_{22})dt.$$

(30)

In order to build these matrices, amplified subsystem matrices must be created. These matrices include the errors ($ϵ$) in the variables to be reduced. Hence, the state variable vectors comprise an amplified vector, which also contains the variables of error. In addition, matrix Q presents in its diagonal the weighting factor for each variable of the new amplified state vector. As can be seen in Equation (A5), in matrix Q, the elements corresponding to the error variables are much more important for prioritizing the tracking error and for diminishing it as much as possible. Moreover, the elements from matrix R are considerable in size, since only necessary control actions should be used instead of excessive ones. However, these values are limited because they could be unreasonable as a result of poor control actions; therefore, there is an equilibrium between the Q and R matrices that provides balanced control responses.

5.2. Model Predictive Control

MPC is a strategy that uses an explicit model of the process where, at each sampling time and with the current information of the process variables, predictions of future process behavior along a horizon are managed. Such predictions are incorporated into a cost index to solve an open-loop optimal control problem (OCP) that is subject to constraints, which results in the sequence of future optimal controls.

Consider the discrete linear system (31), where variables ${\mathfrak{x}}_{k+i}$ and ${\mathfrak{u}}_{k+i}$ represent the state vectors and system inputs, respectively, at the instant $k+i$; A is the state matrix and B is the system input matrix

$${\mathfrak{x}}_{k+i+1}=A{\mathfrak{x}}_{k+i}+B{\mathfrak{u}}_{k+i}.$$

(31)

For every instant k, based on (31) and with the availability of the current state of the plant ${\widehat{\mathfrak{x}}}_k$, predictions are made for the states ${\mathfrak{x}}_{k+i+1|k}$ and inputs ${\mathfrak{u}}_{k+i|k}$ along a prediction horizon N, $\forall i\in \{0,1,\dots ,N-1\}$. These predictions are included in the quadratic cost index (32), which is subsequently minimized at the OCP (33):

$${\mathcal{J}}_N({\widehat{\mathfrak{x}}}_k,{\mathfrak{u}}_k)=\sum _{i=0}^{N-1}\left({\mathfrak{x}}_{k+i|k}^{\top }Q{\mathfrak{x}}_{k+i|k}+{\mathfrak{u}}_{k+i|k}^{\top }R{\mathfrak{u}}_{k+i|k}\right)+{\mathfrak{x}}_{k+N|k}^{\top }{P}_N{\mathfrak{x}}_{k+N|k}$$

(32)

$$\underset{{\mathfrak{u}}_{k|k},{\mathfrak{u}}_{k+1|k},\dots ,{\mathfrak{u}}_{k+N-1|k}}{min}{\mathcal{J}}_N({\widehat{\mathfrak{x}}}_k,{\mathfrak{u}}_k)$$

(33)

$$\mathrm{s}.\mathrm{t}.$$

$${\mathfrak{x}}_{k+i+1|k}=A_{cl}{\mathfrak{x}}_{k+i|k}+B{\mathfrak{u}}_{k+i|k}$$

(34)

$$H{\mathfrak{x}}_{k+i+1|k}\le h$$

(35)

$$D{\mathfrak{u}}_{k+i|k}\le d$$

(36)

$${\mathfrak{x}}_{k|k}={\widehat{\mathfrak{x}}}_k$$

(37)

$$\forall i\in \{0,1,\dots ,N-1\},$$

(38)

where the current state ${\widehat{\mathfrak{x}}}_k$ is known; thus, ${\mathfrak{x}}_{k|k}={\widehat{\mathfrak{x}}}_k$; matrices Q, R, and $P_N$ are weighting matrices that penalize the first N predicted, predicted inputs, and terminal state, respectively. The matrices Q and R (equivalent to matrices Q and R in LQR controllers) are defined by the designer such that $Q\ge 0$ and $R>0$, and $P_N$ is obtained from a quadratic stability analysis [65,66,67]. Notice that the subscript $k+i|k$ indicates the predicted value of the variable for the instant $k+i$ based on the information available at time k.

In each control period, the problem stated in (33) is solved, obtaining the vector of optimal decision variables ${\mathfrak{u}}_k^{*}=\{{\mathfrak{u}}_{k|k}^{*},{\mathfrak{u}}_{k+1|k}^{*},\dots ,{\mathfrak{u}}_{k+N-1|k}^{*}\}$, which drives the system states to a desired operating point [68] or as a regulator towards the origin. Next, applying the receding horizon strategy, only the first element ${\mathfrak{u}}_{k|k}^{*}$ from the control sequence ${\mathfrak{u}}_k^{*}$ is used, so that OCP (33) is performed again at time $k+1$.

The cost function (32) obeys the dual-mode prediction paradigm [66,69,70], which ensures stability for an appropriate (or long enough) horizon N, incorporating a terminal cost ${\mathfrak{x}}_{k+N|k}^{\top }{P}_N{\mathfrak{x}}_{k+N|k}$ that penalizes the terminal state ${\mathfrak{x}}_{k+N|k}$. Another condition that is also used to ensure closed-loop stability is to complement the dual mode by adding the terminal constraint ${\mathfrak{x}}_{k+N|k}$ [69,71] to the OCP (33). The purpose of this is to force the terminal state ${\mathfrak{x}}_{k+N|k}$ and the following states to remain within a safe zone or terminal set [65,72,73].

The OCP (33) is stated as subject to constraints (34)–(38). Notice that constraint (34) indicates that the predicted state ${\mathfrak{x}}_{k+i+1|k}$ is computed recursively using the state and inputs from the previous state $k+i|k$, where ${\mathfrak{x}}_{k|k}={\widehat{\mathfrak{x}}}_k$. Linear inequalities (35) and (36) are constraints for the predicted states and input trajectories, respectively, where matrices H and D and vectors h and d represent the constraint limits.

Given the quadratic and convex nature of (33), the linear model (31), and the types of constraints (35) and (36), a finite-horizon OCP stated as in (33) can be solved at each control period. This OCP is a quadratic programming problem (QP) with a global optimum whose solution results in the optimal control vector.

In addition to the theoretical formulation, Figure 9 intuitively shows how the MPC works.

The control horizon must always be lower than the prediction horizon. Furthermore, an adequate combination of sample time and control and prediction horizons should be made, since a low control horizon or a high sample time can lead to poor control actions and vice versa, while low prediction horizon values could demand excessive or even impossible control actions.

The prediction and control horizon parameters are set to 100 and 10, respectively. Therefore, at each iteration, the controller calculates the performance of the system for a total of $t_p=100·T_s=2.1$ s by means of applying control actions until time $t_c=10·T_s=0.21$ s, selecting the best options among all of the calculations.

Additionally, the physical constraints shown in [40] are introduced into the MPC following the OCP structure. This is because it is not necessary to include a signal filter after the control block, as is done in the LQR controller.

The controller responses are determined by the weighting matrices Q and R applied to the state variables and control variables. Tracking is only performed for the altitude, velocity, and roll and yaw angles.

As shown in Equation (A6), some variables are specifically high in the weighting matrices Q and R; the pitch angle, angular velocity q, and elevator deflection are prioritized to avoid excessive motion in the vertical plane and altitude errors.

6. Results

6.1. Complete Response

The flight simulations utilizing both controllers, i.e., LQR and MPC, were performed under variable mass and wind conditions, as explained in Section 2. The wind velocities in each axis obtained during the simulations are shown in Figure A1. The compressibility effects are not given, since the divergence Mach number was not exceeded.

The code developed to perform all the simulations is available on Matlab Central [74], including the path-planning algorithms, control parameter definitions, and Simulink files, in which the controllers and guidance law that were employed in simulations are implemented.

In Figure 10, the velocity and altitude magnitudes are shown, together with their respective reference values specified by the dynamic trajectory. The MPC case clearly provided a more satisfactory response in velocity tracking, while the reference altitude was followed similarly in both cases. Nevertheless, the LQR controller showed several problems during curves and descents, since the velocities and altitudes presented considerable errors, while the MPC controller achieved a more satisfactory performance.

When great differences with respect to the equilibrium point were given in the flight path and roll angle (in the state–space model, $\gamma$ and $\varphi$ were considered null due to the trimmed point conditions), such as in intense descents or curves, the longitudinal and lateral direction motions were coupled to a considerable extent, which provoked acute errors in the reference tracking, since the LQR and linear time-invariant MPC controllers worked continuously with the same plant model. In this case, both of the controllers’ performance during curves showed appropriate robustness, although the MPC controller more adequately tracked the reference velocity.

Generally, an MPC controller creates more adequate control actions than an LQR controller due to its future prediction capabilities; however, both controllers really depend on the selection of the weighting parameters. In the case of the MPC controller, their suitable selection, which restricts the pitching motion and assigns a high priority to altitude and velocity tracking, provided satisfactory results during the whole simulation. Regarding the LQR case, the parameters of the Q and R matrices and the structure of this controller were selected appropriately. The special configuration on the part of the LQR controller corresponding to the roll and yaw angles led to stronger capabilities of tracking the trajectory.

In Figure 11, several of the principal flight parameters are shown: the angle of attack, sideslip angle, and Euler angles. As can be appreciated, the fluctuations are fairly intense in all variables due to the wind, although they are especially relevant in the sideslip angle.

The angle of attack presented realistic values in almost all of the trajectory, but in some points, it entered into the negative area due to aggressive control actions, which is correlated to the error in altitude tracking shown in Figure 10. These negative values enlarged the probability of stalling in the real behavior of the UAV, so that the LQR controller could produce several failures in this sense, such as, for example, at a time of $t\approx 400$ s.

With regard to the sideslip angle, in no cases was it null because the wind velocity restricts the yaw and course angle from coinciding in straight lines. Also, this parameter showed very well the regions where the turns occurred. Concerning to the notable curves, if they are made in the clockwise direction, the sideslip should be mainly positive; on the contrary, counterclockwise curves provoke negative sideslip angles. Therefore, the LQR results present a more clear development of the aircraft rotation for proceeding with curves properly, while the MPC results represent a more erratic behavior.

Considering to the pitch angle, the motion was more aggressive in the LQR case because the elevator control action was not as anticipatory as that of the MPC controller, which demonstrated excessive oscillations in the angle of attack and altitude tracking errors. Last but not least, the roll and yaw angles were very similar in both cases; however, they presented lower fluctuations and overshoots in the MPC case. Therefore, the guidance law acted slightly more ineffectively for the LQR controller, or the controller was not robust enough in the lateral direction motion.

The control action signals are given in Figure 12. More intense fluctuations in the control actions were obtained in the MPC results, which confirms the previous results, in which the flight parameters oscillated less. The amplitude of the oscillations in the control signals of the rudder was primarily higher in curves in the LQR case, demonstrating a more aggressive control for facing perturbations and adequately tracking the reference path. In relation to the ailerons and elevator deflections, regarding the former, their fluctuations were comparable in both cases; meanwhile, regarding the latter, their oscillations were chiefly more intense in the MPC case, although their average value was very close to that of the LQR controller, owing to the improvement of the stability of the aircraft by the greater robustness of this controller.

The error in altitude can be explained by the non-coordinated control actions of the elevator because the controllers utilized it without taking into account that it also interferes in the lateral direction motion because of the linearization process with respect to the trimmed point.

In addition, the mean value of the oscillations in the rudder along the straight path was not zero. This was due to the fact that the course angle was not exactly the one specified by the guidance algorithm and the sideslip angle fluctuated with respect to an angle higher than zero (see Figure 11). This problem was less significant in the MPC case, which could be due to the well-coordinated control actions between the rudder and ailerons.

Finally, in Figure 13, the trajectories of both cases are shown. The errors in altitude were similar in the LQR and MPC cases, as can be seen in the figure. In relation to the tracking of the path in the $XY$ plane, both controllers provided satisfactory results, since the deviation was minimum; nevertheless, if more intense curves were given, the LQR would accomplish a slightly more appropriate planar tracking due to its adequate coordination of the control signals, as shown by the sideslip angle results in Figure 11. However, the clear advantage of the MPC was in the velocity results, as can be observed in Figure 10.

6.2. Analysis of the Impact, Time, and Fuel Mass Consumption

By means of a dropping point estimation algorithm [45], which calculates the best location of the trajectory to release the bombs and their deviations in the impact, based on a mathematical model of M117 bombs [43,75] (detailed in [45]), the deviations in the dropping point from the reference release coordinates and the error in the impact—defined by the expressions (39) and (39)—were calculated in the LQR and MPC cases. This mathematical model assumes no gyration of the bombs around their x-axis; hence, only the yaw and pitch angles can vary. Furthermore, the mean wind velocity and direction were considered in the calculation of the reference case, while the normalized data were utilized in the real results.

$${Deviation}_{Dropping}=\sqrt{{(x_{Drop,ref}-x_{Drop})}^2+{(y_{Drop,ref}-y_{Drop})}^2}$$

(39)

$${Deviation}_{Impact}=\sqrt{{(x_{Impact}-q_{f,x})}^2+{(y_{Impact}-q_{f,y})}^2}$$

(40)

Figure 14 represents an example of the application of the dropping point estimation algorithm. The bomb trajectories were calculated from each waypoint. Afterward, the most adequate dropping coordinates were obtained by means of diminishing the deviations in the bombs’ impacts, which were determined through the collision on the ground. The terrain was modeled through a bilinear interpolation [76].

As can be seen in

Table 1. Deviations in the release (from reference case) and in the impact of the bombs.

Deviations
Case	Deviation in the Drop (m)	Deviation in Impact (m)
Reference	-	7.7
LQR	14.3	4.0
MPC	3.0	1.6

, the MPC controller offered more adequate results, since the deviations in the dropping point and error in the impact were fairly low, while the LQR’s dropping point was a considerable distance from the reference one, which means that the impact could be particularly inaccurate in terms of the real behavior. This can be explained by the appropriate velocity tracking by the MPC controller, since the error in speed during the drop was greater in the LQR case.

However, these deviations could materially vary due to the fact that the bomb-dropping results are fairly contingent upon the final part of the dynamic trajectory. However, the velocity tracking of the MPC controller is still bound to guarantee a more appropriate response.

In addition, the flight time and fuel consumption were compared in order to evaluate the performance during the mission, since the relevant differences in these magnitudes could determine a greater efficiency of one controller over the other in the net results and because these magnitudes also provide a bigger picture of flight autonomy.

The flight time was slightly smaller in the LQR case, while the fuel consumption was lower in the MPC case, as can be seen in

Table 2. Simulation time and fuel consumption.

Time and Fuel Mass
Case	Simulation Time (s)	Fuel Consumption (kg)
LQR	1001.09	266.77
MPC	1002.67	264.08

, although the differences were not significant. Hence, both controllers would be equally useful for missions.

7. Conclusions and Future Works

A realistic simulation environment was implemented for the F-86 Sabre acting as a UAV based on a group of known algorithms to analyze which multivariable control method provides a more satisfactory performance. This system was able to fulfill all of the mission’s requirements according to Section 2.

The dynamic trajectory-planning algorithms, which are modified versions of those in [14,45,62], demonstrated a proper behavior in the creation of waypoints for trajectories that are perfectly trackable for the aircraft and allow one to avert 3D obstacles. Furthermore, the 3D smooth path-planning code facilitated the reference-following task of the controllers. The reasonable velocity profile and the unusual approach of the neighborhood finding turned out to be appropriate for the path due to the fact that both controllers were able to perform adequate reference tracking. However, an improvement in these elements, as well as in the rewarding function and the selection of the center of the rectangloids, will be key to making further progress in dynamic trajectory building in future designs. For instance, the rewarding function could consider new flight parameters in its calculations and the center selections could take their proximity to obstacles into account to make the route more secure.

The LQR and MPC controllers provided acceptable responses, since the aircraft did not enter prohibited areas in either case, and the aircraft’s limitations were respected; nevertheless, the trajectory obtained by the MPC method was more satisfactory in terms of airworthiness.

Regarding the guidance law, a nonlinear one that improves the proportional guidance capabilities is a particularly appropriate option. All of the references in altitude, velocity, and roll and yaw angles were adequate; hence, the path tracking in the horizontal plane presented low deviations. In future designs, predictive guidance laws created from motion equations could be implemented to better enhance the flying system.

Finally, the bombing was accurate in the MPC case, while the LQR results presented noticeable deviations, even though the differences were not great, since they are dependent on many factors. The excellent reference velocity following of the MPC controller led to a more satisfactory performance in terms of the bombs’ impact, but also in the fuel consumption. The predictive capabilities of the MPC provide a more optimal control than in the LQR case, as the control signals’ configuration is managed more efficiently. Consequently, the MPC controller could be considered a more appropriate option, since it provided acceptable results in terms of obstacle avoidance and reference tracking and achieved only a slight error during the blitz operation.

Two main aspects are open for future works: on one hand, the specific procedures for landing and takeoff maneuvers, and on the other hand, reliable hardware for implementing the proposed algorithms.

Regarding the takeoff, factors related to the runway, such as the friction coefficient, the altitude of its location, or the local temperature, can have significant effects, while the ground effect (GE) [77] substantially modifies the lift and drag. However, a simple physical analysis that takes these aspects into account [43] can be an initial approach.

In relation to landings, the procedure is more awkward. Not only do the runway characteristics and the GE disrupt the usual flight motion, but crosswind and attitude errors are also particularly noteworthy. Therefore, new elements that serve as aids—namely, an instrument landing system (ILS) [78] or image processing techniques [79]—can be implemented to facilitate the operation in this flight phase. Both methodologies provide an important level of autonomy, although it could be said that the ILS is more reliable due to the fact that errors in the image-taking process due to the weather could be disastrous at typical landing velocities. Nevertheless, image processing would be ideal for other aircraft, such as helicopters or quadcopters, that are able to achieve a fixed-point flight.

Finally, real implementation is subject to reliable and high-powered hardware. Recent research, such as that of [80] or [81], suggests that the best alternatives today are systems based on field-programmable gate arrays (FPGAs), which adapt to the demanding requirements of the aerospace sector without losing flexibility and computing power.