Cooperative Control of Underwater Vehicle–Manipulator Systems Based on the SDC Method

Abstract

The paper considers the problem of cooperative control synthesis for a complex of $N$ underwater vehicle–manipulator systems (UVMS) to perform the work of moving a cargo along a given trajectory. Here, we used the approach based on the representation of nonlinear dynamics models in the form of state space with state-dependent coefficients (SDC-form). That allowed us to apply methods of suboptimal control with feedback based on the state-dependent differential Riccati equation (SDDRE) solution at a finite time interval, providing the change in control intensity with the transient effect of the system matrices in SDC form. The paper reveals two approaches to system implementation: a general controller for the whole system and a set of $N$ independent subcontrollers for UVMSs. The results of both approaches are similar; however, for the systems with a small number of manipulators, the common structure is recommended, and for the systems with a large number of manipulators, the approach with independent subcontrollers may be more acceptable. The proposed method of cooperative control was tested on the task of cooperative control for two UVMSs with six-link manipulators Orion 7R. The simulation results are presented in the article and show the effectiveness of the proposed method.

1. Introduction

The cooperation of robotic systems has many applications in the industry that require coordinated control, such as repair and welding work, space transportation of objects, and others [1,2]. Most of them do not exclude the possibility of operating in an underwater environment, and this fact significantly complicates the implementation of control methods for each case.

Underwater vehicles are divided into two common type classes [1,2,3,4,5]: remotely operated underwater vehicles (ROVs) and unmanned underwater vehicles (UUVs) [6,7,8,9,10]. The first type of UV requires operator control from an on-surface sea vessel. The second type of UV is equipped with all necessary hardware and can move autonomously.

Autonomous underwater vehicles (AUVs) have been widely used in recent years as an alternative to extremely costly, time-consuming, and risky human underwater operations. Most of the tasks performed by these vehicles are for data-gathering applications. However, there is an increasing interest for their use in inspection, maintenance, and repair operations that require manipulation and interaction with objects in the underwater environment. Nowadays, this is mainly performed by remotely operated vehicles (ROVs) [11] and one of the strongest research trends regards the use of Underwater Vehicle–Manipulator Systems (UVMS) [12,13]. Underwater vehicle–manipulator systems (UVMS) can fulfill underwater sampling, grabbing, operation, and other tasks in addition to observation. It is widely used in marine scientific investigations, marine engineering, and other fields [14].

Most underwater manipulator tasks can be performed more efficiently when multiple manipulators are used cooperatively [15]. On the other hand, the tasks of coordinated control of several underwater robots with manipulators are quite complex, with the main difficulties in implementation related to the communication limitations of the aquatic environment. The use of a single base robot carrier with multiple manipulators overcomes such limitations. This research focuses on the development of coordinated control algorithms for underwater robot manipulators that hold an object and move it along a specified trajectory.

It is important to consider the following question: why is using a robot with several manipulators with complex dynamics and limitations more effective than using a robot with one manipulator of higher power and carrying capacity?

No additional rationale is needed to move objects with large geometries relative to the size of the robot, as the interaction of multiple arms increases motion stability and payload capacity. The problem is to justify the use of two or more manipulators to move a concentrated mass (a load of insignificant size relative to the robot and manipulators). Some studies have shown that the use of robots with coordinated (cooperative) control increases the carrying capacity even for concentrated mass loads [15,16,17,18]. Therefore, the present study considers the problem of a coordinated operation of several underwater robotic arms when transporting an object with a concentrated mass in a limited working space. The control scheme implies different ways to distribute the load between the manipulators.

Because the underwater manipulator is a complex system, highly non-linear, and vulnerable to perturbations caused by underwater influences, reliable control strategies must be developed for the manipulator to obtain a reliable system.

There are several methods to synthesize control for a complex of robots; for example, for the sliding mode, [19,20] suggests using a control and position controller, and [21] proposes an additional verification of the results by Lyapunov functions. The PID control law is considered in [22]. However, these methods are closer to the classical techniques and are not always effective in these circumstances because the quality of manipulator control is easily affected in the underwater working environment. In [23], an intelligent controller based on a fuzzy neural network RBF (radial basis function) method is proposed for accurate and stable control of an underwater robot. Similar methods using neural networks and fuzzy logic are presented in [24,25,26]. For underwater robots, the technique of control synthesis using a nonlinear behavior prediction model is also acceptable [27].

As the basic method of synthesizing controls in this study, we propose to use the representation of nonlinear models of the manipulators’ dynamics in the linear form of the state-space with state-dependent parameters (state-dependent coefficients techniques). That makes it possible to build simple (close to linear) trajectory controllers for manipulator grasping modules in the form of feedback using the so-called state-dependent differential Riccati equation (SDDRE) method.

Finally, it should be noted that the proposed control scheme can be implemented as a centralized control algorithm for the whole system, as well as in a decentralized form, because the algorithm involves the possibility of calculating control signals for each manipulator separately without exchanging data with each other based on information obtained exclusively from their sensors (for example, position and speed measurements). This significantly increases the reliability of the coordinated control system.

The rest of the paper is organized as follows. Section 2 discusses the features of the proposed methods to solve the problem statement, while Section 3 presents applications of the above methods, as well as a test case. In Section 4, the results and analysis are presented. The discussion is presented in Section 5. Finally, the conclusions are given in Section 6.

2. Materials and Methods

In the present paper, a cooperative control approach based on the solution of the state-dependent differential Riccati equation is proposed. It should be noted that it is not the only way to obtain control for a cooperative system, but it is optimal, which highlights its advantages among fuzzy logic methods or classical techniques [1,15,28].

Furthermore, the paper applies the well-known algorithm for solving the inverse kinematics problem of robot manipulators, which is based on the pseudo-inversion of the geometric Jacobian [29].

One of the main aspects of solving the problem of mathematical modeling and, in particular, the verification of control laws is the synthesis of the system dynamics model.

The majority of the known scientific research in the field of underwater vehicle modeling is based on the publications of T. Fossen [30]. They include models in 3, 4, and 6 degrees of freedom (DoF), description of external disturbances, control systems, and general calculation procedures. The mathematical model is a fundamental element in path planning, control, and navigation; therefore, this problem is extremely important and complicated in solving the problems identified in the article.

Besides identifying dynamic parameters in control synthesis, it is necessary to solve the problem of inverse dynamics, which allows obtaining the desired forces from the side of manipulating robots. The paper considers the approach of equal load distribution between manipulators, which is described in detail in [31,32].

A substantial contribution to the paper was made by [33], which describes in detail the dynamics and kinematics of the UVMS, the refinements of their design, as well as the specifics of the hydrodynamic parameters’ calculation technique. It is necessary to note that the well-known parametrization approach, presented in [15,34] and which is applicable to the task set in the paper, was used for the composition of the dynamic parameters of the system.

Collectively, all the methods described above form the base for the development of a tracking control system for the cooperative complex of underwater manipulation robots during the transfer of an object with a concentrated mass, which is one of the essential goals of this work.

3. Theoretical Part

3.1. General Problem Statement

The proposed approach is based on the development of a nonlinear tracking optimal control scheme for the underwater robot–manipulator system UVMS and the object. We assume that all UVMS are informed about both the desired configuration of the object and the position of obstacles in the workspace. Therefore, the control task is to route the entire formation to the target configuration avoiding collisions with static obstacles in the workspace.

Let us consider $N$ underwater robots with manipulators that have $m$ degrees of freedom and rigidly capture an object of mass $m_O$ in a limited workspace. Figure 1 shows an example of a cooperative system of two robots with manipulators. Assume that each manipulator is fully driven concerning the coordinate system associated with the working tool (gripper) and the manipulators are equipped with appropriate sensors to measure the position and speed of the manipulator links. In addition, the geometric parameters of both robots with manipulators and the object of manipulation are known.

Motion, trajectory, and coordinates of the manipulator joints are measured relative to the main reference frame $\left\{X_0,Y_0,Z_0\right\}$. The center of mass (CoM) coordinates of the manipulated object is denoted as $x_O,y_O,z_O$, the center coordinates of the manipulator working tools (grippers)—as $x_i,y_i,z_i$ when $i=1,2,\dots ,N$. The load is considered as a concentrated mass; therefore, the trajectories of the load and the working bodies are similar, i.e., $\left(x_O,y_O,z_O\right)=\left(x_i,y_i,z_i\right)$. In the case of a load with a concentrated mass, therefore, the knowledge of the gripper orientation is not required. As a result, the minimum degree of freedom of the manipulators for designing the trajectory of planar motion is two, and for spatial motion it is three.

The task is to design distributed control protocols for each of the $N$ manipulators to safely transport the object along the desired trajectory, considering the desired configuration for the manipulated object movement.

3.2. Mathematical Model of the System

3.2.1. The Dynamics Model of the Manipulated Object

Without loss of generality, consider the following model of dynamics for an underwater object, which can be derived on the basis of the Newton–Euler formulations [31,33]:

$$\begin{gathered} or{\dot{p}}_O=J_O{({\eta }_{2,O})}^{-1}{\upsilon }_O, \\ {M}_O\left(p_O\right){\dot{\upsilon }}_O+C_O\left({\upsilon }_O,p_O\right){\upsilon }_O+D_O\left({\upsilon }_O,p_O\right){\upsilon }_O+g_O=-f_e, \end{gathered}$$

(1)

where $p_O=\left[{\eta }_{1,O}^T,{\eta }_{2,O}^T\right]$, ${\eta }_{1,O}^T={[x_O,y_O,z_O]}^T$ and ${\eta }_{2,O}^T={[{\phi }_O,{\theta }_O,{\psi }_O]}^T$, $M_O$—is a positively defined inertia matrix of the object, $C_O$—is the Coriolis matrix, and $D_O$ models dissipative effects. The gravity and buoyancy vector is denoted as $g_O$. The matrix $J_O\left({\eta }_{2,O}\right)$—is the Jacobian of the object, which converts the derivative of the Euler angles to the angular velocity ${\omega }_O$ (angular velocity in the system coupled to the object); it can be defined as:

$$\begin{gathered} J_O\left({\eta }_{2,O}\right)=\left(\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& J_O^I\left({\eta }_{2,O}\right)\end{array}\right), \\ {J}_O^I\left({\eta }_{2,O}\right)=\left(\begin{array}{ccc}1& 0& -sin({\phi }_O)\\ 0& cos({\phi }_O)& cos({\theta }_O)sin({\phi }_O)\\ 0& -sin({\phi }_O)& cos({\theta }_O)cos({\phi }_O)\end{array}\right). \end{gathered}$$

(2)

The external force $f_e\in {ℝ}^6$, acting on the object must be distributed among the manipulators $f_e={\displaystyle \sum }_{i=1}^N{f}_{e,i}$, in order to isolate the effect $f_{e,i}$ from each manipulator.

3.2.2. Kinematic Model of a Robot with Manipulators

We define the coordinates of the working organ of each manipulator as $p_i=\left[{\eta }_{1,p_i}^T,{\eta }_{2,p_i}^T\right]$, where ${\eta }_{1,p_i}^T={[x_{p_i},y_{p_i},z_{p_i}]}^T$ and ${\eta }_{2,p_i}^T={[{\phi }_{p_i},{\theta }_{p_i},{\psi }_{p_i}]}^T$ denote the position and orientation expressed in Euler angles relative to the inertial reference system $\left\{X_0,Y_0,Z_0\right\}$. Let us also determine the generalized velocities of the manipulator’s working tool through ${\upsilon }_i={[{\dot{\eta }}_{1,i}^T,{\omega }_i^T]}^T$, where ${\dot{\eta }}_{1,i}^T$—is the linear velocity, ${\omega }_i$—is the angular velocity. Suppose $q_i\in {ℝ}^m$ describes the position and orientation of the carrier robot, $q_i\in {ℝ}^m$ is a vector of generalized coordinates of manipulator joints positions.

The position and orientation of the manipulator grip are given by the direct kinematics as follows:

$$p_i=F_i\left(q_i\right), i=1,2\dots ,N.$$

(3)

For the differential kinematics of each manipulator, we have

$$\begin{gathered} {\upsilon }_i=J_i\left(q_i\right){\dot{q}}_i,i=1,2\dots ,N, \\ {\dot{q}}_i={[{\dot{q}}_{Vi}^T,{\dot{q}}_{Mi}^T]}^T, \end{gathered}$$

(4)

where ${\dot{q}}_i$ is the velocity vector of the manipulator joint coordinates and $J_i\left(q_i\right)$—is the geometric Jacobian [28,31]. Note that the matrix $J_i\left(q_i\right)$ becomes singular at the points of kinematic specifics (singularities) defined by the set

$$S_i=\left\{q_i\in R^{n_i}:det(J_i\left(q_i\right)J_i\left(q_i{)}^T\right)=0\right\}, i=1,2\dots ,N.$$

(5)

The task of identifying the geometric Jacobian can be varied, but the well-known approach is based on vector multiplication and the application of transformation matrices [35].

The number of rows in the Jacobian matrix equals the number of states in the velocity vector, while the number of columns is dependent on the number of joints in the UVMS:

$$J_i\left(q_i\right)=\left[\begin{array}{ccc}{\mathcal{J}}_1\left(q_i\right)& \cdots & {\mathcal{J}}_{m+6}\left(q_i\right)\end{array}\right].$$

(6)

The components of angular and linear velocities are defined by the following expressions

$$\begin{gathered} {\mathcal{J}}_i\left(q\right)=\left[\begin{array}{c}{\mathcal{J}}_{v_i}\left(q\right)\\ {\mathcal{J}}_{{\omega }_i}\left(q\right)\end{array}\right]=\left[\begin{array}{c}{𝓏}_{i-1}\times \left(o_{m+6}-o_{i-1}\right)\\ {𝓏}_{i-1}\end{array}\right]—\mathrm{if} i \mathrm{link} \mathrm{is} \mathrm{rotational}, \\ {\mathcal{J}}_i\left(q\right)=\left[\begin{array}{c}{\mathcal{J}}_{v_i}\left(q\right)\\ {\mathcal{J}}_{{\omega }_i}\left(q\right)\end{array}\right]=\left[\begin{array}{c}{𝓏}_{i-1}\\ 0_{3\times n}\end{array}\right]—\mathrm{if} i \mathrm{link} \mathrm{is} \mathrm{prismatic}, \end{gathered}$$

(7)

where $o_{i-1}$—the first three elements of the fourth column of the matrix $T_{i-1}\left(q\right)$, ${𝓏}_{i-1}$—the first three elements of the third column of the matrix $T_{i-1}\left(q\right)$.

3.2.3. Dynamics Model of an Underwater Robot with Manipulators

The dynamics of underwater robot manipulators can be written as [31,33]:

$$M\left(q\right)\ddot{q}+C\left(\dot{q},q\right)\dot{q}+D\left(\dot{q},q\right)\dot{q}+g\left(q\right)=u+J^T\left(q\right){\overline{f}}_e,$$

(8)

where $q={[q_1^T,\dots ,q_N^T]}^T\in R^{Nm}$, $u={[u_1^T,\dots ,u_N^T]}^T\in R^{Nm}$, $M=diag\left\{M_i\right\}\in R^{Nm\times Nm}$, $C=diag\left\{C_i\right\}\in R^{Nm\times Nm}$, $D=diag\left\{D_i\right\}\in R^{Nm\times Nm}$, $g={[g_1^T,\dots ,g_N^T]}^T\in R^{Nm}$, ${\overline{f}}_e=\left[f_{e,1}^T,f_{e,2}^T,\dots ,f_{e,N}^T\right]$.

The trajectory of the manipulators is predetermined by the desired motion of the object; therefore, Equation (1) is a known equation, but the values of $f_{e,i}$, $i=1,2,\dots ,N$ are undefined. To solve this problem and divide $f_{e,i}$ between the robots, one can apply the optimal load distribution method [32].

Assume that the robotic arms are rigidly gripping the object. Due to the rigid grip of the object, the following equations are satisfied:

$$p_i=p_O+\left[\begin{array}{c}{}_{}{}^{I}R{}_O{l}_i\\ {\alpha }_i\end{array}\right],i=1,2,\dots ,N,$$

(9)

where vectors $l_i={[l_{ix},l_{iy},l_{iz}]}^T$ and ${\alpha }_i={[{\alpha }_{ix},{\alpha }_{iy},{\alpha }_{iz}]}^T$ represent the constant relative position and orientation of the manipulator working tool relative to the object, expressed in the object frame and ${}_{}{}^{I}R{}_O$ denotes the rotation matrix, which describes the orientation of the object, expressed in the inertial reference frame $\left\{I\right\}$. Therefore, using (9) for each manipulator, it is possible to calculate the position of the object relative to the inertial reference frame $\left\{I\right\}$, because the geometric parameters of the object are considered to be known.

In addition, the kinetostatic duality along with the gripping rigidity assumes that the force $f_e$, acting on the object’s center of mass and the generalized forces $f_{e,i}$, acting on the manipulators at the gripping points are related through [32].

$$\begin{gathered} f_e=\left[\begin{array}{cccc}{J}_{O1}^T& J_{O2}^T& \dots & J_{ON}^T\end{array}\right]{\left[\begin{array}{cccc}{f}_{e,1}^T& f_{e,2}^T& \dots & f_{e,N}^T\end{array}\right]}^T= \\ =J_{O1}^T{f}_{e,1}+J_{O2}^T{f}_{e,2}+\dots +J_{ON}^T{f}_{e,N}={{\displaystyle \sum }}_{i=1}^N{J}_{Oi}^T{f}_{e,i},J_{Oi}=J_{iO}^{-1}. \end{gathered}$$

(10)

Now, consider the constants $k_i$, $i=1,2,\dots ,N$ satisfying the $k_1+k_2+\dots +k_N=1$ condition, which we enter as load distribution coefficients between manipulators.

Given $k_1+k_2+\dots +k_N=1$, the dynamics of the object (1) can be rewritten as (the dependence in the dynamics matrices on the number is omitted to simplify the notation):

$${{\displaystyle \sum }}_{i=1}^N{k}_i\left\{M_O{\dot{\upsilon }}_O+C_O{\upsilon }_O+D_O{\upsilon }_O+g_O\right\}=-{{\displaystyle \sum }}_{i=1}^N{J}_{Oi}^T{f}_{e,i}.$$

(11)

From the last relation, we can calculate the forces for each of the manipulators

$$f_{e,i}=-k_i{J}_{iO}^T{f}_e.$$

(12)

Once you have the load distribution scheme between the manipulators, you can develop a controller to control the entire system (8).

3.3. Control Algorithm

Figure 2 demonstrates a scheme of the UVMS tracking control, which clearly shows the separation of the applied methods described in the second section. Each block has its own task, and together they form a complete architecture of a cooperative control system. The used methodology, encapsulated in each block, is described in the second section of the paper. Therefore:

• «Desired object trajectory» generates the desired trajectory of the object’s center of mass $p_O$;
• «Inverse Dynamics» calculates the desired forces $f_{e,i}$, applied to the manipulation robots;
• «SDDRE Tracking Control» produces optimal control itself;
• «$UVM{S}_i$» is a dynamics model of the underwater vehicle–manipulator system;
• «Desired end-effector position» calculates the desired position of the UVMS tool according to a given trajectory and relative end-effector position;
• «Kinematic Control» transposes from the end-effector’s Cartesian coordinate system $p_{i,des}$ into the space of the manipulator’s generalized coordinates $q_{i,des}$.

The control synthesis algorithm can be written as a set of the following steps:

• Setting the desired trajectory of the object $p_O=\left[{\eta }_{1,O}^T,{\eta }_{2,O}^T\right]$;
• The inverse dynamics problem is solved on the basis of the motion model of the object (1). Knowing the motion, determine which force $f_e={\displaystyle \sum }_{i=1}^N{f}_{e,i}$ should be applied to the object to provide the desired movement. After that, this force is distributed between manipulators in proportion according to (11) and (12);
• Based on the desired motion of the object, the problem of determining the desired motion of the working tools of manipulators is solved through (9). It is necessary to set the parameters of the relative position of the object and the manipulator grips, i.e., to set vectors for each manipulator $l_i={[l_{ix},l_{iy},l_{iz}]}^T$ and ${\alpha }_i={[{\alpha }_{ix},{\alpha }_{iy},{\alpha }_{iz}]}^T$;
• Knowing the required motions of the manipulator working tools, the problem of kinematic control is solved using the given motion of the working tool of the manipulator to determine the required value of the generalized coordinates of the robot–manipulator system, i.e., a kinematic regulator is constructed (Figure 3);
• Knowing the desired value of the generalized coordinates and the required force for each manipulator, the problem of synthesizing a tracking SDDRE controller is solved.

There are a number of methods to find the inverse Jacobian for solving the inverse kinematics problem; however, in this paper, we will consider the so-called damped least-squares (DLS) inverse method proposed in [29]:

$$J^{-1}\left(q_i\right)=J^T\left(q_i\right){\left[J\left(q_i\right)·J^T\left(q_i\right)+k^{2}I\right]}^{-1},$$

(13)

where $k$ is a damping factor that renders the inversion better conditioned from a numerical viewpoint.

3.4. Synthesis of an SDDRE Tracking Controller

To synthesize the controller while solving the problem of tracking the desired value of the manipulator’s generalized coordinates, we use the SDDRE method, which has proven itself in many applications, including the control of manipulators [36].

We denote $x={\left[{\begin{array}{cc}{q}^T& \dot{q}\end{array}}^T\right]}^T$ and rewrite the nonlinear system (8) with new notations:

$$\dot{x}=\left(\begin{array}{c}\dot{q}\\ M{(q)}^{-1}\left(u+J^T\left(q\right){\overline{f}}_e-C\left(\dot{q},q\right)\dot{q}-D\left(\dot{q},q\right)\dot{q}-g\left(q\right)\right)\end{array}\right).$$

(14)

The latest equation can be represented in the form of state-space equations with state-dependent coefficients (SDC form)

$$\begin{gathered} \frac{dx}{dt}=A\left(x,t\right)x+B\left(x,t\right)\overline{u},x\left(t_0\right)=x_o, \\ A\left(x,t\right)x=\left(\begin{array}{cc}{0}_{Nm\times Nm}& I_{Nm\times Nm}\\ 0_{Nm\times Nm}& -M{(x)}^{-1}\left(C\left(x\right)+D\left(x\right)\right)\end{array}\right), B\left(x,t\right)=\left(\begin{array}{c}{0}_{Nm\times Nm}\\ M{(x)}^{-1}\end{array}\right), \\ \overline{u}=u+u_{add}\left(x\right), u_{add}=J^T\left(x\right){\overline{f}}_e-g\left(x\right). \end{gathered}$$

(15)

Note that representation (15) is not unique, and there are different possibilities for the representation of the system matrices. The problem of finding the optimal representation for the system matrices is called the parameterization problem (or factorization), and there are different approaches to its solution. For example, it is possible to select the structure and parameters of the matrices based on preserving the controllability requirements of the system. The parametrization problem itself is rather complicated, so these aspects will not be addressed here.

Consequently, for the obtained system (15), we formulate the problem of the criterion minimization (here and further, we will omit the dependence on time where it is not essential for shortening the notation)

$$\begin{gathered} S_{t_0{t}_f}\left(u\right)=\frac{1}{2}{e}^T\left(t_f\right)Fe\left(t_f\right)+\frac{1}{2}{{\displaystyle \int }}_{t_0}^{t_f}\left[e^{T}Q\left(x\right)e+u^{T}R\left(x\right)u\right]dt, \\ Q\left(x\right)\ge 0,R\left(x\right)>0,F>0. \end{gathered}$$

(16)

where $e=x-x_{ref}$, $x_{ref}$—is the desired value of the system state vector (14), which is determined by the desired value of the generalized coordinates.

If the pair of matrices ($A\left(x,t\right)$, $B\left(x,t\right)$) is pointwise controllable for all $x\left(t\right)$ for $t\in \left[t_0,t_f\right]$, the solution of problem (16) in the form of feedback can be obtained using the SDDRE method.

3.5. Centralized Control Scheme

The solution of problem (16) for feedback control can be written in the same way as for the linear case [34]:

$$u=-R^{-1}\left(x\right)B^T\left(x,t\right)\left(K\left(x\right)x+g\left(x,t\right)\right),$$

(17)

where $\dot{K}\left(x\right)=-A{(x)}^{T}K\left(x\right)-K\left(x\right)A\left(x\right)-Q\left(x\right)+K\left(x\right)B\left(x\right)R{(x)}^{-1}{B}^T\left(x\right)K\left(x\right)$, $K\left(t_f\right)=F$, $\dot{g}=-{(A\left(x\right)-B\left(x\right)R{(x)}^{-1}{B}^T\left(x\right)K\left(x\right))}^{T}g+Q\left(x\right)r,g\left(t_f\right)=F{x}_{ref}\left(t_f\right)$.

Two approaches to the implementation of this solution are possible: solving $N$-times SDDRE type control (17) for each manipulator independently and solving SDDRE control once for the whole system. These approaches were proposed in [17].

This numerical solution requires integration of Equation (17) in the opposite direction from time $t_f$ to $t$, which is not realizable because the state $x$ of the system (15) at future time moments is not defined and, therefore, the state-dependent parameters of matrices $A\left(x,t\right)$, $B\left(x,t\right)$ are also unknown.

A Lyapunov-based approach solves the problem in one forward round and was recommended for a tracking problem [34]. Let time tend to infinity, then $K$ in infinity for (17) results as a constant gain, and the steady-state form of $K$ is reformed to the state-dependent Riccati equation (SDRE). As a result, we have a Lyapunov equation with state and control nonlinearities with the final boundary condition at the right time. The solution of this equation results in the suboptimal gain as

$$K\left(x\right)=K_{ss}\left(x\right)+P^{-1}\left(x\right),$$

(18)

where $P\left(x\right)=E\left(x\right)+e^{A_{cl}\left(x\right)\left(t-t_f\right)}\left(P\left(t_f\right)-E\left(x\right)\right)e^{A_{cl}^T\left(x\right)\left(t-t_f\right)}$, $A_{cl}$ is a closed-loop system matrix, $K_{ss}$ is a negative root of SDRE.

The closed-form solution for $g$ for (17) is presented as follows. This equation is rewritten as

$$\dot{g}=-A\left(x\right)g+Q\left(x\right)r.$$

(19)

and the solution has the form of

$$g=A^{-1}\left(x\right)\left(Q\left(x\right)r+e^{A\left(x\right)\left(t_f-t\right)}A\left(x\right)g\left(t_f\right)-Q\left(x\right)r\right).$$

(20)

The proposed closed-form answer works perfectly. Nevertheless, when the end of the trajectory is not zero, oscillation and an unacceptable jump occur. The cause of this problem is an open question.

3.6. Decentralized Control Scheme

In the first case, we need to calculate the control law for each UVMS

$${\overline{u}}_i=-R_i^{-1}\left(x_i\right)B_i^T\left(x_i,t\right)K_i\left(x_i,t\right)x_i+u_{i,add},$$

(21)

where $x_i\left(t\right)$—are the states of the $i$ manipulator. Matrix $K_i\left(x_i,t\right)$ is obtained from the above algorithm, where instead of block matrices $A\left(x\right)$, $B\left(x\right)$, $R\left(x\right)$, and $Q\left(x\right),$ the corresponding matrices for each manipulator are used separately, and the model of manipulator dynamics in SDC form is

$$\begin{gathered} \frac{d{x}_i}{dt}=A_i\left(x_i,t\right)x+B_i\left(x_i,t\right){\overline{u}}_i, x_i\left(t_0\right)=x_{i,o}, \\ {A}_i\left(x_i,t\right)x_i=\left(\begin{array}{cc}{0}_{m\times m}& I_{m\times m}\\ 0_{m\times m}& -M_i^{-1}\left(x\right)\left(C_i\left(x\right)+D_i\left(x\right)\right)\end{array}\right), B_i\left(x_i,t\right)=\left(\begin{array}{c}{0}_{m\times m}\\ M_i^{-1}\left(x_i\right)\end{array}\right), \\ {\overline{u}}_i=u_i+u_{i,add}\left(x_i\right), u_{i,add}=J_i^T\left(x_i\right)f_{i,e}-g_i\left(x_i\right), i=1,2,\dots ,N. \end{gathered}$$

(22)

The matrix $K\left(x,t\right)$ of the controller (17) when solving the control problems separately for each vehicle manipulator by analogy with [37] can be composed of elements $K_i\left(x_i,t\right)$ in the form

$$K\left(x,t\right)=\left(\begin{array}{ccccc}{K}_1\left(x_1,t\right)& 0& 0& 0& 0\\ 0& K_2\left(x_2,t\right)& 0& ⋮& 0\\ 0& 0& \ddots & 0& ⋮\\ ⋮& ⋮& 0& K_{N-1}\left(x_{N-1},t\right)& 0\\ 0& 0& \cdots & 0& K_N\left(x_N,t\right)\end{array}\right).$$

(23)

In this case, the system has $N$ independent subcontrollers for each vehicle manipulator separately.

The second approach uses parameterized matrices for the entire system (15), requires solving SDDRE only once, and implements the control law (17). This approach provides optimal control of the complete system.

3.7. Test Case

3.7.1. Object Dynamics

In order to verify the method, consider a specific test case. As an object of manipulation, a body in the form of a cylindrical rod (Figure 4) of mass $m_O=23$ kg, length $l_O=2.5$ m, and radius $r_O=0.05$ m is used.

The tensor of inertia of the handled object has the following definition

$$I_O=\left(\begin{array}{ccc}\frac{1}{12}{m}_O{l}_O^2& 0& 0\\ 0& 0& 0\\ 0& 0& \frac{1}{12}{m}_O{l}_O^2\end{array}\right).$$

(24)

Since the working environment is underwater, it is necessary to take into account the hydrodynamic effects. The added masses of the rod, according to [5], are defined as

$$\begin{gathered} X_{\dot{u}}=-0.1m_O, \\ {Y}_{\dot{v}}=-\pi \rho r_O^2{l}_O, \\ {Z}_{\dot{w}}=-\pi \rho r_O^2{l}_O, \\ {K}_{\dot{p}}=0, \\ {M}_{\dot{q}}=-\frac{1}{12}\pi \rho r_O^2{l}_O^3, \\ {N}_{\dot{r}}=-\frac{1}{12}\pi \rho r_O^2{l}_O^3, \end{gathered}$$

(25)

where $\rho =1025$ and represents the density of the fluid environment (salted water).

3.7.2. UVMSs Dynamics

As an example of the method’s operability, consider the situation depicted in Figure 1. The work [38] presents the dynamic parameters of the Sf-30k underwater vehicle (Figure 5), which we use to simulate the mobile manipulator bases.

Dynamics matrices for two identical Sf-30k with consideration of hydrodynamic effects are given below.

$$M_1=\left[\begin{array}{cccccc}2798& 0& 0& 0& 872.9& 0\\ 0& 3314& 0& -872.9& 0& 0\\ 0& 0& 4831& 0& 0& 0\\ 0& -872.9& 0& 942.1& 0& 0\\ 872.9& 0& 0& 0& 1997& 0\\ 0& 0& 0& 0& 0& 1889\end{array}\right];$$

(26)

$$\begin{gathered} D_1\left(v\right)=D_{LIN}+D_{QUAD}\left(v\right); \\ {D}_{QUAD}\left(v\right)=\left[\begin{array}{cccccc}773.6\left|u\right|& 0& 0& 0& 0& 0\\ 0& 1067\left|v\right|& 0& 0& 0& 0\\ 0& 0& 2277\left|w\right|& 0& 0& 0\\ 0& 0& 0& 302.5\left|p\right|& 0& 0\\ 0& 0& 0& 0& 1078\left|q\right|& 0\\ 0& 0& 0& 0& 0& 558.1\left|r\right|\end{array}\right]; \\ {D}_{LIN}=\left[\begin{array}{cccccc}123.8& 0& 0& 0& 0& 0\\ 0& 170.7& 0& 0& 0& 0\\ 0& 0& 364.3& 0& 0& 0\\ 0& 0& 0& 129.1& 0& 0\\ 0& 0& 0& 0& 188.0& 0\\ 0& 0& 0& 0& 0& 97.36\end{array}\right]; \end{gathered}$$

(27)

$$C_1\left(v\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& 4831w& -3314v\\ 0& 0& 0& -4831w& 0& 2798u\\ 0& 0& 0& 3314v& -2798u& 0\\ 0& 4831w& -3314v& 0& 0& 0\\ -4831w& 0& 2798u& -18& 0& 0\\ 3314v& -2798u& 0& 0& 0& 0\end{array}\right];$$

(28)

$$g_1\left(\eta \right)=\left[\begin{array}{c}0\\ 0\\ 0\\ 7081cos\theta sin\varphi \\ 7081sin\theta \\ 0\end{array}\right].$$

(29)

Matrices (26)–(29) already contain hydrodynamic effects calculated according to the strip theory [38].

The six-link robot (Figure 6) with cylindrical rotating links as manipulative machines [35] will be considered below.

Table 1. Manipulator link parameters.

Link i	Link Mass mi, (kg)	Buoyancy Bi (N)	Radius ri (m)	Length li (mm)
1	1.50	0.4	0.1	64.9
2	1.25	0.2	0.1	957.1
3	1.25	0.4	0.1	125
4	2.1	0	0.1	580
5	1.6	0	0.1	259
6	1.05	0.15	0.1	0

shows the parameters of both devices.

Workclass ROVs are often equipped with one or two manipulators, depending on the type of operation the ROV is designed for. The main reason for equipping ROVs with manipulators is to design them for intervention tasks. By mounting the ROV with manipulators, additional hydrodynamic, hydrostatic, and rigid body inertia forces and moments will affect the vehicle [5,32]. If the ROV is large enough, these effects are negligible. However, these effects usually have to be modelled and taken into account when designing workclass ROVs. Here, the manipulator is of considerable size, and the presence of the manipulator will create forces and moments on the ROV that affects the response of the system. In addition, when the manipulators perform intervention tasks, they come in contact with the environment, which creates contact forces that also will affect the ROV. No matter the size difference between the ROV and the manipulator, the contact forces can be of considerable size, which needs to be taken into account when modelling the system.

The joint motion is often a very simple one degree of freedom linear or rotational motion. Manipulator systems therefore consist of either revolute or prismatic joints or a combination of these. The manipulator system considered in this paper, shown in Figure 6, is built up by revolute joints. This gives very favorable characteristics to the robot’s workspace, and it makes the modelling a lot easier, as the homogeneous transformation matrices can be written in a very simple form using D-H parameters in

Table 2. D-H parameters.

Link i	ai (mm)	ai (rad)	di (mm)	qi (rad)
1	64.9	$\pi /2$	0	$q_1$
2	957.1	0	0	$q_2+\pi /2$
3	125	$\pi /2$	0	$q_3-\pi /2$
4	0	$-\pi /2$	580	$q_4$
5	259	$-\pi /2$	0	$q_5$
6	0	$-\pi /2$	0	$q_6$

.

The resulting equations are quite cumbersome, but the method of calculating the manipulator’s dynamic parameters based on the Euler–Lagrange method is redundantly described in papers [35,37,38]. The source [33] provides formulas to calculate the hydrodynamic effects for the cylindrical shape, which were used to calculate the resulting parameters.

It is necessary to take into account the offset of the manipulator’s base relative to the center of mass of the vehicle. The resulting required transformation matrix, which provides all the necessary basic kinematic transformations, is shown below

$$T_0=\left[\begin{array}{cccc}1& 0& 0& 0.75\\ 0& 1& 0& 0\\ 0& 0& 1& -0.75\\ 0& 0& 0& 1\end{array}\right].$$

(30)

From this, one can see the desired offset in the last column of the matrix (30).

3.7.3. Controller Parameters

One of the most important tasks in the synthesis of SDDRE control is the selection of penalty matrices $Q$ and $R$. There are no clearly formulated rules in selecting the numerical values for each of them. This is a creative challenge that requires data calibration, for example, in numerical simulations.

In the context of this paper, the penalty matrices for the first and the second UVMS were matched as follows:

$$Q=\left(\begin{array}{cc}{10}^4·I_{12\times 12}& 0_{12\times 12}\\ 0_{12\times 12}& 0_{12\times 12}\end{array}\right), R={10}^{-4}·I_{12\times 12}.$$

(31)

In this way, parameters (31) restrict the states and provide more freedom to the system control.

4. Results

The following section will present the Simulink model applied to simulate the system, and all the relevant blocks will be reviewed. The Simulink model for the underwater vehicle–manipulator system is presented in Figure 7.

The Simulink model consists of kinematic and dynamic modeling blocks for each link. Reference block “Object” define reference values and constants.

Figure 8 shows the subsystem for calculating the generalized coordinates and position of the working tool using Lagrange dynamics and solving the Jacobian velocity conversion task. The outputs of the block are position and vector q.

Figure 9 shows the module for calculating the desired generalized coordinates based on the method proposed in [29] using Equation (13).

The desired trajectory of the controlled object is shown in Figure 10. Here, you can see the desired angular and translational changes in the position of the object’s center of mass. The object should move along a circular trajectory. The result of the first UVMS movement according to the desired trajectory is shown in Figure 11.

Analogous results for the second UVMS are shown in Figure 12. As a result, the trajectory controller works correctly according to the system parameters.

Figure 13 shows that the tracking control of the object occurs with a slight error; however, the endpoint of the trajectory is obtained.

The magnitude of the object’s deviation from the desired trajectory is determined by the RMSE and MAE metrics, which are summarized in

Table 3. RMSE and MAE metrics of object position/orientation.

DOF	RMSE	MAE
$x$	0.0221	0.0481
$y$	0.0357	0.0664
$z$	0.0106	0.0166
$\varphi$	0.2515	0.6233
$\theta$	0.0749	0.4101
$\psi$	0.6386	1.2101

for each degree of freedom.

5. Discussion

The results obtained in the simulation can be ambiguous for the considered method, but the synthesized control is optimal, which is an indisputable advantage in comparison with the classical methods of control systems.

Undoubtedly, the derived calculation methodology is valid, which is proved by the results of the third section of the paper. Practical applications are quite urgent, because there is a tendency for underwater robotics development in the world. In this case, it is necessary to synthesize a control law, which shall satisfy the optimality requirements. The work results can be used as a calculation method for controllers of robotic complexes in the underwater environment.

A further development vector is directed towards the task of comparing centralized and decentralized control systems. Additionally, it is necessary to consider the more complex case of a control system with many robots in a constrained environment with obstacles.

6. Conclusions

In this research, an algorithm for coordinated control of a robotic complex with $N$ manipulators while performing the work of moving a concentrated mass cargo along a given trajectory was developed. To synthesize the control, we used an approach based on the representation of nonlinear manipulator dynamics models in the linear state-space form with state-dependent parameters (SDC-form). That allowed us to apply suboptimal control methods with the feedback based on the solution of the SDDRE. It should be noted that the SDDRE method provides the construction of control on a finite time interval and provides the change in control intensity taking into account the transient effect of the system matrices in SDC form.

The research highlights two approaches of system implementation: a common controller for the complete system and a set of $N$ independent subcontrollers for RTC manipulators.

The results of both approaches are similar, but for the systems with little interaction between manipulators, a common structure is recommended, and for the systems with a large number of manipulators, the approach with independent subcontrollers may be more acceptable.

In practical implementation, the failure of one subcontroller does not ruin the performance of the entire system. Therefore, it is recommended to use an independent design for experimental operations in order to maintain greater system safety.