Online Collision Avoidance for Human-Robot Collaborative Interaction Concerning Safety and Efficient

With the development of robot technology and the arrival of industry 4.0 era, society pays more attention to collaboration and interaction between human and robots. However, safety is still main concern in the development of human-robot collaboration. In this paper, a novel real-time collision avoidance approach for mobile manipulator is proposed by considering the motion status of the human, which includes the relative minimum distance and velocity (both magnitude and direction) between the robot and the human. The distance and velocity of the human hand are first estimated online using a vision sensor, and then defined as danger factors in the potential function of the potential field. The novel potential function proposed in this paper considers not only the safety problem, but also the efficient problem, i.e., the manipulator can make smart control decision to avoid the collision according to the relative velocity in case of the cross over. To overcome the local minimum problem and choose a best motion direction, we propose a motion sampling mechanism for motion planning. For each sample, the robot calculates the potential function to evaluate the safety and efficiency, and chooses a direction which is best for avoidance. We finally demonstrate our idea on a real mobile manipulator platform in a simulated co-worker environment.


I. INTRODUCTION
In an uncertain and unstructured environment, whether robot can avoid human accurately and timely is always one of the important factors that restrict the development of human-robot collaboration and interaction. In order to ensure safety working in the human coexistence environment, many novel ideas for collision avoidance have been proposed in recent years, which can be divided into local algorithms and global algorithms according to whether the obstacle affects locally or globally. Global algorithms calculate the global path before the motion execution, while local algorithms determine the motion command according to the feedback during the motion execution Z. Ji is with the Robotics and Autonomous Systems Laboratory, School of Engineering, Cardiff University, Cardiff, CF10 3AT UK (jiz1@cardiff.ac.uk). [1]. In addition, global algorithms are usually complete or the solutions are complete, while local algorithms only optimize the local objective function and thus may lead to suboptimal behavior, e.g., traditional potential field methods can have local minima solution [2]. A global sampling based global method is presented in [3], which requires that the working environment of the manipulator is known and the obstacles have to be modeled, and then a growth graph through randomly sampled nodes can be further built. When the target point is added to the graph, a global path can be found by searching. The global methods can find a global optimal solution, but the computational cost is generally high [4]. In addition, the global methods have difficulties to quickly response to the obstacle appearing suddenly or with unpredictable trajectory. Compared with the global path planning algorithms, local algorithms can make real-time response according to the changes of the environment. Artificial potential field (APF) algorithm has the advantages of simple structure and real-time performance, such that it has been widely used in real-time obstacle avoidance algorithms of manipulator. APF builds a repulsion potential field around the obstacle, and a repulsion force is produced using a negative gradient direction of the potential field, which keeps the robot away from the obstacles [5]. For dynamic environment, the APF is extended in [6], which introduces a new potential field form taking not just the path but also the velocity along the path into account. However, due to the lack of global information, APF is easy to fall into the local minimum region where the repulsive force is equal to the attractive force, which results in the target unreachable.
In order to overcome the defect of local minima of APF, a virtual obstacle associated with a new target is proposed in [7], which can help the robot escape from the local minimum region. Sun et al. [8] proposed a dynamic window method to predict the position of the local minimum region, such that the robot can know where the local minimum region is in advance. Harmonic potential functions are introduced to overcome the shortcoming of potential field in [9], which requires that the robot motion and the obstacle motion have to follow harmonic functions. A depth space based method for estimating the minimum distance between manipulator and human is proposed in [10], which is then used with the traditional APF for avoidance.
In this paper, we consider the safety and efficiency problems in the scenario of human-robot coexistence working and living environment, which are solved using a sampling based motion generator with a novel potential function of potential field considering the motion status of the human. The contributions of our work can be summarized as the following: (1) the danger index of the robot is defined as a function of the relative distance and velocity between the robot and human, which is then used in a novel potential function of the artificial potential field for planning a collision free path. Furthermore, the efficiency of the robot is considered by analyzing the magnitude and direction of the relative velocity of the moving human, i.e., the robot can make a smart decision to move ahead or behind human, such that the efficiency can be improved for collaborative situations like cross over. (2) a sampling based motion generator is used to evaluate the quality of the manipulator motion, which is similar to the dynamic window approach [11]. For each sample, we evaluate not only the local minimum region, but also the danger index. In this way, we can avoid the local minimum points and find a solution that is safe and efficient. The motion status of the human hand is estimated by a visual localization technology using 2D marker proposed in [12]. There are also some other choices for human motion estimation, such as skeleton detection [13] or 3D octree map [14] which requires more computational cost and is more noisy. The mobile manipulator TIAGo is used for demonstrating our ideas as shown in Fig.1, and a cross over task between human and robot is used for evaluation.

II. COLLISION FREE MOTION GENERATOR
In this section, we present our framework in detail. We first introduce the standard artificial potential field (APF), and then show how to use a sampling mechanism to avoid the local minimum. Finally, we introduce new danger indexes as key factors of the potential function of the APF, which can be used in the sampling step for evaluating each sample to derive a safe and efficient path.

A. Artificial Potential Field
In the classical artificial potential field (APF), the manipulator moves in the force field. Both the goal and the obstacle create a potential field at the position X of robot. The target point has the attractive force to the robot and guides the robot to move toward it, whereas the obstacles create the repulsion to the robot and keep the robot away from it. The sum of all the repulsive and attractive forces at each position in the robot path controls the motion of robot. The classical APF is static since it only depends on the robot's current position and obstacle position. The common function of the attraction potential field is defined as: where U attr represents the attraction potential energy, X g is the position of target point, and k a is the scale factor. The negative gradient of the attraction potential field function is defined as the attractive force function: where the F attr is a vector pointing to the target position X g . Similarly, the definition of repulsion potential field function is based on the distance between the obstacle and robot: where U rep represents the repulsion potential energy, k r is the scale factor, p(X) represents the distance between the endeffector and the obstacle at position X, and p 0 describes the influence radius of each obstacle. The repulsive force is the negative gradient of the repulsive potential function, which is a vector from the obstacle to the end-effector: Hence the final potential field function when the robot at position X is defined as: The final resultant force is F (X) = F attr (X) + F rep (X). However, the classical APF algorithm has defects: (1) The robot can have non-reachable goal, e.g., the repulsion force is greater than the attraction force when there is an obstacle closing to the goal, such the robot can not reach the target point, which is known as the GNRON problem.
(2) The robot can fall into a local minimum region, e.g., the final resultant force of the manipulator is zero or the direction of the resultant force is collinear with the gravitational and repulsive forces, such that the robot oscillates or stops in this area.
(3) It does not consider the relative motion of the endeffector and obstacles, such that the method can not work well in a dynamic environment.

B. Sampling Based Motion Generator
To solve the GNRON problem, we introduce the influence of the goal into the repulsion potential energy, which ensures that the whole potential field is only minimum at the target position. The new repulsive potential function is where (X −X g ) represents the distance between the robot and the target, and n is a constant that is greater than zero. The improved repulsive force function is the negative gradient of the repulsive potential function: where F rep1 and F rep2 are defined as: We can find out that the direction of vector F rep1 is from the obstacle to the robot, and the direction of vector F rep2 is from the robot to the target.
To keep the robot away from local minimum region, we propose a sampling extension of the APF (SAPF). Our idea is to sample multiple possible positions at next time step based on current position (x t , y t , z t ) and the resultant force direction θ t . Since it is computational cost for sampling in the 3D space, we can select some special planes for sampling, e.g., yz plane, xy plane or xz plane. Here we take the yz plane as an example as shown in Fig. 2, where p min is the radius of the area that the distance impact factor starts to work in the APF, p max is the radius of the area that the velocity impact factor starts to work in the APF, v o is the velocity of the obstacle, ∆θ is the angular step between samples, k v is the impactor factor of the obstacle speed which will be given in the next section. The sampled directions θ w in yz plane are defined as following: where θ t is the direction of the resultant force on the robot at the current position projected to the yz plane, v oz is the Z axis component of the obstacle velocity v o , sgn is the sign function and w = (0 . . . n) is the number of samples. The signs of the k v and v oz have following meanings: (1) k v > 0 represents the fast obstacle. The robot samples the predicted positions in the negative direction of the z-axis component v oz of the obstacle velocity based on θ t , which means the robot plan to avoid the obstacle from the back side.
(2) k v ≤ 0 represents the slow obstacle. The robot selects predicted positions that are in the positive direction of v oz based on θ t , and avoid the obstacle from the front side.
(3) For the static obstacle or the obstacle with v oz = 0, the robot chooses the predicted positions in the positive direction of θ t as the next sampling points, which means the robot avoids the obstacle from the top of the obstacle. A number of possible positions at the next time step that the robot might move can be derived by where (x 0 . . . x n , y 0 . . . y n , z 0 . . . z n ) represents the coordinates of simulated positions, and d is the sampling step-size. The final choice of the motion direction is chosen according to our evaluation function defined as: whose output is to select the position with the minimum sum of the absolute value of forces on the robot. When the end-effector of manipulator enters the influence radius of the obstacle, the SAPF approach is used to simulate the possible positions and calculate the force for each simulated sample. In this way, the robot can evaluate next motion in advance to avoid the local minimum region. Furthermore, the robot also considers the efficient of the avoidance, since the direction and magnitude of the relative velocity are included in the evaluation process.

C. Danger Index for Safety and Efficient
To protect the human and keep high working efficiency, we here use relative distance and velocity between the human and robot as the main impact factors of the danger index, which are defined as following: (1) The distance impact factor is defined as: where η = pmaxpmin pmax−pmin is the distance scale factor, p max and p min represent the max and min influence radius of the moving obstacle respectively.
The distance impact factor is affected by the distance relationship between the end-effector and the dynamic obstacle, e.g., the distance impact factor is greater when the robot is closer to the obstacle.
(2) The velocity impact factor is defined as: where γ is the speed scale factor, |v o | and |v r | are the magnitudes of the dynamic obstacle velocity and the endeffector velocity respectively. and sgn() is the sign function.
If γ times the speed of the dynamic obstacle is greater than the speed of the end-effector, the velocity impact factor is a positive integer and vice versa. The specific velocity repulsive potential field function is defined as a combination of the distance impact factor and the velocity impact factor, which is where represents the relative velocity between the end-effector and the obstacle. U rev represents the velocity potential energy and k ro is the scale factor. α is the angle between the relative velocity vector v = v r − v o and the relative position vector from the end-effector to the obstacle, which can be used to determine whether the robot is moving away from the obstacle. With the velocity impact factor, we can adopt different obstacle avoidance strategies for moving obstacles with different motion states. With the distance influence factor, we can dynamically adjust the velocity repulsion according to the relative distance between the endeffector and the obstacle. For k v > 0 , the obstacle has fast speed, so it is safer that the robot moves behind of the moving obstacle. v = v r + v o can be seen as the relative velocity between the end-effector and the obstacle by considering that the robot moves in the opposite direction at a speed of −v o . The resultant force at this time can guide the end-effector to move behind the obstacle. Similarly, the end-effector moves in front of the obstacle when the obstacle is moving slowly. The velocity repulsive force is the negative gradient of the velocity repulsive potential function defined as The velocity repulsion force starts to work when the distance between the end-effector and the obstacle is less than p max . The direction of velocity repulsion force is opposite to the direction of v. Finally, we add the velocity potential field U rev to SAPF for path planning. The resulting force field on the end-effector of manipulator is defined as:  To summarize, the whole proposed algorithm for collision avoidance is shown in Fig.3.

III. EXPERIMENT SETUP AND RESULTS
The proposed framework is evaluated on a mobile manipulator TIAGo robot with a redundant 7-DOF arm. We here mainly consider the cross over task when the end-effector moves to right while the human hand moves up, which is a common scenario of human-robot collaboration and interaction. The safety must be considered first while the robot can not lose efficiency, which means the robot must have smart control decision according to the motion status of the human hand. The motion status of the human hand is detected using 2D ArUco Marker, which is attached on a box grasped by the human hand. We also tried skeleton based vision methods using RGBD sensors, e.g., Kinect. However, due to the detection noise of joint points of the human skeleton, the estimated velocity of human hand is quite inaccuracy. Therefore, the 2D ArUco Marker can solve such a problem for our evaluation, which is fast and accurate. The detected position of grasped box can be transferred to the robot coordinate system as shown in Fig.4 [15]. The proposed SAPF can find out the collision free path for the end-effector  of the manipulator, which is then used for robot control by inverse kinematics [16]. The parameters of the algorithm used in the experiment are summarized in Table. I.

A. Scenario 1: static human hand
For the first experiment, the human hand is static and located between the end-effector and the goal position, which is modeled as a cylinder with a certain expansion range considering the size of the end-effector and safe distance. The robot detects the human hand using the ArUco Marker, and uses the proposed SAPF for online avoidance which is shown in Fig.5. Fig. 5 (b) shows the recorded trajectory (blue) of the end-effector and the position of the human hand (red). We can see that the manipulator can avoid the suddenly appeared human arm successfully.

B. Scenario 2: cross over with a slow speed
In this scenario, human hand crosses the predefined path of the robot at a relatively slow speed. The robot estimates the position and velocity of the human hand by detecting the ArUco marker with a visual camera. The velocity impact factor is calculated, which can drive the end-effector to move ahead of the obstacle. In this way, the robot can have a high efficiency to achieve the goal. The experiment result is shown in Fig.6.

C. Scenario 3: cross over with a fast speed
Here we consider that the manipulator crosses over a fast moving human hand as shown in Fig.7. In this case, it is safer that the robot moves behind of the human hand. The proposed SAPF can achieve this goal with our novel potential function definitions considering the relative velocity. The experiment result also prove the effectiveness of the SAPF.
From above three experiments, we can see that the proposed SAPF algorithm can work well for collision avoidance in the case of the cross over task. Meanwhile, the introduced framework also has considerations of safety and efficient. The robot can make smart decision according to the motion status of the human, e.g., move ahead or behind of the moving hand, which is quite useful for many human-robot collaboration and interaction tasks.
IV. CONCLUSION AND FUTURE WORK In this paper, we propose a novel method to avoid collision during human-robot collaboration and interaction. Both of the  Fig. 7: The experimental result using the proposed SAPF algorithm for avoiding a moving fast human hand. As expected, the robot decides to move behind of the human hand, which is safer: (a) snapshots of collision avoidance for cross over task with a fast speed. (b) the planned path of the SAPF in 3D view and 2D view respectively. safety and efficiency of the robot are considers in the carefully designed algorithm. We first proposed a sampling based artificial potential field (SAPF) method with a novel repulsive potential function that can handle goal non-reachable problem and the local minimum problem, and then introduced a danger index based velocity potential function considering the relative distance and velocity. Furthermore, we demonstrate our idea on the mobile manipulator TIAGo platform using a cross over task. The robot is smart to make decision according to the motion status of the human, which is detected using a visual sensor. Similar to the human, the robot can move ahead or behind of the human hand based on the information of relative distance and velocity. We believe that the proposed work can be useful for many other human-robot collaborative interaction tasks. In future, we would like to consider more human factors in the danger index, e.g., the gaze direction and emotion of the human.