P
OLITECNICO DI
M
ILANO
Engineer school
Master Degree in Automation and Control Engineering
Electronic and Information department
Combining Power Force Limiting and Speed
Separation Monitoring for collaborative robots
Supervisor:
Andrea Maria Zanchettin
Thesis advisor: Paolo Rocco, Bakir Lacevic
Thesis of:
Niccolo’ Lucci
Serial number: 892141
“Failure is an option here. If things are not failing, you are not innovating enough.”- Elon Musk
iv
Thanks
I would like to thanks, first of all, prof. Rocco and prof. Zanchettin that gave me the possibility to join their research group in the MERLIN laboratory and to develop this thesis. Another big thanks to Bakir, that helped me to develop the algorithm and all the PHDs that have solved most of my doubts and gave me hints and suggestions.
Then I would like to thanks my lab fellows with which I shared six fantastic months sharing and comparing ideas and giving each other the required support when needed. A particular thanks to Giovanni and Daniele that told me how to switch on Yumi, my thesis would have never come to an end without you.
Thanks also to my friends in Milano and my hometown Figline Valdarno, you have al-ways supported me.
Finally, I want to thanks my parents, my sister Elisabetta and my grandmother Rossana for always caring in what I was doing and gave me all the help needed.
Contents
Abstract xi
1 Introduction 1
1.1 General overview of the problem . . . 1
1.2 Goals of the work . . . 4
1.3 Achieved results . . . 5
1.4 Organization of the document . . . 5
2 State of the art 7 3 Background on SSM, PFL and reflected mass 11 3.1 Speed separation monitoring . . . 11
3.2 Power force limiting . . . 14
3.3 The reflected mass . . . 19
4 Implemented algorithms 21 4.1 Surfaces as obstacles . . . 21 4.1.1 SSM + PFL . . . 22 4.2 Points as obstacles . . . 29 4.2.1 PFL . . . 31 4.2.2 SSM . . . 34 4.2.3 SSM + PFL . . . 36
4.3 Comparison of the developed strategies . . . 39
4.4 The clearance parameter . . . 43
5 Data Pre-processing 45 5.1 The Kinect . . . 45
5.2 PoinCloud from Kinect and human filtering . . . 47 v
vi CONTENTS 6 Experimental results 49
6.1 The robot . . . 49
6.1.1 Description . . . 49
6.1.2 Kinematic and dynamic parameters . . . 50
6.2 Experiments . . . 55
6.2.1 Experiment setup . . . 55
6.2.2 Results . . . 58
7 Conclusions 67
A Roots of a third-degree polynomial 71 B Distance link-point solution 75
List of Figures
1.1 Examples of collaborative robots . . . 2
1.2 Safety- efficiency trade-off . . . 3
1.3 SSM and PFL working areas . . . 4
3.1 Distances to be considered in SSM algorithm . . . 13
3.2 In this figure, we see both a quasi-static contact and a transient one. The transient one lasts under0.5 seconds and tolerates a higher force. This contact becomes a quasi-static one if it lasts more than0.5 seconds, and a lower force is tolerated. As it is understandable, the collision is safe if the robot does not transmit a force that is in the red danger area. . . . 15
3.3 Division of the body in areas . . . 16
3.4 Pain threshold for all the body parts . . . 17
3.5 Model of the human as a mass-spring system. . . 18
4.1 The obstacle is a triangle . . . 22
4.2 The real IRB140 and the simulated one . . . 23
4.3 Simulation setup . . . 23
4.4 Simulation setup . . . 29
4.5 Simulation setup with a human-like obstacle . . . 30
4.6 Simulation result with a human obstacle . . . 30
4.7 Two possible cases on how g(s) evolves . . . 32
4.8 Example of how scrit shifts along the link. Four iterations are necessary in this case. . . 34
4.9 Link reshaping depending on the case . . . 37
4.10 Simulation setup . . . 40
4.11 The blue line represents the nominal trajectory of the first joint variable, the red one is with SSM + PFL, the yellow one with SSM and the purple one with PFL. . . 41
4.12 The blue line represents the nominal angular velocity of the first joint, the red one is with SSM + PFL, the yellow one with SSM and the purple
one with PFL. . . 42
4.13 The blue line represents the nominal delta, the red one is with SSM + PFL, the yellow one with SSM and the purple one with PFL. . . 42
4.14 The link with a considered thickness of ∆ . . . 43
5.1 The Kinect camera . . . 45
5.2 The skeletal extraction of Kinect of two operators . . . 46
5.3 The setup with the Kinect inside the SmartRobots device (inside the green circle on top of Yumi) . . . 46
5.4 These phases show the sequence of how the pointCloud data is acquired and properly filtered . . . 48
6.1 IRB 14000 (Yumi) . . . 50
6.2 Reference frames representation following DH convection . . . 51
6.3 Chosen reference system for the inertia tensor . . . 53
6.4 Yumi’s cad. . . 55
6.5 Workplace setup . . . 56
6.6 How the whole program executes . . . 57
6.7 The SFC that has been implemented . . . 58
6.8 SSM and PFL working areas. . . 59
6.9 SSM and PFL working areas from experimental results. The green line represents the mixed algorithm, the red the SSM and the blue the PFL. . 59
6.10 As always the blue represents PFL, the red SSM and the green the mixed algorithm . . . 62
6.11 In this graph is represented the same thing as in the previous one but all the three results are divided into different figures to have a better overview 63 6.12 Box plot for the projected robot velocity for each one of the three algo-rithms. The mixed one confirms to be the best between the three because of its higher median. . . 64
6.13 Box plot for the performance index for each one of the three algorithms. The mixed one confirms to be the best between the three because of its higher median. . . 65
LIST OF TABLES ix B.1 Link-obstacle distance . . . 75
List of Tables
Abstract
Enabling humans and robots to safely work close to each other deserves careful consid-eration. With the publication of ISO directives on this matter, two different strategies, namely the Speed and Separation Monitoring and the Power and Force Limiting, have been proposed.
This thesis proposes a method to efficiently combine the two aforementioned safety strategies for collaborative robotics operations. By exploiting the combination of the two, it is then possible to achieve higher levels of productivity, still preserving the safety of the human operators. In a nutshell, the state of motion of each point of the robot is monitored so that at every time instant the robot can modulate its speed to eventually come into contact with a body region of the human, consistently with the corresponding biomechanical limit.
Validation experiments have been conducted to quantify the benefits of this newly de-veloped strategy with respect to the state-of-the-art.
Chapter 1
Introduction
1.1
General overview of the problem
In recent years with the continued progress of industry 4.0, there has been an increasing need in small-medium enterprises (SME) for changing the asset in production, and so, from this necessity, they had to modify the assembly line configuration. This brought about the necessity of having high production flexibility allowing them to switch prod-uct with a relatively low cost and at high speed.
From one side, this degree of flexibility can be achieved only with a human worker that can carry out highly interchangeable work but with low efficiency. On the other hand, big robotic manipulators have a high efficiency but cannot reach the previously said flexibility. Moreover, they are constrained inside cages to guarantee a high-safety level to the company’s employees.
This gap between these two production methods can be filled with collaborative robots (also called cobots) that integrate excellent flexibility with high efficiency.
2 CHAPTER 1. INTRODUCTION
(a) Yumi from ABB (b) UR5 from Universal Robots
Figure 1.1: Examples of collaborative robots
These robots are characterized by: low inertia, low payload capabilities (to reduce the risk when a collision happens), they share the same workspace of the operator and they are able to cooperate with him even without the need of a safety cage to surround them provided that they are accurately designed both mechanically and with safety al-gorithms. This last point has opened a whole new research trend that needs to integrate at its best these cobots with the human without looking too frightening during their movements but at the same time ensuring a good level of efficiency. The only way to guarantee this is that the operator needs to trust the safety features implemented by the designers and the robot does not have to slow down too much during its pre-computed trajectories.
It is then clear that there is a trade-off between safety and production efficiency of the robot: if the robot is too fast, there could be safety problems or, on the other hand, if the robot is too slow the robot would not be profitable for the company. In the following, a picture that explains this trade-off:
1.1. GENERAL OVERVIEW OF THE PROBLEM 3
Figure 1.2: Safety- efficiency trade-off
As we can see for negative velocities, a robot has the higher efficiency because the human is not along its path and so can increase its velocity without breaking any safety constraints. On the other hand, the smaller the human-robot distance, the smaller is the constrained threshold of the robot velocity, the smaller the productivity and the revenue. The trend of integrating collaborative robots in companies is ascending then the ISO has set some standards [1] defining what collaboration means, which kinds exist, how to ensure worker safety setting pain thresholds and finally how a cobot should be designed. As seen in figure 1.1, the cobots do not have the same shape as the big industrial robots. First of all, they are small and easy to setup and integrate into a production line, they are small and not that heavy (Yumi weights 38 kg) and, most importantly, they are mechanically safe.
With being mechanically safe, we mean that they are made only by rounded edges and constituted of soft material that can absorb the energy of a possible impact.
Four are the case of human-robot collaboration:
• Safety rated monitored stop: No robot motion is allowed when the operator is in the collaborative space
• Hand guiding: the robot motion is dictated only by the operator.
• Speed and separation monitoring: The robot moves when the separation dis-tance is above a certain minimum lower bound. As we will see, the minimum
4 CHAPTER 1. INTRODUCTION separation distance depends on the robot velocity. The higher the robot velocity, the higher the minimum separation distance.
• Power and force limiting: In case of contact, the robot can transmit a limited amount of energy. Also in this case the dissipated energy is proportional to the robot velocity.
The first one is not a collaborative method, while the second one does not comprehend the cases where the robot moves autonomously.
The last two collaborative operations can be further developed and are the two that we have used; both of them can be transformed into a constraint in the robot velocity.
1.2
Goals of the work
The objective of this thesis is to develop a new algorithm that combines two that already exist: Speed Separation Monitoring and Power Force Liming. Singularly, they already ensure always excellent safety features but not a high-efficiency level.
To solve this problem, we develop a mathematical model in the form of an optimiza-tion problem and then solve it while satisfying the previously explained constraints of PFL and SSM while maintaining high production efficiency.
This combination is done because we believe that the combination of the two constraints can bring an enhancement to the overall system.
An intuitive proof of this can be given with the following image:
1.3. ACHIEVED RESULTS 5 As we can see, the light blue represents the working area of PFL. In this case, the velocity is bounded whatever the distance between the robot and the obstacle so to guar-antee that the transmitted energy is below a certain threshold. On the other hand, the red area is the SSM working area, and it strongly depends on the human-robot distance, decreasing linearly down to zero when the distance is null.
Lastly, the dashed line represents what we would like to achieve with the development of our thesis that is the combination of the two.
In the end, we will try it in an experimental setup where it is compared with the pure SSM constraint and the pure PFL constraint making sure that an increased robot velocity is obtained while maintaining a good safety level for the operator.
1.3
Achieved results
The achieved results are more than satisfying: the mixed algorithm is resulted to per-form better than the two single algorithms SSM and PFL while maintaining good safety features. This result ensures that our algorithm can perform the operations at a higher speed while guaranteeing an optimal safety level.
1.4
Organization of the document
The remaining part of the document is organized as follows: a brief state of the art (2) explaining other methods through which the safety-efficiency trade-off can be dealt with, an explanation about how the SSM and PFL algorithms can be converted in robot velocity constraints and what the reflected mass is (3), how the several solutions to the mathematical problem have been dealt with (4), the kind of data we get from the external sensor and how they have been treated (5), the setup of the done experiments (6) and finally the conclusions (7) and two appendices (A and B).
Chapter 2
State of the art
Human and robot interaction is between one of the newest cutting edge technology of industry 4.0 and is always more and more discussed in academy papers. The problems researchers have to deal with is integrating the coexistence in the same working space of a human worker with a robot. This integration brings about several difficulties, but the most important one is the efficiency-safety trade-off.
Because the robot and the human need to collaborate to the same task is essential to guar-antee a safe environment while executing the task. At the same time, the robot needs to complete the task as quickly as possible to satisfy a minimum number of completed pieces that allows it to be profitable for the enterprise. It is then clear that an efficiency-safety trade-off exists because a too fast robot is more profitable but can cause severe damages to the employee while, on the other hand, a too slow robot ensures a high safety level but would not be profitable.
In ISO 15066 [1], an in-depth study has been developed to clarify what a coopera-tive work-space looks like, what kind of collaboration can exist, and how to guarantee the working safety of the employee. The most discussed approaches in research are the Speed Separation Monitoring and Power and Force Limiting.
The first one expresses a constraint on the minimum separation distance that needs to be maintained in order to be sure that the robot prevents a collision.
The second one instead provides a maximal velocity that if it is overcome in case of contact the transmitted energy due to a collision would be too high, and an injury in the worker could happen. This constraint then ensures that in case of contact or, even worst, a quasi-static contact happens, the worker would have no difficulty in pushing the robot away.
8 CHAPTER 2. STATE OF THE ART The SSM is considered in [2] by Zanchettin and Rocco where, after taking in con-sideration the robot braking time, the relative human-robot velocity and distance, have developed a linear optimization problem whose solution returns a velocity scaling pa-rameter.
Then the maximal efficiency is guaranteed by the fact that no changes in the pre-calculated robot trajectory are introduced while safety is guaranteed by slowing down the robot velocity accordingly to the algorithm. The addition of the PFL constraint would have brought benefits to the overall efficiency of the solution.
The conjunction of PFL and SSM methods has been developed in [3] by Petr Svarny et al. Here an RGB-D camera has been used to track the human position in the collabo-rative work-space and, depending on the human-robot distance, three working areas are identified: full speed, reduced speed and zero speed. This division of the working area brings a significant loss of resolution in the optimal reduced speed that the robot can maintain without harming the worker even because considers SSM and PFL as static and not dynamic constrains where velocity could be changed accordingly to their ful-filment. Moreover, a very rough human occupancy has been given approximating the thickness of each of the body key-points extracted by the RGB-D camera as a sphere. Even if these precautions ensure the working safety, the production efficiency is hugely affected negatively.
Ragaglia, Zanchettin, and Rocco kept developing the solution cited above of pure SSM in [4]. In this work, they have extended the previous work passing from considering the obstacle as a set of points to a set of polytopes that better approximate the human. Moreover, they have modelled the upper part of the human body as a set of kinematic parts with realistic constraints and joint limits. This model allowed them to compute (once known the robot stopping time) the swept volumes representing the predicted oc-cupancy of the human in the working space (approximating the human velocity as a constant) and after applying the algorithm developed in [2], they have achieved a more safe and at the same time efficient solution.
Another research paper in which a dynamic scaling of the robot velocity is proposed is in [5] by Byner, Matthias, and Ding. Here the robot is accounted as a series of spheres and only the 2D human position is measured. This approximation allowed the researchers to develop ad 2D dynamic SSM that adapts the robot velocity to the human-robot distance. The solution has been obtained expressing the relationship between the human-robot relative position and the robot velocity that turned out to be a polynomial of 4th degree. Velocity is then changed accordingly to this polynomial solution.
More-9 over, as in [4], they have calculated the predicted occupied area both of the robot and the human during the robot reaction time to better ensure safety.
The paper unluckily uses several conservative approximations that could compromise the efficiency of the solution like the projection in 2D both of the robot and the human and the consequent approximation of the robot as a set of spheres to take into account the robot thickness.
An example that uses a kind of PFL algorithm is [6] of Rossi et al. In this paper, the human-robot collision has been modelled as an inelastic impact. Starting from here they obtained a new energy dissipation index depending on the so called ’reflected mass’. From this index, knowing that it depends on the reflected mass term, a two-phase opti-mization algorithm has been constructed where they implemented a pre-collision control strategy. This strategy consists of maintaining the above mentioned dissipated energy index below a certain safety threshold, and it is achieved exploiting the physical prop-erties of the redundant robot. The robot can then realize different configurations so that to minimize the reflected mass parameter that directly influences the dissipated energy. Another research paper that uses the Power and Force Limiting constraint is the one by Aivaliotis et al. [7]. In this paper, a sensorless approach has been undertaken to estimate when a collision happens, and an appropriate reaction strategy to limit the conveyed energy has been used. Here then the Power and Force Limiting is used to detect a colli-sion. To do so, after the calculation of a given robot path and the consequent estimation of the required current to complete it, the required current is measured, and if the differ-ence between the required current and the predicted one is above a certain threshold, a safety-rated emergency stop is triggered.
Chapter 3
Background on SSM, PFL and
reflected mass
In this chapter, we will give insights about the used safety constraints SSM and PFL, and we will introduce a new method to calculate the robot mass through the ”reflected mass”.
3.1
Speed separation monitoring
As already introduced in 1.1 SSM is a collaborative algorithm that allows a human operator and the robot to cooperate for the completion of a task.
The algorithm continuously computes the minimal distance robot-worker, and in case this goes under a certain threshold, a safety-rated stop algorithm is triggered.
It is though possible to avoid or at least change this threshold in two ways:
• reducing the speed of the robot the minimal separation distance decrease because the robot needs less space when breaking
• executing another path that does not bring a collision between the two allows the robot to do not respect the constraint
As stated in [1], the separation distance can be calculated taking into consideration several hypotheses:
• In case we are considering both the robot and the human to have a constant speed, the worst-case scenarios must be considered. As we can understand, it is a very conservative case that leads to system inefficiencies.
12 CHAPTER 3. BACKGROUND ON SSM, PFL AND REFLECTED MASS • In cases instead ,where we measure both the robot speed and the human one a more accurate calculation of the optimal separation distance will be given. Alter-natively, if we are measure the human-robot distance and the human velocity, we can set the optimal robot speed (as in our case).
The protective separation distance is given by the following formula:
Sp(t0) = SH + SR+ SS+ C + Zd+ Zr (3.1)
where:
Sp(t0) is the safety distance;
t0 is the current time;
SH is the contribution of the human to the separation distance;
SRis the contribution of the robot due to its reaction time;
SS is the contribution of the robot due to its stopping distance;
C is the contribution of the sensor due to its resolution in detecting the human; Zdis the contribution of the sensor due to its uncertainty in detecting the human;
Zris the contribution of the robot position uncertainty;
We will now discuss how all these elements can be accurately calculated or approx-imated.
Starting from the human separation distance contribution SH we can calculate it as:
SH =
Z t0+TR+TB
t0
vH(t)dt
where TR accounts for the robot reaction time (detection of the human, signal
pro-cessing and robot stop activation but not the time required to it to stop) and TB is the
robot stopping time that should depend on the configuration. Another way to calculate it is through a simpler formula that considers the worst-case scenario (vh = 1.6m/s):
SH = 1.6(TR+ TB)
3.1. SPEED SEPARATION MONITORING 13 SR =
Z t0+TR
t0
vR(t)dt
Also in this case we can consider the worst robot velocity, if we cannot measure it, leading to a simplified relation:
SR= vR maxTR
Note that depending on the direction of the robot velocity this contribution could be negative if the robot is moving away from the operator. This term is negative if the product between the robot velocity direction and the versor that reduces the distance human-robot have the opposite directions.
Finally, Ss:
SS =
Z t0+TR+TB
t0
vS(t)dt
where vS(t) is the robot speed when decreasing the velocity until it stops. Again we
need to consider the direction of vS(t) that reduces the separation distance the most. If
we cannot measure it, the previously introduced vR maxis used.
Figure 3.1: Distances to be considered in SSM algorithm
14 CHAPTER 3. BACKGROUND ON SSM, PFL AND REFLECTED MASS to the threshold, in fact, we had preferred to measure the actual human-robot distance and adjust the robot velocity accordingly. We can, in fact, re-formulate the formula (3.1) as (setting C, Zdand Zr as negligible):
Sp(t0) = vR(TR+ TB) + vH(TR+ TB)
and the actual distance D(t0) minus this security distance needs to be above a certain
threshold as SSM claims: D(t0) − Sp(t0) ≥ S D(t0) − vR(TR+ TB) − vH(TR+ TB) ≥ S vR ≤ D(t0) − S TR+ TB − vH
Finally, because most of the time TB TR (or it can be considered absorbed by
TB), S can be easily adsorbed inside D(t0) as a clearance parameter, and we supposed
that the human stands relatively still enough during the operation the formula reduces to:
vR ≤
D(t0)
TB
3.2
Power force limiting
Power and force limiting is the other safety criterion we use in our algorithm.
In this method, contact between the operator and the robot can happen both intentionally or unintentionally and, for this reason, the robot needs to be properly designed mechan-ically as well as equipped with safety algorithms.
In a collaborative situation, contacts can happen both voluntarily (because they are part of the working task) or involuntarily (with robot failure or not).
The ISO 15066 specifications [1] distinguish between two different kinds of collisions: transients and quasi-static ones.
The transient one is a collision that lasts under the 0.5 seconds, and so the worker can retract from the robot. It does not involve any clamping or blockage of any human part. On the other hand, a quasi-static contact consists of blocking a human body part between the robot and whatever part of the working environment and these conditions continue until an intervention is done. Of course, the latter is the most dangerous one.
3.2. POWER FORCE LIMITING 15
Figure 3.2: In this figure, we see both a quasi-static contact and a transient one.
The transient one lasts under0.5 seconds and tolerates a higher force. This contact be-comes a quasi-static one if it lasts more than0.5 seconds, and a lower force is tolerated. As it is understandable, the collision is safe if the robot does not transmit a force that is in the red danger area.
For reducing the risk, both passive and active methods can be used.
With a passive measure, It is meant that the robot is suitable for a collaborative task, so it is mechanically compliant with the presence of a human.
The robot, to be mechanically compliant, needs to be composed of rounded (or at least smooth) surfaces and corners. Moreover, the material should be chosen accordingly so that the energy transfer in case of contact is reduced at the minimum (this can be achieved with padding and using deformable materials).
With active safety methods, It is meant: limiting forces and torque, limiting velocities (both the Cartesian one and joints one) and limiting the mechanical power and energy of masses.
To evaluate how much a collision is dangerous, we need to introduce the concept of dis-sipated energy. When the robot is moving along its trajectory, it has a velocity different
16 CHAPTER 3. BACKGROUND ON SSM, PFL AND REFLECTED MASS by zero, so it must have kinetic energy proportional to the square of the velocity. Now, if a collision with an external object happens all (or at least the most part) of this energy is dissipated in the impact. We have then an energy transfer from the robot to, possibly, a human operator.
We then need to define an energy threshold that defines how dangerous the impact may be and a model used to calculate this dissipated energy.
The following data, as stated in [1], are referred to medical studies like [8], [9] and [10] for the multiplier value used for passing from quasi-static contact force threshold to the transient threshold (see next) and [11] for the force thresholds in the several body parts. First of all, a body model is taken into consideration and has been divided into several parts as the following figure shows:
Figure 3.3: Division of the body in areas
This body model allowed us to identify for each body part a different pain level, from the highest to the lowest and then to compute a rule for scaling the robot velocity to be compliant to the pain limits.
The critical point is then to understand where the contact is most likely to occur and set the specified pain threshold. It is worth to remark that the given thresholds have been found in case the worker does not wear any kind of protective cloth.
3.2. POWER FORCE LIMITING 17
Figure 3.4: Pain threshold for all the body parts
As we can see from the figure for each body part, a maximum permissible pressure and force are defined for the quasi-static contact, as for the transient contact a multiplier is defined that indicates how much the force/pressure can be scaled to still respect the imposed rules.
Let’s now define a model that computes the amount of energy transferred in case of impact. First of all, [1] takes the assumption of totally inelastic impact (all the energy is
18 CHAPTER 3. BACKGROUND ON SSM, PFL AND REFLECTED MASS transferred to the human) modelling the human body as a mass-spring system:
Figure 3.5: Model of the human as a mass-spring system. We know that the energy absorbed by a spring is equal to:
E = 1 2Kx
2
where K is the elastic constant of the impacted body part given by [1] and x is the displacement due to the impact.
The force exerted by a spring is:
F = Kx combining the two we get:
E = F
2
2K
Because the robot is moving, it has kinetic energy equal to: E = 1 2µv 2 rel where µ = m1 H + 1 mR −1
, mH is the mass of the human and mR is the mass of
the robot and vrelis the relative velocity between the robot and the human. Since in our
method we consider the worst-case (quasi-static impact) and the human motionless, we have that mH = ∞ and vrel = vR− vH = vRthen:
E = 1 2mRv
2 R
3.3. THE REFLECTED MASS 19
mrobot
2 + mpayload but, as we will see in the next section we will not use this formula
but something that has been already studied in [6] called reflected mass that takes into account the direction of impact of the robot.
Equating the two energies, we get to a relationship that limits the velocity of the robot: 1 2mRv 2 R= F2 2k vR ≤ F √ k q m−1R (3.2)
3.3
The reflected mass
As already said, in [6] they introduced the concept of reflected mass. In that case, they used the redundancy of the robot to minimize this element to reduce to the minimum the amount of dissipated energy in the impact.
In our case, we will use this concept because the formula for the mass of the robot given by [1] has not been proven to be sufficiently safe during collaborative tasks.
The physical concept behind the reflected mass consists of considering the configuration of the robot projecting it through the versor ~n that represents the direction of the minimal distance human-robot. The formula used in [6] is:
mn= ~n0J (q)B(q)−1J (q)0~n
−1
where J (q) is the jacobian of the robot and B(q) is the inertia matrix of the robot. Substituting then mnto mRin (3.2) we obtain:
vR≤ F √ K p ~n0J (q)B(q)−1J (q)0~n
Chapter 4
Implemented algorithms
In this chapter, we will illustrate several algorithms that we have implemented, explain how we mathematically modelled and solved them and which is the best one.
4.1
Surfaces as obstacles
Before dealing with the implementation of the algorithms, we first need to have an accurate model of the 3D position of the operator to guarantee a high-safety standard. It is important to know that the Kinect camera by Microsoft supervises the collaborative space and extracts the 3D positions of the most critical human joints (hands, elbows, shoulders, hips and a central point inside the body).
From these points, it is possible to construct with some segments and triangles a first skeletal structure, but it is not enough. Starting from these data and confident of the results achieved in [4], we decided to use the swept volumes to create a mesh of triangles that approximate very well the human body.
Then, in this section, we will consider the human body as approximated as a set of thousand of triangles.
The equation used to parameterize the triangle is the following: rObst= r1+ t(r2− r1) + u(r3− r1)
t, u ∈ [0, 1] , t + u ≤ 1
where r1, r2 and r3are the vertices of the triangle and t and u are parameters from 0
to 1 subject to the fact that their sum is lower than 1. 21
22 CHAPTER 4. IMPLEMENTED ALGORITHMS
Figure 4.1: The obstacle is a triangle
In this part, an IRB140 from ABB with six degrees of freedom and an obstacle composed of four surfaces have been used for simulation purposes.
The simulations have been done with the MATLAB software and the Robotics Toolbox by Peter Corke [12].
4.1.1
SSM + PFL
As already said, the IRB140 from ABB has been used to conduct this experiment. The reasons for this choice are several, but the most important one is that we have been able to find the dynamic parameters that are critical data that the robot companies do not share.
Because in our algorithm we need the mass matrix B(q) to compute the reflected mass we also need the inertia of all the links composing the robot, and they have been esti-mated in [13].
4.1. SURFACES AS OBSTACLES 23
(a) IRB140 from ABB (b) Simulated IRB140
Figure 4.2: The real IRB140 and the simulated one
The obstacle, on the other hand, has been extremely simplified to a pyramid com-posed of four triangles so that the simulation would not have been computationally heavy. In the following, the constructed setup for the simulation is detailed.
Figure 4.3: Simulation setup Now, the algorithm is described and how it has been solved.
Remembering the constraint equations of SSM and PFL expressed in Chapter 3: SSM: vR ≤ D(tT 0)
24 CHAPTER 4. IMPLEMENTED ALGORITHMS PFL: vR≤ √FKpn0Js(q)B(q)−1Js(q)0n
where the symbol of vector for n has been dropped for simplicity, this will be our starting point for the developing of the algorithm.
Because we want that these two constraints act together a more general constraint is considered (the sum of the two) then, the robot velocity along whatever point in the link (vs) projected in the minimal direction between the human and the robot (n) is
considered. The resulting equation is the following: n0vs ≤ TB−1krObst− rsk + F √ K p n0J (q)B(q)−1J0(q)n
But, knowing that:
n = rObst− rs krObst− rsk
vs = Jsq˙
and substituting back we arrive at the most general constraint equation: (rObst− rs)0 krObst− rsk Jsq ≤ T˙ B−1krObst−rsk+ F √ K s (rObst− rs)0 krObst− rsk Js(q)B(q)−1Js0(q) rObst− rs krObst− rsk (rObst−rs)0Jsq ≤ T˙ B−1krObst−rsk2+ F √ K p (rObst− rs)0Js(q)B(q)−1Js0(q)(rObst− rs)
Considering then the following substitutions to make our equation more readable: r0os= (rObst− rs)0
C = √F K
X0 = (rObst− rs)0Js= r0osJs
we get (dropping all the dependencies on s and q and noticing that the left-hand side is a scalar quantity, so its transpose is the quantity itself):
˙
q0X ≤ TB−1krosk2+ C
√
X0B−1X (4.1)
4.1. SURFACES AS OBSTACLES 25 PFL and SSM constraint in a unique equation.
By rewriting X in a more convenient way: X(s) = Js0ros
Knowing that Js = Ja+ s(Jb − Ja) and ros = rObst− ra− s(rb− ra) after some
computation we get: X(s) = [. . . ] = Ja0roa | {z } p0 +(Jba0 roa− Ja0rba | {z } p1 )s + (−Jba0 rba | {z } p2 )s2 where roa = rObst− ra. Adding the joint velocities:
˙
q0X = ˙q0p0+ ˙q0p1s + ˙q0p2
|{z}
=0
s2 (4.2) Note that the last term is zeros, as confirmed in [4] and from the MATLAB simula-tions because the link is considered as a rigid body and so the scalar product is always zero since that the two velocities vaand vb are both perpendicular to the link.
Note moreover, that rObstis a point inside a triangular obstacle so it has the form of:
rObst = r1+ ur21+ vr31
Substituting this last equation inside rosand roa we get:
roa = rObst− ra= r1− ra | {z } r1a +ur21+ vr31 ros = rObst− rs = r1− ra | {z } r1a +ur21+ vr31− srba
p0 and p1 that depend on roathe first and rosthe second become:
p0 = Ja0r1a+ (Ja0r21) u + (Ja0r31) v
p1 = (Jba0 r1a− Ja0rba) + (Jba0 r21) u + (Jba0 r31) v
26 CHAPTER 4. IMPLEMENTED ALGORITHMS ˙ q0X = ˙q0Ja0r1a | {z } α0 + ˙q0Ja0r21 | {z } α1 u + ˙q0Ja0r31 | {z } α2 v + ˙q0Jba0 r1a− ˙qJa0rba | {z } α3 s + ˙q0Jba0 r21 | {z } α4 us + ˙q0Jba0 r31 | {z } α5 vs ˙ qX = α0+ α1u + α2v + α3s + α4su + α5sv = f (u, s, v) (4.3)
The constraint in 4.1 then becomes:
f (u, s, v) ≤ TB−1krosk2+ C
p
rosJsB−1Jsros
Let’s now move to the right-hand side considering the term TB−1krosk2. Expressing
its dependencies from u, s and v we obtain: TB−1krosk2 = TB−1kr1ak2 | {z } β0 + TB−1kr21k2 | {z } β1 u2+ TB−1kr31k2 | {z } β2 v2+ TB−1krbak2 | {z } β3 s2+ TB−12r01ar21 | {z } β4 u + TB−12r01ar31 | {z } β5 v + (−TB−12r01arba) | {z } β6 s+ TB−12r021r31 | {z } β7 uv + (−TB−12r210 rba) | {z } β8 us + (−TB−12r310 rba) | {z } β9 sv (4.4) TB−1krosk2 = β0+ β1u2+ β2v2+ β3s2+ β4u + β5v + β6s + β7uv + β8us + β9sv (4.5)
Now, combining (4.3), (4.5) and putting them back to (4.1) we arrive to: (α0 − β0) + (α1− β4)u + (α2 − β5)v+ (α3− β6)s + (α4− β8)sv + (−β7)uv+ (−β12)u2+ (−β2)v2+ (−β3)s2 ≤ C p r0 osJsB−1Js0ros (4.6) The right-hand side of this last equation is always positive because it is composed of a square root, and C that is always a positive term for construction. It would then be sufficient to verify that the maximum of the function on the left-hand side is smaller than zero to verify that the constraint is satisfied. This function is rather particular be-cause is non-convex and it is difficult to find the maximum inside the specified domain:
4.1. SURFACES AS OBSTACLES 27 0 ≤ s ≤ 1, 0 ≤ u ≤ 1, 0 ≤ v ≤ 1 and u + v ≤ 1.
This problem has been solved using an already existing algorithm that takes inspiration from the GJK algorithm that solves these quadratic functions dividing the 3D domain into simpler 2D areas and then looking for the solution in the boundaries or inside the volume.
In case the maximum is above zero, it is not a sufficient condition to state that the constraint is not satisfied. Then, a more in-depth study of it is necessary and approxi-mations to find a feasible solution are required.
Restarting from the equation (4.1) we consider the definition of scalar product ab = kakkbk cos γ (where a and b are two example vectors, and γ is the angle between them) and applying it to the constraint we get:
k ˙qkkXk cos γ − TB−1krosk2 ≤ C
√
X0B−1X
Let’s consider the first approximation: X0B−1X ≥ λmin(B−1)kXk2, where λmin(B−1)
represents the minimum eigenvalue of the B−1matrix. This approximation reduces the possibility of finding the optimal solution for our problem because we are making our bound more conservative.
Going back to our constraint and considering the worst-case scenario so that we can also remove the dependency by γ (cos γ = 1):
k ˙qkkXk − TB−1krosk2 ≤ C p λmin(B−1)kXk k ˙qk − TB−1krosk 2 kXk ≤ C p λmin(B−1) k ˙qk − T−1 B krosk2 kJ0 srosk ≤ Cpλmin(B−1)
The left-hand side can be furthermore simplified taking more approximations: k ˙qk − TB−1 krosk 2 kJ0 srosk ≤ k ˙qk − TB−1 krosk 2 kJ0 skkrosk ≤ Cpλmin(B−1)
Remembering now that Js = Ja + sJba, substituting and taking again the same
28 CHAPTER 4. IMPLEMENTED ALGORITHMS k ˙qk − TB−1 krosk kJ0 a+ sJba0 k ≤ k ˙qk − TB−1 krosk kJ0 ak + skJba0 k ≤ Cpλmin(B−1)
Finally, we will consider the worst case where s = 1 and a sufficient condition for the human-obstacle distance krosk needs to be verified. This sufficient condition
consists in verifying not all the possible values of s but only min
s krosk (that corresponds
to the minimal obstacle-link distance dmin. See Appendix B for more information about
the resolution of this problem). We then get to the final constraint: k ˙qk ≤ T −1 B dmin kJ0 ak + kJ 0 bak + Cpλmin(B−1) (4.7)
We now need to consider the case where a velocity reduction is necessary, and then a scaling parameter δ needs to be calculated.
If this final constraint is satisfied no scaling of the nominal velocity is required otherwise if it is not we need to understand how much the constraint is violated. To understand that we impose the following equality:
k ˙qk = T −1 B dmin kJ0 ak + kJba0 k + Cpλmin(B−1) + m
where m corresponds to how much the constraint is exceeded.
Combining this equation with another equality allows us to introduce the scaling of the joint velocities with the correct delta to apply to have the condition satisfied:
k ˙qkδ = T −1 B dmin kJ0 ak + kJba0 k + Cpλmin(B−1) = A
Combining the two we get to the formula to get the optimal delta: δ = A
A + m
that is repeated until (4.7) is not satisfied ( ˙q is scaled as ˙q = δ ˙q).
Since the more we get near to the optimal δ m becomes smaller and smaller. The equation has then been slightly modified to achieve a faster convergence:
δk+1 =
A A + mδk where k represents the iteration.
4.2. POINTS AS OBSTACLES 29
4.2
Points as obstacles
Considering the obstacle as a series of surfaces brings about serious computational con-sequences that we will discuss in the next section.
For this reason, we developed another algorithm (the one that we used) that models the human as a series of 3D points.
The series of points (that from now on will be called pointCloud) is given from the Kinect camera that supervises the collaborative workspace.
To simulate a series of points a very simple obstacle is considered: a discretized sphere.
Figure 4.4: Simulation setup
Then, after realizing that this was the best algorithm, we decided to pass to a more realistic obstacle.
To do so, we used the MoCap Toolbox [14] that is a series of dataset of frequent human movements like walking, running, and so on. With the included functions, we managed to extract the 3D coordinates of each frame, considering them as an obstacle.
30 CHAPTER 4. IMPLEMENTED ALGORITHMS
Figure 4.5: Simulation setup with a human-like obstacle
As expected, the algorithm performed well also in this case where the obstacle can change position (always a null velocity of the obstacle is considered vH = 0). Here in
the following a sequence of frames where we can see that the robot slows down when the human passes next to him (a higher concentration of frames is present):
4.2. POINTS AS OBSTACLES 31
4.2.1
PFL
In this section, we will develop the solution of the problem satisfying only the PFL constraint. Restarting from the equation (4.1) where both the SSM and PFL constraint are present and eliminating the SSM part:
˙ q0X ≤ TB−1krosk2 | {z } =0 +C√X0B−1X ˙ q0X ≤ C √ X0B−1X
Considering the same computations already done for the previous part for the left-hand side of the equation we get to (4.2):
˙ q0X = ˙q0p0 |{z} a0 + ˙q0p1 |{z} a1 s + ˙q0p2 |{z} =0 s2
where p0and p1 had already been defined, but we repeat for the sake of clarity:
p0 = Ja0roa
p1 = Jba0 roa− Ja0rba
We reach then the following equation: g(s) = a0+ a1s ≤ C
√
X0B−1X
Because the right-hand side is always positive (C is always a positive number for definition, and the result of the square root is always positive) a sufficient condition to see that the constraint is satisfied is to verify that the maximum of g(s) that it is a linear function inside the domain s ∈ [0, 1] is below zero.
Because a linear function limited in a domain has the maximum in one of the two ex-tremes evaluating if it is smaller than zero is trivial:
32 CHAPTER 4. IMPLEMENTED ALGORITHMS
Figure 4.7: Two possible cases on how g(s) evolves In case the constraint is satisfied, no velocity scaling is required.
On the other hand, if it is not, we continue our computations expressing the dependency by s of the right hand-side of the equation.
First of all we need to square both sides:
(a0+ a1s)2 ≤ C2 X0B−1X
Remembering that X = p0+ p1s and substituting we get:
a20+ a21s2+ 2a0a1s ≤ C2 (p0+ p1s) 0
B−1(p0+ p1s)
Doing the computations and rearranging it as an equation depending on s we get: p01B−1p1− a2 1 C2 | {z } γ2 s2 2p00B−1p1 −2a0a1 C2 | {z } γ1 s p00B−1p0− a2 0 C2 | {z } γ0 ≥ 0 g(s) = γ2s2+ γ1s + γ0 ≥ 0
4.2. POINTS AS OBSTACLES 33 We now need to find the minimum value of this function and understand if it satisfies the constraint of being bigger than zero in the domain s ∈ [0, 1]. We need to check the boundaries and the stationary point − γ1
2γ2.
Again if it is satisfied no scaling is necessary, otherwise, we have found the critical point in our robot link (scrit) for which we will need to impose a velocity reduction to make
the constrain consistent. This point coincides with one of the three previously stated points whose function value is smaller than zero and is the minimum between the three (we consider only the most critical one even if more than one is present).
Let’s introduce then the velocity scaling factor δ ( 0 ≤ δ ≤ 1) that is multiplied for the joint velocities in order to reduce them if necessary. Once the previously stated critical point is found, we need to understand how much to scale down the velocity to make our critical point safe. To do so, we need to consider the initial constraint (4.1) evaluated in the critical point with two different equations:
ros0 critJscritq˙nom− C
p X0(s crit)B−1X(scrit) | {z } A = m ros0
critJscritq˙nomδ − C
p X0(s crit)B−1X(scrit) | {z } A = 0 (4.8)
ros0 critJscritq˙nom− A = m
ros0 critJscritq˙nomδ − A = 0
(4.9) the first one is the same constraint equation that has been transformed into equality where m indicates how much the constraint is not satisfied while the second one is the same equation as the previous one but with the optimal scaling parameter.
Combining the two we get to the following solution of delta: δ = A
A + m
After updating the scaled joint velocities ( ˙qk = ˙qk−1δk) we need to check again if
the constraint equation is satisfied.
It may happen that once the optimal delta has been calculated, the considered link still does not fulfil the constraint. This because the critical point in the link is just shifted ahead and it still does not satisfy the constraint. Because of that, we need to iterate this method until the new calculated scrit respects the constraint. Then, for a faster
34 CHAPTER 4. IMPLEMENTED ALGORITHMS δk=
A
A + mδk−1 where k represents the iteration index.
Figure 4.8: Example of how scritshifts along the link. Four iterations are necessary in
this case.
4.2.2
SSM
In this section, will be elaborated the solution of the problem satisfying only the SSM constraint.
The initial reasoning is the same as in the previous part with the only difference that now the PFL part is removed:
˙ q0X ≤ TB−1krosk2+ C √ X0B−1X | {z } =0 ˙ q0X ≤ TB−1krosk2 a0+ a1s ≤ TB−1krosk2
The right-hand side of the equation, after some calculations, (remembering that ros= rObst− rs = rObst− ra− srba = roa− srba), can be decomposed as:
TB−1krosk2 = TB−1kroak2− TB−12r 0
4.2. POINTS AS OBSTACLES 35 The overall equation then becomes:
a0+ a1s ≤ TB−1kroak2− 2TB−1r 0
oarbas + TB−1krbak2s2
Gathering everything up in an equation depending only by s we get: −TB−1krbak2 | {z } α2 s2+ a1+ 2TB−1roa0 rba | {z } α1 s + a0− TB−1kroak2 | {z } α0 ≤ 0 g(s) = α2s2+ α1s + α0 ≤ 0
Also, in this case, the first thing to check is that the maximum of the equation g(s), considering the domain s ∈ [0, 1] (so we consider both the two boundary points and the stationary one), is smaller than zero.
If it is not, in this case, we already have found the critical point scrit (the maximum
between the three previously stated points), and we need then to find how much negative the equation is. This critical point allows us to understand how much to scale down the joint velocities with the scaling factor δ. To do so, as in the previous method, we consider the current constraint but in two forms:
ros0 critJscritq˙nom− T
−1 B krosk2
| {z }
A
= m ros0 critJscritq˙nomδ − T
−1 B krosk 2 | {z } A = 0 (4.10) making the proper substitutions, we again get to:
δ = A m + A
As in the previous method, it can happen that even with the new scaled delta, the constraint is still not completely fulfilled because the critical s is just shifted ahead in the link.
The method is then iterated until a delta valid for the full link is found: δk =
A
36 CHAPTER 4. IMPLEMENTED ALGORITHMS
4.2.3
SSM + PFL
In this section, we will develop the solution of the problem satisfying both the SSM and PFL constraints at the same time.
We need to restart from (4.1), but this time we keep the whole equation: ˙
q0X ≤ TB−1krosk2+ C
√
X0B−1X (4.11)
The left-hand side does not change so it will be decomposed as: ˙ q0X = ˙q0p0 |{z} a0 + ˙q0p1 |{z} a1 s + ˙q0p2 |{z} =0 s2 (4.12) Where p0, p1and p2 have already been defined but we repeat for the sake of clarity:
p0 = Ja0roa
p1 = Jba0 roa− Ja0rba
p2 = −Jbarba
.
Now that we have developed the left hand side of (4.11) let’s pass to the right-hand side:
krosk2 = krObst−rsk2 = kro−ra−s(rb−ra)k2 = kroa−srbak2 = kroak2−2sr0oarba+krbak2s2
Substituting back in (4.11) we have: a0+ a1s ≤ C √ X0B−1X + T−1 B kroak2− 2sTB−1r 0 oarba+ krbak2s2TB−1
and rearranging the equation in a simpler form we get: (−krbak2TB−1 | {z } α2 )s2+ (a1+ 2TB−1r 0 oarba) | {z } α1 s + (a0− TB−1kroak2) | {z } α0 ≤ C√X0B−1X g(s) = α2s2+ α1s + α0 ≤ C √ X0B−1X (4.13)
4.2. POINTS AS OBSTACLES 37 This last equation is important because it allows us to make a quick check if the con-straint is satisfied. The left-hand side is a quadratic function, and its maximum (if we limit the domain s between [0, 1]) is or at the extremes of the domain or in the stationary point. Note that the right side is always a positive number.
Once the stationary point sstathas been found we take the maximum between
[g(sstat), g(0), g(1)] and verify that this is lower than zero. This check is a sufficient
condition to verify that the whole function is negative then the constraint is verified, and there is no necessity of scaling the robot velocity.
If the constraint is not satisfied, we need to understand if the function g(s) changes sign inside its domain of [0, 1].
The next thing to do is then finding the roots for the function that is a second-order polynomial limiting them to the previously said domain. Three cases can happen: both are inside the domain (case 2), or only one between the two is inside the domain (case 1 and case 3). Being in case 1, 2 or 3 has a geometrical implication for the considered link; this undergoes a reshaping because it means that a part of it is ’safe’ and does not need to be checked for the consistency of the constraint while the other part may or may not satisfy (4.11). In the following, it is shown how the link can be modified depending on the case:
Figure 4.9: Link reshaping depending on the case If both the roots are outside the domain no reshape is necessary.
In any case, after the reshaping, it is necessary to develop more the equation consid-ering also the right-hand side that depends on s leading to more complicate computa-tions. Restarting our computations from (4.13) we squared both sides:
α2s2+ α1s + α0
2
≤ C2X0B−1X and remembering X = p0+ p1s
38 CHAPTER 4. IMPLEMENTED ALGORITHMS α2s4+ α11s2+ α02+ 2α2α1s3+ 2α2α0s2+ 2α1α0s ≤ C2(p0+ p1s)0B−1(p0+ p1s)
After some computations and re-arrangements we arrive to a fourth degree polynomial. −α 2 2 C2 s4+ −2α3α1 C2 s3+ p01B−1p1− α12 C2 − 2α2α0 C2 s2+ 2p01B−1p0− 2α1α0 C2 s+ p00B−1p0− α2 0 C2 ≥ 0 (4.14) Or just simply: f (s) = γ4s4 + γ3s3+ γ2s2+ γ1s + γ0 ≥ 0
To check the consistency of the constraint, we need to find the minimum of the func-tion and check that is bigger than zero so that the whole funcfunc-tion satisfies the constraint and no speed reduction is needed.
The derivative of a fourth-order function is, clearly, a third-order function: ∂f ∂s = 4γ4s 3 + 3γ3s2+ 2γ2s + γ1 = γ4∗s3+ γ3∗s2+ γ2∗s + γ1∗ (4.15) To find the stationary points, we need to impose it equal to zero and solve a third- degree polynomial.
The Cardano solution has been used to solve the third-order polynomial (you can find more details about this in A).
For the fundamental algebra theorem, the number of solutions of the polynomial is equal to its degree, so for a third-order polynomial, we have three solutions. Because the imaginary solutions are not of our interest, and remembering that they always come in couple, we can have one or three real solutions that we need to evaluate plus the
4.3. COMPARISON OF THE DEVELOPED STRATEGIES 39 boundaries of the domain.
We need then the minimum between [f (0), f (1), f (s.p.1)] or [f (0), f (1), f (s.p.1),
f (s.p.2), f (s.p.3)] depending on the number of real solutions. Once such a minimum
has been found if it is bigger than zero, no scaling of the velocity would be necessary. Conversely, a scaling parameter is calculated.
If the minimum of the function is smaller than zero, we need to understand how much is below zero so that to calculate the optimal scaling factor. We then need to introduce the scaling parameter δ and calculate it.
To do so, the initial constraint is considered (4.11) and transformed into two equations: ˙ qX = TB−1krosk2+ C √ XB−1X | {z } A +m δ ˙qX = TB−1krosk2+ C √ XB−1X | {z } A (4.16) Solving this system of equations, we get to the optimal delta:
δ = A A + m
As always, this method needs to be repeated until the calculated scrit respects the
constraint. For a faster convergence of delta the previous formula is modified as follows: δk =
A
A + mδk−1 where k indicates the iteration we are at.
4.3
Comparison of the developed strategies
We will now compare the algorithms developed up to now in the following way: • Surface Obstacle vs Point Obstacle
• SSM+PFL vs SSM vs PFL
For what it concerns the choice between modelling the obstacle as a surface or a set of points, the choice has been rather straightforward. The reason for this is that even for a minimal number of surfaces (simulations has been done with only four surfaces while in the real application there would have been hundreds of thousand) the computational
40 CHAPTER 4. IMPLEMENTED ALGORITHMS time is high (around 17 seconds for a trajectory lasting 18.5 seconds). This high com-putational time is due to the fact that we need to solve a quadratic optimization problem for each loop where the function to be optimized is tricky.
If this was not enough, in the procedure to find the solution to the problem several ap-proximations have been taken into consideration, causing the constraint to be too much conservative, leading to slow joint velocities that ensure the safety level required but cause an enormous production inefficiency.
Once decided that dealing with point obstacle is both computationally and efficiently speaking more advantageous, we need to understand if our algorithm SSM+PFL is bet-ter than SSM only and PFL only.
Then, this experimental setup in simulation has been created:
Figure 4.10: Simulation setup
This setup consists of the IRB140 robot from ABB together with a discretized sphere composed of 121 points, and the robot path passes next to the obstacle (without impact-ing) so that we can appreciate the differences between the algorithms.
As we will see in the next figure (and referring to figure 1.3) PFL is the slowest because it does not take into consideration the distance robot-human. It supposes that in whatever point a collision may happen then a limited velocity that does not bring about injuries is imposed. SSM is rather fast almost as SSM + PFL. However, it loses
effi-4.3. COMPARISON OF THE DEVELOPED STRATEGIES 41 ciency when the distance gets smaller and smaller. SSM + PFL is, as initially supposed, the fastest one. The following figure shows the first joint variable q1 that starts at an
initial position of 0 radiant, and it ends at π2 radiant, comparing its nominal trajectory (the blue one) with the ones obtained with the other algorithms (SSM, PFL and SSM + PFL):
Figure 4.11: The blue line represents the nominal trajectory of the first joint variable, the red one is with SSM + PFL, the yellow one with SSM and the purple one with PFL. As we can see the PFL is the slowest one and takes a long time to finish the tra-jectory (the finishing time is around 18 seconds), SSM is faster (around 4.5 seconds to completion) but not as much as SSM + PFL (around 3.8 seconds to completion). In the next graphs, we will also show how the joint velocity for the first joint variable evolves and, of course, SSM + PFL reaches higher velocities before the other two algo-rithms. One thing to notice in the joint velocity graph is the velocity spike that the robot has at around 3 seconds for SSM + PFL and 4 seconds for SSM. These spikes are due to the fact that the robot understands that the obstacle is not anymore in the robot path, and so no collisions can happen, leading to an increase in velocity.
The last graph represents how delta evolves for the three algorithms confirming what we said up to now.
42 CHAPTER 4. IMPLEMENTED ALGORITHMS
Figure 4.12: The blue line represents the nominal angular velocity of the first joint, the red one is with SSM + PFL, the yellow one with SSM and the purple one with PFL.
Figure 4.13: The blue line represents the nominal delta, the red one is with SSM + PFL, the yellow one with SSM and the purple one with PFL.
4.4. THE CLEARANCE PARAMETER 43
4.4
The clearance parameter
Until now the link has been considered without accounting for its thickness, and this could lead to some heavy approximations in our calculations assuming the link to be dimensionally speaking smaller than it is.
To consider it, we need to slightly change the constrain (4.11) going back of some steps, from: n0Jsq ≤ T˙ B−1krosk + C p n0J0 sB−1Jsn (4.17) to n0Jsq ≤ T˙ B−1max {0, krosk − ∆} + C p n0J0 sB−1Jsn (4.18)
where the value ∆ represents the thickness of the considered link, as shown in the following picture:
Figure 4.14: The link with a considered thickness of ∆ Remembering that n = (ro−rs)0
kro−rsk =
r0os
krosk and substituting it back to the equation we
get: r0os krosk Jsq ≤ T˙ B−1max {0, krosk − ∆} + C p n0J sB−1Js0n ˙ q0Js0ros ≤ TB−1krosk max {0, krosk − ∆} + C p r0 osJsB−1Js0ros
44 CHAPTER 4. IMPLEMENTED ALGORITHMS Let’s now consider only the term krosk max {0, krosk − ∆}.
As we can see the maximum value that this can have is krosk2 in case ∆ = 0. We can
then define a lower bound (so a more conservative constraint) for this part as follows: βkrosk2 ≤ krosk max {0, krosk − ∆}
βkrosk ≤ max {0, krosk − ∆}
β ≤ 1 − ∆ krosk
This new lower bound means that, if we manage to make the constraint valid in case we use the equation on the left βkrosk2with β = 1 −kr∆osk then also the initial constraint
will be fulfilled.
In case the clearance is not correctly specified, or we want a large value of it for safety reasons (so the robot link is supposed bigger than it is) the value of β would be negative and would not make sense. For this reason has been modified to:
β = max
0, 1 − ∆ krosk
Then, the equation constraint that comprehends the clearance ∆ is as follows: ˙
q0Js0ros ≤ TB−1βkrosk2+ C
p r0
osJsB−1Js0ros
As we can see differently from the constrain we have taken into consideration until now the value of β can be easily integrated with the constant TB−1then no changes would be done to the previously stated algorithms except for changed constant parameters.
Chapter 5
Data Pre-processing
In this chapter, we will discuss the sensor used for the workspace supervision and how the data it provides are analyzed and processed.
5.1
The Kinect
In this section, we will describe the sensor that supervises the workspace: the Kinect camera. The Kinect is an RGB-D camera developed by Microsoft for motion sensing that integrates an RGB camera with infrared projectors and detectors that allow us to have a 3D depth map of the environment in the field of view along with an artificial in-telligence system that ensures gesture recognition, speech recognition and body skeletal detection [15].
Figure 5.1: The Kinect camera
In our setup, the following features have been used: skeletal body detection and the infrared camera that uses the time-of-flight to extract the pointCloud datatype. The skeletal body detection gives the 3D coordinates of the following body parts: shoulders, elbows, hands, hips, head and a central point of the torso.
46 CHAPTER 5. DATA PRE-PROCESSING From these points, the human has been approximated as a series of cylinders and spheres. Each one of them starts and ends at one of these reference points. To make an example the head (and the hands) has been considered as the centre of a sphere then, after spec-ifying a safety radius, the whole approximate occupied volume of the head is known. In turn, the upper arms have been considered as starting at the extracted shoulder points and finishing at the extracted elbow points and, after specifying a safety radius, the occupied cylindrical volumes are known.
Figure 5.2: The skeletal extraction of Kinect of two operators
Figure 5.3: The setup with the Kinect inside the SmartRobots device (inside the green circle on top of Yumi)
5.2. POINCLOUD FROM KINECT AND HUMAN FILTERING 47 Once known the approximated volume occupied by the operator in the collaborative workspace we extract from the Kinect the pointCloud data.
5.2
PoinCloud from Kinect and human filtering
This pointCloud data gets extracted from one of the Kinect sensors: the infrared camera. This data represents a discretization of the whole workspace in a series of 3D points. From this, we need to filter out all the points not belonging to the operator, and this can be easily done because we know the regions occupied by the worker thanks to the pose estimation functionality.
After having filtered out the points not belonging to the human, we remain with around 25000 points. For an operation of this kind computational speed is essential to guarantee the operator safety then the faster the algorithm computes a new solution, the safer the working environment will be. We need then to have the algorithm as fast as possible. To ensure that, we decided to reduce the number of points considered during each algo-rithm computation loop. This operation allows us to achieve a faster computation, but it also has some drawbacks, in fact, if we decide to reduce too much the number of points that we consider problems may arise.
Neither a slow algorithm nor keeping a low number of points ensure the worker safety. The human can be discretized in so few points that some areas may be left uncovered by points leading to possibly dangerous situations.
There is then a trade-off between the complexity of the algorithm we can achieve and the potential dangers that can arise from this decision.
In the implemented algorithm 1 out of 5 points has been taken. This reduction ensured a good increase of the algorithm speediness while, at the same time, maintaining a good safety level.
In the following, an image that represents how the sequence works. First of all, the pointCloud and the skeletal image is acquired, then their information is combined, ob-taining a pointCloud where only the human is present. Finally, only 1 out of 5 points are considered in the final algorithm.
48 CHAPTER 5. DATA PRE-PROCESSING
Figure 5.4: These phases show the sequence of how the pointCloud data is acquired and properly filtered
Chapter 6
Experimental results
In this chapter, we will discuss: what robot has been used, how its kinematic and dy-namic parameters have been calculated, how the experiments have been set up and their results.
6.1
The robot
6.1.1
Description
The used robot in the conducted experiments is the IRB 14000 (or Yumi).
Yumi is a flexible and agile robot developed by ABB to allow companies (especially the ones that deal with small components like electronics) to allow their employees to safely work side-by-side and without barriers with Yumi granting a higher level of flexibility while maintaining a high level of productivity.
They are endowed of two arms with seven-axis each that can, at the same time, work for the execution of a task reaching a maximum payload of 0.5 kg for each arm.
Each arm is characterized by a camera and interchangeable servo grippers to nimbly and without failing grasp objects.
It is made of a lightweight yet rigid magnesium skeleton covered with a floating plastic casing, padded with soft material that absorbs any kind of shock or impact and designed without sharp edges that may be a danger for the operators (this characteristic makes it a mechanically consistent robot for the ISO 15066 guidelines). Moreover, to ensure an even safer working environment, it already integrates the PFL algorithm.
Because of this, we had been constrained to reduce the possible applied force threshold to make sure that the constraint is activated during operations,
50 CHAPTER 6. EXPERIMENTAL RESULTS During the path planning computation, an embedded collision-free algorithm that checks in real-time if any collision between each part of the arms is going to happen is executed. In case a severe collision with an external object happens, the robot raises a collision error and stops immediately.
Figure 6.1: IRB 14000 (Yumi)
6.1.2
Kinematic and dynamic parameters
To know the 3D position of the robot in the workspace, we need to specify a consistent reference system for each link of the robot (Denavit-Hartenberg convention has been adopted). We need then to define the kinematic parameters that link each reference system to the other up to the end effector and derive the cumulative matrix of each link end, so that, after the robot has sent the angular position measurements of the encoders, we can calculate precisely the 3D position of each link end (corresponding to the last column of the cumulative matrix).
Not only the kinematic parameters are needed but also the dynamical ones as the inertia tensor and masses of each link. If the production company gives the mass data of each link, the same cannot be said for the inertia tensors. In our algorithm, they are necessary for the computation of the mass matrix of the robot B(q), which is used to calculate the robot reflected mass.
Unluckily, no study has been conducted in this new robot for the identification of these parameters (the robot is rather new: 2015).
6.1. THE ROBOT 51 Starting from the kinematic parameters, as already said, the Denavit-Hartenberg convection has been followed and here in the following is reported a sketch model with the reference frame link by link:
Figure 6.2: Reference frames representation following DH convection
Hereafter the Denavit-Hartenberg table done with respect to the previous model has been calculated:
