Internal Model Control-Based Observer for the Sideslip Angle of an Unmanned Surface Vehicle

Abstract

Since the sideslip angle is often ignored or simplified in the process of path following of unmanned surface vehicle by using the line-of-sight (LOS) guidance law because of its fast change and the difficulty of measurement, an observer was proposed by internal model control (IMC) to quickly estimate the sideslip angle in the LOS guidance law. First, a prediction model was established for the tracking error, and a state space model for prediction errors was constructed as an internal model. With the introduction of the auxiliary variables, a new augmented system was set up for a state space model of the prediction errors. Then, the sideslip angle observer was designed by the theory of state feedback with the feature of the control law of a proportional-integral type. Theoretically, the stability of the system was proved based on the Lyapunov criteria. A simulation and experiment verified the effectiveness of the proposed sideslip angle observer in improving the path-following accuracy. The results show that the IMC-based observer introduces a proportional term of tracking error that is not considered by other observers, which is easier to implement and adjust, and has a faster response speed and a smaller steady-state error for the sideslip angle. In addition, the assumption of a small sideslip angle is not introduced in the design process, so the proposed observer provides an accurate estimation method for a large sideslip angle.

1. Introduction

Unmanned surface vehicles (USV) are widely used in military and civil applications due to their small size, low cost, and maneuverability [1]. To accomplish the missions accurately, high-accuracy USV path following is indispensable. The path following includes a guidance algorithm, heading controller, speed controller, etc. The guidance algorithm is the basis for the USV to achieve path following. It guides the USV to follow the desired path in time according to the relative motion of the USV and the desired path, so that the USV can follow the desired path quickly and accurately. At present, there are many guidance algorithms, such as the vector field guidance, which establishes a vector field on the desired path and continuously guides the USV to turn to the optimal direction calculated by the vector field [2,3], as well as the “follow-the-carrot” approach [4], which sets virtual points on the desired path and completes the path following by chasing the virtual points. Those methods are more complicated to apply, while Line-Of-Sight (LOS) has the advantage of being more intuitive and easier to implement by imitating the helmsman’s action to guide the USV to steer in the “line of sight” direction from the current position to the target point.

Since a USV does not have lateral thrust during navigation, it is susceptible to the influence of its structure and the disturbance of the external environment, resulting in a sideslip angle [5]. The influence of the sideslip angle is not considered in the traditional LOS algorithm, so that there is always a deviation of the sideslip angle between the desired heading direction and the actual desired heading direction, which reduces the path-following accuracy. Therefore, compensating for the sideslip angle is an important research point to improve path-following accuracy.

To compensate for the sideslip angle, various advanced LOS algorithms have been proposed, such as integral LOS (ILOS) [6,7,8,9] and adaptive LOS (ALOS) [10,11,12,13,14]. ILOS introduces an integration term into the traditional LOS, which contains the cross error and the lookahead distance. The presence of a sideslip angle results in a cross error, leading to the continuous accumulation of this integration term, which generates an angle used to compensate for and mitigate the effects caused by the sideslip angle. ALOS treats the sideslip angle as an unknown constant parameter and designs an adaptive term that can be estimated in real time based on the cross error and speed, ensuring that the obtained sideslip angle converges exponentially to the actual value.

On the other hand, the observer can estimate the system state in real time. In [15], considering the sideslip angle as an unknown parameter, an LOS based on a reduced-order extended state observer was proposed to achieve real-time estimation of the sideslip angle in the small angle case. In [16], a high-gain extended state observer was used to obtain the time-varying sideslip angle to compensate for external disturbances and achieve accurate path following of a USV in restricted waters. Reference [17] obtained the longitudinal and lateral velocities of the USV through a high-gain observer to indirectly estimate the sideslip angle to compensate for the model uncertainty and unknown external disturbances. In [18], a finite-time sideslip observer was proposed to accurately estimate the time-varying large sideslip angle in finite time, which improved the speed and accuracy of path following. In [19], under the condition that the sideslip angle was small (assuming sin β ≈ β), a monomial containing the prediction error was introduced into the path-tracking error system, a predictor was constructed for predicting the along and cross error of path following, and based on this, a Lyapunov function was constructed to design the sideslip angle observer to achieve the estimation of and compensation for the sideslip angle. In [20], a finite-time predictor-based LOS (FPLOS) algorithm was proposed. Based on [19], it introduced the ρ power function of the prediction error with sign function to construct the observer and realized the estimation of and compensation for the sideslip angle in finite time. However, the FPLOS algorithm used a pure integral observer suitable for small-angle sideslip angle estimation and responded slowly to large angle sideslip angle estimation. In actual navigation, a USV is affected by the channel curvature, current, wind, and waves, which may generate a large sideslip angle, and FPLOS is not able to cope with this. Therefore, it is very important to explore the advanced control method to design the sideslip observer under the condition of unlimited small angle sideslip to solve the problem of fast estimation of the sideslip angle; it is also an effective method to improve the USV path-following accuracy.

The internal model control (IMC) has been widely used for its advantages of strong practicability, intuitive design, and easy adjustment [21,22]. In [21], an adaptive IMC that allowed parameter adjustment and model estimation was proposed to achieve autonomous landing control of a fixed-wing UAV under different wind conditions. In [22], an IMC-based PID controller was proposed for the magnetic levitation system, which improved the transient response and tracking response of the magnetic levitation system. In this paper, an IMC observer-based sideslip angle estimation method is proposed to improve the LOS algorithm (IMCLOS) to achieve fast and accurate USV path tracking. First, a prediction model is established to predict the along and cross error of path following, and the internal mode extended state equations for predicting the along and cross error are constructed. The control law of the observer is designed based on the IMC principle, which is a proportional-integral controller, and the stability of the IMC observer is proved by designing the Lyapunov function. Compared with the FPLOS algorithm, the IMCLOS algorithm is easier to implement and regulate, due to the introduction of the proportional integral of the tracking error in the observer, and has a faster response and smaller steady state error for the sideslip angle observation in the LOS algorithm.

The rest of the paper is organized as follows: Section 2 introduces the USV dynamic model, including kinematics and kinetics. Section 3 mainly introduces the design of the sideslip angle observer based on the IMC and analyzes the stability of the system based on the observer. In Section 4, the effectiveness of the observer is proved by simulation and field experiment. Finally, Section 5 presents conclusions and future work.

2. USV Dynamic Model

In order to design and validate the sideslip angle observer, a mathematical model of the USV is required. Therefore, this section presents the dynamics model of the USV.

2.1. Assumptions

A USV has six degrees of freedom motion in navigation, including surge, sway, heave, yaw, roll, and pitch, but when the USV is controlled for path following, heave, roll, and pitch have little effect on it. Therefore, some simplifications are made to its motion in the horizontal plane. These are simplified as follows:

• We ignore the USV motion in the roll, pitch, and heave directions;
• The USV is a symmetric structure, and its center of mass and the origin of the body-fixed coordinate coincide;
• We ignore the propeller thrust model and directly adopt forward thrust and turning moment as the control inputs.

2.2. USV Plane Motion Model

A USV path following plane diagram is shown in Figure 1, where x, y, and ψ are USV’s northward position, eastward position, and heading angle of the USV in the northeast coordinate system, respectively; u, v, and r are the surge speed, sway speed, and yaw speed of the USV in the hull coordinate system, L(θ) is the reference path, and θ is the path parameterization variable.

The mathematical model of the motion of the USV in the horizontal plane is

$$\{\begin{array}{l}\mathit{M}\dot{\mathit{v}}=-\mathit{C}(\mathit{v})\mathit{v}-\mathit{D}(\mathit{v})\mathit{v}+\mathit{\tau }+{\mathit{\delta }}_{\mathit{d}}\\ \dot{\mathit{\eta }}=\mathit{J}(\mathit{\psi })\mathit{v}\end{array}$$

(1)

Equation (1) contains the kinematic and kinetic equations of the USV, where η = [x y ψ]^T is the attitude vector, v = [u v r]^T is the velocity vector, J(ψ) = [cosψ − sinψ 0; sinψ cosψ 0; 0 0 1] is the coordinate transformation matrix from the USV’s hull coordinate system to the north-east coordinate system; M = [m₁₁ 0 0; 0 m₂₂ 0; 0 0 m₃₃] is the fluid inertia matrix, where $m_{11}=m-X_{\dot{u}}$, $m_{22}=m-Y_{\dot{v}}$, $m_{33}=I_{zz}-N_{\dot{r}}$, m is the mass of USV, I_zz is the inertia of the USV mass during steering, $X_{\dot{u}}$ is the longitudinal added mass, $Y_{\dot{v}}$ is the lateral additional mass, $N_{\dot{r}}$ is the additional mass generated during steering; C(v) = [0 0 c₁₃; 0 0 c₂₃; c₁₃ − c₂₃ 0] is the fluid Coriolis force matrix, where c₁₃ = −c₃₁ = −(v + 0.5)m₂₂, c₂₃= −c₃₂=m₁₁u; D(v) = [d₁₁ 0 0; 0 d₂₂ d₂₃; 0 d₃₂ d₃₃] is the fluid damping matrix, where d₁₁ = −X_u − X_|u|u|u|, d₂₂ = −Y_v − Y_|v|v|v|, d₂₃ = −Y_r, d₃₂ = −N_v, d₃₃ = −N_r − N_|v|r|v|, and X_u, Y_v, Y_r, N_v, N_r is the hydrodynamic coefficient, X_|u|u, Y_|v|v, Y_|r|r, N_|v|v, N_|v|r is the nonlinear damping factor; τ = [τ_u 0 τ_r]^T is the matrix of control forces generated by the actuator, where τ_u is the longitudinal thrust, τ_r is the bow torque; and δ_d = [δ_du δ_dv δ_dr]^T is the disturbance of surge speed, sway speed, and yaw speed by factors such as wind and current.

As seen in Figure 1, a USV navigating along the path L(θ) needs to have a path following technique, which consists of heading control, speed control, and an LOS guiding law. In this paper, we focus on an LOS algorithm regarding the sideslip angle compensation.

3. LOS Algorithm Based on Sideslip Angle Compensation

3.1. Basic Principle of LOS Algorithm

LOS is widely used for path following control. The main reason is that LOS can be independent of the dynamics model of the USV and does not depend on the mathematical model of the system; its output is the desired heading angle, which is determined by the real-time position of the USV and the reference path. The idea of LOS is to imitate the helmsman’s action, keep the heading angle of the USV aligned with the desired heading angle, and control the USV to reach the desired position to achieve path following. Figure 1 gives the geometric principle of an LOS-based USV path following, where (x_p(θ), y_p(θ)) is the projection point of the USV on the reference path, x_e and y_e are the along and cross error of the path following, Δ is the lookahead distance, β is the sideslip angle, which is the angle between the USV resultant velocity U and the surge velocity u, and α_p = arctan(x′_p(θ)/y′_p(θ)) is the angle between the tangent line at the projection point and the vertical coordinate, ψ_d is the desired heading angle, and the LOS algorithm is

$${\psi }_d={\alpha }_p-\arctan (\frac{y_e}{\Delta })-\beta$$

(2)

where β affects the value of ψ_d. In the application of the LOS algorithm, because the β value is not easy to measure, the influence of β on ψ_d is usually ignored [23,24]. As a result, the LOS is prone to produce a large tracking error under the influence of channel curvature, water flow, wind waves, and other factors; an FPLOS algorithm designed the observer to estimate the sideslip angle and compensate for the ψ_d value, thus improving the path following accuracy of a USV [20]. On the condition that the β value is very small, an observer with a tracking error integral was designed. Therefore, it is very important to explore the advanced design method of an observer and realize the fast and accurate estimation of the sideslip angle.

3.2. Sideslip Angle Observer Based on IMC

3.2.1. Path-Following Error Dynamic System

In the design of the sideslip angle observer, the output of the dynamic system of the USV path-tracking error is usually taken as the input [18,19,20]. According to (1), the along error and cross error of USV path following are

$$\left[\begin{array}{l}{x}_e\\ y_e\end{array}\right]=\left[\begin{array}{cc}\cos {\alpha }_p& \sin {\alpha }_p\\ -\sin {\alpha }_p& \cos {\alpha }_p\end{array}\right]\left[\begin{array}{l}x-x_p\\ y-y_p\end{array}\right].$$

(3)

We derive the along error x_e and cross error y_e of path tracking with respect to time:

$$\{\begin{array}{c}{\dot{x}}_e=U\cos (\psi -{\alpha }_p+\beta )+{\dot{\alpha }}_p{y}_e-u_p\\ {\dot{y}}_e=U\sin (\psi -{\alpha }_p+\beta )-{\dot{\alpha }}_p{x}_e\end{array}$$

(4)

where u_p and ${\dot{\alpha }}_p$ are

$$u_p=\dot{\theta }\sqrt{{x^{\prime }}_p{}^2(\theta )+{y^{\prime }}_p{}^2(\theta )}$$

(5)

$${\dot{\alpha }}_p=\frac{d{\alpha }_p}{d\theta }\dot{\theta }=\frac{d{\alpha }_p}{d\theta }\frac{u_p}{\sqrt{{x^{\prime }}_p{}^2(\theta )+{y^{\prime }}_p{}^2(\theta )}}$$

(6)

where u_p is the virtual input used to stabilize the along error x_e, which will be designed later. According to (6), ${\dot{\alpha }}_p$ can be calculated according to u_p. The FPLOS algorithm assumes that β is a small angle, then sinβ ≈ β, cosβ ≈ 1. By substituting into (4), the dynamic system of tracking error can be obtained as ${\dot{x}}_e$ = Ucos(ψ − α_p) − Usin(ψ − α_p)β + $y_e{\dot{\alpha }}_p$ − u_p, ${\dot{y}}_e$ = Usin(ψ − α_p) + Ucos(ψ − α_p)β − $x_e{\dot{\alpha }}_p$. FPLOS derived from this condition limits its application scope and can only be applied to the estimation of a sideslip angle at a small angle [20]. IMCLOS does not limit the value of β and can be applied to any value of β.

Let the parameters β_x and β_y be

$$\{\begin{array}{l}{\beta }_x=\cos (\psi -{\alpha }_p+\beta )\\ {\beta }_y=\sin (\psi -{\alpha }_p+\beta )\end{array}.$$

(7)

Equation (7) can be substituted into (4) then

$$\{\begin{array}{c}{\dot{x}}_e=U{\beta }_x+{\dot{\alpha }}_p{y}_e-u_p\\ {\dot{y}}_e=U{\beta }_y-{\dot{\alpha }}_p{x}_e\end{array}.$$

(8)

Equation (8) is the error state equation of USV path following, and then the IMC observer is proposed.

3.2.2. IMC Observer Design

Since there are coupling components in (8), the prediction model of along error x_e and cross error y_e is defined as

$$\{\begin{array}{c}{\dot{\widehat{x}}}_e=U{\widehat{\beta }}_x+{\dot{\alpha }}_p{\widehat{y}}_e-u_p-k_x{\tilde{x}}_e-{\dot{\alpha }}_p{\tilde{y}}_e\\ {\dot{\widehat{y}}}_e=U{\widehat{\beta }}_y-{\dot{\alpha }}_p{\widehat{x}}_e-k_y{\tilde{y}}_e+{\dot{\alpha }}_p{\tilde{x}}_e\end{array}$$

(9)

where k_x and k_y are positive constants, ${\widehat{x}}_e$ is the estimation of x_e, ${\widehat{y}}_e$ is the estimation of y_e, ${\tilde{x}}_e={\widehat{x}}_e-x_e$ and ${\tilde{y}}_e={\widehat{y}}_e-y_e$ are prediction errors, and ${\widehat{\beta }}_x$ and ${\widehat{\beta }}_y$ are estimations of β_x and β_y.

Subtract (8) from (9), then

$$\{\begin{array}{c}{\dot{\tilde{x}}}_e=U{\tilde{\beta }}_x-k_x{\tilde{x}}_e\\ {\dot{\tilde{y}}}_e=U{\tilde{\beta }}_y-k_y{\tilde{y}}_e\end{array}$$

(10)

where

$$\{\begin{array}{c}{\tilde{\beta }}_x={\widehat{\beta }}_x-{\beta }_x\\ {\tilde{\beta }}_y={\widehat{\beta }}_y-{\beta }_y\end{array}$$

(11)

where ${\tilde{\beta }}_x$ and ${\tilde{\beta }}_y$ are prediction errors. According to (10), the differential term ${\dot{\alpha }}_p$ is eliminated, so this term has no influence on the estimation effect of the final observer. Meanwhile, it can be seen that the convergence rate of the prediction error is positively correlated with the values of k_x and k_y, which will affect the estimation effect of the observer.

According to (10), there is no mutual coupling factor between the equations of state, and the observer can be designed independently. Equation (10) provides a new equation of state for ${\tilde{x}}_e$ by

$$\{\begin{array}{l}{\dot{\tilde{x}}}_e=-k_x{\tilde{x}}_e+U{\tilde{\beta }}_x\\ y={\tilde{x}}_e\end{array}.$$

(12)

Assuming zero reference input, ${\tilde{\beta }}_x$ in (12) is regarded as the control quantity to be designed. The tracking error e is defined as

$$\{\begin{array}{l}e=0-y\\ \dot{e}=-\dot{y}=-{\dot{\tilde{x}}}_e\end{array}.$$

(13)

By introducing two new state variables z and w, it can be obtained as follows

$$\{\begin{array}{l}z={\dot{\tilde{x}}}_e\\ w={\dot{\tilde{\beta }}}_x\\ \dot{z}={\ddot{\tilde{x}}}_e=-k_x{\dot{\tilde{x}}}_e+U{\dot{\tilde{\beta }}}_x\end{array}.$$

(14)

Equations (13) and (14) constitute the new augmented system equation as follows

$$\left[\begin{array}{l}\dot{e}\\ \dot{z}\end{array}\right]=\left[\begin{array}{l}\begin{array}{cc}0& -1\end{array}\\ \begin{array}{cc}0& -k_x\end{array}\end{array}\right] \left[\begin{array}{l}e\\ z\end{array}\right]+\left[\begin{array}{l}0\\ U\end{array}\right]w\to \dot{\mathit{X}}=\mathit{A}\mathit{X}+\mathit{B}\mathit{w}$$

(15)

where A = [0 −1; 0 −k_x], and B = [0 U]^T. According to (15), the rank of the controllability matrix of the system is rank [B AB] = 2, and system Equation (15) is completely controllable. According to the principle of state feedback, the system can be controlled by state feedback. Let w = KX and K = [k₁ k₂]; so, the controlled system (15) can realize asymptotic stability, and the control quantity w can be obtained from (13) and (14) as follows

$${\dot{\tilde{\beta }}}_x=w=-k_{1}e-k_{2}z=-k_1{\tilde{x}}_e-k_2{\dot{\tilde{x}}}_e$$

(16)

Similarly, a control law can be designed for the observer of ${\tilde{y}}_e$ by

$${\dot{\tilde{\beta }}}_y=-k_1{\tilde{y}}_e-k_2{\dot{\tilde{y}}}_e$$

(17)

In order to make the closed-loop system $\dot{\mathit{X}}=(\mathit{A}-\mathit{B}\mathit{K})\mathit{X}$ formed by feedback of (15) asymptotically stable, that is, the eigenvalues of A − BK all have negative real roots, then

$$k_1>0,k_2>\max \{-\frac{k_x}{U},-\frac{k_y}{U}\}.$$

(18)

At the same time, φ_p = ψ − α_p + β is unchanged in a sampling period of the control system, that is, ${\dot{\phi }}_p=0$. This gives us the derivative of (11) as follows

$$\{\begin{array}{l}{\dot{\widehat{\beta }}}_x={\dot{\tilde{\beta }}}_x+{\beta }_x={\dot{\tilde{\beta }}}_x+{\beta }_y{\dot{\varphi }}_p={\dot{\tilde{\beta }}}_x\\ {\dot{\widehat{\beta }}}_y={\dot{\tilde{\beta }}}_y+{\beta }_y={\dot{\tilde{\beta }}}_y-{\beta }_x{\dot{\varphi }}_p={\dot{\tilde{\beta }}}_y\end{array}.$$

(19)

Substituting (16) and (17) into (19), it can be obtained as follows

$$\{\begin{array}{l}{\dot{\widehat{\beta }}}_x=-k_1{\tilde{x}}_e-k_2{\dot{\tilde{x}}}_e\\ {\dot{\widehat{\beta }}}_y=-k_1{\tilde{y}}_e-k_2{\dot{\tilde{y}}}_e\end{array}.$$

(20)

It can be seen from (20) that the control law of IMC observer is composed of a proportional term and integral term about the tracking error, while the observer designed in reference [20] is in pure integral form. Therefore, IMCLOS has a faster response speed and a smaller steady-state error to the sideslip angle. On the other hand, the values of k₁ and k₂ are related to the prediction error, and the excessively large values of k_x and k_y in (10) will cause the prediction error to fluctuate in a small range. In order to accurately predict the sideslip angle, large values of k₁ and k₂ are required, but the control law contains the proportion term, which will cause the sensor measurement error to be amplified. Therefore, the values of k_x and k_y should not be too large.

According to (7) and (20), the estimation of the sideslip angle is

$$\widehat{\beta }=\arctan (\frac{{\widehat{\beta }}_y}{{\widehat{\beta }}_x})-\psi +{\alpha }_p.$$

(21)

To stabilize the along error x_e, u_p in (4) is designed as

$$u_p=k_u{\widehat{x}}_e+U\cos (\psi -{\alpha }_p+\widehat{\beta }).$$

(22)

The predicted sideslip angle $\widehat{\beta }$ obtained in (21) is substituted for the actual sideslip angle β in (2) to complete the design of IMCLOS algorithm.

3.3. Stability Analysis of IMC Observer

The IMC observer is

$$\{\begin{array}{l}{\dot{\widehat{x}}}_e=U{\widehat{\beta }}_x+{\dot{\alpha }}_p{\widehat{y}}_e-u_p-k_x{\tilde{x}}_e-{\dot{\alpha }}_p{\tilde{y}}_e\\ {\dot{\widehat{y}}}_e=U{\widehat{\beta }}_y-{\dot{\alpha }}_p{\widehat{x}}_e-k_y{\tilde{y}}_e+{\dot{\alpha }}_p{\tilde{x}}_e\\ {\dot{\widehat{\beta }}}_x=-k_1{\tilde{x}}_e-k_2{\dot{\tilde{x}}}_e\\ {\dot{\widehat{\beta }}}_y=-k_1{\tilde{y}}_e-k_2{\dot{\tilde{y}}}_e\end{array}.$$

(23)

According to (8), (9), and (23), the prediction error dynamic system is

$$\{\begin{array}{l}{\dot{\tilde{x}}}_e=U{\tilde{\beta }}_x-k_x{\tilde{x}}_e\\ {\dot{\tilde{y}}}_e=U{\tilde{\beta }}_y-k_y{\tilde{y}}_e\\ {\dot{\tilde{\beta }}}_x=-k_1{\tilde{x}}_e-k_2{\dot{\tilde{x}}}_e\\ {\dot{\tilde{\beta }}}_y=-k_1{\tilde{y}}_e-k_2{\dot{\tilde{y}}}_e\end{array}.$$

(24)

It is assumed that the speed controller and heading controller can achieve the control target without steady error, i.e., U = U_d and ψ = ψ_d. Based on this assumption, we assign the following Lyapunov function candidate V(x) as

$$V(\mathit{x})=\frac{1}{2}a{\tilde{x}}_e^2+b{\tilde{x}}_e{\tilde{\beta }}_x+\frac{1}{2}c{\tilde{\beta }}_x^2+\frac{1}{2}a{\tilde{y}}_e^2+b{\tilde{y}}_e{\tilde{\beta }}_y+\frac{1}{2}c{\tilde{\beta }}_y^2={\mathit{x}}^T\mathit{P}\mathit{x}$$

(25)

where $a=k_1+k_2^{2}U$, b = k₂U, c = U, $x={[{\tilde{x}}_e {\tilde{y}}_e {\tilde{\beta }}_x {\tilde{\beta }}_y]}^{\mathrm{T}}$, and the matrix P is

$$\mathit{P}=\frac{1}{2}\left[\begin{array}{cccc}a& b& 0& 0\\ b& c& 0& b\\ 0& 0& a& 0\\ 0& 0& b& c\end{array}\right].$$

(26)

According to the Hurwitz criterion, Δ₁ > 0, Δ₂ > 0, Δ₃ > 0, and Δ₄ > 0; then, the matrix P is a positive definite matrix, i.e., V(x) > 0.

The derivative of (25) is

$$\begin{array}{ll}\dot{V}(\mathit{x})& =a{\tilde{x}}_e{\dot{\tilde{x}}}_e+b({\dot{\tilde{x}}}_e{\tilde{\beta }}_x+{\tilde{x}}_e{\dot{\tilde{\beta }}}_x)+c{\tilde{\beta }}_x{\dot{\tilde{\beta }}}_x+a{\dot{\tilde{y}}}_e{\tilde{y}}_e+b({\dot{\tilde{y}}}_e{\tilde{\beta }}_y+{\tilde{y}}_e{\dot{\tilde{\beta }}}_y)+c{\tilde{\beta }}_y{\dot{\tilde{\beta }}}_y\\ & =-k_1(k_x+k_{2}U){\tilde{x}}_e^2-k_1(k_y+k_{2}U){\tilde{y}}_e^2\end{array}$$

(27)

Substituting (18) into (27), $\dot{\mathit{V}}(\mathit{x})<0$ can be obtained, so the IMC observer (23) is asymptotically stable. Since the design of the controller is not the focus of this paper, we have chosen the more widely used PI controller. The proof is based on the assumption that the controller part is stable; then, the stability of the whole closed-loop system is proved.

4. Simulation and Experiment

4.1. Sideslip Angle Observer Based on IMC

Figure 2 shows the USV experimental platform, which consists of six modules: USV hull, navigation controller, RC receiver, electronic speed controller (ESC)+ brushless motor, GNSS positioning module, and data transmission module, where the USV hull is 1.18 m long, 0.355 m wide, and weighs 8.46 kg, and the USV-related kinetic parameters are given in

Table 1. USV-related kinetic parameters.

Parameters	Units	Value
$I_{zz}$	kg·m2	1.07
$X_{\dot{u}}$	kg	−0.42
$Y_{\dot{v}}$	kg	−1.07
$N_{\dot{r}}$	kg·m	−0.13
$X_u$	kg/s	4.39
$Y_v$	kg/s	20
$Y_r$	kg·m/s	−0.2
$N_v$	kg/s	−0.2
$N_r$	kg·m/s	5
$X_{\left|u\right|u}$	kg/m	−10.65
$Y_{\left|v\right|v}$	kg/m	−15.6
$N_{\left|v\right|r}$	kg·m2	−15

, obtained by experimental tests. The navigation controller model is V5+ with a built-in accelerometer, gyroscope, electronic compass, and barometer, which can obtain real-time speed, acceleration, and a heading angle of the USV at 50 Hz; the model of the GNSS positioning module is a NEO V2 with a sampling frequency of 10 Hz; the USV is propelled by dual brushless motors with differential steering, and the motor model is CYS3660 with maximum power of 1800 W.

In the simulation and outfield experiments, the path following control system shown in Figure 3 was used, and (9) and (20) constituted the observer about ${\widehat{\beta }}_x$. Similarly, the value of ${\widehat{\beta }}_y$ can be obtained. The estimation of the sideslip angle $\widehat{\beta }$ was obtained from (21). The speed was controlled by PI, the input was the expected speed U_d, and the real-time speed measured by the sensor was the feedback state. Its parameters were k_p = 1.0 and k_i = 0.4. The heading was controlled by PI. The expected heading angle obtained by the LOS algorithm was used as the input of the heading controller, and the real-time heading angle measured by the sensor was used as the feedback state. The heading controller parameters were k_p = 0.5 and k_i = 0.2. The parameters of the IMCLOS algorithm were set to k_x = 10, k_y = 10, k_u = 3, ∆ = 1, k₁ = 100, and k₂ = 50. To verify the merits of the IMCLOS algorithm, a comparison was made with the FPLOS algorithm. The parameters of the FPLOS algorithm were set to k_x = 10, k_y = 10, k_u = 3, ∆ = 1, k₁ = 100, and ρ = 0.8, where ρ was set to the same value as in [20]. As the USV is following the path, it is likely to produce a large sideslip angle due to the great curvature change of the path and external interference. Therefore, it was necessary to set a path with a large curvature and analyze the effect of the observer in IMCLOS and FPLOS by the results. We set the desired path following speed to 0.5 m/s and the reference path L(θ) as

$$\{\begin{array}{l}x=\theta \\ y=2\sin (0.5\theta )+\theta \end{array}$$

(28)

where θ ∈ [0,+∞).

4.2. Simulation Verification

In the Matlab simulation environment, the path following control simulation model was established, the following range was set to θ∈ [0, 28), the initial speed was set to 0 m/s, the initial heading was 0 rad, and the desired speed was 0.5 m/s. Because the USV used in the experiments was similar in size and mass to the USV used in the experiments of [20], referring to [20], the disturbance quantities δ_du = 0.3 sin(0.1πt − 0.2π), δ_dv = 0.2 cos(0.5πt + 0.1π), and δ_dr = 0.3 cos(0.2πt + 0.1π) were added artificially. Under the action of the same speed controller and heading angle controller, Figure 4 shows the heading angle tracking error e_ψ for the USV path following process, which was e_ψ = ψ_d − ψ obtained respectively from IMCLOS and FPLOS. Figure 5 shows the actual USV sideslip angle β, the estimated sideslip angle $\widehat{\beta }$, and the sideslip error value $\tilde{\beta }$ obtained by the compensation of the two observers. Figure 6 shows the USV following trajectory based on the two observers, and Figure 7 shows the along error x_e and the cross error y_e in the USV path following process.

As seen in Figure 4, the USV entered the stable tracking state after about 10 s. Compared with Figure 5, it can be seen that when the sideslip angle β was small (30 s < t < 50 s and 70 s < t <90 s), both observers achieved an accurate estimation, which as reflected in Figure 6 was the interval of tending to a straight-line path tracking (10 m < x < 15 m, 8 m < y < 18 m; 22 m < x < 28 m, 20 m < y < 30 m). When the sideslip angle β was large (10 s < t < 30 s and 50 s < t < 70 s), the IMCLOS algorithm still accurately estimated the actual sideslip angle β in real time with a maximum prediction error of about ±0.002 rad. The FPLOS algorithm estimates lagged behind the actual sideslip angle, indicating that the response of FPLOS was slow, resulting in a large estimation error with a maximum prediction error of about ±0.08 rad. Examining the path-tracking effect in Figure 6 for the path turn, IMCLOS had a higher prediction accuracy for the sideslip angle, and its trajectory was closer to the reference path compared to FPLOS. The path-tracking errors are shown in Figure 7. Comparing the along and cross errors, both of these with IMCLOS were smaller than with FPLOS, and the advantage was more obvious in the large sideslip angle interval. The maximum value of the cross error y_e obtained by the IMCLOS algorithm was about 0.02 m, compared with 0.06 m obtained by FPLOS; Similarly, the along error x_e obtained by IMCLOS compensation was about 33% of that obtained by FPLOS. Therefore, the simulation results show that IMCLOS maintained the performance of real-time estimation of a small sideslip angle, while being able to estimate a large sideslip angle and achieve accurate following of the reference path quickly and accurately.

4.3. Experimental Comparison

In the USV experimental platform, the control algorithm was implemented by modifying the flight control firmware ArduPilot (copyright by ArduPilot Dev Team). ArduPilot mainly contains five parts: a vehicle code, shared libraries, a hardware abstraction layer, tools libraries, and an external support code. Based on this, a new operating mode-LOS_Mode was added to the vehicle code, in which library functions were called to obtain the real-time position, speed, and heading angle measured by the USV sensors, and the control algorithms based on IMCLOS and FPLOS were programmed in C++ to output the desired heading angle command and call the motor control functions to realize the USV heading and speed control during the path-following process. After finishing the navigation, the ground station MissionPlanner was used to read the navigation log, and Matlab was used to analyze the data generated in the first 100 s and plot the curve. The flow chart of the algorithm implementation in the USV platform is shown in Figure 8. Figure 9 shows the heading angle tracking error e_ψ for the USV path-following process. Figure 10 shows the estimated USV sideslip angle obtained by the two observers. Figure 11; Figure 12 show the USV following trajectory displayed in the ground station, where the solid line is the actual trajectory, and the dashed line is the reference path. Figure 13 shows the USV following trajectory plotted from the ground station derived data, and Figure 14 shows the along error x_e and cross error y_e in the USV path-following process.

Figure 8 shows that the USV entered the stable tracking state after about 10 s. During the stable tracking phase, comparing Figure 10, the estimation results of IMCLOS and FPLOS were more similar when the sideslip angle β was small (30 s < t < 50 s and 70 s < t < 90 s), which are reflected in Figure 10, Figure 11 and Figure 12; the interval of tending to a straight-line path following (10 m < x < 15 m, 8 m < y < 18 m; 22 m < x < 28 m, 20 m < y < 30 m) and the following effects were similar. For a larger sideslip angle β (10 s < t < 30 s and 50 s < t < 70 s), the estimation results of the two observers differed more, which are reflected in the path-following effect in Figure 11, Figure 12 and Figure 13 for the path turns. The IMCLOS-based trajectory was closer to the reference path, compared to FPLOS. Combined with Figure 14, the maximum cross error y_e after the IMCLOS-estimated compensation was about 0.04 m, which was smaller than the 0.08 m obtained by FPLOS. Similarly, the along error x_e obtained by IMCLOS compensation was about 50% of that obtained by FPLOS. The outfield experiments show that more accurate path following can be achieved by using IMCLOS estimation to compensate for the sideslip angle compared to FPLOS.

Both the experimental and simulation results show that IMCLOS had a better observation compensation effect on the sideslip angle than FPLOS, which could not only maintain the performance of real-time estimation of small sideslip angle but could also estimate a large sideslip angle quickly and accurately, and it obtained a faster response performance and smaller tracking error to achieve better path following.

5. Conclusions

In actual navigation, the sideslip angle of a USV changes rapidly and is difficult to measure. The LOS guidance algorithm often ignores or simplifies the effect of sideslip angle on the heading angle, which will reduce the path-following accuracy. In this paper, we proposed an IMC-based sideslip angle observer to achieve fast and accurate estimation of the sideslip angle to compensate with the LOS algorithm and achieve fast and accurate USV path following. Firstly, the prediction model of the tangential error and normal error was established, and the internal model extended state equation of the tangential error and normal error was constructed by using the IMC principle. Then, a new augmented system was obtained by introducing intermediate variables, and the stabilization of the system was realized by using the pole assignment method. At the same time, the stability of the designed system was proved by constructing Lyapunov function. The simulation results and the external experimental results both showed that

(1)

FPLOS was a pure integral type observer, while IMCLOS was a proportional integral type observer, which theoretically had better observer performance;

(2)

IMCLOS estimated β values faster and more accurately, especially the large-angle β value. In experiments, more accurate estimates of the sideslip angle reduced tracking errors by 50%, with more obvious advantages.

In future research, the influence of the sensor measurement error should be considered, and it is necessary to use multisensor fusion for data optimization. At the same time, obstacles and automatic collision avoidance should be considered in path following. Since the USV used was dual-propeller propulsion, a propeller thrust model should be considered and a model-based controller designed to improve tracking accuracy.