# Control Algorithm for Trajectory Tracking of an Underactuated USV under Multiple Constraints

Authors: Hongbin Wang; Jiao Dong; Zhikun Liu; Li Yan; Shiqi Wang
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

A trajectory tracking control method based on the cascade of optimal guidance and adaptive control laws was proposed for trajectory tracking of an unmanned surface vehicle under multiple motion constraints including an ocean current disturbance environment. Considering the velocity and acceleration constraints, angular velocity, and angular acceleration constraints in guidance law design, motion planning was conducted with three degrees of freedom using a motion model and predictive control, while optimal guidance law was designed through a rolling optimization strategy to stabilize the position error under multiple constraints. Moreover, the stability of the guidance law was proven. In control law design, an adaptive observer was introduced to estimate and compensate for ocean current disturbance, which affected the precision of control. The course and velocity control laws were designed with the adaptive sliding mode control technology to stabilize the course angle and longitudinal velocity errors; the stability of these control laws was further proven. Based on the system design, the stability of the entire closed-loop control system was proven with the cascade system and Lyapunov theory. Theoretical analysis and simulation experiments demonstrated the effectiveness and advancement of the proposed algorithm.


---

## Body

## 1. Introduction

An unmanned surface vehicle (USV) is an autonomous and controllable unmanned platform used on water. With advantages such as high speed, flexibility, stealth, long endurance, and good resistance to extreme conditions, a USV will have a promising future in military and civilian applications [1–3].Trajectory tracking means that a USV under the effect of a control system departs from any initial position, sails via an expected trajectory, and then tracks the expected trajectory points with a minimum of error at any given time [4]. Most USVs have no direct transverse driving force but rely only on the forward thrust and steering torque to control its navigation, so that they form a typical underactuated system [5–7]. Meanwhile, it is quite difficult to devise a control system because of the constrained motion of a USV and the complexity of the ocean environment [8–11]. Therefore, the accurate trajectory tracking of underactuated USVs in an ocean environment has become a hot topic in research related to USV control.Presently, research on USV tracking control problems can be divided into three categories: path tracking that ignores time and motion performance constraints, trajectory tracking that only considers time constraints, and trajectory tracking that considers both time and motion performance constraints. Ignoring time and motion performance constraints, the authors of [12–15] mainly study the path tracking control problem. The authors of [12] used LOS guidance and feedback linearization to design a cascade control system based on a USV three-DOF (degree of freedom) asymmetric model while ignoring external impacts. They also proved the usefulness of having uniform semiglobal exponential stability of the control system, and its effective implementation of a USV path following control. Assuming that ocean currents were known, the authors of [13] presented the compensation for the influence of ocean currents by adding the longitudinal error and putting forward an integral LOS guidance law. Moreover, a controller was devised based on feedback linearization to achieve a chosen path following control under the disturbance of ocean currents. The study also demonstrated the uniform global k-exponential stability of the control system. With the assumption of bounded and slowly varying external disturbance, Fossen et al. used tracking error feedback to estimate and compensate for the drifting angle caused by disturbance such as ocean currents in a real-time manner. It was also proven that the control system and disturbance observation system were uniformly semi-globally exponentially stable, so that real-time and accurate USV trajectory tracking capability was further enhanced [14]. In the study of [15], a disturbance observer was also designed to estimate the influence of wind, wave, and current in a real-time manner. In the meantime, a sliding mode controller was designed based on USV three-DOF kinematics and a Norrbin model to prove the uniform global asymptotic stability of the control system. Focusing on the influence of time factors, the literature [16–18] studied the trajectory tracking control under the time constraint. K.D.DO set a virtual ship and made trajectory tracking equivalent to the dynamic positioning of a real ship following the guidance of the virtual ship. The reaching law for the real ship toward the virtual ship was devised to realize the USV trajectory following control and prove the uniform global asymptotic stability of the entire system [16]. This approach had the advantage in guaranteeing a small tracking error for each state quantity, but its application was limited since the trajectory was generated by the virtual ship based on the same model and relied on a very highly accurate model. In the study of [17], trajectory points were converted into a number of path points with a specific time, so that trajectory tracking was transformed into the tracking of virtual points. Moreover, the Serret–Frenet coordinate frame was used to define the transverse and longitudinal tracking errors, and devise the transverse guidance law and longitudinal LOS guidance law, respectively. In the meantime, the disturbance caused by ocean currents was observed and compensated in a real-time manner to achieve the effective trajectory tracking. This method was highly practical and less model-dependent. Nevertheless, it ignored the coupling between control loops and designed the motion planning to two single-input and single-output systems. Moreover, the influence of ship maneuverability was also not taken into account. While trajectory tracking was also regarded as the tracking of virtual points in the study of [18], an underactuated USV neural network adaptive tracking control strategy was put forward. A nonlinear controller was designed by means of back-stepping, while the neural network adaptive method was used to approximate the USV system uncertainty function. The trajectory tracking control was effectively achieved in the study, but it did not take into account the influence of system coupling and motion constraints. Considering both time and motion performance constraints, in the study of [19], the control constraints and control increment constraints were imposed. On this basis, a predictive control model was employed to transform trajectory tracking into solving the rolling optimization under multiple conditions, so as to effectively implement the USV trajectory tracking control. However, this approach required the approximate linearization of the USV kinematics strongly nonlinear model, which caused a very large modeling error. Additionally, the stability of the designed controller was not analyzed in the reference. After comprehensively analyzing the algorithms proposed in the existing studies on USV trajectory tracking, it is found that some studies overlooked the influence of time and the constraints of ships. Moreover, some studies were highly model-dependent, less practical, and lacked system performance and stability analysis.In this paper, the abovementioned problems are fully considered, and a trajectory tracking control system with strong versatility and high stability is comprehensively designed on the basis of considering the time factor and the constraints of USV motion performance. The structure of the paper is as follows: the trajectory tracking control problem is modeled in Section1, and the control objectives for trajectory tracking are given. In Section 2, a trajectory tracking cascade control system based on optimal steering law and adaptive sliding mode control law is designed. Section 3 analyzes the stability of the entire trajectory tracking control system. In Section 4, simulation experiments and analysis are carried out. Section 5 summarizes the work of this paper.


## 2. USV Modeling and Trajectory Tracking

### 2.1. USV Motion Modeling

The influence of steady state disturbance factors including wind, wave, and current was taken into account when building a three-DOF kinetics and kinematics model for an underactuated USV in the horizontal plane [20]:(1)η˙=Jψυ,MRBυ˙+CRBυυ+MAυ˙r+CAυrυr+Dυrυr=Bf,where η=x,y,ψT indicates the position and course angle of the USV in the geodetic coordinate system i ; υ=u,v,rT represents the absolute longitudinal velocity, transverse velocity, and course angular velocity of the USV in the geodetic coordinate system i; Jψ is the transformation matrix from the hull coordinate system to the geodetic coordinate system; MRB and MA are the rigid body mass inertia matrix and hydrodynamic added mass matrix, respectively; CRBυr and CAυr are the rigid body and hydrodynamic-added Coriolis force and centripetal force matrix, respectively; υr=ur,vr,rT stands for the longitudinal, transverse, and course angular velocities of the USV relative to the wind, wave, and current in the hull coordinate system {b}; D(vr) is the damping coefficient matrix; f=Tu,TrT is the control input matrix with Tu and Tr representing the longitudinal thrust and rudder angle, respectively; and B is the control input configuration matrix. The specific expressions are defined as follows:(2)B=b1100b220b32,Dυr=d11+d11qur000d22d230d32d33,Cxy=00−m22xy2−m23xy300m11xy1m22xy2+m23xy3−m11xy10,Mx=m11x000m22xm23x0m32xm33x,Jψ=cosψ−sinψ0sinψcosψ00          01.For the convenience of system design, coordinate transformation was carried out to transform the origin of the hull coordinate system from its center of gravity to the pivot point [21]. The steering torque and transverse velocity were decoupled to obtain the following:(3)r˙=Fru,v,r+ΦrTu,v,r,ψθr+τr,v˙=Xur,ucr+Yurvr,u˙=Fuu,v,r+ΦuTψ,rθu+τu,ψ˙=r,y˙=usinψ+vcosψ,x˙=ucosψ−vsinψ,where Fuu,v,r, Φuψ,r, θu, θr, Xur,uc , and the expression of Fru,v,r can be found in reference [20]. As revealed in equation (11), the expression of v˙ does not contain the direct driving force after the coordinate transformation.


### 2.2. Trajectory Tracking

It is given that the expected trajectory is a continuous curve formed by a number of path points under time constraints in the geodetic coordinate system. The USV departs from any point, arrives at the expected trajectory, and sails along the expected trajectory with the minimum tracking error at the given value. The trajectory tracking model is shown in Figure1.Figure 1
Trajectory tracking of USV.The trajectory tracking control target was defined as follows:(4)limt⟶∞ψet=limt⟶∞ψt−ψdt=0,limt⟶∞yet=limt⟶∞yt−ydt=0,limt⟶∞xet=limt⟶∞xt−xdt=0,where xdt,ydt, and ψdt are the expected trajectory point coordinates and expected course angle at the time t, respectively.In order to realize the control target of trajectory tracking, the USV trajectory tracking control system is designed now, and its block diagram is shown in Figure2.Figure 2
Block diagram of the USV trajectory tracking control system.The trajectory tracking control system is designed as a cascaded form of guidance subsystem, heading, and speed control subsystems. The guidance subsystem gives the desired heading and desired speed through the motion planner according to the given desired trajectory, motion constraints, optimization objectives, and the real-time state information of the USV; the heading and speed control subsystem tracks the desired heading given by the guidance subsystem. Moreover, the expected speed and real-time compensation for external interference and modeling errors, so as to achieve accurate tracking of the USV's trajectory.


## 2.1. USV Motion Modeling

The influence of steady state disturbance factors including wind, wave, and current was taken into account when building a three-DOF kinetics and kinematics model for an underactuated USV in the horizontal plane [20]:(1)η˙=Jψυ,MRBυ˙+CRBυυ+MAυ˙r+CAυrυr+Dυrυr=Bf,where η=x,y,ψT indicates the position and course angle of the USV in the geodetic coordinate system i ; υ=u,v,rT represents the absolute longitudinal velocity, transverse velocity, and course angular velocity of the USV in the geodetic coordinate system i; Jψ is the transformation matrix from the hull coordinate system to the geodetic coordinate system; MRB and MA are the rigid body mass inertia matrix and hydrodynamic added mass matrix, respectively; CRBυr and CAυr are the rigid body and hydrodynamic-added Coriolis force and centripetal force matrix, respectively; υr=ur,vr,rT stands for the longitudinal, transverse, and course angular velocities of the USV relative to the wind, wave, and current in the hull coordinate system {b}; D(vr) is the damping coefficient matrix; f=Tu,TrT is the control input matrix with Tu and Tr representing the longitudinal thrust and rudder angle, respectively; and B is the control input configuration matrix. The specific expressions are defined as follows:(2)B=b1100b220b32,Dυr=d11+d11qur000d22d230d32d33,Cxy=00−m22xy2−m23xy300m11xy1m22xy2+m23xy3−m11xy10,Mx=m11x000m22xm23x0m32xm33x,Jψ=cosψ−sinψ0sinψcosψ00          01.For the convenience of system design, coordinate transformation was carried out to transform the origin of the hull coordinate system from its center of gravity to the pivot point [21]. The steering torque and transverse velocity were decoupled to obtain the following:(3)r˙=Fru,v,r+ΦrTu,v,r,ψθr+τr,v˙=Xur,ucr+Yurvr,u˙=Fuu,v,r+ΦuTψ,rθu+τu,ψ˙=r,y˙=usinψ+vcosψ,x˙=ucosψ−vsinψ,where Fuu,v,r, Φuψ,r, θu, θr, Xur,uc , and the expression of Fru,v,r can be found in reference [20]. As revealed in equation (11), the expression of v˙ does not contain the direct driving force after the coordinate transformation.


## 2.2. Trajectory Tracking

It is given that the expected trajectory is a continuous curve formed by a number of path points under time constraints in the geodetic coordinate system. The USV departs from any point, arrives at the expected trajectory, and sails along the expected trajectory with the minimum tracking error at the given value. The trajectory tracking model is shown in Figure1.Figure 1
Trajectory tracking of USV.The trajectory tracking control target was defined as follows:(4)limt⟶∞ψet=limt⟶∞ψt−ψdt=0,limt⟶∞yet=limt⟶∞yt−ydt=0,limt⟶∞xet=limt⟶∞xt−xdt=0,where xdt,ydt, and ψdt are the expected trajectory point coordinates and expected course angle at the time t, respectively.In order to realize the control target of trajectory tracking, the USV trajectory tracking control system is designed now, and its block diagram is shown in Figure2.Figure 2
Block diagram of the USV trajectory tracking control system.The trajectory tracking control system is designed as a cascaded form of guidance subsystem, heading, and speed control subsystems. The guidance subsystem gives the desired heading and desired speed through the motion planner according to the given desired trajectory, motion constraints, optimization objectives, and the real-time state information of the USV; the heading and speed control subsystem tracks the desired heading given by the guidance subsystem. Moreover, the expected speed and real-time compensation for external interference and modeling errors, so as to achieve accurate tracking of the USV's trajectory.


## 3. Trajectory Tracking Control Algorithm

### 3.1. Design of the Optimal Guidance Law

An underactuated USV is not subject to a transverse nonintegrable acceleration constraint. As shown in its kinematics model, the transverse acceleration of the USV is a function of its longitudinal, transverse, and angular velocities, so that it is strongly nonlinear. Based on the cascade system theory, a guidance law is designed to restrict the transverse acceleration of the USV by imposing the angular velocity and angular acceleration constraints. The restriction caused by the velocity and acceleration constraints over the USV motion is fully considered to determine the optimal expected course and velocity with the rolling optimization strategy.According to the observations of [22, 23], the kinematics equation of the USV at the small angular velocity is rewritten into the following:(5)x˙=Ucosψy˙=Usinψψ˙=rU∈UcU˙∈Uacr∈rcr˙∈rac,where the variable U is the resultant velocity of the USV, and U≈u; U˙ is the acceleration; r˙ is the angular acceleration; Uc, Uac, rc, and rac stand for the USV velocity constraint, acceleration constraint, angular velocity constraint, and angular acceleration constraint, respectively. The kinematics model in (5) is written into the following state equation:(6)x˙t=fxt,τt,yt=Cxt,where xt=x,y,ψT is the state variable; τt=U,rT is the control input variable; yt is the output quantity, and C=I3×3.


#### 3.1.1. Modeling

The expected trajectory equation is set as follows:(7)x˙r=fxr,τr.The nonlinear system (6) is linearized by the Taylor expansion at any point xr,τr with the higher-order items ignored to obtain the following:(8)x˙=fxr,τr+∂f∂x|x=xrτ=τrx−xr+∂f∂τ|x=xrτ=τrτ−τr.Equations(7) and (8) are subtracted to obtain the state error system:(9)∑1:x˜˙=Atx˜+Btτ˜,where At=∂f/∂x|x=xpτ=τp and Bt=∂f/∂τ|x=xpτ=τp .The above state (9) is discretized. Let(10)Ad=I+TAt,Bd=TBt,where T is the discrete step length. The following equation is obtained:(11)x˜k+1=Adx˜k+Bdτ˜k,y˜k=Cx˜k,where y˜=yk−yrk.Considering the acceleration constraint, the control variation increment is defined asΔτk=τk−τk−1, ζk=x˜kτ˜k−1T to obtain the state space expression as follows:(12)ζk+1=A¯dζk+B¯dΔτk,y˜k=C¯ζk,where A¯d=AdBd02×3I2×2 ， B¯d=BdI2×2 , and C¯=C02×3.Considering the motion planning and motion constraints, the objective function is designed using the system state variable error, input control, and control increment to obtain the optimal expected value of the USV motion. The objective function is expressed by the following:(13)Jσ,ζ=ζN′PζN+∑k=0N−1ζk′Qζk+Δτk′RΔτk,(14)τmin≤τk≤τmax,k=0,....,N−1,(15)Δτmin≤Δτk≤Δτmax,k=0,....,N−1,where ξ=x˜tτ˜tT; N is the estimation length of rolling optimization; P, Q, and R are the weight coefficient matrixes, and their diagonal elements are greater than 0; σ=Δτ0Δτ1...ΔτN−1T. Here, P, Q, and R are the terminal constraint penalty, the USV’s capability of tracking the expected trajectory, and the demand for the stable variation of control quantity and control increment.


#### 3.1.2. Solution of Optimization

The optimization defined in (13)–(15) is converted into secondary planning.(12) can be rewritten as follows:(16)ζk=A¯dkζ+∑j=0k−1A¯kjB¯dΔτk−1−j.Thus, (13) can be rewritten as follows:(17)Jσ,ζ=ζ′Qζ+χ′Q¯χ+σ′R¯σwhere χ=ζ1ζ2⋅⋅⋅ζNT , Q¯=diagQ,Q,…Q︸N−1,P, R¯=diagR,R,…R︸N , χ=ϕσ+Εζ , ϕ=B¯d0⋯0A¯dB¯dB¯d⋯0⋮⋮⋱⋮A¯dN−1B¯dA¯dN−2B¯d⋯B¯d, and Ε=A¯dA¯d2⋯A¯dNT .(17) is further changed to the following:(18)Jσ,ζ=ϕσ+Εζ'Q¯ϕσ+Εζ+σ′R¯σ+ζ′Qζ=σ′R¯+ϕ′Q¯ϕσ+ζ′Ε′Q¯ϕσ+ζ′Q+Ε′Q¯Εζ=12σ′Φσ+ζ′Ψσ+12ζ′ϒζ,where Φ=2R¯+ϕ′Q¯ϕ , Ψ=2Ε′Q¯Ε, and ϒ=2Q+Ε′Q¯Ε.(14) and (15) for constraints are written into the following(19)Gσ¯≤Τ,where G=IN−ININ−INT , σ¯=τ0τ1...τN−1σT , T=T1T2T , T1=τmax⋯τmax︸N−τmin⋯−τmin︸N , and T2=Δτmax⋯Δτmax︸N−Δτmin⋯−Δτmin︸N.Above all, optimization is transformed into the secondary planning under multiple constraints as shown in (20):(20)minσ12σ′Φσ+ζ′ΨσGσ¯≤Τ.SinceΦ≥0 , secondary planning is solvable for any nonnegative weighting matrixes P, Q, and R.


### 3.2. Design of Course and Velocity Control Laws

Sliding mode control has advantages such as fast convergence and strong robustness [24]. Therefore, the adaptive sliding mode control was adopted in this section to devise the course and velocity control laws, as well as to estimate and compensate for the influence of ocean currents. Moreover, course and velocity were controlled for convergence to the expected values given by the guidance laws.The dynamic equation for the tracking errors of velocity, course and angular velocity is given as follows:(21)∑2:u˜˙ψ˜˙ψ˜¨=Fuu,v,r+ΦuTψ,rθu+τu−u˙dr−ψ˙dFru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d.First, the course tracking control law was designed, and the sliding mode plane was selected as follows:(22)s1=κ1ψ˜+ψ˜˙,where κ1>0.(22) is differentiated with (21) to obtain the following:(23)s˙1=κ1r−ψ˙d+Fru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d.The estimation error of the disturbance by ocean currents is defined asθ˜r=θr−θ^r. The Lyapunov function is taken as follows:(24)V1=12s12+12γrθ˜rTθ˜r.(24) can be differentiated, and substituted into (23) to obtain the following:(25)V˙1=s1κ1r−ψ˙d+Fru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d+1γrθ˜rTθ^˙r.The course control law is devised as follows:(26)τr=−κ1r−ψ˙d−Fru,v,r−ΦrTu,v,r,ψθ^r−κrsats1.where sat⋅=1⋅>κs1,⋅/κs1⋅≤κs1−1⋅>−κs1 , κs=κs1>0 is the boundary constant.(26) can be substituted into (25) to obtain the following:(27)V˙1=ΦrTu,v,r,ψθ˜rs1−κrs1ats1+1γrθ˜rTθ^˙r.To prevent a too largeθ^r from causing too large of an input τr, the adaptive law was taken as follows:(28)θ^˙r=γrΦrTu,v,r,ψs1.(28) is substituted into (27) to obtain the following:(29)V˙1=−κrs1sats1≤0,where V˙1=0 only when s1=0.To guarantee that sliding mode control reaches the stable point within the limited time period, we further let(30)V˙1≤−κV1V112.(30) has variables separated, and is integrated in terms of time to obtain the following:(31)V112≤−12κV1t+V1120.Let−1/2κV1t+V11/20  = 0, the limited time constant is obtained as follows:(32)Tr=2V11/20κV1.(24), (29) and (30) were then used to obtain the following:(33)V˙1=−κrs1sats1≤−κV112s12+12γrθ˜rTθ˜r12≤−κV1s12.The following result was further obtained:(34)κr≥κV1κs12.The velocity tracking control law was subsequently designed, and the sliding mode plane was selected as follows:(35)s2=κ2u˜.whereκ2>0.(35) can be differentiated with (21) to obtain the following:(36)s˙2=κ1Fuu,v,r+ΦuTψ,rθu+τu−u˙d.The estimation error of the disturbance by ocean currents is defined asθ˜u=θu−θ^u . The Lyapunov function is taken as follows:(37)V2=12s22+12γuθ˜uTθ˜u(37) can be differentiated and substituted into (36) to obtain the following:(38)V˙2=s2Fuu,v,r+ΦuTψ,rθu+τu−u˙d+1γuθ˜uTθ^˙u.The velocity control law can be designed as follows:(39)τu=−Fuu,v,r−ΦuTψ,rθ^u+u˙d−κusats2,where κs=κs2>0 is also the boundary constant.Similarly, the adaptive law is taken as follows:(40)θ^˙u=γuΦuTu,v,r,ψs2.(40) is substituted into (38) to obtain the following:(41)V˙2=−κus2sats2≤0,where V˙2=0 only when s2=0.Similarly, to guarantee that sliding mode control reaches a stable point within a limited time period, we further let(42)κu≥κV2κs22.The course and velocity control subsystems are independent from each other. Based on (24) and (29) and (37) and (41), the system ∑2 is uniformly globally asymptotically stable at the equilibrium point u˜,ψ˜,ψ˜˙=0,0,0.


## 3.1. Design of the Optimal Guidance Law

An underactuated USV is not subject to a transverse nonintegrable acceleration constraint. As shown in its kinematics model, the transverse acceleration of the USV is a function of its longitudinal, transverse, and angular velocities, so that it is strongly nonlinear. Based on the cascade system theory, a guidance law is designed to restrict the transverse acceleration of the USV by imposing the angular velocity and angular acceleration constraints. The restriction caused by the velocity and acceleration constraints over the USV motion is fully considered to determine the optimal expected course and velocity with the rolling optimization strategy.According to the observations of [22, 23], the kinematics equation of the USV at the small angular velocity is rewritten into the following:(5)x˙=Ucosψy˙=Usinψψ˙=rU∈UcU˙∈Uacr∈rcr˙∈rac,where the variable U is the resultant velocity of the USV, and U≈u; U˙ is the acceleration; r˙ is the angular acceleration; Uc, Uac, rc, and rac stand for the USV velocity constraint, acceleration constraint, angular velocity constraint, and angular acceleration constraint, respectively. The kinematics model in (5) is written into the following state equation:(6)x˙t=fxt,τt,yt=Cxt,where xt=x,y,ψT is the state variable; τt=U,rT is the control input variable; yt is the output quantity, and C=I3×3.


### 3.1.1. Modeling

The expected trajectory equation is set as follows:(7)x˙r=fxr,τr.The nonlinear system (6) is linearized by the Taylor expansion at any point xr,τr with the higher-order items ignored to obtain the following:(8)x˙=fxr,τr+∂f∂x|x=xrτ=τrx−xr+∂f∂τ|x=xrτ=τrτ−τr.Equations(7) and (8) are subtracted to obtain the state error system:(9)∑1:x˜˙=Atx˜+Btτ˜,where At=∂f/∂x|x=xpτ=τp and Bt=∂f/∂τ|x=xpτ=τp .The above state (9) is discretized. Let(10)Ad=I+TAt,Bd=TBt,where T is the discrete step length. The following equation is obtained:(11)x˜k+1=Adx˜k+Bdτ˜k,y˜k=Cx˜k,where y˜=yk−yrk.Considering the acceleration constraint, the control variation increment is defined asΔτk=τk−τk−1, ζk=x˜kτ˜k−1T to obtain the state space expression as follows:(12)ζk+1=A¯dζk+B¯dΔτk,y˜k=C¯ζk,where A¯d=AdBd02×3I2×2 ， B¯d=BdI2×2 , and C¯=C02×3.Considering the motion planning and motion constraints, the objective function is designed using the system state variable error, input control, and control increment to obtain the optimal expected value of the USV motion. The objective function is expressed by the following:(13)Jσ,ζ=ζN′PζN+∑k=0N−1ζk′Qζk+Δτk′RΔτk,(14)τmin≤τk≤τmax,k=0,....,N−1,(15)Δτmin≤Δτk≤Δτmax,k=0,....,N−1,where ξ=x˜tτ˜tT; N is the estimation length of rolling optimization; P, Q, and R are the weight coefficient matrixes, and their diagonal elements are greater than 0; σ=Δτ0Δτ1...ΔτN−1T. Here, P, Q, and R are the terminal constraint penalty, the USV’s capability of tracking the expected trajectory, and the demand for the stable variation of control quantity and control increment.


### 3.1.2. Solution of Optimization

The optimization defined in (13)–(15) is converted into secondary planning.(12) can be rewritten as follows:(16)ζk=A¯dkζ+∑j=0k−1A¯kjB¯dΔτk−1−j.Thus, (13) can be rewritten as follows:(17)Jσ,ζ=ζ′Qζ+χ′Q¯χ+σ′R¯σwhere χ=ζ1ζ2⋅⋅⋅ζNT , Q¯=diagQ,Q,…Q︸N−1,P, R¯=diagR,R,…R︸N , χ=ϕσ+Εζ , ϕ=B¯d0⋯0A¯dB¯dB¯d⋯0⋮⋮⋱⋮A¯dN−1B¯dA¯dN−2B¯d⋯B¯d, and Ε=A¯dA¯d2⋯A¯dNT .(17) is further changed to the following:(18)Jσ,ζ=ϕσ+Εζ'Q¯ϕσ+Εζ+σ′R¯σ+ζ′Qζ=σ′R¯+ϕ′Q¯ϕσ+ζ′Ε′Q¯ϕσ+ζ′Q+Ε′Q¯Εζ=12σ′Φσ+ζ′Ψσ+12ζ′ϒζ,where Φ=2R¯+ϕ′Q¯ϕ , Ψ=2Ε′Q¯Ε, and ϒ=2Q+Ε′Q¯Ε.(14) and (15) for constraints are written into the following(19)Gσ¯≤Τ,where G=IN−ININ−INT , σ¯=τ0τ1...τN−1σT , T=T1T2T , T1=τmax⋯τmax︸N−τmin⋯−τmin︸N , and T2=Δτmax⋯Δτmax︸N−Δτmin⋯−Δτmin︸N.Above all, optimization is transformed into the secondary planning under multiple constraints as shown in (20):(20)minσ12σ′Φσ+ζ′ΨσGσ¯≤Τ.SinceΦ≥0 , secondary planning is solvable for any nonnegative weighting matrixes P, Q, and R.


## 3.1.1. Modeling

The expected trajectory equation is set as follows:(7)x˙r=fxr,τr.The nonlinear system (6) is linearized by the Taylor expansion at any point xr,τr with the higher-order items ignored to obtain the following:(8)x˙=fxr,τr+∂f∂x|x=xrτ=τrx−xr+∂f∂τ|x=xrτ=τrτ−τr.Equations(7) and (8) are subtracted to obtain the state error system:(9)∑1:x˜˙=Atx˜+Btτ˜,where At=∂f/∂x|x=xpτ=τp and Bt=∂f/∂τ|x=xpτ=τp .The above state (9) is discretized. Let(10)Ad=I+TAt,Bd=TBt,where T is the discrete step length. The following equation is obtained:(11)x˜k+1=Adx˜k+Bdτ˜k,y˜k=Cx˜k,where y˜=yk−yrk.Considering the acceleration constraint, the control variation increment is defined asΔτk=τk−τk−1, ζk=x˜kτ˜k−1T to obtain the state space expression as follows:(12)ζk+1=A¯dζk+B¯dΔτk,y˜k=C¯ζk,where A¯d=AdBd02×3I2×2 ， B¯d=BdI2×2 , and C¯=C02×3.Considering the motion planning and motion constraints, the objective function is designed using the system state variable error, input control, and control increment to obtain the optimal expected value of the USV motion. The objective function is expressed by the following:(13)Jσ,ζ=ζN′PζN+∑k=0N−1ζk′Qζk+Δτk′RΔτk,(14)τmin≤τk≤τmax,k=0,....,N−1,(15)Δτmin≤Δτk≤Δτmax,k=0,....,N−1,where ξ=x˜tτ˜tT; N is the estimation length of rolling optimization; P, Q, and R are the weight coefficient matrixes, and their diagonal elements are greater than 0; σ=Δτ0Δτ1...ΔτN−1T. Here, P, Q, and R are the terminal constraint penalty, the USV’s capability of tracking the expected trajectory, and the demand for the stable variation of control quantity and control increment.


## 3.1.2. Solution of Optimization

The optimization defined in (13)–(15) is converted into secondary planning.(12) can be rewritten as follows:(16)ζk=A¯dkζ+∑j=0k−1A¯kjB¯dΔτk−1−j.Thus, (13) can be rewritten as follows:(17)Jσ,ζ=ζ′Qζ+χ′Q¯χ+σ′R¯σwhere χ=ζ1ζ2⋅⋅⋅ζNT , Q¯=diagQ,Q,…Q︸N−1,P, R¯=diagR,R,…R︸N , χ=ϕσ+Εζ , ϕ=B¯d0⋯0A¯dB¯dB¯d⋯0⋮⋮⋱⋮A¯dN−1B¯dA¯dN−2B¯d⋯B¯d, and Ε=A¯dA¯d2⋯A¯dNT .(17) is further changed to the following:(18)Jσ,ζ=ϕσ+Εζ'Q¯ϕσ+Εζ+σ′R¯σ+ζ′Qζ=σ′R¯+ϕ′Q¯ϕσ+ζ′Ε′Q¯ϕσ+ζ′Q+Ε′Q¯Εζ=12σ′Φσ+ζ′Ψσ+12ζ′ϒζ,where Φ=2R¯+ϕ′Q¯ϕ , Ψ=2Ε′Q¯Ε, and ϒ=2Q+Ε′Q¯Ε.(14) and (15) for constraints are written into the following(19)Gσ¯≤Τ,where G=IN−ININ−INT , σ¯=τ0τ1...τN−1σT , T=T1T2T , T1=τmax⋯τmax︸N−τmin⋯−τmin︸N , and T2=Δτmax⋯Δτmax︸N−Δτmin⋯−Δτmin︸N.Above all, optimization is transformed into the secondary planning under multiple constraints as shown in (20):(20)minσ12σ′Φσ+ζ′ΨσGσ¯≤Τ.SinceΦ≥0 , secondary planning is solvable for any nonnegative weighting matrixes P, Q, and R.


## 3.2. Design of Course and Velocity Control Laws

Sliding mode control has advantages such as fast convergence and strong robustness [24]. Therefore, the adaptive sliding mode control was adopted in this section to devise the course and velocity control laws, as well as to estimate and compensate for the influence of ocean currents. Moreover, course and velocity were controlled for convergence to the expected values given by the guidance laws.The dynamic equation for the tracking errors of velocity, course and angular velocity is given as follows:(21)∑2:u˜˙ψ˜˙ψ˜¨=Fuu,v,r+ΦuTψ,rθu+τu−u˙dr−ψ˙dFru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d.First, the course tracking control law was designed, and the sliding mode plane was selected as follows:(22)s1=κ1ψ˜+ψ˜˙,where κ1>0.(22) is differentiated with (21) to obtain the following:(23)s˙1=κ1r−ψ˙d+Fru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d.The estimation error of the disturbance by ocean currents is defined asθ˜r=θr−θ^r. The Lyapunov function is taken as follows:(24)V1=12s12+12γrθ˜rTθ˜r.(24) can be differentiated, and substituted into (23) to obtain the following:(25)V˙1=s1κ1r−ψ˙d+Fru,v,r+ΦrTu,v,r,ψθr+τr−ψ¨d+1γrθ˜rTθ^˙r.The course control law is devised as follows:(26)τr=−κ1r−ψ˙d−Fru,v,r−ΦrTu,v,r,ψθ^r−κrsats1.where sat⋅=1⋅>κs1,⋅/κs1⋅≤κs1−1⋅>−κs1 , κs=κs1>0 is the boundary constant.(26) can be substituted into (25) to obtain the following:(27)V˙1=ΦrTu,v,r,ψθ˜rs1−κrs1ats1+1γrθ˜rTθ^˙r.To prevent a too largeθ^r from causing too large of an input τr, the adaptive law was taken as follows:(28)θ^˙r=γrΦrTu,v,r,ψs1.(28) is substituted into (27) to obtain the following:(29)V˙1=−κrs1sats1≤0,where V˙1=0 only when s1=0.To guarantee that sliding mode control reaches the stable point within the limited time period, we further let(30)V˙1≤−κV1V112.(30) has variables separated, and is integrated in terms of time to obtain the following:(31)V112≤−12κV1t+V1120.Let−1/2κV1t+V11/20  = 0, the limited time constant is obtained as follows:(32)Tr=2V11/20κV1.(24), (29) and (30) were then used to obtain the following:(33)V˙1=−κrs1sats1≤−κV112s12+12γrθ˜rTθ˜r12≤−κV1s12.The following result was further obtained:(34)κr≥κV1κs12.The velocity tracking control law was subsequently designed, and the sliding mode plane was selected as follows:(35)s2=κ2u˜.whereκ2>0.(35) can be differentiated with (21) to obtain the following:(36)s˙2=κ1Fuu,v,r+ΦuTψ,rθu+τu−u˙d.The estimation error of the disturbance by ocean currents is defined asθ˜u=θu−θ^u . The Lyapunov function is taken as follows:(37)V2=12s22+12γuθ˜uTθ˜u(37) can be differentiated and substituted into (36) to obtain the following:(38)V˙2=s2Fuu,v,r+ΦuTψ,rθu+τu−u˙d+1γuθ˜uTθ^˙u.The velocity control law can be designed as follows:(39)τu=−Fuu,v,r−ΦuTψ,rθ^u+u˙d−κusats2,where κs=κs2>0 is also the boundary constant.Similarly, the adaptive law is taken as follows:(40)θ^˙u=γuΦuTu,v,r,ψs2.(40) is substituted into (38) to obtain the following:(41)V˙2=−κus2sats2≤0,where V˙2=0 only when s2=0.Similarly, to guarantee that sliding mode control reaches a stable point within a limited time period, we further let(42)κu≥κV2κs22.The course and velocity control subsystems are independent from each other. Based on (24) and (29) and (37) and (41), the system ∑2 is uniformly globally asymptotically stable at the equilibrium point u˜,ψ˜,ψ˜˙=0,0,0.


## 4. Verification of Stability

### 4.1. Stability of Guidance Law

The stability of guidance law under the control constraint and control increment constraint is verified in the subsequent section.The terminal inequality constraintξk+N∈Ω is introduced to constraint optimization. In the constraint, Ω is a neighboring domain adjacent to the origin, and a positive invariant domain of the system controlled by Δτ=Kζ, that is,(43)ζk+1=A¯d+B¯dKζk,Moreover, it satisfiesA¯d+B¯dKζk∈Ω,∀ζ∈Ω.Considering the control constraint and control increment constraint, a neighboring domain satisfying the abovementioned requirement is given as follows:Lemma 1.
It is assumed thatP>0 is the positive definite solution of the Lyapunov equation P=A¯d+B¯dKTPA¯d+A¯dK+C¯TQC¯+KTRK. Therefore, ∃α>0 makes system (51) in Ω0=ζk∈Rn|ζkTPζk≤α satisfy the control constraint. Moreover, ∃β∈,0,α makes system (51) in Ω=ζk∈Rn|ζkTPζk≤β satisfy the control constraint and control increment constraint.
The optimization is redefined as follows:(44)minΓkJσ,ζ,τmin≤τk≤τmax,k=0,....,N−1,Δτmin≤Δτk≤Δτmax,k=0,....,N−1,Δτmin≤Kζk+N−τk+N−1≤Δτmax,ζk+N∈Ω.Theorem 1.
If optimization (52) is solvable at the timek=0, P>0 is the solution of the Lyapunov equation P=A¯d+B¯dKTPA¯d+BdK+C¯TQC¯+KTRK. Ω is defined by Lemma 1 and satisfies the control constraint and control increment constraint. If external disturbance and model error are ignored, when ∀k>0, optimization (52) is solvable, and system (51) is asymptotically stable.Proof: .
Optimization (52) has the following solution at the time k:(45)Γk∗=Δτ∗k|kΔτ∗k+1|k⋮Δτ∗k+N−1|k.
This satisfies the control increment constraint. The corresponding predictive state sequence isζ∗k+1|k,ζ∗k+2|k,…,ζ∗k+N|k, which satisfies the terminal constraint equation. Its optimized value is as follows:(46)Jk∗=ζ∗k+N|kP2+∑i=0N−1y˜∗k+iQ2+Δτ∗k+iR2.
Its closed-loop control is as follows:(47)Δτk=Δτ∗k|k.
It is substituted into the system (43) to obtain the following:(48)ζk+1=A¯dζk+B¯dΔτ∗k|k.
Moreover, there isζk+1=ζ∗k+1|k.
The control sequence is selected at the timek+1 as follows:(49)Γk+1=Δτ∗k+1|kΔτ∗k+2|k⋮Δτ∗k+N−1|kKζ∗k+N|k.
Sinceζ∗k+N|k∈Ω , there is the following equation:(50)Δτmin≤Kζ∗k+N|k≤Δτmax.
It is known that the selected control sequence satisfies the control increment constraint. Meanwhile, it satisfies the control constraint according to Theorem1.
Due to the following equation,(51)ζk+1+N|k+1=A¯dζk+N|k+1+B¯dΔτk+N|k+1=A¯d+B¯dKζ∗k+N|k,
the corresponding state sequence is as follows:(52)ζk+1+N|k+1=ζ∗k+1+i|k,i=0,⋯,N−1A¯d+B¯dKζ∗k+N|k,i=N.
Moreover,ζ∗k+N|k∈Ω , so that there is the following equation:(53)A¯d+B¯dKζ∗k+N|k∈Ω.
The selected control sequence satisfies the terminal inequality constraint.
(49) is substituted into (44) to obtain the following:(54)Jk+1=ζk+1+N|k+1P2+∑i=0N−1y˜k+1+i|k+1Q2+Δτk+1+i|k+1R2=A¯d+B¯dKζ∗k+N|kP2+∑i=0N−2y˜∗k+1+i|kQ2+Δτ∗k+1+i|kR2+C¯ζ∗k+N|kQ2+Kζ∗k+N|kR2=ζ∗k+N|kP2+∑i=1N−1y˜∗k+i|kQ2+Δτ∗k+i|kR2=ζ∗k+N|kP2+∑i=0N−1y˜∗k+i|kQ2+Δτ∗k+i|kR2−y˜∗k|kQ2−Δτ∗k|kR2.
Lety˜∗k|k=C¯ζk, Δτ∗k|k=Δτk, then, it is obtained that(55)Jk+1=Jk∗−C¯ζkQ2−ΔτkR2.
It is found that the objective function is bounded.
The control sequence is the feasible solution of the optimization (52). If there is an optimal solution, the optimal solution must be better than the feasible solution. Hence, it is obtained that(56)Jk+1∗≤Jk+1≤Jk∗−C¯ζkQ2.
Whenζ=0, τ=0. At this time, τ∗=0 is its feasible solution, and J∗=0. When ∀k≥0, Jk∗≥0 is tenable and monotonically decreasing. Moreover, Jk∗ is minimum at ζ=0.
To prove the continuity ofJk∗ at ζ=0, we let ζ∈Ω and ζ≠0 at the equilibrium point. The feasible solution of the optimization (52) can be selected as follows:(57)Δτk=KζkK≥0.
Considering thatζ∈Ω is random, there is Jk<ε for ∀ζ<ℓ if ε>0 and ℓ=ℓε>0 The optimal solution is certainly better than the feasible solution, and Jk∗≥0. It is obtained that(58)0≤Jk∗≤Jk<ε,
Hence,Jk∗ is continuous at ζ=0.For this reason, Theorem 5.3 in the study of [25] proves that Jk∗ is a Lyapunov function of the closed-loop system. Therefore, the closed-loop system has the uniform global asymptotic stability at the equilibrium point, and then the system ∑1 is uniformly globally asymptotically stable at the equilibrium point.


### 4.2. Verification of System Stability

According to (9) and (21), the correlation matrix of the system ∑1−∑2 is as follows:(59)BG=cosψ00sinψ00001,It is found thatBG≤cosψ+sinψ+1≤2+1. Based on Inference 2.3 in Reference [26], the cascade system ∑1−∑2 is uniformly globally asymptotically stable at xe,ye,u˜,ψ˜,ψ˜˙=0,0,0,0,0.


## 4.1. Stability of Guidance Law

The stability of guidance law under the control constraint and control increment constraint is verified in the subsequent section.The terminal inequality constraintξk+N∈Ω is introduced to constraint optimization. In the constraint, Ω is a neighboring domain adjacent to the origin, and a positive invariant domain of the system controlled by Δτ=Kζ, that is,(43)ζk+1=A¯d+B¯dKζk,Moreover, it satisfiesA¯d+B¯dKζk∈Ω,∀ζ∈Ω.Considering the control constraint and control increment constraint, a neighboring domain satisfying the abovementioned requirement is given as follows:Lemma 1.
It is assumed thatP>0 is the positive definite solution of the Lyapunov equation P=A¯d+B¯dKTPA¯d+A¯dK+C¯TQC¯+KTRK. Therefore, ∃α>0 makes system (51) in Ω0=ζk∈Rn|ζkTPζk≤α satisfy the control constraint. Moreover, ∃β∈,0,α makes system (51) in Ω=ζk∈Rn|ζkTPζk≤β satisfy the control constraint and control increment constraint.
The optimization is redefined as follows:(44)minΓkJσ,ζ,τmin≤τk≤τmax,k=0,....,N−1,Δτmin≤Δτk≤Δτmax,k=0,....,N−1,Δτmin≤Kζk+N−τk+N−1≤Δτmax,ζk+N∈Ω.Theorem 1.
If optimization (52) is solvable at the timek=0, P>0 is the solution of the Lyapunov equation P=A¯d+B¯dKTPA¯d+BdK+C¯TQC¯+KTRK. Ω is defined by Lemma 1 and satisfies the control constraint and control increment constraint. If external disturbance and model error are ignored, when ∀k>0, optimization (52) is solvable, and system (51) is asymptotically stable.Proof: .
Optimization (52) has the following solution at the time k:(45)Γk∗=Δτ∗k|kΔτ∗k+1|k⋮Δτ∗k+N−1|k.
This satisfies the control increment constraint. The corresponding predictive state sequence isζ∗k+1|k,ζ∗k+2|k,…,ζ∗k+N|k, which satisfies the terminal constraint equation. Its optimized value is as follows:(46)Jk∗=ζ∗k+N|kP2+∑i=0N−1y˜∗k+iQ2+Δτ∗k+iR2.
Its closed-loop control is as follows:(47)Δτk=Δτ∗k|k.
It is substituted into the system (43) to obtain the following:(48)ζk+1=A¯dζk+B¯dΔτ∗k|k.
Moreover, there isζk+1=ζ∗k+1|k.
The control sequence is selected at the timek+1 as follows:(49)Γk+1=Δτ∗k+1|kΔτ∗k+2|k⋮Δτ∗k+N−1|kKζ∗k+N|k.
Sinceζ∗k+N|k∈Ω , there is the following equation:(50)Δτmin≤Kζ∗k+N|k≤Δτmax.
It is known that the selected control sequence satisfies the control increment constraint. Meanwhile, it satisfies the control constraint according to Theorem1.
Due to the following equation,(51)ζk+1+N|k+1=A¯dζk+N|k+1+B¯dΔτk+N|k+1=A¯d+B¯dKζ∗k+N|k,
the corresponding state sequence is as follows:(52)ζk+1+N|k+1=ζ∗k+1+i|k,i=0,⋯,N−1A¯d+B¯dKζ∗k+N|k,i=N.
Moreover,ζ∗k+N|k∈Ω , so that there is the following equation:(53)A¯d+B¯dKζ∗k+N|k∈Ω.
The selected control sequence satisfies the terminal inequality constraint.
(49) is substituted into (44) to obtain the following:(54)Jk+1=ζk+1+N|k+1P2+∑i=0N−1y˜k+1+i|k+1Q2+Δτk+1+i|k+1R2=A¯d+B¯dKζ∗k+N|kP2+∑i=0N−2y˜∗k+1+i|kQ2+Δτ∗k+1+i|kR2+C¯ζ∗k+N|kQ2+Kζ∗k+N|kR2=ζ∗k+N|kP2+∑i=1N−1y˜∗k+i|kQ2+Δτ∗k+i|kR2=ζ∗k+N|kP2+∑i=0N−1y˜∗k+i|kQ2+Δτ∗k+i|kR2−y˜∗k|kQ2−Δτ∗k|kR2.
Lety˜∗k|k=C¯ζk, Δτ∗k|k=Δτk, then, it is obtained that(55)Jk+1=Jk∗−C¯ζkQ2−ΔτkR2.
It is found that the objective function is bounded.
The control sequence is the feasible solution of the optimization (52). If there is an optimal solution, the optimal solution must be better than the feasible solution. Hence, it is obtained that(56)Jk+1∗≤Jk+1≤Jk∗−C¯ζkQ2.
Whenζ=0, τ=0. At this time, τ∗=0 is its feasible solution, and J∗=0. When ∀k≥0, Jk∗≥0 is tenable and monotonically decreasing. Moreover, Jk∗ is minimum at ζ=0.
To prove the continuity ofJk∗ at ζ=0, we let ζ∈Ω and ζ≠0 at the equilibrium point. The feasible solution of the optimization (52) can be selected as follows:(57)Δτk=KζkK≥0.
Considering thatζ∈Ω is random, there is Jk<ε for ∀ζ<ℓ if ε>0 and ℓ=ℓε>0 The optimal solution is certainly better than the feasible solution, and Jk∗≥0. It is obtained that(58)0≤Jk∗≤Jk<ε,
Hence,Jk∗ is continuous at ζ=0.For this reason, Theorem 5.3 in the study of [25] proves that Jk∗ is a Lyapunov function of the closed-loop system. Therefore, the closed-loop system has the uniform global asymptotic stability at the equilibrium point, and then the system ∑1 is uniformly globally asymptotically stable at the equilibrium point.


## 4.2. Verification of System Stability

According to (9) and (21), the correlation matrix of the system ∑1−∑2 is as follows:(59)BG=cosψ00sinψ00001,It is found thatBG≤cosψ+sinψ+1≤2+1. Based on Inference 2.3 in Reference [26], the cascade system ∑1−∑2 is uniformly globally asymptotically stable at xe,ye,u˜,ψ˜,ψ˜˙=0,0,0,0,0.


## 5. Simulation Experiment

To verify the effectiveness and advancement of the proposed algorithm in this paper, a CyberShip [20] model was employed in the simulation experiment. The model parameters are shown in Table 1.Table 1
Model parameters of USV.
Model parameterValueModel parameterValuem1125.8b321m2233.8d112.0m23−11.748d227.0m32−11.748d23−2.5425m33, b11, b226.813, 1, 0d32, d33,−2.5425, 1.422,


### 5.1. Straight Trajectory Tracking

Simulation conditions: the sampling period of trajectory equationsxt=5tyt=10φt=0 indicated that T=50ms; the initial position of the USV was 0m,0m; the initial course was (0m, 0m); and the initial velocity was 0 m/s. The resultant velocity of the disturbance by wind, wave, and current was set to 0.5 m/s. The direction in the rectangular coordinate system was −π/4 rad. The estimation length was 40T. The execution length was T. The diagonal elements of the weighting coefficient matrix Q were 1, respectively. The diagonal elements of the weighting coefficient matrix R were 10, respectively. The velocity constraint was 0m/s,6m/s together with the acceleration constraint −1m/s2,1m/s2 , the angular velocity constraint 0.5rad/s,0.5rad/s , and the angular acceleration constraint −0.25rad/s2,0.25rad/s2. The control laws were κ1=8 ， γr=1 ， κr=0.2 ， κs1=0.5 ， κV1=0.5 ， κ2=2 ， γu=1 ， κu=0.2 ， κs1=0.5, and κV1=0.5 .The proposed algorithm was compared with the algorithm in the study of [17] to demonstrate its advancement. The comparison results are shown in Figures 3–9. The results of tracking with the algorithm in the study of [17] form trajectory 1, while the results of tracking with the proposed algorithm are denoted by trajectory 2.Figure 3
Straight trajectory tracking of the USV.Figure 4
Tracking error.Figure 5
Course angle tracking.Figure 6
Longitudinal velocity tracking.Figure 7
Expected angular velocity.Figure 8
Steering torque.Figure 9
Forward thrust.


### 5.2. Curved Trajectory Tracking

Simulation conditions: the trajectory equation wasxt=50sin0.1tyt=60−50∗cos0.1tφt=0.1t, and the initial position of the USV was (0m and -10m). Other parameters were set as above. The simulation results are presented in Figures 10–16.Figure 10
Curved trajectory tracking of the USV.Figure 11
Tracking error.Figure 12
Course angle tracking.Figure 13
Longitudinal velocity tracking.Figure 14
Expected angular velocity.Figure 15
Steering torque.Figure 16
Forward thrust.The simulation results reveal that the control system devised in this paper can satisfy the constraints under the disturbance caused by constant irrotational ocean currents and effectively track the expected trajectory. After analyzing the results in these simulation figures, it was found that the initial position of the USV deviated from the expected trajectory, and the position error was very large. However, an optimal guidance law was designed to give larger than expected course variation and faster velocity while satisfying the velocity and acceleration, annual velocity, and annual acceleration constraints. On this basis, the controller can control steering torque and forward thrust to change rapidly under the guidance of expected course and velocity, and drive the USV toward the expected trajectory. Subsequently, the USV position tracking error converges gradually, and the guidance law generates the stable expected course and velocity. At this time, the USV steers gently with stable thrust, so as to guarantee the accurate trajectory tracking.As shown in Figures2, 3, 9, and 10, the trajectory tracking algorithm designed in this paper can maintain very small overshoot while guaranteeing the convergence speed, so that trajectory tracking is more accurate. Based on Figures 4, 5, 11, and 12, the devised sliding mode course and velocity adaptive control law can overcome the influence of disturbance, so as to ensure stable tracking of expected course and velocity rapidly under the effect of steering torque (Figures 7 and 14) and forward thrust (Figures 8 and 15). Additionally, Figures 5 and 12 show that the velocity is large at the stage of convergence, which guarantees the tracking speed is reached more quickly. Meanwhile, the proposed algorithm satisfies the USV motion constraints with the rolling optimization strategy, so that the USV can slow down steadily in advance and avoid overshooting the targeted location. As revealed in Figures 6 and 13, the proposed algorithm has a large angular velocity at the stage of convergence, but still satisfies the angular velocity constraint in the motion planning. Moreover, it achieves effective stabilization at the expected angular velocity in the steady state. The simulation results are compared to satisfactorily prove the effectiveness and advancement of the proposed trajectory tracking system in this paper.


## 5.1. Straight Trajectory Tracking

Simulation conditions: the sampling period of trajectory equationsxt=5tyt=10φt=0 indicated that T=50ms; the initial position of the USV was 0m,0m; the initial course was (0m, 0m); and the initial velocity was 0 m/s. The resultant velocity of the disturbance by wind, wave, and current was set to 0.5 m/s. The direction in the rectangular coordinate system was −π/4 rad. The estimation length was 40T. The execution length was T. The diagonal elements of the weighting coefficient matrix Q were 1, respectively. The diagonal elements of the weighting coefficient matrix R were 10, respectively. The velocity constraint was 0m/s,6m/s together with the acceleration constraint −1m/s2,1m/s2 , the angular velocity constraint 0.5rad/s,0.5rad/s , and the angular acceleration constraint −0.25rad/s2,0.25rad/s2. The control laws were κ1=8 ， γr=1 ， κr=0.2 ， κs1=0.5 ， κV1=0.5 ， κ2=2 ， γu=1 ， κu=0.2 ， κs1=0.5, and κV1=0.5 .The proposed algorithm was compared with the algorithm in the study of [17] to demonstrate its advancement. The comparison results are shown in Figures 3–9. The results of tracking with the algorithm in the study of [17] form trajectory 1, while the results of tracking with the proposed algorithm are denoted by trajectory 2.Figure 3
Straight trajectory tracking of the USV.Figure 4
Tracking error.Figure 5
Course angle tracking.Figure 6
Longitudinal velocity tracking.Figure 7
Expected angular velocity.Figure 8
Steering torque.Figure 9
Forward thrust.


## 5.2. Curved Trajectory Tracking

Simulation conditions: the trajectory equation wasxt=50sin0.1tyt=60−50∗cos0.1tφt=0.1t, and the initial position of the USV was (0m and -10m). Other parameters were set as above. The simulation results are presented in Figures 10–16.Figure 10
Curved trajectory tracking of the USV.Figure 11
Tracking error.Figure 12
Course angle tracking.Figure 13
Longitudinal velocity tracking.Figure 14
Expected angular velocity.Figure 15
Steering torque.Figure 16
Forward thrust.The simulation results reveal that the control system devised in this paper can satisfy the constraints under the disturbance caused by constant irrotational ocean currents and effectively track the expected trajectory. After analyzing the results in these simulation figures, it was found that the initial position of the USV deviated from the expected trajectory, and the position error was very large. However, an optimal guidance law was designed to give larger than expected course variation and faster velocity while satisfying the velocity and acceleration, annual velocity, and annual acceleration constraints. On this basis, the controller can control steering torque and forward thrust to change rapidly under the guidance of expected course and velocity, and drive the USV toward the expected trajectory. Subsequently, the USV position tracking error converges gradually, and the guidance law generates the stable expected course and velocity. At this time, the USV steers gently with stable thrust, so as to guarantee the accurate trajectory tracking.As shown in Figures2, 3, 9, and 10, the trajectory tracking algorithm designed in this paper can maintain very small overshoot while guaranteeing the convergence speed, so that trajectory tracking is more accurate. Based on Figures 4, 5, 11, and 12, the devised sliding mode course and velocity adaptive control law can overcome the influence of disturbance, so as to ensure stable tracking of expected course and velocity rapidly under the effect of steering torque (Figures 7 and 14) and forward thrust (Figures 8 and 15). Additionally, Figures 5 and 12 show that the velocity is large at the stage of convergence, which guarantees the tracking speed is reached more quickly. Meanwhile, the proposed algorithm satisfies the USV motion constraints with the rolling optimization strategy, so that the USV can slow down steadily in advance and avoid overshooting the targeted location. As revealed in Figures 6 and 13, the proposed algorithm has a large angular velocity at the stage of convergence, but still satisfies the angular velocity constraint in the motion planning. Moreover, it achieves effective stabilization at the expected angular velocity in the steady state. The simulation results are compared to satisfactorily prove the effectiveness and advancement of the proposed trajectory tracking system in this paper.


## 6. Conclusion

(1)
This paper focuses on the trajectory tracking of an asymmetric underactuated USV under multiple motion constraints and disturbance caused by ocean currents, as well as designs and implements a trajectory tracking cascade control system based on the optimal guidance law and adaptive sliding mode control law.(2)
Optimal guidance law is designed following the rolling optimization strategy to fully address multiple motion constraints in the USV trajectory tracking process. Additionally, it effectively lowers the influence of system time delay and overshoot, and enhances the stability of the system.(3)
The course and velocity control laws are designed based on an adaptive sliding mode control to effectively estimate and compensate for the disturbance by ocean currents while stabilizing the course and velocity errors. The control laws can converge within the limited time period through the parameter design.(4)
Based on the system design, the Lyapunov stability theory is introduced to demonstrate the stability of subsystems and the entire cascade system. Moreover, simulation experiment is carried out to verify the feasibility and superiority of the proposed algorithm.



---

*Source: 5274452-2022-05-05.xml*