Manipulator Collision Avoidance System Based on a 3D Potential Field With ISO 15066

Recently, various intelligent technological innovations are being applied to smart factories. As manipulators are widely used in smart factories, the manipulator and the workspace of humans overlap, and interest in cooperative robots and human safety has increased. In relation to this, a collision-avoidance control algorithm applicable in three dimensions (3D) and that also meets existing safety standards for humans and robots is required. In this paper, we propose a 3D potential field-based manipulator collision avoidance algorithm that meets the requirements of the ISO 15066 standard. This algorithm applies Speed and Separation Monitoring(SSM) according to the distance between the manipulator and the obstacle and controls the speed of the manipulator slowly as the risk is higher. This allows us to overcome the limitations that existing studies have not been conducted on 3D potential field-based obstacle avoidance and that it is difficult to apply to the field without considering ISO 15066. The proposed system was verified through simulation and experiments, and through comparison with the existing algorithm, we verified that SSM was well applied to the proposed system.

area for safety design and protection dialects of industrial robots and safety requirements for information in use. ISO 10218-2 is a blue area for safety requirements as a robot system when installed and combined with industrial robot production line peripherals. Finally, ISO 15066 is a standard for cooperative robot driving in the yellow area that specifies different safety standards by dividing a zone into several areas according to the distance between a robot and a person. It is largely composed of four safety components: safety-rated monitored stop(SMS), hand guiding(HG), speed and separation monitoring(SSM), and power and force limiting (PFL). VOLUME 10, 2022 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ Research on safety assurance as this concept pertains to cooperative robots can be divided mainly into studies that focus on passive safety assurance methods and those on active safety assurance methods. Passive safety assurance methods are those that reduce the degree of influence through hardware designs of the manipulators used [6] and those that do so through a shock absorption operation [7], [8], [9]. Another approach controls the robot when an collision is detected between a manipulator based on an impact sensor and a person [10], [11], [12], [13]. However, these methods have the drawback of requiring robot-human contact.
Active safety assurance methods are based on an obstacle detection sensor and have the advantage of being safer as they do not necessarily require human contact. As safety barrier function [14] or reinforcement learning [15], [16], various obstacle avoidance algorithms are used to allow the manipulator to work in different environments. Conventionally, inverse kinematics-based obstacle avoidance algorithms were widely used for manipulator control. Using closed form inverse kinematics, there is a study that optimizes the positioning of mobile manipulators with 7 degrees of freedom [17]. Zhao, L proposed algorithm uses inverse mechanics to avoid obstacles while considering the restriction of the manipulator's joint when multiple manipulators avoid complex obstacles in the workspace [18]. In addition, Zhong,J used deep reinforcement learning and inverse kinematics to control the welding manifold to avoid obstacles and move accurately at the same time [19]. Another study presented an algorithm to avoid obstacles in dynamic situations by considering the speed between obstacle-manipulator and inverse kinematics [20]. However, a disadvantage of the inverse kinematics-based obstacle avoidance algorithms are that numerous errors occur and require a lot of computing power during the control process.
To compensate for this, obstacle avoidance control is conducted using a potential field. Tian, Y reduced computational complexity and solved the local minima problem with improved artificial potential field (APF) [21] Zhang, H also established collision distance thresholds and Jacobian-based speed control with a distance between two manipulators in APF to prevent double robotic arm collisions [22], and studies used potential fields and angle limit functions to reduce errors and improve computational speed [23]. However, research is now focused on moving paths in two dimensions (2D).
To overcome this limitation, Wang, C propose a strategy that avoids obstacles that non-holonomic robots move in three dimensions (3D) [24], inspired by sliding mode control [25]. In addition, research has been conducted to reconstruct obstacles based on machine learning to avoid 3D obstacles in the manipulator [26] or programming languages that efficiently generate sample environments composed of complex relationships between three-dimensional objects by combining probabilistic programming techniques and convex computational geometry [27]. Makita, S proposed a system that implements the position of the manipulator and obstacles in augmented reality [28] and virtual reality to guide them to avoid obstacles in 3D, and B established a 3D-collision-force map to study algorithms in which the manipulator avoids obstacles [29]. Regardless, there is a limitation in that it is difficult to apply to a manipulator system that must be pathindependent or that the location of obstacles must be accurately known in advance.. Furthermore, it is difficult to avoid when obstacles are located in blind spots where manipulators cover the detection area of sensors or cameras, and they are not consider existing cooperative robot safety standards. To overcome these limitations, we need APF-based obstacle avoidance algorithms built in 3D without blind spots and satisfies the ISO 15066 criterion for application to industrial sites simultaneously.
We propose the method of obstacle avoidance control of the manipulator using a 3D potential field without a lack of perception that meets the ISO 15066 standard. The first contribution point of our system is to integrate the 2D APF generated with each sensor into 3D APF by placing eight sensors at 45-degree intervals on the surface of the manipulator link. Based on this, the system creates an obstacle avoidance path for the manipulator without a blind spot. Conventional research related to APF-based obstacle avoidance mainly focused on minimizing the local minima of the manipulator obstacle avoidance path created by placing one sensor or camera outside the manipulator. In the field of collaborative robots, if an APF is generated through a small number of sensors used outside the robot, there is a limitation that a robot, a person, or an obstacle cannot avoid a collision in a blind spot that occurs covering the sensor detection area. Our first novelty can overcome this limitation. The second contribution point is to satisfy the 15066, SSM, for application to cooperative robot systems when controlling manipulator obstacle avoidance. Current research on APF obstacle avoidance pathways has focused on obstacle avoidance with fast and optimal paths, but no consideration has been made for manipulators working with humans. Therefore, we set up a risk zone according to human stride and applied manipulator controls that apply different speed limits for each area. Fig. 2 shows a flowchart of the system proposed in this paper. The system calculates the obstacle's position with the sensor's data and creates a 2D potential field for each sensor. Moreover, it integrates this to generate a 3D potential field and speed control the manipulator according to SSM criteria determined by the distance between the obstacle and the manipulator. Through such a system, the sensor detection range in the cooperative robot is not obscured by the manipulator, and obstacles can be avoided. When avoiding an object in space while satisfying the ISO standard, the trajectory of the end effector can pass by avoiding the object. As a result, in this paper, only the position of the end effector was controlled. Accordingly, to reduce the calculation speed, control was performed by simulating with a simplified threeaxis manipulator, except for the three axes mainly used for orientation control of the manipulator.
The contents of the paper are organized as follows. Section 2 presents the overall composition of the control   system, the dynamic modeling used in each controller, the sensor algorithm constituting the obstacle avoidance control system, and the 3D potential field algorithm. In Chapter 3, the proposed control system is applied to the manipulator to perform simulation, and the existing and proposed algorithms are compared and validated. Section 4 covers the conclusions and presents ideas for future work. Fig. 3 shows the structure of the 3D potential field-based obstacle avoidance system proposed in this paper. First, the distance between the manipulator and the obstacle is measured with eight far infrared (FIR) sensors, and the obstacle detection system is used in a high-level collision controller (HLCAC) to determine how far away the manipulator and the obstacle are and where they are located. Using these results, the low-level collision avoidance controller (LLCAC) determines the current danger level according to ISO 15066, and according to this outcome, speed control and 3D potential field control are performed to allow the manipulator to avoid obstacles.

A. HLCAC 1) KINEMATIC CONFIGURATION
Kinematic modeling of the manipulator is required to obtain the exact location of the obstacle using distance data measured by the FIR sensor in the HLCAC. The three-axis rotary manipulator used in this study involved the locking of the fourth, fifth, and sixth axis of a six-axis manipulator (UR3, Universal Robots) [30], as shown in Fig. 4. The numbers in the figure refer to the numbers of joints.
The three-axis manipulator's Denavit-Hartenberg(D-H) parameters and coordinates are shown in Fig. 5. In addition, specific D-H parameters are presented in Table 1.  Based on the above DH parameters, the relationship between the two adjacent links i − 1 and i may be expressed as (1). For simplicity, c = cos and s = sin.
It allows the global coordinate system from the base to the end effector of the robot to display the transformation matrix as shown in (2).
According to ISO 15066, it is necessary to determine the exact distance and position between the manipulator and the person in order to ascertain the hazardous stage area of the operator. To do this, an obstacle detection sensor must be used to calculate the exact relative position vector between the manipulator and the obstacle. In the system proposed here, eight FIR sensors are attached to the middle of third link of the three-axis rotary manipulator at 45 • intervals, as shown in Fig. 6 (a), measuring the distance of the obstacle to obtain the exact position of the obstacle in HLCAC. It shows the sensor attached to the manipulator to meet the conditions set earlier in Gazebo [31], a simulation program by robot operating system (ROS) [32]. Objects marked in yellow refer to the FIR sensors attached to the manipulator link, and the area marked in yellow indicates the detection range of each sensor. The coordinate system of each sensor attached to the third link can be expressed as Fig. 6 (b). The lower part of the VOLUME 10, 2022 link is the second joint, the upper part is the third joint, and the yellow boxes are the sensors. The z-axis of each sensor is the same direction as the z-axis of third link, and the x-axis is the sensing direction. In order to calculate the exact position of the obstacle detected by each sensor, the transformation matrix from the base joint of the manipulator to each sensor S i can be expressed as (3) and (4).
In this multi-sensor system, a situation in which an obstacle is detected by two sensors is shown in Fig. 7. The meanings of the variables are shown in Table 2.
In this manner, the distance d obs and angle θ obs between the manipulator and the obstacle can be obtained via (5) and (6), respectively.

B. LLCAC 1) ISO 15066 BASED SAFETY ZONE DEFINITION
The proposed system applied SSM in ISO 15066 based on the distance between persons and obstacles calculated by HLCAC. We set three zones according to the preset unit distance d from the robot's location and each zone's risk levels, as shown in Fig. 8. The unit distance d is 0.1775 m, 10% of the average stride length of people aged 22-30, in [33] who surveyed gait parameters for 164 people aged 5-30. When the robot position was configured as the origin, the area r d with a radius r was marked in red as the area with a high-risk level. In this area, the joint speed limit of the UR3 manipulator was initialized to ±0.18rad/s, which is 10% of v max UR . Next, the area where d < r 2d is where the risk level is intermediate and marked in yellow. In this area, we set the joint speed limit to ±0.24 rad/s, which is 15% of v max UR . Finally, the area 2d < r is set as a safe area that can ignore the risk level and is colored in green. In this zone, the manipulator can move without speed limit. We used Universal Robots ROS driver provided by Universal Robots to control the joint motor of the UR3, where the speed scaling parameters that control the manipulator's joint speed among input parameters were set to 1, 0.5 and 0.25 from the low-risk order.

2) JACOBIAN MATRIX
To control the speed of a three-axis rotary manipulator in LLCAC, the speed of the manipulator's joint,q, must be calculated using a jacobian matrix. To find the jacobian matrix, we refer to the coordinate system in Fig. 5 and the D-H parameter in Table 1 to find the regular kinematics of the manipulator by means of (7)- (9).
126596 VOLUME 10, 2022 Through differentiation, the velocity on the x, y, and z axis of each joint can be obtained using the jacobian matrix obtained from (10).

3) 3-D POTENTIAL FIELD
The basic 2D potential field refers to a robot's obstacle avoidance method derived from the movement of electric charges. This method assumes that the robot is charged with a positive charge on a 2D plane, the obstacle is charged with a positive charge identical to that of the robot, and the destination is a point charged with a negative charge opposite to the robot. This is how the robot finds the route to its destination. When the coordinate of the robot is q(x, y), the potential energy U(q) acting on the robot as the robot moves to the destination is divided into two components, as shown in (11). U att (q) is the attraction force of the robot to move to the goal, and U rep (q) is the repulsive force of the robot to move away from the obstacle.
The force, F(q), that moves the robot using the above potential energy is expressed as the sum of the gradient vectors of the potential energy. Additionally, q is the current position of the robot, ρ goal (q) is the distance from the current position q to the destination point as determined by (12), and ρ(q) is the shortest distance from the current position to the obstacle. Also, q goal is the location of the destination point, q obs is the location of the outer point of the obstacle closest to the current location, and ρ o is a distance constant related to the extent to which the repulsive force potential extends from the obstacle.
U att can be expressed as (13). Here, ξ is a coefficient that determines the size of U att and is directly proportional to the size of U att . The force F att that U att applies to the robot is determined by (14).
Next, U rep can be expressed as (15). At this time, η is a coefficient that determines the size of U rep and is directly proportional to the size of U rep . The force F rep that U rep applies to the robot is expressed as (16).
This potential field-based obstacle avoidance algorithm has a limitation in that it can be applied only in a 2D space. This obviously makes it challenging to apply it as it is to a manipulator operating in a 3D space. To overcome this limitation, the potential energy U xyz (q) in the 3D space is divided into U s i (q) to regenerate the potential energy on each 2D plane of ith sensor's coordinate. On the existing path of the manipulator accordingly, by combining these potential fields, an obstacle avoidance path in 3D is created. This algorithm is summarized in Algorithm 1.

III. EXPERIMENT AND RESULT A. SIMULATION
To verify the proposed 3D APF-based obstacle avoidance algorithm in this paper, we conduct experiments to test the ability and time of the manipulator to reach its destination in barrier-free and barrier-free environments. This experiment confirms that the proposed system can construct 3D APF well without blind spots and apply it to obstacle avoidance even if the detection area of the sensor changes as the manipulator moves. To this end, after implementing the UR3 manipulator in the ROS-based Gazebo simulator, eight FIR sensors were attached to the third link of the manipulator at 45 • intervals. The FIR sensor used in the experiment is the Terabee Teraranger multiflex type, and the detection range of the sensor is set to fit the sensor's viewing angle of 20 • . Fig. 9 (a) shows the simulation environment for the experiment without obstacles. The area marked in yellow is the detection range of the FIR sensor, and the area marked in red is the endeffector of the three-axis rotary manipulator. The goals set in consideration of the working radius of the manipulator are indicated by the red cube. Fig. 9 (b) shows the generated trajectory when controlling the manipulator's end-effector to move to the goal when there is no obstacle, along with the end-effector's path at four second intervals. In the upper right corner of each scene, the number of seconds is displayed. During this process, the change of each joint angle and the results of analyzing the path of the end-effector as a graph are shown in Fig. 9 (c) and (d), respectively. In Fig. 9 (d), a round marker displays the manipulator at approximately 4-second intervals from where it starts to move to the destination. As a result of the simulation, when there is no obstacle, a time of 8.04s was required for the end-effector of the manipulator to reach the goal, and it could be confirmed that the manipulator moves along the shortest path without a path change.
Next, a simulation with obstacles was conducted. Fig. 10 (a) shows the simulation environment in Gazebo in this case. The obstacle was set as a sphere with a radius of 0.05m, and the green sphere in the figure is the obstacle. In the presence of an obstacle, the result of controlling the manipulator's end-effector to move toward the goal is shown at three second intervals, as shown in Fig. 10 (b). For each scene, the location and path of the end-effector are marked.
As a result of the simulation, it took 16.40s for the manipulator's end-effector to reach the goal in the presence of an obstacle. In addition, it can be confirmed that the manipulator feasibly controls the speed by detecting the obstacle and calculating the danger level through the joint velocity with risk levels of Fig. 10 (c) and the trajectory of the end-effector in Fig. 10 (d). The gray sphere on the graph represents an obstacles. In Fig. 10 (c), we marked the manipulator joint angle over time with a solid line and the joint speed with a dashed line. In the graph, the time when the obstacle is detected and the time when the risk level changes according to the distance from the obstacle are indicated by the pink dashed line, and the risk levels are indicated in green, yellow, and red in the same order as the color in Fig. 8. In addition, Fig. 10 (d) shows the moment the sensor detects an obstacle using a star-shaped marker, and the round marker is used to mark approximately 4-second intervals from where the manipulator starts to move to the destination. It can be confirmed that a new 3D path is regenerated and then utilized by calculating a potential field to avoid an obstacle.

B. EXPERIMENT
An experiment was conducted to demonstrate the system by applying the 3D potential field-based obstacle avoidance system to an actual manipulator. To do this, the environment used in the simulation was also used here, as shown in Fig. 11 (a). The third axis of the UR3, indicated by the red circle, was set as the end-effector, and eight FIR sensors were attached to the center of the third link at 45 • intervals. In addition, the destination is marked with a white cube. Fig. 11 (b) shows the result of the manipulator's endeffector motion toward the goal in the absence of an obstacle, along with the end-effector's path at four second intervals. In the upper right corner of each scene, the number of seconds is displayed. At this time, the result of analyzing the path of the end-effector as a graph and the change of each joint angle are shown in Fig. 11 (c) and (d), respectively. In Fig. 11 (d), a round marker displays the manipulator at approximately 4-second intervals from where it starts to move to the destination. In the actual experiment, in the absence of an obstacle, it took 7.96s for the end-effector of the manipulator to reach the goal. It was also confirmed that it moved along in the shortest path.
Next, the experiment was conducted without obstacles. The experimental environment was configured as shown in Fig. 12 (a), where the obstacle is indicated by the black sphere, and the radius is set to 0.05m with the same approach used in the simulation. Fig. 12 (b) shows the result of controlling the manipulator's end-effector to move toward the goal in the presence of an obstacle at three second intervals. In each scene, the location and path of the end-effector are marked. In this experiment, it took 16.8s for the end-effector of the manipulator to reach the goal when there was no obstacle. During this process, it could be confirmed that the manipulator speed was controlled through the calculation of the risk level through the joint velocity with risk levels of Fig. 12 (c). We found that it regenerates and moves a new 3D path to avoid obstacles, as shown in Fig. 12 (d). The gray sphere on the graph represents an obstacles. In Fig. 12 (c), we marked the manipulator joint angle over time with a solid line and the joint speed with a dashed line. In the graph, the time when the obstacle is detected and the time when the risk level changes according to the distance from the obstacle are indicated by the pink washed line, and the risk levels are indicated in green, yellow, and red in the same order as the color in Fig. 8. In addition, Fig. 12 (d) shows the moment the sensor detects an obstacle using a star-shaped marker, and the round marker is used to mark approximately 4-second intervals from where the manipulator starts to move to the destination. The difference in time required between the simulation and experiment was 0.98% in the case without an obstacle and 2.38% with an obstacle. Additionally, the error of the generated path was measured at the point with the most considerable path difference, as shown in Fig. 13 (a) without obstacle and (b) with an obstacle. The gray spheres represent obstacles in these figures. The corresponding errors between the simulation and experiment are 0.011m and 0.027m. The path difference was calculated by the Levenberg-Marquardt method [34].

C. COMPARISON WITH EXISTING WORK
Simulations were conducted to compare the conventional potential field-based obstacle avoidance algorithm with the proposed algorithm. The simulation environment is the same as the obstacle avoidance simulation conducted above, and parameters ξ and η, which determine the attraction and repulsion of the potential field, are set the same as both the conventional and proposed algorithms.
When obstacle avoidance was performed, the avoidance path was generated at 0.01 s intervals using Linear Segment Parabolic Blends (LSPB) [35], and the velocity of all joints of the manipulator was measured at each step. Next, the absolute value of the measured joint velocity was classified and compared according to the distance between the obstacle and the manipulator set in LLCAC as shown in Fig. 14. We performed the same obstacle avoidance and obtained the results that the potential field parameters of the conventional means and the proposed means were the same, but the joint speed of the manipulator was different. The reason is that configuring the potential field is different and is affected by sensor noise. The conventional algorithm was marked in blue box, the proposed algorithm was marked in red box, and the point where the manipulator was located was assumed to be the origin O, and the area with high risk according to the distance r d, the area d < r 2d, and the area where the risk could be ignored. As a result, existing algorithms moved without joint speed limitations in all zones and did not meet the ISO 15066 SSM criteria. On the other hand, it was verified that the proposed algorithm increases the joint operation speed as the distance between the obstacle and the manipulator increases according to the speed limit preset for each zone. In addition, the time taken to reach the destination while avoiding obstacles was 13.47 s for the existing algorithm, which had no joint speed limit, 17.86% faster than the proposed algorithm's time of 16.40 s.

IV. CONCLUSION AND FUTURE WORK
In this paper, we proposed a 3-D potential field-based obstacle avoidance control system that meets the requirements of ISO 15066 to ensure the safety of workers when people and manipulators exist in the same work space. To confirm the capabilities of the proposed system, simulations and experiments were conducted by applying it to a three-axis rotary manipulator. As a result, in the absence of an obstacle, conventional inverse kinematics-based control was performed. Furthermore, if there is an obstacle, it detects it and generates a three-dimensional potential field accordingly. Based on this, the end effector avoided obstacles along the generated threeaxis manipulator obstacle avoidance path while controlling the position. Through this, it was verified that the proposed algorithm successfully avoids obstacles. Next, as a result of comparison with traditional algorithms through simulation, it was affirmed that the proposed algorithm, unlike the existing algorithm, is controlled under a preset joint speed limit according to the distance to the obstacle according to the ISO 15066 SSM criterion.
However, because this system uses a potential field, there is a limitation in that the local-minima may fall when the number of obstacles increases. In addition, as the joint speed limit is applied, the longer work time has increased, which means that there is a limitation of poor work efficiency. In the future, we will solve the problem that potential field based algorithms may fall into local minima when avoiding several obstacles and research how to improve work efficiency while satisfying ISO 15066 SSM criteria.