Decentralized Impedance Control for Cooperative Manipulation of Multiple Underwater Vehicle Manipulator Systems under Lean Communication

TL;DR

This paper introduces a decentralized impedance control approach for cooperative underwater vehicle manipulators, enabling obstacle avoidance and load sharing without explicit communication, verified through simulations.

Contribution

It proposes a novel distributed leader-follower architecture with implicit communication and a prescribed performance estimation law for cooperative underwater manipulation.

Findings

Effective obstacle avoidance demonstrated in simulations

Load sharing aligns with payload capabilities

No explicit communication required among robots

Abstract

This paper addresses the problem of cooperative object transportation for multiple Underwater Vehicle Manipulator Systems (UVMSs) in a constrained workspace with static obstacles, where the coordination relies solely on implicit communication arising from the physical interaction of the robots with the commonly grasped object. We propose a novel distributed leader-follower architecture, where the leading UVMS, which has knowledge of the object's desired trajectory, tries to achieve the desired tracking behavior via an impedance control law, navigating in this way, the overall formation towards the goal configuration while avoiding collisions with the obstacles. On the other hand, the following UVMSs estimate the object's desired trajectory via a novel prescribed performance estimation law and implement a similar impedance control law. The feedback relies on each UVMS's force/torque measurements and no explicit data is exchanged online among the robots. Moreover, the control scheme adopts load sharing among the UVMSs according to their specific payload capabilities. Finally, various simulation studies clarify the proposed method and verify its efficiency.

Equations

$${\mathbf{R}}_i=\left[\begin{array}{ccc}{c}_{{\psi }_i}{c}_{{\theta }_i}& c_{{\psi }_i}{s}_{{\theta }_i}{s}_{{\varphi }_i}-s_{{\psi }_i}{c}_{{\varphi }_i}& c_{{\psi }_i}{s}_{{\theta }_i}{c}_{{\varphi }_i}+s_{{\psi }_i}{s}_{{\varphi }_i}\\ s_{{\psi }_i}{c}_{{\theta }_i}& s_{{\psi }_i}{s}_{{\theta }_i}{s}_{{\varphi }_i}+c_{{\psi }_i}{c}_{{\varphi }_i}& s_{{\psi }_i}{s}_{{\theta }_i}{c}_{{\varphi }_i}-c_{{\psi }_i}{s}_{{\varphi }_i}\\ -s_{{\theta }_i}& c_{{\theta }_i}{s}_{{\varphi }_i}& c_{{\theta }_i}{c}_{{\varphi }_i}\end{array}\right]$$

$$\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}{\mathbf{v}}_i={\mathbf{J}}_i({\mathbf{q}}_i){\mathbf{\zeta }}_i,\text{leavevmode}i\in \mathcal{K}$$

$${\mathbf{v}}_i={\mathbf{J}}_{iO}{\mathbf{v}}_O,\text{leavevmode}i\in \mathcal{K}\text{vspace}-2mm$$

$$\displaystyle\bm{J}_{iO}=\left[\begin{array}[]{ccc}\bm{I}_{3\times 3}&-\bm{S}(\bm{l}_{i})\\ \bm{0}_{3\times 3}&\bm{I}_{3\times 3}\end{array}\right]\in\mathbb{R}^{6\times 6},\leavevmode\nobreak\ \leavevmode\nobreak\ i\in\mathcal{K}$$

$${\dot{\mathbf{v}}}_i={\mathbf{J}}_{iO}{\dot{\mathbf{v}}}_O+{\dot{\mathbf{J}}}_{iO}{\mathbf{v}}_O,\text{leavevmode}i\in \mathcal{K}$$

$${\mathbf{M}}_{q_i}({\mathbf{q}}_i){\dot{\mathbf{\zeta }}}_i+{\mathbf{C}}_{q_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i)$$

$$+{\mathbf{d}}_{q_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t)={\mathbf{\tau }}_i+{{\mathbf{J}}_i}^{\top }{\mathbf{\lambda }}_i$$

$$\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}{\dot{\mathbf{v}}}_i={\mathbf{J}}_i({\mathbf{q}}_i){\dot{\mathbf{\zeta }}}_i+{\mathbf{J}}_i^d({\mathbf{\zeta }}_i,{\mathbf{q}}_i){\mathbf{\zeta }}_i,\text{leavevmode}i\in \mathcal{K}$$

$${\mathbf{M}}_{v_i}({\mathbf{q}}_i){\dot{\mathbf{v}}}_i+{\mathbf{C}}_{v_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i){\mathbf{v}}_i+$$

$${\mathbf{d}}_{v_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t)={\mathbf{u}}_i+{\mathbf{\lambda }}_i$$

$${\mathbf{M}}_i({\mathbf{q}}_i){\dot{\mathbf{v}}}_O+{\mathbf{C}}_i({\mathbf{\zeta }}_i,{\mathbf{q}}_i){\mathbf{v}}_O+{\mathbf{D}}_i({\mathbf{\zeta }}_i,$$

$$={\mathbf{J}}_{iO}^{\top }{\mathbf{u}}_i+{\mathbf{J}}_{iO}^{\top }{\mathbf{\lambda }}_i$$

$${\mathbf{M}}_i({\mathbf{q}}_i)={\mathbf{J}}_{iO}^{\top }{\mathbf{M}}_{v_i}({\mathbf{q}}_i){\mathbf{J}}_{iO}$$

$$\displaystyle\bm{{C}}_{i}({\bm{\zeta}}_{i},\bm{q}_{i})=\bm{J}_{iO}^{\top}\big{[}\bm{{C}}_{v_{i}}({\bm{\zeta}}_{i},\bm{q}_{i})\bm{J}_{iO}+\bm{{M}}_{v_{i}}(\bm{q}_{i})\dot{\bm{J}}_{iO}\big{]}$$

$${\mathbf{D}}_i({\mathbf{\zeta }}_i,{\mathbf{q}}_i)={\mathbf{J}}_{iO}^{\top }{\mathbf{D}}_{v_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i){\mathbf{J}}_{iO}$$

$${\mathbf{g}}_i({\mathbf{q}}_i)={\mathbf{J}}_{iO}^{\top }{\mathbf{g}}_{v_i}({\mathbf{q}}_i),\text{leavevmode}{\mathbf{d}}_i({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t)={\mathbf{J}}_{iO}^{\top }{\mathbf{d}}_{v_i}({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t)$$

$$\mathbf{M}{}_i({\mathbf{a}}_i){\mathbf{d}}_i+\mathbf{C}{}_i({\mathbf{a}}_i,{\mathbf{b}}_i){\mathbf{c}}_i+\mathbf{D}{}_i({\mathbf{a}}_i,{\mathbf{b}}_i){\mathbf{c}}_i+\mathbf{g}{}_i({\mathbf{a}}_i)=\mathbf{\Omega }{}_i({\mathbf{a}}_i,{\mathbf{b}}_i,{\mathbf{c}}_i,{\mathbf{d}}_i){\mathbf{\theta }}_i,\text{leavevmode}i\in \mathcal{K}$$

$${\mathbf{d}}_i({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t)={\mathbf{\Delta }}_i({\mathbf{\zeta }}_i,{\mathbf{q}}_i,t){\mathbf{\theta }}_{d,i},\text{leavevmode}i\in \mathcal{K}.$$

$${\dot{\mathbf{x}}}_O={\mathbf{J}}_O({\mathbf{\eta }}_{2,O}){\mathbf{v}}_O$$

$$\mathbf{M}{}_O({\mathbf{x}}_O){\dot{\mathbf{v}}}_O+\mathbf{C}{}_O({\dot{\mathbf{x}}}_O,{\mathbf{x}}_O){\mathbf{v}}_O+\mathbf{D}{}_O({\dot{\mathbf{x}}}_O,{\mathbf{x}}_O){\mathbf{v}}_O+\mathbf{g}{}_O=\mathbf{\lambda }{}_O+\mathbf{\lambda }{}_e$$

$${\mathbf{J}}_O^{\prime }({\mathbf{\eta }}_{2,O})=\left[\begin{array}{ccc}1& \sin ({\varphi }_O)\tan ({\theta }_O)& cos({\varphi }_O)\tan ({\theta }_O)\\ 0& \cos ({\varphi }_O)& -\sin ({\theta }_O)\\ 0& \frac{\sin ({\varphi }_O)}{\cos ({\theta }_O)}& \frac{cos({\varphi }_O)}{\cos ({\theta }_O)}\end{array}\right].$$

$${\mathbf{\lambda }}_O={\mathbf{G}}^{\top }\mathbf{\lambda }$$

$$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \bm{G}\!=\!\Big{[}[\bm{J}_{LO}]^{\top}\!\!,[\bm{J}_{F_{1}O}]^{\top}\!\!,\ldots,[\bm{J}_{F_{N}O}]^{\top}\!\Big{]}^{\top}\!\!\!\in\mathbb{R}^{6(N+1)\times 6}$$

$$\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}{\varphi }_O({\mathbf{x}}_O;{\mathbf{x}}_O^d)=\frac{\gamma ({\mathbf{x}}_O-{\mathbf{x}}_O^d)}{[{\gamma }^k({\mathbf{x}}_O-{\mathbf{x}}_O^d)+\beta ({\mathbf{x}}_O){]}^{\frac{1}{k}}}$$

$$\lim\limits_{\bm{x}_{O}\rightarrow\Bigg{\{}\begin{array}[]{ccc}\text{Boundary}\\ \text{Obstacles}\end{array}}\beta(\bm{x}_{O})=0$$

$$\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}\text{leavevmode}{\mathbf{v}}_{O_L}^d(t)=-K_{NF}{\mathbf{J}}_O^{-1}({\mathbf{\eta }}_{2,O}){\nabla }_{{\mathbf{x}}_O}{\varphi }_O({\mathbf{x}}_{O_L}(t),{\mathbf{x}}_O^d)$$

$$c_i=\frac{1}{N+1},\text{leavevmode}i\in \mathcal{K}.$$

$$\displaystyle\sum_{i\in\mathcal{K}}\!\!\big{\{}\!\bm{M}_{O_{i}}\!(\bm{x}_{O})\dot{\bm{v}}_{O}\!+\!\bm{C}_{O_{i}}\!(\dot{\bm{x}}_{O},\bm{x}_{O})\bm{v}_{O}\!+\!\bm{D}_{O_{i}}\!(\dot{\bm{x}}_{O},\bm{x}_{O})\bm{v}_{O}\!+\!\bm{g}_{O_{i}}\!\big{\}}$$

$$=\sum _{i\in \mathcal{K}}\mathbf{J}{}_{iO}^{\top }{\mathbf{\lambda }}_i+\sum _{i\in \mathcal{K}}{\mathbf{\lambda }}_{e_i}$$

$$\displaystyle\!\!\bm{\zeta}_{i}(t)\!=\!\bm{K}\!_{\mu}\Bigg{(}\!\!\bm{\mu}_{i}(t)\!+\!\!\int_{t_{0}}^{t}\!\!\!\Big{(}\!\bm{C}_{O_{i}}\!\bm{v}_{O}\!+\!\bm{D}_{O_{i}}\!\bm{v}_{O}\!+\!\bm{g}_{O_{i}}\!-\!\bm{\zeta}_{i}(d\tau)\Big{)}d\tau\!\Bigg{)}\!\!\!$$

$$\displaystyle\dot{\bm{\zeta}}_{i}(t)\!=-\!\bm{K}\!_{\mu}\bm{\zeta}_{i}(t)+c_{i}\bm{K}\!_{\mu}\Big{(}\sum_{i\in\mathcal{K}}\!\bm{J}\!_{iO}^{\top}\bm{\lambda}_{i}+\bm{\lambda}_{e}\Big{)}$$

$$\displaystyle\bm{\zeta}_{i}(t)\!\approx c_{i}\Big{(}\sum_{i\in\mathcal{K}}\!\bm{J}\!_{iO}^{\top}\bm{\lambda}_{i}+\bm{\lambda}_{e}\Big{)}$$

$${\mathbf{\lambda }}_{e_i}\approx {\mathbf{\zeta }}_i(t)-\mathbf{J}{}_{iO}^{\top }{\mathbf{\lambda }}_i,\text{leavevmode}i\in \mathcal{K}.$$

$${\mathbf{\lambda }}_i^d={\mathbf{\lambda }}^d{}_{int,i}-{\mathbf{J}}_{iO}^{-\top }(\mathbf{M}{}_{O_i}{\mathbf{y}}_i^{cmd}+\mathbf{C}{}_{O_i}{\mathbf{v}}_O+\mathbf{D}{}_{O_i}{\mathbf{v}}_O+{\mathbf{g}}_{O_i}-{\mathbf{\lambda }}_{e_i})$$

Full text

I Introduction

During the last decades, Unmanned Underwater Vehicles (UUVs) have been widely used in various applications such as marine science (e.g., biology, oceanography, archeology) and offshore industry (e.g., ship maintenance, inspection of oil/gas facilities) [1, 2, 3]. In particular, a vast number of the aforementioned applications, demand the underwater vehicle to be enhanced with intervention capabilities as well [4], thus raising increasing interest on Underwater Vehicle Manipulator System (UVMS). For instance, some recent European projects: TRIDENT [5, 6], PANDORA [7, 8], and the most recent one DexROV [9], have boosted significantly the autonomous underwater interaction tasks.

Most of the underwater manipulation tasks can be carried out more efficiently, if multiple UVMSs are cooperatively involved (see Fig 1). In general, underwater tasks are very demanding, with the most significant challenge being imposed by the strict communication constraints [10]. In general, the communication of multi-robot systems can be classified in two major categories, namely explicit (e.g., conveying information such as sensory data directly to other robots) and implicit (e.g., the interaction forces between the object and the robot). Even though the inter-robot communication is of utmost importance during cooperative manipulation tasks, employing explicit communication in underwater environment may result in severe performance problems owing to the limited bandwidth and update rate of underwater acoustic devices. Moreover, as the number of cooperating robots increases, communication protocols require complex design to deal with the crowed bandwidth [11].

Cooperative manipulation has been well-studied in the literature, especially the centralized schemes [12]. Despite its efficiency, centralized control is less robust, since all units rely on a central system. On the other hand, the decentralized cooperative manipulation schemes depend usually on either explicit communication interchange among the robots (e.g., online transmission of the desired trajectory [13] or off–line knowledge of the objects’ trajectory [14]). This demands an accurate common global localization system for all participating robots [15], which is difficult to be achieved in underwater environment. Therefore, the design of decentralized cooperative manipulation algorithms for underwater tasks employing implicit and lean explicit communication becomes apparent. In recent studies [16], potential fields methods were employed to manage the guidance of UVMSs and the manipulation tasks. Compelling results towards the same direction have been given in [17, 18]. In particular, a three-fold decentralized cooperative control strategy is proposed where initially, each robot individually finds out an optimal task space control velocity, which is transfered afterwards among the robots in order to obtain a commonly agreed velocity via a fusion policy. However, employing the aforementioned strategies, requires each robot to communicate with the whole robot team, which consequently restricts the number of robots involved in the cooperative manipulation task owing to bandwidth limitations.

In this work, the problem of decentralized cooperative object transportation considering multiple UVMSs in a constrained workspace with static obstacles is addressed. The challenge lays in replacing explicit communication with implicit, by incorporating feedback that results from the physical interaction of the robots with the commonly grasped object (i.e., we assume that each UVMS is equipped with a force/torque sensor attached on its end-effector). The proposed scheme is based on a leader-follower architecture, where the leader, which is aware of the object’s desired trajectory, implements it via an impedance control law. On the other hand, the followers estimate the desired trajectory in a distributed way, via observing the object motion, and impose a similar impedance law. All impedance laws linearize the dynamics and incorporate coefficients for load sharing. The estimation process is based on the prescribed performance methodology [19] that drives the estimation error to an arbitrarily small residual set. Moreover, we design adaptive control laws in order to compensate for the parametric uncertainty of the UVMSs dynamics as well as the external disturbances. Finally, the proposed scheme exploits information, i.e., force/torque at the end-effector, position and velocity measurements, acquired solely by onboard sensors, avoiding thus any tedious inter-robot explicit communication.

II Problem Formulation

Consider $N+1$ UVMSs rigidly grasping an object in a constrained workspace with static obstacles. We also assume that each UVMS is fully-actuated at its end-effector frame. It should also be noted that in the proposed scheme, only the leading robot is aware of the obstacles’ position in the workspace and the object’s desired configuration $\bm{x}^{d}_{o}$. On the other hand, the followers estimate locally in a distributed way the object’s desired trajectory profile and manipulate the object in coordination with the leader based solely on their own sensory information. Moreover, we assume that the UVMSs can measure their position and velocity (e.g., employing a fusion technique based on measurements by various onboard sensors), as well as the interaction forces/torques with the object via a force/toque sensor. Additionally, the geometric parameters of both the UVMSs and the commonly grasped object are considered known, whereas their dynamic parameters are completely unknown.

II-A Kinematics

We denote the coordinates of the commonly agreed body-fixed frame on the object as well as the leader’s and followers’ task space (i.e., end-effector) coordinates by $\bm{x}_{O}=[\bm{\eta}_{1,O}^{\top},\bm{\eta}_{2,O}^{\top}]^{\top}$, $\bm{x}_{L}=[\bm{\eta}_{1,L}^{\top},\bm{\eta}_{2,L}^{\top}]^{\top}$ and $\bm{x}_{F_{i}}=[\bm{\eta}_{1,F_{i}}^{\top},\bm{\eta}_{2,F_{i}}^{\top}]^{\top},\leavevmode\nobreak\ i\in\mathcal{N}=\{1,\ldots,N\}$ respectively. More specifically, $\bm{\eta}_{1,i}=[x_{i},y_{i},z_{i}]^{\top}$ and $\bm{\eta}_{2,i}=[\phi_{i},\theta_{i},\psi_{i}]^{\top},\leavevmode\nobreak\ i\in\{O,L,F_{1},\ldots,F_{N}\}$ denote the position and the orientation expressed in Euler angles representation with respect to the inertial frame. Alternatively, the orientation coordinates $\bm{\eta}_{2,i},\leavevmode\nobreak\ i\in\{O,L,F_{1},\ldots,F_{N}\}$ expressed in Euler angles may be described by a rotation matrix $\bm{R}_{i}=[\bm{n}_{i},\bm{o}_{i},\bm{\alpha}_{i}]\in\mathbb{R}^{3},\leavevmode\nobreak\ i\in\{O,L,F_{1},\ldots,F_{N}\}$ [20]:

[TABLE]

where $s_{\star}=\sin(\star)$ and $c_{\star}=\cos(\star)$. Let $\bm{q}_{i}=[\bm{q}^{\top}_{v,i},\leavevmode\nobreak\ \bm{q}^{\top}_{m,i}]^{\top}\in\mathbb{R}^{6+n},i\in\mathcal{K}=\{L,F_{1},\ldots,F_{N}\}$ be the joint state variables of each UVMS, where $\bm{q}_{v,i}\in\mathbb{R}^{6}$ is the vector that involves the position and the orientation of the vehicle and $\bm{q}_{m,i}\in\mathbb{R}^{n}$ is the vector of the angular positions of the manipulator’s joints. Let also define the object as well as the leader’s and followers’ end effector generalized velocities by ${\bm{v}}_{O}=[\dot{\bm{\eta}}_{1,O}^{\top},\bm{\omega}_{O}^{\top}]^{\top}$, ${\bm{v}}_{L}=[\dot{\bm{\eta}}_{1,L}^{\top},\bm{\omega}_{L}^{\top}]^{\top}$ and ${\bm{v}}_{i}=[\dot{\bm{\eta}}_{1,i}^{\top},\bm{\omega}_{i}^{\top}]^{\top},\leavevmode\nobreak\ i\in\{F_{1},\ldots,F_{N}\}$, where $\dot{\bm{\eta}}_{1,i}$ and $\bm{\omega}_{i}$ denote the linear and angular velocity respectively. Without any loss of generality, for the augmented UVMS system we get [21]:

[TABLE]

where $\bm{\zeta}_{i}=[\bm{\upsilon}_{i}^{\top},\dot{\bm{q}}_{m,i}^{\top}]^{\top}\in\mathbb{R}^{6+n}$ is the velocity vector involving the body velocities of the vehicle as well as the joint velocities of the manipulator with $\bm{\upsilon}_{i}$ denoting the velocity of the vehicle expressed in the body-fixed frame and $\bm{J}_{i}(\bm{q}_{i})$ denoting the geometric Jacobian matrix [21]. Furthermore, owing to the rigid grasp of the object and since the object geometric parameters are considered known, each UVMS can compute the object’s position w.r.t the inertial frame $\{I\}$. Furthermore, along with the fact that $\bm{\omega}_{i}=\bm{\omega}_{O},\leavevmode\nobreak\ i\in\mathcal{K}$, one obtains:

[TABLE]

where $\bm{J}_{iO},\leavevmode\nobreak\ i\in\mathcal{K}$ denotes the Jacobian from the end-effector of each UVMS to the object’s center of mass, that is defined as:

[TABLE]

where $\bm{S}(\bm{l}_{i})$ is the skew-symmetric matrix of the constant relative position of the end-effector w.r.t the object $\bm{l}_{i}=[l_{ix},l_{iy},l_{iz}]^{\top}$. Notice that $\bm{J}_{iO},\leavevmode\nobreak\ i\in\mathcal{K}$ are always full-rank owing to the grasp rigidity and hence obtain a well defined inverse. Thus, the object’s velocity can be easily computed via the inverse of (3). Moreover, from (3), one obtains:

[TABLE]

II-B Dynamics

UVMS Dynamics: The dynamics of a UVMS after straightforward algebraic manipulations can be written as [21]:

[TABLE]

for $i\in\mathcal{K}$, where $\bm{\lambda}_{i}$ is the vector of measured interaction forces and torques exerted at the end-effector by the object, $\bm{\tau}_{i}$ denotes the vector of control inputs (forces and torques), $\bm{{M}}_{q_{i}}(\bm{q}_{i})$ is the inertial matrix, $\bm{{C}}_{q_{i}}({\bm{\zeta}}_{i},\bm{q}_{i})$ represents coriolis and centrifugal terms, $\bm{{D}}_{q_{i}}({\bm{\zeta}}_{i},\bm{q}_{i})$ models dissipative effects, $\bm{{g}}_{i}(\bm{q}_{i})$ encapsulates the gravity and buoyancy effects and $\bm{d}_{q_{i}}(\bm{\zeta}_{i},\bm{q}_{i},t)$ is a bounded vector representing unmodeled friction, uncertainties and external disturbances. In view of (2), we have:

[TABLE]

where $\bm{J}^{d}_{i}(\bm{\zeta}_{i},\bm{q}_{i})\in\mathbb{R}^{6\times(6+n)}$ represents the Jacobian derivative function (i.e., $\bm{J}^{d}_{i}(\bm{\zeta}_{i},\bm{q}_{i})\triangleq\dot{\bm{J}}_{i}(\bm{q}_{i})$). Then, by employing the differential kinematics as well as (6), we obtain the transformed task space dynamics [22]:

[TABLE]

with the corresponding task space terms $\bm{M}_{v_{i}}\in\mathbb{R}^{6\times 6}$, $\bm{C}_{v_{i}}\in\mathbb{R}^{6\times 6}$, $\bm{D}_{v_{i}}\in\mathbb{R}^{6\times 6}$, $\bm{g}_{v_{i}}\in\mathbb{R}^{6}$, $\bm{d}_{v_{i}}\in\mathbb{R}^{6}$ and $\bm{u}_{i}$ that denotes the vector of task space generalized forces/torques. Invoking the kinematic relations (3)-(4), we may express the aforementioned dynamics (7) w.r.t the object’s coordinates:

[TABLE]

where:

[TABLE]

Now, the following common properties will be employed in the analysis.

Property 1

The matrix $\bm{M}_{i}(\bm{q}_{i}),\leavevmode\nobreak\ i\in\mathcal{K}$ is positive definite and the matrix $\dot{\bm{M}}_{i}(\bm{q}_{i})-2\bm{{C}}_{i}({\bm{\zeta}}_{i},\bm{q}_{i}),\leavevmode\nobreak\ i\in\mathcal{K}$ is skew-symmetric.

Property 2

The uncertainty of the UVMS model appears linearly in the dynamics (8) in terms of an unknown but constant parameter vector $\bm{\theta}_{i}\in\mathbb{R}^{q_{i}},\leavevmode\nobreak\ i\in\mathcal{K}$ in the following way[23, 24]:

[TABLE]

where $\bm{\Omega}_{i}(\bm{a}_{i},{\bm{b}}_{i},{\bm{c}}_{i},{\bm{d}}_{i})\in\mathbb{R}^{6\times q_{i}},\leavevmode\nobreak\ i\in\mathcal{K}$ is a regressor matrix of known functions of $\bm{a}_{i},{\bm{b}}_{i},{\bm{c}}_{i},{\bm{d}}_{i}\in\mathbb{R}^{6}$ independent of $\bm{\theta}_{i}$.

Now, we introduce the following assumption regarding the unmodeled dynamics/external disturbances.

Assumption 1

There exist unknown constant vector $\bm{\theta}_{d,i}$ and known bounded functions $\bm{\Delta}_{i}\in\mathbb{R}^{6\times q_{i}}$, such that

[TABLE]

Object Dynamics: Without any loss of generality, we consider the following second order dynamics for the object, which can be derived based on the Newton-Euler formulations:

[TABLE]

where $\bm{M}_{O}(\bm{x}_{O})$ is the positive definite inertia matrix, $\bm{C}_{O}(\dot{\bm{x}}_{O},\bm{x}_{O})$ is the Coriolis matrix, $\bm{g}_{O}$ is the vector of gravity and buoyancy effects, $\bm{D}_{O}(\dot{\bm{x}}_{O},\bm{x}_{O})$ models dissipative effects, $\bm{\lambda}_{O}$ is the vector of generalized forces acting on the object’s center of mass, $\bm{\lambda}_{e}$ is a vector representing uncertainties and external disturbances and ${\bm{J}_{O}({\bm{\eta}}_{2,O})}=\text{diag}\{\bm{I}_{3},{\bm{J}^{\prime}_{O}({\bm{\eta}}_{2,O})}\}$ is the object representation Jacobian with:

[TABLE]

Moreover, The kineto-statics duality along with the grasp rigidity suggest that the force $\bm{\lambda}_{O}$ acting on the object’s center of mass and the generalized forces $\bm{\lambda}_{i},\leavevmode\nobreak\ i\in\mathcal{K}$, exerted by the UVMSs at the grasping points, are related through:

[TABLE]

where:

[TABLE]

is the full column-rank grasp matrix and $\bm{\lambda}=[\bm{\lambda_{L}}^{\top},\bm{\lambda_{F_{1}}}^{\top},\ldots,\bm{\lambda_{F_{N}}}^{\top}]^{\top}$ is the vector of the overall interaction forces and torques.

II-C Description of the Workspace

Consider the team of $N+1$ UVMSs operating in a bounded workspace $\mathcal{W}\subseteq\mathbb{R}^{3}$ with boundary $\partial\mathcal{W}$. Without any loss of the generality, the obstacles, the robots as well as the workspace are all modeled by spheres (i.e., we adopt the spherical world representation [25]). In this spirit, let $\mathcal{B}(\bm{x}_{O},r_{0})$ be a closed ball that covers the volume of the object and has radius $r_{0}$. We also define the closed balls $\mathcal{B}(\bm{p}_{i},\bar{r}),i\in\mathcal{K}$, centered at the end-effector of each UVMS that cover the robot volume for all possible configurations. Furthermore, we define a ball area $\mathcal{B}(\bm{x}_{O},R)$ located at $\bm{x}_{O}$ with radius $R=\bar{r}+r_{o}$ that includes the whole volume of the robotic team and the object. Finally, the $\mathcal{M}$ static obstacles are defined as closed spheres described by $\pi_{m}=\mathcal{B}(\bm{p}_{\pi_{m}},r_{\pi_{m}}),\leavevmode\nobreak\ m\in\{1,\ldots,\mathcal{M}\}$, where $\bm{p}_{\pi_{m}}\in\mathbb{R}^{3}$ is the center and the $r_{\pi_{m}}$ the radius of the obstacle $\pi_{m}$. Additionally, based on the property of spherical world representation [25], each pair of obstacles $m,m^{\prime}\in\{1,\ldots,\mathcal{M}\}$ are disjoint in a such a way that the whole team of UVMSs including the object can pass through the free space between them. Therefore, there exists a feasible trajectory $\bm{x}_{O}(t)$ for the whole team that connects the initial configuration $\bm{x}_{O}(t_{0})$ with $\bm{x}^{d}_{O}$.

III Control Methodology

We assume that the leading UVMS is aware of both the desired configuration of the object as well as of the obstacles position in the workspace. Towards this direction, and based on the property of spherical world representation [25] we assume that there is a safe trajectory within the workspace which is known only to the leader:

III-A Safe Navigation

The desired/feasible object trajectory within the workspace $\mathcal{W}$ can be generated based on the Navigation Functions concept originally proposed by Rimon and Koditschek in [25], as follows:

[TABLE]

where $\phi_{O}:\xrightarrow{\mathcal{W}-\overset{\mathcal{M}}{\underset{m=1}{\cap}}\mathcal{B}(\bm{p}_{\pi_{m}},r_{\pi_{m}})}[0,1)$ denotes the potential that derives a safe motion vector field within the free space $\mathcal{W}-\overset{\mathcal{M}}{\underset{m=1}{\cap}}\mathcal{B}(\bm{p}_{\pi_{m}},r_{\pi_{m}})$. Notice that $k>1$ is a design constant, $\gamma(\bm{x}_{O}-\bm{x}^{d}_{O})>0$ with $\gamma(\bm{0})=0$ represents the attractive potential field to the goal configuration $\bm{x}^{d}_{O}$ and $\beta(\bm{x}_{O})>0$ with:

[TABLE]

represents the repulsive potential field by the workspace boundary and the obstacle regions. In that respect, it was proven in [25] that $\phi_{O}(\bm{x}_{O};\bm{x}^{d}_{O})$ has a global minimum at $\bm{x}^{d}_{O}$ and no other local minima for sufficiently large $k$. Thus, a feasible path that leads from any initial obstacle-free configuration to the desired configuration might be generated by following the negated gradient of $\phi_{O}(\bm{x}_{O};\bm{x}^{d}_{O})$. Consequently, the desired velocity profile at the leader’s side is designed as follows:

[TABLE]

where $K_{NF}>0$ is a positive gain. Given the initial configuration, the leading UVMS may calculate the desired trajectory and velocity profile denoted by $\bm{x}^{d}_{O_{L}}(t)$ and $\bm{v}^{d}_{O_{L}}(t)$ respectively, by propagating the model $\dot{\bm{x}}^{d}_{O_{L}}(t)=\bm{v}^{d}_{O_{L}}(t)$.

III-B Control Design

First, we introduce the load sharing coefficients $c_{i},\leavevmode\nobreak\ i\in\mathcal{K}$ that are subject to the following design constraints: $c_{i}\in(0,1),\forall i\in\mathcal{K}\leavevmode\nobreak\ \text{and}\leavevmode\nobreak\ \sum_{i\in\mathcal{K}}c_{i}=1$. Without any loss of generality and to simplify the analysis we select:

[TABLE]

In view of the object dynamics (9b), it can be concluded that the vector of external disturbances $\bm{\lambda}_{e}$ is unknown. Thus, in order to design the impedance control scheme, each UVMS should estimate the aforementioned vector in a distributed way (since explicit communication among UVMSs is not permitted). Therefore, an online estimation method based on the object momentum concept [26] is given in the sequel. Based on the load coefficients (15) the object dynamics of (9b) can be rewritten as:

[TABLE]

where $\bm{M}_{O_{i}}=c_{i}\bm{M}_{O}$, $\bm{C}_{O_{i}}=c_{i}\bm{C}_{O}$, $\bm{D}_{O_{i}}=c_{i}\bm{D}_{O}$ , $\bm{g}_{O_{i}}=c_{i}\bm{g}_{O}$ and $\bm{\lambda}_{e_{i}}=c_{i}\bm{\lambda}_{e}$. In order to estimate $\bm{\lambda}_{e_{i}}$ locally we define the object equivalent momentum [26] $\bm{\mu}_{i}=\bm{M}_{O_{i}}{\bm{v}}_{O}$ and the vector $\bm{\zeta}_{i}(t)\in\mathbb{R}^{6}$ as:

[TABLE]

whose time derivative is given by:

[TABLE]

where $\bm{K}_{\mu}$ is a positive definite matrix gain. Notice that for a sufficiently large gain matrix $\bm{K}_{\mu}$ of the low pass filter (18), we obtain:

[TABLE]

which intuitively means that $\bm{\zeta}_{i}(t)$ represents the effect of overall external forces exerted on the object (i.e., external disturbances and the forces exerted by all the UVMSs on the object). Consequently, an estimation of $\bm{\lambda}_{e_{i}}=c_{i}\bm{\lambda}_{e}$ can be given by:

[TABLE]

Now let us assume that each UVMS is expected to exert the following desired force/torque on the object:

[TABLE]

where $\bm{\lambda}^{d}_{int,i}$ denotes the desired internal forces and ${\bm{y}}^{cmd}_{i}$ is a pre-designed input given by:

[TABLE]

where $\bm{M}_{d_{O}}$, $\bm{D}_{d_{O}}$ and $\bm{K}_{d_{O}}$ are the desired inertia, damping and stiffness matrices for the object dynamics, $\tilde{\bm{v}}_{O}(t)={\bm{v}}_{O}-{\bm{v}}^{d}_{O_{i}}$ denotes the velocity error and $\tilde{\bm{e}}_{O}$ is the object pose error, defined as:

[TABLE]

where

[TABLE]

is the orientation error expressed in the outer product formulation [27]. In view of (16), it can be concluded that if all robots cooperatively apply the desired wrench vector (21) to the object, then

[TABLE]

which intuitively means that the aforementioned selection of $\bm{\lambda}^{d}_{i}$ cancels the object’s nonlinearities, ensures adequate internal forces via $\bm{\lambda}^{d}_{int,i}$ and achieves the desired dynamics of the object. Thus, the control objective for each UVMS $i\in\mathcal{K}$ is to enforce $\lim_{t\rightarrow\infty}\bm{w}_{i}(t)=0$, where the error signal $\bm{w}(t)$ is constructed as:

[TABLE]

where $\bm{M}_{d}$, $\bm{D}_{d}$ and $\bm{K}_{d}$ are the desired inertia, damping and stiffness matrices for the robot dynamics. Thus, we get an augmented impedance error:

[TABLE]

where $\bm{K}_{f}=\bm{M}_{d}^{-1}$, $\bm{K}_{g}=\bm{K}_{f}\bm{D}_{d}$, and $\bm{K}_{p}=\bm{K}_{f}\bm{K}_{d}$. We also choose two positive-definite matrices $\bm{F}$ and $\bm{Y}$ such that:

[TABLE]

and define the filtered force/torque measurement:

[TABLE]

Thus, we may rewrite (27) as:

[TABLE]

Now we define the auxiliary variable $\bm{z}_{i},\leavevmode\nobreak\ i\in\mathcal{K}$ as:

[TABLE]

Hence, the augmented impedance error becomes:

[TABLE]

which represents a stable low pass filter. Therefore, if we achieve $\lim_{t\rightarrow\infty}\bm{z}_{i}(t)=0$, then the initial control objective is readily met, i.e, $\lim_{t\rightarrow\infty}\bm{w}_{i}(t)=0$. In this respect, let us define the augmented state variable:

[TABLE]

Hence, (30) and (32) immediately result in:

[TABLE]

from which the dynamics (8) becomes:

[TABLE]

Invoking Property $2$ and Assumption 1 , we arrive at the open loop dynamics:

[TABLE]

Therefore, we design the following impedance control scheme:

[TABLE]

where $\bm{K}>0$ is a positive definite gain matrix and $\hat{\bm{\theta}}_{i}$, $\hat{\bm{\theta}}_{d,i}$ denote the estimates of the unknown parameters $\bm{\theta}_{i}$, $\bm{\theta}_{d,i}$ respectively, provided by the update laws:

[TABLE]

with $\bm{\Gamma}_{i}$, $\bm{\Gamma}_{d_{i}}$ positive diagonal gain matrices.

Theorem 1

Consider $N+1$ UVMSs that operate in a constrained workspace $\mathcal{W}$, with dynamics (8) obeying Properties 1–2 and grasp rigidly a common object. The control scheme (35) with adaptive laws (36)-(37) guarantees $\lim_{t\rightarrow\infty}\bm{w}_{i}(t)=0$ as well as the boundedness of all signals in the closed loop system.

Proof: Considering the following Lyapunov function candidate:

[TABLE]

where $\tilde{\bm{\theta}}_{i}=\hat{\bm{\theta}}_{i}-{\bm{\theta}}_{i}$ and $\tilde{\bm{\theta}}_{d_{i}}=\hat{\bm{\theta}}_{d_{i}}-{\bm{\theta}}_{d_{i}}$ denote the parametric errors, differentiating with respect to time and invoking Property $1$ and substituting the control scheme (35)-(37), we get:

[TABLE]

Hence, we conclude that $\bm{z}_{i}$, $\tilde{\bm{\theta}}_{i}$, $\tilde{\bm{\theta}}_{d_{i}}\in L_{\infty}$. Moreover, from the definition of $\bm{z}_{i}$ in (33) , we also deduce that $\bm{x}_{O},\bm{v}_{O}\in L_{\infty}$, and consequently $\bm{v}_{O_{i}}^{r},\dot{\bm{v}}_{O_{i}}^{r}\in L_{\infty}$. Furthermore, employing (34) we arrive at $\dot{\bm{z}}\in L_{\infty}$. Therefore, integrating both sides of (38) leads to:

[TABLE]

Thus, \int_{0}^{t}\big{(}\!-\!\bm{z}_{i}^{\top}(\tau)\bm{K}\bm{z}_{i}\!-\!\bm{z}_{i}^{\top}\bm{D}_{i}\bm{z}_{i}(\tau)\big{)}d\tau is bounded, which results in $\bm{z}_{i}\in L_{2}$. Finally, applying Barbalat’s Lemma and invoking (31) we get $\lim_{t\rightarrow\infty}\bm{w}_{i}(t)\rightarrow\bm{0},\leavevmode\nobreak\ \forall i\in\mathcal{K}$ which completes the proof.

III-C Estimation Scheme

It should be noticed that the followers are not aware of either the object’s desired configuration $\bm{x}^{d}_{O}$ or the obstacles’ position in the workspace. However, the followers will estimate the object’s desired trajectory profile by $\hat{\bm{x}}^{d_{i}}_{O}(t)\leavevmode\nobreak\ i\in\mathcal{N}$, via their own state measurements by adopting a prescribed performance estimator. Hence, let us define the error:

[TABLE]

The expression of prescribed performance for each element of $\bm{e}_{i}(t)=[e_{i1}(t),\ldots,e_{i6}(t)]^{\top},\leavevmode\nobreak\ i\in\mathcal{N}$ is given by the following inequalities:

[TABLE]

for all $t\geq 0$, where $\rho_{ij}(t),\leavevmode\nobreak\ j=1,\ldots,6$ and $i\in\mathcal{N}$ denote the corresponding performance functions. A candidate exponential performance function could be:

[TABLE]

where the constant $\lambda$ dictates the exponential convergence rate, $\rho_{ij,\infty},\leavevmode\nobreak\ i\in\mathcal{N}$ denotes the ultimate bound and $\rho_{ij,0}$ is chosen to satisfy $\rho_{ij,0}>|e_{ij}(0)|,\leavevmode\nobreak\ i\in\mathcal{N}$. Hence, following [28], the estimation law is designed as follows:

[TABLE]

for $\leavevmode\nobreak\ i\in\mathcal{N}$, from which the followers’ estimate $\hat{\bm{x}}^{d_{i}}_{O}(t)=[\hat{\bm{x}}^{d_{i}}_{O_{1}}(t),\ldots,\hat{\bm{x}}^{d_{i}}_{O_{6}}(t)]^{\top},\leavevmode\nobreak\ i\in\mathcal{N}$ is calculated via a simple integration. Moreover, differentiating (43) with respect to time, we acquire the desired acceleration signal:

[TABLE]

employing only the velocity $\dot{\bm{x}}_{O}(t)$ of the object and not its acceleration which is unmeasurable. Based on the aforementioned estimation of the object’s desired trajectory profile $\hat{\bm{x}}^{d_{i}}_{O}(t)$, $\dot{\hat{\bm{x}}}^{d_{i}}_{O}(t)$ and $\ddot{\hat{\bm{x}}}^{d_{i}}_{O}(t),\leavevmode\nobreak\ i\in\mathcal{N}$, we can easily derive the corresponding desired trajectory profile for the follower’s End-Effector, as follows:

[TABLE]

It is worth noting that the proposed estimator is more robust against trajectory profiles with non-zero acceleration than previous results presented in [29, 30]. In particular, our method guarantees bounded closed loop signals and practical asymptotic stabilization of the estimation errors. Moreover, the ultimate bounds in (42) can be set arbitrarily small to a value reflecting the resolution of the measurement devices, thus achieving practical convergence of the estimation errors to zero. Furthermore, the transient response depends on the convergence rate of the performance functions $\rho_{ij}(t),j=1,\ldots,6$ and $i\in\mathcal{N}$ that is directly affected by the parameter $\lambda$.

IV Simulation Results

The theoretical findings of this work are verified in a dynamic simulation environment built in MATLAB® presented in our previous works [31], with sampling time $0.1\leavevmode\nobreak\ sec$, which is common in a real time operation with an underwater robotic system. The UVMS model considered in the simulations is an AUV equipped with a small 4 DoF manipulator attached at the bow of the vehicle (see Fig.2). The cooperative transportation is performed by 4 UVMSs grasping the object at its corners. The blue UVMS acts as the leader. Thus, we assume that the desired object’s configuration as well as the obstacles’ position in the workspace are transferred to the leading UVMS beforehand. The obstacles are modeled as spheres ($1$ m radius) and are located in the workspace in order to complicate the transportation task of the object. In this respect, a Navigation Function is constructed following (13) in order to handle the aforementioned constrained workspace. Since, only the leading UVMS (blue) is aware of the object’s desired configuration, the followers will estimate it via the proposed algorithm (43), by simply observing the motion of the object and without communicating explicitly with the leader. Moreover, the dynamics of the UVMS were affected by external disturbances in the form of slowly time varying sea currents modeled by the corresponding velocities ${v}^{c}_{x}=0.3\sin(\frac{\pi}{15}t)\frac{m}{s}$ and ${v}^{c}_{y}=0.3\cos(\frac{\pi}{15}t)\frac{m}{s}$. Finally, the control gains and the parameters of the proposed estimator were chosen as shown in Table-I and Table-II.

Simulation Study

The results are illustrated in Figs.2-4. The evolution of the system under the proposed methodology is given in Fig.2. It should be noticed that the UVMSs have transported cooperatively the grasped object from the initial configuration to the desired one without colliding with obstacles. By observing the object’s tracking error (Fig.3) it can be concluded that even under the influence of external disturbances, the errors in all directions converge very close to zero.

The estimation errors of the proposed estimation scheme are presented in Fig.4. It can be easily seen that the estimation errors converge smoothly to zero and remain always within the performance envelope defined by the corresponding performance functions as it was expected from the aforementioned theoretical analysis.

V Summary and Future Work

In this work, we presented a cooperative object transportation scheme for Underwater Vehicle Manipulator Systems under implicit communication, avoiding thus completely tedious explicit data transmission. In the proposed scheme, only the leading UVMS is aware of the desired configuration of the object and the obstacles’ position in the workspace, and aims at navigating safety the team towards the goal configuration. On the contrary, the followers estimate the object’s desired trajectory and implement an impedance control law. Moreover, the proposed scheme imposes no restrictions on the underwater communication bandwidth. Furthermore, the control scheme adopts load sharing among the UVMSs according to their specific payload capabilities. Future research efforts will be devoted towards extending the proposed methodology for multiple UVMSs with underactuated vehicle dynamics.