Second Path Planning for Unmanned Surface Vehicle Considering the Constraint of Motion Performance

Abstract

When utilizing the traditional path planning method for unmanned surface vehicles (USVs), ‘planning-failure’ is a common phenomenon caused by the inflection points of large curvatures in the planned path, which exceed the performances of USVs. This paper presents a second path planning method (SPP), which is an initial planning path optimization method based on the geometric relationship of the three-point path. First, to describe the motion performance of a USV in conjunction with the limited test data, a method of integral nonlinear least squares identification is proposed to rapidly obtain the motion constraint of the USV merely by employing a zig zag test. It is different from maneuverability identification, which is performed in combination with various tests. Second, the curvature of the planned path is limited according to the motion performance of the USV based on the traditional path planning, and SPP is proposed to make the maximum curvature radius of the optimized path smaller than the rotation curvature radius of the USV. Finally, based on the ‘Dolphin 1’ prototype USV, comparative simulation experiments were carried out. In the experiment, the path directly obtained by the initial path planning and the path optimized by the SPP method were considered as the tracking target path. The artificial potential field method was used as an example for the initial path planning. The experimental results demonstrate that the tracking accuracy of the USV significantly improved after the path optimization using the SPP method.

1. Introduction

Unmanned surface vehicle (USV), which has attracted significant research attention in recent years, is used for dangerous and inhospitable missions [1,2]. At present, a USV mainly consists of a motion control system [3], sensor system, and communication system.

Path planning [4,5], as the core of USV research, represents the intelligence level of the USV to a certain extent. The path planning method can be used to determine an optimal path from the starting point to the end point, which mainly includes the A* algorithm [6,7,8], genetic algorithm [9], artificial potential field method [10], ant colony algorithm [11], and particle swarm algorithm [12,13]. However, all the abovementioned methods of USV path planning present several drawbacks. The optimal set of path planning is solved based on the shortest distance standard, mostly on the premise that the USV is regarded as a mass point, while ignoring its maneuvering and turning performances. This results in the inability of the underactuated USV to track path nodes with large curvatures beyond the limitations of its own motion performance. In practical engineering applications, planning failures, such as the inaccessibility to the target, the inability to complete the obstacle avoidance task, or detouring, may occur during the trajectory tracking of the USV, thus increasing the difficulty of the tasks.

At present, there are several path planning smoothing methods. For the path planning of a mobile robot, based on the A* algorithm [6,14], all nodes in the planning path are traversed in the grid environment. When there is no obstacle on the connecting line of a node before and after, the intermediate node of the extended line is removed to reduce the number of path turns. However, this method does not consider the maneuverability of the USV in optimization, only the reduction of the number of turns to achieve the optimization effect. For the path planning of an automatic underwater vehicle (AUV), based on the genetic algorithm [15,16], the B spline interpolation method was introduced into the objective function for path smoothing. However, when a few path nodes exceed the turning ability of the AUV, irrespective of the fit between the nodes, the curve formed is unreachable. For the path planning of the USV, based on the statistical method of regression [17], the path smoothing method is dependent on a large amount of actual navigation data, and it is based on an improved A* algorithm (FAA*) [18], to achieve path smoothing. This method limits the initial heading angle of the USV and removes the acute angle section in the planned path. For the path planning of a high-speed numerical control machine, the hyperboloid sheet method is used to solve the path smoothing problem of the continuous path, combined with a straight line and arc [19,20]. However, this method is used to perform curve fitting on planned path points, and the slice of the high-speed cutting machine is more flexible and demonstrates good maneuverability.

Although the abovementioned methods are beneficial to the smooth processing of path planning to a certain extent, in the application of USVs, the methods demonstrated poor adaptability, and the results were accidental. In this paper, using the zig zag test, a rapid and one-time identification and analysis method for the motion performance of the USV is proposed. A second path planning (SPP) method is then proposed to consider the constraints of the USV performance. This method can realize the re-optimization based on the existing path planning method to obtain the optimal path within the range of the motion ability of the USV, and to effectively prevent the occurrence of planning failure. Finally, using the improved artificial potential field path planning method [21] as an example, a second optimization strategy was introduced and applied to the ‘Dolphin 1’ mini-type USV. Moreover, a simulation comparison test was carried out to verify the feasibility and effectiveness of the method.

2. Analyzing the Motion Performance of USV

When the traditional path planning method is combined with the trajectory tracking guidance algorithm of the USV, the planning failure phenomena occur, such as detouring and collision, given that this method considers the USV as a mass point, which is an ideal state. Therefore, in the process of the path planning of the USV, the motion ability of the USV, especially the minimum radius of the turning circle, should be considered.

2.1. Modeling the USV Maneuverability

To describe the motion of the USV, two right-handed rectangular coordinate systems were adopted [22]. The first is the inertial frame, $o_0-x_0{y}_0{z}_0$, and the second is the body-coordinate system, $o-xyz$ (Figure 1). The inertial frame was fixed on the earth. The plane, $o_0-x_0{y}_0$, was on the undisturbed free surface, and the axis, $x_0$, was directed forward to the initial straight course of the USV with the axis, $z_0$, directed downward. The body-coordinate system was fixed on the vehicle, and the origin was located in the middle of it. The plane, $o-xy$, was on the undisturbed free surface, the axis, $x$, was in the direction of the bow, the axis, $y$, was in the direction of the starboard, and the axis, $z$, was directed downward.

The USV motion equation of six degrees of freedom can be expressed as follows:

$$\{\begin{array}{l}m(\dot{u}+qw-rv)=X\\ m(\dot{v}+ru-pw)=Y\\ m(\dot{w}+pv-qu)=Z\\ I_x\dot{p}+(I_z-I_y)qr=L\\ I_y\dot{q}+(I_x-I_z)rp=M\\ I_z\dot{r}+(I_y-I_x)pq=N\end{array}$$

(1)

where the first three equations are the expressions of the motion theorem of the mass center in the body coordinate system, and the last three equations are the Euler dynamics equations of the rotation of a rigid body around a fixed point.

Only considering the horizontal motion, $p=q=w=0$, the linear Equation (2) of the vehicular heading response can be obtained through the transformation of Equation (1):

$$T_1{T}_2\ddot{r}+(T_1+T_2)\dot{r}+r=K\delta +K{T}_{\delta }\delta$$

(2)

In 1957, Nomoto [23] suggested that the first order linear differential equation can be approximately substituted for the second order equation, in the case wherein steering is infrequent, as shown in Equation (3):

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

(3)

The first order nonlinear Equation (4) is obtained by adding the nonlinear term to the motion of the large rudder angle, low speed, and the study of small ships:

$$T\dot{r}+r+\alpha r^3=K\delta$$

(4)

The mini-type USV evaluated in this study uses two electric propellers as the power device, with a maximum voltage of 12 V. Different voltage values correspond to different speed values. In combination with the model of thrust and velocity in [24], and with reference to the rudder and heading model, the corresponding relationship between the speed and voltage of the USV was established. The speed model of the USV can be expressed as follows:

$$u=k_1{n}^2+k_{2}n+k_3$$

(5)

where $u$ is the longitudinal speed of the USV, $n$ is the propeller voltage, and $k_1,k_2,k_3$ represent the system parameters. The transverse speed can be neglected due to the low speed of the USV evaluated in this study.

In summary, the maneuverability model of the USV can be expressed as follows:

$$\{\begin{array}{l}\dot{x}=u\cos \psi -v\sin \psi \\ \dot{y}=u\sin \psi +v\cos \psi \\ \dot{\psi }=r\\ \dot{r}=\frac{1}{T}(K\delta -r-\alpha r^3)\\ u=k_1{n}^2+k_{2}n+k_3\end{array}$$

(6)

2.2. Integral Nonlinear Least Squares Method

The least squares method [25] is one of the most commonly used motion parameter identification methods. However, this method was typically used to identify the nonlinear motion equation by a combination of experiments, i.e., the zig zag test and the turning circle test. In this paper, an integral nonlinear least squares method is proposed. The nonlinear parameters of the motion equation are transformed and processed. Using a set of zig zag test data, the nonlinear motion equation can be rapidly identified. Based on the identification results, the course control parameters can be predicted, and the motion ability can be analyzed.

Based on the ‘Dolphin 1’ prototype USV, the integral nonlinear least square method was adopted to identify the parameters of the first-order nonlinear control model using the zig zag test data. According to the identified parameters, the corresponding zig zag test manipulation simulation model was established using MATLAB, and the reliability of the method was confirmed by the error between the simulation and actual case. The experimental data were derived from the zig zag test in the Songhua River in Harbin, Heilongjiang, China. Figure 2 presents the layout of the ‘Dolphin 1’ in the Songhua River, and the test data are shown in Figure 3. The ‘Dolphin 1’ is a mini unmanned catamaran with a single floating body length of 2.0 m, diameter of 0.25 m, and a two floating body spacing of 1.1 m. It is propelled by two propellers with a rear-mounted rudder plate, and the maximum speed is 1.2 m/s.

At 10.0 volts, the ‘Dolphin 1’ carried out the zig zag test control experiment at a speed of 1.08 m/s. The variation curve of the heading and rudder angle with respect to time is shown in Figure 3.

According to the test, the first order nonlinear K-T Equation (6) was identified using integral nonlinear least squares. Referring to Figure 4, given that the vehicle has no real-time measurement record of angular acceleration, the integral of both sides of Equation (6) was carried out based on the time region, [a,b]. By means of the integral, the angular acceleration was eliminated, and the heading angle data was introduced to identify the mini-type USV maneuvering model:

$$T{\displaystyle \underset{\mathrm{a}}{\overset{b}{\int }}\dot{r}dt+{\displaystyle \underset{a}{\overset{b}{\int }}rdt}}+\alpha {\displaystyle \underset{a}{\overset{b}{\int }}{r}^{3}dt=K{\displaystyle \underset{a}{\overset{b}{\int }}{\delta }_{m}dt}}$$

(7)

According to the actual field test data, the operating tempo of the program of the USV control system was 0.15 s. Hence, the N equal interval was adopted, and the equal spacing value was 0.15. For the i-th interval, the linear terms were integrated and then discrete:

$$\{\begin{array}{l}{a}_i={\displaystyle {\int }_a^i{\delta }_m(t)dt}\\ b_i=-[\dot{\phi }(i)-\dot{\phi }(a)]=-[r(i)-r(a)]\\ {\theta }_i=\phi (i)-\phi (a)\end{array}$$

(8)

For the nonlinear term, $c_i={\displaystyle \underset{a}{\overset{i}{\int }}{r}^{3}dt}$, given that r is a function of t, the integral of $r^3(t)$ with respect to the time, t, is complex and difficult to solve. When the experimental data is discrete, the Newton-Cotes quadrature equation is adopted to calculate the product, as shown in Equation (9).

Definition 1.

[26] Integrand, $f(x)\in [a,b]$, given a set of nodes, $a\le x_0<x_1<\dots <x_n\le b$, and given the value of the function, $f(x)$, on these nodes, the Lagrangian interpolation polynomial, $L_n(x)$, is applied; thus:

$$\begin{array}{l}{I}_n(f)={\displaystyle \underset{a}{\overset{b}{\int }}f(x)dx\cong {\displaystyle \underset{a}{\overset{b}{\int }}{L}_n(x)dx={\displaystyle \underset{a}{\overset{b}{\int }}[{\displaystyle \sum _{k=0}^n{l}_k(x)f(x_h)}]dx}}}\\ ={\displaystyle \sum _{k=0}^{n}f(x_k){\displaystyle \underset{a}{\overset{b}{\int }}{l}_k(x)dx}}\\ ={\displaystyle \sum _{k=0}^n{A}_{k}f(x_k)}\end{array}$$

(9)

where $A_k={\displaystyle \underset{a}{\overset{b}{\int }}{l}_k(x)dx}$. Equation (9) is referred to as the interpolation formula. The remainder of the interpolation formula is shown in Equation (10):

$$E(f)={\displaystyle \underset{a}{\overset{b}{\int }}[f(x)-L_n(x)]dx={\displaystyle \underset{a}{\overset{b}{\int }}{R}_n(x)dx={\displaystyle \underset{a}{\overset{b}{\int }}\frac{f^{(n+1)}(\zeta )}{(n+1)!}}}}{\omega }_{n+1}(x)dx$$

(10)

where $\zeta \in (x_0,x_n)$ is dependent on $x$. ${\omega }_{n+1}(x)=(x-x_0)(x-x_1)\dots (x-x_n)$.

The quadrature coefficient, $A_k$, is independent of $f(x)$, and it is related to the equidistant nodes, $x_k$, and integral interval, $[a,b]$. The integral formula of the Newton-Cotes (11) can be obtained by transforming $A_k$:

$$I_n(f)=(b-a){\displaystyle \sum _{k=0}^n{C}_k^{(n)}}f(x_k)$$

(11)

$$C_k^{(n)}=\frac{{(-1)}^{n-k}}{nk!(n-k)!}{\displaystyle \underset{0}{\overset{n}{\int }}t(t-1)\cdot \cdot \cdot }(t-k+1)(t-k-1)\cdot \cdot \cdot (t-n)dt$$

(12)

where $C_k^{(n)}$ is referred to as the cotes coefficient. The coefficient of cotes is shown in

Table 1. Coefficient.

$\mathit{n}$	${\mathit{C}}_{\mathit{k}}^{(\mathit{n})}$
1	$\frac{1}{2}$	$\frac{1}{2}$	/	/	/
2	$\frac{1}{6}$	$\frac{4}{6}$	$\frac{1}{6}$	/	/
3	$\frac{1}{8}$	$\frac{3}{8}$	$\frac{3}{8}$	$\frac{1}{8}$	/
4	$\frac{7}{90}$	$\frac{32}{90}$	$\frac{12}{90}$	$\frac{32}{90}$	$\frac{7}{90}$

.

Based on the actual test data, $h=0.15$, the method takes $n=1.0$, divides the interval, $[a,i]$, into m equal parts, counts $c=1.0$, and uses a composite integral formula to reduce the remaining terms.

According to Equation (10), the remainder of the composite newton-cotes interpolation can be obtained:

$$E_f={\displaystyle \sum _{k=0}^{m-1}\left[-\frac{h^3}{12}\ddot{f}({\zeta }_k)\right]\approx -\frac{h^2}{12}[\dot{f}(i)-\dot{f}(a)]}$$

(13)

When $n=1.0$, the composite function, $g(x)=r^3(x)$, can be considered as $g(r)=r^3$. Moreover, by considering the concavity and convexity of the function, the right side of the equation is divided into two parts in the process of the composite Newton-Cotes quadrature, i.e., the exact solution and the approximate solution. In addition, the gain coefficient is introduced to reduce the interpolation remainder, and $c_i$ is shown in Equation (14):

$$c_i={\displaystyle \underset{a}{\overset{i}{\int }}{r}^3(t)dt=\alpha \cdot \frac{1}{2}\cdot (r^3(i)-r^3(a))\cdot \Delta t+r^3(a)\cdot \Delta t}$$

(14)

where $\alpha =0.5$.

Utilizing the least square method, the left and right sides of Equation (15) are respectively considered as functions, and the square of their difference can be minimized to obtain the values of $K,T,\alpha$:

$$J={\displaystyle \sum _{i=1}^N{(K{a}_i+T{b}_i+\alpha c_i-{\theta }_i)}^2}$$

(15)

$$\{\begin{array}{cc}\frac{\partial J}{\partial K}=0& 2{\displaystyle \sum (K{a}_i^2+T{b}_i{a}_i+\alpha c_i{a}_i-{\theta }_i{a}_i)}=0\\ \frac{\partial J}{\partial T}=0& 2{\displaystyle \sum (K{a}_i{b}_i+T{b}_i^2+\alpha c_i{b}_i-{\theta }_i{b}_i)}=0\\ \frac{\partial J}{\partial \alpha }=0& 2{\displaystyle \sum (K{a}_i{c}_i+T{b}_i{c}_i+\alpha c_i^2-{\theta }_i{c}_i)}=0\end{array}$$

(16)

The same method can also be used to solve the parameters of the velocity model.

2.3. Identification of Maneuverability and Analysis of Motion Ability

2.3.1. Identification of USV Maneuverability by Field Test Data

Given that the data from the zig zag test conducted in the Songhua River had wild values, the outliers were primarily deleted by data fitting. The curve fitting heading angle was a 7th order Fourier function [27], and the curve fitting heading angular velocity was the piecewise cubic polynomial function. The fitting curves of the heading angle and heading angular velocity are shown in Figure 5a–d.

According to the fitting curve, the wild values (data points with large deviation) were eliminated. $\{\begin{array}{l}K=0.286642\\ T=0.410205\\ \alpha =0.008477\end{array}$, as obtained from Equation (16). A zig zag test manipulation experimental simulation model was established in MATLAB. Based on the solved values of $K,T,\alpha$, the predicted heading angle obtained by the simulation under the same rudder angle input was compared with the actual test data. The results are presented in Figure 6 and Figure 7.

According to Figure 7, at the same input rudder angle, the heading angle error was in the $\pm 5^{°}$ interval. Considering the communication delay, inertia of the USV, accuracy of the rudder angle feedback, and that of the heading angle sensor, the fitting curve was consistent with the actual data.

Based on the velocity and voltage data, the least squares identification method was used to obtain $k_1$= −0.006, $k_2$= 0.159, and $k_3$= 0.004. The comparison results are presented in Figure 8 and Figure 9.

2.3.2. Simulation and Analysis of USV Motion

When the USV is sailing in a straight line, the vehicle deviates from the original route and makes a turning movement at a certain rudder angle. The radius of the circle formed by the trajectory of the USV center of gravity is referred to as the radius of steady rotation. K is the constant rotation angular velocity due to the unit rudder angle, which is directly proportional to the radius of rotation, and inversely proportional to the curvature of steady rotation. Therefore, the maximum rotational curvature can be considered as an evaluation index of the USV motion ability.

Using Equation (16), the dynamic model of the ‘Dolphin 1’ USV was solved, and the turning circle simulation model was established based on MATALB software. Moreover, a proportional–integral–derivative (PID) control method was adopted to establish the control law [28], as shown in Equation (17):

$$u=K_{p}e+K_i{\displaystyle \int edt+K_d\frac{de}{dt}}$$

(17)

where u is the output of the controller, and e is the deviation between the given expected value and the actual output value. The simulated steady rotation trajectory is presented in Figure 10. The initial position coordinate of the USV was (0,0), the rudder angle was constant at 30.0°, and the iterative step length and control cycle were 0.15 s.

To confirm the reliability of the motion analysis, the field steady turning test data of ‘Dolphin 1′ were used for comparison. The data were collected and recorded at a rudder angle with a fixed value of 30.0°. The trajectory of the USV is shown in Figure 11.

In Figure 11, the distance between two course path points at A(126.53852,45.79677) and B(126.53851,45.79691) were calculated. Using Equation (18), the longitude and latitude coordinates can be converted from angular units to metric units:

$$R_{tur}=\frac{\pi }{360}\sqrt{{(A_{lon}-B_{lon})}^2+{(A_{lat}-B_{lat})}^2}{R}_{globle}$$

(18)

where $R_{globle}=6371.393$ km is the radius of the earth, and the radius, $R_{tur}$, of ‘Dolphin 1’ is 7.8 m. By analyzing and comparing Figure 10 and Figure 11, the two turning trajectories were found to be identical.

The integral nonlinear least square method was employed to analyze and determine the performance index of the USV, which was found to demonstrate a high efficiency, convenience, and high accuracy.

3. SPP of USV

In this paper, a second path planning method (SPP) is presented, which is not restricted by the USV motion ability. The improved artificial potential field method proposed by Huang Xinghua [21] was selected as the first path planning method in this study. On the premise of the existing planning path, the second path optimization was carried out to calculate an optimal path constrained by the motion performance of the vehicle. This method was used to solve the problem and deficiency of the traditional USV path planning.

3.1. SPP Model

The SPP of the USV refers to the optimal path constrained by the performance of the USV, which was selected according to the existing optimal path and the three-point geometric relationship. This method preserves the preferred criteria of the traditional path planning.

The SPP model can be described as follows.

Assuming that $x,y$ are the coordinates in the inertial coordinate system, and $p_i={[x_i,y_i]}^T$ is defined as the coordinate position of a path point, the set of $n$ path points derived from the traditional path planning can be expressed as $p={[p_1^T\dots \dots p_n^T]}^T$. The check point at time $k$ can be expressed as $p_k={[x_k,y_k]}^T$, and $\varpi$ is the second planning task variable. The task function is established by combining the geometric constraint relationship of the path, and the corresponding task function can be expressed as shown in Equation (19):

$$\varpi =f(p_{k-1},p_k,p_{k+1})$$

(19)

$$\{\begin{array}{ll}\left(\varpi -{\varpi }_d\right)<0,\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}& \{\begin{array}{l}{p}_{k-1}=p_k\\ p_k=p_{k+1}\end{array}\\ \left(\varpi -{\varpi }_d\right)\ge 0,\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}& \{\begin{array}{l}{p}_k=p_{k+1}\\ p_{k+1}=p_{k+2}\end{array}\end{array}$$

(20)

where ${\varpi }_d$ is the constraint index variable of the motion performance of the vehicle. When $\left(\varpi -{\varpi }_d\right)<0$, the current check point is maintained and the next path point is discussed. When $\left(\varpi -{\varpi }_d\right)\ge 0$, the current checkpoint is discarded and the next point of the optimal set of paths is tested until the checkpoint is the endpoint of the path. The schematic diagram of SPP is shown in Figure 12.

The second path planning is applicable to the traditional USV path planning method, which can be used to solve a series of discrete path point sets.

Using the improved artificial potential field method proposed by Huang Xinghua [21] as an example, as shown below, the path planning of the USV involved the implementation of the secondary optimization subject to its motion constraint.

3.2. An Improved Path Planning for the Artificial Potential Field Method

The artificial potential field method (APF), initially proposed by Khatib, was applied to the obstacle avoidance motion planning of a robot manipulator, which had as its objective the realization of the real-time obstacle avoidance of a mechanical arm. The essence of the APF is to define an abstract potential field artificially for the operating space of the robot, which is the superposition of the gravitational field of the target position and the repulsive force field of the obstacle in the environment. The negative gradient of the force field is the virtual force that acts on the robot, and the resultant force of gravity and repulsion represent the accelerating force of the robot.

Although the path planned by the artificial potential field method is efficient, smooth, and safe, there are several drawbacks related to this method [29,30,31,32,33,34], i.e., (1) the local minimum problem; (2) the inability to navigate through closely-situated obstacles; and (3) oscillations near the obstacle. The main objective of the improved artificial potential field method [21] proposed by Huang Xinghua is the solution of these problems to establish a new potential field function. Hence, during the approach of the target point by the robot, the repulsion field is close to zero and the position of the robot target point is the minimum point of the entire situation field; thus, the target can be reached. The repulsion force potential field function of the improved artificial potential field method can be expressed as shown in Equation (21):

$$U_{rep}(X)=\{\begin{array}{ll}\frac{1}{2}{k}_{rep}{(\frac{1}{X-X_o}-\frac{1}{{\rho }_0})}^2{(X-X_g)}^n\hspace{1em}& (X-X_o)\le {\rho }_0\\ 0& (X-X_o)\ge {\rho }_0\end{array}$$

(21)

where 0 ≤ n < 1.

The negative gradient of the potential field function is also considered as the corrected repulsive force of the repulsion field, as follows:

$$F_{rep}(X)=-\nabla U_{rep}(X)=\{\begin{array}{cc}{F}_{rep1}+F_{rep2}\hspace{1em}& (X-X_o)\le r_o\\ 0& (X-X_o)>r_o\end{array}$$

(22)

where:

$$F_{rep1}=k_{rep}(\frac{1}{X-X_o}-\frac{1}{{\rho }_o})\frac{1}{{(X-X_o)}^2}\frac{\partial (X-X_o)}{\partial X}{(X-X_g)}^n$$

(23)

$$F_{rep2}=-\frac{1}{2}{k}_{rep}{(\frac{1}{X-X_o}-\frac{1}{{\rho }_o})}^2\frac{\partial {(X-X_g)}^n}{\partial X}$$

(24)

where $U_{rep}$ is the repulsive field of the obstacle, $\rho$ is the distance between the robot and the obstacle, ${\rho }_0$ is the maximum influencing range of the obstacle, $k_{rep}$ is the constant of the repulsive field, $n$ is positive, $X$ represents the coordinates of the robot, $X_g$ represents the coordinates of the target point, and $\nabla$ represents the sign of the gradient.

The gravitational potential field function can be expressed as follows:

$$U_{att}(X)=\frac{1}{2}k{(X-X_g)}^2$$

(25)

where $U_{att}$ is the gravitational field generated by the target point, and $k$ is the gain constant. Gravity can be expressed as shown in Equation (26):

$$F_{att}(X)=-\nabla U_{att}(X)=k(X_g-X)$$

(26)

3.3. SPP of the Improved Artificial Potential Field Method

Combined with the improved artificial potential field method, the theoretical block diagram of the second path planning constrained by the motion of the USV is shown in Figure 13.

When the vehicle is performing the navigation task, the starting position, target position, and obstacle position are determined according to the mission information and sea chart information. According to the objective function of the artificial potential field method, a set of path points are obtained. The starting position is reserved, and the second planning task function is analyzed from the second point, as shown in Equation (27):

$$\begin{array}{ccc}{D}_k=\Vert p_k-p_{k-1}\Vert \hspace{1em}& D_{k+1}=\Vert p_{k+1}-p_k\Vert \hspace{1em}& D_{k-1}=\Vert p_{k-1}-p_{k+1}\Vert \end{array}$$

(27)

$$\tilde{D}={\displaystyle \sum _{i=0}^2{D}_{k-1+i}}$$

(28)

$$J_k=\frac{1}{2}{[\tilde{D}(\tilde{D}-2D_k)(\tilde{D}-2D_{k-1})(\tilde{D}-2D_{k+1})]}^{\frac{1}{2}}$$

(29)

where $p_k$ represents the location coordinates of the path point that correspond to the current check moment, $k$; $p_{k+1},p_{k-1}$ are the path points of the current test point forward and backward once, respectively; $D_i$ is the distance of the two path points; $\tilde{D}$ is the distance between the path points; and $J_k$ is the size of the region across the three path points. The second planning task variable can be expressed as follows:

$$\varpi =f(p_{k-1},p_k,p_{k+1})=\frac{4J_k}{D_k{D}_{k-1}{D}_{k+1}}$$

(30)

$${\varpi }_d=\frac{1}{r}$$

(31)

where ${\varpi }_d$ is the constraint index variable of the USV motion performance, and its value can be determined by the rotational curvature of the vehicle. Substituting Equations (30) and (31) into Equation (19), the second optimization can be evaluated.

4. Simulation Test of Trajectory Tracking of USV by SPP Method

Considering the ‘Dolphin 1’ mini-type USV as the test object and using the improved artificial potential field method proposed by Huang Xinghua [21] as an example, the path planning optimization method presented in this paper was simulated and verified. The simulation platform was developed using MATLAB software, the PID control method was employed to control the course and speed, and the trajectory tracking method was implemented in conjunction with the PP method.

4.1. Comparative Experiment of Small Planning Step

The initial position of the USV was (10 m, 5 m), the target position was (1000 m, 800 m), the length of the vehicle was $l=2.0$ m, and the planning step, $t_p$, was $t_p=2.0$ m. The locations of the obstacles were (200 m, 150 m) and (500 m, 400 m), respectively. The radius of the obstacles, $r$, corresponded to 10.0 m and 30.0 m, and the influencing radius was defined as $r\_in=r+\lceil t_p/l\rceil \ast l$, where $\lceil \rceil$ is the integer sign considered upward.

The results of the comparison between the initial path planning using the improved artificial potential field method and the second path planning under the consideration of the motion performance of the USV are shown in Figure 14a. Figure 14b presents the local magnification of the path planning near the first obstacle. In Figure 14a, the solid line represents the initial planning path, and the dotted line represents the optimized path by the secondary planning. The figure illustrates that the initial planning path near the obstacle produces zig-zag oscillations, and the intensity of the path oscillation is related to the impact of the obstacle on the connectivity of the starting point and the end point, in addition to the size of the obstacle. As shown in Figure 14b, given that the SPP method is optimized based on the initial path; it retains the fitness function standard of the initial planning and reduces the oscillation of the initial planning path through optimization.

On the Simulink simulation platform, a USV motion model was developed. The iterative step length and control cycle were 0.15 s. The parameters of the course PID controller were set as $p=1.0,i=0.001,d=1.0$, the parameters of the speed controller were $p=2.0,i=0.001,d=0.001$, and the trajectory tracking adopted the pure tracking (PP) method [35,36,37]. In the simulation experiment, when the time exceeded 50.0 s, the target point was replaced, although the vehicle had not reached the distance threshold of the target in the current tracking stage. The simulation results of the USV trajectory tracking of the two methods are respectively shown in Figure 15a,c. The tracking deviation of the two methods is shown in Figure 15b,d.

As can be seen from Figure 15a,c, under the premise of a small planning step length, $t_p=2.0$ m, the vehicle can track the optimized path after the initial planning using an artificial potential field method and secondary planning method, respectively, and successfully complete the obstacle avoidance task. The comparison and analysis of Figure 15b,d illustrate that although both methods can be used to achieve obstacle avoidance and complete the tracking tasks by SPP, the actual course path of the vehicle was more consistent with the planned path than by APF. This is because secondary programming is an optimization standard of the maneuverability of the USV, which leads to a higher tracking accuracy.

4.2. Comparative Experiment of Large Planning Step

With the same simulation environment and control parameters as in the abovementioned experiment, the USV sailed from the same position to the same target, and the planning step, $t_p$, was set greater than the length of the vehicle by a factor of 5 ($t_p=$10.0 m). The results of the comparison between the two methods with respect to the USV trajectory tracking are respectively shown in Figure 16a,b.

As can be seen from Figure 16a, the USV generated roundabout routes when it tracked the planned path points during its approach of the first and second obstacles, which resulted in a failure to complete the obstacle avoidance task (given that the obstacle was a virtual object in the simulation, the task was not interrupted due to collision, and the experiment was continued). Figure 17 presents the tracking deviation of the USV for the duration of the trajectory tracking task. A maximum path deviation of 34 m was generated near the first obstacle, which gradually decreased to a value of approximately 4.0 m, and then remained stable. It should be noted that the distance between the navigation position of the vehicle and the tracking target was reduced to 4.0 m in the simulation; thus, the task was considered as completed. In the vicinity of the second obstacle, the maximum deviation was approximately 77.0 m, and due to its course deviation, the tracking process always sailed in an approximate 3/4 minimum turning circle in the following trajectory tracking process. This resulted in the oscillation of the tracking deviation within the range of 8 m until the target point was reached.

According to the analysis results in Figure 16b and Figure 18, during the large planning step, the USV tracked the planned path optimized by the secondary planning, and then successfully completed the obstacle avoidance task. The tracking deviation, namely, the along track error, was less than 4 m; thus, the vehicle was considered to reach the target point. As can be seen from Figure 18, the obstacle has no effect on the tracking accuracy, and the tracking deviation was approximately 4 m. Figure 17 and Figure 18 illustrate the decrease in the number of track points after optimization, which reduced the possibility of the occurrence of detour road events. The experimental results indicate that the SPP method improves the tracking accuracy of the USV and reduces the length of the detour route, thus reducing the energy consumption and preventing collisions.

4.3. Comparative Experiment of Multi-Obstacle Path Planning

According to the abovementioned experiment, in the case of two obstacles in the small planning step, the USV successfully tracked the path planned by APF and SPP, and completed the obstacle avoidance task. With an identical simulation environment and control parameters to those of the abovementioned experiment, the vehicle navigated from the same starting position to the same target point, thus avoiding six obstacles in the course, which had position coordinates of (60 m, 50 m), (200 m, 150 m), (400 m, 300 m), (500 m, 400 m), (600 m, 400 m), and (720 m, 570 m). In addition, the obstacle radius corresponded to 5.0 m, 10.0 m, 30.0 m, 50.0 m, 30.0 m, and 20.0 m, respectively. The results of the comparison between the initial path planning utilizing the improved artificial potential field method and the second path planning under the consideration of the motion performance of the USV are shown in Figure 19a,b. The trajectory tracking deviations for the two methods are shown in Figure 20 and Figure 21.

As can be seen from Figure 19a and Figure 20, using the APF method, the USV successfully completed the obstacle avoidance task of the first three obstacles, and the trajectory tracking deviation was constant at approximately 4 m. However, when sailing near the fourth and fifth obstacles, due to the limitation of the motion performance of the vehicle, the planned path exceeded its minimum turning radius, which resulted in a collision with the fifth obstacle. Moreover, the trajectory tracking deviation was approximately 78 m, and the obstacle avoidance task was not completed.

Figure 19b and Figure 21 reveal that on the basis of the SPP method, the USV successfully completed the obstacle avoidance task of all six obstacles, and the track tracking deviation was constant at approximately 4.0 m. The optimized path of the second path planning method can therefore effectively ensure the completion efficiency of the obstacle avoidance task and improve the precision of the trajectory tracking control.

5. Discussion

(1)

By the analysis of the path planning theory and the USV control model, the traditional path planning method was found to lead to the ‘planning failure’ phenomenon when applied to the trajectory tracking field of the USV path planning.

(2)

In this study, an integral nonlinear least squares method was developed. In the case of limited test data, a nonlinear motion model of USV was rapidly identified by merely conducting a type of maneuvering experiment, which can effectively predict the motion performance indexes, such as the rotatory curvature of the vehicle.

(3)

The SPP method was presented under the consideration of the USV motion performance, which reduces the influence of the motion performance of the vehicle during the trajectory tracking and helps lower the risk of failing to complete the obstacle avoidance task using the traditional path planning method. The proposed SPP method can effectively prevent the issue of an untraceable USV and improve the USV tracking accuracy.

6. Conclusions

This paper presented an integral nonlinear least squares method to rapidly and effectively obtain the motion constraint of USVs, and an SPP method was proposed under the consideration of the USV motion performance.

In future work, the path optimization method in a bounded environment, especially an uneven boundary, should be considered. In addition, a field test of the method should be carried out.