Control of Unmanned Surface Vehicle Along the Desired Trajectory Using Improved Line of Sight and Estimated Sideslip Angle


 In order to improve the accuracy and robustness of path following control for an Unmanned Surface Vehicle (USV) suffering from unknown and complex disturbances, a variable speed curve path following a control method based on an extended state observer was proposed. Firstly, the effect of the environmental disturbances on the USV is equivalent to an unknown and time-varying sideslip angle, and the sideslip angle is estimated by using the extended state observer (ESO) and compensated in the Line of Sight (LOS) guidance law. Secondly, based on the traditional LOS guidance law, the design of the surge velocity guidance law is added to enable the USV to self-adjust the surge velocity according to the curvature of the curve path, thus further improving the tracking accuracy. Finally, the heading and speed controller of the USV is designed by using a sliding mode control to track the desired heading and speed accurately, and then the path following control of the USV’s curve path is realised. Simulation results verify the effectiveness of the proposed method.


INTRODUCTION
Unmanned Surface Vehicles (USV), as unmanned intelligent marine platforms, can undertake long-term, large-scale and low-cost marine scientific research and engineering tasks in the ocean, and have a broad application prospect. However, in the course of path following, a USV is bound to be affected by wind, waves, currents and other environmental factors. After the environmental disturbance acts on the USV, it will generate sway velocity, thus forming an included angle between the actual movement direction of the USV and the heading of the USV, that is, a sideslip angle. Due to the limitation of the USV's detection means, the actual movement direction of the USV cannot be directly measured, so the sideslip angle cannot be eliminated by feedback control. Therefore, many research works have been carried out on the estimation and compensation of the sideslip angle.
In order to eliminate the sideslip angle, Børhaug [1] and Caharija [2] propose an integrated Line of Sight (LOS) guidance law. An integral term is added into the traditional LOS guidance law to compensate the sideslip angle caused by environmental disturbance. In order to make the selection of the parameters of the integral LOS guidance law more standardised, Lekkas [3] proposed an ILOS guidance law based on speed. This method requires the selection of parameters to satisfy the stability analysis. Qu [4] made improvements on the basis of ILOS guidance law, so that the integral effect could be adaptively adjusted according to the cross-path following errors, and further improved the flexibility of ILOS guidance law. Mu [5] applied the idea of a variable lookahead distance to ILOS guidance law, and improved the convergence rate and stability of the ILOS guidance law. Based on [4] and [5], Chen [6] proposed that the improved ILOS guidance law could simultaneously adjust the strength of the integral and increase the lookahead distance. The above methods are based on the assumption that the sideslip angle is small (3-5 degrees) and constant, but this is difficult to satisfy in practice. Therefore, Fan [7] directly calculated the value of the sideslip angle by using the surge velocity and the total speed of the USV, thus relaxing the assumptions on the sideslip angle. Moe [8] also uses the surge velocity and sway velocity of the USV to directly calculate the sideslip angle. However, this method has a high requirement for the measurement precision of the sensor, and the measurement noise may be amplified during the calculation. Under the assumption that the sideslip angle is small and constant, Gu [9] applies the low-frequency learning method to estimation of the sideslip angle to realise the formation control of USVs. Wang [10] realised the estimation of the sideslip angle in finite time by introducing auxiliary variables and designing a finite time disturbance observer. Aiming at the cooperative formation control of underactuated USVs, several intermediate state variables and virtual control laws are designed based on nonlinear backstepping, and actual control algorithms for the follower USVs to control the surge force and yaw moment are presented by Dong [11]. Wang [12] by modelling the switching of network topologies with the use of a Markov process and considering the effect of waveinduced disturbance, a new sampled-data consensus control protocol is proposed.
Inspired by the above method and based on the research idea of [10], the extended state observer is applied to estimate the sideslip angle, which allows the sideslip angle to be unknown, time-varying and arbitrary, and relaxes the finite time disturbance observer for the smoothness of the sideslip angle, making it more applicable and closer to practical application. At the same time, the LOS guidance law is improved, and the relationship between the curvature of the expected path and the USV surge velocity is established, so that the USV can automatically adjust the surge velocity in the process of following the expected curve path, so as to improve the tracking accuracy. Finally, the heading and speed controller of the USV is designed by using sliding mode control, and the effectiveness and superiority of the algorithm are demonstrated by simulation and comparison experiments.

PROBLEM DESCRIPTION
Firstly, a three-degrees-of-freedom mathematical model of the USV is given, and the assumptions in the model building process are explained. Secondly, the target of the path following control of the USV and the principle of the variable speed curve path following control method based on an extended state observer are described, which lays a foundation for the simulation experiment of the path following control method.

USV MATHEMATICAL MODEL
The USV actually moves with 6 DOF, but in order to simplify the design of the path following controller, when the USV meets the following assumptions, the USV model can be simplified from 6 DOF to 3 DOF. 1) Ignore the motion of the USV in the direction of heave, pitch and roll, and only consider the three-degrees-of-freedom motion of the plane; 2) The USV is a homogeneous mass and is symmetrical about the plane with respect to the x, z coordinate axis; 3) The z axis is the coordinate axis where the center of buoyancy and the center of gravity of the USV are located. The 3DOF mathematical model of the USV can be described as Eq. (1).
where η is the position and heading vector of the USV in the earth-fixed frame; J(ψ) is the rotation matrix between the earth-fixed frame and the body-fixed frame; v is the velocity vector; M is the inertial matrix; C is the Coriolis centripetal force matrix; D is the damping coefficient matrix; τ is the control input vector; b is the external interference vector.
where (x, y, ψ) are the longitudinal and lateral positions and heading angle in the geodetic coordinate system; (u, v, r) describes the surge velocity, the sway velocity, and yaw rate of the USV in the body-fixed frame; τ u , τ r are the surge force and yaw moment; b 1 , b 2 , b 3 represent the external disturbances caused by waves, wind, and ocean currents; m 11 , m 22 , m 33 denote the USV inertia coefficients; d 11 , d 22 and d 33 are hydrodynamic damping in surge, sway, and yaw respectively.

DESCRIPTION OF THE CURVE PATH FOLLOWING PROBLEM
The schematic diagram of the curve path following strategy for the USV is shown in Fig. 1. It is considered that the expected path P(θ) of the USV is composed of a series of continuous points (x p , y p ), wherein θ is the time-dependent path variable with dynamics given by: where yʹ p (θ) = dy p (θ)/dθ, xʹ p (θ) = dx p (θ)/dθ, U = > 0 is the total speed in the coordinate system. γ p is the included angle between the tangent direction and due north in the earth-fixed frame at any point (x p , y p ) on the parametric path, and its calculation formula is as follows: The Serret-Frenet coordinate system was established to better describe the tracking error of the USV curve. The coordinate system takes the free point on the expected path P(θ) as the origin, and the horizontal axis X p along the tangent direction of P(θ), and the vertical axis Y p along the normal direction of P(θ), and points to the right. x e represents the along-path following errors; P los is the line-of-sight guidance vector, and its direction is the expected heading ψ d of the USV; Δ is the lookahead distance of the USV; β is the sideslip angle generated by the ocean disturbance of the USV.
In the geodetic coordinate system, the actual current position of the USV is (x, y). According to the geometric relationship in Fig. 1, the cross-path following error y e is as follows: y e = -(x -x p (θ)) sin(γ p (θ)) + (y -y p (θ)) cos(γ p (θ)) (4) When the cross-path following error y e = 0, this means that the USV is already tracking the path of the curve. Therefore, the curve path following problem of the USV can be described as follows: under the condition of the existence of the sideslip angle, the improved LOS guidance law is adopted to make the cross-path following error y e converge to 0 by controlling the heading and surge velocity of the USV. Thus, the USV path following control strategy is formulated, and its overall control principle is shown in Fig. 2.

CONTROLLER DESIGN
The curve path following control algorithm designed in this paper includes three parts: improved LOS guidance law, extended state observer and sliding mode heading/speed controller. Firstly, the surge velocity guidance law is added on the basis of the traditional LOS guidance law, so that the LSO guidance law provides both the desired heading and the desired surge velocity. Secondly, an extended state observer is designed to estimate the sideslip angle and compensate it in LOS guidance law to eliminate the influence of environmental disturbance on path following. Finally, a sliding mode controller is designed to drive the USV to track the desired heading and desired surge velocity given by LOS guidance law, so as to realise accurate path following.

IMPROVED CURVE LOS GUIDANCE LAW
The traditional LOS (line-of-sight) guidance law can calculate the desired heading of the USV based on the USV's position and desired path, which has been widely used in the field of USV path following. When there is a sideslip angle, the traditional LOS guidance law calculates the desired heading as follows: where Δ is the lookahead distance; β is a time-varying and unknown sideslip angle.
The traditional LOS guidance law only focuses on the calculation of the desired heading, and the surge velocity is generally a user-defined constant value. However, when the curve path following is carried out at a fixed surge velocity, the expected curve path curvature will affect the tracking effect. The greater the curvature, the greater the tracking error. Moreover, the oscillation increases during the path following convergence, and the stability of the system decreases. Therefore, based on the traditional LOS guidance law, the surge velocity guidance law is designed and added in this paper, so that the improved LOS guidance law can provide both the desired heading and desired surge velocity, thus further improving the tracking accuracy of the curve path.
The surge velocity guidance law of the USV is designed by establishing the relationship between the surge velocity and the curvature of the desired path, so that the surge velocity of the USV can be adaptively adjusted according to the curvature. The surge velocity guidance law is designed as follows: where u d represents the desired surge velocity of the USV, and u 1 represents the minimum surge velocity. u 2 represents the maximum increment of the variable surge velocity part; h, δ are the user-defined constant parameters; K is the curvature of the expected curve path, and its calculation formula is as follows: The USV path following process, under the effect of the variable surge velocity guidance law according to the curvature of the curve path, independently regulates the USV speed: the greater the curvature, , the closer it gets to the minimum speed; the smaller the curvature, . By setting parameter δ appropriately, u d can be close to the maximum speed.
In conclusion, the improved LOS guidance law can provide both the desired surge velocity and the desired heading for the USV. Under the desired heading shown in Eq. (5), the USV can converge steadily to the expected path. The design of the surge velocity guidance law enables the USV to adjust the desired surge velocity in real time according to the curvature of the curve path during the heading of track tracking, thus effectively reducing the error of path following and improving the accuracy.

DESIGN OF AN ESO TO ESTIMATE THE SIDESLIP ANGLE
The Extended State Observer (ESO) is the core part of the active disturbance rejection controller [13]. It adopts a discontinuous smooth structure design and can perform real-time estimation and compensation for disturbances. In this paper, the estimation of the sideslip angle by means of an extended state observer is divided into two steps: firstly, an auxiliary variable is introduced to construct a second-order extended state system about the sideslip angle to satisfy the application conditions of the extended state observer; then a second-order extended state observer is designed to estimate the sideslip angle.

Introduction of auxiliary variables
In order to apply the extended state observer to the estimation of the sideslip angle, a second-order system is constructed by using the cross-path following errors y e , and the sideslip angle is independent of the extended state, so as to facilitate the subsequent observer design.
Combining with Eq. (9) yields the following dynamics: Rewriting ϑ = x 1 ; u . tanβ = x 2 ; u . tan(ψ -γ p (θ))p (θ) = h, the above system can be extended to of a second-order system as shown in Eq. (11): So far, with the introduction of auxiliary variable ϑ a secondorder system is constructed, and the function term with an unknown sideslip angle is independent of the extended state x 2 , laying the groundwork for our subsequent extended state observer design.

Design of extended state observer
Next, an extended state observer is designed to estimate the sideslip angle. Eq. (11) is rewritten as Eq. (12): The extended state observer is designed as shown in Eq. (13): The parameters β 1 , β 2 , α 1 and σ are determined according to the empirical formula provided in [14], and the estimation of x 1 , x 2 by z 1 , z 2 can be realised. Since x 2 = utanβ, and u is the surge velocity of the USV, which can be obtained by the sensor, the calculation formula of the final sideslip angle is as follows: = arctan( ) (15) By designing an extended state observer, the sideslip angle β has been estimated and substituted into Eq. (6) to obtain the compensated desired heading of the USV: With the introduction of auxiliary variables and the design of the extended state observer, the estimation of the sideslip angle has been realised. The desired heading ψ d given by the LOS guidance law is corrected by using the estimated sideslip angle to compensate the environmental disturbances. The desired heading given in Eq. (16) can ensure that the USV converges to the desired curve path in the presence of a sideslip angle.

DESIGN OF SLIDING MODE CONTROLLER
In order to ensure that the USV can track the desired heading and desired surge velocity given by the improved LOS guidance law in real time, it is necessary to design the heading and speed controller. However, the complex and changeable environmental disturbance requires greater robustness of the controller. Sliding mode control is a special class of nonlinear control [15], which can switch the structure of the controller according to the degree of system state deviation from the sliding mode, and has strong robustness. In addition, sliding mode control is insensitive to model error and parameter perturbation of the controlled object. Therefore, based on the sliding mode control theory, the heading and speed controllers of the USV are designed respectively in this paper.

Sliding mode heading controller
The improved LOS guidance law provides the desired heading ψ d for the USV, and the actual heading ψ of the USV can be obtained by sensors. Therefore, the heading tracking error is defined as: The sliding surface of heading tracking is designed as: Taking the derivative along Eq. (18) and combining it with Eq. (1) obtains: The control input τ req is originally designed as: τ req = (m 22 -m 11 )uv + (1 + d 33 )r -m 33 ( d + c 1 (r d -r)) (20) In order to overcome possible chattering, the saturation function sat(s 1 ) is adopted in the controller instead of the switching function sgn(s 1 ). The expression of the saturation function is as follows: where ω 1 > 0 is called the boundary layer. Switching control is used outside the boundary layer, and linearised feedback control is used inside the boundary layer, which can effectively reduce chattering [16].
The approach law determines the speed at which the system reaches the sliding mode surface. In order to make the system converge faster without increasing chattering, the exponential approach law is selected: Combining with Eq. (20) and Eq. (23) yields the final control input τ r of the USV as:

Sliding mode speed controller
The design process of the sliding mode speed controller is the same as the heading controller. The surge velocity tracking error is defined as: The sliding surface of the first-order exponential surge velocity tracking of the integrated type is designed as follows: Taking the derivative along Eq. (25) and combining it with Eq. (1) obtains: The control input τ ueq is originally designed as: We select the law of exponential approach: 2 = -k 2 s 2 -ε 2 sat(s 2 ) Through the design of the sliding mode heading/speed controller, the USV can track the desired heading and surge velocity under the control law shown in Eq. (23) and Eq. (29). Moreover, due to the robustness of sliding mode control, the influence of environmental disturbance and model uncertainty on path following control is further restrained and the control performance is improved.

SIMULATION EXPERIMENTS
In order to verify the effectiveness and superiority of the proposed method, MATLAB is used for simulation tests. The mathematical model of the USV in [17] is selected for simulation verification. The specific model parameters are as follows:

Simulation experiment 1
First of all, the simulation experiment of undisturbed path following control for the USV is carried out respectively based on the traditional LOS guidance law and improved LOS guidance law proposed in this paper.
The initial statuses of the USV are as follows: The design expectation curve path is as follows: x p (θ) = θ y p (θ) = 20sin(0.1θ) where θ is a parameter that satisfies Eq. (2).
On the basis of the improved LOS guidance law, the desired heading ψ d and desired surge velocity u d respectively are as follows: The parameters are chosen as u 1 = 0.8, u 2 = 0.7, h = 40, δ = -2, Δ = 1.
In addition, the parameter design of the sliding mode heading controller is c 1 = 3, k 1 = 3, ε 1 = 1, ω 1 = 1. The parameters of the sliding mode speed controller are designed as c 2 = 0.1, k 2 = 2, ε 2 = 0.01, ω 2 = 1. Based on the above simulation conditions, the experimental results are shown in Fig. 1.   Fig. 3 (a) and (b) mainly compare the effect of path following control between the traditional LOS and the improved LOS. From the analysis of Fig. 3 (a), it can be seen that due to the improved LOS guidance law and the design of the surge velocity guidance law, there is a smaller tracking error in the position where the curvature of the curve path is larger. By analysing and comparing the tracking errors before and after the improvement of the LOS guidance law in Fig. 3 (b), it can be calculated that the average tracking error based on the improved LOS guidance law is 0.13 m, and the average tracking error of the traditional LOS guidance law is 0.157 m. The improved LOS guidance law improves the tracking accuracy of USVs by 28%. Thus, Fig. 3 (a) and (b) illustrate the advantages of improving the law of LOS guidance. Fig. 3 (c) is the heading tracking control based on the sliding mode controller. It can be seen that the USV can track the desired heading well under the action of the sliding mode controller, which verifies the effectiveness of the sliding mode controller. Fig. 3 (d) shows the curvature change of the curve path; Fig. 3 (e) first reflects the effectiveness of the rate of change of surge velocity. The USV can adjust the desired surge velocity according to the path curvature during the path following process. Secondly, it verifies that the USV sliding mode speed

Simulation experiment 2
The following is the simulation experiment of the USV curve path following control under oceanic environmental disturbance. Based on the simulation environment given in experiment 1, the design of the extended state observer is added. The parameters are as follows: β 1 = 100, β 2 = 300, α 1 = 0.5, δ = 0.01.
In order to make the simulation effect reflect the USV navigation situation more realistically, the disturbance is assumed as follows: Based on the above simulation conditions, the experimental results are shown in Fig. 2. Fig. 4 is the simulation result for the validity verification of the sideslip angle estimation by the extended state observer. Fig. 4 (a) is the simulation result of the ESO's estimation of the unknown term x 2 with the sideslip angle. It can be clearly seen that the ESO can accurately track a time-varying and unknown extended state, which proves the effectiveness of the proposed method. Fig. 4 (b) shows the simulation results of the USV speed control. The speed guidance law can adjust the surge velocity of the USV in real time. Fig. 4 (c) is the change curve of the estimated sideslip angle calculated by (15). After obtaining , the expected heading is compensated, and the change curve of the expected heading before and after the compensation is shown in Fig. 4 (d). Fig. 4 (e) shows the curve path following control of the USV, and compares the path following effect with that without the extended state observer. By analysing and comparing the curve path following error in Fig. 4 (f), it can be calculated that the average tracking error after adding sideslip angle compensation is 0.75 m, and the Fig. 4. In the presence of disturbance, ESO estimates the effectiveness of sideslip angle average tracking error before sideslip angle compensation is 1.01 m. The design of the extended state observer estimation improves the curve path following accuracy of an unmanned ship under ocean disturbance by 26%.

CONCLUSIONS
In this paper, the sideslip angle problem caused by environmental disturbance is studied in depth and a path following control method for a variable surge velocity curve based on an extended state observer is proposed. The extended state observer is designed to accurately estimate the unknown, time-varying and arbitrary sideslip angle, and then the desired heading given by LOS guidance law is compensated and corrected by using its estimated value, which ensures the stability of the trajectory tracking control of USV curves. Secondly, by analysing the relationship between the surge velocity of the USV and the curvature of the curved path, the design of the surge velocity guidance law is added on the basis of the traditional LOS guidance law, so that the USV can self-adjust the surge velocity according to the curvature of the path and further improve the tracking accuracy of the path. Finally, the sliding mode heading and speed controller is designed to track the desired heading and speed accurately. The effectiveness and superiority of the proposed method are verified by simulation experiments