Safe Human-Robot Collaboration via Collision Checking and Explicit Representation of Danger Zones

This paper deals with safe human-robot collaboration in the context of speed and separation monitoring paradigm. The core of the approach is to continuously track the separation distance between the robot and the human. The robot speed is then adjusted according to the perceived distance so that it will be able to stop before eventually come into contact with the human. We present an approach that aims at maximizing the productivity of the robot, i.e., its speed, while keeping the prescribed safety requirements satisfied. The method is based on explicit representation of danger zones – regions around the robot, where safety requirements are violated. The motion is then generated such that the robot moves as fast as possible, while its danger zone still does not collide with human operators. The approach is validated within an experimental study. Note to Practitioners—This article was motivated by the problem of maximizing productivity of the robotic manipulator while ensuring the safety of human collaborator. The increase in productivity is achieved by a faster traversal of predefined paths without compromising the safety of the human, which is specifically defined by industrial standard. The approach requires limited knowledge on robot’s dynamical properties. More precisely, we only need the braking time as a “lumped” representation of robot’s inertia. The underlying optimization problem is conveniently resolved by introducing danger zones that allow for intuitive visualization and geometrical representation of the regions around the robot that must be avoided. On the other hand, the method assumes the representation of humans via typical geometric primitives, which can be obtained using of-the-shelf depth perception systems. The solution to the problem reduces to a repeated collision checking between danger zones and the human. Such an approach turns out to be suitable for real-time implementation due to availability of fast and efficient collision checking algorithms/libraries.


I. INTRODUCTION
I N TODAY'S industrial contexts, removing physical barriers between human workers and robots has been a regular objective. Contemporary technologies that are in line with Industry 4.0 standards presume adaptable, reliable, and flexible production lines that require people and robots to work together closely. Accordingly, the area of human-robot collaboration (HRC) has grown dramatically from research lab demonstrations to real-world applications that are gaining traction even among small and medium-sized businesses. Emerging control techniques, coupled with contemporary hardware, geared at maintaining the most important feature of HRChuman safety -often make conventional safety standards too rigid and practically outdated [1]- [4].
There are several techniques to investigating and ensuring the safety of humans in the presence of robots in the HRC literature. The initial efforts were focused on developing the first HRC-related safety criteria [5]. Later on, a succession of attempts were made to establish more detailed measures and apply them in a feedback control system to improve safety [6]- [11].
In a series of publications, Haddadin et al. present an exhaustive investigation of physical human-robot interaction (HRI) including the themes of risk assessment, collision detection and reaction, and robot design [12]- [14]. The authors provide a thorough assessment of HRI safety, including several elements of the most important injury mechanisms.
In [15], an integrated framework for safe HRC is presented. It includes both social and physical components for finding an optimal velocity of robot. The authors argue that the inclusion of social component remarkably enhances the capabilities of the framework. The issue of safe collaboration in a wider context of cyber-physical approach is tackled in [16]. The system evaluates safety distance in real-time and uses closed-loop control to invoke actions necessary for preventing collisions. An interesting solution that involves human-in-theloop concept for manipulation planning in the context of safe collaboration can be found in [17]. The developed system allocates the subtasks to robots and humans taking into account  [18] offers a learning-based hazard estimator that arbitrates between safety and efficiency. The training stage learns the control policy, while in the testing phase, guiding goal along a given path is selected as a trade-off between safety and task performance. In [19], the authors propose a Petri-nets-based scheduling method for collaborative assembly task. The approach devises optimal plans for assembly activities using online acquired knowledge. It is able to adapt to variations during a manufacturing process thus minimizing idle times for each agent.
In some recent techniques, the safety in collaborative setups is ensured by maintaining a speed-dependent separation gap between the human and the robot throughout operation [20]- [22]. This method, known as speed and separation monitoring (SSM), is compliant with more recent safety standard [23] and may even be used to make people collaborate with regular industrial robots. In a rather elaborate scheme proposed in [24], a multimodal perception system is used within a fuzzy-based inference system to assess the risk and dynamically compute the minimum protective distance.
Another technique known as power and force limitation (PFL) has been created for scenarios that enable physical interactions [31]. This method is based on establishing an upper bound of energy that may be transferred from the robot to human in the event of a collision [32]. Similarly to SSM, the PFL method is based on a safety standard. In addition, it involves the use of specially constructed manipulators [20], [31]. Recently, several attempts have been made to combine SSM and PFL techniques to further push the boundary of enhancing productivity while satisfying safety requirements according to the safety standard [33], [34]. In [27], the authors proposed an elaborate approach to compute minimum-time yet safe trajectories along specified paths in shared workspaces with humans. The safety is iteratively evaluated via a dedicated module that includes interaction/collision model whose role is to capture inertial properties along directions of interest. Furthermore, the replanning and recovery routines engage when the plan respectively violates safety requirements or when the higher speeds may be resumed. For an interested reader, [35] and [36] offer comprehensive views on humanrobot-interaction safety mechanisms.
In [37], a method for determining dynamic separation distance and adjusting robot speed is provided. The validation is based on a reduced representation of human geometry that only accounts for the centroid of a single human part. In a series of papers, the authors devised a kinematic control technique that assures safety while maintaining a high degree of robot productivity [25], [26], [38]. The robot's velocity is calculated using an optimization-based real-time method in which safety is considered a hard constraint to meet [25]. Rather than examining merely a discrete set of locations, the technique considers the whole geometry of both the human and the robot. However, in order to make the optimization problem tractable, some sufficient conditions have been assumed, resulting in conservative safety requirements. An improvement of the above approach has been presented in [30], where the same problem has been solved with the exclusion of conservative assumptions. The validation of the method showed substantial speed-ups in robot's motion, without jeopardizing the safety requirements. The price was paid by the increase in computational complexity, which turned out to be a limiting factor with respect to real-time implementation involving more detailed geometric representation of robot and/or human.
In this study, we tackle the same problem as in [25], [26], [30] from the geometrical point of view. The main contribution of the paper is the introduction of danger zones: 3D volumes assigned to robot's links that represent regions of space where safety requirements are violated. The paper presents a simple approach to explicitly compute the danger zones. In particular, the possibility to represent the boundary of a danger zone as a polygonal mesh allows to use fast and efficient collision-checking routines for testing the possible violation of safety requirements, by simply checking whether a human intersects the danger zone. This approach reveals an underlying geometrical intuition of the method that is missing in the original analytical solution from [30]. More importantly, it relaxes the computational burden and therefore facilitates real-time implementation of the algorithm. A conceptually similar approach is proposed in [28], [29], where collisions between human and robot are investigated by online intersection tests for bounding volumes surrounding the robot's links and those evaluated around the human. The protective volumes used for bounding the links are simple capsules. Moreover, the approach requires the dynamic model of the robot.
It is worth pointing out that in this paper, we focus on scenarios where the knowledge about the robot's dynamics is limited. More precisely, the full dynamic model of the manipulator is considered unavailable. The only required parameter is the "braking time" T b denoting the worst-case time necessary for the robot to transit from full-speed motion to a full stop, once the stop command has been issued. Table I gives a condensed comparison of features from the proposed approach and a selected set of related methods from the literature.
The preliminary results of this study can be found in [30]. This paper is complemented with the following extensions.
1) Analytical definition of danger zones is provided, along with a detailed procedure for their construction. Two options are outlined: the almost exact representation via triangular mesh, and an approximate convex representation. The complexity of the subsequent collision checking is discussed. 2) An improved method for adjusting the braking time T b is proposed to further increase the performance in terms of shortening the production cycle.
3) The method is generalized to accommodate for more realistic geometric representation of robot's links. 4) The simulation study is broadened to include the comparison to two additional relevant methods. 5) Unlike in [30], the approach is experimentally validated. The remainder of the paper is organized as follows. Section II provides the background of the proposed method, along with a brief description of the analytical approach from [30]. In Section III, we present the main contribution of the paper -the definition of danger zones and the manner in which they are used to generate robot motion. Section IV provides an extensive simulation study where the proposed approach is compared to the existing methods. In Section V, the results of the experimental validation are presented, while concluding remarks are drawn in Section VI.

II. BACKGROUND
This section provides the background on safety constraints used in the approach. Generally, it is based on SSM paradigm [23], and its derivation builds on work published in [25], [26] and [30]. At first, we consider the relationship between a single link and an obstacle. For simplicity, we assume the obstacle to be a point. Eventually, more general obstacle shapes will be considered: first a triangle in space, and finally an arbitrary polygonal mesh surface. Moreover, the robot's link is assumed to be a thin rigid beam (Fig. 1). Later on, we will extend this approach to include more general geometric shapes.
Let a generic point on the link r s = r a + s( 1], and a point r o that belongs to an obstacle (e.g., a triangle with vertices r 1 , r 2 , r 3 ). The safety constraint can be expressed as follows [26]: where r os = r o − r s and T b is the braking time. For now, condition (1) holds for a specific pair of points r s and r o . The optimal trajectory scaling procedure that is based on collision checking with danger zones, which will be explained later, ensures that this condition holds for arbitrary points r s and r o , where r s belongs to a link, and r o belongs to a triangle. The quantity v os is the magnitude of the vector v os , i.e., a projection of the vector v s onto r os . The following holds: Using (2), condition (1) can be expressed as: or alternatively in the following form: whereq is the joint velocity vector and J s the position Jacobian associated to point r s . For a point-obstacle at r o , it is necessary and sufficient to ensure that (3) holds ∀s ∈ [0, 1]. The distance r os is treated as a function of link parameter s and possibly other parameters that define the obstacle surface.
Establishing the value of the braking time T b is pivotal in this analysis. Primarily, this value depends on the chosen mode of the stopping/braking operation that is specified by a relevant standard. In particular, the IEC Standard [39] distinguishes three different "categories" of stopping/braking operations: • Stop Category 0: stopping by immediate removal of power to the machine actuators (i.e. an uncontrolled stop); • Stop Category 1: a controlled stop with power available to the machine actuators to achieve the stop and then removal of power when the stop is achieved; • Stop Category 2: a controlled stop with power remaining available to the machine actuators. In robotic applications, Categories 0 and 1 may include the engagement of the electro-mechanical brakes at certain time instants. Some robot manufacturers declare braking times within the available product specifications (see e.g., [40]). Braking times vary with respect to robot's payload and the workspace zone in which it is operating. The most reasonable choice of the braking time T b for the purposes of our approach is the largest value that corresponds to worst case scenariorobot moving at full speed while in the configuration with the highest inertia. Should this specification not be available, under the assumption that Stop Categories 1 or 2 apply, the braking time can be experimentally determined or estimated using the following formula: Here, J max stands for the robot's maximal moment of inertia, as perceived by the first joint, when the robot is fully outstretched, whileq 1,max and τ 1,max denote maximal joint velocity and torque for the first axis. The quantity J max may be estimated without the full knowledge of the robot's dynamics.
Note that such obtained value of T b can be augmented with the reaction time T r , i.e., a time required by the system to detect and react to changes in the environment. As it will be discussed later, additional safety margins can be absorbed by the braking time T b as well. For a more elaborated treatment of braking behavior in the context of robotic manipulators, the reader is referred to [41]- [44], Supposeq n (t) is a nominal joint velocity vector that corresponds to task execution in absence of human operators. In the context of consistent following of a geometrical path (that corresponds to the trajectoryq n (t)), we aim at maximizing the scaling coefficient δ for computing the joint velocity vectorq in the formq = δq n . In other words, when humans/obstacles are present in the robot's workspace, the goal is to slow down the nominal trajectoryq n (t) as little as possible, while at the same time satisfying the safety constraint (4).
To make the problem computationally more convenient, Zanchettin et al. [26] solved a more conservative version of the problem by forcing a sufficient condition: or: The term min s r os is a constant (distance from the point r o to a link with endpoints r a and r b ). A quadratic condition (3) is therefore reduced to a linear one, which renders the problem significantly easier to solve. Taking into consideration n robot links and using the fact that β and γ are linear with respect to joint velocity vectorq, the safety criterion can be expressed as [26]: where T bi is the maximum braking time up to joint i , The system of inequalities (8) represents constraints within a linear programming optimization problem that aims at maximizing the scaling coefficient δ for computing the joint velocity vector in the formq = δq n (see [26] for details). Such "linearized" version of the problem scales well with respect to more general representation of obstacles (e.g., triangles or convex polyhedra) [25], [38].
In [30], the problem is analytically resolved with the original quadratic constraints (3) or (4), without imposing sufficient conditions to make the optimization problem linear. The solution has been verified in a simulation study that showed significant performance improvements in terms of decreasing the production cycle without jeopardizing the safety constraints. For experimental validation however, it is required to ensure the real-time executability. While this is actually feasible for relatively simple geometric representation of human surface (i.e., a small number of points/triangles), our goal is to extend the scope of the method toward more complex scenarios by preventing safety constraint checks from being a computation bottleneck. In this regard, we propose a more intuitive, geometric solution to the same problem. 1

III. PROPOSED APPROACH -GEOMETRIC SOLUTION
In this section, we describe an alternative approach to solving the same SSM problem. The main idea behind the approach is the following one. Given the state of the link (position and velocity), it is possible to conveniently represent the "danger zone", or the "region to avoid", i.e., the set of all points where the SSM criterion is violated. In particular, we are interested in computing the boundary of this region that is given by a closed surface.
Once this surface is computed, testing whether safety criterion is satisfied reduces to simple collision checks with environmental obstacles. To facilitate collision checks, tight approximations of the danger zones based on simple geometric primitives may be used. Otherwise, more general representation by triangular meshes is an option. In the latter case, a wide variety of libraries are available, e.g., PQP [45] or FCL [46], with extremely fast and efficient collision-checking routines. Assuming convex representation of danger zones and human body parts, specific algorithms such as Gilbert-Johnson-Keerthi (GJK) [47] can be used.

A. Danger Zone Formulation
The computation of the danger zone boundary is based on one simple fact. Observe the point robot at a position r s with velocity v s . All the points r that do not satisfy the condition (3), meet the following inequality: This inequality can be rewritten as: The region corresponding to the above inequality is a ball In other words, the danger zone DZ(r a , where the radius of the sphere changes according to the following formula: Fig. 2 depicts an example of such region. Note that, unlike in [28], where the bounding volume for a link is a simple capsule, the danger zone DZ(r a , r b , v a , v b ) assumes a shape that captures the velocity profile along the link. Due to symmetry, the danger zone results from the rotation (about the line segment o a o b ) of a corresponding plane figure that may be represented in a local frame (see Fig. 3). The boundary p(s) ∈ R 2 of the given plane figure corresponds to the envelope of circle-swept segment and is given by: where It is worth pointing out that the above parameterization of the envelope is valid if ∂ R s ∂s < L, i.e., the radius of the circle should not change faster than the "traversal speed" of the segment. This can however, be easily proven by knowing the explicit formulas for R s and L.

B. Danger Zone Construction
For the practical implementation of danger zones, a proper representation of the surfaces that correspond to their boundaries is necessary. A convenient approach is to compute a number of samples on the boundary ∂DZ of the danger zone DZ. Such set of samples can then be used within any surface reconstruction algorithm (e.g., ball-pivoting [48]) to generate an explicit representation of ∂DZ via triangular mesh. This mesh can be further used for interference computation (collision detection, distance query, penetration estimation) with environment obstacles, which are represented in a same way or by means of other geometric primitives. Fig. 6 shows examples of danger zones represented by triangular meshes.
Clearly, there are many ways to generate the sample points on ∂DZ. In the sequel, we describe two ways to do this. The first approach attempts at capturing the boundary ∂DZ almost exactly, while the second approach is based on the approximation of danger zone by its convex hull, which turns out not to be too conservative.
The idea behind the first approach is depicted in Fig. 4. The first step would be to sample the contour of the danger zone in the 2D local frame from Fig. 3. The frame is spanned by the vectors n and s. For practical purposes, which will be justified later, the sampling of circular caps (each by N 1 points) and the mantle (by N 2 points) is performed separately. Once the contour is sampled, it is rotated with respect to the axis of the danger zone (i.e., the unit carrier vector n) N 3 − 1 times with equal increments within the interval [0, 2π]. Thus, the sampling procedure is complete by which we end up with a total of N 3 (2N 1 + N 2 ) sample points.
The second approach is considerably simpler by taking into consideration only the spherical caps, without sampling the mantle. Convex hull of the spherical caps is actually the hull of danger zone itself. This stems from the fact that the mantle of danger zone is provably not strictly convex. Fig. 5 shows the convex approximation of the danger zone obtained using the second approach. Apart from slightly simpler construction, the main advantage of the second approach is the facilitation of subsequent collision checking, which in general performs better under the assumption of convex geometries. As it will be later shown via numerical investigation, the error due to described approximation is on average considerably small.
A unified pseudocode for both approaches is given in Algorithm 1. A Boolean input CONVEX determines whether first or second approach is used for constructing ∂DZ. The first approach requires the surface to be reconstructed based on samples (e.g., [48]). This is performed in Line 24 of the Algorithm 1 by calling the function ReconstructSurface. This function returns TriangleList -a list of IDs of samples (vertices) that correspond to individual triangles (faces) in the mesh. It is worth pointing out that this function can be called only once, even offline. The resulting TriangleList can be reused for any subsequent mesh reconstruction, provided that the sampling scheme remains the same (i.e., keeping N 1 , N 2 , N 3 the same, as well as the ordering of the samples) for constructing the arbitrary ∂DZ (i.e., for arbitrary position and velocity profile of the link). In topological sense, the graph that corresponds to the mesh remains isomorphic regardless of the danger zone shape (see Fig. 6).

C. Computational Cost -DZ Construction and Collision Checking
The computational cost inherent to the notion of danger zones can be decomposed into two aspects. The first one is the cost of the danger zone construction itself. The second one is related to subsequent collision detection between danger zones and humans/obstacles. From the outline of Algorithm 1, it is clear that the construction phase requires only rather straightforward computations involving forward kinematics, Jacobians corresponding to link endpoints, generating a limited number of samples and performing pure rotations. In case of exact danger zone representation, one possibly expensive operation, i.e., surface reconstruction, can be pre-executed offline to provide the list of triangles that is invariant for a fixed sampling scheme and can be reused later. Therefore, the construction phase does not represent a potential bottleneck for
Here, one should separate two different modes of operation. A computationally more convenient one assumes the convex representation of danger zones. Moreover, it is expected that the human can be represented as a union of convex shapes (see e.g., [38]). In such setting, a natural approach to collision detection is via original GJK algorithm [47], or some of its enhanced incarnations [49], [50]. The convenience of GJK algorithm stems from its linear complexity O(N), where N is the total number of vertices of the two bodies that are being checked for collision [47], [49]. If the convex representation of danger zones is not allowed, one has to seek for an alternative algorithm that allows for non-convex models.
Most of contemporary algorithms for collision detection between general non-convex geometries are heavily based on bounding volume hierarchies (BVHs). BVH models aim at decomposing a body into a tree-like structure. Each node in the tree corresponds to a bounding volume that covers a certain subset of the original body. The root represents a volume that bounds the complete body, whereas the leaf nodes cover, or represent the basic primitives (e.g., triangles). Among others, the typical volumes used in BVHs are axis-aligned bounding boxes (AABBs) [51], oriented bounding boxes (OBBs) [52], spheres [53], sphere-swept volumes (SSVs) [45], and discrete orientation polytopes (k-DOPs) [54]. Whatever BVH model is picked for representing the bodies that need to be checked for collision, the procedure takes two phases. The first one is the construction phase, where BVHs are built, and the second one is the query phase where constructed BVHs are checked for collision. Due to tree-like structure of BVHs, the complexity of O (N log N) can be achieved for construction phase [52], [55], where N is the number of triangles. The query phase is typically of similar expected complexity. However, in the worst case, it can be O(N 2 ) when each pair of primitives need to be checked for collision [53].
As pointed out in [56], the true cost of collision query is to a large extent scenario-specific. The input size, shape and relative proximity play a crucial role in the expected runtime. For this purpose, we have conducted a numerical study in order to select the most convenient approach to checking collision between danger zones and generic obstacles. A number (1000) of random scenarios is generated. Each scenario assumes a random danger zone (with random position/orientation) and a random convex obstacle in the relative vicinity of the danger zone. The proximity between objects is chosen such that the collision occurs in cca 50% of the time. The collision test is performed using the following different representations: convex approximation, AABB-tree, Fig. 6. Triangular mesh representation: danger zones for different velocity profiles of the link, obtained using the same sampling scheme. Graphs that correspond to the association of edges and vertices are isomorphic.  [46] is used. In case of convex approximation of the danger zone, GJK algorithm available in FCL is used. A fixed sampling scheme for constructing the danger zone is assumed using 435 vertices. A randomly generated convex obstacle has cca 100 vertices. Table II shows average runtimes for collision checks when different types of BVHs are used. The results indicate that OBB-and spheretree-based representations imply the fastest collision detection on average. On the other hand, if the convex approximation of danger zone is used, combined with GJK-based distance computation, we obtain the fastest query.

D. Adaptive Braking Time
The analysis so far, along with the methods from [25], [26], [30] considered a constant braking time T b . For obvious reasons, the specific value of T b is estimated assuming worst case scenarios involving maximum robot speeds under configurations with the largest inertia. However, such an assumption may be too conservative in general, since it is not likely that the nominal trajectory of the robot, designed for typical tasks, is such that its state is near its domain's boundary all the time. Consequently, the braking time T b , now observed as a function of the robot's state, is in general smaller than the previously used worst case estimate. The problem of estimating the stopping time based on the current state has been addressed in the literature. For instance, in [28], [29], the stopping time is computed within an optimization procedure that accounts for the robot's current state, its dynamical model and the actuation torque bounds. A kinematic approach from [32] estimates the stopping time based on the current velocity of the robot and the specified joint acceleration and jerk bounds.
In this work, any reasonable reduction of the braking time (compared to the fixed, worst-case estimate) implies the reduction of danger-zone volumes, which further makes room for the increase in robot's speed and hence its productivity. In this regard, we inclined to the kinematic approach for assessing the braking time using the following intuitive formula: whereq i andq i,max are respectively actual joint velocity and joint acceleration bound, of i -th joint. Such formulation clearly requires the knowledge of joint acceleration bounds. Ideally, these are provided in the specifications by manufacturer. Otherwise, they need to be estimated, or determined experimentally. If a limited knowledge on robot's dynamics is available, the acceleration bounds can be induced from the torque bounds using the method from [57]. Alternatively, any informed, even conservative, estimate of acceleration limits may be beneficial in this regard.

E. Trajectory Scaling
The volume of danger zone DZ clearly depends on the robot state, i.e., on configuration q and the joint velocityq. The idea is to first compute the danger zones for all the links that correspond to nominal velocityq n . Once the boundaries of danger zones have been computed, we check if any of them intersect with the human/obstacle. If there is no intersection, the robot continues to move according to the nominal trajectory. Otherwise, to prevent the collision between the human and danger zones, we have to tune the robot velocityq = δq n , by computing a proper value of δ ∈ [0, 1]. The optimal value of δ implies that the geometric shape which represents the human (possibly including additional safety margin) touches at least one danger zone, but does not penetrate any of them. We seek for such δ by using bisection approach, which is given via Algorithm 2. For the sake of simplicity, the danger zone DZ(q,q) denoted in the pseudocode, stands for the union of danger zones with respect to all the robot's links.
Theoretically, it may happen that an abrupt decrease in the scaling factor δ may cause that the consequently desired deceleration cannot be achieved by joint actuators. More precisely, there may be a time instant when the robot will not slow down at the prescribed rate. However, we argue that this potential anomaly has no effect on safety requirements under the initially posed constraint that the robot has to maintain the ability to stop (in time T b ) before colliding with the obstacle.
In the worst case scenario, when the scaling factor δ suddenly changes from 1 to 0 (the robot has to promptly go from the nominal to a zero speed, i.e., a stop command is issued), the robot will clearly not be able to instantly decelerate. However, we still have the guarantee that the robot Algorithm 2 δ-SEARCH (Bisection) 1: δ min ← 0; 2: δ max ← 1; 3: δ c ← 1; 4: DZ ← ConstructDZ(q, δ cqn , N 1 , N 2 , N 3 , CONVEX) 5: if Collision(Obstacles, DZ) then 6: repeat 7: δ c ← 1 2 (δ min + δ max ); 8: DZ ← ConstructDZ(q, δ cqn , N 1 , N 2 , N 3 , CONVEX) 9: if Collision(Obstacles, DZ) then 10: δ max ← δ c ; 11: else 12: δ min ← δ c ; 13: until δ max − δ min < Threshold 14: δ ← δ c ; 15: return δ would stop within the next T b seconds before a collision possibly happens. This is implied by the definition of stopping time, which is by default the result of the worst-case analysis where the motions of links with large inertias at full speed are considered. For an alternative setting, where the adaptive T b is considered, we may apply the similar remarks. In any case, we emphasize that, within the experimental validation, we have not observed any violations of safety constraints related to the above-mentioned consideration.

F. Accommodating for Thick Robot Links
So far, the robot's links have been observed as thin line segments connecting adjacent joints. Clearly, such assumption is too simplistic for many HRC scenarios. To this end, we generalize the proposed approach to account for links with non-zero volumes. Accordingly, we emulate the approach from [38], by introducing the "clearance" parameter , which acts as a buffer zone surrounding an idealized line-segment representation of the link. Thus, the link becomes a capsule, i.e., a Minkowski sum of a line segment and a sphere with the radius . In other words, the capsule shape is used to approximate (as a superset bounding object) an arbitrary link geometry. Thus, a possibly complicated link geometry does not have to be explicitly represented via triangular mesh. Moreover, it is desirable that each capsule has the minimum possible radius, while still enclosing its corresponding link. The radii of such capsules can easily be acquired using typically available CAD models of the robot's links. Note that these capsules are never actually explicitly computed, nor have to be tessellated into triangles. Only their respective radii are necessary (see Fig. 7). The corresponding safety constraint is then given by: Despite the best of our efforts, the above constraint turned out to be difficult to use for the development of an explicit geometric representation of the danger zone. However, in a similar fashion as in [38], a sufficient condition can be imposed in the following form: where β is a suitably chosen constant. Picking a larger value for β implies that the above constraint is less conservative. It is easy to verify that the following value of β satisfies the rightmost inequality in (16): where 1] r os is the minimum distance between the line segment representation of the link and the human/obstacle. Plugging β = β * in the leftmost inequality of (16) the safety constraint becomes: Clearly, if (18) is satisfied, then (15) holds as well. On the other hand, the constraint (18) preserves the structure of the original condition (1). Thus, exactly the same procedure for computing the geometrical representation of the danger zone can be conducted. The difference is that the clearance parameter is "absorbed" by the new, apparent braking time It is worth pointing out that the clearance parameter can simultaneously take into account both link thickness and the additional distance margin around the human/obstacle. Such margin can be interpreted as a threshold distance between the danger zone and a human, which must not be violated.

G. Human Motion
The formulation of the proposed method does not explicitly include the motion of human operator. As such, it may lead to an impression that the approach is not capable of dealing with the moving obstacles. However, we underline that, by using some of the existing methods for incorporating the human velocity (perceived or predicted) for the generation of properly augmented human geometrical models, our method remains 100% applicable as formulated. In any case, we emphasize that, though it represents a crucial part in establishing safe human-robot collaboration, the consideration of human motion is not the focus of the paper.
In this work, we opted for improving the productivity by assuming less conservative safety requirements from the robot's side. The proposed algorithm is completely agnostic for what concerns how a human obstacle is represented and whether this representation accounts for the perceived/predicted motion of the human or not. The simplest approach is to compute the geometrical representation of the currently sensed human position, without considering their velocity. On the other hand, there are powerful methods to generate geometrical models of humans that account for the perceived or predicted motion (e.g., [38]). These models are usually augmented representations of those that are obtained simply by considering the current human's pose. In any case, the resulting model is typically represented via triangular mesh, geometric primitives, or point cloud(s), all being legitimate inputs to collision checking routines used by our approach.
In the process of experimental validation, we have assumed that the motion of human obstacle is limited by the maximal walking speed of v H = 1.6 m s , as suggested in [23] when walking speed is not measured. This velocity bound translates into an additional safety margin within the clearance parameter in order to account for the human motion. This clearly may not be sufficient to cover pathological scenarios with rapid hand/arm motion where certain parts can exceed the assumed velocity bound. In this case, our method cannot guarantee the integrity of safety constraints, but with such assumptions, hardly can any. There is an obvious trade-off between the intention to increase productivity and preserve safety under a wide variety of working conditions. Theoretically, one might assume quite liberal bounds on the human velocity to account for potentially extremely fast movements. This would call for large clearance margins that would substantially slow the robot down.

IV. SIMULATION STUDY
This Section provides a simulation study, which aims at validating the proposed approach and comparing it to existing relevant methods. We compare the approach to the previous work of authors [25], [26] together with the recent works from other research groups [24], [28], [29]. The main objective of the used simulation framework is to enable fully reproducible settings for the fair comparison of considered methods. The study consists of three parts. The first part addresses path following in the environment filled with random obstacles. The second part examines the volumes of danger zones (regions to avoid, or dynamic safety zones) associated with relevant methods. The third part compares respective methods in a simulated human-robot collaboration scenario. The complete study is conducted in MATLAB and Robotics Toolbox [58].

A. Part I: Static Random Obstacles
The first part of the study considers a wire model (a simplified representation via thin segments connecting local Denavit-Hartenberg frames) of Comau SmartSix 6 DOF industrial manipulator, which is exposed to 1000 different simulation runs with randomly generated circumstances. The following are the circumstances that are artificially created for each run: 1) There are N static point-obstacles in the environment, where N is a uniform random integer between Fig. 8. Mapping from d sep to δ from [24]. The middle part is a cubic polynomial that satisfies the continuity of the mapping and its first derivative. 5 and 200. The obstacles are placed with respect to uniform random distribution within the axis-aligned box that defines the robot's workspace. 2) Initial and final configurations q s and q g are chosen at random, with the condition that ρ q s , q g > ρ 0 , ρ being a C-space metric from [59], and ρ 0 a proper threshold.
3) The nominal path traversal time T sim is picked at random (uniformly) between 1s and 5s. 4) The nominal trajectory assumes a quintic polynomial profile with zero boundary velocities and accelerations.
The resulting path has to be collision-free. 5) The braking time is T b = 0.2823s. We are specifically interested in the following. Given a viable nominal trajectory in an obstacle-filled workspace, how fast the robot will traverse the path when its velocity is scaled according to different methods? In this part of the study, we compared the proposed method with the approach from [25], [26], and with the one from [24]. The method from [28], [29] is omitted from this part of the study. Reason for this is that the method does not continuously scale the robot's velocity during the path traversal. It actually executes the nominal trajectory if possible, while maintaining the dynamic safety zones that are computed based on estimated stopping time. If the safety constraint is violated (dynamic safety zone intersects the obstacle), a stopping command is issued, without adjusting the velocity in real-time to prevent robot from stopping if not necessary. However, the method is more relevant for the second part of study where the volumes of dynamic safety zones are compared to volumes of danger zones.
It is worth pointing out that the proposed method is completely equivalent to that from [30], in terms of the resulting scaling factor δ. The main difference is that the proposed method uses geometrical solution based on danger zones instead of the analytical one from [30]. Beside the original approach with constant braking time T b , we examine the option where T b is adjusted on-line by (14). For this purpose, we use a conservative estimate of the acceleration capabilities of the robot by settingq max = [6.5 8.5 10 15 20 20] rad s 2 . In [24], the scaling factor δ is computed as a function of separation distance d sep between the robot and the obstacle. The mapping δ = δ(d sep , S, ν) is depicted in Fig. 8. The minimum protective separation distance S is computed in real-time using a modification of the one from [23]: (19) where v H is the maximum speed of the closest operator; v R the maximum robot speed; T R the time required by the machine to Authorized licensed use limited to the terms of the applicable license agreement with IEEE. Restrictions apply. Fig. 9. Distributions of path traversal times for different methods captured by violin/box plots. Subscript Z stands for the method from [25], [26], C for the one from [24], while pa and p stand for the proposed methods (with and without braking time adaptation, respectively).
respond to the operator presence; T S the response time of the machine that brings the robot to a safe, controlled stop; C the intrusion distance safety margin, i.e., an additional distance, based on the expected intrusion toward the critical zone prior to the actuation of the protective equipment; Z R the robot position uncertainty; Z S the operator position uncertainty, and B is the Euclidean distance traveled by the robot while braking. The function α(t) is the output of a fuzzy inference system that performs a suitable risk assessment.
To simplify the implementation of this scaling method (and hence make it faster), we have assumed that all the terms on the right-hand-side of (19) were zero, except for the variable B -the Euclidean distance traveled by the robot while braking. To relax the circumstances for the method even more, we conservatively estimated the distance B for each link as where v i,max is the maximum velocity of the i -th link (which happens to be the velocity of one of its endpoints). Moreover, a separation distance d sep,i is determined for each link. Then, a tentative scaling factor is computed for each link as δ i = δ(d sep,i , B i , ν), using the mapping from Fig. 8, where the value ν = 2 is assumed (as in the validation part of [24]). Finally, the velocity scaling factor δ is chosen as δ = min i δ i .
We denote the path traversal times T p (k), k = 1, 2, . . . , 1000 for the proposed method based on fixed braking time T b . For the modified approach with adaptive T b , the traversal times are T pa (k). Similarly, the times that correspond to methods from Zanchettin et al. [25], [26] and Costanzo et al. [24] are denoted respectively by T Z (k) and T C (k). Fig. 9 shows distributions of logged traversal times in the form of violin/box plots. Fig. 10 depicts distributions of ratios of traversal times collected from 1000 simulation runs. Universally, the proposed approach implies faster path following when compared to the competing algorithms. On average, the proposed method is more than twice as fast than the one from [25], [26], and ca. 45% faster than the one from [24]. Note that, by construction, the method is still consistent with safety requirements. Moreover, the modified approach with adaptive braking time T b introduced the additional speedup of ca. 10% on average. Fig. 10. Ratios of path traversal times for different methods captured by violin/box plots. Plots are shown separately due to scale differences. Subscript Z stands for the method from [25], [26], C for the one from [24], while pa and p stand for the proposed methods (with and without braking time adaptation, respectively).

B. Part II: Danger Zone Volumes
In this part of the study, the proposed method is compared to the one from Zanchettin et al. [25], [26] and that from Scalera et al. [28], [29]. The objective is to compare the volumes of the danger zones proposed in this work to volumes of their respective analogues: regions to avoid from [25], [26], and dynamic safety zones from [28], [29]. For a simpler nomenclature, we will refer to all of these regions as danger zones. The method from [24] is not included in this study since it does not explicitly address the volumes of space where the safety constraint is violated. 2 We denote the danger zones of the respective methods by DZ p , DZ pa , DZ Z and DZ S . Indices are inherited from the notation used in the first part of the study, with the exception of DZ S denoting the danger zone from Scalera et al. [28], [29]. Fig. 11 depicts typical shapes of danger zones associated to a specific robot's state. Note that the method from [28], [29] requires full dynamical model of the robot. For this purpose, we used a dynamic model of Comau SmartSix robot, which is unnecessary for the approach proposed in this paper. Moreover, to simplify the implementation (and hence make it less conservative), reaction time, obstacle velocity, and potential uncertainties are neglected. Thus, the resulting danger zone reflects only the motion of robot's links (see [28], [29] for details).
Let μ p , μ pa , μ Z and μ S be the volumes of respective danger zones DZ p , DZ pa , DZ Z and DZ S . Here, we refer to the volume of the union of danger zones associated to all the links. Due to their inherent rotational symmetry, the volumes μ p , μ pa and μ S can be obtained analytically. For DZ Z , the volume μ Z is estimated using quasi-Monte Carlo numerical integration [60] that uses a low-discrepancy Hammersley sequence (with 10 5 points in the robot's workspace) [61]. The danger zone volume is estimated as the ratio of number of points that belong to a danger zone versus total number of points. To compare the relevant methods, a 1000 feasible random robot states (q,q) have been generated, for Fig. 11. Danger zones w.r.t. different methods around a 6 DOF manipulator. Far left -DZ p (transparent red), the linear velocities of link endpoints indicated by arrows. Left -DZ p (solid red) and DZ Z (transparent green, [25], [26]). Right -DZ p (solid red) and DZ S (transparent blue, [28], [29]). Far right -DZ p (transparent red) and DZ pa (solid purple). Fig. 12. Distributions of danger zone volumes for different methods captured by violin/box plots. Separated plots are due to scale differences. Subscript Z stands for the method from [25], [26], S for the one from [28], [29], while pa and p stand for the proposed methods (with and without braking time adaptation, respectively). Fig. 13. Ratios of danger zone volumes for different methods captured by violin/box plots. Separated plots are due to scale differences. Subscript Z stands for the method from [25], [26], S for the one from [28], [29], while pa and p stand for the proposed methods (with and without braking time adaptation, respectively).
which the volumes of corresponding danger zones are computed/estimated. Fig. 12 shows distributions of obtained volumes in the form of violin/box plots. Fig. 13 depicts distributions of ratios of traversal times collected from 1000 simulation runs.
Danger zones corresponding to proposed method have substantially smaller volume when compared to other approaches. On average, μ p is ca. 3 times smaller than μ Z , and an order of magnitude smaller than μ S . Furthermore, if the method with adaptive braking time T b is used, the danger zone volume is additionally reduced by more than 20% on average.

C. Part III: Human-Robot Scenario
In this part of the study, we validate the proposed method in a simulation that closely matches the HRC scenario. For this purpose, a human-shaped moving obstacle with a surface specified by triangles (ca. 350) is simulated. The human motion is simulated with the help of the Carnegie Mellon Motion Capture Database [62] and the MATLAB Motion Capture Toolbox [63]. The original motion from [62] considers human skeleton model, which we generalize by associating volume to the human figure and expressing it with triangular meshes. The setup is depicted in Fig. 14. The end-effector follows a predefined path, while the human conducts a series of manual tasks within the manipulator's reach. The simulation is performed for two different parameterizations of the same geometric path and for the same trajectory scaling methods used in the first part of the simulation study. For simplicity, and due to the fact that it does not introduce exceptional performance improvement, we omit the method based on adaptive braking time T b from this part of the study. Resulting time profiles for the scaling factor δ are shown in Fig. 15. The signal δ p denotes the scaling factor resulting from the method proposed in this paper. Similarly, signals δ Z and δ C correspond respectively to methods from Zanchettin et al. [25], [26] and Costanzo et al. [24]. Relative to existing approaches, the new method clearly allows for substantially faster path following. This is fully consistent with the results from the first part od the simulation study. Furthermore, we may note that, as the nominal trajectory becomes slower, the relative difference between traversal times decreases.
Finally, a curious comparison is presented in Fig 16. It juxtaposes the history of scaling factor δ p from the proposed method to "would-be" values of scaling factors δ Z and δ C , which are computed in the background, but not actually used to scale the trajectory. For most of the time, computed values of δ Z and δ C are smaller than δ p . This reveals the advantage of proposed method from another point of view.   15. Scaling factor δ time profiles for two different nominal trajectories along the same geometric path. The first nominal trajectory lasts 1s (top), the second one lasts 2s (bottom). Subscript Z stands for the method from [25], [26], C for the one from [24], while p stands for the proposed methods. Fig. 16. History of the scaling factor δ p compared to "would-be" profiles of factors δ Z and δ C . The first nominal trajectory lasts 1s (top), the second one lasts 2s (bottom). Subscript Z stands for the method from [25], [26], C for the one from [24], while p stands for the proposed methods.

A. Experimental Setup and Implementation Details
The proposed approach has been tested using a Comau SmartSix robot. The robot is controlled by an open version of the industrial Comau C4G controller, that allows for a fast external interface with sampling time of t = 2ms. The algorithm is implemented in C language on an external real-time Linux PC that is connected to the C4Gopen controller via real-time Ethernet. Each 2ms, the PC sends to the controller the control commands. This information is actually the increment of joint variables that the robot has to cover in 2ms to reach the desired position. In particular, the scaling of joint velocity is implemented using the notion of scaled time τ . The evolution of the scaled time, and corresponding desired joint configuration are governed by the following equations: where q n (t) denotes the prescribed nominal trajectory. Analogous approach to scaling can also be found in [12], [24], [26].
The robot can send back to the PC different types of information, among them: joint variables and homogeneous transformation matrix of the end-effector frame.
In order to detect the obstacle, i.e., the human operator, a range camera (Microsoft Kinect V2) has been selected. The camera is connected to a Mini PC Intel NUC that communicates with the Linux PC via Ethernet. Thus, the information from the camera is sent to the Linux PC and used by the algorithm. The sampling frequency of the perception system is 30Hz. Fig. 17 shows the overall system architecture.
The original information retrieved from Kinect has the format of a point-cloud. There are several possible ways to induce a useful volumetric representation of the human from pointcloud-based inputs. One option is to first extract the skeleton model and later use geometric primitives (e.g., spheres, cylinders, capsules, or other shapes) to compute a segmented representation of human body (head, torso, upper arms, forearms, etc.). Each relevant body part can be then represented by a triangular mesh, or a separate point-cloud. Any of these structures can be later used for collision detection with danger zones. Another option is to process the point cloud without segmentation and obtain the unique triangular mesh. The simplest option is to represent the human as a single blob (e.g., a capsule or a cylinder). In principle, our method does not assume anything specific in terms of geometric representation of humans/obstacles. In the description of approach, we tacitly assume the most general representation, i.e., a triangular mesh that could capture a wide variety of surfaces.
Nevertheless, in order to facilitate the implementation, the human operator is represented by a single capsule. The Microsoft Kinect detects the position of the operator's shoulders. The axis of the capsule passes through the midpoint of the segment connecting two shoulders. The radius  of the capsule is computed as the distance between one of the shoulders and the midpoint. The capsule is further augmented by the additional margin -a worst-case distance that the obstacle can cover in the next time interval assuming the usually adopted maximal walking speed v H = 1.6 m s [20], [23]. For collision checking between the human and danger zones, GJK algorithm [47] is used, implemented via openGJK library [49]. GJK algorithm assumes convex geometries of the shapes tested for intersection. Therefore, we used a convex approximation of danger zones (see Fig. 5). Each danger zone is sampled by ca. 200 points, resulting in ca. 400 triangles.
A question arises how large is the approximation error caused by this assumption. To this end, we have performed a numerical study where we compared the volumes μ p of the original danger zones to volumes μ conv obtained via convex representation. We subjected the robot to 10 5 random states and computed the respective danger zone volumes for each link separately. The mean relative approximation errors σ = μ conv −μ p μ p · 100% are reported in Table III. The results indicate that the error implied by approximating the danger zone with its convex hull is nearly marginal. Clearly, if the original danger zones shapes (which are non-convex in general) are used for validation, algorithms other than GJK have to be used. In particular, based on results from Table II, it is advised to use BVH-based representation via OBBs [52], or spheres [53], both available e.g., in [46].
Within experimental study, the robot executes a palletizing operation following the "bridge" path according to typical trapezoidal velocity profile. The duration of the nominal trajectory is 5s, with an acceleration and deceleration phase of 1s each, while the cruise speed is set to 658mms −1 . When the human is not present in the scene, the robot fulfills its task, i.e., follows the path at the nominal programmed speed. As soon as the operator enters the workspace, he/she is tracked by the depth camera and the speed of the robot is adjusted accordingly. The experimental setup is reported in Fig. 18.

B. Results
The experimental study covers three distinct scenarios: • experiment 1: the human operator is far away from the robot, which can follow the path at the programmed, nominal speedq =q n , i.e., δ = 1; • experiment 2: the human operator is closer to the robot, which has to scale down the velocity in order to satisfy the safety constraint. However, the robot is not forced to stop; • experiment 3: the human operator gets excessively close to the robot, which forces it to stop and maintain zero speed until the human moves far enough. In particular, we are interested in the profiles of the following quantities: the scaling factor δ, the the consequent speed of the path traversal, and the distance d(DZ, human) between the danger zone DZ and the human.
From Fig. 19, reporting the results of the first experiment, one can see that the minimum distance d(DZ, human) computed while the manipulator moves at nominal speed, is always greater than the threshold (which is set to zero). Therefore, there is no need to scale down the velocity. Hence, the scaling factor δ is always equal to 1 and the robot follows nominal velocity trajectory. Fig. 20 shows the relevant signals related to the second experiment. Note that, at a certain time instant (t ≈ 2s), the distance d(DZ, human) reaches the threshold, meaning that the human model starts colliding with the danger zone DZ. As soon as this occurs, the scaling factor δ assumes values less than 1. In other words, the robot cannot follow the path with the nominal programmed speed without violating the safety Fig. 19. Velocity, minimum distances and scaling factor δ comparison during experiment 1.  constraints. However, the scaling factor δ is optimal in the sense that the resulting speed is maximum possible, while still keeping the safety constraints satisfied, i.e., maintaining the contact between the human and the danger zone without the penetration. While δ < 1, the distance d(DZ, human) takes zero value (until t ≈ 3.2s). Once the circumstances allow, the value of δ returns to 1 and the robot resumes to follow the path with the nominal speed.
The third experiment (Fig. 21) is conceptually similar to the second one -as soon as the distance d(DZ, human) reaches the critical value, the scaling factor δ becomes smaller than 1 and the nominal velocity profile cannot be sustained. However, due to extreme human-robot proximity, there is no non-zero velocity that can satisfy the safety constraint. Therefore, both the scaling factor δ and the velocity approach zero. When the human-robot distance starts increasing, due to human operator moving away, the scaling factor δ increases as well, until it returns to 1. Finally, the robot may resume the nominal velocity profile.

VI. CONCLUSION
This study presented an approach to ensure safe human-robot collaboration using a geometric method that explicitly represents the danger zones around the robot. Danger zones are regions of space where safety constraints are violated. The constraints have been formulated in accordance to speed and separation monitoring paradigm. A simple way to use danger zones for scaling the trajectory is presented. The overall approach is validated within an experimental study involving an industrial manipulator and a depth-camera-based perception system. Future work will explore the possibilities to further enhance the robot's productivity without jeopardizing safety constraints by considering more detailed knowledge on robot's inertial parameters.