Formation Tracking for a Multi-Auv System Based on an Adaptive Sliding Mode Method in the Water Flow Environment

As shown in Fig. 3, the state vector (position and posture) of the AUV is defined as $\bm{\eta}=\left[\bm{\eta}_{1}^{\mathrm{T}}\ \bm{\eta}_{2}^{\mathrm{T}}\right]^{\mathrm{T}}\in\mathbb{R}^{6}$, where $\bm{\eta}_{1}=\left[x\ {y}\ {z}\right]^{\mathrm{T}}\in\mathbb{R}^{3}$ is the vector of vehicle position coordinates in an earth-fixed or inertial referenced frame and $\bm{\eta}_{2}=\left[\phi\ {\theta}\ {\psi}\right]^{\mathrm{T}}\in\mathbb{R}^{3}$ is the vector of vehicle’s Euler-angle coordinates in an inertial reference frame. At the same time, the dynamic vector of the AUV is defined as $\bm{v}=\left[\bm{v}_{1}^{\mathrm{T}}\ \bm{v}_{2}^{\mathrm{T}}\right]^{\mathrm{T}}\in\mathbb{R}^{6}$, where $\bm{v}_{1}=\left[u\ v\ w\right]^{\mathrm{T}}\in\mathbb{R}^{3}$ is the vector of vehicle linear velocity expressed in the vehicle-fixed reference frame, and $\bm{v}_{2}=\left[p\ q\ r\right]^{\mathrm{T}}\in\mathbb{R}^{3}$ is the vector of vehicle angular velocity expressed in the body-fixed frame.

The parameters of the AUV’s 6-DOF model is show in Table 1. The vehicle-fixed linear and angular velocities and the time derivative of the earth-fixed vehicle coordinates are related by the following equations:

where $\bm{R}_{I}^{C}$ is the rotation matrix expressing the linear velocity transformation from the inertial earth-fixed frame to the body-fixed frame, and the angular velocity transformation matrix $\bm{T}\in\mathbb{R}^{6}$ is Jacobian matrix, which can be both found in former literatures [35, 36, 37].

Considering the coordinate systems illustrated in Fig. 3, the dynamic equations of the AUVs in the vehicle-fixed reference frame are written as

where $\bm{q}=\left[u\ v\ w\ p\ q\ r\ \right]^{\mathrm{T}}$ is the ROV spatial velocity state vector with respect to its body-fixed reference frame, and $\bm{e}=\left[x\ y\ z\ \phi\ \theta\ \psi\right]^{\mathrm{T}}$ is the position and orientation state vector with respect to the inertial reference frame. The spatial transformation matrix between the inertial frame and the AUV’s body-fixed frame can be defined through the Euler angle transformation, denoted by $\mathbf{J}(\bm{e})\in\mathbb{R}^{6\times 6}$. The term $\mathbf{M}(\bm{q})\in\mathbb{R}^{6\times 6}$ is the inertia matrix including the added mass effects. $\mathbf{C}(\bm{q})\in\mathbb{R}^{6\times 6}$ is the matrix of centrifugal and Coriolis terms. $\mathbf{D}(\bm{q})\in\mathbb{R}^{6\times 6}$ is the drag matrix, with that $\mathbf{g}(\bm{e})\in\mathbb{R}^{6\times 6}$ is the vector of gravity and buoyancy forces and moments. $\bm{\tau}\in\mathbb{R}^{6\times 1}$ is the matrix of control forces and moments acting on the AUV center of mass [25, 38, 39]. $\bm{\tau}_{c}\in\mathbb{R}^{6\times 1}$ is the matrix of forces and torques produced by the water flow which is acting on the AUV working under the water. Note that as the disturbance is caused by the water flow, $\bm{\tau}_{c}$ is complex and changeable, so that it is difficult to build a model.

The dynamic equation of motion of AUVs in the inertial reference frame can be presented as

where $\mathbf{M}_{e}(\bm{e})=\mathbf{J}^{-\mathrm{T}}\mathbf{M}\mathbf{J}^{-1}$, $\mathbf{C}_{e}(\bm{q},\bm{e})=\mathbf{J}^{-\mathrm{T}}\left[\mathbf{C}-\mathbf{M}\mathbf{J}^{-1}\dot{\mathbf{J}}\right]\mathbf{J}^{-1}$, $\mathbf{D}_{e}(\bm{q},\bm{e})=\mathbf{J}^{-\mathrm{T}}\mathbf{D}\mathbf{J}^{-1}$, and $\mathbf{g}_{e}(e)=\mathbf{J}^{-\mathrm{T}}\mathbf{g}$. The system dynamics are not exactly known, because the AUV dynamics are underactuated and dominated by hydrodynamic loads. It is difficult to accurately measure or estimate the hydrodynamic coefficients that are valid for AUVs’ operating conditions. Therefore, the system dynamics can be written as the sum of estimated dynamics $\hat{\bm{f}}$ and the unknown dynamics $\tilde{\bm{f}}$ and we have

The estimated dynamics vector is defined as

with $\hat{\bm{h}}(\bm{q},\bm{e})=\hat{\mathbf{C}}_{e}(\bm{q},\bm{e})\bm{e}+\hat{\mathbf{D}}_{e}(\bm{q},\bm{e})\bm{e}+\hat{\mathbf{g}}_{e}(\bm{e})$ and the unknown dynamics vector are defined as

with $\tilde{\bm{h}}(\bm{q},\bm{e})=\tilde{\mathbf{C}}_{e}(\bm{q},\bm{e})\bm{e}+\tilde{\mathbf{D}}_{e}(\bm{q},\bm{e})\bm{e}+\tilde{\mathbf{g}}_{e}(\bm{e})$. $\tilde{\mathbf{M}}_{e}$, $\tilde{\mathbf{C}}_{e}$, $\tilde{\mathbf{D}}_{e}$, $\tilde{\mathbf{g}}_{e}$ represents the unknown items. In (6), as the additional disturbance is bounded in the environment, the nonlinear uncertainty vector $\tilde{\bm{f}}$ and its time domain differential could be assumed to be bounded.

In the previous researches on multi-AUV systems, there are relatively few researches on the ocean current. However, in practice, the influence of the current cannot be ignored, and it is even a very important factor for the successful tasks of the AUV formation. The existence of the water flow can generate drifting motions, making AUVs deviate from the predetermined trajectories.

In the inertial coordinate system, the influence of the regular current can be expressed as the vector $\bm{\tau}_{e,f}$ of the force and torque. $\tilde{\bm{\tau}}_{e,f}$ is the force and torque vector generated by the propeller to offset the disturbance of ocean current, then we have $\tilde{\bm{\tau}}_{e,f}=-\bm{\tau}_{e,f}$ in the ideal condition. It is either a constant or as a quantity that transforms slowly and regularly. For the wind currents and other flows, the speeds change as time varying, generally after the wind stops the flow can continue change for a period. Such currents’ information are hard to predict or measure. In the inertial coordinate system, it is assumed that the change of current disturbance is $\bm{d}_{o}\in\mathbb{R}^{6\times 1}$ equivalent to $\bm{d}_{e,o}$ in the body-fixed frame. If $\bm{d}_{e,o}$ is bounded, adaptive higher-order controller adopted in this paper can deal with the disturbances in a certain range and ensure the normal operation of AUVs in formation. Considering the influence of regular current and variable flow, the total influence on the AUVs is

where $\bm{\tau}_{e,c}=\mathbf{J}^{-\mathrm{T}}\bm{\tau}_{c}$ as depicted in (3).

Since the pattern of ocean current is time-varying, it is difficult to find a unified mathematical model, and the general disturbance of ocean current is bounded. Therefore, we build a function to verify the effectiveness of the algorithm by means of a trial water flow example [40].

The 3-D working environment of AUVs is represented by cartesian coordinate system. Let $z=0$ be the water surface level, and the $z$-axis points to the bottom of the sea or river. The working space is stratified by depth $z$ into $N$ ($N\in\mathbb{N}^{+}$) layers, and each layer is an $x$-$O$-$y$ plane of two dimensions (2-D). Each $z=N$ ($N=1,2,3,...$) plane is rasterized, and each grid is a square with equal side length of $1$ ($m$). The currents in the areas within each grid can be regarded as the same. An east-west flow field with a meandering north-south flow field is adopted, and the mathematical expression of the flow function $C(x,y,t)$ at time $t$ is

In (8), $B(t)=\left(B_{0}+E\cos(\omega t+\theta)\right)$, $B_{0}=1.2$, $w=0.4$, $\theta=\pi/2$, $c=0.12$, $k=0.82$, $E=0.3$. According to the flow function, the current velocity can be calculated as

where $U(x,y,t)$ and $V(x,y,t)$ are the velocity components of the water flow in the $x$ and the $y$ directions at time $t$, respectively.

Suppose that the water flow field in the workspace of a depth of $z\in[-20\ 0]$ ($m$) is equally divided into three layers, and the velocity gradually decreases from the water surface to the bottom layer. $\left|V_{ci}\right|$ ($i=1,2,3$) denotes the the module value of the current velocity in each layer, with $\left|V_{c1}\right|=2.4\left|V_{c2}\right|=4\left|V_{c3}\right|$. Then the 2-D current field is illustrated in Fig. 4 and the 3-D current field is illustrated in Fig. 4. As shown in Fig. 4, the arrow indicates the direction of the current at this position, and the length of the arrow is proportional to the velocity of the current. Taking this kind of water flow field as an example, simulations to test the proposed algorithm can be implemented.

The sliding order $r$ refers to the number of zeros of the continuous full derivatives which equal to zero on the sliding mode surface of the sliding mode variable $s$. The sliding set of order $r$ related to the sliding mode surface is defined as the equation $s=\dot{s}=\ddot{s}=\ldots=s^{(r-1)}=0$. If the sliding set of order $r$ exists, and it is assumed to be the local set in the sense of Filippov, the motion satisfying this equation is named $r$ order sliding mode about the sliding mode surface $s(x,t)=0$, and the order $r-1$ is called the relative order of the system.

The sliding order of the AUV system is designed as a number of continuous total derivatives of in the vicinity of the sliding mode. It fixes the dynamics smoothness degree. The $r$-th order sliding mode is determined by the equation:

The main problem of higher-order sliding mode control is the increment of the demanded information. For example, the system degree is 3, and then a 3-sliding controller needs an input control parameters $\sigma=\dot{\sigma}=\ddot{\sigma}=\sigma^{(r-1)}$. Fortunately, the super twisting algorithm that we adopt needs only the measurement of $\sigma$. Then the higher-order controller’s action $u_{\textrm{HOSMC}}$ can be defined as

Note that $u_{1}$ and $u_{2}$ are both functions of time $t$, corresponding respectively to a continuous function of the sliding mode surface and an integration of the sliding mode surface in the time domain [41]:

and

where $u_{max}\in N^{+}$, and can be normalize to 1. The remaining parameters are set in the simulations. According to [42], the sufficient conditions to ensure convergence to the origin for the sliding mode plane in finite time is

where $W$, $\rho$, $\lambda$, $\Phi$ are positive constants to be adjusted in the simulation.

The algorithm does not need any differential information of sliding mode surface $\sigma$ in time domain, so the computation burden of controller is greatly reduced. Although the algorithm has good robustness, when $\sigma$ is very small, the control signal output is not Lipshitz, which may cause some noise in control output. In the simulations, parameters of the controller are continuously adjusted to find the most suitable values.

On the phase plane defined by the estimable second-order state equation (5), a first order dynamic equation is designed to represent the switching surface $\sigma$:

where $\bm{M}\in\mathbb{R}^{6}$ is positive definite diagonal matrix for the expected error response. When $\sigma=0$, the dynamic response of the system is as expected, which means $\sigma$ can represent the difference between the current system state and the desired state. $\bm{\varepsilon}=\bm{e}-\bm{e}_{d}$ represents the tracking error between the actual system and the desired system. $\bm{e}_{d}$ is the desired AUV position and attitude vector. Substituting $\bm{\varepsilon}$ in (15), we have

where $\dot{\bm{e}}_{r}=\dot{\bm{e}}_{d}-2\mathbf{M}\bm{\varepsilon}-\mathbf{M}^{2}\int\bm{\varepsilon}dt$, and $\bm{r}_{r}$ represents the reference trajectory of the AUV.

As shown in (11), $u_{\textrm{HOSMC}}$ represents the control force acting on the AUV’s center of mass; $u_{1}$ and $u_{2}$ represent the equivalent control and switching control respectively. The equivalent control $u_{1}$ is a continuous function based on the AUV’s model. If there is no uncertainty in the system dynamics, the equivalent control rate can achieve the desired state. However, AUV has a dynamic model and works with external disturbances, so the controller should include a switching control quantity $W\operatorname{sign}(\sigma)$, where $W$ is the upper limit of system model parameter uncertainty. the switching control $u_{2}$ could be a discontinuous feedback function, which is mainly used to compensate the differential characteristics and external disturbances of the expected AUV’s dynamics, so as to make the system stable. However, the switching control rate function is easy to make the system oscillate near the switching surface, causing the chattering phenomenon, so it is necessary to redesign an adaptive switching controller to help the system to achieve stability.

According to the super twisting algorithm, the control law can be written as

Then the control equation is

Let $\hat{\bm{f}}_{r}=\hat{\mathbf{M}}_{e}(\bm{e})\ddot{\bm{e}}_{r}+\hat{\bm{h}}(\bm{q},\bm{e})$ be the predictable dynamic vector $\hat{\bm{f}}$’s equivalent control quantity, then the system input control quantity obtained based on the equivalent control law is

For the system dynamics model without external disturbance, the equivalent control law of $u_{1}$ could meet the requirements. Otherwise, the switching control $u_{2}$ needs to be added adaptive characteristic in order to achieve robustness.

By adding an adaptive control link to the higher-order sliding mode controller, we can obtain a chattering-free and anti-interference controller. The controller’s block diagram is shown in Fig. 5.

As mentioned above, the switching control $u_{2}$ becomes the adaptive controller after adding the adaptive characteristic to it. In order to replace the discontinuous function in the switching controller, we use a continuous adaptive control law

where $\mathbf{K}\in\mathbb{R}^{6\times 6}$ is a positive definite diagonal matrix relative to the convergence rate of the controller. $\tilde{\bm{f}}_{est}$ is an adaptive term realizing the estimation of lumped parameter uncertainty vector $\tilde{\bm{f}}$ in (6). The renewal rate of $\tilde{\bm{f}}$ is designed as

where $\bm{\Gamma}\in\mathbb{R}^{6\times 6}$ is the positive definite diagonal matrix related to the adaptive rate. The adaptive term $\tilde{\bm{f}}_{est}$ is the error estimation of sliding mode surface $\sigma$, which can make the predicted system dynamic state more close to the actual system dynamic state under unknown disturbances. If $\tilde{\bm{f}}_{est}$ is bounded, $\dot{\tilde{\bm{f}}}_{est}$ is also bounded.

If the speed of the formation is slow and $\tilde{\bm{f}}$ is relatively small, then $\bm{w}$ decreases with the effect of adaptive control law. Then the inequality in Assumption. 22 is easier to implement. Even in the worst case, condition in (22) can be meet by adjusting $\mathbf{K}$ and $\mathbf{\Gamma}$.

As we can model the AUV’s dynamics in the physical engine simulation system, a kind of model predictive control method is combined into the adaptive higer-order sliding mode controller to make the control output more smoother.

Considering a team of 3 AUVs in a triangular formation tracking an arbitrary curve path, we assume that the leader AUV moves first and all the followers have essential sensors to contact with the leader and acquire essential informations such as relative distances and angles to the leader. The control object is to make the followers track the desired paths simultaneously as accurate as possible. The whole formation is kept and the path is tracked in this way. The controller adjusts the velocities and the directions of the AUVs in the formation to achieve this goal. In the simulations, we set the time-varied water flow disturbance vector ${\bm{d}_{o}}=[{C_{x}}\ {C_{y}}\ {0}\ {0}\ {0}\ {C_{z}}]$ equivalent to the parameter $\bm{d}$ depicted in Fig. 5. In this way the water flow influence is resolved into three quantities in the $x,y,z$ directions respectively, which is convenient for simulation. As the current velocity is assumed to be bounded, let $\left(C_{x},C_{y},C_{z}\right)\in[-20\ 20]$ $(\mathrm{N})$.

Based on the formation control algorithm, 3 AUVs form a triangular formation at the starting position. The leader runs along the desired path and meanwhile generates the desired path of the 2 followers. The follower AUV also runs along the desired path according to the adaptive higher-order sliding mode control algorithm, until the formation reaches the target position. During the tracking procedure, the trajectory tracking and formation keeping are both well maintained. As shown in Fig. 8, the formation moves along the spiral trajectory in triangular shape. The formation tracking test achieves good results.

Taking the left follower as the research object, the control effects in three directions of $x$, $y$ and $z$ are analyzed, i.e. $\tau_{u}$, $\tau_{v}$ and $\tau_{w}$ correspondingly. $\tau_{u}$ comprises two parts of $u_{1}$ and $u_{2}$ as mentioned above. The input of the controller in $x$ direction is show in Fig. 9.

Similarly, the controller input in the $y$ direction is also comprised of $u_{1}$ and $u_{2}$ as shown in Figure. 10. In the $z$ direction, the controller input is shown in Figure. 11. From the simulation results, we could find that the contoller input $u_{1}$ are bounded and smooth, and $u_{2}$ are bound pulses which are realizable. Unlike the control inputs in the $x$ and $y$ directions, $u_{2}$ in $z$ direction tends to be zero. This is because the water flow is layered in the 3-D space. The forces of the flow is parallel with the $x-y$ plane without vertical influence in the $z$ axis.

Helical trajectory formation tracking results are smooth and interference resistible as illustrated in Fig. 8. In the formation trajectory tracking process, we use the position error value $e$ as the $x$ axis and the speed error value $de$ as the $y$ axis. Then the system phase trajectories with the proposed adaptive higher-order sliding mode controller are depicted in Fig. 12 in the $x$, $y$ and $z$ axis respectively.

As show in the figure, system phase trajectories converge to the expected location after certain times of iterations. In the $x$, $y$ and $z$ directions, the system trajectories chatter a little after each movement of the formation leader. That’s because the algorithm need some iterations to calculate the control input. After the calculation is finished, the trajectories converge.

For the position and velocity errors in the 3 directions of $x$, $y$ and $z$, the errors are limited to a small range after the sliding mode control algorithm converges. Table. 2 gives the tracking error analysis, using the root mean squared error (RMSE) to measure the velocity error and the position error from the time $t=0$. The maximum and minimum errors of the system after convergence are recorded. In this experiment, the convergence moment is $t=11$ ($s$). The velocity error and position error do not converge to zero, but fluctuate regularly within a limited range, which is acceptable in practical applications.

Fig. 13 shows the tracking error comparison of the traditional sliding mode method and the proposed method in the paper, taking the left follower as example, too. Note that the traditional method uses the first order sliding mode controller without the adaptive module proposed in this paper. Fig. 13 compares the tracking errors of the two kinds of methods in the $x$ axis direction. Tracking errors of the position and the speed of the AUV converge to a certain range. As the leader AUV moves, the tracking error increases and then decrease as the controller taking effect. The traditional method results jumping errors with obvious water flow influences, while the tracking error curves are smoother and smaller with the proposed method. In the same way, Fig. 13 shows the tracking error comparison in the $y$ axis direction and Fig. 13 shows the tracking error comparison in the $z$ axis direction. From these simulation results, we could find that the proposed method is effective to the formation control problem in the water flow environment.