Path-Following Control Method for Surface Ships Based on a New Guidance Algorithm

Abstract

A new type of path-following method has been developed to steer marine surface vehicles along desired paths. Path-following is achieved by a new hyperbolic guidance law for straight-line paths and a backstepping control law for curved paths. An optimal controller has been improved for heading control, based on linear quadratic regulator (LQR) theory with nonlinear feedback control techniques. The control algorithm performance is validated by simulation and comparison against the requirements of International Standard IEC62065. Deviations are within the allowable range of the standard. In addition, the experimental results show that the proposed method has higher control accuracy.

1. Introduction

Autopilot is the main equipment for ship motion control; it controls the ship’s course without the participation of the helmsman [1]. On autopilot, the ship follows the target route automatically, which can effectively reduce ship operating costs [2]. This paper presents a new method for ship track control. The goal of the control system is to follow a preset route with good anti-interference ability [3]. Path-following control systems for marine vehicles are usually constructed as three independent blocks: guidance, navigation and control [4]. Guidance is the action or the system that continuously computes the reference position, velocity and acceleration of the vehicle to be used by the control system [5]. Navigation is the action or the system that directs the vehicle, by which the position, course and traveled distance is determined. The control system determines the necessary forces and moments to achieve a certain objective.

Line of sight (LOS) is the most commonly used guidance algorithm. This algorithm was first applied to track control of surface ships by Mcgookin and Fossen and has been widely studied [6,7,8,9]. The advantage of the LOS guidance algorithm is that it controls the ship’s motion by imitating the behavior of the helmsman to eliminate track deviation from the planned route. However, there are some defects in the LOS guidance algorithm. When the distance between the next target waypoint on the target route and the current position of the ship is large, a large track deviation will be produced when the ship tracks the planned route through the guidance algorithm, in addition, the convergence speed of the algorithm is also very slow. In response to this problem, an integral LOS (ILOS) has been proposed and extensively analyzed. Fossen and Lekkas [10] proposed an ILOS based on adaptive control theory that can compensate for drift forces effectively.

Stability analysis is an important topic for navigation and control systems used in autonomous marine vehicles. Proportional-integral-derivative (PID) controllers or proportional derivative controllers are usually used in ship autopilot design [11,12]. In recent years, improved PID control algorithms have emerged. For example, steering parameters for normal adaptable PID autopilots have been developed during the last decade [13,14]. Dlabac et al. [15] presented a particle swarm optimization (PSO)-based PID controller for ship course-keeping. Recently, nonlinear controllers for autopilot motion control of marine vessels have been reported. Oh and Sun [16] presented a model predictive control for path following of underactuated surface vessels. Guerrero et al. [17] employed an adaptive high- order sliding mode controller for trajectory tracking of autonomous underwater vehicles. Designs based on neural networks [18,19], pole placement technique (PPT) [20], fuzzy logic [21], extended state observer technique (ESO) [22] and some other methods are also used. Wang et al. [23] presented a heading control algorithm based on an H-infinite optimization algorithm to counteract the influence of waves and ensure that ships can turn steadily in rough seas. Sun et al. [24] proposed a feedback linearization optimal heading control algorithm to effectively control a ship’s course. Veremey et al. [25] proposed a new approach for the compensative transformation of reference dynamic output controllers. Du et al. [26,27] used adaptive robust nonlinear control to adjust a ship’s course and track. Xiang et al. [28] improved the fuzzy logic method and tested it on underwater ships and marine surface vehicles. Xu et al. [29] proposed a vector field guidance law to follow the ship’s trajectory. Although most of these methods have good results, there is no standard to verify the control effect of their experiments and the ship model they use is no comprehensive consideration for each type of ship, which cannot effectively verify the results. Liu et al. [30] proposed a nonlinear robust control algorithm based on Backstepping method to control ship straight-path tracking. However, there is no curve path tracking method in this study. Zhao et al. [31] based on Serret-Frenet frame transformation develop a tracking error model and the backstepping controller compensates the nonlinearity of the container ship. Xu et al. [32] proposed an adaptive backstepping controller for path-following control of an underactuated ship based on a nonlinear steering model, which can achieve the effect of path control. But the shortcoming in both two is, they present pretty few control scenarios of the researches to verify the reliability of the algorithm, moreover, the scene of track control is too simple.

This paper presents a new path-following control law. The main innovation of this paper is to propose hyperbolic guidance law and apply it to straight-line path-following control. This design improves the convergence of the straight-line path-following control. For curved path-following in the transition between two adjacent straight-line paths, this paper improved a reverse stepping method to calculate heading rate commands to make the system globally asymptotically stable. Compared with the previous research, the extended state observer (ESO) for the controller model is established to estimate and compensate the state in this paper, which can improve the anti-interference ability of the ship in the path following. In the presence of sea current interference, also gives a correction formula. In this paper, linear quadratic regulator (LQR) and nonlinear feedback control are combined to improve the traditional LQR heading controller to improve the control accuracy of heading and yaw rate of the ship. Finally, the track control effect of three different types of ships is verified in simulations and the control effect is evaluated according to International Standard IEC62065 [33].

Section 2 establishes a mathematical model, including ship motion simulation and control and identifies controller parameters. Section 3 describes the design of the heading controller. Section 4 introduces the new guidance algorithm. Section 5 presents analysis of simulation results. Finally, Section 6 contains the conclusions.

2. Mathematical Model of Ship Motion

2.1. Process Plant Model

We describe the ship motion model in five basic compositions, with three degrees of freedom, shown in Figure 1. The model is derived from Newton’s laws of motion using linear equations to relate hydrodynamic forces to the respective motions in the horizontal plane. The equations are as follows [4]:

$$m\left[\dot{u}-vr-x_g{r}^2-y_g\dot{r}\right]=X,$$

(1)

$$m\left[\dot{v}+ur-y_g{r}^2+x_g\dot{r}\right]=Y,$$

(2)

$$I_z\dot{r}+m\left[x_g\left(\dot{v}+ur\right)-y_g\left(\dot{u}-vr\right)\right]=N,$$

(3)

where $X$, $Y$ and $N$ denote external forces and moment, while $\left(x_g,y_g,z_g\right)$ describe the location of the center of gravity. $u$,$v$ and $r$ denote surge velocity, sway velocity and yaw angular velocity, respectively, $\dot{u}$,$\dot{v}$ and $\dot{r}$ are their derivatives. $I_z$ is the yaw moment of inertia, $m$ is the total mass of ship. It is common to let the body frame coordinate origin coincide with the center of gravity; thus, the equations can be rewritten as:

$$m\left(\dot{u}-vr\right)=X,$$

(4)

$$m\left(\dot{v}+ur\right)=Y,$$

(5)

$$I_z\dot{r}=N.$$

(6)

The hydrodynamic force $X$ is linearized and is assumed to be proportional to the ship linear velocity, $u$ in surge. Let $R_u$ denote the linear coefficient of hydrodynamic resistance to forward motion and $X_{thrust}$ denote the thrust provided by the propeller. Thus Equation (4) becomes:

$$m\left(\dot{u}-vr\right)=X_{thrust}-R_{u}u.$$

(7)

Let ${\tau }_u$ denote the time constant of the linear surge response model, $K_u$ denote the coefficient of thrust, $u_{\max }$ denote maximum forward speed, and $X_{\max }$ denote maximum thrust:

$$\begin{array}{l}{\tau }_u=m/R_u,\\ u_{\max }=X_{\max }/R_u,\\ K_u=u_{\max }/{\tau }_u.\end{array}$$

(8)

Substituting Equation (8) into Equation (7) yields:

$$\dot{u}=K_u{X}^{\prime }+vr-u/{\tau }_u.$$

(9)

In the same way, for sway response model let ${\tau }_v=m/R_v$ denote the linear coefficient of hydrodynamic resistance to sway motion, then Equation (5) becomes:

$$\dot{v}=-ur-v/{\tau }_v.$$

(10)

For the yaw response model, Equation (6) becomes:

$$I_z\dot{r}=K_r\left({\delta }_a{K}_u{X}^{\prime }{\tau }_u/L+W\right)+\gamma L{R}_v\left(v-\gamma Lr\right)-R_{r}r,$$

(11)

where $K_r$ is a constant of proportionality, $\gamma$ is the yaw stability factor, $L$ is the length of the ship, ${\delta }_a$ is the real rudder deflection, $R_v$ is the linear coefficient of hydrodynamic resistance to lateral motion, $R_r$ is the linear coefficient of hydrodynamic resistance to yaw and ${\tau }_r=I_z/R_r$ denotes the time constant of the yaw response model. Simplifying by relating the moment of inertia to the mass and length as a uniformly dense rod to its mass and length, then the moment of inertia can be obtained as $I_z=m{L}^2/12$. $K_{r}W$ is the turning moment imparted by wave disturbance, ${K^{\prime }}_r$ is the normalized coefficient of rudder moment, which can be expressed as ${K^{\prime }}_r=K_r{\delta }_{\max }/I_z$.

The wave interference model established in this paper consists of a series of square half wave waves. The duration $T$ and height $H$ of each half wave are random numbers related to the Bretschneider wave spectrum [34]. Finally, a square wave interference signal is generated by the wave model, in which the calculation methods of the duration $T$ and the wave height $H$ are as follows:

$$\{\begin{array}{c}T=0.5T_0\left(ss\right)\times \left(1+0.5Ran{d}_b\right)\\ H=H_0\left(ss\right)\times \left(1+H_r\times Ran{d}_b\right)\times A_a\end{array},$$

(12)

where $Ran{d}_b$ denotes a random number among [−1, 1]. A value of 0.5 for $H_r$ is recommended, while $A_a$ has alternating sign (i.e., ±1) to describe wave direction, $ss$ means the sea state level, $T_0\left(ss\right)$ and $H_0\left(ss\right)$ are the natural period and significant wave height under sea state $ss$, respectively. The range of values is shown in

Table 1. The value of natural period and meaningful wave height under different sea conditions.

Sea State (ss)	T0 (ss)	H0 (ss)
1	2.2	0.1
2	5	0.5
3	7.8	1.25
4	11	2.5
5	14	4
6	17.2	6
7	21.1	9
8	26.3	14

.

Referring to the modeling method of wave interference in IEC62065, this paper ignores the influence of wave interference on ship surge motion and sway motion, only adds the wave interference torque of ship’s yaw motion to the mathematical model of ship’s motion. Rewrite ship yaw Equation (11) as:

$$\dot{r}={K^{\prime }}_r\frac{K_u{X}^{\prime }{\tau }_u}{L}\cdot {\delta }_a{}^{\prime }+\frac{12\gamma \left(v-\gamma Lr\right){\tau }_r}{L{\tau }_v}-\frac{r}{{\tau }_r}+0.01{K^{\prime }}_r{S}_{f}H,$$

(13)

where ${\delta }_a{}^{\prime }$ is the normalized rudder position obtained by ${{\delta }^{\prime }}_a={\delta }_a/{\delta }_{a\max }$, $S_f$ is a Scaling Factor. A value of 20 for the Scaling Factor is recommended.

2.2. Control Plant Model

We simplify the process plant model described in the previous section to design the autopilot control algorithm. The simplified model contains only the main physical properties of the process. Notomo [35] proposed a linearized model for ship steering equations, which is given by a simple transfer function between $r$ and $\delta$.

$$\frac{r}{\delta }\left(s\right)=\frac{K}{\left(1+Ts\right)},$$

(14)

where $T$ and $K$ are the time constant and gain constant, respectively. Neglecting the roll and pitch modes ($\varphi =\theta =0$), such that:

$$\dot{\psi }=r$$

(15)

finally yields:

$$\frac{\psi }{\delta }\left(s\right)=\frac{K}{s\left(1+Ts\right)},$$

(16)

where $\psi$ is the heading angle.

The time domain form of Equation (16) is:

$$T\ddot{\psi }+\dot{\psi }=K\delta .$$

(17)

Notomo’s first order model is usually written as:

$$T\dot{r}+r=K\delta .$$

(18)

When the straight motion of the ship is unstable or critical stable, nonlinear maneuvering models should be used. A nonlinear term can be added in Equation (18); thus, Notomo’s nonlinear first order model can be obtained as:

$$T\dot{r}+r=K(\delta +K_{v}v).$$

(19)

This is the commonly used control plant model for the design of the steering autopilot, in which $K_v$ is the coefficient of the nonlinear term.

2.3. Controller Parameter Identification

In the above-mentioned control model, some parameters cannot be obtained directly but need to be obtained by system parameter identification. In control applications, the controller performance is bound to the assumptions and approximations of the model. To obtain the corresponding controller parameters more accurately, we apply the forgetting factor least square algorithm to Notomo’s nonlinear first order model to identify the ship motion model.

The Recursive Least Square (RLS) algorithm with exponential forgetting is given as follows [36]:

$$\widehat{\theta }(k)=\widehat{\theta }(k-1)+Q(k)\left[y(k)-{\Phi }^{{\rm T}}(k)\widehat{\theta }(k-1)\right],$$

(20)

$$Q(k)=P(k-1)\Phi (k){\left[\lambda I+{\Phi }^{{\rm T}}(k)P(k-1)\Phi (k)\right]}^{-1},$$

(21)

$$P(k)=\frac{1}{\lambda }\left[I-Q(k){\Phi }^{{\rm T}}(k)\right]P(k-1),$$

(22)

where the forgetting factor value of $\lambda$ is $0.98<\lambda <0.995$, $\Phi (k)$ is the data vector and $\widehat{\theta }(k)$ is the estimated value of the parameter vector. $Q(k)$ and $P(k)$ are the intermediate variable help us get $\widehat{\theta }(k)$ we want.

Next, referring to the ship motion simulation model, substituting control Equation (19) into ship motion Equation (13), the values of $T$, $K$ and $K_v$ are obtained as follows:

$$\{\begin{array}{c}T=\frac{{\tau }_v{\tau }_r}{{\tau }_v+12{\tau }_r{\gamma }^2},\\ K=\frac{T{K}_r{}^{\prime }{u}_{\max }{X}^{\prime }}{L{\delta }_{\max }}\\ K_v=\frac{12\gamma T}{KL{\tau }_v}.\end{array},$$

(23)

To facilitate the identification calculation, Equations (11) and (13) are rewritten in the following form:

$$\{\begin{array}{l}{Y}_0={\theta }_0{X}_0+{\theta }_1{X}_1\\ Y_1={\theta }_2{X}_2\end{array}$$

(24)

where

$$\{\begin{array}{c}{Y}_0=\dot{r}-\frac{12\gamma }{L{\tau }_v}{v}_w+\frac{12{\gamma }^2}{{\tau }_v}r\\ Y_1={\dot{v}}_g+u_{g}r\end{array},$$

(25)

$$\{\begin{array}{c}{X}_0=\frac{U}{L}{{\delta }^{\prime }}_a\\ X_1=r\\ X_2=v_w\end{array},$$

(26)

$$\{\begin{array}{c}{\theta }_0=\frac{KL}{TU}\\ {\theta }_1=-\frac{1}{T}+\frac{12{\gamma }^2}{{\tau }_v}\\ {\theta }_2=-\frac{1}{{\tau }_v}\end{array}.$$

(27)

According to the recursive formula of the forgetting factor least squares identification algorithm in Equations (20)–(22), the corresponding identification results ${\theta }_0$, ${\theta }_1$ and ${\theta }_2$ can be obtained. Through ${\theta }_0$, ${\theta }_1$ and ${\theta }_2$, the three controller parameters $\widehat{T}$, $\widehat{K}$ and ${\widehat{K}}_v$ of the nonlinear first-order Notomo model can be solved:

$$\{\begin{array}{c}\widehat{T}=\frac{1}{-{\widehat{\theta }}_1+\frac{12{\gamma }^2}{{\tau }_v}},\\ \widehat{K}=\frac{{\widehat{\theta }}_0\widehat{T}U}{L},\\ {\widehat{K}}_v=\frac{12\gamma \widehat{T}}{L\widehat{K}{\tau }_v}.\end{array}$$

(28)

3. Optimal Heading Controller

This section describes the design of the optimal heading controller. The main goal is to calculate the controller parameters in terms of the Notomo constants obtained by the identification algorithm in the previous section and introduce them in the LQR controller law. Then, nonlinear feedback is added to improve LQR controller to achieve nonlinear control and better control accuracy. In addition, to filter out the influence of sea wave interference on the acquisition of ship heading signal, an extended state observer (ESO) is added to this design to obtain the estimated value of the low-frequency heading signal. Thus, the controller can obtain accurate and fast course control.

3.1. LQR Controller

Assume that the heading signal $\psi$ can be obtained by a compass and the yaw rate $r$ can be obtained by a rate gyro or by a state observer. Notomo’s first order model in Equation (18) can be written as the state space form:

$$\dot{X}=AX+B{u}_c,$$

(29)

where $X={\left(r,\psi ,\delta \right)}^T$ is the state vector and $u_c$ is the controller input, which denotes the desired rudder rate. $A$ and $B$ are the coefficient matrixes with:

$$A=\left(\begin{array}{ccc}-1/T& 0& K/T\\ 1& 0& 0\\ 0& 0& 0\end{array}\right),$$

(30)

$$B={\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T,$$

(31)

$X_d={\left(r_d,{\psi }_d,\delta \right)}^T$ denotes the objective state vector. The goal of the controller design is to converge the state $X$ to the objective $X_d$:

$$\underset{t\to \infty }{\lim }X=X_d,$$

(32)

$\tilde{X}$ denotes the error vector:

$$\tilde{X}=X-X_d={\left(\tilde{r}\tilde{\psi }\delta \right)}^T,$$

(33)

where $\tilde{r}=r-r_d$, $\tilde{\psi }=\psi -{\psi }_d$. The control performance specification can be measured in terms of:

$$J={\displaystyle {\int }_0^{\infty }\left({\tilde{X}}^{T}QX+u_c{}^{T}R{u}_c\right)dt},$$

(34)

where $Q$ is a 3-dimensional diagonal matrix, in which diagonal elements are the weighting factors. $R$ is the weighting factor of the input. The LQR problem is to find the optimal control $u_c(t)$ such that $J$ in Equation (34) is minimized,

$$u_c=-R^{-1}{B}^{T}P\tilde{X},$$

(35)

where $P$ is the positive defined solution of the Riccati Equation [37]:

$$A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0$$

(36)

Let

$${\left(k_1,k_2,k_3\right)}^T=-R^{-1}{B}^{T}P,$$

(37)

where $k_1$, $k_2$ and $k_3$ are the controller gains obtained by the LQR law. Thus, the representation of the controller can be written as:

$${\delta }_{LQR}={\displaystyle {\int }_0^t\left(k_1\tilde{r}+k_2\tilde{\psi }+k_3\delta \right)d\tau }.$$

(38)

3.2. Feedback Nonlinearization Compensation

In the controller design here, a nonlinear feedback term is added in the control law for compensating the nonlinear maneuvering of the ship. Notomo’s nonlinear first order model can be rewritten as:

$$T\dot{r}+r=K(\delta +K_{v}v).$$

(39)

Comparing with the linear controller law obtained in the previous section, the following equation can be obtained:

$${\delta }_{LQR}={\delta }_{com}+K_{v}v,$$

(40)

where ${\delta }_{com}$ is the command rudder angle, $K_{v}v$ can be treated as the feedback term, such that:

$${\delta }_{com}={\delta }_{LQR}-{\delta }_{FL},$$

(41)

with ${\delta }_{FL}=K_{v}v$ and the value of $K_v$ is obtained from the identification algorithm in the previous section.

3.3. Extended State Observer

Both surface ships and underwater vehicles need state observers to process signal data from sensors and navigation equipment. The observer in this paper is designed in terms of the nonlinear ship model and the wave disturbance model. The nonlinear ship model and the wave disturbance model are as follows:

$$\{\begin{array}{l}T\dot{r}+r=K(\delta -{\delta }_n+K_{v}v)+{\omega }_r\\ {\ddot{\psi }}_{\omega }+2\varsigma {\omega }_0{\dot{\psi }}_{\omega }+{\omega }_0^2{\psi }_{\omega }={\omega }_{\omega }\end{array}$$

(42)

where ${\delta }_n$ is the rudder offset, ${\omega }_r$ and ${\omega }_{\omega }$ are the zero-mean Gaussian measurement white noise; ${\psi }_{\omega }$ represents the first-order wave-induced motion, and $\varsigma$ is the relative damping ratio, conventionally assigned a value of 0.075. ${\omega }_0$ is the wave frequency.

Based on Equation (42), the state space equation of the extended state observer is:

$$\dot{\widehat{X}}=A\widehat{X}+B\Gamma +G(Y-C\widehat{X}),$$

(43)

where $\widehat{X}$ denotes the estimation of the state, $\widehat{X}={\left({\widehat{\delta }}_n,\widehat{r},\psi ,{\widehat{\xi }}_{\omega },{\widehat{\psi }}_{\omega }\right)}^T$ and ${\dot{\widehat{\xi }}}_{\omega }={\widehat{\psi }}_{\omega }$. $\Gamma$ is the input of the model and $\Gamma =\delta +K_{v}v$; $G$ is the gain matrix; ${\widehat{\delta }}_n$ is the estimation of the rudder offset; $Y$ is the heading signal obtained by the compass with $Y=\psi +{\psi }_{\omega }$.$A$, $B$ and $C$ are the coefficient matrixes with:

$$A=\left(\begin{array}{ccccc}0& 0& 0& 0& 0\\ -\frac{K}{T}& -\frac{1}{T}& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& -{\omega }_0^2& -2\varsigma {\omega }_0\end{array}\right),$$

(44)

$$B=\left(\begin{array}{c}0\\ \frac{K}{T}\\ 0\\ 0\\ 0\end{array}\right),$$

(45)

$$C=\left(\begin{array}{c}0\\ 0\\ 1\\ 0\\ 1\end{array}\right).$$

(46)

The error of the state vector can be denoted as $E=\widehat{X}-X$, of which the differential equation is:

$$\dot{E}=(A-GC)E.$$

(47)

To keep the asymptotic stability of the error vector, the solution of Equation (47) should be $E\left(t\right)=E_0{e}^{-kt}$, where $E_0$ is the initial error value and $k$ is a scale factor. Thus, all of the eigenvalues of the matrix $\left(A-GC\right)$ have negative real parts. The gain matrix $G$ can be found by pole placement in terms of the eigenvalues. In this case, the eigenvector of the matrix $\left(A-GC\right)$ is chosen as: $P=\left(P_0,P_1,P_2,P_3,P_4\right)=\left(1.5/T,1.5/T,1.5/T,15\varsigma {\omega }_0,15\varsigma {\omega }_0\right)$ and the gain matrix can be obtained by using the MATLAB function.

4. Guidance Law

Systems for guidance consist of a waypoint generator with human interface. A new hyperbolic guidance method for straight-line path-following control is presented in this paper. A Lyapunov function analysis [38] is used to prove the stability of this method. For curve guidance, based on the Lyapunov stability function, this paper presents a reverse stepping method to calculate heading rate command and make the system globally asymptotically stable, with a correction formula for current interference.

4.1. Error Coordinates

Consider a straight-line path implicitly defined by two waypoints $p_k={\left(x_k,y_k\right)}^T\in R^2$ and $p_{k+1}={\left(x_{k+1},y_{k+1}\right)}^T\in R^2$, respectively. Also, consider the position of the ship denoted by the point $p_t={\left(x_t,y_t\right)}^T\in R^2$. Then the direction of the path can be defined as:

$${\alpha }_k:=\mathrm{atan}2(x_{k+1}-x_k,y_{k+1}-y_k).$$

(48)

Fossen [4] gives the formula to calculate the cross-track error:

$$\tilde{P}(t)=-\left[x_t-x_k\right]\cos ({\alpha }_k)+\left[y_t-y_k\right]sin({\alpha }_k).$$

(49)

The calculation of the cross-track error in the transition curved path between two adjacent straight-line paths is complicated. Figure 2 shows two adjacent straight-line paths described by the waypoints $p_k$,$p_{k+1}$ and $p_{k+2}$. $R_{k+1}$ is the radius of the transition curve and the cross-track error of the transition curved path can be obtained by:

First, determine the direction of the curved path (clockwise or counterclockwise). Based on Equation (48), the direction ${\alpha }_k$ of the straight-line path $p_k{p}_{k+1}$ and the direction ${\alpha }_{k+1}$ of the straight-line path $p_{k+1}{p}_{k+2}$ can be calculated. Then the direction of the curved path is:

$$F_{k+1}=\mathrm{sign}\left(sin({\alpha }_{k+1}-{\alpha }_k)\right);$$

(50)

here, $F_{k+1}$ denotes the direction of the curved path, a positive sign means clockwise and a negative sign means counterclockwise.

Second, determine the center point of the curved path. The angle between the two adjacent straight-line paths can be obtained in terms of the direction ${\alpha }_k$, ${\alpha }_{k+1}$ and $F_{k+1}$.

$${\gamma }_{k+1}=\pi -F_{k+1}({\alpha }_{k+1}-{\alpha }_k).$$

(51)

Then the distance between the center point and the waypoint $p_{k+1}$ can be determined in terms of the angle ${\gamma }_{k+1}$ and the radius $R_{k+1}$.

$$D_{k+1}=R_{k+1}/\sin \left(\frac{{\gamma }_{k+1}}{2}\right).$$

(52)

The angle between the vector from the center point $O_{k+1}$ to the waypoint $p_{k+1}$ and North can be obtained in terms of the angle ${\gamma }_{k+1}$, the direction $F_{k+1}$ and ${\alpha }_k$.

$${\alpha }_{O_{k+1}}={\alpha }_k-F_{k+1}{\gamma }_{k+1}.$$

(53)

Based on the waypoint $p_{k+1}$, the direction ${\alpha }_{O_{k+1}}$ and the distance $D_{k+1}$ from the center point $O_{k+1}$ to the waypoint $p_{k+1}$, the coordinate $P_{O_{k+1}}=\left(x_{O_{k+1}},y_{O_{k+1}}\right)$ of the center point $O_{k+1}$ can be determined:

$${\mathit{p}}_{O_{k+1}}={\mathit{p}}_{k+1}-R_{k+1}\left(\begin{array}{c}\sin {\alpha }_{O_{k+1}}\\ \cos {\alpha }_{O_{k+1}}\end{array}\right).$$

(54)

Finally, the cross-track error can be calculated by:

$$\tilde{P}(t)=F_{k+1}\Vert {\mathit{p}}_t-{\mathit{p}}_{O_{k+1}}\Vert -R_{k+1}.$$

(55)

The course angle command is the direction of the vector tangential to the point on the path that is closest to the vehicle. The course angle command can be calculated by:

$$\tilde{\chi }(t)=\chi (t)-atan2\left(x(t)-x_{O_{k+1}},y(t)-y_{O_{k+1}}\right)+\frac{\pi }{2}{F}_{k+1};$$

(56)

here, $\chi \left(t\right)$ is the current course.

4.2. Straight-Line Path Guidance

The main goal of straight-line path-following control is to eliminate cross-track and heading errors. Hyperbolic guidance will achieve a more rapid convergence of path-following maneuvering because of its smoothing and transition properties. Thus, a hyperbolic guidance methodology is adopted with a hyperbolic tangent function. As shown in Figure 3, the curve OC denotes a hyperbola, of which the asymptote is the straight-line path. The hyperbola equation is:

$$y=R_c\frac{e^{k_{c}x}-e^{-k_{c}x}}{e^{k_{c}x}+e^{-k_{c}x}},$$

(57)

where $R_c$ denotes the distance between the origin $O$ and the straight-line and $\tilde{P}(t)$ is the distance from the ship to the planned route at time $t$ with $R_c>\tilde{P}(t)$.

Then, the position of the ship $p_t$ can be denoted by $\left(x_0,R_c-\tilde{P}\left(t\right)\right)$ and the angle $\zeta$ shown in Figure 3 can be calculated by:

$$\zeta (t)=\mathrm{sign}\left(\tilde{P}(t)\right)\arctan \left[k_c\left(2\left|\tilde{P}(t)\right|-\frac{\tilde{P}{(t)}^2}{R_c}\right)\right],$$

(58)

where $k_c$ is a constant coefficient with $k_c>0$, denoting the rate of approach to the straight-line path.

Then the course command can be obtained by:

$${\chi }_c={\alpha }_k+\zeta (t)={\alpha }_k+\mathrm{sign}\left(\tilde{P}(t)\right)\arctan \left[k_c\left(2\left|\tilde{P}(t)\right|-\frac{\tilde{P}{(t)}^2}{R_c}\right)\right],$$

(59)

where ${\chi }_c$ is the command course and ${\alpha }_k$ is initial heading angle.

4.3. Curved Path Guidance

For curved path following, a kinematic controller generates the desired states for motion control. The control method can be designed using a dynamic model of the ship by specifying a reference frame that moves along the path. A Serret-Frenet reference frame [39] is usually chosen. Figure 4 shows an inertia reference frame $\left\{i\right\}=\left(x_i,y_i,z_i\right)$, a body-fixed reference frame $\left\{s\right\}=\left(x_s,y_s,z_s\right)$ and a Serret-Frenet frame $\left\{m\right\}=\left(x_m,y_m,z_m\right)$. The origin $O_m$ of $\left\{m\right\}$ is attached to the point on the path that is closest to the vehicle. R denotes the radius of the curved path.

The notation $H_{a/b}^c$ is adopted in this paper, where $\left\{a\right\}$, $\left\{b\right\}$ and $\left\{c\right\}$ denote the three different frames and $H$ denotes the coordinate of $\left\{a\right\}$ in frame $\left\{b\right\}$ relative to $\left\{c\right\}$. Such that:

$$r_{m/i}^i+r_{s/m}^i=r_{s/i}^i,$$

(60)

where $r$ denotes the distance vector.

Time differentiation of Equation (60) yields:

$$v_{m/i}^i+\frac{d}{dt}{r}_{s/m}^i=v_{s/i}^i,$$

(61)

where $v$ denotes the speed vector. And $\frac{d}{dt}{r}_{s/m}^i$ can be obtained by:

$$\frac{d}{dt}{r}_{s/m}^i=R_m^i{\omega }_{m/i}^i\times r_{s/m}^m+R_m^i{v}_{s/m}^m,$$

(62)

where $R_m^i$ denotes the transformation matrix from $\left\{m\right\}$ to $\left\{i\right\}$ and ${\omega }_{m/i}^i={\left(0,0,-\dot{\alpha }\right)}^T$ denotes the rotation vector between the $\left\{m\right\}$ and $\left\{i\right\}$ frames.

Substituting Equation (62) into Equation (61) and multiplying by $R_m^i$ on both sides of the equation, yields:

$$v_{m/i}^m+{\omega }_{m/i}^i\times r_{s/m}^m+v_{s/m}^m=R_s^m{v}_{s/i}^s.$$

(63)

Then the kinematic equations can be obtained:

$$\{\begin{array}{l}\dot{\tilde{P}}=-u\sin \tilde{\chi }\\ \dot{\alpha }=\frac{u\cos \tilde{\chi }}{R+F\tilde{P}}\end{array},$$

(64)

where $\tilde{P}$ is the position deviation, $\tilde{\chi }$ is the course deviation, $\alpha$ is the planned course and $F$ indicates the direction of the transition curve. $F=1$ means turning clockwise, $F=-1$ means turning counterclockwise.

Thus, goal of the curved path-following controller is to calculate the yaw rate commands, forcing the ship to approach the curved path. This paper presents a reverse stepping calculation method based on the Lyapunov function to obtain the yaw rate commands.

First, calculate the course commands. Consider the Lyapunov function candidate as follows:

$$V_1=\frac{1}{2}{\tilde{P}}^2.$$

(65)

Time differentiation of Equation (65) yields:

$${\dot{V}}_1=-\tilde{P}u\sin \left(\chi -\alpha \right).$$

(66)

To satisfy global asymptotic stability (GAS), it is necessary to guarantee that $V_1<0$ as $\tilde{P}\ne 0$ and ${\dot{V}}_1=0$ as $\tilde{P}=0$. Thus, the following equation is adopted:

$${\dot{V}}_1=-k_1{\tilde{P}}^2\le 0,$$

(67)

where $k_1$ is the scaling factor, with $k_1>0$.

Based on Equation (65) and Equation (66), the course commands can be calculated by:

$${\chi }_c=\alpha +\arcsin \left(\frac{k_1\tilde{P}}{u}\right).$$

(68)

Equation (68) suggests that the course command ${\chi }_c$ will approach the path direction $\alpha$ to eliminate the course error $\tilde{\chi }$, as the cross-track error is decreasing. However, the path direction $\alpha$ is varying with ship motion during the curved path-following control. Thus, it is necessary to calculate the yaw rate commands. Consider the Lyapunov function candidate:

$$V_2=\frac{1}{2}{\tilde{\chi }}^2,$$

(69)

with $\tilde{\chi }=\chi -{\chi }_c$. Time differentiation of Equation (69) yields:

$${\dot{V}}_2=\left(\chi -{\chi }_c\right)\left(\dot{\chi }-\dot{\alpha }-\frac{\dot{\tilde{P}}}{\sqrt{{\left(u/k_1\right)}^2-{\tilde{P}}^2}}\right).$$

(70)

To satisfy GAS, it is necessary to guarantee that ${\dot{V}}_2<0$ as $\chi ={\chi }_c$ and ${\dot{V}}_2=0$ as $\chi ={\chi }_c$. Thus, the following equation is adopted:

$${\dot{V}}_2=-k_2{\tilde{\chi }}^2,$$

(71)

where $k_2$ is the scaling factor, with $k_2>0$.

Based on Equations (64), (68), (70) and (71), the course commands can be calculated by:

$${\dot{\chi }}_c=\frac{u\cos \tilde{\chi }}{R+F\tilde{P}}-\frac{-u\sin \tilde{\chi }}{\sqrt{{(u/k_1)}^2-{\tilde{P}}^2}}-k_2\tilde{\chi },$$

(72)

where ${\dot{\chi }}_c$ is the derivative of ${\chi }_c$, indicating the command track angular rate. We neglect the varying of the drift angle $\beta$, thus the varying rate course command can be treated as the yaw rate command, approximately. However, this does not fit in the presence of wave disturbance. In the presence of sea current interference, the yaw rate commands need to be adjusted as:

$$r_c={\dot{\chi }}_c\frac{u_g}{u_g-u_f\cos ({\phi }_g-{\phi }_f)},$$

(73)

where $r_c$ denotes the yaw rate command, $u_g$ denotes the ship speed over the ground, with direction ${\phi }_g$ and $u_f$ is the current speed through water, with direction ${\phi }_f$.

Figure 5 is the three-layer control structure of ship track control proposed in this paper.

The track control module calculates the current planned navigation section of the ship according to the current ship position information and environmental interference information and calculates the track deviation and heading deviation, then modifies the planned heading angle according to the deviation information, calculates the command heading angle and sends it to the heading control module. According to the current compass signal, rudder angle signal and yaw speed signal, the course control module calculates the course deviation after comparing with the command heading and calculates the command rudder angle through the course control algorithm.

Finally, the rudder angle control layer drives the rudder to make the actual rudder angle consistent with the command rudder angle, so as to realize the track control of the ship.

Figure 6 shows the path-following control overall flow chart:

5. Simulation Results

International Standard IEC62065 is used to test the performance of the path-following control algorithm presented in this paper. Based on the standard, three classes of ship are adopted: class A fast ferry, class B container and class C tanker. See

Table 2. Parameters for three ships.

Parameter	Notation	Ship A Fast Ferry	Ship B Container	Ship C Tanker
Ship length $\left(m\right)$	$L$	60	250	350
Thrust lever ramp time $\left(s\right)$	$T_p$	20	30	30
Rudder ramp time $\left(\mathrm{s}\right)$	$T_{\delta }$	12	30	30
Rudder follow-up offset $(\%)$	${\delta }_n$	0	0	0
Maximum speed $\left(\mathrm{kn}\right)$	$u_{max}$	30	25	10
Rudder moment coefficient $\left(\mathrm{deg}/\mathrm{s}/\%\right)$	$K_r’$	0.025	0.01	0.005
Surge response time constant $\left(\mathrm{s}\right)$	${\tau }_u$	150	600	800
Sway response time constant $\left(\mathrm{s}\right)$	${\tau }_v$	2	4	36
Yaw response time constant $\left(\mathrm{s}\right)$	${\tau }_r$	4	23	46
Stability coefficient	$\gamma$	−0.05	0	0

for details.

The heading controller parameters are given in

Table 3. Heading controller parameters.

	$\widehat{\mathbf{K}}$	$\widehat{\mathbf{T}}$	$\widehat{\mathbf{K}}$v	Q	R	k1	k2	k3
Ship A	0.0798	11.1343	1.9899	diag(100, 0.5, 1)	5	3.0261	0.1414	0.2083
Ship B	0.0794	10.8759	1.9863	diag(100, 0.5, 1)	5	2.9936	0.1414	0.2090
Ship C	0.0794	10.8966	1.9913	diag(100, 0.5, 1)	5	2.9964	0.1414	0.2090

:

The ships’ initial states for the three test scenarios are as follows:

Ship A:(u0, v0, r0, ψ0) = (15.4 m/s, 0 m/s, 0 rad/s, 0 deg);

Ship B:(u0, v0, r0, ψ0) = (12.9 m/s, 0 m/s, 0 rad/s, 0 deg);

Ship C:(u0, v0, r0, ψ0) = (5.1 m/s, 0 m/s, 0 rad/s, 65 deg);

In control applications, it is important to validate the controller experimentally, this paper takes ship class B as the simulation object and the ship parameters are shown in Table 2. The step steering signal is selected as the input signal and the rotation experiment is used as the experimental method of controller parameter identification. The trajectory is shown in Figure 7 and the simulation data is shown in

Table 4. Simulation data of class B ship rotation experiment.

Data Name	Experimental Data	Actual Data	Error
Rotation diameter (nm)	0.337	0.336	0.001
Fixed length rotation speed (deg/min)	71	71	0
Steady surge speed (kn)	12.9	12.9	0
Steady sway speed (kn)	−1.03	−1.03	0
Tactical cycle diameter (nm)	0.640	0.640	0
Advance (nm)	0.555	0.551	0.004
Departure (nm)	0.325	0.323	0.002

.

It can be seen from the experimental results that the control model established in this paper meets the ship’s turning characteristics and the established model is reliable.

Secondly, the course control algorithms of the three types of ships in Table 2 are verified. For ship class A, the initial speed of the ship is 15.4m/s, the command thrust is 0.67, under sea state 3 and the command heading angle is 30 degrees. As for ship class B, the initial speed is 12.9 m/s, the command thrust is 0.8, under sea state 3 and the command heading angle is 30 degrees. For ship class C, the initial speed of the ship is 5.1m/s, the command thrust is 1, under sea state 3 and the command heading angle is 30 degrees. Figure 8, Figure 9 and Figure 10 shows that the improved LQR control algorithm in this paper is compared with the traditional LQR algorithm and the simulation experimental chart of course control is obtained. The simulation data is shown in

Table 5. The results of the heading control test.

Types of Ships	Control Method	Overshoot (°)	Steady State Mean Deviation (°)	Steady State Maximum Deviation (°)	Response Time(s)
Ship class A Fast ferry	Traditional LQR	1.700	0.625	0.500	253.3
	Improved LQR	2.600	0.365	0.200	142.5
Ship class B Container	Traditional LQR	4.600	0.395	0.600	215.1
	Improved LQR	0.100	0.075	0.100	125.7
Ship class C Tanker	Traditional LQR	0.900	0.450	0.900	190.5
	Improved LQR	0.300	0.275	0.100	459.6

.

Based on the above experimental results, the following conclusions can be drawn: the improved algorithm combining LQR with feedback nonlinearization control can achieve the function of ship course control, the overshoot, control accuracy and corresponding time are better than the traditional LQR control method. In addition, by adding a state observer can ensure that the ship can sail according to the expected instructions under sea state 3 without frequent steering.

There are three test scenarios used to evaluate performance of the path-following controller. Details of the scenarios are given in

Table 6. The parameters of the track for ship class A.

Waypoint No.	Latitude	Longitude	Track [deg]	Distance [NM]	Radius [NM]	Estimated ROT [deg/min]
001	00°01.000′ S	000°01.000′ W	000.0	2.00	0.25	80
002	00°01.000′ N	000°01.000′ W	090.0	2.00	0.25	80
003	00°01.000′ N	000°01.000′ E	315.0	1.41	0.10	200
004	00°02.000′ N	000°00.000′ E	225.0	1.41	0.20	100
005	00°01.000′ N	000°01.000′ W	135.0	2.83	0.60	33
006	00°01.000′ S	000°01.000′ E	270.0	2.00	0.20	100
007	00°01.000′ S	000°01.000′ W	045.0	2.83	0.25	80
008	00°01.000′ N	000°01.000′ E	180.0	2.00	0.40	50
009	00°01.000′ S	000°01.000′ E

,

Table 7. Parameters of the track for ship class B.

Waypoint No.	Latitude	Longitude	Track [deg]	Distance [NM]	Radius [NM]	Estimated ROT [deg/min]
001	65°00.000′ N	000°20.000′ W	040.2	6.54	0.50	40
002	65°05.000′ N	000°10.000′ W	139.8	13.09	1.0	20
003	64°55.000′ N	000°10.000′ E	040.2	6.55	2.0	10
004	65°00.000′ N	000°20.000′ E

and

Table 8. Parameters of the track for ship class C.

Waypoint No.	Latitude	Longitude	Track [deg]	Distance [NM]	Radius [NM]	Estimated ROT [deg/min]
001	00°03.000′ S	179°57.000′ W	000.0	6.00	1.00	10
002	00°03.000′ N	179°57.000′ W	270.0	6.00	1.00	10
003	00°03.000′ N	179°57.000′ E	045.0	4.24	0.50	20
004	00°06.000′ N	180°00.000′ W	135.0	4.24	1.00	10
005	00°03.000′ N	179°57.000′ W	225.0	8.49	1.50	7
006	00°03.000′ S	179°57.000′ E	090.0	6.00	1.00	10
007	00°03.000′ S	179°57.000′ W	315.0	8.49	0.75	13
008	00°03.000′ N	179°57.000′ E	180.0	6.00	1.25	8
009	00°03.000′ S	179°57.000′ E

.

Results of testing the three classes of ship are shown in Figure 11, Figure 12 and Figure 13. WPTs in the figure are waypoints.

From Figure 11, we can see that ship class A has a maximum cross-track error of 24.8 m and the maximum course error 11.3°, which satisfies the requirements set by IEC62065 of setting the course deviation limit to 25° and the cross-track deviation to 100 m. The experimental results show that this algorithm has the advantage of less steering time.

Results for ship class B are shown in Figure 12, with a maximum cross-track error of 22 m and maximum course error of 1.6°, which satisfies the requirements of setting the course deviation limit to 15° and the cross-track deviation to 60 m.

Results for ship class C are shown in Figure 13, where the experimental environment is under rough sea state 5, with maximum cross-track error of 27.24 m and maximum course error of 3.9°, which satisfies the requirements of setting the course deviation limit to 15° and the cross-track deviation to 100 m.

Tests of ship class B in the presence of wave disturbance and a simulated current of 5 kn are also performed. The results are shown in Figure 14.

As shown in Figure 14, in the presence of sea state 3 and a simulated current of 5 kn, 30°N and the other requirements for test are the same as those for ship class B listed earlier, the results for ship class B satisfy the IEC62065 requirements with maximum cross-track error of 30.1 m and maximum course error of 3.96°. The experimental result shows that the proposed algorithm has demonstrated good robustness.

To further verify the control effect of the proposed algorithm, additional comparative experiments have been done. For ship class B, the traditional and adaptive PID control algorithms were used for track control, as shown in Figure 15. The experimental results are also compared with traditional LQR controller and the improved LQR control algorithm with nonlinear feedback term proposed in this paper and the track deviations are shown in Figure 16. Specific data are in

Table 9. Experimental result for comparison of different control algorithms.

Parameters	Overshoot (m)	Peak Time (s)	Rising Time (s)	Setting Time (s)
Traditional PID	176.9	532, 1305	243	1650
Adaptive PID	48.51	527	135	1080
Traditional LQR	23.32	1406	162	1097
Improved LQR	22	1024	94	690

. It can be seen from the experimental results that the traditional LQR control method has better tracking effect in the tracking task of direct sail or low speed tracking simple curve. However, it is difficult to guarantee the tracking accuracy in tracking complex task path. The maximum deviation under traditional PID controller, adaptive PID controller, traditional LQR controller and improved LQR controller are as follows: 176.9 m, 48.51 m, 23.32m and 22 m. In terms of stability, the traditional LQR controller performs poorly. And for the improved LQR controller, the deviation is around 0 for more than 3000 s. The performance of the control method proposed in this paper is significantly better than that of the other three control methods.

6. Conclusions

A new practical guidance and control algorithm for marine vehicles is introduced in this paper. The main innovation is the hyperbolic guidance law used in straight-line path-following control. For curved path-following in the transition between two adjacent straight-line paths, this paper improves the reverse stepping method for the controller to calculate heading rate command to make the system globally asymptotically stable, with a correction formula for sea current interference. Traditional LQR and feedback nonlinear control are combined to improve the accuracy of course control. Different types of ships are verified in this paper, simulation results based on the International Standard IEC62065 are included to demonstrate system performance. The algorithms satisfy the IEC62065 testing requirements. Three additional experiments verified that the control method has good robustness and good control effect.