Autonomous Catamaran Unmanned Surface Vessel Based on GPS And Radar

1. Introduction

Since the unmanned surface vessels (USVs) are cost-effective, flexible and fast, safe and reliable, small, intelligent, and able to cope with various climatic environments, USV will play an extremely important role in rivers, lakes and other environments as well as in the future military aspects of the sea [1,2]. The independent mathematical model of ship motion was proposed by the MMG group of the Japanese Towing Tank Conference (JTTC) [3]. Since the MMG model is physically easier to analyze and understand, it is more applicable to all types of USVs and its applications are more widespread. Xia et al. [4] only the horizontal maneuvering motion of the ship on still water was considered and the MMG model was used to analyze the forces on the rudder for the prediction of the maneuvering motion of a single ship. Reichel et al. [5] designed an MMG model to analyze and discuss the horizontal surface motion characteristics of a single USV with pod propulsion in order to quickly simulate the ship maneuverability. Chen et al. [6] developed a black box model of USV maneuvering motion with rudder angle and propeller speed based on the MMG model. Then, LS-SVM is used to identify the offline black box model and validate the motion prediction of USV. However, they only designed the online simulation framework in the simulation, which is still some distance from the experiment. More MMG models can be found in reference [7,8,9,10,11]. Therefore, the modeling idea of MMG is fully applicable to the USV of autonomous design. Trajectory tracking controllers commonly use PID control [7,12,13,14], Dong et al. [12] applied it to high-speed USV heading PD control by designing an improved bacteria foraging algorithm, which accelerated the convergence speed and improved the control accuracy; Horel et al. [13] studied the characteristics of rudder-controlled catamaran heading change based on a system approach. Ryoo et al. [15] used fuzzy control algorithm to achieve heading control with low accuracy. Wang et al. [16] created a finite-time fuzzy monocular vision servo scheme using adaptive fuzzy PID and adaptive residual feedback in the presence of unmeasurable attitude and velocity. In this paper, the fuzzy adaptive PID algorithm with simple structure and better robustness is considered for heading control design. Target detection is an important foundation for unmanned ships to achieve tasks such as obstacle avoidance and trajectory tracking. LIDAR was first applied to unmanned vehicles and other fields, and is commonly used for obstacle detection and environment map construction. With the development of USV technology, LIDAR is also applied to obstacle avoidance and other aspects in the field of unmanned ships [17,18,19]. Villa et al. [20] use LIDAR for navigation, control and obstacle avoidance and compute a predicted safety bounding box for each moving obstacle based on a system identification approach. Stateczny et al. [21] designed HydroDron, a universal autonomous control and management system for multipurpose USVs, which can also be used for tasks other than hydrographic surveys, such as monitoring rivers, lakes and other waters, environmental quality testing, etc.

This paper focuses on the design and engineering implementation of a catamaran USV system based on an attached coordinate system, MMG model and kinematic model with simplified degrees of freedom. A conventional PID controller is combined with fuzzy rules to achieve trajectory tracking under wind and wave disturbances. Path planning is accomplished by using differential GPS pre-targeting trajectories, and simulations and experimental results demonstrate the accuracy of the controller. The LIDAR acquires point cloud data, extends and erodes the obtained 3D point cloud data into the obstacle grid map and re-clusters it to complete the obstacle contour extraction and achieve simple obstacle avoidance.

The rest of the paper is organized as follows. Section II designs and improves the MMG model, and the data obtained from the velocity-thrust and rotational experiments are fitted to complete the parameter identification. Section III designs the path planning and trajectory tracking controller. The fuzzy adaptive PID controller is designed by establishing fuzzy rules and fuzzy affiliation functions. Section IV verifies the robustness of the fuzzy adaptive controller under wind and wave disturbances through simulation results. Section V verifies the effectiveness of the autonomous navigation and tracking system through experiments and implements the obstacle detection by LIDAR. Section VI summarizes the paper.

2. Motion Model and Parameter Identification of Vessel

The foundation of the investigation into USV motion modeling and control issues is the kinematic model. If the kinematic model is too exact, its equations of motion will invariably be nonlinear, the parameters will be tough to obtain, and it will be challenging to realize; if it is too basic, it will not accurately reflect the actual motion process of an USV, and the model will become meaningless. The kinematic analysis is divided into two aspects: from kinematic analysis, the catamaran USV is regarded as a mass point, and the relationship between its motion speed, heading angle and other variables and position is analyzed; from kinetic analysis, that is, the thrust of thrusters, fluid forces and interaction forces acting on the USV are studied. MMG model is suitable for self-research USV motion modeling analysis by virtue of its advantages of comprehensive force analysis and high flexibility.

2.1. MMG Motion Model

As shown in Figure 1, the USV in the water environment is generally described based on two coordinate systems [22]: the inertial coordinate system, i.e., the center point of the earth as the coordinate origin; and the attached coordinate system, i.e., the USV itself as the coordinate origin. The motion of the USV is divided into six states of motion with six degrees of freedom: displacement of the ship in the x, y and z axes, and rotation about the x, y and z axes. The USV motion state is represented in two different coordinate systems [22], and its transformation relationship is in (1):

$$\begin{array}{c}\left[\begin{array}{c}{\dot{x}}_0\hfill \\ {\dot{y}}_0\hfill \\ {\dot{z}}_0\hfill \end{array}\right]=T_V{\left[\begin{array}{c}u\hfill \\ v\hfill \\ w\hfill \end{array}\right]}_{,}\left[\begin{array}{c}\dot{\varphi }\hfill \\ \dot{\theta }\hfill \\ \dot{\psi }\hfill \end{array}\right]=T_{\Omega }{\left[\begin{array}{c}p\hfill \\ q\hfill \\ r\hfill \end{array}\right]}_{,}\end{array}$$

(1)

where u and p represent the x-axis velocity and rotational angular velocity, v and q represent the y-axis velocity and rotational angular velocity, and w and r represent the z-axis velocity and rotational angular velocity. $T_V$ and $T_{\Omega }$ represent transformation matrices, expressed in (2) and (3):

$$\begin{array}{c}{T}_V=\left[\begin{array}{ccc}cos\theta cos\psi & sin\varphi sin\theta cos\psi -cos\varphi sin\psi & cos\varphi sin\theta cos\psi +sin\varphi sin\psi \\ cos\theta sin\psi & sin\varphi sin\theta sin\psi +cos\varphi cos\psi & cos\varphi sin\theta sin\psi -sin\varphi sin\psi \\ -sin\theta & sin\varphi cos\theta & cos\varphi cos\theta \end{array}\right]\hfill \end{array}$$

(2)

and

$$T_{\Omega }={\left[\begin{array}{ccc}1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi sec\theta & cos\varphi sec\theta \end{array}\right]}_{.}$$

(3)

More detailed variable symbols are shown in Table 1.

Neglecting the longitudinal rotational angular velocity, transverse rotational angular velocity and the velocity of the USV along the z-axis in the water environment makes $w=0$, $p=0$, $q=0$, $\varphi =0$, $\theta =0$. The six-degree-of-freedom motion is simplified to a three-degree-of-freedom model, and the kinematic model is derived as (4) by combining (1), (2) and (3):

$$\left\{\begin{array}{c}{\dot{x}}_0=ucos\psi -vsin\psi \hfill \\ {\dot{y}}_0=usin\psi +vcos\psi \hfill \\ \dot{\psi }=r\hfill \end{array}\right.$$

(4)

USVs are subjected to thrust of thrusters and drag of water, etc. when sailing in still waters. Each force and moment in the x-axis, y-axis and z-axes are shown in (5):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}X=X_{H_0}+X_P\hfill \\ Y=Y_{H_0}+Y_P\hfill \\ N=N_{H_0}+N_P,\hfill \end{array}\right.\end{array}$$

(5)

where $H_0$ denotes the hydrodynamic forces and moments acting on the hull and P denotes the hydrodynamic forces and moments of the thruster (thrust of the thruster). The forces and moments acting on the hull can be decomposed into inertial hydrodynamic forces, viscous hydrodynamic forces [23], as shown in (6):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{X}_{H_0}=X_I+X_H\hfill \\ Y_{H_0}=Y_I+Y_H\hfill \\ N_{H_0}=N_I+N_H,\hfill \end{array}\right.\end{array}$$

(6)

where I denotes inertial fluid and H denotes viscous fluid.

When the origin of the attached coordinate system is at the center of mass of the USV, the equation of motion of the USV is as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}m(\dot{u}-vr)\hfill & =X\hfill \\ m(\dot{v}+ur)\hfill & =Y\hfill \\ I_{zz}\dot{r}\hfill & =N,\hfill \end{array}\right.\end{array}$$

(7)

where m denotes the fully loaded mass of the USV, $I_{zz}$ denotes the moment of inertia at the z axis, and N denotes the moment of momentum.

Assuming that the catamaran USV moves mainly on the stationary water surface, the three-degree-of-freedom equations of motion of the USV are obtained by combining (5), (6), and (7):

$$\begin{array}{c}\hfill \left\{\begin{array}{c}m(\dot{u}-vr)=X_I+X_H+X_P\hfill \\ m(\dot{v}+ur)=Y_I+Y_H+Y_P\hfill \\ I_{zz}\dot{r}=N_I+N_H+N_P,\hfill \end{array}\right.\end{array}$$

(8)

where P denotes dual propellers. Usually the viscous class fluid dynamics is much smaller than the other forces and can be neglected [24], so only the inertial class fluid dynamics and dual thruster dynamics are analyzed and calculated in this study.

The force generated by the surrounding fluid medium on the USV due to the perturbation caused by the USV during its navigation is called inertial class hydrodynamics. According to the velocity potential flow theory of the flow field, when the USV makes a variable speed motion in an ideal water environment, it generates a 6×6 attached mass inertia matrix m as (9):

$$m={\left[\begin{array}{cccccc}{m}_{11}& m_{12}& m_{13}& m_{14}& m_{15}& m_{16}\\ m_{21}& m_{22}& m_{23}& m_{24}& m_{25}& m_{26}\\ m_{31}& m_{32}& m_{33}& m_{34}& m_{35}& m_{36}\\ m_{41}& m_{42}& m_{43}& m_{44}& m_{45}& m_{46}\\ m_{51}& m_{52}& m_{53}& m_{54}& m_{55}& m_{56}\\ m_{61}& m_{62}& m_{63}& m_{64}& m_{65}& m_{66}\end{array}\right]}_{.}$$

(9)

Since the motion of the USV drives the motion of the surrounding fluid, the mass and moment of inertia of this fluid can be represented by the m-matrix, which is calculated as (10):

$$m_{ij}=-{\int }_{\sigma }\rho {\phi }_j\frac{\partial {\phi }_i}{\partial n}d\sigma ,i=1,2,\dots ,6,$$

(10)

where $\sigma$ represents the surface area of the waterline of the USV, ${\phi }_i$ represents the velocity potential in the direction of degree of freedom i, and n represents the normal vector.

For a fixed USV profile, the attachment inertia matrix $m_{ij}$ is a constant independent of the motion variables. Since the object of study is based on symmetry in the x-o-z and x-o-y planes, for the 36 elements of the matrix, there are and only 8 independent non-zero elements, denoted as follows: (11):

$$m={\left[\begin{array}{cccccc}{m}_{11}& 0& 0& 0& 0& 0\\ 0& m_{22}& 0& 0& 0& m_{26}\\ 0& 0& m_{33}& 0& m_{35}& 0\\ 0& 0& 0& m_{44}& 0& 0\\ 0& 0& m_{53}& 0& m_{55}& 0\\ 0& m_{62}& 0& 0& 0& m_{66}\end{array}\right]}_{,}$$

(11)

where $m_{11}$, $m_{22}$, and $m_{33}$ are the attachment masses along the x, y, and z axes, respectively; $m_{44}$, $m_{55}$, and $m_{66}$ are the attachment moments of inertia in the x, y, and z axes, and $m_{26}$ and $m_{35}$ denote the attachment mass static moments, which are not zero because the y-o-z plane is not symmetric. The kinetic energy of the surrounding fluid flow caused by the motion of an unmanned vessel in a water environment can be expressed as (12):

$$T=\frac{1}{2}\sum _i\sum _j{m}_{ij}{u}_i{u}_j,i,j=1,2,\dots ,6,$$

(12)

where $u_1=u$, $u_2=v$, $u_3=w$, $u_4=p$, $u_5=q$, $u_6=r$, Substituting (11) into (12), we get

$$T=\frac{1}{2}(m_{11}{u}^2+m_{22}{v}^2+m_{33}{w}^2+m_{44}{p}^2+m_{55}{q}^2+m_{66}{r}^2).$$

(13)

The momentum of the fluid perturbation motion is $H_i=\begin{array}{c}\hfill \frac{\partial T}{\partial u_i}(i=1,2,...,6)\end{array}$, and the fluid momentum and momentum moments projected on each coordinate axis can be expressed as (14)

$$\left\{\begin{array}{c}{H}_x=\frac{\partial T}{\partial u}=m_{11}u+m_{13}w+m_{15}q\hfill \\ H_y=\frac{\partial T}{\partial v}=m_{22}v+m_{24}p+m_{26}r\hfill \\ H_z=\frac{\partial T}{\partial w}=m_{33}w+m_{13}u+m_{35}q\hfill \\ L_x=\frac{\partial T}{\partial p}=m_{44}p+m_{24}v+m_{46}r\hfill \\ L_y=\frac{\partial T}{\partial q}=m_{55}q+m_{15}u+m_{15}w\hfill \\ L_z=\frac{\partial T}{\partial r}=m_{66}r+m_{26}v+m_{46}p,\hfill \end{array}\right.$$

(14)

where H denotes the fluid momentum and L denotes the fluid momentum moment.

Since the hydrodynamic force $F=\frac{-dH}{dt}$ and the hydrodynamic moment $M=\frac{-dL}{dt}$, the expressions for the inertia-like hydrodynamic force and dynamic moment of the USV are obtained in (15):

$$\left\{\begin{array}{c}{X}_I=m_{22}vr-m_{11}\dot{u}-m_{33}wq\hfill \\ Y_I=m_{33}wp-m_{22}\dot{v}-m_{11}ur\hfill \\ Z_I=m_{11}uq-m_{33}\dot{w}-m_{22}vp\hfill \\ K_I=\left(m_{22}-m_{33}\right)vw+\left(m_{55}-m_{66}\right)qr-m_{44}\dot{p}\hfill \\ M_I=\left(m_{33}-m_{11}\right)uw+\left(m_{66}-m_{44}\right)pr-m_{55}\dot{q}\hfill \\ N_I=\left(m_{11}-m_{22}\right)uv+\left(m_{44}-m_{55}\right)qr-m_{66}\dot{r}.\hfill \end{array}\right.$$

(15)

According to the the motion of the USV with three degrees of freedom, namely longitudinal displacement, lateral displacement and bow rocking, is simplified and analyzed. Let $w=0$, $p=0$ and $q=0$, the expressions of inertia-like hydrodynamic forces and dynamic moments under the three-degree-of-freedom motion of the USV are obtained in (16):

$$\left\{\begin{array}{c}{X}_I=m_{{}_{22}}vr-m_{{}_{11}}\dot{u}\hfill \\ Y_I=-m_{{}_{22}}\dot{v}-m_{{}_{11}}ur\hfill \\ N_I=(m_{{}_{11}}-m_{{}_{22}})uv-m_{66}\dot{r}.\hfill \end{array}\right.$$

(16)

There are two methods to calculate the attachment mass and the moment of inertia of the USV: the regression equation of Zhou et al. and the regression equation of Clarke et al. [25]. Zhou et al. performed a multiple regression analysis of the famous Motora’s atlas [26] as:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\frac{m_{11}}{m}=\frac{1}{100}\left\{0.398+11.97C_b\left(1+3.73\frac{d}{B}\right)-2.89C_b\frac{L}{B}\left(1+1.13\frac{d}{B}\right)+0.175C_b{\left(\frac{L}{B}\right)}^2\left(1+0.541\frac{d}{B}\right)-1.107\frac{L}{B}·\frac{d}{B}\right\}\hfill \\ \frac{m_{22}}{m}=0.882-0.54C_b\left(1-1.6\frac{d}{B}\right)-0.156\frac{L}{B}\left(1-0.673C_b\right)+0.826\frac{L}{B}·\frac{d}{B}\left(1-0.678\frac{d}{B}\right)-0.638C_b·\frac{L}{B}·\frac{d}{B}\left(1-0.669\frac{d}{B}\right)\hfill \\ \frac{m_{66}}{L^2}=\frac{1}{100}\left\{33-76.85C_b\left(1-0.784C_b\right)+3.43\frac{L}{B}\left(1-0.63C_b\right)\right\},\hfill \end{array}\right.\end{array}$$

(17)

where B is the hull width, L is the unmanned vessel length, $C_b$ is the square factor, and d is the sea draught.

When the thruster rotates in a uniform water environment, its thrust and torque are related to the paddle diameter $D_P$, forward velocity $V_A$, water density $\rho$, rotational speed n, water motion viscosity coefficient v, gravitational acceleration g, etc. According to the applied gauge analysis, the hydrodynamic force (thrust) of the thruster can be expressed as

$$T=\rho n^2{D}^4{f}_T{\left(\mathrm{Propeller}\mathrm{geometry},\frac{V_A}{n{D}_P},\frac{n{D}_P^2}{v},\frac{n^2{D}_P^2}{g{D}_P}\right)}_{.}$$

(18)

Let $k_T=f_T\left(\mathrm{Propeller}\mathrm{geometry},\frac{V_A}{n{D}_P},\frac{n{D}_P^2}{v},\frac{n^2{D}_P^2}{g{D}_P}\right)$, then we have:

$$T=\rho n^2{D}_P^4{k}_T.$$

(19)

Where, $k_T$ is the thrust coefficient. Similarly, the torque absorbed by the thruster, i.e., the reverse torque caused by the rotation of the water on the thruster, is found as

$$Q=\rho n^2{D}_P^5{f}_Q{\left(\mathrm{Propeller}\mathrm{geometry},\frac{V_A}{n{D}_P},\frac{n{D}_P^2}{v},\frac{n^2{D}_P^2}{g{D}_P}\right)}_{.}$$

(20)

Let $k_Q=f_Q\left(\mathrm{Propeller}\mathrm{geometry},\frac{V_A}{n{D}_P},\frac{n{D}_P^2}{v},\frac{n^2{D}_P^2}{g{D}_P}\right)$, then we have:

$$Q=\rho n^2{D}_P^5{k}_Q.$$

(21)

where $k_Q$ is the torque coefficient. The thrust coefficient $k_T$ and torque coefficient $k_Q$ are both correlation functions of the incoming speed coefficient $J=\frac{V_A}{n{D}_P}$. For the purpose of computer simulation, they can be expressed in the study of USV motion as

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{k}_T=a_0+a_{1}J+a_2{J}^2\hfill \\ k_Q=b_0+b_{1}J+b_2{J}^2.\hfill \end{array}\right.\end{array}$$

(22)

Where, $a_0$, $a_1$, $a_2$ and $b_0$, $b_1$, $b_2$ are the regression coefficients of the propeller open water characteristic curve [27]. The unmanned vessel is usually accompanied by slight rotational transport and lateral motion during the forward motion, and the effect of the latter two motions is usually summarized as the effect on the propulsion derating, i.e., the effect of the maneuvering motion on $(1-t_p)$. The Norrbin model can be used [28]:

$$1-t_p=\left(1-t_{p0}\right)+f,f=\left[0.00023\left(\frac{B}{d}\right)\left\{1.3\left(1-C_b\right)-310\frac{x_C}{L}\right\}·\frac{L}{D_P}-0.028\right]·\left(\beta -l_R\frac{r}{V}\right),$$

(23)

where B denotes the hull width, $D_P$ denotes the diameter of the propeller, $x_C$ denotes the longitudinal coordinates of the floating center, $l_R$ = -0.9L and using the Hankescher formula [29], for a double propeller boat, $C_b$ = 0.54, $t_{p0}$ = 0.5$C_p$-0.18, $C_p$ is the vessel’s rhombic coefficient, $C_p=\frac{C_b}{C_m}=\frac{Bd{C}_b}{S_{\mathrm{IM}}}$ , and $S_{\mathrm{IM}}$ denotes the vessel immersion area.

The thrust from the thruster rotation is not all applied to the USV, part of it is the effective thrust, defined as $X_P$, and the other part is used to overcome the drag increment caused by the thruster installed at the stern of the USV, defined as the thrust derating $\Delta R$. In the practical engineering application of the USV, the dimensionless ratio of the thrust derating $\Delta R$ to the thruster thrust T is defined as the thrust derating factor $t_p$, expressed as follows:

$$t_p=\frac{\Delta R}{T}={\frac{T-R}{T}}_{.}$$

(24)

The USV traveling at low speed is approximately in uniform motion, so the effective thrust $X_{P0}$ of a single thruster is equal to the drag R in (24), with

$$X_{P0}=\left(1-t_p\right)T.$$

(25)

The combined thrust force of a catamaran unmanned vessel is expressed in the longitudinal direction as

$$X_P=X_{lp}+X_{rp},$$

(26)

where $X_{lp}$ denotes the effective thrust of the left thruster and $X_{rp}$ denotes the effective thrust of the right thruster. The twin thrusters produce torques

$$N_P=\left(X_{lp}-X_{rp}\right)·d_m,$$

(27)

where $d_m$ denotes the distance from the two thrusters to the ship’s central axis.

The simplified dynamics model [30], analyzed by MMG modeling of the USV, is

$$M\dot{\mathbf{v}}+C\left(\mathbf{v}\right)\mathbf{v}+D\mathbf{v}=\tau ,$$

(28)

where $\mathbf{v}={\left[\begin{array}{ccc}u& v& r\end{array}\right]}^T$, is the velocity vector of the USV in three degrees of freedom; M is the inertial mass matrix of the USV; C is the centripetal and Koch force coefficient matrix; D denotes the drag coefficient matrix, and $\tau$ denotes the forces along the longitudinal and lateral directions of the thruster and the torque along the z axis direction, $\tau ={\left[\begin{array}{ccc}{\tau }_u& {\tau }_v& {\tau }_r\end{array}\right]}^T$. According to [30], the coefficient matrix (28) is defined for each coefficient matrix as follows:

$$M={\left[\begin{array}{ccc}{m_{11}}^{{}^{\prime }}& 0& 0\\ 0& {m_{22}}^{{}^{\prime }}& 0\\ 0& 0& {m_{66}}^{{}^{\prime }}\end{array}\right]}_{,}C\left(\mathbf{v}\right)={\left[\begin{array}{ccc}0& 0& -{m_{22}}^{{}^{\prime }}v\\ 0& 0& {m_{11}}^{{}^{\prime }}u\\ {m_{22}}^{{}^{\prime }}v& -{m_{11}}^{{}^{\prime }}u& 0\end{array}\right]}_{,}D={\left[\begin{array}{ccc}{d}_{11}& 0& 0\\ 0& d_{22}& 0\\ 0& 0& d_{33}\end{array}\right]}_{.}$$

(29)

Based on the above analysis, a mathematical model of the three-degree-of-freedom motion of a twin-propulsion catamaran USV is established as shown below:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\dot{x}}_0=ucos\psi -vsin\psi \hfill \\ {\dot{y}}_0=usin\psi +vcos\psi \hfill \\ \dot{\psi }=r\hfill \\ {m_{11}}^{{}^{\prime }}\dot{u}-{m_{22}}^{{}^{\prime }}vr+d_{11}u=X_{lp}+X_{rp}\hfill \\ {m_{22}}^{{}^{\prime }}\dot{v}+{m_{11}}^{{}^{\prime }}ur+d_{22}v=0\hfill \\ {m_{66}}^{{}^{\prime }}\dot{r}+\left({m_{22}}^{{}^{\prime }}-{m_{11}}^{{}^{\prime }}\right)uv+d_{33}r=\left(X_{lp}-X_{rp}\right)·d_m.\hfill \end{array}\right.\end{array}$$

(30)

2.2. Parameter Identification

The mathematical model of three-degree-of-freedom motion of the catamaran is obtained according to the principle of MMG model, as shown in (30). The unknown parameters include inertial mass coefficient matrix M, centripetal force and Koch force coefficient matrix C, and drag coefficient matrix D. Among them, M and C can be calculated by physics equations according to the hull structure and weight; D needs to be identified by USV condition test data for parameter identification. Table 2 shows the basic USV parameters.

According to the basic parameters in Table 2, $C_b=\frac{V_{\mathrm{DR}}}{LBd}=0.37$. Bringing $C_b$ into equation (17), it is calculated according to [30] that ${m_{11}}^{{}^{\prime }}=m+m_{11}=71.4$, ${m_{22}}^{{}^{\prime }}=m+m_{22}=117.01$, ${m_{66}}^{{}^{\prime }}=I+m_{66}=20.24$.

In (29), $d_{11}=\frac{{\partial \tau }_u}{\partial u}$, $d_{22}=\frac{{\partial \tau }_v}{\partial v}=0$, $d_{33}=\frac{{\partial \tau }_r}{\partial r}$. The relationship between the thrust of the USV thruster and the speed of the USV is difficult to be found by the theoretical formula, and the parameters of the matrix D are identified by experimental data in this study.

In the control system designed in this paper, the output power of the thruster is controlled by setting the control code n (0→120), which corresponds to the input control voltage of the thruster (0V→5V). For a single thruster, the linear relationship between $n_l$ and the voltage interval $V_l$ is

$$V_l=0.042n_l.$$

(31)

The change in thruster control voltage in turn affects the thrust applied to the USV. According to the thruster data sheet, the relationship between individual thruster thrust and the controlled voltage is as follows:

$$F_l=9.5V_l.$$

(32)

The forward direction of the USV is subject to the combined thrust force of the dual thrusters as $F={\tau }_u=9.5\left(V_l+V_r\right)$ and the torque ${\tau }_r=9.5d_m\left(V_l-V_r\right)$. Therefore, when the left and right thrusters have the same speed, the relationship between the control code n and the combined thrust force can be obtained by substituting (31) into (32) as follows:

$$F=0.798n_l.$$

(33)

The experimental data were fitted to the relationship between the combined thrust force and the velocity using Matlab, and the linear relationship was obtained as shown in Figure 2.

The linear relationship between the velocity and the combined thrust force is obtained by fitting: $F=47.85u+4.509$. Therefore, $d_{11}=\frac{{\partial \tau }_u}{\partial u}=\frac{\partial F}{\partial u}=47.85$. When the left and right thrusters rotate at different speeds, the dual thrusters do slewing motion with torque ${\tau }_r=3.23\left(V_l-V_r\right)$, and similarly, $d_{33}=\frac{{\partial \tau }_r}{\partial r}=0.57$, as shown in Figure 3. Therefore, the mathematical model of the motion of the dual-propulsion catamaran is obtained as shown in (34).

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\dot{u}=-0.67u+0.133\left(V_l+V_r\right)\hfill \\ \dot{r}=-0.028r+0.160\left(V_l-V_r\right)\hfill \\ {\dot{x}}_0=ucos\psi -vsin\psi \hfill \\ {\dot{y}}_0=usin\psi +vcos\psi \hfill \\ \dot{\psi }=r.\hfill \end{array}\right.\end{array}$$

(34)

In order to verify the accuracy of the model constructed in this paper, the motion model is designed for slewing motion simulation using Matlab tools, and compared with the USV in the real water environment for slewing motion test to verify. The left and right thruster control codes are set to 20 and 15 respectively, the initial speed is 0 m/s, and the initial heading angle is 0${}^{\circ }$. After stabilization, the speed is 0.209 m/s, and the simulation result of slewing motion is shown in Figure 3, and the calculated slewing diameter is about 7.38 m.

The test environment had a light wind, the control codes of the left and right thrusters of the USV were given as 20 and 15 respectively, the speed was about 0.246 m/s, three sets of slewing motion were completed, the software platform of the USV collected the longitude and latitude information to generate data tables, and Matlab was used to plot the slewing motion trajectory in the water environment as shown in Figure 4, and the calculated slewing diameter was about 8 m.

Comprehensive analysis of Figure 3 and Figure 4 shows that the simulation results of the gyratory motion of the USV are consistent with its gyratory motion in the real water environment, which indicates that the motion model of the twin-thruster USV in this paper is more compatible with the real catamaran USV.

3. Trajectory Control

The GPS can provide real-time feedback of latitude and longitude information and heading angle of the current position of the unmanned vessel, so the current angle to be tracked can be obtained by calculating the azimuth angle, and the track point tracking can be realized according to the distance between the current position and the target point. Azimuth angle and distance between two points: Let the current position of the USV $A(lat1,lnt1)$, the next target point position $B(lat2,lnt2)$, and the current heading angle calculation equation are shown in (35):

$${\psi }_r=arctan{[\frac{lnt2-lnt1}{lat2-lat1}·cos\left(\frac{lat1+lat2}{2}\right)]}_{,}$$

(35)

where $lat1,lat2$ denote the latitude of positions A and B, $lnt1,lnt2$ denote the longitude of positions A and B, and $cos\left(\right(lat1+lat2)/2)$ denotes the latitude correction.

Since the length of the latitudinal circle on the earth’s surface changes with latitude, the error in the calculation directly using the Pythagorean theorem is too large. According to [31], the distance between two points A and B is calculated as (36):

$$\begin{array}{c}\hfill S=\widehat{AB}=2R·arcsin{\sqrt{{sin}^2\frac{lat1-lat2}{2}+cos\left(lat1\right)·cos\left(lat2\right)·{sin}^2\frac{Int1-Int2}{2}}}_{,}\end{array}$$

(36)

where the radius of the Earth $R=6378.137$ km.

Trajectory tracking controller with incremental PID control [15]:

$$\begin{array}{c}\hfill \left\{\begin{array}{cc}\Delta u\left[k\right]\hfill & =K_P\left\{e\left[k\right]-e[k-1]\right\}+K_{i}e\left[k\right]+K_D\left(e\left[k\right]-2e[k-1]+e[k-2]\right)\hfill \\ u\left[k\right]\hfill & =u[k-1]+4u\left[k\right].\hfill \end{array}\right.\end{array}$$

(37)

Using the incremental PID algorithm to correct the deviation of the current heading angle from the target heading angle, the heading control equation is in (38):

$$\begin{array}{cc}& \Delta {\psi }_r=K_P\Delta \psi +K_I\int \Delta \psi dt+K_D\Delta \dot{\psi }\hfill \end{array}.$$

(38)

In order to improve the control performance of the heading controller, the PID control with high accuracy is combined with the fuzzy control algorithm with high robustness to overcome the shortcomings of the traditional PID parameters that cannot adjust the PID parameters in real time.

From Figure 5 , the deviation e and the variation of the deviation $e_c$ are the inputs to the fuzzy adaptive controller, and the PID controller parameters are adjusted using the output values determined by fuzzy judgments as the adjustment of the proportional, integral, and differential coefficients [32], with the following expressions:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{K}_P=K_{P0}+\Delta K_P\hfill \\ K_I=K_{I0}+\Delta K_I\hfill \\ K_D=K_{D0}+\Delta K_D,\hfill \end{array}\right.\end{array}$$

(39)

where $K_{P0}$, $K_{I0}$, and $K_{D0}$ are the reference values, and $\Delta K_P$, $\Delta K_I$, and $\Delta K_D$ are the adjustment amounts obtained by the fuzzy adaptive controller.

The heading fuzzy adaptive PID controller has two input variables: heading deviation e and the rate of change of heading deviation $e_c$. The actual ranges of the two input variables are converted into the corresponding fuzzy theoretical domains E and $EC$, while $K_P$, $K_I$, and $K_D$ in the fuzzy adaptive PID control algorithm are used as the fuzzy adaptive controller outputs, and the five variable output domains are all designed as $[-3,3]$, respectively. Considering the influence of the number of fuzzy subsets on the accuracy of unmanned ship heading control, each fuzzy theoretical domain is quantified into seven classes, which are negative large (NB), negative medium (NM), negative small (NS), zero (Z), positive small (PS), positive medium (PM), and positive large (PB). Each subset needs to be described by the affiliation function, and the more gentle S function and Z function are used in the larger input region to improve the stability, while the more sharp triangular function is chosen in the smaller input region to improve the sensitivity, and the mathematical principle expression is as (40):

$$f(x,a,b,c)=\left\{\begin{array}{c}0,x\le a\hfill \\ \frac{x-a}{b-a},a\le x\le b\hfill \\ \frac{c-x}{c-b},b\le x\le c\hfill \\ 0,c\le x,\hfill \end{array}\right.$$

(40)

where x is used to specify the range of the domain of the variable, and the parameters a, b, and c specify the shape of the triangular function. In this paper, we design the affiliation functions of E, $EC$, $K_P$, $K_I$, and $K_D$ as shown in Figure 6.

$K_P$, $K_I$ and $K_D$ in the fuzzy adaptive PID control algorithm can be adjusted under different heading deviation e and heading deviation change rate $e_c$ according to the following Table 3, Table 4 and Table 5.

In Table 3, when the value of $\left|e\right|$ is small, the actual heading of the USV is close to the set heading value. At this time, if the value of $\left|e_c\right|$ is small, a larger $K_I$ and a moderate $K_D$ should be taken in order to make the system reach the steady state faster; if the value of $\left|e_c\right|$ is large, a smaller $K_I$ and $K_D$ should be taken in order to prevent the system from overshooting too much.

In Table 4, when $\left|e\right|$, $\left|e_c\right|$ value is moderate, in order to reduce the amount of system overshoot, $K_P$ value should be taken smaller, while considering the response speed of the controlled system, $K_I$, $K_D$ value should be moderate.

In Table 5, when the value of $\left|e\right|$ is large, the value of $K_P$ should be larger to ensure the rapidity of the system response, and the value of $K_I$ should be close to zero and the value of $K_D$ should be smaller to strengthen the suppression of overshoot.

The formula for the weighted average method of defuzzification is $u=\frac{{\sum }_{i=1}^n{k}_i{x}_i}{{\sum }_i^n{k}_i}$, where u is the exact value of the defuzzy output, $x_i$ is the subset of the output fuzzy domain, and $k_i$ is the affiliation degree corresponding to the output subset.

4. Simulation Results

Under the action of no disturbing force, PID simulation and fuzzy adaptive PID simulation are performed on the USV dynamics model, setting the initial angle of the USV 0${}^{\circ }$, the starting point position (0, 0), and the azimuth angle is calculated as 74${}^{\circ }$ according to the tracking track point (70, 20), and the dual thruster control volume change curve and heading angle adjustment curve are shown in Figure 7.

From Figure 7, it can be seen that the PID control effect is slightly inferior to the fuzzy adaptive PID control effect under the effect of no external disturbance.

In order to more realistically describe the heading control performance of an unmanned vessel in real waters, the effect of wind and wave disturbance is added to the simulation, and a linear wind and wave model is used instead of a complex nonlinear wind and wave model [33,34], and the linear wind and wave model expression is approximated as follows:

$$y\left(s\right)=h\left(s\right)w\left(s\right),$$

(41)

where $w\left(s\right)$ denotes Gaussian white noise with zero mean and power spectral density of 0.1, and $h\left(s\right)$ denotes the second-order wave transfer function with the following expressions:

$$h\left(s\right)={\frac{K_{\omega }s}{s^2+2\xi {\omega }_{0}s+{{\omega }_0}^2}}_{,}$$

(42)

where ${\omega }_0$ denotes the dominant wave frequency, $\xi$ denotes the damping coefficient, $K_{\omega }=2\xi {\omega }_0{\sigma }_m$ denotes the gain constant, and ${\sigma }_m$ denotes the wave intensity. The simulation environment is set as wind level four with small waves, with $K_{\omega }$=0.20, ${\sigma }_m$=0.54 and ${\omega }_0$=0.61.

Under the action of wind and wave disturbance force, PID simulation and fuzzy adaptive PID simulation are carried out for the USV dynamics model. The initial angle of the USV is set to 0${}^{\circ }$, the starting point position (0, 0), and the azimuth angle is calculated as 74${}^{\circ }$ according to the tracking track point (70, 20), and the change curve of the dual thruster control volume and heading angle adjustment curve are shown in Figure 8, and the fuzzy adaptive controller adjusts the $K_P$, $K_I$ and $K_D$ parameters in real time as shown in Figure 9.

The simulation results show that the closed-loop system with fuzzy adaptive PID controller has improved response time, overshoot, and dynamic performance compared with the conventional PID controller with the same initial parameters, especially in the case of disturbances.

Further, track point tracking simulation is performed. The starting point is set to point (100, 100), the starting heading is 90${}^{\circ }$, with five tracking points (200, 200), (300, 200), (400, 300), (500, 300), (600, 400), and the simulation results are shown in Figure 10.

As shown in Figure 10, with the effect of wind and wave disturbance, the track error accumulates under the PID control, and converges and remains stable after the fuzzy adaptive PID regulation, indicating that the track point tracking based on the fuzzy adaptive PID machine is better than that of the PID controller.

5. Analysis of Experimental Results

During the sailing of the USV, the 4G router and differential GPS are used to obtain information such as latitude and longitude, sailing speed, current heading angle, etc. The software platform control system is built on the IPC to correct the error angle in real time and send control commands to the microcontroller to control the left and right propellers respectively to achieve heading control and track point tracking, and the sailing test is shown in Figure 11.

The designed catamaran USV consists of two 450W propeller thrusters, thruster controller, IPC, differential GPS, 4G router and power supply system, the external design is shown in Figure 12 and the internal design is shown in Figure 13.

The GPS data sheet of the pre-navigation track was collected in advance, and the coordinates (30°46’21.6403 "N, 104°58’48.6782 "E) were used as the starting point in this latitude and longitude range, and the starting heading was 270°, and eight track points to be tracked were selected, as shown in Figure 11 and Table 6.

As shown in Figure 14. The test results show that the differential propulsion catamaran designed independently in this paper has completed the track point tracking in the lake. The test results fully verify the accuracy of the fuzzy adaptive PID heading controller.

5.1. Obstacle Detection

By acquiring point cloud data through 3D LiDAR, the original data obtained from the scan is under the spherical coordinate system, which is converted to the attached coordinate system for the convenience of subsequent data processing and use (43):

$$P_{crmr}=\left[\begin{array}{c}R\times cos\left(\beta \right)\times sin\left(\alpha \right)\\ R\times cos\left(\beta \right)\times cos\left(\alpha \right)\\ R\times sin\left(\beta \right)\end{array}\right]+{\left[\begin{array}{c}derX\\ derY\\ derZ\end{array}\right]}_{,}$$

(43)

where $P_{crmr}$ denotes the position point information under the attached coordinate system, R denotes the distance of the scanning point from the LIDAR, $\beta$ denotes the pitch angle of the scan line where the point is located, $\alpha$ denotes the heading angle in the horizontal direction, and $derX$, $derY$ and $derZ$ denote the displacement of the origin of the attached coordinate system relative to the origin of the coordinate system where the point cloud data is located in the X, Y and Z directions, respectively.

According to the height of the USV and the installation position of LiDAR, this paper establishes the right-hand coordinate system with LiDAR as the coordinate origin, sets the 3D points with z-value greater than 1 meter as unnecessary point cloud data, and completes the shore point cloud data filtering by z-value filtering method, and the comparison before and after processing is shown in Figure 15. It can be seen from the figure that the amount of point cloud data is obviously reduced.

When there are multiple points, the maximum relative height difference $\Delta H$ is obtained by counting the highest value $H_{\max }$ and the lowest value $H{z}_{\min }$ in the raster, and if the difference $\Delta H$ is greater than the threshold n, the raster is marked as an obstacle point. This relative height difference calculation method effectively reduces the errors caused by the horizontal and vertical rocking motion of the USV and the water surface trash on the point cloud data.

The processing results are obtained as shown in Figure 16. From Figure 5 and Figure 6, it can be seen that the raster attributes are determined by z-value filtering method and threshold segmentation method, and finally the obstacle raster map is obtained as Figure 16 (b), compared with Figure 16 (a), the unnecessary point cloud data is effectively reduced without affecting the main obstacle structure information.

The raster processed data are all discrete points, and there are isolated points collected under the influence of variable environment in some of the raster, so in order to filter out the isolated points and connect the neighboring points at the same time, the expansion corrosion operation is used to achieve clustering and contour extraction.

Based on the above method for contour extraction of the top view, and the detection results are shown in Figure 17 after the expansion and erosion process of the obstacle raster map. According to the USV raster coordinates $(150,0)$, it can be seen that the box marked as 4 in Figure 17 (a) is the obstacle ship. From Figure 17, it can be seen that when the minimum value of rectangular box sizemin is obtained larger, the missed detection may occur, such as the obstacle marked as 1 in Figure 17 (a) is not detected. Therefore, the design takes sizemin$=40$ to ensure the effectiveness of obstacle detection.

6. Conclusions

In this work, a new three-degree-of-freedom unmanned boat model based on the MMG model is proposed. Different from the usual USV dynamics model [22], the hydrodynamics and Newtonian mechanics of the two-thruster catamaran USV are considered, and the unknown parameters are identified by velocity-thrust experiments and rotational motion experiments. In addition, PID heading controller and fuzzy adaptive PID heading controller are designed, and the better effect of fuzzy adaptive PID heading controller in the presence of disturbance is proved by simulation analysis and experimental data, and the trajectory point tracking experiment is designed on this basis, and the accuracy of the heading controller is verified by simulation and verification. Further, the obstacle detection is realized by using 3D LiDAR, and the obstacle contour extraction is completed by filtering and re-clustering the point cloud map. In our future work, we will focus on the collision avoidance control of USV motion in complex scenes, which is challenging under uncertain disturbances and strong nonlinear dynamics [35,36].