Path Following Based on Waypoints and Real-Time Obstacle Avoidance Control of an Autonomous Underwater Vehicle

Abstract

This paper studies three-dimensional (3D) straight line path following and obstacle avoidance control for an underactuated autonomous underwater vehicle (AUV) without lateral and vertical driving forces. Firstly, the expected angular velocities are designed by using two different methods in the kinematic controller. The first one is a traditional method based on Line-of-sight (LOS) guidance law, and the second one is an improved method based on model predictive control (MPC). At the same time, a penalty item is designed by using the obstacle information detected by onboard sensors, which can realize the real-time obstacle avoidance of the unknown obstacle. Then, in order to overcome the uncertainty of the dynamics model and the saturation of actual control input, the dynamic controller is designed by using sliding mode control (SMC) technology. Finally, in the simulation experiment, the performance of the improved control method is verified by comparison with two traditional control methods based on LOS guidance law. Since the constraint of an AUV’s angular velocities are considered in MPC, simulation results show that the improved control method uses MPC, and SMC not only improves the tracking quality of the AUV when switching paths near the waypoints and realizes real-time obstacle avoidance but also effectively reduces the mean square error (MSE) and saturation rate of the rudder angle. Therefore, this control method is more conducive to the system stability and saves energy.

1. Introduction

At present, autonomous underwater vehicles (AUVs) have gradually become an important tool in many fields such as ocean exploitation and scientific research. Because an AUV always follows the planned path during its mission, its path-following control is critical technology. When an AUV is operated in the open sea, its path is generally planned through a set of waypoints, with a straight line connecting every two adjacent waypoints. Its advantages are simple path planning and low computation. However, the controller design is not an easy task because the AUVs’ motions and model are coupled, nonlinear, and uncertain. For these reasons, the path-following control of the AUVs has been studied extensively worldwide [1].

Underactuated marine surface vessels and AUVs must calculate their ideal attitude in real time to realize path-following control. This task is accomplished by the kinematic controller according to the guidance law. In [2,3,4], the LOS guidance law was applied for path following. In order to improve the dynamic characteristics of tracking errors, the LOS guidance law with time-varying lookahead distance was presented in [5,6]. Alternatively, the vector field (VF) methodis also a popular guidance law [7,8]. Similar to the guidance problem, a lot of work has been done on the design of a dynamic controller in path following. The prevailing control methods for marine surface vessels and AUVs in path following include feedback linearizing control techniques [9,10,11], proportional-integral-derivative (PID) control [12,13], the backstepping method [14,15], Lyapunov direct method [16], robust control [17,18,19,20], adaptive control [21,22,23], gain scheduling control theory [24], sliding mode control (SMC) [25,26,27,28,29], neural network control [30], and fuzzy logic control [31], etc.

Generally, there are inherent physical constraints on the input of the control system, which will not only degrade the control performance but will also affect the stability of the system. None of the abovementioned papers has considered this problem. Model predictive control (MPC) is known as an optimization-based control method. MPC has a clear advantage to handle constraints, which represents the trend of the control of constrained systems. In [32], the constraint of the input was considered, and the LOS guidance law was optimized by using MPC algorithm to improve the waypoint tracking quality of an underactuated ship. In [33], the velocity and amplitude constraints of the rudder were considered, and the AUV’s attitude and depth control were optimized by using MPC algorithm. The limitation of the aforementioned papers is that the controllers are based on a nominal model. The principle of MPC is that it predicts the future state and output by using the current state and prediction model. Therefore, the accuracy of the prediction model has a great influence on the performance of MPC. Since the hydrodynamic parameters of the AUVs are uncertain and the motions of each degree of freedom are coupled and nonlinear, it is difficult to obtain the exact dynamic model of the AUVs. Therefore, using a nominal model to design MPC is difficult to guarantee the robustness of the system.

Straight-line path following based on waypoints is convenient for application, but the path is not smooth at each waypoint. The rudder angle is easily saturated when the path is switched near the waypoints during the AUV tracking of the path. Frequent saturation of the rudder angle will not only affect the stability of the system but will also increase the resistance and energy consumption of the AUV. Reference [34] applies the backstepping method to design the ship’s path-tracking controller and considers the system stability when the rudder angle is saturated. However, the saturation of the rudder angle does not fundamentally be alleviated in [34]. In addition, obstacle avoidance is often needed in path following. An obstacle avoidance guidance law is designed in [35]. However, this method needs to obtain the global information of obstacles in real-time, and it has high requirements for sensor configuration. In [36,37], based on the obstacle information detected by the onboard sensors, a spline curve is applied to replan the original path near the obstacle, and real-time obstacle avoidance is realized. However, replanning the path in real time greatly increases the computational burden of the controller.

In order to reduce the computation and simplify the structure of the controller, this paper designs the kinematics and dynamics controllers by using cascade control strategy, thereby providing a solution for the 3D straight line path following and obstacle avoidance of the AUVs. In the design of the kinematics controller, an improved control method is proposed by using MPC. In addition, in order to realize real-time obstacle avoidance, the penalty item for obstacle avoidance is designed according to the obstacle information detected by the onboard sensors. The optimal expected angular velocity satisfying the constraint can be obtained by using the improved control method, which can not only reduce the MSE and saturation rate of the rudder angle but also realize real-time obstacle avoidance. In the design of the dynamics controller, the saturation of control input is considered. The actual control signal is designed by SMC to control the expected speed of the AUV, which can not only overcome the uncertainty of the dynamics model but also ensure the stability of the system.

The remaining four sections of this paper are about the analysis of the control problem, the design of the kinematics and dynamics controller, the results and analysis of the simulation experiment, and the conclusion.

2. Analysis of the Control Problem

The underactuated AUV studied in this paper is not equipped with lateral and vertical driving forces but is equipped at the tail with a propeller to control the longitudinal speed, a pair of vertical rudders to control the yaw, and a pair of horizontal rudders to control the pitch. This is a very typical configuration of an AUV, which has the advantages of simplifying its mechanical structure, reducing cost, and improving reliability.

2.1. Kinematics and Dynamics Modeling

As shown in Figure 1, in order to establish the six degrees of freedom (DOF) motion model of the AUV, the fixed coordinate system {I}:E−ξηζ and moving coordinate system {B}:O−xyz are introduced. AUV’s gravity and buoyancy are equal. The center of gravity (CG) is directly below the center of buoyancy (CB), which will generate rolling and pitching restoring moments to ensure the stability of the AUV. The distance between the CG and the CB (metacentric height) is $z_g$. The origin of {I} coordinate system is defined as a fixed point with the ξ-axis pointing north, the η-axis pointing east, and the ζ-axis pointing down. The origin of the {B} coordinate system is defined at the CB, with the x-axis pointing in front of the AUV, the y-axis pointing to the right, and the z-axis pointing down. Since the roll of the AUV is very small and does not need to be controlled, the kinematics and dynamics model of the AUV can be simplified to the following five DOFs after the roll is ignored:

$$\dot{\mathit{\eta }}=\mathit{J}\left(\mathit{\eta }\right)\mathit{v},$$

(1)

$$\mathit{M}\dot{\mathit{v}}+\mathit{C}\left(\mathit{v}\right)\mathit{v}+\mathit{D}\left(\mathit{v}\right)\mathit{v}+\mathit{g}\left(\mathit{\eta }\right)=\mathit{\tau }+\mathit{b},$$

(2)

where $\mathit{\eta }={\left[\begin{array}{ccccc}\xi & \eta & \zeta & \theta & \psi \end{array}\right]}^T$. $\left(\xi ,\eta ,\zeta \right)$ represents the coordinates of CB in {I}, that is, the position of the AUV. $\theta$ and $\psi$ represent the orientation of the AUV in {I}, that is, the pitching angle and yaw angle, respectively. $\mathit{J}\left(\mathit{\eta }\right)$ represents the coordinate transformation from {B} to {I},

$$\mathit{J}\left(\mathit{\eta }\right)=\left[\begin{array}{cc}{\mathit{J}}_1\left(\mathit{\eta }\right)& 0_{3\times 2}\\ 0_{2\times 3}& {\mathit{J}}_2\left(\mathit{\eta }\right)\end{array}\right],{\mathit{J}}_1\left(\mathit{\eta }\right)=\left[\begin{array}{ccc}\cos \psi \cos \theta & -\sin \psi & \cos \psi \sin \theta \\ \sin \psi \cos \theta & \cos \psi & \sin \psi \sin \theta \\ -\sin \theta & 0& \cos \theta \end{array}\right],{\mathit{J}}_2\left(\mathit{\eta }\right)=\left[\begin{array}{cc}1& 0\\ 0& \frac{1}{cos\theta }\end{array}\right].$$

(3)

$\mathit{v}={\left[\begin{array}{ccccc}u& v& w& q& r\end{array}\right]}^T$ denotes the velocities of the AUV defined in {B}. $u,v,w$ are displacement velocities, that is, the respective surge, sway and heave velocities. $q$ and $r$ are angular velocities, that is, the pitch and yaw rates. The system matrices $\mathit{M},\mathit{C}\left(\mathit{v}\right),\mathit{D}\left(\mathit{v}\right)$ satisfy the properties $\mathit{M}={\mathit{M}}^T$, $\mathit{C}\left(\mathit{v}\right)=-\mathit{C}{\left(\mathit{v}\right)}^T,\mathit{D}\left(\mathit{v}\right)>0$. The restoring moments are defined as $\mathit{g}\left(\mathit{\eta }\right)={\left[\begin{array}{ccccc}0& 0& 0& M_{HS}& 0\end{array}\right]}^T$, where $M_{HS}=-z_{g}G\sin \theta$ is the pitch restoring moment, and $G$ is the gravity of the AUV. $\mathit{\tau }={\left[\begin{array}{ccccc}{X}_{prop}& 0& 0& M_{fin}& N_{fin}\end{array}\right]}^T$ is the control vector, where $X_{prop},M_{fin},N_{fin}$ are the thrust and torque produced by the propeller and rudder. Vector $\mathit{b}$ describes the model uncertainties. In order to make it convenient for the dynamic controller design, dynamics model (2) can be simplified as follows:

$$\begin{array}{l}\dot{u}=\frac{1}{m_{11}}\left(m_{22}vr-m_{33}wq+d_{11}u+X_{prop}+b_u\right),\\ \dot{v}=\frac{1}{m_{22}}\left(-m_{11}ur+d_{22}v+b_v\right),\\ \dot{w}=\frac{1}{m_{33}}\left(m_{11}uq+d_{33}w+b_w\right),\\ \dot{q}=\frac{1}{m_{55}}\left[\left(m_{33}-m_{11}\right)uw+d_{55}q+M_{HS}+M_{fin}+b_q\right],\\ \dot{r}=\frac{1}{m_{66}}\left[\left(m_{11}-m_{22}\right)uv+d_{66}r+N_{fin}+b_r\right],\end{array}$$

(4)

where $m_{11}=m-X_{\dot{u}},m_{22}=m-Y_{\dot{v}},m_{33}=m-Z_{\dot{w}},m_{55}=I_y-M_{\dot{q}},m_{66}=I_z-N_{\dot{r}},d_{11}=X_u+X_{\left|u\right|u}\left|u\right|,d_{22}=Y_v+Y_{\left|v\right|v}\left|v\right|,$ $d_{33}=Z_w+Z_{\left|w\right|w}\left|w\right|,d_{55}=M_q+M_{\left|q\right|q}\left|q\right|,d_{66}=N_r+N_{\left|r\right|r}\left|r\right|.$ The symbol $m$ denotes the mass of the AUV. $I_y$ and $I_z$ denote the moment of inertia. $b_i\left(i=u,v,w,q,r\right)$ represents model uncertainties. The remaining parameters are hydrodynamic coefficients.

2.2. Error Model

As shown in Figure 1, the routes are described in terms of waypoints which are fixed points in {I} frame. The reference path is the straight line between two adjacent waypoints. In order to establish the error model of the straight-line path followed, the path coordinate system $\left\{F\right\}:P-x_F{y}_F{z}_F$ is introduced. The current straight path $L_k$ starting at $p_k$ and ending at $p_{k+1}$ is defined. Here, the subscript k represents the sequence number of the straight path or waypoints.$\left({\xi }_k,{\eta }_k,{\zeta }_k\right)$ represent coordinates of $p_k$, and $\left({\xi }_{k+1},{\eta }_{k+1},{\zeta }_{k+1}\right)$ coordinates of $p_{k+1}$. The origin of $\left\{F\right\}$ is placed at $p_k$ with the $x_F$-axis pointing towards $p_{k+1}$, the $y_F$-axis pointing to the right, and the $z_F$-axis pointing down. The $y_F$ and $z_F$ coordinates of the AUV in the $\left\{F\right\}$ frame equal the lateral error and vertical error, respectively. The orientation of {B} in {I} is defined as ${\mathit{A}}_B={\left[\begin{array}{cc}\theta & \psi \end{array}\right]}^T$, and the orientation of {F} in {I} as ${\mathit{A}}_F={\left[\begin{array}{cc}{\theta }_F& {\psi }_F\end{array}\right]}^T$. ${\theta }_F$ and ${\psi }_F$ are calculated as follows:

$$\begin{array}{l}{\theta }_F=-\arctan 2\left(\Delta \zeta ,\sqrt{{\left(\Delta \xi \right)}^2+{\left(\Delta \eta \right)}^2}\right),\\ {\psi }_F=\arctan 2\left(\Delta \eta ,\Delta \xi \right),\end{array}$$

(5)

where $\Delta \xi ={\xi }_{k+1}-{\xi }_k,\Delta \eta ={\eta }_{k+1}-{\eta }_k,\Delta \zeta ={\zeta }_{k+1}-{\zeta }_k$.

The orientation of {B} relative to the {F} frame is defined ${\mathit{A}}_e={\left[\begin{array}{cc}{\theta }_e& {\psi }_e\end{array}\right]}^T$, then

$${\mathit{A}}_e=\left[\begin{array}{c}{\theta }_e\\ {\psi }_e\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& \cos {\theta }_F\end{array}\right]\left({\mathit{A}}_B-{\mathit{A}}_F\right)$$

(6)

Since ${\mathit{A}}_F$ is constant in the current straight path, then the derivative of Equation (6) yields

$${\dot{\mathit{A}}}_e=\left[\begin{array}{c}{\dot{\theta }}_e\\ {\dot{\psi }}_e\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& \cos {\theta }_F\end{array}\right]\left[\begin{array}{c}\dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}\dot{\theta }\\ \dot{\psi }\cos {\theta }_F\end{array}\right]=\left[\begin{array}{c}q\\ \frac{\cos {\theta }_F}{\cos \theta }r\end{array}\right].$$

(7)

The inertial coordinates of O in {I} frame are ${\mathit{P}}_O={\left[\begin{array}{ccc}{\xi }_O& {\eta }_O& {\zeta }_O\end{array}\right]}^T$, the inertial coordinates of P in {I} frame are ${\mathit{P}}_P={\left[\begin{array}{ccc}{\xi }_k& {\eta }_k& {\zeta }_k\end{array}\right]}^T$, and the coordinates of O in {F} frame are ${\mathit{P}}_e={\left[\begin{array}{ccc}{x}_e& y_e& z_e\end{array}\right]}^T$, then

$$\begin{gathered} {\mathit{P}}_e={\left[\begin{array}{ccc}{x}_e& y_e& z_e\end{array}\right]}^{\mathrm{T}}={\mathit{R}}_I^F\left({\mathit{P}}_O-{\mathit{P}}_P\right), \\ {\mathit{R}}_I^F=\left[\begin{array}{ccc}\cos {\psi }_F\cos {\theta }_F& \sin {\psi }_F\cos {\theta }_F& -\sin {\theta }_F\\ -\sin {\psi }_F& \cos {\psi }_F& 0\\ \cos {\psi }_F\sin {\theta }_F& \sin {\psi }_F\sin {\theta }_F& \cos {\theta }_F\end{array}\right]. \end{gathered}$$

(8)

${\mathit{R}}_I^F$ is the coordinate transformation from {I} to {F}.

Then, the derivative of Equation (8) yields

$${\dot{\mathit{P}}}_e={\dot{\mathit{R}}}_I^F\left({\mathit{P}}_O-{\mathit{P}}_P\right)+{\mathit{R}}_I^F\left({\dot{\mathit{P}}}_O-{\dot{\mathit{P}}}_P\right)={\mathit{R}}_I^F{\dot{\mathit{P}}}_O={\mathit{R}}_I^F{\mathit{J}}_1\left(\mathit{\eta }\right)\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{c}u\cos {\psi }_e\cos {\theta }_e-v\sin {\psi }_e+w\cos {\psi }_e\sin {\theta }_e\\ u\sin {\psi }_e\cos {\theta }_e+v\cos {\psi }_e+w\sin {\psi }_e\sin {\theta }_e\\ -u\sin {\theta }_e+w\cos {\theta }_e\end{array}\right].$$

(9)

3. Controller Design

In this paper, the control objective is to make the AUV track the straight lines path between the waypoints at a constant speed $u_d>0$, which is more conducive to energy saving. In the kinematics controller, the expected angular velocities $q_d$ and $r_d$ are designed by using two different methods, and then the obstacle avoidance penalty is designed based on the obstacle information detected by onboard sensors, where $q_d$ and $r_d$ are the expected pitch rate and yaw rate, respectively. The expected velocities are developed as virtual control inputs, and they are fed to the dynamic controller. Then, the actual control input variables $n_p,{\delta }_s,{\delta }_r$ are derived in the dynamic controller by using sliding mode control (SMC) technology, where $n_p,{\delta }_s,{\delta }_r$ are the respective speed of the propeller, sternplane rudder angle, and vertical rudder angle.

3.1. Kinematics Controller

3.1.1. Kinematics Controller Design Based on LOS Guidance Law

Since the along-track error $x_e$ does not need to be controlled, error model (9) can be simplified as follows:

$$\left[\begin{array}{c}{\dot{\overline{y}}}_e\\ {\dot{\overline{z}}}_e\end{array}\right]=\left[\begin{array}{c}u\sin {\psi }_e\cos {\theta }_e+b_y\\ -u\sin {\theta }_e+b_z\end{array}\right],$$

(10)

where ${\overline{y}}_e=y_e-y_{ed}$, ${\overline{z}}_e=z_e-z_{ed}$. $y_{ed}$ and $z_{ed}$ are penalty items added for obstacle avoidance, which are set to zero when there are no obstacles. When there is an obstacle, setting $y_{ed}$ or $z_{ed}$ to a positive value can avoid the obstacle from the right or below, and vice versa. The calculation method of the penalty item will be introduced in detail later. Because the AUV studied in this paper lacks lateral and vertical driving forces, the displacement velocities v and w are very small. $b_y=v\cos {\psi }_e+w\sin {\psi }_e\sin {\theta }_e,b_z=w\cos {\theta }_e$ are bounded. By applying the LOS guidance law, the expected values of ${\theta }_e$ and ${\psi }_e$ can be designed as follows:

$$\begin{array}{l}{\theta }_{ed}=\arctan \left({\overline{z}}_e/{\Delta }_{\theta }\right),\\ {\psi }_{ed}=-\arctan \left({\overline{y}}_e/{\Delta }_{\psi }\right),\end{array}$$

(11)

where ${\Delta }_{\theta }>0$ and ${\Delta }_{\psi }>0$ are look-ahead distances.

After defining the error variable ${\tilde{\theta }}_e={\theta }_{ed}-{\theta }_e,{\tilde{\psi }}_e={\psi }_{ed}-{\psi }_e$, the error model (10) can be rewritten as follows:

$$\left[\begin{array}{c}{\dot{\overline{y}}}_e\\ {\dot{\overline{z}}}_e\end{array}\right]=\mathit{A}\left(t\right)\left[\begin{array}{c}{\overline{y}}_e\\ {\overline{z}}_e\end{array}\right]+\mathit{B}\left(t\right)\left[\begin{array}{c}{\tilde{\theta }}_e\\ {\tilde{\psi }}_e\end{array}\right]+\left[\begin{array}{c}{b}_y\\ b_z\end{array}\right],$$

(12)

where

$$\begin{array}{c}\mathit{A}\left(t\right)=\left[\begin{array}{cc}{A}_{11}& 0\\ 0& A_{22}\end{array}\right],\mathit{B}\left(t\right)=\left[\begin{array}{cc}{B}_{11}& B_{12}\\ B_{21}& 0\end{array}\right],\\ A_{11}=-\frac{u}{\sqrt{{\overline{y}}_e^2+{\Delta }_{\psi }^2}}\frac{{\Delta }_{\theta }}{\sqrt{{\overline{z}}_e^2+{\Delta }_{\theta }^2}},A_{22}=-\frac{u}{\sqrt{{\overline{z}}_e^2+{\Delta }_{\theta }^2}},B_{11}=u\sin {\psi }_{ed}\cos {\tilde{\psi }}_e\left[\frac{\left(\cos {\tilde{\theta }}_e-1\right)}{{\tilde{\theta }}_e}\cos {\theta }_{ed}+\frac{\sin {\tilde{\theta }}_e}{{\tilde{\theta }}_e}\sin {\theta }_{ed}\right],\\ B_{12}=u\left[\frac{\left(\cos {\tilde{\psi }}_e-1\right)}{{\tilde{\psi }}_e}\sin {\psi }_{ed}\cos {\theta }_{ed}-\frac{\sin {\tilde{\psi }}_e}{{\tilde{\psi }}_e}\cos {\psi }_{ed}\cos {\theta }_e\right],B_{21}=u\left[\frac{\sin {\tilde{\theta }}_e}{{\tilde{\theta }}_e}\cos {\theta }_{ed}-\frac{\left(\cos {\tilde{\theta }}_e-1\right)}{{\tilde{\theta }}_e}\sin {\theta }_{ed}\right].\end{array}$$

Since $\mathit{A}\left(t\right)$ is negative and $\mathit{B}\left(t\right),b_y,b_z$ are bounded, according to the cascade system theory, the stability of the system (12) can be guaranteed as long as ${\tilde{\theta }}_e$ and ${\tilde{\psi }}_e$ are stable. Next, the virtual control input is designed through the backstepping method to ensure the stability of ${\tilde{\theta }}_e$ and ${\tilde{\psi }}_e$. The Lyapunov function is defined as follows:

$$V_1=\frac{1}{2}{\tilde{\theta }}_e^2+\frac{1}{2}{\tilde{\psi }}_e^2.$$

(13)

The expected angular velocities can be designed as

$$\begin{array}{l}{q}_d={\dot{\theta }}_{ed}+k_q{\tilde{\theta }}_e,\\ r_d=\frac{\cos \theta }{\cos {\theta }_F}\left({\dot{\psi }}_{ed}+k_r{\tilde{\psi }}_e\right).\end{array}$$

(14)

Then, the derivative of $V_1$ yields ${\dot{V}}_1=-k_q{\tilde{\theta }}_e^2-k_r{\tilde{\psi }}_e^2\le -2k_{V_1}{V}_1\le 0$, where $k_q>0,k_r>0,k_{V_1}=\min \left[k_q,k_r\right]$. Although the above traditional control method based on the LOS guidance law is easy to calculate and apply, it still has some shortcomings. When ${\Delta }_{\theta }$ and ${\Delta }_{\psi }$ are set to be small, the path tracking is prone to overshoot, otherwise the tracking errors converge slowly. When $k_q$ and $k_r$ are set to be small, the path tracking quality will decrease, otherwise the rudder angle is prone to saturation. The reason is that the expected angular velocities obtained by the above traditional control method are not the optimal value and do not meet the constraint conditions. In order to improve the control quality, the next step is to use MPC to redesign the kinematic controller.

3.1.2. Kinematics Controller Design Based on MPC

The response of the dynamic controller to the expected angular velocity can be described as follows:

$$\begin{array}{l}\dot{q}=\frac{1}{T_1}\left(q_d-q\right),\\ \dot{r}=\frac{1}{T_2}\left(r_d-r\right),\end{array}$$

(15)

where $T_1>0$ and $T_2>0$ are adjustable time constants.

The Predictive Model

Because the AUV studied in this paper lacks lateral and vertical driving forces, the displacement velocities v and w are very small, which can be ignored to simplify the controller. Since along-track error x_e does not need to be controlled, according to Equations (7), (9), and (15), the control objective can be simplified to the stabilization problem as follows:

$$\left[\begin{array}{c}{\dot{\overline{y}}}_e\\ {\dot{\overline{z}}}_e\\ {\dot{\theta }}_e\\ {\dot{\psi }}_e\\ \dot{q}\\ \dot{r}\end{array}\right]=\left[\begin{array}{c}u\sin {\psi }_e\cos {\theta }_e\\ -u\sin {\theta }_e\\ q\\ \frac{\cos {\theta }_F}{\cos \theta }r\\ \frac{1}{T_1}\left(q_d-q\right)\\ \frac{1}{T_2}{\left(r_d-r\right)}_2\end{array}\right]=\left[\begin{array}{c}u{k}_y{\psi }_e\\ -u{k}_z{\theta }_e\\ q\\ \frac{\cos {\theta }_F}{\cos \theta }r\\ \frac{1}{T_1}\left(q_d-q\right)\\ \frac{1}{T_2}\left(r_d-r\right)\end{array}\right],$$

(16)

where $k_y=\cos {\theta }_e\sin {\psi }_e/{\psi }_e,k_z=\sin {\theta }_e/{\theta }_e$. In order to avoid singularities, $k_y$ is set to $k_y=\cos {\theta }_e$ when $\left|{\psi }_e\right|\le \pi /12$ and $k_z$ is set to $k_z=1$ when $\left|{\theta }_e\right|\le \pi /12$. The system (16) can be rewritten as

$$\dot{\mathit{x}}=f\left(\mathit{x},\mathit{u}\right),$$

(17)

where $\mathit{x}={\left[\begin{array}{llllll}{\overline{y}}_e\hfill & {\overline{z}}_e\hfill & {\theta }_e\hfill & {\psi }_e\hfill & q\hfill & r\hfill \end{array}\right]}^T$, $\mathit{u}={\left[\begin{array}{cc}{q}_d& r_d\end{array}\right]}^T$. Obviously, the equilibrium points of system (17) are x = 0 and u = 0. By discretizing system (17), the following can be obtained:

$$\begin{array}{l}{\mathit{x}}_{k+1,k}={\mathit{A}}_k{\mathit{x}}_{k,k}+{\mathit{B}}_k{\mathit{u}}_{k,k},\\ {\mathit{y}}_{k,k}={\mathit{C}}_k{\mathit{x}}_{k,k},\end{array}$$

(18)

where

$${\mathit{A}}_k=\left[\begin{array}{cccccc}1& 0& 0& Tu{k}_y& 0& 0\\ 0& 1& -Tu{k}_z& 0& 0& 0\\ 0& 0& 1& 0& T& 0\\ 0& 0& 0& 1& 0& \frac{T\cos {\theta }_F}{\cos \theta }\\ 0& 0& 0& 0& 1-\frac{T}{T_1}& 0\\ 0& 0& 0& 0& 0& 1-\frac{T}{T_2}\end{array}\right],{\mathit{B}}_k=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ \frac{T}{T_1}& 0\\ 0& \frac{T}{T_2}\end{array}\right],{\mathit{C}}_k=I_6.$$

$T$ is the sampling time. Here the subscript k represents the sampling time series. Using (18), the future system state can be predicted as

$$\begin{array}{ccc}\hfill {\mathit{x}}_{k+1,k}& =\hfill & {\mathit{A}}_k{\mathit{x}}_{k,k}+{\mathit{B}}_k{\mathit{u}}_{k,k},\hfill \\ \hfill {\mathit{x}}_{k+2,k}& =\hfill & {\mathit{A}}_k{\mathit{x}}_{k+1,k}+{\mathit{B}}_k{\mathit{u}}_{k+1,k}={\mathit{A}}_k^2{\mathit{x}}_{k,k}+{\mathit{A}}_k{\mathit{B}}_k{\mathit{u}}_{k,k}+{\mathit{B}}_k{\mathit{u}}_{k+1,k},\hfill \\ \hfill {\mathit{x}}_{k+3,k}& =\hfill & {\mathit{A}}_k{\mathit{x}}_{k+2,k}+{\mathit{B}}_k{\mathit{u}}_{k+2,k}={\mathit{A}}_k^3{\mathit{x}}_{k,k}+{\mathit{A}}_k^2{\mathit{B}}_k{\mathit{u}}_{k,k}+{\mathit{A}}_k{\mathit{B}}_k{\mathit{u}}_{k+1,k}+{\mathit{B}}_k{\mathit{u}}_{k+2,k},\hfill \\ & ⋮& \\ {\mathit{x}}_{k+N_p,k}\hfill & =\hfill & {\mathit{A}}_k^{N_p}{\mathit{x}}_{k,k}+{\mathit{A}}_k^{N_p-1}{\mathit{B}}_k{\mathit{u}}_{k,k}+\cdots +{\mathit{A}}_k^{N_p-N_c}{\mathit{B}}_k{\mathit{u}}_{k+N_c-1,k},\hfill \end{array}$$

(19)

where $N_c\le N_p$, $N_c$ is the control horizon, $N_p$ is the prediction horizon. Then, the future system output can be predicted as

$$\begin{array}{ccc}\hfill {\mathit{y}}_{k+1,k}& =\hfill & {\mathit{C}}_k{\mathit{A}}_k{\mathit{x}}_{k,k}+{\mathit{C}}_k{\mathit{B}}_k{\mathit{u}}_{k,k},\hfill \\ \hfill {\mathit{y}}_{k+2,k}& =\hfill & {\mathit{C}}_k{\mathit{A}}_k^2{\mathit{x}}_{k,k}+{\mathit{C}}_k{\mathit{A}}_k{\mathit{B}}_k{\mathit{u}}_{k,k}+{\mathit{C}}_k{\mathit{B}}_k{\mathit{u}}_{k+1,k},\hfill \\ \hfill {\mathit{y}}_{k+3,k}& =\hfill & {\mathit{C}}_k{\mathit{A}}_k^3{\mathit{x}}_{k,k}+{\mathit{C}}_k{\mathit{A}}_k^2{\mathit{B}}_k{\mathit{u}}_{k,k}+{\mathit{C}}_k{\mathit{A}}_k{\mathit{B}}_k{\mathit{u}}_{k+1,k}+{\mathit{C}}_k{\mathit{B}}_k{\mathit{u}}_{k+2,k},\hfill \\ & ⋮& \\ \hfill {\mathit{y}}_{k+N_p,k}& =\hfill & {\mathit{C}}_k{\mathit{A}}_k^{N_p}{\mathit{x}}_{k,k}+{\mathit{C}}_k{\mathit{A}}_k^{N_p-1}{\mathit{B}}_k{\mathit{u}}_{k,k}+\cdots +{\mathit{C}}_k{\mathit{A}}_k^{N_p-N_c}{\mathit{B}}_k{\mathit{u}}_{k+N_c-1,k}.\hfill \end{array}$$

(20)

The future system output can be rewritten as

$${\mathit{Y}}_{k+1,k}=\mathit{\Psi }{\mathit{x}}_{k,k}+\mathit{\Theta }{\mathit{U}}_{k,k},$$

(21)

where

$${\mathit{Y}}_{k+1,k}=\left[\begin{array}{c}{\mathit{y}}_{k+1,k}\\ {\mathit{y}}_{k+2,k}\\ {\mathit{y}}_{k+3,k}\\ ⋮\\ {\mathit{y}}_{k+N_p,k}\end{array}\right],{\mathit{U}}_{k,k}=\left[\begin{array}{c}{\mathit{u}}_{k,k}\\ {\mathit{u}}_{k+1,k}\\ {\mathit{u}}_{k+2,k}\\ ⋮\\ {\mathit{u}}_{k+N_c-1,k}\end{array}\right],\mathit{\Psi }=\left[\begin{array}{c}{\mathit{C}}_k{\mathit{A}}_k\\ {\mathit{C}}_k{\mathit{A}}_k^2\\ {\mathit{C}}_k{\mathit{A}}_k^3\\ ⋮\\ {\mathit{C}}_k{\mathit{A}}_k^{N_p}\end{array}\right],\mathit{\Theta }=\left[\begin{array}{ccccc}{\mathit{C}}_k{\mathit{B}}_k& 0& 0& \cdots & 0\\ {\mathit{C}}_k{\mathit{A}}_k{\mathit{B}}_k& {\mathit{C}}_k{\mathit{B}}_k& 0& \cdots & 0\\ {\mathit{C}}_k{\mathit{A}}_k^2{\mathit{B}}_k& {\mathit{C}}_k{\mathit{A}}_k{\mathit{B}}_k& {\mathit{C}}_k{\mathit{B}}_k& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\mathit{C}}_k{\mathit{A}}_k^{N_p-1}{\mathit{B}}_k& {\mathit{C}}_k{\mathit{A}}_k^{N_p-2}{\mathit{B}}_k& {\mathit{C}}_k{\mathit{A}}_k^{N_p-3}{\mathit{B}}_k& \cdots & {\mathit{C}}_k{\mathit{A}}_k^{N_p-N_c}{\mathit{B}}_k\end{array}\right].$$

The Control Constraint

In the actual control system, due to the saturation limitation of the rudder angle, the AUV’s angular velocity is also constrained. Here, the control constraints are considered as

$${\mathit{u}}_{\min }\le {\mathit{u}}_{k+t,k}\le {\mathit{u}}_{\max },t=0,1,\cdots N_c-1.$$

(22)

The control constraints (22) can be translated into linear inequalities as follows:

$$\left[\begin{array}{r}{\mathit{M}}_c\\ -{\mathit{M}}_c\end{array}\right]{\mathit{U}}_{k,k}\le \left[\begin{array}{c}{\mathit{N}}_{\max }\\ {\mathit{N}}_{\min }\end{array}\right],$$

(23)

where ${\mathit{M}}_c={\mathit{I}}_{N_c}\otimes {\mathit{I}}_2,{\mathit{N}}_{\max }={\mathbf{1}}_{N_c}\otimes {\mathit{u}}_{\max },{\mathit{N}}_{\min }=-{\mathbf{1}}_{N_c}\otimes {\mathit{u}}_{\min },{\mathbf{1}}_{N_c}^T={\left[\begin{array}{ccccc}1& 1& 1& \cdots & 1\end{array}\right]}_{1\times N_c}.$ The symbol ⊗ is Kronecker product.

Optimization with Control Constraint

For system (18), the task of MPC can be equivalent to calculating the optimal control input ${\mathit{U}}_{k,k}$ at each sampling time $k$ to minimize cost functionas follows:

$$J_{k,k}={\mathit{Y}}_{k+1,k}^T\overline{\mathit{Q}}{\mathit{Y}}_{k+1,k}+{\mathit{U}}_{k,k}^T\overline{\mathit{R}}{\mathit{U}}_{k,k},$$

(24)

where $\overline{\mathit{Q}}={\mathit{I}}_{N_p}\otimes \mathit{Q}$, $\overline{\mathit{R}}={\mathit{I}}_{N_c}\otimes \mathit{R}$, $\mathit{Q}=diag\left(Q_{11},Q_{22},Q_{33},Q_{44},Q_{55},Q_{66}\right)$, $\mathit{R}=diag\left(R_{11},R_{22}\right)$ are positive definite weighting matrices. The next step is to design constraints to ensure the stability of MPC.

The cost function at sampling time $k-1$ is defined as

$$J_{k-1,k-1}={\mathit{Y}}_{k,k-1}^T\overline{\mathit{Q}}{\mathit{Y}}_{k,k-1}+{\mathit{U}}_{k-1,k-1}^T\overline{\mathit{R}}{\mathit{U}}_{k-1,k-1},$$

(25)

where

$${\mathit{U}}_{k-1,k-1}=\left[\begin{array}{c}{\mathit{u}}_{k-1,k-1}\\ {\mathit{u}}_{k,k-1}\\ {\mathit{u}}_{k+1,k-1}\\ ⋮\\ {\mathit{u}}_{k+N_c-2,k-1}\end{array}\right],{\mathit{Y}}_{k,k-1}=\left[\begin{array}{c}{\mathit{y}}_{k,k-1}\\ {\mathit{y}}_{k+1,k-1}\\ {\mathit{y}}_{k+2,k-1}\\ ⋮\\ {\mathit{y}}_{k+N_p-1,k-1}\end{array}\right].$$

A feasible control variable at time $k$ is defined, and the predicted output variable is

$${\overline{\mathit{U}}}_{k,k}=\left[\begin{array}{c}{\mathit{u}}_{k,k-1}\\ {\mathit{u}}_{k+1,k-1}\\ ⋮\\ {\mathit{u}}_{k+N_c-2,k-1}\\ {\mathit{u}}_{k+N_c-1,k}\end{array}\right],{\overline{\mathit{Y}}}_{k+1,k}=\left[\begin{array}{c}{\mathit{y}}_{k+1,k-1}\\ {\mathit{y}}_{k+2,k-1}\\ ⋮\\ {\mathit{y}}_{k+N_p-1,k-1}\\ {\overline{\mathit{y}}}_{k+N_p,k}\end{array}\right].$$

(26)

Define $\Delta J_{k,k}=J_{k,k}-J_{k-1,k-1}$, then

$$\begin{array}{ll}\Delta J_{k,k}& =J_{k,k}-J_{k-1,k-1},\\ & ={\mathit{Y}}_{k+1,k}^T\overline{\mathit{Q}}{\mathit{Y}}_{k+1,k}+{\mathit{U}}_{k,k}^T\overline{\mathit{R}}{\mathit{U}}_{k,k}-{\mathit{Y}}_{k,k-1}^T\overline{\mathit{Q}}{\mathit{Y}}_{k,k-1}-{\mathit{U}}_{k-1,k-1}^T\overline{\mathit{R}}{\mathit{U}}_{k-1,k-1},\\ & \le {\overline{\mathit{Y}}}_{k+1,k}^T\overline{\mathit{Q}}{\overline{\mathit{Y}}}_{k+1,k}+{\overline{\mathit{U}}}_{k,k}^T\overline{\mathit{R}}{\overline{\mathit{U}}}_{k,k}-{\mathit{Y}}_{k,k-1}^T\overline{\mathit{Q}}{\mathit{Y}}_{k,k-1}-{\mathit{U}}_{k-1,k-1}^T\overline{\mathit{R}}{\mathit{U}}_{k-1,k-1},\\ & \le {\overline{\mathit{y}}}_{k+N_p,k}^T\mathit{Q}{\overline{\mathit{y}}}_{k+N_p,k}-{\mathit{y}}_{k,k-1}^T\mathit{Q}{\mathit{y}}_{k,k-1}+{\mathit{u}}_{k+N_c-1,k}^T\mathit{R}{\mathit{u}}_{k+N_c-1,k}-{\mathit{u}}_{k-1,k-1}^T\mathit{R}{\mathit{u}}_{k-1,k-1}.\end{array}$$

The stability of MPC can be guaranteed as long as the following constraints are met,

$${\overline{\mathit{y}}}_{k+N_p,k}^T\mathit{Q}{\overline{\mathit{y}}}_{k+N_p,k}-{\mathit{y}}_{k,k}^T\mathit{Q}{\mathit{y}}_{k,k}+{\mathit{u}}_{k+N_c-1,k}^T\mathit{R}{\mathit{u}}_{k+N_c-1,k}-{\mathit{u}}_{k-1,k-1}^T\mathit{R}{\mathit{u}}_{k-1,k-1}\le 0,$$

(27)

where

$$\begin{array}{l}{\overline{\mathit{y}}}_{k+N_p,k}^T=\overline{\mathit{\Psi }}{\mathit{x}}_{k,k}+{\overline{\mathit{\Theta }}}_1{\mathit{C}}_1{\mathit{U}}_{k-1,k-1}+{\overline{\mathit{\Theta }}}_2{\mathit{C}}_2{\mathit{U}}_{k,k},{\mathit{u}}_{k+N_c-1,k}={\mathit{C}}_2{\mathit{U}}_{k,k},{\mathit{u}}_{k-1,k-1}={\mathit{C}}_3{\mathit{U}}_{k-1,k-1},\\ \overline{\mathit{\Psi }}={\mathit{C}}_k{\mathit{A}}_k^{N_p},{\overline{\mathit{\Theta }}}_1={\mathit{C}}_k\left[\begin{array}{ccccc}{\mathit{A}}_k^{N_p-1}{\mathit{B}}_k& {\mathit{A}}_k^{N_p-2}{\mathit{B}}_k& {\mathit{A}}_k^{N_p-3}{\mathit{B}}_k& \cdots & {\mathit{A}}_k^{N_p-N_c+1}{\mathit{B}}_k\end{array}\right],{\overline{\mathit{\Theta }}}_2={\mathit{C}}_k{\mathit{A}}_k^{N_p-N_c}{\mathit{B}}_k,\\ {\mathit{C}}_1=\left[\begin{array}{cc}{\mathbf{0}}_{\left(N_c-1\right)\times 1}& {\mathit{I}}_{N_c-1}\end{array}\right]\otimes {\mathit{I}}_2,{\mathit{C}}_2=\left[\begin{array}{cc}{\mathbf{0}}_{1\times \left(N_c-1\right)}& 1\end{array}\right]\otimes {\mathit{I}}_2,{\mathit{C}}_3=\left[\begin{array}{cc}1& {\mathbf{0}}_{1\times \left(N_c-1\right)}\end{array}\right]\otimes {\mathit{I}}_2.\end{array}$$

The constrained variables are parameterized by parameter vector ${\mathit{U}}_{k,k}$, making it convenient to solve the optimization problem. By inserting Equation (21) into Equation (24), $J_{k,k}$ is expressed as

$$J_{k,k}={\left[\mathit{\Psi }{\mathit{x}}_{k,k}\right]}^T\overline{\mathit{Q}}\mathit{\Psi }{\mathit{x}}_{k,k}+{\mathit{U}}_{k,k}^T\mathit{E}{\mathit{U}}_{k,k}+2{\mathit{U}}_{k,k}^T\mathit{F},$$

(28)

where $\mathit{E}={\mathit{\Theta }}^T\overline{\mathit{Q}}\mathit{\Theta }+\overline{\mathit{R}},\mathit{F}={\mathit{\Theta }}^T\overline{\mathit{Q}}\mathit{\Psi }{\mathit{x}}_{k,k}$. Because ${\left[\mathit{\Psi }{\mathit{x}}_{k,k}\right]}^T\overline{\mathit{Q}}\mathit{\Psi }{\mathit{x}}_{k,k}$ is constant, the solving of optimization problems under the constraints at every sampling time is rewritten as the following QP problem:

$$\begin{array}{l}\underset{{\mathit{U}}_{k,k}}{\min }{J}_{k,k}={\mathit{U}}_{k,k}^T\mathit{E}{\mathit{U}}_{k,k}+2{\mathit{U}}_{k,k}^T\mathit{F},\\ \mathrm{s}.\mathrm{t}.\left[\begin{array}{r}{\mathit{M}}_c\\ -{\mathit{M}}_c\end{array}\right]{\mathit{U}}_{k,k}\le \left[\begin{array}{c}{\mathit{N}}_{\max }\\ {\mathit{N}}_{\min }\end{array}\right],\\ \begin{array}{cc}& {\overline{\mathit{y}}}_{k+N_p,k}^T\mathit{Q}{\overline{\mathit{y}}}_{k+N_p,k}-{\mathit{y}}_{k,k}^T\mathit{Q}{\mathit{y}}_{k,k}+{\mathit{u}}_{k+N_c-1,k}^T\mathit{R}{\mathit{u}}_{k+N_c-1,k}-{\mathit{u}}_{k-1,k-1}^T\mathit{R}{\mathit{u}}_{k-1,k-1}\le 0\end{array}.\end{array}$$

(29)

At each sampling time, the optimal control input ${\mathit{U}}_{k,k}$ can be obtained by solving Equation (29). Then, the first element ${\mathit{u}}_{k,k}$ is implemented as the optimal expected value of the angular velocity as follows:

$${\left[\begin{array}{c}{q}_d\\ r_d\end{array}\right]}_{k,k}={\mathit{u}}_{k,k}.$$

(30)

By repeating the above calculation process, the optimal expected angular velocity at each sampling moment can be obtained in real time.

3.1.3. Obstacle Detection and Calculation of the Penalty Term for Obstacle Avoidance

Nowadays, several types of sensors can be used to detect obstacles. For example, in [38,39], light detection and ranging (LiDAR) sensors or stereoscopic cameras are used in obstacle avoidance control of robots. In [37,40], sonar is used as the sensor to detect obstacles for obstacle avoidance control of underwater robots. In this paper, the AUV is configured with sonaras shown in Figure 2, which can obtain the information of obstacles in real time. The detection range, angle of view, detection period, and beam number of the sonar are 100 m, 180°, 0.5 s, and 61, respectively. The angle between any two beams of the sonar is $\Delta \psi$. The sensor can return a set of data $\mathit{\rho }=\left[{\rho }_1,{\rho }_2,{\rho }_i,\cdots {\rho }_s\right]$ in real time where ${\rho }_i,\left(i=1\cdots s\right)$ represents the distance between AUV and the obstacle in the direction of the ith beam of sonar, that is, the distance between O and the intersection point $p_i$. If there are no obstacles, then ${\rho }_i=0,\left(i=1\cdots s\right)$. If there is a set of continuous non-zero data $\left[{\rho }_i,\cdots {\rho }_j\right],\left(1\le i\le j\le s\right)$ in the received data ρ, it means there is an obstacle ahead. At this point, it is necessary to calculate the penalty term in real time to avoid the obstacle.

Since the AUV sails in the horizontal plane for most of the time, the obstacle avoidance in the horizontal plane is taken as an example to introduce the calculation procedure of the penalty item.

Step 1:

The coordinates of intersection points $p_c,\left(c=i\cdots j\right)$ in a fixed coordinate system are calculated according to the following equations:

$$\begin{array}{l}\left[\begin{array}{c}{\xi }_c\\ {\eta }_c\end{array}\right]=\left[\begin{array}{cc}\cos {\psi }_c& -\sin {\psi }_c\\ \sin {\psi }_c& \cos {\psi }_c\end{array}\right]\left[\begin{array}{c}{\rho }_c\\ 0\end{array}\right]+\left[\begin{array}{c}\xi \\ \eta \end{array}\right],\\ {\psi }_c=\psi +\left(\frac{s+1}{2}-c\right)\Delta \psi ,\left(c=i,\cdots j\right).\end{array}$$

(31)

Step 2:

The distance between the intersection point $p_c,\left(c=i\cdots j\right)$ and the current path is calculated according to the following equation:

$$y_e^c=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{cc}\cos {\psi }_F& \sin {\psi }_F\\ -\sin {\psi }_F& \cos {\psi }_F\end{array}\right]\left(\left[\begin{array}{c}{\xi }_c\\ {\eta }_c\end{array}\right]-\left[\begin{array}{c}{\xi }_k\\ {\eta }_k\end{array}\right]\right),\left(c=i,\cdots j\right).$$

(32)

Step 3:

If all the data $y_e^c,\left(c=i,\cdots j\right)$ are greater than zero, then the obstacle is on the right side of the path. At this point, if $\min \left(y_e^c\right)>y_e^s,\left(c=i,\cdots j\right)$ is satisfied, then set $y_{ed}=0$. Otherwise, choose to avoid the obstacle from the left side of the obstacle and set $y_{ed}=\min \left(y_e^c\right)-y_e^s$ until all the received data ρ values are zero, then set $y_{ed}=0$, where $y_e^s>0$ is the safe distance between the AUV and an obstacle.

Step 4:

If all the data $y_e^c,\left(c=i,\cdots j\right)$ are less than zero, then the obstacle is on the left side of the path. At this point, if $\min \left(\left|y_e^c\right|\right)>y_e^s,\left(c=i,\cdots j\right)$ is satisfied, then set $y_{ed}=0$. Otherwise, choose to avoid the obstacle from the right side of the obstacle and set $y_{ed}=\max \left(y_e^c\right)+y_e^s$ until all the received data ρ values are zero, then set $y_{ed}=0$.

Step 5:

If $\min \left(y_e^c\right)<0,\max \left(y_e^c\right)>0,\left(c=i,\cdots j\right)$ is satisfied, then the obstacle is on the path ahead. At this point, if $\left|\min \left(y_e^c\right)\right|<\left|\max \left(y_e^c\right)\right|,\left(c=i,\cdots j\right)$ is satisfied, then choose to avoid the obstacle from the left side of the obstacle and set $y_{ed}=\min \left(y_e^c\right)-y_e^s$. Otherwise, choose to avoid the obstacle from the right side of the obstacle and set $y_{ed}=\max \left(y_e^c\right)+y_e^s$ until all the received data ρ values are zero, then set $y_{ed}=0$.

3.2. Dynamics Controller

Next, the design of the dynamic controller is introduced to realize the tracking control of the expected velocity signal. Since the underactuated AUV lacks lateral and vertical thrusters, dynamic Equation (4) can be simplified as follows:

$${\dot{\mathit{v}}}_a={\mathit{C}}_a{\mathit{v}}_a+{\mathit{G}}_a+{\mathit{D}}_a{\overline{\mathit{\tau }}}_a+{\mathit{b}}_a,$$

(33)

where

$${\mathit{v}}_a=\left[\begin{array}{c}u\\ q\\ r\end{array}\right],{\mathit{C}}_a=\left[\begin{array}{ccc}\frac{d_{11}}{m_{11}}& -\frac{m_{33}}{m_{11}}w& \frac{m_{22}}{m_{11}}v\\ \frac{m_{33}-m_{11}}{m_{55}}w& \frac{d_{55}}{m_{55}}& 0\\ \frac{m_{11}-m_{22}}{m_{66}}v& 0& \frac{d_{66}}{m_{66}}\end{array}\right],{\mathit{G}}_a=\left[\begin{array}{c}0\\ \frac{M_{HS}}{m_{55}}\\ 0\end{array}\right],{\mathit{D}}_a=\left[\begin{array}{ccc}\frac{k_p}{m_{11}}& 0& 0\\ 0& \frac{k_{\delta }}{m_{55}}& 0\\ 0& 0& \frac{k_{\delta }}{m_{66}}\end{array}\right],{\overline{\mathit{\tau }}}_a=\left[\begin{array}{c}{\overline{n}}_p^2\\ {\overline{\delta }}_s\\ {\overline{\delta }}_r\end{array}\right],{\mathit{b}}_a=\left[\begin{array}{c}{b}_u\\ b_q\\ b_r\end{array}\right].$$

$k_{\delta }$,$k_p$ are the lift coefficient of the rudder and thrust coefficient of the propeller, respectively.

The actual control inputs have saturation limits, and the saturation value of the control input is defined as follows:

$$\Delta {\mathit{\tau }}_a={\overline{\mathit{\tau }}}_a-{\mathit{\tau }}_a,$$

(34)

where ${\mathit{\tau }}_a={\left[\begin{array}{ccc}{n}_p^2& {\delta }_s& {\delta }_r\end{array}\right]}^T$.

The following auxiliary systems are designed to compensate for input saturation

$$\begin{array}{l}{\dot{\mathit{\lambda }}}_1=-{\mathit{c}}_1{\mathit{\lambda }}_1+{\mathit{\lambda }}_2,\\ {\dot{\mathit{\lambda }}}_2=-{\mathit{c}}_2{\mathit{\lambda }}_2+{\mathit{D}}_a\Delta {\mathit{\tau }}_a,\end{array}$$

(35)

where

$${\mathit{\lambda }}_1=\left[\begin{array}{c}{\lambda }_{11}\\ {\lambda }_{12}\\ {\lambda }_{13}\end{array}\right],{\mathit{\lambda }}_2=\left[\begin{array}{c}{\lambda }_{21}\\ {\lambda }_{22}\\ {\lambda }_{23}\end{array}\right],{\mathit{c}}_1=\left[\begin{array}{ccc}{c}_{11}& 0& 0\\ 0& c_{12}& 0\\ 0& 0& c_{13}\end{array}\right],{\mathit{c}}_2=\left[\begin{array}{ccc}{c}_{21}& 0& 0\\ 0& c_{22}& 0\\ 0& 0& c_{23}\end{array}\right],c_{i,j}>0,i=1,2,j=1,2,3.$$

The sliding mode functions are defined as follows

$$\begin{array}{l}{\mathit{z}}_1={\displaystyle {\mathbf{\int }}_0^t\left({\mathit{v}}_d-{\mathit{v}}_a\right)}d\tau +{\mathit{\lambda }}_1,\\ {\mathit{z}}_2={\mathit{c}}_3{\mathit{z}}_1+{\dot{\mathit{z}}}_1,\\ {\mathit{z}}_3={\mathit{c}}_4{\mathit{z}}_1+{\mathit{z}}_2,\end{array}$$

(36)

where ${\mathit{v}}_d={\left[\begin{array}{ccc}{u}_d& q_d& r_d\end{array}\right]}^T$. The Lyapunov function is defined as follows:

$$V_2=\frac{1}{2}\left({\mathit{z}}_1^T{\mathit{z}}_1+{\mathit{z}}_3^T{\mathit{z}}_3\right)$$

(37)

The control law is defined as follows:

$${\mathit{\tau }}_a={\mathit{D}}_a^{-1}\left({\mathit{c}}_4{\dot{\mathit{z}}}_1+{\mathit{c}}_3{\dot{\mathit{z}}}_1+{\dot{\mathit{v}}}_d-{\mathit{C}}_a{\mathit{v}}_a-{\mathit{G}}_a-{\mathit{c}}_1\left(-{\mathit{c}}_1{\mathit{\lambda }}_1+{\mathit{\lambda }}_2\right)-{\mathit{c}}_2{\mathit{\lambda }}_2+\frac{1}{2{\gamma }^2}{\mathit{z}}_3+\frac{1}{2}{\mathit{z}}_3+{\mathit{c}}_5{\mathit{z}}_3\right),$$

(38)

where

$${\mathit{c}}_3=\left[\begin{array}{ccc}{c}_{31}& 0& 0\\ 0& c_{32}& 0\\ 0& 0& c_{33}\end{array}\right],{\mathit{c}}_4=\left[\begin{array}{ccc}{c}_{41}& 0& 0\\ 0& c_{42}& 0\\ 0& 0& c_{43}\end{array}\right],{\mathit{c}}_5=\left[\begin{array}{ccc}{c}_{51}& 0& 0\\ 0& c_{52}& 0\\ 0& 0& c_{53}\end{array}\right],c_{i,j}>0,i=3,4,5,j=1,2,3.$$

Then, the derivative of $V_2$ yields

$$\begin{array}{ll}{\dot{V}}_2& ={\mathit{z}}_1^T{\dot{\mathit{z}}}_1+{\mathit{z}}_3^T{\dot{\mathit{z}}}_3,\\ & ={\mathit{z}}_1^T\left({\mathit{z}}_2-{\mathit{c}}_3{\mathit{z}}_1\right)+{\mathit{z}}_3^T{\dot{\mathit{z}}}_3,\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2+{\mathit{z}}_3^T\left({\mathit{c}}_4{\dot{\mathit{z}}}_1+{\dot{\mathit{z}}}_2\right),\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2+{\mathit{z}}_3^T\left[{\mathit{c}}_4{\dot{\mathit{z}}}_1+{\mathit{c}}_3{\dot{\mathit{z}}}_1+{\dot{\mathit{v}}}_d-{\dot{\mathit{v}}}_a+{\ddot{\mathit{\lambda }}}_1\right],\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2+{\mathit{z}}_3^T\left[{\mathit{c}}_4{\dot{\mathit{z}}}_1+{\mathit{c}}_3{\dot{\mathit{z}}}_1+{\dot{\mathit{v}}}_d-\left({\mathit{C}}_a{\mathit{v}}_a+G_a+{\mathit{D}}_a{\overline{\mathit{\tau }}}_a+{\mathit{b}}_a\right)+\left(-{\mathit{c}}_1{\dot{\mathit{\lambda }}}_1+{\dot{\mathit{\lambda }}}_2\right)\right],\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2+{\mathit{z}}_3^T\left[{\mathit{c}}_4{\dot{\mathit{z}}}_1+{\mathit{c}}_3{\dot{\mathit{z}}}_1+{\dot{\mathit{v}}}_d-\left({\mathit{C}}_a{\mathit{v}}_a+G_a+{\mathit{D}}_a{\overline{\mathit{\tau }}}_a+{\mathit{b}}_a\right)-{\mathit{c}}_1\left(-{\mathit{c}}_1{\mathit{\lambda }}_1+{\mathit{\lambda }}_2\right)+\left(-{\mathit{c}}_2{\mathit{\lambda }}_2+{\mathit{D}}_a\Delta {\mathit{\tau }}_a\right)\right],\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2+{\mathit{z}}_3^T\left[{\mathit{c}}_4{\dot{\mathit{z}}}_1+{\mathit{c}}_3{\dot{\mathit{z}}}_1+{\dot{\mathit{v}}}_d-\left({\mathit{C}}_a{\mathit{v}}_a+G_a+{\mathit{D}}_a{\mathit{\tau }}_a+{\mathit{b}}_a\right)-{\mathit{c}}_1\left(-{\mathit{c}}_1{\mathit{\lambda }}_1+{\mathit{\lambda }}_2\right)-{\mathit{c}}_2{\mathit{\lambda }}_2\right],\\ & =-{\mathit{z}}_1^T{\mathit{c}}_3{\mathit{z}}_1+{\mathit{z}}_1^T{\mathit{z}}_2-{\mathit{z}}_3^T{\mathit{c}}_5{\mathit{z}}_3-\left(\frac{1}{2{\gamma }^2}{\mathit{z}}_3^T{\mathit{z}}_3+\frac{1}{2}{\mathit{z}}_3^T{\mathit{z}}_3+{\mathit{z}}_3^T{\mathit{b}}_a\right).\end{array}$$

Since

$$-\left(\frac{1}{2{\gamma }^2}{\mathit{z}}_3^T{\mathit{z}}_3+\frac{1}{2}{\mathit{z}}_3^T{\mathit{z}}_3+{\mathit{z}}_3^T{\mathit{b}}_a\right)-\frac{1}{2}{\gamma }^2{\Vert {\mathit{b}}_a\Vert }^2+\frac{1}{2}{\Vert {\mathit{z}}_3\Vert }^2=-\frac{1}{2}{\Vert \frac{1}{\gamma }{\mathit{z}}_3+\gamma {\mathit{b}}_a\Vert }^2\le 0,$$

then, the derivative of $V_2$ yields

$${\dot{V}}_2\le -\left[\begin{array}{cc}{\mathit{z}}_1^T& {\mathit{z}}_2^T\end{array}\right]Q_V\left[\begin{array}{c}{\mathit{z}}_1\\ {\mathit{z}}_2\end{array}\right]+\left(\frac{1}{2}{\gamma }^2{\Vert {\mathit{b}}_a\Vert }^2-\frac{1}{2}{\Vert {\mathit{z}}_3\Vert }^2\right),$$

where $Q_V=\left[\begin{array}{cc}{\mathit{c}}_3+{\mathit{c}}_5{\mathit{c}}_4^2& {\mathit{c}}_5{\mathit{c}}_4-\frac{1}{2}{I}_3\\ {\mathit{c}}_5{\mathit{c}}_4-\frac{1}{2}{I}_3& {\mathit{c}}_5\end{array}\right]$. Set the parameters ${\mathit{c}}_3,{\mathit{c}}_4,{\mathit{c}}_5$ to make $\left|Q_V\right|>0$, then

$${\dot{V}}_2\le \frac{1}{2}{\gamma }^2{\Vert {\mathit{b}}_a\Vert }^2-\frac{1}{2}{\Vert {\mathit{z}}_3\Vert }^2.$$

As long as the conditions $\Vert {\mathit{z}}_3\Vert \ge \gamma \Vert {\mathit{b}}_a\Vert$ are satisfied, then ${\dot{V}}_2\le 0$. Consequently, the system is uniformly ultimately bounded can be guaranteed.

3.3. Stability Analysis of Sway and Heave

The dynamic equations of the lateral and vertical velocities can be rewritten as

$$\begin{array}{l}\dot{v}=\frac{d_{22}}{m_{22}}\left(v+\frac{b_v-m_{11}ur}{d_{22}}\right),\\ \dot{w}=\frac{d_{33}}{m_{33}}\left(w+\frac{b_w+m_{11}uq}{d_{33}}\right).\end{array}$$

(39)

The Lyapunov function is defined:

$$V_3=\frac{1}{2}\left(m_{22}{v}^2+m_{33}{w}^2\right).$$

(40)

Since the conditions $d_{22}<0$ and $d_{33}<0$ are satisfied, the derivative of $V_3$ yields

$${\dot{V}}_3=d_{22}\left(v^2+\frac{b_v-m_{11}ur}{d_{22}}v\right)+d_{33}\left(w^2+\frac{b_w+m_{11}uq}{d_{33}}w\right)\le d_{22}\left(v^2-\left|\frac{b_v-m_{11}ur}{d_{22}}\right|\left|v\right|\right)+d_{33}\left(w^2-\left|\frac{b_w+m_{11}uq}{d_{33}}\right|\left|w\right|\right).$$

As long as the conditions $\left|v\right|\ge \left|\frac{b_v-m_{11}ur}{d_{22}}\right|,\left|w\right|\ge \left|\frac{b_w+m_{11}uq}{d_{33}}\right|$ are satisfied, then ${\dot{V}}_3\le 0$. Consequently, v and w are passive-bounded, and uniform ultimate bounding can be concluded [41].

4. The Results and Analysis of the Simulation Experiment

In order to verify the performance of the controller, the simulation experiment is carried out. The AUV used in the simulation is the REMUS 100 [42]. The desired path is generated based on a series of waypoints which are displayed in

Table 1. Waypoints.

Waypoints	1	2	3	4	5	6	7	8	9	10
$\xi /m$	10	10	−50	−50	10	40	100	130	130	100
$\eta /m$	0	30	30	−30	−30	−30	30	30	−30	−30
$\zeta /m$	0	4	12	20	28	28	28	28	28	28

. The initial position, orientation, and velocity of the AUV are zero. The expected forward speed is $u_d=1$ $\mathrm{m}.{\mathrm{s}}^{-1}$. The radius of turning circle $R_k$ is 10 m. In the simulation, three different control methods are used for comparison. The first and the second methods are traditional control methods based on LOS guidance law. The third method is the improved control method based on MPC. Because the PID controller is simple and does not depend on the system model, it is the most widely used control method in various control fields. In the first control method (LOS+PID), the kinematics controller uses LOS guidance law (11), and the dynamics controller uses the PID controller as follows

$$\begin{array}{l}{n}_p=k_{{}_p}^p\left(u-u_d\right)+k_{{}_i}^p{\displaystyle {\int }_0^t\left(u-u_d\right)d\tau }+k_{{}_d}^p\dot{u},\\ {\delta }_s=k_{{}_p}^s\left({\theta }_e-{\theta }_{ed}\right)+k_{{}_i}^s{\displaystyle {\int }_0^t\left({\theta }_e-{\theta }_{ed}\right)d\tau }+k_{{}_d}^s\left({\dot{\theta }}_e-{\dot{\theta }}_{ed}\right),\\ {\delta }_r=k_{{}_p}^r\left({\psi }_e-{\psi }_{ed}\right)+k_{{}_i}^r{\displaystyle {\int }_0^t\left({\psi }_e-{\psi }_{ed}\right)d\tau }+k_{{}_d}^r\left({\dot{\psi }}_e-{\dot{\psi }}_{ed}\right).\end{array}$$

(41)

The other two methods are proposed in this paper. In the second control method (LOS+SMC), the kinematics and the dynamics controller uses LOS and SMC for path following and obstacle avoidance. In the third control method (MPC+SMC), the kinematics and dynamics controller uses MPC and SMC for path following and obstacle avoidance. The main parameters of the controller are displayed in

Table 2. Parameters of the controller.

MPC	MPC	LOS+PID	LOS+PID	SMC	SMC
$T=0.1$	$T_1=1$	${\Delta }_{\theta }=10$	${\Delta }_{\psi }=10$	$c_{11}=1$	$c_{12}=1$
$T_2=1$	$Q_{11}=2$	$k_p^p=20$	$k_i^p=3$	$c_{13}=1$	$c_{21}=1$
$Q_{22}=2$	$Q_{33}=2$	$k_d^p=8$	$k_p^s=2$	$c_{22}=1$	$c_{23}=1$
$Q_{44}=2$	$Q_{55}=1$	$k_i^s=0.1$	$k_d^s=2$	$c_{31}=0.1$	$c_{32}=0.1$
$Q_{66}=1$	$R_{11}=2$	$k_p^r=2$	$k_i^r=0.1$	$c_{33}=0.1$	$c_{41}=0.5$
$R_{22}=2$	$N_P=8$	$k_d^r=2$	$k_q=0.2$	$c_{42}=0.5$	$c_{43}=0.5$
${\mathit{u}}_{max}={\left[0.15,0.2\right]}^T$	$N_c=3$	$k_r=0.2$	$R_k=10$	$c_{51}=1$	$c_{52}=1$
${\mathit{u}}_{min}={\left[-0.15,-0.2\right]}^T$		$u_d=1$	$y_e^s=2$	$c_{53}=1$	$\gamma =0.2$

. The parameters of three controllers are obtained after tuning. The principle of parameter tuning is to make the AUV converge to the desired path as quickly as possible without overshooting. Two obstacles with a radius of 5 m are set, and their center coordinates in the fixed coordinate system are [72 m, 0 m, 28 m]^T and [136 m, 0 m, 28 m]^T. In the simulation, the hydrodynamic parameters are increased by 20% from 0 s–150 s and decreased by 20% from 250 s–430 s, which can verify the robustness of the controller.

The simulation results are displayed in Figure 3, Figure 4, Figure 5 and Figure 6. Figure 3a shows the simulation results which are displayed in 3D. Figure 3b is the path following result projection in the horizontal plane. Figure 3c–d are partial zooms of horizontal obstacle avoidance. Figure 3e is the path following results projection in the vertical plane. The simulation results show that the tracking performance of the three control methods is nearly ideal. However, LOS+SMC and MPC+SMC work slightly better than LOS+PID when path switching. In addition, MPC+SMC can converge to the expected lateral error faster and more accurately so as to better achieve obstacle avoidance.

Figure 4 illustrates the path following errors. Obviously, all tracking errors converge to zero on each straight path. We can see that in order for the AUV to avoid obstacles at a safe distance, obstacle avoidance from the right is achieved by setting the expected lateral error to about 5 m when the AUV passes the first obstacle, and obstacle avoidance from the left is achieved by setting the expected lateral error to about −1 m when the AUV passes the second obstacle. When obstacle avoidance is completed, the AUV returns to the desired path by setting the expected lateral error to zero. At the same time, it can be seen that MPC+SMC controls the depth error more smoothly than the other two methods. Figure 5 shows the actual control inputs of the AUV. It is clearly visible that control signals have regular chattering. The chattering is caused by the path switching, because the path is not smooth at each waypoint. The maximum amplitude of the rudder angle is set to ±25°. When LOS+PID and LOS+SMC are adopted, the vertical rudder angle is prone to saturation when the path is switched. When the improved control method (MPC+SMC) is adopted, there is no saturation of the rudder angle, because the constraints on angular velocity are taken into account by the MPC algorithm.

In addition, the MSE of vertical rudder angle is reduced by about 40% compared with that of the traditional control methods, which is more conducive to the system stability and saves more energy. Figure 6 shows the velocities of the AUV and the stability constraints of MPC. The forward speed can converge to the expected value. However, the performance of LOS+SMC and MPC+SMC is better than that of LOS+PID because the forward speed is more stable. At the same time, it can be seen clearly that although the AUV generates certain lateral and vertical velocities due to the coupling between motions, both lateral and vertical velocities are small and convergent. When LOS+SMC is adopted, the expected angular velocities are too large during path switching, which is the reason for the saturation of the rudder angle. The angular velocity of the AUV can not only converge to the expected value but is also within the constraint range with the MPC+SMC method. Figure 6h illustrates the stability constraints of MPC.

5. Conclusions

This paper studies two important control tasks of the underactuated AUV, that is, path following and obstacle avoidance. The proposed control method improves the performance of the control system through the following measures. Firstly, in the kinematics controller, the optimal expected angular velocity is designed by using MPC, and the penalty item for obstacle avoidance is designed by using the obstacle information detected by onboard sensors, which can not only reduce the MSE and saturation of the rudder angle but also realize real-time obstacle avoidance. Secondly, the stability constraint conditions are designed, which can guarantee the stability of MPC. The prediction model in MPC adopts the linear time-varying model, which can effectively reduce the computation of the controller. The simulation is implemented in Matlab and Equation (29) is solved by the QP algorithm on a PC (CPU: Intel i5-3230M, 2.6GHz; RAM: 4GB). The average time used to calculate MPC guidance law is within 10 ms. Therefore, the controller can meet the real-time requirement of path following. Because the kinematics level is not affected by the dynamics model uncertainties, the accuracy of MPC can be guaranteed. Thirdly, in the dynamics controller, the saturation of the control input is considered. The actual control signal is designed by SMC to realize the velocity control, which can not only overcome the uncertainty of dynamics model but also ensure the stability of the system. A comparison of the improved control method (MPC+SMC) and the traditional control methods based on LOS guidance law shows that MPC+SMC can yield a superior performance. The MPC+SMC not only better realize the path following and obstacle avoidance but also reduce the MSE and saturation of the rudder angle effectively. Therefore, the MPC+SMC can be more conducive to the stability of the system and can save energy. Fourthly, the obstacle avoidance method designed in this paper is easier to calculate than other obstacle avoidance methods based on path replanning, and therefore it is more convenient for practical application. How to extend the method to solve the problem of curve path tracking and dynamic obstacle avoidance will be considered in the future.