Rao-Blackwellized Particle Filtering for Probing-Based 6-DOF Localization in Robotic Assembly

Transcription

1 MITSUBISHI ELECTRIC RESEARCH LABORATORIES hp:// Rao-Blackwellized Paricle Filering for Probing-Based 6-DOF Localizaion in Roboic Assembly Yuichi Taguchi, Tim Marks, Haruhisa Okuda TR1-8 June 1, 1 Absrac This paper presens a probing-based mehod for probabilisic localizaion in auomaed roboic assembly. We consider peg-in-hole problems in which a needle-like peg has a single poin of conac wih he objec ha conains he hole, and in which he iniial uncerainy in he relaive pose 3D posiion and 3D angle beween he peg and he objec is much greaer han he required accuracy assembly clearance. We solve his 6 degree-of-freedom 6-DOF localizaion problem using a Rao-Blackwellized paricle filer, in which he probabiliy disribuion over he peg s pose is facorized ino wo componens: The disribuion over posiion 3-DOF is represened by paricles, while he disribuion over angle 3-DOF is approximaed as a Gaussian disribuion for each paricle, updaed using an exended Kalman filer. This facorizaion reduces he number of paricles required for localizaion by orders of magniude, enabling real-ime online 6-DOF pose esimaion. Each measuremen is simply he conac posiion obained by randomly reposiioning he peg and moving owards he objec unil here is conac. To compue he likelihood of each measuremen, we use a map a mesh model of he objec ha is based on he CAD model bu also explicily models he uncerainy in he map. The mesh uncerainy model makes our sysem robus o cases in which he acual measuremen is differen from he expeced one. We demonsrae he advanages of our approach over previous mehods using simulaions as well as physical experimens wih a roboic arm and a meal peg and objec. IEEE Inernaional Conference on Roboics and Auomaion ICRA This work may no be copied or reproduced in whole or in par for any commercial purpose. Permission o copy in whole or in par wihou paymen of fee is graned for nonprofi educaional and research purposes provided ha all such whole or parial copies include he following: a noice ha such copying is by permission of Misubishi Elecric Research Laboraories, Inc.; an acknowledgmen of he auhors and individual conribuions o he work; and all applicable porions of he copyrigh noice. Copying, reproducion, or republishing for any oher purpose shall require a license wih paymen of fee o Misubishi Elecric Research Laboraories, Inc. All righs reserved. Copyrigh c Misubishi Elecric Research Laboraories, Inc., 1 1 Broadway, Cambridge, Massachuses 139

2

3 Rao-Blackwellized Paricle Filering for Probing-Based 6-DOF Localizaion in Roboic Assembly Yuichi Taguchi, Tim K. Marks, and Haruhisa Okuda Misubishi Elecric Research Laboraories, Cambridge, MA, USA Advanced Technology R&D Cener, Misubishi Elecric Corporaion, Amagasaki, Japan {aguchi, Absrac This paper presens a probing-based mehod for probabilisic localizaion in auomaed roboic assembly. We consider peg-in-hole problems in which a needle-like peg has a single poin of conac wih he objec ha conains he hole, and in which he iniial uncerainy in he relaive pose 3D posiion and 3D angle beween he peg and he objec is much greaer han he required accuracy assembly clearance. We solve his 6 degree-of-freedom 6-DOF localizaion problem using a Rao- Blackwellized paricle filer, in which he probabiliy disribuion over he peg s pose is facorized ino wo componens: The disribuion over posiion 3-DOF is represened by paricles, while he disribuion over angle 3-DOF is approximaed as a Gaussian disribuion for each paricle, updaed using an exended Kalman filer. This facorizaion reduces he number of paricles required for localizaion by orders of magniude, enabling real-ime online 6-DOF pose esimaion. Each measuremen is simply he conac posiion obained by randomly reposiioning he peg and moving owards he objec unil here is conac. To compue he likelihood of each measuremen, we use as a map a mesh model of he objec ha is based on he CAD model bu also explicily models he uncerainy in he map. The mesh uncerainy model makes our sysem robus o cases in which he acual measuremen is differen from he expeced one. We demonsrae he advanages of our approach over previous mehods using simulaions as well as physical experimens wih a roboic arm and a meal peg and objec. I. INTRODUCTION This paper presens a probing-based mehod for probabilisic localizaion in auomaed roboic assembly. We consider peg-in-hole problems in which he peg is needle-like has a single poin of conac when probing and in which he iniial esimae of he pose of he peg wih respec o he objec may be quie inaccurae. Pose uncerainy of he peg wih respec o he objec may grealy exceed he assembly clearance he desired accuracy in all 6 degrees of freedom 6-DOF, comprising 3-DOF uncerainy in he peg s posiion and 3-DOF uncerainy in he angle of he peg wih respec o he objec. We use as measuremens conac posiions obained by reposiioning a robo arm ha holds he peg and hen moving he peg in he general direcion of he objec unil here is conac. We assume ha oher han conac deecion and robo arm encoders, here are no oher sensors such as cameras, hough of course anoher sensor could be used o bring he peg ino he viciniy of he par before implemening our algorihm. The goal of our algorihm is o deal wih levels of uncerainy for which spiral search and oher neighborhood search sraegies [1], [] would fail due o large iniial uncerainy, oo many degrees of freedom, and he exisence of numerous local minima. We herefore address he localizaion problem deermining he 6-DOF relaive pose of he peg and objec raher han he dynamics of peg inserion. A. Relaion o Previous Work Here we describe previous work on paricle-based Mone- Carlo localizaion for assembly using probing. Chhapar and Branicky presened a localizaion mehod using probing and paricle filering for lock-key assembly [3] which is quie similar o he needle-like peg-in-hole problem ha we address and round/square peg-in-hole problems [4]. They firs exhausively probe every x, y locaion of he objec wih he peg o generae a conac configuraion-space map, which describes all possible ransformaions in which he peg has a conac wih he objec. Afer his preprocessing sep of dense probing, hey perform paricle filering by sequenially probing he objec and using he conac posiions as he observaions. Thomas e al. [5] similarly describe an exhausive preprocessing sep, densely probing an objec wih a force/orque sensor o generae a force/orque map ha consiss of conac force and orque a every possible conac pose. They also esimaed a force-orque map direcly from a CAD model, bu his esimaed force-orque map was no as accurae as he map acquired by probing, so hey did no use he CADmodel-based map for localizaion. Thomas e al. [5] also used paricle filering o mach each force/orque observaion o he map and o incorporae observaions from a camera. Since he number of paricles required for sandard paricle filering increases roughly exponenially wih he number of dimensions in he search space, he aforemenioned previous mehods, which use paricle filering for all dimensions, are no well-suied for full 6-DOF localizaion. Alhough he formulaions of hese previous mehods are described for 6-DOF uncerainy, in pracice hey were only used for localizaion in lower-dimensional search spaces - or 3- DOF, such as he -DOF case in which uncerainy only exiss in x, y ranslaion. In his paper, we solve he full 6-DOF localizaion problem using a Rao-Blackwellized paricle filer RBPF [6], in which he probabiliy disribuion over posiion 3-DOF is represened using paricles, and he disribuion over angle

4 3-DOF is approximaed by a Gaussian disribuion condiioned on he posiion of each paricle. This facorizaion grealy improves he efficiency of he algorihm, resuling in an orders-of-magniude reducion in he number of paricles and compuaional resources required as compared o sandard paricle filering. We use paricles o represen he posiion of he peg wih respec o he objec, because given weak prior informaion and only a small amoun of evidence, he poserior disribuion over peg posiion ends o be mulimodal. Given he posiion of he peg a he curren and previous ime seps, however, he poserior disribuion over angle is more well-behaved, and hus a Gaussian approximaion is suiable. In our RBPF approach, a each ime sep we firs use paricle filering o updae he probabiliy disribuion over he posiion of he peg. We hen use exended Kalman filering EKF for each paricle o updae he paricle s weigh and probabiliy disribuion over angle, condiioned on he paricle s posiion. We use as he map a mesh model of he objec ha is being probed, which we generae before probing he objec using a CAD model as well as any prior knowledge abou he uncerainy of he CAD model. The mesh model explicily models uncerainy abou he posiion of is faces, edges, and verices. This probabilisic map of he objec s surface enables our algorihm o succeed in siuaions in which measuremens are no consisen wih he CAD model. Such differences can arise from measuremen errors due o numerous facors such as slipping and sensor imprecision, as well as from differences beween he CAD model and he objec ha can arise from causes such as manufacuring limiaions, undocumened design changes, and reverseengineered approximae CAD models. Prior o he acual pose esimaion, previous approaches include an exhausive preprocessing sep in which he objec is probed wih he peg a every possible conac locaion or pose, and he conac posiion or force/orque values are measured. The resuling deailed map is laer used o accuraely evaluae he measuremen probabiliy for pose esimaion. Our new mesh represenaion, which explicily accouns for varying levels of uncerainy in he map, is designed o allow robus localizaion wih only an approximae map, hus obviaing he need boh for a ime-consuming map measuremen preprocessing sep and for soring a large amoun of deailed dense map daa. We describe our sysem model and RBPF inference algorihm in Secion II. Then in Secion III, we demonsrae our algorihm s marked improvemen in efficiency over previous mehods using physical experimens wih a robo arm and a meal peg and objec and simulaions using a virual model of he meal objec as well as a more complex simulaed objec. We conclude wih a brief discussion in Secion IV. II. LOCALIZATION METHOD Our problem is o find he pose of a needle-like peg wih respec o an objec, by probing he objec wih he peg. The 6-DOF uncerainy beween he peg and he objec is represened as s,θ, where s = x,y,z T and θ = α,β,γ T are Conrol Posiion Measuremen Angle u -1 u u +1 s -1 s s +1 z -1 z z +1 Fig. 1. Graphical model showing he probabilisic dependencies of our localizaion mehod. The angle variables θ can be considered as analogous o he map variables in ypical mehods for SLAM in mobile roboics. The shaded nodes represen variables ha are measured, while he whie nodes represen variables ha are inferred. relaive posiions and angles, respecively, beween he peg and he objec. The angles α,β,γ are defined as angles of roaion around each of he x, y, z axes, respecively. The key o our approach is o facorize he probabiliy disribuion over pose separaely ino wo pars: 3D posiion s and 3D roaion θ. The probabiliy disribuion over posiion is represened by paricles, which enables our sysem o represen mulimodal disribuions for posiion. The probabiliy disribuion over roaion angles is represened as a Gaussian disribuion for each paricle, condiioned on he curren and previous posiions of he paricle. The graphical model in Fig. 1 shows he probabilisic dependencies of our sysem. Given he sequence of moion commands from ime 1 o, denoed u 1:, and he sequence of observaions, z :, our goal is o infer he poserior disribuion over he posiion and angle, ps :,θ z :,u 1:. We facorize his poserior probabiliy as follows: ps :,θ z :,u 1: = ps : z :,u 1: pθ s :,z :. 1 This facorizaion enables us o separaely esimae he poserior disribuion over posiion and angle using a Rao- Blackwellized paricle filer [6], as follows. We firs updae he posiion of each paricle using he moion model and compue he paricle s weigh using he measuremen model and he paricle s previous Gaussian disribuion over angles. We hen updae he paricle s Gaussian esimae of angles by using exended Kalman filering EKF. In our framework, herefore, each paricle mainains a 3D posiion esimae s and a 3D Gaussian disribuion over angle, which is represened using he sufficien saisics, mean µ and covariance Σ. Noe ha he observaion variable is binary, z {1,}, where z = 1 if a ime he peg has a conac wih he objec a posiion s, and z = oherwise. However, since we updae he sae only a he ime insan when he robo senses a conac, z = 1 a every ime sep. We herefore simplify he noaion pz = 1 using he shorhand pz hroughou his paper. The facorizaion 1 is well-sudied for simulaneous localizaion and mapping SLAM problems in mobile roboics [7], in which he pose of a mobile robo is esimaed using paricle filering, and each paricle s map is independenly updaed using EKF. Our approach o localizaion in roboic assembly can be viewed as analogous o SLAM in mobile

5 roboics, as follows: Our posiion vecor s is analogous o he pose of a mobile robo, and our angle vecor θ is analogous o he mobile robo s map of he environmen. When RBPF is applied o he SLAM problem [7], each paricle mainains is own esimae of he map ha depends on ha paricle s pose hisory. Analogously, in our sysem each paricle mainains is own esimae of disribuion over angle ha depends on ha paricle s posiion hisory. A. Coordinae Transformaions The posiion of he peg a ime, denoed s, is represened in a frame of reference ha we call he base coordinae sysem. The ransformaion beween he robo coordinae sysem he frame of reference of he robo arm and he base coordinae sysem can be seleced arbirarily; here we assume i is represened by a ranslaion, wihou roaion. Because our base and robo coordinae sysems differ by a pure ranslaion, any moion of he peg is represened by he same ranslaion in he base coordinae sysem as in he robo coordinae sysem. There is also an objec coordinae sysem, which is fixed wih respec o he objec ha conains he hole. Whereas s represens he peg s posiion a ime in he base coordinae sysem, we denoe he peg s posiion in he objec coordinae sysem a ime as s o. The base coordinae sysem and he objec coordinae sysem are relaed by a 3D roaion of θ abou he iniial posiion of he peg, s, as illusraed in Figure. Thus, he iniial posiion of he peg is he same in boh he base and objec coordinae sysems: s = s o. In our seup, θ is he unknown relaive roaion beween he robo and he objec. B. Moion Model A each ime sep, he robo arm moves he peg from one conac posiion one poin of conac beween he peg and he objec o anoher. The posiion of he peg in he robo coordinae sysem is obained from he robo arm s inernal encoders. As we described above, he ranslaion of he peg from ime 1 o ime, which we denoe u, is idenical in he robo coordinae sysem and he base coordinae sysem. The moion of he peg in he base coordinae sysem, from s 1 o s, is herefore given by σ x s = s 1 + u + ǫ, ǫ N, σy, σz where Nµ, Σ represens he mulivariae normal disribuion wih mean µ and covariance marix Σ. In our experimens, we used σ x = σ y = σ z =.1 mm. Since our robo arm has accurae conrol, he moion error is in realiy much smaller even han his small amoun of noise ha we assume. We noneheless include his small noise erm in our moion model o reduce he paricle deprivaion paricle impoverishmen problem. Tha is, none of he paricles will have an exacly correc perfec esimae of he iniial conac posiion s, and including a small noise erm in he moion model enables he paricle filer o compensae for his inaccuracy over ime. To deermine s o, he posiion of he peg a ime in he objec coordinae sysem, we compue a roaion marix R s Moion u 1 z o Objec coordinae sysem z o s 1 x Base coordinae sysem Esimaed pose of mesh model a = 1 x o Esimaed pose of mesh model a = = Fig.. Visualizaion of he base and objec coordinae sysems and of updaes o posiion and angle esimaes. The surface of he objec is represened by gray dashed lines in he base coordinaes sysem or by red lines in he objec coordinae sysem. The posiion of he peg a ime, denoed s, is defined in he base coordinae sysem. The objec coordinae sysem red axes is roaed wih respec o he base coordinae sysem black axes by he angle θ abou he iniial posiion of he peg, s. The mesh model represenaion and he measuremen probabiliy compuaion resides in he objec coordinae sysem. A ime =, each paricle s esimae of he relaive angle θ beween robo and objec coordinaes is a Gaussian disribuion wih mean his value θ = corresponds o he objec and base coordinae sysems being idenical. A ime = 1, he paricle s esimae of θ is updaed, causing a corresponding roaion of he is esimae of he pose of he mesh model i.e., a roaion of he paricle s esimae of he objec coordinae frame. using he angle θ = α,β,γ T, as follows: c γ c β c γ s β s α s γ c α c γ s β c α + s γ s α Rθ = s γ c β s γ s β s α + c γ c α s γ s β c α c γ s α, 3 s β c β s α c β c α where s α and c α are shorhand for sin α and cos α, respecively. According o he definiion of our coordinae sysems, he peg s posiion in objec coordinaes is given by s o = Rθs s + s. 4 Since he conac posiion in he robo coordinae sysem does no depend on he angle of he peg, our moion model only includes 3D ranslaion. Hence, in our experimens we held he angle of he peg fixed o he z direcion in he robo coordinae sysem. Our inference algorihm noneheless performs full 6-DOF localizaion, because he peg s posiion in he objec coordinae sysem depends no only on he 3D ranslaion moion conrol signal bu also on he 3D roaion angle θ. C. Map Represenaion and Measuremen Model We use as he map of he objec a mesh model consising of verices, edges, and riangular faces, all of which we define as feaures in he model. The mesh model can be generaed from a CAD model of he objec. To deal wih measuremen errors as well as differences beween he CAD model and he acual objec, we model measuremen uncerainy using a Gaussian probabiliy densiy funcion pdf of he disance beween he conac posiion and he feaures of he mesh model. For each feaure face, edge, or verex f k, where k = {1,..., K}, he sandard deviaion of ha feaure s measuremen uncerainy is denoed σ k, as shown in Fig. 3. The moivaion behind his represenaion of map uncerainy is ha differen feaures can exhibi differen ypes

6 Conac posiion f 1 s o a σ 1 f Equiprobabiliy conour σ f 3 σ3 Conac posiion f 4 σ 4 f 5 s o b σ 5 Mesh model f 1, f 3, f 5 : verex or edge f, f 4 : edge or face Fig. 3. Our map represenaion. We define every face, edge, and verex in he mesh model as a feaure f k, which has uncerainy σ k. For a given conac posiion in objec coordinaes, s o, he conac feaure is defined as he feaure f k whose disance o he conac posiion, normalized by σ k, is smalles. For example, f and f 3 are he conac feaures for peg posiions s o a and s o b, respecively. Measuremen probabiliy is defined as a Gaussian funcion wih sandard deviaion σ k of he disance from he conac poin o he conac feaure f k. of measuremen errors. As evidenced in he video ha accompanies his paper, in our experimens he peg is more likely o slip or bend when i conacs faces ha are seeply slaned faces ha form a small angle wih he probing direcion of he peg. For his reason, when generaing he map from a CAD model, we evaluae he normals of each face and assign a larger uncerainy a larger value of σ k o faces f k for which he probing direcion forms a small angle wih he plane of he face according o he mean of he iniial disribuion over angle, which in our experimens was θ =. Anoher ypical source of error is differences beween he CAD model and he acual objec. For insance, when he CAD model dicaes ha adjacen faces mee a a sharp angle, hese corners where he faces mee may in fac be rounded due o limis of he manufacuring process. For his reason, we assign larger uncerainy o edges and verices a he inersecion of planes whose normals form large angles wih each oher. More precisely, we compue he maximum angle beween he normals of all possible pairs of he faces ha border each edge/verex, and assign larger uncerainy o hose edges/verices ha have larger angles. Noe ha our mesh model does no require he use of he paricular heurisics described above. I only requires ha some uncerainy value be assigned o every feaure. This enables users o incorporae knowledge of heir paricular indusrial seings or manufacuring processes ino he uncerainy maps. In fac, here is no requiremen ha he uncerainy model be Gaussian. I could be defined by any of a wide range of probabiliy fields or energy funcions. In our implemenaion, he measuremen probabiliy is compued based on he disance beween he mesh model and he peg s posiion in objec coordinaes, s o, as well as he measuremen uncerainy of he conac feaure. The disance from he peg s posiion o each feaure f k is denoed ds o,f k, where he funcion d compues he Euclidean disance beween he posiion s o and feaure f k. Noe ha he disance o a face or edge is only defined if he perpendicular projecion of he poin o he corresponding plane or line lies wihin ha face or edge. We compue he disance o he peg s conac locaion from all feaures and selec as he conac feaure he one whose disance, normalized by he corresponding sandard deviaion, is smalles. The index of he conac feaure, k c, is herefore given by k c = arg min k ds o,f k σ k. 5 Since he peg s posiion in objec coordinaes, s o, is compued from s he peg s posiion in base coordinaes and θ he angle beween base coordinaes and objec coordinaes using 4, he disance measure h can be expressed equivalenly in eiher objec coordinaes as a funcion of s o or in base coordinaes as a funcion of s and θ: h kc s,θ = ds o,f kc. 6 Our measuremen probabiliy a ime is compued using his disance measure and he uncerainy of he mesh model: pz s,θ = Nh kc s,θ;,σ k c. 7 D. Inference Algorihm As described a he beginning of Secion II, our inference algorihm capializes on facorizaion 1 of he poserior probabiliy by using an RBPF [6] o infer he relaive pose of he peg and he objec. We esimae he 3D posiion of he peg a ime in base coordinaes, s, using paricles, where each paricle represens a single discree posiion esimae. Each paricle also mainains a Gaussian probabiliy disribuion over he 3D roaion, θ, from base coordinaes o objec coordinaes he relaive roaion beween he peg and objec. Each ime he peg is moved o a new conac posiion, our inference algorihm incorporaes he new observaion by firs updaing each paricle s esimae of he peg posiion using he moion model. We hen updae each paricle s disribuion over angle, given ha paricle s posiion, using an EKF. A ime, each paricle j mainains a 3D poin esimae, s, of he posiion of he peg a ime, as well as a normal disribuion over angle, pθ = N θ;µ,σ. 1 Iniializaion: We iniially sample J paricles from a uniform disribuion over he posiion x, y in base coordinaes, bounded by he maximum iniial x, y uncerainy which in our experimens is 3 mm square, as illusraed in Fig. 5a. The iniial value of z for each paricle j is deermined from ha paricle s x, y values such ha he paricle posiion s = x,y,z is se on he surface of he map of he mesh model. Noe ha he iniial posiion s is also used as he cener of roaion for paricle j, which is consan across all ime seps. Every paricle s Gaussian disribuion over he 3D angle θ is iniialized wih mean µ = and a diagonal covariance marix Σ represening he iniial angular uncerainy in our experimens, we assumed an iniial Gaussian uncerainy wih sandard deviaion 1 abou each roaion axis. Paricle Updae a Each Time Sep: When he robo arm moves he peg from one conac posiion o he nex, we updae he sae of each paricle j = {1,...,J} based on he moion u in base coordinaes obained from he robo arm encoders and he observaion z ha he robo deecs a conac a ha posiion. For he RBPF updae, we use similar

7 echniques o hose used in FasSLAM [7], [8]. Algorihm 1 gives a pseudocode summary of he updae algorihm a each ime sep, which is deailed below. In Algorihm 1, X represens he collecion of paricles a ime. Figure illusraes he paricle updae from = o = 1. Every paricle s probabiliy disribuion over angle is iniialized wih mean µ =. A his mean value of θ =, he base coordinae sysem and he objec coordinae sysem would be idenical, which corresponds o he esimae of he mesh model a = shown in Fig.. Afer a new observaion a ime = 1, he paricle s esimae of posiion is updaed from s o s 1, according o he conrol signal u 1 and he moion model, as described below in 8. Based on he paricle s new posiion, he paricle s esimae of θ is updaed o a new disribuion. The paricle s represenaion of he objec coordinae sysem and hence he mesh model a = 1 will be roaed abou he poin s by he angle θ wih respec o he base coordinae sysem, as indicaed in Fig.. For illusraion purposes, he figure shows a single value of he angle θ a each ime sep, bu in fac each paricle mainains and updaes an enire Gaussian disribuion over he angle θ, as described below. a Posiion Updae: The posiion s of paricle j a ime is sampled from he proposal disribuion given by he moion model, he paricle s previous posiion s 1, and he conrol u : s p s s 1,u. 8 b Angle Updae: Based on each paricle s updaed posiion in base coordinaes, s, and he observaion z ha here was a conac beween he peg and he objec, we compue he paricle s poserior disribuion over he angle θ using an EKF updae. To do so, we express he measuremen probabiliy as a funcion of θ and linearize ha measuremen probabiliy abou θ = µ 1, he mean of he paricle s previous esimae of he angular disribuion. We firs use he value θ = µ 1 in 4 o compue he prediced paricle posiion in he objec coordinae sysem, ŝ o : ŝ o = R µ 1 s s + s. 9 We hen use 5 o deermine he index of he conac feaure, k c, based on his prediced paricle posiion. We use he measuremen probabiliy 7 defined wih respec o he conac feaure, f kc, o updae he poserior probabiliy over angle, which is he second facor of 1. This poserior furher facorizes as pθ s :,z : pz θ,s :,z : 1 pθ s :,z : 1 = pz s,θ }{{} N h kc s,θ;,σ kc pθ s : 1,z : 1. 1 }{{} N θ;µ 1,Σ 1 The second facor in 1, he previous poserior pθ s : 1,z : 1, is represened by a Gaussian wih mean µ 1 and covariance Σ 1, bu he firs facor in 1 is no a Gaussian disribuion over θ since he disance measure 6 is no linear in θ. Noneheless, we can use 1 o approximae he poserior probabiliy of he angle esimae as a Gaussian disribuion. This updae is performed using EKF by linearizing he measuremen funcion abou θ = µ 1 : h kc s,θ h kc s,µ 1 = ĥ k + c, H + h kc θ s,µ 1 θ µ 1 θ µ 1, 11 where ĥ k = d c, ŝ o,f kc is he disance measure compued a he prediced posiion ŝ o, and H is Jacobian of he disance measure wih respec o θ compued a he prediced posiion and angle: H hkc = α, h k c β, h k c γ. 1 s,θ = s, µ 1 The paricle s poserior disribuion over angle is hen compued using he sandard EKF measuremen updae rule: K = Σ T 1 H H Σ T 1 H + σ 1 kc 13 µ Σ = µ 1 K = I K H ĥ k c, 14 Σ 1 15 c Imporance Weigh Updae and Resampling: Since we use he moion model 8 as he proposal disribuion, he imporance weigh for each paricle, w, is compued by marginalizing he measuremen probabiliy over he paricle s previous angle esimae: w w 1 p z s = w 1 = w 1 p z s,θ N p z s,θ p θ s }{{} h kc s,θ;,σ kc dθ p θ s : 1,z : 1 dθ. 16 }{{} N θ;µ 1,Σ 1 The final inegraion in 16 is compued in closed form by using he same linear approximaion ha we used for he angle updae, as follows [7]: w w 1 πq q 1/ exp ĥ k c, / q, 17 = H Σ T 1 H + σ kc. 18 To mainain good paricle diversiy, a each ime sep we esimae he effecive number of paricles [9]: 1 J eff = J. 19 j=1 w If J eff < J/, we perform resampling. Paricles are resampled wih probabiliy proporional o heir weighs w ; afer resampling, all paricle weighs are rese o w = 1/J. Oherwise J eff J/, we do no resample, and all paricles keep he curren weighs ha were compued using 17. Since our inference sars a = wih global uncerainy, we iniially use a relaively large number of paricles. Afer several measuremens, however, he number of paricles required for localizaion decreases. We herefore reduce he oal number of paricles a each resampling sep whenever J eff < J/, unil he number of paricles reaches a predefined number, J min. Specifically, if he number of paricles

8 Algorihm 1 Paricle updae algorihm Paricle Updae X 1, u, z X = he empy se for j = 1 o J do rerieve s, s 1, µ 1,Σ 1, 1 w from X 1 // Updae posiion s p`s s 1, u // Find conac feaure wih maximum likelihood ŝ o = R`µ `s 1 s + s for k = 1 o K do ĥ o k, = d`ŝ, f k end for k c = arg min k ĥ k, σk // Updae angle and imporance weigh H = h kc `s θ, µ 1 q = H Σ T 1 H + σ kc K = Σ T 1 H q µ = µ 1 K ĥ Σ w = `I K H = w 1 s `πq k c, Σ 1 1/ exp` `ĥ k c, o X add,, µ,σ, w end for normalize w such ha P j w = 1 // Resampling J eff = 1 P `w j if J eff < J/ hen if J > J min hen J = J/ end if q X = resample J paricles from X wih probabiliies w rese imporance weighs o w = 1/J end if J is greaer han J min a a resampling sep, hen we se J = J/ i.e., only he half number of paricles is resampled from he curren paricle se. In fuure work, we may insead use more efficien sraegies, such as KLD-sampling [1], o reduce he number of paricles over ime. 3 Convergence Check: For checking convergence, we compue he weighed average and weighed covariance of he paricle posiions in he objec coordinae sysem: s o = J j=1 w J Ψ o j=1 = w s o s o s o s o T 1 J, 1 j=1 w s o where s o is compued using 4 wih he mean of each paricle s poserior disribuion over angle a ime : s o = R µ s s + s. We coninue o probe he objec wih he peg coninue o collec observaions for addiional ime seps unil he race of he covariance marix Ψ o is less han some predeermined hreshold indicaing ha uncerainy among paricle posiions is small. Once his convergence condiion is achieved, he peg is moved o he esimaed posiion and angle of he hole. The esimaed posiion of he hole is compued based on he weighed average of all paricle posiions, s o. The a Fig. 4. Mesh models of: a he objec wih he hole picured in Fig. 7a, and b a randomly generaed map. Model a spans 75 mm in each horizonal direcion, wih a 5 mm heigh difference beween he uppermos and lowermos poins on he surface. The random model b was generaed by selecing z values uniformly from [, 5] mm on a regular x, y grid, wih inerval 5 mm and oal horizonal dimensions of 7 mm square. esimaed angle of he hole is compued based on a weighed average of paricle angles, µ, which we compue using a subse of paricles hose wih he larges weighs, since we found ha in pracice, he paricles wih small weighs can have very differen angle esimaes. b III. EXPERIMENTS In his secion, we firs show simulaion resuls o compare our RBPF approach o sandard paricle filering, which is he basis for previous mehods [3], [4], [5]. The resuls demonsrae ha for he 6-DOF localizaion problem, our sysem is orders of magniude more efficien han previous approaches. We hen describe physical experimens using a robo arm o inser a needle-like peg ino a small hole in a meal objec. The video ha accompanies his paper shows an example sequence of localizaion and peg inserion using he robo arm. A. Simulaions We used wo differen mesh models, shown in Fig. 4, o es our localizaion algorihm in simulaions. The firs mesh model, shown in Fig. 4a, was generaed from he CAD model of he acual physical objec ha we used for he robo experimens. The second, much more complex mesh model was he random surface shown in Fig. 4b. In our simulaions wih he firs mesh model, he goal of localizaion was o deermine he pose posiion and orienaion of he cenral hole. Wih he second model random surface, he goal was o find he posiion and orienaion of he cener of he surface he origin of he objec coordinae sysem. In each simulaion rial, he ground ruh iniial posiion of he peg was randomly chosen from a uniform disribuion 3 mm square from [ 15,15] mm in he x and y direcions, wih z coordinae given by he surface of he objec, and he ground ruh roaion angle beween peg and objec was randomly chosen from a uniform disribuion from [ 1, 1] degrees abou each axis. As described in Secion II-D.1, paricles were iniialized wih posiions sampled from, and angle disribuions covering, hese same uncerainy regions. Each conac measuremen was obained by randomly moving he peg o a new horizonal posiion wihin a [ 15,15] mm range of he peg s iniial posiion, hen moving he peg in he z direcion in base coordinaes unil conacing he surface. Figure 5 shows he disribuion of paricles a differen ime seps of one simulaion rial.

9 a = b = c = 4 d = 7 converged e Final inserion Fig. 5. Disribuion of paricles a each ime sep of one simulaion rial. Each paricle is represened by a red line, indicaing he paricle s esimae of he peg posiion and he mean of he paricle s disribuion over he peg angle, boh represened in he objec coordinae sysem. A each ime sep, he whie arrow indicaes he ground-ruh posiion of he peg. For he iniializaion sep, shown in a, paricle posiions are sampled from he iniial 3 mm 3 mm uncerainy region of x, y, and paricle angle disribuions all have zero mean. The disribuion of paricles afer updae seps, 4, and 7 are shown in b, c, and d, respecively. Once he algorihm deermines convergence d, i moves he peg o he inferred pose posiion and angle of he hole e based on a weighed mean across paricles of he esimaed pose. Posiion esimaion error [mm] RBPF PF Angle esimaion error [degrees] 6 4 RBPF PF Posiion esimaion error [mm] RBPF PF Angle esimaion error [degrees] 6 4 RBPF PF Number of paricles Number of paricles Number of paricles Number of paricles a Resuls obained using he objec wih he hole b Resuls obained using he random surface Fig. 6. Comparison of our Rao-Blackwellized paricle filer RBPF wih a sandard paricle filer PF, using as he map a he objec picured in Figs. 4a and 5, and b he random surface picured in Fig. 4b. In each par a and b, he posiion error is shown in he graph on he lef as Euclidean disance in mm, while he angle error is shown on he righ as he absolue angle in degrees beween he correc peg inserion direcion and he esimaed peg inserion direcion. The number of paricles is shown in a logarihmic scale on he x-axis of each graph. The number of paricles required by our RBPF approach is grealy reduced from he number required by previous PF approaches o his problem, wih only a sligh increase in he ime required o updae each paricle. Figure 6 compares our RBPF approach o a sandard paricle filer, which represens all 6-DOF of uncerainy using paricles corresponding o a 6-DOF version of previous approaches [3], [4]. For hese simulaions, we se he uncerainy in he mesh model map o σ k =. mm for all feaures k. The performance is indicaed in Fig. 6 by he final esimaion error in posiion and angle. The posiion error is he Euclidean disance from he correc posiion, while he angle error is he magniude of he angle beween he ground ruh z direcion and he esimaed z direcion beween he correc and esimaed peg inserion angle. Each daa poin in he figure represens he average of 1 rials. Here he sandard paricle filer PF was implemened in all 6-DOF by esimaing boh he posiion and he angle of a paricle using he mehod we described for posiion esimaion alone in Secion II, so ha each paricle mainains a poin esimae for angle raher han a Gaussian disribuion over angle. For he sandard PF, we included angular noise in he moion model, which corresponds o perurbing each paricle s angle esimae a every ime sep of he inference algorihm, wih sandard deviaion of.5 abou each axis. As shown in Fig. 6, he sandard PF requires a much larger number of paricles by orders of magniude han our RBPF approach o achieve he same accuracy. This is because our approach reduces he sae space ha mus be sampled by paricles from 6 down o jus 3 dimensions. Furhermore, he ime required per paricle for our RBPF approach is only slighly slower han ha required per paricle for he sandard PF: The average compuaion ime o updae 64 paricles wih resampling was.95 sec for our RBPF algorihm versus.91 sec for he sandard PF, on a.66 GHz PC wih unopimized code. This shows ha our mehod grealy reduces compuaional ime as compared o he sandard PF. B. Experimens wih a Robo Arm We performed physical experimens using a Misubishi MELFA RV-6SL 6-axis robo arm see he accompanying video. As shown in Fig. 7a, a needle-like peg was aached o he robo s end effecor along he z-direcion in he robo coordinae sysem, and he objec was placed on an approximaely horizonal able. The diameer of he peg is.5 mm, and he hole apers from 5 mm diameer down o 3mm diameer over a verical disance of 3 mm. Inserion will succeed if he esimaion error of he posiion is wihin he clearance range and he angular error is small. A he sar of he experimens, we firs measured he ground ruh posiion of he hole by moving he robo arm manually. We hen sared rials by randomly selecing he iniial posiion of he peg from he [-15, 15] mm range in x, y direcion around he hole jus as in simulaion. For each rial, he paricle posiions are iniially sampled from ha same disribuion. In our curren implemenaion, he

10 a Angle esimaion error [degrees] Posiion esimaion error [mm] b.5 c d y esimaion error [mm] z esimaion error [mm] x esimaion error [mm] x esimaion error [mm] Fig. 7. a Experimenal seup. b Error plo of he esimaed posiions and angles for 5 rials. The measures of posiion and angle errors are he same as in Fig. 6. To plo hese resuls, we assume ha he correc ground ruh direcion of inserion is parallel o he z-axis. In c and d, esimaion errors ploed in he x, y and x, z spaces show ha he esimaion is biased especially in z direcion mainly because of a sligh bending of he peg. robo arm lifs he peg in he posiive z-direcion from is previous conac posiion, moves he peg o randomly chosen x, y coordinaes as in he simulaion, hen lowers he peg in he negaive z direcion unil conac is deeced. We used he robo arm s buil-in impac deecion funcion o deec conac, obaining he conac posiion from he robo arm encoders. For hese physical experimens, we assigned differen measuremen uncerainies o each feaure of he mesh model, using he heurisics described in Secion II-C. We used 64 paricles in our RBPF algorihm. Figure 7b plos he error of he final esimaed posiions and angles in each of 5 rials, measured wih respec o he ground ruh pose of he hole in robo coordinaes. All 5 rials resuled in a correc inserion as can be seen in he figure, all he esimaed posiions were wihin he clearance range. Figures 7c and d deail he 3D posiion errors, illusraing a bias due o he fac ha he peg we used had a endency o bend in a paricular direcion. The number of conac measuremens needed for convergence ranged from 6 o 15, wih an average of 9.8. IV. CONCLUSION We have presened a facorizaion approach for localizaion in roboic assembly. Using Rao-Blackwellized paricle filering, we separae pose esimaion ino a paricle-based esimaor which can easily represen mulimodal disribuions for posiion, and a unimodal Gaussian esimaor for angle condiioned on he paricle posiion. This represenaion makes 6-DOF localizaion in peg-in-hole problems racable by grealy reducing he number of paricles required, resuling in a similar reducion in compuaional ime. We have also described a novel map represenaion o explicily model he uncerainy of each feaure in he mesh, wihou he need for a dense memory-inensive map represenaion. The explici incorporaion of uncerainy ino our mesh model enables us o perform localizaion using a map ha was direcly obained from a CAD model, wihou he need for a ime-consuming preprocessing sep such as probing a every possible locaion. As evidenced by boh simulaed and physical experimenal resuls, our algorihm benefis from is sequenial esimaion approach, which makes i possible o efficienly solve he localizaion problem using a small number of measuremens. In addiion, he compuaional complexiy of each sequenial updae is consan, independen of he oal number of measuremens. One limiaion of our curren approach is ha we randomly selec each probing posiion. In fuure work, we will modify he sysem o choose he nex probing posiion based on he sysem s curren esimae of he poserior disribuion over pose, which will make he sequenial esimaion approach even more powerful. In addiion, we plan o generalize our approach o peg-in-hole problems wih more complex geomeries. REFERENCES [1] W. S. Newman, M. S. Branicky, H. A. Podgurski, S. Chhapar, L. Huang, J. Swaminahan, and H. Zhang, Force-responsive roboic assembly of ransmission componens, in Proc. IEEE In. Conf. Roboics Auomaion ICRA, vol. 3, May 1999, pp [] S. R. Chhapar and M. S. Branicky, Search sraegies for peg-in-hole assemblies wih posiion uncerainy, in Proc. IEEE/RSJ In. Conf. Inelligen Robos Sysems IROS, vol. 3, Oc. 1, pp [3], Localizaion for roboic assemblies using probing and paricle filering, in Proc. IEEE/ASME In. Conf. Advanced Inelligen Mecharonics AIM, July 5, pp [4], Paricle filering for localizaion in roboic assemblies wih posiion uncerainy, in Proc. IEEE/RSJ In. Conf. Inelligen Robos Sysems IROS, Aug. 5, pp [5] U. Thomas, S. Molkensruck, R. Iser, and F. M. Wahl, Muli sensor fusion in robo assembly using paricle filers, in Proc. IEEE In. Conf. Roboics Auomaion ICRA, Apr. 7, pp [6] A. Douce, N. de Freias, K. P. Murphy, and S. J. Russell, Rao- Blackwellised paricle filering for dynamic Bayesian neworks, in Proc. 16h Conf. Uncerainy in Arificial Inelligence, June, pp [7] S. Thrun, W. Burgard, and D. Fox, Probabilisic Roboics. The MIT Press, 6, ch. 13. [8] M. Monemerlo, S. Thrun, D. Koller, and B. Wegbrei, FasSLAM: A facored soluion o he simulaneous localizaion and mapping problem, in Proc. AAAI Naional Conf. Arificial Inelligence,. [9] A. Douce, S. Godsill, and C. Andrieu, On sequenial Mone Carlo sampling mehods for Bayesian filering, Saisics and Compuing, vol. 1, no. 3, pp , July. [1] D. Fox, Adaping he sample size in paricle filers hrough KLDsampling, In. J. Roboics Research, vol., no. 1, pp , Dec. 3.