A NONLINEAR OPTIMAL CONTROL APPROACH FOR THE UAV AND SUSPENDED PAYLOAD SYSTEM

The article proposes a nonlinear optimal control approach for the UAV and suspended load system. The dynamic model of the UAV and payload system undergoes approximate linearization with the use of Taylor series expansion around a temporary operating point which recomputed at each iteration of the control method. For the approximately linearized model an H-inﬁnity feedback controller is designed. The linearization procedure relies on the computation of the Jacobian matrices of the state-space model of the system. The proposed control method stands for the solution of the optimal control problem for the nonlinear and multivariable dynamics of the UAV and payload system, under model uncertainties and external perturbations. For the computation of the controller’s feedback gains an algebraic Riccati equation is solved at each time-step of the control method. The new nonlinear optimal control approach achieves fast and accurate tracking for all state variables of the UAV and payload system, under moderate variations of the control inputs. The stability properties of the control scheme are proven through Lyapunov analysis.


Introduction
The control problem of the UAV and suspended payload system is an important one if one considers that UAVs are not only used in free-of-load applications, such as surveillance and patrolling, but are frequently involved in transportation tasks [Liang et al., 2018], , [Cime and Fiero, 2017], [Rigatos et al., 2018], [Tartaglione et al., 2017]. Actually UAVs have often to accomplish lift and transfer for payloads of different weights, as part of commercial products or ammunition delivery tasks [Orsag et al., 2017], [Pizetta et al., 2015], [Yang et Xian, 2017], [Lanni, 2017] . Control of the UAV and suspended payload system is a non-trivial problem, taking into account the strong nonlinearities and multi-variable structure of the system's state-space model [Palunko et al., 2012], [Goodarzi and Lee, 2018], [Bernard and Kondek, 2009], [Bisgaard et al., 2010]. Besides, the problem becomes even more complicated due to underactuation , [Nicotra et al., 2014], [Min et al., 2011], . It is remarkable that for lift and transfer tasks the UAV and suspended payload system has four degrees of freedom, while there are only two control inputs applied to it [Pizetta et al., 2016], [Goodarzi and Lee, 2018], [Raffo and Almeida, 2016], [Gonzales et al., 2015]. Actually, one has to achieve dexterous manipulation of the UAV and stabilization of its suspended payload by using as control inputs the lift force which is provided by the UAV's motors and the torque which is generated in aggregate by them. In this article a novel nonlinear optimal control method is developed for the UAV and suspended payload system [Rigatos, 2016], [Rigatos et al., 2015a]. [Rigatos et al., 2015b]. The article elaborates on the results of [Rigatos et al., 2018]. The dynamic model of the aerial manipulator undergoes approximate linearization around a temporary operating point (equilibrium) which is recomputed at each iteration of the control method. The linearization point consists of the present value of the aerial robotic system's state vector and of the last sampled value of the control inputs vector. The linearization procedure relies on first-order Taylor series expansion and on the computation of the Jacobian matrices of the system [Rigatos and Tzafestas, 2007], [Basseville and Nikiforov, 1993], [Rigatosa and Zhang, 2009]. The modelling error which is due to the truncation of higher-order terms in the Taylor series expansion is considered to be a perturbation which is asymptotically compensated by the robustness of the control algorithm. For the approximately linearized model of the aerial robotic system an H-infinity feedback controller is designed. Actually, the proposed H-infinity controller provides the solution of the optimal control problem under model uncertainty and external perturbations. The H-infinity control represents the solution of a max-min differential game in which the control inputs try to minimize a quadratic cost function depending on the square of the state-vector's tracking error whereas the model uncertainty and perturbation inputs try to maximize this cost function. For the computation of the feedback gains of the H-infinity controller, an algebraic Riccati equation is repetitively solved at each iteration of the control algorithm [Rigatos and Busawon, 2018], [Rigatos, 2011], . The stability properties of the control scheme are proven through Lyapunov analysis. First, it is demonstrated that the control loop satisfies the Hinfinity tracking performance criterion, which signifies elevated robustness under model uncertainty and exogenous disturbances [Toussaint et al., 2020], [Lublin and Athans, 1995]. Next, it is proven that conditionally the control loop is globally asymptotically stable. Finally, to implement state estimation-based control, through the processing of measurements coming from a small number of sensors, the H-infinity Kalman Filter is introduced as a robust state estimator [Gibbs, 2011]. The present article provides one of the few existing solutions to the nonlinear optimal control problem of the UAV and suspended payload system which is of proven global stability while also remaining computationally efficient [Rigatos and Busawon, 2018]. Preceding results on the use of H-infinity control to nonlinear dynamical systems were limited to the case of affine-in-the-input systems with drift-only dynamics and considered that the control inputs gain matrix is not dependent on the values of the system's state vector. Moreover, in these approaches the linearization was performed around points of the desirable trajectory whereas in the present article's control method the linearization points are related with the value of the state vector at each sampling instance as well as with the last sampled value of the control inputs vector. The Riccati equation which has been proposed for computing the feedback gains of the controller is novel, so is the presented global stability proof through Lyapunov analysis. The above comments have been included in a new paragraph which appears in the Simulation tests section of the revised article. The article's scientific contribution is outlined as follows: (i) the presented nonlinear optimal control method has improved performance when compared against other nonlinear control schemes that one can consider for the dynamic model of the UAV and suspended payload system (such as Lie algebra-based control, differential flat-ness theory-based control, Model-based Predictive Control, Nonlinear Model-based Predictive Control, Slidingmode control, Backstepping control), etc., (ii) it achieves fast and accurate tracking of all reference setpoints for the UAV and suspended payload system under moderate variations of the control inputs, (iii) it minimizes the consumption of energy by the actuators of the UAV and suspended payload system, thus improving the functioning cost and efficiency in tasks' execution by such robotic systems. The structure of the article is as follows: In Section 2 the dynamic model of the UAV and of the suspended payload system is analyzed and its state-space description is formulated. In Section 3 approximate linearization is performed for the state-space model of the aerial robotic system, through Taylor series expansion and the computation of the related Jacobian matrices. In Section 4 an H-infinity feedback controller is designed for the approximately linearized model of the system. In Section 5 the stability properties of the control scheme are proven through Lyapunov analysis. In Section 6 the H-infinity Kalman Filter is used as a robust state estimator capable of solving the state estimation-based control problem for the aerial robotic system. In Section 7, the excellent tracking performance of the control method is further confirmed through simulation experiments. Finally, in Section 8 concluding remarks are stated.

Dynamic Model of the UAV and Suspended Payload System
The UAV and suspended payload robotic system, and the associated reference frames, are shown in Fig. 1. The main variables of the dynamic model of this aerial robotic system are defined as follows: φ: is the roll angle of the UAV with respect to the horizontal axis of the inertial reference frame system, θ: is the rotation angle of the payload wit respect to the vertical axis of the inertial reference frame, l: is the length of the string connecting the payload with the center of gravity of the UAV. The mass of the UAV is denoted as M whereas the mass of the load is denoted as m.
In the previous dynamic model it is considered that the motion of the UAV is due to (i) a cumulative lift force denoted as f which is defined by the turn speed and power of the rotors. This lift force is distributed to a propulsion force along the horizontal Oy axis and a propulsion force along the vertical Oz axis, (ii) a torque τ which causes the roll turn motion of the UAV by an angle φ. This torque is due to uneven values in the turn speed of the UAV's rotors. Therefore to make the UAV turn clockwise (counter-clockwise) it suffices to make the rotors being placed at the left part of the UAV with respect to its axis of symmetry, turn faster (slower) than the rotors being placed at the right part of the UAV (iii) it is considered that airflow remains constant therefore the functioning of the rotors is unperturbed and is determined only by the control voltage which is supplied to them.

Approximate Linearization for the UAV and Payload System
The dynamic model of the UAV and payload system is in the form of Eq. (13), that iṡ where X∈R 8×1 , u∈R 3×1 , F (X)∈R 8×1 and G(X)∈R 8×3 . The aerial robotic system undergoes approximate linearization around a temporary operating point (equilibrium) which is defined as (x * , u * ), where x * is the present value of the state vector of the system, and u * is the last value of the control inputs vector that was applied on it.
After using first-order Taylor series expansion around the temporary operating point (x * , u * ) one obtains the following state-space description of the aerial robotic system:Ẋ whered is the disturbances vector comprising the approximate linearization error (due to truncation of higher-order terms in the Taylor series expansion), as well as external perturbations. Moreover, matrices A and B are given by: For the approximately linearized aerial robotic system an H-infinity feedback controller can be designed. The Jacobian matrix ∇ x F (x) | (x * ,u * ) of the robotic system is computed as follows: About the first row of the Jacobian matrix About the second row of the Jacobian matrix About the third row of the Jacobian matrix About the fourth row of the Jacobian matrix About the fifth row of the Jacobian matrix About the sixth row of the Jacobian matrix About the seventh row of the Jacobian matrix About the eight row of the Jacobian matrix The Jacobian matrix ∇ x G 1 (x) | (x * ,u * ) of the robotic system is computed as follows: The Jacobian matrix ∇ x G 2 (x) | (x * ,u * ) of the robotic system is computed as follows: It holds that: ∂g12 ∂x5 = ml(cos 2 (x 5 ) − sin 2 (x 5 )), Finally, about the Jacobian matrix ∇ x G 3 (x) | (x * ,u * ) of the robotic system it holds that ∇

The Nonlinear H-infinity Control
The initial nonlinear model of the UAV and suspended payload system is in the forṁ Linearization of the model of the UAV and suspended payload system is performed at each iteration of the control algorithm round its present operating point (x * , u * ) = (x(t), u(t − T s )). The linearized equivalent of the system is described bẏ where matrices A and B are obtained from the computation of the previously defined Jacobians and vectord denotes disturbance terms due to linearization errors. The problem of disturbance rejection for the linearized model that is described bẏ where x∈R n , u∈R m ,d∈R q and y∈R p , cannot be handled efficiently if the classical LQR control scheme is applied. This is because of the existence of the perturbation termd. The disturbance termd can represent (i) modeling (parametric) uncertainty and external perturbation terms that affect the aerial robotic system, (ii) noise terms of any distribution.
In the H ∞ control approach, a feedback control scheme is designed for trajectory tracking by the system's state vector and simultaneous disturbance rejection, considering that the disturbance affects the system in the worst possible manner. The effects that disturbances have on the UAV and suspended payload system are incorporated in the following quadratic cost function: Eq. (23) denotes a mini-max differential game taking place between disturbance and control inputs. Actually, the control inputs try to minimize this cost function while the disturbance inputs try to maximize it. Then, in the case of the tracking problem, the optimal feedback control law is given by (Fig. 2) with K = 1 r B T P , while P is a positive semi-definite symmetric matrix which is obtained from the solution of the Riccati equation of the form and e = x − x d is the tracking error. The transients of the control algorithm are determined by matrix Q and also by gains r and ρ. The latter gain is the H-infinity attenuation coefficient and its minimum value that allows solution of Eq. (25) is the one that provides maximum robustness to the control algorithm for the UAV and suspended payload system. The worst case disturbance that the nonlinear optimal control loop can sustain isd = 1 ρ 2 L T P e [Rigatos and Busawon, 2018].

Lyapunov Stability Analysis
Through Lyapunov stability analysis it will be shown that the proposed nonlinear control scheme assures H ∞ tracking performance for the UAV and suspended payload system, and that under moderate conditions about the disturbance terms, asymptotic convergence to the reference setpoints is succeeded. The tracking error dynamics for the UAV and suspended payload system is written in the formė where in the UAV and suspended payload system's case L = I∈R 8×8 with I being the identity matrix. Variablẽ d denotes model uncertainties and external disturbances of the motor's model. The following Lyapunov equation is considered where e = x−x d is the tracking error. By differentiating with respect to time one obtainṡ V = 1 2 e T A T P e + 1 2 u T B T P e + 1 2d T L T P e+ 1 2 e T P Ae + 1 2 e T P Bu + 1 2 e T P Ld The previous equation is rewritten asV = 1 2 e T (A T P + P A)e + ( 1 2 u T B T P e + 1 2 e T P Bu)+ +( 1 2d T L T P e + 1 2 e T P Ld) (31) Assumption: For given positive definite matrix Q and coefficients r and ρ there exists a positive definite matrix P , which is the solution of the following matrix equation Moreover, the following feedback control law is applied to the system By substituting Eq. (32) and Eq. (33) one obtainṡ V = − 1 2 e T Qe + ( 2 r P BB T P e − 1 2ρ 2 e T P LL T P e − 1 r e T P BB T P e) + e T P Ld (35) which after intermediate operations giveṡ V = − 1 2 e T Qe − 1 2ρ 2 e T P LL T P e + e T P Ld (36) or, equivalentlẏ V = − 1 2 e T Qe − 1 2ρ 2 e T P LL T P e+ + 1 2 e T P Ld + 1 2d T L T P e (37) Lemma: The following inequality holds 1 2 e T P Ld + 1 2d L T P e − 1 2ρ 2 e T P LL T P e≤ 1 2 ρ 2dTd (38) Proof: The binomial (ρα − 1 ρ b) 2 is considered. Expanding the left part of the above inequality one gets The following substitutions are carried out: a =d and b = e T P L and the previous relation becomes 1 2d T L T P e + 1 2 e T P Ld − 1 2ρ 2 e T P LL T P e≤ 1 2 ρ 2dTd (40) Eq. (40) is substituted in Eq. (37) and the inequality is enforced, thus giving Eq. (41) shows that the H ∞ tracking performance criterion is satisfied. The integration ofV from 0 to T gives then one gets Thus, the integral ∞ 0 ||e|| 2 Q dt is bounded. Moreover, V (T ) is bounded and from the definition of the Lyapunov function V in Eq. (27) it becomes clear that e(t) will be also bounded since e(t) ∈ Ω e = {e|e T P e≤2V (0) + ρ 2 M d }.
According to the above and with the use of Barbalat's Lemma one obtains lim t→∞ e(t) = 0.
The outline of the global stability proof is that at each iteration of the control algorithm the state vector of the UAV and suspended payload system converges towards the temporary equilibrium and the temporary equilibrium in turn converges towards the reference trajectory [Rigatos and Busawon, 2018]. Thus, the control scheme exhibits global asymptotic stability properties and not local stability. Assume the i-th iteration of the control algorithm and the i-th time interval about which a positive definite symmetric matrix P is obtained from the solution of the Riccati equation appearing in Eq. (32). By following the stages of the stability proof one arrives at Eq. (41) which shows that the H-infinity tracking performance criterion holds. By selecting the attenuation coefficient ρ to be sufficiently small and in particular to satisfy ρ 2 < ||e|| 2 Q /||d|| 2 one has that the first derivative of the Lyapunov function is upper bounded by 0. Therefore for the i-th time interval it is proven that the Lyapunov function defined in Eq (27) is a decreasing one. This signifies that between the beginning and the end of the i-th time interval there will be a drop of the value of the Lyapunov function and since matrix P is a positive definite one, the only way for this to happen is the Euclidean norm of the state vector error e to be decreasing. This means that comparing to the beginning of each time interval, the distance of the state vector error from 0 at the end of the time interval has diminished. Consequently as the iterations of the control algorithm advance the tracking error will approach zero, and this is a global asymptotic stability condition.

Robust State Estimation with the Use of the H ∞ Kalman Filter
The control loop can be implemented with the use of information provided by a small number of sensors and by processing only a small number of state variables.
To reconstruct the missing information about the state vector of the UAV and suspended payload system it is proposed to use a filtering scheme and based on it to apply state estimation-based control , [Gibbs, 2011]. The recursion of the H ∞ Kalman Filter, for the model of the aerial robotic system, can be formulated in terms of a measurement update and a time update part Measurement update: Time update: where it is assumed that parameter θ is sufficiently small to assure that the covariance matrix P − (k) −1 −θW (k)+ C T (k)R(k) −1 C(k) will be positive definite. When θ = 0 the H ∞ Kalman Filter becomes equivalent to the standard Kalman Filter. One can measure only a part of the state vector of the UAV and suspended payload system, such as state variables x 1 = y, x 3 = z, x 5 = θ and x 7 = φ can estimate through filtering the rest of the state vector elements. Moreover, the proposed Kalman filtering method can be used for sensor fusion purposes.

Simulation Tests
In continuation to the previous global asymptotic stability proof, the performance of the nonlinear optimal control scheme for the UAV and suspended payload system, has been further confirmed through simulation experiments. The obtained results are depicted in Fig.  3 to Fig. 14. It can be observed that fast and accurate tracking of the reference setpoints was achieved for all state variables x i , i = 1, · · · , 8 of the UAV and suspended payload system. Moreover it can be noticed that the control inputs applied to the UAV and suspended payload system exhibited moderate variations. For the computation of the feedback gains of the nonlinear optimal controller, the algebraic Riccati equation appearing in Eq. (32) had to be solved at each time-step of the control method. The transient performance of the control scheme relied on the control loop gains r and ρ and well as on the value of the diagonal elements of matrix Q. As explained above, the smallest value of the attenuation coefficient ρ for which the algebraic Riccati equation of Eq. (32) admits a solution, is the one that provides maximum robustness to the control system. It is also noted that by using the H-infinity Kalman Filter a state estimation-based implementation of the control method has been achieved. This allows the reliable functioning of the control loop after receiving measurements from a small number of sensors.
The advantages from the application of the proposed nonlinear optimal control method are outlined as follows: (i) unlike global linearization-based control schemes, the proposed nonlinear optimal control method does not require changes of variables (diffeomorphisms) and application of complicated transformations of the system's state-space model (ii) the new optimal control method is applied directly on the initial nonlinear model of the UAV and suspended payload system and avoids inverse transformations which are met in global linearization-based control and which may come against singularities, (iii) for the case of underactuated electromechanical systems, as for instance the UAV and suspended payload system, finding global linearization transformations is a non-trivial and often hard to manage problem, that ends-up to dynamic extensions methods. The proposed nonlinear optimal control method skips such a type of tedious solutions (iv) the new control approach retains the advantages of typical optimal control, that is fast and accurate tracking of the reference setpoints, under moderate variations of the control inputs,(v) unlike NMPC approaches the proposed control method is of proven convergence and stability, (vi) unlike sliding-mode control approaches the proposed control method does not rely on intuitive definition of cost functions and does not need prior transformation of the state-space model into the canonical form.
Regarding the contribution of this research work specifically to Physics the following can be stated: (a) the manuscript contributes ot the area of applied mechanics since it demonstrates a solution to the stabilization and path tracking problem for underactuated mechanical systems (b) the proposed nonlinear optimal control method is generic and is not addressed only to electromechanical systems such as UAVs. It can be applied equally well to models of umderactuated stand-alone and coupled oscillators. (c) the article offers one of the few feasible and applicable solutions to the nonlinear optimal control problem of underactuated physical and cyberphysical systems, by proving that the proposed control scheme is globally stable and computationally tractable. Since the payload of the UAV is unactuated, the joint UAV-payload system is controllable regarding the motion of the UAV and stabilizable regarding the motion of the payload. Therefore, although the setpoints for the position of the center of gravity and for the roll angle of the UAV can take any value, the setpoints for the turn angle of the payload can take only the zero value. Consequently, the payload should always stabilize at the vertical position and the link connecting the payload with the UAV should align with the Oz vertical axis of the inertial reference frame.
The proposed control method is ideal for underactuated dynamical systems and is not limited to systems with a control inputs gain matrix in quadratic form. The linearization point of the proposed control method is updated at each sampling instance and is determined by the present value of the state vector and of UAV and suspended payload system and by the last sampled value of the control inputs vector. The computation of the control signal is developed on the continuoustime dynamics of the UAV and suspended payload system. A continuous-time algebraic Riccati equation is solved at each time-step and based on this solution a continuous-time control signal is obtained. Next, for the computer-based implementation of the control method. the control signal is sampled and applied on the discretized model of the UAV's dynamics at each sampling instance. The sampling period which has been used in the Simulation experiments was T s = 0.01 sec and was sufficiently small to ensure that Nyquist's sampling theorem conditions are satisfied and that the control loop of the UAV and suspended payload system is not undersampled.

Conclusions
The use of UAVs in products transport and in ammunition tasks has necessitated the development of elaborated controllers for such robotic systems. In this article a novel nonlinear optimal controller has been applied to the dynamic model of a quadrotor UAV ans suspended payload system. First, the dynamic model of the aerial robotic system has undergone approximate linearization around a temporary operating point (equilibrium) which was updated at each iteration of the control method. The linearization procedure relied on first-order Taylor series expansion of the state-space model of the robotic system and on the computation of the associated Jacobian matrices. For the approximately linearized model of the aerial robotic system system an H-infinity feedback controller was designed. It was shown that the H-infinity controller stands for the solution of the optimal control problem for the UAV and suspended payload system, under modelling uncertainty and external perturbations. For the computation of the controller's feedback gains an algebraic Riccati equation had to be repetitively solved at each time-step of the control method. The stability properties of the control scheme have been proven through Lyapunov analysis. By showing that the control loop of the aerial robotic system satisfies the H-infinity tracking performance criterion, the robustness properties of the control scheme have been confirmed. Moreover, under moderate conditions, it has been proven that the control loop of the UAV and suspended payload system is globally asymptotically stable. Finally, to implement state estimationbased control through the processing of measurements from a small number of the UAV's sensors, the H-infinity Kalman Filter has been used as a robust state estimator.