Speed and Heading Control of an Unmanned Surface Vehicle Based on State Error PCH Principle

This paper proposes a novel nonlinear control scheme based on energy-shaping (ES) principle and state error port-controlled Hamiltonian (PCH) systems for unmanned surface vehicles (USV) system. The PCH model of three degrees of freedom for USV kinetics system is established. By the ES principle, interconnection assignment and damping injection method is applied to the speed and heading control of the closed-loop USV system to realize an overall stability of control mechanism. Simulation results show that the validity and stability of control algorithm can be satisfied with the performance in speed and heading tracking of which the high simplification and portability make it applicable to the various region.


Introduction
Unmanned Surface Vehicle (USV) is operated on the surface of the water without crew operation. The first appearance of USV can be traced back to World War II, in which they were developed for the purpose of military use. With the development of correlative technique, USV becomes widespread in areas both military and civilian such as Mine Countermeasures, Environmental Monitoring, Maritime Security, Anti-Submarine Warfare, Electronic Warfare, Surface Warfare, Special Operation Forces, and Maritime Interdiction Operation. USV has been widely used for special missions. Now, with the innovation in electric propulsion technology, the small high speed unmanned systems will have wide applications. The motion control problem of USV is attracting more and more attention from scholars all over the world [1].
The dynamic performance characteristics of USV have an important part in the development of the automatic system for motion control. It is still a problem in the field of both control theory and robotics to have high quality motion control for USV systems. And a group of international and domestic academics have devoted much of their research on the nonlinear control of such vehicles. According to the related literature, various designed controller approaches have been proposed like sliding mode control, adaptive control, backstepping control, cascaded control theory, fuzzy logic control, and so on. As discussed by Liu et al. [2] comprehensive reviews present recent progress of the control approaches from the points of applications, methodologies, and challenges. As to the adaptive control, an active mechanism for unmanned vehicles, Klinger et al. [3] implemented an adaptive algorithm with the modified backstepping surge controller which has been field tested. Sonnenburg [4] and Sonnenburg and Woolsey [5] direct a speed controller algorithm by backstepping and Lyapunov's direct method, which also has been tested by USV. Dong et al. [6] present a state feedback based backstepping control algorithm to address the speed and trajectory tracking problem. Sean Kragelund et al. [7] proposed three different adaptive speed controllers and a model reference adaptive controller of a floating turbine. The major solutions of trajectory tracking problem are the method of feedback linearization, backstepping approach, Lyapunov's direct method, cascade system method, robust control, sliding mode control, and so on. Some scholars used the backstepping approach and Lyapunov's direct method to resolve the trajectory tracking problem of the USV system 2 Mathematical Problems in Engineering [8,9]; the result showed that the controller can still force the trajectory. In paper [10], a sliding mode trajectory tracking controller was developed, and the result showed that the USV could track circular and straight line trajectory. Kahveci and Ioannou [11] proposed an adaptive law which is combined with a control design including a Linear Quadratic (LQ) controller to resolve the steering control for uncertain ship dynamic. However, the nonlinear control methods mentioned above are still not implemented because the process of control is too complex, and there is still not a comprehensive and practical control law which can be robust in vessel dynamics.
Recently, some scholars pay more attention to the portcontrolled Hamiltonian (PCH) theory and interconnection and damping assignment (IDA-PBC) method [12][13][14][15], and in the design of nonlinear control systems field the interconnection and damping assignment approach has gradually become a significant method. In this paper, our main objective is to develop a new speed and heading angle controller of USV which combined interconnection and damping assignment method and state error approach. The organization of this paper is as follows: mathematical PCH kinetics model of the USV is presented in Section 2, a detailed design of the controller is in Section 3, Section 4 analyzes the stability of the controller, and the simulation results are shown in Section 5.

PCH Model of Unmanned Surface Vehicles
2.1. The Model of USV. The structure of USV is shown in Figure 1. The propulsion system of USV is consisted of two propellers derived by two electric-powered motors. By the force and steering torque control, USV can keep moving in the condition of surge, sway, and yaw. Because, in this structure of three appreciable degrees of freedom, only two degrees can be actuated, USV under this structure is underactuated. In this context, dynamic model of USV has been extensively studied. In order to better facilitate the modeling design, USV is assumed to be moved in ideal fluid, and the mass is uniformly distributed. When building the reference frame, an origin of USV body coordinates coincides with the center of gravity, and both the center of gravity and buoyancy are perpendicular to the -axis. In physical design, USV is set to be port-starboard symmetrical; hence surge subsystem and sway-yaw subsystem are essentially decoupled [2].
From a physical standpoint, we should consider the impact of the nonlinear hydrodynamic damping in kinetics model to cover the applications from high speed to low speed. USV is assumed to be moved in ideal fluid, and the mass is uniformly distributed, so the uncertainties and disturbances are linear with velocity or slowly varying relative to the USV dynamics. Based on all of these above assumptions, the kinetics model [3,[16][17][18][19][20][21] of USV can be obtained as (1) Figure 1: The motion coordinate system for USV.
The Kinematics model of heading subsystem of USV can be obtained aṡ= , where is surge velocity, V is sway velocity, and is yaw rate in body fixed reference frame. are inertia coefficients of USV including mass effects added, are hydrodynamic damping coefficients in conditions of surge, sway, and yaw, are the forces of propulsion system, and is the steering torque. denotes orientation angle of the vessel.
Then (1) can be expressed in matrix form: M is an inertia parameters matrix including the added body mass, ( ) is the so-called Coriolis and centripetal matrix, and is hydrodynamic damping matrix.
The thrust forces and steering torque are functions of the two surge control thrust forces which are from each propeller: where 1 is the thrust force which is produced by the first motor and 2 is the thrust force which is produced by the second motor. is the distance between the propellers. From (4), the thrust allocated to each propeller, 1 and 2 , can be calculated as Mathematical Problems in Engineering 3

PCH Systems.
The general form for a nonlinear dynamical system can be shown as follows: where ∈ R is the state vector, ∈ R is the output vector, and ∈ R is the input vector. From [13,14] we can see that if there is possible way to find a nonnegative function ( ) ( (0) = 0) such that the system described by (6) is passive. Then, the PCH system with dissipation can be described as follows [14]: where ( ) is positive semidefinite symmetric matrix and ( ) = ( ) ≥ 0. It represents the dissipation of the system. The interconnection structure of the system is represented by the skew-symmetric matrix ( ) = − ( ) and matrix ( ).
( ) is the Hamiltonian function which defines the stored energy function of the system.
The variation of internal energy of the dynamical system (8) equals the power which was provided with the system by the environment plus the dissipated power. The PCH system (8) model's energy balance equation is as follows: In time-interval [0, ], (9) establishes the passivity properties of the PCH system. which is the same as (7):

PCH Model of USV Kinetics
System. From the system described by (3) and (6), the state vector and the input vector of the system are defined as follows: The Hamiltonian function of the USV dynamic system can be defined as Combine (3), (8), and (12); then the PCH model of USV dynamic system can be obtained as follows: where

The Controller Design of Speed and Heading
In the design of energy controller, energy optimizing is realized by port-controlled Hamiltonian model. As to the PCH system described by (8), how to obtain a feedback defined by (17) that can keep the closed-loop system stable is the key point. Assuming * is a desired equilibrium, then the state error will bẽ= − * . The final objective of IDA-PBC [12] is to find ( ), , matching the condition Then the closed-loop system (8) follows a state error PCH form:̇= 4

Mathematical Problems in Engineering
The desired Hamilton function is chosen as Then we choose where 12 , 13 , 23 , and 1 , 2 , 3 are the designed parameters.
Consider the closed-loop system (18) with feedback control by (17); substituting =̃+ * into PCH system (8), we geṫ= In USV PCH structure model, we could get the hydrodynamic damping matrix as constant matrix. So we used the symbol to represent the symbol ( ). From the PCH system (8), where * is the input vector which corresponds to coming to the equilibrium point in (1). If the condition (23) can be set up, then substitute formulas (22) and (23) into (21); the state error model can be obtained bẏ According to (16) and (17), the above formula can be written as (18), so the feedback control can be obtained by We define * and * , respectively, referring to the desired surge speed and yaw angle; then the state [ , V, , ] is globally uniformly asymptotically convergent to [ * , 0, 0, * ] [22]. So the equilibrium point of * will be [ 11 * , 0, 0] . When the system is coming to the equilibrium point, from Substituting (16), (20), and (26) into the above formula (25), the energy controller becomes Let 13 = 22 V, 23 = − 11 , and 12 = 33 ; then the energy controller further becomes As to the controller development of heading, the state error method is taken as the feedback control law for the yaw subsystem. The yaw subsystem of motion is given by (1) and (2) can be rewritten as Which is typical of cascade control system. If the heading tracking error is defined as where * is the desired heading angle, and the selected linearization control is in the form oḟ=̇ * − , then the heading tracking error dynamics will bė When the values of the surge velocity gain are positive, the error dynamics for would remain stable. The error dynamics controller will be 2 = − . Combining the equation mentioned before with the third formula of (28), then the heading controller is It is obvious that the surge velocity always couples with yaw rate, as a result of which it is impossible to control either one of them independently. Checking the sway velocity subsystem which is the second formula of (1), By using the control law of heading angle and surge speed, we can see that the sway subsystem in (33) reduces to When the time → ∞, finally the sway velocity always has V → 0. That means the sway velocity V is exponentially stable for the case oḟ * = 0.

Mathematical Problems in Engineering 5
From all the above analysis of the controller, the combined surge speed and heading controller are taken as (35)

The Stability of the Controller
We will analyze the stability of the energy controller in this section. Liao et al. [22] pointed that the combined surge speed and heading control laws make the state [ , V, , ] globally uniformly asymptotically convergent to [ * , 0, 0, * ] and bounded. Here we consider the Lyapunov function of the close-loop system defined as We can compute the time derivative of with respect to time along the solutions of the close-loop systeṁ As (̃) is the skew-symmetric matrix, we can obtain According to (̃) being positive semidefinite symmetric matrix, hence Obviously, we can get is positive definite anḋis negative semidefinite. By using the Lyapunov stability theory, the closed-loop system establishes stability. So [ * , 0, 0, * ] is taken as the equilibrium of the closed-loop system. Additionally, if the largest invariant set of system equals {0}, the system is asymptotically stable. From (14), we can get that the structures of the matrices ( ) and ( ) are maintained if the parameters are with uncertain displacement and drag. ( ) is also positive semidefinite symmetric matrix, and ( ) = − ( ). ( ) is also positive semidefinite symmetric matrix, and ( ) = ( ) ≥ 0. Because the interconnection and damping structures of the system remain unchanged, the stability of the system is also asymptotically stable.

System Simulation
The numerical simulation is performed by MATLAB/ Simulink. USV for modeling has a length of 1.2 m and a mass From the stability analysis of the controller, the parameters should be 1 , 3 ≥ 0, ≥ 0. And the controller parameters can be ranged through the spectrum. Figure 2 gives the surge speed responses of different damping parameters ( 1 = 100, 1 = 2000, and 1 = 4000). From Figure 2, we can know that the surge speed response has better performance when 1 = 2000. Figure 3 gives the heading angle responses of different parameters ( 3 = 100, = 500, 3 = 500, = 2000, 3 = 1000, and = 3000). We can know that the heading angle response has better performance when  Figure 8 shows that the state [ , V, , ] is globally uniformly asymptotically convergent to [ * , 0, 0, * ] . When the system is coming to the equilibrium point, the sway velocity V and the yaw rate always tend to Zero.
We use the classical PID speed and heading controller which is shown in Figure 9 to compare with the proposed control approach. Figure 10 gives the surge speed responses of different parameters ( = 500, = 10, = 100; = 1000, = 10, = 100; = 5000, = 10, and = 100). We can know that the surge speed response has better performance when = 5000, = 10, and = 100. disturbances Δ = 10 N and Δ = 10Nm are added separately to the different controllers at = 10 s. From Figures  14 and 15, we can see that the state error PCH controller has load disturbances attenuation performance similar to the classical PID controller.
The above simulation results show that the proposed control approach has good performance in dynamic and steady state. From the simulation results shown above, the proposed control method could to some extent achieve a better performance for the signal tracking of the given speed and heading angle.

Conclusions
In this article, the state error port-controlled Hamiltonian theory has been discussed. A novel controller based on state error port-controlled Hamiltonian approach is proposed in this paper for speed and heading angle tracking control of  underactuated USV. The desired state error port-controlled Hamiltonian structure is assigned to the closed-loop USV system which based on interconnection assignment and damping injection method. To realize the overall stability of the control, Lyapunov theory and La Salle's invariance principle are introduced to improve the clearance of physical meanings. Simulation results confirm the validity and stability of control algorithm. Compared with the classical PID controller, the designed controller has similar tracking and load disturbances attenuation performances. The designed controller has good steady state performance and simple structure. And the proposed controller provides an effective approach to analyze stability of the closed-loop system. The high simplification and portability of the controller make it a candidate choice for vast application in various region.

Conflicts of Interest
The authors declare that they have no conflicts of interest.