Trajectory Planning of Autonomous Underwater Vehicles Based on Gauss Pseudospectral Method

Abstract

This paper aims to address the obstacle avoidance problem of autonomous underwater vehicles (AUVs) in complex environments by proposing a trajectory planning method based on the Gauss pseudospectral method (GPM). According to the kinematics and dynamics constraints, and the obstacle avoidance requirement in AUV navigation, a multi-constraint trajectory planning model is established. The model takes energy consumption and sailing time as optimization objectives. The optimal control problem is transformed into a nonlinear programming problem by the GPM. The trajectory satisfying the optimization objective can be obtained by solving the problem with a sequential quadratic programming (SQP) algorithm. For the optimization of calculation parameters, the cubic spline interpolation method is proposed to generate initial value. Finally, through comparison with the linear fitting method, the rapidity of the solution of the cubic spline interpolation method is verified. The simulation results show that the cubic spline interpolation method improves the operation performance by 49.35% compared with the linear fitting method, which verifies the effectiveness of the cubic spline interpolation method in solving the optimal control problem.

1. Introduction

With the increasing attention to marine resources, underwater exploration and development technology have become hotspots in marine research in various countries. The autonomous underwater vehicle (AUV) has become an important tool for marine resources development due to its strong autonomy and controllability, low operational risk, economic advantages and other performance characteristics [1]. In a complex and limited marine environment, an AUV should avoid obstacles by moving forward and backward, traversing, floating and sinking, while completing tasks such as maintenance [2], underwater search [3] and detection [4]. As it is difficult for an AUV to supplement energy in the process of performing tasks, it is of great practical significance to find an optimal trajectory with low energy consumption and autonomous obstacle avoidance capability.

The trajectory planning problem is also called the trajectory optimization problem. It is essentially a nonlinear optimal control problem, which satisfies state constraints, control constraints and path constraints. According to the given starting point and destination, a trajectory is planned that satisfies the task requirements in time and space [5]. At present, most of the research results of motion trajectory planning theory originate from the fields of aircraft [6], industrial robots [7], unmanned aerial vehicles [8] and autonomous vehicles [9], while research on trajectory planning in the field of AUVs is still in the initial exploration stage. Trajectory optimization is usually solved in two steps: trajectory optimization model transformation and parameter optimization.

The main methods of trajectory optimization model transformation are the indirect and the direct methods [10]. The indirect methods solve the optimal control problem using the maximum principle. Although their calculation accuracy is high, there are some problems, such as the cumbersome derivation of the optimal solution and the difficult estimation of the covariant initial values. The process of computing problems with complex constraints is very difficult, and no relevant research has been found in the field of AUV trajectory planning. The direct methods have received wide attention. Among these methods, the pseudospectral method has been the hotspot of trajectory planning research in recent years [11]. There are three main types of traditional pseudospectral methods: the Legendre pseudospectral method (LPM), the Gauss pseudospectral method (GPM) and the Radau pseudospectral method (RPM). The differences lie in the selection and configuration of the collocation points, which affect the accuracy of the optimal solution, the convergence speed and the calculation efficiency [12]. In [13], a solution strategy based on the GPM was presented to optimize the fuel-efficient winglet retractable climb trajectory for near-space variable-wing aircraft. In [14], a multi-objective excavation trajectory optimization framework based on the RPM is proposed for the trajectory planning of the unmanned electric shovel in autonomous mining scenarios. In [15], the trajectory optimization problem is numerically solved via a hp-pseudospectral sequential convex programming method, which iteratively solves a sequence of second-order cone programming subproblems until convergence is attained. The hp-adaptive pseudospectral method combines the finite element method with the traditional pseudospectral method. It can adjust the position and the number of discrete points adaptively, and has a high solving accuracy and speed [16]. In [17], the hp-adaptive pseudospectral method was applied to the unmanned aerial vehicle trajectory planning and the minimum fuel consumption and minimum climb time obtained were optimized. However, in the field of trajectory planning of underwater vehicles, most of the existing methods do not take into account the dynamic characteristics and constraints of the controlled object in trajectory planning, which makes it difficult to track the planned trajectory and to achieve real optimization.

For parameter optimization problems, common methods include the sequential quadratic programming (SQP) algorithm, the dynamic programming algorithm, Pontragin’s method, etc. At present, the SQP algorithm is usually used to solve nonlinear programming problems [18]. Pontryagin’s method allows for control based on the comparison of the current attitude to a time-varying desired attitude, allowing for much better use of control effort and command over slew orientation [19]. In [20], where the simulation was compiled using both sinusoidal and Pontryagin trajectories, Pontryagin’s method produced an autonomous trajectory that was more optimal than the sinusoidal autonomous trajectory. The Gauss Pseudospectral Optimization Software (GPOPS), which is based on the quadratic programming algorithm, is the most widely used toolbox for the optimization of trajectory parameters [21]. In [22], the MATLAB software package Sparse Nonlinear Optimizer (SNOPT) was used to solve the nonlinear programming problem. However, in the process of parameter optimization, the existing research focuses more on the accuracy of motion trajectory solutions. Because the position and constraint conditions of an AUV are dynamic, it requires that the AUV trajectory planning must be a rolling optimization method based on time windows, and its parameter optimization must have a rapid solution speed.

With the aim of addressing the above problems, combined with the working characteristics of AUVs, a multi-constraint trajectory optimization model is constructed and solved. The main work of this paper is as follows:

(1)

The motion model and the obstacle model are established, in which low energy consumption and short time are considered as the joint optimization objectives. The GPM and SQP are used to optimize the obstacle avoidance trajectory of AUVs, and the trajectory satisfying the optimization objectives and constraints is obtained.

(2)

Considering the dynamic characteristics of AUVs in the complex underwater environments, the mathematical model of AUV trajectory planning is improved by introducing dynamic constraints. The navigation trajectory obtained by the solution is more in line with the actual requirements and has better traceability.

(3)

When solving the trajectory planning problem based on the GPM, cubic spline interpolation is used to generate the initial values of variables. The search cost is reduced, the calculation speed is increased, the motion trajectory obtained by solving is smoother and the fitting effect is better.

The main structure of this paper is as follows: In Section 2, the trajectory planning problem of AUVs is introduced, and the motion model, the obstacle constraint model, the energy consumption and sailing time optimization model of AUVs are established. The trajectory optimization problem of AUVs is described by a mathematical model. In Section 3, the principle and process of the GPM and the SQP algorithm used to solve the trajectory planning problem are introduced. The initial value is generated by cubic spline interpolation and the optimal solution of the trajectory planning problem is obtained by iterative calculation. In Section 4, a motion trajectory satisfying the optimization objective and multiple constraints is obtained through simulation. The effectiveness of the proposed method for the trajectory planning is verified. Section 5 summarizes the research conclusions of this paper.

2. Problem Description

The trajectory planning problem of autonomous underwater vehicles (AUVs) in this paper can be described as follows: under the condition that the motion constraints and obstacle avoidance constraints are satisfied, finding the appropriate control amount, so that the AUV has the lowest energy consumption and the shortest sailing time when it reaches the target point. In contrast with path planning, the mathematical model of trajectory planning is generally expressed as a function related to time. Moreover, when the Gauss pseudospectral method (GPM) is used to solve the optimal control problem, the functions and equations describing the trajectory planning problem must be continuous and differentiable. Therefore, the motion constraints and obstacle constraints in the trajectory planning model need to meet the continuously differentiable conditions.

2.1. Motion Model of Underwater Robot

Under water, the motion of an AUV is generally 6 degrees of freedom, which can be decoupled into horizontal and vertical plane motion. This paper only discusses the horizontal motion of an AUV. An AUV’s mathematical model mainly includes the kinematics model and the dynamics model. The kinematics model mainly describes the relationship between the position, velocity, acceleration and time. The dynamic model mainly describes the motion characteristics of an AUV according to its mass and force.

If the rolling and pitching motions of the AUV are ignored, the kinematic model of the AUV in the horizontal motion plane can be expressed as follows:

$$\left\{\begin{array}{l}\dot{x}=u\cdot \cos \psi -v\cdot \sin \psi ,\\ \dot{y}=u\cdot \sin \psi +v\cdot \cos \psi ,\\ \dot{\psi }=r.\end{array}\right.$$

(1)

where x, y and ψ represent the position and the direction angle of an AUV in a fixed coordinate system, and u, v and r represent the velocity and the angular velocity of an AUV in a moving coordinate system.

An AUV is mainly subject to thrust, self-gravity and buoyancy when moving underwater. The dynamic equations of an AUV in the horizontal motion plane are expressed as follows [23]:

$$\left\{\begin{array}{l}{m}_{\mathrm{u}}\cdot {\displaystyle \dot{u}}=m_{\mathrm{v}}\cdot v\cdot r-d_{\mathrm{u}}\cdot u+T_{\mathrm{u}},\\ m_{\mathrm{u}}\cdot \dot{v}=-m_{\mathrm{u}}\cdot u\cdot r-d_{\mathrm{v}}\cdot v+T_{\mathrm{v}},\\ m_{\mathrm{r}}\cdot \dot{r}=m_{\mathrm{uv}}\cdot u\cdot v-d_{\mathrm{r}}\cdot r+T_{\mathrm{r}}.\end{array}\right.$$

(2)

$$\left\{\begin{array}{l}{m}_{\mathrm{u}}=m-X_{\dot{\mathrm{u}}},m_{\mathrm{v}}=m-Y_{\dot{\mathrm{v}}},\\ m_{\mathrm{r}}=I_{\mathrm{z}}-N_{\dot{\mathrm{r}}},m_{\mathrm{uv}}=m_{\mathrm{u}}-m_{\mathrm{v}},\\ d_{\mathrm{u}}=-X_{\mathrm{u}}-X_{\left|\mathrm{u}\right|\cdot \mathrm{u}}\cdot \left|u\right|,\\ d_{\mathrm{v}}=-Y_{\mathrm{v}}-Y_{\left|\mathrm{v}\right|\cdot \mathrm{v}}\cdot \left|v\right|,\\ d_{\mathrm{r}}=-N_{\mathrm{r}}-N_{\left|\mathrm{r}\right|\cdot \mathrm{r}}\cdot \left|r\right|.\end{array}\right.$$

(3)

where m is the mass of the AUV; $I_{\mathrm{z}}$ is the moment of inertia; ${(u,v,r)}^{\mathrm{T}}$ is the velocity vector; X_(·), Y_(·) and N_(·) are the hydrodynamic coefficients; and $T={(T_{\mathrm{u}},T_{\mathrm{v}},T_{\mathrm{r}})}^{\mathrm{T}}$ is the system input consisting of thrust and thrust torque.

2.2. Obstacle Constraint Model

Considering the complexity of entity modelling and the real-time computing ability of a computer, the underwater dangerous area and obstacle models are abstracted into several simple geometric figures in this study. As mentioned earlier, the function used to describe obstacles in trajectory planning must be continuous. Therefore, the function of approximate shape is used to define obstacles. At present, most of the mathematical models describing obstacles use the P criterion method. This method can conveniently represent circles, rectangles, squares, etc. [24]. Although p = 2 can be used to describe spherical or cylindrical obstacles, a larger p value should be selected in practical application to make the obstacle description more accurate. However, an excessive p value will increase computational cost, and the solution of complex dynamic equations may be not convergent [25]. In this paper, p = 2 is selected to represent the shape of obstacles with circular or elliptical envelopes.

The position relationship between the trajectory points of the AUV and the obstacle can be expressed as follows:

$$h_i={\left(\frac{x\left(t\right)-x_i}{a}\right)}^2+{\left(\frac{y\left(t\right)-y_i}{b}\right)}^2-1,i=1,\dots ,n$$

(4)

where (x(t), y(t)) represent the trajectory coordinates of the AUV, and (x_i, y_i) represent the center position of the i-th obstacle. The radius of the obstacle in both directions is defined by a and b. Changing a and b can adjust the size of the obstacle.

Any point (x, y) that makes h = 0 is on the boundary of the obstacle. When planning the obstacle avoidance trajectory of the AUV, the point (x, y) must be outside the obstacle to ensure that h > 0.

2.3. Energy Consumption and Sailing Time Optimization Model

When performing tasks, an AUV carries limited energy. To ensure the safe recovery of an AUV, energy consumption must be considered. The structural design, sailing time, sailing distance and propeller affect energy consumption, and establishing an appropriate energy consumption model is essential to achieve energy consumption optimization. The working area of an AUV is continuously changing, and the sailing speed also changes with time, so the model can be solved in the form of an integral. Another optimization objective in the trajectory planning problem is the sailing time. In numerical analysis and calculation, the position, attitude, velocity and acceleration of an AUV can be expressed as a function or equation of sailing time.

The goal of trajectory planning in this paper is to realize the joint optimization of energy consumption and sailing time, and to calculate the trajectory of an AUV reaching the destination with the lowest energy consumption and the shortest sailing time. In order to achieve the dynamic balance of the two types of objectives, weight coefficients ω₁ and ω₂ are introduced, and the weighted coefficient method is used to construct the joint optimization objective function as follows:

$$J={\omega }_1{t}_{\mathrm{f}}+{\omega }_2{{\displaystyle \int }}_0^{t_{\mathrm{f}}}\left(T_{\mathrm{u}}{}^2+T_{\mathrm{v}}{}^2+T_{\mathrm{r}}{}^2\right)\mathrm{d}t$$

(5)

where ω₁ + ω₂ = 1 and t_f represent the terminal time of navigation, and $T_{\mathrm{u}},T_{\mathrm{v}},T_{\mathrm{r}}$ represent the thrust and torque generated by the propeller.

To meet the navigation planning of different requirements, the weight coefficients ω₁ and ω₂ of the energy consumption and navigation time should be adjusted according to the actual situation. When ω₁ = 1 or ω₂ = 1, the trajectory planning achieves the single objective of the shortest sailing time or the lowest energy consumption, respectively.

3. Motion Trajectory Planning

The Gauss pseudospectral method (GPM) studied in this paper is a kind of collocation method, which uses a global interpolation polynomial to approximate the trajectory of state variables and control variables on a series of Legendre-Gauss (LG) points. Compared with the general collocation method, the GPM can obtain higher solution accuracy and faster convergence speed with fewer nodes when solving optimal control problems [26]. Trajectory planning based on the GPM mainly includes two parts: optimal control problem transformation and nonlinear programming problem solving.

3.1. Transformation of Optimal Control Problem

The direct method to solve the trajectory planning problem is to transform it into a nonlinear programming (NLP) problem through discretization [27]. Because the collocation points of the GPM are all distributed on the interval [−1,1], the time variable $\tau \in$[−1,1] is introduced. The interval $t\in [t_0,t_{\mathrm{f}}]$ is discretized to [−1,1] and the transformation form is represented as follows:

$$\tau =\frac{2t}{t_{\mathrm{f}}-t_0}-\frac{t_{\mathrm{f}}+t_0}{t_{\mathrm{f}}-t_0}$$

(6)

N LG points and the initial time ${\tau }_0=-1$ are selected as the collocation points and the state variables are approximated by N + 1 degree Lagrange interpolation polynomials.

$$x\left(\tau \right)\approx X\left(\tau \right)={\displaystyle \sum }_{i=0}^N{L}_i\left(\tau \right)x\left({\tau }_i\right)$$

(7)

where the interpolation basis function $L_i\left(\tau \right)\left(i=0,1,2,\cdots ,N\right)$ is defined as follows:

$$L_i\left(\tau \right)={\displaystyle \prod }_{j=0,j\ne i}^N\frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j}$$

(8)

The control variable is approximated by the N degree Lagrange interpolation polynomial.

$$u\left(\tau \right)\approx U\left(\tau \right)={\displaystyle \sum }_{i=1}^N\widetilde{L_i}\left(\tau \right)U\left({\tau }_i\right)$$

(9)

where the interpolation basis function $\overline{L_i}\left(\tau \right)\left(i=1,2,\cdots ,N\right)$ is defined as follows:

$$L_i\left(\tau \right)={\displaystyle \prod }_{j=1,j\ne i}^N\frac{\tau -{\tau }_j}{{\tau }_i-{\tau }_j}$$

(10)

Differentiating Equation (7), $\dot{x}\left({\tau }_k\right)$ can be obtained as follows:

$$\dot{x}\left({\tau }_k\right)\approx \dot{X}\left(\tau \right)={\displaystyle \sum }_{i=0}^N\dot{L_i}\left({\tau }_k\right)X\left({\tau }_i\right)={\displaystyle \sum }_{i=0}^N{D}_{ki}X\left({\tau }_i\right)$$

(11)

The differential of the Lagrange polynomial at the LG points can be determined by off-line calculation of the differential approximation matrix $D\in R^{N\times \left(N+1\right)}$.

The state variables on each LG point are represented as $X_{1N},X_{2N},X_{3N},X_{4N},X_{5N},X_{6N}\in R^N$, where X_1N, X_2N, X_3N represent the position (x, y) and heading angle ψ of AUV, respectively, and X_4N, X_5N, X_6N represent velocity (u, v) and angular velocity r, respectively. The control variables are represented as U_1N, U_2N and U_3N. The dynamic constraints of the optimal control problem are transformed into algebraic constraints through the differential approximation matrix, and the integral form of the state equation is obtained as follows:

$$\left\{\begin{array}{l}D{X}_{1N}=\frac{tf-t0}{2}\left(X_{4N}\cos X_{3N}-X_{5N}\sin X_{3N}\right)\\ D{X}_{2N}=\frac{t_{\mathrm{f}}-t_0}{2}\left(X_{4N}\sin X_{3N}+X_{5N}\cos X_{3N}\right)\\ D{X}_{3N}=\frac{t_{\mathrm{f}}-t_0}{2}{X}_{6N}\\ D{X}_{4N}=\frac{t_{\mathrm{f}}-t_0}{2}\left(m_{\mathrm{v}}{X}_{5N}{X}_{6N}-d_{\mathrm{u}}{X}_{4N}+U_{1N}\right)/m_{\mathrm{u}}\\ D{X}_{5N}=\frac{t_{\mathrm{f}}-t_0}{2}\left(-m_{\mathrm{u}}{X}_{4N}{X}_{6N}-d_{\mathrm{v}}{X}_{5N}+U_{2N}\right)/m_{\mathrm{u}}\\ D{X}_{6N}=\frac{t_{\mathrm{f}}-t_0}{2}\left(m_{\mathrm{uv}}{X}_{4N}{X}_{5N}-d_{\mathrm{r}}{X}_{6N}+U_{3N}\right)/m_{\mathrm{r}}\end{array}\right.$$

(12)

The terminal state $X_{\mathrm{f}}$ is approximated by the Gauss integral in discretization.

$$X_{\mathrm{f}}=X_0+\frac{t_{\mathrm{f}}-t_0}{2}{\displaystyle \sum }_{k=1}^N{\omega }_{k}f\left(X\left({\tau }_k\right),U\left({\tau }_k\right),{\tau }_k,t_0,t_{\mathrm{f}}\right)$$

(13)

where ${\tau }_k\left(1,2,\cdots ,N\right)$ is the LG point and ${\omega }_k={{\displaystyle \int }}_{-1}^1{L}_i\left(\tau \right)\mathrm{d}\tau$ is the Gauss weight.

The obstacle constraint is expressed as follows:

$${\left(\frac{X_{1N_i}-x_i}{a}\right)}^2+{\left(\frac{X_{2N_i}-y_i}{b}\right)}^2-1\ge 0$$

(14)

The performance index function obtained by the Gauss integral approximation is expressed as follows:

$$J={\omega }_1{t}_{\mathrm{f}}+\frac{t_{\mathrm{f}}-t_0}{2}\cdot {\omega }_2\cdot {\displaystyle \sum }_{k=1}^N\left[{\omega }_k\cdot \left(T_{\mathrm{u}}{}^2+T_{\mathrm{v}}{}^2+T_{\mathrm{r}}{}^2\right)\right]$$

(15)

So far, the GPM has transformed the optimal control problem into an algebraically constrained nonlinear programming problem through discretization.

3.2. Rapid Solution Method of Trajectory

Based on the above numerical approximation method, the continuous optimal control trajectory planning problem is discretized and transformed into a nonlinear programming problem, whose solution is an approximate solution of the continuous Bolza problem. The nonlinear programming problem can be solved by SQP, which essentially transforms the nonlinear optimization problem into a series of quadratic programming sub-problems.

According to Equations (12)–(15), the transformed NLP constraint problem is described as follows:

$$\begin{array}{l}\min J={\omega }_1{t}_{\mathrm{f}}+\frac{t_{\mathrm{f}}-t_0}{2}\cdot {\omega }_2\cdot {\displaystyle {\displaystyle \sum }_{k=1}^N}\left[{\omega }_k\cdot \left(T_{\mathrm{u}}{}^2+T_{\mathrm{v}}{}^2+T_{\mathrm{r}}{}^2\right)\right]\\ \mathrm{s}.\mathrm{t}. \left\{\begin{array}{l}D{X}_{1N}-\frac{tf-t0}{2}\left(X_{4N}\cos X_{3N}-X_{5N}\sin X_{3N}\right)=0\\ D{X}_{2N}-\frac{t_{\mathrm{f}}-t_0}{2}\left(X_{4N}\sin X_{3N}+X_{5N}\cos X_{3N}\right)=0\\ D{X}_{3N}-\frac{t_{\mathrm{f}}-t_0}{2}{X}_{6N}=0\\ D{X}_{4N}-\frac{t_{\mathrm{f}}-t_0}{2}\left(m_{\mathrm{v}}{X}_{5N}{X}_{6N}-d_{\mathrm{u}}{X}_{4N}+U_{1N}\right)/m_{\mathrm{u}}=0\\ D{X}_{5N}-\frac{t_{\mathrm{f}}-t_0}{2}\left(-m_{\mathrm{u}}{X}_{4N}{X}_{6N}-d_{\mathrm{v}}{X}_{5N}+U_{2N}\right)/m_{\mathrm{u}}=0\\ D{X}_{6N}-\frac{t_{\mathrm{f}}-t_0}{2}\left(m_{\mathrm{uv}}{X}_{4N}{X}_{5N}-d_{\mathrm{r}}{X}_{6N}+U_{3N}\right)/m_{\mathrm{r}}=0\\ X_{\mathrm{f}}-X_0+\frac{t_{\mathrm{f}}-t_0}{2}{\displaystyle {\displaystyle \sum }_{k=1}^N}{\omega }_{k}f\left(X\left({\tau }_k\right),U\left({\tau }_k\right),{\tau }_k,t_0,t_{\mathrm{f}}\right)=0\\ {\left(\frac{X_{1N_i}-x_i}{a}\right)}^2+{\left(\frac{X_{2N_i}-y_i}{b}\right)}^2-1\ge 0\end{array}\right.\end{array}$$

(16)

Taylor expansion is used to simplify the nonlinear constraint problem into a linear function, and the following quadratic programming problem is obtained as follows:

$$\begin{array}{l}\min \frac{1}{2}{d}_k^{\mathrm{T}}{B}_k{d}_k+{\left(\nabla J\right)}^{\mathrm{T}}{d}_k\\ \mathrm{s}.\mathrm{t}.\left\{\begin{array}{l}{\left(\nabla C_{\mathrm{e}}\left(x\right)\right)}^{\mathrm{T}}{d}_k+C_{\mathrm{e}}\left(x\right)=0\\ {\left(\nabla C_{\mathrm{i}}\left(x\right)\right)}^{\mathrm{T}}{d}_k+C_{\mathrm{i}}\left(x\right)=0\end{array}\right.\end{array}$$

(17)

where $C_{\mathrm{e}},C_{\mathrm{i}}$ represent the equality constraints and inequality constraints in Equation (16), respectively.

The search direction is obtained by solving Equation (17), and the iterative equation is calculated step by step to obtain the optimal solution to the original problem.

When solving based on the GPM, the selection of the initial value is very important for the real-time or fast calculation of optimization problems. A prerequisite for real-time or fast optimization is an efficient initial value generation algorithm. Random initial values or initial values without any processing will increase the computational cost. If the calculation is performed with poor quality initial values, then the optimization problem may not converge to a feasible solution.

For discrete points between the start and target points, a simple way to obtain values is linear fitting. A straight line connects the discrete points, and the initial value is obtained by taking values with equal intervals in the interval. Because the linear fitting function is not smooth enough, the solution calculation speed is slow when the initial value is substituted into the trajectory planning problem, and the resulting navigation trajectory is more complicated. Therefore, the cubic spline interpolation method is considered to calculate the initial value of variables. The variable value [X₀, X_f] from the initial state to the terminal state is divided into n intervals $[(x_0,x_1),(x_1,x_2),\dots ,(x_{n-1},x_n)]$, with a total of n + 1 points, where $x_0=X_0,x_n=X_{\mathrm{f}}$. In the interval, the constructed cubic spline function is defined as follows:

$$S_i\left(x\right)=a_i+b_{i}x+c_i{x}^2+d_i{x}^{3}i=0,\dots ,n$$

(18)

It can be seen from Equation (18) that each interval has four unknowns, and the cubic spline equation of each interval satisfies the following conditions:

$$\left\{\begin{array}{l}S(x_i)=y_i\\ S_i(x_{i+1})=y_{i+1}\\ S_{i+1}(x_{i+1})=y_{i+1}\\ S_i^{\prime }(x_{i+1})=S_{i+1}^{\prime }(x_{i+1})\\ S_i^{″}(x_{i+1})=S_{i+1}^{″}(x_{i+1})\\ S^{″}(x_0)=S^{″}(x_{\mathrm{n}})=0\end{array}\right.$$

(19)

The spline coefficients $a_i,b_i,c_i,d_i$ are solved by Equation (19), and the cubic spline function in each interval is obtained after substituting $a_i,b_i,c_i,d_i$ into $S_i(x)$. The values at the LG points are taken as the initial values for solving the trajectory planning problem. By introducing the cubic spline interpolation method, the fitted curve trajectory is smoother and more realistic, and the initial value of the interpolation calculation can improve the solution speed of the optimization problem.

4. Simulation Analysis

In order to verify the effectiveness of the above trajectory planning algorithm, this paper conducts an optimization simulation for the joint objective of low energy consumption and short sailing time. The starting state of the autonomous underwater vehicle (AUV) is set to $X_0=\left(0,0,\pi /3,0,0,0\right)$ and the target state is set to $X_{\mathrm{f}}=\left(25,25,\pi /6,0,0,0\right)$. The constraints of state variables and control variables during sailing are expressed as follows:

$$\left\{\begin{array}{l}0 \mathrm{m}\le x\left(t\right)\le 30 \mathrm{m}\\ 0 \mathrm{m}\le y\left(t\right)\le 30 \mathrm{m}\\ -\pi \le \psi (t)\le \pi \\ -1 \mathrm{m}/\mathrm{s}\le u\left(t\right)\le 1 \mathrm{m}/\mathrm{s}\\ -1 \mathrm{m}/\mathrm{s}\le v\left(t\right)\le 1 \mathrm{m}/\mathrm{s}\\ -\pi /6 \mathrm{rad}/\mathrm{s}\le r\left(t\right)\le \pi /6 \mathrm{rad}/\mathrm{s}\end{array}\right.$$

(20)

$$\left\{\begin{array}{l}-53 \mathrm{N}\cdot \mathrm{m}\le {\tau }_{\mathrm{u}}\le 74 \mathrm{N}\cdot \mathrm{m}\\ -53 \mathrm{N}\cdot \mathrm{m}\le {\tau }_{\mathrm{v}}\le 74 \mathrm{N}\cdot \mathrm{m}\\ -53 \mathrm{N}\cdot \mathrm{m}\le {\tau }_{\mathrm{r}}\le 74 \mathrm{N}\cdot \mathrm{m}\end{array}\right.$$

(21)

The relevant parameters in the kinetic constraints are shown in

Table 1. Parameters related to dynamic constraints.

Name	Value	Name	Value
$X\dot{\mathrm{u}}$	−30 kg	$N\dot{\mathrm{r}}$	−30 kg∙m2
$X\mathrm{u}$	−70 kg/s	$N\mathrm{r}$	−50 kg∙m/s
$X\left|\mathrm{u}\right|\cdot \mathrm{u}$	−100 kg/m	$N\left|\mathrm{r}\right|\cdot \mathrm{r}$	−100 kg/m
$Y\dot{\mathrm{v}}$	−80 kg	$I\mathrm{z}$	50 kg∙m2
$Y\mathrm{v}$	−100 kg/s	m	185 kg
$Y\left|\mathrm{v}\right|\cdot \mathrm{v}$	−200 kg/m

.

The obstacle parameters in the navigation environment are shown in

Table 2. Obstacle parameters.

Obstacle Area	Center Coordinates	Radius
1	(10 m, 10 m)	2
2	(12 m, 15 m)	1
3	(17 m, 14 m)	1
4	(20 m, 20 m)	2

, and there are four obstacles in total.

In the above simulation environment, 50 LG points are selected to discretize the model and then an SQP algorithm is used to solve the problem.

Scenario 1: The shortest sailing time is set as the objective function. The cubic spline interpolation method is used to calculate the initial value for the solution. After the simulation calculation, the navigation trajectory of the AUV and the change trend of the control variables and state variables are obtained as shown in Figure 1. The whole simulation calculation takes 16.581 seconds.

In Figure 1a, the planned trajectory can complete the navigation from the starting point to the destination and avoid collision with obstacles. Due to the requirement of the objective function of the shortest sailing time, the AUV’s navigation path tends to be smooth. The sailing time is 73.002 seconds. In Figure 1b, under the condition of meeting the requirements of the AUV’s own thrust and torque, the thrust T_u in the control variable is always in the upper limit during the whole navigation process. In Figure 1c, within the limit of speed, the power generated by thrust and torque forces the AUV to sail at the upper limit of speed. Thus, the AUV can reach the destination with the shortest sailing time.

Scenario 2: Similarly to scenario 1, the shortest sailing time is still set as the objective function. However, the difference is that the initial value is generated by linear fitting that takes values at equal intervals between the start point and the end point. After substituting the initial value for calculation, the convergence solution of the optimal control problem can be obtained. The obtained trajectory is shown in Figure 2. The whole simulation calculation takes 24.763 seconds.

Figure 2a shows that the AUV can achieve obstacle avoidance navigation. The whole navigation path is generally smooth, but after bypassing the last obstacle, the AUV’s course deviates. The sailing time is 70.713 seconds. In Figure 2b, the change trend of T_u in the control variables is generally smooth. Within the limits of thrust, the AUV is driven to sail with the upper limit of thrust. The values of control variables T_v and T_r have obvious fluctuations, which affected the speed change of the AUV. During the whole voyage, the speed fluctuates frequently, as shown in Figure 2c.

By comparing the simulation results of scenario 1 and scenario 2, it can be concluded that the initial values generated by the cubic spline interpolation method and the linear fitting method, respectively, can be used to find the convergent solution. The trajectory planned from the starting point to the target point can avoid collision with obstacles, and the control variables can be optimized within the range of variable constraints. However, the average values of control variables and the average values of the speed of the two methods are significantly different, as shown in Figure 3.

In Figure 3a, the thrust T_u in the linear fitting method is 21.769 N higher than that in the cubic spline interpolation method. In Figure 3b, the change of thrust makes the speed V_y in the linear fitting method 0.147 m/s higher than that in the cubic spline interpolation method. In the case of similar sailing time, the thrust and speed calculated by the cubic spline interpolation method can reduce energy consumption and optimize the control variable value.

Under the same simulation environment, the calculation time of the initial value obtained by the cubic spline interpolation method is shorter than that of the initial value obtained by taking the values at equal intervals. The solution time of the optimal control problem is reduced from 24.763 s to 16.581 s, and the calculation speed is increased by 49.35%. The calculation results meet certain accuracy requirements, which can verify the validity and applicability of the GPM in trajectory planning applications, with the initial value obtained by the cubic spline interpolation method.

Scenario 3: Short time taken and low energy consumption are considered as the joint optimization objectives, and the cubic spline interpolation method is used to generate the initial value. The motion trajectory obtained after solving is shown in Figure 4. The whole simulation calculation takes 16.427 s.

The navigation trajectory in Figure 4a shows that the solved motion trajectory is smoother with the joint objectives of short time taken and low energy consumption than that in scenario 1 and scenario 2. The whole sailing distance is shorter. After bypassing the obstacles, the AUV sailed almost in a straight line. In Figure 4b, the control variable T_u is stable at about 15 N. The sailing time is 199.305 s. It can be seen that the reduction in energy consumption is at the expense of increased sailing time.

By comparing scenario 1 and scenario 3, it can be seen that the calculation results of the optimal control quantity are different with different set objective functions, as shown in Figure 5. When the AUV is required to reduce energy consumption, the average value of the control variable T_u is only 14.291 N from the starting point to the destination, and the total distance is the shortest in these three cases. The calculation times of scenario 1 and scenario 3 are similar, which also proves that the initial value generated by the cubic spline interpolation method can improve the speed of solving the optimal control problem.

From the above simulation results, it can be concluded that these two methods of generating initial values can quickly calculate the motion trajectory and the values of the optimal control variables. Under different objective function scenarios, the obtained trajectories can meet the requirements of obstacle avoidance. In the same simulation environment, the solution speed of the cubic spline interpolation method is faster than that of the linear fitting method. When the objective function is changed and the cubic spline interpolation method is used simultaneously, the thrust distribution results are significantly different. When only focusing on the goal of sailing time, the propeller drives the AUV to sail with the upper limit of thrust to ensure that it reaches the destination in the shortest time.

5. Conclusions

In this paper, the problem of autonomous underwater vehicles (AUVs) motion trajectory planning based on the GPM is studied, and low energy consumption and short sailing time are considered as joint optimization objectives. At the same time, dynamic constraints are added to the model to make the planned trajectory more reasonable and more trackable. The optimal solution of the control variables can be obtained after solving the trajectory optimization problem. The planned trajectory of the AUV can meet the requirements of obstacle avoidance. The model can be solved according to different objective functions, and the value of control variables can be adjusted to meet the actual requirements of underwater navigation.

In the process of parameter optimization, the selection of the initial value affects the speed of solving the optimal control problem. The linear fitting method and the cubic spline interpolation method are proposed to calculate the initial value. The simulation results show that the calculation time of the cubic spline interpolation method is less than that of the linear fitting method under the same simulation environment. Compared with the linear fitting method, the cubic spline interpolation method reduces the global calculation amount of the trajectory optimization problem and improves the computational performance by 49.35%. Moreover, the trajectory calculated by the cubic spline interpolation method is smoother and the control variable value is more stable.

As a rapid solution for the optimal control problem, the simulation results verify the effectiveness of the initial value generated by the cubic spline interpolation method, which provides a useful reference for the real-time planning of a trajectory. In future work, in accordance with the rapidity of the solution of this model, we will embed this model into the AUV control system to verify the effectiveness of real-time planning and realize online planning.