The AUV-Follower Control System Based on the Prediction of the AUV-Leader Movement Using Data from the Onboard Video Camera

Abstract

The paper proposes a new method for the synthesis of spatial motion control systems of the AUV-leader and a group of AUV-followers during their cooperative movement in a desired formation. This system allows the provision of an accurate positioning of the followers relative to the leader using information received with a low frequency only from their onboard video cameras. To improve the accuracy of the created system it proposes a method of estimation of the parameters of the movement of the AUV-leader (its speeds and accelerations) to predict its movement relative to the AUV-follower in the time intervals between the updates of information received from video cameras.

1. Introduction

Now, the use of groups of autonomous underwater vehicles (AUVs) is the most promising way to increase the efficiency of underwater operations performed by AUV [1]. This is especially important for the operations of bottom mapping, searching, the transportation of underwater objects, and others. The strategy “leader-followers” is one of the most popular approaches for AUV cooperative control. This strategy supposes that one AUV is a leader, and the others are followers. The AUV-leader has full information about the mission and plans the trajectory, and the AUV-followers move the leader and keep a setting position relative to the leader [1].

Many approaches to the synthesis of control systems for the formation control of groups of robots for various purposes are known. These approaches are based on various methods of the control theory and make it possible to hold the position of robot-followers in the desired formation, while considering the uncertainty of their parameters and the influence of external disturbances [2,3,4,5,6]. At the same time, it is assumed that all the robots of the group should determine its position and receive information about the position and movement parameters of the leader. In the case of AUV, the specified information must be received through acoustic communication channels [7,8,9,10], which have a limited bandwidth and long delays in data transmission.

Various approaches are proposed to consider the features of these communication channels when implementing AUV formation control systems. To reduce the impact of the limited bandwidth of these communication channels, the use of an extended Kalman filter is proposed in [11]. Using this filter, the AUV-followers predict the position of the AUV-leader based on data on its speed and position obtained with a low frequency. However, the implementation of this approach requires the matching navigation systems of the AUV-followers and the AUV-leader for the correct determination of the AUV-followers’ position relative to the leader in the absolute coordinate system (ACS). To solve this problem, the methods described in [12,13,14] can be used. However, their use can significantly complicate the synthesis of AUV cooperative control systems.

Other solutions that do not require the use of data about the AUV-leader and AUV-followers’ coordinates in ASCs are the formation control methods based on data about position of the AUV-followers relative to the leader, obtained using their onboard sensors [15]. In the work of [16], a formation control method of AUVs is proposed. In this method, the AUV-leader sends to the AUV-followers information about its current speed, the yaw angle, and yaw angular velocity via acoustic communication channels. At the same time, the followers, with the help of onboard acoustic sensors, determine the distance and direction to the AUV-leader. That allows the calculation of the current deviation of the follower from a desired position in the formation. However, these communication channels used for data exchange between AUVs have low a bandwidth and long delays in data transmission, which do not allow the AUV-followers to move safely and accurately at a small distance from each other. One of the ways to solve these problems is the use of AUV formation control systems based on data received from their onboard video cameras. It allows the exclusion of the data exchange through the acoustic communication channels and the matching of the navigation systems of the AUV-leader and the AUV-followers. In addition, the use of the data about the position of the AUV-followers relative to the leader, obtained from onboard video cameras, makes it possible to not use expensive hydroacoustic navigation systems on board the AUV-followers.

It should be noted that the main difficulty with using video information is the relatively small distance at which the onboard video camera can operate. Usually, depending on the state of the underwater environment, this distance is 5–15 m [17]. However, there is a large class of tasks for which such distances between the AUVs of the group are the working distances. For example, geotechnical survey can be attributed to these tasks. When performing these operations, the AUV group implements a field of hydrophones, where they must accurately hold the specified distances between themselves, which equal a few meters [18]. Also relevant is the task of creating an AUV assistant [19], which should move behind the diver at a given short distance. Currently, video information is widely used to create AUV automatic docking systems [17,20,21]. In these systems, the position and orientation of the AUV relative to the docking stations is determined by means of light beacons installed at these stations. At the same time, during the docking process, the linear and angular velocities of the AUV relative to the stations are determined only by the AUV’s own movement as the stations are stationary. However, when AUVs are moving in a group, the speed and direction of the AUV-leader’s movement may change unexpectedly, especially when the leader bypasses obstacles. This requires an additional estimation of all the motion parameters in the presence of video information. Moreover, the data about the position and orientation of the AUV-followers’ relative leader, formed by means of video information, can be updated slowly, which greatly reduces the accuracy of the AUV-followers’ movement as this cannot allow the generation of the control signals with high frequency. In [22], a method for solving this problem is proposed for industrial manipulators tracking a moving object using a video camera. In this method, a Kalman filter and a special predictor are used for estimating the speed of a moving object, which can compensate for the delays that occur in the process of obtaining the required estimates. The position of the video camera and the position of the object are formed in the ACS, which allows the prediction of the movement of the object independently of the movement of the manipulator. However, the position and orientation of the AUV-follower’s relative leader depend on the spatial movement and orientation of the followers themselves, and this factor requires additional consideration to generate control signals for the AUV-followers.

In [23], the method for the control of the AUV-followers’ movement based on the information received from their onboard video cameras is proposed. However, in this work, the estimation of the AUV-leader’s speed relative to the follower was carried out using numerical differentiation of the estimation of the AUV-leader’s position, which significantly reduced the accuracy of this estimation and, consequently, the control accuracy. Moreover, the proposed method assumed that the AUV group moves only in the horizontal plane, and it cannot be used in the case when the AUV leader moves in space, such as when holding a given height above a bottom with difficult relief.

Thus, in this paper the set task is to develop a control system for the AUV-follower which provides the high-precision holding of a desired position of the AUV-follower relative to the AUV-leader based on data received from the AUV-follower’s onboard video camera. The approach proposed in the paper is based on an estimation of the position and movement parameters (speed and acceleration) of the target point, which sets the desired position of the AUV-follower relative to the AUV-leader. This estimation is performed by means of a data series received from the onboard video camera of the AUV-follower for a certain period. The position of the target point is formed as a polynomial time function describing the change of the target point coordinates relative to the AUV-follower. The obtained functions are used to predict the movement of the target point in the time interval between the updates of the data received from the video camera, with consideration of the AUV-follower’s own movement. These estimates are used to generate program signals, which are worked out from the AUV-follower’s onboard control system, which considers the uncertainties of the parameters of the slave.

This approach will ensure the movement of AUV-followers without the use of acoustic communication channels and hydroacoustic navigation systems. It will reduce the cost of using AUV groups to perform various underwater operations requiring the movement of AUVs in a group with relatively short distances between AUVs (bottom photography, environmental monitoring, seismicity studies, etc.).

The contributions of this paper are the following.

1. A method of estimating the vectors of the position, speed, and acceleration of the target point, which determine the position of the AUV-follower in the formation, relative to this AUV-follower. This estimation is based on the series of measurements of the position and orientation of the AUV-leader relative to the follower and is formed as polynomial time functions. This approach makes it possible to reduce the influence of the errors that occur when measuring the position and orientation of the AUV-leader, as well as to reduce the delay in determining speeds and accelerations compared to the traditional methods.

2. A method for predicting the movement of the target point of the AUV-follower in the time interval between the updates of data received from the video camera, with consideration of the AUV-follower’s own movement. This makes it possible to significantly increase the frequency of the updating information about the current deviation of the follower from the desired position in the formation. It can provide the possibility of using high-precision control systems.

The main notations used in the article are described in

Table 1. List of notations.

Name	Description
$x_L{y}_L{z}_L$	AUV-leader body-fixed coordinate system (CS)
$x_f{y}_f{z}_f$	AUV-follower body-fixed CS
$x_c{y}_c{z}_c$	CS of AUV-follower onboard camera
$x_{LR}{y}_{LR}{z}_{LR}$	CS, the beginning of which coincides with the CS $x_L{y}_L{z}_L$, and the $z_{LR}$ axis is parallel to the z axis of the absolute CS
$x_{FR}{y}_{FR}{z}_{FR}$	CS, the beginning of which coincides with the CS $x_F{y}_F{z}_F$, and the $z_{FR}$ axis is parallel to the z axis of the absolute CS
${\mathit{X}}_F^{*}$	the desired position of AUV-follower in CS $x_{FR}{y}_{FR}{z}_{FR}$
${\mathit{X}}_F^{L*}$	the desired position of AUV-follower in CS $x_{LR}{y}_{LR}{z}_{LR}$
B = (B1, B2, …, Bn)	the coordinates of the beacons in the AUV-leader CS
$\mathit{P}=\left({\mathit{p}}_1, {\mathit{p}}_2, \dots , {\mathit{p}}_n\right),$	the pixel coordinates of the beacons in the image
${\tilde{\mathit{X}}}_F^{*}$	the estimation of the position of the target point ${\mathit{X}}_F^{*}$
${\widehat{\mathit{X}}}_F^{*}$, ${\widehat{\mathit{\upsilon }}}_F^{*}$, ${\widehat{\mathit{a}}}_F^{*}$	the vectors of position ${\widehat{\mathit{X}}}_F^{*}$, the velocities ${\widehat{\mathit{\upsilon }}}_F^{*}$, and the accelerations ${\widehat{\mathit{a}}}_F^{*}$, which estimate the movement of the point ${\mathit{X}}_F^{*}$ by polynomial functions
${\widehat{\mathit{X}}}_F^{0*}$, ${\widehat{\mathit{\upsilon }}}_F^{0*}$, ${\widehat{\mathit{a}}}_F^{0*}$	the estimated vectors of position, speed, and acceleration of movement of target points for follower at the current time
${\check{\mathit{X}}}_{F, p}^{*}$, ${\check{\mathit{\upsilon }}}_{F,p}^{*}$	the predicted vectors of position and speed of movement of the point ${\mathit{X}}_F^{*}$
${\mathit{\upsilon }}_F={\left[{\upsilon }_{Fx}, {\upsilon }_{Fy}, {\upsilon }_{Fz}, {\omega }_{Fx}, {\omega }_{Fy}, {\omega }_{Fz}\right]}^T$	the vector of linear and angular velocities in the AUV BCS
$\xi ={[ {\phi }_F, {\theta }_F, {\psi }_F]}^T$	the vector of the AUV orientation angles in absolute CS
$\mathit{\tau }={[{\tau }_x, {\tau }_y, {\tau }_z, M_x, M_y, M_z]}^T$	the vector of propulsion forces and moments in the AUV body-fixed CS
${\mathit{R}}_L^C$	the rotation matrix of leader CS relative to the CS of camera
${\mathit{R}}_C^F$	the rotation matrix of camera CS relative to the AUV-follower body-fixed CS
${\mathit{R}}_F^R$	the rotation matrix of AUV-follower body-fixed CS relative to CS $x_{FR}{y}_{FR}{z}_{FR}$
${\mathit{R}}_F^A$	the rotation matrix of AUV-follower body-fixed CS relative to absolute CS
${\mathit{R}}_L^{FR}$	the orientation matrix of AUV-leader in CS $x_{FR}{y}_{FR}{z}_{FR}$
${\mathit{R}}_x, {\mathit{R}}_y, {\mathit{R}}_z$	the matrixes of the elementary rotations around axes of the absolute CS

.

2. Problem Definition

The paper considers the group of AUVs, consisting of the AUV-leader and the AUV-followers. The leader has information about the mission and plans the trajectory of movement in the process of its performance. Each AUV-follower moves for the leader and holds its desired position in the formation (point ${\mathit{X}}_F^{*}$ in Figure 1).

The desired position ${\mathit{X}}_F^{L*}$ of each AUV-follower is set in the coordinate system (CS) $x_{LR}{y}_{LR}{z}_{LR}$, the beginning of which coincides with the CS $x_L{y}_L{z}_L$ joined with the AUV-leader, and the z_LR axis is parallel to the z axis of the absolute CS (ACS). This ensures that the desired positions of the followers relative to the leader in the vertical axes do not change when the leader moves in space with a change of the roll and pitch angles.

It is assumed that each follower does not receive data about the movement of the leader transmitted via acoustic communication channels, and the AUV-followers do not have information about their position in the ACS. Each follower has onboard inertial sensors that allow the measurement of the orientation angles, linear and angular velocities, and accelerations of the AUV-follower. Several light beacons are installed on the AUV-leader. Their locations relative to the center mass of the leader is known and have the coordinates B = (B₁, B₂, …, B_n), where B_i = (x_bi, y_bi, z_bi)^T are the coordinates of the i-th beacon in the AUV-leader’s CS. These beacons have characteristics that make it possible to identify them in the image received from the video camera and to calculate their pixel coordinates in this image. These pixel coordinates form a vector $\mathit{P}=\left({\mathit{p}}_1, {\mathit{p}}_2,\dots , {\mathit{p}}_n\right), \mathrm{where} {\mathit{p}}_i=\left(p_{xi},p_{yi}\right)$ are the pixel coordinates of the i–th beacon in the image.

The model for converting the three-dimensional coordinates of a point existing in the field of view of a video camera into pixel coordinates, considering the effects of distortion, has the form [24]:

$$\begin{gathered} z_s\left[\begin{array}{c}{p}_{x0}\\ p_{y0}\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]X_s, r^2=p_{x0}{}^2+p_{y0}{}^2, \\ {p}_{rx}=p_{x0}\left(k_1{r}^2+k_2{r}^4+k_3{r}^6\right), p_{ry}=p_{y0}\left(k_1{r}^2+k_2{r}^4+k_3{r}^6\right), p_{tx}=2u_1{p}_{x0}{p}_{y0}+u_2\left(r^2+2p_{x0}{}^2\right) \\ {p}_{ty}=u_1\left(r^2+2p_{y0}{}^2\right)+2u_2{p}_{x0}{p}_{y0}, p_x=p_{x0}+p_{rx}+p_{tx}, p_y=p_{y0}+p_{ry}+p_{ty} \end{gathered}$$

(1)

where c_p = (f_x, f_y, c_x, c_y) is the vector of the internal parameters of the video camera; U = (k₁, k₂, k₃, u₁, u₂) is the vector of the parameters of the radial and tangential distortion of the camera; $X_s={\left[x_s, y_s, z_s\right]}^T$ is the vector of the coordinates of a point in the camera CS; $p={\left[{\mathit{p}}_x,p_y\right]}^T$ is the vector of the pixel coordinates corresponding to a point $X_s$. The internal parameters of the video camera and the distortion parameters are determined using standard calibration procedures.

It is assumed that the updating frequency of the frames from the onboard video camera may be insufficient for the implementation of the high-precision motion control systems of the AUV-followers. The dynamics of the AUV-follower in its body-fixed CS (BCS) are described by the equations system [25]:

$$\mathit{M}\dot{{\mathit{\upsilon }}_F}+\left(\mathit{C}\left(\mathit{M},{\mathit{\upsilon }}_F\right)+\mathit{D}\left(d_1,d_2,{\mathit{\upsilon }}_F\right)\right){\mathit{\upsilon }}_F+\mathit{g}\left(\xi \right)=\mathit{\tau },$$

(2)

where $\mathit{M}={\mathit{M}}_R+{\mathit{M}}_A\in R^{6\times 6}$; $M_R\in R^{6\times 6}$ is an AUV inertia matrix; ${\mathit{M}}_A\in R^{6\times 6}$ is a matrix of added mass and moment inertia; $\mathit{C}\left(\mathit{M},{\mathit{\upsilon }}_F\right)\in R^{6\times 6}$ is a matrix of Coriolis and centripetal forces and torques; D$\left(d_1,d_2,{\upsilon }_F\right)\in R^{6\times 6}$ is a matrix of hydrodynamic forces and moments; $\mathit{g}\left(\xi \right)\in R^6$ is a vector of hydrostatic forces and torques; $\xi ={[ {\phi }_F, {\theta }_F, {\psi }_F]}^T$ is a vector of the AUV orientation angles in the ACS; $\mathit{\tau }={[{\tau }_x, {\tau }_y, {\tau }_z, M_x, M_y, M_z]}^T\in R^6$ is a vector of propulsion forces and torques in the AUV BCS; and ${\upsilon }_F={[{\upsilon }_{Fx}, {\upsilon }_{Fy}, {\upsilon }_{Fz}, {\omega }_{Fx}, {\omega }_{Fy}, {\omega }_{Fz}]}^T\in R^6$ is a vector of linear and angular velocities in the AUV BCS.

The matrix of Coriolis and centripetal forces and torques is described by the expressions [25]:

$$\begin{gathered} \mathit{C}\left(\mathit{M},{\mathit{\upsilon }}_F \right)={\mathit{C}}_{RB}\left({\mathit{M}}_R,{\mathit{\upsilon }}_F\right)+{\mathit{C}}_A\left({\mathit{M}}_A,{\mathit{\upsilon }}_F\right), \\ {\mathit{C}}_{RB}\left({\mathit{M}}_R,{\mathit{\upsilon }}_F\right)=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& -S\left(M_{11}{\upsilon }_1+M_{12}{\upsilon }_2\right)\\ -S\left(M_{11}{\upsilon }_1+M_{12}{\upsilon }_2\right)& -S\left(M_{21}{\upsilon }_1+M_{22}{\upsilon }_2\right)\end{array}\right], {\mathit{C}}_A\left({\mathit{M}}_A,{\mathit{\upsilon }}_F\right)=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& -S\left(A_{11}{\upsilon }_1+A_{12}{\upsilon }_2\right)\\ -S\left(A_{11}{\upsilon }_1+A_{12}{\upsilon }_2\right)& -S\left(A_{21}{\upsilon }_1+A_{22}{\upsilon }_2\right)\end{array}\right], \\ {\mathit{M}}_R=\left[\begin{array}{cc}{M}_{11}& M_{12}\\ M_{21}& M_{22}\end{array}\right], M_A=\left[\begin{array}{cc}{\mathit{A}}_{11}& A_{12}\\ A_{21}& A_{22}\end{array}\right], \end{gathered}$$

(3)

where ${\upsilon }_1={\left[\begin{array}{ccc}{\upsilon }_{Fx}& {\upsilon }_{Fy}& {\upsilon }_{Fz}\end{array}\right]}^T$, ${\upsilon }_2={\left[\begin{array}{ccc}{\omega }_{Fx}& {\omega }_{Fy}& {\omega }_{Fz}\end{array}\right]}^T$, and operator $S\left(·\right)$ is described by the expressions:

$$S\left(\lambda \right)=\left[\begin{array}{ccc}0& -{\lambda }_3& {\lambda }_2\\ {\lambda }_3& 0& -{\lambda }_1\\ -{\lambda }_2& {\lambda }_1& 0\end{array}\right], \lambda ={\left[\begin{array}{ccc}{\lambda }_1& {\lambda }_2& {\lambda }_3\end{array}\right]}^T.$$

The matrix of hydrodynamical forces and torques is diagonal, and its entries have following view [25]:

$$D_{ii}=d_{1i}+d_{2i}\left|{\upsilon }_i\right|, i=\left(\overline{1,6}\right),$$

(4)

where d_1i and d_2i are the hydrodynamic coefficients.

The vector of the hydrostatic forces and torques has the following form [25]:

$$\mathit{g}\left(\xi \right)=\left[\begin{array}{c}\left(W-B\right)\sin \theta \\ -\left(W-B\right)\cos \theta \sin \phi \\ -\left(W-B\right)\cos \theta \cos \phi \\ -B_y\cos \theta \cos \phi +B_z\cos \theta \sin \phi \\ B_z\sin \theta +B_x\cos \theta \cos \phi \\ -B_x\cos \theta \sin \phi -B_y\sin \theta \end{array}\right],$$

where W_a is the gravity force; B_a is the buoyancy force; $B_x=W_a{x}_G-B_a{x}_B$, $B_y=W_a{y}_G-B_a{y}_B$, and $B_z=W_a{z}_G-B_a{z}_B$; $x_G, y_G, z_G$ are the coordinates of the center of gravity (CG) in the AUV BCS; $x_B, y_B, z_B$ are the coordinates of the center of buoyancy (CB) in the AUV BCS.

The AUV parameters are not known exactly and can change, for example during the transportation of cargo which is unknown in advance. Because the parameters M, d₁, and d₂ enter into Equations (2)–(4) linearly, the AUV dynamic can be described in the following form:

$$\left({\mathit{M}}_n+{\mathit{M}}_{\Delta }\right){\dot{\mathit{\upsilon }}}_F+\left(\left({\mathit{C}}_n\left({\mathit{M}}_n,{\mathit{\upsilon }}_F\right)+{\mathit{C}}_{\Delta }\left({\mathit{M}}_{\Delta }{\mathit{\upsilon }}_F\right)\right)\right){\mathit{\upsilon }}_F+\left({\mathit{D}}_n\left(d_{n1},d_{n2},{\upsilon }_F\right)+{\mathit{D}}_{\Delta }\left(d_{\Delta 1},d_{\Delta 2},{\mathit{\upsilon }}_F\right)\right){\mathit{\upsilon }}_F+\mathit{g}\left(\xi \right) =\mathit{\tau },$$

(5)

where ${\mathit{C}}_n\in R^{6\times 6}, {\mathit{D}}_n\in R^{6\times 6}$ are the forces and torques that occur when the AUV is moving and depend on the nominal values of the AUV parameters; ${\mathit{C}}_{\Delta }\in R^{6\times 6}, {\mathit{D}}_{\Delta }\in R^{6\times 6}$ are the forces and torques that occur when the AUV is moving and depend on the uncertain or variable part of the AUV parameters; ${\mathit{M}}_n\in R^{6\times 6}, d_{n1}\in R^6, d_{n2}\in R^6$ are the nominal values of the AUV parameters; and ${\mathit{M}}_{\Delta }\in R^{6\times 6}, {\mathit{d}}_{\Delta 1}\in R^6,$ ${\mathit{d}}_{\Delta 2}\in R^6$ are the uncertain or variable parts of the AUV parameters. The matrixes ${\mathit{C}}_{\Delta } \mathrm{and} {\mathit{D}}_{\Delta }$ are calculated by Expressions (3) and (4) with the use of ${\mathit{M}}_{\Delta }, d_{\Delta 1} \mathrm{and} d_{\Delta 2}$.

It is supposed that the following conditions are satisfied for the matrixes ${\mathit{M}}_n$ and ${\mathit{M}}_{\Delta }$, and the entries of matrix ${\mathit{M}}_{\Delta }, {\mathit{C}}_{\Delta }$, and the vectors ${\mathit{d}}_{\Delta 1}, {\mathit{d}}_{\Delta 2}$:

$$\mathit{M}>0, |m_{\Delta ,ij}\left|\le {\overline{m}}_{\Delta ,ij}, |c_{\Delta ,ij}\right|\le {\overline{c}}_{\Delta ij}, |d_{\Delta ,1j}\left|\le {\overline{d}}_{\Delta 1j}, |d_{\Delta 2,j}\right|\le {\overline{d}}_{\Delta 2j}, i=\left(\overline{1,6}\right), j, i=\left(\overline{1,6}\right),$$

(6)

where ${\overline{m}}_{ij}$, ${\overline{d}}_{\Delta 1j}$, ${\overline{d}}_{\Delta 2j}$, and ${\overline{c}}_{\Delta ij}$, ${\overline{g}}_{\Delta i}$ are the estimations of the maximum deviations of the AUV’s parameters and the entries of the corresponding matrixes from their nominal values.

The following task is set in the paper with consideration of the models describing the AUVs and their video cameras. Based on the information about the pixel coordinates of the light beacon located on the AUV-leader, it is to develop a method for determining the position and speed of a target point movement, which sets the desired position of each follower relative to the leader. The information about the target point movement must be generated with a given discreteness, regardless of the frequency of the updating frames from the onboard video cameras. A control system of the AUV-follower should be synthesized that ensures a given accuracy of the positioning of the AUV-follower with consideration of the unknown vales of the AUV-follower’s parameters described by the Expression (6).

3. The Formation of the Position, Orientation, and Speeds of Target Point Relative to the AUV-Follower with a Desired Period

To solve this problem, we propose a control system, the diagram of which is shown in Figure 2. From this diagram, the formation of the control signal of the AUV-follower includes several steps.

Initially, the AUV-leader light beacons are allocated on the frames received from the onboard video camera using standard image-processing algorithms, and the P_B vector of their pixel coordinates is formed. This vector is used to determine the position and orientation of the leader relative to the follower and for the calculation of the target point position ${\tilde{\mathit{X}}}_{\mathit{F}}^{*}$ of each AUV-follower in its BCS. Information about ${\tilde{\mathit{X}}}_{\mathit{F}}^{*}$ contains inaccuracies arising due to errors in determining the pixel coordinates of the light beacons and the parameters of the video camera. These inaccuracies lead to errors in determining the velocity ${\mathit{\upsilon }}_{\mathit{F}}^{*}$ and acceleration ${\mathit{a}}_{\mathit{F}}^{*}$ of the target point movement of the AUV-follower in the BCS, and as a result, this reduces the control accuracy of the AUV-follower movement.

To improve the control accuracy, the forming of special polynomial functions approximating the changing coordinates of the ${\widehat{\mathit{X}}}_F^{*}$ target points of the AUV-follower is proposed. These functions allow the prediction of the changes of these coordinates at the period of the data update from the onboard video cameras. Moreover, an analytical description of the motion of point ${\widehat{\mathit{X}}}_F^{*}$ is formed, which is used to determine the changes of its position ${\check{\mathit{X}}}_{F,p}^{*}$ velocities ${\check{\mathit{\upsilon }}}_{F,p}^{*}$ with consideration of the AUV-follower’s own movement. The estimations ${\check{X}}_{F,p}^{*}$ and ${\check{\upsilon }}_{F,p}^{*}$ are formed with a necessary period, which is independent of the update period of the data from the onboard camera.

After the formation of the position and speed of each target point relative to the corresponding follower, the program signals of movement are calculated, considering the kinematic features of this follower. These program signals are worked out by the AUV control system, which provides the desired accuracy of the AUV-follower’s movement relative to the leader with any changes in their parameters in the ranges (6).

Next, we will consider sequentially the implementation of all the stages of the system.

3.1. The Estimation of the Positions and Movement Parameters of the Target Points of Each AUV-Follower Based on Data Received from Their Onboard Video Cameras

At the first stage, the position and orientation of the AUV-leader relative to the follower are determined using the vector P_B of the pixel coordinates of the beacons, the vector c_p of the internal parameters of the video camera, and the vector U of the distortion parameters obtained using standard calibration procedures.

The pixel coordinates ${\mathit{p}}_i$ of the i-th light beacon and the position and orientation of the AUV-leader in the CS of the camera, with consideration of model (1), are determined by the equation system:

$${\mathit{p}}_i=F_{B2P}\left(c_p, U,{\mathit{B}}_i, {\mathit{X}}_L^C, {\mathit{A}}_L^C\right), {\mathit{X}}_{Bi}^C={\mathit{R}}_L^C\left({\mathit{A}}_L^C\right){\mathit{B}}_i^{}+{\mathit{X}}_L^C, i=\left(\overline{1,n}\right),$$

(7)

where ${\mathit{X}}_L^C={\left[x_L^C, y_L^C,z_L^C\right]}^T$ is the AUV-leader position in the CS of the AUV-follower camera; ${\mathit{A}}_L^C={\left[{\phi }_L^C,{\theta }_L^C, {\psi }_L^C\right]}^T$ is the vector of the AUV-leader orientation angles in the CS of the AUV-follower camera; and ${\mathit{R}}_L^C$ is the rotation matrix of the leader CS relative to the CS of the camera.

System (7) contains six unknown variables included in the vectors ${\mathit{X}}_L^C$ and ${\mathit{A}}_L^C$. To determine these variables, at least three light beacons must be placed on the AUV-leader.

System (7) is essentially nonlinear and cannot be solved analytically. Therefore, to determine the values of ${\mathit{X}}_L^C$ and ${\mathit{A}}_L^C$, it is advisable to use the Levenberg–Markgvard numerical optimization method [26], which has sufficiently fast convergence and relatively low computational complexity. This will allow it to be used in the onboard control systems working in real time.

The cost function for estimating ${\mathit{X}}_L^C$ and ${\mathit{A}}_L^C$ has the following form:

$$J={({\mathit{P}}_B-{\tilde{\mathit{P}}}_B)}^T\left({\mathit{P}}_B-{\tilde{\mathit{P}}}_B\right),$$

(8)

$${\tilde{\mathit{P}}}_B=\left[\begin{array}{c}{\tilde{\mathit{p}}}_1\\ ⋮\\ {\tilde{\mathit{p}}}_n\end{array}\right], {\tilde{\mathit{p}}}_i=F_{B2P}\left(c_p, U, {\mathit{B}}_i, {\tilde{\mathit{\eta }}}_L^C\right)$$

where ${\tilde{\mathit{p}}}_i$ is the estimation of the pixel coordinates of i-th beacon calculated by the means of Equations (1) and (7) and the current estimation of the unknown vector ${\tilde{\mathit{\eta }}}_L^C=\left[{\tilde{\mathit{X}}}_L^C;{\tilde{\mathit{A}}}_L^C\right]ϵR^6$.

For using the Livenberg–Markgwardt method, it is necessary to calculate the matrix $\mathit{G}=\partial {\tilde{\mathit{P}}}_B/\partial {\tilde{\mathit{\eta }}}_L^C\in R^{2n\times 6}$ of the first partial derivatives of the vector ${\tilde{\mathit{P}}}_B$ according to the estimated parameters [26]. Due to the complexity of Equation (7), analytical expressions for calculating the matrix G contain many operations. To simplify the calculation of G, it is better to use the numerical method of calculating derivatives described in [27]. Using this method, the matrix $\partial {\tilde{\mathit{P}}}_B/\partial {\tilde{\mathit{\eta }}}_L^C$ is formed as:

$$\partial {\tilde{\mathit{P}}}_{Bi}/\partial {\tilde{\mathit{\eta }}}_L^C=Im\left[F_{B2P}\left({\tilde{x}}_L^C+ih\right), F_{B2P}\left({\tilde{y}}_L^C+ih\right), F_{B2P}\left({\tilde{z}}_L^C+ih\right), F_{B2P}\left({\tilde{\phi }}_L^C+ih\right), F_{B2P}\left({\tilde{\theta }}_L^C+ih\right), F_{B2P}\left({\tilde{\psi }}_L^C+ih\right)\right]h^{-1},$$

$$\mathit{G}=\left[\begin{array}{c}\frac{\partial {\tilde{P}}_{B1}}{\partial {\tilde{\eta }}_L^C}\\ ⋮\\ \frac{\partial {\tilde{P}}_{Bn}}{\partial {\tilde{\eta }}_L^C}\end{array}\right], i=\left(\overline{1,n}\right),$$

(9)

where ih is the small imaginary value, which is an increment of the corresponding component of the vector ${\tilde{\mathit{\eta }}}_L^C$, while all other arguments of the function $F_{B2P}\left(·\right)$ remain unchanged; Im is the imaginary part.

The vector G (9), calculated for the current value of the vector ${\tilde{\mathit{\eta }}}_L^C$, is used to determine the increment of this vector using the expressions [26]:

$$\Delta {\tilde{\mathit{\eta }}}_L^C\left(k\right)=-{\left[{\mathit{G}}^T\mathit{G}+\mu \left(k\right)\mathit{I}\right]}^{-1}{\mathit{G}}^T{\mathit{P}}_{\mathit{B}},$$

(10)

$${\tilde{\mathit{\eta }}}_L^C\left(k+1\right)=\left\{\begin{array}{c}{\tilde{\mathit{\eta }}}_L^C\left(k\right)+\mathbf{\Delta }{\tilde{\mathit{\eta }}}_L^C\left(k\right), \mu \left(k+1\right)=\frac{\mu \left(k\right)}{\eta }, if J\left({\tilde{\mathit{\eta }}}_L^C\left(k\right)+\mathbf{\Delta }{\tilde{\mathbf{\eta }}}_L^C\left(k\right)\right)<J\left({\tilde{\mathbf{\eta }}}_L^C\left(k\right)\right)\\ {\tilde{\mathit{\eta }}}_L^C\left(k\right),\mu \left(k+1\right)=\mu \left(k\right)\eta , if J\left({\tilde{\mathit{\eta }}}_L^C\left(k\right)+\mathbf{\Delta }{\tilde{\mathit{\eta }}}_L^C\left(k\right)\right)>J\left({\tilde{\mathit{\eta }}}_L^C\left(k\right)\right)\end{array}\right.$$

where k is the iteration number; I∈R^6×6 is the unity diagonal matrix; μ(k) is the step of adjusting; and 0 < η < 1 is the coefficient of step changing.

The vector ${\tilde{\mathit{\eta }}}_L^C$ is set in the onboard video camera CS and used for calculating the position of the target point relative to the AUV-follower. This calculation is performed in two stages.

In the first stage, the position and orientation of the leader in the CS $x_{FR}{y}_{FR}{z}_{FR}$ is calculated. The beginning of the CS $x_{FR}{y}_{FR}{z}_{FR}$ coincides with the beginning of the AUV-follower BCS, and the $z_{FR}$ axis is parallel to the z axis of the ACS (see Figure 1). These values are calculated using the expressions:

$${\tilde{\mathit{X}}}_L^{FR}={\mathit{R}}_F^R\left({\mathit{R}}_C^F{\tilde{\mathit{X}}}_L^C+{\mathit{X}}_C^F\right), {\mathit{R}}_L^{FR}={\mathit{R}}_F^R {\mathit{R}}_C^F{\mathit{R}}_L^C,$$

$${\tilde{\mathit{A}}}_L^{FR}=\left[\begin{array}{c}{\tilde{\phi }}_L^{FR}\\ {\tilde{\theta }}_L^{FR}\\ {\tilde{\psi }}_L^{FR}\end{array}\right]=\left[\begin{array}{c}\mathrm{atan}\left(r_{32}^{LFR}/r_{33}^{LFR}\right)\\ -\mathrm{atan}(r_{31}^{LFR}/{\left(r_{32}^{LFR}{}^2+r_{33}^{LFR}{}^2\right)}^{1/2}\\ \mathrm{atan}\left(r_{21}^{LFR}/r_{11}^{LFR}\right)\end{array}\right],$$

(11)

where ${\tilde{\mathit{X}}}_L^{FR}={\left[{\tilde{x}}_L^{FR}, {\tilde{y}}_L^{FR}, {\tilde{z}}_L^{FR}\right]}^T, {\tilde{\mathit{A}}}_L^{FR}={\left[{\tilde{\phi }}_L^{FR},{\tilde{\theta }}_L^{FR},{\tilde{\psi }}_L^{FR}\right]}^T$ are the vectors of the position and orientation angles of the AUV-leader in the CS $x_{FR}{y}_{FR}{z}_{FR}$; ${\mathit{R}}_C^F$ is the rotation matrix of the camera CS relative to the AUV-follower BCS; ${\mathit{R}}_F^R={\mathit{R}}_z{\left({\psi }_F\right)}^T{\mathit{R}}_F^A\left({\phi }_F,{\theta }_F,{\psi }_F\right)$ is the rotation matrix of the AUV-follower BCS relative to the CS $x_{FR}{y}_{FR}{z}_{FR}$; ${\mathit{R}}_F^A$ is the rotation matrix of the AUV-follower BCS relative to the ACS; ${\mathit{R}}_Z$ is the rotation matrix around the z axis of the ACS; ${\mathit{X}}_C^F$ is the coordinate vector in the AUV-follower BCS; $r_{ii}^{LFR}$ is the entry of orientation matrix ${\mathit{R}}_L^{FR}$ of the AUV-leader in the CS $x_{FR}{y}_{FR}{z}_{FR}$.

The estimation of the position of the target point ${\tilde{\mathit{X}}}_F^{*}$ for the AUV-follower relative to the AUV-leader in the CS $x_{FR}{y}_{FR}{z}_{FR}$ is determined by the expression:

$${\tilde{\mathit{X}}}_F^{*}={\mathit{R}}_z{\left({\tilde{\psi }}_L^{FR}\right)}^T{\mathit{X}}_F^{L*}+{\tilde{\mathit{X}}}_L^{FR},$$

(12)

where ${\mathit{X}}_F^{L*}$ is the desired position of the AUV-follower in the CS $x_{LR}{y}_{LR}{z}_{LR}$.

For the high-precision movement of the AUV-follower in conditions of the uncertainty and variability of their parameters, it is necessary to use high-precision control systems, the implementation of which requires a high-frequency updating of the control signals.

However, as the update of the vector ${\tilde{\mathit{X}}}_F^{*}$, as a rule, occurs with a low frequency, it is necessary to form a control signal with an increased frequency at intervals between the updates of the data coming from the video cameras to provide the required precision of the control. To do this, the changes of ${\tilde{\mathit{X}}}_F^{*}$ should be predicted using information about the speeds and accelerations of these changes. However, it is not possible to predict changes in the vectors ${\tilde{\mathit{X}}}_F^{*}$ precisely enough, considering the significant errors in calculating the pixel coordinates of the light beacons and the inaccuracies in determining the parameters of the onboard video cameras. This leads to a reduction in the control precision of the AUV-follower. To eliminate this drawback, the use of the following approach for the improvement of the estimations of vector ${\tilde{\mathit{X}}}_F^{*}$ and its speeds and accelerations is proposed (see Figure 3).

This figure shows the positions of the AUV-leader and one AUV-follower at the four points in time (at the moment of updating data from the video camera). The calculated positions and orientations of the AUV-leader differ from the real ones. This leads to errors in determining the positions of the target points for the current positions of the follower (black circles ${\mathit{X}}_F^{*}$ near the dashed line are the true positions of the target points, and the white ones ${\tilde{\mathit{X}}}_F^{*}$ are positions calculated using Expressions (7)–(12)).

To reduce the influence of these errors on the precision of the AUV-followers’ control systems, it is required to include in these systems information about the speeds and accelerations of the movement of these target points [25]. This information can be obtained by approximating the sequence $\left\{{\tilde{\mathit{X}}}_F^{*}\left(0\right), {\tilde{\mathit{X}}}_F^{*}\left(-t_{\Delta }\right),\dots ,{\tilde{\mathit{X}}}_F^{*}\left(-M{t}_{\Delta }\right)\right\}$ of the previously calculated points ${\tilde{\mathit{X}}}_F^{* }$ by a smooth polynomial function. As result, this sequence can be described by the function ${\widehat{\mathit{X}}}_F^{*}\left(t, \mathit{\lambda }\right)$, graphically depicted in Figure 3 by a dashed line, where $\mathit{\lambda }\in {\mathit{R}}^{\mathit{N}}$ is the vector of coefficients of the polynomial function, N is the specified order of the polynomial. In this case, the points ${\widehat{\mathit{X}}}_F^{*}\left(-i{t}_{\Delta }\right), i=\left(\overline{0,M}\right)$ (corresponding to the gray circles on the dashed line) correspond to new estimates of the position of the point ${\mathit{X}}_F^{*}$ at the time $\left(-i{t}_{\Delta }\right)$.

It is better to use the least squares method to calculate the vector λ. This method minimizes the sum of the distances between the points ${\tilde{\mathit{X}}}_F^{*}\left(-i{t}_{\Delta }\right)$ и ${\widehat{\mathit{X}}}_F^{*}\left(-i{t}_{\Delta }\right), i=\left(\overline{0,M}\right)$. Therefore, the obtained analytical description of the approximating curve (dashed line in Figure 3) describes the movement of the target point ${\mathit{X}}_F^{*}$.

As the vectors ${\tilde{\mathit{X}}}_F^{*}$ are calculated by taking into account the corresponding positions of the AUV-follower, the sequence $\left\{{\tilde{\mathit{X}}}_F^{*}\left(0\right), {\tilde{\mathit{X}}}_F^{*}\left(-t_{\Delta }\right),\dots ,{\tilde{\mathit{X}}}_F^{*}\left(-M{t}_{\Delta }\right)\right\}$ will depend not only on the movement of the AUV-leader, but also on the follower’s own movements relative to the leader. This complicates the prediction of the movement of points ${\mathit{X}}_F^{*}$ and requires consideration of the previous movement of the follower. The positions of the points ${\tilde{\mathit{X}}}_F^{*}\left(-i{t}_{\Delta }\right), i=\left(\overline{0,M}\right)$ should be recalculated relative to the current position and orientations of the BCS of the follower to facilitate the solution to this problem. This allows the separation of the movements of the points ${\mathit{X}}_F^{*}$, determined by the movement of the AUV-leader and the movements of the AUV-followers themselves. To do this, it is necessary to consider the speed and orientation of the followers at the appropriate time moments.

This approach allows the performance of the following:

-

To reduce the calculation errors of ${\mathit{X}}_F^{*}$ by smoothing the sequence of the values of ${\tilde{\mathit{X}}}_F^{*}$ on a given period by means of a polynomial function;

-

To estimate the values of the elements of the vectors of position ${\widehat{\mathit{X}}}_F^{*}$, the velocities ${\widehat{\mathit{\upsilon }}}_F^{*}$, and the accelerations ${\widehat{\mathit{a}}}_F^{*}$ of the movement of the point ${\mathit{X}}_F^{*}$ without entering additional delays;

-

To obtain an analytical description of the movement of the point ${\mathit{X}}_F^{*}$, which is used to predict its movement relative to the follower in the time interval between updating data from the onboard camera.

It is obvious that approximation errors may appear with a sharp change in the direction of movement of the AUV-leader, such as, for example, when the leader bypasses obstacles that unexpectedly appear in the way of its movement. However, it will be shown later in the simulation that these errors do not lead to significant deviations of the followers from their desired position.

Below is a description of the proposed method.

Let us assume that during the movement of the AUV group, for each follower the array Ω of size M + 1 is formed, and the following data are stored with period $t_{\Delta }$ in each of its elements:

-

The vector ${\tilde{\mathit{X}}}_F^{*}$ of the coordinates of the target point of the AUV-follower in the CS $x_{FR}{y}_{FR}{z}_{FR}$ (Figure 3);

-

The vector ${\mathit{\upsilon }}_{Fl}={\left[{\upsilon }_{Fx}, {\upsilon }_{Fy}, {\upsilon }_{Fz}\right]}^T$ of the linear speed of the AUV-follower in its BCS;

-

The vector ${\mathit{A}}_F={\left[{\phi }_F, {\theta }_F, {\psi }_F\right]}^T$ of the orientation angles of the AUV-follower in the ACS.

As a result, the i-th element of array $\mathbf{\Omega }$ contains the vectors ${\tilde{\mathit{X}}}_F^{*}\left(i\right), {\mathit{\upsilon }}_{Fl}\left(i\right), {\mathit{A}}_F\left(i\right),$ where i = 0 corresponds to the current time $t=0$, and i = M corresponds to time $t=-M{t}_{\Delta }$.

If new data are received from the onboard video camera, then the array Ω is updated: outdated data from the end of the array Ω are removed, and new ones are added to the beginning.

The vector ${\tilde{\mathit{X}}}_F^{*}\left(i\right)$ stored in the array Ω, corresponding to the position of the target point at the i-th moment of time, are recalculated into the CS $x_{FR}{y}_{FR}{z}_{FR}$ at the time corresponding to i = 0 (see Figure 3). This recalculation occurs iteratively from the first element of the array Ω to the last one according to the formulas:

$${\mathit{X}}_F\left(i+1\right)={\mathit{X}}_F\left(i\right)-{\mathit{R}}_z{\left({\psi }_0\right)}^T{\mathit{R}}_F^A\left(i\right){\mathit{\upsilon }}_{Fl}\left(i\right)t_{\Delta }),$$

(13)

$${\tilde{\mathit{X}}}_F^{0*}\left(i\right)={\mathit{R}}_z{\left({\psi }_1-{\psi }_0\right)}^T{\tilde{\mathit{X}}}_F^{*}\left(i\right)+{\mathit{X}}_F\left(i\right), i=\left(\overline{0,M}\right)$$

where ${\tilde{\mathit{X}}}_F^{0*}\left(i\right)={\left[{\tilde{x}}_F^{0*}\left(-i{t}_{\Delta }\right), y_F^{0*}\left(-i{t}_{\Delta }\right), {\tilde{z}}_F^{0*}\left(-i{t}_{\Delta }\right), \right]}^T, i=\left(\overline{0,M}\right)$ are the coordinates of the target point in the time moment $-i{t}_{\Delta }$ in the CS $x_{FR}{y}_{FR}{z}_{FR}$ when t = 0; ${\mathit{X}}_F\left(i\right)$ is the position of the AUV-follower in the time moment $-i{t}_{\Delta }$ in the CS $x_{FR}{y}_{FR}{z}_{FR}$ when t = 0 (${\mathit{X}}_F\left(0\right)={\left(0,0,0\right)}^T)$;

${\mathit{R}}_F^A\left(i\right)=\left[\begin{array}{ccc}{c}_{\psi i}{c}_{\theta i}& -s_{\psi i}{c}_{\phi i}+c_{\psi i}{s}_{\theta i}{s}_{\phi i}& s_{\psi i}{s}_{\phi i}+c_{\psi i}{c}_{\phi i}{s}_{\theta i}\\ s_{\psi i}{c}_{\theta i}& c_{\psi i}{c}_{\phi i}+s_{\phi i}{s}_{\theta i}{s}_{\psi i}& -c_{\psi i}{s}_{\phi i}+s_{\theta i}{s}_{\psi i}{c}_{\phi i}\\ -s_{\theta i}& c_{\theta i}{s}_{\phi i}& c_{\theta i}{c}_{\varphi i}\end{array}\right]$ is the rotation matrix of AUV-follower BCS with respect to the ACS [25]; the indexes $\phi i, \theta i, \psi i$ correspond to the elements of vector ${\mathit{A}}_F\left(i\right)$; ${\psi }_0 \mathrm{and} {\psi }_i$ are the values of the yaw angle in time moments 0 and $-i{t}_{\Delta },$ respectively.

This procedure forms a new array of ${\mathbf{\Omega }}_X^{*}$, containing data on the time changes of the vectors ${\tilde{\mathit{X}}}_F^{0*}\left(i\right), i=\left(\overline{0,M}\right)$ of the coordinates of the target points relative to the current positions of the SC $x_{FR}{y}_{FR}{z}_{FR}$. As a result, the sequence of target points in array ${\mathbf{\Omega }}_X^{*}$ can be approximated using polynomials of order N. That is, changes in the coordinates $x_F^{*}\left(t\right)$, $y_F^{*}\left(t\right)$, $z_F^{*}\left(t\right)$ of the vectors ${\mathit{X}}_F^{*}\left(t\right)$ in time can be described using the expressions:

$$\begin{gathered} x_F^{*}\left(t\right)=x_F^{0*}+{\upsilon }_{Fx}^{0*}t+a_{Fx}^{0*}{t}^2/2+k_{x1}{t}^3+\dots +k_{xN-2}{t}^N, \\ {y}_F^{*}\left(t\right)=y_F^{0*}+{\upsilon }_{Fy}^{0*}t+a_{Fy}^{0*}{t}^2/2+k_{y1}{t}^3+\dots +k_{yN-2}{t}^N, \\ {z}_F^{*}\left(t\right)=z_F^{0*}+{\upsilon }_{Fz}^{0*}t+a_{z0}^{*}{t}^2/2+k_{x1}{t}^3+\dots +k_{xN-2}{t}^N, \end{gathered}$$

(14)

where $x_F^{0*}, y_F^{0*}, z_F^{0*}$ are the initial values of the coordinates of the target point; ${\upsilon }_{xF}^{0*}, {\upsilon }_{yF}^{0*}, {\upsilon }_{zF}^{0*}$ are the initial values of the speeds of these coordinates; a $a_{xF}^{0*}, a_{yF}^{0*}, a_{zF}^{0*}$ are the initial values of their accelerations; k_x1,…, k_xN−2, k_y1,…, k_yN−2, k_z1,…, k_zN−2 are the coefficients of the polynomial approximant.

Furthermore, to reduce the description, we consider the method of determining the coefficients of the polynomial (14) only for the coordinate $x_F^{*}\left(t\right)$. The coefficients for the other two polynomials are determined similarly. It is better to use the least mean squares method to determine the values $x_F^{0*}, {\upsilon }_{Fx}^{0*}, a_{Fx}^{0*}, k_{x1},\dots , k_{xN-2}$. Let us rewrite the equality (14) in the matrix form:

$$x_F^{*}\left(t\right)=\left[\begin{array}{ccc}{x}_F^{0*}& {\upsilon }_{Fx}^{0*}& a_{Fx}^{0*} \end{array} \begin{array}{ccc}{k}_{x1}& \dots & k_{xN-2}\end{array}\right]{\left[\begin{array}{ccc}1& t& t^2/2\end{array} \begin{array}{ccc}{t}^3& \cdots & t^N\end{array}\right]}^T$$

(15)

The measurement sequence $x_F^{*}\left(t\right)$ is set by vector S:

$$\mathit{S}={\left[{\tilde{x}}_F^{0*}\left(0\right),{\tilde{x}}_F^{0*}\left(-t_{\Delta }\right),{\tilde{x}}_F^{0*}\left(-2t_{\Delta }\right),\dots , {\tilde{x}}_F^{0*}\left(-M{t}_{\Delta }\right)\right]}^T.$$

(16)

The form of vector S allows the determination of the first three coefficients of the polynomial (15), which coincides with the estimations of the values of the vector $X_F^{*}$ and its speeds and accelerations at the current time. This allows them to be used immediately in further calculations without additional transformations.

One can write, with consideration of Expressions (15) and (16):

$$\mathit{S}=\mathbf{\Phi }\mathit{\lambda },$$

(17)

where $\mathbf{\Phi }=\left[\begin{array}{cccccc} 1& 0& 0& 0& \dots & 0\\ 1& -t_{\Delta }& {t_{\Delta }}^2/2& -{t_{\Delta }}^3& \dots & (-t_{\Delta }{)}^N\\ 1& -2t_{\Delta }& (2t_{\Delta }{)}^2/2& (-2t_{\Delta }{)}^3& \dots & (-2t_{\Delta }{)}^N\\ \dots & \dots & \dots & \dots & \dots & \dots \\ 1& -M{t}_{\Delta }& (M{t}_{\Delta }{)}^2/2& (-M{t}_{\Delta }{)}^3& \dots & (M{t}_{\Delta }{)}^N\end{array}\right]\in R^{M\times N}$, $\lambda ={\left[\begin{array}{ccc}{x}_0^{*}& {\upsilon }_0^{*}& a_0^{*} \end{array} \begin{array}{ccc}{k}_1& \dots & k_{N-2}\end{array}\right]}^T\in R^{N+1}$ is the vector of the coefficients of the polynomial.

The estimate of the coefficient vector λ with consideration of (17) will be calculated by the formula [28]:

$$\widehat{\mathit{\lambda }}={\left[{\Phi }^T\Phi \right]}^{-1}{\Phi }^T\mathit{S}.$$

(18)

If the period of updating the data into the vector S is constant, then the matrix ${\left[{\Phi }^T\Phi \right]}^{-1}{\Phi }^T$ is constant too, and it can be calculated in advance. This significantly reduces the computational complexity of the created algorithm. The use of the proposed algorithm requires the choosing of the size M of the array Ω and the order N of the approximating polynomial.

The value of N determines the accuracy of the approximation and depends on the types of trajectories of the AUV-leader. The higher the order of the approximating polynomial, the more accurate the estimation of the process of changing the coordinates of the target point will be, but less noise will be suppressed in the estimates of the velocities and accelerations. As most of the trajectories of the AUV can be described by curves of the second or third order, the value N can be assumed to be equal to 3 or 4.

The more data are used for the approximation, the more accurate are the given estimates until the type of trajectory of the AUV-leader does not change, but with sharp changes in the trajectories of the leader, the errors also increase. In addition, the large amount of data increases the computational complexity of the proposed algorithm. Therefore, the data series used for the approximation should be chosen for a duration of several seconds because the trajectory of the AUV-leader during such a time interval cannot change significantly.

Expressions (13)–(18) estimate the vectors ${\widehat{\mathit{X}}}_F^{0*}={\left[{\widehat{x}}_F^{0*}, {\widehat{y}}_F^{0*}{\widehat{z}}_F^{0*}\right]}^T$ positions, ${\widehat{\mathit{\upsilon }}}_F^{0*}={\left[{\widehat{\upsilon }}_{Fx}^{0*}, {\widehat{\upsilon }}_{Fy}^{0*},{\widehat{\upsilon }}_{Fz}^{0*}\right]}^T$ speeds, and ${\widehat{\mathit{a}}}_F^{0*}={\left[{\widehat{a}}_{Fx}^{0*}, {\widehat{a}}_{Fy}^{0*},{\widehat{a}}_{Fz}^{0*}\right]}^T$ accelerations of the movement of the target points for the follower at the current time. They are set in the CS $x_{FR}{y}_{FR}{z}_{FR}$.

These estimates do not consider the real movements of the follower when forming input signals for their onboard control systems. At the same time, these estimates can be used at the next stage, which consists of the prediction of the movements of the target points in the time intervals between the updates of data from the video cameras, with consideration of the real movements of the AUV-follower.

3.2. The Prediction Algorithm of Changing Position, Speed, and Accelerations of Target Point Movement

During the process of prediction, we will assume that changes of the positions of the target points relative to the positions of the follower at the time of the last receipt of data from the video cameras are described by the expression:

$${\widehat{\mathit{X}}}_F^{*}\left(t\right)={\widehat{\mathit{X}}}_F^{0*}+{\widehat{\mathit{\upsilon }}}_F^{0*}t+{\widehat{\mathit{a}}}_F^{0*}{t}^2/2.$$

(19)

In Expression (19), there are no coefficients at high degrees of the approximating polynomial (15) as the prediction time intervals are rather small, and the main influence in the polynomial will be exerted by its first terms.

The coordinates of the target point in the CS $x_{FR}{y}_{FR}{z}_{FR}$, with consideration of the AUV-follower’s movement, are defined by expression:

$${\mathit{X}}_{F, p}^{*}\left(t\right)={\mathit{R}}_z{\left({\psi }_{\Delta }\right)}^T\left({\widehat{\mathit{X}}}_F^{*}\left(t\right)-{\mathit{X}}_{F\Delta }\left(t\right)\right),$$

(20)

where ${\mathit{R}}_z\left({\psi }_{\Delta }\right)=\left[\begin{array}{ccc}\cos {\psi }_{\Delta }& -\sin {\psi }_{\Delta }& 0\\ \sin {\psi }_{\Delta }& \cos {\psi }_{\Delta }& 0\\ 0& 0& 1\end{array}\right]$; ${\psi }_{\Delta }={\psi }_F-{\psi }_{F0}$ is the increment of the yaw angle of the AUV-follower from the beginning the prediction process, and ${\mathit{X}}_{F\Delta }$ is the follower’s movement from the beginning of the prediction process. The value ${\mathit{X}}_{F\Delta }$ is calculated by the formula:

$${\mathit{X}}_{F\Delta }={{\displaystyle \int }}_{t_0}^{t_0+\Delta }{\mathit{R}}_z{\left({\psi }_{F0}\right)}^T{\mathit{R}}_F^A{\mathit{\upsilon }}_{F}dt,$$

where $\Delta$ is the time interval of the prediction.

Differentiating Equation (20), we obtain:

$${\mathit{\upsilon }}_{F,p}^{*}\left(t\right)={\mathit{R}}_z{\left({\psi }_{\Delta }\right)}^T{\widehat{\mathit{\upsilon }}}_F^{*}\left(t\right)+\dot{{\mathit{R}}_z}{\left({\psi }_{\Delta }\right)}^T\left({\widehat{\mathit{X}}}_F^{*}\left(t\right)-{\mathit{X}}_{F\Delta }\right)\dot{{\psi }_F}-{\mathit{R}}_F^R{\mathit{\upsilon }}_F,$$

(21)

where ${\widehat{\mathit{\upsilon }}}_F^{*}\left(t\right)={\widehat{\mathit{\upsilon }}}_F^{0*}+{\widehat{\mathit{a}}}_F^{0*}t$; $\dot{{\mathit{R}}_z}\left({\psi }_{\Delta }\right)=\left[\begin{array}{ccc}-{\mathrm{s}}_{\psi \Delta }& -c_{\psi \Delta }& 0\\ c_{\psi \Delta }& -s_{\psi \Delta }& 0\\ 0& 0& 0\end{array}\right]$, ${\mathit{R}}_F^R={\mathit{R}}_z{\left({\psi }_{\Delta }\right)}^T{\mathit{R}}_z{\left({\psi }_{F0}\right)}^T{\mathit{R}}_F^A{\mathit{\upsilon }}_F$.

Differentiating Equation (21), we obtain:

$$\begin{gathered} {\mathit{a}}_{F,p}^{*}\left(t\right)=\mathit{R}{\left({\psi }_{\Delta }\right)}^T{\widehat{\mathit{a}}}_F^{0*}\left(t\right)+\dot{\mathit{R}}{\left({\psi }_{\Delta }\right)}^T\left( \left({\widehat{\mathit{\upsilon }}}_F^{*}\left(t\right)-{\mathit{R}}_z{\left({\psi }_{F0}\right)}^T {\mathit{R}}_F^A{\mathit{\upsilon }}_F\right){\dot{\psi }}_F+\left({\mathit{X}}_F^{*}-{\mathit{X}}_{F\Delta }\right)\ddot{{\psi }_F}\right) \\ +\ddot{\mathit{R}}{\left({\psi }_{\Delta }\right)}^T\left({\widehat{\mathit{X}}}_F^{*}\left(t\right)-{\mathit{X}}_{F\Delta }\right){\dot{\psi }}_F-{\mathit{R}}_F^R{\mathit{a}}_F-{\mathit{R}}_z{\left({\psi }_F\right)}^T{\dot{\mathit{R}}}_F^A{\mathit{\upsilon }}_F \end{gathered}$$

(22)

where ${\dot{\mathit{R}}}_F^A={\dot{\mathit{R}}}_x{\mathit{R}}_y{\mathit{R}}_z{\dot{\phi }}_F+{\mathit{R}}_x{\dot{\mathit{R}}}_y{\mathit{R}}_z{\dot{\theta }}_F+{\mathit{R}}_x{\mathit{R}}_y{\dot{\mathit{R}}}_z{\dot{\psi }}_F$ $\ddot{\mathit{R}}\left({\psi }_{\Delta }\right)=\left[\begin{array}{ccc}-{\mathrm{c}}_{\psi \Delta }& s_{\psi \Delta }& 0\\ -s_{\psi \Delta }& -c_{\psi \Delta }& 0\\ 0& 0& 0\end{array}\right];$ ${\mathit{R}}_x, {\mathit{R}}_y, {\mathit{R}}_z$ are matrixes of the elementary rotations around the axes of the ACS.

In Expressions (21) and (22), the derivatives of the angles are used, which depend on the angular velocities measured by the onboard sensors of the AUV-follower, using the following expression [22]:

$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& s_{\phi }{t}_{\theta }& c_{\phi }{t}_{\theta }\\ 0& c_{\phi }& -s_{\phi }\\ 0& s_{\phi }/c_{\theta }& c_{\phi }/c_{\theta }\end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right].$$

Considering the presence of moments of stability of the AUV, it is assumed that ${\phi }_F\approx 0$. So, we can simplify the calculations in Expressions (21) and (22) by using the following expressions:

$$\dot{\phi }={\omega }_x+t_{\theta }{\omega }_z, \dot{\theta }={\omega }_y, \dot{\psi }=\frac{{\omega }_z}{c_{\theta }}, \ddot{\psi }=\frac{{\dot{\omega }}_z+{\omega }_y{\omega }_z{t}_{\theta }}{c_{\theta }}.$$

Equations (20)–(22) are used to estimate the speeds and accelerations of the target points relative to the AUV-follower in the time intervals between updating the data about the current position and the orientation of the AUV-leader.

The prediction of changes in the coordinates ${\mathit{X}}_{F, p}^{*}\left(t\right)$ when considering the current movement of the follower is proposed to be performed using a Kalman filter algorithm. Its block diagram is shown in Figure 4.

At the initialization stage of this algorithm, the initial values of the covariance matrices ${\mathit{P}}_p\in R^{6\times 6}$, ${\mathit{Q}}_p\in R^{6\times 6}$, ${\mathit{R}}_p\in R^{6\times 6}$ are set. The initial values of the filter state vector ${\mathit{\chi }}_{F,p}^{*}\left(0\right)=\left[{\check{\mathit{\upsilon }}}_{F,p}^{*};{\check{X}}_{F,p}^{*}\right]\in R^6$, containing estimates of the speed and the position of the target points relative to the follower, are usually assumed to be zero.

After initialization, the main filter cycle works, which contains two stages: the prediction stage, where the positions and speeds of the movement of the target point relative to the follower are estimated with a desired period, and the correction stage, where the previously predicted values are corrected based on the data received from the onboard video cameras.

The following expressions are used at the prediction stage:

$${\mathit{\chi }}_{F,p}^{*}\left(k\right)={\mathit{A}}_p{\mathit{\chi }}_{F,p}^{*}\left(k-1\right)+{\mathit{B}}_p{\mathit{a}}_{F,p}^{*}\left(k\right), {\mathit{P}}_p^{+}\left(k\right)={\mathit{A}}_p{\mathit{P}}_p^{+}\left(k-1\right){\mathit{A}}_p^T+\mathit{Q},$$

(23)

where ${\mathit{A}}_p=\left[\begin{array}{c}\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array} \begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\\ \begin{array}{ccc}{h}_{\Delta }& 0& 0\\ 0& h_{\Delta }& 0\\ 0& 0& h_{\Delta }\end{array} \begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\end{array}\right]$; ${\mathit{B}}_p=\left[\begin{array}{c}\begin{array}{ccc}{h}_{\Delta }& 0& 0\\ 0& h_{\Delta }& 0\\ 0& 0& h_{\Delta }\end{array}\\ \begin{array}{ccc}\frac{h_{\Delta }^2}{2}& 0& 0\\ 0& \frac{h_{\Delta }^2}{2}& 0\\ 0& 0& \frac{h_{\Delta }^2}{2}\end{array}\end{array}\right]$; $h_{\Delta }<t_{\Delta }$ is the period of calculation of the predicted value of vector ${\mathit{\chi }}_{F,p}^{*}$; ${\mathit{P}}_p^{+}$ is the covariation matrix, calculated at the prediction stage.

It can be seen from (23) that the input signal for the model is the acceleration vector of the target point, obtained based on Expression (22).

The prediction stage continues until the updated data is received from the onboard video camera. Vector ${\mathit{\chi }}_{F,p}^{*}$ is the estimation of the error of the position and speed of the AUV-follower and is used to generate the control signals for their movements.

After updating the data from the video camera, the correction stage begins, in which the previously predicted values are corrected using the latest measurements.

The measurement model has following form:

$$\widehat{\mathit{Z}}\left(k\right)={\mathit{\chi }}_{F,p}^{*}\left(k\right),$$

(24)

where $\widehat{\mathit{Z}}=\left[{\widehat{\mathit{\upsilon }}}_F^{0*}; {\widehat{\mathit{X}}}_F^{0*} \right]$ is the vector of the current estimations of the position and speed of the target point relative to the AUV-follower.

This correction occurs by the following expressions:

$$\begin{gathered} \mathit{Y}\left(k\right)=\mathit{Z}\left(k\right)-{\mathit{\chi }}_{F,p}^{*}\left(k\right) \\ \mathit{H}\left(k\right)={\mathit{P}}_p^{+}\left(k\right)+\mathit{R}, \\ \mathit{K}\left(k\right)={\mathit{P}}_p^{+}\left(k\right)\mathit{H}{\left(k\right)}^{-1}, \\ {\check{\mathit{\chi }}}_{F,p}^{*}\left(k\right)={\mathit{\chi }}_{F,p}^{*}\left(k\right)+\mathit{K}\left(k\right)\mathit{Y}\left(k\right), \\ {\check{\mathit{P}}}_p\left(k\right)=\left(I-\mathit{K}\left(k\right)\right){\mathit{P}}_p\left(k\right) \end{gathered}$$

(25)

where ${\check{\mathit{\chi }}}_{F,p}^{*}\left(k\right)$, ${\check{\mathit{P}}}_p\left(k\right)$ is the state vector and the covariation matrix corrected by means of the new data received from the onboard camera.

After the correction, the prediction stage begins, in which the values of ${\check{\mathit{\chi }}}_{F,p}^{*}\left(k\right)$, ${\check{\mathit{P}}}_p\left(k\right)$ are used as the initial values of the state vector and the covariance matrix.

After determining the errors of the positions and speeds of the follower, and considering its movement, these errors are used to generate the control signals.

4. The Synthesis of AUV-Follower Control System

The AUV-follower must move oriented towards its target point so that this point is on the $x_f$ axis of the BCS that coincides with its longitudinal axis. So, the desired values of the orientation angles of pitch ${\theta }_F^{*}$ and yaw ${\psi }_F^{*}$ and the rate of their change are calculated using the formulas:

$${\theta }_F^{*}=\mathrm{atan}\left(z_F^{*}/\left|x_F^{*}\right|\right), {\psi }_F^{*}=\mathrm{atan}\left(y_F^{*}/x_F^{*}\right)+{\psi }_F, {\omega }_y^{*}=\frac{\left|x_F^{*}\right|{\upsilon }_{Fz}^{*}-{\upsilon }_{Fx}^{*}{z}_F^{*}sign\left(x_F^{*}\right)}{x_F^{*}{}^2+z_F^{*}{}^2}={\upsilon }_{Fz}^{*}\cos \left({\theta }_F^{*}\right)-{\upsilon }_{Fx}^{*}, \sin \left({\theta }_F^{*}\right),$$

$${\omega }_z^{*}=\frac{x_F^{*}{\upsilon }_{Fy}^{*}-{\upsilon }_{Fx}^{*}{y}_F^{*}}{x_F^{*}{}^2+y_F^{*}{}^2}+{\dot{\psi }}_F={\upsilon }_{Fy}^{*}\cos \left({\psi }_F^{*}\right)-{\upsilon }_{Fx}^{*}, \sin \left({\psi }_F^{*}\right)+{\dot{\psi }}_F,$$

(26)

where ${\omega }_y^{*}, {\omega }_z^{*}$ are the rate of change of the desired values of angles ${\theta }_F^{*}$ and ${\psi }_F^{*}$. When the AUV-follower is in a given neighborhood of the target point, the values of the desired angles of its orientation are equal to ${\theta }_F^{*}=z_F^{*},{\psi }_F^{*}=y_F^{*}$.

The control signals of the AUV-follower are set in the BCS, and the positions of their target points ${\check{\mathit{X}}}_{F,p}^{*}$ are formed in the CS $x_{FR}{y}_{FR}{z}_{FR}$. Therefore, the movements of the follower, considering the models in (2), can be described by the equations:

$$\left[\begin{array}{c}{\dot{\check{\mathit{X}}}}_{F,p}^{*}\\ {\dot{\mathit{E}}}_A^{*}\end{array}\right]=\left[\begin{array}{c}{\check{\mathit{\upsilon }}}_{F,p}^{*}\\ {\mathit{\omega }}^{*}\end{array}\right]-\left[\begin{array}{cc}{\mathit{R}}_F^R& 0\\ 0& {\mathit{R}}_{\mathit{\omega }}\end{array}\right]{\mathit{\upsilon }}_{\mathit{F}},$$

(27)

$$\mathit{M}{\dot{\mathit{\upsilon }}}_F+\left(\mathit{C}\left(\mathit{M},{\mathit{\upsilon }}_{\mathit{F}}\right)+\mathit{D}\left(d_1,d_2,{\mathit{\upsilon }}_{\mathit{F}}\right)\right){\mathit{\upsilon }}_{\mathit{F}}+\mathit{g}\left(\xi \right)=\mathit{\tau },$$

where ${\mathit{\upsilon }}_F^{*}={\check{\mathit{\upsilon }}}_{F,p}^{*}+{\mathit{R}}_F^R{\mathit{\upsilon }}_F$ is the vector of the desired speed of the AUV-follower in the CS $x_{FR}{y}_{FR}{z}_{FR}$; ${\mathit{E}}_A^{}=\left[\begin{array}{c}0\\ {\theta }_F^{*}\\ {\psi }_F^{*}\end{array}\right]-\left[\begin{array}{c}{\phi }_F\\ {\theta }_F\\ {\psi }_F\end{array}\right]$ is the vector of error of the orientation angles; ${\mathit{\omega }}^{*}={\left[0, {\omega }_y^{*}, {\omega }_z^{*}\right]}^T$ is the vector of the desired speed of the follower; ${\mathit{R}}_{\omega }=\left[\begin{array}{ccc}1& 0& t_{\theta }\\ 0& 1& 0\\ 0& 0& 1/c_{\theta }\end{array}\right]$.

Multiplying the first equation of system (27) by the matrix ${\mathit{R}}_R={\left[\begin{array}{cc}{\mathit{R}}_F^R& 0\\ 0& {\mathit{R}}_{\mathit{\omega }}\end{array}\right]}^{-1}$, we obtain

$$\left[\begin{array}{c}{\dot{\mathit{E}}}_X\\ {\dot{\mathit{E}}}_{AF}^{}\end{array}\right]=\left[\begin{array}{c}{\mathit{\upsilon }}^{*} \\ {\mathit{\omega }}^{*}\end{array}\right]-{\mathit{\upsilon }}_F,$$

(28)

$$\mathit{M}{\dot{\mathit{\upsilon }}}_F+\left(\mathit{C}\left(\mathit{M},{\mathit{\upsilon }}_F\right)+\mathit{D}\left(d_1,d_2,{\mathit{\upsilon }}_F\right)\right){\mathit{\upsilon }}_F+\mathit{g}\left(\xi \right)=\mathit{\tau },$$

where ${\dot{\mathit{E}}}_X$, ${\dot{\mathit{E}}}_{\mathit{A}\mathit{F}}$ are the derivatives of the errors of the linear and angular speeds in the BCS of the follower, and ${\mathit{\upsilon }}^{*}, {\mathit{\omega }}_{}^{*}$ are the vectors of desired linear and angular speeds in the BCS of the follower.

To control the movements of the follower, a combined control system was used. This control system consists of a nonlinear controller that forms the thrust ${\tau }_n$, which ensures high accuracy of the follower movement when its parameters have the nominal values, and a sliding controller that forms the thrust ${\mathit{\tau }}_s$, which compensates for the deviations of the follower parameters in the ranges (6) [29]. At the same time, the total thrust of the follower forms by the following expression

$$\mathit{\tau }={\mathit{\tau }}_n+{\mathit{\tau }}_s$$

(29)

The nonlinear controller has the following form [26]:

$$\alpha ={\mathit{V}}^{*}+\mathsf{\Lambda }\mathit{E},$$

$${\mathit{\tau }}_n={\mathit{M}}_n\dot{\alpha }+\left({\mathit{C}}_n+{\mathit{D}}_n\right)\alpha +\mathit{g},$$

(30)

where $\mathsf{\Lambda }$ is the diagonal matrix of the positive coefficients; $\mathit{E}=\left[\begin{array}{c}{\mathit{E}}_X\\ {\mathit{E}}_{AF}^{}\end{array}\right], {\mathit{V}}^{*}=\left[\begin{array}{c}{\mathit{\upsilon }}^{*}\\ {\mathit{\omega }}^{*}\end{array}\right]$.

The sliding controller has the following form:

$${\mathit{\tau }}_s=K_s\left|\mathit{E}\right|sign\left(s\right), s={\mathit{E}}_V+\mathsf{\Lambda }\mathit{E},$$

(31)

where ${\mathit{E}}_V={\mathit{V}}^{*}-{\mathit{\upsilon }}_F$; $K_s$ is the diagonal matrix of the positive coefficients.

Let us define the following Lyapunov function $V=0.5s^{T}Ms$. The derivative of V, with consideration of (2), has the following form:

$$\dot{V}=s^T\mathit{M}\dot{s}=s^T\mathit{M}\left({\dot{\mathit{V}}}^{*}-{\dot{\mathit{\upsilon }}}_F+\mathsf{\Lambda }\dot{\mathit{E}}\right)=s^T\left(\mathit{M}{\dot{\mathit{V}}}^{*}+\left(\mathit{C}+\mathit{D}\right){\mathit{\upsilon }}_F+\mathit{g}+\mathit{M}\Lambda \dot{\mathit{E}}-\mathit{\tau }\right).$$

(32)

Substituting Expressions (28)–(31) in (32), and considering the model (5), we obtain:

$$\begin{gathered} \dot{V}=s^T\left(\mathit{M}{\dot{\mathit{V}}}^{*}+\left(\mathit{C}+\mathit{D}\right){\mathit{\upsilon }}_F+\mathit{g}+\mathit{M}\Lambda \dot{\mathit{E}}-{\mathit{M}}_n\dot{\alpha }-\left({\mathit{C}}_n+{\mathit{D}}_n\right)\alpha -\mathit{g}-{\mathit{\tau }}_s\right)=s^T\left({\mathit{M}}_{\Delta }\dot{\alpha }+\left({\mathit{C}}_{\Delta }+{\mathit{D}}_{\Delta }\right){\mathit{\upsilon }}_F- \\ \left({\mathit{C}}_n+{\mathit{D}}_n\right)s-{\mathit{\tau }}_s\right)=s^T\left({\mathit{M}}_{\Delta }\dot{\alpha }+\left({\mathit{C}}_{\Delta }+{\mathit{D}}_{\Delta }\right){\mathit{\upsilon }}_F-K_s\left|\mathit{E}\right|sign\left(s\right)\right)-s^T{\mathit{C}}_{n}s-s^T{\mathit{D}}_{n}s. \end{gathered}$$

(33)

As $s^T{\mathit{C}}_{n}s\equiv 0$ [25] and $s^T{\mathit{D}}_{n}s\ge 0$, to then fulfill the condition $\dot{V}<0$ the coefficient $K_s$ was chosen in accordance with the inequality

$$K_{sii}>\max {\left({\mathit{M}}_{\Delta }\dot{\alpha }+\left({\mathit{C}}_{\Delta }+{\mathit{D}}_{\Delta }\right){\mathit{\upsilon }}_F\right)}_i/E_{imin}$$

(34)

where $E_{imin}$ is the acceptable dynamical error of control.

The variables ${\mathit{E}}_V$ and E for the synthesis of the control law (29)–(31) are formed using Expressions (20)–(25).

It is shown below that the synthesized control law (29)–(31), (34) provides the AUV-follower with the desired dynamic properties for any changes in its parameters in the ranges (6).

5. The Simulation Results

To check the features of the operation of the proposed system, including Expressions (18), (22)–(25) for predicting the movement of the target point and the control system (29), (30), and (34), the mathematical simulation was carried out. During this simulation the AUV-leader moved along the spatial trajectory.

The simulation was carried out using the CoppeliaSim environment, which allows us to simulate the work of the different robotic systems and their equipment in three-dimensional scenes. In this environment, the AUV-leader with light beacons installed on its body and the AUV-follower with an onboard video camera mounted on it were described. The work of the camera simulated in this environment. The image of the leader from the onboard camera of the follower was transmitted to an external program for further processing using Expressions (7)–(11). The movements of the leader and the follower and the work of the proposed system were studied in this external program. The calculated positions and orientations of the AUV-leader and follower were transferred to the CoppeliaSim environment to place their models three-dimensionally in the calculated positions with the calculated orientation.

The dynamics of the AUV-follower during simulation were described by a system of differential Equation (2), with the following nominal parameters [29]: $M=diag\left(170 \mathrm{kg}, 170 \mathrm{kg}, 170 \mathrm{kg}, 10.2 {\mathrm{kgm}}^2, 23.4 {\mathrm{kgm}}^2,23.4 {\mathrm{kgm}}^2\right)$, d₁ = (18 Ns/m, 105 Ns/m, 105 Ns/m, 20 Nms, 80 Nms, 80 Nms), d₂ = (18 Ns²/m², 105 Ns²/m², 105 Ns²/m², 20 Nms², 80 Nms², 80 Nms²), $x_G=0.0 \mathrm{m}, y_G=0.0 \mathrm{m}, z_G=-0.01 \mathrm{m}, x_B=0.0 \mathrm{m}, y_B=0.0 \mathrm{m}, z_B=0.0 \mathrm{m}$, M_A = (15 kg, 185 kg, 185 kg, 5 kgm², 9.6 kgm², 9.6 kgm²). These parameters are used for the synthesis of the control system (29)–(34). It is supposed that the deviations of the AUV-follower’s parameters have the following values: $M_{\Delta }=diag(140 \mathrm{kg}, 140 \mathrm{kg}, 140 \mathrm{kg}, 4 {\mathrm{kgm}}^2,$ $7 {\mathrm{kgm}}^2,7 {\mathrm{kgm}}^2)$, $d_{\Delta 1}^{}$ =(9 Ns/m, 50 Ns/m, 50 Ns/m, 10 Nms, 30 Nms, 30 Nms), $d_{\Delta 2}^{}$ = (9 Ns²/m², 50 Ns²/m², 50 Ns²/m², 10 Nms², 30 Nms², 30 Nms²).

The parameters of the control system have the following values: $\mathsf{\Lambda }=diag\left(0.2, 0.2, 0.2, 0.2, 0.4, 0.4\right),$ K $=diag\left(100, 100, 100, 100, 100, 100\right])$.

The update period of the data received from the onboard video camera is 0.1 s. The prediction of the movement of the target point was carried out with a period of 0.001 s. The parameters of Expressions (11)–(18) for the estimation of the motion parameters of the target point had the values: N = 4, M = 50

During simulation, the AUV-leader moved in the ACS along the trajectory consisting of two parts. The first part corresponded to the movement of the leader along a straight line $x=3.0+1.2t$ (m), y = 0 = const (m), and z = 1.2 = const (m), with a constant speed of 1.2 m/s. The second part of the leader’s trajectory was described by the equations $=3.0+1.2t, y=-7.5+7.5\cos 0.1\left(t-32\right),$ $z=6.2-5.0\cos 0.05\left(t-32\right)$. During the simulation, the longitudinal axis of the AUV-leader was always oriented along the trajectory of its movement. The projection of this complex trajectory onto a horizontal plane is shown in Figure 5, where the dotted curve is the trajectory of the leader movement, and the solid gray one is the desired trajectory of the follower movement. The vector defining the desired position of the follower in the AUV-leader BCS was following ${\mathit{X}}_F^{L*}=\left[-1.5,-6, 0\right]$. The use of these trajectories for the simulation allows the investigation of the efficiency of the proposed system (11)–(18), (22)–(25), when the type of the AUV-leader trajectory is changed.

Two studies of the proposed prediction algorithm (Equations (18) and (22)–(25)) and the combined control system, including the nonlinear controller and a sliding controller defined by Equations (29), (30) and (34), were conducted.

In the first study, it was assumed that the current position and orientation of the AUV-leader were known exactly (they were not calculated using Expressions (7)–(11) from the images of the leader’s beacons received from the video camera). This made it possible to estimate not only the quality of the proposed prediction algorithm, but also the impact of possible prediction errors on the accuracy of the control of the AUV-follower’s movement.

In the second study, the position and orientation of the leader were calculated using Expressions (7)–(11), based on the images received from the CoppeliaSim environment. This study made it possible to evaluate the quality of the entire synthesized system using the data obtained after the image processing.

The results of the first study are shown in Figure 6. In this figure, the gray curves show changes in the coordinates and speeds of the target point moving along the program trajectory of the follower (gray curve in Figure 5), in the $x_{FR}{y}_{FR}{z}_{FR}$ CS. The black curves show the estimates of these coordinates and velocities in this CS, calculated using Expressions (18), (22)–(25).

Before starting the movement in the initial position, the main axes of the leader and the follower coincide with the axes of the ACS; the array Ω is filled with zero values; the coordinates of the AUV-leader in the ACS are (3; 0; 1.2); and those of the AUV-follower are (−1.5; −6; 1.2). That is, the follower in the initial position was specially placed on the $x_{FR}$ axis further from its program position relative to the leader, and the coordinates of the $y_{FR}$ and the $z_{FR}$ remained the same. Moreover, in the simulation, the movement of the AUV-follower began 4 s after the start of the movement of the leader. During this time, the position of the program point on the $x_{FR}$ coordinate changed from 3 m to 7.8 m (see Figure 6a). This was conducted to check the accuracy of the system’s recovery of the required program position of the follower relative to the leader in the case of external disturbances.

The initial error between the target point and the AUV-follower (see Figure 6a) along the $x_{FR}$ axis was worked out by its control system up to 0.2 m by the time of t = 28 s and did not increase further. During this time, the speed of movement of the target point relative to the follower along the $x_{FR}$ axis (see Figure 6d) varied from 1.2 m/s (gray curve) to −1.1 m/s (at the stage of acceleration of the follower). Then, the changes in this speed did not exceed 0.02 m/s, which indicates the synchronization of the movement of the AUV-leader and the AUV-follower.

Negative values of the speeds in Figure 6d–f mean that the follower moves faster than the leader by the corresponding coordinate, approaching the target point, and the positive ones indicate that it moves slower than the leader, moving away from the target point.

From Figure 6a,d, it is seen that the prediction of the changes in the coordinates and the speed of the target point along the $x_{FR}$ coordinate occurs with significant errors during the 6 s after the start of the slave movement. It takes place until the array Ω is filled with the data necessary for the correct operation of Expressions (18), (22)–(25). After filling this array, the deviations of the predicted position of the target point from its real position in the $x_{FR}$ coordinate do not exceed 0.02 m, in the $y_{FR}$ coordinate—0.002 m, and in the $z_{FR}$ coordinate—0.01 m. This is quite enough for the high-quality control of the follower.

The analysis of the features of the system operation showed (see Figure 6c) that at the initial stage of movement, short-term deviations of the target point along the $z_{FR}$ coordinate may occur. This is explained by the interactions between some degrees of freedom of the AUV, when acceleration along the x-axis of the BCS leads to the appearance of a torque along the pitch angle and the displacement of the AUV-follower along the $z_{FR}$ axis. However, the interaction is quickly compensated for by the control system of the follower.

At the moment of changing the type of trajectory of the AUV-leader (t = 32 s in Figure 6) in the CS $x_{FR}{y}_{FR}{z}_{FR}$, the error between the true and predicted values of the coordinates and speeds of movement of the target point appears. This is due to the fact that not all the data contained in the array Ω (see Section 3.1) correspond to the new trajectory of the AUV-leader. However, this error is quickly (in a few seconds) worked out by the system. Studies have shown that the described effect occurs only with a sharp transition from a rectilinear to a curved trajectory. If these transitions are smoothed, then the described effect is weak.

Thus, the presented simulation results generally confirm the possibility of ensuring the high-precision movement of the AUV group with the help of the proposed control system, if the AUV-leader moves with a speed of up to 1.4 m/s, even with a long period (0.1 s) of updating the data about its position relative to the followers.

Figure 7 shows the control signals generated by the system (29), (30), and (34) by three degrees of freedom when using Expressions (18), (22)–(25) for prediction of the movement of the target point, which are obtained with accurate information about the position and orientation of the AUV-leader.

Although the updating period of the data about the position and orientation of the AUV-leader is 0.1 s, the control signals (due to the use of a system with a sliding controller) are updated 100 times faster. Thus, the control system (29), (30), and (34) provides the required accuracy of the movement of the AUV-follower. At the same time, at the initial stage of movement (from 0 c to 32 s), the main control signal is formed by the x_F coordinate (see Figure 7a) to work out in a sliding mode a large initial error between the follower and the target point. Additionally, at the moment of 32 s, the resulting large errors are also successfully worked out by the system in a sliding mode when the transition from the linear to the spatial trajectory has happened. However, in other cases, a sliding controller begins to work only when the nonlinear regulator cannot accurately compensate for the interactions between the degrees of freedom of the AUV-follower.

It should be noted that there is a discontinuous component with a sufficiently large amplitude in the control signals (Figure 7a,c). Currently, there are many methods for suppressing chattering, but for this study it was important to show that a sliding mode occurs in the control system, and the proposed prediction algorithm allows the ensuring of the operability of such robust control systems.

The results of the second study of the operation of the follower control system (29), (30), and (34), taking into account the errors in determining the position and orientation of the AUV-leader, are shown in Figure 8 and Figure 9. As in the previous figures, a solid gray line shows the change in the coordinates and speeds of movement of the target point in the CS $x_{FR}{y}_{FR}{z}_{FR}$, and the black curves are generated by Expressions (18), (22)–(25). In addition, in Figure 9a–c the gray dotted line corresponds to the estimates of the coordinates of the target point calculated using Expressions (7)–(11), based on the images obtained from the video camera. The values of these estimates in the form of input data are used for the operation of Expressions (11)–(18), (22)–(25). This is different from the first study, where the exact values of the coordinates of the AUV-leader in the CS $x_{FR}{y}_{FR}{z}_{FR}$, were used as input data.

In general, the process of tracking the movements of the target point by the AUV-follower is stable and fairly accurate.

At the same time, the nature of this x_F coordinate tracking is the same in both the first and second studies (see Figure 6a and Figure 8a, Figure 6b and Figure 8b, and Figure 7a and Figure 9a, respectively). However, it is a system in which the coordinates of the target point are determined precisely (see Figure 6); it works without jumps of errors and expectedly provides more accurate tracking.

In particular, the presence of errors in the information about the position and orientation of the AUV-leader leads to a deviation of the follower from its desired position up to 0.5 m at the x_F coordinate, 0.25 m at the y_F coordinate, and 0.2 m at the z_F coordinate. This is explained by the appearance of sufficiently large errors in the operation of Expressions (7)–(11) for calculating the position of the target point (these errors can reach 0.3 m) because of errors in the array Ω, which lead to a less accurate approximation of the movement of the target point. However, despite the presence of such errors, Expressions (18), (22)–(25) allow the calculation of the speed of movement of the target point accurately and without delay and the prediction of its movement.

The errors value is greater the lower the frequency of the updating of the data received from the video camera and the greater the speed of AUV-leader because in the time interval between data updates the AUV-follower moves in accordance with the prediction that is being formed based on the inaccurate data from the onboard video camera. However, the presence of these errors still makes it possible to ensure the operability of the control system (29), (30) and (34) and the movement of the AUV-follower with dynamic accuracy sufficient to perform all types of underwater operations. Even if errors in the appropriate control channels increase sharply, then they are quickly worked out by the control system.

Figure 9 shows the control signals generated by the control system in the second study in the presence of errors in the estimates of the position of the target point.

It should be noted that in comparison with the results of work in [23], the proposed approach allows for sufficiently accurate movement of the AUV-follower in the case of the spatial movement of the group, as well as with speeds of more than 1.4 times and twice the data update period from the onboard video camera. At the same time, the system [23] loses its operability under these conditions.

Thus, the simulation results confirmed the workability and effectiveness of the proposed new method of forming the AUV cooperative control using light beacons that can be placed not only on the AUV-leader, but also on those following. It allows the use of the AUV-followers with simple onboard equipment for forming a complex AUV group structure.

6. Discussion

The results obtained in this work allow the confirmation of the possibility of implementing AUV formation control based on information received from the on-board cameras of the AUV-followers. At the same time, due to the features of obtaining video information under water, the distances between the AUV in the group cannot exceed 5–15 m. Movement at such distances with a relatively high speed requires fast updating of information about the leader position, as well as the use of control systems that allow for accurate movement of the followers and rapid response to changes in its trajectory. The proposed approach makes it possible to comprehensively solve these problems: the proposed algorithm for predicting the movement of the target point provides fast updating of information about the movement of the leader regardless of the period of data receipt from the onboard video camera of the AUV-follower, and the control system provides high accuracy of the AUV-follower movement.

At the same time, it should be noted that there were a number of tasks that were not considered in this work and will be solved in the future.

1.

The development of a strategy for the movement of the AUV-followers in the case of loss of visual contact with the leader, as well as methods to avoid this.

2.

The development of group control systems with a large number of AUVs and the study of the stability of the movement of such groups.

3.

The modification of the proposed system for the case of movement in an environment containing obstacles.

7. Conclusions

The paper proposes a new method for the synthesis of spatial motion control systems of the AUV-leader and a group of AUV-followers during their cooperative movement in desired formation. This system allows the provision of an accurate positioning of the followers relative to the leader using information received with a low frequency only from their onboard video cameras. In order to improve the accuracy of the created system, it proposed the method of the estimation of the parameters of the movement of the AUV-leader (its speeds and accelerations) to predict its movement relative to the AUV-follower in the time intervals between updates of information received from the video cameras.

The main advantage of the proposed approach is the implementation of group movement in the leader–followers mode without the use of slow data exchange between them via acoustic communication channels, as well as without the use in the AUV-followers of expensive hydroacoustic navigation systems.

With the help of the proposed method, it is possible to organize the movement of large groups of AUV, when followers can act as leaders for one or more followers walking from the front or from the side.

This makes it possible to remove restrictions on the transparency of the underwater environment for the operation of AUV onboard video cameras, on the number of followers being moved, and on their location in the group.