Learning Center
Plans & pricing Sign in
Sign Out

Peg-In-Hole with a Blind Robot Contact Identification and Analysis


									  Peg-In-Hole with a Blind Robot : Contact Identification and Analysis
                                 Joseph Salini, Franck Geffard, Olivier David and Olivier Sigaud

   Abstract— We consider the problem of peg-in-hole operation                       developed to find contact information, like force direction,
with a robot. Due to nuclear environment constraints, we                            contact point or contact normal. In our study, we add to
assume that vision is not available, so we want to use “tactile”                    current means a least square optimization to find a better
information to realize this entire operation automatically. We
use a manipulator arm equipped with a wrist force-torque                            force direction, and a Kalman filter to estimate the contact
sensor. The shape of the tool are supposed to be known and we                       normal.
collect force-torque information to find contact characteristics
(force direction, contact point, contact normal).
                                                                                                     II. STATE OF THE ART
                        I. INTRODUCTION
   Our goal is to proceed to a peg-in-hole operation in order                       A. Contact Detection
to initialize a room reference. Our manipulator arm has a                              Moorehead et al. [8] use a flexible arm on a robot to
wrist force-torque sensor and it can change the tool at its end                     detect collisions. They check if the derivate of constraint
effector. During our operation, the robot chooses a special                         signal reach a threshold to identify contact. Shimokura et
tool which can be inserted in the hole. In robotics literature,                     al. [5] use a force-torque sensor and analyse the transition
many articles deal with peg-in-hole operation, and they often                       between free state and contact state. They control inline both
tackle one or two sub problems, and seldom the entire prob-                         force-torque signal and transition duration to validate contact
lem. We manage to classify them in four sub problems: the                           identification. Finally, Dutre et al. [4] use the derivate of the
contact identification problem, which gives the information                          energy signal to detect contact offline. In all these articles,
when the robot arrives in contact with the environment.                             the goal is to know when a signal reaches a threshold.
The search problem, which worries about efficient methods
to find the hole. The contact analysis problem, which
collects information about the contact characteristics, like                        B. Search Method
the force direction, the contact point, the contact normal.                            The second problem is the search method. Chhatpar et
And finally the insertion problem, which brings insertion                            al. [3] give approaches to complete this operation. First, they
methods. Some authors want to resolve the entire problem                            use a spray-paint method to cover a define search space.
and present their results, but they use vision and sound in                         After, they resolve a Traveller Salesman Problem (TSP) on
addition with force-torque information. In this paper, we                           a discrete space. Finally, they propose a search method with
want to complete the entire peg-in-hole operation with the                          concentric circles, or with a spiral. All trajectories would be
only help of the tool shape knowledge and a force-torque                            suitable as it can cover the entire search space.
sensor. In the following sub sections, we treat the 4 sub
problems independently and want to improve the existing
methods. For contact identification, many articles present                           C. Contact Analysis
this sub problem as a signal reaching a threshold (constraint,                         The third problem is the contact analysis. Here, the goal
time, energy). In order to improve this method, we propose                          is to identify contact characteristics, like force direction,
to use several signals, as a signal vector, and a boundary                          point contact, normal contact, etc. Zhou et al. [11] compute
function as our threshold. About search method, we apply                            the point of contact from force-torque information. They
the methods found in the literature on an unknown search                            notice that on contact situation, force is repulsive, and they
plane. About contact analysis, numerous methods have been                           define it mathematically. Chen et al. [2] work on object
   J. Salini studies in the Computer Science Laboratory of Paris 6 University       reconstruction from tactile information. They manage to get
(LIP6), University of Paris 6, 75006 Paris, France                                  the normal of contact to rebuild the object. As they know
   He does his internship in the Teleoperation and Cobotic Laboratory (LTC)         the tool shape, they use the geometric normal, and not the
of the Atomic Energy Commission (CEA), 92265 Fontenay-aux-Roses,
France                                                    force normal. Kitagaki et al. [6] study a pseudo-contact
   Franck Geffard works in Teleoperation and Cobotic Laboratory (LTC) if            point between the force direction and a reference (a plane,
the Atomic Energy Commission (CEA), 92265 Fontenay-aux-Roses, France                a sphere, etc.). This method seems to be more robust than a
   Olivier David works in Teleoperation and Cobotic Laboratory (LTC) of             method which seeks the real contact point because it does not
the Atomic Energy Commission (CEA), 92265 Fontenay-aux-Roses, France                worry about real tool position (it can be slightly shift from its                                                                referenced position). Newman et al. [9] want to define the
  Olivier Sigaud works in the Computer Science Laboratory of Paris 6
University (LIP6) in the ISIR Department, University of Paris 6, 75006              hole axis during a peg-in-hole operation from force-torque
Paris, France                                                information. All of these results are used to analyse contact.

D. Insertion Method                                                    manage to compensate for the tool weight), and each signal
   The last problem is insertion. We can use active methods,           has its own standard deviation (named σF x , σF y , σF z , σM x ,
as Bruyninckx et al. [1]. They develop a method from a                 σM y and σM z ). Threshold functions have been developed.
“three-points contact state”. From this state, they give the           They must return a detection from force-torque information
cinematic to insert the peg. The other methods are passive,            when a contact appears.
as that presented by Sturges et al. [10]. They develop a                  Noises of our sensor are supposed to be Gaussian noises.
Remote Center Compliance (RCC), sort of complex spring,                Thus, we analyse the standard normal distribution. If we look
which adapt passively the peg orientation and position to fit           at its table, we get information on the probability of a value to
the hole during insertion.                                             appear in a signal. We can use this information. If there is no
   Notice the paper of Milighetti et al. [7]. Our problem is           contact, the force-torque sensor gives a signal with a mean
very close with their one, but they use vision and sound. We           of zero and a standard deviation of σ. For example, there
follow their frame, and we explain our methods which can               is a probability that the signal reaches ±3.σ (probability is
solve the four sub problems exposed in this introduction.              0.027%). But if there is contact, the signal certainly exceeds
                                                                       3.σ in absolute value. If we call α our confidence factor, we
              III. METHODS AND MEANS                                   can set a threshold (α.σ) for one signal. Then we must deal
A. Frame of Study                                                      with several signals. We use the 3 force signals and consider
                                                                       them as a signal vector. The origin of this vector is the origin
    We carry out this experiment in laboratory in order to
                                                                       of the sensor, and the top of the vector is force signals. If the
validate it for real application. For our peg-in hole operation,
                                                                       top of the vector is out of a boundary function, then there is
the tool is a aluminium cylinder with a spherical tip, with 2
                                                                       contact. We define 3 functions.
little pegs beside our cylinder to ensure the rotation around
the main axis of our tool. The environment is a fixed steel
plane, with a principal hole orthogonal with the plane, and
2 drillings to receive tool pegs (Fig. 1). We know all tool
and environment characteristics, except the position and
orientation of the search plane. We can define these ones
when we find the hole.
    Our operation follows this frame: first, we want to ensure
contact with the environment. So we need to detect contact.
Next, we want to know its orientation. We need to find
normal contact from force-torque information. Next, we
initiate a compliant motion, in order to search and detect
the hole. We need an efficient search and detection method.
Finally, we want to insert the hole, and we need an insertion
method. The next section presents all these means to realize
the operation.
                                                                                         Fig. 2.     parallelepiped boundary

               Fig. 1.   our tool and the environment

B. Contact Detection and Direction Estimation
   1) Contact Detection: Contact must be detected from
force-torque information. Shimokura et al. [5] work on
transition detection. We start from their study. Here, we have
6 signals, 3 forces and 3 torques (Fx, Fy, Fz, Mx, My, Mz).                                Fig. 3.     spherical boundary
When the robot is unloaded, these signals means are null (we

                    Fig. 4.   ellipsoidal boundary
                                                                                 Fig. 5.   sketch of the contact direction problem

   Our first boundary function is a parallelepiped (Fig. 2). We
set the threshold function of standard deviations and confi-              At this point the gradient is null. We get the coordinate of
dence factor, and the contact appears when the inequation              H:
                                                                                          ∂E      ∂E      ∂E
(1) is true.                                                                                   =       =      =0
                                                                                          XH      YH      ZH
   (|F x| ≥ α.σF x ) ∪ (|F y| ≥ α.σF y ) ∪ (|F z| ≥ α.σF z ) (1)
                                                                                                  M z.F y − M y.F z
  Our second boundary function is a sphere (Fig. 3). The                              XH =
contact is detected when (2) is true.                                                             F x2 + F y 2 + F z 2

    F x2 + F y 2 + F z 2 ≥ max(α.σF x ; α.σF y ; α.σF z ) (2)                                     M x.F z − M z.F x
                                                                                       YH =                                          (7)
                                                                                                  F x2 + F y 2 + F z 2
  Finally, our last boundary function is an ellipsoid (Fig. 4).
The contact is detected when (3) is true.                                                         M y.F x − M x.F y
                                                                                      ZH =
                2                  2                 2                                            F x2 + F y 2 + F z 2
         Fx            Fy             Fz
                  +             +              ≥1         (3)             We manage to compute the coordinate of H. So, we
        α.σF x        α.σF y        α.σF z
                                                                       can define the contact direction which fit the most (4) and
   2) Contact Direction: Here, we want to define the contact            (5), just from noisy force-torque information. Note that if
direction. To define this direction, we need a point, and a             denominator is null, it means force is null, which can be
direction vector. Call O the origin of the sensor. Call D              easily understood: no contact direction can exist if there is
the line which support the contact direction. If we use the            no force.
same notation as in III-B.1, the direction vector is F =                  3) Contact Point: Now that we have the contact direction,
              t                         t
(F x F y F z) . Call H = (XH YH ZH ) the closest point of              we want to find the contact point. The idea of many papers
O on line D (Fig. 5). We suppose a contact with no friction.           (as in the works of Zhou et al. [11]) is to compute the
By definition, torques are null on this line, so they are also          intersection between the tool shape and the contact direction.
null on H. The sensor gives us force-torque information on                                              t
                                                                       One point M = (XM YM ZM ) on D is define by XM =
O, so we compute the torque at this point.                             XH + K.F x, YM = YH + K.F y and ZM = ZH + K.F z,
                            M x = F z.YH − F y.ZH                     where K is a scalar. Our tool is a sphere and we define
 MO = MH +OH ×F              M y = F x.ZH − F z.XH (4)                 it analytically. The equation of a sphere with center C =
                             M z = F y.XH − F x.YH                     (XC YC ZC ) and radius R is (x − XC )2 + (y − YC )2 +
                                                                       (z − ZC ) = R2 . We know H, F, C and R, and we look for
   OH · F = 0        F x.XH + F y.YH + F z.ZH = 0           (5)        K that can solve (8).
   Our goal is to find the position of H. Due to noise,                                     (XH + K.F x − XC )2
imprecision, etc. we need to use an optimization method to                                 +(YH + K.F y − YC )2
find the point which can fit (4) and (5) with the maximum                               +(ZH + K.F z − ZC )2 = R2                      (8)
likelihood. We know force-torque, and we seek for H which
can minimize eq. (6):                                                    We solve a second degree equation:
                min(E = A + B + C + D)                      (6)                               ∆x = (XH − XC )
              A = (M x + F y.ZH − F z.YH )                                                    ∆y = (YH − YC )
              B = (M y + F z.XH − F x.ZH )                                                    ∆z = (ZH − ZC )
              C = (M z + F x.YH − F y.XH )                                         ∆ = 4.(F x.∆x + F y.∆y + F z.∆z)
            D = (F x.XH + F y.YH + F z.ZH )2                              −4.(F x2 + F y 2 + F z 2 ).(∆x2 + ∆y 2 + ∆z 2 − R2 )

  if ∆ ≥ 0:                                                                     prediction step, and then the update step:
                 F x.∆x + F y.∆y + F z.∆z ±               ∆                                            xk/k−1 = xk−1/k−1                    (12)
       K1,2 =                                                        (9)
                        F x2 + F y 2 + F z 2                                                      Pk/k−1 = Pk−1/k−1 + Qk

   When ∆ is positive, a problem appears because there are
2 solutions (Fig. 6). Of course, only one is valid, and we                                   Kk = Pk/k−1 · (Pk/k−1 + Rk )−1                 (13)
refer on Zhou et al. [11] paper. As we suppose a one-point                                 xk/k = xk/k−1 + Kk · (yk − xk/k−1 )
contact, the force push the tool, and the point M has to verify                                   Pk/k = (I − Kk ) · Pk/k−1
CM · F ≤ 0. Thus, we obtain the contact point from tool
shape and force-torque information.                                                Where: x is the estimated state vector (the estimated
                                                                                normal), P is the estimating error covariance matrix, Q is
                                                                                the process noise covariance matrix, R the measure noise
                                                                                covariance matrix, and K is the optimal Kalman gain. The
                                                                                estimated normal x move for a small period, and reach a
                                                                                value: this is our estimation of the normal. We should not
                                                                                forget to initialize the filter and tune the covariance matrices
                                                                                in order to have a quick estimation. This method let us define
                                                                                the normal of the search plane (call it Nplane ) when we
                                                                                contact the environment.
                                                                                C. About Search Method
Fig. 6. M1 (where force is repulsive) and M2 (where force is attractive),          Our search step is inspired from Chhatpar et al. [3].
solutions of equation (9)                                                       Every trajectory which can realize an efficient search can
                                                                                be use. We select a spiral, because we have no information
  4) Contact Normal: Using the previous section, we can                         about hole position and search space when contact. It is an
compute the contact point M, and thanks to Chen et al. [2]                      Archimedean spiral, and its equation in a polar frame is r =
works, contact normal can be computed. As we know the                           2.Π .θ, where r is the radius, θ the angle, and p the spiral
tool shape, we know the geometric normal at any point M.                        step (Fig. 7).
We just meet this geometric normal with the contact normal,
and we define this normal N :

                         N=                                         (10)

   The advantage of this method is that normal is not biased
by friction. Notice that N can be computed only if M is
found (if contact direction intersect tool shape). We manage
to characterize our contact (point, normal, force direction),
which allows us to describe the environment.
   5) Kalman Filter: An improvement can be set in the
plane normal identification with the help of a Kalman filter.
Actually, we get contact normal, but this result could be
imprecise because of noises, imprecision, etc. Furthermore,                                 Fig. 7.   definition of the Archimedean spiral
we need a good identification of plane normal to make a good
search and a good insertion during the peg-in-hole operation.
That is why we use a Kalman filter to estimate it. We set our                    D. About Hole Detection
system with a state-model. The state vector x is the search                        The hole detection step is close to the contact detection
plane normal vector. This normal is invariant with time (the                    step. Here, the goal is to detect the transition between the
environment is fixed), and when we contact the plane, we                         search state and the overlapping state. Thus, the ideas in
directly give the computed normal vector as the measure y                       III-B.1 are the same here, we look for thresholds. We can
of the system. Our model at time k is simple:                                   control 2 signals. The first signal is the normal contact. If
                                                                                the measured normal direction is “too far” from the plane
                             xk+1 = xk                              (11)        normal, it can be due to overlapping. Here, our boundary
                               yk = x k                                         function is a cylinder which its principal axis is the plane
                                                                                normal, and its base is an ellipse. The normal is a 3-
  We obtain the 2 steps of the Kalman filter, first the                           dimensional vector, and we note σnx , σny and σnz the

standard deviations on the plane frame (x and y axis are                      complete insertion. Notice that we can use these methods
in the plane, z axis is colinear to the plane normal). With α                 one after the other. We can compute the hole axis a priori,
our confidence factor, we define our boundary function, the                     align the tool, and then insert it with the force control loop.
cylinder, as:
                                                                                        IV. EXPERIMENTS AND RESULTS
                           2                2
                  Fx                Fy                                           We carry out these experiments on the manipulator arm
                               +                ≥1                 (14)
                 α.σnx             α.σny                                      Staubli RX90. It is equipped with a wrist force-torque sensor,
   The second signal is the velocity of the tool. During the                  a Delta 660/60, built by ATI Industrial Automation, and can
search, the tool moves on x and y axis (in the plane frame                    change its tool. Our tool with a spherical tip is used during
define above). The velocity on z axis should be almost equal                   the peg-hole-hole operation, and it is changed when the
to 0. If we call Vz the velocity along z axis and σvz its                     operation is finished. In the following sections, the different
standard deviation, we set a basic threshold :                                sub problems are tested.

                         V z ≥ α.σvz                               (15)       A. Contact Identification
                                                                                 1) Gaussian Noise: We want to validate our method
E. About Insertion
                                                                              explain in III-B.1. The first point is to check if the noise
   Our last issue is about insertion. Most papers deal with                   is Gaussian. The Probability Density Function (PDF) of
extruded tools with a circular or rectangular base, which have                this type of noise looks like a “bell shape curve”. We get
to go into a hole with tight tolerance. Although we have to                   force-torque from the unloaded sensor of our manipulator
insert a sphere in a circular hole in our study with tight                    arm for 30 seconds, which represent 30.711 measures. We
tolerance, the problem is easier because there is no jamming                  examine the discrete probability density function of our
risk. The problem is to find the hole axis. We try 2 methods                   measures (Fig. 9) and confirm our hypothesis that the noise
to solve it. The first one computes the axis before insertion.                 is Gaussian.
This axis is defined by a point and a direction. The direction
of the hole is colinear to plan normal, so we need to know
one point on this axis. We suppose the tool overlaps the
hole. So, it is possible to find a contact point M and contact
normal N . By definition, M is on the tool shape and on the
hole rim, so if we note I the center of the hole circle on
the plane IM = R (R the radius of the hole and the tool).
Furthermore, N gives us the direction toward the center of
the hole. Thus we can compute I:
                         N − (N · Nplane ).Nplane
       IM = −R.                                                    (16)
                         N − (N · Nplane ).Nplane

                                                                                      Fig. 9.   PDF of our signals. we notice the bell shapes

                                                                                 2) Boundary Function: In this section, we want to test the
                                                                              performance of our boundaries. As we develop in III-B.1, a
                                                                              confidence factor α define the limits of these functions. If
                                                                              we set it to a known and constant value, the only differences
       Fig. 8.   sketch of the tool with and without overlapping              between all these functions are their shapes. They have
                                                                              different characteristics and it depend on the gap between
   The second method consists in controlling forces along 2                   the different standard deviation. For example, we have our
axes. We place our tool in the plane frame define above. We                    standard deviations σF x , σF y and σF z , and assume they
suppose our tool has detected the hole and overlaps it. As we                 are equal. Then, the spherical boundary is surrounded by
know the plane normal, we know the direction of insertion,                    the parallelepiped boundary, and detection by the second
and start to move the tool along it. The tool overlap the hole,               function involves detection by the first one. The spherical
and a force will appear, with one part along plane normal,                    boundary is more sensitive than the parallelepiped one. In the
and other one in the plane. The goal is to control the force                  opposite, if one of our standard deviation is much higher than
with a loop to equalize this force at 0, by moving the tool                   the others, the parallelepiped boundary is almost surrounded
in the direction of this force. For example, the tool dive into               by the spherical boundary. It becomes the most sensitive
the hole, contact the hole rim and feel a force, shift on the                 (Fig. 10). The ellipsoid function is not study here because it
force direction, go on diving, contact the hole rim, etc. until               is always enclosed in the two others boundaries, and is the

most sensitive of the 3 functions defined in III-B.1. The goal                       deviations. Then the force direction (colinear to force vector)
is to seek the less sensitive boundary for our application.                         should move with a great magnitude, due to noise. But
   We measure the sensor standard deviations. We get σF x =                         if the force norm is much bigger than the order of the
0.331N , σF y = 0.398N and σF z = 0.738N . We set differ-                           standard deviations, noise should have no consequence on
ent confidence factors to simulate 10000 random “touches”                            force direction. Here, We move the tool to contact the
on sensor. We test on each touch if it is detected by                               plane along the sensor z-axis, and we want to define the
parallelepiped and spherical boundary, and we compare the                           standard deviation of the force direction F , the point which
number of touches detect by each function. The results are                          characterize the force direction H, the contact point M and
present in the table I.                                                             the contact normal N , function of the force norm F . Call
                                                                                    β the angle between z-axis and F , and γ the angle between
                                                                                    z-axis and CM or −N (Fig. 11). Our experiment is done
                                                                                    as follow. The tool is vertical, at 1cm from the horizontal
                                                                                    plane. When we start the measures, it begins to move down
                                                                                    and applies a defined force. We make 10000 measures. When
                                                                                    contact is detected, we save the dataset about β, and when a
                                                                                    normal is computed, we save the dataset about γ (sometimes,
                                                                                    force direction does not intersect the tool shape, and no
Fig. 10. boundaries can contain each other, depend on standard deviations.          normal is computed). The tables II and III give the standard
On left, the parallelepiped surrounds the sphere. On center, the opposite. On       deviation of the 2 angles function of F .
right, our case

           α      parallelepiped detection     spherical detection
          1.00              7215                      5340
          1.25              6199                      4297
          1.50              4367                      2038
          1.75              3185                      1649
          2.00              1826                      348
          2.25              1242                      544
          2.50               696                      324
                                 TABLE I

   For our sensor, the function which has the lowest sensi-
tivity is the spherical boundary. It is selected for the rest
of our study. Our last issue is the confidence factor α. In
one hand, if this factor is too high, the detection should be                                            Fig. 11.   sketch of the angle β and γ
more difficult, and the contact force may damage the robot
or the environment. On the other hand, if this factor is too
low, some bad detections should occur, and the sensor may                             F (N)     detect      StdF X      StdF Y     StdF Z     Stdβ (rad)/(deg)
be too sensitive. We set this factor to α = 51 , and thus our                           5       9516         0.084       0.066      0.013      0.108 / 6.188
                                                                                       10       9824         0.045       0.034      0.003      0.056 / 3.262
detection threshold is 5.σ. Of course, the set of this value
                                                                                       15       9842         0.034       0.023      0.012      0.044 / 2.526
depends on the user estimation.                                                        20       9838         0.027       0.018      0.005      0.033 / 1.919
                                                                                       25       9843         0.022       0.015      0.004      0.027 / 1.588
B. Contact Analysis                                                                    30       9850         0.021       0.014      0.003      0.026 / 1.514
   Now, we want to test the performances of our contact                                35       9852         0.023       0.014      0.013      0.032 / 1.835
                                                                                       40       9853         0.018       0.013      0.004      0.023 / 1.349
analysis methods. When the tool touch the environment, the
force direction, the contact point and the contact normal                                                              TABLE II
                                                                                              STANDARD DEVIATIONS OF FORCE DIRECTION VECTOR
can be compute from force-torque information. But this
information is linked with a Gaussian noise (see IV-A.1).
Thus, we want to know the influence of this noise on our
                                                                                       Thus, the sensitivity of our computation is known from
                                                                                    the force applied. Notice that this sensitivity is defined for
   1) Force Influence on Computed Values: The noise dis-
                                                                                    one direction (here the sensor z-axis). This test must be
turbs our computation, but it depends on the force applied
                                                                                    done for each direction we want the standard deviations. We
when contact. For example, assume that the force is along z-
                                                                                    choose the sensor z-axis because the search of the hole is
axis and its norm has the same order as the signals standard
                                                                                    done with z-axis colinear with the estimated plane normal.
  1 if α = 3 the confidence in a good value is 99.73%, which is typically a          Furthermore, these data can help use to define correctly the
good value, but some wires pull our sensor, that is why we choose α = 5             Kalman parameters.

 F (N)   normal   StdN X    StdN Y     StdN Z   Stdγ (rad)/(deg)                 τ (deg)    (deg)    time 5%(ms)   time 1%(ms)
   5      6800     0.412     0.374      0.381    0.795 / 45.57                      0      2.717         859           1437
  10      9682     0.370     0.311      0.181    0.543 / 31.16                      3      4.791         843           1453
  15      9815     0.259     0.218      0.085    0.356 / 20.44                      5      5.087         922           1547
  20      9831     0.204     0.166      0.052    0.272 / 15.61                      8      8.173         875           1438
  25      9836     0.163     0.135      0.039    0.217 / 12.43                     10      10.08         890           1562
  30      9841     0.137     0.115      0.037    0.182 / 10.47                     13      13.03         938           1766
  35      9842     0.117     0.098      0.036    0.155 / 8.912                                      TABLE IV
  40      9845     0.105     0.088      0.034    0.139 / 8.000
                                                                        K ALMAN F ILTER P ERFORMANCES WITH APPLICATION FORCE = 30N
                           TABLE III
                                                                                 τ (deg)    (deg)    time 5%(ms)   time 1%(ms)
                                                                                    0      3.308         765           1344
                                                                                    3      3.609         718           1265
                                                                                    5      5.084         782           1391
   2) Kalman Filter Estimation: The Kalman filter allows                             8      6.760         782           1391
us to define precisely the search plane normal. We want to                          10      8.661         719           1329
know its performance. On one hand we test the precision of                         13      12.38         797           1375
our estimation compared to a known normal. On the other                                          TABLE V
hand, we control the estimation speed function of Kalman                K ALMAN F ILTER P ERFORMANCES WITH APPLICATION FORCE = 40N
Parameters. The tool is displaced vertically and contacts an
oriented search plane with a force of 30N or 40N. We note
τ the angle between this plane and the horizontal plane and
  the estimated normal. We note the time when the Kalman               relatively good results. About the estimation time, We need
estimation reaches 5% and 1% of its final value. The results            almost one second and a half. If the force applied increases,
are presented in tables IV and V. A good initialization of             the time seems to decrease, but it is not very significant.
the Kalman filter parameters is needed to work properly and             Do not forget that the tool need to move down 1cm before
quickly. As it is a recursive filter, it needs a first value for         contacting the plane.
the contact normal and P, the estimating error covariance              C. Search Method
matrix (see III-B.4). Moreover, we need to fill Q and R.
Our operating point is where the tool z-axis is colinear with             The last issue of our study is the hole detection problem.
the search plane normal. So, tool need to be aligned close             The hole is searched during a spiral trajectory. But to define
to this normal. We don’t know it, but when we contact for              this spiral, we need a spiral step, call p in III-C. The goal it
the first time, we can compute a contact normal. This is a              to define it not too big in order to not miss the hole, but big
gross approximation, but it initializes our filter. We set P as         enough to describe the search quickly. Suppose that during a
a diagonal matrix diag(10.0 10.0 10.0) (the value is chosen            search, the tool almost overlap the hole, the contact point is
from experiments). It supposes that we are not confident                on the rim but the hole is not detected. On its next passage,
in our first value, which is true. Q is the process noise               we must detect it. The gap between these 2 passages is our
covariance matrix, and represents the covariance of the plane          spiral step p.
normal in the 3 directions. They are involved by bumps,                     V. CONCLUSIONS AND FUTURE WORKS
roughness and others physical issues. They move our model
from reality, and all their actions are concentrated in this           A. Conclusions
matrix. We supposed the variances are independent along he                This study deals with peg-in-hole operation, with only tool
3 axes around the operating point and we set it as a diagonal          shape and force-torque information. This problem can be
matrix diag(10−6 10−6 10−6 ). We set low values because                decomposed in 4 sub problems: the contact detection, the
we are very confident in our model and we do not move the               search method, the contact analysis and the insertion method.
tool during estimation. R is the measure noise covariance              Most papers deal with one or two sub problems. We study all
matrix, and represents the covariance of the noises from the           these sub problems, because it corresponds to the different
different measures. Here, our measure for the Kalman filter is          steps of the peg-in-hole operation. We improve the methods
the computed normal from force-torque information. So we               in the literature to make this operation. Indeed, we manage
use the information above to fill R. As for Q, we suppose               to detect a contact, to find force direction from force-torque
the variances are independent around the operating point and           signals thanks to a square less optimization, and to compute
we set it as a diagonal matrix diag(0.137 0.115 0.037) if              contact point and contact normal from tool shape. A Kalman
the force is 30N, and diag(0.105 0.088 0.034) if the force             filter has been used to identify environment characteristics,
is 40N. When all this parameters are set, we can start the             the plane normal. Our search method is an Archimedean
estimation.                                                            spiral, and we apply it on the search plane. Finally, we set
   Some comments can be made from the tables IV and V.                 methods to detect the hole during the search, and to insert
About precision, it seems that our method is very sensitive. If        the tool into the hole.
τ is enclosed in 0 to 3 degree (0 to 0.052 rad), the estimation           During our experiments, we want to validate our methods.
does not work properly. But if it exceeds 3 degrees, we have           The first point is the contact identification. We have checked

that the noises from force-torque signals are Gaussian. Next,                     [6] Kosei Kitagaki, Motoyoshi Fujiwara, Takashi Suehiro, and Tsukasa
we have compared the sensitivity between the different                                Ogasawara. Pseudo contact point monitoring for contact state estima-
                                                                                      tion. IEEE, 1999.
functions set in this study, and we have found that the                           [7] G. Milighetti, H.-B. Kuntze, C.W.Frey, B. Diestel-Fedderesen, and
spherical boundary is the less sensitive in our application.                          J. Balzer. On a primitive skill-based supervisory robot control
Then, we have tested our contact analysis methods. The stan-                          architecture. 2004.
                                                                                  [8] Stewart J. Moorehead and David Wang. Collision detection using a
dard deviation of the computed normal has been searched,                              flexible link manipulator - a feasibility study. IEEE, April 1996.
function of the force norm applied on the plane. These data                       [9] Wyatt S. Newman, Yonghong Zhao, and Yoh-Han Pao. Interpretation
are useful because they allow us to set properly our Kalman                           of force and moment signals for compliant peg in hole assembly. IEEE,
                                                                                      May 2001.
filter. Thus, we can estimate the plane normal. We have tested                    [10] R.H. Sturges and Schitt Laowattana. Fine motion planning through
the performance of our filter (speed and the precision) for                            constraint network analysis. IEEE, 1995.
a force of 30N and 40N. Quickly (about one second and                            [11] Xuecai Zhou, Qi Shi, and Zexiang Li. Contact localization using force
                                                                                      - torque measurment. IEEE, April 1996.
an half), we can estimate the normal with a good precision,
but some problems appear when we want to identify small

B. Future Works
   Although these methods can handle the all peg-in-hole
operation, we can discuss about some points in this study.
The first thing is about filtering. Here, we use the measures
from the force-torque sensor without any treatment. Filtering
may improve our methods about contact analysis, but it may
delay our contact detection.
   Next, we can discuss about estimation. We use a Kalman
filter in order to identify plane normal, and we measure this
normal to estimate it. The problem is that our measure is
the force-torque, but the relation between it and the normal
is non-linear. It may be possible to use an extended Kalman
filter to have better solutions.
   Furthermore, we can improve our knowledge of the plane
during the search step. The position can be helpful. Until the
hole is not detected, we can compute the plane which fit the
most our measured value during the search step. It gives us
a better estimation of the plane and can help for insertion
   Finally, we can talk about the characteristics of the tool.
We set them in our methods a priori. But some errors in the
measures of these ones could involve some errors. It may be
possible to set a protocol in order to “learn” the main ones.
The best solution should be to build methods which need no
tool shape information.

                 VI. ACKNOWLEDGMENTS
   The authors gratefully acknowledges the contribution of
the Atomic Energy Commission (CEA) and the Computer
Science Laboratory of Paris 6 University (LIP6).

                            R EFERENCES
 [1] Herman Bruyninckx, Stefan Dutre, and Joris De Schutter. Peg-on-hole
     - a model based solution to peg and hole alignment. IEEE, 1995.
 [2] Ning Chen, Ray Rink, and Hong Zhang. Local object shape from
     tactile sensing. IEEE, April 1996.
 [3] Siddharth R. Chhatpar and Michael S. Branicky. Search strategies for
     peg-in-hole assemblies with position uncertainty. IEEE, November
 [4] Stefan Dutre, Herman Bruyninckx, and Joris De Schutter. Contact
     identification and monitoring based on energy. IEEE, April 1996.
 [5] Ken ichiro Shimokura and Shin yo Muto. A high-speed method of
     detecting contact-state transitions and its implementation in a task-
     coordinate manipulation system. IEEE, April 1996.


To top