WB5-01
Proceedings of the 2005 IEEE/ASME International Conference on Advanced Intelligent Mechatronics Monterey, California, USA, 24-28 July, 2005
Localization for Robotic Assemblies Using Probing and Particle Filtering Siddharth R. Chhatpar Mechanical and Aerospace Engineering Dept Case Western Reserve University 10900 Euclid Ave., Cleveland, OH 44106, USA
Michael S. Branicky Electrical Engineering and Computer Science Dept Case Western Reserve University 10900 Euclid Ave., Cleveland, OH 44106, USA
[email protected]
[email protected]
Abstract— This paper deals with the class of robotic assemblies where position uncertainty far exceeds assembly clearance, and visual assistance is not available to resolve the uncertainty. Specifically, we focus on the assembly of a key in a lock, with uncertainty in the position of the lock in (x, y, z). Under this scenario, we implement a localization strategy that resolves the uncertainty using a pre-acquired map of key-lock contact configurations. Prior to assembly, the strategy explores the contact configuration space (C-space) by using the key to probe the stationary (fixtured) lock-surface at various different positions and matching the contact configurations thus recorded with the map. Thus, the strategy is progressively able to localize the lock-position in (x, y, z) and achieve assembly. However, with a sampled map of the contact C-space, discretization errors are introduced, and implementing deterministic matching (at the fine-grained level necessary for assembly) would soon become prohibitively expensive in terms of computation. Additionally, with global initial uncertainty, multiple solutions abound in our localization problem. Here, we use a particle filter implementation, which can not only handle the discretization errors in map-matching, but also track multiple solutions simultaneously. The particle filter implementation was highly successful in localizing the lock-position, reducing the uncertainty by more than 95% and making it easy for a compliant strategy to achieve assembly. Results from 50 trials of the lock-key assembly are reported.
I. INTRODUCTION There is a class of robotic assemblies where position uncertainty far exceeds the assembly clearance, and visual assistance is not available to help resolve the uncertainty. Hence, there is no definite a priori trajectory to lead the mating parts into assembly. This shifts the focus from the dynamics of the assembly to the problem of searching for part alignment. Thus, the automated assembly has to rely on search strategies, and the assembly task becomes a problem of motion planning under uncertainty, with or without the use of feedback. To fix ideas throughout the paper, we use a particular example of this type of assembly, described below. Our problem is the insertion of a key in a lock, with large initial uncertainty in the position of the lock in (x, y, z). The assembly set-up is shown in Fig. 1. Needless to say, the assembly clearance for the lock and key is very small. The initial uncertainty in the position of the lock in (x, y) is bounded by a circle of radius 10 mm. Also, the position of the lock along the vertical (z) is unknown. Thus, to insert the key in the lock, we have to get the key in close alignment
0-7803-9046-6/05/$20.00 ©2005 IEEE.
Fig. 1. Assembly set-up: fixtured lock and ParaDex robot [10] holding key
with the key-hole, which can be anywhere within a circle of radius 10 mm. The above lock-key example can be subsumed under a more general problem of localizing a probe on a surface under position uncertainty. This problem can be formulated as follows. A. Problem Formulation For the lock-key assembly we limit the uncertainty to the location of the lock in (x, y, z). The rotation of the lock about all three axes is assumed to be known. However, our overall goal is to tackle the more complete problem of position and orientation uncertainty in the full six-dimensional space. For example, consider the problem of localizing a probe on a surface (shown in Fig. 2), with bounded uncertainty in the position (x, y, z) and the orientation (rotations about x, y, z) of the surface. Let FS be the coordinate frame attached to the surface, and FP represent the probe coordinate frame. There is uncertainty in the position and orientation of the surface, and hence also FS . The goal of localizing the probe on the surface can be expressed in terms of finding the transformation TSP from FP to FS .
1379
Fig. 2.
Coordinate frames attached to the probe and surface Fig. 3.
Sampled map of the contact surface for the lock-key problem
B. Localization Strategy We propose a strategy that localizes the relative probesurface configuration (TSP ) using a pre-acquired map of the probe contact-configurations (i.e., configurations of the probe while in contact with the surface). Prior to assembly, the strategy explores the contact configuration space by sequentially bringing the probe into contact with the surface at various different positions and matching the contact configurations thus recorded with the map. Consider n contact point observations, Oi = (xi , yi , zi ), i = 1, . . . , n, corresponding to n different configurations of the probe relative to the surface. The surface is fixtured and so FS is stationary. The observations correspond to n distinct probe-positions— effected by the robot and hence, known. Our problem is to find the tranformation TSP by matching these observations to the stored map of the contact surface. For certain regular surfaces it might be possible to generate the contact surface map analytically. However, for most real-world applications (eg. our lock-key problem) it would require painstaking effort to generate the analytical map. On the other hand, the contact surface map for such problems can be sampled using a robot with minimum operator intervention. For example, Fig. 3 shows a sampled map of the contact C-space for the lockkey problem. C. Localization Strategy Using Particle Filters The implementation of our localization strategy requires the matching of each observation to the map to progressively reduce the uncertainty. In an error-free, simulated assembly, deterministic matching using either an analytical map or a sampled map can guarantee uncertainty resolution up to any required degree of accuracy. However, the errors in measurement and actuation associated with an actual robotic assembly would plague the deterministic approach with the curse of dimensionality. In the mobile robot localization field, widespread success has been reported with the use of particle filters for map-matching even in the presence of large errors [1]. Particle filters allow us to concentrate resources on promising solutions without completely discarding slightly less promising ones, which can turn out to be the real solutions camouflaged by error. This approach also allows us
to track multiple possible solutions, which occur frequently in our particular problem. (E.g., for the lock-key problem, if the key touches the elevated portion of the lock, all the points on that surface become possible solutions to the observation.) Our aim in implementing the localization strategy using particle filters is to reduce large configuration uncertainties to smaller values in a relatively short time. As mentioned before, it is possible to achieve any required degree of accuracy in localization depending upon the degree of discretization of the sampled map. However, a coarsely sampled map is advantageous in terms of storage size and sampling time. Also, there is a drop in time-efficiency in localizing beyond a certain degree of accuracy [2]. Moreover, from an assembly line point of view, to accommodate critical cycle-time requirements [3], we need to comply with a preset maximum localization time. Hence, after a pre-determined number of iterations we switch from the localization strategy to a compliant form of assembly [4] that can handle the reduced uncertainty—as will be described below—or a blind search [5], [6], [7]. Below we present the algorithm for our implementation. We represent the coordinate transformation from the probe coordinate frame to the surface coordinate frame by TSP . Our goal is to find the actual transform between the probe and surface, Tact . The map is a coarse collection of contact coordinate transformations, where the probe is in contact with the surface. We represent the map as T map . A belief set, representing the best estimate of the current probe-surface coordinate transformation, is given by Belk = {TSPik , wi }, for i = 1 . . . n, where n is the number of particles. Here, each TSPik is an estimate of the current (k th iteration) probesurface coordinate transformation, and is called a particle. The weight, wi , associated with each particle, represents the confidence level for that estimate. There is global (yet bounded) initial uncertainty in the position of the surface. Hence, we initially distribute the particles uniformly throughout this bounded region. Also, the weights are set equal. In the algorithm presented below, the belief set is updated after every probe move (each probe move can be represented as a
1380
P
coordinate transformation, TPkk+1 ) by advancing each particle P by TPkk+1 , and matching it to the map. With each iteration, the belief set (ideally) gives an improved estimate of the actual probe-surface coordinate transform. The algorithm exits with success when the probe-surface transform has been estimated with sufficient accuracy. If the algorithm is not successful within a fixed number of iterations, then localization is terminated. However, an estimate is still computed based on a weighted average of the particles. Step 0: • • Step 1: • • • • • Step 2: • • • Step 3: • Step 4: •
Set k = 0 Initialize Bel0 , wi = 1/n, ∀i = 1 . . . n Move key, update k = k + 1 Update particles: P P TSik+1 = TPkk+1 TSPik , ∀i = 1 . . . n Match each particle to the map: min P Erri = j {TSik+1 − Tj }, Tj ∈ T map Assign relative weights: wi = N(Erri ; 0, σi ), ∀i = 1 . . . n Resample particles according to these weights, then reset weights: wi = 1/n, ∀i = 1 . . . n Check Belk+1 : if satisfactory ⇒ goto Step 3 Check Iteration Number ⇒ goto Step 4 Goto Step 1 (Localization successful) (Localization terminated)
II. T WO - DIMENSIONAL A SSEMBLY E XAMPLE The implementation of our localization strategy using particle filters can be suitably illustrated with the following two-dimensional example. Consider the probe-surface set-up shown in Fig. 4. There is uncertainty in the surface-location along x (horizontal), and along y (vertical). To implement our localization strategy, we generate a contact C-space map. This map is a collection of coordinate transformations from the probe-frame to the surface-frame with the probe contacting the surface as shown in Fig. 6(a,b,c). (The map is an inverted image of the surface because it represents the surface positions w.r.t. the probe.) Fig. 6(a) also highlights the initial coordinate transform TSP0 , which forms a point on the map.
3 2
D. Organization of the Paper This paper is organized as follows. In Sect. II, we explain the implementation of our localization strategy with a simple two-dimensional example. We also draw comparisons between the use of particle filters in the field of mobile robot localization and our implementation. In Sect. III, we describe in detail the lock-key assembly implementation, and report results from actual robotic assemblies. For additional information and to see a video of the assemblies described here, see
Probe
FS
0 −1
FP
P
TS
y
1
The above algorithm is explained in detail with the help of a simple two-dimensional example in the following section. In related research, Chhatpar and Branicky [2], [7] apply the above localization strategy to implement round and square peg-in-hole assemblies. For these assemblies, if the peg is tilted w.r.t. the hole, then a very informative contact hyper-surface is obtained. They map this contact hypersurface and use the map to localize the hole-configuration w.r.t. the peg in (x, y) for the round peg-in-hole and in (x, y, θ) for the square peg-in-hole. Among other closely related research is the work done towards sensing by probing and point sampling [8]. The main goal of work in this area is to identify part pose by probing efficiently with a minimum number of probe points. The probing can be done using a set of mechanical probes or using a set of point light emitters and detectors to avoid contact with the part.
http://dora.case.edu/˜src/AIM05.
Finally, Sect. IV draws conclusions and discusses future work.
x 0
2
Surface 4
6
8
10
Fig. 4. Two-dimensional example: coordinate frames on probe and surface
Once the contact C-space map has been acquired, localization can be implemented. The probe is moved to a random point1 in (x, y) and is brought into contact with the surface. At this time, there is global uncertainty and hence the particles are distributed uniformly throughout the uncertainty region, as shown in Fig. 6(a). The weights are all set equal and normalized. This forms our initial belief set, Bel0 . Now the probe is moved to another random contact point1 and the probe-move between the two contact points is noted. Let the probe-move be TPP10 as shown in Fig. 5(a). According to Step 1(b) of the algorithm given in Sect. I-C, we update the particles by translating each by TPP01 . Now each updated particle is matched to the map (Step 1(c)) and the weights are updated (Step 1(d)). The matching error can be computed in two steps: first, we interpolate the mapped configurations to find the point that matches the particle in x. Then, we find the error in y between the interpolated point and the particle. We update the relative weight associated with each particle based on this error. The weight is computed from a Gaussian distribution on the error with a mean of zero and a variance, σ 2 , that is lowered with each iteration. As in Step 1(e), we resample the particles, according to the updated (normalized) weights, with replacement. At this time we may have multiple copies of several particles, and we can add a small random disturbance to each particle to increase 1 Can also move to fixed points (Sect. III), or choose move actively (Sect. III).
1381
variation. The weights of the resampled particles are reset to equal. The resampled particles are shown in Fig. 6(b). As can be seen, the particles become concentrated around the regions of low error (in this case, the particles bunch up in two separate regions, hence, two different solutions are possible). This computation is repeated at every iteration with each new probe move. In the current example, it can be seen from Fig. 6(c), that after the second stage of particle resampling the particles are concentrated around the actual probe-surface transform and localization is successful.
1 P
TS 0
0 −1 −2 −3 −10
Particles −8
Map −6
−4
−2
0
1 P
TP 1
0
0
−1 3
New probe position
2
P
−2
Old probe position
1
T
0
−3 −10
P
TS 1 −8
−6
2
0
P
TP 2
0 0
−2
1
0
P1
−1
−4
4
6
8
1
10 −1
3 −2
Old probe position
2
New probe position
−3 −10
P
P
TS 2
TS 1
−8
−6
−4
−2
0
1
T
0
P1
Fig. 6. (a) Uniformly distributed particles representing global initial uncertainty, (b) Particles resampled after first probe move, (c) Probe successfully localized on surface
P2
−1
0
2
Fig. 5.
4
6
8
10
Probe moves: iterations 1 and 2
There are many similarities between our problem and the mobile robot localization problem (MRL), in particular, the global localization problem [1], [9]. Global localization involves determining the robot’s pose (position and orientation) by matching observations to a map of the environment, starting from global initial uncertainty. The MRL problem is complicated by uncertainties in the action and perception of mobile robots. In our problem, although the discretization of the map introduces some error, actions and observations can be assumed to be highly accurate. This is because of the high accuracy and repeatability of the ParaDex robot [10], which we use for our assembly experiments. Thus, our focus is not so much on tracking as on localizing the actual probe-surface misalignment from global initial uncertainty by eliminating the multiple solutions possible. Most approaches to the MRL problem use a probabilistic formulation to represent and update the pose of the robot. Two main approaches are Markov localization and Kalman filter-based localization. In particular, Markovian methods use a discrete representation of the probability distribution over the pose-space. Finegrained grid approximations [11], while providing higher resolution, are computationally expensive. Recently, Monte Carlo methods, like particle filtering, have been employed to overcome this problem [1]. With particle filters the probability distribution can be maintained over a set of sampled states (or particles) and the number of particles can be adjusted depending on the computational resources available. In our
problem, even coarsely-sampled maps of the contact surface contain close to 104 configurations. Hence, maintaining a probability distribution over the entire map would be computationally very expensive. Thus, particle filters provide a tractable means for implementing our localization strategy. III. I MPLEMENTATION For a general probe-surface localization problem, the uncertainty space can extend to six dimensions. Even though our strategy is applicable in the full six-dimensional uncertainty space, here we consider a restricted uncertainty region for the example lock-key localization problem as a first step. We restrict the lock-uncertainty to three dimensions: position in (x, y, z). This restriction is valid because the uncertainty in the orientation of the lock plane is eliminated through fixturing. Also, very small orientation errors can be handled by the particle filter implementation without having to incorporate the complete map in six-dimensional space. The implementation is carried out in two steps: mapping and localization. Before the localization strategy can be implemented, we need to acquire a map of the contact surface. To avoid the painstaking effort required to generate an analytical map for arbitrary contact surfaces, we choose to use a sampled map. The sampling of the contact surface map is carried out in the mapping stage using a robot as described below. We use the ParaDex robot (whose gripper can be seen in Fig. 1) for both mapping and localization. The ParaDex is a closed-chain mechanism with six parallel
1382
A. Mapping As mentioned in Sect. I, the lock-position uncertainty is bounded by a circle of radius 10 mm. We also have to set a bound on the allowed key (probe) motion. We bound the key motion-region by a circle of radius 10 mm. Thus, the total (x, y) region of relative key-lock positions to be mapped is given by a circle of radius 20 mm.2 To keep operator intervention in the mapping process at a minimum, we start the process with the key inserted into the lock. The mapping algorithm interprets this initial position as point (0,0) in relative key-lock coordinates. It then disassembles the key from the lock and moves it to each (x, y) relative key-lock coordinate to be mapped. At each point it moves the key along z into contact with the lock surface and records the sampled (x, y, z) coordinate. The set of coordinates to be mapped is easily generated by the algorithm if it is given the lock uncertainty-region bound, the key motion-region bound, and the discretization parameter for the mapped region. The discretization parameter decides how dense the map will be—the denser the map, the more accurate it is, but takes longer to sample. Note that the relative key-lock coordinates mapped give the lock coordinate frame w.r.t. the key coordinate frame. Once the map has been sampled, it can be used to implement localization.
are intended to force maximum coverage of the map, and thus produce maximum diversity in the probe-surface contact configurations. We recorded 50 trials for the lock-key assembly. Localization exits successfully if all the particles are contained within a circle of 2 mm radius. Otherwise, localization is terminated after a fixed number of iterations, and the key-lock coordinate transform estimate is set to equal a probability-weighted average of the particles. C. Results Out of the 50 assembly trials, we had 45 successful assemblies, where the key was able to insert into the lock. Thus, 90% of the trials were successful. However, for three of the successful insertions, the error in the estimation was slightly higher than 2.0 mm and so for these trials the localization phase was classified as a failure. Fig. 7 shows the error in the estimated position of the lock for 47 trials (estimates from 3 trials are off the plot). Resolving the
3
Lock position estimates
2
r=2.33mm 1
y (mm)
prismatic actuators. A seventh degree of freedom from a central tool roll (about an axis parallel to the six actuators) provides kinematic redundancy. High stiffness, low friction, and a high internal payload make the ParaDex especially suitable for force control applications. For further details, see [10].
0
−1
r=2.0mm −2
Actual lock position
−3
B. Localization and Assembly As mentioned above, the lock-position uncertainty is bounded by a circle of radius 10 mm. Our goal is to localize the lock-position to within a circle of radius 2 mm. After the uncertainty is resolved to within this region, we can attempt insertion of the key into the lock using a compliant strategy. The compliant strategy simply attempts to press the key into the lock and permits the key to be moved horizontally by the contact forces generated. This strategy is successful because around the key-hole there is a spherical depression (of approximately 2 mm radius) designed to guide the key into the key-hole. The selection of the probe move at each iteration can be achieved in one of three ways: from a fixed set, at random, or actively. Active localization involves finding the action (in our case, probe move) that most efficiently resolves the current uncertainty [12]. We plan to implement active localization in the future. Currently for the lock-key assembly, we use a combination of fixed and random moves. We first move the probe to the south, east, north, and west boundaries of the probe motion region before switching to selecting the probe moves randomly. The fixed probe moves 2 The maximum relative distance possible between the key and lock coordinates is 20 mm. Eg. suppose the lock is at (10,0) in (x, y) and the key is moved to (-10,0).
−3
−2
−1
0
1
2
3
x (mm)
Fig. 7.
Results from 47 localization trials for the lock-key assembly
position uncertainty from a circle of radius 10 mm to a circle of radius 2 mm corresponds to an uncertainty resolution of 96%. 43 out of the 50 assembly trials were successful in resolving the uncertainty by at least this amount. To see a video of the ParaDex performing the lock-key assembly described above, see http://dora.case.edu/˜src/AIM05. IV. CONCLUSIONS AND FUTURE WORK Our primary goal in this research was to implement our localization strategy using probing and particle filtering. With errors arising from the discretization of a sampled map and uncertainties in observations, any deterministic algorithm would suffer from the curse of dimensionality [13]. Among the many probabilistic formulations available, the particle filter implementation specifically allows us to track multiple possible solutions, which abound in our problem. We tested the particle filter implementation on the lockkey assembly. 90% of the assembly trials were successful in bringing about the insertion of the key in the lock. We were
1383
able to achieve uncertainty resolution of more than 95% in 43 of the 50 assembly attempts. In future work, we would like to extend the uncertainty to the full six-dimensional space. To maintain the map to approximately the same size as current, we will need to use a much coarser sampling of the contact C-space. Moreover, the number of solutions possible for a given sequence of observations will also be much greater. This should prove to be an interesting challenge for our particle filter implementation.
20
15
Actual lock position
10
y (mm)
5
0
−5
−10
Initial uncertainty region
−15
−20 −20
−15
−10
−5
0
5
10
15
20
5
10
15
20
5
10
15
20
x (mm)
V. ACKNOWLEDGMENTS (a)
This work was supported by the NSF Embedded and Hybrid Systems program, under the direction of Dr. Helen Gill (grant CCR-0208919); it does not necessarily reflect the views of the NSF.
(b)
20
15
R EFERENCES
10
[1] Thrun, S.: Probabilistic Algorithms in Robotics. AI Magazine, 21(4):93–109, (2000). [2] Chhatpar, S.R., Branicky, M.S.: Localization for Robotic Assemblies with Position Uncertainty. Proc. IEEE/RSJ Intl. Conf. Intelligent Robots & Systems, Las Vegas, NV, pp. 2534–2540, (2003). [3] Gravel, D.: Forced assembly. Assembly Magazine, (2002). [4] Newman, W., et al: Towards Automatic Transfer of Human Skills for Robotic Assembly. Proc. IEEE/RSJ Intl. Conf. Intelligent Robots & Systems, Las Vegas, NV, pp. 2528–2533, (2003). [5] Newman, W.S., et al: Force-Responsive Robotic Assembly of Transmission Components. Proc. IEEE Intl. Conf. Robotics & Automation, Detroit, MI, pp. 2096–2102, (1999). [6] Newman, W.S., et al: Impedance Based Assembly. Video Proc. IEEE Intl. Conf. Robotics & Automation, Detroit, MI, (1999). [7] Chhatpar, S.R., Branicky, M.S.: Search Strategies for Peg-in-hole Assemblies with Position Uncertainty. Proc. IEEE/RSJ Intl. Conf. Intelligent Robots & Systems, Maui, HI, PP. 1465–70, (2001). [8] Jia, Y.-B., Erdmann, M.: The Complexity of Sensing by Point Sampling. Workshop on the Algorithmic Foundations of Robotics, San Francisco, CA, (1994). [9] Gutmann, J.-S., Fox, D.: An Experimental Comparison of Localization Methods Continued. Proc. IEEE/RSJ Intl. Conf. Intelligent Robots & Systems, Lausanne, Switzerland, (2002). [10] Wang, Y., et al: Workspace analysis of the ParaDex robot—a novel, closed-chain, kinematically-redundant manipulator. Proc. IEEE Intl. Conf. Robotics & Automation, pp. 2392–2397, (2000). [11] Burgard, W., et al: Estimating the absolute position of a mobile robot using position probability grids. Proc. 13th Natl. Conf. Artificial Intelligence, Portland, OR, pp. 896–901, (1996). [12] Fox, D., Burgard, W., Thrun, S.: Active Markov Localization for Mobile Robots. Robotics and Autonomous Systems, 25:195–207, (1998). [13] Chhatpar, S.R., Branicky, M.S.: A Hybrid Systems Approach to Forceguided Robotic Assemblies. Proc. IEEE Intl. Symp. on Assembly & Task Planning, Porto, Portugal, pp. 301–306, (1999). [14] Hebbar, R., et al: Flexible Robotic Assembly for Power-train Components (FRAPA). Video Proc. IEEE Intl. Conf. Robotics & Automation, Washington, D.C., (2002).
y (mm)
5
0
−5
−10
−15
−20 −20
−15
−10
−5
0
x (mm)
(c)
(d)
20
15
10
y (mm)
5
0
−5
−10
−15
−20 −20
−15
−10
−5
0
x (mm)
(e)
(f)
(g) Fig. 8. The updated belief sets after the first, fourth, and eighth observations for the lock-key assembly. (a,c,e): Key contacting lock-surface. (b,d,f): particles in the belief set. Successive observations shrink the belief set. After eight iterations, lock-position is estimated from the weighted average of the particles. (g): Assembly accomplished by compliant phase using estimated lock-position.
1384
