3.4 Simulation results
3.4.1 Simulation setup
A setup composed of N = 3 Comau Smart SiX manipulators (6 DOFs) mounted on mobile bases (2 DOFs) is considered (see Fig-ure 3.5) for a cooperative load transportation task performed in presence of human operators. Therefore, it holds ni = 8 and p = 6 in (2.1). As in Section 2.5.3, Matlab environment and CoppeliaSim simulator are used for the simulation setup.
The cooperative task function σ(x) is expressed in terms of the absolute-relative formulation defined in Section 2.4. In the case of cooperative load transportation task, the absolute variables are suitable for describing the position and the orientation of the grasped object, while the relative variables for specifying how the end effectors are placed around the object in order to grasp it.
Furthermore, in order to perform the proposed human multi-robot avoidance strategy, the following expressions of functions α1 and
1https://m-lippi.github.io/safe-hmri/
3.4. Simulation results 69
R1
R2
R3
BS1
BS2
PS1
PS2
DS Σw
Figure 3.5: Simulation setup composed of 3 cooperative robots (Ri, i= 1, 2, 3), stations for picking (PSi, i= 1, 2) and depositing (DS) loads and base stations for human operators (BSi, i = 1, 2); the relevant points con-sidered for the human operator and the world reference frame Σw are also reported.
α2 that comply with Properties 1 and 2 in (3.1) are adopted (α1(d) = k1d
α2( ˙d) = k2tanh( ˙d) (3.41) with k1, k2 ∈ R+, leading the safety index f to the form
f (p, ˙p, po, ˙po) = k1d + k2tanh( ˙d). (3.42) The rationale behind the choice in (3.41) is to consider a term which linearly grows with the distance and a term that is increas-ing with the distance derivative but contributes positively to the safety index if the distance is growing, i.e., ˙d > 0, and negatively if the distance is decreasing, i.e., ˙d < 0. Concerning the human rele-vant points, the following points, as highlighted in Figure 3.5 and as common in human skeleton tracking approaches [71], are con-sidered: head, neck, torso, left and right shoulder, elbow, hand, hip, knee and foot, which leads to no = 15. The safety index in (3.42) is, then, extended to each human point and to the whole structure of each manipulator, resulting in the computation of ¯Fi
according to (3.3), (3.4) and (3.5). The computation time to ob-tain ¯Fi is discussed in Section 3.4.5.
Now, by considering that robots are redundant (as it holds ni > p, i = 1, 2, 3), the devised strategy might also envisage the exploita-tion of this redundancy to maximize the level of human safety.
For this purpose, the joint acceleration vector ¨qn,i in (3.14) of each robot is designed as follows
¨
qn,i= kn,1(Ini − J†iJi)( ˙q0,i− ˙qi) (3.43) being kn,1 a positive constant and ˙q0,i ∈ Rni the vector of joint ve-locities for achieving arbitrary secondary tasks. A possible choice of ˙q0,i leverages the procedure defined in [60] that maps the cumu-lative gradient ∇ ¯F into desired velocity of the robot links extreme points and, then, converts them into joint velocities as
˙q0,i = −kn,2
ni
X
l=1
J1i,lT ∇ ¯F + J0i,1T ∇ ¯F
!
(3.44)
with kn,2 a positive constant. With regards to the trajectory scal-ing procedure, the update law in (3.27) is selected which allows to recover the nominal velocity when scaling is no longer necessary, i.e., it is considered that this task does not require to restore the position after the scaling phase. Concerning the emergency pro-cedure envisaged in Section 3.3.2 and highlighted in the scheme in Figure 3.3, a monitored stop procedure is considered. More specifically, let ts be the time when the emergency procedure is activated, such a procedure is based on bringing and keeping the scaling parameters to the following values: c(t) = c(ts), ˙c(t) = 0 and ¨c(t) = 0, implying that a zero velocity reference trajectory is imposed when this procedure is active. Moreover, zero null space velocities are set in order to prevent internal motions ( ˙q0,i = 0ni).
In the considered scenario, the motion of the human operator is characterized by two phases: in the first phase, the human moves from the configuration in Figure 3.6.a to the base station BS1 as in Figure 3.6.b while, in the second phase, he crosses the work-cell to reach the second base station BS2as shown in Figure 3.6.c. At the same time, the robot team cooperatively performs two load
trans-3.4. Simulation results 71
portation tasks: the first one from the top-left conveyor belt PS1
to the right one DS (see Figure 3.6.a) and the second one from the bottom-left conveyor belt PS2 to DS (see Figure 3.6.c). In particular during the first transportation, the human operator is stationary at first base station (Figure 3.6.b), whereas during the second transportation the human operator is on the path of the robot team (Figure 3.6.c). A video of the simulation is available in the respective section of the website1.
a) b) c)
Figure 3.6: Snapshots of the key phases of the simulation: the initial config-uration of the work-cell is in Figure (a); the first and the second multi-robot cooperative transport motions are in Figures (b) and (c), respectively.
3.4.2 First case study: centralized solution
Results relative to the centralized architecture described in Sec-tion 3.3.2 are here reported. Gains are selected as kσ,d = 20, kσ,p = 100, kc,d = 4.5, kn,1 = 1, kn,2 = 2 in (3.14), (3.27), (3.43) and (3.44), respectively. Moreover, ¯Fmin = 105 ( ˙¯Fmin = 0) is as-sumed in Problem 3.1, which is defined by requiring d∗ ≥ 0.1 m in (3.8) and by following the computations in the Appendix B for the robots and the safety index at hand.
The combined motion of the human operator and the robots out-lined above leads the avoidance strategy to mainly modify the nominal cooperative trajectory in two phases:
1. a scaling phase, indicated with Scin Figures 3.7, 3.8 and 3.9, which occurs during the first cooperative transport, when the robots pass next to the base station where the human is standing. In this case, the trajectory is slowed down in order to comply with the minimum safety index;
2. an emergency phase, indicated with Em in the figures, which occurs during the second cooperative transport, when the person crosses the nominal path of the robots. In this case, the velocity modulation is not sufficient to ensure the min-imum safety index, thus a monitored stop of the robots is performed as detailed in Section 3.4.1.
The occurrence of these phases depend on how the scaling pa-rameter c(t) and its derivatives vary over time in response to the human multi-robot coexistence. In detail, as shown in Figure 3.7, where the evolution of the scaling parameters (blue lines) with respect to their nominal values (green lines) is reported, the scal-ing phase Sc is characterized by a decrease of the coefficient ˙c(t) from its nominal value (namely, 1), thus implying a slowing down of the nominal trajectory; then, in accordance to the update law in (3.27), the nominal velocity is always restored after a scaling phase ( ˙c(t) = ˙t = 1). Note that no nominal position is restored as it is assumed, as an example, that the latter is not necessary for the task at hand and the velocity recovery is sufficient. During the emergency phase Em, no representation of these parameters is pro-vided in Figure 3.7 as the task of the robots is completely aborted ( ˙c(t) reaches the origin); however, it can be noticed that the emer-gency phase starts since the scaling procedure leads to violate the constraint on the reverse motion along the path, that is ˙c(t) < 0.
Then, when the safety conditions are restored ( ¯F > ¯Fmin) due to the increasing of the distance of the operator from the robots, the nominal cooperative trajectory is restored as well.
Such phases are also evident in Figure 3.8, where the evolution over time of the cumulative safety index ¯F (blue line) with respect to the minimum value ¯Fmin (red line) is shown. In detail, the scal-ing phase starts when the safety index reaches the minimum value and is mainly characterized by the saturation of this index to this minimum value. Concerning the emergency phase, it determines the interruption of the robots task and their consequent stop; this means that, once the robots are stationary, the safety index can fall below the minimum only if the person continues to approach the robots. It is worth remarking that this situation has been
3.4. Simulation results 73
t [s]
t [s]
t [s]
c
˙c
¨ c t
˙t
¨t Sc
Sc
Sc Em
Em
Em
0
0 00 00
5
10 10 10
20 20 20
20
30 30 30
40 40 40
40
50 50 50
60 60 60
60
0.5 1
-5
Figure 3.7: First case study. Evolution of the scaling parameters c(t), ˙c(t),
¨
c(t) (in blue) compared with their nominal values (in green); scaling and emergency phases are highlighted with Sc and Em, respectively. No plots are provided during the latter phase since the task is interrupted.
stressed in the simulation for the sake of completeness but that the values of ¯F below ¯Fmin are only related to the human motion towards the inactive robots, situation that does not endanger the human operator.
Finally, Figure 3.9 shows the reference task trajectory σr(t) (in blue), which is cooperatively tracked by the robots, compared with the nominal one σn(t) (in green); more specifically, the positional components of the absolute task variables σ1 are shown, which highlight how the devised strategy modifies the nominal behav-ior. In particular, the scaling phase determines the slowing down of the trajectory and, after this, in accordance to the update law in (3.27), no recovery of the introduced shift between the nominal and reference positional trajectories is performed and the latter evolves with the same variation as t; concerning the emergency phase, the monitored stop procedure generates a constant trajec-tory that is equal to the one at the stopping time t ≈ 51 s.
t [s]
F¯ F¯min
Sc Em
0 120 180 240
10 20 30 40 50 60
Figure 3.8: First case study. Evolution of the cumulative safety index (in blue) with respect to its minimum allowed value (in red); scaling and emergency phases are marked with Sc and Em, respectively.
[m][m][m]
t [s]
t [s]
t [s]
Sc
Sc
Sc
Em
Em
Em
nr
nr
n r
0 00 0
0
0.84 0.835 0.83 0.825 -5
3 5
10 10 10
20 20 20
30 30 30
40 40 40
50 50 50
60 60 60
1 2
Figure 3.9: First case study. Evolution of the nominal (n, in green) and reference (r, in blue) trajectories; in detail, the positional components of the team centroid σ1are shown (namely, σ1,x, σ1,y, σ1,z); scaling and emergency phases are marked with Sc and Em, respectively. When the emergency pro-cedure is active (Em), the nominal trajectory is not shown since its tracking is interrupted.
3.4. Simulation results 75