Real Time Collision Detection and Identification for Robotic Manipulators  Elena Galbally and Mikael Jorda  Abstract — The majority of everyday tasks involve interacting with unstructured environments. This implies that, in order for robots to be truly useful they must be able to handle contacts. This paper explores how a particle filter can be used to localize a contact point and estimate the external force. We demonstrate the capability of the particle filter on a simulated 4DoF planar robotic arm, and compare it to a well-established analytical approach.  I. INTRODUCTION For several decades, robots have been able to reliably fol- low precise trajectories making them ideal tools for assembly lines. However, in 2017, we still do not see any complex robot capable of manipulating objects in unstructured envi- ronments such as homes or offices. Robots remain confined to industrial settings. One of the main factors preventing robots from conquering other types of environments is their inability to robustly handle single or multi contact collisions. In fact, as seen for example during the DARPA Robotics Challenge in 2015, contacts lead to robot failures and safety concerns. In order to be able to handle contacts, robots must first gain the ability to detect and identify them. Several solutions have been proposed in the past years. Some approaches involve using tactile skins to detect the contacts [1]. This solution is ideal in theory, however, there are still no commercially available tactile skins that are reliable, and affordable. There- fore, this approach is usually limited to robotic hands which is not enough to handle situations where the arm of the robot touches the environment. Other proposed solutions use Force/Torque sensors pre-positioned on the robot (usually at the wrist or base) [2] [3], but once again this approach is limited by the locations of the force sensors selected during the design phase. A different approach using only proprioceptive sensors was introduced in 1989 [4]. The idea was to compare the applied joint torques to the sensed torques. Disturbance torques due to contact would make these two differ. Using this approach, the sensed free space torques are simply ob- tained from the robot’s dynamic equation. This idea has been refined to get more robust and easy to compute observers [5] (the main advantage being that they do not need accel- eration measurements of the joints anymore). Additionally, it has been extended to mobile manipulators [6], flexible joint robots [7], and humanoids [8]. Recently, people have also addressed the problem using particle filters [9] with promising results. In this paper, we propose to compare the analytical ap- proach described in [10] and the particle filter approach in [9] for the problem of contact detection and localization in the context of controlling a multi DoF planar robot to perform a certain task while in contact with the environment. II. PROBLEM FORMULATION For a robot performing a task in free space or in contact, it can be advantageous to brace on the environment in order to save power and to reduce the uncertainties in the movement. However, once in contact with the environment, the dynamics of the robot change and we need to account for this variation in our control law. An example is shown fig 1, where we would like to know both the exact contact point location between the blue shape and the robot, and the value of the contact force. This procedure is comprised of three steps :  •   Collision detection - consists on detecting that a col- lision occurred and identifying the related disturbance torques.  •   Collision isolation - involves finding the exact location of the collision.  •   Collision identification - requires quantifying the force at the collision.  Fig. 1.   Planar robot performing a task at its end effector (red circle) while bracing on the environment (blue circle).  A. MOMENTUM OBSERVER FOR COLLISION DETEC- TION  Drawing inspiration from [10], we will use a momentum based observer for the collision detection step. Let us review this approach here. The dynamics equation of an articulated rigid body manipulator is:  M   ( q ) ̈ q   +   C ( q,    ̇ q )  ̇ q   +   g ( q ) =   τ m   +   τ c   (1) where   M   ( q )   is the robot mass matrix,   C ( q,    ̇ q )  ̇ q   is the centrifugal and Coriolis vector, factorized with the matrix   C  arXiv:1802.00546v1 [cs.RO] 2 Feb 2018
of Christoffel’s symbols,   g ( q )   is the vector of joint torques,  τ c   is the vector of contact torques (torques on the joints due to contacts with the environment), and   τ m   is the vector of motor torques. In the following, we will systematically omit the dependencies on   q   when writing these terms so  M   ( q ) =   M   ,   C ( q,    ̇ q ) =   C , and   g ( q ) =   g . The generalized momentum of the robot is given by:  p   =   M    ̇ q   (2) Therefore,   ̇ p   =    ̇ M    ̇ q   +   M    ̈ q.   (3) Combining this expression with the dynamics equation yields:   ̇ p   =   τ m   +   τ c   −   g   −   C    ̇ q   +    ̇ M    ̇ q   (4) A basic property of the robot mass matrix is the skew- symmetry of    ̇ M   −   2 C   which is equivalent to the equation   ̇ M   =   C   +   C T   . Hence, we have:   ̇ p   =   τ m   +   τ c   −   g   +   C T    ̇ q   (5) Let us define the residual vector   γ   as:  γ   =   K [ p   +  ∫  ( g   −   C T    ̇ q   −   τ m   −   γ ) dt ]   (6) From this definition, it follows that:   ̇ γ   =   K ( τ c   −   γ ) .   (7) With diagonal   K >   0 , this is a stable, linear, decoupled, first-order estimation of the contact torques   τ c , and if   K   is sufficiently large, we can assume   γ   ≈   τ c . When the robot moves in free space, all the coefficients of   γ   will be close to zero. When a collision occurs, all the coefficients of   γ   corresponding to the joints that come before the collision point will become non-zero. Computing   γ   is a good way to get three key pieces of information :  •   Whether a collision is happening  •   At which link the collision is happening  •   What are the corresponding torques at the joints Now, we will present two different ways of performing the collision isolation and identification that make use of this momentum observer.  B. COLLISION IDENTIFICATION AND ISOLATION : ANALYTICAL APPROACH  We assume a single contact point. This implies that there is a contact force   F c , but no moment. Let   J c   be the Jacobian matrix at the contact point, then we know   γ   =   J T c   F c . The problem is that we don’t know neither   J c   (it depends on the exact contact point) nor   F c . Nevertheless, we can compute the equivalent force and moment at any point where we do know the value of jacobian, such as the joint (see fig 2). If we choose the base link, then there is a force   F i   and a moment   M i   such that:  γ   =   J T i  [   F i  M i  ]  (8) and we have the relations  F i   =   F c   (9)  M i   =   r c   ×   F c   (10) If we can invert equation 8 to get   F i   and   M i , then we will know both the force applied at the contact point and the line of action (which can be extracted from   M i ). Knowing the line of action and the geometry of the link, we get two possible contact points. Finally, the constraint that the contact force can only push the robot away from the obstacle allows us to chose between these two points. In general, this is only possible if   J T i   has more than 3 rows, which means that we need at least 3 proprioceptive sensors between the contact point and the base of the robot. This method has the advantage of being easy to implement and fast to compute. However, it is difficult to generalize to multiple contacts, and it cannot identify successfully contacts on the first links of the manipulator.  Fig. 2.   Schematic explaining the analytical solution. We can resolve the contact torques   γ   into force and moment at the joint frame, and then use these values and the link geometry to get the contact position.  C. COLLISION IDENTIFICATION AND ISOLATION : PARTICLE FILTER APPROACH  Another approach is to use a particle filter to locate the contact and estimate the force at that point. In [9], such a filter is used to obtain the contact location only. However, as described below, we can easily adapt this method to also get an estimate of the contact force. Once again, we start from the momentum observer   γ . The idea here is to consider the possible contact locations as our state space, and to use a particle filter without rejection to converge to the true state.  a) Measurement Model:   We want to know how well a particle explains the observed measurement   γ . So we want to know, for each particle location   r i , if there is a contact force   F c   such that   γ   =   J T i   F c   where   J i   is the Jacobian at   r i . Thus, we have the following minimization problem :  min  F c  ‖ γ   −   J T i   F c ‖ 2 2   (11) Subject to   F c   ∈   S ( i )   (12) Where   S ( i )   is the half plane that creates forces that push the robot away from the environment at particle location   i .
This is a quadratic program (QP) with a linear inequality constraint, and there exist many solvers for these sort of problems. The argument of the minimization is the contact force we are looking for. Finally, our measurement model is:  P   ( γ | r i )   ∝   exp( −   1 2   QP   ( γ | r i ))   (13) We   must   normalize   the   expression   above   to   get   the weights.  b) Motion Model:   We will use a simple model where the particles move on the surface of the link with some noise that follows a Gaussian distribution.  r t +1  i   =   r t i   +   αd   (14) Where   d   is the vector that follows the boundary of the link on which   r t i   is, and   α   ∼   N   (0 , σ 2 )   is Gaussian noise with mean zero and variance   σ 2 . The algorithm is outlined below and illustrated in (fig 3).  Algorithm 1   Single Contact Particle Filter  1:   Contact detection :  2:   if   γ t   <  mu   then  3:   X t   =   ∅  4:   return   X ( t )  end if  5: 6:   Sample particles :  7:   if   X t − 1   =   ∅   then  8:   X ′  t   =   X init  9:   else  10:   X ′  t   =   M otionM odel ( X t − 1 )  end if  11: 12:   Resample according to weights :  13:   for   r t i   in   X ′  t   do  14:   w t i   =   P   ( γ | r t i   )  end for  15:   X t   =   Resample ( X ′  t )  16: 17:   return   X t  Where    mu   is the threshold force value above which we consider that there has been a collision,   γ t   is our momentum observer   measurement,   and   X init   is   a   set   of   uniformly distributed samples on the surface of the link (in this case we used 50 particles). As our resampling strategy we used a multimodal approach in which you choose a particle with a probability proportional to its weight by generating a random number [11]. III. EXPERIMENTS IN SIMULATION We compare the performance of the two previous ap- proaches in simulation, using the simulation environment SAI2 developed in the Stanford Robotics Lab. We will test two scenarios in which the robot is controlled to perform a  Fig. 3.   Example of the particle set (in red) from the prior uniform distribution to the estimate of the actual contact point in 4 consecutive timesteps.  Fig. 4.   In green : the trajectory that the end effector follows in the second scenario  task at the end effector while the fourth link of the robot is in contact with the environment. In the first scenario, the task is simply to hold the end effector position. In the second scenario, the task consists on tracking a trajectory (fig 4). To perform these tasks we use the operational space formulation [12], which allows us to directly control the end effector of the robot with a PD controller. For both tasks, the simulation starts with the robot in free space and then falls after a certain time and collides with the blue object (see the attached video). In all of our experiments, we implemented a Butterworth filter of order 2 at 15Hz for both the forces and positions. This allows us to filter out noise and obtain smoother plots. IV. RESULTS AND ANALYSIS  A. Task performance figures  We used continuous lines to represent the real quantities, while the estimated quantities are dashed lines.
0   1   2   3   4   5   6   7   8   9  −40  −30  −20  −10  0  10  20  30  40  50  Force (N)  Hold Position, Analytical approach Force  Actual Force (z)  Estimated Force (z)  Actual Force (y)  Estimated Force (z)  0   1   2   3   4   5   6   7   8   9  time (s)  0.0  0.2  0.4  0.6  0.8  1.0  1.2  1.4  1.6  Position (m)  Contact Position  Actual contact position (z)  Estimated contact position (z)  Actual contact position (y)  Estimated contact position (y) Fig. 5.   Estimated force and real force in the YZ plane for the holding position scenario using the analytical method  As we can see in the plots for both methods and tasks, there is an initial peak in the estimated force values at the moment of the collision. This is expected because the impact causes a discontinuity in the dynamics that the momentum observer is not able to instantaneously account for. Other than this, the estimates seem really close to the true values in both cases, with the force estimate being more accurate than the position (the errors are less than 0.5 N and less than 1 cm in the position).  B. Localization and force estimation  For both tasks, as illustrated in the plots, the methods used are able to accurately locate the contact point and provide a reasonable estimate for   F c . Hence, it is possible to say that they perform equally well in this planar example when the contact occurs on the fourth link (or after). In the analytical method, as mentioned earlier, we need at least 3 proprioceptive sensors between the contact and the base of the robot in order to be able to recover the force and position values. For the particle filter, there is no such theoretical limitation in the equations and we can apply the method to a contact that occurs on the second link for example. When we do so, however, we see the particles randomly move back and forth along the side of the link where the collision happened, and the force changes with 2   3   4   5   6   7   8   9   10  −100  −50  0  50  Force (N)  Following a trajectory, Analytical approach Force  Actual Force (z)  Estimated Force (z)  Actual Force (y)  Estimated Force (z)  2   3   4   5   6   7   8   9   10  time (s)  0.0  0.2  0.4  0.6  0.8  1.0  1.2  1.4  1.6  Position (m)  Contact Position  Actual contact position (z)  Estimated contact position (z)  Actual contact position (y)  Estimated contact position (y)  Fig. 6.   Estimated force and real force in the YZ plane for the following trajectory scenario using the analytical method  the location of the particle. This comes from the fact that that all of those particle locations and force pairs are equally likely to explain the disturbance torques that we observe and there is not enough information to localize the true contact and force simultaneously.  C. Approach limitations  It is worth noting here that the particle filter is guaranteed to provide a particle location that is on the link, whereas the analytical solution could return a line of action that does not intersect the robot and, therefore, lead to a non tractable solution if there is too much noise in the model estimates. Additionally, although it is a hard problem and computationally very expensive, the particle filter can be extended to multiple contacts by using several particle sets. The main disadvantage of the particle filter is that it is computationally much more expensive and takes longer to converge than the analytical approach. The particle filter requires   solving   n   optimization   problems   per   time   step (where   n   is the number of particles), whereas the analytical approach solves only one optimization per time step. Even in this simple example, we had to run the particle filter slower (100Hz) than the controller (1 kHz) and on a separate thread.
0   2   4   6   8   10   12   14  −40  −30  −20  −10  0  10  20  30  40  Force (N)  Hold Position, Particle Filter approach Force  Actual Force (z)  Estimated Force (z)  Actual Force (y)  Estimated Force (z)  0   2   4   6   8   10   12   14  time (s)  0.0  0.2  0.4  0.6  0.8  1.0  1.2  1.4  1.6  Position (m)  Contact Position  Actual contact position (z)  Estimated contact position (z)  Actual contact position (y)  Estimated contact position (y) Fig. 7.   Estimated force and real force in the YZ plane for the holding position scenario using the particle filter method  V. CONCLUSIONS This paper shows two different methods to detect, locate, and estimate the force at a contact point in a planar robotic arm. We use an analytical method as a benchmark for a parti- cle based approach. Both approaches use only proprioceptive sensing. In   simulation,   we   are   able   to   demonstrate   successful localization of a contact point and accurate force estimation for single point collisions during the performance of two different tasks. We showed that both methods work very well, and talked about their advantages and drawbacks. It is worth mentioning that both methods would immediately extend to a non planar robot. As a future work, it would be interesting to test them on a non planar robot, and on real robots. We believe that being able to account for collisions with the environment in real time is crucial for the development of robots that can perform complex tasks in unstructured environments. ACKNOWLEDGMENTS Thank you to Margot’s fairwell mojito for helping boost our creativity. 2   3   4   5   6   7   8   9   10  −100  −50  0  50  Force (N)  Following a trajectory, Particle Filter approach Force  Actual Force (z)  Estimated Force (z)  Actual Force (y)  Estimated Force (z)  2   3   4   5   6   7   8   9   10  time (s)  0.0  0.2  0.4  0.6  0.8  1.0  1.2  1.4  1.6  Position (m)  Contact Position  Actual contact position (z)  Estimated contact position (z)  Actual contact position (y)  Estimated contact position (y)  Fig. 8.   Estimated force and real force in the YZ plane for the following trajectory scenario using the particle filter method  R EFERENCES [1]   Ravinder S. Dahiya, Philipp Mittendorfer, Maurizio Valle, Gordon Cheng, and Vladimir J. Lumelsky. Directions toward effective utiliza- tion of tactile skin: A review.   IEEE Sensors Journal , 13(11):4121– 4138, 2013. [2]   Shujun Lu, Jae H. Chung, and Steven A. Velinsky. Human-robot colli- sion detection and identification based on wrist and base force/torque sensors.   Proceedings - IEEE International Conference on Robotics and Automation , 2005(April):3796–3801, 2005. [3]   Takayoshi Yamada, Tetsuya Mouri, Nobuharu Mimura, Yasuyuki Fu- nahashi, and Hidehiko Yamamoto. Identification of contact conditions by   active   force   sensing.   Studies   in   Computational   Intelligence , 252:275–305, 2009. [4]   Shinji Takakura, Toshiyuki Murakami, and Kouhei Ohnishi.   An Approach to Collision Detection and Recovery Motion in Industrial Robot.   15th Annual Conference of IEEE Industrial Electronics Society , pages 421–426, 1989. [5]   Alessandro De Luca and Raffaella Mattone. Sensorless robot collision detection and hybrid force/motion control.   Proceedings - IEEE Inter- national Conference on Robotics and Automation , 2005(April):999– 1004, 2005. [6]   Kwan Suk Kim, Travis Llado, and Luis Sentis.   Full-body collision detection and reaction with omnidirectional mobile platforms: a step towards safe humanrobot interaction.   Autonomous Robots , 40(2):325– 341, 2016. [7]   Sang-Duck Lee and Jae-Bok Song.   Sensorless collision detection based on friction model for a robot manipulator.   International Journal of Precision Engineering and Manufacturing , 17(1):11–17, 2016. [8]   Jonathan Vorndamme, Moritz Schappler, and Sami Haddadin.   Col- lision Detection, Isolation and Identification for Humanoids.   Icra , (c):4754–4761, 2017.
[9]   Lucas Manuelli and Russ Tedrake. Localizing External Contact Using Proprioceptive Sensors : The Contact Particle Filter. pages 5062–5069, 2016. [10]   Sami Haddadin, Alessandro De Luca, and Alin Albu-sch.   Robot Collisions : A Survey on Detection , Isolation , and Identification. pages 1–21, 2017. [11]   Randal Douc, Olivier Capp ́ e, and Eric Moulines.   Comparison of resampling schemes for particle filtering.   CoRR , abs/cs/0507025, 2005. [12]   Oussama Khatib. A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation.   IEEE Journal on Robotics and Automation , 3(1):43–53, 1987.