Robust Trajectory Tracking Control for Fully Actuated Marine Surface Vehicle

In this paper we present a robust trajectory tracking control for a fully actuated marine surface vehicle. The tracking controller is obtained using a port-Hamiltonian model of the marine craft and includes an integral action to compensate for constant disturbances. The proposed approach adds damping into both the position and integrator coordinates, leading to input-to-state stability with respect to time-varying disturbances. We exemplify this controller with a simulation for an unmanned surface vehicle subjected to constant and time-varying wind disturbances. The tracking controller rejects the disturbances achieving global exponential stability for constant disturbances and input state stability for time-varying disturbances.


I. INTRODUCTION
In recent years, unmanned surface vehicles (USV) have been increasingly adopted for scientific, commercial and government applications [1]. As USVs are nonlinear, design of control systems for regulation and tracking tasks is non-trivial. Further complicating the task, marine system are unavoidably affected by wind and ocean disturbances, which can affect the stability properties of the system. Several methods for controlling USVs have been reported in the literature, including backstepping, sliding mode and passivity-based control.
Backstepping has been utilised by several authors for a combination of sea keeping (set-point regulation), tracking control and disturbance rejection-see [2]- [4] and enclosed references for an overview of this approach. Using this approach, exponential stability is ensured by first designing the controller about the tracking error dynamics and then backstepping to the velocity dynamics, recovering the control law. The drawback of this approach, however, is that it can lead to complex control laws that neglect the underlying physics of the system.
Sliding mode controllers have also successfully been applied to USVs for tracking control [5]- [7].
The associate editor coordinating the review of this manuscript and approving it for publication was Liang Hu .
Such controllers produce finite-time convergence and are well known to be robust against bounded matched disturbances. The drawback, however, is the existence of a control discontinuity at the origin of the closed-loop dynamics which can lead to chattering. This limitation has been relaxed in recent years with the introduction of higher-order sliding modes, which comes at the expense of increased controller complexity.
Intelligent tracking controllers commonly use fuzzy logic algorithms to reject unknown disturbances acting over surface vessels [8], [9]. The results with the implementation of fuzzy aproximators show exponential stability, however, the rules must be constantly updated, this implies online connection and also computational complexity. In [10] is shown that for complex unknown disturbances tracking errors can converge to a neighborhood of zero, then the stability for the error dynamics is uniformly bounded. Downsides of this approach include the computational requirements of fuzzy control and the need to update the fuzzy rules online. These disadvantages make these controllers hard to implement on autonomous USVs.
Passivity-based control is an alternate approach to control design that emphasises the role of power and energy within the system [11]. The approach has been applied to tracking control problems in [12], [13] and disturbance rejection problems in [14], [15]. The approach was applied to the problem of tracking control and disturbance rejection for USV system in [16].
In this work, we combine the methods of trajectory tracking control and disturbance rejection using the pH framework. Following this approach, damping is added to all coordinates of the system, allowing verification of strong stability properties such as exponential stability and input-to-state stability (ISS). Extending on the work [16], we avoid the need for a coordinate transformations.
Notation: 0 n×m denotes a matrix n × m of zeros, I n denotes a n × n identity matrix. For x ∈ R n , x 2 = x x. All functions are assumed to be sufficiently differentiable. For a mapping H : R n → R, the gradient transpose is denoted as ∇H := ( ∂H ∂x ) . For a symmetric matrix A = A ∈ R n , λ min (A) denotes the minimum (real) eigenvalue of A.

II. BACKGROUND AND PROBLEM FORMULATION A. SYSTEM MODEL
In this work, we consider USVs restricted to operate in the horizontal plane with three degrees of freedom. The USV, shown in Figure 1, is a catamaran with two thrusters installed along the gravity centre line . Each thruster is fixed to a gearbox which controls the rotation of the thruster from 0 up to 360 • . Using this configuration, this vehicle can produce control forces in surge, sway and yaw independently.
The dynamic behaviour of USV systems can be described by the dynamic equationsη where η = [x, y, ψ] , are the translational positions and heading angle of the vessel, ν = [u, v, r] are the body-fixed velocities containing surge, sway and yaw-rate, M = M > 0 is the body-fixed mass matrix which includes added mass due to fluid-body interactions, C(ν) = −C (ν) is centripetal and the Coriolis acceleration matrix, D(ν) = D (ν) > 0 is the hydrodynamic damping matrix, g(η) is the generalized forces vector due to gravity and buoyancy [17], τ c is a vector of control inputs and τ d (t) is a vector of unknown, and possibly time-varying, disturbance forces. The matrix R(ψ) is a rotation that maps from the body-fixed velocities ν to the inertial velocitiesη. For the problem considered in this work, the matrix R(ψ) has the particular form and satisfies the property R −1 (ψ) = R (ψ). As shown in [18], by defining the systems momentum as the system (1) can be written as an input-state-output port-Hamiltonian system of the form where is the Hamiltonian containing the kinetic and potential energies of the system and (p) = [C(ν) + D(ν)] | ν=M −1 p which satisfies (p) + (p) > 0.

B. PROBLEM FORMULATION
In this note, we consider the trajectory tracking problem for the system (4). That is, given a pre-defined twice differentiable reference trajectory the objective is to design a dynamic control laẇ such that the signalη converges to the origin at an exponential rate.

C. CONTRIBUTIONS
In this paper, we combine tracking control with integral action within the port-Hamiltonian framework to achieve exponential tracking of reference trajectories in the presence of unknown constant disturbances. Such disturbances are representative of wave and wind disturbance which are unavoidable in practice. The scheme is also shown to be ISS with respect to arbitrary time-varying disturbances. In contrast with the work [16], we avoid the need for a momentum coordinate transformation, simplifying the control design process and implementation for autonomous vehicles.

III. TRAJECTORY TRACKING CONTROL
In this section, we propose a trajectory tracking control law to solve the problem formulation of Section II-B. The approach combines the work [16] with aspects of the integral action scheme [19], avoiding the need for a momentum coordinate transformation. By combining these methods, damping is injected into all coordinates, allowing the verification of strong stability properties such as global exponential stability and ISS. For this construction, we consider the disturbance term τ d (t) to be comprised of a constant component d c and a time- An integral action controller will be introduced to asymptotically reject the effects of the constant disturbance d c whereas the system will be shown to be ISS with respect to the time-varying disturbance d t (t). In addition, we define an augmented model for η and p including the new vector ξ , which represents the dynamics for the integral states in the energy shaped Hamiltonian function H d defined below. Proposition 1: are positive definite tuning parameters and The closed-loop dynamics can be written as a port- (15) Proof: The proof follows from direct matching of the dynamics (14) with those of (4). First consider the dynamics ofη in (14) which can be expressed aṡ which is the time derivative ofη, defined in (9). Now, the dynamics of ξ in (14) can be rewritten aṡ which agrees with (11). Finally, we consider the dynamics of p. From the definition ofp in (13) we have thaṫ where we have expressed d c in terms of α from (15). This final expression agrees with (14) as desired.
Remark 1: Notice that the last term in (15) introduce a cross term in the Hamiltonian function that results in a controller (11), (12), which is simpler than that of [16]. Also, notice thatṗ d and thus the control signal depend on the term d dt R (ψ), which can be easily computed as function of ψ anḋ ψ.
The closed-loop dynamics (14) have a pH structure, which ensures stability, but are subject to an external disturbance term d t (t) that can impact on the stability properties of the closed-loop. In the following proposition, it is shown that if the disturbance is constant (τ d = d c ), the trajectory error exponentially converge to zero. In the case of a time-varying disturbance (τ d (t) = d c + d t (t)), it is shown that the closedloop system (14) is input-to-state stable with respect to the time-varying disturbance. In lay terms, this means that if the time-varying disturbance is bounded, the deviation of the system from the desired trajectory will also be bounded. The size of these bounds can be changed using the tuning parameters provided in Proposition 1.
Proposition 2: The closed-loop dynamics (14) have the following properties: is globally exponentially stable, which implies that the tracking errorη converges to the zero at an exponential rate.

2) If the disturbance is time-varying
then the closed-loop system is ISS with respect to the disturbance d t (t). Proof: The proof follows by taking H d , defined in (15), to be a (ISS) Lyapunov candidate for the closed-loop dynamics (14). Before proving the claims, first notice that H d VOLUME 8, 2020 can be written as where As Q is positive definite, H d satisfies for some k 1 , k 2 ∈ R + . Now, computing the time derivative of H d along the trajectories of (14) results iṅ where c 1 > 0 is an arbitrary constant from application of Young's inequality. Taking c 1 = λ min (K d 2 ), (23) simplifies tȯ where To verify claim 1 notice that in the case of constant disturbances only (d t (t) = 0 n×1 ), (24) simplifies tȯ for some > 0. Global exponential stability follows from Theorem 4.10 of [20]. To verify claim 2, notice that H d is decreasing whenever verifying the closed-loop dynamics are ISS with respect to the time-varying disturbance d t (t). Remark 2: From (26) it can be seen that the rate of convergence can be increased by increasing the tuning gains K d 1 , K d 2 and K d 3 . From (27) it can be seen that the final bound of the state due to time-varying disturbances d t (t) is related to the gain K d 2 , which is a parameter to be selected.

IV. FULLY ACTUATED UNMANNED SURFACE VEHICLE
The actuator configuration described in Section II.A allows for forces and moment in all degrees of freedom of interest. This characteristic of the vehicle is exploited for trajectory tracking and rejection of slow-varying disturbances, such as wind.
Moreover, the USV is an over-actuated system since there actuators commands that will produce a demanded generalised forces is non unique. Then, if the controller demands a particular vector of generalised forces, the actuator commands are obtained using a control allocation algorithm [21].

A. CONTROL ALLOCATION
As shown in Section II.A, the forces produced by the actuators can be decomposed in rectangular components noted as F Lx and F Ly for the left thruster, and F Rx and F Ry for the right thruster. These forces can be map into generalised forces τ c using the geometry of the USV, which in this case results as follows where Lx , Ly , Rx , and Ry are the distances from the thruster to the vehicle's centre of mass. Notice that the x and y components of the actuator forces can be written as for i = {L, R}, and where f i is the thrust force and θ i azimuth angle of the thruster i. Given the vector of generalised forces τ c computed by the controller, we need to determine the actuator commands u. A classical solution to this problem is to use the Moore-Penrose pseudoinverse of the transformation matrix T , that is with T † = T (TT ) −1 is the pseudoinverse of T . Once the actuator command vector u is known, we can compute the thrust forces and azimuth angles as follows where k a force coefficient, which is constant for trolling thrusters [22].

B. HYDRODYNAMIC CHARACTERISTICS AND WIND DISTURBANCES
In this subsection, we describe the hydrodynamic parameters of the dynamic model of the USV. Moreover, the characteristic of the wind disturbances are obtained using data reported in the literature for a USV performing surveys on a specific geospatial area. The wind forces are described in the horizontal plane, and the fluid dynamics behaviour as eddies or wake effects circulating around the USV are neglected. We consider the linear theory for surface gravity waves where there are no lifting forces considering the long-wave approximation assumption, thus, the hydrostatic forces can be neglected. For slender vessels this approximation allows to consider the free surface as rigid surface with infinite depth [23]. The physical characteristics of the USV considered in this paper are shown in the Table 1. We also approximate the values of the hydrodynamic parameters and provide the system characterisation used for standard manoeuvres of surface vessels [24].
We used a normalization factor reported in [25] with vehicle parameters similar to those used for the DELFIM vehicle described in [26].
The Components in M A are hydrodynamics parameters Xu = −23.7kg for surge added mass, Yv = −702.09kg for sway added mass, Nṙ = −409.3kgm 2 for the inertial effects due to added masses and sway added masses related with the angular velocity Yṙ = Nv = −30.01kgm.
The Coriolis and centripetal forces effects of acceleration in the USV are captured by the matrix The damping matrix describes the hydrodynamic resistance forces, that is the energy dissipation in the interaction between the USV and the water volume [17]. The dissipation phenomena is represented by the matrix The generalised forces produced by the wind can be modeled as the sum of a constant component, and a time-varying

component. The time-varying component is modelled as
where c x , c y and c z are coefficients that can be estimated based on experimental results from [27]; V rw (u rw , v rw ) is the relative wind velocity with its components in surge u rw and sway v rw from (39); ρ a is the air density at standard state conditions; A Fw is the wind frontal contact area with the USV and A Lw is the wind lateral contact area with the USV; L oa is the length overall along the USV; γ rw is the relative angle of attack. The non-static and fast time-varying wind speed in the horizontal plane is calculated as follow where V w i is the amplitude, ω i is the frequency and ϕ i is the phase [28]. The relative wind velocity for surge and sway in the body-frame has the form The angle of attack in the body frame is obtained from the relative velocities γ rw = −arctan2(v rw , u rw ). Therefore, V rw = u rw +v rw is the relative velocity used to compute the generalized forces vector in the Eq. (36). The Fig. 2 presents the body frame wind disturbance velocity acting on the USV.

C. INTEGRAL ACTION CONTROL FOR THE USV
In order to visualize the IAC response to reject constant and time varying disturbances a well known parametric curve for zig-zag maneuvering is used for tracking control

1) WIND DISTURBANCES PROFILE
This trajectory allows us to focus on the pH model response to the wind forces with a preset amplitude and frequency, disturbing the USV motion. For the purpose of simulation, we use the wind speed model (37)-(38) with the parameters ω x = 0.2, ϕ x = 0, ω y = 0.2, ϕ y = 1. The wind velocities profiles u w and v w are based on surveys carried out by CIGoM and IRPHE-CNRS for the Gulf of Mexico and the Caribbean Sea [29]. The wind constant disturbances d c start at t 1 = 30s with a wind speed of 25ms −1 . At t 2 = 70s start the wind timevarying disturbances as is shown in the Fig. 3 for surge u w and sway v w shows the wind profile for the velocities disturbances.
Recalling the Eq. (36), the wind vector for the generalized forces τ d corresponding to the constant and time varying wind force acting over USV are shown in Fig 3, notice that wind force for heading are [Nm] units.

D. TRACKING CONTROL WITH INTEGRAL ACTION
The methodology presented in Proposition 1 guarantees exponentially stable tracking control when the system is subjected to only constant disturbances in the actuated coordinates. Furthermore, ISS with respect to time-varying disturbances is ensured.
The values for the control tuning matrices for closed-loop dynamics (14):  Fig.4. The K d 2 matrix has a particular importance because of acts on the momentum states, hence, is more sensitive to changes in its values.
Tracking controller achieves the reference trajectory from the initial point. Then at t 1 = 30s the constant disturbances are introduced to the dynamics and rejected for the tracking controller achieving global exponential stability GES. The last test at t 2 = 70s the time-varying disturbances are applied to the vehicle and rejected for the tracking controller achieving ISS. Fig. 4 shows that the tracking controller positions (blue lines) rejects the disturbances, always fits the reference positions(red lines), for x, y and ψ positions.
In the velocity plots, see Fig. 5, is pointed the constant disturbances affecting the marinecraft velocities in surge, sway and heading. At t 1 when d c is acting over the vehicle and creating a deviation that is exponentially rejected, this behavior is marked in surge and sway velocities. After t 1 + 15s, the velocities recover the tracking reference. For the time-varying disturbances d t (t), in t 2 the velocities deviation are bounded. The IAC rejects τ d this can be seen in the position plots Fig. 4.
The generalized velocities are presented in the rigid body frame, this low cruise speed are useful for oceanographic USV due to the fact that surveying with transects techniques has a real-time positioning precision at low speeds that is extremely useful for collecting data. The red lines represents the reference velocities and the blue line the tracking control.  The control for the generalized forces input vector are shown in the Fig.6. From the initial conditions with no disturbances the desired position is achieved exponentially when the vector forces achieve the zero at 10s. For constant disturbances d c at t 1 the tracking controller generates the input forces and torque for surge, sway and heading, respectively. Then for t 2 the time varying disturbance d t (t) generate the disturbance rejection forces. Commonly the thruster for small marinecrafts in this case the USV have a mechanical force up to 300N, so for this example the energy needed to reject the disturbances represents one third of the total energy available.
The error dynamics positions are presented in log 10 in the Fig. 7. From the initial position to the reference position the tracking control shows exponential stability. For t 2 to t = 100 the error dynamics for surge and sway are bounded for the known disturbances effect. The error states norm χ , Fig. 7, shows the global exponential stability for the constant disturbances d c at t 1 < t 2 as was presented in the proposition 2 Eq. (24). For the time-varying disturbances when t > t 2 , the error dynamics χ shows input state stability recalling proposition 2 the Eq. (27). These stability properties imply that the states of the vehicle exponentially converge to the desired trajectory and that the states are bounded when the disturbances are bounded.

V. CONCLUSION
In this paper we designed a passivity-based controller motivated by the well-known robust properties of this class of controller. In this sense, we proposed a trajectory tracking controller designed for fully actuated unmanned surface vehicle preserves the port-Hamiltonian structure for the closedloop dynamics with the integration state augmented model. The control law was exemplified by simulating the USV dynamic model subjected to both constant and time-varying wind disturbances. The external disturbances are rejected, ensuring the error dynamics convergence with GES for constant disturbances and ISS for time-varying disturbances. The tracking controller is performed in the body frame which means no coordinate transformations and no computational complexity. The proposed controller is simpler than previous passivity-based controllers in the literature. The robust properties of the proposed control system against disturbances together with the robust properties inherited by the passivity-based design methods are fundamental for our future implementation.