# Trajectory Planning of Autonomous Underwater Vehicles Based on Gauss Pseudospectral Method

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

- (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.

## 2. Problem Description

#### 2.1. Motion Model of Underwater Robot

_{(·),}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

_{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.

#### 2.3. Energy Consumption and Sailing Time Optimization Model

_{1}and ω

_{2}are introduced, and the weighted coefficient method is used to construct the joint optimization objective function as follows:

_{1}+ ω

_{2}= 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.

_{1}and ω

_{2}of the energy consumption and navigation time should be adjusted according to the actual situation. When ω

_{1}= 1 or ω

_{2}= 1, the trajectory planning achieves the single objective of the shortest sailing time or the lowest energy consumption, respectively.

## 3. Motion Trajectory Planning

#### 3.1. Transformation of Optimal Control Problem

_{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:

#### 3.2. Rapid Solution Method of Trajectory

_{0}, 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:

## 4. Simulation Analysis

_{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.

_{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.

_{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.

_{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.

_{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.

## 5. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## Abbreviations

AUV | Autonomous Underwater Vehicle |

GPM | Gauss Pseudospectral Method |

SQP | Sequential Quadratic Programming |

LPM | Legendre Pseudospectral Method |

RPM | Radau Pseudospectral Method |

GPOPS | Gauss Pseudospectral Optimization Software |

SNOPT | Sparse Nonlinear Optimizer |

LG points | Legendre-Gauss points |

NLP | Nonlinear Programming |

## Nomenclatures

x | Coordinate position in x direction |

y | Coordinate position in y direction |

ψ | Direction angle of the AUV |

u | Velocity in x direction |

v | Velocity in y direction |

r | Angular velocity of the AUV |

m | Mass of the AUV |

I_{z} | Moment of inertia |

X_{(·)} | Hydrodynamic coefficient |

Y_{(·)} | Hydrodynamic coefficient |

N_{(·)} | Hydrodynamic coefficient |

T_{u} | Thrust generated by the propeller |

T_{v} | Thrust generated by the propeller |

T_{r} | Thrust torque generated by the propeller |

t_{f} | Terminal time of navigation |

h_{i} | Obstacle function |

a | Radius of the obstacle |

b | Radius of the obstacle |

J | Objective function |

ω_{1} | Weight coefficient |

ω_{2} | Weight coefficient |

τ | Time variable |

L_{i}(τ) | Interpolation basis function |

D | Differential approximation matrix |

x(τ) | State variable function |

u(τ) | Control variable function |

X_{f} | Terminal state |

τ_{k} | LG points |

ω_{k} | Gauss weight |

C_{e} | Equality constraints |

C_{i} | Inequality constraints |

a_{i} | Spline coefficient |

b_{i} | Spline coefficient |

c_{i} | Spline coefficient |

d_{i} | Spline coefficient |

S_{i}(x) | Cubic spline function |

## References

- Xiao, Q. Research status and trend analysis of underwater robots. Ind. Innov.
**2021**, 20, 25–27. [Google Scholar] - Tang, Z.; Shao, H.; Zhang, B. Overview of inspection technology for submarine optical cable. Digit. Ocean Underw. Warf.
**2020**, 3, 499–503. [Google Scholar] [CrossRef] - Zhu, D.; Pang, W.; Ren, K. Research status and prospect of multi-autonomous underwater vehicle cooperative search control. J. Univ. Shanghai Sci. Technol.
**2022**, 44, 417–428. [Google Scholar] [CrossRef] - Chu, Z.; Wang, F.; Lei, T.; Luo, C. Path planning based on deep reinforcement learning for autonomous underwater vehicles under ocean current disturbance. IEEE Trans. Intell. Veh.
**2023**, 8, 108–120. [Google Scholar] [CrossRef] - Kim, Y.; Kim, B. Time-optimal trajectory planning based on dynamics for differential- wheeled mobile robots with a geometric corridor. IEEE Trans. Ind. Electron.
**2017**, 64, 5502–5512. [Google Scholar] [CrossRef] - Qu, W.; Zhang, H.; Wu, L.; You, Y.; Dong, Y. The trajectory design on the captive flight test of booster rocket and the combined-cycled aircraft based on segmented gaussian pseudo-spectral method. Aerosp. Control
**2021**, 39, 28–35. [Google Scholar] - Wang, G.; Li, W.; Jiang, C.; Zhu, D.; Li, Z.; Xu, W.; Zhao, H.; Ding, H. Trajectory planning and optimization for robotic machining based on measured point cloud. IEEE Trans. Robot.
**2022**, 38, 1621–1637. [Google Scholar] [CrossRef] - Li, Z.; Chen, J.; Peng, B. UAV Cluster Path Planning Based on Pseudo-spectral Method. Air Space Def.
**2021**, 4, 52–59. [Google Scholar] - Tang, X.; Yang, L.; Yuan, J. Study on the states of autonomous vehicle based on gaussian pseudo-spectral method. Automot. Eng.
**2020**, 42, 567–573. [Google Scholar] - Yao, H.; Qi, R. A research progress of trajectory optimization and guidance for mars lander. Aerosp. Control
**2021**, 39, 3–12. [Google Scholar] - Mahmoudi, M.; Ghovatmand, M.; Skandari, M.H.N. A new convergent pseudospectral method for delay differential equations. Iran J. Sci. Technol. Trans. Sci.
**2020**, 44, 203–211. [Google Scholar] [CrossRef] - Ross, M.; Karpenko, M. A review of pseudospectral optimal control: From theory to flight. Annu. Rev. Control
**2012**, 36, 182–197. [Google Scholar] [CrossRef] - Xu, W.; Jiang, J.; Jiang, S. Scale optimization of wings in the climbing section of a near-space morphing hypersonic aircraft. J. Harbin Eng. Univ.
**2019**, 40, 1–9. [Google Scholar] - Zhang, T.; Fu, T.; Song, X.; Qu, F. Multi-objective excavation trajectory optimization for unmanned electric shovels based on pseudospectral method. Autom. Constr.
**2022**, 136, 104176.1–104176.16. [Google Scholar] [CrossRef] - Xia, W.; Wang, W.; Gao, C. Trajectory optimization with obstacles avoidance via strong duality equivalent and hp-pseudospectral sequential convex programming. Optim. Control Appl. Methods
**2022**, 43, 566–587. [Google Scholar] [CrossRef] - Zhong, X.; Shao, X.; Xu, L.; Sun, G. Free-floating space manipulator trajectory optimization based on adaptive radau pseudospectral method. J. Harbin Inst. Technol.
**2018**, 50, 49. [Google Scholar] - Song, X.; Yao, X.; Ye, S. Trajectory optimization of small supersonic unmanned aerial vehicle based on pseudo-spectral method. J. Zhejiang Univ. (Eng. Sci.)
**2022**, 56, 193–201. [Google Scholar] - Sun, Z.; Liu, Z.; Zhang, P. Mars entry trajectory quick optimization method for lifting vehicle based on adaptive GPM. Chin. J. Space Sci.
**2020**, 40, 547–553. [Google Scholar] - Sandberg, A.; Sands, T. Autonomous trajectory generation algorithms for spacecraft slew maneuvers. Aerospace
**2022**, 9, 135. [Google Scholar] [CrossRef] - Raigoza, K.; Sands, T. Autonomous trajectory generation comparison for de-orbiting with multiple collision avoidance. Sensors
**2022**, 22, 7066. [Google Scholar] [CrossRef] - Garg, D.; Hager, W.; Rao, A. Pseudospectral methods for solving infinite-horizon optimal control problems. Automatica
**2011**, 47, 829–837. [Google Scholar] [CrossRef] - Zhang, L.; Wang, Z.; Yang, X.; Song, Q. Ascent Trajectory Planning for Stratospheric Airship Based on Gauss Pseudospectral Method. J. Shanghai Jiaotong Univ.
**2013**, 47, 1205–1209+1216. [Google Scholar] - Ma, L.; Cui, W. Path following control of autonomous underwater vehicle based upon fuzzy hybrid control. Control Theory Appl.
**2006**, 3, 341–346. [Google Scholar] - Gatzke, B. Trajectory Optimization for Helicopter Unmanned Aerial Vehicles; Naval Postgraduate School: Monterey, CA, USA, 2010; Available online: http://hdl.handle.net/10945/5315 (accessed on 16 October 2021).
- Meng, S.; Xiang, J.; Luo, Z.; Ren, Y. Optimal trajectory planning for small-scale unmanned helicopter obstacle avoidance. J. Beijing Univ. Aeronaut. Astronaut.
**2014**, 40, 246–251. [Google Scholar] [CrossRef] - Zhang, Y.; Zhang, W.; Chen, J.; Shen, L. Air-to-ground weapon delivery trajectory planning for UCAVs using Gauss pseudospectral method. Acta Aeronaut. Et Astronaut. Sin.
**2011**, 32, 1240–1251. [Google Scholar] - Huang, J.; Liu, Z.; Liu, Z.; Wang, Q. Pseudo-spectral method for optimal control problem: Theory and application. Electron. Opt. Control
**2020**, 27, 63–70. [Google Scholar] [CrossRef]

**Figure 1.**Result of each state in scenario 1: (

**a**) Trajectory planning of the AUV; (

**b**) Variation diagram of thrust; (

**c**) Diagram of velocity change in x and y directions.

**Figure 2.**Result of each state in scenario 2: (

**a**) Trajectory planning of the AUV; (

**b**) Variation diagram of thrust; (

**c**) Diagram of velocity change in x and y directions.

**Figure 3.**Average value of control variables and state variables in scenario 1 and scenario 2: (

**a**) Average value of thrust and torque; (

**b**) Average value of speed in x and y directions.

**Figure 4.**Result of each state in scenario 3: (

**a**) Trajectory planning of the AUV; (

**b**) Variation diagram of thrust; (

**c**) Diagram of velocity change in x and y directions.

Name | Value | Name | Value |
---|---|---|---|

$X\dot{\mathrm{u}}$ | −30 kg | $N\dot{\mathrm{r}}$ | −30 kg∙m^{2} |

$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∙m^{2} |

$Y\mathrm{v}$ | −100 kg/s | m | 185 kg |

$Y\left|\mathrm{v}\right|\cdot \mathrm{v}$ | −200 kg/m |

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 |

Disclaimer/Publisher’s Note: The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content. |

© 2023 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Gan, W.; Su, L.; Chu, Z.
Trajectory Planning of Autonomous Underwater Vehicles Based on Gauss Pseudospectral Method. *Sensors* **2023**, *23*, 2350.
https://doi.org/10.3390/s23042350

**AMA Style**

Gan W, Su L, Chu Z.
Trajectory Planning of Autonomous Underwater Vehicles Based on Gauss Pseudospectral Method. *Sensors*. 2023; 23(4):2350.
https://doi.org/10.3390/s23042350

**Chicago/Turabian Style**

Gan, Wenyang, Lixia Su, and Zhenzhong Chu.
2023. "Trajectory Planning of Autonomous Underwater Vehicles Based on Gauss Pseudospectral Method" *Sensors* 23, no. 4: 2350.
https://doi.org/10.3390/s23042350