Dynamics Simulation of Grasping Process of Underwater Vehicle-Manipulator System

Abstract

Underwater vehicle-manipulator system (UVMS) can be applied to fulfill different complex underwater tasks such as grasping, drilling, sampling, etc. It is widely used in the field of oceanographic research, marine exploration, military, and commercial applications. In this paper, the dynamic simulation of UVMS is presented in the process of grasping an object. First, the dynamic model of UVMS, which considers the change of the load of manipulator when the end effector of manipulator grasps the object, is developed. To compare different conditions, numerical simulation of grasping processes without/with vehicle attitude control are carried out. The simulation results show that the coupling dynamics between the vehicle and the manipulator in the grasping process are clearly illustrated. It deteriorates the positioning accuracy of the end effector of the manipulator and is harmful to underwater precision operations. The tracking position error of end effector without vehicle control is large and UVMS cannot complete the grasping task under this condition. Vehicle control can compensate the motion of the vehicle due to the coupling effect caused by the motion of the manipulator. This study will contribute to underwater operation mission for UVMS with floating base.

1. Introduction

Underwater vehicle-manipulator system (UVMS) is a powerful tool for ocean exploration and marine environment observation [1,2,3]. It can also be applied to fulfill the different complex underwater operation tasks such as grasping, drilling, sampling, etc., [4,5,6]. Due to the unknown underwater environment caused by poor knowledge of the hydrodynamic coefficients, the floating base and under-actuated of the UVMS, strongly nonlinear and coupled dynamics, and unpredictable external disturbances, these bring huge challenge for the underwater operation mission. Therefore, motion coordination and disturbance compensation for UVMS need to be considered to reduce the coupling effect between the vehicle and the manipulator [7,8,9,10]. Underwater grasping operation of UVMS is also a significant point in research and engineering practice.

Plenty of kinds of UVMSs have been studied and developed in recent years. ALIVE robot (see Figure 1a) equipped with two hydraulic grasps for autonomous underwater docking and a 7 degree-of-freedoms (DOF) hydraulic manipulator used for valve turning task [11]. SAUVIM (see Figure 1b), a semi-autonomous UVMS equipped with an Ansaldo 7 DOF manipulator developed in the AMADEUS project [12], was proposed for free floating underwater operation [13]. In RAUVI, TRIDENT, and TRITON project, GIRONA 500 (see Figure 1c) is a lighter I-AUV of less than 200 kg, whose mass is lighter than ALIVE (3.5 ton) and SAUVIM (6 ton) [14]. Therefore, the effects of the mass and geometry of vehicle and manipulator on dynamic coupling condition should be considered [15,16]. A lightweight multi-link symmetrical structure was carried out and designed to reduce the coupling effect on the vehicle caused by the motion of manipulator [17]. Meanwhile, dual arm UVMS was proposed and developed for cooperating manipulation in recent years [18]. Some UVMS with dual arms have been studied, such as DexROV (see Figure 1d) [19], Ocean One (see Figure 1e) [20], Aquanaut [21], etc. Dual arms can balance the operation in both sides and improve the stability of UVMS.

Underwater grasping is an important function for underwater operation. A UVMS requires coordinated motion and more robust and optimal control scheme to overcome some adverse factors. Sarkar and Podder [24] proposed a motion coordination planning strategy based on the optimized hydrodynamic drag force. Considering the redundancy of UVMS, Antonelli and Chiaverini [25] adopted a fuzzy task-priority inverse kinematics resolution approach to manage the vehicle-manipulator coordination. Han et al. [26] proposed a performance index for redundancy resolution of a UVMS, the restoring moments of the UVMS during manipulation could be reduced by optimizing the index. In order to maintain dynamic stability of the UVMS, zero moment point method is adopted in reference [27]. Considering manipulator joint limit, weighted minimum norm method can be applied in motion planning of UVMS [28]. Considering the collision avoidance, a local motion planning method for inspection mission had been presented and a trained artificial neural network was utilized [29]. According to sampling-based, search-based, and optimization-based motion planners, these algorithms and their performance with different metrics were analyzed and compared by qualitative/quantitative [30].

The body of UVMS has three working modes: under-actuated, fully actuated, and over-actuated [5,31,32]. It is obvious that fully actuated and over-actuated mode can ensure the vehicle station keeping and dynamic positioning. However, under-actuated mode can reduce costs and energy consumption. An indirect adaptive control method of UVMS for underwater manipulation tasks (pick and place operation) based on an extended Kalman filter was presented by Mohan and Kim. This method overcomes the disadvantages of direct adaptive control scheme and existing disturbance observers [33]. Then they carried out a coordinated motion control scheme using disturbance observer in task space [34]. Whereas a tracking controller with both joint-space and task-space tracking errors was proposed by Han et al. [35]. Barbalata et al. [36] utilized force/motion controllers to handle a lightweight UVMS interaction with the underwater environment and compensate the coupling effects between the vehicle and the manipulator. Huang et al. [37] investigated the state-of-the-art about dexterous operation underwater robot, underwater autonomous environmental perception, UVMS modeling and coordinated control, target grasping, etc. Acceleration-level task priority redundancy resolution method was proposed and remotely autonomous underwater with organism absorb or grasp function were analyzed and compared. Conti et al. [38] proposed a control architecture for intervention UVMS and studied on a suitable grasp planning strategy. Anderlini et al. [39] investigated an ROV with the consequent changes system dynamics when carried an object, an adaptive model predictive control scheme was proposed and its performance was compared with PID and sliding mode control. Many different control strategies for UVMS to track task trajectories were designed and carried out in recent years. Because UVMS consists of two subsystems, vehicle station-keeping and manipulator motion control are also important. There are many different control methods, such as PID control [40], model-based motion control [9,41], adaptive backstepping control [42,43,44,45], model predictive control [46,47,48,49,50,51,52], adaptive control [53,54], active disturbance rejection control [55,56], force/position control [36,57], dynamic surface control [58,59], and so on. In addition, sliding mode control of UVMS for second-order system is common and reliable, such as sliding mode impedance control [60], terminal sliding control [43,53,61,62,63,64], dynamic sliding mode control [65], robust sliding mode control [5], multiple sliding mode methods [66], dynamic neural network [67], double-loop sliding mode control [68,69,70], adaptive sliding mode PID control [71], fuzzy sliding mode control [72,73,74,75], and so on. Furthermore, vision-based dexterous manipulation is also a useful form for underwater operations [41,76,77,78]. However, motion planning, advance control, and sensing identification need higher technical support, and are hard to implement in practice. Therefore, a simple method is carried out in this paper by using PID control scheme.

Considering the dynamic coupling of the manipulator and the vehicle, interaction between the manipulator and the object, the changing of dynamic properties (such as mass and inertia of moment of end effector) when grasping an object, the dynamic simulation of grasping process of a floating underwater vehicle with a 3 DOF planar underwater manipulator is studied by using PID control scheme. This paper is organized into five sections. Following the Introduction in Section 1, the remainder of this paper is structured as follows. Section 2 introduces the dynamic modeling of UVMS, including kinematic and dynamic equations of the UVMS, configuration of thrusters, environment contacting force, actuator saturation, and modeling of end effector carrying an object. The PID controller is applied, and the parameters of UVMS and controller are listed in Section 3. In Section 4, numerical simulation experiments are performed to verify without/with attitude control for vehicle. Finally, conclusions are made in Section 5.

2. Dynamic Modeling of UVMS

A dynamic model of the UVMS is used to estimate the vehicle and the manipualtor’s behavior in various situations. In this section, the 6-DOF vehicle and n-DOF manipulator are considered. The vehicle with eight thrusters is described based on an over-actuated unmanned underwater vehicle. Besides, end effector contacts with the environment, actuator saturation, and modeling of end effector and object are also carried out.

2.1. Assumptions

The dynamic model of the UVMS is highly coupled, strongly nonlinear, and shows kinematic redundancy. It needs many parameters. Therefore, the following assumptions are used to simplify the model.

(1)

The body of UVMS is fairly symmetrical about its three planes. The center of buoyancy of the vehicle is located on the geometric symmetry plane. The origin of the body-fixed frame is located at the center of buoyancy of the vehicle. The center of gravity (mass) of the vehicle is below its buoyancy center.

(2)

The center of buoyancy and gravity of each link of manipulator coincides and is located on its center respectively. It has neutral buoyancy. All the links of manipulator are hinge joint.

(3)

There are no ocean current and wave acting on the UVMS and object.

2.2. Kinematic of UVMS

According to the SNAME notation [79], the defined frames are as shown in Figure 2. I(x, y, z) and B(x_b, y_b, z_b) are earth-fixed (inertial) frame and vehicle body-fixed reference frame, respectively. E(x_i, y_i, z_i) is the manipulator-end effector frame and O(x_o, y_o, z_o) is the object reference frame.

First, the vehicle general coordinates are defined in the earth-fixed frame.

$${\eta }_v=\left[\begin{array}{c}{\eta }_1\\ {\eta }_2\end{array}\right]={\left[\begin{array}{cccccc}x& y& z& \varphi & \theta & \psi \end{array}\right]}^{\mathrm{T}},$$

(1)

where ${\eta }_1=[x y z{]}^{\mathrm{T}}$ represent motion displacement surge, sway and heave, respectively. ${\eta }_2=[\varphi \theta \psi {]}^{\mathrm{T}}$ represent Euler angles roll, pitch and yaw, respectively. Subscript $v$ represents the vehicle. The vector ${\dot{\eta }}_v$ is expressed in the earth-fixed frame, which is the time derivative of the position and attitude ${\eta }_v$.

Considering the UVMS equipped with an n-DOF manipulator, $q_m={[\begin{array}{ccc}{q}_1& \cdots & q_n\end{array}]}^{\mathrm{T}}\in {ℝ}^{n\times 1}$ is the vector represents joints position (angle) of the corresponding underwater manipulator links, where n is the number of joints. Subscript $m$ represents manipulator.

Then, define $\zeta ={[{\begin{array}{cc}{v}_v{}^{\mathrm{T}}& \dot{q}\end{array}}_m{}^{\mathrm{T}}]}^{\mathrm{T}}\in {ℝ}^{(6+n)\times 1}$ as the generalized velocity of the UVMS in the body-fixed frame. The linear and angular velocities of the origin of the body of UVMS in the body-fixed frame are define as

$$v_v=\left[\begin{array}{c}{v}_v\\ {\omega }_v\end{array}\right]\in {ℝ}^{6\times 1},$$

(2)

where $v_v\in {ℝ}^{3\times 1}$ and ${\omega }_v\in {ℝ}^{3\times 1}$ denote the linear and angular velocity of the vehicle in the body-fixed frame.

Therefore, the generalized velocity in the earth-fixed frame can be obtained as follows:

$$\left[\begin{array}{c}{\dot{\eta }}_v\\ {\dot{q}}_m\end{array}\right]=J({\eta }_v)\zeta ,$$

(3)

The matrix $J({\eta }_v)\in {ℝ}^{(6+n)\times (6+n)}$ is given by:

$$J({\eta }_v)=\left[\begin{array}{ccc}{R}_B^I({\eta }_2)& O_{3\times 3}& O_{3\times n}\\ O_{3\times 3}& J_o({\eta }_2)& O_{3\times n}\\ O_{n\times 3}& O_{n\times 3}& I_{n\times n}\end{array}\right],$$

(4)

in which

$$R_B^I({\eta }_2)=\left[\begin{array}{ccc}c\psi c\theta & c\psi s\varphi s\theta -s\psi c\varphi & s\psi s\varphi +c\psi s\theta c\varphi \\ s\psi c\theta & c\psi c\varphi +s\psi s\theta s\varphi & -c\psi s\varphi +s\psi s\theta c\varphi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]$$

and

$$J_o({\eta }_2)=\left[\begin{array}{ccc}1& s\varphi s\theta /c\theta & c\varphi s\theta /c\theta \\ 0& c\varphi & -s\varphi \\ 0& s\varphi /c\theta & c\varphi /c\theta \end{array}\right]$$

where $R_B^I\in {ℝ}^{3\times 3}$ is the linear velocity transformation matrix, $J_o\in {ℝ}^{3\times 3}$ is the angular velocity transformation matrix with $s•=\sin (•)$ and $c•=\cos (•)$. $I_{n\times n}\in {ℝ}^{n\times n}$ indicates the identity matrix and $O_{n_1\times n_2}\in {ℝ}^{n_1\times n_2}$ is the null matrix.

Let ${\eta }_{ee}={[\begin{array}{cc}{\eta }_{ee1}& {\eta }_{ee2}\end{array}]}^{\mathrm{T}}$ denote the position and attitude vector of the end effector in the earth-fixed frame. ${\eta }_{ee1}\in {ℝ}^{3\times 1}$ and ${\eta }_{ee2}\in {ℝ}^{3\times 1}$ are the vector represents position and attitude of the end effector, respectively. The relationship between end effector velocity ${\dot{\eta }}_{ee}$ (expressed in the earth-fixed frame) and generalized velocity $\zeta$ (the body-fixed system velocity expressed in the earth-fixed frame) can be obtained by:

$${\dot{\eta }}_{ee}=J_k\left(R_B^I,q_m\right)\zeta ,$$

(5)

where $J_k\left(R_B^I,q_m\right)\in {ℝ}^{6\times (6+n)}$ is the Jacobian matrix of the UVMS.

2.3. Dynamic Equation of UVMS

The dynamic equation of motion in the body-fixed frame of UVMS can be established as follows [7,80]:

$$M({\eta }_v,q_m)\dot{\zeta }+C({\eta }_v,q_m,\zeta )\zeta +D({\eta }_v,q_m,\zeta )\zeta +G({\eta }_v,q_m)={\tau }_e+{\tau }_c,$$

(6)

in which

$$\begin{array}{l}M({\eta }_v,q_m)=\left[\begin{array}{cc}{M}_v({\eta }_v)& 0_{6\times n}\\ 0_{n\times 6}& M_m(q_m)\end{array}\right],C({\eta }_v,q_m,\zeta )=\left[\begin{array}{cc}{C}_v({\eta }_v,v_v)& 0\\ 0& C_m(q_m,{\dot{q}}_m)\end{array}\right]\\ D({\eta }_v,q_m,\zeta )=\left[\begin{array}{cc}{D}_v({\eta }_v,v_v)& 0\\ 0& D_m(q_m,{\dot{q}}_m)\end{array}\right],G({\eta }_v,q_m)=\left[\begin{array}{c}{G}_v({\eta }_v)\\ G_m(q_m)\end{array}\right],{\tau }_e=\left[\begin{array}{c}{\tau }_{ev}+{\tau }_{mv}\\ {\tau }_{em}\end{array}\right],{\tau }_c=\left[\begin{array}{c}{\tau }_{cv}\\ {\tau }_{cm}\end{array}\right]\end{array}$$

where $M({\eta }_v,q_m)\in {ℝ}^{(6+n)\times (6+n)}$ is the mass matrix including added mass term, $C({\eta }_v,q_m,\zeta )\in {ℝ}^{(6+n)\times (6+n)}$ represents the Coriolis and centripetal effects including added mass terms, $D({\eta }_v,q_m,\zeta )\in {ℝ}^{(6+n)\times (6+n)}$ denotes friction of the links, the hydrodynamic force and damping matrix, $G({\eta }_v,q_m)\in {ℝ}^{(6+n)\times 1}$ is the restoring matrix. ${\tau }_e\in {ℝ}^{(6+n)\times 1}$ represents the external disturbances (such as end effector payloads, ocean currents, etc.,) on the UVMS, ${\tau }_c\in {ℝ}^{(6+n)\times 1}$ represents the controller input force/torques acting on the vehicle as well as joint. Comparing with single underwater robot and/or base-fixed manipulators, UVMS are more complex due to multi-body coupling and multi-body-fluid coupling effects.

2.4. Configuration of Thrusters

In this paper, the body of UVMS is equipped with eight thrusters as shown in Figure 3. Thus, the 6 DOF motions of vehicle can be controlled by the eight thrusters. Four horizontal thrusters T₁, T₂, T₃, and T₄, which are installed at the front, aft and two sides of the vehicle, are responsible for the motions in the horizontal plane. Meanwhile, four vertical thrusters T₅, T₆, T₇, and T₈ are responsible for the motions in the horizontal plane.

The relationship between the required force/moment acting on the vehicle in each DOF ${\tau }_{cv}$ and thruster propulsion force ${\tau }_u$ is

$${\tau }_{cv}=B_v{\tau }_u,$$

(7)

where $B_v\in {ℝ}^{6\times 8}$ is a thruster control matrix which is related to the thruster configuration. Desired control force/moment ${\tau }_{cv}$ and desired force of each thruster ${\tau }_u$ are defined as

$${\tau }_{cv}={\left[\begin{array}{cccccc}{F}_{vx}& F_{vy}& F_{vz}& M_{vx}& M_{vy}& M_{vz}\end{array}\right]}^{\mathrm{T}},$$

(8)

$${\tau }_u={\left[\begin{array}{cccccccc}{F}_1& F_2& F_3& F_4& F_5& F_6& F_7& F_8\end{array}\right]}^{\mathrm{T}},$$

(9)

Then, the thruster configuration control matrix $B_v$ can be obtained.

$$B_v=\left[\begin{array}{cccccccc}1& 0& 1& 0& 0& 0& 0& 0\\ 0& -1& 0& -1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 1& 1& 1\\ 0& 0& 0& 0& -l_v& -l_v& l_v& l_v\\ 0& 0& 0& 0& -d_v& d_v& d_v& -d_v\\ l_h& d_h& -l_h& -d_h& 0& 0& 0& 0\end{array}\right],$$

(10)

in which l_v, l_h, d_v, and d_v are the lengths of moment which provide the torque in roll, pitch, and yaw.

2.5. Contact with Environment

If the end effector contacts with the environment (e.g., ground, wall, object, etc.,), the force/moment at the tip of the manipulator acting on the whole UVMS can be expressed as

$${\tau }_e=J_k{}^{\mathrm{T}}\left(R_B^I,q_m\right)h_e,$$

(11)

where $J_k$ is the Jacobain matrix defined in Equation (5) and the vector $h_e\in {ℝ}^{6\times 1}$ is defined as

$$h_e=\left[\begin{array}{c}{f}_e\\ t_e\end{array}\right],$$

(12)

in which $f_e$ and $t_e$ are the vector of forces and moments at the tip of the manipulator. Therefore, $J_k$ is divided into two parts.

$$J_k\left(R_B^I,q_m\right)=\left[\begin{array}{c}{J}_{k,pos}\left(R_B^I,q_m\right)\\ J_{k,or}\left(R_B^I,q_m\right)\end{array}\right]\in {ℝ}^{6\times (6+3)},$$

(13)

We only consider the contact force at the end effector in this paper, and $J_{k,pos}\left(R_B^I,q_m\right)\in {ℝ}^{3\times (6+3)}$. Then combined with Equations (11)–(13) and rewritten contact force equation

$${\tau }_e=J_{k,pos}{}^{\mathrm{T}}\left(R_B^I,q_m\right)f_e,$$

(14)

in here, $f_e=K({\eta }_{ee1}-{\eta }_o)$, $K>0$ is the stillness matrix which has different value due to different environment and material, ${\eta }_{ee1}$ and ${\eta }_o$ are the position of the tip of end effector and the surface of the object.

2.6. Actuator Saturation

In fact, the actuator and motor control outputs are usually bounded by the thruster and motor physical limits. In here, to simplify this limits, desired control force/moment ${\tau }_c$ is bounded.

$${\tau }_{ci}=\left\{\begin{array}{c}\begin{array}{cc}{\tau }_{ci}& \left|{\tau }_{ci}\right|\le {\tau }_{ci,\max }\end{array}\\ \begin{array}{cc}{\tau }_{ci,\max }& \left|{\tau }_{ci}\right|>{\tau }_{ci,\max }\end{array}\end{array}\right.\begin{array}{cc},& i=1,\dots ,(6+n)\end{array},$$

(15)

where ${\tau }_{ci,\max }$ is the maximum control output limit for each DOF of UVMS. Desired input and actual output are shown in Figure 4.

Because of the thruster and motor’s physical limits and constraints, the position of the manipulator, the velocity and acceleration of the UVMS are also bounded by the mechanical and physical limits.

$$\left\{\begin{array}{ll}\left|{\eta }_{vi}\right|\le {\eta }_{vi,\max },\hfill & i=1,\cdots ,6\hfill \\ \left|q_i\right|\le q_{i,\max },\hfill & i=1,\cdots ,n\hfill \\ \left|{\zeta }_i\right|\le {\zeta }_{i,\max },\hfill & i=1,\dots ,(6+n)\hfill \\ \left|{\dot{\zeta }}_i\right|\le {\dot{\zeta }}_{i,\max }\hfill & i=1,\dots ,(6+n)\hfill \end{array}\right.,$$

(16)

where ${\eta }_{vi,\max }$, $q_{i,\max }$, ${\zeta }_{i,\max }$ and ${\dot{\zeta }}_{i,\max }$ are the maximum allowed movement limit for each DOF of UVMS.

2.7. Modeling of End Effector Carrying an Object

When the end effector of UVMS carries an object, its dynamics are affected (see Figure 5). Hence, the model focuses on the scenario when the end effector has already grasped the body of the object. Therefore, the object is assumed to be fixed in the end effector so that its position and attitude with respect of the end effector or end-link of the manipulator will not change. As a result, the end effector and the object can be modeled as a new rigid body, whose motions can be described by the same equations. However, the dynamic properties of end effector are changed in the process of grasping.

Before end effector grasps the object, the total mass of end effector or end-link of the manipulator is m_e, its volume is V_e, the centers of buoyancy and gravity in the end effector body-fixed frame are ${}^{b}l_e^g$ and ${}^{b}l_e^b$, respectively. Then, the variables relating to the end effector are labeled as e, those relating to the object are labeled as o. Those corresponding to the combined body between end effector and object as &. $B=\rho gV$ and $G=mg$ are buoyancy and gravity of rigid body, respectively. $\rho$ is seawater density and $g$ gravitational acceleration. Hence the volume, mass, and position of the centers of buoyancy and gravity of the total end effector and object can be computed as follows:

$$V_{\&}=V_e+V_o,$$

(17)

$$m_{\&}=m_e+m_o,$$

(18)

$${}^{b}l_{\&}^b=\frac{V_e{}^{b}l_e^b+V_o{}^{b}l_o^b}{V_{\&}},$$

(19)

$${}^{b}l_{\&}^g=\frac{m_e{}^{b}l_e^g+m_o{}^{b}l_o^g}{m_{\&}},$$

(20)

By using the parallel axis theorem, the inertia matrix of changed end effector referenced to its body-fixed frame can be obtained as:

$$\begin{array}{l}{}^{b}I_{\&}{}_{,}{}_e={}^{b}I_e-m_e{S}^2({}^{b}l_{\&}^g-{}^{b}l_e^g)\\ {}^{b}I_{\&}{}_{,}{}_o={}^{b}I_o-m_o{S}^2({}^{b}l_{\&}^g-{}^{b}l_o^g),\\ {}^{b}I_{\&}={}^{b}I_{\&}{}_{,}{}_e+{}^{b}I_{\&}{}_{,}{}_o\end{array}$$

(21)

where ${}^{b}I_e$ and ${}^{b}I_o$ are the inertia matrix of each body referenced to their center of gravity, and $S(•)\in {ℝ}^{3\times 3}$ is the skew symmetric matrix which is defined as follows:

$$S(x)=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right],$$

(22)

3. Proportional-Integral-Derivative (PID) Control and Parameters

In this section, PID control scheme for UVMS system is briefly introduced. Meanwhile, for the next step of the simulation, the main parameters of the model of UVMS and controller are provided.

3.1. Proportional-Integral-Derivative (PID) Control

In the process of UVMS grasping an object, the desired trajectory of end effector is planned in the inertial frame. The desired position ${\eta }_{eed}$, desired velocity ${\dot{\eta }}_{eed}$, and desired acceleration ${\ddot{\eta }}_{eed}$ of end effector are obtained by the quintic polynomial to generate fifth-order trajectories in task space. According to Equations (3) and (5), the desired position, velocity, and acceleration of the vehicle and manipulator joints in the body-fixed frame and earth-fixed frame can be obtained, respectively. The vector of position and velocity error of trajectory tracking can be expressed as:

$$\left[\begin{array}{c}{\tilde{\eta }}_v\\ {\tilde{q}}_m\end{array}\right]=\left[\begin{array}{c}{\eta }_{vd}-{\eta }_v\\ q_{md}-q_m\end{array}\right],$$

(23)

$$\left[\begin{array}{c}{\dot{\tilde{\eta }}}_v\\ {\dot{\tilde{q}}}_m\end{array}\right]=\left[\begin{array}{c}{\dot{\eta }}_{vd}-{\dot{\eta }}_v\\ {\dot{q}}_{md}-{\dot{q}}_m\end{array}\right],$$

(24)

The desired control force/torque acting on vehicle and manipulator joints in the inertial frame can be obtained as:

$$\left[\begin{array}{c}{\tau }_v\\ {\tau }_m\end{array}\right]=K_P\left[\begin{array}{c}{\tilde{\eta }}_v\\ {\tilde{q}}_m\end{array}\right]+K_I{\displaystyle {\mathrm{\int }}_0^t\left[\begin{array}{c}{\tilde{\eta }}_v(t’)\\ {\tilde{q}}_m(t’)\end{array}\right]}dt’+K_D\left[\begin{array}{c}{\dot{\tilde{\eta }}}_v\\ {\dot{\tilde{q}}}_m\end{array}\right],$$

(25)

where $K_P$, $K_I$, and $K_D$ are gain matrices and they are diagonal. ${\tau }_v$ and ${\tau }_m$ are desired control force/torque in inertial frame. Then the control scheme is converted to the body-fixed frame as

$${\tau }_c=\left[\begin{array}{c}{\tau }_{cv}\\ {\tau }_{cm}\end{array}\right]=J^{\mathrm{T}}({\eta }_v)\left[\begin{array}{c}{\tau }_v\\ {\tau }_m\end{array}\right],$$

(26)

where ${\tau }_c$ is defined in Equation (6) and its constraints are introduced in Equation (15). The whole control schematic diagram is shown in Figure 6.

3.2. Model Parameters

The parameters of UVMS for simulation are also needed. The data of the vehicle are referred to [81], and some main physical parameters are listed in

Table 1. Physical parameters of the vehicle.

Parameters		Values	Parameters		Values
Length [m]		5.3	Mass [kg]		5454.54
Buoyancy [N]		53,400	Gravity [N]		53,400
Center of buoyancy [m]		[0,0,0]	Center of mass [m]		[0,0,0.061]
Maximum translational velocity [m/s]		[0.2,0.2,0.2]	Maximum rotational velocity [rad/s]		[0.2,0.2,0.2]
Maximum control force [N]		[200,200,400]	Maximum control moment [Nm]		[200,200,200]
Moment of inertia [kg•m2]	Ixx	2038	Moment of inertia [kg•m2]	Ixy	−13.58
	Iyy	13,587		Iyz	−13.58
	Izz	13,587		Ixz	−13.58

.

Because the body of UVMS has three planes of symmetry and the velocity is low, the inertia matrix, including the added mass terms, is given by:

$$M_v=\mathrm{diag}\left\{\begin{array}{cccccc}6.019\times {10}^3,& 9.551\times {10}^3,& 2.332\times {10}^4,& 4.129\times {10}^3,& 4.913\times {10}^4,& 2.069\times {10}^4\end{array}\right\},$$

The hydrodynamic derivatives of the vehicle are given by:

$$\begin{array}{c}\begin{array}{ccc}{X}_{p\left|p\right|}=2.8\times {10}^3& X_{q\left|q\right|}=-5.9\times {10}^3& X_{r\left|r\right|}=1.6\times {10}^3\end{array}\hfill \\ \begin{array}{ccc}{X}_{pr}=2.9\times {10}^2& X_{\dot{u}}=-5.6\times {10}^2& X_{\omega q}=-1.5\times {10}^4\end{array}\hfill \\ \begin{array}{cccc}{X}_{vp}=-2.2\times {10}^2& X_{vr}=1.5\times {10}^3& X_{v\left|v\right|}=7.4\times {10}^2& X_{\omega \left|\omega \right|}=2.4\times {10}^3\end{array}\hfill \\ \begin{array}{ccc}{Y}_{\dot{p}}=4.7\times {10}^1& Y_{\dot{r}}=4.7\times {10}^2& Y_{pq}=1.6\times {10}^3\end{array}\hfill \\ \begin{array}{ccc}{Y}_{pr}=-2.6\times {10}^3& Y_{\dot{v}}=-4.1\times {10}^3& Y_{up}=2.2\times {10}^2\end{array}\hfill \\ \begin{array}{cccc}{Y}_r=2.2\times {10}^3& Y_{vq}=1.8\times {10}^3& X_{\omega p}=1.7\times {10}^4& \end{array}\hfill \\ \begin{array}{ccc}{Y}_{\omega r}=-1.4\times {10}^3& Y_v=-1.4\times {10}^3& Y_{v\omega }=9.5\times {10}^2\end{array}\hfill \\ \begin{array}{ccc}{Z}_{\dot{q}}=-2.7\times {10}^3& Z_{p\left|p\right|}=5.1\times {10}^1& Z_{pr}=2.6\times {10}^3\end{array}\hfill \\ \begin{array}{ccc}{Z}_{r\left|r\right|}=-2.9\times {10}^2& Z_{\dot{\omega }}=-1.8\times {10}^4& Z_{uq}=-1.0\times {10}^4\end{array}\hfill \\ \begin{array}{cccc}{Z}_{vp}=-3.6\times {10}^3& Z_{vr}=3.3\times {10}^3& Z_{u\omega }=-4.2\times {10}^3& Z_{v\left|v\right|}=-9.5\times {10}^2\end{array}\hfill \\ \begin{array}{ccc}{K}_{\dot{p}}=-2.0\times {10}^3& K_{\dot{r}}=7.1\times {10}^1& K_{pq}=-1.4\times {10}^2\end{array}\hfill \\ \begin{array}{ccc}{K}_{pr}=3.5\times {10}^4& K_{\dot{v}}=4.7\times {10}^1& K_{up}=-4.3\times {10}^3\end{array}\hfill \\ \begin{array}{cccc}{K}_{ur}=-3.3\times {10}^2& K_{vq}=-2.0\times {10}^3& K_{\omega p}=-5.1\times {10}^1& \end{array}\hfill \\ \begin{array}{ccc}{K}_{\omega r}=5.5\times {10}^3& K_{uv}=2.3\times {10}^2& K_{v\omega }=-1.4\times {10}^4\end{array}\hfill \\ \begin{array}{ccc}{M}_{\dot{q}}=-3.5\times {10}^4& M_{p\left|p\right|}=1.1\times {10}^2& M_{pr}=1.0\times {10}^4\end{array}\hfill \\ \begin{array}{ccc}{M}_{r\left|r\right|}=6.0\times {10}^3& M_{\dot{\omega }}=-2.7\times {10}^3& M_{uq}=-2.7\times {10}^4\end{array}\hfill \\ \begin{array}{cccc}{M}_{vp}=4.7\times {10}^2& M_{vr}=6.7\times {10}^3& M_{u\omega }=7.4\times {10}^3& M_{v\left|v\right|}=-1.9\times {10}^3\end{array}\hfill \\ \begin{array}{ccc}{N}_{\dot{p}}=-7.1\times {10}^1& N_{\dot{r}}=-7.1\times {10}^3& N_{pq}=-4.4\times {10}^4,\end{array}\hfill \\ \begin{array}{ccc}{N}_{pr}=5.6\times {10}^3& N_{\dot{v}}=4.7\times {10}^2& N_{up}=-3.3\times {10}^2\end{array}\hfill \\ \begin{array}{cccc}{N}_{ur}=-6.3\times {10}^3& N_{vq}=-3.9\times {10}^3& N_{\omega p}=-6.7\times {10}^3& \end{array}\hfill \\ \begin{array}{ccc}{N}_{\omega r}=2.9\times {10}^3& N_{uv}=-5.5\times {10}^2& N_{v\omega }=-2.0\times {10}^3\end{array}\hfill \\ \end{array}$$

The manipulator is supposed to be mounted under the vehicle in the center of its length, the position of the base of manipulator is [0, 0, 0.5]^T m in the vehicle body fixed frame. All the links are modeled as cylinders, the centers of mass and buoyancy of each links are [0, 0, 0.5]^T m in their link frame. The volumes of each links are computed as ${\delta }_i=\pi L_i{r}_i^2$, where $L_i$ and $r_i$ are the link lengths and radius, respectively. For each of the cylinder the mass is computed as:

$$M_i=\mathrm{diag}\left\{\begin{array}{cccccc}{m}_i+\rho {\delta }_i,& m_i+\rho {\delta }_i,& m_i+0.1m_i,& I_{xx,i}+\rho L_i^2{\delta }_i/12,& I_{yy,i}+\rho L_i^2{\delta }_i/12,& I_{zz,i}\end{array}\right\},$$

in which the main parameters of manipulator are reported in

Table 2. Main parameters of manipulator with 3-DOF links.

Parameters		Link 1	Link 2	Link 3
Mass [kg]		31.4	20.1	15.4
qm [rad]		q1	q2	q3
Radius [m]		0.1	0.08	0.07
Length [m]		1	1	1
Joint angular velocity [deg/s]		20	20	20
Maximum joint torque [Nm]		300	200	150
Viscous friction [Nms]		30	20	5
Moment of inertia [kg•m2]	Ixx	1.65	0.75	0.4
	Iyy	11	6.3	4
	Izz	11	6.3	4

. The cylinder length is modeled along the $z_i$ axis. The dry friction has not been considered to avoid chattering behavior. The linear skin and quadratic drag coefficients are given by $D_s=0.4$ and $C_d=0.6$, respectively.

Assuming that the object modeled is considered as a sphere, and its parameters are reported in

Table 3. Main parameters of the object.

Parameters		Values	Parameters		Values
Sphere radius [m]		0.1	Mass [kg]		9.21
Buoyancy [N]		50.96	Gravity [N]		90.26
Moment of inertia [kg•m2]	Ixx	0.037	Moment of inertia [kg•m2]	Ixy	0
	Iyy	0.037		Iyz	0
	Izz	0.037		Ixz	0

. It is obvious that the weight of the object is greater than its buoyancy.

3.3. Controller Parameters

Assume that the body of UVMS mounts thrusters can control vehicle translation and rotation (see Figure 2 and Figure 3). For PID control, high gains can improve the controller response due to small trajectory tracking error. However, it will have different gains in different system. In this paper, the controller parameters that we choose for this UVMS simulation system are as follows:

$$\begin{array}{l}{K}_P=\mathrm{diag}\{\begin{array}{c}4000,\end{array}\begin{array}{c}\end{array}4000,\begin{array}{c}\end{array}6000,\begin{array}{c}\end{array}50,000,\begin{array}{c}\end{array}50,000,\begin{array}{c}\end{array}50,000,\begin{array}{c}\end{array}4000,\begin{array}{c}\end{array}1000,\begin{array}{c}\end{array}500\}\\ K_I=\mathrm{diag}\{0.2,\begin{array}{c}\end{array}0.2,\begin{array}{c}\end{array}0.5,\begin{array}{c}\end{array}5,\begin{array}{c}\end{array}3.55,\begin{array}{c}\end{array}3,\begin{array}{c}\end{array}2,\begin{array}{c}\end{array}1,\begin{array}{c}\end{array}0.5\},\\ K_D=\mathrm{diag}\{\begin{array}{c}8000,\end{array}\begin{array}{c}\end{array}8000,\begin{array}{c}\end{array}20,000,\begin{array}{c}\end{array}50,000,\begin{array}{c}\end{array}65,000,\begin{array}{c}\end{array}50,000,\begin{array}{c}\end{array}2000,\begin{array}{c}\end{array}500,\begin{array}{c}\end{array}200\}\end{array}$$

(27)

4. Simulation and Discussion

The ocean environment (wind, waves, and currents) and disturbance force which affect the UVMS dynamics are neglected in this paper. Although the UVMS model is built in the three dimensional space, to simplify the problem, the motion of the UVMS and object are simulated only in the vertical plane. Therefore, the sway, roll, and yaw motions of the vehicle are always zero. In order to indicate that the motion of the manipulator has an impact on the vehicle, before two cases vehicle control (with/without attitude control), all thrusters do not work. Then with and without attitude control of vehicle is considered and performed. UVMS is under-actuated system when the vehicle is without attitude control, however, UVMS is fully actuated system when the vehicle is with attitude control.

4.1. Simulations

The UVMS trajectory is planned with moving the end effector along a straight line in the absence of currents. Comparison simulation task is to pick up the object at start position [0.5, 0, 2.5]^T m by the UVMS with the initial position and attitude of vehicle [0, 0, 0, 0, 0, 0]^T m or deg and the initial configuration of manipulator [−60, 90, 90]^T deg. The goal position of the object is at [0, 0, 1.5]^T m. The whole process of manipulator for grasping and carrying the object is shown in Figure 7, and the time process is listed in

Table 4. Time process of grasping operation.

Number	Time [s]	Action of Manipulator
①	0~25	From initial position to start position
②	25~40	From start position to goal position
③	40~55	From goal position to start position
-	55~60	Grasping operation
④	60~75	From start position to goal position
⑤	75~90	From goal position to start position

.

The desired motion of the vehicle is to keep stationary and only rotate manipulator joint. The desired position and attitude of vehicle ${\eta }_{vd}$, the desired velocity of vehicle ${\dot{\eta }}_{vd}$, the desired angle of manipulator joints $q_{md}$, and the desired angular velocity of manipulator joints ${\dot{q}}_{md}$ are planned by trajectory planner using the quintic polynomial. The processes of grasping are simulated using MATLAB software. The total simulation time is 90 s and time step is 0.01 s.

The condition that the vehicle is floating and the thrusters cannot work to keep the vehicle position stationary while operation of manipulator is considered first. Through numerical simulation, the actual position of the vehicle is shown in Figure 8. During the motion of the manipulator, the vehicle moves due to the coupling effect between the manipulator and the vehicle. This results in the actual distance between the tip of end effector and the position of start object position to exceed 10 cm when picking up the object at t = 55 s (Figure 9). Hence, the grasping task cannot be completed without vehicle control.

4.1.1. Case-I: Without Attitude Control for Vehicle

The roll and pitch of the body of UVMS are hydrostatically stable because the center of gravity of the vehicle is below the center of its buoyancy. When only the position of the vehicle is controlled, the whole UVMS system is under-actuated. In this situation, the desired control torque for control the attitude of the vehicle is set as zero. The desired and actual position of end effector responses are shown in Figure 10, and the position trajectory tracking error of end effector is shown in Figure 11.

From Figure 10 and Figure 11, when the manipulator is picking up the object (at t = 55 s), the distance between the tip of manipulator and the object is 4.39 cm. Although the vehicle attitude is not controlled, the distance error of the end effector can be acceptable. Hence, in this case the grasping task can be completed.

4.1.2. Case-II: With Attitude Control for Vehicle

For a fully actuated UVMS, the position and attitude of vehicle are controlled. Under this situation, the process of grasping operation is simulated. The desired and actual position of end effector responses are shown in Figure 12, and the position trajectory tracking error of end effector is shown in Figure 13.

From Figure 12 and Figure 13, when the manipulator picking up the object (at t = 55 s), the distance between the tip of manipulator and the object is 1.02 cm. Under full control of the vehicle, the position and attitude of vehicle vary over a small range. Hence, the grasping task in this situation can also be completed.

4.2. Results and Discussions

Without the control for vehicle, the operation of manipulator cannot be completed for the coupling effect between the vehicle and the manipulator. Hence comparative analyses of Case-I and Case-II are carried out. Due to the manipulator only moves in the vertical plane, the vehicle position $y$ and vehicle Euler angle $\varphi$ and $\psi$ are always zero. So, the trajectory of UVMS can be shown in Figure 14. The results show that PID control scheme is able to suit system dynamics changing and control UVMS carrying an object successfully.

The position of vehicle $x$ and $z$, the Euler (pitch) angle of vehicle $\theta$, and manipulator joints $q$ responses are shown in Figure 15. Their tracking errors are shown in Figure 16.

From Figure 15 and Figure 16, the pitch angle of the vehicle with fully controlled is less than only control for position. However, hydrostatically stable makes the vehicle pitch angle vary within a certain range below 2 deg (see Figure 15c). The trajectories tracking errors are close in two cases except the attitude error of the vehicle.

Besides, the forces and moments of the vehicle and joint torques of the manipulator are shown in Figure 17 and Figure 18, respectively. We can find that none of the control forces and moments exceed the limit in Table 1 and Table 2. Similarly, the forces acting on the vehicle and joint torques of the manipulator show a small difference in the two cases. Because the attitude of the vehicle is not controlled in Case-I, the moment acting on the vehicle is always zero.

According to the root mean square error (RMSE) between actual and desired trajectories over the whole simulation duration for the two types of analyzed situation, the values of RMSE of the position and attitude of the vehicle (V-X, V-Z, V-$\theta$), the manipulator joint angles (q₁, q₂, q₃), and the position error of end effector (EE-X, EE-Z) are shown in Figure 19.

Due to vehicle attitude control, the values of RMSE of vehicle attitude are different in two cases. It is obvious that with vehicle attitude control can reduce attitude trajectory tracking error effectively. However, there is an interesting thing that the values of RMSE of end effector with vehicle attitude control are greater than without attitude control. This phenomenon shows that it may be because of the fixed PID parameters and the hydrostatic restoring force/moment of the vehicle. On the one hand, the system parameters of UVMS are changed, but PID parameters are fixed. On the other hand, the input attitude control force/moment and hydrostatic restoring force/moment affect each other, and this effect deteriorates the positioning accuracy of the end effector.

Therefore, different control schemes and different grasping situations should be considered and analyzed in the future. In this paper, the vehicle and each joint of manipulator are assumed to be neutral buoyancy, so, it is hard to achieve in reality. Hence, the motion planning of the vehicle and the manipulator should be carried out when dynamic grasping, for example minimizing restoring moments method [26] and zero moment point method [27]. Further experiments should be performed in future to verify the correctness of the theoretical and simulation results.

5. Conclusions

Considering the dynamic coupling of the manipulator and the vehicle, interaction between the manipulator and the object, actuator saturation, and so on, dynamic simulation of the process of UVMS grasping an object is carried out and two case studies that without/with vehicle attitude control are analyzed and compared in this article. Some conclusions are summarized as follows.

(1)

The position and attitude of the vehicle cannot remain stationary due to the coupling effect between the manipulator and the vehicle. It deteriorates the positioning accuracy of the end effector of the manipulator and is harmful to underwater precision operations or other task.

(2)

Trajectory tracking errors and forces/moments acting on the UVMS under fully controlled are performed in comparison with vehicle position control. Vehicle fully controlled can reduce attitude trajectory tracking error effectively.

(3)

The hydrostatic restoring force/moment is helpful for the stability of the UVMS system. However, the combined effect between hydrostatic restoring force/moment and system control force will affect the precise positioning of the end effector.

In the future, different control schemes and different grasping situations should be considered and analyzed. The motion planning of the vehicle and the manipulator should be carried out in the process of grasping. experiments should be performed to verify the correctness of the theoretical and simulation results.