The motion of an AUV, like that of any marine vehicle, is conveniently described in 6 degrees of freedom (DOF) with its pose specified by its position p=(x, y, z) and its orientation θ=(ϕ, θ, ψ) (in terms of Euler angles) in an Earth-fixed coordinate frame X_E-Y_E-Z_E and its linear and angular velocities v=(u, v, w) and ω=(p, q, r) specified in a body-fixed reference frame X_B-Y_B-Z_B as illustrated in Figure 1. The origin O of the body-fixed frame is chosen at the vehicle’s center of gravity (CG) and its axes are chosen to coincide with the three principal axes of inertia – longitudinal, transverse and normal. So, the linear velocity components u, v and w correspond to surge, sway and heave speeds and the angular velocity components p, q and r correspond to roll, pitch and yaw speeds.

Figure 1: 
Coordinate frames for describing AUV motion

The kinematic relationship between the pose vectors and the velocity vectors are given by

p˙=J1θv,   θ˙=J2θω

where J₁ (θ) and J₂ (θ) are 3×3 transformation matrices with elements depending on ϕ, θ, ψ (see, e.g., [30] for the details).

In this paper, we consider underwater vehicles equipped with metacentric restoring forces preventing roll and pitch motions so that the roll and pitch angles ϕ and θ and the angular speeds p and q are negligible and close to zero. Under this assumption, the reduced-order kinematic relationship is described by

Further, the dynamics of the vehicle are specified in terms of the combined velocity vector υ=(v, r) by the relationship

where M is the matrix of mass/inertia components (including the added mass/inertia), C(υ) is the matrix of Coriolis terms (including that of added mass/inertia), D(υ) is the hydrodynamic damping matrix, g is the vector of gravitational forces and moments, τ is the control input vector comprising forces and torques and d is the vector of disturbance forces/torques, mainly due to water currents. Assuming the AUV body to be symmetric about the x-y and x-z planes, and that it has a slightly positive buoyancy, we have,

where W and B are the weight and the buoyancy of the vehicle, and, m_X, m_Y, m_Z, I_Z are mass/inertia terms (including added mass/inertia), X_u, Y_v, Z_w, N_r and X_u|u|, Y_v|v|, Z_w|w|, N_r|r| are hydrodynamic damping terms, τ_X, τ_Y, τ_Z, τ_N are the control forces/torque generated by the thrusters and d_X, d_Y, d_Z, d_N are disturbance forces/torque effective along/about the surge, sway, heave and yaw directions.

Thus, combining Equations (1) and (2), we can write the mathematical description of the AUV motion in the 3D space in the form of the following nonlinear state-space model

where x=x  y  z  ψ  u  v  w  rT∈R8 is the state vector, u=τ∈R4 is the control input vector, d∈R4 is the disturbance input vector, and

Further, the output vector y=CX=x  y  z  ψT∈R4 so that =[I4     04×4].

Remark 1: The motion of an AVU described by (3) has 4 DOF and is supported by control inputs along the 4 directions. The AUV can still be maneuvered if the sway input component τ_Y is unavailable or the corresponding actuator is faulty. A vehicle in such a situation is considered to be underactuated. Several works on AUV trajectory tracking control have focused on underactuated AUVs (e.g., [27])

We wish to control the motion of an AUV for missions that re-quire tracking a constant target point or tracking a specified time-varying reference trajectory in some optimal way while satisfying all requirements including system constraints.

Among various control approaches, MPC is perhaps one of the best suited control schemes to achieve the control goal men-tioned in Section 2.2.3 for a constrained AUV system. MPC uses the system dynamics model, usually in the DT framework, to numerically optimize the control input over some time horizon so as to minimize a suitably defined cost function over that horizon. This optimization is done regularly in real time, usually at every discrete time step to update the control input to be applied to the system.

Consider a DT version of model (3), viz.,

Here, x(k), u(k), d(k) and y(k) represent the values of the signals at the discrete sampling time kT,k∈Z, where T is the sampling period. An MPC scheme meant to achieve the desired trajectory tracking by the AUV uses a cost function of the form

with lx,u,k=x-xdkQ2+u-udkR2

where Q and R are positive (semi-)definite matrices defining the stage cost function l(., ., .) and

is the stacked vector of inputs to be optimized. The MPC optimization problem to be solved at each time step k is stated as

such that

where matrices M_x and M_u in the last two constraints are so chosen that they represent xk+ik∈X and uk+ik∈U.

A basic MPC algorithm based on problem (8) is outlined below.

The cost function (7) used in the MPC algorithm can be made to approximate (5) with the choices Q=C^TC and R=0. Still, there are various difficulties in successfully achieving the control objective by implementing Algorithm 1. These include theoretical and practical concerns related to stability, robustness and implementation.

If the time horizon [t₀, t_f] of the reference trajectory is significant, covering it entirely in (7) may be computationally prohibitive. It is desired that a repeated solution of (8) with a modest value of N in (7) as outlined in Algorithm 1 ensures the asymptotic or exponential convergence of the tracking error Δx(k) = x(k) - x_d(k) to the origin or to a set around it.

Let us consider the disturbance-free dynamics

where ϕ_o (x, u) = ϕ(x, u, 0).We make the following assumption about the reference trajectory.

Assumption 1: The desired reference trajectory (x_d(k), u_d(k)) satisfies the DT state dynamics in (9) :

Under Assumption 1, the tracking error dynamics are given by

where Δu(k) = u(k) - u_d(k). A local linearization of the error dynamics about a reference point r=(x_d(k), u_d(k)) in the desired trajectory results in

where Ar=∂ϕo∂xr, Br=∂ϕo∂ur and ϑ(.,.) represents the higher order terms.

If (A_r, B_r) is stabilizable, for a Δx(k) sufficiently close to the origin, there exists a state feedback gain matrix K_r such that with u(k) = u_d(k) + K_rΔx(k), the tracking error satisfies the contractivity property

with ρ ∈ (0, 1) for some positive definite Lyapunov matrix P_r [21]. This fact can be used to construct terminal state feedback controllers together with associated invariant sets to be used as terminal state constraints in the online MPC optimization problem to guarantee stability. For a class of tracking problems, a parameterized state feedback gain and the associated invariant set may be computed and used for a range of desired state-input pairs [29]. However, computing terminal state-feedback gains and the associated invariant sets is usually computationally burdensome. As explored in [21], under the local stabilizability condition (which the AUV system satisfies), Algorithm 1 with a positive definite matrix Q in (7) and a sufficiently large horizon length N ensures that the tracking error exponentially converges to the origin. However, the theoretical bound on the required horizon length N can be quite conservative.

The AUV motion model includes the disturbance term d(t) which is used to mainly model the effect of ocean currents. This creates a difficulty in the evaluation of the cost function and in ensuring the constraints in (8). Since the bounds on the disturbance components can be estimated, we can use a robust approach to cost evaluation and constraint satisfaction. Potential robust approaches include the tube-based approach [33], disturbance-feedback-based approach (e.g., [34]), and so on. The latter deals with linear time-varying systems, and, as we shall see in Section 3.4, when problem (8) is solved using the sQP approach, nonlinear dynamics constraints are linearized about the existing state-input trajectories effectively resulting in an MPC problem for a linear-time-varying system.

The robust approaches, however, usually lead to conservative results while significantly increasing the computational burden. In our problem, the disturbance input is an input disturbance that enters the system in the same way as the control input. It can be estimated using an appropriate filter and its effect countered ef-fectively using the control input. Since the disturbance is slowly time-varying, it can be assumed to remain constant through the prediction horizon. Slightly tightened input constraints can be used to account for potential variations in the disturbance values.

Since the system model that we use is based on several simpli-fying assumptions and the model uses a number of parameters whose values may only be approximately known, there are other potential sources of uncertainties. However, the inherent robust-ness of the MPC approach due to its receding horizon implemen-tation can be trusted to handle the effects of these uncertainties.

The desired performance of the control scheme outlined in Al-gorithm 1 is achieved only under the condition that the DT dy-namics model is accurate and that the nonlinear optimization problem can be numerically solved within a fraction of the sam-pling period. An exact equivalent DT model of a continuous-time nonlinear system is not readily available. Numerical approxima-tions using Euler difference methods or RK methods are often used in applications. However, these numerical approximations are accurate only for small sampling intervals. Since we envisage the possibility of considering optimizations over longer horizon lengths, we consider a more accurate modeling of the DT dynam-ics using alternative approximations. A suitable approximator in this context is a single-layer feedforward neural-network (ff-NN) which does not involve nested nonlinear function evaluations. This simplifies and speeds up future state computations and also provides a simple analytical expression for gradient evaluations when solving the numerical optimization problem.

Extensive simulations are carried out to assess the performance of the proposed approach. Parameters specifying the dynamics of the AUV and the constraints applicable on speeds and thruster actions are mentioned in Table 1.

We obtain two ff-NNs – NN-1 and NN-2 to represent nonlinear DT dynamics for two different sampling periods – T₁ = 1s and T₂ = 0.25s. Both are trained with about 10000 samples of input-output data. Input data samples are selected randomly from the set 1.1(-π, π×GX×U) where the set [-π, π] is the range chosen for the yaw angle ψ. The factor 1.1 is used to provide some margin at the constraint boundaries. Outputs corresponding to input samples are obtained using the ode45 solver in MATLAB. Networks NN-1 and NN-2 are designed with η₁ = 48 and η₂ = 40 hidden layer neurons which use the tanh() activation function. After sufficient training, their mean squared error (MSE) performances are found to be of the order 10^-5 and 10^-6 respectively.

Table 2 gives a comparison, in terms of accuracy and computational efficiency, of model NN-2 with the popular RK-4 method (which is used, e.g., in [22], [26]). Here, the model accuracy is indicated by the MSE over all training and test samples, and, the computational efficiency is expressed in terms of the ratio of the average model function evaluation time over the average time taken by the accurate ode45 method in the same machine. While NN-2 is expectedly more accurate for this step size, what is significant is its comparative computational advantage which should remain significant (for both function evaluation and gradient evaluation) even for small step sizes for which the accuracy advantage may not be significant. Note that the RK-4 method is not stable with the time step size h = 1.

For tracking control performance evaluation, we consider an AUV mission comprising predefined time-varying trajectory tracking and free set-point tracking segments. In particular, the following phases are considered in the mission:

• 1) Descending phase: Trajectory tracking phase 1 (100s)
  xdt=2+0.2t,  ydt=1+0.05t, 
  zdt=22-20e-xdt-22+ydt-1225
• 2) Free set-point tracking phase (No fixed target time)
  xd,yd,zd=60,10,30 
• 3) Object inspection phase (Trajectory tracking phase 2 (800s))
  xdt=71-10cos⁡0.05(t-t2) + 0.01(t-t2)
  ydt=10+8sin⁡(0.05(t-t2)),  
  zd(t) = 30-0.025(t-t2)

where t₂ is the time when Phase 2 is completed.

We assume that the AUV is initially at rest at (0, 0, 0).

4.3.1 Performance in Disturbance-Free Scenario

We first consider the case without disturbances in the simulation model. In the MPC cost function, we use the cost matrix Q = diag(I₄, 0.1I₄ ) in all phases, and since the desired inputs are not specified, we mildly penalize the actual inputs with R = 10^-5I₄ in Phases 1 and 3 and R = 10^-3I₄ in Phase 2. We also look for a horizon length N that is sufficient to ensure that the predicted terminal state is close to the desired pose throughout the mission. It is found that with model NN-1 (T₁ = 1s), a horizon length of N = 10 is generally sufficient when the initial state is not very far from the corresponding reference point. In Phase 2, the target point is initially not close but to avoid using a large horizon length, we consider a virtual reference converging to the target point (60, 10, 30) and update the virtual reference in real time.

Figure 2 shows the reference and actual AUV position trajec-tories that we achieve with the MPC scheme using model NN-1 in the disturbance-free scenario. It can be seen that the MPC is able to drive the AUV closely along the desired trajectory.

Figure 2: 
AUV trajectory obtained with the MPC scheme in disturbance-free scenario, alongside the reference trajectory

Figure 3 shows the position and velocity components of the AUV state and the thruster forces in x-, y-, z- directions. It can be seen that the z-direction thruster reaches the limit during Phase 1 when the decent is steep. Similarly, the surge and sway speed limits are reached during Phase 2. Figure 4(a) shows the position tracking errors during Phase 1 and 3, and it can be seen from the figure that the tracking error is within ±0.025m in each dimension. Note that the tracking error is not relevant in Phase 2.

Figure 3: 
Position, velocity and thruster force components during AUV mission in disturbance-free scenario

Figure 4: 
Position tracking errors in Phases 1 and 3 in disturbance-free scenario: (a) with model NN-1, (b) with model NN-2

In order to assess the impact of the accuracy of the dynamics model, we also simulate the performance of the MPC scheme with model NN-2. Since NN-2 uses a sampling time of 0.25s, a horizon length of 10s would require N = 40. However, since the input can be updated quickly, we consider N = 10 as earlier. The tracking response is similar to the one obtained with NN-1. However, as we can see in Figure 4(b), which presents the position tracking errors during Phase 1 and Phase 3, the tracking errors are smaller with NN-2 than with NN-1.

This can be expected since model NN-2 is at least one order of magnitude more accurate (in terms of MSE values) than NN-1 and it also updates the control actions 4 times more frequently.

4.3.2 Performance in the Presence of Disturbances

We next consider a scenario with disturbance inputs. Disturb-ances are mainly due to water currents inside the sea and are slowing varying in time but there can be other sources too. We consider a disturbance vector of the following form:

d=8sin⁡0.1t+2v18cos⁡(0.1t-0.3)+2v28cos⁡(0.1t+0.8)+2v32sin⁡0.1t+0.5+0.05v4

where v1,v2,v3,v4 are zero-mean Gaussian random variables with unit variance.

Figure 5 shows the tracking errors in Phases 1 and 3 in the presence of disturbance inputs. The errors in Part (a) are for the scheme with NN1 and those in Part (b) are for that with NN-2. Note that the transient phase is not shown. Evidently, the error magnitudes in the presence of disturbances are larger by a factor of about 5 (compare with Figure 4). Also, the error magnitudes are lower by about 50% with NN-2 than with NN-1 for reasons mentioned above. Nevertheless, the MPC scheme is able to limit the tracking errors within about 10cm in each direction when NN-1 is used and within about 5cm when NN-2 is used.

Figure 5: 
Position tracking errors in Phases 1 and 3 in the presence of disturbances: (a) with model NN-1, (b) with model NN-2

4.3.3 Performance in the Absence of Sway Control

In this part, we explore the performance of the MPC scheme in a scenario with a faulty sway (transverse direction) actuator. Even without a fault, the sway control considered in our scheme is limited because of the limitation in sway speed and sway-direction thrust. When sway control is completely absent, the AUV is considered to be underactuated and its maneuvering ability may be restricted in some situations. We consider this situation in the presence of disturbances as mentioned in Section 4.3.2. Figure 6 shows the state and control components evolving under the MPC scheme (using NN-1) in this scenario. It can be observed that while the control inputs (except τ_Y, which is zero) and the velocities react and fluctuate to correct the effect of the disturbances that affect the system, position components vary rather smoothly with time.

Figure 6: 
The evolution of state and control components during the mission without sway control in the presence of disturbances

The plots in Figure 7 show 3D position errors (e=x-xd2+y-yd2+z-zd2 )  during Phase 3 in the three different scenarios considered (in Sections 4.3.1~4.3.3).

Figure 7: 
3D position tracking errors in various scenarios with dynamics models NN-1 and NN2

The errors in the first figure are obtained with model NN-1 and those in the second figure are obtained with model NN-2. Clearly, the presence of disturbances increases the tracking error and the actuator fault further increases it. And, because of higher accuracy and smaller sampling time, with model NN-2, errors are smaller by a factor of about 2.