Energy Based Control of a Swash Mass Helicopter Through Decoupling Change of Coordinates

We consider a small helicopter structure that is maneuvered through the control of moving masses. It is referred to as a swash mass helicopter (SMH). This paper addresses the trajectory tracking control problem for the SMH, with a specific focus on the decoupling change of coordinates of both rotational and translational dynamics. We propose a control scheme in which position tracking is the primary objective, while the attitude tracking task is considered as a secondary objective. The intermediate control signals related to the attitude dynamics exploit the structural properties of the SMH and are enhanced with terms that grant a more accurate tracking of the target trajectory. The closed-loop system stability under the trajectory tracking objective is obtained following the Interconnection and Damping Assignment Passivity-Based Control (IDA-PBC) approach. In addition, the presence of external disturbances can diminish the trajectory tracking performance. For this reason, a nonlinear outer loop controller is added to the IDA-PBC to compensate the disturbances. Finally, the results of several simulations are reported to evaluate the performance of the control strategy.


I. INTRODUCTION
The design of unmanned aerial vehicles (UAVs) has gained significant interest due to the large set of applications ranging from environmental monitoring to packet delivery. Most of the literature focuses on quadrotor helicopters [1]- [8].
In this paper, we instead consider a helicopter structure where maneuvering is obtained by controlling the displacement of masses as shown in Fig. 1. In reference to the UAV structure, the basic idea is to deploy a double blade coaxial shaft rotor and to maneuver the helicopter through the linear movement of four masses positioned on the main body plane. We refer to it as the swashed mass helicopter (SMH). The rotors provide a drag thrust, while the inertial masses (through the gravitational forces) induce a certain orientation so that to attain a certain roll, pitch and yaw. Details on the SMH structure, dynamical system model, and basic control strategy based on the backstepping approach can be found in [9].
Basically, from a theoretical point of view, the problem discussed in this paper is an example of a non-linear control system [10] in the formẋ = f (x, u) + g(x, u) that can The associate editor coordinating the review of this manuscript and approving it for publication was Sing Kiong Nguang . be transformed into a canonical formż • = f (z • , υ), after applying a globally invertible non-linear change of coordinates to obtain z • = 1 (x), υ = 2 (x, u) [i.e., x = −1 1 (z 0 )], u = −1 2 (x, υ). Here υ is our new control input, and z • is used to denote the entire state of our system.
A solution to the problem can be obtained by decoupling both the translational and rotational dynamics [11]. VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ The decoupling methodology that we propose is obtained in two steps. The first step is the design of a smooth input moment that decouples the rotational dynamics of our system. In the second step, we design a feedback linearization law that decouples the translational dynamics of our system. In developing our methodology we take as well inspiration from [12], in which the emphasis is on defining classes of decoupling approaches. Moreover, our contribution extends the results of [12], which only deals with a simple planner aircraft. This extension is (non-trivially) accomplished via the application of an exact input-output linearization [13]. This decoupling change of coordinates simplifies the structure of the so-called matching condition and, consequently, it expands the set of target dynamics that can be achieved.
With the decoupling methodology the canonical form of the system can be obtained, and consequently the control mechanism can be designed. Various control methodologies have been analyzed for UAVs. A linear quadratic regulator (LQR) strategy has been considered in [14], [15] for a quadrotor UAV in the presence of external disturbances. Stochastic feedback control was proposed for a fixed-wing UAV in [16]. From the non-linear control point of view, several control laws have been also proposed in recent years to control the UAV. Back-stepping and sliding mode methodologies were compared in [17]. It was shown that the back-stepping control handles the rotational motion in the presence of large disturbances. A nonlinear geometric control design is developed for a quadrotor in [18]. The Immersion and Invariance (I&I) approach has been originally presented in [19] and applied in [20] to deal with the attitude control for an octo-copter. Interconnection and damping assignment passivity based control (IDA-PBC) has been applied in [21]- [23] for the vertical takeoff and landing of a degree one under actuated aircraft with strong input coupling. In [24], the control of a quadrotor based on the IDA-PBC approach is presented. However, it is limited to tracking of the attitude, only. In [25], the idea of a quaternion-passivity-based control is used without the need to solve partial differential equations (PDEs).
Considering the SMH, the possibility to include information about the underactuated mechanical structure and to deal with the concept of energy shaping in the IDA-PBC control design has motivated the work in this paper. Despite the advantage of IDA-PBC in stabilization, there is a lack of work about IDA-PBC for trajectory tracking. The contribution in this paper extends the results of [26], which only deals with set-point regulation (constant reference) tasks. In addition, we show that our control design provides robust tracking by adding a nonlinear outer loop controller. As it will be shown, the proposed control design is simple since it does not require solving the nonlinear partial differential equations (PDEs) describing the SMH dynamics.
The outline of the paper is as follows. The functionality of the SMH structure and the system model are given in Section II. In Section III, we explain the derivation of the canonical form and the Euler-Lagrange form. The trajectory tracking problem and the problem of control robustification are formulated and discussed in Section IV. Numerical results are presented in Section V. The conclusions then follow.
Notation. R n denotes the n-dimensional Euclidean space and R m×n the set of m × n real matrices. I n and O n are the n × n identity and zeros matrices. For any matrix A ∈ R n×n , (A i ) denotes the i-th column and (A ij ) the ij-th element. The set SO(3) := {R ∈ R 3×3 : R T R = I 3 , det(R) = 1} denotes the third-order Special Orthogonal group. The vector cross product for vectors a and b in R 3 is defined as a×b . Given a function f : R n → R, we define the differential operators ∇f := (∂f /∂x) ,

II. SYSTEM MODEL
The proposed SMH deploys a coaxial double blade rotor with the rotor connected to the main helicopter body via a rigid shaft (main shaft) Fig. 1. To steer the helicopter, four masses are displaced on an orthogonal plane w.r.t. to the rotor shaft and can be moved with linear cross shaft servos. Assuming the rotor and blades to be concentrated in one point in the rotor shaft edge, we refer to such a point as the rotor center (RC). The intersection of the rotor shaft and the swash masses plane is referred to as geometrical center (GC) of the SMH. The blades rotation induces a thrust aligned with the rotor shaft and the swash masses shift the center of mass (CM) of the SMH on an orthogonal plane w.r.t. the thrust vector, i.e.,. it tilts the SMH body. A yaw movement is generated by changing the relative speed of the two blades since a drag torque imbalance is generated, while roll and pitch are regulated by displacing the swash masses asymmetrically w.r.t. the GC. It is possible to derive the dynamical system model by using the Newton's framework. We assume the swash masses to be identical and equal to m. The total SMH mass is given by m t = m b + 4m, where the mass of the structure m b (not considering the swash masses) is concentrated in the GC (assuming the mass of the rotors in the RC negligible). The net thrust generated by the rotors is T 1 , while the gravitational acceleration is g. Details can be found in [9]. Herein, for compactness, we report only the system model result which reads as follows  with where the terms s . and c . denote the sine and cosine functions of the argument in the subscript, respectively, and I † ∈ R 3×3 is the absolute inertia matrix of the system which can be expressed as follows: The elements of the absolute inertia matrix in (4) can be computed as follows The control inputs can be grouped in the control input vector U = [T 1 y x M ψ ] T and they are: the thrust T 1 , the swash mass position −L ≤ y ≤ L responsible for the roll torque, the swash mass position −L ≤ x ≤ L responsible for the pitch torque, and, finally, the yaw torque M ψ . We assume the two swash masses to be mutually constrained at constant distance L and the rest position of each masses has a distance L 2 from the center point of the shaft of length L. We define β as the ratio of the swash mass m and the total SMH mass m t , i.e., β = m m t . The dynamical system model in (1)-(2) is highly nonlinear and shows strong coupling among the rotational and translation relations. This renders the development of a control design not straightforward.

III. DERIVATION OF THE CANONICAL AND THE EULER-LAGRANGE FORM
In this section, the aim is to decouple the rotational dynamics of our system model. Then, we linearize the translational dynamics of the SMH. Finally, the canonical and the Euler-Lagrange form of the system will be derived.

A. DECOUPLING METHODOLOGY FOR THE ROTATIONAL DYNAMICS
Basically, our aim is to obtain a representation of (2) in canonical form. However, observing (2) we can say that the matrix (3) operating on the control inputs y , x and M ψ is nonsingular and we can decouple the rotational system by applying the globally defined change of coordinates given in the following theorem [12]. Since the rotational dynamics in (2) is a fully actuated mechanical system for φ = ± π 2 , θ = ± π 2 and, ψ = ±π , it is exactly feedback linearizable. Theorem 1: The following change of coordinates: decouples the rotational dynamics in (2). The elements A i are given in the Appendix and υ = [υ 1 , υ 2 , υ 3 ] T ∈ R 3 is the new control input vector for the rotational dynamics in (2).
Corollary 1: The global change of coordinates in (5) applied to the rotational dynamics in (2) automatically gives the canonical form.

B. FEEDBACK LINEARIZATION FOR THE TRANSLATIONAL DYNAMICS
We begin by considering the translational dynamics in (1) that can be linearized as follows  where are new control inputs. Note that the desired roll φ * and pitch θ * angles are considered as inputs for the rotational subsystem. Therefore, we can obtain the total thrust T 1 , the desired roll φ * and pitch θ * angles as below via the inverse transformation of equations (9-11), as follows: The desired yaw angle ψ * will be obtained in a way that SMH's heading and direction of motion in the x-y plane (top view) are on the same line. Geometrically, the desired yaw angle ψ * can be obtained as follows: Having the desired yaw angle ψ * , the desired roll φ * and pitch θ * angle can be easily obtained using (13), and (14), respectively. From the obtained results, we have the canonical form of our full system model C. EULER-LAGRANGE FORM Now, by using (16) and (17) the SMH can be written in the Euler-Lagrange form, as follows where the term q = [η 1 η 2 ] = [x y z φ θ ψ] ∈ R 6 are the configuration variables, M = M = I 6×6 ∈ R 6×6 is the inertia matrix and C(q,q) = O 6×6 ∈ R 6×6 denotes the Coriolis term, u = [u x u y u zῡ1ῡ2ῡ3 ] ∈ R 6 is the input vector, matrices ∇V (q) and G are given by where α = 1 m t .

IV. TRAJECTORY TRACKING FOR THE SMH USING IDA-PBC
The development of the canonical form using the decoupling methodology obtained in the previous section, simplifies the control design. This is because now the SMH can be treated as a fully-actuated mechanical system. The main objective of the proposed control law is to act on the rotor thrust and swash masses position so that the SMH can track the desired target trajectory q * (t) with stable Euler angles.

A. PROBLEM FORMULATION
Let us consider the SMH model in (18). The objective is to design a control law u = u(t, q,q) that ensures for any arbitrary bounded target time dependent trajectory q * (t) ∈ C 2 .

B. IDA-PBC METHODOLOGY
Now, we can write the Hamiltonian H (q, p) [21] to be the summation of the kinetic energy and the potential energy (total energy), respectively, in the form where q ∈ R 6 and p ∈ R 6 are the generalized position and momentum (we have introduced the notation ). Therefore, the SMH dynamical system model can be written in the port-controlled Hamiltonian (PCH) form as follows: with Remark 1: From (22) and (23) it can be observed thaṫ q = M −1 p. In addition, in the following to ease the notation, we will omit in some cases the dependency of vectors and matrices on the state q.
Suppose that [q * (t) p * (t)] is a bounded target trajectory and p * can be calculated by p * = M (q * )q * . Then, the total desired energy function (closed-loop) can be 77452 VOLUME 8, 2020 modified in a more convenient form. In fact, we can transform the closed-loop system model into the port-controlled Hamiltonian form [21].
Modification of the total energy function: Motivated by (22), we propose the desired trajectory dependent Hamiltonian function where V d (q) and M d = M d > 0 represent the desired closed-loop potential energy function and inertia matrix, respectively. Notice that the stabilization problem (q, p) = (q * , 0), q * ∈ R 6 is a special case of the tracking problem (q, p) = (q * (t), p * (t)). We will show that with small modifications on the desired potential function in (26), tracking can be realized. Now, the desired PCH model can be written as where L d (q, p) is a skew symmetric matrix and K P = K P > 0 contains design parameters. J (q, p) is left free to the designer and follows the skew-symmetry property. Energy shaping and damping injection: The control law can be written as The control law in (30) consists of two terms responsible for the energy shaping and the damping injection, respectively.

C. CONCLUSIVE RESULTS
As already mentioned, the desired potential function can be considered as a quadratic function plus a gravitational compensator V d (q) = V (q) + 1 2 q − δ(t) 2 K p to ensure tracking ability. To do so, we set M d = M and J (q, p) = 0 and propose the following desired storage function where δ(t) = q * (t) + K −1 p (ṗ * + ∇V (q * )). To proceed, we extend the well-known controller PD+ [27], from an IDA-PBC point of view. This is done by starting from (31) and obtaining the final controller in the following form where K d is a positive definite matrix. The control law in (33) withq = M −1 p can be seen as a PD+ controller plus a time dependent shift. The architecture of the controller in (33) consists of six cascade PD+ controllers where the cascade comprises the control of the roll ↔ x directional movement and pitch ↔ y directional movement as depicted in Fig. 2. VOLUME 8, 2020

D. DEVELOPMENT OF A CONTROLLER IN THE PRESENCE OF DISTURBANCES
Herein, we consider the effect of constant, matched disturbances in the port-controlled Hamiltonian system (23). Let us consider the dynamical system model in (23) with the controller in (33). The disturbances propagate and must be compensated with the dynamic outer loop control. To proceed, we consider the system (23) perturbated by an input disturbance in closed-loop with the controller u n = u + u o , leading to the following model and control problem. Perturbated dynamical system model: Given the perturbated system withd ∈ R 6 , we want to find (if possible) a dynamic controller u o = β(q, p, ζ ), where ζ ∈ R 6 is the state of the controller, that • ensures tracking the target trajectory q * (t) for all initial condition inside the sphere centered at q * (0) and with radius T , for some ζ * ∈ R 6 . Proposition 1: Consider the perturbated dynamical system model in (34) with constant matrices K P > 0, K I > 0 and Introduce the globally defined change of coordinates γ = (q, p, ζ ) given by [28] Then, the closed-loop dynamics can be written in trajectory dependent port-controlled Hamiltonian form as follows: with Hamiltonian Proof 2: We differentiate (37) and (38) to obtaiṅ which is the second row of the closed-loop dynamics (40). Finally, from the last row of (40), we obtiaṅ To prove global tracking of the SMH under disturbances we take as well inspiration from [29], in which the emphasis was on exploiting contraction analysis [30]. To do so, we consider the following proposition Proposition 2: Consider the trajectory dependent portcontrolled Hamiltonian in (40). H γ (γ ) is the desired trajectory dependent Hamiltonian function which satisfies the following conditions Condition 1: Condition 2: for positive constants ρ, β and ρ < β. D T is an open subset of R 6 which for all t ≥ 0 contains q * (t) and a sphere with constant radius T around q * (t).
If conditions (1) and (2) hold true, then the tracking is granted globally.
Proof 3: Consider (40), q * (t) is a trajectory of this system. We can write the virtual dynamics 1 of (40) as follows: To make our notation simple, we define Now, we need to prove that there exists a constant for which the corresponding virtual dynamics is contracting. To proceed, we define 1 For more details and the generalization of the proposed theory see [29]. 77454 VOLUME 8, 2020 Therefore, it will be shown that L + L is uniformly negative definite. In detail, Now, we consider ∇ 2 H γ < βI : The inequality in (52) with some straightforward calculation and considering ρI < ∇ 2 H γ < βI , yields: In fact, (49) is Hurwitz, so there exists a positive definite matrix P such that the following algebraic Riccati equation is satisfied: Pre-and post-multiplying of (54) by P − 1 2 gives: Now, we can state that the right-hand side of the inequality in (56) is negative definite, which completes the proof.

V. NUMERICAL RESULTS
In this section, numerical results are reported to evaluate and validate the proposed control strategy. A simulator of the dynamical system model (1) and (2)

A. REST TO REST MANEUVER
We start by considering a rest to rest maneuver. The SMH is commanded to fly from a predefined initial to a final position in the 3D space (rest to rest maneuver). To model the trajectory, at least fifth-order polynomial functions should be considered with the six boundary conditions In the considered case, the coefficients of the desired polynomial in (57) are Therefore, the desired trajectory for the x direction q * 1 (t) = x * (t) is given by Since we have defined q * (t) = [x * (t), y * (t), z * (t), φ * (t) , θ * (t) , ψ * (t)] , the desired trajectories for q * 2 (t) = y * (t) and q * 3 (t) = z * (t) directions are given by similar derivations. The proposed desired trajectory (57) with coefficients (64) ensures that the velocities and accelerations at the rest point are zero. Remark 2: It should be noted that the desired roll φ * and pitch θ * angles can be obtained by the inverse kinematic in (13) and (14), respectively.
The control gains (33) and (35) for this scenario are In Fig. 3, we report the details of the trajectory obtained from the control law u n = u + u o when the SMH is commanded to follow the rest to rest maneuver in the 3D space. The details of the 3D trajectory is shown in Fig. 4. Based on the results shown in Fig. 4, it can be concluded that the transient performance is quite good and the SMH can reach the stable position in a short time. Finally, the control inputs are shown in Fig. 5.
The control gains (33) and (35) are The considered target trajectory for the SMH is In Fig. 6, we report the details of the trajectory obtained from the control law u n = u + u o when the SMH is commanded to follow the given target trajectory in the 3D space. To show the effectiveness of the proposed controller in the presence of a disturbance, we add a matched constant disturbanced = 0.8 at time t = 20s (Fig. 6) to the system. The details of the 3D trajectory are shown in Fig. 7. The blue, green dashed, and red lines represent the target trajectory, and the ones obtained with the IDA-PBC controller and, the robust IDA-PBC controller, respectively. Fig. 7 shows the tracking performance of the translational position, x, y, and z, and the yaw angle ψ, and the stabilization of the roll and pitch angles, φ and θ, respectively. As expected, the presence of the disturbance produces an error in tracking, so that the SMH diverges from the target path. However, by adding the PI controller we can reject the disturbance. Despite the aggressiveness of the target trajectory, the SMH is able to well follow it. The real-target trajectory overall RMSE is reported in Table 1.    8 shows the time history of the controller state ζ , which provides the nice disturbance rejection. As can be seen, the controller can track the disturbance and reject it perfectly. The proposed robust control law based on IDA-PBC does not include the second time derivatives of the desired states. This is of great advantage because in practice, the numerical computation of the derivatives generates noise in the control signal. Finally, the control inputs are shown in Fig. 9. On the other hand, during the initial states, when the desired attitude is not achievable, the SMH is working and moving in an energy-efficient way in the sense of Proposition 2 to damp and modify the attitude reference so that stabilization is guaranteed.

VI. CONCLUSION
We have briefly presented a novel small aerial vehicle structure, referred to as swash mass helicopter (SMH) that allows maneuvering it through the control of four swash masses and a double coaxial rotor. We have then focused the attention to the design of an automatic control mechanism so that the SMH tracks a target trajectory with stable Euler angles. The dynamical system of equations that describes the SMH dynamics is non-linear and rather unique. In fact, it consists of sub-sets of differential equations highly-coupled through the control inputs. Considering the full 3D case, we essentially highlighted that a key point for the control of the SMH is the ability to decouple its three second-order subsystems using a global change of coordinates. This allows us to transform the dynamical system model into the canonical form so that an IDA-PBC control approach can be applied. The control laws have been derived including a modified version to cope with external disturbances. An analysis of robustness exploiting contraction theory has also been developed. Several results from simulations have been presented to assess the performance of the SMH control. They show that good controllability of the SMH is attainable.