System Identification and Nonlinear Model Predictive Control with Collision Avoidance Applied in Hexacopters UAVs

Abstract

Accurate trajectory tracking is a critical property of unmanned aerial vehicles (UAVs) due to system nonlinearities, under-actuated properties and constraints. Specifically, the use of unmanned rotorcrafts with accuracy trajectory tracking controllers in dynamic environments has the potential to improve the fields of environment monitoring, safety, search and rescue, border surveillance, geology and mining, agriculture industry, and traffic control. Monitoring operations in dynamic environments produce significant complications with respect to accuracy and obstacles in the surrounding environment and, in many cases, it is difficult to perform even with state-of-the-art controllers. This work presents a nonlinear model predictive control (NMPC) with collision avoidance for hexacopters’ trajectory tracking in dynamic environments, as well as shows a comparative study between the accuracies of the Euler–Lagrange formulation and the dynamic mode decomposition (DMD) models in order to find the precise representation of the system dynamics. The proposed controller includes limits on the maneuverability velocities, system dynamics, obstacles and the tracking error in the optimization control problem (OCP). In order to show the good performance of this control proposal, computational simulations and real experiments were carried out using a six rotary-wind unmanned aerial vehicle (hexacopter—DJI MATRICE 600). The experimental results prove the good performance of the predictive scheme and its ability to regenerate the optimal control policy. Simulation results expand the proposed controller in simulating highly dynamic environments that showing the scalability of the controller.

1. Introduction

1.1. Motivation

In recent years, the field of robotics has experienced exponential growth due to fast technological advances. The applications are not restricted to the industrial field, as nowadays robots are equipped with sophisticated and intelligent algorithms that are useful for performing complex task in unstructured and natural environments with a high degree of autonomy. The integration between robots and humans through service robotics has the objective of facilitating activities of daily life or improving repetitive tasks. As such, we can find mobile manipulators that are used to load transportation in structured environments [1,2], robotic systems that help people during their rehabilitation process [3] and aerial robotic vehicles that are useful in rescue or transportation operations [4]. Robot applications can be divided into four main groups: (i) Learning robots, which are robotic systems for whom the main objective is to improve learning and stimulation processes [5,6]. (ii) Construction robots, which are systems that can be programmed to develop complex repetitive tasks, where the main contributions are the decrease in production time and the prevention of accidents in dangerous workplaces [7,8]. (iii) Field robots, which show a variety of improvements in mining industries, agricultural and livestock, increasing the economy of the population [9,10]. (iv) Military robots, which are designed to perform exploration, security, rescue, among other applications in hostile environments that can be dangerous for humans, and these applications are highly developed using unmanned aerial vehicles (UAVs) [11].

The use of unmanned aerial vehicles have grown in a wide range of fields and has been extensively studied by the scientific community due to the versatility and the possibility to perform the fully autonomous tasks that humans are unable to. The extremely agility of the UAVs with four or more rotors has attracted attention due to their ability to vertically take off and land in confined or difficult to access spaces, which eases construction and enables having a higher payload capacity, which are useful in critical missions such as transportation, rescue, search, surveillance and disaster assistance. In these applications, the execution time is an extremely important feature due to the task only being executed for high-speed trajectories. To take advantage of UAVs, it is necessary to guarantee the safe execution of the application; however, dynamic environments are spaces with highly probabilities of collisions, where obstacle information is unknown at the beginning of the task, and thus the UAV must be capable of avoiding the obstacles using online-scheme information. Obstacle avoidance is an important feature that guarantees the correct functionality of UAV applications. In this context, research efforts are still needed to safely introduce UAVs as a technological tool with great potential to solve various current problems such as a collision avoidance control structure that can perform high-speed trajectories in dynamic environments.

1.2. Related Work

Depending on the environment, UAV applications can be divided in two groups: (i) Outdoor environments where many studies have employed UAVs with onboard sensors, e.g., Mellinger et al. presented collaborative grasping to logistic applications [12]; Bircher et al. performed path planning for structural inspection using UAVs [13]; search and rescue operations were developed by Oetter- shagen et al. in [14]; and an application consisting of tasks related to physical interaction with the environment was developed by Garimella et al. [15]. (ii) Indoor environments are the counterpart applications, developed in cluttered indoor experiments, examples of which include the study by Song and Hsu who presented navigation based on factor graph optimization (FGO) [16]; Paredes et al., who developed a hybrid acoustic and optical positioning system for accurate 3D movements [17]; and Sandino et al., who proposed an autonomous navigation system using partial observable and uncertainty sensor measurements [18]. Due to the different types of UAVs, these systems have the ability to perform a wide range of applications that are emerging. The most popular configuration of multi-rotors is the quadrotor UAV, which is applied in a variety of aerial fields, e.g., Radoglou–Grammatiski et al., who presented the compilation of the most important applications for precision agriculture using quadrotors [19]; and Gupta et al., who presented the advances of UAVs and the applications in transportation systems [20]. This configuration only presents four motors attached to the mechanical frame generating a limiting power that restricts the fields of use, especially for rescue and transportation tasks. However, nowadays the design of a multi-rotor with more than four rotors is a rapidly growing industry because multi-rotor configuration has presented important advances due to its high maneuverability and transportation capacity [21,22]. More works were conducted by Belmonte et al., who designed an octocopter for the inspection of mobile cranes [23]; Phong-Nguyen et al., who presented a sliding mode control algorithm with a fuzzy inference system considering variable gains for hexacopters [24]; and Ali et al., who developed adaptive backstepping for the attitude and altitude of coaxial octorotors [25].

The use of more rotors can produce high costs in production, a considerably increased size of the UAV and complex dynamics. Nowadays, it still a challenge to design effective controllers that ensure good performance in multi-rotors at high-speed trajectories. Precise trajectory tracking is an important feature for multi-rotors operating in real-world environments due to the considerable size of the structure and possible collision with obstacles. The main reasons that make this configuration more complex than the others are presented below: (i) System actuation: An inherently under-actuated system due to there only being four control inputs over the six degrees of freedom (DOF). (ii) Dynamics behavior: high nonlinearities produced by translational and rotational dynamics due to the considerable size of the system. (iii) Dynamic model mismatch: Unmodeled dynamics, uncertainties and disturbances produced for high-speed trajectories and the possibility of external forces such as wind in outdoor environments. All of these problems incited the special attention of researchers from different fields. As a consequence, a wide variety of control strategies have been proposed for the problem of trajectory tracking for hexacopters. Initially, some linear control methods were presented, and these works considered small angles’ assumptions that are necessary for linear control techniques, and the most common structures were presented by Alaimo et al., who developed a proportional–integral and derivative controller (PID) under linear assumptions in UAVs with an hexacopter configuration [26] and Salim et al. presented optimum linear control that stabilizes the attitude of a micro-hexacopter in indoor environments [27].

Linear models are only valid around the hover position, providing slow movements where the controllers cannot guarantee the convergence to the desired trajectories. In order to improve the performance and to satisfy the requirements of high-speed trajectories, a large number of studies have used the principles of nonlinear control, e.g., Chen et al., who proposed a nonlinear trajectory controller for UAVs based on backstepping and nonlinear observer [28] and Wang et al. presented the sliding mode control (SMC) with a variable structure to generate the control inputs [29]. Recently, many authors have proposed using geometric tracking controllers, e.g., Lee et al. showed the results of trajectory tracking using a nonlinear controller developed in a Euclidean group $SE\left(3\right)$ [30]; Mellinger et al. revealed the differential flatness properties in UAVs and the ability to derive the attitude, acceleration and angular rate [31]. The property of differential flatness improves the accurate trajectory tracking under high-speed references and it was demonstrated by [32,33]. In recent years, intelligent control approaches were used to developed advance control systems, and these schemes included artificial neural networks and fuzzy logic controllers that are highly used in multi-rotor UAVs; the results present interesting behaviors under disturbances and parameter changes [34,35,36,37].

Unlike multi-rotor control techniques, obstacle avoidance has not been highly developed by the research community. Obstacle avoidance is a relatively new field and most works have used the concept of artificial potential fields, a method which is based on the generation of repulsive and attractive forces that allow the movements of the system around obstacles. The literature presents the extensive applications with UAVs, but these works revealed the problem of a local minimal that cannot guarantee the execution of the task [38,39]. Nowadays, other methods have been proposed to solve the problem of obstacle avoidance, the most popular among which include: (i) rapidly exploring random tree (RRT), a formulation which generates a tree rooted at the initial configuration of the system and using random samples to create edges between feasible points. The work by Achtelik et al., who presented an RRT algorithm with a modification rapidly exploring random belief tree (RRBT), which was used to generate the collision free path using quadrotor UAVs [40]; (ii) The belief roadmap, which is a probabilistic version of the roadmap, which uses the Kalman filter for the planning motion, and has the ability to generate trajectories by avoiding obstacles; this structure was presented by [41]. (iii) The minimum snap trajectory generation this algorithm includes navigation through corridors whilst considering constraints on velocities and accelerations, the solution to which is based on the generation of optimal trajectories that minimizes a functional cost confirmed by square norm of the snap, an application of which was presented in [31]. All the obstacle avoidance algorithms presented beforehand only consider the prior knowledge of the obstacle in the environment, whilst also requiring high computational times and complex operations.

Taking into account the problems of no-dynamics constraints and only the prior knowledge of environments, the results thus produce unfeasible control actions and problems in dynamic environments with highly probabilities of collisions during the task execution. To fully exploit hexacopters’ capabilities and take advantage of the available computation power, optimization-based control techniques are becoming suitable for real-time problems. Model predictive control (MPC) has been used in many fields of robotics due to the accuracy of its tracking trajectories, robust performance and the ability to include constraints in the controller scheme. In function of the field, the general applications can be divided into the following groups:

• Ground mobile robots: where Sani et al. presented nonlinear model predictive control (NMPC) to solve the competitive games between ground robots [42], another work undertaken developed by Subramanian et al. increased the ability to avoid dynamic obstacles [43], and finally, a leader–follower structure control using NMPC through visual information to mobile robots was presented by Ribeiro et al. [44].
• Robotic arms and manipulation: In the last year, this field of research has been the focus of many studies due to the complexity of the systems and the scalability of the NMPC structure. The work of Osman et al. presented a task-space controller based on (NMPC) to control a mobile manipulator 10 (DOF) [45].
• Aerial mobile robots: this field has also been the subject of a large number of studies due to the high use of aerial vehicles in real-world applications, some applications of which were studies Neunert et al., who developed an unconstrained nonlinear predictive model for generation and tracking trajectories for the AscTec Firefly hexacopter [46]; Aoki et al., who presented an NMPC for position and attitude control applied in a hexacopter without three of the six motor configurations [47]; and finally, the contribution of Tzoumanikas et al., a letter which presented the NMPC designed for micro aerial vehicles (MAVs) equipped with a robotic arm [48].

This kind of controller has had a huge impact on the field of robotics thanks to the predictive behavior and ability to introduce system constraints in the controller scheme. The main idea of this controller is to generate the control actions trying to minimize a cost function over a prediction horizon. The cost function is solved using constrained optimization techniques; however, the resolution of constrained optimization problems is computationally expensive. Due to the recent developments in hardware and nonlinear optimization solvers, nonlinear predictive control is computationally applicable in many fields and has received the attention of a hexacopter control field [49,50,51,52,53].

1.3. Main Contributions of This Work

Run NMPC structures are realizable in modern computers and guarantee the execution of complex tasks but still require more computational time than the schemes mentioned beforehand. It is necessary to show performance under tracking accuracy and the ability to extend this technique to dynamic environments in order to add obstacles in the optimization problem. With all the aforementioned controllers and the significant results in many areas, this work presents the nonlinear model predictive control to fast trajectory tracking in dynamic environments applied in hexacopter platforms. The controller best generates the control policy that moves the hexacopter to the reference trajectory while avoiding obstacles through the environment. To construct the approach, this work developed the important contributions presented below:

• System dynamics: a large number of research platforms cannot be controlled through torque commands; as such, this work developed a reduced dynamic model of the hexacopter using general control velocities, which is possible with the incorporation of low-level PID schemes in mathematical representation. The PIDs guarantee the required velocities generated by the high-level controllers. The mathematical representation was developed through an Euler–Lagrange formulation and the data-driven technique known as dynamic mode decomposition (DMDc). Both approaches were identified using experimental data from the (DJI MATRICE 600 PRO); furthermore, this work presents accuracy comparative results between both techniques and selects the best formulation to build the controller scheme.
• NMPC formulation: this includes the differential kinematics and the reduced dynamic representation; furthermore, due to the ability of the NMPC controller, the system constraints can be included in the optimization problem. The constraints included in this work are: control action limits, control rate of change, and the system dynamics and static and dynamic obstacles. The nonlinear optimization problem was solved using CasADI framework and the problem was transformed into a nonlinear programming problem (NLP) form using the direct multiple-shooting method. The CasADI optimization toolkit guarantees a fast convergence with the possibility of extending the solution to onboard hardware implementation.
• Results: the experiments were conducted in real-world outdoor environments, where the system has a model mismatch and external disturbance product by air flows and delay in system communications. The experiments were developed through the hexacopter platform (DJI MATRICE 600 PRO) where multiple reference trajectories were selected in order to verify the performance of the controller. The metrics used in the experiments are the following: tracking accuracy, computational time and disturbance rejection. Additionally, the simulation results show the scalability in simulated high dynamic environments, considering the identified dynamics and obstacles; these results will be used as a start point in future research due to the fact that the hexacopter aerial platform does not have any sensor to measure obstacles during the experiments.

1.4. Outline

This work is structured as follows. Section 2 presents the instantaneous kinematics with the reduced dynamic model and system identification and validation. Furthermore, the controller formulation is presented in this section, which consists of the nonlinear model predictive controller with an obstacles avoidance scheme. Section 3 shows the real-world experiments and simulation results; furthermore, considering the experiment results, this section exhibits a review of the important aspects and the future research direction. The conclusions of the work are presented in Section 4.

2. Materials and Methods

2.1. Hexacopter System Preliminaries

This section presents the instantaneous kinematics and the dynamic model. The dynamic model was formulated in maneuverability velocities space using two different formulations: the Euler–Lagrange and dynamic mode decomposition. After the formulation process, this work presents the validation results in order to verify the performance of each formulation.

Figure 1 shows the hexacopter platform DJI MATRICE 600 PRO, where the world-fixed inertial frame is represented by $<\mathbb{I}>$ with the following unit vectors $\left\{{\mathbf{I}}_x,{\mathbf{I}}_y,{\mathbf{I}}_z\right\}$ and the body-fixed frame attached to hexacopter movements is defined by $<\mathbb{B}>$ with the unit vectors $\left\{{\mathbf{B}}_x,{\mathbf{B}}_y,{\mathbf{B}}_z\right\}$, where the center of mass (CoM) is aligned; furthermore, the hexacopter is configured with six motors in the mechanical frame which allows the movement of the system.

2.1.1. Kinematics Model

The robot has the ability to move through the inertial frame $<\mathbb{I}>$ and it is enabled to rotate only in yaw $\psi$ defined in the vertical axis ${\mathbf{B}}_z$, whilst the others’ angular rotations roll and pitch $(\varphi ,\theta )$ are not considered in this work due to this hexacopter platform having a low level flight controller which guarantees the hover position. The position and orientation of the point to be controlled is defined by the vector $\mathbf{\eta }={\left[\begin{array}{cccc}{\eta }_x& {\eta }_y& {\eta }_z& {\eta }_{\psi }\end{array}\right]}^T\in {\mathbb{R}}^4$ with respect to the frame $<\mathbb{I}>$, and the following equation defines these elements in greater detail:

$$\left\{\begin{array}{c}{\eta }_x={\eta }_{x_0}+d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right)\hfill \\ {\eta }_y={\eta }_{y_0}+d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right)\hfill \\ {\eta }_z={\eta }_{z_0}+d_z\hfill \\ {\eta }_{\psi }=\psi \hfill \end{array}\right.$$

(1)

where the values $({\eta }_{x_0},{\eta }_{y_0},{\eta }_{z_0})$ are the locations of the CoM and $(d_x,d_y,d_z)$ are the distances in the body-fixed frame $<\mathbb{B}>$ to the point of interest $\mathbf{\eta }$. In order to know the evolution of the interest point, it is necessary to introduce the concept of the instantaneous kinematic, defining the time derivative as $\dot{\mathbf{\eta }}=\frac{\partial \mathbf{\eta }}{\partial t}\mathbf{\mu }$. With these considerations, the velocity of the point is defined by the vector $\dot{\mathbf{\eta }}={\left[\begin{array}{cccc}{\dot{\eta }}_x& {\dot{\eta }}_y& {\dot{\eta }}_z& {\dot{\eta }}_{\psi }\end{array}\right]}^T\in {\mathbb{R}}^4$ with respect to the frame $<\mathbb{I}>$; furthermore, the time derivative produces maneuverability velocities $\mathbf{\mu }$ attached to the frame $<\mathbb{B}>$. Due to the low-level PIDs controllers, the maneuverability vector is defined by $\mathbf{\mu }={\left[\begin{array}{cccc}{\mu }_l& {\mu }_m& {\mu }_n& \omega \end{array}\right]}^T\in {\mathbb{R}}^4$, with the longitudinal velocities $({\mu }_l,{\mu }_m,{\mu }_n)$ through the axis $({\mathbf{B}}_x,{\mathbf{B}}_y,{\mathbf{B}}_z)$ and the angular rate of change $\omega$ over unit vector ${\mathbf{B}}_z$. Using the definition of instantaneous kinematics, the evolution can be defined by the following equation:

$$\left\{\begin{array}{c}{\dot{\eta }}_x={\mu }_l\cos \left(\psi \right)-{\mu }_m\sin \left(\psi \right)-(d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right))\omega \hfill \\ {\dot{\eta }}_y={\mu }_l\sin \left(\psi \right)+{\mu }_m\cos \left(\psi \right)+(d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))\omega \hfill \\ {\dot{\eta }}_z={\mu }_n\hfill \\ {\dot{\eta }}_{\psi }=\omega \hfill \end{array}\right.$$

(2)

the equation presented before can be written in a matrix form, defined as:

$$\left[\begin{array}{c}{\dot{\eta }}_x\\ {\dot{\eta }}_y\\ {\dot{\eta }}_z\\ {\dot{\eta }}_{\psi }\end{array}\right]=\left[\begin{array}{cccc}\cos \left(\psi \right)& -\sin \left(\psi \right)& 0& -(d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right))\\ \sin \left(\psi \right)& \cos \left(\psi \right)& 0& (d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mu }_l\\ {\mu }_m\\ {\mu }_n\\ \omega \end{array}\right]$$

(3)

where the expressions $-(d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right))$ and $(d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))$ represent the additional behavior considering the displacement of the point of interest. To simplify the notation, Equation (3) is expressed in the compact form in (4)

$$\dot{\mathbf{\eta }}\left(t\right)=\mathbf{J}\left(\psi \left(t\right)\right)\mathbf{\mu }\left(t\right)$$

(4)

where $\mathbf{J}\left(\psi \left(t\right)\right)\in {\mathbb{R}}^{4\times 4}$ is the Jacobian matrix which allows the linear mapping between the control maneuverability velocities $\mathbf{\mu }$ to the evolution of the point of interest $\dot{\mathbf{\eta }}$, Equation (3) is expressed in vector-function form as:

$$\dot{\mathbf{\eta }}\left(t\right)={\mathbf{f}}_k(\psi \left(t\right),\mathbf{\mu }\left(t\right))$$

(5)

Considering that $\ddot{\mathbf{\eta }}\left(t\right)=\frac{d}{dt}\dot{\mathbf{\eta }}$, the following relation was developed:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\ddot{\eta }}_x\\ {\ddot{\eta }}_y\\ {\ddot{\eta }}_z\\ {\ddot{\eta }}_{\psi }\end{array}\right]=\left[\begin{array}{cccc}\cos \left(\psi \right)& -\sin \left(\psi \right)& 0& -(d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right))\\ \sin \left(\psi \right)& \cos \left(\psi \right)& 0& (d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}\dot{{\mu }_l}\\ \dot{{\mu }_m}\\ \dot{{\mu }_n}\\ \dot{\omega }\end{array}\right]+\cdots \\ \hfill \left[\begin{array}{cccc}-\omega \sin \left(\psi \right)& -\omega \cos \left(\psi \right)& 0& -\omega (d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))\\ \omega \cos \left(\psi \right)& -\omega \sin \left(\psi \right)& 0& \omega (d_x\sin \left(\psi \right)+d_y\cos \left(\psi \right))\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mu }_l\\ {\mu }_m\\ {\mu }_n\\ \omega \end{array}\right]\end{array}$$

(6)

it can be written in compact form as $\ddot{\mathbf{\eta }}\left(t\right)=\mathbf{J}\left(\psi \right)\dot{\mathbf{\mu }}\left(t\right)+\dot{\mathbf{J}}(\psi ,\omega )\mathbf{\mu }\left(t\right)$, where $\dot{\mathbf{\mu }}={\left[\begin{array}{cccc}\dot{{\mu }_l}& \dot{{\mu }_m}& \dot{{\mu }_n}& \dot{\omega }\end{array}\right]}^T\in {\mathbb{R}}^4$ is the vector of maneuverability accelerations, and this formulation will be used in the dynamic model section.

2.1.2. Dynamic Model

This section presents the dynamic model, which has been developed considering low-level PIDs controllers that only generate longitudinal movements in $\left\{{\mathbf{I}}_x,{\mathbf{I}}_y,{\mathbf{I}}_z\right\}$ and the angular rotation through ${\mathbf{B}}_z$. The dynamic model is useful for guaranteeing the stability of the proposed controller in real-world applications.

Euler–Lagrange Formulation

One of the approaches to find the mathematical model is the Euler–Lagrange formulation. This formulation is a convenient analytical method for obtaining the dynamic model and studying the physical phenomena of the hexacopter. The Euler–Lagrange formulation uses the total energy of the system confirmed by kinetic and potential energy. The total kinetic energy is defined by the following equation:

$$T\left(\dot{\mathbf{\eta }}\right)=\frac{1}{2}{\dot{\mathbf{\eta }}}^T\mathbf{M}\dot{\mathbf{\eta }}$$

(7)

where $\mathbf{M}=diag(m,m,m,I)$ is a diagonal matrix confirmed by the mass of the hexacopter m and the moment of inertia I around the vertical axis ${\mathbf{B}}_z$. On the other hand, the potential energy is the position or configuration of the system with respect to the world-fixed frame $<\mathbb{I}>$, and the potential energy is described by:

$$V\left(\mathbf{\eta }\right)=mg({\eta }_z+d_z)$$

(8)

where g represents the gravitational acceleration. The Lagrange formulation is obtained by the subtraction of the kinetic $T\left(\dot{\mathbf{\eta }}\right)$ and potential $V\left(\mathbf{\eta }\right)$ energy expressed as:

$$L=\frac{1}{2}{\dot{\mathbf{\eta }}}^T\mathbf{M}\dot{\mathbf{\eta }}-mg({\eta }_z+d_z)$$

(9)

Now, applying the Euler–Lagrange formulation:

$$\frac{d}{dt}\left(\frac{dL}{\dot{\mathbf{\eta }}}\right)-\frac{dL}{\mathbf{\eta }}={\mathbf{f}}_{\mathbf{I}}$$

(10)

where ${\mathbf{f}}_{\mathbf{I}}={\left[\begin{array}{cccc}{f}_{x_I}& f_{y_I}& f_{z_I}& {\tau }_{{\psi }_I}\end{array}\right]}^T\in {\mathbb{R}}^4$ are the generalized forces and torque vector with respect to the frame $<\mathbb{I}>$; with this formulation, it is possible to obtain the nonlinear model by the following equations:

$$\left\{\begin{array}{c}m\ddot{{\eta }_x}-d_{y}m\cos \left(\psi \right)\ddot{{\eta }_{\psi }}-d_{x}m\sin \left(\psi \right)\ddot{{\eta }_{\psi }}-d_{x}m\cos \left(\psi \right){\dot{{\eta }_{\psi }}}^2+d_{y}m\sin \left(\psi \right){\dot{{\eta }_{\psi }}}^2=f_{x_I}\hfill \\ m\ddot{{\eta }_y}+d_{x}m\cos \left(\psi \right)\ddot{{\eta }_{\psi }}-d_{y}m\sin \left(\psi \right)\ddot{{\eta }_{\psi }}-d_{y}m\cos \left(\psi \right){\dot{{\eta }_{\psi }}}^2-d_{x}m\sin \left(\psi \right){\dot{{\eta }_{\psi }}}^2=f_{y_I}\hfill \\ m\ddot{{\eta }_z}+mg=f_{z_I}\hfill \\ I\ddot{{\eta }_{\psi }}+{d_x}^{2}m\ddot{{\eta }_{\psi }}+{d_y}^{2}m\ddot{{\eta }_{\psi }}-d_{y}m\cos \left(\psi \right)\ddot{{\eta }_x}+d_{x}m\cos \left(\psi \right)\ddot{{\eta }_y}-d_{x}m\sin \left(\psi \right)\ddot{{\eta }_x}-d_{y}m\sin \left(\psi \right)\ddot{{\eta }_y}={\tau }_{{\psi }_I}\hfill \end{array}\right.$$

(11)

the equations presented in (11) can be written in a matrix form, resulting in:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{f}_{x_I}\\ f_{y_I}\\ f_{z_I}\\ {\tau }_{{\psi }_I}\end{array}\right]=\left[\begin{array}{cccc}m& 0& 0& -m(d_y\cos \left(\psi \right)+d_x\sin \left(\psi \right))\\ 0& m& 0& m(d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))\\ 0& 0& m& 0\\ -m(d_y\cos \left(\psi \right)+d_x\sin \left(\psi \right))& m(d_x\cos \left(\psi \right)-d_y\sin \left(\psi \right))& 0& m{d_x}^2+m{d_y}^2+I\end{array}\right]\left[\begin{array}{c}{\ddot{\eta }}_x\\ {\ddot{\eta }}_y\\ {\ddot{\eta }}_z\\ \ddot{{\eta }_{\psi }}\end{array}\right]\cdots \\ \hfill \cdots +\left[\begin{array}{cccc}0& 0& 0& d_{y}m\sin \left(\psi \right)\dot{{\eta }_{\psi }}-d_{x}m\cos \left(\psi \right)\dot{{\eta }_{\psi }}\\ 0& 0& 0& -d_{y}m\cos \left(\psi \right)\dot{{\eta }_{\psi }}-d_{x}m\sin \left(\psi \right)\dot{{\eta }_{\psi }}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\dot{\eta }}_x\\ {\dot{\eta }}_y\\ {\dot{\eta }}_z\\ \dot{{\eta }_{\psi }}\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ gm\\ 0\end{array}\right]\end{array}$$

(12)

To simplify the notation, this work compacts Equation (12), resulting in the following classical representation:

$${\mathbf{f}}_{\mathbf{I}}\left(t\right)=\overline{\mathbf{H}}\left(\mathbf{\eta }\right)\ddot{\mathbf{\eta }}\left(t\right)+\overline{\mathbf{C}}(\mathbf{\eta },\dot{\mathbf{\eta }})\dot{\mathbf{\eta }}\left(t\right)+\overline{\mathbf{g}}$$

(13)

where $\overline{\mathbf{H}}\in {\mathbb{R}}^{4\times 4}$ is the mass and inertia matrix of the hexacopter system, in addition to being positive and symmetric definite; $\overline{\mathbf{C}}\in {\mathbb{R}}^{4\times 4}$ is the matrix of Coriolis forces and $\overline{\mathbf{g}}\in {\mathbb{R}}^4$ is known as the gravitational vector. Vectors $\ddot{\mathbf{\eta }}=\left[\begin{array}{cccc}{\ddot{\eta }}_x& {\ddot{\eta }}_y& {\ddot{\eta }}_z& \ddot{{\eta }_{\psi }}\end{array}\right]$ and $\dot{\mathbf{\eta }}=\left[\begin{array}{cccc}{\dot{\eta }}_x& {\dot{\eta }}_y& {\dot{\eta }}_z& \dot{{\eta }_{\psi }}\end{array}\right]$ are the acceleration and velocity of the point of interest in the inertia frame $<\mathbb{I}>$.

Due to the fact that the hexacopter platform can be controlled through reference maneuverability velocity commands, this work converts the generalized force and torque inputs of the dynamic model (13) into reference maneuverability velocities as system control inputs. Considering the position dynamic model presented in [54] which considers all DOF in a multirotor:

$$\begin{array}{c}\hfill \left[\begin{array}{c}\dot{{\mu }_l}\\ \dot{{\mu }_m}\\ \dot{{\mu }_n}\end{array}\right]=\left[\begin{array}{ccc}0& w_z& -w_y\\ -w_z& 0& w_x\\ w_y& -w_x& 0\end{array}\right]\left[\begin{array}{c}{\mu }_l\\ {\mu }_m\\ {\mu }_n\end{array}\right]-\frac{f_{z_I}}{m}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]+\cdots \\ \hfill g\left[\begin{array}{ccc}\cos \left(\psi \right)\cos \theta & -\sin \psi \cos \varphi +\cos \psi \sin \theta \sin \varphi & \sin \psi \sin \varphi +\cos \psi \cos \varphi \sin \theta \\ \sin \left(\psi \right)\cos \theta & \cos \psi \cos \theta +\sin \varphi \sin \theta \sin \psi & -\cos \psi \sin \varphi +\sin \theta \sin \psi \cos \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}$$

(14)

it can be written in a compact form as $\dot{\mathbf{\mu }}={\left[\begin{array}{c}\mathbf{w}\end{array}\right]}_x\mathbf{\mu }+g\mathbf{R}{(\theta ,\varphi ,\psi )}^T{\mathbf{e}}_{\mathbf{3}}-\frac{f_z}{m}{\mathbf{e}}_{\mathbf{3}}$, where $\mathbf{w}=\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]$ are the angular velocities associated with the frame $<\mathbb{B}>$, ${\left[\begin{array}{c}\mathbf{w}\end{array}\right]}_x$ represents skew symmetric matrix and finally $\mathbf{R}(\theta ,\varphi ,\psi )$ represents the rotation matrix from the frame $<\mathbb{B}>$ to $<\mathbb{I}>$. For any multicopter, drags applied to rotating blades are in the direction of the body axes; from (14), the position dynamic model considering the aerodynamic drag model is represented as:

$$\left[\begin{array}{c}\dot{{\mu }_l}\\ \dot{{\mu }_m}\end{array}\right]=\left[\begin{array}{c}{\mu }_m{\omega }_z-{\mu }_n{\omega }_y-g\sin \theta -\frac{k_{drag}}{m}{\mu }_l\\ {\mu }_n{\omega }_x-{\mu }_l{\omega }_z+g\cos \theta \sin \varphi -\frac{k_{drag}}{m}{\mu }_m\end{array}\right]$$

(15)

where $\dot{{\mu }_l}$, $\dot{{\mu }_m}$ are the accelerations in the body frame $<\mathbb{B}>$ and $k_{drag}$ is the drag constant. To include the drag coefficients in the dynamic model, this work split Equation (14) and can be written as:

$$\left[\begin{array}{c}{a}_x\\ a_y\end{array}\right]=\left[\begin{array}{c}{\mu }_m{\omega }_z-{\mu }_n{\omega }_y-\frac{k_{drag}}{m}{\mu }_l\\ {\mu }_n{\omega }_x-{\mu }_l{\omega }_z-\frac{k_{drag}}{m}{\mu }_m\end{array}\right]$$

(16)

where $a_x=\dot{{\mu }_l}+g\sin \left(\theta \right)$ and $a_y=\dot{{\mu }_m}-g\cos \left(\theta \right)\sin \left(\varphi \right)$. Since the low-level controller guarantees hover, the cross terms are ignored, so the specific force satisfies:

$$\left[\begin{array}{c}{f}_x\\ f_y\end{array}\right]=\left[\begin{array}{c}-k_{drag}{\mu }_l\\ -k_{drag}{\mu }_m\end{array}\right]$$

(17)

where the forces produced in the system ${\mathbf{f}}_{hB}=\left[\begin{array}{cc}{f}_x& f_y\end{array}\right]$ are generate by a PD controller for the horizontal plane. The PD controller was designed as:

$${\mathbf{f}}_{hB}=k_{pl}({\mathbf{\mu }}_{h_{ref}}-{\mathbf{\mu }}_h)+k_{dl}({\dot{\mathbf{\mu }}}_{h_{ref}}-{\dot{\mathbf{\mu }}}_h)$$

(18)

where ${\mathbf{\mu }}_h={\left[\begin{array}{cc}{\mu }_l& {\mu }_m\end{array}\right]}^T\in {\mathbb{R}}^2$ is the vector of the horizontal maneuverability velocities, ${\mathbf{\mu }}_{h_{ref}}={\left[\begin{array}{cc}{\mu }_{lref}& {\mu }_{mref}\end{array}\right]}^T\in {\mathbb{R}}^2$ is the reference desired velocity and $k_{pl}$, $k_{dl}$ are positive definite matrices. For the PD controller, this work used the following assumptions if ${\dot{\mathbf{\mu }}}_{h_{ref}}=0$ then ${\lim }_{t\to \infty }\parallel {\mathbf{f}}_h\parallel =0$ and ${\lim }_{t\to \infty }\parallel {\tilde{\mathbf{\mu }}}_h\parallel =0$, respectively.

A similar concept was applied to the vertical force attached to the body frame $f_{z_B}=f_{ref}+mg$. Designing a PD controller for the vertical thrust as:

$$f_{z_B}=k_{pn}({\mu }_{n_{ref}}-{\mu }_n)+k_{dn}({\dot{\mu }}_{n_{ref}}-{\dot{\mu }}_n)+mg$$

(19)

where $k_{pn}$, $k_{dn}$ are positive definite matrices, this work considers the following assumptions if ${\dot{\mu }}_{n_{ref}}=0$ then the ${\lim }_{t\to \infty }\parallel f_{z_I}\parallel =0$ and ${\lim }_{t\to \infty }\parallel {\tilde{\mu }}_n\parallel =0$, respectively.

The same approach can be extended to the attitude dynamic model presented in [54], which is defined as:

$${\mathbf{J}}_{\mathbf{B}}·\dot{\mathbf{w}}=-\mathbf{w}\times ({\mathbf{J}}_{\mathbf{B}}·\mathbf{w})+{\mathbf{G}}_{\mathbf{B}}+\mathbf{\tau }$$

(20)

where $\mathbf{\tau }\triangleq \left[\begin{array}{ccc}{\tau }_x& {\tau }_y& {\tau }_{\psi }\end{array}\right]\in {\mathbb{R}}^3$ is the vector of moments in the body axes $<\mathbb{B}>$; ${\mathbf{J}}_{\mathbf{B}}\in {\mathbb{R}}^{3\times 3}$ includes the moment of inertia and ${\mathbf{G}}_{\mathbf{B}}\triangleq {\left[\begin{array}{ccc}{G}_{B,\varphi }& G_{B,\theta }& G_{B,\psi }\end{array}\right]}^T\in {\mathbb{R}}^3$ represents the gyroscopic torques. Ignoring the term $-\mathbf{w}\times ({\mathbf{J}}_{\mathbf{B}}·\mathbf{w})+{\mathbf{G}}_{\mathbf{B}}$, the simplified attitude dynamic model is defined as: ${\mathbf{J}}_{\mathbf{B}}\dot{\mathbf{w}}={\tau }_{\psi }$, where a PD controller for angular translations is defined as:

$${\tau }_{\psi }=k_{p\omega }({\omega }_{ref}-\omega )+k_{d\omega }({\dot{\omega }}_{ref}-\dot{\omega })$$

(21)

where $k_{p\omega },k_{d\omega }\in \mathbb{R}$ are positive scalars and the following assumptions are true if ${\dot{\omega }}_{ref}=0$; then, ${\lim }_{t\to \infty }\parallel {\tau }_{\psi }\parallel =0$ and ${\lim }_{t\to \infty }\parallel \tilde{\omega }\parallel =0$, respectively.

After all the considerations presented beforehand, this work introduces the general structure of low-level PD controllers combining Equations (18), (19) and (21), finally the structure is presented in Equation (22).

$$\left[\begin{array}{c}{f}_{x_B}\\ f_{y_B}\\ f_{z_B}\\ {\tau }_{{\psi }_B}\end{array}\right]=\left[\begin{array}{cccc}{k}_{pl}& 0& 0& 0\\ 0& k_{pl}& 0& 0\\ 0& 0& k_{pn}& 0\\ 0& 0& 0& k_{p\omega }\end{array}\right]\left[\begin{array}{c}{\mu }_{lref}\\ {\mu }_{mref}\\ {\mu }_{nref}\\ {\omega }_{ref}\end{array}\right]-\left[\begin{array}{cccc}{k}_{pl}& 0& 0& 0\\ 0& k_{pl}& 0& 0\\ 0& 0& k_{pn}& 0\\ 0& 0& 0& k_{p\omega }\end{array}\right]\left[\begin{array}{c}{\mu }_l\\ {\mu }_m\\ {\mu }_n\\ \omega \end{array}\right]-\left[\begin{array}{cccc}{k}_{dl}& 0& 0& 0\\ 0& k_{dl}& 0& 0\\ 0& 0& k_{dn}& 0\\ 0& 0& 0& k_{d\omega }\end{array}\right]\left[\begin{array}{c}{\dot{\mu }}_l\\ {\dot{\mu }}_m\\ {\dot{\mu }}_n\\ \dot{\omega }\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ mg\\ 0\end{array}\right]$$

(22)

Equation (22) can be written in a compact form as ${\mathbf{f}}_{\mathbf{B}}\left(t\right)={\mathbf{K}}_{\mathbf{p}}{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right)-{\mathbf{K}}_{\mathbf{p}}\mathbf{\mu }\left(t\right)-{\mathbf{K}}_{\mathbf{d}}\dot{\mathbf{\mu }}\left(t\right)+\mathbf{g}$, where the values $k_{pl}$, $k_{pm}$ and $k_{p\omega }$ are the proportional values of the PD controller; $k_{dl}$, $k_{dm}$ and $k_{d\omega }$ are the derivative gains; ${\mathbf{\mu }}_{\mathbf{ref}}={\left[\begin{array}{cccc}{\mu }_{lref}& {\mu }_{mref}& {\mu }_{nref}& {\omega }_{ref}\end{array}\right]}^T\in {\mathbb{R}}^4$ are the reference maneuverability velocities or control vector of the hexacopter; $\mathbf{\mu }$ and $\dot{\mathbf{\mu }}$ are the real velocities and accelerations generated in the system.

Finally, with all the transformations considered beforehand, Equations (4) and (6) are substituted into a dynamic hexarotor model (12), and equating the expression to (22), a new reduced dynamic model can written as:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\mu }_{lref}\\ {\mu }_{mref}\\ {\mu }_{nref}\\ {\omega }_{ref}\end{array}\right]=\left[\begin{array}{cccc}{\zeta }_1& 0& 0& {\zeta }_2\\ 0& {\zeta }_3& 0& {\zeta }_4\\ 0& 0& {\zeta }_5& 0\\ b{\zeta }_6& a{\zeta }_7& 0& {\zeta }_8(a^2+b^2)+{\zeta }_9\end{array}\right]\left[\begin{array}{c}{\dot{\mu }}_l\\ {\dot{\mu }}_m\\ {\dot{\mu }}_n\\ \dot{\omega }\end{array}\right]+\left[\begin{array}{cccc}{\zeta }_{10}& \omega {\zeta }_{11}& 0& a\omega {\zeta }_{12}\\ \omega {\zeta }_{13}& {\zeta }_{14}& 0& b\omega {\zeta }_{15}\\ 0& 0& {\zeta }_{16}& 0\\ a\omega {\zeta }_{17}& b\omega {\zeta }_{18}& 0& {\zeta }_{19}\end{array}\right]\left[\begin{array}{c}{\mu }_l\\ {\mu }_m\\ {\mu }_n\\ \omega \end{array}\right]\end{array}$$

(23)

This new formulation represents the evolution of the hexacopter’s general velocities with the reference maneuverability velocities as the input of the system. This representation is useful because commercial aerial platforms can be directly controlled with maneuverability velocities, ignoring the low-level control. Equation (23) can be compactly written as:

$${\mathbf{\mu }}_{\mathbf{ref}}\left(t\right)=\mathbf{H}(\mathbf{\zeta },a,b)\dot{\mathbf{\mu }}\left(t\right)+\mathbf{C}(\mathbf{\zeta },\mathbf{\mu })\mathbf{\mu }\left(t\right)$$

(24)

where $\mathbf{H}={\left({\mathbf{RK}}_{\mathbf{p}}\right)}^{-1}\left(\right(\overline{\mathbf{H}}\mathbf{J}+\mathbf{R}{\mathbf{K}}_{\mathbf{d}})$, $\mathbf{C}={\left({\mathbf{RK}}_{\mathbf{p}}\right)}^{-1}(\overline{\mathbf{H}}\dot{\mathbf{J}}+\mathbf{R}{\mathbf{K}}_{\mathbf{p}}+\overline{\mathbf{C}}\mathbf{J})$, $\mathbf{G}={\left({\mathbf{RK}}_{\mathbf{p}}\right)}^{-1}(\mathbf{g}-\mathbf{R}\overline{\mathbf{G}}))=0$. Thus, $\mathbf{H}(\zeta ,a,b)\in {\mathbb{R}}^{4\times 4}$ is a positive definite matrix, which is the new mass and inertia matrix of the hexacopter robot, the new Coriolis and Centripetal matrix is defined by $\mathbf{C}(\mathbf{\zeta },\mathbf{\mu })\in {\mathbb{R}}^{4\times 4}$, $\mathbf{R}$ represents the rotation matrix under the z axis from frame $<\mathbb{B}>$ to frame $<\mathbb{I}>$. The vector of dynamic parameters represents the combination of all the internal values in the hexacopter robot such as the physical, mechanical, electrical and PD values, and the vector is defined by $\mathbf{\zeta }=\left[\begin{array}{cccc}{\zeta }_1& {\zeta }_2& \dots & {\zeta }_{l_d}\end{array}\right]\in {\mathbb{R}}^{l_d}$ where $l_d=19$ is the number of variables that this work needs to identify as the details of each parameters are presented in Appendix A for Equations (A1) and (A2).

Euler–Lagrange Identification and Validation

This section presents the identification and validation of the dynamic parameters, as this work uses real experimental data to estimate these values in order to use the model in the proposed controller. This work transforms the mathematical model (24) in the linear parametric representation into:

$${\mathbf{\mu }}_{\mathbf{ref}}\left(t\right)=\mathbf{\Theta }(\dot{\mathbf{\mu }}\left(t\right),\mathbf{\mu }\left(t\right))\mathbf{\zeta }$$

(25)

where the matrix $\mathbf{\Theta }$ confirms the values of velocities and accelerations obtained from real-world experiments. In order to estimate the vector of dynamic values, this work uses snapshot measurements over the time as $l\in [t,t+t_f]$ where l is an instant of measure and $t_f$ is the final time of the experimental information. The identification process was developed through optimization techniques defining ${\ell }_m(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t),\mathbf{\zeta }):{\mathbb{R}}^4\to {\mathbb{R}}_{\ge 0}$, which is a positive semi-definite function confirmed by:

$${\ell }_m(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t),\mathbf{\zeta })=\frac{1}{2}\stackrel{Modelidentificationcost}{\overbrace{\left|\right|{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t)-\mathbf{\Theta }(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t))\mathbf{\zeta }{\left|\right|}_{{\mathbf{Q}}_m}^2}}$$

(26)

Model identification cost: this function was formulated using the subtraction between the reference maneuverability ${\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t)$ velocities applied in the hexacopter and the system measurements defined by the matrix $\mathbf{\Theta }(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t))$; the operator $\left|\right|·\left|\right|$ is known as the Euclidean norm; and ${\mathbf{Q}}_m\in {\mathbb{R}}_{>0}^{4\times 4}$ is a positive definite weighting matrix.

With Equation (26), it is possible to generate the functional cost or performance index over the experimental data, which is formulated as follows:

$$V_m(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t),\mathbf{\zeta })={\int }_t^{t+t_f}{\ell }_m(\dot{\mathbf{\mu }}\left(\tau \right|t),\mathbf{\mu }\left(\tau \right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(\tau \right|t),\mathbf{\zeta })d\tau$$

(27)

Considering the performance index function presented in (27), the optimization structures are defined in (28), which was solved using the sequential quadratic programming (SQP) technique.

$${\mathcal{P}}_m:P_m(\dot{\mathbf{\mu }}\left(t\right),\mathbf{\mu }\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right))=\underset{\mathbf{\zeta }}{\min }{V}_m(\dot{\mathbf{\mu }}\left(l\right|t),\mathbf{\mu }\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t),\mathbf{\zeta })$$

(28)

This work used the experimental information of the hexacopter platform DJI MATRICE 600 PRO to identify the dynamic parameters. The aerial system has a low-level flight controller called A3, which is confirmed by three global positioning systems (GPS) and three inertial measurement unit systems (IMUs). Furthermore, this system provides highly precise measurements from the system states. The identified parameters are presented in

Table 1. Dynamic parameters of the hexacopter aerial platform.

System	Dynamic Parameters
DJI MATRICE 600 PRO	${\zeta }_1=2.11$	${\zeta }_2=-0.005$	${\zeta }_3=1.8$
	${\zeta }_4=3.17$	${\zeta }_5=1.78$	${\zeta }_6=0.39$
	${\zeta }_7=-0.003$	${\zeta }_8=-0.03$	${\zeta }_9=0.006$
	${\zeta }_{10}=0.02$	${\zeta }_{11}=0.002$	${\zeta }_{12}=0.06$
	${\zeta }_{13}=0.70$	${\zeta }_{14}=0.02$	${\zeta }_{15}=-0.05$
	${\zeta }_{16}=-0.01$	${\zeta }_{17}=-0.005$	${\zeta }_{18}=-0.01$
	${\zeta }_{19}=0.831$

; furthermore, for the identification and validation process, this work used a different kind of reference velocity signals.

Figure 2 shows the results of the identification process, where the signals ${\mu }_l$, ${\mu }_m$, ${\mu }_n$ and $\omega$ were obtained by real-world experiments; and ${\mu }_{lm}$, ${\mu }_{mm}$, ${\mu }_{nm}$ and ${\omega }_m$ are the estimated values of the dynamic model using the optimization formulation presented previously. The presence of noise in the signals is an important factor due to the fact that it probably causes problems in the identification process; this work uses a filter defined as: $\lambda /(s+\lambda )$ with $\lambda =1$ to eliminate noise in the real measurements. The filter was applied to the system measurements and the reference maneuverability velocities.

The validation process of the dynamic parameters is a very important factor; hence, the model needs to estimate the behavior of the system with different signals in order to represent the real dynamics of the system. The validation signals are shown in Figure 3, the validation process shows the performance of the estimate dynamic model, which will be useful for the comparative section.

With all the previously presented results and the validation of the dynamic parameters, this work formulates the identified model using Euler-Lagrange as a vector-function presented as:

$$\dot{\mathbf{\mu }}\left(t\right)={\mathbf{f}}_{SQP}(\mathbf{\zeta },\mathbf{\mu }\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right))$$

(29)

Dynamic Mode Decomposition Formulation

This section presents the second approach of the dynamic model formulation which, due to the hexacopter platform, only has internal low-level PIDs; and the system tries to stay close to the hover-position with small angular variations in roll and pitch angles $(\varphi ,\theta )$. Furthermore, this kind of hexacopter platforms cannot be controlled with the torques commands, and the control inputs at the system are the reference maneuverability velocities ${\mathbf{\mu }}_{\mathbf{ref}}$. Considering the small angle assumptions and the inputs in the system, the model can hence be formulated with a linear approximation for time-varying systems resulting in:

$$\dot{\mathbf{\mu }}\left(t\right)=\mathbf{A}\mathbf{\mu }\left(t\right)+\mathbf{B}{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right)$$

(30)

where $\mathbf{\mu }$ and $\dot{\mathbf{\mu }}$ are the vectors of the maneuverability velocities and accelerations; ${\mathbf{\mu }}_{\mathbf{ref}}$ is the reference maneuverability velocity vector also known as the control vector; matrices $\mathbf{A}\in {\mathbb{R}}^{4\times 4}$ and $\mathbf{B}\in {\mathbb{R}}^{4\times 4}$ represent the unforced system and the contribution of the control vector, respectively.

One of the emerging techniques to identify systems by experimental information is that of dynamic mode decomposition (DMD) [55,56,57], where the objective is to approximate the matrices $\mathbf{A}$ and $\mathbf{B}$. The DMD algorithm proposes the construction of snapshot measurements s and the formation of augmented matrices, resulting in a new formulation of the system as follows:

$$\dot{\mathbf{\chi }}\approx \mathbf{A}\mathbf{\chi }+\mathbf{B}\Gamma$$

(31)

where the new matrices are defined below:

$$\dot{\mathbf{\chi }}=\left[\begin{array}{cccc}|& |& & |\\ \dot{{\mathbf{\mu }}_1}& \dot{{\mathbf{\mu }}_2}& \cdots & \dot{{\mathbf{\mu }}_s}\\ |& |& & |\end{array}\right]$$

$$\mathbf{\chi }=\left[\begin{array}{cccc}|& |& & |\\ {\mathbf{\mu }}_1& {\mathbf{\mu }}_2& \cdots & {\mathbf{\mu }}_s\\ |& |& & |\end{array}\right]$$

$$\Gamma =\left[\begin{array}{cccc}|& |& & |\\ {\mathbf{\mu }}_{{\mathbf{ref}}_{\mathbf{1}}}& {\mathbf{\mu }}_{{\mathbf{ref}}_{\mathbf{2}}}& \cdots & {\mathbf{\mu }}_{{\mathbf{ref}}_{\mathbf{s}}}\\ |& |& & |\end{array}\right]$$

$\dot{\mathbf{\chi }}$ and $\mathbf{\chi }$ are the augmented matrices confirmed by the maneuverability accelerations and velocities, respectively; and $\Gamma$ is the matrix of the reference velocities applied in the system over the s number of snapshots.

For estimation purposes, the system can be formulated as:

$$\dot{\mathbf{\chi }}\approx \mathsf{\Phi }\mathsf{\Omega }$$

(32)

where $\mathsf{\Phi }=\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\end{array}\right]\in {\mathbb{R}}^{4\times (4+4)}$ is the matrix confirmed by unknown values of $\mathbf{A}$ and $\mathbf{B}$; and $\mathsf{\Omega }={\left[\begin{array}{cc}\mathbf{\chi }& \mathsf{\Gamma }\end{array}\right]}^T\in {\mathbb{R}}^{(4+4)\times s}$ is the data matrix obtained from the system. This work finds the best values of the unknown matrix using the Frobenius norm $\left|\right|\dot{\mathbf{\chi }}-\mathsf{\Phi }\mathsf{\Omega }{\left|\right|}_F$, defining the values in the following equation:

$$\mathsf{\Phi }\approx \dot{\mathbf{\chi }}{\mathsf{\Omega }}^{†}$$

(33)

One of the best methods to find the solution of the Frobenius norm is the singular value decomposition (SVD); therefore, applying this method to the augmented data matrix resulting as $\mathsf{\Omega }\approx \overline{\mathbf{U}}\overline{\Sigma }{\overline{\mathbf{V}}}^T$, where $\overline{\mathbf{U}}$, $\overline{\Sigma }$ and $\overline{\mathbf{V}}$ are matrices with a truncation value $\overline{r}$, with these considerations presented above, the estimation is defined by the following equation:

$$\mathsf{\Phi }\approx \dot{\mathbf{\chi }}\overline{\mathbf{V}}{\overline{\Sigma }}^{-1}{\overline{\mathbf{U}}}^T$$

(34)

To approximate the values of the matrices $\mathbf{A}$ and $\mathbf{B}$, the Equation (34) can be written as (35), considering the splitting values of the singular vector $\overline{\mathbf{U}}$.

$$\left[\begin{array}{cc}\overline{\mathbf{A}}& \overline{\mathbf{B}}\end{array}\right]\approx \left[\begin{array}{cc}\dot{\mathbf{\chi }}\overline{\mathbf{V}}{\overline{\Sigma }}^{-1}{\overline{\mathbf{U}}}_1^T& \dot{\mathbf{\chi }}\overline{\mathbf{V}}{\overline{\Sigma }}^{-1}{\overline{\mathbf{U}}}_2^T\end{array}\right]$$

(35)

The values of the matrix $\mathbf{A}$ can be approximated using the split matrix ${\overline{\mathbf{U}}}_1\in {\mathbb{R}}^{4\times \overline{r}}$ and the values of $\mathbf{B}$ are approximated using the values of ${\overline{\mathbf{U}}}_2\in {\mathbb{R}}^{4\times \overline{r}}$.

Dynamic Mode Decomposition Identification and Validation

This section presents the identification and validation process of the dynamic model using the DMD formulation. The values of matrices $\dot{\mathbf{\chi }}$, $\mathbf{\chi }$ and $\Gamma$ were constructed by measure snapshots information of the aerial platform DJI MATRICE 600 PRO and the application of Algorithm 1, which represents all the necessary steps to identify the values of unknown matrices.

Algorithm 1 Identification process Using DMD

Input: DJI MATRICE 600 PRO measurements $(\dot{\mathbf{\chi }},\mathbf{\chi },\Gamma )$ and truncation value $\overline{r}$. Output: Matrices $\overline{\mathbf{A}}$ and $\overline{\mathbf{B}}$.     Ident$(\dot{\mathbf{\chi }},\mathbf{\chi },\Gamma ,\overline{r})$do      $\mathsf{\Omega }\leftarrow {\left[\begin{array}{cc}\mathbf{\chi }& \mathbf{{\rm Y}}\end{array}\right]}^T$      $(\mathbf{U},\Sigma ,\mathbf{V})\leftarrow SVD\left(\mathsf{\Omega }\right)$      $(\overline{\mathbf{U}},\overline{\Sigma },\overline{\mathbf{V}})\leftarrow Trunc(\mathbf{U},\Sigma ,\mathbf{V},\overline{r})$      $({\overline{\mathbf{U}}}_1,{\overline{\mathbf{U}}}_2)\leftarrow Split\left(\overline{\mathbf{U}}\right)$      $\overline{\mathbf{A}}\leftarrow \dot{\mathbf{\chi }}\overline{\mathbf{V}}{\overline{\Sigma }}^{-1}{\overline{\mathbf{U}}}_1^T$      $\overline{\mathbf{B}}\leftarrow \dot{\mathbf{\chi }}\overline{\mathbf{V}}{\overline{\Sigma }}^{-1}{\overline{\mathbf{U}}}_2^T$     end Ident     return$(\overline{\mathbf{A}},\overline{\mathbf{B}})$

The approximation values of the unknown matrix are presented in

Table 2. Approximation values of unforced matrix.

Matrix	Approximated Values
$\overline{\mathbf{A}}$	$a_{11}=-0.5827$	$a_{12}=-0.0721$	$a_{13}=0.0587$	$a_{14}=-0.0280$
	$a_{21}=0.0214$	$a_{22}=-0.4770$	$a_{23}=0.0731$	$a_{24}=-0.1840$
	$a_{31}=0.0152$	$a_{32}=0.0102$	$a_{33}=-3.5696$	$a_{34}=0.0064$
	$a_{41}=0.0377$	$a_{42}=-0.0113$	$a_{43}=-0.0058$	$a_{44}=-8.2424$

and

Table 3. Approximate values of control actuation matrix.

Matrix	Approximated Values
$\overline{\mathbf{B}}$	$b_{11}=0.8048$	$b_{12}=-0.0376$	$b_{13}=-0.0793$	$b_{14}=-0.1797$
	$b_{21}=0.0634$	$b_{22}=0.8527$	$b_{23}=-0.0783$	$b_{24}=0.3821$
	$b_{31}=-0.0202$	$b_{32}=-0.0046$	$b_{33}=3.5776$	$b_{34}=0.0607$
	$b_{41}=-0.0251$	$b_{42}=-0.0029$	$b_{43}=-0.0288$	$b_{44}=8.1294$

, where, $a_{ij}$ and $b_{ij}$ are the individual elements of $\overline{\mathbf{A}}$ and $\overline{\mathbf{B}}$, respectively.

The signals used for the identification process are presented in Figure 4, which are the same as those used for the identification process employing Euler–Lagrange formulation, due to the fact that both techniques need to estimate the values under the same conditions.

The estimated model displays good behavior due to the fact it replicates the signals obtained from experimental data. The noise in the angular maneuverability velocity $\omega$ is an important factor, however, dynamic mode decomposition and the robustness of the Frobenius norm can deal with this noise and the application of filters is not necessary in the identification process.

In the same way presented in the validation process using the Lagrange formulation, this section presents the validation process under DMD formulation, the results of which are shown in Figure 5, which shows the good performance of the proposed technique.

Finally, considering the approximation values of the unknown matrices, the dynamic model can be written in a vector-function form as:

$$\dot{\mathbf{\mu }}\left(t\right)={\mathbf{f}}_{DMD}(\Gamma ,\mathbf{\mu }\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right))$$

(36)

2.1.3. Accuracy Comparative Results

This section presents the comparative results between the Euler–Lagrange (29) and dynamic mode decomposition (36) formulations. This work uses the integral square error (ISE) to show the performance of each formulation and selects the best one for the nonlinear model predictive controller structure.

Figure 6 shows the ISE of each formulation, where the model for frontal maneuverability velocity ${\mu }_{lm}$ presents a considerable error using the Euler–Lagrange formulation—with a value of precisely $1.26$—whilst on the other hand, DMD results show a better performance with a value of $0.56$. The proposed formulations show similar results with respect to the lateral velocity ${\mu }_{mm}$, and the values using the Euler–Lagrange and DMD are $0.35$ and $0.16$, respectively. The results for the estimated upper maneuverability velocity ${\mu }_{nm}$ are really close with the following values $0.0104$ and $0.0100$. Finally, the ISE of the estimated models for the angular maneuverability velocity are $0.104$ and $0.038$ for the Euler–Lagrange and DMD formulations, respectively.

With the previously presented results, this work uses the DMD estimated model to construct the proposed controller. Combining Equations (5) and (36), the general mathematical representation is defined as:

$$\dot{\mathbf{x}}\left(t\right)=\mathbf{f}(\mathbf{x}\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right))$$

(37)

$$\mathbf{f}(\mathbf{x}\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right)):=\left[\begin{array}{c}{\mathbf{f}}_k(\psi \left(t\right),\mathbf{\mu }\left(t\right))\\ {\mathbf{f}}_{DMD}(\Gamma ,\mathbf{\mu }\left(t\right),{\mathbf{\mu }}_{\mathbf{ref}}\left(t\right))\end{array}\right]$$

where $\mathbf{x}={\left[\begin{array}{cc}{\mathbf{\eta }}^T& {\mathbf{\mu }}^T\end{array}\right]}^T\in {\mathbb{R}}^8$ is the generalized vector of the system states that is confirmed by $\mathbf{\eta }$, which is the pose of the point of interest in the hexacopter and $\mathbf{\mu }$ is the maneuverability velocities.

2.2. Control Methodology

This section presents the formulation of the proposed controller, where the main objective is the trajectory tracking over the desired reference trajectory using an hexacopter platform and due to the dynamic effects and the important size of the system, this work includes obstacle avoidance to improve the behavior of the system in dynamic environments. This work presents the nonlinear model predictive controller (NMPC); furthermore, the constraints included in the optimization problem were: generalized system dynamics, bounded maneuverability velocities, rate of change of control velocities and obstacles in the dynamic environment. The main objectives of the proposed controller are: to solve the control problem associated with the trajectory tracking subject to the system and the environment and system constraints. The control problem is solved by the scheme shown in Figure 7, where the structure uses the system information and possible obstacles in the environment to generate an optimal control policy under the system constraints.

2.2.1. Nonlinear Model Predictive Control

In order to solve the NMPC proposed scheme, this work formulates the generalized dynamics (37) as a prediction, resulting in:

$$\dot{\mathbf{x}}\left(l\right|t)=\mathbf{f}(\mathbf{x}\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t))$$

(38)

where $l\in [t,t+T]$ is the instant value evolution between the initial time t and the prediction horizon T. Due to the prediction behavior of this scheme, the NMPC formulation requires index performances over the prediction horizon T that guarantees the solution of the control problem and the system constraints. The generation of the performance functions are presented below.

2.2.2. Tracking Trajectory Formulation

This subsection presents the generation of the cost function over the evolution instants l considering the following property: ${\ell }_t(\mathbf{x}\left(l\right|t),{\mathbf{\eta }}_{\mathbf{ref}}\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t)):{\mathbb{R}}^4\times {\mathbb{R}}^4\to {\mathbb{R}}_{\ge 0}$ to formulate the function as:

$${\ell }_t(\mathbf{x}\left(l\right|t),{\mathbf{\eta }}_{\mathbf{ref}}\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t))=\stackrel{Trajectorystagecost}{\overbrace{\frac{1}{2}\left|\right|{\mathbf{\eta }}_{\mathbf{ref}}\left(l\right|t)-{\mathbf{W}}_{\mathbf{t}}\mathbf{x}\left(l\right|t){\left|\right|}_{{\mathbf{Q}}_t}^2}}+\underset{Controlstagecost}{\underbrace{\frac{1}{2}\left|\right|{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t){\left|\right|}_{{\mathbf{Q}}_u}^2}}$$

(39)

Trajectory Stage Cost: this part of the function is structured by the control error between the desired trajectory ${\mathbf{\eta }}_{\mathbf{ref}}={\left[\begin{array}{cccc}{\eta }_{x_{ref}}& {\eta }_{y_{ref}}& {\eta }_{z_{ref}}& {\eta }_{{\psi }_{ref}}\end{array}\right]}^T\in {\mathbb{R}}^4$ and the measurements of the system $\mathbf{x}$; the constant matrix ${\mathbf{W}}_{\mathbf{t}}\in {\mathbb{R}}^{4\times 8}$ produces linear mapping between all the states of the system and the outputs ${\eta }_x$, ${\eta }_y$, ${\eta }_z$ and ${\eta }_{\psi }$; the Euclidean norm is defined by the operator $\left|\right|·\left|\right|$; finally, ${\mathbf{Q}}_t\in {\mathbb{R}}_{>0}^{4\times 4}$ is a constant positive definite matrix.

Control stage cost: this part of the cost function defines the variations between the maneuverability control velocities ${\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t))$ considering that ${\mathbf{Q}}_u\in {\mathbb{R}}_{>0}^{4\times 4}$ is a positive definite weighting matrix.

The cost function at the last step of the prediction horizon T was formulated with following property: ${\ell }_T(\mathbf{x}(t+T|t),{\mathbf{\eta }}_{\mathbf{ref}}(t+T|t)):{\mathbb{R}}^4\to {\mathbb{R}}_{\ge 0}$; with this consideration, the cost function is defined as:

$${\ell }_T(\mathbf{x}(t+T|t),{\mathbf{\eta }}_{\mathbf{ref}}(t+T|t))=\stackrel{TerminalTrajectorycost}{\overbrace{\frac{1}{2}\left|\right|{\mathbf{\eta }}_{\mathbf{ref}}(t+T|t)-\mathbf{W}\mathbf{x}(t+T|t){\left|\right|}_{{\mathbf{Q}}_T}^2}}$$

(40)

Terminal trajectory cost: this term considers the control error between the reference trajectory at the last step of the prediction horizon T, where ${\mathbf{Q}}_T\in {\mathbb{R}}_{>0}^{4\times 4}$ is a positive definite weighting matrix.

This work uses Equations (39) and (40) to generate the performance index over the prediction horizon, which was developed in the continuous time formulation, where $\tau$ is the interval analyzed in the integral operation, and the performance index is defined as:

$$V(\mathbf{x}\left(l\right|t),{\mathbf{\eta }}_{\mathbf{ref}}\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t))={\int }_t^{t+T}{\ell }_t(\mathbf{x}\left(\tau \right|t),{\mathbf{\eta }}_{\mathbf{ref}}\left(\tau \right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(\tau \right|t))d\tau +{\ell }_T(\mathbf{x}(t+T|t),{\mathbf{\eta }}_{\mathbf{ref}}(t+T|t))$$

(41)

The performance index function (41) guarantees the trajectory tracking propriety of the controller. The tuning process of the positive definite matrices were developed in the simulations in order to prevent accidents in real-world experiments.

2.2.3. Maneuverability Velocities Constraints

Due to the hexacopter platform having maximum maneuverability velocities, this work proposes control input constraints on the NMPC structure, in addition to further constraints on the successive difference of the maneuverability velocities which were included to prevent aggressive behaviors. The input constraints are defined in (42) in order to maintain the control signals generated by the proposed structure under the limits of the hexacopter platform

$$\mathcal{U}=\{{\mathbf{\mu }}_{\mathbf{ref}}\in {\mathcal{R}}^4:{\mathbf{\mu }}_{{\mathbf{ref}}_{min}}\le {\mathbf{\mu }}_{\mathbf{ref}}\le {\mathbf{\mu }}_{{\mathbf{ref}}_{max}};|{\mathbf{\mu }}_{\mathbf{ref}}(j+1|t)-{\mathbf{\mu }}_{\mathbf{ref}}\left(j\right|t)|\le \Delta {\mathbf{\mu }}_{{\mathbf{ref}}_{max}}\}$$

(42)

where ${\mathbf{\mu }}_{{\mathbf{ref}}_{min}}\in {\mathbb{R}}^4$ and ${\mathbf{\mu }}_{{\mathbf{ref}}_{max}}\in {\mathbb{R}}^4$ are the vectors of lower and upper limits in maneuverability velocities; to the successive difference, this works considers $j\in [t,t+(T-1\left)\right]$ and $\Delta {\mathbf{\mu }}_{{\mathbf{ref}}_{max}}$ as the vector of the maximum rate of change between the maneuverability velocities.

2.2.4. Obstacle Constraints

This section presents the obstacle constraints in order to track the desired trajectory and avoid obstacles in the dynamic environments. The obstacles can be defined as ${\mathbf{\xi }}^{obs}\left(i\right|z)={\left[\begin{array}{ccc}{\eta }_x^{obs}& {\eta }_y^{obs}& {\eta }_z^{obs}\end{array}\right]}^T\in {\mathbb{R}}^3$ like a position vector with respect to the inertia frame $<\mathbb{I}>$, where $i\in [z,z+Z]$ considering the analyzed obstacle as z and Z the number of obstacles detected by the system, the representation of the obstacles is shown in Figure 8.

The function of the distance between obstacles and the hexacopter has the following property: ${\ell }_{obs}(\mathbf{x}\left(l\right|t),{\mathbf{\xi }}^{obs}\left(i\right|z)):{\mathbb{R}}^4\to {\mathbb{R}}_{\ge 0}$, and the equation is formulated as:

$${\ell }_{obs}(\mathbf{x}\left(l\right|t),{\mathbf{\xi }}^{obs}\left(i\right|z))=\stackrel{DistancetoObstacles}{\overbrace{\left|\right|{\mathbf{\xi }}^{obs}\left(i\right|z)-{\mathbf{W}}_{\mathbf{o}}\mathbf{x}\left(l\right|t){\left|\right|}^2}}$$

(43)

where ${\mathbf{W}}_{\mathbf{o}}\in {\mathbb{R}}^{3x8}$ is a constant matrix that produces linear mapping between all the states of the system and the outputs ${\eta }_x$, ${\eta }_y$ and ${\eta }_z$. With the previously presented considerations, this work formulates the system states constraints in (44), and where the safe region was defined as $r_{obs}\in {\mathbb{R}}_{>0}$, this value includes the safely region and the rotating blades’ positions.

$$\mathcal{O}=\{\mathbf{x}\in {\mathcal{R}}^4:r_{obs}-{\ell }_{obs}(\mathbf{x}\left(l\right|t),{\mathbf{\xi }}^{obs}\left(i\right|z))\le 0\}$$

(44)

2.2.5. Nonlinear Model Predictive Control Formulation

Considering the performance index presented before (41) and the system states (44) and control (42) constraints, this work can formulate the optimization problem as:

$$\begin{array}{c}\hfill \mathcal{P}:P\left({\mathbf{\eta }}_{\mathbf{ref}}\left(t\right)\right)=\underset{{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t),\mathbf{x}\left(l\right|t)}{\min }{\int }_t^{t+T}{\ell }_t(\mathbf{x}\left(\tau \right|t),{\mathbf{\eta }}_{\mathbf{ref}}\left(\tau \right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(\tau \right|t))d\tau +{\ell }_T(\mathbf{x}(t+T|t),{\mathbf{\eta }}_{\mathbf{ref}}(t+T|t))\end{array}$$

(45a)

$$\begin{array}{c}\hfill \mathrm{subject}\mathrm{to}:\dot{\mathbf{x}}\left(l\right|t)-\mathbf{f}(\mathbf{x}\left(l\right|t),{\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t))=0\end{array}$$

(45b)

with the following constrains:

$$\begin{array}{c}\hfill \mathbf{x}\left(l\right|t)\in r_{obs}-{\ell }_{obs}(\mathbf{x}\left(l\right|t),{\mathbf{\xi }}^{obs}\left(i\right|z))\le 0\end{array}$$

(45c)

$$\begin{array}{c}\hfill {\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t)\in {\mathbf{\mu }}_{{\mathbf{ref}}_{min}}\le {\mathbf{\mu }}_{\mathbf{ref}}\le {\mathbf{\mu }}_{{\mathbf{ref}}_{max}};|{\mathbf{\mu }}_{\mathbf{ref}}(j+1|t)-{\mathbf{\mu }}_{\mathbf{ref}}\left(j\right|t)|\le \Delta {\mathbf{\mu }}_{{\mathbf{ref}}_{max}}\end{array}$$

(45d)

$\mathcal{P}$ is the function that guarantees the correct trajectory tracking and obstacle avoidance of the hexacopter system. The optimal control problem (45a) was converted into a nonlinear programming formulation (NLP) using the direct multiple shooting method, where the control maneuverability velocities ${\mathbf{\mu }}_{\mathbf{ref}}\left(l\right|t)$ and the system states $\mathbf{x}\left(l\right|t)$ are the decision variables of the optimization problem. A multiple shooting technique is more computationally efficient when it is compared with other discretization formulation, e.g., single shooting, more information in [58]. The system model was considered as the optimization constrain defined by (45b), the system and control constraints formulated in (45c) and (45d), respectively. With these considerations, this work minimizes the nonlinear programming problem using CasADI as a nonlinear optimization framework [59].

3. Results and Discussions

This section presents the simulations and experimental results to validate the proposed controller for the hexacopter platform DJI MATRICE 600 PRO. The experiments were performed considering: (i) Real-world experiment: the results presented in this section were developed using the nonlinear model predictive controller and environment obstacles are not considered in this experiments due to the hexacopter platform not having the required sensor for real-world measurements. (ii) Simulation experiment: simulation experiments show the effectiveness of the proposed controller in highly dynamic simulation environments with simulations of the dynamic behavior of the hexacopter in order to make it more realistic and prove the scalability of the controller.

3.1. Real-World Experiments

Several experiments on trajectory tracing were performed in order to demonstrate the performance of the proposed controller. In order to demonstrate the effectiveness of the optimization structure, the controller was implemented at onboard computer of the hexacopter through the software Matlab, additionally the hexacopter platform is shown in Figure 9 with the following hardware features Intel processor i7-7700HQ and CPU 2.80 GHz × 8. The controller was developed considering the optimal control problem (45a), the evolution of the identified model was developed using the fourth-order Runge–Kutta method, with a sample time of $t_s=0.1\left[\mathbf{s}\right]$ and the final time of the experiment is defined as $t_f=100\left[\mathbf{s}\right]$.

The experiment was performed in the city of Ambato, in the province of Tungurahua, Ecuador, and started at 09:36 on 12 January 2022. The wind velocity at the time of the experiment was approximately 10.1 km/h, as shown in Figure 10.

The desired reference trajectory for the hexacopter and the initial conditions are defined in

Table 4. Desired reference trajectory in a real-world experiment.

Initial Positions	Initial Maneuverability Velocities	Reference Trajectory
${\eta }_{x_o}=4.18\left[\mathrm{m}\right]$	${\mu }_{l_o}=0.01[\mathrm{m}/\mathrm{s}]$	${\eta }_{x_{ref}}=8\sin \left(0.1t\right)\left[\mathrm{m}\right]$
${\eta }_{y_o}=1.88\left[\mathrm{m}\right]$	${\mu }_{m_o}=-0.04[\mathrm{m}/\mathrm{s}]$	${\eta }_{y_{ref}}=6\sin \left(0.2t\right)+1\left[\mathrm{m}\right]$
${\eta }_{z_o}=3.97\left[\mathrm{m}\right]$	${\mu }_{n_o}=-0.02[\mathrm{m}/\mathrm{s}]$	${\eta }_{z_{ref}}=0.35\sin \left(0.5t\right)+7\left[\mathrm{m}\right]$
${\eta }_{{\psi }_o}=1.05\left[\mathrm{rad}\right]$	${\omega }_o=-0.02[\mathrm{rad}/\mathrm{s}]$	${\eta }_{{\psi }_{ref}}=0\left[\mathrm{rad}\right]$

, where $({\eta }_{x_o},{\eta }_{y_o},{\eta }_{z_o},{\eta }_{{\psi }_o})$ and $({\mu }_{l_o},{\mu }_{m_o},{\mu }_{n_o},{\omega }_o)$ are the initial position and maneuverability velocities of the hexacopter, respectively.

To implement the proposed controller, the constant values of positive definite matrices are defined in

Table 5. Proposed values for the NMPC scheme in a real-world experiment.

Parameters	Values	Parameters	Values
${\mathbf{Q}}_T$	$diag\left(1\right)\in {\mathbb{R}}^{4\times 4}$	${\mathbf{Q}}_t$	$diag\left(1\right)\in {\mathbb{R}}^{4\times 4}$
${\mathbf{Q}}_u$	$diag\left(0.005\right)\in {\mathbb{R}}^{4\times 4}$	T	$2\left[\mathrm{s}\right]$
${\mathbf{\mu }}_{{\mathbf{ref}}_{min}}$	$-[1,1,1,1.5][\mathrm{m}/\mathrm{s},\mathrm{rad}/\mathrm{s}]$	${\mathbf{\mu }}_{{\mathbf{ref}}_{max}}$	$[1,1,1,1.5][\mathrm{m}/\mathrm{s},\mathrm{rad}/\mathrm{s}]$
$\Delta {\mathbf{\mu }}_{{\mathbf{ref}}_{max}}$	$0.05[\mathrm{m}/\mathrm{s},\mathrm{rad}/\mathrm{s}]$

, and were obtained through a variety of experiments in order to improve the performance of the controller scheme.

The results of the experiment are illustrated in Figure 11, Figure 12, Figure 13 and Figure 14. Figure 11 shows the movement of the hexacopter platform based on real information over the experiment. The hexacopter tracks the desired reference trajectory; however, the results present a small control error in the trajectory due to the wind velocity that acts as an external disturbance. The reference trajectory is defined as ${\mathbf{\eta }}_{\mathbf{ref}}=\left[\begin{array}{cccc}{\eta }_{x_{ref}}& {\eta }_{y_{ref}}& {\eta }_{z_{ref}}& {\eta }_{{\psi }_{ref}}\end{array}\right]$ and the hexacopter system states during the experiment were confirmed by $\mathbf{\eta }$.

The previously presented results show the performance of the controller that guarantees the execution of the reference trajectory over different axes.

The control signals represented in Figure 12 are generated by the proposed control scheme, the external disturbances produce a miss match in the model but the controller generates the adequate smooth control actions.

Figure 13 shows the control errors of the proposed controller, where ${\tilde{\eta }}_x,{\tilde{\eta }}_y,{\tilde{\eta }}_z$ and ${\tilde{\eta }}_{\psi }$ are the results from the steady state error that asymptotically converge to values close to zero, i.e., since these position errors are bounded and different from zero ${\mathbf{\eta }}_{\mathbf{ref}}\left(t\right)-{\mathbf{W}}_{\mathbf{t}}\mathbf{x}\left(t\right)=0\in R^3$, it is achieved that the errors in the steady state are $|\tilde{\eta }|<0.15\left[\mathrm{m}\right]$. In addition, the control error maintains a dependency on the external disturbances in the outdoor environments, and external disturbances are the product of the wind velocity which was approximately 10 km/h.

The time required to solve the optimal control problem is shown in Figure 14, and the computation time stays always below the sample time $t_s=0.1\left[\mathrm{s}\right]$, guaranteeing the efficient computation of the proposed control considering the time horizon of $T=2\left[\mathrm{s}\right]$ in the optimal control problem.

3.2. Simulation Experiments

This section presents the simulation result of the proposed controller. In order to improve the results, this experiment was performed using the identified dynamics in Equation (36) with the identified parameters presented in Table 2 and Table 3. Furthermore, a Gaussian noise with the following consideration in all the states ${\mathbf{\eta }}_n\sim \mathcal{N}(-0.05,0.05)$ and ${\mathbf{\mu }}_n\sim \mathcal{N}(-0.01,0.01)$, where ${\mathbf{\eta }}_n$ and ${\mathbf{\mu }}_n$ are the additive noise in position and velocity states, was identified.

The desired reference trajectories and initial values over the frame $<\mathbb{I}>$ are defined in

Table 6. Desired reference trajectory in the simulation experiment.

Initial Positions	Initial Maneuverability Velocities	Reference Trajectory
${\eta }_{x_o}=4.18\left[\mathrm{m}\right]$	${\mu }_{l_o}=0[\mathrm{m}/\mathrm{s}]$	${\eta }_{x_{ref}}=8\sin \left(0.1t\right)\left[\mathrm{m}\right]$
${\eta }_{y_o}=1.88\left[\mathrm{m}\right]$	${\mu }_{m_o}=0[\mathrm{m}/\mathrm{s}]$	${\eta }_{y_{ref}}=6\sin \left(0.2t\right)+1\left[\mathrm{m}\right]$
${\eta }_{z_o}=3.97\left[\mathrm{m}\right]$	${\mu }_{n_o}=0[\mathrm{m}/\mathrm{s}]$	${\eta }_{z_{ref}}=0.35\sin \left(0.5t\right)+7\left[\mathrm{m}\right]$
${\eta }_{{\psi }_o}=1.05\left[\mathrm{rad}\right]$	${\omega }_o=0[\mathrm{rad}/\mathrm{s}]$	${\eta }_{{\psi }_{ref}}=0\left[\mathrm{rad}\right]$

.

The desired values of the reference trajectory are the same as the proposed in real-world experiments; therefore, the results present similar behavior. The controller constant values are the same as those presented in Table 5. However, the safe region distance was defined as $r_{obs}=0.4\left[\mathrm{m}\right]$.

The trajectory described by the hexacopter platform and the reference trajectory are presented in Figure 15. The results show that the hexacopter tracks the desired reference trajectory, in addition to the avoidance obstacles property being demonstrated due to the system following the reference and trying to maintain the distance to obstacles considering the safety radius $r_{obs}$. The three obstacles were positioned over the reference trajectory, two of which were positioned at the corners are dynamic obstacles and they have movements during the experiment.

The values of the control errors presented in Figure 16 tend to increase when the hexacopter avoids the obstacles satisfying the system constraints, a phenomenon which is represented in the rectangles representing obstacles close to the hexarotor. Given that the errors in steady state ${\tilde{\eta }}_x,{\tilde{\eta }}_y,{\tilde{\eta }}_z$ and ${\tilde{\eta }}_{\psi }$ symptomatically converge towards zero in the presence of the Gaussian noise applied in the simulation, the results show the robustness under this type of disturbance and the ability to keep the system under the reference trajectory, i.e., the position errors are bounded and are different from zero ${\mathbf{\eta }}_{\mathbf{ref}}\left(t\right)-{\mathbf{W}}_{\mathbf{t}}\mathbf{x}\left(t\right)=0$, achieving a final characteristic error with a max $|\tilde{\eta }|<0.9\left[\mathrm{m}\right]$ when the obstacles are close to the hexarotor and $|\tilde{\eta }|<0.1\left[\mathrm{m}\right]$ without obstacles near the system reference trajectory.

The control signals are presented in Figure 17, which are generated by the proposed control scheme. Taking into consideration the dynamics of the robotic system and the simulated environmental conditions, the behavior of the reference velocities are close to the dynamic model values. Furthermore, the values are presented as smooth curves with the presence of peaks when the obstacles are close to the hexacopter which are emphasized by the rectangles shown in Figure 17. The behavior of the control actions prevents collisions between the hexacopter and the obstacles during the simulation.

The time required to solve the proposed controller and the Euclidean distance are presented in Figure 18, and the computational time is an important factor to demonstrate its ability to generate an adequate control policy that guarantees the system constraints. The computational time presents peaks produced by the obstacles that are close to the hexacopter during the trajectory whilst the controller finds a sub-optimal solution to guarantee that the computational time remains under the sample time; however, this problem can be efficiently solved using another low-level language to implement the proposed controller or with a more powerful computer, which is therefore a solvable technological issue. On the other hand, the distance to each obstacle shows that the proposed controller respects the safely radius, guaranteeing the avoidance property of the system.

3.3. Discussion

The experiments presented in the last section show that the nonlinear model predictive scheme solves the control problem associated with the trajectory tracking subject to the system and environment constraints. Specifically, the real-world experiments showed the robustness of the proposed controller due to the excessive wind velocity during the experiment which acted as an external disturbance. Despite the excessive wind velocity, the controller generates the adequate smooth control actions guaranteeing that the steady state error that converges towards values close to zero with the dependency of the external disturbances. The use of CasADI such as an optimization framework guarantees the fact that the computational time always stays below the sample time, which is one of main problems in NMPC schemes due to the highly computational time that these solutions required, as similar computing times were presented in [45].

On the other hand, the simulation results show the ability of the proposed scheme to generates the adequate control signals guaranteeing the corrected tracking trajectories while the system avoids obstacles during the simulation. The presence of Gaussian noise provides uncertainty in the measurements demonstrating the power of the controller as it maintains the distance to obstacles in consideration of the safety radius. One of the main differences presented in this experiment was the control error, which tends to increase when the hexacopter avoids obstacles satisfying the system constraints. Furthermore, the computational time presents peaks produced by the hard constraints included in the optimization problem, and despite the hard constraint, the controller finds a sub-optimal solution to guarantee a low computational time. Other works have used the concept of soft constraints, low-level programming language and embeddable optimization methods such as (SQP) in order to increase the convergence of the optimization algorithm and reduce the computational time [60,61,62,63,64]. This work uses the concept of soft constraints and the approximation of the nonlinear problem in order to improve the computational time and the future implementation on a single on-board PC.

4. Conclusions

This work carried out the identification of a dynamic system using the dynamic mode decomposition with control technique to develop the optimal control problem for trajectory tracking with obstacle avoidance, a decision which was made due to the precision of the comparative results between the Euler–Lagrange and the dynamic mode decomposition formulations. The optimal control problem was translated into a nonlinear programming problem (NLP) using the multiple shooting technique and considering the control actions and systems states as decision variables, which is a technique that improves the computational time and the convergence of the algorithm. The nonlinear model predictive control (NMPC) generates the maneuverability velocities policy that allows the hexacopter platform to track the reference trajectory while avoiding obstacles presented in the environment. The NMPC formulation was developed in consideration of the reference desired trajectory and constraints in the system such as the bounded control actions, the generalized dynamics and static and dynamic obstacles. Furthermore, the NMPC problem was solved through the nonlinear programming framework CasADI, which solves high-dimensional optimization problems and has advantages that can be expanded to a low-level programming language to improve the computational time.

The simulation results show how the hexacopter avoids both static and dynamic obstacles whose locations vary over time while executing the trajectory tracking task, and always maintaining a safe distance defined as a radius of repulsion. Since there is no sensor for obstacle measurements in the hexacopter platform, the real experimental tests were not carried out with obstacle avoidance; however it was verified that although the experimental tests were carried out during the period of time in which the average wind speed was approximately 10 km/h, the behavior of the control law on the hexacopter system fulfills the desired task, so that the control errors in stable state converge closer to zero. In addition, it can be observed that the steady state error asymptotically converges towards values closer to zero under conditions wherein the control actions are saturate when they reach the established restrictions. On the other hand, the computational time of the proposed control algorithm is maintained under 100 ms in real experimentation due to the optimization technique used to solve the control problem. During the simulation tests, it is clearly verified that the computational time remains under 100 ms; however, when obstacle avoidance occurs, this time increases but remains under the specified sample time with a sup-optimal solution. Other works use the concept of soft constraints to guarantee the rapid convergence of the optimization algorithm, as this work will use the same concept to improve the computational time and the possible expansion to visual-servoing systems under an MPC structure.