Integrated approach to AUV docking based on nonlinear offset-free model predictive control

Abstract

This study proposes an integrated docking approach for autonomous underwater vehicle (AUV), which directly generate control force and torque from visual guidance features to drive precise AUV docking. Accurate docking control has been constructed as nonlinear offset-free model predictive control (OFNMPC) problem to effectively deal with external disturbances, model mismatch, and various other constraints. For the problem of state and disturbance estimation required by OFNMPC, observers have been presented based on the extended Kalman filter (EKF) and moving horizon estimation (MHE). A visual observation model is proposed so that the observer can directly use visual measurements. OFNMPC was integrated with EKF and MHE, and the proposed OFNMPC-EKF and OFNMPC-MHE docking approaches were applied to an actual AUV model. The simulation results show that the proposed approaches could drive the AUV to complete accurate docking along a reasonable trajectory based on visual measurements. The navigation, guidance, and control functions of docking were effectively integrated. It was observed that in the presence of external disturbances and measurement noise, OFNMPC-EKF has better robustness.

Introduction

The transition of an autonomous underwater vehicle (AUV) from a state of free travel to being physically connected to another station or platform is called docking. ¹ AUV can dock to underwater station, such as standalone docking station (DS) carrying large capacity energy or underwater network node, to get energy supplement. This enables the AUV to reside and operate underwater for long durations. In addition, automatic recovery can be realized based on docking technology, and an AUV can become an automatic tool for larger surfaces or underwater vehicles. Owing to the significance of docking technology in the expansion of AUV applications, it has become a research hotspot in the field of AUV technology since the 1990s.

The fundamental elements of AUV docking are guidance, navigation and control components. ² The navigation integrates sensor information to estimate the state required for docking guidance and control, such as, the position, attitude and velocity of the AUV relative to the DS. A large number of filtering algorithms are used for AUV integrated navigation, such as, the extended Kalman filter (EKF),^3,4 unscented Kalman filter (UKF), ⁵ particle filter (PF), ⁶ Sum of Gaussian (SoG) filter, ⁷ and fuzzy Q-learning. ⁸ Navigation sensors typically include Doppler velocity logs (DVL), inertial navigation systems (INS), and pressure sensors. The position of an AUV relative to the DS is typically obtained using acoustic equipment or vision methods.^3,9–13 The vision-based approach extracts guidance features from underwater images, based on which, the position and attitude of an AUV are estimated. The update rate and accuracy of visual positioning are higher and it is typically used in the terminal stage of docking.

According to the navigation information, an AUV uses a certain strategy to guide and control itself into the DS. In general, the guidance law generates the motion target or feasible docking trajectory of the AUV, and the controller executes the command or tracks the trajectory to drive the AUV to the DS. Typical guidance laws include, pure pursuit, ¹⁴ line-of-sight (LOS), ¹⁵ and optimal control. ¹⁶ Some studies have considered the effects of ocean currents.^17,18 The methods of docking trajectory planning include inverse dynamics in the virtual domain (IDVD), ¹⁹ geometric analysis, ²⁰ and curve fitting. ²¹ Docking control methods include a variety of improved PID controls,^17,22,23 sliding mode control, ²⁴ backstepping control, ²⁰ adaptive robust control law, ²⁵ model-free robust control, ²⁶ and neural network adaptive controller. ²⁷

However, only a few studies have focused on a unified docking approach. This makes the systematic design and integral optimization of AUV docking difficult. Model predictive control (MPC) is an architecture that integrates guidance and control. The predicted state and input trajectory are generated by solving the optimization problem and the first item of the input trajectory is applied to the controlled plant.^28,29 By constructing a constraint optimization problem, MPC can deal well with various constraints, such as, actuator ability, position and field of view (FOV) limits of the AUV in the docking process. ³⁰ There have been some studies on the application of MPC to AUV docking.^31–33 However, the AUV has strong coupling, high nonlinearity, and parameter uncertainty; therefore, it is difficult to obtain an accurate prediction model. ³⁴ To solve the problems of model mismatch and external disturbance, nonlinear offset-free model predictive control (OFNMPC) has been proposed.^35–39 OFNMPC uses an observer to estimate the state and disturbance. According to the disturbance estimation and reference, an equilibrium problem is constructed, which is added to the optimization problem to realize zero-offset control. OFNMPC has been applied to the docking of AUV in two previous studies.^40,41Both the studies assume that the navigation system can provide direct position and attitude measurements. Intuitively, the observer function of OFNMPC can be extended such that it can directly use visual measurements to estimate the state and disturbance of the docking process. A method for camera pose estimation based on the EKF has been proposed ⁴² ; however, it does not include the dynamics model and cannot be used to estimate the disturbance. Compared with the EKF, the moving horizon estimation (MHE) can directly use an exact nonlinear model,^43,44 and can be considered to realize the required observer.

Motivated by the aforementioned overviews, the aim of this study is to propose an integrated approach to AUV docking based on OFNMPC. The navigation, guidance, and control modules of docking were integrated into the framework, as shown in Figure 1. In the presence of measurement noise, external disturbances and model mismatch, the proposed approach should drive the AUV to achieve accurate docking and various constraints in the docking process can be met simultaneously. The approach is planned to be applied to an AUV driven by multiple propellers, which was developed for the scientific investigation of ocean trenches.

Figure 1.

Typical AUV docking architecture (part enclosed in the blue dotted line was integrated using the proposed approach).

The main contributions of this study are summarized as follows: (1) A docking approach based on OFNMPC is proposed, which can effectively deal with external disturbances, model mismatch, and multiple constraints. (2) The OFNMPC observer was realized using EKF and MHE. The visual guidance features were fused directly by the observer to estimate the state and disturbance of the docking process. (3) A real multi-propellers AUV model was used as the controlled plant, and the effectiveness and robustness of the proposed approaches were verified through simulations in multiple scenarios. (4) The effects of docking in OFNMPC-EKF and OFNMPC-MHE were compared, and the results were analyzed.

The remainder of this paper is organized as follows: Section 2 introduces the AUV docking system and describes the problem of offset-free docking. Section 3 presents the design methods of OFNMPC-EKF and OFNMPC-MHE. The simulation results and analysis are presented in Section 4. Finally, the conclusions are presented in Section 5.

Problem statement

The docking system included a fully actuated AUV and DS placed on the seafloor. The AUV had a depth rating of 11,000 m and was developed for scientific investigation of ocean trenches and long-term residence. Seven thrusters were installed on the AUV, as shown in Figure 2. The AUV had six degrees-of-freedom of motion. The main parameters of the AUV are listed in Table 1. Six underwater lights were symmetrically installed around the entrance of the DS to obtain significant visual guidance features. A camera was installed on the AUV to capture underwater images. A special visual processing module was used to extract the point features from underwater images. In addition, the AUV was equipped with INS and DVL and the linear and angular velocities of the AUV could be measured.

Figure 2.

AUV propulsion configuration.

Table 1.

Primary parameters of the AUV.

Items	Parameter
Length (m)	3.5
Width (m)	1.1
Height (m)	0.7
Weight in air (kg)	1200
Depth rating (m)	11,000

It is convenient to define the DS coordinate frame and body-fixed reference frame as indicated in Figure 3. In addition, a camera coordinate system is needed for the analysis of visual information. The DS coordinate system $\{d\}=(x,y,z)$ has the origin $O_d$ located at the center of the DS entrance, and the x-axis is along the longitudinal axis direction of DS. The body-fixed reference frame $\{b\}=(x_b,y_b,z_b)$ with origin $O_b$ is a moving coordinate frame, which is fixed to the AUV, $O_b$ is located at the buoyancy center of the AUV. Additionally, the camera coordinate frame $\{c\}=(x_c,y_c,z_c)$ is fixed on the AUV, and the position of its coordinate origin $O_c$ in $\{b\}$ is a constant vector ${}^b{\mathit{p}}_c$ (the superscript b indicates that the position is defined in $\{b\}$ ). All the definitions of the coordinate frames are in accordance with the right-hand rule. The velocity vector of AUV in $\{b\}$ is defined as $\mathit{\nu }=[u,v,w,p,q,r{]}^T$ , and the position and attitude in $\{d\}$ are $\mathit{\eta }=[x,y,z,\varphi ,\theta ,\psi {]}^T$ .

Figure 3.

Coordinate system for AUV docking.

A The AUV approaches the DS from a distance and docks after recognizing the underwater light. This study focuses on the docking stage. In the case of docking to a static DS, the goal is that the AUV reaches the entrance of the DS and its attitude is consistent with that of the DS. The guide cover at the entrance of the DS was designed to be funnel shaped to provide a fault-tolerant space to the AUV for docking. If the arrival deviation of the AUV is within the allowable range, it can be regarded as a successful docking. The allowable deviations in the $y_d$ direction and heading are defined as $\left|y_{\mathrm{e}}\right|$ and $\left|{\psi }_{\mathrm{e}}\right|$ respectively, as shown in Figure 4. $\left|z_e\right|$ is the allowable deviation in the $z_d$ direction. A range $x_e$ in the $x_d$ axis direction is defined. When the AUV is within $x_e$ and satisfies $\left|y_e\right|$ , $\left|z_e\right|$ , and $\left|{\psi }_e\right|$ , the AUV sails forward into the DS. The allowable ranges are listed in Table 2. The allowable deviations compared with the AUV size in Table 1 are very small, and AUV docking requires almost zero-offset control.

Figure 4.

Schematic of docking allowable deviations in the horizontal plane.

Table 2.

Allowable range of docking deviations.

Parameter item	Parameter value
$x_{\mathrm{e}}$ (m)	[−1, 0]
$\left|y_{\mathrm{e}}\right|$ (m)	0.2
$\left|z_e\right|$ (m)	0.2
$\left|{\psi }_e\right|$ (deg)	10

The docking process is affected by the external disturbances and measurement noise. The dynamic model of the AUV used for the design is inaccurate; therefore, the AUV docking task includes the following three aspects.

1. The state and disturbance of the docking process can be effectively estimated based on the measurements with noise.

2. Steering the AUV to the DS accurately to meet the allowable deviation requirements, even if there are external disturbances and model mismatch.

3. The trajectory of AUV reaching DS is reasonable and economical.

Docking approach design

In this section, the proposed docking approach is introduced. First, the architecture of the docking scheme is described briefly. The designs of the OFNMPC and the two observers are explained. Finally, the method to integrate the OFNMPC and observer is proposed.

Docking scheme considerations

Figure 5 shows the architecture of the docking approach, which is primarily composed of the OFNMPC and observer (portion in blue). The velocity measurements of the sensors and visual features extracted from the images are provided to the observer. Combined with the control input from the OFNMPC, the state and external disturbances of the system are estimated by the observer. The OFNMPC outputs the force and torque commands to complete the closed-loop control. Our study does not include control allocation; however, some methods can be applied. ⁴⁵

Figure 5.

Architecture of the docking approach.

Nonlinear offset-free model predictive control

Prediction model

The dynamics of an AUV are highly nonlinear and coupled and its vector representation of the nonlinear equations of motion can be described as:

$$\mathit{M}\stackrel{·}{\mathit{v}}+\mathit{C}(\mathit{\nu })\mathit{\nu }+\mathit{D}(\mathit{\nu })\mathit{\nu }+\mathit{g}({\mathit{\eta }}_2)={\mathbf{\tau }}_c,$$

(1)

where ${\mathit{\eta }}_2=[\varphi ,\theta ,\psi {]}^{\mathrm{T}}$ , ${\mathbf{\tau }}_c=[{\tau }_X,{\tau }_Y,{\tau }_Z,{\tau }_K,{\tau }_M,{\tau }_N{]}^T$ is the control force and moment vector. The specific forms of matrices $\mathit{M}$ , $\mathit{C}(\mathit{\nu })$ , $\mathit{D}(\mathit{\nu })$ and $\mathit{g}({\mathit{\eta }}_2)$ are described in Appendix A.

The kinematic model of an AUV can be written in concise matrix form as

$$\stackrel{·}{\mathit{\eta }}=\mathit{J}({\mathit{\eta }}_2)\mathit{\nu }.$$

(2)

The expressions for $\mathit{J}({\mathit{\eta }}_2)$ is based on the Euler angle representation as:

$$\mathit{J}({\mathit{\eta }}_2)=\left[\begin{array}{cc}\mathit{R}({\mathit{\eta }}_2)\hfill & \hfill \\ \multicolumn{1}{c}{}& \mathit{L}({\mathit{\eta }}_2)\hfill \end{array}\right],,$$

(3)

where $\mathit{R}({\mathit{\eta }}_2)$ and $\mathit{L}({\mathit{\eta }}_2)$ are the translational and rotational rotation matrices, respectively. The specific form of $\mathit{R}({\mathit{\eta }}_2)$ is:

$$\mathit{R}({\mathit{\eta }}_2)=\left[\begin{array}{ccc}\cos \psi \cos \theta \hfill & -\sin \psi \cos \varphi +\cos \psi \sin \theta \sin \varphi \hfill & \sin \psi \sin \varphi +\cos \psi \cos \varphi \sin \theta \hfill \\ \multicolumn{1}{c}{\sin \psi \cos \theta }& \cos \psi \cos \varphi +\sin \varphi \sin \theta \sin \psi \hfill & -\cos \psi \sin \varphi +\sin \theta \sin \psi \cos \varphi \hfill \\ \multicolumn{1}{c}{-\sin \theta }& \cos \theta \sin \varphi \hfill & \cos \theta \cos \varphi \hfill \end{array}\right]$$

(4)

and $L({\eta }_2)$ is described by (A5) in Appendix A.

Simplifying the dynamics model in equation (1), ignoring $\mathit{C}(\mathit{\nu })$ , $\mathit{g}({\mathit{\eta }}_2)$ , and the non-diagonal elements of $\mathit{D}(\mathit{\nu })$ , we obtain the following simplified equation:

$$\mathit{M}\stackrel{·}{\mathit{v}}+{\mathit{D}}_{\mathrm{diag}}(\mathit{\nu })\mathit{\nu }={\mathbf{\tau }}_c.$$

(5)

The surge, sway, heave, and yaw, to be controlled by the OFNMPC are selected. The roll and pitch are controlled near zero using conventional controllers, which is reasonable if the DS is properly arranged on the seabed. The status x and input u of the OFNMPC are defined as follows:

$$\begin{array}{c}\mathit{x}={[u,v,w,r,x,y,z,\psi ]}^{\mathrm{T}}\hfill \\ \mathit{u}={[{\tau }_X,{\tau }_Y,{\tau }_Z,{\tau }_N]}^{\mathrm{T}}\hfill \end{array}$$

(6)

The nominal velocity vector ${\mathit{\nu }}_0=[u_0,0,0,0,0,0{]}^T$ was selected to linearize equation (5), where $u_0$ is the selected nominal surge velocity. The prediction model for docking control is obtained, and its dynamics part is as follows:

$$\begin{array}{c}\left[\begin{array}{cccc}m-X_{\stackrel{·}{u}}\hfill & 0\hfill & -X_{\stackrel{·}{w}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& m-Y_{\stackrel{·}{v}}\hfill & 0\hfill & m{x}_g-Y_{\stackrel{·}{r}}\hfill \\ \multicolumn{1}{c}{-X_{\stackrel{·}{w}}}& 0\hfill & m-Z_{\stackrel{·}{w}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& m{x}_g-Y_{\stackrel{·}{r}}\hfill & 0\hfill & I_z-N_{\stackrel{·}{r}}\hfill \end{array}\right]\left[\begin{array}{c}\stackrel{·}{u}\hfill \\ \multicolumn{1}{c}{\stackrel{·}{v}}\\ \multicolumn{1}{c}{\stackrel{·}{w}}\\ \multicolumn{1}{c}{\stackrel{·}{r}}\end{array}\right]\hfill \\ -\left[\begin{array}{cccc}{X}_u{u}_0+X_{u\left|u\right|}\left|u_0\right|\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& Y_v{u}_0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & Z_w{u}_0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & N_r{u}_0\hfill \end{array}\right]\left[\begin{array}{c}u\hfill \\ \multicolumn{1}{c}{v}\\ \multicolumn{1}{c}{w}\\ \multicolumn{1}{c}{r}\end{array}\right]\hfill \\ =\left[\begin{array}{c}{\tau }_X\hfill \\ \multicolumn{1}{c}{{\tau }_Y}\\ \multicolumn{1}{c}{{\tau }_Z}\\ \multicolumn{1}{c}{{\tau }_N}\end{array}\right]\hfill \end{array}$$

(7)

where m is the mass of the AUV and $I_z$ is the moment of inertia about the $z_b$ axis. $x_g$ is the position of the center of gravity in the $x_b$ direction and $X_{\stackrel{·}{u}}$ , $X_{u\left|u\right|}$ and other similar symbols are the hydrodynamic parameters.

The kinematic part of the prediction model is:

$$\begin{array}{c}\stackrel{·}{x}=u\cos \psi -v\sin \psi \hfill \\ \stackrel{·}{y}=u\sin \psi +v\cos \psi \hfill \\ \stackrel{·}{z}=w\hfill \\ \stackrel{·}{\psi }=r\hfill \end{array}.$$

(8)

Constraints during docking

AUV actuators, such as, propellers and rudders, have ability limits, which can be described using input constraints as:

$${\mathit{u}}_{\min }\le \mathit{u}\le {\mathit{u}}_{\max }$$

(9)

During docking, it is undesirable for the AUV to deviate too far from the DS and it is not allowed to enter the area behind the DS entrance or too close to the seabed. To obtain visual guidance, the DS must always remain within the FOV of the AUV camera. These constraints are shown in Figure 6, and represented by the inequality described in equation (10), with controller state x and input u as variables.

$$\mathit{h}(\mathit{x},\mathit{u})\le 0$$

(10)

Figure 6.

Examples of constraints during docking: (a) horizontal position constraint, (b) vertical position constraint, and (c) FOV constraint.

OFNMPC design

The prediction model composed of equations (7) and (8) is written in the function form after discretization as:

$$\mathit{x}(k+1)=\mathit{f}(\mathit{x}(k),\mathit{u}(k))$$

(11)

To address the problem of model mismatch and external disturbance, the disturbance variable $\mathit{d}(k)$ is added to the model. A pure input disturbance model is used as:

$$\begin{array}{c}\mathit{x}(k+1)=\mathit{f}(\mathit{x}(k),\mathit{u}(k)+\mathit{d}(k))\hfill \\ \mathit{d}(k+1)=\mathit{d}(k),\hfill \end{array}$$

(12)

and the output model of the system is

$$\mathit{y}(k)=\mathit{g}(\mathit{x}(k)).$$

(13)

OFNMPC obtains the state estimation $\hat{\mathit{x}}(k)$ and disturbance estimation $\hat{\mathit{d}}(k)$ from the observer, whose form is introduced in Section 3.3. The new targets of the state and input $(\overline{\mathit{x}},\overline{\mathit{u}})$ are generated by solving the following equilibrium problem:

$$\begin{array}{c}\overline{\mathit{x}}=\mathit{f}(\overline{\mathit{x}},\overline{\mathit{u}}+\hat{\mathit{d}}(k))\hfill \\ \mathit{r}(k)=\mathit{g}(\overline{\mathit{x}})\hfill \end{array}.$$

(14)

The target condition is integrated into the NMPC formulation as equality constraints, and there is no need to solve the analytical form of $(\overline{\mathit{x}},\overline{\mathit{u}})$ . An NLP problem, shown in equation (15), can be constructed to obtain the optimized control and state sequences. A classical quadratic cost function was used in this study as:

$$\begin{array}{c}\underset{\genfrac{}{}{0ex}{}{{\mathit{x}}_1,\dots ,{\mathit{x}}_N}{\genfrac{}{}{0ex}{}{{\mathit{u}}_0,\dots ,{\mathit{u}}_{N-1}}{\overline{\mathit{x}},\overline{\mathit{u}}}}}{min}\frac{1}{2}\parallel {\mathit{x}}_N-\overline{\mathit{x}}{\parallel }_{{\mathit{Q}}_N}^2+\frac{1}{2}\sum _{t=0}^{N-1}\parallel {\mathit{x}}_t-\overline{\mathit{x}}{\parallel }_{\mathit{Q}}^2+\parallel {\mathit{u}}_t-\overline{\mathit{u}}{\parallel }_{\mathit{R}}^2\hfill \\ \multicolumn{1}{c}{\mathrm{subj}.\mathrm{to}{\mathit{x}}_0=\hat{\mathit{x}}(k),{\mathit{d}}_0=\hat{\mathit{d}}(k),}\\ \overline{\mathit{x}}=\mathit{f}(\overline{\mathit{x}},\overline{\mathit{u}}+{\mathit{d}}_0),\hfill \\ \mathit{r}(k)=\mathit{g}(\overline{\mathit{x}}),\hfill \\ \mathit{h}({\mathit{x}}_t,{\mathit{u}}_t)\le \mathit{0},t=0,\dots ,N-1,\hfill \\ {\mathit{x}}_{t+1}=\mathit{f}({\mathit{x}}_t,{\mathit{u}}_t+{\mathit{d}}_0),t=0,\dots ,N-1,\hfill \end{array}$$

(15)

where Q and R are the weight coefficients of the state and input, respectively; ${\mathit{Q}}_N$ is the terminal weight; and N is the prediction horizon. To solve the NLP problem in equation (15), the optimized input trajectory is:

$${\mathit{U}}^{*}(k)=\{{\mathit{u}}_0^{*}(k),\dots ,{\mathit{u}}_{N-1}^{*}(k)\}.$$

(16)

Here, ${\mathit{u}}_0^{*}(k)$ , the first element of the solution sequence, is applied to the AUV in the period $[k,k+1]$ . The system is closed loop controlled.

Disturbance and state observer

This section presents the design of the observers for OFNMPC, based on EKF and MHE. In addition, the visual observation model required by the observers is described.

Extended Kalman filter

The Kalman filter is an optimal estimator for cases, in which the process and measurement noise are zero-mean Gaussian. For the state estimation problem, if the motion model or sensor model is nonlinear, the EKF is widely used. A nonlinear model was approximated using its first-order Taylor expansion in the iterative process.

The augmented form of the model, as in equation (12), is used as the process model as:

$${\mathit{x}}_{\mathrm{aug}}(k+1)={\mathit{f}}_{\mathrm{aug}}({\mathit{x}}_{\mathrm{aug}}(k),\mathit{u}(k))+\mathit{w}(k),$$

(17)

where ${\mathit{x}}_{\mathrm{aug}}(k)=[\mathit{x}(k{)}^{\mathrm{T}},\mathit{d}(k{)}^{\mathrm{T}}{]}^{\mathrm{T}}$ and $\mathit{w}(k)~N(0,{\mathit{Q}}_{\mathrm{EKF}})$ .

The form of observation model is:

$$\mathit{z}(k)={\mathit{h}}_{\mathrm{ob}}(\mathit{x}(k))+\mathit{v}(k),$$

(18)

where $\mathit{z}(k)$ is the measurement and $\mathit{v}(k)~N(0,{\mathit{R}}_{\mathrm{EKF}})$ .

The EKF is initialized as:

$${\hat{\mathit{x}}}_{\mathrm{aug}}(0)=E({\mathit{x}}_{\mathrm{aug}}(0))$$

(19)

$$\mathit{P}(0)=E[({\mathit{x}}_{\mathrm{aug}}(0)-{\hat{\mathit{x}}}_{\mathrm{aug}}(0))({\mathit{x}}_{\mathrm{aug}}(0)-{\hat{\mathit{x}}}_{\mathrm{aug}}(0){)}^{\mathrm{T}}]$$

(20)

The estimation of the augmented state ${\hat{\mathit{x}}}_{\mathrm{aug}}(k)$ and the estimated covariance $\mathit{P}(k)$ are calculated iteratively according to the following steps:

The Jacobians $\mathit{F}(k)$ and $\mathit{L}(k)$ of ${\mathit{f}}_{\mathrm{aug}}(·)$ are computed as:

$$\mathit{F}(k)={\frac{\partial {\mathit{f}}_{\mathrm{aug}}}{\partial {\mathit{x}}_{\mathrm{aug}}}|}_{{\hat{\mathit{x}}}_{\mathrm{aug}}(k)}$$

(21)

$$\mathit{L}(k)={\frac{\partial {\mathit{f}}_{\mathrm{aug}}}{\partial \mathit{w}}|}_{{\hat{\mathit{x}}}_{\mathrm{aug}}(k)}$$

(22)

The prediction step can be written as:

$${\hat{\mathit{x}}}_{\mathrm{aug}}^{-}={\mathit{f}}_{\mathrm{aug}}({\hat{\mathit{x}}}_{\mathrm{aug}}(k),\mathit{u}(k)),$$

(23)

$${\mathit{P}}^{-}=\mathit{F}(k)\mathit{P}(k)\mathit{F}(k{)}^{\mathrm{T}}+\mathit{L}(k){\mathit{Q}}_{\mathrm{EKF}}\mathit{L}(k{)}^{\mathrm{T}}.$$

(24)

The Jacobians $\mathit{H}(k)$ and $\mathit{M}(k)$ of ${\mathit{h}}_{\mathrm{ob}}(·)$ are computed as:

$$\mathit{H}(k)={\frac{\partial {\mathit{h}}_{\mathrm{ob}}}{\partial {\mathit{x}}_{\mathrm{aug}}}|}_{{\hat{\mathit{x}}}_{\mathrm{aug}}^{-}}$$

(25)

$$\mathit{M}(k)={\frac{\partial {\mathit{h}}_{\mathrm{ob}}}{\partial \mathit{v}}|}_{{\hat{\mathit{x}}}_{\mathrm{aug}}^{-}}$$

(26)

The updated steps of state estimation and the estimated error covariance are:

$${\mathit{K}}_k={\mathit{P}}^{-}\mathit{H}(k{)}^{\mathrm{T}}(\mathit{H}(k){\mathit{P}}^{-}\mathit{H}(k{)}^{\mathrm{T}}+\mathit{M}(k){\mathit{R}}_{\mathrm{EKF}}\mathit{M}(k{)}^{\mathrm{T}}{)}^{-1}$$

(27)

$${\hat{\mathit{x}}}_{\mathrm{aug}}(k+1)={\hat{\mathit{x}}}_{\mathrm{aug}}^{-}+{\mathit{K}}_k(\mathit{z}(k+1)-{\mathit{h}}_{\mathrm{ob}}({\hat{\mathit{x}}}_{\mathrm{aug}}^{-}))$$

(28)

$$\mathit{P}(k+1)=(\mathit{I}-{\mathit{K}}_k\mathit{H}(k)){\mathit{P}}^{-}.$$

(29)

${\hat{\mathit{x}}}_{\mathrm{aug}}(k+1)$ is the augmented state estimation obtained using the EKF, and the disturbance estimation $\hat{\mathit{d}}(k+1)$ is included in ${\hat{\mathit{x}}}_{\mathrm{aug}}(k+1)$ .

Moving horizon estimation

For an estimator under the framework of the Kalman filter, it is generally assumed that the system is Markov; that is, the current state contains the information of all the previous states. In contrast, the MHE transforms the state estimation problem into optimization problem constructed using window information and estimates the system state through nonlinear optimization. The MHE can directly use an exact nonlinear model, which is one of its advantages.

Considering the systems described in equation (11), at the current time k of the system, the input of the optimization problem includes N measurements and nominal control inputs closest to the current time as:

$$\begin{array}{c}\mathit{z}(t),t=k-N+1,\dots ,k,\hfill \\ \mathit{u}(t),t=k-N,\dots ,k-1.\hfill \end{array}$$

(30)

The system state and real input are used as optimization variables as:

$$\begin{array}{c}\hat{\mathit{X}}(k)=\{{\hat{\mathit{x}}}_{k-N}(k),\dots ,{\hat{\mathit{x}}}_k(k)\},\hfill \\ \mathit{U}(k)=\{{\mathit{u}}_{k-N}(k),\dots ,{\mathit{u}}_{k-1}(k)\}.\hfill \end{array}$$

(31)

A nonlinear optimization problem was constructed to estimate the state and input, where ${\mathit{Q}}_x$ , ${\mathit{Q}}_z$ and ${\mathit{R}}_u$ are the corresponding weight matrices, as:

$$\begin{array}{c}\underset{\genfrac{}{}{0ex}{}{{\hat{\mathit{x}}}_{k-N},\dots ,{\hat{\mathit{x}}}_k}{{\mathit{u}}_{k-N},\dots ,{\mathit{u}}_{k-1}}}{min}\frac{1}{2}\parallel {\hat{\mathit{x}}}_{k-N}-{\mathit{x}}_a{\parallel }_{{\mathit{Q}}_x}^2+\frac{1}{2}\sum _{t=k-N+1}^k\parallel \mathit{z}(t)-{\mathit{h}}_{\mathrm{ob}}({\hat{\mathit{x}}}_t){\parallel }_{{\mathit{Q}}_z}^2+\frac{1}{2}\sum _{t=k-N}^{k-1}\parallel \mathit{u}(t)-{\mathit{u}}_t{\parallel }_{{\mathit{R}}_u}^2\hfill \\ \multicolumn{1}{c}{\mathrm{subj}.\mathrm{to}{\hat{\mathit{x}}}_{t+1}=\mathit{f}({\hat{\mathit{x}}}_t,{\mathit{u}}_t),t=k-N,\dots ,k-1}\\ {\mathit{h}}_{\mathrm{MHE}}({\hat{\mathit{x}}}_t,{\mathit{u}}_t)\le 0,t=k-N,\dots ,k-1\hfill \end{array}$$

(32)

By solving the optimization problem, the state trajectory estimation ${\hat{\mathit{X}}}^{*}(k)=\{{\hat{\mathit{x}}}_{k-N}^{*}(k),\dots ,{\hat{\mathit{x}}}_k^{*}(k)\}$ and input estimation sequence ${\mathit{U}}^{*}(k)=\{{\mathit{u}}_{k-N}^{*}(k),\dots ,{\mathit{u}}_{k-1}^{*}(k)\}$ can be obtained. Considering ${\hat{\mathit{x}}}_k^{*}(k)$ as the estimation of the current state, ${\mathit{u}}_{k-1}^{*}(k)$ contains the disturbance information.

As only finite time domain data are used, the $k-N$ time state estimation ${\mathit{x}}_a={\hat{\mathit{x}}}_{k-N}^{*}(k-1)$ of the previous sampling period is added to the cost function, and the historical information is retained. The inequality constraints described by ${\mathit{h}}_{\mathrm{MHE}}(·)$ can be relaxed appropriately compared to the MPC problem to obtain better estimation results.

Observation model

The visual observation model describes the relationship between the pixel coordinates of the visual features and the system state x . The position of the guidance light in $\{c\}$ is ${}^c{\mathit{p}}_l=[{}^c{x}_l,{}^c{y}_l,{}^c{z}_l{]}^{\mathrm{T}}$ , and the pixel coordinates mapped to the camera image plane are ${}^I{\mathit{p}}_l=[\overline{u},\overline{v}{]}^{\mathrm{T}}$ . According to the central-projection model of the camera, the mapping relationship between ${}^I{\mathit{p}}_l$ and ${}^c{\mathit{p}}_l$ is established, which is described by the function:

$${\mathit{h}}^{\mathrm{cI}}({}^c{\mathit{p}}_l)={[f\frac{{}^c{x}_l}{{}^c{z}_l},f\frac{{}^c{y}_l}{{}^c{z}_l}]}^{\mathrm{T}},$$

(33)

where f is the focal length of the camera.

The position of the AUV in $\{d\}$ is $\mathit{p}=[x,y,z{]}^T$ , and the attitude relative to the DS is ${\mathit{\eta }}_2$ . The position of the guidance light in $\{d\}$ is ${\mathit{p}}_l=[x_l,y_l,z_l{]}^T$ . Next, the relationship between ${}^c{\mathit{p}}_l$ and the pose of the AUV must be deduced, which can be described by a rotation matrix.

$${}^c{\mathit{p}}_l={}^c{\mathit{R}}_b(\mathit{R}(-{\mathit{\eta }}_2)({\mathit{p}}_l-\mathit{p})-{}^b{\mathit{p}}_c)$$

(34)

where ${}^c{\mathit{R}}_b$ is the rotation matrix from $\{b\}$ to $\{c\}$ and ${}^c{\mathit{p}}_b$ is the position of the camera in $\{b\}$ . equation (34) can be expressed as a function ${\mathit{h}}^{\mathrm{dc}}(\mathit{x},{\mathit{p}}_l)$ , where x is the system state. The visual observation model can be derived as:

$${\mathit{h}}_{\mathrm{ob}}(\mathit{x},{\mathit{p}}_l)={\mathit{h}}^{\mathrm{cI}}({\mathit{h}}^{\mathrm{dc}}(\mathit{x},{\mathit{p}}_l)).$$

(35)

The transformation relationship in the visual observation model is illustrated in Figure 7.

Figure 7.

Schematic of the transformation relationship of the observation model.

The observation model can be used directly by MHE. The EKF requires the Jacobian of the observation model to estimate the Kalman gain and posteriori estimated error covariance. The EKF uses an augmented model, and the Jacobian is derived according to equations (35) and (25).

$$\mathit{H}=\frac{d{\mathit{h}}_{\mathrm{ob}}}{d{\mathit{x}}_{\mathrm{aug}}}=\frac{d{\mathit{h}}^{\mathrm{cI}}}{d{\mathit{h}}^{\mathrm{dc}}}\frac{d{\mathit{h}}^{\mathrm{dc}}}{d{\mathit{x}}_{\mathrm{aug}}}$$

(36)

According to equation (33), we can obtain:

$$\frac{d{\mathit{h}}^{\mathrm{cI}}}{d{\mathit{h}}^{\mathrm{dc}}}=f\left[\begin{array}{ccc}\frac{1}{{}^c{z}_l}\hfill & 0\hfill & -\frac{{}^c{x}_l}{{({}^c{z}_l)}^2}\hfill \\ \multicolumn{1}{c}{0}& \frac{1}{{}^c{z}_l}\hfill & -\frac{{}^c{y}_l}{{({}^c{z}_l)}^2}\hfill \end{array}\right]$$

(37)

The augmented state consists of velocity, pose, and disturbance. Defining the pose of the AUV as ${\mathit{\eta }}_p=[x,y,z,\psi {]}^{\mathrm{T}}$ , $d{\mathit{h}}^{\mathrm{dc}}/d{\mathit{x}}_{\mathrm{aug}}$ can be derived as:

$$\frac{d{\mathit{h}}^{\mathrm{dc}}}{d{\mathit{x}}_{\mathrm{aug}}}=\left[\begin{array}{ccc}{\mathit{0}}_{3\times 4}\hfill & \frac{d{\mathit{h}}^{\mathrm{dc}}}{d{\mathit{\eta }}_p}\hfill & {\mathit{0}}_{3\times 4}\hfill \end{array}\right]$$

(38)

The form of $d{\mathit{h}}^{\mathrm{dc}}/d{\mathit{\eta }}_p$ is:

$$\begin{array}{c}\frac{d{\mathit{h}}^{\mathrm{hc}}}{d{\mathit{\eta }}_p}=\hfill \\ {}^c{\mathit{R}}_b\left[\begin{array}{cccc}-\cos \psi \hfill & -\sin \psi \hfill & 0\hfill & \sin \psi (x-x_l)-\cos \psi (y-y_l)\hfill \\ \multicolumn{1}{c}{\sin \psi }& -\cos \psi \hfill & 0\hfill & \cos \psi (x-x_l)+\sin \psi (y-y_l)\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & -1\hfill & 0\hfill \end{array}\right].\hfill \end{array}$$

(39)

A visual observation model and its Jacobian have been presented. The observation model of velocity is linear and can be easily combined with equations (35) and (36) for the EKF and MHE.

Integrated OFNMPC and observer

The AUV is a continuous-time dynamic system that must be discretized. In this study, integration over one shooting interval was performed using the Runge-Kutta method, and a numerically approximate discrete system model was obtained. Several methods for model discretization are described in Gros et al. ⁴⁶

The OFNMPC-EKF is a natural method. The EKF is designed with an augmented model and provides state and disturbance estimations for the OFNMPC. The implementation of OFNMPC-EKF is described in Algorithm 1.

Algorithm 1:

OFNMPC-EKF

1.  $k=0$ . 2. Initialize state estimation $\hat{\mathbf{x}}(0)$ , disturbance estimation $\hat{\mathbf{d}}(0)$ , covariance matrix $\mathbf{P}(0)$ , state and input initial guess trajectories   $\mathbf{X}(0)=\{{\mathbf{x}}_0(0),\dots ,{\mathbf{x}}_N(0)\}$ ,   $\mathbf{U}(0)=\{{\mathbf{u}}_0(0),\dots ,{\mathbf{u}}_{N-1}(0)\}$ . 3. procedure 4. The optimization problem in equation (15) is constructed from the $\hat{\mathbf{x}}(k)$ , $\hat{\mathbf{d}}(k)$ , and reference $\mathbf{r}(k)$ . $\mathbf{X}(k)$ and $\mathbf{U}(k)$ are used as the initial values of the optimization. 5. Solve the optimization problem to obtain the state and input optimization trajectories as:   ${\mathbf{X}}^{*}(k)=\{{\mathbf{x}}_0^{*}(k),\dots ,{\mathbf{x}}_N^{*}(k)\}$ ,   ${\mathbf{U}}^{*}(k)=\{{\mathbf{u}}_0^{*}(k),\dots ,{\mathbf{u}}_{N-1}^{*}(k)\}$ . 6. Apply ${\mathbf{u}}_0^{*}(k)$ as the control input to the AUV. 7. Calculate ${\hat{\mathbf{x}}}_{\mathrm{aug}}^{-}$ and $\mathbf{F}(k)$ using numerical integration based on $\hat{\mathbf{x}}(k)$ , $\hat{\mathbf{d}}(k)$ and ${\mathbf{u}}_0^{*}(k)$ . 8. According to the observation model, use ${\hat{\mathbf{x}}}_{\mathrm{aug}}^{-}$ to calculate the measurement prediction ${\mathbf{h}}_{\mathrm{ob}}({\hat{\mathbf{x}}}_{\mathrm{aug}}^{-})$ and Jacobian $\mathbf{H}(k)$ . 9. Obtain the measurement $\mathbf{z}(k+1)$ . 10. Use equations (27)–(29) to calculate ${\hat{\mathbf{x}}}_{\mathrm{aug}}(k+1)=[\hat{\mathbf{x}}(k+1{)}^{\mathrm{T}},\hat{\mathbf{d}}(k+1{)}^{\mathrm{T}}{]}^{\mathrm{T}}$ and $\mathbf{P}(k+1)$ . 11. Shift ${\mathbf{X}}^{*}(k)$ and ${\mathbf{U}}^{*}(k)$ left as the new optimization initial values   $\mathbf{X}(k+1)=\{{\mathbf{x}}_1^{*}(k),\dots ,{\mathbf{x}}_N^{*}(k),{\mathbf{x}}_N^{*}(k)\}$ ,   $\mathbf{U}(k+1)=\{{\mathbf{u}}_1^{*}(k),\dots ,{\mathbf{u}}_{N-1}^{*}(k),{\mathbf{u}}_{N-1}^{*}(k)\}$ . 12.  $k=k+1$ . 13. end procedure 14. Repeat procedure until the requirements for successful docking are met.

In MHE, we use the original prediction model instead of the augmented model. Algorithm 2 describes the implementation of the OFNMPC-MHE.

Algorithm 2:

OFNMPC-MHE

1.  $k=0$ . 2. Initialize the state estimation $\hat{\mathbf{x}}(0)$ , disturbance estimation $\hat{\mathbf{d}}(0)$ , and state and input initial guess trajectories $\mathbf{X}(0)$ and $\mathbf{U}(0)$ . Initial estimates of the MHE are $\hat{\mathbf{X}}(1)$ and $\hat{\mathbf{U}}(1)$ . 3. procedure 4. The optimization problem in equation (15) is constructed from the $\hat{\mathbf{x}}(k)$ , $\hat{\mathbf{d}}(k)$ , and reference $\mathbf{r}(k)$ . $\mathbf{X}(k)$ and $\mathbf{U}(k)$ are used as the initial values of the optimization. 5. Solve the optimization problem to obtain the state and input optimization trajectories ${\mathbf{X}}^{*}(k)$ and ${\mathbf{U}}^{*}(k)$ . 6. Apply the first item of ${\mathbf{U}}^{*}(k)$ as the control input to the AUV. 7. Obtain the measurement $\mathbf{z}(k+1)$ . 8.  $k=k+1$ . 9. Build measurement window data as in equation (30), and the input window data $\tilde{\mathbf{u}}(t)=\mathbf{u}(t)+\hat{\mathbf{d}}(t),t=k-N,\dots ,k-1$ . 10. Based on the observation model and the prediction model in equation (11), the window data in Step 9 are used as the input to solve the MHE problem in equation (32) and obtain   ${\hat{\mathbf{X}}}^{*}(k)=\{{\hat{\mathbf{x}}}_{k-N}^{*}(k),\dots ,{\hat{\mathbf{x}}}_k^{*}(k)\}$ ,   ${\hat{\mathbf{U}}}^{*}(k)=\{{\hat{\mathbf{u}}}_{k-N}^{*}(k),\dots ,{\hat{\mathbf{u}}}_{k-1}^{*}(k)\}$ . 11.  $\hat{\mathbf{x}}(k)={\hat{\mathbf{x}}}_k^{*}(k)$ , $\hat{\mathbf{d}}(k)={\hat{\mathbf{u}}}_{k-1}^{*}(k)-{\mathbf{u}}_{k-1}$ 12. Shift ${\mathbf{X}}^{*}(k-1)$ and ${\mathbf{U}}^{*}(k-1)$ left as the new MPC optimization initial value   $\mathbf{X}(k)=\{{\mathbf{x}}_1^{*}(k-1),\dots ,{\mathbf{x}}_N^{*}(k-1),{\mathbf{x}}_N^{*}(k-1)\}$ ,   $\mathbf{U}(k)=\{{\mathbf{u}}_1^{*}(k-1),\dots ,{\mathbf{u}}_{N-1}^{*}(k-1),{\mathbf{u}}_{N-1}^{*}(k-1)\}$ . 13. Shift ${\hat{\mathbf{X}}}^{*}(k)$ and ${\hat{\mathbf{U}}}^{*}(k)$ left as the new MHE optimization initial value   $\hat{\mathbf{X}}(k+1)=\{{\hat{\mathbf{x}}}_{k-N+1}^{*}(k),\dots ,{\hat{\mathbf{x}}}_k^{*}(k),{\hat{\mathbf{x}}}_k^{*}(k)\}$ , ${\mathbf{x}}_a={\hat{\mathbf{x}}}_{k-N+1}^{*}(k)$ ,   $\hat{\mathbf{U}}(k+1)=\{{\hat{\mathbf{u}}}_{k-N+1}^{*}(k),\dots ,{\hat{\mathbf{u}}}_{k-1}^{*}(k),{\hat{\mathbf{u}}}_{k-1}^{*}(k)\}$ . 14. end procedure 15. Repeat procedure until the requirements for successful docking are met.

The input disturbance model used by OFNMPC is depicted in equation (12), and the use of Algorithm 2 does not affect the zero-offset control effect of the OFNMPC.

Simulation results and discussion

In this section, the proposed docking approaches, OFNMPC-EKF and OFNMPC-MHE, are applied to a fully actuated AUV, which was briefly introduced in Section 2. The model and parameter settings used in the simulation are explained, simulation results are presented and results are discussed.

Simulation model and parameter setting

A six degrees-of-freedom AUV motion model with full parameters was used as the controlled plant in the simulation. The disturbance of the ocean current was coupled in the model. The equations of motion, including ocean currents, are presented in (A6)–(A8) in Appendix A.

The dynamics part of the prediction model for docking control is shown in equation (7), and the specific form used for the simulation is as follows:

$$\begin{array}{c}\left[\begin{array}{cccc}1460\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 2066\hfill & 0\hfill & -78.6\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 3007\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& -78.6\hfill & 0\hfill & 1051\hfill \end{array}\right]\left[\begin{array}{c}\stackrel{·}{u}\hfill \\ \multicolumn{1}{c}{\stackrel{·}{v}}\\ \multicolumn{1}{c}{\stackrel{·}{w}}\\ \multicolumn{1}{c}{\stackrel{·}{r}}\end{array}\right]\hfill \\ +\left[\begin{array}{cccc}15.53\hfill & 0\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 101.6\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 169.2\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 115.8\hfill \end{array}\right]\left[\begin{array}{c}u\hfill \\ \multicolumn{1}{c}{v}\\ \multicolumn{1}{c}{w}\\ \multicolumn{1}{c}{r}\end{array}\right]=\left[\begin{array}{c}{\tau }_X\hfill \\ \multicolumn{1}{c}{{\tau }_Y}\\ \multicolumn{1}{c}{{\tau }_Z}\\ \multicolumn{1}{c}{{\tau }_N}\end{array}\right]\hfill \end{array}$$

(40)

There was a distinct model mismatch between the prediction model and the controlled plant. This mismatch can be found by comparing the diagonal elements of matrix $\mathit{D}(\mathit{\nu })$ , even if the neglected off-diagonal elements are not considered. A comparison is presented in Table 3.

Table 3.

Comparison of the diagonal elements of matrix D between the plant and prediction model.

Prediction model	Plant
−15.53	$-77.67\left|u\right|$
−101.6	$-508u-328.3\sqrt{v^2+w^2}$
−169.2	$-846.1\mathrm{u}-634\sqrt{v^2+w^2}$
−115.8	$-579.1\mathrm{u}-360\left|r\right|$

The process models of EKF and MHE are the same as the prediction model of MPC, in which the EKF uses the augmented model. The observation models of EKF and MHE are introduced in Section 3.3.3.

In the simulation, the AUV started from the initial pose ${\mathit{\eta }}_0$ = [−15 m, 5 m, −2 m, 0°]^T and reached the DS entrance ${\mathit{\eta }}_r$ = [0, 0,and 0 m, 0°]^T. ${\mathit{\eta }}_r$ is the reference of the OFNMPC. The maximum value of the ocean current was V_c = 0.3 m/s. Ocean currents in different directions were applied to the AUV during the simulations. A positive buoyancy of 10 N was configured in the controlled plant model, which is equivalent to superimposing a disturbance in heave.

In the simulation, six underwater lights were set as visual guidance. OFNMPC could obtain six pairs of pixel coordinates as the measurements. The resolution of the camera was $640\times 480$ pixels and f was 320 pixels. Other measurements included horizontal velocity and yaw rate.

The prediction horizon of OFNMPC was $N=30$ , and the sampling period was 0.5 s. Cost function weight $\mathit{Q}=\mathrm{diag}([1,5,0,0,0.015,0.22,0.11,1.62])$ , ${\mathit{Q}}_N=\mathit{Q}$ , and $\mathit{R}=6.9\times {10}^{-7}{\mathit{I}}_4$ . Control input constraints ${\mathbf{\tau }}_{min}$ = [−120 N, −80 N, −80 N, −120 Nm]^T and ${\mathbf{\tau }}_{max}=-{\mathbf{\tau }}_{min}$ . The constraints on the position and attitude of the AUV were ${\mathit{\eta }}_{min}$ = [−16 m, −1 m, −3 m, −30°]^T and ${\mathit{\eta }}_{max}$ = [0 m, 6 m, 1 m, 30°]^T. The constraints on the AUV velocity were ${\mathit{\nu }}_{min}$ = [−0.75 m/s, −0.3 m/s, −0.3 m/s, −10°/s]^T and ${\mathit{\nu }}_{max}=-{\mathit{\nu }}_{min}$ .

The process noise covariance matrix ${\mathit{Q}}_{\mathrm{EKF}}$ and measurement noise covariance matrix ${\mathit{R}}_{\mathrm{EKF}}$ of the EKF were set as:

$${\mathit{Q}}_{\mathrm{EKF}}=\mathrm{diag}([0.01{\mathit{I}}_3,{10}^{-4},0.01{\mathit{I}}_3,{10}^{-6},400{\mathit{I}}_4]),$$

$${\mathit{R}}_{\mathrm{EKF}}=\mathrm{diag}([100{\mathit{I}}_{12},4\times {10}^{-4}{\mathit{I}}_2,{10}^{-8}]).$$

The optimal horizon of MHE was $N_{\mathrm{MHE}}=30$ . Cost function weight ${\mathit{Q}}_z=\mathrm{diag}([0.01{\mathit{I}}_{12},2500{\mathit{I}}_2,5\times {10}^{-7}])$ , ${\mathit{Q}}_x=15{\mathit{I}}_8$ , ${\mathit{R}}_u=0.0008{\mathit{I}}_4$ .

Both OFNMPC and MHE were constructed and solved using CasADi software framework. ⁴⁷

Simulation results and analysis

Two simulation scenarios are designed to verify the effectiveness and robustness of the proposed method. In Scenario 1, the current disturbance and measurement noise were not added, and there was only a model mismatch to prove the feasibility of the proposed approach. In Scenario 2, the measurement noise and current disturbance were added to verify the robustness of the proposed approach. The effects of control of the OFNMPC-EKF and OFNMPC-MHE were compared and analyzed.

Scenario 1 with model mismatch

Figure 8 shows a comparison of the docking trajectories of OFNMPC-EKF and OFNMPC-MHE in Scenario 1. Both approaches drive the AUV to the set point. From the comparison of the output response of the three-dimensional position and heading shown in Figure 9, there is no distinct difference between the control effects of the two methods, except that the response of OFNMPC-MHE in the vertical direction is slightly slow. The estimation of the disturbance using the two methods is illustrated in Figure 10. Both methods obtained reasonable disturbance estimation results and accurately estimated the 10 N positive buoyancy in the steady state. It can be seen that OFNMPC realizes zero-offset docking control in the presence of a distinct model mismatch by estimating the disturbance.

Figure 8.

3D docking trajectory comparison in Scenario 1.

Figure 9.

Output response comparison in Scenario 1 (dotted line indicates the allowable deviation).

Figure 10.

Disturbance estimation comparison in Scenario 1.

Figure 11 shows the trajectories of the visual features in the image plane, which were used as the measurements of the two approaches.

Figure 11.

Trajectories of visual feature measurements on the image plane (O is the start point of the track, * is the end point of the track).

OFNMPC uses a fixed reference ${\mathit{\eta }}_r$ without an additional reference trajectory generator. Through moving-horizon optimization, the proposed approach produced a reasonable docking trajectory. The horizontal docking path and corresponding AUV heading shown in Figure 12 indicate that the proposed approach integrates the guidance function.

Figure 12.

Horizontal docking path generated using OFNMPC-EKF (arrows indicate the heading of the AUV).

Scenario 2 with measurement noise and current disturbance

Current disturbance and measurement noise were added to the simulation. The maximum current was maintained at 0.3 m/s, and the angle of attack was ${\alpha }_c=0$ . Different current slide angles were applied to the docking process of the AUV as:

$${\beta }_c=(\frac{5}{6}\pi ,-\frac{5}{6}\pi ,-\frac{1}{2}\pi ,\pi ,\frac{1}{2}\pi ).$$

The random noise with standard deviation of 5 pixels was added to the visual feature measurements, and the random noise with standard deviation of 0.02 m/s was added to the horizontal velocity measurements. The measurements with noise are presented in Figures 13 and 14.

Figure 13.

Vision measurements and their real trajectories (O is the start point of the trajectory, * is the end point of the trajectory).

Figure 14.

Velocity measurements and their real values.

Figures 15 and 16 show the 3D docking trajectories of OFNMPC-EKF and OFNMPC-MHE, respectively, under the interference of currents in five different directions. It can be observed that the OFNMPC-MHE has a large position deviation in two of the five simulations.

Figure 15.

OFNMPC-EKF 3D docking trajectories in Scenario 2.

Figure 16.

OFNMPC-MHE 3D docking trajectories in Scenario 2.

The time taken to drive the AUV from the initial position to the allowable deviation range is defined as the docking time. The docking times for the two approaches are compared in Table 4.

Table 4.

Comparison of docking time in Scenario 2.

Case	β c of the currents	OFNMPC-EKF (s)	OFNMPC-MHE (s)
1	$5\pi /6$	55	40
2	$-5\pi /6$	35.5	Failure
3	$-\pi /2$	32	32
4	$\pi$	36	36
5	$\pi /2$	29.5	Failure

It can be observed that in the successful docking, the docking time of OFNMPC-MHE is slightly less than that of OFNMPC-EKF. For the case of failure, the simulation of case 2 was analyzed. The output response and disturbance estimation of the two approaches are shown in Figures 17 and 18, respectively. As shown in Figure 17, the disturbance estimation of the MHE to sway and yaw diverges over a period of time. The incorrect estimation of the external disturbance leads to a large amplitude swing in the bow direction of the AUV, which can be clearly seen in Figure 18. This causes the pilot light to move out of the camera FOV, resulting in a docking failure. Figure 19 shows a comparison of the control inputs. The input constraint was not broken. Similarly, an incorrect estimation of the MHE leads to the opposite control. Figure 20 shows a comparison of the pose estimation errors. Both EKF and MHE obtained accurate estimation results.

Figure 17.

Disturbance estimation comparison in Scenario 2.

Figure 18.

Output response comparison in Scenario 2 (dotted line indicates the allowable deviation).

Figure 19.

Control input comparison in Scenario 2.

Figure 20.

Pose estimation errors comparison in Scenario 2.

In this study, a simplified AUV dynamics model is used, which makes the MHE’s advantage of directly using an exact nonlinear model not give full play. The observer design method to improve the robustness and ensure convergence of the MHE should also be applied.

The OFNMPC-EKF approach achieved successful docking in all the five cases in Scenario 2 and had good robustness.

Conclusion

This study presents an integrated approach for AUV docking based on OFNMPC. The proposed approach integrates the navigation, guidance, and control of AUV docking so that docking can be systematically designed and optimized. First, the typical architecture of the AUV docking approach is introduced. Based on the demand for precise AUV docking, an offset-free control problem of AUV docking was formulated. Subsequently, the design of the OFNMPC is explained. The observer required by the OFNMPC was realized using EKF and MHE. Both the observers used the AUV dynamics model and observation model of visual feature measurements, which enabled them to estimate the state and disturbance of the docking process directly from the visual features. Special visual pose estimation was omitted. A method to integrate the NMPC and observer was designed. Two approaches, OFNMPC-EKF and OFNMPC-MHE, were realized for AUV zero-offset docking. Simulations were carried out, and a real AUV model was used as the controlled plant. The simulation results show that the proposed OFNMPC-EKF approach can reliably realize accurate docking of AUV in the presence of measurement noise, continuous current disturbance and distinct model mismatch. The docking process was optimized using the MPC framework. We plan to study the real-time performance of the OFNMPC-EKF approach so that it can run on the embedded control unit of the AUV and complete an offshore test.