Linear Parameter-Varying Model Predictive Control of AUV for Docking Scenarios

Abstract

A control system for driving an Autonomous Underwater Vehicle (AUV) performing docking operations in presence of tidal current disturbances is proposed. The nonlinear model of the vehicle has been modelled in a Linear Parameter-Varying (LPV) form. This is suitable for the design of the control system using a model-based approach. The LPV model was used for a Model Predictive Control (MPC) design for computing the set of forces and moments driving the nonlinear vehicle model. The LPV-MPC control action is mapped into the reference signals for the actuators by using a Thrust Allocation (TA) algorithm. This was based on the nonlinear models for the actuators and their position and orientation on the vehicle’s hull. The structural decomposition of MPC and TA reduces the computational burden involved in computing the control law on-line on an embedded control board. Both MPC and TA algorithms use the vehicle’s linear and angular positions, and velocities that are estimated by an LPV based Kalman Filter (KF). The proposed control system has been tested in different docking scenarios using various tidal current disturbances acting on the vehicle as an unmeasured disturbance. The simulation results show the controller is effective in controlling the AUV over the range of control scenarios meeting the constraints and specifications.

1. Introduction

The control of an Autonomous Underwater Vehicle (AUV) is considered. This has no tether and is used for large-area subsea survey or seabed mapping. The AUV operating time is limited because the power source is an internal battery, typically a lithium ion secondary battery, and its energy is limited, even though battery technology has been improved recently. Launch and recovery from a surface ship may be needed for the continuous operation of the AUV for power supply and data uploading. This is an inefficient operation for the total mission and will be reflected in the operational cost. Moreover, the recovery of the AUV, which has no tether cable, is a troublesome and dangerous offshore operation.

For the improvement of the efficiency of AUV operations, there have been many research studies on the use of subsea docking stations [1,2,3]. In this case the AUV docks to the subsea docking station and power is supplied and data are transferred through subsea cables between the docking station and the surface ship.

In Reference [4], the concept of underwater docking using AUV and ROV (Remotely Operated Vehicle) docking stations was proposed. The operation involves the AUV moving close to the standby ROV docking station, stopping and hovering in the vicinity of the ROV, and then the docking station on the ROV capturing the AUV with the docking device. When the AUV and the ROV docking station are in a separate positional relationship, the respective positions can be obtained from the surface ship, controlling the ROV docking station using an ultra-short baseline (USBL) acoustic positioning system. This transmits information to the AUV using underwater acoustic communications. However, when the AUV is close to the ROV docking station, it is important for the AUV to transition to the optimum position and orientation. The ROV docking station can then easily capture the AUV in the limited view of the underwater camera. The concept here is to install a 3D imaging sonar on the AUV to estimate the position and the attitude of the ROV docking station. The transfer can then be achieved to the optimum position and attitude using an advanced hovering control [5,6]. In the following, we introduce new trajectory and attitude control for the AUV.

After estimating the orientation of the ROV docking station with a 3D imaging sonar, the AUV transitions to a final hovering position where the ROV docking station can easily capture it. It then holds a fixed point in a predetermined orientation, and awaits capture by the ROV docking station. During cruising at a certain speed or higher, the AUV position and attitude are controlled by the main thruster and the rudder. However, during low speeds, or holding at a fixed point, the hydrodynamic action from the rudder is insufficient, so the AUV is controlled by a plurality of side thrusters that generate thrust in the vertical and horizontal directions.

In this way, it is necessary to perform control by a number of actuators including a side thruster in order to shift the position and the attitude while decelerating the speed in a relatively short distance and finally holding a fixed target position. It is necessary to consider constraints for an actuator, such as a rudder, or side thruster, including limits on rudder angle and upper limits on thrust. Model Predictive Control (MPC) is often used for the optimal control of industrial systems with hard constraints but this is a relatively nonlinear process that is faster than many applications. The solution proposed is, therefore, tailored to the application.

The equations of motion are six degrees of freedom for the AUV. These are required for model predictive control. They have nonlinear characteristics for velocity and angular velocity given fluid dynamic and Coriolis forces. The actuator characteristics such as the fins and thrusters also have non-linear characteristics. If such a nonlinear and complex motion model is used directly for model predictive control the prediction stage results in an excessive computational load. It is not in general therefore suitable for real time control for a system such as an AUV.

Different solutions have been proposed for facing the AUV control by MPC. For example, in Reference [7], the MPC is applied to the trajectory tracking of an AUV and the equation of motion is limited to the motion in the horizontal plane. It is also linearized around a constant forward speed to reduce the computational load. In [8], the MPC is applied to the trajectory tracking of an AUV, where motion is limited to the nonlinear motion in the horizontal plane. The three forces (moments) are used to control the three degrees of freedom (u, v, r) distributed models, and ways to reduce computational load are considered for nonlinear MPC problems. In both these cases, the control problem is limited to the horizontal plane.

For the docking control problem there is a combination of a tracking/path following mode and finally a dynamic positioning control mode. In this latter case at the docking position the attitude in the vertical plane, pitch and the roll must be controlled. We therefore examined the transfer of position and attitude control, and then the dynamic positioning control for the six degrees of freedom model.

To reduce the computational load, the basic configuration of the control system is divided into two modules: an MPC control module for outputting the virtual force required to follow a predetermined position, speed, and attitude, and a Thruster Allocation (TA) module for determining the command value to each actuator required to generate the virtual force [9]. The TA problem is also involved in the dynamic positioning control of a ship. The calculation load is reduced by dividing a complicated optimization problem into the two more manageable optimization problems. In addition, a Kalman filter is implemented to estimate states and disturbances, such as tidal currents.

To reduce the computational load of the MPC control module, a Linear Parameter Varying (LPV) modelling approach was adopted [10,11]. The LPV-MPC can be treated as a linear problem, although it can represent a good approximation of a nonlinear model and the system matrices are parameter dependent and change with time. As a result, a linear quadratic programming problem can be generated, so that predictive control can be performed in real-time with reasonable economy. A method to compensate for the prediction error caused by using locally linear models, representing the inherently nonlinear model, was introduced. The proposed control system has been tested in simulation considering different docking scenarios featured by several tidal current acting on the vehicle as an external unmeasured disturbance.

The optimum control of the AUV for docking under the constraint conditions is a challenging problem. Furthermore, the critical hardware development items, such as the docking devices, were developed in parallel. Therefore, for the control system design, the control algorithm was developed for the concept model AUV, and was verified based on simulation. This paper reports the development and simulation verification of this control algorithm. The paper covers the modeling in Section 2, the control system in Section 3, the simulation results in Section 4, and the finally the conclusions in Section 5.

2. AUV Model

In this section, the nonlinear model of the AUV is described. The nonlinear equations describing vehicle and actuators are discussed in Appendix A. The rigid-body dynamics, actuators, sensors, and tidal current dynamics are described below.

2.1. AUV Rigid-Body Model

The equations of motion of the vehicle involve the rigid–body dynamic terms and components related to hydrodynamic forces and moments. The model includes the three main effects including restoring forces (depending on vehicle geometry and mass), the added mass (due to forced harmonic vehicle motion induced by force/moment pressure) and the damping terms (caused by skin friction and vortex shedding). The nonlinear dynamics model of the AUV may be represented in a compact matrix form introduced in Appendix A and expressed as follows:

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

(1)

where $M$ is the mass matrix component, $C$ represents the Coriolis effects, $D$ represents the dumping effects, $g$ corresponds to restoring forces and moments, ${\tau }_A$ represents actuators thrust, $v$ is the vector of linear and angular velocities and $\eta$ is the array of linear and angular positions of the AUV. A detailed description of the elements of Equation (1) is given in Appendix A. The numerical values of the parameters are collected in Appendix B.

2.2. Actuators

The AUV is equipped with a set of actuators including fins and thrusters mounted in different positions over the vehicle hull, as shown in Figure 1.

Figure 1. AUV Conceptual Layout.

The actuators include the main thruster, side thrusters (front and rear) and fins (stern planes and rudders). They are characterized in terms of the type, the position, and the orientation with respect to the vehicle-fixed reference frame and are described below. The combination of the forces and moments provided by the actuators determine the thrust to control the vehicle according to the controller command signals. A detailed description of the actuator dynamics for the AUV is presented in Appendix A and the parameter values are collected in Appendix B.

2.3. Sensors

The AUV is equipped with a set of INS sensors to measure the pose and velocities. The sensors measure position and orientation of the vehicle with respect to the inertial reference frame, and the instantaneous linear velocities with respect to the body-fixed reference frame. A delay of 20 ms affects the sensor measurements. The accuracy of the linear position sensors is 0.005 m and the linear velocities accuracy is 0.1 m/s. The angular position measurements have an accuracy of $5.42\times {10}^{-5}$ rad., and the angular velocities accuracy is $5.42\times {10}^{-3}$ rad/s.

2.4. Tidal Current Dynamics

In this work, the vehicle is subject to underwater currents that need countering. Considering the rigid-body AUV model of Equation (1), expressed in terms of the vehicle instantaneous velocities vector v, the tidal current is introduced in the model by considering the linear speed components of this disturbance with respect to the body-fixed reference frame ${\nu }_{\mathrm{T}}={\left[u_T,v_T,w_T,p_T,q_T,r_T\right]}^T$. The relative velocity of the vehicle with respect to the tidal current can be computed as:

$${\nu }_R=\nu -{\nu }_T$$

(2)

The effect of the tidal current can be introduced in the rigid body dynamics of Equation (1) by replacing $\nu$ with ${\nu }_R$ so that:

$$M{\dot{\nu }}_R+C\left({\nu }_R\right){\nu }_R+D\left({\nu }_R\right){\nu }_R+g\left(\eta \right)={\tau }_A$$

(3)

Remark 1.

The tidal current${\nu }_T$is constant in direction and velocity (${\dot{u}}_T=\dot{v_T}=\dot{w_T}=0m/s^2$), irrotational ($p_T=q_T=r_T=0m/s$) and the vertical component of the current is neglectable ($w_T=0m/s$).

Remark 2.

The tidal current speed$V_T=\sqrt{u_T^2+v_T^2}$is bounded such that$V_T\in \left[0,0.514\right]m/s$.

Remark 3.

The constant tidal current heading orientation is${\psi }_T={\psi }_{ROV}+\beta$where${\psi }_{ROV}$is the docking station heading and$\beta \in \left[-0.1745,+0.1745\right]rad$is constant.

3. AUV Control System

The proposed control system for driving the AUV is now presented. Initially, the architecture of the controller is described. The different aspects of the controller design are then introduced.

3.1. Control System Architecture

The proposed control system structure is shown in Figure 2. The nonlinear AUV system, including sensors and actuators (blue block) as described in the previous section, is affected by the tidal current (green block) speed ${\nu }_T$ and the control system (grey block). The controller is composed of three main algorithms: a predictive control algorithm, an estimator and an allocation algorithm.

Figure 2. AUV Control System Architecture.

The Model Predictive Control (MPC, yellow block) computes the virtual control signals (forces and moments) ${\tau }_V$ by solving an optimal control problem given the estimated state vector ${\widehat{x}}_{AUV}$ and the control reference set-point signal vector $r_{SP}$. A Kalman Filter (KF, orange block) is used to estimate the AUV system state ${\widehat{x}}_{AUV}$, by using the measured output signals $y_{AUV}$ and the virtual control signals ${\tau }_V$. The Thrust Allocation algorithm (TA, red block) converts the virtual control ${\tau }_V$, to a set of signals driving the actuators $u_A$ such that these provide a set of forces and moments minimizing the difference with ${\tau }_V$ given the actuator limits and physical properties.

Predictive control methods normally involve a MPC algorithm to solve a single optimization problem, which computes the vector of future controls. This solution provides the sequence of control signals driving the actuators. It requires a single MPC design model involving the vehicle and actuator dynamics, and the actuator thruster allocation algorithm. This approach was investigated but an alternative route was taken to obtain a design that was more practical to implement.

In the following, a different control architecture was used to simplify the optimal problem by introducing two sub-problems, instead of one large optimization problem. This has the advantage that it provides a simpler (reduced number of optimization variables and control action constraints) optimization problem for the MPC. The TA policy also involves a simpler resources allocation problem that is to be solved only for the actual time instant. The controller structure avoids input and output constraints in the predictive controller, so that the MPC for this application is an unconstrained optimization problem that can be solved with a reduced computational effort with respect to other, constrained MPC problem solutions [12].

3.2. AUV Linear Parameter-Varying Model

To develop an MPC to control the AUV nonlinear dynamics, a design model permitting the prediction of the future AUV dynamics over a given future horizon is required. In this work, the design model is obtained in a Linear Parameter-Varying (LPV) form. The LPV modelling approach involves approximating a nonlinear dynamic model in a time-varying linear model form given the value of a set of measured/estimated variables. The LPV model of the AUV system involved represents the nonlinear dynamics model of Equation (6) in the following discrete time-varying state-space form:

$$x_{AUV}\left(k+1\right)=A_d\left(k\right)x_{AUV}\left(k\right)+B_d\left(k\right){\tau }_V\left(k\right)+d_d\left(k\right)$$

(4)

$$y_{AUV}\left(k\right)=C_d\left(k\right)x_{AUV}\left(k\right)$$

(5)

where the state vector at the $k$-th time instance is:

$$x_{AUV}\left(k\right)={\left[x\left(k\right),y\left(k\right),z\left(k\right),\varphi \left(k\right),\theta \left(k\right),\psi \left(k\right),u\left(k\right),v\left(k\right),w\left(k\right),p\left(k\right),q\left(k\right),r\left(k\right)\right]}^T$$

(6)

The measurement vector at the k-th time instant is:

$$\begin{array}{l}{y}_{AUV}\left(k\right)=\\ {\left[x_m\left(k\right),y_m\left(k\right),z_m\left(k\right),{\varphi }_m\left(k\right),{\theta }_m\left(k\right),{\psi }_m\left(k\right),u_m\left(k\right),v_m\left(k\right),w_m\left(k\right),p_m\left(k\right),q_m\left(k\right),r_m\left(k\right)\right]}^T\end{array}$$

(7)

The virtual control signal vector provided by the MPC for driving the AUV rigid-body dynamics at the kth time instant is:

$${\tau }_V\left(k\right)={\left[X_V\left(k\right),Y_V\left(k\right),Z_V\left(k\right),K_V\left(k\right),M_V\left(k\right),N_V\left(k\right)\right]}^T$$

(8)

considering $X_V\left(k\right),Y_V\left(k\right)$ and $Z_V\left(k\right)$ virtual forces and $K_V\left(k\right),M_V\left(k\right)$ and $N_V\left(k\right)$ virtual moments generated over the x, y and z-axis of the body-fixed reference frame. The time-varying state-space variables of the LPV system are iteratively computed with respect to the nonlinear dynamics model of Equation (1), such that:

$$A\left(t\right)=\left[\begin{array}{cc}{0}_{6x6}& -J\left(\eta \left(k\right)\right)M^{-1}(C\left(v\left(t\right)\right)+D\left(v\left(t\right)\right)\\ 0_{6x6}& -M^{-1}(C\left(v\left(t\right)\right)+D\left(v\left(t\right)\right)\end{array}\right];$$

(9)

$$B\left(t\right)=\left[\begin{array}{c}J\left(\eta \left(t\right)\right)M^{-1}\\ M^{-1}\end{array}\right];$$

(10)

$$C\left(t\right)=\left[I_{12x12}\right],d\left(t\right)=\left[\begin{array}{c}-J\left(\eta \left(t\right)\right)M^{-1}g\left(\eta \left(t\right)\right)\\ -M^{-1}g\left(\eta \left(t\right)\right)\end{array}\right]$$

(11)

are the LPV continuous-time state-space matrices discretized with respect to the controller sampling-time $Ts$ as $A_d\left(k\right)=A\left(v\left(t\right)\right)·Ts+I$, $B_d\left(k\right)=B\left(\eta \left(t\right)\right)·Ts$, $C_d\left(k\right)=C\left(t\right)$ and $d_d\left(k\right)=d\left(\eta \left(t\right)\right)·Ts$. Considering the LPV model of the vehicle, the vector of time-varying parameters is defined as $\rho \left(t\right)={\left[\eta \left(t\right),v\left(t\right)\right]}^T$ where $A_d\left(k\right)=A_d,B_d\left(k\right)=B_d\left(\rho \left(k\right)\right),C_d\left(k\right)=C_d\left(\rho \left(k\right)\right)$ and $d_d\left(k\right)=d_d\left(\rho \left(k\right)\right).$

3.3. Linear Parameter-Varying Model Predictive Control

The MPC iteratively solves a finite-horizon, optimal control problem using a prediction model of the controlled system and an estimation of the current state. When the cost function is quadratic and constraints are affine, the LPV-MPC optimization problem to be solved at each time-step can be summarized as:

$$\begin{gathered} \underset{u}{\min }{\sum }_{i=1}^{N_{p-1}}||Q\left(y_{AUV}\left(k+i|k\right)-r_{SP}\left(k\right)\right)|{|}_2^2+{\sum }_{j=1}^{N_u}||R\left(\Delta {\tau }_V\left(k+j|k\right)\right)|{|}_2^2 \\ +||P(y_{AUV}\left(k+N_p|k\right)-r_{SP}\left(k\right)|{|}_2^2 \end{gathered}$$

(12)

$$s.t.x_{AUV}(k+i+1|k)=A_d\left(k\right)x\left(k+i|k\right)+B_d\left(k\right){\tau }_V\left(k+i\right)+d_d\left(k\right)$$

(13)

$$y_{AUV}\left(k+i|k\right)=C\left(k\right)x_{AUV}(k+i|k)$$

(14)

$$x_{AUV}\left(k|k\right)={\widehat{x}}_{\mathrm{AUV}}\left(k\right)$$

(15)

$${\tau }_V\left(k+i+1\right)={\tau }_V\left(k+i\right)fori>N_u$$

(16)

$${\tau }_V\left(k+i+1\right)={\tau }_V\left(k+i\right)+\Delta {\tau }_V\left(k+i+1\right)fori\le N_u$$

(17)

$$\Delta u_V\left(k+i\right)∊D$$

(18)

$${\tau }_V\left(k+i\right)∊U$$

(19)

$$y_{AUV}\left(k+i\right)∊Y$$

(20)

$$N_p\ge N_u$$

(21)

where $P$, $Q$ and $R$ the matrices for weighting the predicted output error, input rate and final output value (terminal weight), respectively, $y_{AUV}\left(k+i|k\right)$ is the output predicted at the $i$-th prediction step, $r_{SP}\left(k\right)$ is the reference output considered constant over the prediction/control horizon, $\Delta {\tau }_V\left(k+j|k\right)$ and ${\tau }_V\left(k+j|k\right)$ are the predicted control input-rate and input-magnitude, respectively, $N_p$ is the prediction horizon and $N_u$ is the control horizon, $D$, $U$ and $Y$ are the convex sets on input rate, input and controlled output, respectively.

A standard LPV-MPC problem involves the assumption that the prediction model can be assumed constant over the prediction horizon, so that the model should match the real system for the first prediction step. Because the control action computed by MPC is mapped into the actuators control signals space by TA, there would be a mismatch between virtual forces and moments computed by MPC and real control forces and moments driving the AUV. This approximation introduces some sub-optimality in the solution, but enables the computational effort to be reduced, particularly with respect to more expensive nonlinear optimal control policies (e.g., nonlinear MPC). However, the controller performance can degrade because of the iterative introduction of an error in the prediction. To compensate for this problem, it is possible to iteratively evaluate the prediction of the plant model at least for the first prediction step.

The prediction error $e_p\left(k\right)$ can be iteratively computed by using the previous control effort so that,

$$e_p\left(k\right)=\widehat{x}\left(k\right)-\left[A_d\left(k-1\right){\widehat{x}}_{AUV}\left(k-1\right)+B_d\left(k-1\right){\tau }_V\left(k-1\right)+d_d\left(k-1\right)\right]$$

(22)

where ${\widehat{x}}_{AUV}\left(k\right)$ is the state-estimate (provided by the Kalman filter), ${\widehat{x}}_{AUV}\left(k-1\right)$ is the state-estimate at the previous time step, ${\tau }_V\left(k-1\right)$ is the last computed control signal, $d_d\left(k-1\right)$ is the last computed disturbance signal and $A_d\left(k-1\right),B_d\left(k-1\right)$ are the matrices describing the model at the previous time instant. The prediction error, computed as in Equation (22), can be introduced in the prediction model by updating the disturbance vector $d_d\left(k\right)$ iteratively, so that at each time instant it will be replaced by:

$$\overline{d}\left(k\right)=d_d\left(k\right)+e_p\left(k\right)$$

(23)

with $d_d\left(k\right)$ the original disturbance vector. To reduce the computational complexity of the controller, the LPV-MPC problem can be represented as a condensed Quadratic Programming (QP) problem that can be implemented and solved on an embedded platform:

$$\underset{z}{\min }\frac{1}{2}{z}^{\prime }{H}_{k}z+{\gamma }_k^{\prime }{F}_k{}^{\prime }z$$

(24)

$$s.t.G_{k}z\le h_k{\gamma }_k+w_k$$

(25)

where $z$ is the vector of optimization variables, ${\gamma }_k$ is the vector of MPC parameters belonging to a given set of interest, $H_k$ is a symmetric and positive definite matrix, $F_k$ is the linear cost–term matrix and $G_k$, $h_k$ and $w_k$ are the terms defining constraints on input and output.

3.4. Thrust Allocation

The TA algorithm maps the set of virtual forces and moments ${\tau }_V\left(k\right)$ into the space of actuator set-points $u_A\left(k\right)$. The mapping is computed, at the controller sample times, given the instantaneous value of velocities (linear and angular) and orientation of the vehicle, also the orientation of the thrusters and the fins mounted on the vessel’s hull. The TA problem is formulated as a quadratic programming optimization problem with equality constraints:

$$\underset{u}{\min }{u}_c{}^{T}W{u}_A+u_c{}^{T}h$$

(26)

$$s.t.{\tau }_R\left(t\right)=G\left(t\right)u_A$$

(27)

$$u_A{}^m<u_A<u_A{}^M$$

(28)

where $W$ is the weighting matrix on the quadratic term, $h$ is the linear term, the vector of optimization variables grouping the actuator set-point values (main thruster, rudders and stern-planes, front and rear side-thrusters) is:

$$u_A={\left[u_M,{\delta }_{r1},{\delta }_{r2},{\delta }_{s1},{\delta }_{s2},u_{R1},u_{R2},u_{R3},u_{R4},u_{F1},u_{F2},u_{F3},u_{F4}\right]}^T$$

(29)

and $u_A{}^m$ and $u_A{}^M$ are the vectors of minimum and maximum admissible values of the optimization variables (actuator saturations), respectively. The $G\left(t\right)$ is the matrix representing a combination of optimization variables combining actuator forces and moments according their position on the vessel hull and vehicle’s pose:

$${\tau }_V={\left[X_V-X_A,Y_V-Y_A,Z_V-Z_A,K_V-K_A,M_V-M_A,N_V-N_A\right]}^T$$

(30)

is the vector of reference moment and forces. It is computed considering the difference between virtual forces and moments ($X_V,Y_V,Z_V,K_V,M_V,N_V$) provided externally (from the MPC) and the effort provided by the interaction between actuators and the underwater environment, according to the vehicle’s layout and dynamics ($X_A,Y_A,Z_A,K_A,M_A,N_A$). These last terms are given by the resultant of the set of actuators, so that:

$$X_A=0$$

(31)

$$Y_A=\frac{1}{2}{Y}_{uu\delta r}\left(-uv\right)+\frac{1}{2}{Y}_{urf}ur$$

(32)

$$Z_A=\frac{1}{2}{Z}_{uu\delta s}\left(-uw\right)+\frac{1}{2}{Y}_{urf}uq$$

(33)

$$K_A=0$$

(34)

$$M_A=-\frac{1}{2}{Y}_{urf}\left(-uw\right)+\frac{1}{2}{M}_{uqf}uq$$

(35)

$$N_A=\frac{1}{2}{N}_{uu\delta r}\left(-uv\right)+\frac{1}{2}{M}_{uqf}ur$$

(36)

The matrix $G$ is a time-varying configuration matrix defined as:

$$G=\left[\begin{array}{cccccc}{G}_{11}& 0& 0& 0& 0& G_{s\left(1\right)}\\ 0& G_Y& G_Y& 0& 0& G_{s\left(2\right)}\\ 0& 0& 0& G_z& G_z& G_{s\left(3\right)}\\ 0& G_k& G_k& G_k& G_k& G_{s\left(4\right)}\\ 0& 0& 0& G_m& G_m& G_{s\left(5\right)}\\ 0& G_n& G_n& 0& 0& G_{s\left(6\right)}\end{array}\right]$$

(37)

with

$$G_{11}=-0.46u\rho D_0^3/n_{k-1}+0.66\rho D_0^4\mathrm{if}\frac{n_{k-1}}{u}>=0$$

(38)

$$G_{11}=-0.46\rho D_0^4\mathrm{if}\frac{n_k-1}{u}<0$$

(39)

$$G_Y=u^{2}0.25Y_{uu\delta r}$$

(40)

$$G_z=u^{2}0.25Z_{uu\delta s}$$

(41)

$$G_k=K_{uu\delta r}{u}^{2}0.25$$

(42)

$$G_m=u^{2}0.25Y_{urf}$$

(43)

$$G_n=u^{2}0.25N_{uu\delta r}$$

(44)

$$\begin{gathered} G_s={\left[G_{s\left(1\right)},G_{s\left(2\right)},G_{s\left(3\right)},G_{s\left(4\right)},G_{s\left(5\right)},G_{s\left(6\right)}\right]}^T \\ =\left[\begin{array}{cccccccc}0& 0& 0& 0& 0& 0& 0& 0\\ -c\left(\alpha \right)& -c\left(\alpha \right)& c\left(\alpha \right)& c\left(\alpha \right)& -c\left(\alpha \right)& -c\left(\alpha \right)& c\left(\alpha \right)& c\left(\alpha \right)\\ -c\left(\alpha \right)& c\left(\alpha \right)& c\left(\alpha \right)& -c\left(\alpha \right)& -c\left(\alpha \right)& c\left(\alpha \right)& c\left(\alpha \right)& -c\left(\alpha \right)\\ -d& d& -d& d& -d& d& -d& d\\ D_{F}c\left(\alpha \right)& -D_{F}c\left(\alpha \right)& -D_{F}c\left(\alpha \right)& D_{F}c\left(\alpha \right)& -D_{R}c\left(\alpha \right)& D_{R}c\left(\alpha \right)& D_{R}c\left(\alpha \right)& -D_{R}c\left(\alpha \right)\\ -D_{F}c\left(\alpha \right)& -D_{F}c\left(\alpha \right)& D_{F}c\left(\alpha \right)& D_{F}c\left(\alpha \right)& D_{R}c\left(\alpha \right)& D_{R}c\left(\alpha \right)& -D_{R}c\left(\alpha \right)& -D_{R}c\left(\alpha \right)\end{array}\right] \end{gathered}$$

(45)

where $c\left(\alpha \right)=\cos \left(\alpha \right)$, and $\alpha =45$ degrees is the angular position of the thrusters with respect to the body-fixed reference frame.

Let $u_c\left(k\right)$ represent the actuator set-points computed at the actual iteration of the algorithm, $u_c\left(k-1\right)$ the signals provided to the actuators at the previous time step and $\Delta u_c{}^m$ and $\Delta u_c{}^M$ the minimum and maximum values on the rate of change of the computed signals that define the constraints. These constraints were introduced in the TA problem above. Iteratively evaluating the value of the rate constraints, the magnitude constraints may be computed as:

$$u_{c_i}{}^m\left(k\right)=\max \left(u_{c_i}{}^m,u_{c_i}\left(k-1\right)-\Delta u_{c_i}{}^m\right)$$

(46)

$$u_{c_i}{}^M\left(k\right)=\min \left(u_{c_i}{}^M,u_{c_i}\left(k-1\right)+\Delta u_{c_i}{}^M\right)$$

(47)

where the index i indicates the i-th actuator set-point signal and the related magnitude and rate constraint values.

3.5. Kalman Filter

A Kalman Filter (KF) has been considered to estimate the vehicle state vector. Because of the hard nonlinearities in the AUV dynamics, a Linear Parameter-Varying (LPV) version of the Kalman filter was developed to iteratively compute the angular velocities of the vehicle. The LPV-KF improves the standard linear KF by updating the state–space model of the system given the last measured value of the time-varying parameters (measured or estimated) that defines the system’s behavior [13,14,15]. The LPV-KF algorithm consists of the following steps:

$$S\left(k\right)=C_d\left(k\right)P_{KF}\left(k|k-1\right)C_d^T\left(k\right)+R_{KF}$$

(48)

$$L\left(k\right)=P_{KF}\left(k|k-1\right)C_d^T\left(k\right)S^{-1}\left(k\right)$$

(49)

$${\tilde{y}}_{AUV}\left(k\right)=y_{AUV}\left(k\right)-C_d\left(k\right){\widehat{x}}_{AUV}(k|k-1)$$

(50)

$${\widehat{x}}_{AUV}\left(k|k\right)={\widehat{x}}_{AUV}\left(k|k-1\right)+L\left(k\right){\tilde{y}}_{AUV}\left(k\right)$$

(51)

$$P_{KF}\left(k|k\right)=P_{KF}\left(k|k-1\right)-L\left(k\right)S\left(k\right)L^T\left(k\right)$$

(52)

$${\widehat{x}}_{AUV}\left(k+1|k\right)=A_d\left(k\right){\widehat{x}}_{AUV}\left(k|k\right)+B_d\left(k\right){\tau }_V\left(k\right)+d_d\left(k\right)$$

(53)

$$P_{KF}\left(k+1|k\right)=A_d\left(k\right)P_{KF}\left(k|k\right)A_d^T\left(k\right)+Q_{KF}$$

(54)

where matrices $R_{KF}$ and $Q_{KF}$ describe the variances of the measurement and the process noise terms. In the current problem they are assumed constant and defined during the algorithm set-up. The covariance matrix $P_{KF}$ and the system states are evaluated on-line iteratively.

4. Simulation Result

In this section, simulation results are presented. The AUV represented by the nonlinear dynamic model is driven to the docking position, where the docking station can catch it. The AUV model, the environment operating scenario and the control system was developed within the MATLAB/Simulink environment. The set of different simulation modules, functions and subsystems are described in Section 3 and Section 4 and in Appendix A, using the parameters collected in Appendix B.

The effect of environmental conditions can change when performing such a task, and in the following two sets of simulation results are given. The first test considers an environmental scenario with no tidal current, so that the vehicle is driven in ideal conditions. The second test considers the AUV performing the docking task in the presence of a tidal current with a speed $V_T=0.514$ m/s and oriented with an angle $\beta =-0.1745$ rad with respect to the final docking position heading angle. The control system used in the following has the tuning parameters defined as in Table 1.

Table 1. AUV Controller Tuning Parameters.

AUV Control System Tuning Parameters
Parameter	Symbol	Value
Prediction horizon	$N_p$	$10$
Control horizon	$N_u$	$1$
Control rate weights	$R$	$\left[1,1,1,0.1,0.1,0.1\right]$
Controlled output weights	$Q$	$\left[0.1,0.1,0.1,0.01,0.01,0.01\right]$
Terminal weights	$P$	$\left[2\times {10}^3,3\times {10}^7,{10}^8,{10}^7,5\times {10}^7,{10}^7\right]$

4.1. No Current Test

The results of the first test, without any tidal current acting on the vehicle, are first presented. The objective of this test was to evaluate the performance of the AUV when moving to the docking position in zero current ideal conditions. The measured linear and angular positions of the controlled vehicle during the test are shown in Figure 3. This result shows the ability of the control system to drive the vehicle to the docking station meeting point in terms of linear and angular positions. The steady-state tracking errors in vertical position and in the pitch-angle are due to the mismatch between the actuators’ nonlinear model and the LPV design model. The mismatch causes an error in mapping the MPC control action into the actuator control signals space. Despite this mismatch, the performance of the controlled system satisfies the design specification.

Figure 3. No current test: controlled variables (blue plot) with respect to the target position (dashed black plot).

The control actions driving the AUV actuators and computed by the TA policy are shown in Figure 4 that shows the main thruster control signal with respect to the saturation and backlash characteristics of the actuator. This reveals that the control signal managed by the TA satisfies the saturation (±2000 rpm) limit values and is adjusted to values (±400 rpm) due to the backlash. The main thruster control signal jumps between the limits of the backlash zone (±400 rpm), as defined in the control policy. When the MPC controller computes a large control effort to be provided by this thruster, the TA policy maps it into a control signal satisfying the saturation max/min constraints (e.g., at time $t=5$ s).

Figure 4. No current test: main thruster control signal (blue plot) with respect to the saturation and backlash values (dashed red plot).

Figure 5 shows the signals controlling the AUV fins, as they are driven almost symmetrically by the TA, such that for the rudders the control signal trajectory is different only for $t=8$ s, whereas for the stern planes the control signals are coincident. Furthermore the fin controls satisfy the related constraints. Finally, in Figure 6 and Figure 7 the control signals are shown to drive the side thruster within the saturation limits. The control policy correctly drives the thrusters, despite the nonlinearities and constraints for the actuators. The fins and side thruster results show how the control system is able to generate the actuator control signals that satisfy the specified constraints.

Figure 5. No current test: fins control signals (blue and green plot) with respect to the saturation values (dashed red plot).

Figure 6. No current test: rear side thrusters’ control signals (blue plot) with respect to the saturation values (dashed red plot).

Figure 7. No current test: rear side thrusters’ control signals (blue plot) with respect to the saturation values (dashed red plot).

4.2. Max Current Test

In the following the control system is tested by driving the AUV as before, but introducing the effect of the tidal current acting on the vehicle movement. The dynamics have been set to provide the worst case operating conditions (according to the specifications provided) by considering the maximum angular orientation ($\beta =-0.1745$ rad) with respect to the final docking position and the maximum current speed ($V_T=0.514$ m/s) acting on the opposite direction with respect to the vehicle’s orientation. The performance is shown in Figure 8, which reveals the variables controlled with respect to the set-point positions. Compared with the previous results, the performance indicates the effect of the current that increases oscillations. Despite this effect, the control system has the ability to control the system in this worst case scenario, maintaining the settling time and the steady-state performance. Figure 9 shows the main thruster control signal, where the current disturbance effect includes the additional oscillations.

Figure 8. Max current test: controlled variables (blue plot) with respect to the target position (dashed black plot).

Figure 9. Max current test: main thruster control signal (blue plot) with respect to the saturation and backslash values (dashed red plot).

Figure 10 shows the fin control signals. Compared to the previous fin controls, the stern plane signals are particularly affected by the tidal current, due to their effect on the AUV dynamics. Finally, in Figure 11 and Figure 12 the side thrusters’ performance is shown. In this control scenario, the constraints are satisfied, and the control signals accommodate the saturation and backlash. The stabilization of the vehicle in the docking position satisfies the design specifications, with limited oscillations within the accuracy range of the sensors.

Figure 10. Max current test: fins control signals (blue and green plot) with respect to the saturation values (dashed red plot).

Figure 11. Max current test: rear side thrusters’ control signals (blue plot) with respect to the saturation values (dashed red plot).

Figure 12. Max current test: rear side thrusters’ control signals (blue plot) with respect to the saturation values (dashed red plot).

Due to the presence of the current forces, these control signals are more aggressive. In fact, when the vehicle reaches zero forward speed, only the side thrusters and main thruster are used to maintain the required positions opposing the current forces.

5. Conclusions

The modelling and control of an Autonomous Underwater Vehicle (AUV) in a docking task was presented. The nonlinear dynamics model of the AUV was developed for the vehicle dynamics, actuators and sensor characteristics, and the effect of tidal currents acting on the vehicle was treated as an unmeasured disturbance. A control system based on an optimal predictive constrained control design approach was developed [16].

The control system is composed of three major subsystems. A Thrust Allocation (TA) algorithm subsystem was based on the models of the actuators. A Model Predictive Control (MPC) block was designed using a Linear Parameter-Varying (LPV) model of the AUV derived from the nonlinear model of the vehicle. A time varying Kalman Filter (KF) was used to estimate the state of the system. The control system performance was tested by driving the AUV over a set of control scenarios featuring different tidal current disturbance conditions. The results illustrated that the proposed controller could control the vehicle effectively in the docking task. Further developments involve the integration of the proposed control system within an advanced path planning policy that can minimize energy while approaching the docking station.

Mitsubishi Heavy Industries is also developing the AUV prototype for a docking technology demonstration based on the concept shown in Figure 1. This is in parallel with this study, which is called “Naminow-D”. The “Naminow-D” study has the addition of side thrusters and docking devices to the existing mono-axial propulsion AUV “Namiow”. The side thrusters and docking devices, including the underwater wireless power receiver, are installed externally to minimize the development cost. Figure 13 shows the “Naminow-D” under functional testing in the water tank.

Figure 13. “Naminow-D” docking technology demonstrator.

Future studies will require physical parameter modifications, changes to the control software for implementation into embedded computer systems, and the sea trials, including comparison with simulation results.