VIEWS: 166 PAGES: 8 POSTED ON: 8/7/2010
Peg-In-Hole with a Blind Robot : Contact Identiﬁcation and Analysis Joseph Salini, Franck Geffard, Olivier David and Olivier Sigaud Abstract— We consider the problem of peg-in-hole operation developed to ﬁnd 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 ﬁnd 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 ﬁlter to estimate the contact sensor. The shape of the tool are supposed to be known and we normal. collect force-torque information to ﬁnd 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 ﬂexible 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 identiﬁcation. Finally, Dutre et al. [4] use the derivate of the contact identiﬁcation problem, which gives the information energy signal to detect contact ofﬂine. 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 efﬁcient methods to ﬁnd 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 ﬁnally 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 deﬁne 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 identiﬁcation, 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 deﬁne 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 joseph.salini@hotmail.com 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 franck.geffard@cea.fr 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 olivier.david@cea.fr referenced position). Newman et al. [9] want to deﬁne 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 Olivier.Sigaud@upmc.fr information. All of these results are used to analyse contact. 1 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 ﬁt 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 conﬁdence 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 deﬁne 3 functions. little pegs beside our cylinder to ensure the rotation around the main axis of our tool. The environment is a ﬁxed 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 deﬁne these ones when we ﬁnd the hole. Our operation follows this frame: ﬁrst, we want to ensure contact with the environment. So we need to detect contact. Next, we want to know its orientation. We need to ﬁnd normal contact from force-torque information. Next, we initiate a compliant motion, in order to search and detect the hole. We need an efﬁcient 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 2 Fig. 4. ellipsoidal boundary Fig. 5. sketch of the contact direction problem Our ﬁrst boundary function is a parallelepiped (Fig. 2). We set the threshold function of standard deviations and conﬁ- 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 deﬁne the contact direction which ﬁt the most (4) and 2) Contact Direction: Here, we want to deﬁne the contact (5), just from noisy force-torque information. Note that if direction. To deﬁne 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 ﬁnd 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 deﬁnition, 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 deﬁne 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 deﬁne MO = MH +OH ×F M y = F x.ZH − F z.XH (4) it analytically. The equation of a sphere with center C = t M z = F y.XH − F x.YH (XC YC ZC ) and radius R is (x − XC )2 + (y − YC )2 + 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 ﬁnd 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 ﬁnd the point which can ﬁt (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 ) 2 A = (M x + F y.ZH − F z.YH ) ∆y = (YH − YC ) 2 B = (M y + F z.XH − F x.ZH ) ∆z = (ZH − ZC ) 2 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 ) 3 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 ﬁlter and tune the covariance matrices in order to have a quick estimation. This method let us deﬁne 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 efﬁcient 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 = p 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 deﬁne this normal N : MC N= (10) MC 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 identiﬁcation with the help of a Kalman ﬁlter. Actually, we get contact normal, but this result could be imprecise because of noises, imprecision, etc. Furthermore, Fig. 7. deﬁnition of the Archimedean spiral we need a good identiﬁcation 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 ﬁlter 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 ﬁxed), 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 ﬁrst 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 ﬁlter, ﬁrst the dimensional vector, and we note σnx , σny and σnz the 4 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 conﬁdence factor, we deﬁne 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 deﬁne 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 ﬁnished. In the following sections, the different standard deviation, we set a basic threshold : sub problems are tested. V z ≥ α.σvz (15) A. Contact Identiﬁcation 1) Gaussian Noise: We want to validate our method E. About Insertion explain in III-B.1. The ﬁrst 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 ﬁnd the hole axis. We try 2 methods measures (Fig. 9) and conﬁrm our hypothesis that the noise to solve it. The ﬁrst one computes the axis before insertion. is Gaussian. This axis is deﬁned 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 ﬁnd a contact point M and contact normal N . By deﬁnition, 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 conﬁdence factor α deﬁne 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 deﬁne 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 ﬁrst 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 5 most sensitive of the 3 functions deﬁned 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 conﬁdence 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 deﬁne 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 deﬁned 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 COMPARISON BETWEEN SPHERICAL AND PARALLELEPIPED BOUNDARY 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 conﬁdence factor α. In one hand, if this factor is too high, the detection should be Fig. 11. sketch of the angle β and γ more difﬁcult, 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 inﬂuence of this noise on our Thus, the sensitivity of our computation is known from results. the force applied. Notice that this sensitivity is deﬁned for 1) Force Inﬂuence 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 conﬁdence in a good value is 99.73%, which is typically a Furthermore, these data can help use to deﬁne correctly the good value, but some wires pull our sensor, that is why we choose α = 5 Kalman parameters. 6 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 STANDARD DEVIATIONS OF NORMAL VECTOR τ (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 ﬁlter allows 8 6.760 782 1391 us to deﬁne 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 ﬁnal 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 signiﬁcant. the Kalman ﬁlter 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 ﬁlter, it needs a ﬁrst 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 ﬁll 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 deﬁne 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 ﬁrst time, we can compute a contact normal. This is a to deﬁne it not too big in order to not miss the hole, but big gross approximation, but it initializes our ﬁlter. 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 conﬁdent on the rim but the hole is not detected. On its next passage, in our ﬁrst 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 conﬁdent 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 ﬁlter 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 ﬁll R. As for Q, we suppose to detect a contact, to ﬁnd 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 ﬁlter 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 ﬁrst point is the contact identiﬁcation. We have checked 7 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, ﬂexible 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. ﬁlter. 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 ﬁlter (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 angles. B. Future Works Although these methods can handle the all peg-in-hole operation, we can discuss about some points in this study. The ﬁrst thing is about ﬁltering. 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 ﬁlter 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 ﬁlter 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 ﬁt the most our measured value during the search step. It gives us a better estimation of the plane and can help for insertion step. 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 2001. [4] Stefan Dutre, Herman Bruyninckx, and Joris De Schutter. Contact identiﬁcation 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. 8