Development and Control of an Innovative Underwater Vehicle Manipulator System

Abstract

Recently, as humans have become increasingly interested in ocean resources, underwater vehicle-manipulator systems (UVMSs) have played an increasingly important role in ocean exploitation. To realize precise operation in underwater narrow spaces, the fly arm underwater vehicle manipulator system (FAUVMS) is proposed with manipulators as its core. However, this system suffers severe dynamic coupling effects due to the combination of small vehicle and big manipulators. To resolve this issue, we propose a robust adaptive controller that contains two parts. In the first part, the extended Kalman filter (EKF) is designed to estimate the system states and predicts external disturbances to achieve adaptive control. In the second part, a chattering-free sliding mode control (SMC) is designed to converge the tracking errors to zero, thus guaranteeing the robustness of the controller. We constructed the simulation platform based on the geometric model of FAUVMS, and various simulations are carried out under different situations. Compared to the traditional methods, the proposed method has a faster convergent speed, a better robustness and adaptiveness to external disturbances, and the tracking errors of positions of the vehicle and each end-effector are much smaller.

1. Introduction

Recently, as humans have become increasingly interested in ocean resources, underwater vehicle-manipulator systems (UVMSs) have played an increasingly important role in ocean exploitation. Different kinds of UVMSs have been proposed to perform underwater tasks, such as grasping ocean animals, underwater welding, lifting heavy objects, opening and closing valves, biological sampling and salvaging of sunken objects [1,2,3,4].

Jiaolong [5] is a human-occupied UVMS with individual manipulators for underwater operation. This kind of underwater vehicle has a strong anti-interference capability because of its very large restoring moment, and the combination of heavy vehicles and light manipulators also improves the working stability, i.e., the dynamic coupling is small for the large vehicle. However, such a vehicle of large volume limits the working space of Jiaolong, causing work in narrow spaces, such as underwater structures, to be difficult.

To improve the environmental adaptability of such systems with individual manipulators, some UVMSs with small dimensions have been proposed to complete underwater operations [6,7]. Their UVMSs mainly consist of small vehicles and manipulators. Moreover, the combination of small vehicles and manipulators allows UVMSs to swim in an underwater structure. However, such a small vehicle cannot generate enough restoring moments, which leads to a poor anti-interference capability. Additionally, the movement of the manipulator can cause severe coupling dynamics. Researchers usually choose a small manipulator that is manufactured of light material, although it will reduce the load capacity of the UVMS.

UVMSs with individual manipulators have difficulties performing precise tasks due to floating vehicles. The dual-arm UVMSs are designed to carry cooperative manipulators [8,9] that could work with one manipulator while the other manipulator grasps underwater objects to remain stable; this characteristic helps the dual-arm UVMSs to overcome external disturbances with small sizes, and it has been proven that the dual-arm system has a higher working effectiveness than a single-arm system in valve tasks [10]. However, dual-arm UVMSs also suffer the same problem as single-arm UVMSs, i.e., severe dynamic coupling between the vehicle and manipulator, and the payload capacity is also limited by the size of the vehicle.

In this paper, a novel UVMS is proposed to complete underwater operations with certain payloads in narrow spaces; this UVMS is equipped with two cooperative heavy load manipulators to achieve a heavy load capability and six thrusters to achieve six degrees of freedom (DOF) to maneuver in complex environments, as shown in Figure 1. The special combination of two heavy load manipulators and a small vehicle indicates that the main components of the system are the manipulators; thus, we name the novel system the fly arm underwater vehicle manipulator system (FAUVMS) (Supplementary Materials). FAUVMS is designed with a navigation mode and operation mode for performing underwater tasks. It is placed into water with the navigation mode and navigates to the working place automatically, then switches to the operation mode to perform tasks with one manipulator while the other manipulator grasps an underwater object to remain stable, i.e., an underwater pipeline. With the help of two manipulators, FAUVMS can lift 10 kg loads in the air, but the high-load manipulators cause the system to break the 10 percent law [11]; this law states that the mass ratio between the manipulator and vehicle should be below 10 percent to gain motion stability. The dynamic coupling in the system is severe, and the direct effects on FAUVMS are hard-control attitude, highly influenced end-effector and increased system hysteresis. The severe dynamic coupling is complex and nonlinear and varies with the motions of the manipulators. The coupling will decrease the system stability and tracking performance, which causes controlling FAUVMS to become a major challenge.

Several control techniques appear in literatures. For a conventional UVMS equipped with a manipulator that is light compared to the vehicle [12], i.e., the mass ratio between the manipulators and vehicle is less than 10 percent, the system dynamic coupling is small and often directly considered an external disturbance. The proportion-integration-differentiation (PID) technique is the most widely used control scheme [13,14] due to its simplicity of development, small computation cost and good transient response when facing linear systems or linear disturbances. However, it has been verified that the performance will be unsatisfactory even during the tracking of a second-order trajectory. The sliding mode control (SMC) technique is a robust variable structure control strategy, and many robust control algorithms based on SMC have been developed for UVMS motion control [15,16]. Controllers derived from the SMC framework mainly suffer from two problems, i.e., the chattering problem and the singularity problem; moreover, researchers have proposed methods to solve the controller’s singularity problem and chattering problem by improving the sliding surface or using the continuous reaching law [17,18,19]. The SMC is also used to develop an auxiliary controller with tube-MPC to guarantee the tracking performance of the whole system [20]. However, the SMC will have a slow convergent speed to the equilibrium point when the system state is far from the equilibrium point.

For UVMSs having a manipulator with big mass, i.e., the mass ratio is higher than 10 percent, the motion of the manipulator highly influences the motion control stability. In this case, it is significant for designing the motion controller to estimate the dynamic coupling effects in the system accurately. Numerical analysis methods are widely used to gain a clear understanding of the system dynamics of UVMSs [21,22,23], and the recursive Newton-Euler method (RNEM) can provide more simple and accurate estimation of dynamic coupling between vehicles and manipulators [24]. Researchers incorporate the estimated coupling force into the control law and obtain robust controllers during the motion of the manipulators [25,26]. However, the RNEM is built on the accurate kinematic and dynamic model of the UVMS systems, which is difficult to derive in reality.

Controlling a UVMS is barely well-performed due to the existence of external disturbances, modeling uncertainties and measurement noises. The extended Kalman filter (EKF) is a well-known nonlinear filter for fusing measurement data with white noises [27,28] and estimating constant external disturbances [29,30], and the combination of EKF and model predictive control (MPC) [20,31] decreases the model dependency of MPC with EKF making the initial estimation of the nonlinear system uncertainties and modeling errors. However, the tracking performance of EKF-based controllers cannot be satisfied with the presence of irregular measurement noises and time-varying external disturbances.

FAUVMS suffers severe dynamic coupling, persistent modeling uncertainties, time-varying external disturbances and non-Gaussian distribution measurement noises in the underwater environment, which is a major challenge for the motion control of FAUVMS. Inspired by previous works, we designed a robust adaptive controller based on the above techniques, in which the RNEM method is used to evaluate the dynamic coupling effects and the EKF is used to fuse the measurement data and estimate the external disturbance. The control law is derived from the computed torque control (CTC) framework based on the estimated states and external disturbances, which guarantees the adaptiveness of the controller; moreover, an auxiliary chattering-free sliding controller is used to converge system tracking errors to zero, which guarantees the robustness of the controller.

Compared with related works in the literature, the main contributions of our work can be summarized.

• A novel concept of a dual-arm UVMS with manipulators as its core is proposed, the mechanical structure is designed in detail based on the modular design approach, and the control architecture is constructed based on the Robot Operating System (ROS).
• The dynamic model of FAUVMS that considers dynamic coupling is developed, and the RNEM is used to evaluate the dynamic coupling effects in the system.
• A robust adaptive controller is designed based on CTC, EKF and chattering-free SMC, and the closed-loop stability is guaranteed by Lyapunov theory.
• The simulation platform for FAUVMS is proposed based on Gazebo and the unmanned underwater vehicle simulator (UUVSimulator), and simulations are carried out to verify the effectiveness of the proposed control method.

The rest paper is arranged as follows. First, in Section 2, the detailed design of FAUVMS, including the mechanical structure and control architecture, is introduced. Then, the dynamic model of FAUVMS is presented in Section 3. The proposed controller is formulated in Section 4. In Section 5, simulations are demonstrated to verify the adaptiveness and robustness of the proposed control method. Finally, in Section 6, our work is summarized with conclusions and future works.

2. Design of FAUVMS

In this section, we introduce the mechanical structure and control architecture of FAUVMS for underwater tasks. We construct the mechanical structure of FAUVMS based on the modular design approach to easily maintain and reconfigure the robot for different underwater operations, and we construct the control architecture with ROS to achieve distributed communication.

2.1. Mechanical Structure

The detailed mechanical structure of FAUVMS is represented in Figure 2a, and the main parameters are shown in

Table 1. Technical parameters of the FAUVMS prototype.

Parameters	Values	Parameters	Values
Vehicle mass	63 kg	Metacentric height	<5 mm
Vehicle length	640 mm	Vehicle width	450 mm
Vehicle height	450 mm	Maximum depth	1000 m
Max/Min thrust	5 kg/−4 kg	Number of thrusters	6
Payload of arm	5 kg	Number of joints	4
Link mass	1.5/2.5/2.5/2.5 kg	Link length	220/330/250/50 mm
Link radius	60/60/60/50 mm	IMU&Compass	Pixhawk
Altimeter	Ping30	Velocity logger	Waterlinked A50

. FAUVMS is composed of an aluminum alloy frame, a control warehouse, two neutral buoyancy manipulators, six thrusters and two buoyancy shells. The neutral buoyancy manipulators are mounted on both sides of the frame, which is surrounded by two buoyancy shells to offer buoyancy and restoring moments. Six thrusters are used to serve as the propulsion system of FAUVMS.

The thruster configuration method is shown in Figure 2b. Six thrusters are used to achieve six DOF, two thrusters are mounted vertically, the other two are mounted laterally, and the rest are mounted horizontally. This configuration method is easy to install and layout, and the thrust control matrix (TCM) can be easily generated. The rotation and locomotion of each dimension of the space are only controlled by two thrusters, and each thruster is surrounded by a thruster tube to decrease the coupling between thrusters. The thruster layout increases the maneuverability of the FAUVMS and helps the vehicle to counter the dynamic coupling while the manipulators move.

The control warehouse is shown in Figure 2c. The control warehouse consists of Raspberry Pi, an STM32 controller, a DC battery, a DC-DC converter, an inertia measure unit (IMU) and a depth sensor. The battery, which can supply 16.8 V of power to the thrusters and 48 V of power to the manipulator actuators, is mounted at the end of the control warehouse. The STM32 controller and Raspberry Pi are mounted at the top of the control warehouse, and the STM32 controller can collect data streams from navigation sensors and transfer them to the Raspberry Pi through Recommended Standard 232 (RS232) serial port. An Nvidia Jetson Xavier is used to collect and process video streams from the binocular camera, which is mounted in front of FAUVMS.

Figure 2d is the structural diagram of the manipulator. The manipulator is surrounded by buoyancy shells to achieve neutral buoyancy, and this design minimizes dynamic coupling when the manipulators perform tasks. Each manipulator is composed of four joints and one gripper, and the compensation hydraulic oil supplied by the compensator fills the inner space of the manipulator to balance the pressure between the inner side and outside of the cylindrical shell, and this configuration method allows the manipulator to work in the deep ocean. The joint module consists of a DC brushless motor, a motor driver, a reduction mechanism and two encoders. The brushless motor, which is controlled by an Elmo Golden motor driver, actuates the joint through the reduction mechanism, i.e., a harmonic reducer and the encoders are used to feed the absolute joint positions back. The gripper is actuated with a screw and a gripper transmission mechanism, the screw converts the rotation of the motor into linear motion and actuates the transmission mechanism to achieve the actions of the gripper.

2.2. Control Architecture

ROS is an open-source framework for robot systems that offers a rich set of libraries and tools, extensive tutorials, a modular architecture and platform independence. Furthermore, these features help remove the barriers of robotics development, making ROS a popular choice for building robot control systems. As shown in Figure 3, the ROS-based control architecture for FAUVMS is grouped into four distinct parts, i.e., a ground control station (GCS), a mission layer, an execution layer, and a perception layer. ROS runs on all computers to build TCP/IP communication between ROS nodes with the help of ROS master via an Ethernet connection, which is a convenient way to realize distributed communication.

2.2.1. Ground Control Station

The ground control station (GCS) runs on a land monitor computer to achieve system monitoring and task distribution. It consists of an ROS-based navigation commander, operation commander, state monitor and camera monitor. The commanders receive human orders and translate them into manipulator commands and vehicle commands, and then publish those commands to the ROS master running on the companion computer Raspberry Pi. The state monitor and camera monitor receive processed system states and video streams from ROS master and show them on the GCS.

2.2.2. Mission Layer

The mission layer running on the Raspberry Pi mainly consists of a robot navigation node, vehicle control node, manipulator control node, and thrust control node. Raspberry Pi receives navigation sensor streams from the STM32 chip through RS232 serial and processed them by the navigation node, and the processed navigation data is then published to ROS master for vehicle and manipulator control nodes. Moreover, Raspberry Pi processes system commands from the GCS through vehicle and manipulator control nodes and sends actual control signals to the execution layer, i.e., thrusts and joint angles.

MoveIt! [32] is used to plan a 3D trajectory of manipulators based on commands in manipulator control node, and a sequential quadratic programming method [33] is exploited to allocate thrust based on the vehicle control node outputs. The proposed algorithms runs on the vehicle and manipulator control nodes.

2.2.3. Execution Layer

The execution layer consists of an STM32 chip, vehicle and manipulator actuators and navigation sensors. The STM32 chip collects navigation sensor streams and transfers them to Raspberry Pi via Inter-Integrated Circuit (IIC) and RS232 serial. The actual control signals from the mission layer are processed by STM32 and then translated into pulse-width modulation (PWM) signals via a timer to control the rotation speed of the thrusters and controller area network (CAN) signals via a CAN controller to control the joints of the manipulators.

2.2.4. Perception Layer

The perception layer consists of a front-looking binocular camera and an Nvidia Jetson Xavier, which is an embedded system with a GPU lying in the control warehouse. The binocular camera, which captures two images from different angles simultaneously, can gain the depth information of an object after image process procedure. The ROS-based video processor running on the embedded system receives the video streams of the front-looking binocular camera and exploits them for underwater video enhancements and pose estimations of underwater objects, and the enhanced videos as well as the poses of underwater objects are then published to ROS master.

3. Dynamic Modeling of FAUVMS

3.1. General Model of FAUVMS

The basic frames pf FAUVMS is defined as shown in Figure 4. Then, the mathematical model for FAUVMS is presented as follows [34,35]

$$M\left(q\right)\ddot{q}+C(q,\dot{q})\dot{q}+D(q,\dot{q})\dot{q}+G\left(q\right)+F(q,\dot{q})=\tau +{\tau }_d$$

(1)

where

$$\begin{array}{l}M\left(q\right)=\left[\begin{array}{cc}{M}_{\nu }\left(q_{\nu }\right)& H^T\left(q_m\right)\\ H\left(q_m\right)& M_m\left(q_m\right)\end{array}\right],C(q,\dot{q})=\left[\begin{array}{cc}{C}_{\nu }(q_{\nu },{\dot{q}}_{\nu })& 0\\ 0& C_m(q_m,{\dot{q}}_m)\end{array}\right]\\ D(q,\dot{q})=\left[\begin{array}{cc}{C}_{\nu }(q_{\nu },{\dot{q}}_{\nu })& 0\\ 0& C_m(q_m,{\dot{q}}_m)\end{array}\right],G\left(q\right)=\left[\begin{array}{c}{g}_{\nu }\left(q_{\nu }\right)\\ g_m\left(q_m\right)\end{array}\right]\\ F(q,\dot{q})=\left[\begin{array}{c}{F}_{\nu }(q,\dot{q})\\ F_m(q,\dot{q})\end{array}\right],\tau =\left[\begin{array}{c}{\tau }_{\nu }\\ {\tau }_m\end{array}\right],{\tau }_d=\left[\begin{array}{c}{\tau }_{\nu d}\\ {\tau }_{md}\end{array}\right]\end{array}$$

(2)

$q={\left[q_{\nu }^T{q}_m^T\right]}^T$ and $q_{\nu }={\left[xyz\varphi \theta \psi \right]}^T$ denote the absolute positions and Euler angles of FAUVMS, respectively. $xyz$ are surge, sway and heave positions, $\varphi \theta \psi$ are roll, pitch and yaw angles. $q_m={\left[q_{l1}{q}_{l2}{q}_{l3}{q}_{l4}{q}_{r1}{q}_{r2}{q}_{r3}{q}_{r4}\right]}^T$ denotes joint positions of manipulators, $q_{l1}{q}_{l2}{q}_{l3}{q}_{l4}$ denote joint positions of the left manipulator, and $q_{r1}{q}_{r2}{q}_{r3}{q}_{r4}$ denote joint positions of the right manipulator. $M\left(q\right)$ is the mass and inertia matrix (including the added mass effects), $C(q,\dot{q})$ is the Coriolis matrix including the added mass effects, $D(q,\dot{q})$ is the hydrodynamic force matrix, $G\left(q\right)$ is the force and moments vector generated by buoyancy and gravity, $F(q,\dot{q})$ is the coupling effects term, $F_{\nu }(q,\dot{q})$ is the coupling acting on the vehicle due to motions of manipulators, $F_{\nu }(q,\dot{q})$ is the coupling acting on manipulators due to motions of the vehicle, $\tau$ denotes the control inputs of FAUVMS, ${\tau }_{\nu }$ and ${\tau }_m$ are control inputs of the vehicle and manipulators, ${\tau }_d$ denotes the force/torque of FAUVMS generated by persistent modeling errors and external disturbances, and ${\tau }_{\nu d}$ and ${\tau }_{md}$ are disturbances suffered by the vehicle and manipulators.

Let

$$H(q,\dot{q})=C(q,\dot{q})\dot{q}+D(q,\dot{q})\dot{q}+G\left(q\right)+F(q,\dot{q})$$

Then, Equation (1) can be rewritten as follows:

$$M\left(q\right)\ddot{q}+H(q,\dot{q})=\tau +{\tau }_d$$

(3)

3.2. Coupling Force Estimation

The term $F_{\nu }(q,\dot{q})$, which is significantly nonlinear and related to the motion of the manipulators, represents the system coupling effects. We will work on estimating the coupling force with the RNEM.

The velocity of FAUVMS is $\nu ={\left[{\nu }_1^T{\nu }_2^T\right]}^T$ expressed in the body frame.Then, the velocity of Frame 0 of a manipulator can be presented as

$$\begin{array}{cc}\hfill {\dot{q}}_{\nu }& \hfill =J\nu \hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill {}^0\nu _0& \hfill =R_B^0{\nu }_1+{\nu }_2\times r_{0,B}^0\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {}^0\omega _0& \hfill =R_B^0{\nu }_2\hfill \end{array}$$

(6)

where J denotes the corresponding Jacobian matrix, $R_B^0$ is the rotation matrix from the body frame to Frame 0 of the manipulator, $r_{0,B}^0$ is the position vector from the origin of Frame 0 toward the origin of the body frame expressed in Frame 0, ${}^0\nu _0$ and ${}^0\omega _0$ are the linear and angular velocity of Frame 0 expressed in Frame 0.

Then, the velocity and acceleration of all links can be derived by substituting the velocity of Frame 0 of the manipulator with an iterative method [25].

$$\begin{array}{cc}\hfill {}^{i+1}\omega _{i+1}& \hfill =R_i^{i+1}({}^i\omega _i+{\dot{q}}_{i+1}{\overrightarrow{z}}_i)\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill {}^{i+1}\dot{\omega }_{i+1}& \hfill =R_i^{i+1}({}^i\dot{\omega }_i+{}^i\omega _i\times {\dot{q}}_{i+1}{\overrightarrow{z}}_i+{\ddot{q}}_{i+1}{\overrightarrow{z}}_{i+1})\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {}^{i+1}\nu _{i+1}& \hfill =R_i^{i+1}{}^i\nu _i+{}^{i+1}\omega _{i+1}\times {}^{i+1}r_{i,i+1}\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill {}^{i+1}\nu _{i+1,c}& \hfill =R_i^{i+1}{}^i\nu _i+{}^{i+1}\omega _{i+1}\times {}^{i+1}r_{i,c}\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {}^{i+1}\dot{\nu }_{i+c}& \hfill =R_i^{i+1}{}^i\dot{\nu }_i+{}^{i+1}\dot{\omega }_{i+1}\times {}^{i+1}r_{i,i+1}+{}^{i+1}\omega _{i+1}\times ({}^{i+1}\omega _{i+1}\times {}^{i+1}r_{i,i+1})\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {}^{i+1}\dot{\nu }_{i+1,c}& \hfill =R_i^{i+1}{}^i\dot{\nu }_i+{}^{i+1}\dot{\omega }_{i+1}\times {}^{i+1}r_{i,c}+{}^{i+1}\omega _{i+1}\times ({}^{i+1}\omega _{i+1}\times {}^{i+1}r_{i,c})\hfill \end{array}$$

(12)

where ${}^{i+1}\nu _{i+1}$ and ${}^{i+1}\omega _{i+1}$ denote the translational and rotational velocities of Frame $i+1$ expressed in Frame $i+1$, ${}^{i+1}\nu _{i+1,c}$ denotes the translational speed of the center of mass (COM) of Link $i+1$, ${\dot{q}}_{i+1}$ denotes the rotational speed of Joint $i+1$ in the joint space, ${\overrightarrow{z}}_i$ denotes the rotational unit vector of Joint $i+1$, ${}^{i+1}r_{i,i+1}$ is the position vector from the origin of Frame i toward the origin of Frame $i+1$ expressed in Frame $i+1$, and ${}^{i+1}r_{i,c}$ is the position vector from the origin of Frame i toward the COM of Link $i+1$ expressed in Frame $i+1$.

With the velocity and acceleration of each link, we can obtain the inertia force ${}^{i+1}F_{i+1}$ and moments ${}^{i+1}T_{i+1}$ acting on Link $i+1$ based on the following equations:

$$\begin{array}{cc}\hfill {}^{i+1}F_{i+1}& \hfill =M_{i+1}({}^{i+1}\dot{\nu }_{i+1}+{}^{i+1}\dot{\omega }_{i+1}\times {}^{i+1}r_{i,\mathrm{c}}+{}^{i+1}\omega _{i+1}\times {}^{i+1}\omega _{i+1}\times {}^{i+1}r_{i,c}))\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {}^{i+1}T_{i+1}& \hfill =I_{i+1}{}^{i+1}\dot{\omega }_{i+1}+{}^{i+1}\omega _{i+1}\times \left(I_{i+1}{}^{i+1}\omega _{i+1}\right)\hfill \end{array}$$

(14)

where $M_{i+1}$ and $I_{i+1}$ are the mass and inertia matrix of Link $i+1$ including the added mass effects expressed in Frame $i+1$.

Considering underwater environments, we have

$${}^{i}f_i=R_{i+1}^i{}^{i+1}f_{i+1}+{}^{i}F_i-m_i{g}_i+\rho g_i{▽}_i+{}^{i}d_i$$

(15)

$$\begin{array}{ll}{}^{i}n_i& \hfill =R_{i+1}^i{}^{i+1}n_{i+1}+{}^{i}r_{i-1,i}\times R_{i+1}^i{}^{i+1}f_{i+1}+{}^{i}r_{i-1,c}\times {}^{i}F_i\hfill \\ & +{}^{i}T_i+{}^{i}r_{i-1,c}\times (-m_i{g}_i+{}^{i}d_i)+{}^{i}r_{i-1,b}\times \rho g_i{▽}_i\end{array}$$

(16)

where ${}^{i}f_i$ and ${}^{i}n_i$ denote the force and torque acting on Joint i, ${▽}_i$ is the volume of Link i, $g_i$ is the gravity acceleration vector in Frame i, ${}^{i}r_{i-1,b}$ and ${}^{i}r_{i-1,c}$ are the position vector from the origin of the Frame $i-1$ toward the center of buoyancy (COB) and COM of Link i, $m_i$ is the mass of Link i, ${}^{i}d_i$ is the damping force of Link i expressed in Frame i, and $\rho$ denotes the density of environmental fluid.

Referring to Equations (15) and (16), it is capable of deriving the dynamic coupling acting on the vehicle:

$$\begin{array}{cc}\hfill {}^{0}f_0& \hfill =R_1^0{}^{1}f_1\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {}^{0}n_0& \hfill =R_1^0({}^{1}n_1+{}^{1}r_{0,1}\times {}^{0}f_0)\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill {}^{B}f_0& \hfill =R_0^B{}^{0}f_0\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {}^{B}n_0& \hfill =R_0^B{}^{0}n_0+{}^{1}r_{0,1}\times {}^{B}f_0\hfill \end{array}$$

(20)

where ${}^{B}f_0$ and ${}^{B}n_0$ are the estimated coupling force and torque expressed in the body frame, $R_0^B$ denotes the rotation matrix from Frame 0 of the manipulator to the body frame.

Considering that FAUVMS carries two manipulators arm-l and arm-r, we can easily obtain the forces and torques generated by arm-l ${}^{B}f_{0,l},{}^{B}n_{0,l}$ and arm-r ${}^{B}f_{0,r},{}^{B}n_{0,r}$, and $F_{\nu }(q,\dot{q})$ in the mathematical model of FAUVMS is derived:

$$\begin{array}{cc}\hfill & \hfill F_{\nu }(q,\dot{q})={\left[f_m^T{n}_m^T\right]}^T\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill & \hfill f_m={}^{B}f_{0,l}+{}^{B}f_{0,r}\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill & \hfill n_m={}^{B}n_{0,l}+{}^{B}n_{0,r}\hfill \end{array}$$

(23)

4. Robust Adaptive Control of FAUVMS

The control diagram is shown in Figure 5. Note that trajectory planning and inverse kinematics are not part of our research, so it is assumed that we already have the desired states of FAUVMS. Considering the complex underwater environment and highly nonlinear dynamic coupling, the robust adaptive controller is arranged into two parts. The first part is the main control law derived by the EKF and CTC framework; the famous nonlinear filter EKF can fuse the measurement data with zero-mean Gaussian noise and estimate the external disturbances, which are then incorporated into the control law to gain better performance. However, the EKF technique is formulated with zero-mean Gaussian noise, which never exists in the real world; thus, this method can only fuse data with zero-mean Gaussian noise and cannot guarantee robust control performance. To overcome this issue, the second part is developed with the system error model through the SMC technique. The main purpose of the SMC controller is to converge the system tracking errors to zero.

4.1. EKF Disturbance Observer Design

The famous nonlinear filter EKF is formulated to estimate the states of FAUVMS for robot localization and the external disturbances generated by the water current, model uncertainties and measurement noises. The state variables are defined as follows:

$$X={\left[q^T{\dot{q}}^T{\tau }_d^T\right]}^T$$

The augmented system dynamics are expressed as:

$$\dot{X}=f(X,\tau ,e_1)$$

where $e_1\sim (0,E_1)$ is the system noise vector, and

$$f(X,\tau ,e_1)=\left[\begin{array}{c}\stackrel{q}{\dot{}}\\ M{\left(q\right)}^{-1}(\tau +{\tau }_d-H(q,\dot{q}))+e_1\\ 0\end{array}\right]$$

The measurement equation is designed as:

$$Z=h\left(X\right)+e_2$$

where

$$h\left(X\right)={\left[xyz\varphi \theta \psi q_{l1}{q}_{l2}{q}_{l3}{q}_{l4}{q}_{r1}{q}_{r2}{q}_{r3}{q}_{r4}\right]}^T$$

$e_2\sim (0,E_2)$ is the sensory noise vector.

$e_1$ and $e_2$ are white noise vectors with zero mean Gaussian distribution, $E_1$ and $E_2$ denote their covariance matrices related to the filter sampling time T, i.e.,

$$\begin{array}{cc}\hfill E_1& =\left[\begin{array}{cccccc}0.25T^4& 0.5T^3& 0& 0& 0& 0\\ 0.5T^3& T^2& 0& 0& 0& 0\\ 0& 0& 0.25T^4& 0.5T^3& 0& 0\\ 0& 0& 0.5T^3& T^2& 0& 0\\ 0& 0& 0& 0& 0.25T^4& 0.5T^3\\ 0& 0& 0& 0& 0.5T^3& T^2\end{array}\right]\in R^{6\times 6}\hfill \\ \hfill E_2& =diag\left(TT...TT\right)\in R^{14\times 14}\hfill \end{array}$$

(24)

The proposed EKF consists of the prediction step and the correction step, which are designed as follows.

Prediction step:

$$\begin{array}{c}{\widehat{X}}_k^{-}=f({\widehat{X}}_{k-1},{\tau }_{k-1},0)\end{array}$$

(25)

$$\begin{array}{c}{P}_k^{-}=A{P}_{k-1}{A}^T+WQ{W}^T\end{array}$$

(26)

Correction step:

$$\begin{array}{cc}\hfill K_k& \hfill =\left(P_k^{-}{H}^T\right){\left(H{P}_k^{-}{H}^T+VR{V}^T\right)}^{-}\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill {\widehat{X}}_k& \hfill ={\widehat{X}}_{k-1}^{-}+K_k(Z_k-h({\widehat{X}}_k^{-},0))\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill P_k& \hfill =(I-K_{k}H)P_k^{-}\hfill \end{array}$$

(29)

where

$$\begin{array}{cccc}\hfill A& \hfill =\frac{\partial f}{\partial X}{|}_{{\widehat{X}}_{k-1},{\tau }_{k-1}}\hfill & \hfill W& \hfill =\frac{\partial f}{\partial w}{|}_{{\widehat{X}}_{k-1}}\hfill \\ \hfill H& \hfill =\frac{\partial h}{\partial X}{|}_{{\widehat{X}}_{k-1}}\hfill & \hfill V& \hfill =\frac{\partial h}{\partial \nu }{|}_{{\widehat{X}}_{k-1}}\hfill \end{array}$$

4.2. Chattering-Free Sliding Mode Controller Design

We let $q_d$ be the desired position for FAUVMS, $\tilde{q}=q-q_d$ denote the position error, $\dot{\tilde{q}}=\dot{q}-{\dot{q}}_d$ denote the velocity error, and $\ddot{\tilde{q}}=\ddot{q}-{\ddot{q}}_d$ denote the acceleration error. The control law for FAUVMS is designed as follows:

$$\begin{array}{cc}\hfill & \hfill \tau =M\left(q\right)({\ddot{q}}_d-K_{CP}\tilde{q}-K_{CD}\dot{\tilde{q}})+H(q,\dot{q})-{\widehat{\tau }}_d+{\tau }_{SMC}\hfill \end{array}$$

(30)

where $K_{CP}$ and $K_{CD}$ are diagonal positive definite weight matrices, ${\widehat{\tau }}_d$ is the estimated disturbance vector, and ${\tau }_{SMC}$ is the control law designed by the sliding mode control technique.

By substituting Equation (30) into Equation (3), the error system can be derived:

$$\begin{array}{c}\hfill M\left(q\right)(\ddot{\tilde{q}}+K_{CP}\tilde{q}+K_{CD}\dot{\tilde{q}})={\tau }_{SMC}+d\end{array}$$

(31)

where $d={\tau }_d-{\widehat{\tau }}_d$ denotes the uncompensated disturbance vector.

The following notations are used to simplify the expression and design of the sliding mode controller.

$$\begin{array}{cc}\hfill & \hfill sig{\left(q\right)}^{\gamma }={\left[\begin{array}{ccc}|q_1{|}^{{\gamma }_1}sign\left(q_1\right)& ...& |q_n{|}^{{\gamma }_n}sign\left(q_n\right)\end{array}\right]}^T\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill & \hfill q^{\gamma }={\left[\begin{array}{ccc}{q}_1^{{\gamma }_1}& ...& q_n^{{\gamma }_n}\end{array}\right]}^T,{\left|q\right|}^{\gamma }={\left[\begin{array}{ccc}|q_1{|}^{{\gamma }_1}& ...& |q_n{|}^{{\gamma }_n}\end{array}\right]}^T\hfill \end{array}$$

(33)

Hence, the sliding surface is designed as

$$\begin{array}{c}\hfill s=\tilde{q}+\alpha sig{\left(\dot{\tilde{q}}\right)}^{\gamma }=0\end{array}$$

(34)

where $s={[s_1...s_{14}]}^T\in R^{14}$, $\alpha =diag({\alpha }_1,...,{\alpha }_{14})$, and $1<{\gamma }_1,...,{\gamma }_{14}<2$. A fast-type reaching law is designed as

$$\begin{array}{c}\hfill \dot{s}=-K_{s1}s-K_{s2}sig{\left(s\right)}^{\varrho }\end{array}$$

(35)

where $K_{s1}$ and $K_{s2}$ are the diagonal positive definite weight matrices, $\varrho =[{\varrho }_1...{\varrho }_{14}],0<{\varrho }_1=...={\varrho }_{14}<1$.

Theorem 1.

Regarding the UVMS error dynamic system Equation (31), if the sliding surface is chosen as Equation (34), the reaching law is designed as Equation (35), and the chattering-free sliding mode control law is designed as

$$\begin{array}{cc}\hfill & {\tau }_{SMC}={\tau }_0+{\tau }_1\hfill \\ \hfill & {\tau }_0=M\left(q\right)(K_{CD}\dot{\tilde{q}}+K_{CP}\tilde{q}-{\alpha }^{-1}{\overline{\gamma }}^{-1}sig{\left(\dot{\tilde{q}}\right)}^{2-\gamma })\hfill \\ \hfill & {\tau }_1=-M\left(q\right)(K_{s1}s+K_{s2}sig{\left(s\right)}^{\varrho })\hfill \end{array}$$

(36)

where$\overline{\gamma }=diag({\gamma }_1,...,{\gamma }_{14})$. Moreover, we can deduce the following results.

(i) 

In the absence of uncompensated disturbances, i.e., $d=0$, the system error $\tilde{q}$ will converge to zero in finite time.

(ii) 

In the presence of uncompensated disturbances, i.e., $d\ne 0$, the system error will converge to the region

$$\begin{array}{cc}\hfill & \hfill ||s||\le \mathsf{\Delta }=min\{\frac{{\left|\right|M\left(q\right)}^{-1}\left|\right|d}{\overline{k_{s1}}},{\left(\frac{{\left|\right|M\left(q\right)}^{-1}\left|\right|d}{\overline{k_{s2}}}\right)}^{1/\varrho }\}\hfill \end{array}$$

in finite time, where $\overline{k_{s1}}$ and $\overline{k_{s2}}$ are the minimum eigenvalues of $K_{s1}$ and $K_{s2}$. In addition, the position errors $\widetilde{q_i}$ and velocity errors $\tilde{\dot{q}}$ converge to the regions

$$\begin{array}{c}\hfill |\widetilde{q_i}|\le 2\mathsf{\Delta },|\widetilde{\dot{q_i}}|\le {\left(\frac{\mathsf{\Delta }}{{\alpha }_i}\right)}^{1/{\gamma }_i},\end{array}$$

(37)

Proof. 

Consider the Lyapunov candidate $V={\left|\right|s\left|\right|}_2^2$. We have

$$\begin{array}{cc}\hfill & \hfill {\dot{s}}_n=\dot{\tilde{q}}+{\alpha }_n{\gamma }_n{\left|\dot{\tilde{q}}\right|}^{{\gamma }_i-1}\ddot{\tilde{q}}\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill & \hfill \dot{V}=s^T\dot{s}=s^T\alpha \overline{\gamma }diag\left({\dot{\tilde{q}}}^{\gamma -1}\right)M{\left(q\right)}^{-1}({\tau }_1+{\tilde{\tau }}_d)\hfill \end{array}$$

(39)

and the rest of the proof is the same as in [18]. □

5. Simulation and Analysis

5.1. Description of the Simulation Platform

In this section, the adaptiveness and robustness of the proposed control scheme are demonstrated by simulations carried out on the UUVSimulator platform [36] based on the ROS. The thrust allocation matrix is derived through the geometric model of FAUVMS, and thrusts are allocated with sequential quadratic programming (SQP) [33].

The vehicle is trimmed to have the COB nearly vertically above the COM and the origin of the body frame locates on the COM. The manipulator links are not strictly neutrally buoyant but are trimmed to have the COB coincide with the COM of each link, and each link is set with a 0.5 kg net weight in total. The whole system is adjusted to nearly neutrally buoyant. Considering that the vehicle has an ellipsoid shape and the manipulator links are all cylindrical, the added mass term and hydrodynamic term are then estimated with experimental equations [35]. Other parameters, such as mass and inertia, COM, COB and volume, are derived from the geometrical design of the FAUVMS. The total parameters are shown in

Table 2. Simulation parameters.

Parameters	Values	Parameters	Values
Inertia of vehicle	$diag\left[11.57.111.5\right]$ ${\mathrm{kgm}}^2$	COM of vehicle	${\left[000\right]}^T$ mm
COB of vehicle	${\left[003\right]}^T$ mm	Ellipsoid parameters A	280 mm
Ellipsoid parameters B	350 mm	Ellipsoid parameters C	280 mm
Inertia of link1	$diag\left[7.67.34.5\right]$ ${\mathrm{gm}}^2$	Inertia of link2	$diag\left[5.819.117.2\right]$ ${\mathrm{gm}}^2$
Inertia of link3	$diag\left[7.520.619.7\right]$ ${\mathrm{gm}}^2$	Inertia of link4	$diag\left[7.67.34.5\right]$ ${\mathrm{gm}}^2$
COM of link1	${\left[017.8147.6\right]}^T$ mm	COM of link1	${\left[124.94.730.7\right]}^T$ mm
COM of link3	${\left[108.92.19.6\right]}^T$ mm	COM of link4	${\left[00238.6\right]}^T$ mm
Volume of Link1	$2.13\times {10}^{-3}$ ${\mathrm{m}}^3$	Volume of Link2	$1.64\times {10}^{-3}$ ${\mathrm{m}}^3$
Volume of Link3	$1.64\times {10}^{-3}$ ${\mathrm{m}}^3$	Volume of Link4	$1.21\times {10}^{-3}$ ${\mathrm{m}}^3$

.

In the proposed performance analysis, comparisons between the proposed controller and three conventional controllers are made to demonstrate the tracking performance and effectiveness. The conventional controllers used in simulations are the PID controller, EKF-based computed torque controller proposed by Mohan [29], and EKF-based computed torque control with the H-infinite controller proposed by Dai [30].

The PID control law used for comparisons in simulations is as follows

$$\begin{array}{c}\hfill \tau =K_{Pm}\tilde{q}+K_{Dm}\dot{\tilde{q}}+K_{Im}\int \tilde{q}dt\end{array}$$

(40)

where $K_{Pm},K_{Dm},K_{Im}$ are the positive definite gain matrices of the PID controllers.

The EKF-based computed torque controller law proposed by Mohan is as follows

$$\begin{array}{cc}\hfill & \hfill \tau =M\left(\widehat{q}\right)({\ddot{q}}_d-K_{Pn}\tilde{q}-K_{Dn}\dot{\tilde{q}})+H(\widehat{q},\dot{\widehat{q}})-{\widehat{\tau }}_d\hfill \end{array}$$

(41)

where $K_{Pn},K_{Dn}$ are the positive definite gain matrices of Mohan’s controllers.

The control law presented by Dai is as follows

$$\begin{array}{cc}\hfill & \hfill \tau =M\left(q\right)({\ddot{q}}_d-K_{Pd}\tilde{q}-K_{Dd}\dot{\tilde{q}})+H(q,\dot{q})-{\widehat{\tau }}_d-M\left(q\right)R^{-1}{B}^{T}P\hfill \end{array}$$

(42)

where $K_{Pd},K_{Dd}$ are the positive definite gain matrices of Dai’s controllers, $R,P$ are the positive definite matrices, and B is the coefficient matrix of the control input.

5.2. Description of the Scenarios

Three scenarios with four different cases have been considered in our simulations. All scenarios are carried out based on a simple task. The task is that FAUVMS moves the manipulator from the initial state to the working state when the vehicle performs a dynamic positioning (DP) task at the initial state ${[00-6000]}^T$, as shown in Figure 6.

The first scenario is that FAUVMS carries out a simple task without payloads, modeling uncertainties, measurement noises and external disturbances to illustrate the basic performance of the proposed controllers. The second scenario is that FAUVMS performs the same task with payloads, modeling uncertainties, measurement noises and external disturbances. A small constant water current and payloads are added to the simulations to show the tracking performance and adaptiveness of the proposed controller. The third scenario is the same as the second scenario with the application of a time-varying water current. The time-varying water current is added to verify the effectiveness and robustness of the proposed controller. All scenarios are carried out four times with different controllers to obtain comparisons.

5.3. Results and Discussion

5.3.1. Scenario 1

The first scenario is carried out without measurement noises, external disturbances and payloads. It is noted that this scenario will never occur in the real world; however, the first scenario can illustrate the basic performance of the robust adaptive controllers. Figure 7 and Figure 8 show the tracking errors of the vehicle and each end-effector.

Table 3. Mean square errors (MSEs) of positions of each end-effector with different controllers in Scenario 1.

MSE/cm	PID	Mohan	Dai	Proposed
Left X	1.89	1.23	3.36	1.31
Left Y	2.19	1.39	3.98	1.65
Left Z	1.78	2.31	1.45	0.29
Right X	1.96	0.99	3.17	1.74
Right Y	2.04	1.24	3.59	1.69
Right Z	1.93	2.74	1.85	0.21

shows the mean square errors (MSEs) of positions of each end-effector. Since tracking errors of the vehicle will eventually be reflected in the tracking errors of end-effectors, we only provide MSEs of positions of each end-effector. The results show that all controllers have good performance with small tracking errors maintained in both the position and attitude and the MSEs of the positions of each end-effector are all less than 4 cm. The pitch tracking errors are slightly higher than the roll and yaw errors with a maximum of 0.1 rad because each link of the manipulators has 0.5 kg net weight. Overall, the proposed controller has the same basic tracking performance as the conventional three controllers.

5.3.2. Scenario 2

The second scenario is carried out with measurement noises, external disturbances and payloads. In this scenario, 1 kg payloads are added to each manipulator and a 0.15 m/s water current with a horizontal angle of 0.1 rad and a vertical angle of 0.1 rad is also added to the simulation. In addition, noises affect the sensory measurements, Gaussian noises with a square deviation of 0.01 ${\mathrm{m}}^2$ are added to the position measurements, and Gaussian noises with a square deviation of 0.01 ${\mathrm{rad}}^2$ are added to the attitude measurements. Figure 9 and Figure 10 show the tracking errors of the vehicle and each end-effector.

Table 4. MSEs of positions of each end-effector with different controllers in Scenario 2.

MSE/cm	PID	Mohan	Dai	Proposed
Left X	1.74	1.92	1.49	0.93
Left Y	1.95	2.33	1.35	1.23
Left Z	4.99	4.15	3.89	1.02
Right X	1.08	1.31	1.61	1.72
Right Y	1.59	2.69	1.13	1.15
Right Z	5.17	6.22	6.27	0.61

shows MSEs of positions of each end-effector.

The results show that all controllers except the PID controller are better in the DP task and end-effector tracking by adapting the constant external disturbance, Gaussian noises and payloads, the positions holding errors of the PID controller is much larger than others. However, Mohan’s controller and Dai’s controller have poor performances in maintaining the pitch angle due to the movement of the manipulators, and there exists a steady state error of 0.1 rad, thus leading to a poor tracking performance of the end-effector in the Z direction with MSEs of 4.15 cm and 3.89 cm of the left end-effector and MSEs of 6.22 cm and 6.27 cm of the right end-effector, respectively. The proposed controller is satisfactory in maintaining the pitch angle and has a faster convergent time, which is contributed by the SMC control law for compensating the payloads and converging the tracking errors to zero, and the MSEs of the left and right end-effectors in the Z direction are 1.02 cm and 0.61 cm, respectively. Overall, the proposed controller, Mohan’s controller and Dai’s controller all have adaptiveness to constant external disturbances, Gaussian noise and payloads, but the proposed controller has a faster convergent speed and small tracking errors.

5.3.3. Scenario 3

The third scenario is carried out with non-Gaussian measurement noises, payloads and time-varying water currents. The same payloads are added to the simulation, the water current still has a horizontal angle of 0.1 rad and a vertical angle of 0.1 rad, and the current speed is generated by the following sinusoidal function. Non-Gaussian measurement noises, which have the same deviation as the noises in scenario 2, are added to the simulation, and the mean values of the noises are 0.01 m and 0.01 rad, respectively. Figure 11 and Figure 12 show the tracking errors of the vehicle and end-effector.

Table 5. MSEs of positions of each end-effector with different controllers in Scenario 3.

MSE/cm	PID	Mohan	Dai	Proposed
Left X	9.28	9.24	9.71	1.63
Left Y	1.88	3.37	4.33	1.59
Left Z	5.23	3.95	3.82	1.92
Right X	6.24	9.55	10.59	1.86
Right Y	0.78	2.05	2.57	1.34
Right Z	5.99	6.38	6.72	1.83

shows MSEs of positions of each end-effector.

$$\begin{array}{c}\hfill {\nu }_c=0.25sin\left(t\right)\end{array}$$

The PID controller performs poorly in maintaining the position, where the tracking errors are even divergent when facing the time-varying water current. The time-varying water current mainly affects the linear motion of the vehicle, but Mohan’s controller and Dai’s controller perform well in position tracking. However, their controllers are highly influenced in attitude tracking, thus having poor end-effector tracking performances in the X and Z directions with the smallest MSE of 3.82 cm and the largest MSE of 10.59 cm. The proposed controller has the smallest tracking errors both in the DP task and end-effector tracking task, the MSEs of positions in highly influenced directions are all less than 2 cm, which are much smaller than the conventional methods.

The advantages of our proposed controller are that the EKF makes the initial compensation of the Gaussian noises and model uncertainties, i.e., unknown payloads; then, the torque derived by the computed torque techniques is applied to FAUVMS, and the torque derived by the error system and SMC technique is added to the vehicle to compensate for the non-Gaussian term and converge the tracking errors. The EKF and SMC are all insensitive to external disturbances and payloads, thus ensuring the robustness of the proposed controllers.

Overall, the third scenario verifies the effectiveness and robustness of the proposed controllers facing non-Gaussian measurement noises, payloads and time-varying water currents. Parameter tuning is carried out with practical engineering experience, and parameter tuning will be considered in our future work.

6. Conclusions

The goal of this article is to propose a novel UVMS system with manipulators as its core. The advantages of the proposed FAUVMS, which is a combination of a small vehicle and big manipulators, are its high environmental adaptiveness, ability to handle heavy payloads and high maneuverability. Since FAUVMS suffers severe dynamic coupling due to its unique mechanical structure, we designed a robust adaptive controller to estimate the coupling force, modeling uncertainties and payloads to overcome the severe dynamic issue. The EKF is used to fuse the measuring data and estimate the payloads and external disturbances. Then, a chattering-free SMC controller is used to compensate for the rest of the disturbances and converge the tracking errors to zero.

Simulations are demonstrated to illustrate the robustness and adaptiveness of the proposed control method based on Gazebo and UUVSimulator platforms. It is observed that the PID controller is easily influenced by disturbances such as non-linear water currents and payloads on each end-effector. Methods proposed by Dai and Mohan have a good performance in position holding of the vehicle, but there exist steady state errors in pitch holding of the vehicle, thus leading to a poor tracking performance of the end-effectors. Compared to the traditional methods, the proposed method has a faster convergent speed, a better robustness and adaptiveness to external disturbances, and the tracking errors of positions of the vehicle and each end-effector are much smaller.

In the future, parameter tuning of the controller and an experimental pool test will be carried out for further research.