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

Authors: Chengxing Lv; Haisheng Yu; Zhili Hua; Lei Li; Jieru Chi
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

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.


---

## Body

## 1. 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 [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 port-controlled Hamiltonian (PCH) theory and interconnection and damping assignment (IDA–PBC) method [12–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.


## 2. PCH Model of Unmanned Surface Vehicles

### 2.1. The Model of USV

The structure of USV is shown in Figure1. 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 under-actuated. 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 Z-axis. In physical design, USV is set to be port-starboard symmetrical; hence surge subsystem and sway-yaw subsystem are essentially decoupled [2].Figure 1
The motion coordinate system for USV.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–21] of USV can be obtained as(1)m11u˙-m22vr+d11u=fp,m22v˙+m11ur+d22v=0,m33r˙+m22-m11uv+d33r=Ts.The Kinematics model of heading subsystem of USV can be obtained as(2)ψ˙=r,where u is surge velocity, v is sway velocity, and r is yaw rate in body fixed reference frame. mii are inertia coefficients of USV including mass effects added, dii are hydrodynamic damping coefficients in conditions of surge, sway, and yaw, fp are the forces of propulsion system, and Ts is the steering torque. ψ denotes orientation angle of the vessel.Then (1) can be expressed in matrix form:(3)Mυ˙+Cυυ+Dυ=us.Here υ=u,v,rT, us=fp,0,TsT, M=diagm11,m22,m33Cυ=00-m22v00m11um22v-m11u0, and D=diagd11,d22,d33.M is an inertia parameters matrix including the added body mass, Cυ is the so-called Coriolis and centripetal matrix, and D is hydrodynamic damping matrix.The thrust forcesfp and steering torque Ts are functions of the two surge control thrust forces which are from each propeller:(4)fp=f1+f2,Ts=Bf1-f22,where f1 is the thrust force which is produced by the first motor and f2 is the thrust force which is produced by the second motor. B is the distance between the propellers. From (4), the thrust allocated to each propeller, f1 and f2, can be calculated as(5)f1=fp2+TsB,f2=fp2-TsB.


### 2.2. PCH Systems

The general form for a nonlinear dynamical system can be shown as follows:(6)x˙=fx+gxus,y=hx,where x∈Rn is the state vector, y∈Rm is the output vector, and us∈Rm is the input vector. From [13, 14] we can see that if there is possible way to find a nonnegative function V(x)(V(0)=0) such that(7)Vxt-Vx0≤∫0tyTτusτdτthe system described by (6) is passive. Then, the PCH system with dissipation can be described as follows [14]:(8)x˙=Jx-Dx∂Hx∂x+gxus,y=gTx∂Hx∂x,where Dx is positive semidefinite symmetric matrix and Dx=DTx≥0. It represents the dissipation of the system. The interconnection structure of the system is represented by the skew-symmetric matrix Jx=-JTx and matrix gx. Hx 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:(9)dHxdt=∂Hx∂xTx˙=yTus-∂Hx∂xTDx∂Hx∂x≤yTus.In time-interval 0,t, (9) establishes the passivity properties of the PCH system. which is the same as (7):(10)Hxt-Hx0≤∫0tyTτusτdτ.


### 2.3. 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:(11)x=x1x2x3=m11um22vm33r,us=u1u2u3=fp0Ts.The Hamiltonian function of the USV dynamic system can be defined as(12)Hx=12xTM-1x=12m11u2+m22v2+m33r2.Combine (3), (8), and (12); then the PCH model of USV dynamic system can be obtained as follows:(13)x˙=00m22v00-m11u-m22vm11u0-d11000d22000d33∂H∂x+100010001us,where(14)Jx=00m22v00-m11u-m22vm11u0,Dx=d11000d22000d33,gx=100010001,(15)∂H∂x=M-1x=uvr.


## 2.1. The Model of USV

The structure of USV is shown in Figure1. 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 under-actuated. 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 Z-axis. In physical design, USV is set to be port-starboard symmetrical; hence surge subsystem and sway-yaw subsystem are essentially decoupled [2].Figure 1
The motion coordinate system for USV.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–21] of USV can be obtained as(1)m11u˙-m22vr+d11u=fp,m22v˙+m11ur+d22v=0,m33r˙+m22-m11uv+d33r=Ts.The Kinematics model of heading subsystem of USV can be obtained as(2)ψ˙=r,where u is surge velocity, v is sway velocity, and r is yaw rate in body fixed reference frame. mii are inertia coefficients of USV including mass effects added, dii are hydrodynamic damping coefficients in conditions of surge, sway, and yaw, fp are the forces of propulsion system, and Ts is the steering torque. ψ denotes orientation angle of the vessel.Then (1) can be expressed in matrix form:(3)Mυ˙+Cυυ+Dυ=us.Here υ=u,v,rT, us=fp,0,TsT, M=diagm11,m22,m33Cυ=00-m22v00m11um22v-m11u0, and D=diagd11,d22,d33.M is an inertia parameters matrix including the added body mass, Cυ is the so-called Coriolis and centripetal matrix, and D is hydrodynamic damping matrix.The thrust forcesfp and steering torque Ts are functions of the two surge control thrust forces which are from each propeller:(4)fp=f1+f2,Ts=Bf1-f22,where f1 is the thrust force which is produced by the first motor and f2 is the thrust force which is produced by the second motor. B is the distance between the propellers. From (4), the thrust allocated to each propeller, f1 and f2, can be calculated as(5)f1=fp2+TsB,f2=fp2-TsB.


## 2.2. PCH Systems

The general form for a nonlinear dynamical system can be shown as follows:(6)x˙=fx+gxus,y=hx,where x∈Rn is the state vector, y∈Rm is the output vector, and us∈Rm is the input vector. From [13, 14] we can see that if there is possible way to find a nonnegative function V(x)(V(0)=0) such that(7)Vxt-Vx0≤∫0tyTτusτdτthe system described by (6) is passive. Then, the PCH system with dissipation can be described as follows [14]:(8)x˙=Jx-Dx∂Hx∂x+gxus,y=gTx∂Hx∂x,where Dx is positive semidefinite symmetric matrix and Dx=DTx≥0. It represents the dissipation of the system. The interconnection structure of the system is represented by the skew-symmetric matrix Jx=-JTx and matrix gx. Hx 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:(9)dHxdt=∂Hx∂xTx˙=yTus-∂Hx∂xTDx∂Hx∂x≤yTus.In time-interval 0,t, (9) establishes the passivity properties of the PCH system. which is the same as (7):(10)Hxt-Hx0≤∫0tyTτusτdτ.


## 2.3. 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:(11)x=x1x2x3=m11um22vm33r,us=u1u2u3=fp0Ts.The Hamiltonian function of the USV dynamic system can be defined as(12)Hx=12xTM-1x=12m11u2+m22v2+m33r2.Combine (3), (8), and (12); then the PCH model of USV dynamic system can be obtained as follows:(13)x˙=00m22v00-m11u-m22vm11u0-d11000d22000d33∂H∂x+100010001us,where(14)Jx=00m22v00-m11u-m22vm11u0,Dx=d11000d22000d33,gx=100010001,(15)∂H∂x=M-1x=uvr.


## 3. 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.Assumingx∗ is a desired equilibrium, then the state error will be x~=x-x∗. The final objective of IDA–PBC [12] is to find β(x), Ja, Da matching the condition(16)Jdx~=Jx~+Ja=-JdTx~,Ddx~=Dx~+Da=DdTx~≥0,(17)us=βx.Then the closed-loop system (8) follows a state error PCH form:(18)x~˙=Jdx~-Ddx~∂Hd∂x~.The desired Hamilton function is chosen as(19)Hdx~=12m11u-u∗2+m22v2+m33r2.Then we choose(20)Ja=0-J12J13J120J23-J13-J230,Da=da1000da2000da3,where J12, J13, J23, and da1, da2, da3 are the designed parameters.Consider the closed-loop system (18) with feedback control by (17); substituting x=x~+x∗ into PCH system (8), we get(21)x~˙=Jx~+x∗-DM-1x~+x∗+gx~+x∗βx-x˙∗.In USV PCH structure model, we could get the hydrodynamic damping matrix as constant matrix. So we used the symbol D to represent the symbol Dx.From the PCH system (8), we can derive(22)x˙∗=Jx∗-DM-1x∗+gx∗us∗,where us∗ is the input vector which corresponds to coming to the equilibrium point in (1).If the condition (23) can be set up,(23)Jx~+x∗=Jx~+Jx∗,then substitute formulas (22) and (23) into (21); the state error model can be obtained by(24)x~˙=Jx~-DM-1x~+Jx∗M-1x~+Jx~M-1x∗+gx~+x∗βx-gx∗us∗.According to (16) and (17), the above formula can be written as (18), so the feedback control can be obtained by(25)gxβx=Ja-Da-Jx∗M-1x~-Jx~M-1x∗+gx∗us∗.We define u∗ and ψ∗, respectively, referring to the desired surge speed and yaw angle; then the state u,v,r,ψT is globally uniformly asymptotically convergent to u∗,0,0,ψ∗T [22]. So the equilibrium point of x∗ will be m11u∗,0,0T. When the system is coming to the equilibrium point, from (1) we can obtain(26)us∗=d11u∗00.Substituting (16), (20), and (26) into the above formula (25), the energy controller becomes(27)fp=J13r-J12v-da1u-u∗+d11u∗,0=J12u-u∗+J23r-da2v+m11u∗r,Ts1=u∗m22v-J23+m11u∗v-J13u-u∗-da3r.Let J13=m22v, J23=-m11u, and J12=m33r; then the energy controller further becomes(28)fp=m22-m33vr-da1u-u∗+d11u∗,0=m33-m11u-u∗r-da2v,Ts=m11-m22u-u∗v+m22vu∗-da3r.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(29)m33r˙+m22-m11uv+d33r=Ts,ψ˙=r.Which is typical of cascade control system. If the heading tracking error is defined as(30)eψ=ψ-ψ∗,where ψ∗ is the desired heading angle, and the selected linearization control is in the form of ψ˙=ψ˙∗-kψeψ, then the heading tracking error dynamics will be(31)e˙ψ+kψeψ=0.When the values of the surge velocity gain kψ are positive, the error dynamics for eψ would remain stable. The error dynamics controller will be Ts2=-kψeψ. Combining the equation mentioned before with the third formula of (28), then the heading controller is(32)Ts=Ts1+Ts2=m11-m22u-u∗v+m22vu∗-da3r-kψeψ.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),(33)m22v˙+m11ur+d22v=0.By using the control law of heading angle and surge speed, we can see that the sway subsystem in (33) reduces to(34)m22v˙=-m11u∗ψ˙∗-d22v.When the time t→∞, finally the sway velocity always has v→0. That means the sway velocity v is exponentially stable for the case of ψ˙∗=0.From all the above analysis of the controller, the combined surge speed and heading controller are taken as(35)fp=m22-m33vr-da1u-u∗+d11u∗,Ts=m11-m22u-u∗v+m22vu∗-da3r-kψeψ.


## 4. 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 u,v,r,ψT globally uniformly asymptotically convergent to u∗,0,0,ψ∗T and bounded. Here we consider the Lyapunov function of the close-loop system defined as(36)Ve=Hdx~+12kψeψ2.We can compute the time derivative of Ve with respect to time along the solutions of the close-loop system(37)V˙e=dHdx~dt+kψeψe˙ψ=∂Hdx~∂x~Tx~˙-kψ2eψ2=∂Hdx~∂x~TJdx~-Ddx~∂Hd∂x~-kψ2eψ2.As Jdx~ is the skew-symmetric matrix, we can obtain(38)∂Hdx~∂x~TJdx~∂Hd∂x~=0.According to Ddx~ being positive semidefinite symmetric matrix, hence(39)-∂Hdx~∂x~TDdx~∂Hd∂x~≤0.Obviously, we can get Ve is positive definite and V˙e is negative semidefinite. By using the Lyapunov stability theory, the closed-loop system establishes stability. So u∗,0,0,ψ∗T 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 Jx and Dx are maintained if the parameters are with uncertain displacement and drag. Jx is also positive semidefinite symmetric matrix, and Jx=-JTx. Dx is also positive semidefinite symmetric matrix, and Dx=DTx≥0. Because the interconnection and damping structures of the system remain unchanged, the stability of the system is also asymptotically stable.


## 5. System Simulation

The numerical simulation is performed by MATLAB/Simulink. USV for modeling has a length of 1.2 m and a mass of 17.5 kg, and two DC motors were equipped for providing surge force and yaw moment by driving two propellers. The ship model’s parameters are calculated [23]:(40)m11=25.8kg,m22=33.8kg,m33=2.76kg·m2,r11=12kg/s,r22=17kg/s,r33=0.5kg·m2/s.From the stability analysis of the controller, the parameters should be da1,da3≥0, kψ≥0. And the controller parameters can be ranged through the spectrum. Figure 2 gives the surge speed responses of different damping parameters (da1=100, da1=2000, and da1=4000). From Figure 2, we can know that the surge speed response has better performance when da1=2000. Figure 3 gives the heading angle responses of different parameters (da3=100, kψ=500, da3=500, kψ=2000, da3=1000, and kψ=3000). We can know that the heading angle response has better performance when da3=500, kψ=2000. So the design parameters are chosen as da1=2000, da3=500, and kψ=2000. The desired speed and heading references are u∗,0,0,ψ∗T=1,0,0,0.5T. At the moment of t=10 s, load disturbances Δξfp=10N and ΔξTs=10Nm are added separately to the system, and duration of the disturbances added to the system is 1 s. Dynamic response and disturbance attenuation of the control system are, respectively, shown in Figures 4 and 5, from which the satisfactory results are obtained by methods mentioned in Section 3. At the moment of t=10 s, the desired surge speed and heading angle are set to u∗,0,0,ψ∗T=1.3,0,0,1T, respectively. Figures 6 and 7 are shown where the proposed control approach has extremely quick tracking performance.Figure 2
Surge speed curves with different parameters.Figure 3
Heading angle curves with different parameters.Figure 4
Surge speed curve of USV.Figure 5
Heading angle curve of USV.Figure 6
Surge speed tracking curve.Figure 7
Heading angle tracking curve.Figure8 shows that the state u,v,r,ψT is globally uniformly asymptotically convergent to u∗,0,0,ψ∗T. When the system is coming to the equilibrium point, the sway velocity v and the yaw rate r always tend to Zero.Figure 8
Sway velocity and yaw rate curve of USV when using the proposed controller.We use the classical PID speed and heading controller which is shown in Figure9 to compare with the proposed control approach. Figure 10 gives the surge speed responses of different parameters (kp=500, ki=10, kd=100; kp=1000, ki=10, kd=100; kp=5000, ki=10, and kd=100). We can know that the surge speed response has better performance when kp=5000, ki=10, and kd=100. Figure 11 gives heading angle responses of different parameters (kp=200, ki=0, kd=100; kp=1000, ki=0, kd=300; kp=2000, ki=0, and kd=500). We can know that the heading angle response has better performance when kp=2000, ki=0, and kd=500. Figures 12 and 13 are shown where the proposed control approach has tracking performance similar to the classical PID approach. Figures 14 and 15 show the surge speed and heading angle responses when the load disturbances Δξfp=10N and ΔξTs=10Nm are added separately to the different controllers at t=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.Figure 9
Structure of the classical PID controller.Figure 10
Surge speed curves with different parameters (classical PID controller).Figure 11
Heading angle curves with different parameters (classical PID controller).Figure 12
Surge Speed tracking curves when using different controllers.Figure 13
Heading angle tracking curves when using different controllers.Figure 14
Surge speed response when using different controllers.Figure 15
Heading angle response when using different controllers.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.


## 6. 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.



---

*Source: 7371829-2018-01-18.xml*