Identification of Contact Conditions from ... - Semantic Scholar
Proceedings of the 2000 IEEE International Conference on Robotics & Automation Seoul, Korea • May 21-26, 2001
Identification of Contact Conditions from Contaminated Data of Contact Force and Moment Tetsuya MOURI *, Takayoshi YAMADA**, Ayako IWAI**, Nobuharu MIMURA***, and Yasuyuki FUNAHASHI** *
Virtual System Laboratory, Gifu University, Yanagido 1-1, Gifu 501-1193, Japan ** Department of Mechanical Engineering, Nagoya Institute of Technology, Gokiso-cho, Showa-ku, Nagoya 466-8555, Japan *** Department of Biocybernetics, Niigata University, Ninomachi, Ikarashi, Niigata 950-2181, Japan [email protected], [email protected]
Abstract This paper discusses a method for identification of contact conditions from the information of 6-axes force sensor equipped with a robot hand. The previous paper has the following problems. (1) The noise of sensing force was not considered. (2) Hence, the estimates obtained by the previous method are biased from true value, and the identification of contact types depends on an unknown contact position. This paper thinks over the noise of force. We propose a method of removing the bias from the estimates. Hence, it is guaranteed that asymptotically unbiased estimates are obtained. The contact types can be judged by eigenvalues of a covariance matrix, which is derived from estimates of contact moment. The effectiveness of the algorithm is demonstrated by simulations.
1. Introduction When a robot manipulates an object which is in contact with external environment and performs assembly tasks, it is required to recognize contact conditions. The contact conditions mean contact type, contact position, and contact force. For example, let us imagine the simple task such that a box is set on the table by a robot manipulator. If the manipulator is equipped only with position control, it cannot judge which contact type happens, and cannot accomplish the task. On the other hand, human can recognize the current contact type by only using a sense of forces and accomplish the task easily. Therefore, it is necessary to identify and control the contact conditions in order to endue a robot with the skill of human. So far, there are various researches about contact conditions using force information. To our knowledge, Akella et al. [1] and Hyde et al. [5] proposed the method of controlling contact transition from free motion to constrained motion. D&u&tre et al. [3] identified geometrical uncertainties and detected topological transitions in contact situations. Tsujimura et al. [16], Kaneko et al. [6] and Ueno et al. [17] detected contact 0-7803-6475-9/01/$10.00© 2001 IEEE
597
position by a probe, Active Antenna and Multiple Active Antenna, respectively. Bicchi et al. [2] identified contact position and contact normal direction in case of point contact type and soft finger one. Zhou et al. [19] inferred accuracy of the proposed approach within the limit that the contact type was point one. Salisbury et al. [14] proposed a method for identification of contact position on the surface of sensor using multiple active sensing. Nagata et al. [13] and Kitagaki et al. [7] proposed methods of identifying the contact position between unknown object and environment using active sensing. In these methods, however, it was assumed that the contact condition is point contact type with friction. Oussalah [12] proposed a method of identifying the contact normal by measured force and velocity without friction. The authors [9][10][11][18] discussed identification of contact conditions including soft finger, line and plane contact type in addition to the point one. Ref. [9] showed the possibility of identifying the contact conditions between unknown grasped object and external environment. Ref. [10] expressed constraints on contact moment in an explicit form and reduced the identification of contact conditions into a linear problem. Ref. [18] treated the case where moment data were contaminated with noise. Ref. [11] introduced standard deviation of contact moment in order to express the restriction of contact moment. A method of identifying contact conditions was statistically provided by using the eigenvalues of covariance matrix of estimated contact moment. However, Ref. [11] did not consider the noise of force. This paper treats the case when both force data and moment one are contaminated with noise. If we try to use the method of Ref. [11], the bias of estimates occurs and the identification of contact types depends on contact position. We provide a method of removing the bias and identifying contact conditions, where the covariance of noise is known from the sensor calibration beforehand. An algorithm for identification of contact conditions is proposed. The effectiveness of the algorithm is
demonstrated by simulations.
2. Problem Formulation In this paper, the system shown in Fig. 1 is considered. Unknown object is grasped by a robot hand and is made in contact with external environment.
2.1. Symbols We define the following symbols, as shown in Fig. 1. o : origin of a force sensor equipped with the hand, c : contact point between the grasped object and external environment, Σ o : sensor coordinate frame fixed at o , Σ c : contact coordinate frame fixed at c , f o , no : measured force and moment in Σ o , f c , n c : contact force and moment in Σ c , rc : position vector of c in Σ o , where Σ c is the same orientation as Σ o . z
y
x
Σo o
Grasped Object
rc
? External
f o , no c
Environment
Σc
hypothesis. Assumption (A2) is illustrated in Fig. 2. Contact types are classified by DOF of contact [8]. It is denoted by m . m = 0 : Plane contact type with friction m = 1 : Line contact type with friction m = 2 : Soft finger contact type with friction m = 3 : Point contact type with friction The soft finger contact type does not exist between rigid objects. From Assumption (A5), the covariance matrices of noise are given by Cov[ε f ] = diag[ γ 2fx , γ 2fy , γ 2fz ] =: Γ f , 2 2 2 Cov[ε n ] = diag[ γ nx , γ ny , γ nz ] =: Γn ,
(1)
where diag[•] implies a diagonal matrix, and γ means standard deviation of noise. Γ f and Γn are known by the calibration of force sensor beforehand. Under these assumptions, the problem of identifying contact conditions (type, position, and force) is investigated. The essential difference of this paper from Ref. [11] is consideration of the noise ε f . In Section 3.3 and 3.4, we will make it clear that the estimates of contact conditions are biased, if the method of Ref. [11] is simply used. And we propose a revised method.
f c , nc
z'
z’
Fig. 1: Interaction between grasped object and external environment
2.2. Assumptions We make the following assumptions for clarification of discussions. (A1) The object is firmly grasped by the robot hand and can be manipulated arbitrarily by the robot manipulator. (A2) The grasped object is in contact with external environment through point, soft finger, line, or plane contact type with friction. However the contact position and types are unknown. (A3) The contact position and type remain unchanged during identification process. And no slip occurs at the contact position c . (A4) The force f o , the moment n o , and the position o are measurable and controllable. The force f c and the moment nc are immeasurable. (A5) The measured force f o and the moment n o are contaminated with noise ε f and ε n , respectively. The noise ε f and ε n are independent of each other and occur randomly. (A6) The contact force f c and the moment nc occur randomly by active sensing. (A7) The number of times of active sensing motion is sufficient large, and our analysis is based on ergodic
560
Σ c′
Σ c′
y' x'
x' (a) Point contact
(b) Soft finger contact
z’
z’
Σ c′ x'
y'
Σ c′
y'
y'
x'
x’ Plane contact (c) Line contact (d) Fig. 2: Contact types
2.3. Constraint of Contact Moment According to Ref. [11], we consider some appropriate frame Σ c′ as shown in Fig. 2. The origin of Σ c′ is fixed at c , and the direction of z ′ axis is normal to the contact plane. In case of line contact type, moreover, the contact line should be taken as x ′ axis. Contact moment with respect to Σ c′ is denoted by ′ , ncy ′ , ncz ′ ]T . We define the covariance matrix of nc′ = [ ncx nc′ . 2 2 2 Cov[n c′ ] = diag[ σ nx , σ ny , σ nz ] =: V , (2)
where σ nj ( j = x, y, z ) denotes a standard deviation of components of contact moment nc′ . From Assumption (A7), each contact type is characterized by the standard deviation σ nj . Point contact : σ nx = σ ny = σ nz = 0 Soft finger contact : σ nx = σ ny = 0 (3) Line contact : σ nx = 0 Plane contact : no constraint
nc is expressed by
nc = Rnc′ ,
(4)
n ci := n ci −
1 k 1 k n cj , ε fi := ε fi − ∑ ε fj , ∑ k j =1 k j =1
Ε fi := Ε fi −
1 k 1 k Ε , ε : = ε − ∑ fj ni ni k ∑ ε nj . k j =1 j =1
From Eq. (10), Eq. (9) is rewritten by ∗
3×3
∗
(11)
According to Ref. [11], contact moment with noise is represented by ∗
∗
ν i := n ci + ε ni − Ε fi x .
(12)
Eq. (11) becomes
bi = Ai x ∗ + ν i , i = 1,2, L , k .
is orientation of Σ c′ with respect to Σ c .
(13)
3.3. Identification of Contact Position
3. Identification of Contact conditions We will identify contact conditions such as contact type, position, and force from the measured data contaminated with noise.
3.1. Force and Moment Equilibrium The equilibrium equation of force and moment is represented by rc × f o + n c = n o . (5) In Eq. (5), the measurable parameters are f o and n o , while immeasurable ones are nc and rc from Assumption (A4). In order to clarify expression of the equation, we use the following notations. (6) a := f o , A := −[a ×] , b := n o , x := rc . From k times of active sensing motion, we have bi∗ = Ai∗ x ∗ + n ci∗ , i = 1,2, L , k ,
∗
bi = Ai x + nci + ε ni − Ε fi x .
where
R := [rx , ry , rz ] ∈ ℜ
(10)
(7) ∗
where the subscript “i” implies the i-th value, • means the true value of • , and x remains unchanged by active sensing. From Assumption (A5), the measured force ai and moment bi are expressed by the following equations.
Ai = Ai∗ + Ε fi , bi = bi∗ + ε ni , (8) where Ε fi := −[ε fi ×] . Substituting Eq. (8) into (7) yields bi = Ai x ∗ + nci∗ + ε ni − Ε fi x ∗ . (9) Note that the force data are contaminated with noise, and Eq. (9) includes the unknown term “ Ε fi x ∗ ”. In the Section 3.3 and 3.4, we will find an effect of the term on estimates.
3.2 Generation of Deviated Data We generate deviated force and moment, whose averages are equal to zero, because four contact types shown in Fig.2 are treated even when the averages of measured force and moment are not zero. Deviated force and moment from the average are given by 1 k 1 k a i := a i − ∑ a j , bi := bi − ∑ b j , k j =1 k j =1
561
In this subsection we use the method of Ref. [11] for identification of contact conditions. Since an average of component of ν i is equal to zero, we identify x by minimizing a performance index 2 1 k J = ∑ Ai x − bi . (14) k i =1 The estimate is given as 1 k T 1 k ∑ Ai Ai xˆ (k ) = ∑ AiT bi . k k i =1 i =1 Substituting Eqs. (8), (9) and (11) into (15) yields 1 k ∑ ( Ai∗ + Ε fi ) T ( Ai∗ + Ε fi ) xˆ (k ) k i =1 . 1 k T ∗ ∗ ∗ ∗ = ∑ ( Ai + Ε fi ) ( Ai x + n ci + ε ni ) k i =1
(15)
(16)
Here, from Assumptions (A5)~(A7), the estimate of contact position, xˆ (∞) , is xˆ (∞) = lim xˆ ( k ) k →∞
2 σ fy = x* +
2 2 2 + σ fz + γ fy + γ fz γ 2fz + γ 2fx x∗ 0 0 σ 2fz + σ 2fx + γ 2fz + γ 2fx 2 2 γ fx + γ fy 0 0 2 2 2 2 σ fx + σ fy + γ fx + γ fy γ 2fy + γ 2fz
0
0
, (17) where the covariance of true value of measured force ai∗ is represented by
Cov[a ∗ ] = diag[σ 2fx , σ 2fy , σ 2fz ] .
(18)
The bias of the estimate xˆ from true value is given by the second term of Eq. (17). In order to remove the bias from xˆ , we use Assumption (A5) in which the covariance of noise of force is known. We replace Eq. (15) with the following equation, and identify the contact position.
1 k T 1 k ∑ Ai Ai - Γ xˆ ′(k ) = ∑ AiT bi , k k i =1 i =1
θ > λ1 λ > θ > λ 1 2 λ > θ > λ 2 3 λ 3 > θ
(19)
where
Γ := diag[ γ
2 fy
+γ
2 fz ,
γ
2 fz
+γ
2 fx ,
γ
2 fx
+γ
2 fy
].
(20)
Here, Eq. (19) coincides with the estimate by CLS [15]. From Assumptions (A5)~(A7), it is guaranteed that the estimate xˆ ′(k ) is asymptotically unbiased. xˆ ′(∞) = x ∗ (21)
(31)
λ ■λ1(∞)
The estimated value of contact moment with noise is represented by νˆi (k ) := bi − Ai xˆ ′(k ) . (22)
●λ2(∞)
θ
▲λ3(∞)
Point Soft finger
From Eqs. (8), (10) and (11), Eq. (22) is expressed by ∗ ∗ ∗ ∗ νˆi (k ) = Ai x + nci + ε ni − ( Ai + Ε fi ) xˆ ′(k ) . (23)
Line
Plane
Fig. 3 Relation between contact types and eigenvalues
Using Assumptions (A5)~(A7) and putting k close to ∞ , ∗ ∗ νˆi (∞) = lim νˆ i (k ) = n ci + ε ni − Ε fi x = ν i . (24)
4. Identification of Unknown Parameters In the previous section, the estimated contact position xˆ ' (k ) and moment with noise νˆi (k ) were obtained, and it was shown that the contact types could be identified in case of the infinite times of active sensing motion. In that case, however, the estimate error cannot be ignored. Also, we did not consider the constraint of contact moment such as Eq. (3), when the contact position was estimated in Section 3.3. Therefore, in this section, we investigate the finite time case and propose a method for more accurate identification of unknown parameters. A performance index is represented by 2 1 k J type = ∑ Ai x + n ci − bi . (32) k i =1
k →∞
So it is guaranteed that νˆi (∞) in Eq. (24) coincides with ν i in Eq. (12).
3.4. Identification of Contact Types In this subsection we analyze the relation between contact types and the eigenvalues of covariance matrix obtained by estimated contact moment νˆi . From Eq. (12), we define a covariance matrix of ν i . (25) M := Cov [ν ] From Eqs. (1) and (4), the matrix M becomes M = M ′ + Γn + [ x * ×]Γ f [ x * ×]T ,
for a point contact type for a soft finger contact type for a line contact type for a plane contact type
(26)
where
M ′ = RVRT . (27) M depends on contact position x , noise of force ε f and moment ε n . M ′ is independent of them. Here, we define N (k ) : 1 k N (k ) := ∑νˆi (k )νˆi (k )T − Γn − [ xˆ (k )×]Γ f [ xˆ (k )×]T . (28) k i =1 When we put k close to ∞ in Eq. (28) from Assumption (A7), N (∞) = M ′ . (29)
4.1 Case of Point Contact Type In case of point contact type, contact moment nci is restricted to zero. The index of Eq. (32) is represented by the same form as Eq. (14). Considering the noise of force, the estimate xˆ is given by Eq. (19).
4.2. Case of Soft Finger Contact Type In case of soft finger contact type, contact moment is restricted to the form: ′ = [rz1 rz 2 rz 3 ]T nczi ′ , nci = rz nczi (33)
Performing the singular value decomposition, the matrix N (k ) is rewritten by 0 0 λ1 (k ) N (k ) = [r1 , r2 , r3 ] 0 λ2 (k ) 0 [r1 , r2 , r3 ]T , (30) 0 0 λ3 (k ) where λ1 (k ) ≥ λ2 (k ) ≥ λ3 (k ) .
where r z denotes the unit normal vector at contact point, and remains unchanged. If the element of maximum absolute value of rz is defined as rz max , the moment can be rewritten by [1 y1 y 2 ]T z i for rz max = rz1 n ci = [ y1 1 y 2 ]T z i for rz max = rz 2 . (34) [ y y 1]T z for r 2 i z max = rz 3 1 For example, in case of rz max = rz1 , the index is represented by 2 1 k J s = ∑ Ai x + yz i − bi , y = [1 y1 y 2 ]T . (35) k i =1
Using the matrix N (∞) and Eq. (3), we can identify the contact conditions by the method of Ref. [11]. The distribution of eigenvalues in case of each contact type is illustrated in Fig. 3. If a threshold θ is set at a value larger than 0 , we can judge which contact type happens.
562
When we identify the unknown parameters by minimizing the index J s , it is necessary to solve the following nonlinear equations. ∂J s ∂J ∂J ∂J = 0, s = 0, s = 0, s = 0 . (36) ∂x ∂y1 ∂y 2 ∂z i Here, z i is estimated as z i = yˆ + (bi − Ai xˆ ) ,
(37)
where
y + = [ y1+
y2+
y3+ ] := ( y T y ) −1 y T =
[1 y1
y2 ]
1 + y1 + y 2 2 2
.
Considering the noise of force, Eq. (37) can be rewritten by z i = z i* − yˆ + (ε ni − E fi xˆ ) . (38) Eq. (38) implies that z i has the bias from true value. From Eq. (38), x, y1 , and y 2 are estimated by 1 1 1 T A2 iT zi + Dˆ 2T A3 iT zi + Dˆ 3T ∑ Ai Ai − Γ ∑ ∑ k k k x 1 1 2 ˆ y1 ∑ A2 izi + Dˆ 2 ( zi ) − ζ 0 ∑ k k y2 1 2 1 A z + Dˆ ˆ 0 ∑ ( zi ) − ζ 3 k ∑ 3i i k 1 T T T ∑ ( Ai bi − A1 i z i ) − Dˆ 1 k 1 + , (39) = b2i z i − γ ny 2 yˆ 2 ∑ k 1 b3i z i − γ nz 2 yˆ 3+ ∑ k where bi1 Ai1 Ai = Ai 2 , bi = bi 2 , bi 3 Ai 3
γ 2fx ( xˆ 2 yˆ 2 − xˆ3 yˆ1 ) Dˆ 1 2 ˆ T −1 D2 := ( yˆ yˆ ) γ fy ( xˆ3 − xˆ1 yˆ 2 ) × Dˆ γ 2 ( xˆ yˆ − xˆ ) 3 fz 1 1 2 ,
and select the index J s .
4.3. Case of Line Contact Type In case of line contact type, contact moment is restricted to the form: ry1 ry 2 nci = rz1 rz 2
T
′ ry 3 ncyi ′ . rz 3 nczi
(40)
The index is represented by Eqs. (32) and (40). In a similar way of soft finger contact type, we can identify unknown parameters. This formulation is omitted for lack of space.
4.4. Case of Plane Contact Type In case of plane contact type, we cannot identify unknown parameters by the active sensing method.
4.5. Algorithm for Identification of Contact Conditions Summarizing the result above, the algorithm for identification of contact conditions is proposed as follows. Step 1: Command active sensing motion. Step 2: Calculate deviated force and moment from Eq. (10). Step 3: Estimate contact position xˆ from Eq. (19). Step 4: Estimate contact moment νˆ from Eq. (22). Step 5: Calculate matrix N from Eq. (28) and analyze the eigenvalues λ1 , λ2 , λ3 . Step 6: Judge which contact type happens by Eq. (31). Step 7: If it is judged as soft finger contact type, then unknown parameters are re-estimated by Eqs. (38) and (39). Step 8: If it is judged as line contact type, then unknown parameters are re-estimated. Step 9: Go to Step 1. By using this algorithm, the contact conditions can be identified continuously by active sensing motion. Also, in case of plane contact type, unknown parameters cannot be identified, however contact type can be judged.
5. Simulation
+ T + T ζˆ := yˆ (Γn + [ xˆ ×]Γ f [ xˆ ×] )( yˆ ) .
Therefore, we summarize the result as follows [11]. Step 1: Initial values are set as the estimates in section 3. Step 2: x, y1 , y2 are fixed and z i is identified by minimizing J s by Eq. (37). Step 3: zi is fixed and x, y1 , y 2 are identified by minimizing J s by Eq. (39). Steps 2 and 3 are repeated until some convergence condition is satisfied. In fact, though the normal vector rz is unknown, the eigenvector r1 , which is close to rz , is obtained by Eq. (30). Therefore, we can set as rz = r1
563
In this section, we demonstrate the effectiveness of our proposed method by using some simulations. The measured force f o and moment no are generated under the following conditions, where nc follows Eq. (3).
x * = [2.00, 2.00, 2.00]T , R = diag[1,1,1] ,
Cov[ f c ] = diag[ 2.00 2 , 1.50 2 , 1.00 2 ] , V = Cov[nc' ] = diag[ 2.00 2 ,1.50 2 ,1.00 2 ] , Γ f = Cov[ε f ] = diag[0.10 2 ,0.20 2 ,0.30 2 ] ,
Γn = Cov[ε n ] = diag[0.10 2 ,0.20 2 ,0.30 2 ] . Figs. 4 and 5 show the distribution of eigenvalues calculated by the method of Ref. [11] and that of our proposed method, respectively. These figures are obtained
[3] D&u&tre , S., et al., “Contact Identification and Monitoring Based on Energy,” Proc. IEEE Conf. Robotics and Automation, pp. 1333-1338, 1996. [4] Horn, R., A. and John, C. R., Topics in Matrix Analysis, Cambridge Univ. Press, 1991. [5] Hyde, J. M., and Cutkosky, M. R., “Contact Transition Control: An Experimental Study,” Proc. IEEE Conf. Robotics and Automation, pp. 363-368, 1993. [6] Kaneko, M., et al., “Active Antenna,” Proc. IEEE Conf. Robotics and Automation, pp. 2665-2671, 1994. [7] Kitagaki, K., et al., “Methods to Detect State by Force Sensing in an Edge Mating Task,” Proc. IEEE Conf. Robotics and Automation, pp.701-706, 1993. [8] Mason, M. T. and Salisbury, J. K., Robot Hands and the Mechanics of Manipulation, Cambridge, MA, MIT Press, 1985. [9] Mimura, N, and Funahashi, Y., “Parameter Identification of Contact Conditions by Active Force Sensing,” Proc. IEEE Conf. Robotics and Automation, pp. 2645-2650, 1994 [10] Mimura, N., et al., “An Algorithm for Identification of Contact Conditions,” Trans. Jpn. Soc. Mech. Eng., Vol. 63, No. 610, Series C, pp. 2061-2068, 1997 (in Japanese). [11] Mouri, T., et al., “Identification of Contact Conditions from Contaminated Data of Contact Moment,” Proc. IEEE Conf. Robotics and Automation, pp. 585-591, 1999. [12] Nagata, K., et al., “Pose Estimation of Grasped Object from Contact Force or Joint Data of Manipulator,” SICE, Vol. 28, No. 7, pp. 783-789, 1992 (in Japanese) [13] Oussalah, M., “Fuzzy linear regression for contact identification,” Proc. IEEE Conf. Robotics and Automation, pp. 3616-3621, 2000 [14] Salisbury, J. K., et al., “Interpretation of Contact Geometries from Force Measurements,” Proc. 1st Int. Symp. on Robotics Research, pp. 565-577, 1983. [15] Stoica, P., and Soderstorm, T., “Bias Correction in Least Square Identification”, Int. J. Control, 35, pp. 449-457 [16] Tsujimura, T., and Yabuta, T., “Object Detection by Tactile Sensing Method Employing Force/Torque Information,” IEEE Trans. Robotics and Automation, Vol. 5-4, pp. 444-450, 1989. [17] Ueno, N., and Kaneko, M., “Contact Localization by Multiple Active Antenna,” Proc. IEEE Conf. Robotics and Automation, pp. 1942-1947, 1999. [18] Yamada, T., et al., “Identification of Contact Conditions from Contaminated Data,” Trans. Jpn. Soc. Mech. Eng., Vol. 64, No. 618, Series C, pp. 584-489, 1998 (in Japanese).
by 100 trials and 100 times of active sensing motion per 1 trial. In Fig. 4, the eigenvalues are not separated into two types of the variance, which means the variance of noise and that of moment. These eigenvalues depend on unknown contact position, as explained in Eq. (26). Hence, the threshold cannot be determined, and the contact type cannot be judged. Compared with Fig. 4, the eigenvalues shown in Fig. 5 are clearly separated into the two types. From Eq. (31), the contact types can be judged if the threshold θ is selected between 0 and 1.02 (=min{ σ nx2 , σ ny2 , σ nz2 }). From Fig. 5, it can be set as about 0.4. Figs. 6 and 7 show the estimates of contact position and normal direction in case of soft finger contact type, respectively. Figs. 6(a) and 7(a) show linear estimates, which are estimated by the method of Section 3. Figs. 6(b) and 7(b) show typed estimates which are re-identified as described in Section 4. Both linear estimates and typed ones are converged to a true value. However, Fig. 6 shows that the typed estimates are more accurate than the linear estimates.
6. Conclusion The identification of contact conditions was investigated when the shape of grasped object is unknown and both force and moment data are contaminated with noise. First, it was shown that the bias of estimates occured if the previous method was used. Secondly, a method of obtaining an asymptotically unbiased estimate of contact position was established, where the variance of noise of force and moment were evident by the sensor calibration beforehand. The relation between contact types and the eigenvalues of covariance matrix of estimated contact moment was analyzed. Thirdly, an algorithm for more accurate identification of unknown parameters was proposed. Finally, the effectiveness of our proposed method was demonstrated by numerical examples. This method can easily estimate the normal direction in case of soft finger contact type and the direction of contact line in case of line contact type. This method can be applied to the case where a walking robot recognizes contact conditions between floor and its own legs.
References [1] Akella, P. N., and Cutkosky, M. R., “Contact Transition Control with Semiactive Soft Fingertips,” IEEE Trans. Robotics and Automation, Vol. 11, No. 6, pp. 859-867, 1995. [2] Bicchi, A., et al,. “Contact Sensing from Force Measurements,” Int. J. Robotics Research, Vol. 12, No. 3, pp. 249-262, 1993.
564
[19] Zhou, X., et al, “Contact Localization Using Force/Torque Measurements,” Proc. IEEE Conf.
1
1
10
10 λ3
λ1
θ=1.0
0
10
-1
10
0
10
θ=0.4 -1
10
40 60 80 Number of sensing times k
10
100
20
(a) Point contact λ2
20
40 60 80 Number of sensing times k
100
0
10
10
λ2
λ3
θ=0.4
10
-1
10
-2
20
40 60 80 Number of sensing times k
θ=1.0
λ2
λ3
λ1
2.2
100
x1
x2
x3
2.1 2.0 1.9 1.8 20
100
40 60 80 Number of sensing times k
100
(b) Typed estimates Fig. 6: Contact point
λ2
λ3
0
10
θ=0.4 -1
10
-2
-2
20
40 60 80 Number of sensing times k
10
100
20
1
0
θ=1.0
-1
10
λ1
λ2
λ3
40
60
80
Number of sensing times k
(d) Plane contact Fig. 4: Ref. [11]
10
1
100
0.8
10
0
0.4 0.2 0.0
-0.2 20
θ=0.4
10
100
10
rz1 rz2 rz3
0.6
40 60 80 Number of sensing times k
100
(a) Linear Estimates
-1
λ1
-2
20
Eigenvalues λ
10
10
40 60 80 Number of sensing times k
1.0
(c) Line contact
(c) Line contact
Eigenvalues λ
40 60 80 Number of sensing times k
1
-1
10
20
(a) Linear estimates
0
10
λ1
10
1.8
100
Estimated normal direction rz
10
1.9
(b) Soft finger contact
Eigenvalues λ
Eigenvalues λ
1
x3
1
(b) Soft finger contact 10
x2
2.0
λ2
λ3
-2
20
40 60 80 Number of sensing times k
(d) Plane contact Fig. 5: This paper
100
Estimated normal direction rz
-2
40 60 80 Number of sensing times k
λ1 θ=1.0
0
10
10
λ3
Eigenvalues λ
Eigenvalues λ
λ1
10
x1
2.1
(a) Point contact
1
-1
2.2
Estimated contact point x
20
10
λ3
-2
-2
10
λ2
Estimated contact point x
λ2
Eigenvalues λ
Eigenvalues λ
λ1
10
Robotics and Automation, pp. 1339-1344, 1996.
1.0 0.8
rz1 rz2 rz3
0.6 0.4 0.2 0.0
-0.2 20
40 60 80 Number of sensing times k
(b) Typed estimates Fig. 7: Normal direction
565
100
Identification of Contact Conditions from Contaminated Data of Contact Force and Moment Tetsuya MOURI *, Takayoshi YAMADA**, Ayako IWAI**, Nobuharu MIMURA***, and Yasuyuki FUNAHASHI** *
Virtual System Laboratory, Gifu University, Yanagido 1-1, Gifu 501-1193, Japan ** Department of Mechanical Engineering, Nagoya Institute of Technology, Gokiso-cho, Showa-ku, Nagoya 466-8555, Japan *** Department of Biocybernetics, Niigata University, Ninomachi, Ikarashi, Niigata 950-2181, Japan [email protected], [email protected]
Abstract This paper discusses a method for identification of contact conditions from the information of 6-axes force sensor equipped with a robot hand. The previous paper has the following problems. (1) The noise of sensing force was not considered. (2) Hence, the estimates obtained by the previous method are biased from true value, and the identification of contact types depends on an unknown contact position. This paper thinks over the noise of force. We propose a method of removing the bias from the estimates. Hence, it is guaranteed that asymptotically unbiased estimates are obtained. The contact types can be judged by eigenvalues of a covariance matrix, which is derived from estimates of contact moment. The effectiveness of the algorithm is demonstrated by simulations.
1. Introduction When a robot manipulates an object which is in contact with external environment and performs assembly tasks, it is required to recognize contact conditions. The contact conditions mean contact type, contact position, and contact force. For example, let us imagine the simple task such that a box is set on the table by a robot manipulator. If the manipulator is equipped only with position control, it cannot judge which contact type happens, and cannot accomplish the task. On the other hand, human can recognize the current contact type by only using a sense of forces and accomplish the task easily. Therefore, it is necessary to identify and control the contact conditions in order to endue a robot with the skill of human. So far, there are various researches about contact conditions using force information. To our knowledge, Akella et al. [1] and Hyde et al. [5] proposed the method of controlling contact transition from free motion to constrained motion. D&u&tre et al. [3] identified geometrical uncertainties and detected topological transitions in contact situations. Tsujimura et al. [16], Kaneko et al. [6] and Ueno et al. [17] detected contact 0-7803-6475-9/01/$10.00© 2001 IEEE
597
position by a probe, Active Antenna and Multiple Active Antenna, respectively. Bicchi et al. [2] identified contact position and contact normal direction in case of point contact type and soft finger one. Zhou et al. [19] inferred accuracy of the proposed approach within the limit that the contact type was point one. Salisbury et al. [14] proposed a method for identification of contact position on the surface of sensor using multiple active sensing. Nagata et al. [13] and Kitagaki et al. [7] proposed methods of identifying the contact position between unknown object and environment using active sensing. In these methods, however, it was assumed that the contact condition is point contact type with friction. Oussalah [12] proposed a method of identifying the contact normal by measured force and velocity without friction. The authors [9][10][11][18] discussed identification of contact conditions including soft finger, line and plane contact type in addition to the point one. Ref. [9] showed the possibility of identifying the contact conditions between unknown grasped object and external environment. Ref. [10] expressed constraints on contact moment in an explicit form and reduced the identification of contact conditions into a linear problem. Ref. [18] treated the case where moment data were contaminated with noise. Ref. [11] introduced standard deviation of contact moment in order to express the restriction of contact moment. A method of identifying contact conditions was statistically provided by using the eigenvalues of covariance matrix of estimated contact moment. However, Ref. [11] did not consider the noise of force. This paper treats the case when both force data and moment one are contaminated with noise. If we try to use the method of Ref. [11], the bias of estimates occurs and the identification of contact types depends on contact position. We provide a method of removing the bias and identifying contact conditions, where the covariance of noise is known from the sensor calibration beforehand. An algorithm for identification of contact conditions is proposed. The effectiveness of the algorithm is
demonstrated by simulations.
2. Problem Formulation In this paper, the system shown in Fig. 1 is considered. Unknown object is grasped by a robot hand and is made in contact with external environment.
2.1. Symbols We define the following symbols, as shown in Fig. 1. o : origin of a force sensor equipped with the hand, c : contact point between the grasped object and external environment, Σ o : sensor coordinate frame fixed at o , Σ c : contact coordinate frame fixed at c , f o , no : measured force and moment in Σ o , f c , n c : contact force and moment in Σ c , rc : position vector of c in Σ o , where Σ c is the same orientation as Σ o . z
y
x
Σo o
Grasped Object
rc
? External
f o , no c
Environment
Σc
hypothesis. Assumption (A2) is illustrated in Fig. 2. Contact types are classified by DOF of contact [8]. It is denoted by m . m = 0 : Plane contact type with friction m = 1 : Line contact type with friction m = 2 : Soft finger contact type with friction m = 3 : Point contact type with friction The soft finger contact type does not exist between rigid objects. From Assumption (A5), the covariance matrices of noise are given by Cov[ε f ] = diag[ γ 2fx , γ 2fy , γ 2fz ] =: Γ f , 2 2 2 Cov[ε n ] = diag[ γ nx , γ ny , γ nz ] =: Γn ,
(1)
where diag[•] implies a diagonal matrix, and γ means standard deviation of noise. Γ f and Γn are known by the calibration of force sensor beforehand. Under these assumptions, the problem of identifying contact conditions (type, position, and force) is investigated. The essential difference of this paper from Ref. [11] is consideration of the noise ε f . In Section 3.3 and 3.4, we will make it clear that the estimates of contact conditions are biased, if the method of Ref. [11] is simply used. And we propose a revised method.
f c , nc
z'
z’
Fig. 1: Interaction between grasped object and external environment
2.2. Assumptions We make the following assumptions for clarification of discussions. (A1) The object is firmly grasped by the robot hand and can be manipulated arbitrarily by the robot manipulator. (A2) The grasped object is in contact with external environment through point, soft finger, line, or plane contact type with friction. However the contact position and types are unknown. (A3) The contact position and type remain unchanged during identification process. And no slip occurs at the contact position c . (A4) The force f o , the moment n o , and the position o are measurable and controllable. The force f c and the moment nc are immeasurable. (A5) The measured force f o and the moment n o are contaminated with noise ε f and ε n , respectively. The noise ε f and ε n are independent of each other and occur randomly. (A6) The contact force f c and the moment nc occur randomly by active sensing. (A7) The number of times of active sensing motion is sufficient large, and our analysis is based on ergodic
560
Σ c′
Σ c′
y' x'
x' (a) Point contact
(b) Soft finger contact
z’
z’
Σ c′ x'
y'
Σ c′
y'
y'
x'
x’ Plane contact (c) Line contact (d) Fig. 2: Contact types
2.3. Constraint of Contact Moment According to Ref. [11], we consider some appropriate frame Σ c′ as shown in Fig. 2. The origin of Σ c′ is fixed at c , and the direction of z ′ axis is normal to the contact plane. In case of line contact type, moreover, the contact line should be taken as x ′ axis. Contact moment with respect to Σ c′ is denoted by ′ , ncy ′ , ncz ′ ]T . We define the covariance matrix of nc′ = [ ncx nc′ . 2 2 2 Cov[n c′ ] = diag[ σ nx , σ ny , σ nz ] =: V , (2)
where σ nj ( j = x, y, z ) denotes a standard deviation of components of contact moment nc′ . From Assumption (A7), each contact type is characterized by the standard deviation σ nj . Point contact : σ nx = σ ny = σ nz = 0 Soft finger contact : σ nx = σ ny = 0 (3) Line contact : σ nx = 0 Plane contact : no constraint
nc is expressed by
nc = Rnc′ ,
(4)
n ci := n ci −
1 k 1 k n cj , ε fi := ε fi − ∑ ε fj , ∑ k j =1 k j =1
Ε fi := Ε fi −
1 k 1 k Ε , ε : = ε − ∑ fj ni ni k ∑ ε nj . k j =1 j =1
From Eq. (10), Eq. (9) is rewritten by ∗
3×3
∗
(11)
According to Ref. [11], contact moment with noise is represented by ∗
∗
ν i := n ci + ε ni − Ε fi x .
(12)
Eq. (11) becomes
bi = Ai x ∗ + ν i , i = 1,2, L , k .
is orientation of Σ c′ with respect to Σ c .
(13)
3.3. Identification of Contact Position
3. Identification of Contact conditions We will identify contact conditions such as contact type, position, and force from the measured data contaminated with noise.
3.1. Force and Moment Equilibrium The equilibrium equation of force and moment is represented by rc × f o + n c = n o . (5) In Eq. (5), the measurable parameters are f o and n o , while immeasurable ones are nc and rc from Assumption (A4). In order to clarify expression of the equation, we use the following notations. (6) a := f o , A := −[a ×] , b := n o , x := rc . From k times of active sensing motion, we have bi∗ = Ai∗ x ∗ + n ci∗ , i = 1,2, L , k ,
∗
bi = Ai x + nci + ε ni − Ε fi x .
where
R := [rx , ry , rz ] ∈ ℜ
(10)
(7) ∗
where the subscript “i” implies the i-th value, • means the true value of • , and x remains unchanged by active sensing. From Assumption (A5), the measured force ai and moment bi are expressed by the following equations.
Ai = Ai∗ + Ε fi , bi = bi∗ + ε ni , (8) where Ε fi := −[ε fi ×] . Substituting Eq. (8) into (7) yields bi = Ai x ∗ + nci∗ + ε ni − Ε fi x ∗ . (9) Note that the force data are contaminated with noise, and Eq. (9) includes the unknown term “ Ε fi x ∗ ”. In the Section 3.3 and 3.4, we will find an effect of the term on estimates.
3.2 Generation of Deviated Data We generate deviated force and moment, whose averages are equal to zero, because four contact types shown in Fig.2 are treated even when the averages of measured force and moment are not zero. Deviated force and moment from the average are given by 1 k 1 k a i := a i − ∑ a j , bi := bi − ∑ b j , k j =1 k j =1
561
In this subsection we use the method of Ref. [11] for identification of contact conditions. Since an average of component of ν i is equal to zero, we identify x by minimizing a performance index 2 1 k J = ∑ Ai x − bi . (14) k i =1 The estimate is given as 1 k T 1 k ∑ Ai Ai xˆ (k ) = ∑ AiT bi . k k i =1 i =1 Substituting Eqs. (8), (9) and (11) into (15) yields 1 k ∑ ( Ai∗ + Ε fi ) T ( Ai∗ + Ε fi ) xˆ (k ) k i =1 . 1 k T ∗ ∗ ∗ ∗ = ∑ ( Ai + Ε fi ) ( Ai x + n ci + ε ni ) k i =1
(15)
(16)
Here, from Assumptions (A5)~(A7), the estimate of contact position, xˆ (∞) , is xˆ (∞) = lim xˆ ( k ) k →∞
2 σ fy = x* +
2 2 2 + σ fz + γ fy + γ fz γ 2fz + γ 2fx x∗ 0 0 σ 2fz + σ 2fx + γ 2fz + γ 2fx 2 2 γ fx + γ fy 0 0 2 2 2 2 σ fx + σ fy + γ fx + γ fy γ 2fy + γ 2fz
0
0
, (17) where the covariance of true value of measured force ai∗ is represented by
Cov[a ∗ ] = diag[σ 2fx , σ 2fy , σ 2fz ] .
(18)
The bias of the estimate xˆ from true value is given by the second term of Eq. (17). In order to remove the bias from xˆ , we use Assumption (A5) in which the covariance of noise of force is known. We replace Eq. (15) with the following equation, and identify the contact position.
1 k T 1 k ∑ Ai Ai - Γ xˆ ′(k ) = ∑ AiT bi , k k i =1 i =1
θ > λ1 λ > θ > λ 1 2 λ > θ > λ 2 3 λ 3 > θ
(19)
where
Γ := diag[ γ
2 fy
+γ
2 fz ,
γ
2 fz
+γ
2 fx ,
γ
2 fx
+γ
2 fy
].
(20)
Here, Eq. (19) coincides with the estimate by CLS [15]. From Assumptions (A5)~(A7), it is guaranteed that the estimate xˆ ′(k ) is asymptotically unbiased. xˆ ′(∞) = x ∗ (21)
(31)
λ ■λ1(∞)
The estimated value of contact moment with noise is represented by νˆi (k ) := bi − Ai xˆ ′(k ) . (22)
●λ2(∞)
θ
▲λ3(∞)
Point Soft finger
From Eqs. (8), (10) and (11), Eq. (22) is expressed by ∗ ∗ ∗ ∗ νˆi (k ) = Ai x + nci + ε ni − ( Ai + Ε fi ) xˆ ′(k ) . (23)
Line
Plane
Fig. 3 Relation between contact types and eigenvalues
Using Assumptions (A5)~(A7) and putting k close to ∞ , ∗ ∗ νˆi (∞) = lim νˆ i (k ) = n ci + ε ni − Ε fi x = ν i . (24)
4. Identification of Unknown Parameters In the previous section, the estimated contact position xˆ ' (k ) and moment with noise νˆi (k ) were obtained, and it was shown that the contact types could be identified in case of the infinite times of active sensing motion. In that case, however, the estimate error cannot be ignored. Also, we did not consider the constraint of contact moment such as Eq. (3), when the contact position was estimated in Section 3.3. Therefore, in this section, we investigate the finite time case and propose a method for more accurate identification of unknown parameters. A performance index is represented by 2 1 k J type = ∑ Ai x + n ci − bi . (32) k i =1
k →∞
So it is guaranteed that νˆi (∞) in Eq. (24) coincides with ν i in Eq. (12).
3.4. Identification of Contact Types In this subsection we analyze the relation between contact types and the eigenvalues of covariance matrix obtained by estimated contact moment νˆi . From Eq. (12), we define a covariance matrix of ν i . (25) M := Cov [ν ] From Eqs. (1) and (4), the matrix M becomes M = M ′ + Γn + [ x * ×]Γ f [ x * ×]T ,
for a point contact type for a soft finger contact type for a line contact type for a plane contact type
(26)
where
M ′ = RVRT . (27) M depends on contact position x , noise of force ε f and moment ε n . M ′ is independent of them. Here, we define N (k ) : 1 k N (k ) := ∑νˆi (k )νˆi (k )T − Γn − [ xˆ (k )×]Γ f [ xˆ (k )×]T . (28) k i =1 When we put k close to ∞ in Eq. (28) from Assumption (A7), N (∞) = M ′ . (29)
4.1 Case of Point Contact Type In case of point contact type, contact moment nci is restricted to zero. The index of Eq. (32) is represented by the same form as Eq. (14). Considering the noise of force, the estimate xˆ is given by Eq. (19).
4.2. Case of Soft Finger Contact Type In case of soft finger contact type, contact moment is restricted to the form: ′ = [rz1 rz 2 rz 3 ]T nczi ′ , nci = rz nczi (33)
Performing the singular value decomposition, the matrix N (k ) is rewritten by 0 0 λ1 (k ) N (k ) = [r1 , r2 , r3 ] 0 λ2 (k ) 0 [r1 , r2 , r3 ]T , (30) 0 0 λ3 (k ) where λ1 (k ) ≥ λ2 (k ) ≥ λ3 (k ) .
where r z denotes the unit normal vector at contact point, and remains unchanged. If the element of maximum absolute value of rz is defined as rz max , the moment can be rewritten by [1 y1 y 2 ]T z i for rz max = rz1 n ci = [ y1 1 y 2 ]T z i for rz max = rz 2 . (34) [ y y 1]T z for r 2 i z max = rz 3 1 For example, in case of rz max = rz1 , the index is represented by 2 1 k J s = ∑ Ai x + yz i − bi , y = [1 y1 y 2 ]T . (35) k i =1
Using the matrix N (∞) and Eq. (3), we can identify the contact conditions by the method of Ref. [11]. The distribution of eigenvalues in case of each contact type is illustrated in Fig. 3. If a threshold θ is set at a value larger than 0 , we can judge which contact type happens.
562
When we identify the unknown parameters by minimizing the index J s , it is necessary to solve the following nonlinear equations. ∂J s ∂J ∂J ∂J = 0, s = 0, s = 0, s = 0 . (36) ∂x ∂y1 ∂y 2 ∂z i Here, z i is estimated as z i = yˆ + (bi − Ai xˆ ) ,
(37)
where
y + = [ y1+
y2+
y3+ ] := ( y T y ) −1 y T =
[1 y1
y2 ]
1 + y1 + y 2 2 2
.
Considering the noise of force, Eq. (37) can be rewritten by z i = z i* − yˆ + (ε ni − E fi xˆ ) . (38) Eq. (38) implies that z i has the bias from true value. From Eq. (38), x, y1 , and y 2 are estimated by 1 1 1 T A2 iT zi + Dˆ 2T A3 iT zi + Dˆ 3T ∑ Ai Ai − Γ ∑ ∑ k k k x 1 1 2 ˆ y1 ∑ A2 izi + Dˆ 2 ( zi ) − ζ 0 ∑ k k y2 1 2 1 A z + Dˆ ˆ 0 ∑ ( zi ) − ζ 3 k ∑ 3i i k 1 T T T ∑ ( Ai bi − A1 i z i ) − Dˆ 1 k 1 + , (39) = b2i z i − γ ny 2 yˆ 2 ∑ k 1 b3i z i − γ nz 2 yˆ 3+ ∑ k where bi1 Ai1 Ai = Ai 2 , bi = bi 2 , bi 3 Ai 3
γ 2fx ( xˆ 2 yˆ 2 − xˆ3 yˆ1 ) Dˆ 1 2 ˆ T −1 D2 := ( yˆ yˆ ) γ fy ( xˆ3 − xˆ1 yˆ 2 ) × Dˆ γ 2 ( xˆ yˆ − xˆ ) 3 fz 1 1 2 ,
and select the index J s .
4.3. Case of Line Contact Type In case of line contact type, contact moment is restricted to the form: ry1 ry 2 nci = rz1 rz 2
T
′ ry 3 ncyi ′ . rz 3 nczi
(40)
The index is represented by Eqs. (32) and (40). In a similar way of soft finger contact type, we can identify unknown parameters. This formulation is omitted for lack of space.
4.4. Case of Plane Contact Type In case of plane contact type, we cannot identify unknown parameters by the active sensing method.
4.5. Algorithm for Identification of Contact Conditions Summarizing the result above, the algorithm for identification of contact conditions is proposed as follows. Step 1: Command active sensing motion. Step 2: Calculate deviated force and moment from Eq. (10). Step 3: Estimate contact position xˆ from Eq. (19). Step 4: Estimate contact moment νˆ from Eq. (22). Step 5: Calculate matrix N from Eq. (28) and analyze the eigenvalues λ1 , λ2 , λ3 . Step 6: Judge which contact type happens by Eq. (31). Step 7: If it is judged as soft finger contact type, then unknown parameters are re-estimated by Eqs. (38) and (39). Step 8: If it is judged as line contact type, then unknown parameters are re-estimated. Step 9: Go to Step 1. By using this algorithm, the contact conditions can be identified continuously by active sensing motion. Also, in case of plane contact type, unknown parameters cannot be identified, however contact type can be judged.
5. Simulation
+ T + T ζˆ := yˆ (Γn + [ xˆ ×]Γ f [ xˆ ×] )( yˆ ) .
Therefore, we summarize the result as follows [11]. Step 1: Initial values are set as the estimates in section 3. Step 2: x, y1 , y2 are fixed and z i is identified by minimizing J s by Eq. (37). Step 3: zi is fixed and x, y1 , y 2 are identified by minimizing J s by Eq. (39). Steps 2 and 3 are repeated until some convergence condition is satisfied. In fact, though the normal vector rz is unknown, the eigenvector r1 , which is close to rz , is obtained by Eq. (30). Therefore, we can set as rz = r1
563
In this section, we demonstrate the effectiveness of our proposed method by using some simulations. The measured force f o and moment no are generated under the following conditions, where nc follows Eq. (3).
x * = [2.00, 2.00, 2.00]T , R = diag[1,1,1] ,
Cov[ f c ] = diag[ 2.00 2 , 1.50 2 , 1.00 2 ] , V = Cov[nc' ] = diag[ 2.00 2 ,1.50 2 ,1.00 2 ] , Γ f = Cov[ε f ] = diag[0.10 2 ,0.20 2 ,0.30 2 ] ,
Γn = Cov[ε n ] = diag[0.10 2 ,0.20 2 ,0.30 2 ] . Figs. 4 and 5 show the distribution of eigenvalues calculated by the method of Ref. [11] and that of our proposed method, respectively. These figures are obtained
[3] D&u&tre , S., et al., “Contact Identification and Monitoring Based on Energy,” Proc. IEEE Conf. Robotics and Automation, pp. 1333-1338, 1996. [4] Horn, R., A. and John, C. R., Topics in Matrix Analysis, Cambridge Univ. Press, 1991. [5] Hyde, J. M., and Cutkosky, M. R., “Contact Transition Control: An Experimental Study,” Proc. IEEE Conf. Robotics and Automation, pp. 363-368, 1993. [6] Kaneko, M., et al., “Active Antenna,” Proc. IEEE Conf. Robotics and Automation, pp. 2665-2671, 1994. [7] Kitagaki, K., et al., “Methods to Detect State by Force Sensing in an Edge Mating Task,” Proc. IEEE Conf. Robotics and Automation, pp.701-706, 1993. [8] Mason, M. T. and Salisbury, J. K., Robot Hands and the Mechanics of Manipulation, Cambridge, MA, MIT Press, 1985. [9] Mimura, N, and Funahashi, Y., “Parameter Identification of Contact Conditions by Active Force Sensing,” Proc. IEEE Conf. Robotics and Automation, pp. 2645-2650, 1994 [10] Mimura, N., et al., “An Algorithm for Identification of Contact Conditions,” Trans. Jpn. Soc. Mech. Eng., Vol. 63, No. 610, Series C, pp. 2061-2068, 1997 (in Japanese). [11] Mouri, T., et al., “Identification of Contact Conditions from Contaminated Data of Contact Moment,” Proc. IEEE Conf. Robotics and Automation, pp. 585-591, 1999. [12] Nagata, K., et al., “Pose Estimation of Grasped Object from Contact Force or Joint Data of Manipulator,” SICE, Vol. 28, No. 7, pp. 783-789, 1992 (in Japanese) [13] Oussalah, M., “Fuzzy linear regression for contact identification,” Proc. IEEE Conf. Robotics and Automation, pp. 3616-3621, 2000 [14] Salisbury, J. K., et al., “Interpretation of Contact Geometries from Force Measurements,” Proc. 1st Int. Symp. on Robotics Research, pp. 565-577, 1983. [15] Stoica, P., and Soderstorm, T., “Bias Correction in Least Square Identification”, Int. J. Control, 35, pp. 449-457 [16] Tsujimura, T., and Yabuta, T., “Object Detection by Tactile Sensing Method Employing Force/Torque Information,” IEEE Trans. Robotics and Automation, Vol. 5-4, pp. 444-450, 1989. [17] Ueno, N., and Kaneko, M., “Contact Localization by Multiple Active Antenna,” Proc. IEEE Conf. Robotics and Automation, pp. 1942-1947, 1999. [18] Yamada, T., et al., “Identification of Contact Conditions from Contaminated Data,” Trans. Jpn. Soc. Mech. Eng., Vol. 64, No. 618, Series C, pp. 584-489, 1998 (in Japanese).
by 100 trials and 100 times of active sensing motion per 1 trial. In Fig. 4, the eigenvalues are not separated into two types of the variance, which means the variance of noise and that of moment. These eigenvalues depend on unknown contact position, as explained in Eq. (26). Hence, the threshold cannot be determined, and the contact type cannot be judged. Compared with Fig. 4, the eigenvalues shown in Fig. 5 are clearly separated into the two types. From Eq. (31), the contact types can be judged if the threshold θ is selected between 0 and 1.02 (=min{ σ nx2 , σ ny2 , σ nz2 }). From Fig. 5, it can be set as about 0.4. Figs. 6 and 7 show the estimates of contact position and normal direction in case of soft finger contact type, respectively. Figs. 6(a) and 7(a) show linear estimates, which are estimated by the method of Section 3. Figs. 6(b) and 7(b) show typed estimates which are re-identified as described in Section 4. Both linear estimates and typed ones are converged to a true value. However, Fig. 6 shows that the typed estimates are more accurate than the linear estimates.
6. Conclusion The identification of contact conditions was investigated when the shape of grasped object is unknown and both force and moment data are contaminated with noise. First, it was shown that the bias of estimates occured if the previous method was used. Secondly, a method of obtaining an asymptotically unbiased estimate of contact position was established, where the variance of noise of force and moment were evident by the sensor calibration beforehand. The relation between contact types and the eigenvalues of covariance matrix of estimated contact moment was analyzed. Thirdly, an algorithm for more accurate identification of unknown parameters was proposed. Finally, the effectiveness of our proposed method was demonstrated by numerical examples. This method can easily estimate the normal direction in case of soft finger contact type and the direction of contact line in case of line contact type. This method can be applied to the case where a walking robot recognizes contact conditions between floor and its own legs.
References [1] Akella, P. N., and Cutkosky, M. R., “Contact Transition Control with Semiactive Soft Fingertips,” IEEE Trans. Robotics and Automation, Vol. 11, No. 6, pp. 859-867, 1995. [2] Bicchi, A., et al,. “Contact Sensing from Force Measurements,” Int. J. Robotics Research, Vol. 12, No. 3, pp. 249-262, 1993.
564
[19] Zhou, X., et al, “Contact Localization Using Force/Torque Measurements,” Proc. IEEE Conf.
1
1
10
10 λ3
λ1
θ=1.0
0
10
-1
10
0
10
θ=0.4 -1
10
40 60 80 Number of sensing times k
10
100
20
(a) Point contact λ2
20
40 60 80 Number of sensing times k
100
0
10
10
λ2
λ3
θ=0.4
10
-1
10
-2
20
40 60 80 Number of sensing times k
θ=1.0
λ2
λ3
λ1
2.2
100
x1
x2
x3
2.1 2.0 1.9 1.8 20
100
40 60 80 Number of sensing times k
100
(b) Typed estimates Fig. 6: Contact point
λ2
λ3
0
10
θ=0.4 -1
10
-2
-2
20
40 60 80 Number of sensing times k
10
100
20
1
0
θ=1.0
-1
10
λ1
λ2
λ3
40
60
80
Number of sensing times k
(d) Plane contact Fig. 4: Ref. [11]
10
1
100
0.8
10
0
0.4 0.2 0.0
-0.2 20
θ=0.4
10
100
10
rz1 rz2 rz3
0.6
40 60 80 Number of sensing times k
100
(a) Linear Estimates
-1
λ1
-2
20
Eigenvalues λ
10
10
40 60 80 Number of sensing times k
1.0
(c) Line contact
(c) Line contact
Eigenvalues λ
40 60 80 Number of sensing times k
1
-1
10
20
(a) Linear estimates
0
10
λ1
10
1.8
100
Estimated normal direction rz
10
1.9
(b) Soft finger contact
Eigenvalues λ
Eigenvalues λ
1
x3
1
(b) Soft finger contact 10
x2
2.0
λ2
λ3
-2
20
40 60 80 Number of sensing times k
(d) Plane contact Fig. 5: This paper
100
Estimated normal direction rz
-2
40 60 80 Number of sensing times k
λ1 θ=1.0
0
10
10
λ3
Eigenvalues λ
Eigenvalues λ
λ1
10
x1
2.1
(a) Point contact
1
-1
2.2
Estimated contact point x
20
10
λ3
-2
-2
10
λ2
Estimated contact point x
λ2
Eigenvalues λ
Eigenvalues λ
λ1
10
Robotics and Automation, pp. 1339-1344, 1996.
1.0 0.8
rz1 rz2 rz3
0.6 0.4 0.2 0.0
-0.2 20
40 60 80 Number of sensing times k
(b) Typed estimates Fig. 7: Normal direction
565
100
