Dmitry Popov

Alexandr Klimchik

Anatol Pashkevich

Real-Time Estimation of Multiple Potential Contact Locations and Forces

Keywords: Physical Human-Robot Interaction, Force and Tactile Sensing, Human-Robot Collaboration, Contact Modeling

In this work, we propose a contact point localization and external force estimation algorithm for collaborative robots. In comparison with existing approaches, the proposed algorithm can detect and evaluate multiple solutions and more than 3x faster without loss of accuracy. To achieve real-time performance, a hierarchical robot representation and surface mesh preprocessing was used, allowing us to achieve a 50x speedup in a run-time, compared to checking all contact points. Mesh preprocessing includes two-step clustering in the space of vertices normals vectors and vertices positions. The localization method was tested in a simulated Kinova Jaco 2 and real KUKA iiwa LBR 14 collaborative robots. Our solution allows estimating the contact point on the robot surface with 2.3 cm average accuracy in a more than 600 Hz loop.

I. INTRODUCTION

C OLLABORATIVE robots, especially manipulators are becoming more popular nowadays. One of their main advantages is the ability to work together with a human on the same task. Usually, this task is located in a shared environment between a robot and a human worker. As a result, a robot is forced to work in a complex environment with dynamic obstacles [START_REF] Magrini | Human-robot coexistence and interaction in open industrial cells[END_REF]. This will increase requirements for robot control to predict human intentions and actions. During this interaction, physical contact between a robot and a human could be unavoidable. However, the contact between a human body and a robot is not always a negative event. In some cases, physical interaction could be used to control the robot, program, or correct its path.

To ensure human safety in this human-robot collaboration, the interaction force should be estimated and controlled during the execution of the robot program. In addition, not only the magnitude of the force is important, but also its direction and position where it is applied. The time, required to estimate the contact point and the force is also critical. Since the robots can achieve high speeds, only a split second is enough to harm a human or the environment.

Our work is dedicated to creating a method for fast estimation of a contact point on the robot surface. We assume that the fact of the collision is already known by using any of the existing approaches [START_REF] De Luca | Sensorless robot collision detection and hybrid force/motion control[END_REF], [START_REF] Haddadin | Robot collisions: A survey on detection, isolation, and identification[END_REF], [START_REF] Heo | Collision detection for industrial collaborative robots: A deep learning approach[END_REF] and the robot is equipped only with 1D joint torque sensors.

The problem of contact localization for collaborative robots was formulated in the past decade. The authors in [START_REF] Likar | External joint torque-based estimation of contact information[END_REF] proposed some simplifications of the robot shape to formulate the problem by only two constrained parameters. Later this method was used in [START_REF] Popov | Collision detection, localization & classification for industrial robots with joint torque sensors[END_REF], [START_REF] Zwiener | Contact point localization for articulated manipulators with proprioceptive sensors and machine learning[END_REF] and compared with machine learning approaches. Manuelli and Tedrake [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF] suggested using a particle filter for collision localization. In this approach, particles are projected on the robot surface, and for each particle probability of contact is estimated. The authors also introduced a friction cone constraint for the external force. In [START_REF] Bimbo | Collision detection and isolation on a robot using joint torque sensing[END_REF] the motion model of particles was modified with a combination of small self-movements of the robot to increase accuracy. Monte Carlo localization was used in [START_REF] Zwiener | ARMCL -ARM Contact point Localization via Monte Carlo Localization[END_REF] and compared to direct optimization and machine learning methods. In both works the authors estimated only the forces acting normal to the surface to decrease computational demand.

In one of our previous works [START_REF] Popov | Real-time external contact force estimation and localization for collaborative robot[END_REF] the contact isolation was done using two-step optimization on the mesh surface of the robot KUKA iiwa 14. In another work [START_REF]Transfer learning for collision localization in collaborative robotics[END_REF], we used a neural network trained to find a contact location in simulation and then transferred this network to the real robot.

Unfortunately, no of the existing approaches deals with the uncertainties of a localization algorithm, when only single-axis torque sensors are available. These methods are designed to output only one contact position. Depending on the robot pose and an external force direction, multiple solutions are possible as shown in Fig. 1. Torques, generated by one physical collision at P2, could be also described by another force at P1. So, in this particular configuration with estimated torques, the real collision could be in P1 or P2. In practice, the location of the collision will define the possible reaction of the robot. For example, we have a real contact with a human in the robot elbow which also could be represented by contact at the robot end-effector. The reaction, in the case of contact in the elbow, is to increase robot compliance and limit interaction force, but in the case of end-effector is to compensate external force, which are opposite reactions. Here, the choice of incorrect reaction could harm the human operator. Another example could be a collision-avoidance reaction which will be different for collision in elbow and end-effector.

In this work, we propose an algorithm that is capable of estimating all possible solutions, even if they are located on a different link. We present a robot mesh preprocessing procedure that allows to implement a real-time execution.

II. PROBLEM STATEMENT

In the event of a collision between a robot arm and an external object, it is important to identify where exactly this collision happened. This information can be further used to chose a reaction action to ensure safety. In this work, we will address the solution of this problem for a serial manipulator, equipped with 1D torque sensors in the joints.

Lets us consider serial manipulator with a n degrees of freedom. The robot state at rest can be described by the joint coordinate vector q ∈ R n and the joint torque vector τ ∈ R n . The robot has a collision in the point p c ∈ R 3 with an external object and this point is located on the robot surface p c ∈ C. This physical interaction leads to the external wrench

W ext ∈ R 6 applied in p c . Here W ext = [F ext , M ext ] T ,
where F ext ∈ R 3 is an external force and M ext ∈ R 3 is an external moment. For simplicity, the τ values here caused only by the external wrench W ext .

τ = J (q, p c ) T W ext (1)
In general, mapping between forces and torques in Cartesian and joint coordinate systems can be expressed with a robot kinematic Jacobian matrix J (q, p c ) ∈ R 6×n where p is a point in the local coordinate frame of the link with W ext [START_REF] Magrini | Human-robot coexistence and interaction in open industrial cells[END_REF]. One column of a Jacobian matrix, can be found by:

J j = J v j J w j = Z j-1 × (P -O j-1 ) Z j-1 (2)
where J j is a jth column of a Jacobian matrix, J v j corresponds to linear, J w j to angular velocity Jacobian. P is a position of an arbitrary point p ∈ C in the base coordinate frame, O j-1 is an origin of a previous joint and Z j-1 its axis of rotation. All of these values depend on q.

In Eq. (1) q, τ are known from the robot state, and to solve a localization task we need to find W ext and p c . The straightforward solution is to use a pseudo-inverse of the Jacobian J (i) (q, p) for all points on the robot surface C. To do so, we fix the potential collision point p and a link i where this point lies to compute Jacobian.

The problem can be simplified by the assumption of a point contact collision. This leads to M ext = 0 and it is sufficient to consider only F ext component of the W ext . In this case Jacobian becomes J v (i) (q, p c ) ∈ R 3×n . When i = 3 there will be exactly one solution for all point on the surface of ith link, but for the i > 3 we can find an approximate solution for an over-determined system by: min

F , p∈C τ -J v (i) (q, p) T F (3)
Assuming that we also need to iterate through points on the curved robot surface C, multiple solutions are possible. The example of these multiple minimum cases is shown in Fig. 1 where torque in joints can be described by the contact in point P 1 or P 2. This multiple minima behavior could be explained by the existence of point-force pairs that could generate torque equal to the observed from the robot sensors.

In practice, it could lead to wrong external force and collision point estimation and as result wrong robot reaction to the collision. Thus, this behavior should be studied.

III. PRESENCE OF MULTIPLE SOLUTIONS A. Planar Robot Examples

To illustrate multiple solution cases, let us consider a planar 4 degree of freedom robot with link length L = [1, 1, 1, 1]. The external force F ∈ R 2×1 in the contact point p and vector of external torque τ ∈ R 4×1 caused by the original force F ext .

The solutions of (3) can be found by numerically estimating the residual r(i, p) for points on the link surface p:

r(i, p) = τ -τ p (4)
where τ p found from (1) for a contact point p on the i-th link using pseudo-inverse. The residual value of 0 means exact recreation of input torque τ , while other values mean inability to find a force vector that will result in τ torque. The presence of multiple solutions will result in multiple minima in r(i, p).

Let us examine few cases where multiple minima can be detected. At the first stage, consider a planar robot with rod links, where the contact point can be placed on the axis of the link. The summary of test cases is presented in Table I.

1) Case 1: The robot is in a configuration q = [0 • 45 • -90 • 45 • ] T , has a collision in point i = 4 p c = [0.5, 0] T , interaction force is F ext = [0, 1] T . This robot configuration is not singular kinematically and from the external force point of view. As result, we have only one point with zero residual and one corresponding external force.

2) Case 2:

q = [0 • 45 • -90 • 45 • ] T , collision in i = 4 p c = [0.5, 0] T , interaction force is F ext = [1, 0] T .
From the kinematic point of view, this configuration is not singular, but the direction of F ext intersects the 4th joint axis and produces a zero torque τ 4 making it singular to the given external force. As a result, any point on the 4th link can be used to describe an interaction. The estimated external force vector will be equal to the original contact force at any point of the 4th link.

3) Case 3:

q = [0 • 0 • 0 • 45 • ] T , collision in i = 4 p c = [0.5, 0] T , interaction force is F ext = [0, -1] T .
The Jacobian rank rank(J p ) = 2, this configuration is not kinematically singular, external force does not intersect joints axis, but has multiple minima. The main reason for singularity here is the position of all joint axis, where all of them lies on the line. In this situation, at any point of the 4th link, we can have an infinite number of forces that will result in a zero residual.

Robot configuration

Cost function r eq. ( 4) Case 1: Case 2: Case 3:

Case 4: 4) Case 4: q = [0 • 45 • -90 • 45 • ] T , collision in i = 4 p c = [0.5, 0] T , interaction force is F ext = [1, 1] T .
Here the Jacobian rank rank(J p ) = 1 and robot pose is kinematically singular, all joint centers lie on the line. This case has one point of collision and an infinite number of force vectors which can be applied to this point to achieve a zero residual.

For before mentioned cases, the multiple minima behavior mostly observable in a singular configuration. Here the term singular corresponds to not only kinematic singularity but also for the instances with aligned joint centers and when the force intersects the last joint.

So far we observed multiple solutions in a rod link robot, but in the real world, the robot's link could have a complex shape. In addition, the external force could be applied only to the surface. The presence of a surface could increase the number of possible solutions even in non-singular cases.

Let us now consider the same 4 DoF planar robot with a thicker link, where the contact point can lie only on the right or left surface of the the Γ shaped last link. The summary of test cases is presented in Table II.

5) Case 5: The robot with q = [0

• 45 • -90 • 45 • ] T has a collision in i = 4 p c = [0.4, 0.15] T , the interaction force is F ext = [-1, 1] T .
The robot pose is not singular kinematically and from the external force point of view, since the force vector does not intersect the last joint axis. The line of force action intersects the 4th link surfaces in 4 points P 1, P 2, P 3, P 4, where P 1 is an original point of contact. This scenario is similar to Case 1, but the existence of a surface increased the number of potential solutions to 4. All potential

Robot configuration

Cost function r eq. ( 4) Case 5: Case 6:

intersection points have a zero residual value. Therefore the same force could be applied in any of these points without any possibility to distinguish between them in static.

6) Case 6:

q = [0 • 45 • -90 • 45 • ] T , collision in i = 4 p c = [1.15, -0.3] T , the force is F ext = [1.15, 0.3] T .
The robot is in the same configuration as in Case 5, but the force vector now intersects the 4th joint axis and produces a zero torque in τ 4 = 0. Points P 2, P 3, P 4 lie on the 4th link and P 1 on the 3rd. This case is similar to the Case 2, but because of the link surface, we no longer have an infinite number of possible contact points. Now, it is impossible to distinguish even the link of the robot with a collision.

B. Existing Approaches for Multiple Minima Resolution

Some of the current approaches in contact localization reduce the number of possible solutions by putting constraints on the direction of the external force. Assuming that there is physical contact between the robot and an object, there should be some friction forces defined by the friction coefficient. Thus, the external force will lie inside of a friction cone with a friction coefficient µ > 0, directed by the normal of a robot surface. The eq. ( 3) can be supplemented with an additional condition F ∈ F C p where F C p is a friction cone in point p defined by its normal vector p n and a µ. We can simplify this problem by approximation of friction cone with the tetrahedral pyramid

F = 4 i=1 α i F cone,i .
Here, α i is the weight of each support vector F cone,i . It allows to rewrite the optimization problem (3) in terms of quadratic programming for a fixed contact point p:

min a≥0 a T Ha + g T (5)
where H = J a J T a is a square of Jacobians, variable a is weight for a support vectors 4 × 1, g = -J a τ ext . More information about this approach could be found in [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF].

This allows us to reduce the number of solutions by eliminating points where the force is directed opposite to the normal vector or vice versa. For example, in Case 5 it will drop out points P 2 and P 4. It should be noted that this constraint will not help in the scenario where external force intersects the joint axis and a potential collision point could be on a different link.

The approximation of the friction cone could also be a source of errors for the force vectors that lie inside of the friction cone, and outside of the pyramid.

IV. PROPOSED ALGORITHMS

In this section, we propose our way of finding a solution for the system (3). The system of the equation has two unknowns, p as a contact point and F as an external force. In particular, the point of contact p is constrained by the known surface C and an external force F by the normal vector in the contact point p n and a friction coefficient µ. Since the robot surface is usually formulated as a triangular mesh, it already includes information about link surface and link normal vectors. Thus, by analyzing the robot mesh, one can reduce the search space of the problem and as result reduce computational complexity, enabling real-time execution. However, robot mesh is mostly used for visualization purposes, therefore should be modified.

One way to avoid iteration through all points on the link surface is to introduce a tree-like structure. The idea behind that is to divide the surface into segments, each segment will be represented by the reference point. One significant requirement for the reference point and all segment points is that they should have similar properties in a sense of position and orientation. According to that, points in one segment should be clustered by their position and their normals orientation, so classical 3D clustering algorithms like k-means are not applicable. In this way, the robot will be the root of a tree, the first layer of leaves are links, each link has leaves as segments and each segment has bottom leaves as points on the surface. By isolating the link with a potential collision we evaluate the residual (4) in the reference points of this link a choice segments with the lowest residual value and finally search contact point only in these segments. As result, we greatly decrease the number of evaluations, but only if we chose the right segment.

The problem of multiple solutions is solved by estimating interaction force in the last link with non-zero torque in its joint and then recursively checking for an intersection between this force line of action and the next joint axis. If the line of force action intersects the axis, then this force could also appear on the next link as was shown in Case 6.

A. Surface Clustering

Let us assume that surface is described by a discrete triangular mesh which consists of a finite number of vertices v ∈ R 3×m and a list of facets f ∈ R 3×n . Each facet consists of three edges between three points from v. In addition, each vertex has a normal vector n ∈ R 3×m , that locally follows the surface normal.

The segmentation or clustering allows to group a large number of vertices and represents them by one reference point. The first approach for making k clusters is to sample k uniformly distributed points on the robot surface and then use a Voronoi segmentation for all vertices. This can be achieved by the Farthest Point Sampling algorithm and following Lloyd's relaxation. The algorithm for this is presented in Algorithm 1. Now, let us describe this method in more detail.

Suppose there is a triangular mesh that we need to divide into four segments. The first step is to sample the first segment reference point by randomly choosing it from the vertices list and then find a distance to all other vertices v (Fig. 2a). The distance function here is not a Euclidean distance, but a geodesic distance that shows the distance on the mesh surface. In our algorithm, it implemented using the heat method [START_REF] Crane | Geodesics in heat: A new approach to computing distance based on heat flow[END_REF] as a part of gptoolbox [START_REF] Jacobson | gptoolbox: Geometry processing toolbox[END_REF].

In the second step after measuring a distance to all points, select the farthest one as a second cluster reference point (Fig. 2b). Now the distance to some arbitrary point p on the surface can be estimated as minimum distance from existing reference points dist = min(geoDist(P 1, p), geoDist(P 2, p)). After that we repeat until the number of segments reference points is equal to the desired number of clusters (Fig. 2c,d).

The next step is to apply the Voronoi partition and assign a cluster to all points on the surface. It can be done to any surface point by finding the closest reference point and assigning the same class. The clustering result is shown in Fig. 3a. Since the reference points were fixed during the previous step, obtained clusters are unbalanced, reference points sometimes located in the corners of its segments (P 2 and P 3).

To balance the clusters and move the reference points we apply Lloyd's relaxation. By iteratively moving the reference points to the center of its segments, we can acquire balanced segments with almost the same size and reference points in their centers (Fig. 3b). It should be noted that the choice of initial point does not show any significant difference in final cluster size or quality.

If we look at the first and the fourth cluster with reference points P 1, P 4, they include two kinds of points: one of them lies on the vertical wall of a mesh and the second on the curved surface. Vertices normals in these two groups will be orthogonal, thus the reference point is capable of describing a position of neighborhood vertices, but not able to describe their orientation or normals. Although it is possible to increase the number of clusters, it will not guarantee that two points in the same cluster will have orthogonal normal vectors.

To overcome this problem, we can repeat the same steps of Algorithm 1, but for the normal vectors n. The result for cluster segmentation in a normal vectors space is shown in Fig. 4a. By mapping clusters back to the vertices positions v, the clusters are now grouped according to their normals, all vertices from the vertical wall are in the same cluster and a curved surface is divided by other three clusters (Fig. 4b).

B. Robot Mesh Preprocessing

The robot mesh preprocessing procedure is executed offline and allows to obtain a tree-like structure for each link. In this procedure, all robot link meshes are clusterized in a similar way that was described in a previous section.

Usually, link mesh files are derived in a .stl format with a list of vertices coordinates v, a list of facets f , and a list of normal vectors for each facet. The image of such mesh for the second link of the KUKA iiwa robot is shown in Fig. 5a.

The standard optimization routine for localization assumes a continuous function for the potential contact point p, so in the case of a triangular mesh, it means that the contact point could be in a vertex or a facet of a mesh. The searching process implies a gradient descent-like method to identify the exact location of the local minimum on the surface C and a lot of computational power. At the same time, the noise in external torque measurements will limit the accuracy of localization. Thus, in our approach, we scatter a set of equally distributed points on the robot surface and estimate contacts only in these points. The average distance between scattered points is set to a value, which is several times lower than the desired localization accuracy. To obtain a new mesh with equally scattered vertices, we used an isotropic remesh algorithm from [START_REF] Yan | Isotropic remeshing with fast and exact computation of restricted voronoi diagram[END_REF]. In addition to remeshing, the original link mesh file (Fig. 5a) was manually edited to remove internal areas of the link (A). The resulting mesh is presented in Fig. 5b. The area marked with (B) had very inconsistent triangles sizes compared to the same region of remeshed link (C).

Mesh clustering could be done using Algorithm 1 for a vertices coordinates v. The output of this algorithm is shown in Fig. 5c. Again, as was described previously, two points of the same segment (shown in the zoomed image of a Fig. 5c) have almost orthogonal normals directions. This will result in a bad representation of a cluster with a reference point. To overcome this, Algorithm 1 was used in a vertices normals space n (Fig. 5d). The same zoomed region now shows that there are two separate clusters. Despite that, obtained clusters are unbalanced in size (D) and can have unconnected parts. Therefore we propose to use two-step clustering.

The stages of a two-step clustering, used for the link mesh preprocessing are demonstrated in Algorithm 2. The first step is the clustering of a link in a vertices normals space, and after that, clustering of all obtained clusters on subclusters. In this way, we achieve clusters of the desired size and approximately the same orientation. The result could be evaluated in Fig. 5e.

Compared to traditional clustering with a weighted objective function that considers vertices position and normals, our approach better describes small details. In traditional clustering with a weighted coefficient between vertices positions and normals (separated optimization is required to find optimal weights) will not give clusters with a fixed maximum size and most likely miss-classify minor parts of the surface. The twostage algorithm generates clusters with limited maximum size to ensure algorithm run-time and is not limited with minimum size to preserve smaller surface elements and features.

After the segmentation of mesh vertices, data about each segment is stored in a Robot structure. Information about stored values is presented in Fig. 6.

C. Localization Algorithm

In this section, we present an algorithm for collision point localization. In contrast with other available approaches, the algorithm is capable of estimating multiple solutions for the cases where the force vector intersects the last joint axis. The Algorithm 3 consists of two parts, the first part (localization link) deals with localization on the robot link, and the second part (localization robot) with the whole robot. Let us consider the first part in more detail.

As was mentioned previously, we try to exploit the proposed tree-like structure of the robot by estimating residual r in cluster reference points. For the reference points, their location in the global and local frame is known, the normal vector is given, joints position and torque are known from the robot state. Consequently, the external force vector F could be estimated using pseudo inversion. Typically there will be at least two points where the line of external force action intersects the link surface, but only one of them is possible due to friction cone constraint. This is why we manually set the residual as a large value for the points where an external force is out of the friction cone. It should be noted that instead of pseudo inversion we can solve a quadratic program [START_REF] Likar | External joint torque-based estimation of contact information[END_REF] with a friction cone approximation, but it could decrease the performance of the algorithm.

Next, we pick only n segments with the lowest r in their reference points and find the residual for the vertices of the segments. In this way, we avoid iterating through all link vertices and reduce computation requirements.

As the last stage, we need to check the possibility of multiple solutions. Multiple solutions on the same link are basically multiple minima on the already estimated residual rp, so it is sufficient to output the local minima. However, the external force, estimated in the current link could intersect following after the link joint axis and generate zero torque in it.

To address this issue we introduce a simple metric of a distance d between the estimated external force and the following joint axis. If there is an intersection, then we need to check the next link for a collision too.

The localization for the whole robot starts with link isolation. Firstly, the link following the last non-zero torque is isolated. In the practice, of course, there are some threshold values that depend on the joint torque noise. The isolated link is also could be extracted from the estimated collision location on the previous timestamp (p prev) if there is one. This isolated link is checked for a collision in a localization link routine and the following multiple solution evaluation. In this evaluation, we compare the distance between the estimated line of force action and the joint axis. In other words, if there is an intersection, we start to iteratively check the adjacent links until there will be no intersection, or we reach the last link. In this way, we can acquire multiple solutions on one Algorithm 3: Localization on the robot surface link and an additional solution from the following links.

Consider the example on the Fig. 7a. The KUKA iiwa robot has a collision on the 6th link. The last joint with non-zero torque is a joint 6, so we isolate the 6th link and try to estimate a contact point on it. There are two possible solutions, one of them is where the actual external force is applied and the second one is on the other side of the link which represents the same force but in the opposite direction. The second contact point is ignored due to friction cone constraint. The estimated force for the first potential contact point does not intersect the axis of a 7th joint. Thus, the robot in a given pose and interaction has only one solution.

Another example from the Fig. 7b shows multiple minima behavior. The real contact is on the 4th link and a 4th joint is the last joint with non-zero torque. The external force, estimated after localization on a 4th link will intersect the axis of a 5th and 6th joint. By searching another collision point for these links we can see that there is another solution in a 6th link. From the current robot state, it not possible to decide which of them is a true contact location, everything we can do is to mark these points and see if changes in robot pose or external force will leave only one of them.

The real-time capabilities of the proposed method can be explained by the hierarchical robot surface representation and the fact that the most complicated part in estimation is the inversion of 3 × 3 matrix for each potential contact point. The most time-consuming part is a Jacobian calculation, but using an only J v (i) part of a (2) we need to once in a timestamp find the forward kinematics to each joint origin and extract Z j , O j . As a result, this algorithm could be run in real-time.

V. EXPERIMENTS & RESULTS

For the experimental study, the KUKA iiwa 14 collaborative robot was used. The obtained method was tested in simulation and hardware. For the simulation, we generated 1000 random robot poses and applied external force in 22 contact points giving 22000 samples in total. For the real robot, only 10 test configurations were used to verify our results. All points of contact were located in the last four links where a random force from a friction cone was applied. To match a contact point location on the real robot and our virtual model the ATOS 5 measurement system was used. 22 ATOS 5 markers were placed on the real robot surface to estimate their exact location with sub-millimeter accuracy. During the experiment on the real robot, the operator applied external force only in the marked point with a known location on the robot surface. This allowed us to obtain ground truth information about contact point p c . The ground truth for force vector amplitude or direction was not provided, but in the future, we plan to use a special tool with a force sensor.

The developed algorithm was tested on Intel Core i5-4210H 3GHz CPU, 8Gb RAM PC as a Matlab program.

The result of virtual and real experiments presented in Table III. For our test, we used 5 different parameters for our algorithm and one case where we simply iterate through all vertices of robot mesh. The vertices on the mesh were located with a 0.5 cm mean distance between them. As a parameter, desired size of a cluster d size from Algorithm 2 was set as 10, 25, 50, 100 vertices, that give approximately 500, 200, 100, 50 segments per link. In the simulation scenario, N (0, 0.5N m) noise was added to the torque values to emulate dynamic model inaccuracies in external torque estimation. The accuracy metrics in this table correspond to the average distance between the ground truth point of contact and the closest estimated local minimum from a set of possible contacts. The run-time evaluates the average time to estimate a contact point.

Fig. 1 .

 1 Fig. 1. Contact localization for the KUKA iiwa robot. Left: Robot pose and the actual point of contact. Right: estimated residual, collision in any of two potential contact points (P1 and P2) can produce the measured torque τ .

Algorithm 1 : 3 g 4 for 1 : num cl do 5 D 8 c

 134158 Link mesh clustering 1 Function geoCluster(v, f , num cl): Input : Mesh geometry (v, f ), number of clusters num cl Output: List of clusters c 2 1st step: Farthest Point Sampling; = random(v); = geoDist(v, f , g); 6 n ind = f ind(max(D)); 7 g.add(n ind); = voronoi(v, f , g); 9 2nd step: Lloyd's Relaxation; 10 while c = c prev do 11 for each c do 12 vi, f i = c(v, f ); 13 g = f indGeoCenter(vi, f i); 14 c = c prev; 15 c = voronoi(v, f , g);

Fig. 2 .

 2 Fig. 2. The evolution of the Farthest Sampling Point algorithm. The color shows the geodesic distance (the distance on the surface of the mesh) between reference points and all mesh vertices.

Fig. 3 .Fig. 4 .

 34 Fig. 3. Clusters normalization. (a) shows segments for the reference points located as in Fig. 2d, clusters size is unbalanced, reference points are static. (b) shows segments after Lloyd's relaxation, where reference points iteratively moved to the center of each cluster.

Algorithm 2 : 7 for each cn do 8 c 9 for each c i do 10 if 12 c 14 c = c i; 15 store

 278910121415 Mesh preprocessing 1 Function clusterMesh(f ile): Input : Mesh file in .stl format Output: Data structure link 2 d size desired size of a cluster; 3 v stl, f stl, f n stl = importST L(f ile); 4 v, f , f n = isoRemesh(v stl, f stl, f n stl); 5 vn = calcN ormals(v, f , f n); 6 cn = geoCluster(vn, f , n cl); i = connectedComponents(cn); size(c i ≥ d size) then 11 n cl = size(v(c i))/d size ; = geoCluster(v(c i), f (c i), n cl); 13 else c data in a link;

Fig. 5 .

 5 Fig. 5. Link mesh clustering steps. Imported mesh (a) from .stl model have unreachable areas (A) and inconsistent vertex distribution (D). This mesh is preprocessed (b) by removing unnecessary areas and remeshed by uniformly placed vertex points (C). In (c) we applied Algorithm 1 for vertices coordinates, zoom-up image shows that two points in the same segment have almost perpendicular normals. We can avoid this by applying the same algorithm to vertices normals (d), but one can expect clusters with inconsistent size (D) and unconnected areas. As an alternative, a two-step clustering algorithm (e) is proposed (Algorithm 2) which eliminates the disadvantages of (c, d).

Fig. 6 .

 6 Fig. 6. Data structure for the robot state and internal surface representation.

24link=Fig. 7 .

 7 Fig. 7. Localization algorithm for KUKA iiwa. Probability at all points is estimated for visualization. The green circle is a possible collision location, the red cross is a true contact location.

TABLE I SINGULAR

 I AND NON-SINGULAR CASES FOR ROD ROBOT MODEL.

TABLE II NON

 II -SINGULAR CASES FOR ROBOT MODEL WITH SURFACE.

1

  Function estimate_F(ind, Robot): Input : Surface point index ind, structure Robot Output: External force F , residual rp

	2	J = Jac(Robot.Links(link).v(ind));
	3	F = (J J T ) -1 J τext;
	4	if (F , Robot.Links(link).vn(ind)) < atan(µ) then
	7	else
	8	rp = Inf ;
	9 Function localization_link(link, Robot):
		Input : Link index link, Structure Robot
		Output: Local collision point p l, external force F l, residual
		r, dist. between F l and joint axis d
	10	for each Robot.Links(link).vc do
	11	F , rp = estimate F (ind, Robot);
	12	pick top n clusters c top with min rp;
	13	for each c top do
	14	for each Robot.Links(link).c(c top).v do
	15	F , rp = estimate F (ind, Robot);
	16	p l = f ind(min(rp));
	17	F l = F (p l);
	18	check for joint sensor axis intersection;
	21 Function localization_robot(Robot):
		Input : Structure Robot, collision point on the previous
		timestamp p prev
		Output: Collision point p c, external force F ext
	22	if isEmpty(p prev) then
	23	isolate last link with nonzero torque;

5 τt = J T F ; 6 rp = τextτt ; 19 O = link + 1-th joint axis; 20 d = dist(F , O);

TABLE III LOCALIZATION

 III ALGORITHMS RESULTS SIMULATED AND REAL ROBOT KUKA IIWA 14.

	Mesh parameters	Accuracy, cm Sim. Real	ms	Loop run-time Hz IF*
	All points	2.32	2.17	75	13.3	-
	Cluster d size = 100	3.19	3.04	1.8	555.6	41.6
	Cluster d size = 50	2.56	2.34	1.6	625	46.9
	Cluster d size = 25	2.48	2.33	1.5	666.7	50
	Cluster d size = 10	2.51	2.31	1.7	588.2	44.1
				*Improvement Factor
		TABLE IV
	COMPARISON BETWEEN PROPOSED AND EXISTING LOCALIZATION
		ALGORITHMS.

The authors are with Center for Technologies in Robotics and Mechatronics Components and Institute of Robotics and Computer Vision, Innopolis University, Innopolis, Russia {d.popov, a.klimchik}@innopolis.

ru[START_REF] De Luca | Sensorless robot collision detection and hybrid force/motion control[END_REF] The authors are with IMT-Atlantique Nantes, Nantes, France Laboratoire des Sciences du Numérique de Nantes (LS2N), Nantes, France anatol.pashkevich@imt-atlantique.fr Digital Object Identifier (DOI): see top of this page.

According to the results, our algorithm allows achieving 50x speedup for the localization, without significant loss of accuracy. The model with d size = 100 shows worse performance due to a large cluster that is hard to represent with one reference point, which leads to picking the wrong segments and results in the loss of accuracy. Using a very small cluster d size = 10 requires evaluating more segments and slower due to increased cluster initialization time. The most optimal parameter is a d size = 25 since it gives a good balance between run-time and accuracy.

The accuracy of an algorithm depends on the link with a collision. The last links tend to have a better accuracy due to the larger number of sensors. With d size = 25, estimated accuracy in simulation with N (0, 0.5N m) noise is 0.9, 1.2, 2.2, 4.2 cm for the 7th, 6th, 5th, and 4th link respectively.

The proposed algorithm with d size = 25 was also compared to the existing localization algorithms for a KUKA iiwa and simulated Kinova Jaco 2 (with N (0, 0.5N m) noise for torque measurements) robots in Table IV. None of the existing approaches was able to find more than one possible solution. It should be noted that run-time results are achieved with a different computational power. Compared to algorithms based on a particle filter [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF], [START_REF] Zwiener | ARMCL -ARM Contact point Localization via Monte Carlo Localization[END_REF], our approach shows better runtime frequency since we do not spend time on resampling and projecting points on surface steps. The absence of a numerical optimization procedure inside like Direct in [START_REF] Popov | Collision detection, localization & classification for industrial robots with joint torque sensors[END_REF], [START_REF] Zwiener | Contact point localization for articulated manipulators with proprioceptive sensors and machine learning[END_REF], or QP in [START_REF] Popov | Real-time external contact force estimation and localization for collaborative robot[END_REF], [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF], [START_REF] Zwiener | ARMCL -ARM Contact point Localization via Monte Carlo Localization[END_REF] also helps to improve run-time.

VI. CONCLUSION

In this work, we propose an algorithm for contact point localization on the surface of the robot. The algorithm is capable to detect more than one possible solution for the cases where external force produces zero torque on a joint. The estimation is based on the current robot state, using 1D joints torque sensors values, and a preprocessed robot mesh. The mesh preprocessing includes isotropic remeshing and a two-stage clustering to achieve a hierarchical structure. By exploiting this structure we achieve a real-time execution of our algorithm on frequencies more than 600 Hz. The approach was tested in the simulation on Kinova Jaco 2 and KUKA iiwa robot with an average accuracy of 1.8 and 2.5 cm. For the real KUKA iiwa robot, the average accuracy of a solution is 2.3 cm.


Dmitry Popov

Stanislav Mikhel

Rauf Yagfarov

Alexandr Klimchik

Anatol Pashkevich

Multi-Scenario Contacts Handling for Collaborative Robots Applications

The goal of this work is to propose a way of dealing with physical interactions for collaborative robots that will ensure the safety of a human operator and improve the performance of a common task by implementing multiple robot behavior scenarios. In this scope, all collisions of a robotic arm are detected and analyzed to chooses an appropriate reaction strategy. The points of contact on the robot's surface for each collision are estimated, the external forces are identified and collisions are classified by the set of predefined categories. Based on these categories and the current robot state, the algorithm chose an appropriate behavior scenario.

All presented algorithms are based only on proprioceptive sensors information and were tested in a simulated environment and on the real collaborative robots KUKA iiwa and Universal Robots UR10e. The result for contact localization showed 4 cm mean accuracy, the classification algorithm was able to identify collisions with hard and soft objects with 98% accuracy for KUKA iiwa 14.

I. INTRODUCTION

Human-robot collaboration is most important in production lines with large variety of products when the production operations require human-specific agility and skills. In this case a robot and a human work on the same task in the shared environment. By combining human intelligence and agility with robot strength, accuracy and robustness it is possible to improve the performance of this work cell. On the one hand, the robot will help the human with physically demanding tasks, on the other hand, the robot will have to work in a complex dynamic environment. This will increase requirements for robot control to predict human intentions, actions, and provide necessary assistance. During this interaction physical contacts between a robot and a human are unavoidable and should be taken into consideration [START_REF] Santis | An atlas of physical human-robot interaction[END_REF].

Human safety is the number one priority in humanrobot collaboration and that is why the majority of existing solutions are mainly concentrated around the safety aspect where the robot slows down, stops in the presence of a human, or turns on compliance mode. But physical contact is not only a negative event, it could be used to improve performance by enabling safe physical interaction. Besides, physical interaction is one of the simplest and most intuitive interaction methods even between humans, so it allows us to enable additional control or communication channel with a robot. To achieve this goal, the robot needs to properly 1 Center for Technologies in Robotics and Mechatronics Components, Innopolis University, Innopolis, Russia {d.popov, s.mikhel, r.yagfarov, a.klimchik}@innopolis.ru 2 IMT Atlantique Nantes, Nantes, France and Laboratoire des Sciences du Numérique de Nantes (LS2N), Nantes, France anatol.pashkevich@imt-atlantique.fr handle all collision events which could include contact with a human, environment, another robot, workpiece, etc.

In this work, the methodology for multi-scenario contact handling is proposed. It consists of several steps combined into one pipeline and allows to easily implement safe and complex collaboration scenarios. These steps are: (i) collision detection is done by external torque estimation; (ii) collision localization, where the exact point of contact in the global or local coordinate frame is estimated; (iii) contact event classification using the set of predefined characteristics; (iv) information about the collision class and location is analyzed to activate one of the reaction policies.

The contributions of our work are the following: 1) We introduced the methodology to handle a physical interaction between robot-human/obstacle/task by utilizing different scenarios and dynamical switching between them.

2) The proposed approach covers all stages of a contact: from initial detection to classification and robot reaction to it. Most of the existing works consider only one aspect of interaction: contact detection, localization or classification. 3) We presented a modified version of the contact particle filter for contact localization and compared its performance with existing methods in simulation and on the real KUKA iiwa 14 robot. 4) We proposed a classification topology, which distinguishes contacts with soft and hard objects based on a deep neural network. 5) We demonstrated the reaction module, implemented as a finite state machine. This module chose an appropriate reaction according to the current state of the robot and the type of collision. Together the developed algorithms could not only improve the safety of a human in a robot workspace but also increase the performance of the work cell by activating different scenarios of collaboration.

II. RELATED WORK

Each step in multi-scenario contact handling pipeline is a challenging task itself. Here we give a brief overview of existing works on four main steps of a contact processing: Detection, Localization, Classification and Reaction.

A. Detection

The collision detection gives a binary answer to the question "does the robot have a collision with an external object?". Contact points can be located anywhere on the robot surface, from the base to the end-effector or tool. The main demands for this step are response time (from the moment when collision event happens to the moment when software sets the collision flag), which should be as short as possible, and detection precision that minimizes the number of false-positive classifications. Since it is a binary problem, most of the methods for collision detection use comparison of the input signals with some threshold. The simplest way is to monitor changes in motor current or torque values [START_REF] Harden | On the use of joint torque sensors for collision detection in a confined environment[END_REF], [START_REF] Takakura | An approach to collision detection and recovery motion in industrial robot[END_REF]. These methods do not require a robot model. Another approach is based on the nominal robot model and comparing the model to the actual joint torques. This technique is similar to direct torque estimation or inverse dynamics approach [START_REF] Haddadin | Collision detection and reaction: A contribution to safe physical humanrobot interaction[END_REF]. More complicated algorithms allow not only to detect a collision but also to give an estimation of external torque (external torque here is referring to the part of the joint torque which was caused by the external force in the contact event), they are based on disturbance observers.

Currently, the momentum observer proposed by De Luca [START_REF] De Luca | Sensorless robot collision detection and hybrid force/motion control[END_REF] is one of the most popular solutions for estimation of external load, not only for robotics arm but also for other kinds of robotic systems like drones [START_REF] Tomić | External wrench estimation, collision detection, and reflex reaction for flying robots[END_REF]. The momentum observer does not require inertia matrix inversion and joint acceleration values, but at the same time provides a firstorder filtered version of the external torques and, as a result, has asymptotic convergence for the constant external load only. A survey on the detection and estimation methods is presented in [START_REF] Haddadin | Robot collisions: A survey on detection, isolation, and identification[END_REF]. Collision detection based on deep neural networks was proposed in [START_REF] Heo | Collision detection for industrial collaborative robots: A deep learning approach[END_REF], which showed a reliable detection for cyclic operations.

B. Localization

When the fact of the robot collision with an external object is detected, the exact location of the collision area should be identified. Here we consider methods based on proprioceptive sensors only, in most cases, these are 1D joint torque sensors. Knowledge about the collision point is important in the scope of choosing the right reaction policy later. One of the first works in this area considered a planar 3 DoF robot and identified a collision link surface [START_REF] Ralph | Detection and localization of unmodeled manipulator collisions[END_REF]. The result of their algorithm depended on simplified robot shape and observed disturbance torques. Buondonno and De Luca in [START_REF] Buondonno | Combining real and virtual sensors for measuring interaction forces and moments acting on a robot[END_REF] studied external force isolation using a force sensor mounted at the robot base, which gave a reliable solution at any point of the robot no matter how close contact is to the base.

In [START_REF] Likar | External joint torque-based estimation of contact information[END_REF] the authors analyzed the localization of the collision point placed on the internal axis of the robot links. Such a method reduced the dimensionality of the problem and allowed to use global optimization by only two constrained parameters. Later this method was used in [START_REF] Popov | Collision detection, localization & classification for industrial robots with joint torque sensors[END_REF], [START_REF] Zwiener | Contact point localization for articulated manipulators with proprioceptive sensors and machine learning[END_REF] and compared with machine learning approaches.

Manuelli and Tedrake [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF] suggested using a particle filter for collision localization. In this method, each particle includes information about its location on the surface of the link, and weight is considered as a probability of contact at this point. Probability is estimated with the assumption that external force is applied in the friction cone. After this publication, several other research groups investigated similar methods. In [START_REF] Bimbo | Collision detection and isolation on a robot using joint torque sensing[END_REF] the motion model of particles was modified with a combination of small self-movements of the robot to increase accuracy. Monte Carlo localization was used in [START_REF] Zwiener | ARMCL -ARM Contact point Localization via Monte Carlo Localization[END_REF] and compared to direct optimization and machine learning methods. In both works, the authors examined forces acting to the normal of the robot surface constrained with a friction cone, which allowed to create more particles and decrease the computational load. In one of our previous works [START_REF] Popov | Real-time external contact force estimation and localization for collaborative robot[END_REF] the contact isolation was done using two-step optimization on the mesh surface of the robot KUKA iiwa 14.

It should be noted that collision localization can be also done using external sensors. Tactile artificial skin makes the problem trivial [START_REF] Jain | Reaching in clutter with whole-arm tactile sensing[END_REF], [START_REF] Chavez | Contact force and joint torque estimation using skin[END_REF] and solves detection and localization tasks at the same time, but it is expensive and requires additional calibration for the robot. Vision sensors such as a camera or RGB-D sensor Kinect [START_REF] Magrini | Estimation of contact forces using a virtual force sensor[END_REF], [START_REF] Magrini | Human-robot coexistence and interaction in open industrial cells[END_REF] on the one hand, can also help, especially in cases of multiple collisions and safe human-robot coexistence, where robot can avoid unnecessary collisions [START_REF] De Luca | Integrated control for phri: Collision avoidance, detection, reaction and collaboration[END_REF]. On the other hand, vision systems depend on the field of view and lighting conditions.

C. Classification and Reaction

By evaluating the characteristics of the collision it is possible to attribute it to one of the predefined classes. This allows us to understand the context of the contact event and make a decision for the reaction. In [START_REF] Golz | Using tactile sensation for learning contact knowledge: Discriminate collision from physical interaction[END_REF], [START_REF] Kouris | A frequency domain approach for contact type distinction in human-robot collaboration[END_REF] contacts with a human are divided into intentional and unintentional. In another work contacts are classified as collision with a soft object (human) and a hard (workpiece, another robot, walls) object, and duration was either single or continuous [START_REF] Popov | Collision detection, localization & classification for industrial robots with joint torque sensors[END_REF]. Parusel at el. [START_REF] Parusel | Modular state-based behavior control for safe human-robot interaction: A lightweight control architecture for a lightweight robot[END_REF] designed a control architecture for realizing human-friendly behaviors and intuitive state-based programming with four control modes. The first mode is autonomous in case of human absence, the second mode is compliance with human presence, the third is a collaborative mode with a human in the loop and the last mode is the case of a fault. Magrini and De Luca in their work [START_REF] Magrini | Human-robot coexistence and contact handling with redundant robots[END_REF] implemented a finite state machine with 3 basic robot states: idle state, null space redundancy state and high compliance mode. In our previous works [START_REF] Mikhel | Collision driven multi scenario approach for human collaboration with industrial robot[END_REF], [START_REF] Mikhel | Development of typical collision reactions in combination with algorithms for external impacts identification[END_REF] a similar finite state machine was proposed for switching between six different scenarios of robot behavior.

III. METHODS

Now we describe the implemented methods that were used in multi-scenario contacts handling algorithm. The detection part relies on two already well-known approaches: momentum observer [START_REF] De Luca | Sensorless robot collision detection and hybrid force/motion control[END_REF] and disturbance observer [START_REF] Mohammadi | Nonlinear disturbance observer design for robotic manipulators[END_REF]. In the localization part, we propose to use a modified contact particle filter [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF], in contrast to which we change a robot surface representation and the motion model to reduce computation time. For the classification, the deep neural network is used to classify "hard" and "soft" collisions. The finite state machine is used for the reaction part of our pipeline. Here states of finite state machine represent all possible reactions, transitions between these states are defined by the collision events and their characteristics.

A. Detection Algorithms

Different techniques allow to identify collisions based on proprioceptive sensors only, without additional external sensors. The general idea behind them is to compare measured data with the predicted robot state. The predicted state relies on the robot dynamic model. Thus, if the dynamic properties of the robot are identified then the prediction of the current parameters could be obtained.

The disturbance observation technique is usually based on the assumption that the perturbation rate of some parameter is proportional to the current difference between the perturbed and estimated values. It allows us to estimate the disturbance by integration. The parameters can be torques, moments, velocities, or some others depending on the observer algorithm. The choice is usually determined by the desire to simplify calculation or reduce errors. In particular, it is the common approach to exclude joint acceleration because its calculation is based on numerical differentiation and decreases accuracy.

Three external torque observers were implemented. The first one is a momentum observer [START_REF] De Luca | Sensorless robot collision detection and hybrid force/motion control[END_REF]. Authors use rate of change in momentum in order to estimate residual torque. Authors of the second method, disturbance observer [START_REF] Mohammadi | Nonlinear disturbance observer design for robotic manipulators[END_REF], eliminate acceleration from equations with the help of special matrix L(q, q), where q, q are the vectors of joint angles and velocities. The third observer is a modification of the momentum observer with a sliding mode technique that should deal with a wider class of disturbances [START_REF] Garofalo | Sliding mode momentum observers for estimation of external torques and joint acceleration[END_REF].

B. Localization Algorithm

Robot Surface Representation: Although the surface of the robot is often approximated with primitive shapes like cylinders, for the collision isolation task it will produce a poor result, especially with a robot of a complex shape like KUKA iiwa. To overcome this, we propose to use a modified and extracted surface of the robot links from its .STL model. The robot .STL models are widely used for visualization purposes, but their accuracy is well enough for the localization task. The initial model, imported from the .STL file, presented in Fig. 1a. It has an internal area of the link and some surfaces, unavailable during physical interaction with a robot. Also, it is more convenient and computationally easier to work with a finite number of points, sampled on the robot surface, than to find a point on the facets of the link. Of course, the points should be equally distributed and distance between these sampled points should be less than desired accuracy. The modified mesh of the robot link is shown in Fig. 1b and obtained by removing unreachable regions and isotropically resampling mesh.

The remeshed link could be represented in the form of the graph where the vertex is a node (N ) of the graph and faces are edges (E) as G = (N , E). Each node N i = p i , n i , nb c includes information about its position in a joint coordinate frame p i , n c is a normal vector in this

where J p is a Jacobian for a collision point p, r is an estimation of external torque, F p is an external force, applied to the contact point.

It should be noted that here we assume only external forces, without external torques. The use of only force vector applied to the robot surface will make identification possible for links after 3rd in the case of 1D joint torque sensors. To make a result of this optimization more physically correct, additional constraints are needed.

The first constraint assumes that contact point p ∈ C, where C is a surface of the robot.

The second assumption is that F p lies inside a friction cone with friction coefficient µ, defined by the normal vector in point p. This constraint could be approximated with a polyhedron for computational advantages:

F = n i=1 a i f f c,i , min a≥0 r -J T f c,a a (2)
Here J f c,a = J f c f f c1 ... f f cn is Jacobian m × n for each support vector f f c in contact point, m is a number of joints, n is an approximation degree, a is a support vector weights n × 1. Since r T r is a constant, and friction cone is a convex set, it is sufficient to formulate it as a quadratic program:

QP = min a≥0 a T Ha + g T a (3)
where

H = J f c,a J T f c,a is a square of Jacobian, g = -2J f c,a r.
The minimal value of QP is zero, which corresponds to complete coincidence, and the maximum value depends on the model error.

The exact collision location is estimated using particle filter [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF], which in contrast to the original work operates on the robot surface graph G. This allows avoiding the computational complexity of a motion model update, where new positions of the particles are projected on the surface of the robot by finding ray-mesh intersections.

To describe the proposed particle filter motion and measurement model are presented.

Motion Model: The motion model is used for updating the particle location. Due to the unpredictable behavior of the interaction point on the robot surface, it is not possible to define a model with good generalization. For example, assuming contact point motion on the surface will have good results when a human grabs the robot. At the same time, robot collision with a wall will benefit from a model with a contact point in the base frame. For our particle filter, we use a random walk policy as the motion model, which will generate reasonable results for most cases.

The random walk policy on the graph means choosing a random neighbor node to the current node iteratively. The number of iterations is defined by a random integer between 0 to max steps variable. This variable depends on the weight of the current particle, so the particle with a high weight will have a lower maximum walk distance.

Measurement Model: The measurement step applies weights to the particles. The weight of each particle is equal to the residual between the measured external torque and recreated torque in this particle. For further analysis, it is useful to map the result into the interval from 0 to 1:

w ∼ exp(-α QP ) (4)
where α is scaling coefficient, normalization constant is omitted here.

Multi-collision Cases:

To extend the approach to a multiple collision identification, an additional particle set should be added similarly as it was described in [START_REF] Manuelli | Localizing external contact using proprioceptive sensors: The contact particle filter[END_REF]. In this case, each particle set will have its own solution with a contact point, force in this point, and a generated torque. The sum of generated torques by all particle sets will be equal to total external torque.

Unfortunately for collaborative robots with a 1D torque sensor per joint, it's hard to detect more than 2 collisions in practice. The first problem is that the resulting torque from all collisions has no unique solution and could be described as a single collision. It can be partly avoided by assuming that collisions happen sequentially. The second problem is an amplitude of collision impact: with a large difference in amplitudes, one of the collisions could be identified as noise for a significantly larger second collision after torque normalization. To improve identifiability, we made an additional assumption that one of the collisions is placed at the end-effector of the robot. Here end-effector payload is not modeled in the robot dynamics. If a robot performing a contact operation like pick and place, drilling, and so on, there is one collision associated with a task at the endeffector and a second collision in the intermediate point of the robot in case of a collision with the environment/human is possible. This leads to set up with particle filter with one set and one fixed particle from a second set for representing collision in intermediate point and end-effector of the robot.

C. Collision Classification

Depending on the context of a collision event, the outcome will be different. Here we try to understand the context of a collision based only on the robot's internal sensors. The classification starts when a collision is detected. Currently, we defined 6 criteria for the collision event, but it could be easily extended. The classification diagram is presented in Fig. 2.

The first criterion is the nature of a colliding object, we divide all possible variants into hard objects (could be a wall, workpiece, etc.) and soft objects (human in most cases). The object nature could be classified by analyzing the time series of joints torques and positions. Since we use only the internal sensors of the robot, it is not possible to reliably distinguish contact with humans and contact with cushioned furniture, for example. Thus, we have to assume all soft contact as contacts with a human. In the case of industrial applications, soft and hard contacts usually correspond to the operator and the robot environment. Consequently, if a collision happens with a human, we can define if it was intentional as a part of a human-robot collaboration or the collision was incidental.

Every collision could be purposeful or accidental. The collision is marked as purposeful if it is expected at the current stage of the robot program, otherwise, it is accidental. If the process is non-contact, the collision in most cases would be accidental, but if we expect collaborative work, conditions of the purposeful interaction could be defined.

The duration of the contact event defines if it is single or continuous. When the contact time is longer than a certain threshold value, the collision is continuous, otherwise, it is single and can be caused by an accidental human touch, for example.

By the contact location extracted from the localization algorithm, we can mark this collision as static or dynamic. Depending on the chosen coordinate frame, collision is static when coordinates of the contact point are fixed with relation to frame and dynamic if coordinates are changing in time.

The idea behind that is to separate dynamic objects such as a human or another robot with static immovable objects like walls in the robot workspace.

The accidental/purposeful class is given on the stage of robot programming and depends on a specific task. Instead, single/continuous, soft/hard, and static/dynamic classes are estimated online from the properties of the collision. Particularly the soft/hard class identification done by the deep neural network is shown in Fig. 3.

The input for the network consists of external torque values for a fixed time window. In the case of multiple collisions, only part of the total torque that was generated by this collision is used. The use of velocities in the case of a single collision could help with classification, but in practice, it decreases the accuracy of the model when used for a second collision. The output of our network is a class of collision: soft or hard. The classification works only if at least one collision is detected.

A deep neural network for collision classification consists of 3 identical sequential Residual blocks. Each Residual block has two paths for input data. The first path consists of sequential 1D convolution, batch normalization, and ReLU activation layers. The group of these three blocks is repeated 3 times and connected sequentially. The second path includes one 1D convolution layer. Next, both paths are connected using the addition operation. After 3 Residual blocks, 1D global average pooling is performed and then the network has the last feedforward layer with a softmax activation function.

Data from proprioceptive sensors is not enough to estimate the context and intention of the collision events. This implies the use of external sensors like a camera or RGB-D with classification algorithms based on deep convolutional networks, image processing, and that is out of the scope of this work.

D. Reactions

The main goal for the collaborative robot is to be safe for humans, the environment, and themselves, and simultaneously execute the task most efficiently. To achieve this goal the robot should have a strategy for every possible collision and a switch that will turn this strategy on and off. Here, we propose to use a finite state machine.

In the scope of our methodology, states of the finite state machine are reaction strategies, contact events are transitions and contact properties are transition conditions. The end-user can define states and transitions between them according to the desired robot behavior. Each state includes several robot actions, each transition has a condition on collision classes.

The following example shows the possibilities of our approach. Consider a collaborative work cell, where a robot has the task of tightening screws. The end-effector of the robot is equipped with a camera for screw detection and a gripper tool. The finite state machine for this example is presented in Fig. 4. The robot has 2 main tasks. The first task executes non-contact operation ("Normal NonContact" state), for example, visual detection of screws in a workpiece. The second task is contact operation ("Normal Contact" state), for example tightening of a screw with a tool.

The basic reaction for all accidental collisions ("A") is defined by the safety policy and forces the robot to stop. There are two kinds of stop states in our reactions: the touch reaction and the stop reaction. The stop reaction is basically an emergency stop command to prevent harm to a human or an environment. It also used a transition block to other reactions. The touch reaction activates only if the contact is with a human. This state is used to stop execution for a short period. The manual control can be implemented as reactions to touches: the first touch means the robot stop, the second one is a resume. It can be used to change the workpiece by a human, or correct the workpiece position. If contact is persisting and becomes continuous, the touch reaction becomes a stop reaction, and then for a soft ("S") contact in the robot end-effector switches to the compliant mode. In the compliant mode, the position and orientation of the robot can be changed manually. The robot could be guided by a human to the next screw position if it was unrecognized by the vision system. Also if there was accidental contact with a human, firstly robot will stop, then, if the collision still exists, become compliant. In this way, we try to limit interaction force for safety.

In the case where something touches the robot in the intermediate point or "elbow" ("EL"), the robot enables elbow reaction. In this mode, the robot will try to use its kinematic redundancy to avoid the collision, while following the desired trajectory by the end-effector. If the end-effector ("EE") itself has collided with a hard object ("H") during the non-contact operation, we also could try to avoid this obstacle. In our example, the robot could have a collision between its tool and a workpiece/environment during the inspection phase. To implement this, we used a potential field approach, where a point with a collision in the base frame was added as a new point with high potential.

The states and transitions in the finite state machine presented in this section could be extended or altered to better describe robot behavior during multi-scenario contact operations.

IV. EXPERIMENTS AND RESULTS

A. Hardware and Datasets

For the experimental study, two collaborative robots were used: KUKA iiwa 14 and Universal Robots UR 10e. The obtained results were tested in simulation and hardware.

We used two separate datasets, one for classification and the other for localization. The dataset for classification consists of 430000 frames with a 100Hz rate, with a total of 300 random trajectories and 500 collision events. It includes the collision data with hard dynamic objects, hard static objects, and three different persons. The contacts with the humans occurred not only in a collaborative way, where a human guided the robot, but also in an accidental way, where the robot unintentionally collided with a human's arm, back, or body.

The second dataset for localization consists of 100 random robot configurations for 22 contact points, 2200 samples in total. For the real robot, only 10 test configurations were used to verify our results. All points of contact were located in the last four links where a random force from a friction cone was applied. Contact point location was estimated with the ATOS 5 measurement system with less than millimeter accuracy, all 22 contact points were marked on the real robot and its model in simulation as shown in Fig. 5. During the experiment on the real robot, the operator applied external force for the marked point with a known location on the robot surface. The ground truth for force vector amplitude or direction is not provided, but in the future, we plan to use a special tool with a force sensor.

The developed algorithm was tested on Intel Core i5-4210H 3GHz CPU, 8Gb RAM PC as a Matlab 2-thread program.

B. Contact Detection Results

We have tested three algorithms on the UR10e robot: momentum observer, disturbance observer, and sliding mode momentum observers. This robot is not equipped with torque sensors in each joint but allows to measure motor currents. After that, the current-to-torque matrix can be used to estimate the load in motors. Each observer has its parameters for configuration. We used the Fmincon optimization technique to find the optimal values. More information about dynamic parameters identification and observers can be found in [START_REF] Mamedov | Practical aspects of model-based collision detection[END_REF].

During the experiment, we pushed the robot in different positions and measured angles, velocities, and currents. The result of external force estimation is shown in Fig. 6.

In general, all observers have demonstrated similar output. In terms of the detection speed, the results of the momentum and the disturbance observers are slightly better than the sliding mode. From a practical point of view, the disturbance observer algorithm is more compact for implementation but requires the inversion of the robot inertia matrix and configuration of three values regardless of the joint number. Momentum observer does not use matrix inversion but requires a transposed version of the matrix with centripetal and Coriolis terms. The amount of parameters for configuration is equal to the joint number. Thus the algorithms are almost equal and the choice could depend on secondary factors.

To estimate part of torque that was generated only by the external force for KUKA iiwa and UR10, we used

C. Contact Localization Results

Four localization algorithms were chosen to compare their performance. To make a fair comparison, the single contact situation is considered. The first algorithm is a proposed modification of particle filter on the graph (PFG) with 50 particles, the second one is a two-step optimization (TS) algorithm from [START_REF] Popov | Real-time external contact force estimation and localization for collaborative robot[END_REF], the third one is a one-layer feed-forward neural network with 100 neurons in a hidden layer (NN) and the fourth one is Direct-based optimization (DR) from [START_REF] Likar | External joint torque-based estimation of contact information[END_REF]. The first two approaches use information about the robot surface and find a force inside a friction cone, the rest two approaches use a cylindrical approximation of a robot shape and assume an external force normal to the link.

The results of the simulations and tests on the real hardware are presented in Table I. Without noise in torque measurements, PF and TS algorithms gave almost zero error in position and external force. With added noise, these algorithms showed 4-5 cm mean error in position estimation and 16-17 % error in external force. With the real robot, PF gave slightly better localization than a TS but had a slower update loop, 60 Hz vs. 90 Hz. In practice, a larger mean error of TS can be explained by obtaining a local minimum in the wrong link.

Two other approaches, NN and DR showed more than 10 cm mean error even without noise. It could be explained by the complex shape of a robot surface, which these algorithms neglect. The force estimation has a large error due to the assumption of force acting on the normal. The machine learning-based approach is the fastest of the studied methods and can be used when the precise location of contact is not very important. NN can also be used for the classification of a collision point from a set of predefined locations.

The examples of PFG estimation for real KUKA iiwa and UR10e robots are presented in Fig. 7 (a-d). The multicollision case is shown in Fig. 7 (e), here the first external force is caused by the constant gravity force applied to the end-effector, and the second force applied by a human.

D. Contact Classification Results

The classification of hard/soft contacts is performed using deep neural networks. To train the network for collision type classification, the previously mentioned dataset was cut into samples with a window of 1 second with overlap. Each sample has a size of 100 by 14, where 100 is the window size, and 14 is the number of values with torques and positions for robot joints. After the partition, an undersampling technique was used to balance data by classes, resulting in 3300 samples of each class in the dataset. The neural network was trained using the backpropagation algorithm with Adam optimizer. Binary cross-entropy was used as a loss function. The trained neural network showed an accuracy of 98.8% on the test data compared to 89.5% from a previous frequency based approach [START_REF] Popov | Collision detection, localization & classification for industrial robots with joint torque sensors[END_REF]. In practice, the algorithm was able to

  Fig.1. KUKA iiwa link mesh representation. In (a) mesh has unreachable for contact areas. In mesh (b) those areas were removed and the mesh surface was isotropically resampled.

Fig. 2 .

 2 Fig.2. Collision classes. Collision duration, nature, and type classes could be obtained using only internal sensors of the robot. By using an external vision system it is also possible to extract the intention and context of interaction. The possibility of a planned contact could be predefined for each step of the robot program.

Fig. 3 .

 3 Fig. 3. Structure of proposed DNN for soft/hard collision classification. As input, a fixed time window for the contact external torques is used. The class of the collision is output.

Fig. 4 .

 4 Fig. 4. Proposed finite state machine with states and transitions. Collision by location: in end-effector (EE), in elbow (EL); by nature: soft (S), hard (H); by current task: purposeful (P ), accidental (A).

Fig. 5 .

 5 Fig. 5. Real and simulated KUKA iiwa with ATOS5 markers. By measuring markers coordinate is a robot base frame, it is possible to match markers position in the real robot and its virtual model, to obtain ground truth for localization.

Fig. 6 .

 6 Fig. 6. Force estimation with different types of observers for UR10 robot during the contact with a human.

Fig. 7 .

 7 Fig. 7. Localization algorithm for KUKA iiwa and UR10e. Probability at all points is estimated for visualization. The green dot is a collision location, the red arrow is the estimated contact location and force direction.

TABLE I LOCALIZATION

 I ALGORITHMS RESULTS FOR SIMULATED AND REAL ROBOT KUKA IIWA 14.

			Simulated results			Real experiment
	Noise for torque measurements	0		N (0, 0.5N m)		*
	Method for collision localization	error ∆d, m Average std.	error ∆d, m Average std.	error ∆d, m Average std.	Run-time, ms Average std.
	Proposed Particle Filter on Graph (PFG)	0	0.01	0.04	0.06	0.04	0.05	17	3
	Two-step Optimization (TS)	0	0.05	0.05	0.08	0.06	0.09	10	5
	Feed-forward NN (NN)	0.05	0.09	0.08	0.13	0.09	0.08	5	2
	Direct Optimization (DR)	0.05	0.11	0.08	0.11	0.07	0.09	52	6
	moment observer. It provides slightly better time response
	characteristic compared to the two other methods.