1. Introduction
Robotic manipulators are used in a wide range of applications, including handling of hazardous materials, sorting facilities, manufacturing and assembling lines, construction, and health care, to name a few [Reference Christensen, Amato, Yanco, Mataric, Choset, Drobnis, Goldberg, Grizzle, Hager, Hollerbach, Hutchinson, Krovi, Lee, Smart, Trinkle and Sukhatme1]. In all these scenarios, the manipulators are expected to interact with their environment, which may involve the presence of obstacles, people, and other robots. For instance, consider the two 3-degrees-of-freedom (DOF) articulated robots in Fig. 1. The robots need to avoid collisions not only with static and dynamic objects in their environment but also with other robots and with themselves. Therefore, it is critical to develop control protocols that can guarantee the safety of the manipulators at all times.
1.1. Related work
Collision avoidance for robotic manipulators can be solved using motion planning algorithms, where the objective is to design a safe trajectory for the manipulator to follow over some period of time, or real-time control, where control inputs are updated online based on the continuous interaction and perception of the robot with its surrounding. Several motion planning algorithms have been proposed over the years [Reference Palmieri and Scoccia2]. Examples include the use of C-space obstacle [Reference Lozano-Pérez3], rapidly exploring random trees [Reference Karaman and Frazzoli4–Reference Rybus, Prokopczuk, Wojtunik, Aleksiejuk and Musiał6], probabilistic roadmaps [Reference Hsu, Kindel, Latombe and Rock7], and model predictive control [Reference Avanzini, Zanchettin and Rocco8, Reference Xu, Wang, Dai and Zuo9]. Other frameworks incorporate the use of artificial potential field functions to compute a safe path [Reference Tang, Zhou and Xu10, Reference Yang, Dong and Dai11], define geometric algorithms to detect potential collisions [Reference Agarwal, Srivatsan and Bandyopadhyay12–Reference Schulman, Duan, Ho, Lee, Awwal, Bradlow, Pan, Patil, Goldberg and Abbeel16], or use machine learning (ML) tools to learn obstacle avoidance strategies [Reference Sangiovanni, Incremona, Piastra and Ferrara17, Reference Zhang, Zhang, Lin, Louie and Huang18]. Nevertheless, many of these methods assume global knowledge of the environment (i.e., a centralized approach which might be impractical) are not rigorously developed to guarantee collision avoidance at all times or may be based on kinematic models [Reference Zlajpah and Nemec19] that ignore dynamic properties of the manipulators such as inertia, leading to non-smooth trajectories and potential collisions when maneuvering at high speeds. Furthermore, several applications, such as teleoperation, may involve robots that are not fully autonomous, for which the amount of planning might be constrained and where a real-time approach might be more suitable.
Real-time avoidance control, also known as reactive avoidance control, generates forces or control inputs that respond to information gathered at the moment either via sensing or communication. Of particular interest are distributed real-time avoidance frameworks that are provably safe, that is, that are theoretically guaranteed to avoid collisions at all times. For instance, frameworks based on artificial potential fields [Reference Khatib20, Reference Leitmann and Skowronski21] can be implemented in a decentralized manner and can be rigorously proven to avoid collision by applying Lyapunov theory. Examples include the works in [Reference Deka, Li, Stipanović and Kesavadas22–Reference Zhu, Mao, Han, Zhang and Yang27]. A common drawback of these approaches is the modeling of links and segments of obstacles as spheres, a collection of spheres [Reference Xie, Patel, Kalaycioglu and Asmer26, Reference Zhang, Rodríguez-Seda, Deka, Amrouche, Zhou, Stipanović and Leitmann28] (see Fig. 1 for the obstacle in the example), or other primitive shapes [Reference Yang, Dong and Dai11, Reference Stavridis, Papageorgiou and Doulgeri29, Reference Zimmermann, Busenhart, Huber, Poranne and Coros30] which may lead to the implementation of several artificial potential field functions or unnecessary conservatism due to the loose approximations. Other approaches introduce the idea of critical points along the surfaces of obstacles [Reference Khatib20, Reference Zhu, Mao, Han, Zhang and Yang27], which in turn results in control discontinuities due to the considerations of discrete proximity points. Furthermore, some require optimization algorithms that may not yield control torques in closed form [Reference Zhu, Mao, Han, Zhang and Yang27].
Illustration of two robotic manipulators with an obstacle. Two different shape approximations or envelopes conventionally used in collision avoidance are represented: the use of a single convex shape on the manipulators and the use of multiple spheres on the obstacle.
Similar to the use of artificial potential field functions, recent efforts have proposed the concept of control barrier functions [Reference Ames, Coogan, Egerstedt, Notomista, Sreenath and Tabuada31]. The aim is to design real-time controllers that keep the robot trajectories within a safety set [Reference Singletary, Klingebiel, Bourne, Browning, Tokumaru and Ames32]. These approaches, however, tend to be based on similar conservative ideas, such as the approximation of obstacles as spheres [Reference Singletary, Kolathaya and Ames33] or approximations of the shortest distance which yield nonsmooth controllers [Reference Singletary, Guffey, Molnar, Sinnet and Ames34].
Recently, the use of ML methods to approximate the shape, shortest distance to collision, or safe boundaries has been proposed. For instance, in ref. [Reference Salehian, Figueroa and Billard35], the authors propose a computationally efficient signed distance-based avoidance control [Reference Ratliff, Zucker, Bagnell and Srinivasa36] where the self-collision boundary is approximated using neural networks. Similarly, [Reference Koptev, Figueroa and Billard37] presents a fast method to learn the shortest distance from the robot to an obstacle as a neural joint space implicit distance function. Yet, as is the case of most ML-based avoidance control methods, these techniques cannot rigorously guarantee stability and safety at all times.
1.2. Contributions
In this paper, we propose a decentralized, cooperative artificial potential field method based on avoidance functions for rigid manipulators. The control law is decentralized since only information from other robots and obstacles within a bounded predefined distance needs to be considered and cooperative, given that it can guarantee collision avoidance at all times if other robots and dynamics obstacles abide by the same control policy [Reference Stipanović, Hokayem, Spong and Šiljak38]. The novelty of the approach is the proposal of a shape- and orientation-dependent, continuously differentiable safety minimum distance between two links or obstacles that yield continuous, closed-form controllers. The safety distance is a smooth analytical function that does not require optimization or discrete algorithms and which derivatives can be computed offline. The use of a continuous differentiable shortest distance eliminates the discontinuity of defining critical points or the conservatism of assuming spherical or ellipsoidal shapes. Moreover, our approach provides the development of closed-form analytical control input torques that are smooth and well defined. We prove, via Lyapunov analysis, that the cooperative control strategy guarantees the safety of the robotic manipulators at all times. We then provide explicit distance continuously differentiable formulas for manipulators with cylindrical links as well as simulations for two- and three-dimensional manipulators that illustrate the performance and efficacy of the proposed approach in comparison to the traditional use of spherical approximations. Finally, we formulate extensions of the proposed collision avoidance framework to other Lyapunov-based controllers, trajectory tracking controllers, and robotic systems with bounded control torques.
2. Preliminaries
2.1. Robot kinematics
We consider the task of safely coordinating the motion of
$N$
$n_i$
-DOF rigid, nonlinear manipulators with generalized coordinates (or joint configurations) given by:
\begin{align} \textbf{q}_i(t)=\;&[q_{i1}(t), \cdots, q_{i{n_i}}(t)]^T, & \mathrm{for}\ i\in \mathbb{N} \end{align}
where
$\mathbb{N}=\{1,\cdots, N\}$
denotes the set of robots. The joints and links are labeled in ascending order from the most proximal to the most distal, as shown in Fig. 1. The position of the geometric center of each link with respect to a common world reference frame is denoted in Cartesian coordinates by
$\textbf{p}_{ij}(t)\;:\!=\;\textbf{p}_{ij}(\textbf{q}_i(t))\in \mathbb{R}^n$
, where
$n$
is the dimension of the robots’ workspace. The joint velocity
$\dot{\textbf{q}}_i(t)$
and the velocity of the geometric center
$\dot{\textbf{p}}_{ij}(t)$
are related by the translational Jacobian
$J_{ij}$
:
\begin{align} \dot{\textbf{p}}_{ij}(t) =\;& J_{ij}(\textbf{q}_i(t))\dot{\textbf{q}}_i(t),& J_{ij}(\textbf{q}_i(t)) = \frac{\partial \textbf{p}_{ij}(t)}{\partial \textbf{q}_i(t)}. \end{align}
In addition, we define the angular orientation of each link with respect to the common world frame as:
\begin{align} \boldsymbol{\theta }_{ij}(t)&\;:\!=\; [\phi _{ij}(\textbf{q}_i(t)), \vartheta _{ij}(\textbf{q}_i(t)), \psi _{ij}(\textbf{q}_i(t))]^T \end{align}
where
$\phi _{ij}$
,
$\vartheta _{ij}$
, and
$\psi _{ij}$
denote the orientation of
$i$
th robot’s
$j$
th link in Euler angles with respect to the
$xy$
,
$zx$
, and
$yz$
planes (or, equivalently, about the
$z$
,
$y$
, and
$x$
axes), respectively. Similar to the linear velocities, the angular velocities are related to joint velocities by the rotational Jacobian
$\mathcal{J}_{ij}$
:
\begin{align} \dot{\boldsymbol{\theta }}_{ij}(t) =\;& \mathcal{J}_{ij}(\textbf{q}_i(t))\dot{\textbf{q}}_i(t),& \mathcal{ J}_{ij}(\textbf{q}_i(t)) = \frac{\partial \boldsymbol{\theta }_{ij}(t)}{\partial \textbf{q}_i(t)}. \end{align}
Note that in the case of planar manipulators, that is,
$n=2$
, only one angular orientation is required. In what follows, we will omit the time argument of signals.
2.2. Robot dynamics
The manipulators are assumed to be fully actuated with nonlinear Lagrangian dynamics given by:
\begin{align} M_i(\textbf{q}_i)\ddot{\textbf{q}}_i+C_i(\textbf{q}_i,\dot{\textbf{q}}_i)\dot{\textbf{q}}_i+\textbf{g}_i(\textbf{q}_i) =\;& \boldsymbol{\tau }_i, & i\in \mathbb{N} \end{align}
where
$M_i(\textbf{q}_i)\in \mathbb{R}^{n_i\times n_i}$
are the positive definite inertia matrices,
$C_i(\textbf{q}_i,\dot{\textbf{q}}_i)\in \mathbb{R}^{n_i\times n_i}$
are the matrices of Coriolis and centrifugal terms,
$\textbf{g}_i(\textbf{q}_i)=[g_{i1}(\textbf{q}_i), \cdots, g_{in_i}(\textbf{q}_i)]^T$
are the gravitational torques and forces, and
$\boldsymbol{\tau }_i=[\tau _{i1}, \cdots, \tau _{in_i}]^T$
are the control inputs for the
$i$
th robot. We assume that
$\textbf{g}_i(\textbf{q}_i)\in \mathbb{R}^{n_i}$
are known and, therefore, can be compensated via active control and that the manipulators satisfy the following standard properties for all
$\textbf{q}_i$
and
$\dot{\textbf{q}}_i$
[Reference From, Schjølberg, Gravdahl, Pettersen and Fossen39].
Property 1.
$\exists \overline{m}_i\geq \underline{m}_i\gt 0$
such that
$\underline{m}_i \leq \left \|M(\textbf{q}_i)\right \| \leq \overline{m}_i$
.
Property 2.
$\dot{M}_i(\textbf{q}_i) - 2C_i(\textbf{q}_i,\dot{\textbf{q}}_i)$
is skew-symmetric.
2.3. Control objective
For simplicity, we assume that the links can be approximated (or enclosed) by elongated convex shapes, as shown in Fig. 1. Furthermore, we assume that adjacent links (i.e., those that share a joint) are offset from each other, which implies that they can move freely without colliding. Using the geometric centers as reference points, we define a minimum safe distance between two nonadjacent links,
$\textbf{p}_{ij}$
and
$\textbf{p}_{kl}$
, as:
\begin{align} r_{ij}^{kl}\;:\!=\;& r_{ij}^{kl}(\textbf{p}_{ij}, \textbf{p}_{kl}, \boldsymbol{\theta }_{ij},\boldsymbol{\theta }_{kl}) = r_{kl}^{ij}, &\mathrm{for}\ k \in \mathbb{N}, l \in \mathbb{L}_{ij}^k \end{align}
where
$r_{ij}^{kl}$
is a continuous, differentiable function of the links’ positions, orientations, and systems geometrical parameters, including length, depth, and width, and where
\begin{align} \mathbb{L}_{ij}^k\;:\!=\;&\{l \in \{1,\cdots, n_k\} \ |\ k\in \mathbb{N}/i\} \cup \{l \in \{1,\cdots, n_i\}/\{j-1,j,j+1\}\ |\ k=i\} \end{align}
represents the set of all nonadjacent links to the
$i$
th robot’s
$j$
th link. It is assumed that this minimum safe distance is known or that it can be approximated from above. Then, we can formulate the control objective as follows. Design
$\boldsymbol{\tau }_i$
such that
$\left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \| \gt r_{ij}^{kl}$
$\forall \ t\geq 0$
,
$j\in \{1,\cdots, n_i\},k\in \mathbb{N},l\in \mathbb{L}_{ij}^k$
.
2.4. Approximation functions
In what follows, we make use of the following continuous differentiable functions to approximate the minimum, the maximum, and the absolute value functions. Let
$\{a_1,\cdots, a_m, \varepsilon, \delta \}$
be a set of positive scalar constants. Then, the following functions
\begin{align} \tilde{\lambda }_\delta (a_1,\cdots, a_m) &\;:\!=\; \left (\sum _{i=1}^m a_i^{-\delta }\right )^{-\frac{1}{\delta }} \geq \min \{a_1,\cdots, a_m\} \end{align}
\begin{align} \tilde{\Lambda }_\delta (a_1,\cdots, a_m) &\;:\!=\; \left (\frac{1}{m}\sum _{i=1}^m a_i^{\delta }\right )^{\frac{1}{\delta }} \hspace{-0.25ex}\hspace{-0.25ex}\leq \max \{a_1,\cdots, a_m\} \end{align}
\begin{align} \tilde{\alpha }_\varepsilon (a_1) &\;:\!=\; \sqrt{a_1^2 + \varepsilon ^2} = \tilde{\alpha }_\varepsilon (- a_1) \gt \left |a_1\right | \end{align}
are approximations of the minimum (from above), maximum (from below) [Reference Stipanović, Tomlin and Leitmann40], and absolute value (from above). Moreover, note that
$ \tilde{\lambda }_\delta (\!\cdot\!) \rightarrow \min \{\cdot \}$
,
$ \tilde{\Lambda }_\delta (\!\cdot\!) \rightarrow \max \{\cdot \}$
, and
$\tilde{\alpha }_\varepsilon (\!\cdot\!)\rightarrow |\cdot |$
when
$\delta \rightarrow \infty$
and
$\varepsilon \rightarrow 0$
.
3. Collision avoidance control
3.1. Avoidance functions
We use the concept of avoidance control [Reference Leitmann and Skowronski21, Reference Stipanović, Hokayem, Spong and Šiljak38] and define an avoidance function,
$V_{ij}^{kl}\;:\!=\;V_{ij}^{kl}(\textbf{p}_{ij},\textbf{p}_{kl},r_{ij}^{kl})$
, between two nonadjacent links as:
\begin{align} V_{ij}^{kl}\;:\!=\;\left (\min \left \{0,\frac{\left \|\textbf{p}_{ij}- \textbf{p}_{kl}\right \|^2 - \left (r_{ij}^{kl}+\rho _{ij}^{kl}\right )^2}{\left \|\textbf{p}_{ij}- \textbf{p}_{kl}\right \|^2 - \left (r_{ij}^{kl}\right )^2}\right \}\right )^2 \end{align}
where
$\rho _{ij}^{kl} = \rho _{kl}^{ij} \gt 0$
is a constant parameter. Herein, the term
$r_{ij}^{kl}+\rho _{ij}^{kl}$
represents the distance at which the
$i$
th robot’s
$j$
th link should start avoiding the
$k$
th robot’s
$l$
th link. Note that
$V_{ij}^{kl} = 0$
$\forall \ \left \|\textbf{p}_{ij}- \textbf{p}_{kl}\right \| \geq r_{ij}^{kl}+\rho _{ij}^{kl}$
,
$V_{ij}^{kl}\rightarrow \infty$
as
$\left \|\textbf{p}_{ij}- \textbf{p}_{kl}\right \|\rightarrow r_{ij}^{kl}$
, and its gradient with respect to
$\textbf{p}_i$
and
$r_{ij}^{kl}$
are
\begin{align} \left [\begin{array}{c}\displaystyle \frac{\partial V_{ij}^{{kl}^T}}{\partial \textbf{p}_{ij}} \\[5pt] \displaystyle \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\end{array}\right ] =\; & \begin{cases} f_{ij}^{kl} \left [\begin{array}{c}\textbf{p}_{kl}-\textbf{p}_{ij} \\[5pt] 2r_{ij}^{kl}\rho _{ij}^{kl}+{\rho _{ij}^{kl}}^2+\left \|\textbf{z}_i-\textbf{z}_j\right \|^2\end{array}\right ] & \mathrm{if}\ \left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \|\in (r_{ij}^{kl},r_{ij}^{kl}+\rho _{ij}^{kl}) \\[5pt] \hfil \mathrm{undefined}, & \mathrm{if}\ \left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \| = r_{ij}^{kl} \\[5pt] \hfil \textbf{0}^T, & \mathrm{otherwise} \end{cases} \end{align}
where
\begin{align} f_{ij}^{kl} =\;&{4\rho _{ij}^{kl}\left (\left (r_{ij}^{kl}+\rho _{ij}^{kl}\right )^2-\left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \|^2\right )}{\left (\left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \|^2-\left (r_{ij}^{kl}\right )^2\right )^{-3}}. \end{align}
Furthermore, one can show that
$V_{ij}^{kl}$
is almost everywhere continuously differentiable that
\begin{align} \frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{ij}} =\;& \frac{\partial V_{kl}^{ij}}{\partial \textbf{p}_{ij}}= -\frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{kl}}= -\frac{\partial V_{kl}^{ij}}{\partial \textbf{p}_{kl}}, & \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}} =\;& \frac{\partial V_{kl}^{ij}}{\partial r_{kl}^{ij}} \end{align}
and that
\begin{align} \nonumber \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k} \dot{V}_{ij}^{kl} &= \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\left (\frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{ij}}\frac{\partial \textbf{p}_{ij}}{\partial \textbf{q}_i}\dot{\textbf{q}}_{i} + \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \textbf{p}_{ij}}\frac{\partial \textbf{p}_{ij}}{\partial \textbf{q}_i}\dot{\textbf{q}}_{i}+ \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \boldsymbol{\theta }_{ij}}\frac{\partial \boldsymbol{\theta }_{ij}}{\partial \textbf{q}_i}\dot{\textbf{q}}_{i}\right . \\[5pt] &\quad \nonumber + \left . \frac{\partial V_{kl}^{ij}}{\partial \textbf{p}_{kl}}\frac{\partial \textbf{p}_{kl}}{\partial \textbf{q}_k}\dot{\textbf{q}}_{k} + \frac{\partial V_{kl}^{ij}}{\partial r_{kl}^{ij}}\frac{\partial r_{kl}^{ij}}{\partial \textbf{p}_{kl}}\frac{\partial \textbf{p}_{kl}}{\partial \textbf{q}_k}\dot{\textbf{q}}_{k}+ \frac{\partial V_{kl}^{ij}}{\partial r_{kl}^{ij}}\frac{\partial r_{kl}^{ij}}{\partial \boldsymbol{\theta }_{kl}}\frac{\partial \boldsymbol{\theta }_{kl}}{\partial \textbf{q}_k}\dot{\textbf{q}}_{k} \right ) \\[5pt] &= \sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\left (\frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{ij}}J_{ij} + \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \textbf{p}_{ij}}J_{ij}+ \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \boldsymbol{\theta }_{ij}}\mathcal{J}_{ij}\right )\dot{\textbf{q}}_{i}. \end{align}
Note that in contrast to other definitions of avoidance functions [Reference Rodríguez-Seda and Stipanović23, Reference Stipanović, Hokayem, Spong and Šiljak38], the ones defined here use a nonconstant safe distance radius.
3.2. Control law
Herein, we will consider the control task of achieving a desired, constant joint configuration,
$\textbf{q}_i^d\in \mathbb{R}^{n_i}$
, while avoiding collisions. Accordingly, we propose a proportional derivative (PD) control law with gravity compensation and collision avoidance given by:
\begin{align} \boldsymbol{\tau }_i =\;& \textbf{g}_i - A_i(\textbf{q}_i-\textbf{q}^d_i) - B_i \dot{\textbf{q}}_i -\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} \end{align}
\begin{align} \textbf{u}_{ij}^{kl} =\; & J_{ij}^T\left (\frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{ij}} + \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \textbf{p}_{ij}}\right )^T\hspace{-0.25ex}\hspace{-0.25ex}\hspace{-0.25ex}\hspace{-0.25ex}\hspace{-0.25ex}\hspace{-0.25ex} + \mathcal{J}_{ij}^T \left (\frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\frac{\partial r_{ij}^{kl}}{\partial \boldsymbol{\theta }_{ij}}\right )^T \end{align}
where
$\textbf{u}_{ij}^{kl}\in \mathbb{R}^{n_i}$
represents the avoidance control torque generated by the interaction between the
$i$
th robot’s
$j$
th link and the
$k$
th robot’s
$l$
th link and where
$A_i=\mathrm{diag}(a_{i1},\cdots, a_{in_i})$
and
$B_i=\mathrm{diag}(b_{i1},\cdots, b_{in_i})$
are positive-definite, diagonal matrices. As it will be shown next, the closed-loop control law (16) guarantees a safe minimum distance among nonadjacent links at all times.
Proposition 1 (Collision Avoidance). Consider a group of
$N$
$n_i$
-DOF manipulators with dynamics and control inputs given by (5) and (16). Assume that
$\left \|\textbf{p}_{ij}(0)-\textbf{p}_{kl}(0)\right \| \gt r_{ij}^{kl}$
for all
$i,k \in \mathbb{N},\ j\in \{1,\cdots, n_i\},\ l\in \mathbb{L}_{ij}^k$
. Then,
$\left \|\textbf{p}_{ij}(t)-\textbf{p}_{kl}(t)\right \| \gt r_{ij}^{kl}$
$\forall \ t\geq 0$
.
Proof. Consider the Lyapunov candidate function:
\begin{align} \mathcal{V} =\;& \frac{1}{2}\sum _{i=1}^N\left ((\textbf{q}_i-\textbf{q}^d_i)^T\hspace{-0.25ex} A_i(\textbf{q}_i-\textbf{q}^d_i)+ \dot{\textbf{q}}_i^T M_i \dot{\textbf{q}}_i\right ) + \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k} V_{ij}^{kl} \end{align}
which is positive for all
$[\textbf{q}_i-\textbf{q}^d_i, \dot{\textbf{q}}_i]^T \neq \textbf{0}$
(from Property 1). Taking its time derivative and using equations (5), (15), and (16) yields
\begin{align} \dot{\mathcal{V}} =&\hspace{-0.25ex}\hspace{-0.25ex} \sum _{i=1}^N\hspace{-0.25ex}\left ((\textbf{q}_i-\textbf{q}^d_i)^T\hspace{-0.25ex} A_i\dot{\textbf{q}}_i+\dot{\textbf{q}}_i^T\hspace{-0.25ex}\hspace{-0.25ex}\left ({M}_i\ddot{\textbf{q}}_i+\hspace{-0.25ex}\frac{\dot{M}_i}{2}\dot{\textbf{q}}_i\hspace{-0.25ex}\right )\hspace{-0.25ex}\right ) \hspace{-0.25ex} + \hspace{-0.25ex}\hspace{-0.25ex}\sum _{i=1}^N\hspace{-0.25ex}\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\hspace{-0.25ex}\hspace{-0.25ex}\left (\hspace{-0.25ex}\hspace{-0.25ex}\frac{\partial V_{ij}^{kl}}{\partial \textbf{p}_{ij}}J_{ij} + \frac{\partial V_{ij}^{kl}}{\partial r_{ij}^{kl}}\hspace{-0.25ex}\hspace{-0.25ex}\left (\hspace{-0.25ex}\hspace{-0.25ex}\frac{\partial r_{ij}^{kl}}{\partial \textbf{p}_{ij}}J_{ij}+\frac{\partial r_{ij}^{kl}}{\partial \boldsymbol{\theta }_{ij}}\mathcal{J}_{ij}\hspace{-0.25ex}\hspace{-0.25ex}\right )\hspace{-0.25ex}\hspace{-0.25ex}\right )\hspace{-0.25ex}\dot{\textbf{q}}_{i} \nonumber \\[5pt] =& \sum _{i=1}^N\left ((\textbf{q}_i-\textbf{q}^d_i)^TA_i\dot{\textbf{q}}_i-\dot{\textbf{q}}_i^T\left ( C_i\dot{\textbf{q}}_i + A_i(\textbf{q}_i-\textbf{q}^d_i)+B_i\dot{\textbf{q}}_i +\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl}+\frac{\dot{M}_i}{2}\dot{\textbf{q}}_i \right )\hspace{-0.25ex}\right )\nonumber \\[5pt] & + \sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}{\textbf{u}_{ij}^{kl}}^T\dot{\textbf{q}}_i = -\sum _{i=1}^N\dot{\textbf{q}}_i^TB_i\dot{\textbf{q}}_i + \frac{1}{2}\sum _{i=1}^N\dot{\textbf{q}}_i^T(\dot{M}-2C_i)\dot{\textbf{q}}_i = -\sum _{i=1}^N\dot{\textbf{q}}_i^TB_i\dot{\textbf{q}}_i \end{align}
where we used Property 2. Since
$\dot{\mathcal{V}} \leq -\sum _{i=1}^N\underline{b}_i \left \|\dot{\textbf{q}}_i\right \|^2 \leq 0$
, where
$\underline{b}_i\gt 0$
is the smallest eigenvalue of
$B_i$
, we have that
$\mathcal{V}$
is non-increasing and bounded by
$\mathcal{V}(0)$
for all
$t\geq 0$
. Now, suppose that for some nonadjacent links
$\left \|\textbf{p}_{ij}(t) - \textbf{p}_{kl}(t)\right \|\rightarrow r_{ij}^{kl}$
for some
$t$
. The latter would imply that
$V_{ij}^{kl}\rightarrow \infty \Rightarrow \mathcal{V} \rightarrow \infty$
, which is a contradiction. Since the solutions of equation (5) are continuous, one has that
$\left \|\textbf{p}_{ij}(t)-\textbf{p}_{kl}(t)\right \| \gt r_{ij}^{kl}$
for all
$t\geq 0$
and the proof is complete.
3.3. Extensions, considerations, and limitations
3.3.1. Lyapunov-based controllers
It is worth noting that other closed-loop controllers can be used in place of a traditional PD-type control, including nonlinear controllers. For instance, consider a controller of the form:
\begin{align} \tau _i = \textbf{g}_i - \frac{\partial \mathcal{A}_i}{\partial \textbf{q}_i} - \frac{\partial \mathcal{B}_i}{\partial \dot{\textbf{q}}_i} -\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} \end{align}
where
$\mathcal{A}_i\;:\!=\;\mathcal{A}_i(\textbf{q}_i-\textbf{q}_i^d)$
and
$\mathcal{B}_i\;:\!=\;\mathcal{B}_i(\dot{\textbf{q}}_i)$
are functions with the following properties:
-
•
$\mathcal{A}_i$
is positive-definite, that is,
$\mathcal{A}_i = 0$
for
$\textbf{q}_i-\textbf{q}_i^d= \textbf{0}$
and
$\mathcal{A}_i \gt 0$
otherwise. -
•
$\mathcal{A}_i$
is radially unbounded, that is,
$\mathcal{A}_i \rightarrow \infty$
if
$\left \|\textbf{q}_i-\textbf{q}_i^d\right \| \rightarrow \infty$
. -
•
$(\partial \mathcal{B}_i/\partial \dot{\textbf{q}}_i)\dot{\textbf{q}}_i \geq 0$
$\forall \dot{\textbf{q}}_i$
.
Then, following the steps of the proof of Proposition 1, one can show that the time derivative of the following candidate Lyapunov function
\begin{align} \overline{\mathcal{V}}_i =\; & \frac{1}{2}\sum _{i=1}^N\left (\mathcal{A}_i+ \dot{\textbf{q}}_i^T M_i \dot{\textbf{q}}_i\right ) + \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k} u_{ij}^{kl} \end{align}
is
$\dot{{\overline{\mathcal{V}}}}_i = -\sum _{i=1}^N(\partial{\mathcal{B}_i}/ \partial{\dot{\textbf{q}}_i})\dot{\textbf{q}}_i \leq 0$
and the same statements from Proposition 1 will hold.
Note that the PD-type controller in (16a) belongs to this class controllers with
$\mathcal{A}_i = (\textbf{q}_i-\textbf{q}^d_i)^T A_i(\textbf{q}_i-\textbf{q}^d_i)$
and
$\mathcal{ B}_i = \dot{\textbf{q}}_i^TB_i\dot{\textbf{q}}_i$
.
3.3.2. Trajectory tracking
The analysis performed in this section assumes a desired constant configuration
$\dot{\textbf{q}}_i^d\equiv 0$
. However, many applications require the robotic manipulator to move along a trajectory. Therefore, let’s consider a desired trajectory
$(\textbf{q}_i^d, \dot{\textbf{q}}_i^d, \ddot{\textbf{q}}_i^d)$
along with an inverse dynamics controller of the form:
\begin{align} \tau _i = M_i\ddot{\textbf{q}}_i^d+C_i\dot{\textbf{q}}_i^d + \textbf{g}_i - A_i(\textbf{q}_i-\textbf{q}^d_i) - B_i (\dot{\textbf{q}}_i-\dot{\textbf{q}}_i^d) -\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} . \end{align}
Substituting (21) into (1) yields
\begin{align} M_i\ddot{\tilde{\textbf{q}}}_i + C_i\dot{\tilde{\textbf{q}}}_i =\;& -A_i\tilde{\textbf{q}}_i-B_i\dot{\tilde{\textbf{q}}}_i-\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} \end{align}
where
$\tilde{\textbf{q}}_i=\textbf{q}_i-\textbf{q}_i^d$
. Then, the following result about collision avoidance follows.
Proposition 2 (Collision Avoidance with Trajectory Tracking). Consider a group of
$N$
$n_i$
-DOF manipulators with feedback dynamics given by (22). Assume that
$\left \|\textbf{p}_{ij}(0)-\textbf{p}_{kl}(0)\right \| \gt r_{ij}^{kl}$
for all
$i,k \in \mathbb{N},\ j\in \{1,\cdots, n_i\},\ l\in \mathbb{L}_{ij}^k$
and that desired trajectory chosen such that the following inequality is satisfied
\begin{align} \underline{b}_i \left \|\dot{\tilde{\textbf{q}}}_i\right \|^2 + \dot{\textbf{q}}_i^{d^T}\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} &\geq 0, & \forall \ i\in \{1,\cdots, N\}. \end{align}
Then,
$\left \|\textbf{p}_{ij}(t)-\textbf{p}_{kl}(t)\right \| \gt r_{ij}^{kl}$
$\forall \ t\geq 0$
.
Proof. The proof follows similar to the steps from Proposition 1. Consider the following Lyapunov candidate function:
\begin{align} \tilde{\mathcal{V}} =\;& \frac{1}{2}\sum _{i=1}^N\left (\tilde{\textbf{q}}_i^T A_i\tilde{\textbf{q}}_i+ \dot{\tilde{\textbf{q}}}_i^T M_i \dot{\tilde{\textbf{q}}}_i\right ) + \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k} V_{ij}^{kl}. \end{align}
Taking its time derivative, one obtains
\begin{align} \dot{\tilde{\mathcal{V}}} =\;& -\sum _{i=1}^N\dot{\tilde{\textbf{q}}}_i^TB_i\dot{\tilde{\textbf{q}}}_i -\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}(\dot{\textbf{q}}_i-\dot{\textbf{q}}_i^d)^T\textbf{u}_{ij}^{kl} + \sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}{\textbf{u}_{ij}^{kl}}^T\dot{\textbf{q}}_i \nonumber \\[5pt] \leq & -\sum _{i=1}^N\left (\underline{b}_i \left \|\dot{\tilde{\textbf{q}}}_i\right \|^2 + \sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\dot{\textbf{q}}_i^{d^T}\textbf{u}_{ij}^{kl}\right ) \leq 0 \end{align}
where we used assumption (23). Since
$\dot{\tilde{\mathcal{V}}}$
is nonpositive, one has that
$\tilde{\mathcal{V}}$
is non-increasing and bounded. Using similar arguments as in Proposition 1, one can finally conclude that a collision cannot take place.
Note that (23) is always satisfied for
$\dot{\textbf{q}}_i = 0$
. Therefore, to avoid violation of (23), one can alternatively freeze the desired trajectory until the conflict is resolved as proposed in ref. [Reference Mastellone, Stipanović, Graunke, Intlekofer and Spong41]. It is also worth noting that the inverse dynamics control law (21) requires accurate estimation of the system parameters, and so does (16) for the compensation of the gravitational torques. In ref. [Reference Spong, Hutchinson and Vidyasagar42], the authors discuss adaptive and robust inverse dynamics control laws that can be applied in lieu of (21) with little alteration to the collision avoidance results. Similarly, the work in ref. [Reference Wu, Wang and You43] summarizes different system identification methods for robotics manipulators that could be combined with the proposed avoidance control.
3.3.3. Path planning
The proposed minimum safe distance approach has been formulated for real-time, reactive collision avoidance control laws. It is a function of the agents’ relative position, orientation, and shapes. The proposed formulation of the minimum distance can potentially be adapted to path planning algorithms that employ potential field functions similar to the work in refs. [Reference Tang, Zhou and Xu10, Reference Wang, Zhu, Wang, He, He and Xu25]. Yet, the time-varying property of the minimum safe distance, which molds the shape of the artificial potential field functions, adds another degree of complexity to the path planning problem. The latter is particularly significant if obstacles in the robot’s environment are also moving.
3.3.4. Deadlocks
A deadlock or an unwanted local minimum refers to the convergence of the manipulator to a configuration different from the desired one. This is a common drawback of decentralized motion planning algorithms and potential field methods, including the one proposed in this paper. Conventional solutions to help avoid or escape deadlocks include changing, at least temporarily, the desired configuration when a deadlock is reached or adding a perturbation control torque to (16) as in ref. [Reference Rodríguez-Seda, Stipanović and Spong44]. Yet, none of these methods can guarantee with certainty that the robot will not return to a deadlock.
3.3.5. Dynamic obstacles
The proposed control law is cooperative, meaning that it assumes that all other robots apply the same avoidance control. The results of collision avoidance naturally extend to static obstacles of any shape, given that their velocities are zero. Problems may arise when interacting with dynamic obstacles, for which collision avoidance is, in general, not guaranteed. For instance, a manipulator with a stationary base cannot avoid a collision with an approaching obstacle. Similarly, a manipulator trapped in a deadlock might not be able to resolve the conflict with other noncooperative, fast-moving objects. Yet, the use of artificial potential field functions has been shown to be robust against non-threatening, slow-moving dynamic obstacles [Reference Rodríguez-Seda, Stipanović and Spong44]. The work in ref. [Reference Stipanović, Melikyan and Hovakimyan45] presents sufficient conditions for avoidance control laws based on artificial potential field-like functions.
3.3.6. Kinematic and dynamic constraints
Of practical interest is the consideration of kinematic and dynamic constraints, such as joint and control torque limits. Joint limitations can be handled as obstacles that move with the links and, in general, may not represent a threat of collision when interacting with cooperative objects, as these other robots or links will aim to avoid a collision. Dynamic constraints, on the other hand, can be handled by careful consideration of the control law. For example, consider a group of robotic manipulators with bounded control torques, that is,
$\exists T_{ij} \in (0,\infty )$
such that
$|{\tau }_{ij}| \leq T_{ij}$
for all
$i\in \{i,\cdots, N\},\ j\in \{1,\cdots, n_i\}$
. In addition, assume that the gravity torques are also bounded, that is,
$\exists G_{ij}\in [0,\infty )$
such that
$|{g}_{ij}| \leq G_{ij}$
. The latter property is satisfied by a wide range of robotic manipulators, including those with only revolute joints [Reference López-Araujo, Zavala-Río, Santibáñez and Reyes46]. Now, consider the following saturated control PD-type controller with gravity compensation:
\begin{align}{\tau }_i =\;& \textbf{g}_{i} -\textbf{sat}_i\left (A_i\tilde{\textbf{q}}_i + B_i\dot{\textbf{q}}_i\right ), & \mathrm{sat}_{ij}(a) = \begin{cases} a, & \mathrm{if}\ |a| \leq \omega _{ij} \\[5pt] \omega _{ij}\frac{a}{|a|}, & \mathrm{otherwise} \end{cases} \end{align}
where
$\textbf{sat}_i(\!\cdot\!)=[\mathrm{sat}_{i1}(\!\cdot\!), \cdots, \mathrm{sat}_{in_i}(\!\cdot\!)]^T$
is the saturation function and
$0 \lt \omega _{ij} \leq T_{ij} - G_{ij}$
are some control parameters. Note that
$|\tau _{ij}| \leq T_i$
$\forall \ i,j$
. Consider then the following positive-definite function:
\begin{align} \mathcal{W}_i = \frac{1}{2}\dot{\textbf{q}}_i^T M_i \dot{\textbf{q}}_i + \int _{\textbf{0}}^{\tilde{\textbf{q}}_i}\textbf{sat}_i(A_i\textbf{r})d\textbf{r}. \end{align}
In ref. [Reference Zavala-Río and Santibáñez47], it is shown that
$\dot{\mathcal{W}}_i \leq 0$
$\forall \ t\geq 0$
and that
$\textbf{q}_i\rightarrow \textbf{q}_i^d$
,
$\dot{\textbf{q}}_i \rightarrow \textbf{0}$
as
$t\rightarrow \infty$
. Using these results, one can prove the following statement.
Proposition 3 (Collision Avoidance with Bounded Inputs). Consider a group of
$N$
$n_i$
-DOF manipulators with control input given by:
\begin{align}{\tau }_i =\;& \textbf{g}_{i} -\textbf{sat}_i\left (A_i\tilde{\textbf{q}}_i + B_i\dot{\textbf{q}}_i\right ) - \sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} \end{align}
and define
\begin{align} \overline{u}_{ij} \;:\!=\;\textbf{e}_j^T\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}\textbf{u}_{ij}^{kl} \end{align}
as the
$i$
th robot’s
$j$
th joint’s total avoidance control law, where
$\textbf{e}_j$
is the zero vector with
$j$
th element 1. Assume that
$\left \|\textbf{p}_{ij}(0)-\textbf{p}_{kl}(0)\right \| \gt r_{ij}^{kl}$
for all
$i,k \in \mathbb{N},\ j\in \{1,\cdots, n_i\},\ l\in \mathbb{L}_{ij}^k$
and that
\begin{align} |\overline{u}_{ij}| &\leq U_{ij} = T_{ij}-G_{ij}-\omega _{ij}, &\forall \ i,j. \end{align}
Then,
$|\tau _{ij}| \leq T_{ij}$
and
$\left \|\textbf{p}_{ij}(t)-\textbf{p}_{kl}(t)\right \| \gt r_{ij}^{kl}$
$\forall \ t\geq 0$
.
Proof. The first statement follows from
\begin{align} |\tau _{ij}| \leq & |g_{ij}| + |\mathrm{sat}_{ij}(a_{ij}(q_{ij}-q_{ij}^d)+b_{ij}\dot{q}_{ij})| + |u_{ij}| \leq G_{ij} + \omega _{ij} + U_{ij} \leq T_{ij}. \end{align}
To prove the second statement, consider the following Lyapunov candidate function:
\begin{align} \mathcal{W}= \sum _{i=1}^N\mathcal{W}_i + \frac{1}{2}\sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k} V_{ij}^{kl}. \end{align}
Following the steps of the proof of [Reference Zavala-Río and Santibáñez47, Proposition 1], one can show that
\begin{align} \dot{\mathcal{W}}_i \leq & - \sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}{\textbf{u}_{ij}^{kl}}^T\dot{\textbf{q}}_i \end{align}
and, therefore
\begin{align} \dot{\mathcal{W}}=\;& \sum _{i=1}^N\dot{\mathcal{W}}_i - \sum _{i=1}^N\sum _{j=1}^{n_i}\sum _{k=1}^N \sum _{l\in \mathbb{L}_{ij}^k}{\textbf{u}_{ij}^{kl}}^T\dot{\textbf{q}}_i \leq 0. \end{align}
Using the same arguments as in Proposition 1, one can conclude that boundedness of
$\mathcal{W}$
implies boundedness of
$V_{ij}^{kl}$
, which in turns implies collision avoidance at all times.
Note that assumption (30) cannot always be enforced and, therefore, collisions might not be averted at all times. The work in ref. [Reference Rodríguez-Seda and Dawkins48] discusses alternative solutions that could potentially be adapted to robotic manipulators to guarantee collision avoidance in the presence of these control limitations. These include controlling the desired trajectory such that control bounds are enforced.
Projections of two cylinders in a plane and the minimum safe distance relative to their orientations.
3.3.7. Computational complexity
The proposed avoidance control framework defines the avoidance (or potential field) functions (11) with respect to a state-dependent radius (6). Since the radius is not constant, the avoidance control input (16b) includes as extra terms the partial derivatives of the radius, which increases the complexity of the algorithm. However, these terms, which are a function of the dimensions of the links and objects as well as the position and orientation, can be given in closed form and do not have to be derived online. In addition, the multi-agent control algorithm has time complexity
$\mathcal{O}(N^2\overline{n}^2)$
, where
$N$
is the number of robots and
$\overline{n}$
is the number of links per robot. In contrast, traditional artificial potential field functions that approximate links as an arrangement of spheres have a time complexity of
$\mathcal{O}(N^2\overline{n}_i^2m^2)$
, where
$m$
is the number of spheres per link.
4. Examples:
$n_i$
-manipulators with right cylindrical links
Herein, we will consider the case in which the links of the robotic manipulators can be approximated by right circular cylinders. Accordingly, consider two cylindrical bodies, namely the
$a$
th and
$b$
th links, as seen in Figure 2a. Let the lengths and diameters for the
$a$
th and
$b$
th links be given by
$(\ell _a,w_a)$
and
$(\ell _b,w_b)$
, respectively. We will now approximate the minimum safe distance of the link’s rectangular projections in all three orthogonal planes,
$xy$
,
$yz$
, and
$zx$
.
For generality, consider an orthogonal
$\chi \mu$
-plane, where
$\chi$
is the abscissa and
$\mu$
is the ordinate (refer to Figure 2b). Let
$\overline{\ell }_a$
and
$\overline{\ell }_b$
be the projected lengths along the abscissa and
$\overline{w}_a$
and
$\overline{w}_b$
the projected widths for the
$a$
th and
$b$
th links. Define
$\sigma _a$
and
$\sigma _b$
as the orientations of the links with respect to the
$\chi$
-axis and let
\begin{align} \beta _a^b\;:\!=\;\beta (\overline{\ell }_a,\overline{\ell }_b,\overline{w}_b,\sigma _a,\sigma _b) =\;& \frac{\overline{\ell }_a}{2} + \frac{\overline{\ell }_b}{2}\tilde{\alpha }_\varepsilon (\cos (\sigma _{a}-\sigma _{b})) + \frac{\overline{w}_{b}}{2}\tilde{\alpha }_\varepsilon (\sin (\sigma _{a}-\sigma _{b})) \end{align}
\begin{align} \gamma _a^b\;:\!=\;\gamma (\overline{\ell }_b,\overline{w}_a,\overline{w}_b,\sigma _a,\sigma _b) =\;& \frac{\overline{w}_{a}}{2} + \frac{\overline{\ell }_{b}}{2}\tilde{\alpha }_\varepsilon (\sin (\sigma _{a}-\sigma _{b})) + \frac{\overline{w}_{b}}{2}\tilde{\alpha }_\varepsilon (\cos (\sigma _{a}-\sigma _{b})) \end{align}
represent the combined length of the two links along the
$\chi$
- and
$\mu$
-axis, respectively, and where
$\tilde{\alpha }_\varepsilon (\!\cdot\!)$
is the smooth approximation of the minimum function defined in (10). Moreover, let
$(\chi _a,\mu _a)$
and
$(\chi _b,\mu _b)$
be the coordinates of the
$a$
th and
$b$
th link’s centers and define
$\varphi _{a}^b = \mathrm{atan2}(\mu _b-\mu _a,\chi _b-\chi _a)-\sigma _a$
as the relative orientation of the
$b$
th link’s center with respect to the
$a$
th link. Then, the equation of the rectangle with sides
$\beta _a^b$
and
$\gamma _a^b$
(rotated by
$\sigma _a$
) in polar coordinates
$(\varrho _a^b,\varphi _a^b)$
:
\begin{align} \zeta _a^b \;:\!=\; &\zeta (\overline{\ell }_a,\overline{\ell }_b,\overline{w}_a,\overline{w}_b,\sigma _a,\sigma _b,\varphi _a^b) = \tilde{\alpha }_\varepsilon (\gamma _{a}^{b}\cos \varphi _{a}^{b} + \beta _{a}^{b}\sin \varphi _{a}^{b}) \end{align}
\begin{align} \eta _a^b\;:\!=\;&\eta (\overline{\ell }_a,\overline{\ell }_b,\overline{w}_a,\overline{w}_b,\sigma _a,\sigma _b,\varphi _a^b) = \tilde{\alpha }_\varepsilon (\gamma _{a}^{b}\cos \varphi _{a}^{b} - \beta _{a}^{b}\sin \varphi _{a}^{b}) \end{align}
\begin{align} \varrho _{a}^{b} \;:\!=\; & \varrho (\overline{\ell }_a,\overline{\ell }_b,\overline{w}_a,\overline{w}_b,\sigma _a,\sigma _b,\varphi _a^b) = \frac{2\beta _{a}^{b}\gamma _{a}^{b}}{\zeta _{a}^{b} + \eta _{a}^{b} - 2\varepsilon } \gt 0. \end{align}
Figure 2c shows a representation of
$\varrho _a^b$
as a function
$\varphi _a^b$
for fixed parameters
$\overline{\ell }_a,\overline{\ell }_b,\overline{w}_a,\overline{w}_b,\sigma _a$
, and
$\sigma _b$
. Now, let us repeat the process but from the perspective of the
$b$
th link. That is, compute
$\beta _b^a \;:\!=\;\beta (\overline{\ell }_b,\overline{\ell }_a,\overline{w}_a,\sigma _b,\sigma _a)$
,
$\gamma _b^a\;:\!=\;\gamma (\overline{\ell }_b,\overline{\ell }_a,\overline{w}_a,\sigma _b,\sigma _a)$
,
$\zeta _b^a\;:\!=\;\zeta (\overline{\ell }_b,\overline{\ell }_a,\overline{w}_b,\overline{w}_a,\sigma _b,\sigma _a,\varphi _b^a)$
,
$\eta _b^a\;:\!=\;\eta (\overline{\ell }_b,\overline{\ell }_a,\overline{w}_b,\overline{w}_a,\sigma _b,\sigma _a,\varphi _b^a)$
, and
$\varrho _b^a\;:\!=\;\varrho (\overline{\ell }_b,\overline{\ell }_a,\overline{w}_b,\overline{w}_a,\sigma _b,\sigma _a,\varphi _b^a)$
using equations (35)-(38), where
$\varphi _b^a = \mathrm{atan2}(\mu _a-\mu _b,\chi _a-\chi _b)-\sigma _b$
. This yields the equation in polar coordinates (
$\varrho _b^a,\varphi _{b}^a$
) of the rectangle with sides
$\beta _b^a$
and
$\gamma _b^a$
, rotated by
$\sigma _b$
. Then, the minimum safe distance between the rectangular projection of both links in the
$\chi \mu$
-plane, denoted as
$\overline{r}_a^b$
, can be approximated from above by:
\begin{align} \overline{r}_a^b =\;& \overline{r}_b^a = \tilde{\lambda }_\delta (\varrho _{a}^{b},\varrho _{b}^{a}), & \mathrm{for\ some\ }\delta \gt 0 \end{align}
where
$\tilde{\lambda }_\delta$
is the approximation of the minimum function (8). Figure 2c illustrates how the minimum distance
$\overline{r}_a^b$
changes in magnitude as a function of
$\varphi _a^b$
.
4.1. Example: 2D manipulators
Consider a group of
$N$
$n_i$
-DOF planar manipulators in the
$xy$
-plane with link center coordinates
$\textbf{p}_{ij}=[x_{ij},y_{ij}]^T\in \mathbb{R}^2$
and orientation
$\boldsymbol{\theta }_{ij}=\phi _{ij}\in \mathbb{R}$
. We assume that the links can be approximated by right circular cylinders with length
$\ell _{ij}\gt 0$
and width
$w_{ij}\gt 0$
, with lengths aligned with the
$x$
-axis. Define distance formulas
$\varrho _{ij}^{kl}\;:\!=\;\varrho (\ell _{ij},\ell _{kl},w_{ij},w_{kl},\phi _{ij},\phi _{kl},\varphi _{ij}^{kl})$
and
$\varrho _{kl}^{ij}\;:\!=\;\varrho (\ell _{kl},\ell _{ij},w_{kl},w_{ij},\phi _{kl},\phi _{ij},\varphi _{kl}^{ij})$
using (38), where
$\varphi _{ij}^{kl} = \mathrm{atan2}(y_{kl}-y_{ij},x_{kl}-x_{ij} )-\phi _{ij}$
and
$\varphi _{kl}^{ij} = \mathrm{atan2}(y_{ij}-y_{kl},x_{ij}-x_{kl} )-\phi _{kl}$
. Then, one can approximate the minimum safe radius from above as:
\begin{align} r_{ij}^{kl} =\;& r_{kl}^{ij} = \tilde{\lambda }_\delta (\varrho _{ij}^{kl},\varrho _{kl}^{ij}), & \mathrm{for\ some\ }\delta \gt 0. \end{align}
To illustrate the performance of the control law, we now simulate the interaction of two distinct
$4$
-DOF revolute planar manipulators with parameters listed in Table I, where
$m_{ij}$
denotes the mass of the
$i$
th robot’s
$j$
th link. The manipulators implement the control law in (16), taking into account the safe interactions with a rigid wall at
$y=0\,(\mathrm{m})$
and the centers of each nonadjacent link. The control parameters are then given by
$A_i = \mathrm{diag}(9,6,4,4)$
,
$B_i=\mathrm{diag(8,8,8,8)}$
,
$\rho _{ij}^{kl}=0.25\,(\mathrm{m})$
,
$\varepsilon =0.02$
, and
$\delta =8$
, with desired configurations
$\textbf{q}_1^d =[\frac 12 \pi, -\frac 12 \pi, -\frac 12 \pi, \frac 12 \pi ]^T \,(\mathrm{rad})$
and
$\textbf{q}_2^d =[\frac 12 \pi, \frac 12 \pi, \frac 16 \pi, \frac 12 \pi ]^T \,(\mathrm{rad})$
.
Parameters for the planar manipulators.
Simulation of two
$4$
-DOF planar manipulators interacting with a wall. The top row (a)–(e) Illustrates the case of no avoidance control, the row in the middle (f)–(j) depicts the case of the proposed avoidance control, while the lower row (k)–(o) depicts the use of artificial potential field functions (11) with links approximated by a collection of spheres. The first and second robots are denoted in blue and green, respectively, with the desired configurations given in red. The sequential motion of both manipulators is illustrated by the transparent configurations time spaced by
$0.5\,(\mathrm{s})$
.
Vector norm of the configuration errors for both manipulators.
Vector norm of the control torques. Figures (c) and (d) depict the avoidance control torques for the first and second manipulators when avoiding self-collisions (thick line), collisions with the other manipulator (dotted line), and collisions with the wall (fine line).
The results for the case in which no-collision control is applied, that is,
$\textbf{u}_{ij}^{kl} \equiv \textbf{0}$
, are given in Figs. 3
(a)-(e). Note that the robots, illustrated in blue and green with desired configurations in red, encounter a collision with each other at
$t\approx 8\,(\mathrm{s})$
(see Fig. 3
(c)). The first robot collides with the wall at
$t\approx 8\,(\mathrm{s})$
and with itself moments later (see Fig. 3
(d)), before stabilizing at the final desired configuration.
The manipulators are then simulated with the proposed control strategy, with results illustrated in Figs. 3
(f)-(j). The static wall is simulated as another rectangular link, centered at
$x=y=0\,(\mathrm{m})$
, with zero orientation, and with length and width equal to
$6\,(\mathrm{m})$
and
$0.05 \,(\mathrm{m})$
. Note that the robotic manipulators can successfully converge to the desired configurations while avoiding collisions with each other, with themselves, and with the wall.
For the sake of comparison, Figs. 3 (k)– (o) show the sequential motion of the manipulators when the traditional artificial potential functions (11) are applied, and the links are approximated by a collection of spheres centered along the links’ axes. The number of spheres per link, as well as their radius, are given in Table I, where the location and radius were chosen such that the links are entirely covered without any overlapping. The manipulators avoid all collisions, but as seen in Figs. 3 (n) and 3 (o), the manipulators took a longer time to converge to their desired configurations.
Finally, Figs. 4 and 5 depict the norm of the tracking errors and control torques when using the proposed method and the traditional artificial potential field functions with spherical approximations. Note that, in general, the proposed avoidance control had a slightly faster convergence and lower torques. This might be due, in part, to the consideration of more artificial potential field functions when computing the control torques using the traditional approach, which approximates links and obstacles with a collection of spheres. In addition, the proposed continuously differentiable minimum safe distance can provide, in general, a tighter bound on the distance to a collision, reducing the interference of artificial potential field forces.
4.2. Example: 3D manipulators
Consider now a group of
$3-$
dimensional
$n_i$
-DOF manipulators with dynamics given by (5) and link coordinates
$\textbf{p}_{ij}=[x_{ij},y_{ij},z_{ij}]^T$
and
$\boldsymbol{\theta }_{ij}= [\phi _{ij}, \vartheta _{ij}, \psi _{ij}]^T$
. Once again, we assume that the links can be approximated by right circular cylinders with length
$\ell _{ij}$
and diameter
$w_{ij}$
and that their lengths are defined along the
$x$
-axis. To approximate the minimum distance, we will start by projecting the links into the
$xy$
-
$zx$
-, and
$yz$
-planes as rectangles and approximating the minimum safe distance in each case using (39).
Planar projections of a 3D cylinder into the
$xy$
-,
$yz$
-, and
$zx$
-planes.
Define
$R_x(a)$
,
$R_y(b)$
, and
$R_z(c)$
as the
$3\times 3$
orthogonal rotational matrices representing rotations by
$a$
,
$b$
, and
$c$
about the
$x$
-,
$y$
-, and
$z$
-axis, respectively. Define
\begin{align} \left [\begin{array}{c}\hat{x}_{ij} \\[5pt] \hat{y}_{ij} \\[5pt] \hat{z}_{ij}\end{array}\right ] = R_z(\phi _{ij})R_y(\vartheta _{ij})R_x(\psi _{ij})\left [\begin{array}{c}\ell _{ij}\\[5pt] 0 \\[5pt] 0\end{array}\right ] \end{align}
which represents the
$(x,y,z)$
coordinates of the line
$\vec{\ell }_{ij}$
passing through the center of the cylinder after the three rotations, as denoted in Figure 6a. Then, the projected lengths and orientations of the links in all three Cartesian planes can be approximated from above by:
\begin{align} \overline{\ell }_{ij} =\;& \sqrt{\hat{x}_{ij}^2 + \hat{y}_{ij}^2 + \varepsilon ^2} + \frac{w_{ij} \tilde{\alpha }_\varepsilon (\hat{z}_{ij})}{\ell _{ij}}, & \overline{\sigma }_{ij} =\;& \mathrm{atan2}(\hat{y}_{ij},\hat{x}_{ij}) \end{align}
\begin{align} \overline{\overline{\ell }}_{ij} =\;& \sqrt{\hat{x}_{ij}^2 + \hat{z}_{ij}^2 + \varepsilon ^2} + \frac{w_{ij} \tilde{\alpha }_\varepsilon (\hat{y}_{ij})}{\ell _{ij}}, & \overline{\overline{\sigma }}_{ij} =\;& -\mathrm{atan2}(\hat{z}_{ij},\hat{x}_{ij}) \end{align}
\begin{align} \overline{\overline{\overline{\ell }}}_{ij} =\;& \sqrt{\hat{y}_{ij}^2 + \hat{z}_{ij}^2 + \varepsilon ^2} + \frac{w_{ij} \tilde{\alpha }_\varepsilon (\hat{x}_{ij})}{\ell _{ij}}, & \overline{\overline{\overline{\sigma }}}_{ij} =\;& \mathrm{atan2}(\hat{z}_{ij},\hat{y}_{ij}) \end{align}
for some small
$\varepsilon \gt 0$
, where (
$\overline{\ell }_{ij},\overline{\sigma }_{ij}$
), (
$\overline{\overline{\ell }}_{ij},\overline{\overline{\sigma }}_{ij}$
), and (
$\overline{\overline{\overline{\ell }}}_{ij},\overline{\overline{\overline{\sigma }}}_{ij}$
) denote the lengths and orientations of the links when projected to the
$xy$
-,
$zx$
-, and
$yz$
-planes, respectively. In what follows, we will use the accents
$\scriptstyle -$
,
$\scriptstyle =$
, and
$\scriptstyle \equiv$
to denote variables in the
$xy$
-,
$yz$
-, and
$zx$
-planes. The addition of
$\varepsilon$
and
$\tilde{\alpha }_\varepsilon (\!\cdot\!)$
in the approximations is to guarantee that the projected lengths are continuous differentiable functions. The width of the rectangular shapes, on the other hand, is always equal to the diameter of the cylinder,
$w_{ij}$
. Fig. 6 illustrates an example of the equivalent rectangular shapes projected in each plane.
Once the dimensions and orientations of each pair of links in each Cartesian plane is defined, one can approximate the minimum safe distance between two projected links in
$xy$
-,
$zx$
-, and
$yz$
-plane as:
\begin{align} \overline{r}_{ij}{}^{\!\!\!kl} =\;& \tilde{\lambda }_\delta (\overline{\varrho }_{ij}{}^{\!\!\!kl}, \overline{\varrho }_{kl}{}^{\!\!\!ij}), & \overline{\overline{r}}_{ij}{}^{\!\!\!kl} =\;& \tilde{\lambda }_\delta (\overline{\overline{\varrho }}_{ij}{}^{\!\!\!kl}, \overline{\overline{\varrho }}_{kl}{}^{\!\!\!ij}), & \overline{\overline{\overline{r}}}_{ij}{}^{\!\!\!kl} =\;& \tilde{\lambda }_\delta (\overline{\overline{\overline{\varrho }}}_{ij}{}^{\!\!\!kl}, \overline{\overline{\overline{\varrho }}}_{kl}{}^{\!\!\!ij}) \end{align}
for some large
$\delta \gt 0$
, where
$\tilde{\lambda }_\delta (\!\cdot\!)$
is the minimum approximation function from above and
for 
. Now, let’s define the minimum safe distance (in 3D) between the centers of
$ij$
th and
$kl$
th links along the vector
$\textbf{p}_{ij}-\textbf{p}_{kl}$
as:
\begin{align} r_{ij}^{kl} = r_{kl}^{ij} = h\left \|\textbf{p}_{ij}-\textbf{p}_{kl}\right \| \end{align}
where
$h\gt 0$
. To ensure that the links do not collide, one needs to guarantee that at least one of the Cartesian plane’s minimum safe distance is satisfied. Mathematically, this means that one of the following inequalities holds
\begin{align} h\cdot \left ((x_{ij}-x_{kl})^2 + (y_{ij}-y_{kl})^2\right ) & \geq (\overline{r}_{ij}^{kl})^2 \end{align}
\begin{align} h\cdot \left ((x_{ij}-x_{kl})^2 + (z_{ij}-z_{kl})^2\right ) & \geq (\overline{\overline{r}}_{ij}{}^{\!\!\!kl})^2 \end{align}
\begin{align} h\cdot \left ((y_{ij}-y_{kl})^2 + (z_{ij}-z_{kl})^2\right ) & \geq (\overline{\overline{\overline{r}}}_{ij}{}^{\!\!\!kl})^2. \end{align}
Accordingly, one can pick
$h$
as:
\begin{align} h &= \left (\tilde{\Lambda }_\delta \left (\frac{(x_{ij}-x_{kl})^2 + (y_{ij}-y_{kl})^2}{(\overline{r}_{ij}{}^{\!\!\!kl})^2}, \frac{(x_{ij}-x_{kl})^2 + (z_{ij}-z_{kl})^2}{(\overline{\overline{r}}_{ij}{}^{\!\!\!kl})^2}, \frac{(y_{ij}-y_{kl})^2 + (z_{ij}-z_{kl})^2}{(\overline{\overline{\overline{r}}}_{ij}{}^{\!\!\!kl})^2} \right )\right )^{-1} \end{align}
where
$\tilde{\Lambda }_\delta (\!\cdot\!)$
is the smooth approximation of the maximum function from above (9). Note that
$\overline{r}_{ij}^{kl}$
,
$\overline{\overline{r}}_{ij}^{kl}$
, and
$\overline{\overline{\overline{r}}}_{ij}^{kl}$
are by definition larger than the cross-sectional radius of the links, that is, larger than
$(w_{ij}+w_{kl})/2 \gt 0$
, and therefore,
$h$
and
$r_{ij}^{kl}$
are well defined and continuously differentiable.
An example with two identical
$5$
-DOF revolute robotic manipulators is now presented. The system parameters for the robots are given in Table II. The robots are assumed to be tasked with lifting an object centered at
$(x,y,z)=(0.5,0,-0.3) \,(\mathrm{m})$
. Accordingly, the desired configurations are chosen as
$\textbf{q}_1^d =[1.13, -2.23, 2.59, 1.44, 3.01]^T \,(\mathrm{rad})$
and
$\textbf{q}_2^d =[0.32, -2.23, 2.59, 1.44, 3.01]^T \,(\mathrm{rad})$
. The robots must also avoid a wall located at
$x=-0.15\,(\mathrm{m})$
.
Parameters for the 3D manipulators.
The simulation results with the proposed avoidance control are given in Fig. 7. The control parameters are chosen as
$A_i=\mathrm{diag}(15,15,12,12,12)$
,
$B_i=\mathrm{diag(8,8,8,8,8)}$
,
$\rho _{ij}^{kl}=0.25\,(\mathrm{m})$
,
$\varepsilon =0.02$
, and
$\delta =8$
. As can be observed from the snapshots, both manipulators stayed away from collisions and converged to the desired configuration in less than 15 s.
Simulation of two identical three-dimensional robots with proposed avoidance control. The desired configurations are illustrated in red, with the simulated object to be picked up in yellow.
Simulation of two identical three-dimensional robots using spherical approximations for the links and the wall. The desired configurations are illustrated in red, with the simulated object to be picked up in yellow.
We also simulated the robotic manipulators using spherical approximations and applying the traditional artificial potential field approach (11) with a constant radius. Given the fact that the spherical approximation augments the size of the links, a slightly smaller safety range of
$\rho _{ij}^{kl}=0.2 \,(\mathrm{m})$
was chosen to ease mobility. The results are shown in Fig. 8. Note that the robots avoided collisions but took longer to converge. In fact, after
$15\,(\mathrm{s})$
, the robots have not quite stabilized yet at the desired position.
For comparison, Figs. 9 and 10 illustrate the norm of tracking error in generalized coordinates and the control torques for both manipulators, respectively. Note that while the manipulators under the proposed avoidance control converged to the desired configuration in approximately
$10 \,(\mathrm{s})$
, it took more than
$50\,(\mathrm{s})$
for both manipulators to converge under the traditional use of spherical approximations. This can be attributed to the interference with the desired task by several repulsive field forces generated by multiple spheres. In addition, the proposed continuously differentiable minimum safe distance may provide a tighter distance to collision bound than the approximation of elongated obstacles with spheres, increasing the maneuverability of links through obstructed spaces. Finally, the control torques under the traditional approach are also significantly higher than with the proposed avoidance control. The latter can also be attributed to the interaction of multiple repulsive potential field functions. It is worth noting that the spikes in the magnitude of the control torques are due to the repulsive forces, which tend to rapidly increase when the distance between the link and the obstacle approaches zero.
Vector norm of the configuration errors for both 5-DOF manipulators.
Vector norm of the control torques for both 5-DOF manipulators. Figures (c) and (d) depict the avoidance control torques for the first and second manipulators when avoiding self-collisions (thick line), collisions with the other manipulator (dotted line), and collisions with the wall (fine line).
4.3. Example: 3D manipulators with bounded torques
Next, we consider the case of robotic manipulators with bounded control torques.
Consider the two-robot system from Section 4.2 with parameters given in Table II. Assume now that the control torque for each link is bounded by
$T_{i1}=T_{i2}=100\,(\mathrm{Nm})$
,
$T_{i3}=80\,(\mathrm{Nm})$
, and
$T_{i4}=T_{i5}=60\,(\mathrm{Nm})$
for
$i\in \{1,2\}$
and that the system implements the saturated PD-type control with gravity compensation and collision avoidance strategy from Section 3.3.6, equation (28). Let the control parameters for the minimum safety distance and PD control be the same as in Section 4.2 but with saturation bounds for the PD control given by
$\omega _{ij}=\frac{1}{2}T_{ij}$
$\forall \ i,j$
. Similarly, let the bounds on the gravitational torques be
$G_{i1}=G_{i2}=0\,(\mathrm{Nm})$
,
$G_{i3}= 30\,(\mathrm{Nm})$
,
$G_{i4}=16 \,(\mathrm{Nm})$
, and
$G_{i5}=6 \,(\mathrm{Nm})$
, which have been obtained by numerical estimation. Finally, since the avoidance control term in (28) can exceed the manipulators’ input control bounds, the total input torque
$\tau _{ij}$
is further saturated at
$-T_{ij}$
and
$T_{ij}$
whenever it exceeds these bounds.
Simulation of two identical three-dimensional robots with bounded control torques and proposed avoidance control. The desired configurations are illustrated in red, with the simulated object to be picked up in yellow.
The simulation results under the saturated PD-type control with gravity compensation and collision avoidance strategy are illustrated in Fig. 11. Note that both manipulators are able to converge safely to the desired configurations within
$15\,(\mathrm{s})$
, similar to the use of the unbounded control law as presented in Section 4.2. Fig. 12 compares the implementation of both controllers in terms of tracking error. Observe that the responses of the unsaturated control (16) and the saturated one (28) exhibit similar behaviors, with the saturated PD-type control taking just slightly longer to converge.
Norm of links’ control torques for the first robot (left-side) and second robot (right-side).
Fig. 13 shows the magnitude of the total control torque (i.e.,
$|\tau _{ij}|$
), the saturated PD-type control (i.e.,
$|\mathrm{sat}_{ij}(a_{ij}\tilde{q}_{ij}+b_{ij}\dot{q}_{ij})|$
), the gravitational torque (i.e.,
$|g_{ij}|$
), and the total collision avoidance control torque (i.e.,
$|\overline{u}_{ij}|$
) for each manipulator’s joint. Note that the total control torque remains within its bound
$T_{ij}$
at all times, reaching the maximum only when the avoidance control is also large. These maximum, rapidly increasing control torque values result from the repulsive forces when a link rapidly approaches an obstacle and are to be expected among potential field-based methods. Note that the saturated PD-type control torque remains within its bound
$\omega _{ij}$
, and the sum of the gravitational compensation torque and the PD-type control never exceeds the total control bound. The avoidance control also remains within the required bound
$U_{ij}=T_{ij}-G_{ij}-\omega _{ij}$
for guaranteed collision avoidance most of the time except at critically safety instances when the links come close to a collision. Nevertheless, both manipulators are able to avoid collisions at all times.
5. Conclusions and Future Work
This paper presented a cooperative, distributed avoidance control law for multiple robotic manipulators. The control strategy builds on the concepts of potential field functions and approximates the links and obstacles as shapes of smooth radii. However, in contrast to previous work, the proposed avoidance control law reduces the conservatism of other potential field methods by avoiding the traditional spherical and elliptical shape assumptions. Similarly, the control input torques are smooth, well-defined, closed-form functions, simplifying the implementation and avoiding the discontinuities of other shortest distance-based methods. Furthermore, the control approach is also rigorously shown to prevent collisions at all times using Lyapunov analysis. Extensions to robotic manipulators with bounded control torques and other Lyapunov-based tracking controllers are also given. Simulation examples with 2D and 3D revolute manipulators demonstrate the performance of the avoidance control strategy in comparison to traditional spherical approximations.
Author contributions
E. Rodriguez-Seda conceived the general idea, designed the control framework, developed the theory, conducted the simulations and analysis, and prepared the first draft of the manuscript. M. Kutzer contributed to the motivation and conception of the control problem, provided critical expertise on robotic manipulator dynamics and simulation, and revised the manuscript.
Financial support
This research was supported by the Office of Naval Research FY23 Grant No. N0001423WX00850 and N0001423WX02035, and FY24 Grant No. N0001424WX00524 and N0001424WX01610.
Competing interests
The authors declare no conflicts of interest exist.
Ethical approval
None.
Open access
