Adaptive Obstacle Avoidance for a Class of Collaborative Robots

Abstract

In a human–robot collaboration scenario, operator safety is the main problem and must be guaranteed under all conditions. Collision avoidance control techniques are essential to improve operator safety and robot flexibility by preventing impacts that can occur between the robot and humans or with objects inadvertently left within the operational workspace. On this basis, collision avoidance algorithms for moving obstacles are presented in this paper: inspired by algorithms already developed by the authors for planar manipulators, algorithms are adapted for the 6-DOF collaborative manipulators by Universal Robots, and some new contributions are introduced. First, in this work, the safety region wrapping each link of the manipulator assumes a cylindrical shape whose radius varies according to the speed of the colliding obstacle, so that dynamical obstacles are avoided with increased safety regions in order to reduce the risk, whereas fixed obstacles allow us to use smaller safety regions, facilitating the motion of the robot. In addition, three different modalities for the collision avoidance control law are proposed, which differ in the type of motion admitted for the perturbation of the end-effector: the general mode allows for a 6-DOF perturbation, but restrictions can be imposed on the orientation part of the avoidance motion using 4-DOF or 3-DOF modes. In order to demonstrate the effectiveness of the control strategy, simulations with dynamic and fixed obstacles are presented and discussed. Simulations are also used to estimate the required computational effort in order to verify the transferability to a real system.

1. Introduction

As robot technology has advanced, new types of robots with improved safety functions have been developed. Nowadays, the trend toward collaborative robotics is growing as a consequence of more dynamic and flexible industrial scenarios. Collaborative robotics in the industrial systems allow robots and humans to share the workspace and interact to accomplish collaborative tasks; an overview of this trending topic can be found in [1,2]. From [3,4,5], it emerges that safety issues are central when the coexistence or collaboration between humans and robots is expected, which is increasing the need for standards and methods used to validate and certify the safety of a collaborative application before it can be installed at the industrial site. Moreover, since collaborative robotic systems should be configured to assist the operator during their work, they are often equipped with sensor systems able to perceive the human motion and to detect the position of objects inside the workspace that may generate a risk of collision with the manipulator. Simple 2D cameras can be used for detecting [6] and predicting [7] the motion of the human arm on a working surface, or to calibrate the user-frame of the robot [8]. Vision systems able to generate depth space images are typically used to perceive the human pose in the 3D space [9,10,11]; data from such sensors can be easily elaborated to extrapolate the coordinates of skeleton models replicating the human motion [12,13]. Other types of optical sensors can be used, e.g., eye tracking systems able to detect the points of the screen on which the human is focusing its sight, translating such information into a command for the robot [14].

In addition to dedicated hardware and design principles, collaborative robotics implies specific control strategies, such as collision avoidance control techniques for dynamically varying environments: once fixed or moving obstacles are detected inside the workspace, the control must be able to modify in real-time the motion of the robot in order to avoid impacts at any point of its kinematic chain. Fixed obstacles can be thought of as objects inadvertently left inside the workspace, such as furniture elements or mechanical tools, whereas dynamic moving obstacles may represent humans operating in the shared workspace.

Several methods are available in the literature for collision avoidance; a common approach exploits the same principles of the artificial potential fields method, typically used for the path planning of mobile robots [15,16,17]: a potential field can be associated with each obstacle, so that a repulsive velocity inversely proportional to the distance from that obstacle is assigned to the control points along the kinematic chain of the manipulator. Thus, in addition to the motion imposed to the end-effector, a repulsive velocity vector can be applied to the point of the robot that is closer to one of the obstacles, adding a task to the control system [18,19,20,21,22,23,24,25]. This kind of approach was studied by the authors in [26,27] for a planar case; in addition to the standard method, a modified repulsive velocity method was introduced, which depended on the velocity, in addition to the position, of the obstacle, improving the capability of the algorithm to compensate for moving obstacles. Starting from this stage of the research, the present paper adapts the collision avoidance algorithms to a class of 6-DOF industrial manipulators, i.e., the Universal Robots collaborative robots family [28]. Furthermore, some significant new contributions are introduced:

• While in previous works the safety regions enveloping the structure of the robot were defined as spheres centred on a series of control points equally spaced along the serial kinematic chain, a more efficient strategy is proposed here defining only one safety volume for each link; such volume is the combination of a cylinder, with a height equal to the length of the link, and two hemispheres at the extremities of the link.
• In order to adapt the algorithm to different cases, with fixed or moving obstacles, the radius of the cylindrical/hemispherical safety volume of each link is variable, as a function of the obstacle velocity: a small radius is adopted for fixed obstacles, facilitating the mobility of the robot, whereas the radius increases with the speed of the obstacle, so that the risk of collision is reduced for the dynamic case.
• Three different modalities for the collision avoidance control law are proposed, which differ in the type of motion admitted for the perturbation of the end-effector. The first mode, the most general, allows for a 6-DOF perturbation of the end-effector, whose pose may vary in position and orientation in order to avoid a collision. If such condition is not admissible due to the specific application, some restrictions can be imposed. As an example, in this paper, two additional modes characterized by a reduced mobility of the perturbation are proposed. The second mode allows for a 4-DOF Schoenflies-like perturbation, where all translations and the rotation about the vertical axis are admitted. If the last rotation is not admitted, the third mode can be used, where the orientation of the end-effector remains unchanged during the perturbation due to a collision avoidance.

Details on the abovementioned formulation of the control law and the implementation of the three modalities are given in Section 2. Section 3 presents a series of simulations performed in order to test the algorithms and to verify the transferability to a real system, in terms of computational time. Results and insights on future works are then discussed in the conclusion (Section 4).

2. Motion Control Law

Starting from results obtained in previous studies [26], in this paper, the algorithms for collision avoidance are extended and implemented for Universal Robots manipulators; the UR5 robot is used as test case, but the implementation to other UR manipulators is straightforward due to the identical kinematic structure, except for the lengths of the links. The kinematic chain, characterized by eight control points ($\mathbf{A},\mathbf{B},\mathbf{C},{\mathbf{C}}_{\mathbf{1}},\mathbf{D},\mathbf{E},\mathbf{F},\mathbf{G}$), is shown in Figure 1, whereas

Table 1. Kinematic parameters for the UR5 robot.

${\mathit{d}}_1\left[\mathbf{m}\right]$	${\mathit{s}}_1\left[\mathbf{m}\right]$b	${\mathit{a}}_2\left[\mathbf{m}\right]$	${\mathit{s}}_3\left[\mathbf{m}\right]$	${\mathit{a}}_3\left[\mathbf{m}\right]$	${\mathit{d}}_4\left[\mathbf{m}\right]$	${\mathit{d}}_5\left[\mathbf{m}\right]$	${\mathit{d}}_6\left[\mathbf{m}\right]$
0.09	0.14	0.42	0.12	0.39	0.11	0.09	0.05

summarizes the dimensions of the links. The 6-DOF mobility is conferred by all revolute joints: the base (${\theta }_1$), the shoulder (${\theta }_2$), the elbow (${\theta }_3$) and three wrist joints (${\theta }_4,{\theta }_5,{\theta }_6$). They are the components of the joint space position vector: $\mathbf{q}={\left[{\theta }_1{\theta }_2{\theta }_3{\theta }_4{\theta }_5{\theta }_6\right]}^T$. In the Cartesian space, according to the Euler angle ZYZ convention, the pose of the end-effector is defined by $\mathbf{x}={[xyz\alpha \beta \gamma ]}^T$. The forward kinematics of the manipulator are described by:

$$\mathbf{x}=\mathbf{f}\left(\mathbf{q}\right)$$

(1)

$$\dot{\mathbf{x}}=\left[\begin{array}{c}{\dot{\mathbf{x}}}_p\\ \omega \end{array}\right]=\left[\begin{array}{c}{\mathbf{J}}_p\\ {\mathbf{J}}_o\end{array}\right]\dot{\mathbf{q}}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}$$

(2)

where $\mathbf{f}$ is the expression of the position forward kinematics; $\dot{\mathbf{x}}$ is the velocity vector composed by the linear velocity vector ${\dot{\mathbf{x}}}_p$ and the angular velocity $\omega$; $\mathbf{J}$ is the $(6\times 6)$ analytic Jacobian matrix, composed by ${\mathbf{J}}_p$ and ${\mathbf{J}}_o$, which are the $(3\times 6)$ position and orientation Jacobian matrices, respectively. The inverse velocity problem can be solved by:

$$\dot{\mathbf{q}}=\mathbf{J}{\left(\mathbf{q}\right)}^{-1}\dot{\mathbf{x}}$$

(3)

Nevertheless, a damped least-square strategy [29] is adopted to prevent singularity problems which appear when the manipulator is close to a singular configuration. According to [30], the damped inverse matrix can be obtained as:

$${\mathbf{J}}^{*}={\mathbf{J}}^{\mathbf{T}}{({\mathbf{JJ}}^{\mathbf{T}}+{\lambda }^2\mathbf{I})}^{-1}$$

(4)

where $\lambda$ is the damping factor, modulated as a function of the smaller singular value of the Jacobian matrix. In particular, if the smallest singular value ${\sigma }_{min}$ is higher than a predefined threshold $ϵ$, then the damped inverse law coincides with the standard inversion $(\lambda =0)$; otherwise, if ${\sigma }_{min}<ϵ$, then the value of $\lambda$ increases inversely to ${\sigma }_{min}$ up to a maximum value of ${\lambda }_{max}$.

Thus, knowing the initial joint position ${\mathbf{q}}_0$, the joint position law can be obtained by a discrete integration in time domain:

$$\mathbf{q}(t+\mathrm{d}t)={\mathbf{q}}_0\left(t\right)+\dot{\mathbf{q}}\left(t\right)\mathrm{d}t\dot{\mathbf{q}}={\mathbf{J}}^{*}\dot{\mathbf{x}}$$

(5)

In order to define an obstacle avoidance control strategy, it is necessary to detect the link of the manipulator with the highest risk of collision with the obstacles inside of the workspace. Thus, it is necessary to calculate the distance of each obstacle from each link, in order to detect the obstacle–link pair characterized by the minimum distance. Eight segments, delimited by the control points represented in Figure 1, are considered in this algorithm. The distance of an obstacle ${\mathbf{P}}_O$ from a link [20,31] is calculated according to the schemes of Figure 2, where ${\mathbf{d}}_l$ is the vector connecting the proximal to the distal extremity of the link; ${\mathbf{d}}_p$ and ${\mathbf{d}}_d$ are the distances between the obstacle and the proximal and distal extremities of the link, respectively; ${\mathbf{P}}_r$ is the point of the link closest to the obstacle. In detail:

(a)

If $\cos \alpha \ge 0$ and $cos\beta \ge 0$, then the distance ${\mathbf{d}}_o={\mathbf{P}}_r-{\mathbf{P}}_o$ is orthogonal to the link, and the position of ${\mathbf{P}}_r$ along the link is defined by the scalar parameter x:

$$d_o={\displaystyle \frac{|{\mathbf{d}}_l\times {\mathbf{d}}_p|}{d_l}}x={\displaystyle \frac{d_{p}cos\alpha }{d_l}},0<x<1$$

(6)

(b)

If $cos\beta <0$, then the point ${\mathbf{P}}_r$ is coincident with the distal extremity of the link, thus:

$${\mathbf{d}}_o={\mathbf{d}}_{d}x=1$$

(7)

(c)

If $cos\alpha <0$, then the point ${\mathbf{P}}_r$ is coincident with the proximal extremity of the link, thus:

$${\mathbf{d}}_o={\mathbf{d}}_{p}x=0$$

(8)

Figure 3 shows a condition conforming to the case (a). The obstacle influences the motion of the robot if the distance $d_o$ is lower than the radius r, which is a tunable parameter of the algorithm. Thus, the region of interest of each link can be visualized as a cylinder with two hemispheres at its extremities, all of radius r. The repulsive velocity ${\dot{\mathbf{x}}}_0$ is applied to ${\mathbf{P}}_r$, whose position is known from one of Equations (6)–(8). Only one of the links of the manipulator will be subjected to the repulsive velocity, i.e., the one which is closest to one of the obstacles.

The choice of a proper value for r is a critical issue for the tuning of the algorithm. A small radius leads to reduced regions of influence, with a limitation of the cases where an obstacle may interfere with the robot; on the other hand, for quickly moving obstacles, the required readiness of the anti-collision control may be too demanding for standard controllers. Thus, a larger radius is desirable for moving obstacles, whereas fixed obstacles can also be avoided with small regions of influence. Consequently, the strategy adopted in this work is to vary r according to the ramp plotted in Figure 4a, as a function of the velocity $v_o$ of the obstacle closest to the link [31]. A possible alternative could be to consider the relative speed between the obstacle and the robot instead of $v_o$, which takes into account different scenarios, such as a fixed manipulator with moving obstacles and vice versa, or obstacles and robots both in motion.

The control strategy used for the collision avoidance is based on an additional velocity task assigned the point ${\mathbf{P}}_r$ of the robot, in addition to the velocity imposed on the end-effector in order to execute the planned trajectory. Three different modalities are described in detail in the following subsections.

2.1. Mode I: 6-DOF Perturbation

The control law is based on a Closed-Loop Inverse Kinematics (CLIK) approach, with two velocity tasks imposed on the manipulator. The first task assigns the velocity ${\dot{\mathbf{x}}}_e$ to the end effector, by means of the analytic Jacobian $\mathbf{J}$:

$$\mathbf{J}\dot{\mathbf{q}}={\dot{\mathbf{x}}}_e\left[\begin{array}{c}{\mathbf{J}}_p\\ {\mathbf{J}}_o\end{array}\right]\dot{\mathbf{q}}=\left[\begin{array}{c}{\dot{\mathbf{x}}}_{ep}\\ \omega \end{array}\right]$$

(9)

The second task imposes the translation velocity ${\dot{\mathbf{x}}}_0$ assigned to the point ${\mathbf{P}}_r$ of the kinematic chain in order to avoid the collision; in this case, only the upper $(3\times 6)$ part of the Jacobian ${\mathbf{J}}_0$ associated with the linear velocity of ${\mathbf{P}}_r$ is used:

$${\mathbf{J}}_0=\left[\begin{array}{c}{\mathbf{J}}_{0p}\\ {\mathbf{J}}_{0o}\end{array}\right]{\mathbf{J}}_{0p}\dot{\mathbf{q}}={\dot{\mathbf{x}}}_0$$

(10)

The damped least square algorithm is applied to invert $\mathbf{J}$ and ${\mathbf{J}}_{0p}$, guaranteeing numerical stability even in proximity to singular configurations. Thus, the combinations of the velocity tasks expressed in Equations (9) and (10) give

$$\dot{\mathbf{q}}={\mathbf{J}}^{*}\left({\dot{\mathbf{x}}}_e+\mathbf{K}\mathbf{e}\right)+{\mathbf{J}}_{0p}^{*}{a}_v{v}_{rep}{\hat{\mathbf{d}}}_o$$

(11)

where:

• $\mathbf{K}$ is a positive-defined gain matrix, for simplicity set as $\mathbf{K}=k_e\mathbf{I}$.
• The position error $\mathbf{e}$ between the desired ${\mathbf{x}}_{\mathbf{e}}$ and actual position $\mathbf{x}$ is defined as [30]:

  $$\mathbf{e}=\left[\begin{array}{c}{\mathbf{e}}_p\\ {\mathbf{e}}_o\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_e-\mathbf{p}\\ {\displaystyle \frac{1}{2}}\left(\mathbf{n}\times {\mathbf{n}}_e+\mathbf{s}\times {\mathbf{s}}_e+\mathbf{a}\times {\mathbf{a}}_e\right)\end{array}\right]$$

  (12)

  where ${\mathbf{e}}_p$ is the translation error vector and ${\mathbf{e}}_o$ is the orientation error vector; the position and orientation of the end-effector are, respectively, expressed by the vector $\mathbf{p}$ and the rotation matrix $\mathbf{R}=\left[\mathbf{n}\mathbf{s}\mathbf{a}\right]$, with $\mathbf{n},\mathbf{s},\mathbf{a}$ being the unit vectors of the end-effector frame.
• The repulsive velocity ${\dot{\mathbf{x}}}_0$ is modulated as ${\dot{\mathbf{x}}}_0=a_v{v}_{rep}{\hat{\mathbf{d}}}_o$, where $v_{rep}$ is a tunable magnitude and $a_v$ is an activation factor that is function of the distance $d_o$ (Figure 4b) according to

  $$\begin{array}{c}\hfill a_v=\left\{\begin{array}{ccc}1\hfill & \hfill & d_o\le r_{min}\hfill \\ {\left({\displaystyle \frac{d_o-r}{r_{min}-r}}\right)}^2\hfill & \hfill & r_{min}<d_o<r\hfill \\ 0\hfill & \hfill & d_o\ge r\hfill \end{array}\right.\end{array}$$

  (13)

The first term of Equation (11) ensures the planned velocity of the end-effector with a minimum effort in terms of joints speeds; when there is a risk of collision, the second term is activated, generating a 6-DOF perturbation of the end-effector motion. The error is then compensated by the proportional corrective term when the risk of collision is exceeded.

2.2. Mode II: 4-DOF Schoenflies Perturbation

A 6-DOF perturbation of the end-effector motion, as in the previous case, could be unacceptable in some conditions, for example, if the orientation of the tool cannot vary during its manipulation. In this case, only a 4-DOF perturbation of the Schoenflies type is admitted. Thus, positioning errors due to translations along the three axes and a rotation about the vertical axis z are allowed. The control law is modified as follows:

$$\dot{\mathbf{q}}={\mathbf{J}}^{*}\left({\dot{\mathbf{x}}}_e+\mathbf{K}\mathbf{e}\right)+{\mathbf{J}}_{II}^{*}\left[\begin{array}{c}{a}_v{v}_{rep}{\hat{\mathbf{d}}}_o\\ {\mathbf{0}}_{2\times 1}\end{array}\right]$$

(14)

The Jacobian matrix of the mode II, of dimension $(5\times 6)$, is defined as:

$${\mathbf{J}}_{II}=\left[\begin{array}{c}{\mathbf{J}}_{0p}\\ {\mathbf{j}}_{o4}\\ {\mathbf{j}}_{o5}\end{array}\right]$$

(15)

where ${\mathbf{J}}_{0p}$ is the translation part of the Jacobian associated with ${\mathbf{P}}_r$, whereas ${\mathbf{j}}_{o4}$ and ${\mathbf{j}}_{o5}$ are the fourth and fifth rows of the orientation Jacobian matrix ${\mathbf{J}}_o$ of the end-effector, respectively. In this way, the second task assigns to ${\mathbf{P}}_r$ the velocity required to avoid the collision, imposing at the same time null angular velocity components along x and y axes for the consequent perturbation motion of the end-effector.

2.3. Mode III: Perturbation with Fixed Orientation

In this last case, a further restriction is given to the type of motion allowed for the end-effector perturbation: only translations are admitted, whereas the orientation must remain unchanged during the motion. Similarly to the previous case, it is:

$$\dot{\mathbf{q}}={\mathbf{J}}^{*}\left({\dot{\mathbf{x}}}_e+\mathbf{K}\mathbf{e}\right)+{\mathbf{J}}_{III}^{*}\left[\begin{array}{c}{a}_v{v}_{rep}{\hat{\mathbf{d}}}_o\\ {\mathbf{0}}_{3\times 1}\end{array}\right]$$

(16)

Now, the Jacobian matrix of the second term has a $(6\times 6)$ dimension; it is composed by the translation part of ${\mathbf{J}}_0$ and the orientation part of $\mathbf{J}$:

$${\mathbf{J}}_{III}=\left[\begin{array}{c}{\mathbf{J}}_{0p}\\ {\mathbf{J}}_o\end{array}\right]$$

(17)

Thus, a null angular velocity is imposed on the perturbation term of the control law.

3. Simulations

Algorithms were tested in Matlab in a series of simulations using in all cases a common set of parameters, summarized in

Table 2. Parameters values used for simulations.

${\mathit{r}}_{\mathit{inf}}\left[\mathbf{m}\right]$	${\mathit{r}}_{\mathit{sup}}\left[\mathbf{m}\right]$	${\mathit{v}}_{\mathit{inf}}[\mathbf{m}/\mathbf{s}]$	${\mathit{v}}_{\mathit{sup}}[\mathbf{m}/\mathbf{s}]$	$\mathbf{d}\mathit{t}\left[\mathbf{s}\right]$	${\mathit{k}}_{\mathit{e}}$	$\mathbf{T}\left[\mathbf{s}\right]$	${\dot{\mathit{\theta }}}_{\mathit{max}}[\mathbf{rad}/\mathbf{s}]$
0.09	0.15	0.1	0.5	${10}^{-3}$	20	2	$\pi$

. A limit ${\dot{\theta }}_{max}$ of the angular velocity was imposed to all the joints as a saturation threshold, in order to avoid high speeds at joints that may occur when a collision avoidance task is planned in the Cartesian space.

The error due to saturation was subsequently recovered by the proportional corrective term of the control law.

In the first three examples, illustrated in Figure 5, Figure 6 and Figure 7, the robot holds in a fixed position, whereas a moving obstacle will interfere with its end-effector. Such obstacle has a linear velocity of $0.25$ m/s along the y direction. Six steps of the motion are plotted, from 0 to $T=2\mathrm{s}$; when the obstacle reaches the region of influence of the end effector $(t\simeq 0.6\mathrm{s})$, the radius of the safety region increases, due to the speed of the obstacle, and the robot starts to react to keep the obstacle outside that area.

In the first example (Figure 5), the robot avoids the obstacle with the mode I, thus with a 6-DOF perturbation of the end-effector pose. In the second example (Figure 6) the control law is in mode II, thus the robot reacts with a 3-DOF translation and a rotation about z, as clearly visible at $t=1.3\mathrm{s}$, whereas the other angular velocity components are constantly zero. The example in Figure 7 refers to the mode III: the manipulator avoids the obstacle, maintaining the orientation of the end-effector.

The profiles of joints rotations and speeds related to the abovementioned examples are plotted in Figure 8, Figure 9 and Figure 10. In all cases, the saturation of joint speed is reached for at least one motor. Moreover, mode I (Figure 8) provides a response where all motors are quite uniformly recruited for the anti-collision task; transitioning into mode II and III, the role of motors 4 and 5 is reduced.

In order to compare the effort required of the motors in the three different modes, the norm of the joints angular velocity vector $\left|\right|\dot{\mathbf{q}}\left|\right|$ and the norm of the joints angular acceleration vector $\left|\right|\ddot{\mathbf{q}}\left|\right|$ were analyzed, and their maximum values assumed during the motion, $\left|\right|\dot{\mathbf{q}}{\left|\right|}_{max}$ and $\left|\right|\ddot{\mathbf{q}}{\left|\right|}_{max}$, respectively, were used as indexes.

Table 3. Comparison between different modes. Example of Figure 5, Figure 6 and Figure 7.

Mode	T	L	$\left|\right|\dot{\mathbf{q}}{\left|\right|}_{\mathit{max}}$	$\left|\right|\ddot{\mathbf{q}}{\left|\right|}_{\mathit{max}}$	Avg. Comp. Time
	$\left[\mathbf{s}\right]$	$\left[\mathbf{mm}\right]$	$[\mathbf{rad}/\mathbf{s}]$	$[\mathbf{rad}/{\mathbf{s}}^2]$	(1 Cycle) [ms]
I	2	486	5.58	180.27	0.79
II	2	491	4.68	180.27	0.82
II	2	491	4.96	180.27	0.86

shows the results of the simulations: the three modes are equivalent in terms of maximum acceleration; however, the length of the path is slightly lower for mode I, with a consequent higher value of maximum joint speed. The three modes are also substantially equivalent in terms of computational time, referring to the time required to compute a single iteration of the control loop. A further comparison of the modes should be conducted considering different metrics, such as the power consumption or torque peaks; thus, a dynamical modeling of the robot in order to find an optimal solution will be the object of future work.

Finally, an example of obstacle avoidance in mode I is given in Figure 11 considering the following scenario: the robot moves on a linear path, while three obstacles will interfere; two of them are fixed, and the other moves along x at a speed of $0.1\mathrm{m}/\mathrm{s}$. The first obstacle encountered will collide with the end-effector at approximately $t=1.6\mathrm{s}$; due to the obstacle speed, the radius of the region of interest is increased, reducing the risk of collision. At $t\simeq 2.8\mathrm{s}$, the effect of the second obstacle is visible: the radius r is smaller in this case, being the fixed obstacle. The third obstacle, also fixed, is reached at $t\simeq 3.2\mathrm{s}$; its effect is to push downwards on the 5th link of the manipulator, generating an error in the trajectory of the end-effector that is suddenly recovered to reach the correct final position at $t=5\mathrm{s}$.

For this example, a quantitative comparison is also presented between the three different control modalities (the trajectories obtained with modes II and III are not reported for the sake of brevity, but are visible on the videos provided as Supplementary Material).

Table 4. Comparison between different modes. Example of Figure 11.

Mode	T	L	$\left|\right|\dot{\mathbf{q}}{\left|\right|}_{\mathit{max}}$	$\left|\right|\ddot{\mathbf{q}}{\left|\right|}_{\mathit{max}}$	Avg. Comp. Time
	$\left[\mathbf{s}\right]$	$\left[\mathbf{mm}\right]$	$[\mathbf{rad}/\mathbf{s}]$	$[\mathbf{rad}/{\mathbf{s}}^2]$	(1 Cycle) [ms]
I	2	1219	3.85	171.02	0.79
II	2	1176	3.30	171.02	0.82
II	2	1176	4.13	171.02	0.84

shows that in this case, modes II and III give the shortest path, whereas mode II is also the most advantageous in terms of maximum speed. There are no differences for the accelerations, as for the computational times, which are very similar. In summary, considering the results collected in Table 3 and Table 4, the three modes are very similar in terms of motor effort. Therefore, the control mode must be selected according to the application: if, for example, a potentially dangerous instrument is moved, it may be advisable to maintain it in a fixed orientation, avoiding potential collisions or exposures to the face of the collaborating human. On the other hand, if the orientation of the end-effector is not a critical issue, the control mode I can be selected to increase the ability to avoid collisions; in fact, the greater number of degrees of freedom available to create repulsive motions helps in general to avoid obstacles.

In addition to verifying the correctness of the method, the simulations presented in this section also have the purpose of evaluating the computational effort required to implement the related algorithms in a real system. In this regard, the final objective of this research is to transfer the control framework to a real system, now under construction in laboratory, where the robot will be equipped with a vision system composed by multiple 3D cameras, able to acquire the surrounding environment and extrapolate the position of objects and humans within the workspace. Fixed obstacles simulated in this study can be thought of as fixed objects inadvertently left inside the workspace, such as furniture elements or mechanical tools, whereas dynamic moving obstacles may represent humans operating in the shared workspace.

In the implementation in a real system, the speed of execution of the control loop is expected to be a critical issue. Based on the results presented in this paper, it can be summarized that the on-line motion control is able to run with a cycle time of approximately ${10}^{-3}\mathrm{s}$, lower than rates typically used in the communication between external controllers and the main controllers of robots. Furthermore, a standard laptop (i7 CPU @1.8 GHz, 16 GB RAM) was used for simulations; thus, a reduction in computational times can be expected if a higher performing hardware is used.

4. Conclusions

In this document, an obstacle avoidance strategy for UR manipulators moving in dynamically variable environments is presented and verified by simulation. Starting from algorithms already developed by the authors, some new contributions are introduced, such as the cylindrical shape of the regions of influence that wraps the kinematic chain of the manipulator with a variable radius according to the speed of the obstacle in collision. In addition, three different ways to avoid collisions are proposed, which differ in the type of movement allowed for the perturbation of the end-effector.

In addition to to confirming the effectiveness of the control strategy, the simulations provide an estimate of the computational effort required to run the algorithms, which is in line with the typical requirements of robot controllers and can be improved by using higher performing hardware.

Future work will involve extending the algorithms to 7-DOF redundant manipulators, where the robot’s additional degree of freedom can be exploited to further increase collision avoidance.

Furthermore, the implementation in real systems is currently under development, and preliminary tests confirm the validity of the proposed control strategy.