Model Predictive Collision Avoidance Control for Object Transport of Unmanned Underwater Vehicle-Dual-Manipulator Systems

Abstract

Unmanned underwater vehicle-dual-manipulator systems (UVDMSs) have attracted much research due to their humanoid operation capabilities, which have the advantage of cooperative manipulations and transporting underwater objects. Meanwhile, collision avoidance of UVDMSs is more challenging than that of unmanned underwater vehicle-dual manipulator systems (UVMSs). In this work, a model predictive control (MPC) approach is proposed for collision avoidance in objects transporting tasks of UVDMSs. The minimum distances of mutual manipulators and frame obstacles are handled as velocity constraints in the optimization of the UVDMS’s object tracking control. The command velocity generated by the model predictive kinematic controller is tracked by a dynamic inversion control scheme while model uncertainties are compensated by a neural network. Moreover, the tracking errors of the proposed dynamic controller are proved to be convergent by the Lyapunov method. At last, a three-dimensional (3D) UVDMS simulation platform is developed to verify the effectiveness of the proposed control strategy in the tasks of collision avoidance and object transport.

1. Introduction

Unmanned underwater vehicles (UUVs), including autonomous underwater vehicles (AUVs) and remotely operated vehicles (ROVs), have played a significant role in exploring the deep sea. However, human interest in underwater research is not only limited to observation but also requires a certain degree of intervention [1], a capability that classical UUVs lack. Therefore, UUVs equipped with underwater manipulators, named underwater vehicle-manipulator systems (UVMSs), are widely used in marine energy and architecture industries [2,3]. To achieve a larger work range and certain cooperation abilities, UVMSs often assemble two manipulators for specific tasks. This type of UVMS is called underwater vehicle-dual-manipulator systems (UVDMSs), which have the intuitive advantage of dealing with precise tasks since they are expected to be substitutes for human bodies underwater.

Research related to dual arms has always been a hot topic in the field of robotics, including but not limited to space robotics [4], medical robotics [5], drones [6], wheeled robotics [7], and humanoid robotics [8]. An obvious advantage of dual-arm robots is their ability to capture and transport target objects. An adaptive hybrid control scheme is designed to capture unknown objects by dual arms cooperatively in [9]. Refs. [10,11] propose an integrated multi-agent global planning and central strategy for reactive dual arm manipulation while modeling the dual arm cooperative control by dual qualifications, which performs perfectly in transporting objects. Compared to single-arm robots, the problem of mutual collisions between the two arms is a significant constraint for motion planning of dual-arm robots. Therefore, collision avoidance is essential for the cooperative control of dual manipulators. Ref. [12] describes a control scheme to avoid conflicts between dual manipulators with high accuracy in tracking the desired path while regarding distances of obstacles as constraints in the optimization model. The obstacle avoidance strategy in [11], using the artificial potential field method, can avoid obstacles in the transporting task effectively. In terms of UVDMSs, cooperative control, and obstacle avoidance are more complicated due to the complex dynamics, limited workspace, and lack of obstacle information. In [13], a multiple impedance dual-arm control for the task of object manipulation is proposed with reliable performance when contacting the environment. Ref. [14] introduces the coordination control problems of UVMSs and discusses the application of the proposed framework to transportation problems of UVDMSs at the end. Their later research [15] improves the previous framework to adapt to more scenarios.

UVDMSs are composed of multiple rigid bodies, which result in complicated modeling with couplings and uncertainties, so the motion planning and control of UVDMSs are challenging. The motion redundancy caused by multiple degrees of freedom makes it difficult to obtain an inversion solution from Cartesian space. The most common method to solve the redundancy problem is the task priority strategy [15], which decomposes tasks into several sub-tasks in forms of target velocities and executes them in order. The sub-tasks are projected into the zero space of the main task, which will not be affected. Although this method fully utilizes the freedoms of UVDMS, selecting a proper task priority is not so easy, in that inappropriate configurations may lead to task failures. Optimization methods that regard the kinematic control as an optimization problem perform effectively in the motion planning of UVDMSs by limiting the solution to a small range through appropriate constraints. Extensive research on dual-arm robotics uses the quadratic programming scheme to deal with redundancy control [16,17,18]. On this basis, the model predictive control (MPC), which has better dynamic performance, is suitable for the control of systems with high degrees of freedom. Static obstacles involved in a constrained workspace for multiple UVMSs’ cooperative transportation control are studied by [19], which considers the kinematic singularities, obstacles, joint limits, and input saturation. The effectiveness of their results has been proved in both simulations and real experiments. Ref. [20] developed a model predictive kinematic visual servoing control strategy by decoupling the control of the UVMS to separated vehicle and manipulator controls. For trajectory tracking of UVMSs, ref. [21] proposes a fast MPC method with the extended Kalman filter (EKF) to compensate for model uncertainties and disturbances. A fast tube MPC scheme for moving target tracking of UVMSs is proposed in [22], with an EKF to overcome the sensor noise. Similarly, to deal with noises, disturbances, and model uncertainties, ref. [23] presents a sliding mode MPC strategy for coordinated control of the UVMS. Model uncertainties are also studied by other researchers, with robust and adaptive controls. In [24], a robust controller, which performs well in trajectory tracking in the presence of system uncertainties and disturbances, is introduced. The adaptive control scheme and observer-based method for uncertainties of the UVMS are studied in [25], utilizing the neural network (NN) to approximate the unknown dynamics. An NN is also used in [26] as both a disturbance observer and state predictor. By this operation, a composite control scheme consisting of an NN and MPC is established to generate optimal control actions in the presence of uncertainties and input saturation.

Based on the previous research, this work focuses on the collision avoidance and trajectory tracking of UVDMS. Both mutual collision and frame collision are considered by defining constraints of the minimum distance velocities of all manipulator links. A model predictive kinematic control scheme is proposed to track the reference trajectory of the grasped object. Then, a neural network-based dynamic inversion control framework is used to track the command velocity generated by the kinematic controller. At last, a 3D UVDMS platform is established to test the performances of the proposed control strategy.

This work is organized as follows. Section 2 provides a brief mathematical model of the UVDMS and formulates the collision avoidance constraints, as well as the object transport model. In Section 3, the kinematic MPC tracking scheme and neural network-based dynamic inversion controller with stability analysis are established. Finally, simulation experiments are tested on the platform based on Creo 10.0.1.0, Unity 2022.3.15f1c1, and Matlab R2023b in Section 4.

2. Problem Formulation

In this section, the kinematic and dynamic models of UVDMS are described briefly. Initially, the kinematic model is given by defining relevant coordinates and variables. Then, the collision avoidance method is established as constraints in the control system. Moreover, the object transport model is established.

2.1. UVDMS Model

Figure 1 shows the necessary coordinates and variables that describe the motions of UVDMSs. The vehicle moves in the inertial frame ${\Sigma }_i$, which is always fixed on the ground with the z-axis downward. In ${\Sigma }_i$, positions of the vehicle and end effectors are defined as $\eta ={[x,y,z,\varphi ,\theta ,\psi ]}^T$ (including the global position ${\eta }_1={[x,y,z]}^T$ and Euler angles ${\eta }_2={[\varphi ,\theta ,\psi ]}^T$) and ${\eta }_{ee}={[{{\eta }_{eel}}^T,{{\eta }_{eer}}^T]}^T$ with ${\eta }_{eel}={[x_l,y_l,z_l,{\phi }_l,{\theta }_l,{\psi }_l]}^T$ for the left end effector and ${\eta }_{eer}={[x_r,y_r,z_r,{\phi }_r,{\theta }_r,{\psi }_r]}^T$ for the right one. The vehicle velocity, in frame ${\Sigma }_b$ fixed with the vehicle body, is $v={[u,v,w,p,q,r]}^T$, which contains the linear velocity $v_1={[u,v,w]}^T$ and the angular velocity $v_2={[p,q,r]}^T$. The joint angles of both underwater manipulators are $q={[q_l^T,q_r^T]}^T$ where $q_l={[q_{l1},q_{l2},q_{l3},q_{l4},q_{l5},q_{l6}]}^T$ is the angle vector of the left manipulator and $q_r={[q_{r1},q_{r2},q_{r3},q_{r4},q_{r5},q_{r6}]}^T$ for the right one. And the joint velocities $\dot{q}={[{\dot{q}}_l^T,{\dot{q}}_l^T]}^T$ have the same forms. In addition, ${\Sigma }_{0r}$ and ${\Sigma }_{er}$, in Figure 1, represent the right manipulator base frame and the right end-effector frame, respectively.

By defining the system velocity as $\zeta ={[v^T,{\dot{q}}^T]}^T\in {\Re }^{18}$, the rotation matrix $R_B^I\in {\Re }^{3\times 3}$ from ${\Sigma }_b$ to ${\Sigma }_i$, and the Jacobian matrix $J_{k,o}(\eta )\in {\Re }^{3\times 3}$ of the vehicle angular velocity we obtain the system velocity in the frame ${\Sigma }_i$, as follows.

$$\zeta =\left[\begin{array}{ccc}{R}_I^B& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 12}\\ {\mathbf{0}}_{3\times 3}& J_{k,o}(\eta )& {\mathbf{0}}_{3\times 12}\\ {\mathbf{0}}_{12\times 3}& {\mathbf{0}}_{12\times 3}& I_{12}\end{array}\right]\left[\begin{array}{c}{\dot{\eta }}_1\\ {\dot{\eta }}_2\\ \dot{q}\end{array}\right]=J_k\left[\begin{array}{c}{\dot{\eta }}_1\\ {\dot{\eta }}_2\\ \dot{q}\end{array}\right],$$

(1)

Then, the relationship between the system velocity and the end effector velocity can be formulated as

$${\dot{\eta }}_{ee}=\left[\begin{array}{c}{\dot{\eta }}_{eel}\\ {\dot{\eta }}_{eer}\end{array}\right]=\left[\begin{array}{c}{J}_l(R_B^I,q_l)\\ J_r(R_B^I,q_r)\end{array}\right]\zeta =\left[\begin{array}{c}{J}_{posl}(R_B^I,q_l)\\ J_{orl}(R_B^I,q_l)\\ J_{posr}(R_B^I,q_r)\\ J_{orr}(R_B^I,q_r)\end{array}\right]\zeta =J\zeta ,$$

(2)

where the Jacobian matrix $J\in {\Re }^{12\times 18}$ transfors the system velocity to the end effector velocity, contains the linear velocity transfomation $J_{posl},J_{posr}\in {\Re }^{3\times 18}$, and the angular velocity transformation $J_{orl},J_{orr}\in {\Re }^{3\times 18}$.

According to [27,28], the dynamic model of UVDMS is expressed as follows.

$$M(q)\dot{\zeta }+C(q,\zeta )\zeta +D(q,\zeta )\zeta +g(q,{\eta }_2)=\tau ,$$

(3)

where $M(q)\in {\Re }^{18\times 18}$ is the inertial matrix in which the diagonal elements include the inertial of both the vehicle and the manipulators while other elements represent the couplings. $C(q,\zeta )\in {\Re }^{18\times 18}$ is the added Coriolis and centripetal matrix while $D(q,\zeta )\in {\Re }^{18\times 18}$ is the damping and hydrodynamic lift matrix. It should be noted that the current velocity is ignored in this model because UVDMS usually works in stable environments. The system input (torques of the vehicle and joints) and gravity are denoted by the vectors $\tau ={[{\tau }_v,{\tau }_q]}^T$ and $g(q,{\eta }_2)$. Thus, the UVDMS model can be represented by

$$\{\begin{array}{c}\left[\begin{array}{c}\dot{\eta }\\ \dot{q}\\ {\dot{\eta }}_{ee}\end{array}\right]=\left[\begin{array}{c}{J}_k^{-1}\\ J\end{array}\right]\zeta \\ \dot{\zeta }=f(\tau ,\zeta ,q,{\eta }_2)\end{array},$$

(4)

where the dynamic model is written in a general function form as $f(\tau ,\zeta ,q,{\eta }_2)=M^{-1}(q)\tau -C(q,\zeta )\zeta -D(q,\zeta )\zeta -g(q,{\eta }_2)$ to simplify the control formulation.

2.2. Collision Avoidance

The collision avoidance study of UVDMSs differs significantly from the manipulators on land. Firstly, UVDMSs are always designed in the form of ROVs with outer frames which limit the workspace severely. Secondly, the limited space may result in a close distance between two manipulators so that the probability of interference increases. Moreover, the thick watertight joint shells further reduce the available workspace. At last, collision monitoring using global information (i.e., the visual system) is difficult underwater. In this section, a collision avoidance strategy considering the UVDMS configurations is established.

In this study, the manipulators’ mutual collisions and the outer frame collisions are discussed. Both of them are handled by calculating their minimum distances to develop velocity constraints. It is reasonable that when one manipulator (i.e., the right one without loss of generality) is working, another one is regarded as an obstacle. The purpose of collision prevention is to ensure that the minimum distance between the manipulators does not exceed the limit. The minimum distance is defined in Figure 2 where the i-th link of the right manipulator is represented by the line segment AB with A and B being the endpoints of the link. The distance between AB and the left manipulator is defined by the minimal one among the distances between AB and all the links of the left manipulator. In this way, the line segment PE in Figure 2 is computed as the distance between AB and the left manipulator. After calculating all 6 right links, one obtains the distances from them to the left manipulator as d(1)~d(6).

Algorithm 1 provides a numerical method to obtain the approximate value which is higher than the actuate value.

Algorithm 1 Manipulators Distances Calculation Algorithm
Manipulators Minimum Distances Calculation Algorithm
1.	Load DH_l(q), DH_r(q), A(0), C(0);
2.	Calculate transforms T_l(1~7) and T_r(1~7) using DH(q);
3.	A(1)←T_r(1)(A(0)), C(1)←T_l(1)(C(0));
4.	for k = 1 : 6
5.	A(k+1)←T_r(k + 1)(A(0))
6.	AB(k)←vector(A(k), A(k + 1))
7.	Initialize D’(0)
8.	for i = 1 : n
9.	Initialize D(i, 0)
10.	P’(i)←A(k) + i/n*AB(k)
11.	for m = 1 : 6
12.	C(m+1)←T_l(m + 1)(C(0))
13.	CD(m)←line segment(C(m), C(m + 1))
14.	D(i,m)←distance(P’(i), CD(m))
15.	if Di(i,m) < D(i, m − 1)
16.	D’(i)←D(i, m)
17.	Generate E’(i)
18.	end if
19.	end for
20.	if D’(i) < D’(i − 1)
21.	d(k)←D’(i), P(k)←P’(i), E(k)←E’(i)
22.	end if
23.	end for
24.	end for
25.	Output d(1)~d(6), P(1)~P(6), E(1)~E(6)

Theorem 1.

With d’(k) = d(k) − r as the modified distance, where r is the maximum interval when sampling P(i) and d(k) is the approximate distance calculated by Algorithm 1, the inequality d’(k) < d*(k) holds.

Proof. 

Obviously, the above inequality holds once the two links are coplanar. The non-coplanar case is shown in Figure 3 where it is assumed that P(i)E is the computed distance between link AB(k) and link CD(m). The actual closest point is marked as P* between P(i) and P(i + 1) without loss of generality (P* cannot be out of the adjacent intervals of P(i) otherwise the algorithm would output another P(i′) and d(k′)). Thus, we have r + d*(k) = r + P*E* > P(i)P* + P*E* > P(i)E* > P(i)E = d(k), that is d’(k) = d(k) − r < d*(k). □

On the other hand, the outer frames are considered as several planar obstacles. In this case, these planes are the front frame, the left frame, the right frame, and the bottom frame, while other frames cannot be reached by the manipulators (or be reached after the former planes). The right-hand links are taken as an example in Figure 2 to define the link-plane distances. As before, the i-th link AB has the nearest distance, represented by the line segment MN, to plane F. After calculating the other 5 link-plane distances, as well as the left-hand distances, one obtains the distances from both manipulators’ links to the nearest plane as dr(1)~dr(6) and dl(1)~dl(6). The link-plane distances can also be calculated using Algorithm 1 by replacing the point-line-segment distance with the point-plane distance. Similarly, the modified dr^ = dr − r and dl^ = dl − r are defined and the inequality similar to Theorem 1 still holds. In addition, it should be noted that the accuracy of the algorithm depends on r. In this case, considering the length of all links, the error is within 1cm while more than n = 20 critical points are sampled for each link.

Summarizing the results above, we obtain

$${\sigma }_i=\frac{d_{\sigma i}^T}{\Vert d_{\sigma i}\Vert }{J}_{\sigma i}\zeta ,{\pi }_i=\frac{d_{\pi i}^T}{\Vert d_{\pi i}\Vert }{J}_{\pi i}\zeta ,{\omega }_i=\frac{d_{\omega i}^T}{\Vert d_{\omega i}\Vert }{J}_{\omega i}\zeta ,i=1,6,$$

(5)

where, $d_{\sigma i}$ is the i-th link-manipulator distance vector (PE(i)) of the right manipulator, $d_{\pi i}$ is the i-th link-plane distance vector (MNr(i)) of the right manipulator, and $d_{\omega i}$ is the i-th link-plane distance vector (MNl(i)) of the left manipulator. $J_{\sigma i},J_{\pi i}$ and $J_{\omega i}$ are the Jacobian matrices for the velocities of points P(i), Mr(i), and Ml(i). ${\sigma }_i,{\pi }_i$ and ${\omega }_i$ indicate the velocities of the critical points toward the obstacles. The distance velocities can be written in a compact form as

$$\begin{array}{l}\sigma =J_{\sigma }\zeta ,\pi =J_{\pi }\zeta ,\omega =J_{\omega }\zeta \\ J_{\sigma }=\left[\begin{array}{c}\frac{d_{\sigma 1}^T}{\Vert d_{\sigma 1}\Vert }{J}_{\sigma 1}\\ ⋮\\ \frac{d_{\sigma 6}^T}{\Vert d_{\sigma 6}\Vert }{J}_{\sigma 6}\end{array}\right],J_{\pi }=\left[\begin{array}{c}\frac{d_{\pi 1}^T}{\Vert d_{\pi 1}\Vert }{J}_{\pi 1}\\ ⋮\\ \frac{d_{\pi 6}^T}{\Vert d_{\pi 6}\Vert }{J}_{\pi 6}\end{array}\right],J_{\omega }=\left[\begin{array}{c}\frac{d_{\omega 1}^T}{\Vert d_{\omega 1}\Vert }{J}_{\omega 1}\\ ⋮\\ \frac{d_{\omega 6}^T}{\Vert d_{\omega 6}\Vert }{J}_{\omega 6}\end{array}\right]\end{array},$$

(6)

and

$$v_{dis}=\left[\begin{array}{c}\sigma \\ \pi \\ \omega \end{array}\right]=\left[\begin{array}{c}{J}_{\sigma }\\ J_{\pi }\\ J_{\omega }\end{array}\right]\zeta =J_{dis}\zeta .$$

(7)

The objective of collision avoidance is to limit $v_{dis}$ so that the corresponding minimum distances will not decrease. However, direct collision avoidance will have a negative effect once the obstacles are far away or where the UVDMS’ motion is not encumbered. Therefore, the collision detection area, shown in Figure 2, is divided into three zones by the threshold d1 and d2. When the obstacle reaches the red zone (the distance lower than d1), there is a high probability of collision, so the controller must stop the motion or generate a large escape velocity. When the obstacle (as the orange obstacle shown in Figure 2) is in the anti-collision zone (the distance between d1 and d2), the normal avoidance velocity is generated to drive the manipulators away. While obstacles are in the safe area (distance higher than d2) no special movement needs to be implemented. According to [29], a gradually changing operation is defined as

$$\begin{array}{l}{v}_{\lim }=S(d)\max \{v_{dis}{|}_{d=d_2},\mathbf{0}\}\\ S(d)=\{\begin{array}{cc}\begin{array}{l}1,\\ {\sin }^2(\frac{\pi }{2}\cdot \frac{d-d_1}{d_2-d_1}),\\ 0,\end{array}& \begin{array}{cc}\begin{array}{l}if\\ if\\ if\end{array}& \begin{array}{l}d\ge d_2\\ d_1<d\le d_2\\ d\le d_1\end{array}\end{array}\end{array}\end{array}.$$

(8)

It should be noted that the operation above is not a strict form of a vector function and the scalar variable $d$ represents each component of the distance vectors $d_{\sigma },d_{\omega }$ and $d_{\pi }$ to simplify the formulation. Thus, the constraints for collision avoidance are given as follows.

$$v_{dis}(i)\le v_{\lim }(i),i=1~18,$$

(9)

2.3. Object Transport Model

Before modeling the object, it is assumed that the object has already been captured by the end effectors with rigid body connections. As shown in Figure 4, the cylinder object is gripped by two clamps fixed with the end effectors. The object’s kinematic is provided as

$$\begin{array}{l}{\eta }_o={[x_o,y_o,z_o,{\phi }_o,{\theta }_o,{\psi }_o]}^T=\left[\begin{array}{c}{\eta }_{o1}\\ {\eta }_{o2}\end{array}\right]=T_r^o{T}_b^r(\eta )\\ {\dot{\eta }}_o=\left[\begin{array}{c}{\dot{\eta }}_{o1}\\ {\dot{\eta }}_{o2}\end{array}\right]=J_{oer}{\dot{\eta }}_{eer}=J_{oer}{J}_r\zeta =J_o\zeta \end{array},$$

(10)

where $J_{oer}$ is the Jacobian matrix from the right end effector velocity to the object velocity in the inertial frame. $T_r^o$ and $T_b^r$ are the homogeneous transformations that can be obtained by the DH parameters.

Similar to the vehicle model, the underwater object dynamic is described as

$$M_o{\ddot{\eta }}_{\mathrm{o}}+C_o({\dot{\eta }}_{\mathrm{o}}){\dot{\eta }}_{\mathrm{o}}+D_o({\dot{\eta }}_{\mathrm{o}}){\dot{\eta }}_{\mathrm{o}}+g_o({\eta }_{\mathrm{o}})=F_o,$$

(11)

where $M_o$ is the inertial matrix, $C({\dot{\eta }}_o)$ is the added Coriolis and centripetal matrix, $D({\dot{\eta }}_o)$ is the damping and hydrodynamic lift matrix, and $g_o({\eta }_{\mathrm{o}})$ is the gravity vector. The force $F_o$ (force and torque) comes from the end effectors as

$$F_o=J_{gra}{F}_e=J_{gra}\left[\begin{array}{c}{F}_{el}\\ F_{er}\end{array}\right],$$

(12)

where $J_{gra}$ is the force Jacobian matrix from frame ${\Sigma }_{el}$ and ${\Sigma }_{er}$ to frame ${\Sigma }_o$ (the object coordinate frame) and $F_{el}$ and $F_{er}$ are the forces affected by the two end effectors. By adding the reaction force from the end effectors to the UVDMS dynamic model, we obtain

$$\begin{array}{l}M(q)\dot{\zeta }+C(q,\zeta )\zeta +D(q,\zeta )\zeta +g(q,{\eta }_2)=\tau -J_e{F}_e\\ J_e=\left[\begin{array}{cc}{J}_{el}& J_{er}\end{array}\right]\end{array},$$

(13)

where $J_{el},J_{er}\in {\Re }^{18\times 6}$ are the force Jacobian matrices from the end effectors frame to the vehicle body frame. By substituting Equations (11) and (12), and ${\ddot{\eta }}_o={\dot{J}}_o\zeta +J_o\dot{\zeta }$ into Equation (13), we obtain the dynamic of the whole transport system as

$$\{\begin{array}{l}{\dot{\eta }}_o=J_o\zeta \\ M_w\dot{\zeta }+H_w\zeta +g_w=\tau \end{array},$$

(14)

where

$$\begin{array}{l}{M}_w=M(q)+J_e{J}_{gra}^{+}{M}_o{J}_o\\ H_w=C(q,\zeta )+D(q,\zeta )\zeta +J_e{J}_{gra}^{+}{M}_o{\dot{J}}_o+J_e{J}_{gra}^{+}{C}_o\left({\eta }_o\right)J_o+J_e{J}_{gra}^{+}{D}_o\left({\eta }_o\right)J_o\\ g_w=g(q,{\eta }_2)+J_e{J}_{gra}^{+}g\left({\eta }_o\right)\end{array}.$$

(15)

Thus, the whole UVDMS-object system model is converted to a compact Lagrange model. The objective is to design a controller to track the reference trajectory of the object.

3. Object Tracking Control

In this section, the control strategy, shown in Figure 5, is studied to track the desired trajectory of the object, while considering the collision avoidance problem and the dynamic uncertainties. The control scheme in Figure 5 includes two parts: kinematic and dynamic controls. In the kinematic control, the command velocity is produced by the model predictive controller with certain constraints, including the obstacle velocities limit. Then, the radial basis function (RBF) neural network-based dynamic inversion control framework for the transport task is developed to track the command velocity and compensate for the model uncertainties. At last, the stability of the dynamic control is analyzed briefly.

3.1. Model Predictive Kinematic Tracking Control

The desired objective tracking problem can be solved by an MPC approach once the object kinematic is converted to the discrete-time model as

$${\eta }_o(k+1)={\eta }_o(k)+T_c\cdot J_o\zeta (k)=f_o({\eta }_o(k),\zeta (k),\eta (k),q(k)),$$

(16)

where ${\eta }_o(k),\zeta (k),\eta (k),$ and $q(k))$ represent the object pose, system velocity, vehicle pose, and joint angles at time step $k$, respectively. The sampling period is $T_c$. To establish the MPC optimization model, necessary constraints should be satisfied.

Joint angles and velocities of the underwater manipulators are strictly limited as

$$\begin{array}{l}{q}_{l,i}^{\min }\le q_{l,i}\le q_{l,i}^{\max },q_{r,i}^{\min }\le q_{r,i}\le q_{r,i}^{\max }\\ \max ({\dot{q}}_{l,i}^{\min },{\epsilon }_v(q_{l,i}^{\min }-q_{l,i}))\le {\dot{q}}_{l,i}\le \min ({\epsilon }_v({\dot{q}}_{l,i}^{\max },q_{l,i}^{\max }-q_{l,i}))\\ \max ({\dot{q}}_{r,i}^{\min },{\epsilon }_v(q_{r,i}^{\min }-q_{r,i}))\le {\dot{q}}_{r,i}\le \min ({\epsilon }_v({\dot{q}}_{r,i}^{\max },q_{r,i}^{\max }-q_{r,i}))\\ i=1~6\end{array},$$

(17)

where $q_{l,i}^{\min }$ and $q_{l,i}^{\max }$ are the minimum and maximum angle values of the i-th joint of the left manipulator while the right one has the same definitions. Similarly, ${\dot{q}}_{l,i}$ and ${\dot{q}}_{r,i}$ are the joints velocities with their limits represented by the current joint angles, where ${\epsilon }_v$ is a positive scale parameter and ${\dot{q}}_{l/r,i}^{\min }/{\dot{q}}_{l/r,i}^{\max }$ is the physical velocity limit. These conditions can be noted as

$$q\in {\Omega }_q,\dot{q}\in {\Omega }_{\dot{q}}.$$

(18)

Velocities of the vehicle are also bounded as $|v_1(i)|\le v_1^{\max }(i),|v_2(i)|\le v_2^{\max }(i),i=1~3$ due to the thruster limits and the water resistance. Combining them with the joint velocity limits, we obtain

$$\zeta \in U_{\zeta }.$$

(19)

The obstacle approaching velocity is provided in Section 2.2 and the collision avoidance constraints can be written as

$$v_{dis}\in {\Omega }_{oa}=\left\{v_{dis}\right|v_{dis}(i)\le v_{\lim }(i)\},i=1~18.$$

(20)

Then, the model predictive object tracking kinematic control is formulated as: Given the UVDMS object transport model in Equation (14), find the optimized kinematic inputs by solving the optimization problem

$$\underset{U_{\zeta }(k)}{\min }J(k,{\eta }_o(k),U(k)),$$

(21)

with

$$\begin{array}{l}J(k,{\eta }_o(k),U(k))={\displaystyle \sum _{j=1}^{N_p}{({\eta }_{od}(j,k)-{\overline{\eta }}_o(j,k))}^T{Q}_{\eta }}({\eta }_{od}(j,k)-{\overline{\eta }}_o(j,k))\\ +{\displaystyle \sum _{j=1}^{N_c-1}{\overline{\zeta }}^T(j,k)}{R}_{\zeta }\overline{\zeta }(j,k)\end{array},$$

(22)

subject to

$$\begin{array}{l}{\overline{\eta }}_o(j,k+1)=f_o({\overline{\eta }}_o(j,k),\overline{\zeta }(j,k),\overline{\eta }(j,k),\overline{q}(j,k))\\ \overline{q}(j,k+1)=\overline{q}(j,k)+T_c\overline{\dot{q}}(j,k)\\ {\overline{\eta }}_1(j,k+1)={\overline{\eta }}_1(j,k)+T_c{\overline{v}}_1(j,k)\\ {\overline{\eta }}_2(j,k+1)={\overline{\eta }}_2(j,k)+T_c{J}_{k,o}^{-1}{\overline{v}}_2(j,k)\\ \overline{q}(j,k)\in {\Omega }_q\\ J_{dis}\overline{\zeta }(j,k)\in {\Omega }_{oa}\\ \overline{\zeta }(j,k)\in U_{\zeta }\end{array},$$

(23)

where $J(k,{\eta }_o(k),U(k))$ is the cost function of the object tracking error and candidate velocity inputs $U(k)=[\overline{\zeta }(0,k),\cdots ,\overline{\zeta }(N_c-1,k)]\in {\Re }^{18\times N_c}$. ${\overline{\eta }}_o(j,k),\overline{\eta }(j,k),$ and $\overline{q}(j,k)$ are the predicted the pose of the object, pose of the vehicle, and joint angles. ${\overline{v}}_{dis}(j,k)$ is the predicted value of the obstacle distance velocity. The prediction and control horizons are $N_p$ and $N_c$ with $N_c\le N_p$. The positive defined $Q_{\eta }$ and $R_{\zeta }$ are weighting matrices.

Once the optimization problem of Equation (21) is solved with the input sequence $U^{\ast }(k)=[{\overline{\zeta }}^{\ast }(0,k),\cdots ,{\overline{\zeta }}^{\ast }(N_c-1,k)]$ at time $k$, the first element ${\overline{\zeta }}^{\ast }(0,k)$ will be applied to the tracking control as the command velocity ${\zeta }_{cmd}$.

3.2. Dynamic Inversion and Neural Network-Based Control

The control law of the dynamic system in (14) is designed as

$$\tau ={\overline{M}}_{w}a+{\overline{H}}_w\zeta +{\overline{g}}_w,$$

(24)

with the virtual control $a$ as the expected acceleration of the system. ${\overline{M}}_w,{\overline{H}}_w,$ and ${\overline{g}}_w$ are the estimates of $M_w,H_w,$ and $g_w$. The control equation can be written as

$$a={{\overline{M}}_w}^{-1}\tau -{\overline{H}}_w\zeta -\overline{g}=\overline{f}(\tau ,\zeta ,q,{\eta }_2).$$

(25)

Similarly, the origin dynamic system can be denoted as $\dot{\zeta }=f(\tau ,\zeta ,q,{\eta }_2)$ with $\Delta (\tau ,\zeta ,q,{\eta }_2)=f(\tau ,\zeta ,q,{\eta }_2)-\overline{f}(\tau ,\zeta ,q,{\eta }_2)$ as the modeling error. Then, we obtain

$$\dot{\zeta }=a+\Delta (\tau ,\zeta ,q,{\eta }_2).$$

(26)

A one-order inertial system $a_{rm}=K_{rm}({\zeta }_{cmd}-{\zeta }_{rm})$ ($K_{rm}$ is a positive diagonal matrix) is designed as the reference model (having the initial conditions as the origin system) to track the command velocity. Then, the tracking error of the reference velocity ${\zeta }_{rm}$ is defined as follows.

$$e=\left[\begin{array}{c}{\displaystyle {\int }_0^t({\zeta }_{rm}-\zeta )d_{\tau }}\\ {\zeta }_{rm}-\zeta \end{array}\right].$$

(27)

By defining the virtual control as $a=a_{rm}+a_{lc}+a_{nn}$ the error system is formulated as

$$\begin{array}{l}\dot{e}=\left[\begin{array}{c}{\zeta }_{rm}-\zeta \\ a_{rm}-\dot{\zeta }\end{array}\right]=\left[\begin{array}{cc}\mathbf{0}& I\\ -Ki& -Kp\end{array}\right]e+\left[\begin{array}{c}\mathbf{0}\\ I\end{array}\right](a_{nn}-\Delta (\tau ,\zeta ,q,{\eta }_2))\\ =\Lambda e+B(a_{nn}-\Delta (\tau ,\zeta ,q,{\eta }_2))\end{array},$$

(28)

where $a_{lc}$ is a PI controller with $Kp$ and $Ki$ as the control parameters and $a_{nn}$ is output of the neural network to be designed later.

The RBF neural network $a_{nn}={\widehat{W}}^{T}h(x)$ is used to compensate the model uncertainties for its arbitrary approximation ability with $x={[\tau ,\zeta ,q,{\eta }_2]}^T$. $\widehat{W}$ and $h(x)$ are the estimated weighting matrix and Gauss function with the adaptive law as

$$\dot{\widehat{W}}=-\kappa e^{T}PBh(x).$$

(29)

3.3. Stability Analysis

It is assumed that an ideal weighting matrix $W^{\ast }$ exists so that the system uncertainties can be approximated as

$$\Delta =W^{\ast T}h(x)-\epsilon ,$$

(30)

where $\epsilon$ is the approximation error. The ideal network can be represented by $W^{\ast }$ and $\tilde{W}=\widehat{W}-W^{\ast }$ as

$$\Delta =(\widehat{W}-\tilde{W}{)}^{T}h(x)-\epsilon =a_{nn}-{\tilde{W}}^{T}h(x)-\epsilon .$$

(31)

Substituting (31) into (28), we obtain

$$\dot{e}=\Lambda e+B({\tilde{W}}^{T}h(x)+\epsilon ).$$

(32)

A Lyapunov candidate can be defined as

$$V=\frac{1}{2}{e}^{T}Pe+\frac{1}{2\kappa }{\tilde{W}}^T\tilde{W},$$

(33)

where $P>0$ is the solution of ${\Lambda }^{T}P+P\Lambda =-Q,Q>0$. $V$ can be derived as

$$\begin{array}{l}\dot{V}=\frac{1}{2}{\dot{e}}^{T}Pe+\frac{1}{2}{e}^{T}P\dot{e}+\frac{1}{\kappa }{\tilde{W}}^T\dot{\widehat{W}}\\ =\frac{1}{2}{e}^T({\Lambda }^{T}P+P\Lambda )e+\frac{1}{2}{(B({\tilde{W}}^{T}h(x)+\epsilon ))}^{T}Pe+\frac{1}{2}{e}^{T}PB({\tilde{W}}^{T}h(x)+\epsilon )+\frac{1}{\kappa }{\tilde{W}}^T\dot{\widehat{W}}\\ =-\frac{1}{2}{e}^T(Q)e+e^{T}PB({\tilde{W}}^{T}h(x)+\epsilon )+\frac{1}{\kappa }{\tilde{W}}^T\dot{\widehat{W}}\\ =-\frac{1}{2}{e}^T(Q)e+e^{T}PB{\tilde{W}}^{T}h(x)+e^{T}PB\epsilon +\frac{1}{\kappa }{\tilde{W}}^T\dot{\widehat{W}}\\ =-\frac{1}{2}{e}^T(Q)e+e^{T}PB\epsilon +{\tilde{W}}^T{e}^{T}PBh(x)+\frac{1}{\kappa }{\tilde{W}}^T\dot{\widehat{W}}\\ =-\frac{1}{2}{e}^T(Q)e+e^{T}PB\epsilon +{\tilde{W}}^T(e^{T}PBh(x)+\frac{1}{\kappa }\dot{\widehat{W}})\\ \le -\frac{1}{2}{\lambda }_{\min }(Q){\Vert e\Vert }^2+{\lambda }_{\max }(P)\Vert e\Vert \epsilon \\ =-\frac{1}{2}\Vert e\Vert ({\lambda }_{\min }(Q)\Vert e\Vert -2{\lambda }_{\max }(P)\epsilon )\end{array}.$$

(34)

It can be seen that when $\Vert e\Vert \ge \frac{2{\lambda }_{\max }(P){\epsilon }_{\max }}{{\lambda }_{\min }(Q)}$, $\dot{V}\le 0$, where $\left|\epsilon \right|\le {\epsilon }_{\max }$, ${\lambda }_{\max }(P)$ and ${\lambda }_{\min }(Q)$ are the maximum and minimum eigenvalues of $P$ and $Q$. $\dot{V}=0$ if and only if $\Vert e\Vert =\frac{2{\lambda }_{\max }(P){\epsilon }_{\max }}{{\lambda }_{\min }(Q)}$, so $\Vert e\Vert \to \frac{2{\lambda }_{\max }(P){\epsilon }_{\max }}{{\lambda }_{\min }(Q)}$ when $t\to \infty$, the convergence rate depends on ${\lambda }_{\min }(Q)$. $\tilde{W}$ is bounded since $V$ is bounded; however, its convergence cannot be guaranteed due to the adaptive mechanism.

4. Simulation Experiments

In this section, the 3D UVDMS simulation platform is developed using Creo 10.0 (designing the 3D model and calculating part dynamic parameters), Matlab R2023b (modeling and control), and Unity 2022 (visualization) to evaluate the performance of the proposed control strategy. The necessary dynamic model parameters, UUV geometry parameters (shown in

Table 1. Parameters of the UUV.

Variable	Value	Unit	Description
$L,W,H$	0.92, 0.62, 0.56	m	Length, width, height
$m$	106	kg	Weight
$B$	1058	N	Buoyancy
$r_B$	${[0,0,0.1]}^T$	m	Center of buoyancy
$r_G$	${[0,0,-0.1]}^T$	m	Center of gravity
$T_B^r$	${[0.2,0.2,0.1]}^T$	m	Transform to base-r
$T_B^l$	${[-0.2,0.2,0.1]}^T$	m	Transform to base-l

), and DH parameters of the manipulator (shown in

Table 2. DH parameters of the underwater manipulator.

i	a	Alpha	d	$\mathit{\theta }$	Length/Weight
0	0	90	0	0	0.116 m/2.8 kg
1	0	−90	0.193 m	q1	0.077 m/1.3 kg
2	0	90	0	q2	0.0805 m/1.2 kg
3	0	−90	0.2805 m	q3	0.2 m/1.6 kg
4	0	90	0	q4	0.0805 m/1.2 kg
5	0	−90	0.2805 m	q5	0.2 m/1.6 kg
6	0	90	0	q6	0.19/1.5 kg

) are listed as follows.

$$\begin{array}{l}{M}_{RB}=diag\{106,106,106,9.5,10.2,12\}\\ M_{AM}=diag\{63,32.2,31.3,5.6,3.7,3.4\}\\ D(v)=diag\{110+90|v|,90+90|v|,140+110|v|,20+8|v|,13+10|v|,17+12|v\left|\right\}\end{array}$$

4.1. Collision Avoidance

In this section, the MPC for collision avoidance is tested by tracking certain trajectories. The initial conditions of the vehicle and joint angles are ${[2,0,3,0,0,0]}^T$ and ${[{10}^{\circ },{50}^{\circ },-{15}^{\circ },{50}^{\circ },{15}^{\circ },-{10}^{\circ },{20}^{\circ },{60}^{\circ },-{20}^{\circ },{55}^{\circ },{20}^{\circ },-{25}^{\circ }]}^T$. The first case is to track two circles by the end effectors with centers of ${[2.4,-0.25,3.08]}^T$ and ${[2.4,0.25,3.08]}^T$ and radius of 0.18 m. Circle centers and radius of the second case are ${[2.5,-0.2,3.1]}^T$, ${[2.5,0.2,3.1]}^T$, and 0.14 m. Figure 6 shows the screenshots of the moments that collisions occur during the trajectory tracking, with Case 1 in the upper left image and Case 2 in the lower left one. It can be seen that the 3rd and 5th links of the right manipulator have touched the right frame and the 3rd link of the left manipulator, respectively. The same moments with collision avoidance control in the right images show better performances in the same areas. It is shown in Figure 7 and Figure 8 that the proposed control can track the desired trajectories successfully under the influence of collision avoidance operation, although there is a slight decrease in performance compared to the MPC without collision avoidance constraints. The minimum distances for link 3 and link 5 of the right manipulator in Figure 9 are taken for further study while other links are ignored since their distances to obstacles are mostly safe all the time. In both cases, thresholds of the collision zone and anti-collision zone are 0.05 m (the link radius is considered) and 0.12 m. The comparison between the MPC and CAMPC indicates that the proposed constraints prevent the minimum distance curves from passing through the collision threshold so that the UVDMS succeeds in avoiding frame and mutual collisions.

4.2. Object Transport

In this section, a task in which the UVDMS transports a cylinder object (after being grasped) using the proposed control strategy is simulated. Figure 10 shows the UVDMS movement from the start point ${[2,1,30,0,0,0]}^T$ to end point ${[3,0.5,28,0,0,{90}^{\circ }]}^T$ following a reference trajectory, which is generated by the polynomial trajectory module in Matlab R2023b. The initial joint configuration is ${[0,0,0,0,0,0,0,0,0,0,0,0]}^T$, which means the vehicle position is also fixed. The length and radius of the cylinder are 0.6 m and 0.05 m with a uniformly distributed mass and a density of 1.1 g/cm³. The green, purple, and red lines represent the reference trajectory, object trajectory, and vehicle trajectories, respectively. It can be seen that the proposed control method shows a good performance in tracking certain trajectories.

States of positions are shown in Figure 11 and Figure 12. The poses and Euler angles in Figure 11 record both trajectories (traveling from the start point to the endpoint through several waypoints and being stable finally) of the object and the reference. In this case, the pitch and roll angles of the vehicle are controlled to zero. The pose of the vehicle is in Figure 12 and Figure 13 provides the joint angles of the manipulators. It should be noted that both manipulators have the same joint angles because their end effectors are fixed with the objective, which reduces the degrees of freedom.

Velocities in Figure 14 and Figure 15a are all within the system limits and stable at zero finally. Similarly, to the joint angles, the joint velocities of both manipulators are the same due to reduced degrees of freedom. Figure 15b and Figure 16a provide input torques for the vehicle and manipulators. It can be seen that it takes more energy to obtain equilibrium in roll and pitch control. Moreover, as long as the manipulators’ configurations are not stable, the actuators continue to work. Figure 16b shows the signals to compensate for the model uncertainties, which are generated by the RBF neural networks with 80 hidden nodes.

5. Conclusions

A model predictive control for collision avoidance and grasped object transport of UVDMSs is developed in this paper. Collisions caused by the outer frames and the dual manipulators are considered distance velocity constraints, where a minimum link distances algorithm is proposed to calculate the distance to the obstacle of each link. The kinematic tracking based on MPC generates command velocity for the dynamic control, which is built using the model reference dynamic inversion structure. Uncertainties of the UVDMS-object model are compensated by the neural network for better performance. The stability of the velocity tracking control is analyzed by the Lyapunov theory and the error is ultimately bounded. In addition, simulations of an 18-dof UVDMS in different scenes show good performance of the control strategy for avoiding collisions and object transport.

The obstacles considered in this work are vehicle frames and mutual manipulator links with few perceptions of the environment. Outer obstacles, especially the non-static ones, are another challenge for UVDMSs. In addition, the proposed control scheme deals with objects that are fixed with the end effectors which reduces the degrees of freedom and ability of collaboration. Therefore, outer obstacle avoidance and cooperative control will be the focus of our future work.