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:

(1) \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}$ :

(2) \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:

(3) \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}$ :

(4) \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:

(5) \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].

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:

(6) \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

(7) \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

(8) \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}

(9) \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}

(10) \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.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:

(11) \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

(12) \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

(13) \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

(14) \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

(15) \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:

(16a) \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}

(16b) \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.

Proof. Consider the Lyapunov candidate function:

(17) \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

(18) \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:

(19) \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

(20) \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:

(21) \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

(22) \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

(23) \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:

(24) \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

(25) \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.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:

(26) \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:

(27) \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:

(28) \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

(29) \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

(30) \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

(31) \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:

(32) \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

(33) \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

(34) \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.

Figure 2. Projections of two cylinders in a plane and the minimum safe distance relative to their orientations.

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:

(40) \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})$ .

Table I. Parameters for the planar manipulators.

Figure 3. 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})$ .

Figure 4. Vector norm of the configuration errors for both manipulators.

Figure 5. 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).

Figure 6. 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

(41) \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:

(42a) \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}

(42b) \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}

(42c) \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:

(43) \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

(44a)

(44b)

(44c)

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:

(45) \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

(46a) \begin{align} h\cdot \left ((x_{ij}-x_{kl})^2 + (y_{ij}-y_{kl})^2\right ) & \geq (\overline{r}_{ij}^{kl})^2 \end{align}

(46b) \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}

(46c) \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:

(47) \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})$ .

Table II. 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.

Figure 7. 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.

Figure 8. 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.

Figure 9. Vector norm of the configuration errors for both 5-DOF manipulators.

Figure 10. 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.

Figure 11. 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.

Figure 12. Comparison of the norm of the tracking error, $\|\tilde{\textbf{q}}_1\|+\|\tilde{\textbf{q}}_2\|$ , when using the unbounded PD-type control (16a) and the saturated PD-type control (28).

Figure 13. 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.