Underactuated AUV Nonlinear Finite-Time Tracking Control Based on Command Filter and Disturbance Observer

The three-dimensional (3D) path following problem of an underactuated autonomous underwater vehicle with ocean currents disturbances is addressed in this paper. Firstly, the motion equation under the ocean currents disturbance is established, and the dynamic model of 3D tracking error is constructed based on virtual guidance method. Then, a finite-time control scheme based on super-twisting observer and command filtered backstepping technology is proposed. We adopt super-twisting observer based on finite-time theory to observe the ocean currents disturbances for improving the system robust. A command filtered backstepping is proposed to replace the differential process in the conventional backstepping method for avoiding the differential expansion problem. The filter compensation loop is designed to ensure the accuracy of the filtered signal, and the anti-integration saturation link is designed considering the influence of integral saturation. Lyapunov stability theory is used to prove the stability of the underactuated AUV. Simulation studies are conducted to show the effectiveness and robustness of the controller.


Introduction
Autonomous underwater vehicles have become more and more practical in many fields, such as civil and military, underwater monitoring, deep sea environmental resource exploration and development, ocean data observation and collection, seabed topographic scanning, submarine pipeline detection and marine mine clearance, etc. [1][2][3]. The completion of related tasks generally requires the AUV to follow an ideal path. Therefore, the path following technology of AUV has attracted the attention of many scholars and has become a hot research topic. AUVs are often designed to be under-actuated, such as the REMUS series [4,5], for reasons of energy savings, weight reduction and increased reliability. Most of underactuated AUVs cannot directly provide lateral and vertical forces, and can only control surge velocity, yaw angle velocity and pitch angle velocity directly. In addition, AUV is also affected by external disturbances such as waves, currents and uncertainties of its own hydrodynamic parameters in the actual environment, which increases the difficulty of controller design [6,7].
In the past few decades, researchers have tried various control methods for path following control of underactuated AUVs [8,9]. A cascade structure consisting of an integral LOS guidance and feedback linearized proportional derivative controller was designed in [9]. A tracking control law based on the extended state observer and the optimal reference signals were proposed in [10]. The control strategy based on the input-output feedback linearization method was designed in [11]. The robust adaptive path (1) The differential filtering problem caused by the traditional backstepping calculation complexity is avoided by the command filtering backstepping method, and the part of filtering error compensation is proposed to ensure the accordance of virtual control signal and the filtered signal. (2) Since the integral action of the filter part, the anti-integration saturation is considered in the control loop to deal with the problem of integral saturation in the control signals.
(3) A current disturbance observer is presented to reduce the impact of external ocean currents disturbances on the system and increase the tracking controller robustness. (4) The 3D path following controller, based on the finite-time control theory, is designed to improve the response speed and control accuracy of the controller.

Coordinate System and Parameter Definition
As is depicted in Figure 1

AUV Kinematic and Dynamic Equations
According to Reference [27], the nonlinear motion equations of AUV can be described: where η denotes the position and orientation vector with coordinates in the earth fixed frame, ( ) J η is the transform matrix between the body-fixed and inertial coordinates, v denotes the linear and angular velocity vector with coordinates in the body-fixed frame, M is the inertial and add inertial matrix, ( ) C v is the matrix od Coriolis and Centrifugal terms, ( ) D v is the matrix of hydrodynamics terms, ( ) g η is the vector of gravity and buoyant force, τ denotes the forces and moments in the body-fixed frame. It must be mentioned that the ocean currents were assumed constant in frame { } B in many earlier researches. A more natural and real assumption are made in this paper: the ocean currents are constant in frame { } I . The ocean currents in frame { } I are proposed as [27] [ , , ] With the Euler angle rotation matrix, it can be transformed to the frame { } B : Figure 1. Three-dimensional (3D) path following frame of AUV.

AUV Kinematic and Dynamic Equations
According to Reference [27], the nonlinear motion equations of AUV can be described: where η denotes the position and orientation vector with coordinates in the earth fixed frame, J(η) is the transform matrix between the body-fixed and inertial coordinates, v denotes the linear and angular velocity vector with coordinates in the body-fixed frame, M is the inertial and add inertial matrix, C(v) is the matrix od Coriolis and Centrifugal terms, D(v) is the matrix of hydrodynamics terms, g(η) is the vector of gravity and buoyant force, τ denotes the forces and moments in the body-fixed frame. It must be mentioned that the ocean currents were assumed constant in frame {B} in many earlier researches. A more natural and real assumption are made in this paper: the ocean currents are constant in frame {I}. The ocean currents in frame {I} are proposed as [27] v With the Euler angle rotation matrix, it can be transformed to the frame {B}: where c * and s * are abbreviations for cos * and sin * , respectively. Assume the ocean currents change slowly, the acceleration of the current is negligible such that Usually, the roll motion of AUV can be ignored due to its left-right symmetrical structure [28]. Under the assumption, the motion equations can be given as where m is the AUV mass, I y and I z denote the inertia moments about the pitch and yaw rotation. u r , v r and w r denote the real linear velocities, where q, r are the velocities of pitch and yaw. r and ψ present the pitch and yaw angle. The corresponding hydrodynamic derivatives are denoted by X ( * ) , Y ( * ) , Z ( * ) , M ( * ) and N ( * ) . z g and z b are the vertical positions of the gravity and buoyancy center within body-fixed frame. X denotes the force generated by the stern thruster. M and N are the moments provided by horizontal and vertical rudders.

AUV Error Systems
The error formulations can be derived by using the Serret-Frenet frame {SF} according to Figure 1 [29]. We define point P that is the origin of {SF} as the virtual moving target AUV that describes the path, and the position in {I} is η e d = (x d , y d , z d ) T . The Serret-Frenet frame is rotated with angle θ F , ψ F relative to the inertial frame.
The desired path is described by parameter s. Rotation angles can be also defined as where c 1 (s) and c 2 (s) are the torsion and curvature of the virtual target point in spatial curve. The AUV's position is η e = (x, y, z) T , and represented by point Q. The position of the AUV is usually obtained by GPS, but GPS is not available when the AUV is underwater. At that time, it can only be obtained by dead reckoning or long/short baseline instruments [30]. Hence, the error of path following can be defined in frame {B}.
where R e b is the rotation matrix from {B} to {I}.
is the vector of angular velocity. .
cθ e cψ e −sψ e sθ e cψ e cθ e sψ e cψ e sθ e sψ e −sθ e 0 cθ e The angular velocities of virtual target are presented as Equation (9). Then we define the AUV path following error: x e = ry e − qz e + u − u r cψ e cθ e . y e = −rx e + v − u r sψ e cθ e . z e = qx e + w + u r sθ e . ψ e = −c 1 (s)

Design of Path Following Control
This section introduces the nonlinear control law to solve the path following control problem of underactuated AUV in the presence of the ocean current. The control flow chart is shown in Figure 2. Controller design is divided into two stages. The first stage handles the design of the ocean current observer based on super-twisting technology. Observer is used to compensate for the effects of currents on the system. The second stage addresses the tracking controller based on command filtered backstepping. Taking the designed virtual control as the input of the command filter, the derivative of the virtual control is obtained through the integration rather than the differential process. underactuated AUV in the presence of the ocean current. The control flow chart is shown in Figure 2. Controller design is divided into two stages. The first stage handles the design of the ocean current observer based on super-twisting technology. Observer is used to compensate for the effects of currents on the system. The second stage addresses the tracking controller based on command filtered backstepping. Taking the designed virtual control as the input of the command filter, the derivative of the virtual control is obtained through the integration rather than the differential process. Controller of the AUV is developed in this section such that the real AUV can track the virtual AUV, namely where ( 1, 2,3, 4) i i ε = denote arbitrary small positive numbers.
Referring to the LOS guidance theory, pitch and heading guidance laws are defined as  Controller of the AUV is developed in this section such that the real AUV can track the virtual AUV, namely sup where ε i (i = 1, 2, 3, 4) denote arbitrary small positive numbers.
Referring to the LOS guidance theory, pitch and heading guidance laws are defined as where θ a ∈ 0, π 2 and ψ a ∈ 0, π 2 are chosen to let θ LOS ∈ − π 2 , π 2 and ψ LOS ∈ − π 2 , π 2 , k θ > 0 and k ψ > 0 are gains. When the vertical error between AUV and desired path becomes larger, θ LOS also increases, and when AUV is in the desired path, the error becomes zero, and θ LOS = 0. ψ LOS is the same. AUV will have better endurance and the redundant range can be shortened to save energy by introducing the approach angle [29].
In an attempt to eliminate effects of differential expansion in the conventional method, a command filter is considered to add to the backstepping control loop. In this approach, the control signals are passed via filter to get derivative instead of the differentiation process.
are the filtered control signals vector. ζ and ω f are the filter parameters, where 0 < ζ < 1, ω f > 0. The flow chart of command filter is shown as Figure 3.
When the vertical error between AUV and desired path becomes larger, LOS θ also increases, and when AUV is in the desired path, the error becomes zero, and 0 LOS θ = . LOS ψ is the same. AUV will have better endurance and the redundant range can be shortened to save energy by introducing the approach angle [29].
In an attempt to eliminate effects of differential expansion in the conventional method, a command filter is considered to add to the backstepping control loop. In this approach, the control signals are passed via filter to get derivative instead of the differentiation process. 2 ( )

design of super-twisting disturbance observer.
In this section, an ocean currents observer based on super-twisting technology is designed. Let D and x denote the observation of D and x , we can get where,

Design of Super-Twisting Disturbance Observer
In this section, an ocean currents observer based on super-twisting technology is designed. LetD andx denote the observation of D and x, we can get .
Error vector of state x and observation error D can be present as The observer of disturbance was designed aŝ where k 1 and k 2 are positive parameters. Together with Equations (18) and (19), we have Obviously, when the observation error of x is finite-time converged, the observation error of disturbance also converges in finite-time. Following is the proof.

Proof. For convenience of proof, select the intermediate quantities
Differentiating the above equation, we can get Let β i = [M i N i ] T , and differentiate it: Consider the Lyapunov function as follows where Equation (26) meets the following conditions Then According to Equations (28) and (30), we get where µ > 0, at this moment, Equation (31) meets the lemma 1. Hence, the error of system states and observation can converge in finite-time. The disturbance error converges in a small neighborhood, and defined as D ≤ ζ M .

Position Control
According to Equation (14), we design the Lyapunov function: Then, differentiate V p along with Equation (14), we can get: . V p = x e (u − u r cψ e cθ e ) + y e (−u r sψ e cθ e + v) + z e (u r sθ e + w) According to References [8], the virtual control signals are designed as: where k v1 > 0, k v2 > 0 and k v3 > 0 are constant gains, and substitute Equation (34) into Equation (33), we get For avoiding the differential expansion problem, command filtered backstepping method is used to replace the differential process in the conventional backstepping method. We define the tracking error of position: Then, differentiating the above equation alone with Equation (14) yields following error systems.
where u = u − u ce , ψ = ψ e − ψ ce , θ = θ e − θ ce are the errors of filter, and where lim The desired signal . can be written as: Substitute Equations (37) and (39) into Equation (38), and the errors of position are yielded as

Attitude Control
Differentiate ψ = ψ e − ψ ce and θ = θ e − θ ce , we get where the errors of angular velocities are defined as r = r − r c , q = q − q c . Based on Equations (41) and (42), q o c and r o c represent the desired virtual control signals of angular velocities q and r, and can be proposed as where k ψ > 0, k θ > 0 are the controller parameters, ψ bs and θ bs represent the robust terms. The robust terms are defined in Section 4. Substitute Equation (43) into Equations (41) and (42), Equations (41) and (42) can be rewritten as

Velocity and Angular Velocity Control
For improving the robustness of the controller, we design the integral term ε 1 , ε 2 and ε 3 , where ε 3 = r. The finite-time control input of AUV 3D path following can be proposed as where f u = X uu u r 2 + X vv v r 2 + X ww w r 2 + X qq q 2 , f q = −(z g w − z b B) sin θ + M q|q| q q − M uw u r w r − M uq u r q − mz g (w r q − v r r) and f r = N uv u r v r + N v|v| v r |v r | + N ur u r r are nonlinear dynamic terms. sig γ (x) is given as sig γ (x) = sgn(x)|x| γ .
where x ∈ R, α ∈ (0, 1), and sig α (·) is continuous and increasing, and sig α (0) = 0. Considering the observer into the Equation (45), the controller of the underactuated AUV proposed in this paper can be obtained: When the command filter controller is not considered, Equation (47) is changed as follows Then we substitute Equation (45) into Equation (7) and get the following error dynamics Let σ u = u, σ r = r and σ q = q, we get the filter error compensation dynamics The above Equation (50) can be finally rewritten as where G = −K P = diag −k u , −k q , −k r , k u , k q and k r are positive parameters.

Design of Compensation Loop
For guaranteeing that the error between the desired signals and the filter signals can be converged, a compensation of filter error is proposed in the part. The errors of compensation are defined as following where ζ x , ζ y , ζ z denote the filter compensation, according Equation (40), they can be defined where ζ ψ and ζ θ are designed in section of stability analysis, ζ x (0) = ζ y (0) = ζ z (0) = 0. We choose the following Lyapunov function By differentiating Equation (54) along with Equations (53) and (40), we get .
where σ q = q and σ r = r.

Design of Anti-Windup
The signal after the anti-windup scheme is present as where u m is the upper limited of control output. We design the anti-windup part as where K o denote the anti-windup gain coefficient. In summary, the main equations of the proposed path following controller are described as: Figure 4 shows the structure of the path following control system.

Stability Analysis
The stability of path following control system is proposed in this section. Firstly, we give the provident of the proposed controller without command filter. Substitute Equation (48) into Equation (7) and yield the following error equations (63) Then Equation (64) can be present by A Lyapunov function candidate: where M ≤ D ζ  , according to Young's inequality, it can be changed Finite-time controller

Stability Analysis
The stability of path following control system is proposed in this section. Firstly, we give the provident of the proposed controller without command filter. Substitute Equation (48) into Equation (7) and yield the following error equations Then Equation (64) can be present by where K p = diag(k u , k q , k r ), K f = diag(k f u , k f q , k f r ) and D = F u ,N,M T . A Lyapunov function candidate: where σ = σ u , σ q , σ r T denote the attitude error, and differentiating Equation (66), we get where D ≤ ζ M , according to Young's inequality, it can be changed where δ = diag(δ 1 , δ 2 , δ 3 ), δ i are arbitrary positive constant.
Furthermore, Equation (69) can be changed According to the Lemma 2, the finite-time controller without command filter is finite-time stable.
Then, to meet . V < 0, the robust terms ψ bs , θ bs , u bs , q bs and r bs can be design as Substitute Equation (73) into Equation (72), and we get where J is a negative matrix. Hence, .
V < 0, it indicates that path following errors are convergent. The entire control close-loop system is global stable. The proof is completed.

Numerical Simulations
In this section, to verify and analyze the proposed controller (FTCFPC) base on the super-twisting observer, finite-time technology and command filtered backstepping methodology of this paper, numerical simulations on an underactuate AUV WL-4 ( Figure 5)., which is developed by Harbin Engineering University in China, were conduct in MATLAB-Simulink.

Numerical Simulations
In this section, to verify and analyze the proposed controller (FTCFPC) base on the supertwisting observer, finite-time technology and command filtered backstepping methodology of this paper, numerical simulations on an underactuate AUV WL-4 ( Figure 5)., which is developed by Harbin Engineering University in China, were conduct in MATLAB-Simulink. In order to simulate a more realistic environment, the environment disturbances are considered in simulations. The disturbances can be expressed as [2,1,1, 2, 2] T = K represents the gain parameter matrix. [20,20,20,20,20] For the underactuated AUV spiral dive operation, the following 3D desired path is design.
The initial position of the AUV is  In order to simulate a more realistic environment, the environment disturbances are considered in simulations. The disturbances can be expressed as where d = [τ ud , τ vd , τ wd , τ qd , τ rd ] T is the external disturbances vector. ε is a vector of white Gaussian noise with the largest amplitude of ε = 1, and represents the high measurement noise. K = [2, 1, 1, 2, 2] T represents the gain parameter matrix. T = [20,20,20,20,20] T denotes the time constant matrix. For the underactuated AUV spiral dive operation, the following 3D desired path is design.
In the numerical simulation, we design AUV controllers according to (47) and the parameters are given by k x = 5, k y = 1, k z = 2, k ψ = k θ = 2, k u = 8, k q = k r = 3, k f u = k f q = k f r = 1, p 21 = 5, p 22 = p 23 = 5. The parameters of the filter are selected as w n = 20rad/s, ζ = 0.9.
Finally, the simulation results by using finite-time command filtered backstepping control and the traditional backstepping control are shown in Figures 6-12 In the numerical simulation, we design AUV controllers according to (47) and the parameters are given by Finally, the simulation results by using finite-time command filtered backstepping control and the traditional backstepping control are shown in Figure 6-12.
The real and desired paths in the spatial space are shown in Figure 6, Figure 7 and Figure 8 respectively show the projection curves of horizontal plane and vertical plane. It can be seen from these figures that the actual path of the AUV deviates from the desired path due to the influence of the ocean current. As can be seen from Figures 6-8. The path under FTCFPC control is closer to the desired path than that under traditional backstepping control. The FTCFPC controllers has the smaller offset and achieve significantly higher accuracy than traditional backstepping controllers in the presence of unknown disturbance and ocean currents. Figure 9 shows the tracking errors of 3D path. It can be seen that the error of real and desired path with the FTCFPC controller can converge in a short time. The position error of two controllers has some fluctuations, but the fluctuation of the FTCFPC method is much smaller. These can be proved that the FTCFPC method present in this paper has good robustness and effectiveness. Figure 10 and Figure 11 depict the response of velocity and angle respectively, from which we can see that, FTCFPC method can achieve the desired path more quickly and has good anti-interference ability. The control signals of different channels are shown Figure 12.
In summary, the FTCFPC method proposed in this paper can make the AUV tracking to the desired path smoothly and robustly with the ocean currents. Following are the advantages which can been seen from the above results: 1. The controller has strong robustness under the interference of ocean current. 2. It has faster convergence, and the AUV can follow the desired path in a short time. 3. The control has better following accuracy under current disturbance.                  The real and desired paths in the spatial space are shown in Figures 6-8 respectively show the projection curves of horizontal plane and vertical plane. It can be seen from these figures that the actual path of the AUV deviates from the desired path due to the influence of the ocean current. As can be seen from Figures 6-8. The path under FTCFPC control is closer to the desired path than that under traditional backstepping control. The FTCFPC controllers has the smaller offset and achieve significantly higher accuracy than traditional backstepping controllers in the presence of unknown disturbance and ocean currents. Figure 9 shows the tracking errors of 3D path. It can be seen that the error of real and desired path with the FTCFPC controller can converge in a short time. The position error of two controllers has some fluctuations, but the fluctuation of the FTCFPC method is much smaller. These can be proved that the FTCFPC method present in this paper has good robustness and effectiveness. Figures 10 and 11 depict the response of velocity and angle respectively, from which we can see that, FTCFPC method can achieve the desired path more quickly and has good anti-interference ability. The control signals of different channels are shown Figure 12.
In summary, the FTCFPC method proposed in this paper can make the AUV tracking to the desired path smoothly and robustly with the ocean currents. Following are the advantages which can been seen from the above results: 1.
The controller has strong robustness under the interference of ocean current.

2.
It has faster convergence, and the AUV can follow the desired path in a short time.

3.
The control has better following accuracy under current disturbance.

Conclusions
This paper proposes a FTCFPC controller to improve an underactuated AUV path following performance in the presence of ocean currents and unknown disturbances. The designed controller is based on command filtered backstepping method, finite-time theory and super-twisting observer techniques. A second-order filter is designed to achieve derivative of virtual control signals such that the computational complexity can be reduced and the differential expansion can be avoided. A filtered error compensation loop is developed to ensure the error between the desired signals and the filter signals converge. A super-twisting observer is proposed to reduce the impact of unknown disturbances and ocean currents. A finite-time controller is introduced to ensure the system can be stable in a short time. The system stability is analyzed based on the Lyapunov stability theory. Simulation results indicates that the 3D path following controller proposed in this paper for underactuated AUVs is more effective and robust than the conventional backstepping method in the presence of ocean currents and unknown disturbances.
For the future works, more real external disturbance will be considered such as impermanent currents and waves near the surface. The cooperative path following of AUVs and the limited performance of actuators should be developed.