The introduction and diffusion of collaborative robotics within the industrial environments has allowed to create shared workspace where humans and robots can work closely. While this new paradigm has led to an increase in the flexibility of production lines, the lack of physical barriers requires to pay more attention on how to guarantee human safety. Therefore, the robot safety standards have been updated to address this new collaborative scenarios . In particular, the ISO 10218-1 and the ISO 10218-2 [8, 9]
standards classify the collaborative modes in four different categories:safety-rated monitored stop (SMS), hand guiding (HG), speed and separation monitoring (SSM) and power and force limiting (PFL). Additionally, the technical specification ISO/TS 15066  provides further information to assess the risk for each collaboration mode. In case of applications where industrial robots are used, the SSM is typically adopted. In this collaborative mode the speed of the robot is reduced according to the relative human-robot velocity and position. However, this approach is overly conservative, since the robot speed should not be limited if its motion is directed away from the human. Moreover, by monitoring the human speed the performance of the robot can be further increased without violating the safety constraints.
Different approaches were presented in the literature to deal with human safety and collision avoidance in a human-robot collaboration (HRC) scenario. In  the authors propose a real-time solution to evaluate the future human occupancy and scale the robot speed accordingly, ensuring safety. The idea is to use a 3D camera and a simple human kinematic model to predict the future human occupancy. In  an optimization which treats safety as an hard constraint to be satisfied is presented. This strategy leads to obtain a proportional reduction of the speed, with a consequent higher productivity, while ensuring safety. In  the authors present a safety framework for collaborative tasks where multiple robots have to share the workspace with human operators. The idea is to scale the velocity preventing that a safety index falls below a certain value. When the scaling procedure is not enough, an emergency stop is applied.
Reducing the speed of the robot is not always the best solution, especially when the workspace conditions allow the robot to modify the pre-planned path. In  the authors exploit the concept of static and kinetostatic danger field on a mobile robot in order to prevent collision with human operators in a tire workshop. In  the concept of potential field around the whole robot body is used to generate collision-free trajectory. The entire workspace is surrounded by multiple depth sensors that track both dynamic and static object. In  authors implement virtual fixtures, which combine attractive and repulsive potential field, in a teleoperated environment. Even if these methods are effective in guaranteeing safety requirements, potential fields can easily cause the system to be stuck in local minima, compromising the task execution.
For this reason, optimization-based algorithms have been exploited to achieve a collision free behavior by applying the minimum correction to the desired path. Safety is embedded through the constraints in the optimization problem. In  an optimization problem is solved in real-time in order to force the robot to stay inside a safe set, evaluating the variation of a safety index. In , the authors propose an optimization-based control algorithm that explicitly considers safety in order to avoid the human operator while trying to preserve the desired path. Their strategy exploits the use of control barrier functions  around the robot body to maintain a collision-free trajectory while fulfilling the ISO/TS 15066.
Adopting the optimal behavior to avoid collision in highly dynamic environments could be computationally challenging, especially in a real industrial scenario where the number of obstacles to be considered is very high. In [15, 14] the authors use kinodynamic rapidly-exploring random tree (RRT) to plan collision free trajectory under kinodynamic constraints. However, these solutions are only suitable for constraints that do not change during the execution of the path, while the safety kinodynamic constraints change in real time based on human behavior.
Other solutions, like , propose to ensure safety by making the robot behave like a passively compliant system during the execution of a task. In spite of that, these approaches treat the human operator as an external disturb for the system, without exploiting a human tracking strategy.
In this paper we propose a novel framework for trajectory planning and velocity scaling for HRC scenario that is aware of the highly dynamic of the environment and ensures safety for the human operator by explicitly considering safety regulations. The proposed framework is composed by two layers. Given a desired configuration to reach, a trajectory planner layer computes and adapts online the trajectory that the robot has to follow. The trajectory scaling layer, according to the safety constraints imposed by the safety standards, scales the robot velocity ensuring safety for the human operator. Moreover, in order to avoid drastic drops of the robot velocity with consequent poorly efficient robot behaviors, mutual communication between the two layers is enabled. When required, the trajectory scaling can request for a replan of a new trajectory, increasing the robot performances.
The main contributions of this paper are:
A novel adaptive framework for trajectory planning and scaling that takes into account the high dynamicity of the environment, adapting in real-time the trajectory.
A strategy for trajectory scaling that is computationally cheap, i.e. suitable for real industrial application, and that explicitly considers the kindoynamic safety constraint.
The overall architecture that integrates the trajectory planning and scaling strategies in order to improve the efficiency of the system.
The paper is organized as follows: in Sec. II the trajectory planning and scaling problem is detailed while in Sec. III the SSM collaborative mode is treated. In Sec. IV the overall architecture is presented: in Sec. IV-A the trajectory planning strategy is detailed, while Sec IV-B the trajectory scaling problem considering safety constraints is presented. Finally in Sec. V an experimental validation of the proposed architecture is presented while in Sec. VI some conclusions and future works are addressed.
Ii Problem Statement
We consider a HRC application where a robot manipulator with joints has to move from an initial configuration to a desired final configuration in order to execute a task. The trajectory that the robot has to perform can be decomposed with a path-velocity decomposition:
where is the curvilinear abscissa that parametrizes the geometrical path . The variation of represents the time law of the desired path (i.e. the velocity profile).
Differentiating (1) we obtain:
is the vector tangent to the desired path, whileconstitutes the magnitude of the joint velocity.
The trajectory is considerate feasible and collision-free when:
where is the line segment representing the -th link when the robot is in configuration . it the line segment of the -th human body link, e.g. the human arm, and is the number of the human body link. represents the distance between the two line segments and
is the minimum admissible distance. For this reason, the shared workspace is equipped with a monitoring system that allows to track the human movements and estimate the human speed. Several strategies to track the human body are available in literature: skeleton tracking with multiple cameras, placing markers on the human body 
, machine learning techniques, to name a few.
In this work, we aim at designing a safety kinodynamic architecture that:
Computes a nominal trajectory that is always collision-free, i.e. a trajectory that the robot can execute at maximum speed. Exploiting the tracking of the human movements, the planning strategy aims at preserving the feasibility of the trajectory, replanning a new trajectory when the actual trajectory becomes infeasible.
Starting from the nominal trajectory, scales the robot velocity according to the limits imposed by the ISO/TS 15066 standard. The scaling aims at maintaining safety for the human operator taking into account both the distance between human and robot and the velocity of the human towards the robot.
Iii Speed and Separation Monitoring
In modern industrial applications of collaborative robotics, the Speed and Separation Monitoring collaboration mode is widely used. In this collaborative mode, the speed of the robot is continuously adapted depending on the position and velocity of the human operator into the collaborative workspace. Typically the human velocity is not monitored and the workspace is divided into three different areas based on the distance between the human and the robot. This scenario is represented in Fig. 1. The robot is allowed to operate at full speed when the human is in the green area, at reduced speed when the human is in the yellow area and it stops when the human is in the red area.
The ISO/TS 15066 provides the guidelines for calculating the sizes of these areas, namely the minimum protective separation distance , considering also the relative speed between the robot and the human operator. can be computed as:
is the protective separation distance at time , while is the current time. represents the contribution to the protective separation distance due to the operator’s movements, is the one derived from the robot reaction time and is the contribution caused by the robot stopping time. represents the intrusion distance, i.e. the distance that a part of the body can intrude into the sensing field before it is detected. and are the position uncertainties of the human operator inside the workspace and of the robot system respectively.
The first terms of (4) can be expressed as:
where and represents the robot stopping time and the robot reaction time respectively. is the directed speed of the human operator towards the robot, is the directed speed of the robot towards the human operator and is the speed of the robot in the course of stopping.
Under the assumptions that the velocity of the robot is constant during the robot reaction time, that the acceleration remains constant during the stopping phase and that the dynamics of the human operator is slower than the robot dynamics, which is true in the case of a generic HRC application, the equations (5) – (7) can be approximated as follow:
The equation (11) is the safety constraint imposed by the ISO/TS 15066, i.e. it expresses the maximum speed allowed to the robot in the direction of the human operator.
Iv Safety Kinodynamic Architecture
The proposed dynamic trajectory planning and scaling strategy can be represented by the architecture in Fig. 2, where two main layers can be distinguished:
The trajectory planning layer. It is responsible of generating the initial nominal trajectory that the robot can execute at maximum speed, i.e. it considers only the robot limits. Subsequently, it continuously adapts this trajectory exploiting the human tracking information.
The trajectory scaling layer. It is responsible of scaling the robot velocity along the planned path, explicitly taking into account the velocity limits imposed by the safety constraints.
Once the trajectory planning computes the initial nominal trajectory, it sends it to the trajectory scaling and it remains active until the robot reaches the desired final configuration . The trajectory planning layers does not take into account the safety regulation, i.e. it computes a trajectory that the robot could ideally execute at maximum speed.
The trajectory scaling firstly applies a path-velocity decomposition to the desired trajectory as shown in (1) and (2). Subsequently, it computes online the optimal scaled velocities in order to satisfy the constraint imposed by ISO/TS 15066 (11).
During the execution of the motion, mutual communication between the two layers is enabled. The trajectory planning exploits the human tracking information and replans a new trajectory when the previous one becomes infeasible, as explained in Sec. IV-A. The trajectory scaling immediately parameterizes the new trajectory and starts following the new path. At each iteration, it returns to the trajectory planning the actual state of the trajectory. Moreover, when the scaling factor decreases too much, the trajectory scaling sends a signal to the trajectory planning requesting for a new trajectory to be planned, as it is becoming inefficient, see Sec. IV-B.
It is worth noting that during the real-time execution the two algorithms work in parallel, relying on the last available data sent by the other algorithm. For an optimal behavior, the scaling algorithm should work at a frequency at most equal to that of the robot control.
Iv-a Trajectory Planning
The role of this layer is to find a trajectory for the robot that is collision-free and that the robot can execute at maximum speed. Since the human behavior is in general unpredictable it is not possible to use a strategy that computes offline an optimal trajectory, as in short time it could become infeasible causing collisions between the human and the robot. The trajectory planning aims at continuously maintaining a collision-free trajectory, adapting it online when required.
The trajectory planning is implemented according to the pseudo-code reported in Alg. 1.
The trajectory planning needs as input the initial and the final configuration, respectively and , and the length of the horizon trajectory that will be checked (Line 1). It immediately plans the maximum speed trajectory that the robot could perform (Line 2). The function can be implemented using different strategies available for robotic applications (see e.g. [16, 11, 24]). Subsequently it sends the trajectory to the trajectory scaling layer (Line 3) and it sets the current trajectory state equal to the initial configuration (Line 4). From this point the algorithm starts to loop until the entire trajectory has been executed (Line 5). In the loop, the dynamic planner first creates the horizon starting from the actual state (Line 6). This horizon represents the set of the future configuration that are analyzed to check if the trajectory is still feasible (Line 7 – 8). In case an infeasible configuration is found a new feasible trajectory is planned through the function (Line 9). The function is responsible of planning a new trajectory that goes from a desired configuration, in this case the last feasible one , to the final goal. Moreover, the function merges the new trajectory with the previous one and sends the resulting trajectory to the trajectory scaling. Subsequently, the dynamic planning algorithm reads if there is a request to replan a new trajectory due to the inefficiency of the current one, i.e. is equal to one (Line 13). This request is given by trajectory scaling layer, as described in Sec. IV-B. If there is the request, a new trajectory starting from the actual configuration is computed (Line 14). Lastly, the actual configuration is updated exploiting the information coming from the trajectory scaling in (15) (Line 16).
The replan algorithm is presented in Alg. 2.
The algorithm takes as input the actual planned trajectory , the starting configuration of the new trajectory and the final desired configuration (Line 1). It firstly plan a new trajectory that goes from the starting configuration of the new trajectory to the desired goal (Line 2). The new trajectory is then merged with the old one (Line 3). This merging procedure replaces the part of the old trajectory from to with the new trajectory. Lastly, the updated trajectory is sent to the trajectory scaling (Line 4) and returned to the dynamic planner (Line 5).
Iv-B Trajectory Scaling
Starting from the output of the dynamic planner, the goal of the trajectory scaling is to regulate the robot velocity without violating the safety constraint expressed in (11). When a human and a robot cooperate the environment could be highly dynamic, for this reason the robot must follow exactly the same path coming from the upper layer, since a deviation from the planned path could cause a collision. The trajectory scaling aims at scaling only the magnitude of the velocity , assuring that the executed path is collision-free.
is the optimization variable and represents the scaling factor. is a modified jacobian that takes into account only the scalar velocity towards the human operator of the -th link. This modified version of the jacobian is required as the velocity constraint imposed by the ISO/TS 15066 (11) limits only the velocity that reduce the human-robot distance, i.e. the velocity towards the human. is a vector whose each component is the velocity limit imposed by the ISO/TS 15066. and are the joint velocity lower bounds and the joint velocity upper bounds, respectively. While and are the acceleration limits. is the actual robot velocity and is the robot execution time.
The modified jacobian is expressed as:
where is the versor representing the direction that goes from the -th robot link to the human.The method used to compute this versor is a design parameter, e.g. it can be found representing both the robot and the human links as capsules and computing the minimum distance . is the -th jacobian, i.e. the jacobian matrix that relates the firsts joint velocity to the linear and angular velocity of the -th link, and is a matrix with all zero elements.
The optimization problem (12) is a convex problem and computationally cheap, since the only factor that affects the convergence is the problem dimension, i.e. the number of joints and links. Thanks to its convexity, the solution obtained by the solver is always the global minimum of the cost function, i.e. the maximum admissible scaling factor. Moreover the problem has always a feasible solution. When the human operator is very far from the robot, the robot is allowed to move at the desired speed, i.e. that is the maximum speed as seen in Sec. IV-A. When the human approaches the robot, the safety standards require to decrease the velocity until, in the worst case, stopping the robot. This is guaranteed by the solution .
The output of the trajectory scaling is then used to send the desired velocity to the robot:
However, greatly reduce the robot velocity is a very conservative strategy and it strictly decreases the overall efficiency. Sometimes it could be more convenient for the robot to move away from the human and execute another trajectory. For this reason it has been implemented a step signal that requests to the dynamic planner the replan of new trajectory:
where is a predefined threshold and represents the lower desired bound for the scaling factor.
The proposed two-layers framework has been experimentally validated on a Pilz PRBT, a 6-DoF manipulator for industrial application. We decided to exploit six OptiTrack Prime cameras with the OptiTrack Motive software  in order to track the movements of the human right arm. A complete setup of the experiments is shown in Fig. 3.
All the software components were developed using ROS Melodic Morenia meta-operating system and they ran on a Intel(R) Core(TM) i7-10510U with Ubuntu 18.04. The dynamic planner layer is based on the RRT-Connect algorithm  and it is implemented using MoveIt Motion Planning Framework . The trajectory scaling layer exploits the C code generated by CVXGEN  to solve the optimization problem (12). For simplicity, the modified jacobian is applied only to the end-effector and the versor is the versor of minimum distance between the robot link and the human operator arm. The minimum distance is computed representing both the robot links and the human arm as capsules (see ).
Concerning the frequencies, the communication with the robot works at while the optimization problem converges in . The OptiTrack, instead, works at a frequency of . Since the PRBT has not a real-time velocity ROS interface, it has been decided to position control the robot integrating the solution coming from (12) at .
In the experiment the robot has to go continually from the start configuration to the final configuration , and vice versa. A complete video of the demonstration is attached. Initially, the human operator is very far from the robot, i.e. he is in the green area of the SSM (see Fig. 1). In this phase the robot is allowed to move at maximum speed, following the nominal planned trajectory as shown in Fig. 4.
Subsequently, the human operator approaches the robot causing the scaling of the trajectory, as shown in Fig. (c)c. This is due to the fact that, according to the safety limit imposed by ISO/TS 15066 (11), the maximum speed allowed towards the human operator decreases. The Fig. (a)a and (b)b show the position and the velocity of the nearest human point in the robot reference frame, respectively. As a consequence of the approaching behavior, in the first phase the component increases and its velocity is positive. While during the scaling, the velocity components are very low. The Fig (d)d demonstrates that the safety constraint is not violated. In the graph only the velocity of the end-effector towards the human is shown. It is worth noting that the robot slows down only in the first part of the trajectory, i.e. from to . This is because the robot is going towards the human operator. In the second part, i.e. when it moves away, it goes at higher speed, restoring the nominal behavior. As a matter of fact, at the scaling factor increases. A comparison between the planned trajectory and the scaled one can be found in Fig 6.
In the next part of the experiment the human operator hinders the robot, making the trajectory infeasible. The dynamic planner layer takes care of planning a new one, avoiding the human operator, and the robot is able to reach the desired configuration.
In the last part of the experiment, the human operator goes very close to the robot, causing a drop of the scaling factor. When the trajectory scaling layer sends a the step signal to the dynamic planner requesting for a replan of a more efficient trajectory, as explained in Sec. IV. The evolution of the scaling factor and the signal is shown in Fig. 7.
The replanning strategy is successful and the robot is free to restore its behavior, completing the trajectory. Please note that the robot remains stopped for about . This is due to the fact that the dynamic planner requires a certain amount of time to find the more efficient trajectory.
In order to demonstrate the effectiveness of the architecture, the same experiment is performed without sending the replan request when the scaling factor is too small. In the last part of the video is shown that, when the human operator goes very close, the robot stops and it stays stuck until the human operator leaves. It is worth noting that a comparison on the execution times of the different strategies would not be very interesting. This is because the solution without the replan signal strictly depends on how long the human operator stays close to the robot.
In this paper we propose a two-layers framework for trajectory planning and velocity scaling. Taking into account the human motion, the first layer, i.e. the dynamic planner layer, continuously checks if the trajectory becomes infeasible and and reacts accordingly. The second layer, i.e. the trajectory scaling layer, explicitly considers the safety standards and scales the robot velocity in order to ensure safety. Moreover, when the scaling factor decreases too much, the trajectory scaling sends a signal to the dynamic planner requesting for a replanning of a new trajectory. The experimental evaluation shows the effectiveness of the framework both when the human operator hinders the robot and when the two agents get too close.
Future work aims to exploit the model predictive control approach to generate smoother speed profiles. Furthermore, considering a single direction from each robot link to the human operator can be quite unreliable from a safety point of view. For this reason a strategy that considers multiple directions could be implemented. This, will be tested in a real scenario with the presence of many obstacles, e.g. a cluttered environment. Finally, the work could be extended for use also in the case of mobile manipulators, exploiting the redundancy to better improve the safety.
-  (2016) Control barrier function based quadratic programs for safety critical systems. IEEE Transactions on Automatic Control 62 (8), pp. 3861–3876. Cited by: §I.
-  (2018) Collision-free motion planning for human-robot collaborative safety under cartesian constraint. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–7. Cited by: §I.
-  (2014) Reducing the barrier to entry of complex robotic software: a moveit! case study. arXiv preprint arXiv:1404.3785. Cited by: §V.
Human tracking using convolutional neural networks.
IEEE Transactions on Neural Networks21 (10), pp. 1610–1623. Cited by: §II.
-  (2020) A control barrier function approach for maximizing performance while fulfilling to iso/ts 15066 regulations. IEEE Robotics and Automation Letters 5 (4), pp. 5921–5928. Cited by: §I.
-  (2020) Safety barrier functions and multi-camera tracking for human–robot shared environment. Robotics and Autonomous Systems 124, pp. 103388. Cited by: §IV-B, §V.
-  (2015) Bilateral teleoperation of a dual arms surgical robot with passive virtual fixtures generation. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4223–4228. Cited by: §I.
-  (2011-07) Robots and Robotic Devices–Safety Requirements for Industrial Robots–Part 1: Robots. Standard Vol. 2011, International Organization for Standardization, Geneva, CH. Cited by: §I.
-  (2011-07) Robots and Robotic Devices–Safety Requirements for Industrial Robots–Part 2: Robot systems and integration. Standard Vol. 2011, International Organization for Standardization, Geneva, CH. Cited by: §I.
-  (2016-02) Robots and robotic devices–Collaborative robots. Technical Specification Vol. 2016, International Organization for Standardization, Geneva, CH. Cited by: §I.
-  (2004) A prm-based motion planner for dynamically changing environments. In 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566), Vol. 2, pp. 1606–1611. Cited by: §IV-A.
-  (2005) Teleoperation of a robot manipulator using a vision-based human-robot interface. IEEE transactions on industrial electronics 52 (5), pp. 1206–1219. Cited by: §II.
-  (2000) RRT-connect: an efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), Vol. 2, pp. 995–1001. Cited by: §V.
-  (2014) Probabilistically complete kinodynamic planning for robot manipulators with acceleration limits. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3713–3719. Cited by: §I.
-  (2001) Randomized kinodynamic planning. The international journal of robotics research 20 (5), pp. 378–400. Cited by: §I.
-  (1998) Rapidly-exploring random trees: a new tool for path planning. Cited by: §IV-A.
-  (2019) TIREBOT: a collaborative robot for the tire workshop. Robotics and Computer-Integrated Manufacturing 57, pp. 129–137. Cited by: §I.
-  (2017) Real-time collision avoidance algorithm on industrial manipulators. In 2017 IEEE Conference on Control Technology and Applications (CCTA), pp. 1294–1299. Cited by: §I.
-  (2020) Human multi-robot safe interaction: a trajectory scaling approach based on safety assessment. IEEE Transactions on Control Systems Technology. Cited by: §I.
-  (2012) CVXGEN: a code generator for embedded convex optimization. Optimization and Engineering 12 (1), pp. 1–27. Cited by: §V.
Multiple kinect sensor fusion for human skeleton tracking using kalman filtering. International Journal of Advanced Robotic Systems 13 (2), pp. 65. Cited by: §II.
-  (2020)(Website) External Links: Cited by: §V.
-  (2015) Safety-aware trajectory scaling for human-robot collaboration with prediction of human occupancy. In 2015 International Conference on Advanced Robotics (ICAR), pp. 85–90. Cited by: §I.
-  (2009) CHOMP: gradient optimization techniques for efficient motion planning. In 2009 IEEE International Conference on Robotics and Automation, pp. 489–494. Cited by: §IV-A.
-  (2014) Parallel active/passive force control of industrial robots with joint compliance. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4511–4516. Cited by: §I.
-  (2018) Survey on human–robot collaboration in industrial settings: safety, intuitive interfaces and applications. Mechatronics 55, pp. 248–266. Cited by: §I.
-  (2015) Safety in human-robot collaborative manufacturing environments: metrics and control. IEEE Transactions on Automation Science and Engineering 13 (2), pp. 882–893. Cited by: §I.