RSD-1 8-87 Design and Motion Constraints of Part-Mating Planning in the Presence of Uncertainties Jing Xiao Richard A. Volz Robot Systems Division Center for Research on Integrated Manufacturing The University of Michigan Ann Arbor, Michigan 48109 September 1987 Center for Research on Integrated Manufacturing Robot Systems Division College of Engineering The University of Michigan Ann Arbor, Michigan 48109-2110

RSD-TR- 18-87 TABLE OF CONTENTS 1. Introduction.................................................................................. 1 2. System Framework & General Restrictions............................... 3 3. Error M odel......................................................................... 4 4. Peg-in-hole Study................................................ 6 5. Conclusions.................................................................................. 22 Acknowledgements.............................................................................. 22 References.............................................................................................. 22 i

Design and Motion Constraints of Part-Mating Planning in the Presence of Uncertainties Jing Xiao Richard A. Volz Robot Systems Division* Center for Research on Integrated Manufacturing University of Michigan September 10, 1987 Abstract In order to achieve the ultimate goal of automatically generating assembly programs for robots from design information, it is necessary that one be able to devise part mating strategies that will work in spite of sensor, control and manufacturing errors. In general, this is almost certainly unachievable. However, if appropriate design constraints are made, progress can be made. In this paper. we assume a nominal motion plan for the zero error situation, and then devise a simple replanning strategy based upon the availability of force, moment and position sensors to handle errors that may arise during program execution. We develop design constraints relating the parameters of the strategy. parameters of the sensor, control and manufacturing errors, and nominal design parameters. If the constraints are satisfied, the replanning strategy can theoretically be guaranteed to be successful. The constraints are shown to be reasonable in the sense that they do not impose unrealistic conditions on typical designs. Simulation results uphold the theoretical derivations and show empirically that the theoretical constraints can be relaxed somewhat with excellent results still obtained. 1 Introduction The manual development of robot programs for assembly is a laborious, error prone task. It would be highly desireable to be able to derive them automatically from design information. In order to do so, several important problems must be solved: 1) *This research was supported by the Air Force Office of Scientific Research under contract number F33615-85-C-5 105. 1

automatic determination of an assembly plan (sequence of assembly operations), 2) automatic determination of parts presentation and fixturing, 3) gross motion planning, 4) fine motion planning, and 5) automatic generation of error recovery routines. This paper addresses the last of these problems. It has often been stated that the largest part of any robot assembly program is made up of fixups to handle things that don't quite work as planned. Algorithms for generating these fixups are crucial to successful automatic program generation. Problems in the execution of nominal robot programs arise for the following reasons: * mechanical and control errors: robots and all other mechanical devices are only accurate to within certain bounds; * sensor errors: all sensors have limited sensitivity and accuracy; * manufacturing tolerances: different instances of the same model are not exactly identical because of manufacturing limitations. Often such errors can lead to the failure of a nominal program that would theoretically accomplish a task such as part mating. The automatic generation of robot programs for assembly cannot be successful until some means of either avoiding or correcting these errors is found. Several approaches towards handling these uncertainties have appeared in the literature. Taylor[ll] introduced an error-propagation method to estimate compound errors from the uncertainty bound or tolerance of each individual part involved in a task (including the robot hand). Brooks[l] extended Taylor's method by making the process backward, so that the constraints on some compound errors to make the task succeed can lead to constraints on individual parts. This method. however, suffers from high computational complexity. And it does not provide means to reduce errors dynamically. The inductive learning approach by Dufay and Latombe[3] corrects runtime errors by adding rules into the system as a corrective plan. In this approach. error-handling is not fully automatic. since rules must be provided by human users. The pre-image approach introduced by Lozano-Perez, Nason, and Taylor[6], and simplified by Erdmann [4] incorporates the effect of uncertainty directly within one planning phase. The goal is to create motion plans that will avoid errors. However. this approach has unsolved theoretical problems and its applicability to practical cases is questionable. A more practical force control method was developped by N Whitney[12]. His remote center compliance device can correct small insertion errors for a peg-in-hole task by applying correct forces to the peg. But it only works when the peg is in or partly in the hole. All the previous approaches contribute to solve the problem in some ways, but none fully solves it. The problem is so complex that new solutions are still in need. The approach presented in this paper is based upon three hypotheses: 1. The problem cannot be solved in general. Design constraints relating nominal parameters, tolerances and error parameters must be satisfied if success is to be guaranteed. 2

Nominal PlI Patch-Plan Verifier _ _ — | World Model Sensors - Replanner Figure 1: System framework 2. A two phase planning process, an off-line nominal planner and an on-line replanner to correct for run-time errors, simplifies the overall system. 3. Replanning can be based upon knowledge of which surfaces of the parts involved are in contact with which other surfaces. Desai [2] has introduced the notion of contact formations to describe the different topological contacts among the parts being assembled. and has developed a technique for, under certain design constraints, uniquely determining the contact formation in which the parts lie in spite of sensor and control errors. When an error occurs in a nominal plan. an unintended contact occurs among surface elements of the parts involved. While there can, in general. be a large number of contact formations. there are typically only a modest number that are achievable with small deviations from the nominal plan. Thus. one can consider devising replanning strategies based upon the contact formation in which the sNystem completes an attempted move (we presume that there are force/moment guards that stop motions that make unintended contacts). Further. since the actual positions may be presumed to be close to the nominal trajectory. simple replanning strategies, such as straight sliding motions or single axis rotations may be considered without having to worry about obstacle avoidance. In this paper. we develop such simple replanning strategies and determine design and motion constraints, which if satisfied. can be used to guarantee that the replanning strategy will be successful. 2 System Framework and General Restrictions The system framework relevent to the research is shown in Fig. 1. The system consists of three parts: the verifier, the replanner, and the plan controller. Given the task environment(i.e. the robot, the assembly parts, and the sensors), a nominal plan, and a world model which consists of geometrical and physical descriptions of the task 3

environment, the verifier determines contacts among the assembly parts whenever an unexpected collision occurs, and then the replanner generates patch-plans to correct errors. Each patch-plan is a path connecting the unexpected configuration to one of the expected configurations so that the nominal motion can continue. The plan controller plugs in a patch-plan to the nominal plan whenever an unexpected contact is verified. It acts like a switch controlled by the verifier. We restrict the replanning to motions in contact[5], for errors are more crucial and more easily identified when contact occurs. Also, the desired goal of most partmating operations involves contacts among the parts involved. We further assume that compliant motions [8][9][10] are applied wherever necessary. 3 Error Model For a successful replanning, obviously, it is necessary to establish an appropriate error model which covers all (dominant) static and dynamic (including sensory) errors in a system. We begin by discussing the error model we use for sensory controls and robot motions. As we know, sensor and robot motion errors (which consist of mechanical and control errors) are major errors in a system. We suppose that only a position/orientation sensor and a force/torque sensor are available. Then, under the general restrictions discussed in the last section, the following system uncertainties must be modeled: * position/orientation sensor uncertainty; * robot motion uncertainty (resulting in both position and orientation errors at the destination); * force/torque sensor uncertainty. In the following subsections, parameters to model those three kinds of uncertainties are defined respectively. 3.1 Modeling Position/Orientation Sensor Uncertainty Let (Pa, <a) denote a location in the system configuration space constrained by the Compliant Surface (C-Surface), where Pa refers to the position and ~a refers to the orientation. Let (P,, 4,) denote the corresponding sensed position and orientation on a position/orientation sensor. Definition 1:'p is defined as the positional uncertainty of a position/orientation sensor, iff ep > 0, and it has the smallest value satisfying the condition that VPa, VP,, if Pa is sensed by the position/orientation sensor as P,. then IIP. - Pall -< P. 4

Definition 2: Ce is defined as the orientational uncertainty of a position/orientation sensor, iff e0 > 0, and it has the smallest value satisfying the condition that V<a, V),J if 4a is sensed by the position/orientation sensor as 0,, then II - 4all < Co. 3.2 Modeling Force/Torque Sensor Uncertainty Let F, denote a force sensed by a force/torque sensor. Let Fa be the actual force. Definition 3: ef is defined as the force uncertainty of a force/torque sensor, iff ef > 0, and it has the smallest value satisfying the condition that VFa, VF,, if Fa is sensed by the position/orientation sensor as F,, then IFS- Fall < Cf. Let M, denote a moment sensed by a force/torque sensor. Let Ma be the actual moment. Definition 4: Em is defined as the momental uncertainty of a force/torque sensor iff Cm > 0, and it has the smallest value satisfying the condition that VMa, VMS, if Ma is sensed by the position/orientation sensor as M3, then IM, - Mal < Cm. 3.3 Modeling Motion Uncertainty Let vd denote the desired linear velocity of a robot for a desired straight-line translation. Let Va denote the actual linear velocity. Definition 5: e, is defined as the linear velocity uncertainty of a robot 1 iff 6e, > 0, and it has the smallest value satisfying the condition that VVd. VVa, I||Vd- Val <_ ~-. Let Wd denote the desired angular velocity of a robot for a desired pure rotation. Let wa denote the actual angular velocity. Definition 6: e is defined as the angular velocity uncertainty of a robot, iff eI > 0, and it has the smallest value satisfying the condition that V'd. VL'a, lK\d -'all K< -w During a pure translation, a robot should not rotate. However, since there are various errors, such as mechanical, control, and sensory errors. a translation may not be so pure and the robot may rotate slightly. For the same reason, a not-so-pure rotation results in translation of the robot. To model these phenomena, let variable w) represent the "illegal" angular speed of the robot in an actual translation, and let variable represent the illegal" linear speed of the robot in an actual rotation. The following parameters can be defined. 5

Definition 7: wt is defined as the translational onrientation-uncertainty-rate of a robot,iff w > 0, and it has the smallest value satisfying the condition that Vwt, IKall < Wt. Definition 8: Vr is defined as the rotational position-uncertainty-rate of a robot, iff Vr > 0, and it has the smallest value satisfying the condition that Vva, all < V'. 4 Peg-in-hole Study Peg-in-Hole tasks are most commonly studied in the area of uncertainty handling. But previous research either does not guarantee the success of the task or is too complex to be practical. Another important issue that hampers the previous progress is that different shaped pegs and holes require seemingly different uncertainty handling strategies. And apparently, the more complex the peg-in-hole structure is. the more complex the strategies should be. In this section, however, we apply our new approach to peg-in-hole tasks by introducing a simple replanning strategy, and discussing proper design and motion constraints necessary to the success of the strategy. For a peg-in-hole task, it is most possible that the peg is not put properly above the hole before the insertion, so that the peg hits the entry surface of the hole and the task fails. The replanning that is of interest starts from that point. The objective is to move the peg above the hole, with error within the tolerance, so that the insertion can be done. ext we state esome restrictions or assumptions of the study: * the replanning motion is compliant, and the Compliant surface (C-surface) is the entry surface of the hole; * the entry surface and the bottom surface of the peg which contacts the entry surface are planar; * the bottom surface of the peg fully contacts the entry surface of the hole, so that the task space has only two translational and one rotational degrees of freedom(Fig. 2); * there are no other objects (i.e. obstacles) except for the peg and the hole. 4.1 Replanning Strategy and Constraints Let (PP, OP) and (Ph, o^h) be the locations of the peg and the hole respectively. Let da = Ph - P, with da = |dall be the actual distance. Let A<a = b - <>. Ideally, since it is assumed that tathere is no obstacle around, the motion for replanning can be simply a (compliant) translation of the peg in the direction of d: 6

Figure 2: Peg-in-hole task space rotate slide 4 d a Figure 3: Simple replanning strategy in ideal case 7

with moving length I = da, and then a rotation of the peg to make sure that a proper OP is obtained for the peg to enter the hole (Fig. 3). However in a real case, because * only a sensed d, for da and a sensed AO, for Aa can be possibly obtained, * d, and A<, are different from da and Aa respectively due to sensory errors, * there are also motion errors, that ideal strategy with only one translation and one rotation can not guarantee the success of replanning. A modified, more realistic strategy must be set up. In essence. the proposed strategy consists of more than one motion step. Each desired motion step is formed by a (compliant) translation along the direction of the sensed d., and a rotation to correct the orientation error caused by the translation (if necessary). The distance I of the translation is constrained with respect to both the sensor error in d, and the motion error in actual moving, so that after each motion step, d: always becomes smaller. In this way, the positional replanning can be convergent. After the success of the positional replanning (i.e. after da is less than the positional tolerance of the task), the peg is rotated to correct Ac. Note that since motion error contributed to Ama is corrected in each motion step, there must be A$a < e,. (recalling that eo is the uncertainty bound in orientation sensing). If a rotation step can correct A<a, while not destroying the success in position, the success of the whole replannning can be guaranteed. For realizing the strategy, developing proper design constraints and motion constraints (on I of each motion step) is fundamental. In the following subsections, we will discuss constraints for successful replanning in two situations: * only position/orientation sensing is available: * in addition to position/orientation sensing, force/torque sensing is also available. 4.2 Using Position/Orientation Sensing only For convenience, yet without losing generality, we restrict our study to those peg-andholes with regular polygon bases. Let rh be the nominal radius of the inner tangent circle of a hole. and r, be an actual radius. Let rp be the nominal radius of the inner tangent circle of the corresponding peg, and rp be an actual radius. Definition 9: 6h is defined as the tolerance of the hole, iff 6h > 0, and it has the smallest value satisfying the condition that Vrh, llrh - rhll < bh. Definition 10: 6p is defined as the tolerance of the peg, iff 6, > 0, and it has the smallest value satisfying the condition that Vrp, lrp - rpal < ah. 8

da S a b Figure 4: Relationship between d" and d, Definition 11: The tolerance of the task is defined as 6, such that 6 = r- rp - -h - 6pOur objective is to seek relations among the position/orientation sensor uncertaintvy p, CE: the motion uncertainty c,., E,e. vr, Lt': the (desired) nominal linear speed t'd and angular speed L'd of the robot; and the task tolerance 6. such that under those relations (or constraints), the success of the replanning strategy (described previously) can be assured. Let dP indicates the sensed distance vector. i.e.. d= P -P (1) where P in d- indicates position sensing. and d" = ilidti indicates the sensed dis ance. Fig. 4 shows the relationship between da and d'. Obvius!y. the position sensor uncertainty e; may cause dP to differ from da both in direction and in magnitude. Consider when d, < 2t (as shown in Fig. 4a). the direction of dr can be as bad as the opposite of the direction of da. And from the magnitude dP. it can not be assured whether the goal relationship in position between the peg and the hole (i.e. da <') has been achieved. Consequently. the next replanning motion can not be determined. However. in Fig. 5 where dP > 2p. the angle = arcsin( d-) indicates the difference between the direction of dP and the direction of da in tworstcase. (Note that the worst-case direction of motion is obtained by finding the line tangent to circles of radius ep centered at PP and Ph respectively). Clearly 9 becomes bigger as dP gets smaller. Thus, for a small dP > 2ep, 9 can still be large enough that the direction of dP, namely the commanded direction of the motion, mary not result in 9

Cp a (worst-case) CP S c h S p C a Figure 5: The sensed direction vs. the worst-case actual direction cP S n P C' a Ch a p e worst-case motion direction Figure 6: The worst-case direction under position sensin reducing d,. Thus. we need to determine a bound. I,. on 0 such that if \\0i\ < F,. the direction of motion can be controlled to those directions that lead to the reduction of da. Since when the robot actually moves, its mechanical and control error also results in direction uncertainty. we must have the constraint 0 < O,. + Or < 7r/2 (2) hold(Fig. 6). where szn(^) = =-/t'd (3) (Fig. 7). It is obvious that the determination of Op depends on the value of e, and the nominal speed Vdd. We call the parameter 6p the maximum, angle allowed between dP and d,. Recalling the relationship between da and dP (Fig. 4). when dr 2ep da, <.-p sin(OP) 10

p A 3 A e Figure 7: Relationship between e, and 0, | -Hole —/P Hole -,O.* P Pp a Figure 8: The position sensing uncertain area da might be as small as zero and is bounded by 0 < d, < d' + 2ep < dp where dp= i( + 2e. sin(O\ P) (4) Call dp the minimum distance allowed by position sensing. Then the area inside the circle about P' of radius dp can be defined as the position sensing uncertain area. denoted by Ut as in Fig. 8. If positional replanning is to be successful. we must require that d < 6. i.e. the position sensing uncertain area is contained in the goal position area of the peg. However, to also guarantee the success of the orientational replanning. the requirement must be stronger. According to our replanning strategy. the orientational replanning takes place when the positional replanning succeeds, i.e., when da < 6 and Al< ~ ~. Thus. the orientational replanning is a simple rotation to correct A<) until the peg enters the hole. To accomplish this, we must guarantee that after rotating the peg Aoa the peg will still be in a goal position. In other words. if db(< dp) indicates 11

r Figure 9: lrar and motion constraint the actual distance between the peg and the hole before the rotation. and Ad, indicates the distance of the illegal translation caused by the rotation. the requirement do + d, < 6 must be satisfied. For the requirement to hold. the following design constraint is sufficient: d + Vro < 6. (5)'d - d In order to make the positional replanning convergent. in addition to the design constraint ( 2 ). proper motion constraint on the length of the translation step I must be enforced. I must be less than some maximum value Ima (as shown in Fig. 9). so that after each motion step (i.e. a translation of distance I and a rotation to correct the orientation error caused by the translation if the error exceeds the orientation sensing uncertainty c,). the actual distance between the peg and the hole is shorter than that before the motion step. In particular. it can be shown that 2(d, - 26p)(cos(arcsin(2,/d<) +,) - f) lr,,= (6) 1 - f2 where dr is the sensed distance. and f = (7) (Vd - f-)(,d - ) Also. for I < ln,.a the following design constraint is necessary: f < cos(p + O,). (8) Due to motion error c,, the actual distance of translation la may be different from the desired distance of translation 1. Specifically, we have IIl- la < Et 12

Figure 10: The peg partially overlaps the hole where t = l/t'd is the desired time of translation, assuming that the timer is perfect. Thus. to guarantee the convergence of the replanning. we must have,t. < I < Ima1 - t which lead to the design constraint Vd > 2e, (9) and the motion constraint Ilma, < I < (1- -).Irmar: (10)'d td In summary. to guarantee the success of both the positional and the orientational replanning. the design constraints formulated as inequalities ( 5 ). ( 2 ). ( 8 ), and ( 9 ). and the motion constraint expressed by ( 10 ) have to be inforced. 4.3 Using Force/Torque Sensing to Compensate Position/ Orientation Sensing 4.3.1 General Discussion As shown previously, the area inside the circle about Ph of radius d4 (see equation 4) is defined as the position sensing uncertain area. denoted by Up as in Fig. 8. When only a position/orientation sensor is used to guide the replanning. the position sensing uncertain area must be contained in the goal position area of the peg. i.e. dp < 6 must be satisfied. so that the replanning can succeed. However. if force/torque sensing is also involved. the constraint dp < 6 can be weakened or even dropped. Consider the situation where PP is in Up. The fact that a force/torque sensor can possibly sense a moment when the peg is partially overlapping the hole(Fig. 10) suggests us that even if the peg reaches the position uncertain area, the replanning process may not have to be terminated, since an ineffective position sensing might be compensated by an effective moment sensing to continue guiding the replanning. 13

Figure 11: Directional difference between M, and Al, in worst case In the following discussion, we will define the moment-sensible area, denoted S,,, as the set of actual position volume of the peg for which moment sensing alone can 1. detect the goal locations: 2. determine a direction of motion along which d: can be reduced. Ob-iously we will then want S, D Up. In order to describe Sm, more precisely and use the above containment relation to obtain design constraints, we need to introduce two parameters Orr and dm. with the idea similar to that behind the introduction of 6O and dp. As an alternative to determining the direction of motion from the position sensor. we can determine a direction of motion from the moment sensed, i.e., the direction of motion would be orthogonal to the projection of M, onto the contact plane. Hence. the uncertainty in the commanded direction of motion can be estimated exactly as the directional uncertainty of M,. Fig. 11 shows the directional difference between M, and Mh in worst case. as indicated by the angle 0. The smaller is.,. the bigoer 6 can be. Thus in analogy to the introduction of Op. we need to introduce a bound 8,r on 0. such that when 1[j11 < m,, in other words when - sin,M Ms gives a direction of motion along which da can be reduced. Also in analogy to the case of position sensing. the design constraint 0 <. + 8M < r/2 (11) where a8, is expressed by equation ( 3 ), is necessary while taking into account the motion error in direction. The determination of Om depends on the value of S.. We call the parameter Om the maximum angle allowed between the sensed moment M, and the actual moment Ma. It is easily observed that given a specific environment (i.e. the robot, the solid peg and hole, and the force applied to the peg by the robot, etc), the relative location 14

I7 Liu~~ U O a condiFigure 12: Relationship betat Vd if de >d then such that at (d R. M < (ie (done can not g)arant to determines thean adquctual moment (applied to the forcetorque sensor).ed moment). Then Vd Vparameter if d d,. and it is possibl ca be to determined. a direction of ~~Rrn~~~~~~~. ~ Definition 12: dof called the mabimnmi moment-sensing distan ce allou ed of a pegFigure(as shown in Fig. 12a), thbeten and R, in-hole task with a force/torque sensor, has the largest value (> 0) satisf-ing the condition that Vda. if da > dm. then 3Ao,. such that at (d,. AOa). A, < Son. (i.e. tions' one can not guarantee to determine an adquate d' from the sensed moment). Then V2.d i Aa if d o < dm Atn, > sin and it is possible to determine a direction of of motion from tae moment sensed that guarantee to reduce d,. dr is a function of. cf. 0m,. the sensed force F, and the task geometry. Denote the circular area centered at P b of radius d, by Rm. Then clearly W, r Rm. As the result of the above definitis both posiionssss s it is obious that if Ua C R (as shown in Fig. 12a), then 1. in combination, the oto a foe ran dtec the s e te goal locations: 2. in combination, the position and force/torque sensors can determine a direction of motion along which da can be reduced; since the position uncertain area is compensated by the moment-sensible area. \Wherease, if Up D RM, since the area UIp-Rm is both position sensing uncertain and moment sensing uncertain(Fig. 12b), adding force/torque sensing can not help in droping the constraint dp < 6 to guarantee the success of the replanning. Note that one can increase dm by increasing the applied force. 15

Figure 13: A top view of cylindrical peg and hole 4.3.2 Cylindrical Peg-in-Hole Case Study In general. it is very difficult to obtain constraints under which the replanning strategy can always be successful. when both position/orientation and force/torque sensings are involved. It is still one of our research topics. However. for a special case: cyindrical peg and hole (CPH) tasks. we have obtained an analysis on the design and motion constraints that shows the applicability of the idea (i.e. using both the position/orientation and the force/torque sensing to guide the replanning). Consider a cylindrical peg-in-hole task (Fig. 13). First. we need to find out how to determine d,. so that the constraint dp < d, (12) (or Lp C R, as shown in Fig. 12a) can be explicitly expressed. Then we need to find out how to estimate da from force/moment sensing when the position of the peg is within L r. so that lir.n and the motion constraint that lead to the reduction of d: can be formed. Note that for CPH tasks. orientations do not need to be considered due to s-mmetry. Thus, each replanning step does not involve a rotation. and the whole replanning is positional only. Therefore. orientation-related uncertainties c. e. v and "w: will not appear in the constraints. For convenience, let the center of the base of the peg CP be the reference position of the peg. and the center of the base of the hole CA be the reference position of the hole. Let rp be the nominal radius of the peg and 6p denote the tolerance of the peg (defined as in the polygon case). Let rh be the nominal radius of the hole. and 6b be the tolerance of the hole. Then. similar to the regular polygon case (discussed previously), we can define the tolerance 6 of a cylindrical peg and hole case: 6 = rh - rp - h - 6p. For simplicity, in the following discussions, we will not count for 6h and b6, for it can be seen from the derivations below that the effects of 6h and 6, are negligible. W\e now use a simple and rather conservative method to determine dm and to estimate d,. Let F, be the value of the sensed force. Let IS be the value of the 16

d a Figure 14: C[ is on the left of line AB sensed moment about CP. Consider the situation shown in Fig. 14 where C. is on the left of line AB. Let the distance between CAB (i.e. the center of the line AB) and CP be A. Then it can be easily shown that da (rp + rh) (13) The next step is to estimate A from Fa and M,, so that an estimated value for da can be obtained. since we can not get the exact da. Note that we always want to tnderestimate da in order to get a conservative Ir,a for each translation step 1. Assume that the contact force distribution is symmetric about the line going through the two centers. The real action-point of the contact force should be within the shaded area. and somewhere on the line. Let PJ F Then R, is the sensed distance between the action-point and CT. Since the actual distance between the action-point and CP must be greater thar or equal to A. taking into account the sensing uncertainties f- and f, the following must hold: M, + >r, >. Fs -~ - If we use M, + m, Rmar -= F- (14) to replace A in equation ( 13 ), then we get da, such that d = /(rp + rh) + R - Rma (15) Clearly, d < da 17

Figure 15: Possible range for dm i.e. d' underestimates da. Note that it makes sense to use this method to estimate da only when C7 is on the left of line AB. Therefore. dm in this case. should be less than or equal to the distance between CP and C' when CP = CAB (Fig. 15). More explicitly, dM < (rp+rh). Since it is extremely difficult to obtain the contact force distribution. As the result. the exact d, following definition 12 is almost impossible to be known. Thus. we can only expect to get a close dm sufficient to the condition that Vda if da < dm. AM, > s (16) sin 0m so that it is possible to determine a direction of motion from MI that guarantee to reduce da. Here we introduce a simple method to determine such a d,. Since FaA < Maf (see Fig. 14 and Fig. 15). and it is sufficient for ( 16 ) to hold if MI, -em > m sin 0 thus it is sufficient for ( 16 ) to hold if Fa - em > sin 0, or A (+m 1 A > (1+ i ) Fa sin Om From the above inequality and equation ( 13 ), we can finally have fm 1 cm(1 1 d = /6( + rp) + (T(1 + 1i O)) a- )(1 + O7) V rs, sinF o a( sin Vm 18

Since we usually only get F,, replace Fa in ( 17 ) by F. - ef, we get the alternative dm: d (rh + rp) + + (F - 1 + i - + (18) mV h l, - f sin O F, - cf sin m Now the design constraint ( 12 ) can be explicitly expressed by ( 4 ) and ( 18 ). Under this and the other design constraint expressed by inequality ( 2 ), when dP >.ip) position sensing is effective. On the other hand, when dP <, -- the $ sin (9;sin(Gp)' robot motion should follow dm determined by force/moment sensing, subject to the further design constraint ( 11 ) holding. The design constraint ( 9 ) also has to be enforced so that the motion constraint in the form represented by inequality (10 ) can hold. It can be easily shown that in CPH case Pla = 2( - 2ep) cos(80 + arcsin(2ep/d)), (19) where P indicates sensing by position sensor, and dP is formulated by equation ( 1 ), and that, ma = 2dc cos(0, + arcsin(em/M,)), (20) where m indicates sensing by force/moment sensor, and dm is formulated by equation ( 15 ). In both equations ( 19 ) and ( 20 ), v, is the angle formulated by equation ( 3 ). In summary, in order for the replanning in CPH case to succeed, the design constraints ( 12 ). ( 9 ), and 0 < arcsin(e./vd) + max(p, 0m) < (21) (where the value of Op and Om can be determined by the constraints), and the motion constraint n< I < <(1 - - a if dP > Si(p) (22) Vd mt'x d Im r< < (1 - )a)Im otherwise (23) Vd t'd where lP a is expressed by equation ( 19 ) and IMa is expressed by equation ( 20 ), have to be satisfied. 4.4 Simulation Results To test the constraints and the replanning strategy, we designed computer programs to simulate the replanning process for both the regular-polygon-based peg-in-hole (RPH) cases and the CPH cases. The computer simulation we have done shows that the design constraints obtained are correct and practically reasonable, and that under the design constraints, the motion constraint is necessary for guaranteeing the success and efficiency of the strategy. Furthermore, we observed that as long as the design and motion constraints are satisfied, 19

* the replanning process is not sensitive to changes in the initial relative positions of the peg and the hole, * the replanning process is not sensitive to changes of the value of c, over a reasonably large range, * for RPH tasks, the error effects of Vr, wt are negligible, * for CPH tasks, the introduction of force/moment sensing can guarantee the strategy to be successful with a 6 much less than it must be when only position sensing is available. All observations above show the advantage of the replanning strategy. In particular, the second observation means that the motion error will not significantly affect the replanning strategy. Therefore, a rough over-estimation of e, could be enough for use of the strategy. The third observation suggests that no rotation to correct the effects of Vr and wt will be necessary, which simplifies the strategy. The last observation is most exciting, which shows that using force/moment sensing to compensate position/orientation sensing may greately improve the quality of the replanning process, and it means that more research on this may eventually widens the range of the part-mating tasks that can be automatically done by robots. Table 1 and Table 2 show some of the simulation results for CPH cases, given the following position sensor and force/torque sensor uncertainties 1 p =0.025 mm ef = 0.014 kg Em = 0.740mm-kg. We also choose the commanded linear speed of the robot to be Vd = 20 mm/s, with motion uncertainty e, = 1.0 mm/s. Here we use * MAX#SS: Maximum Number of Steps to Success, and * MEAN-#SS: Mean Number of Steps to Success as parameters to evaluate the replanning strategy, under the condition that the success is guaranteed by all design and motion constraints being satisfied. We also use the parameter %SUCCESS as the percentage of successful replanning cases when not all design constraints are satisfied. In Table 1 and Table 2, the statistical parameters were obtained by running over 100 trials, while MAX#SS and MEAN'#SS were'The values chosen were based on commercially available sensors. 20

p = 80~ dp= 0.101 mm 6 (mm) MAX#SS MEAN#SS %SUCCESS d < 6 0.105 6 2 100 satisfied 0.101 7 3 100 0.095 7 3 67 violated 0.090 8 3 45 Table 1: Use position sensing only p = 0 = 80 dp = 0.101 mm rh = 5.0 mm Fa= 1.0 kg r,(mm) 6(mm) dm(mm) MAX#SS MEAN#SS %SUCCESS dp< dr, 4.968 0.032 0.103 4 2 100 satisfied 4.970 0.030 0.097 4 2 100 violated 4.975 0.025 0.081 4 2 99 4.980 0.020 0.065 5 3 97 4.985 0.015 0.049 6 3 92 4.987 0.013 0.043 6 4 85 4.990 0.010 0.033 8 4 64 ___ Table 2: Use position and force/torque sensing 21

among successful trials. The two tables show how the task tolerance 6 affects the replanning strategy without and with force/moment sensing respectively. Here dp was obtained by equation ( 4 ), and dm by ( 17 ). Table 1 shows that the design constraint dp < 6 is just necessary since when it is violated, %SUCCESS decreases sharply. But from Table 2, we see that even when dp is greater than the computed dM, the replanning strategy still performs well. That suggests that the selection for dm could be relaxed a little, and the real case could be more optimistic than what we get theoretically. 5 Conclusions With the idea of exploring the relationships between tolerances for successfully accomplishing a task and bounds on uncertainties so that an uncertainty handling strategy can guarantee the success, we have developped a replanning motion strategy and the design and motion constraints necessary for the success of the strategy for peg-in-hole tasks. We hav-e also interpreted the constraints and tested the results by simulation. Our next step is to establish a testbed on the puma 560 robot, to further explore the applicability of the strategy. Acknowledgements We would like to thank Professor J. R. Barber for his helpful suggestions. References [1] R. A. Brooks, 1982. Symbolic error analysis and robot planning. Int. J. Robotics Res. 1(4):29. [2] R. S. Desai, and R. A. Volz, 1986(Sept.). Contact formations - a new approach to local motion in mechanical assembly. IBM Technical Report, Robotics Research Laboratory, Department of Electrical Engineering and Computer Science, University of Michigan. [3] B. Dufay. and J. C. Latombe, 1983(Aug.). An approach to automatic robot programming based on inductive learning. 1st International Symposium on Robotics Research. [4] M. Erdmann, 1986. Using backprojections for fine motion planning with uncertainty. Int. J. Robotics Res. 5(1):19. [5] J. Hopcroft, and G. Wilfong, 1986. Motion of objects in contact Int. J. Robotics Res. 4(4):32. [6] T. Lozano-Perez, M. T. Mason, and R. H. Taylor, 1984. Automatic synthesis of finemotion strategies for robot. Int. J. Robotics Res. 3(1 ):3. 22

[7] T. Lozano-Perez, 1983. Task planning. Robot Motion: Planning and Control, M.Brady et al. Eds. [8] M. T. Mason, 1981. Compliance and force control for computer controlled manipulators. Robot Motion: Planning and Control, M.Brady et al. Eds. [9] M. T. Mason, 1982. Compliant motion Robot Motion: Planning and Control, M.Brady et al. Eds. [10] M. H. Raibert, and J. J. Graig, 1981. Hybird position/force control of manipulators. Robot Motion: Planning and Control, M.Brady et al. Eds. [11] R. H. Taylor, 1976. The synthesis of manipulator control programs from task-level specifications. AIM-282. Stanford Univ. [12] D. E. Whitney, 1977. Force feedback control of manipulator fine motions J. Dynamics Sys. Measurement, and Control. 98:91. 23

'3