2.5 Robustness to faults
2.5.4 Experimental validation
errors grow in correspondence of the fault occurrence at t = 14 s.
Figure 2.7 reports the norm of the residuals computed by the dif-ferent robots and relative to the faulty robot 1 (kir1k, ∀ i in solid lines) together with the respective adaptive thresholds (kiµ1k, ∀ i in dashed lines) and computed according to (2.38); without loss of generality, it has been considered k˜y⋆k(0)k = 0. The figure makes evident that all the robots are able to detect the fault, i.e. the condition kir1k ≥iµ1 is verified ∀i after the fault occurrence, even if there is no direct communication with the faulty robot.
||˜σ||||
⋆||˜z||˙ ˜σ||
t[s]
t[s]
t[s]
00 00 00
2 2
4 4
5 5 5 6
10
10 10 10
15 15 15
20 30
Figure 2.6: Simulation case study. Evolution of the norm of the task error
||˜σ|| (top), the norm of its derivative || ˙˜σ|| (middle), and the norm of the state estimation error ||˜z⋆|| (bottom).
2.5. Robustness to faults 37
replacemen
||1r1||
1µ1
||2r1||
2µ1
||3r1||
3µ1
||4r1||
4µ1
t[s]
t[s]
t[s]
t[s]
00
00
00
00 0.5
0.5
0.5
0.5 1
1
1
1
5
5
5
5
10
10
10
10
15
15
15
15
Figure 2.7: Simulation case study. Evolution of the norm of residual compo-nents relative to robot 1 (faulty robot) computed by the 4 robots (solid line) compared with the respective adaptive threshold (dashed line).
7 DOFs Kinova ultra lightweight Gen2 Jaco arm, for which it holds n1 = 7; 2) a Kinova Movo mobile robot equipped with a Kinova Gen2 Jaco arm as in the simulations, for which it holds n2 = 11; 3) a Kinova Movo mobile robot consisting of an omnidirectional base (3 DOFs), a variable height torso (1 DOF) and a 2-link robot arm (2 DOFs), for which it holds n3 = 6. The Kinova Movo robots
R1
R2 R3
Figure 2.8: Experimetal setup composed of three heterogeneous robots.
are also equipped with RGB-D sensors, in particular Microsoft
Kinect v2, which allow to possibly monitor the scene; this sensor is considered as end effector of the latter robot. By resorting to the taxonomy in Section 2.4, the aformentioned robots will be referred to as worker, positioner and watcher, respectively.
Concerning the hardware specifications, Movo mobile robots are provided with two dedicated Intel NUC Kits NUC5i7RYH with Intel Core i7-5557U processor and 16 GB RAM, whereas the worker is controlled by a standard PC with Intel Core i7-5500U processor and 8 GB RAM; moreover, wi-fi modules TP-Link TL-WN821N are used for each robot which enable the intra-robot communication through a local network set on a TP-Link TD-W8960N router. Finally, the software architecture relies on ROS middleware and ArUco markers [57] have been introduced to ini-tially localize mobile robots in the environment.
A cooperative service task is considered where the worker is in charge of pouring the contents of a bottle into a glass, while the positioner is in charge of holding that glass; finally, the watcher may be exploited to provide a different point of view for scene monitoring. As in the simulation case study, the desired trajec-tory σd(t) ( ˙σd(t), ¨σd(t)) is defined in terms of absolute-relative coordinates and the control law in (2.40) is considered.
The following set of gains is used: Kp = I18 and Kd = 2I18
in (2.40) and ko = 5 in (2.28). Concerning the communication graph, the directed strongly connected graph composed of the fol-lowing set of edges E = {(1, 2), (2, 3), (3, 1)} has been adopted.
Starting from the initial configuration shown in Figure 2.9.a, the desired task consists of the following steps:
1. the positioner and the watcher move closer to the fixed-base manipulator;
2. the positioner hangs out the glass as in Figure 2.9.b;
3. the fixed-base manipulator pours the contents of the bottle into the glass as in Figure 2.9.c;
4. the mobile base manipulator delivers the glass while the fixed-base manipulator returns in a configuration with the
2.5. Robustness to faults 39
bottle vertical.
a) b)
c) d)
Figure 2.9: Snapshots of the experiment. In detail, a) represents the starting configuration, b) shows the positioner hanging out the glass, c) is the pouring phase and d) represents the fault occurrence.
A fault occurs on the fixed-base manipulator (with index i = 1) during last phase. In particular, the following fault term is intro-duced at time t ≈ 35 s
f1 = J†M,1ψ with ψ =
0 0 1 0 0 0T
m/s2 (2.41) which induces a downwards motion of the manipulator’s end ef-fector as shown in Figure 2.9.d.
As for the simulation case study, it is considered that, when a fault is detected, a shutdown procedure is activated.
Figure 2.10 reports the norm of the residuals computed by all the robots and associated with the faulty robot 1 (solid lines), i.e., kir1k, ∀ i, compared with the respective adaptive thresh-olds (dashed lines), i.e., kiµ1k ∀ i, which are computed according to (2.38) by considering ¯¯d = 0.1 and, without loss of generality,
k˜v⋆k(0)k = 0. Hence, the figure shows that all the robots are able to detect the occurrence of the fault without the need for direct communication with the faulty robot, that is all the robots verify the condition kir1k ≥ iµ1 ∀i after the fault occurrence. In addi-tion, the norms of the residuals associated with the healthy robot 2 (solid lines) are reported in Figure 2.11. The respective adaptive thresholds are also reported (dashed lines) which are shown to always be greater than the residual signals; this verifies that the fault of one robot does not affect residuals of the other teammates.
Analogous norms are obtained for the residuals of robot 3 which are not reported here for the sake of brevity.
||1r1||
1µ1
||2r1||
2µ1
||3r1||
3µ1
t[s]
t[s]
t[s]
00 00 00
0.5 0.5 0.5
5 5 5
10 10 10
15 15 15
20 20 20
25 25 25
30 30 30
35 35 35
40 40 40
Figure 2.10: Evolution of the norm of residual components relative to robot 1 (faulty robot) computed by the 3 robots (solid lines) compared with the re-spective adaptive thresholds (dashed lines).
2.5. Robustness to faults 41
||1r2||
1µ2
||2r2||
2µ2
||3r2||
3µ2
t[s]
t[s]
t[s]
00 00 00
0.1 0.1 0.1
0.2
5 5 5
10 10 10
15 15 15
20 20 20
25 25 25
30 30 30
35 35 35
40 40 40
Figure 2.11: Evolution of the norm of residual components relative to robot 2 (healthy robot) computed by the 3 robots (solid lines) compared with the respective adaptive thresholds (dashed lines).
Chapter 3
Human multi-robot workspace sharing
Human robot interaction
Close collab.
(Online int.)
Coexistence Physical
interaction
workspacePure
sharing Coordination Physical shared control
Physical human guidance
Remote interaction
Human demonstr.
(Offline int.) Teleoperation
(Online int.)
Remote shared control
Remote human guidance
Figure 3.1: Taxonomy for HRI scenarios where the scenario considered in this chapter is highlighted in green.
In this chapter, a human multi-robot coexistence scenario is considered in which pure workspace sharing occurs, highlighted in
green in the taxonomy Figure 3.1. In this scenario, depicted in Figure 3.2, humans and robots work side-by-side on independent tasks. As discussed in the Introduction, this implies the need to guarantee the primary requirement of human safety at all times and, thus, to devise real-time replanning strategies that allow to dynamically adapt the robots’ behavior depending on the human one to prevent unsafe contacts. Indeed, since in this scenario inde-pendent tasks are carried out by humans and robots, the human safety requirement translates into that of human avoidance.
Σw
Figure 3.2: Representation of the system in a human multi-robot workspace sharing setup. The system is composed of multiple collaborative robots and human operator. The world reference frame Σw is reported.
Obviously, depending on the constraints of the specific work context, different strategies, based on various methodologies and rationales, can be developed to ensure human avoidance. In par-ticular, in the case the robots are not strictly required to follow a planned path to carry out their task, gradient-based techniques can be adopted to adjust the trajectory and drive the manipulators away from the human operators reducing the probability of colli-sion. In contrast, when the robots’ task requires the motion along a specific path to be accomplished, the replanning strategy should avoid altering the path to ensure human safety, unless strictly necessary. In this case, in order not to violate the path preser-vation constraint, trajectory scaling approaches can be leveraged that only modulate the robots’ velocity along the planned path
45
without modifying the latter. Note that this requirement of path preserving is rather common in industrial settings where it is gen-erally recommended to follow the desired path without deviating from it in order to accomplish the robotic tasks, such as in welding or assembly tasks.
In addition, when multiple manipulators are involved in the system and work in presence of human operators, the robotic strategy also needs to handle any possible constraint that the multi-robot system itself introduces. For example, in the case of cooperative manipulation tasks, the robots cannot move inde-pendently from each other when performing actions for human avoidance, but their motions are coupled and kinematically con-strained. This coordination is even more challenging when a dis-tributed architecture is required that allows, as discussed in the Chapter 2, to improve flexibility and reconfigurability of the work environment at the expense of higher control complexity, due to the lack of global knowledge of the system.
The focus of this Chapter is thus to present a general archi-tecture, incrementally built in [58], [59] and [16], for performing cooperative tasks with multiple robots working in environments shared with human operators. In detail, a cumulative safety index is defined which takes into account the relative motion considering both position and velocity between the human operator and each point of each manipulator in the team. Then, a control strategy based on trajectory scaling is presented to modulate the velocity of the cooperative task preventing the safety index from falling below a time-varying minimum threshold; however, in the case velocity modulation is not sufficient to guarantee the minimum safety in-dex, an emergency procedure is envisaged which relaxes the robots cooperative task. The proposed strategy is finally extended to the decentralized case according to a leader-follower paradigm and is validated both in a simulation and a real-world setup.
The rest of the Chapter is structured as follows: the main methodologies for safe workspace sharing in the state-of-the-art are summarized in Section 3.1, the safety index formulation is de-vised in Section 3.2 and, building on this, a solution for multi-robot
trajectory scaling, also characterized by a distributed implemen-tation, is presented in Section 3.3, finally, simulation and experi-mental results to validate the proposed approach are reported in Sections 3.4 and 3.5, respectively.
3.1 Literature review
In case the robotic task does not require to strictly follow a planned path to be realized, evasive actions that move the manipulators in the opposite direction from the human operators can be generally adopted. In this regard, a first possible approach consists in defin-ing a measure which quantifies the level of human danger and then in carrying out appropriate evasive actions to minimize it. This idea is, for example, pursued in [60] where the concept of Danger Field is introduced. More specifically, first, an index is defined which assesses the level of human danger with respect to a point-mass robot on the basis of both relative human-robot distance and point-mass robot velocity; then, this index is integrated along the overall structure of the manipulator and is adopted to define joint space velocity commands used to generate internal motions (in the case of redundant robots) and to give a reactive evasive behavior to the robot. In this context, on the basis of the con-cept of danger field introduced in [61] for a point-mass robot, the study in [62] considers the different factors that affect the impact force during a potential collision as a measure of danger: human-point distance, human-robot relative velocity and robot inertia.
Then, this index is evaluated for each critical point which is the closest point to the operator for each link, and is exploited in the definition of a virtual force according to the impedance paradigm which drives the robot away from the operator. The work in [63]
focuses, instead, on the way the perception of the environment is performed; indeed, a method based on depth data is defined to fast compute the distances between the manipulator and possible obstacles in the workspace. Information regarding these distances and obstacles velocities are then exploited to generate repulsive
ve-3.1. Literature review 47
locity commands in the Cartesian space according to the artificial potential field theory [64].
Concerning the case of tasks with path preservation con-straints, the study in [17] proposes a scaling procedure where only velocity reduction along the nominal path is allowed. In detail, a constrained optimization problem is solved to minimize the slow-ing down of the trajectory, while guaranteeslow-ing a condition on the minimum distance between each point of the robot and the hu-man operator. However, the proposed solution does not account for the human-robot relative motion, but only considers the robot velocity in the direction of the human operator; more specifically, only the maximum value of the human operator velocity can be in-cluded in a clearance parameter but no measure of his/her actual velocity in the direction of the robot is explicitly considered. The work in [65] proposes a trajectory scaling method which relies on the separation distance between the operator and the robot with-out considering where their motion is directed: if the separation distance is above a safe value, no velocity reduction is applied, if it is less than a dangerous value, zero velocity is imposed or, oth-erwise, a velocity reduction factor inversely proportional to the distance is considered. The study in [66] presents an approach based on dynamical systems which combines a velocity reduction action with a path reshaping one; more specifically, the former is based on the human-robot distance and is higher as the distance decreases, while the latter is such as to reduce the velocity in the plane normal to the human surface and to project the motion into the one tangent to the surface, thus avoiding the surface collision.
A trajectory scaling approach for Dynamical Movement Primitives (DMPs), successfully applied in many learning from demonstra-tion contexts, is then provided in [67] where velocity constraints are taken into account.
In addition, in order for the robot to proactively react to the hu-man behavior, a prediction of his/her intentional motion [68] can be involved in the avoidance strategy as proposed in [69] where the human workspace occupancy is predicted and exploited by a trajectory planner to minimize the penetration cost in it. Finally,
a hybrid solution that addresses both workspace sharing and phys-ical interaction can be found in [70], where a reactive controller is proposed that responds to both virtual and real contact forces.
Note that, although great effort has been dedicated by the research community to the topic of human-robot safe workspace sharing, none of the mentioned contributions deal with multiple manipulator systems working in presence of human operators for which, as stated above, additional constraints need to be taken into account. In the following sections, a formulation and a solution for the latter scenario is provided.