Analytical Expressions for Positioning Uncertainty Propagation in Networks of Robots

Transcription

1 Analytical Expessions fo Positioning Uncetainty Popagation in Netwoks of Robots Ioannis M Rekleitis and Stegios I Roumeliotis Abstact In this pape we pesent an analysis of the positioning uncetainty incease ate fo a goup of mobile obots The simplified vesion fo a goup of N obots moving along one dimension is consideed The one dimension estiction pemits us to extact an exact expession fo the accumulation of positioning uncetainty in a goup of obots equipped with popioceptive (odometic in this case) and exteoceptive (elative distance between obots) sensos The solution obtained povides insight in the stuctue of the multiobot localization poblem In addition, it seves both as an appoximation and a stating point fo examining the moe ealistic case of N obots moving on a plane Ou deivation is based on a Kalman filte estimato that combines all measuements fom all obots in the goup Futhemoe, we analyze the effect of initial uncetainty, numbe of obots (N) and senso noise on the ate of positioning uncetainty incease The analytical esults deived in this pape and the impact of the diffeent paametes ae validated in simulation Keywods Multiobot Localization, Coopeative Localization, Netwoks of Robots I INTRODUCTION This pape studies the localization accuacy of a team of mobile obots that closely coopeate while navigating an aea We conside pimaily the most challenging scenaio whee the absolute positions of the obots cannot be measued o infeed, and no extenal positioning infomation is obtained fom a GPS eceive o a map of the envionment One of the advantages of multi-obot systems is that obots can accuately localize by measuing thei elative position and/o oientation and communicating localization infomation thoughout the goup In this case the uncetainty in the position estimates fo all obots will continuously incease Howeve, pevious wok on coopeative localization [], [], [] has demonstated that the localization uncetainty incease acoss goups of obots is lowe compaed to the situation whee each obot is estimating its position without coopeation with the est of the team The theoetical analysis of the positioning uncetainty popagation duing coopeative localization has been an open poblem to this date In this pape we pesent the necessay fist step fo detemining uppe bounds on the position uncetainty accumulation fo a goup of N obots The -D case is examined whee the N obots move only along one dimension and they continuously measue the elative distance to each othe The uppe bound is calculated by solving the continuous time Riccati equation fo the covaiance of the eos in the position estimates The key element in ou deivation is the sepaation of the covaiance matix into two sets of submatices, those that convege to steady state values and those that captue the time dependence of the uncetainty incease duing coopeative localization Thoughout the pape we assume that all obots move at the same time I Rekleitis is with the Mechanical Engineeing, Canegie Mellon Univesity, Pittsbugh, PA, USA, ekleitis@cmuedu S Roumeliotis is with the Depatment of Compute Science & Engineeing, Univesity of Minnesota, MN, USA, stegios@csumnedu The -D case is inteesting by itself as it povides insights in the solution of the Riccati equation by examining the evolution of the covaiance matix until it eaches a steady state The behaviou of the Riccati equation in -D also seves as a stating point fo solving the moe geneal -D case Discussion of the -D case is out of the scope of this pape and can be found in [] In the following section we outline the main appoaches to coopeative localization In Section III we pesent the fomulation of the multi-obot localization poblem in D and study the effect of consecutive elative position updates on the stuctue of the Riccati equation which descibes the time evolution of the uncetainty in the position estimates In Section IV simulation esults ae pesented that validate the deived analytical expessions fo the ate of localization incease Finally, Section V daws the conclusions fom this analysis and suggests diections of futue wok II RELATED WORK An example of a system designed fo coopeative localization was fist epoted in [] A goup of obots is divided into two teams in ode to pefom coopeative positioning At each instant, one team is in motion while the othe team emains stationay and acts as a landmak Impovements ove this system and optimum motion stategies ae discussed in [], [] and [] Similaly, in [8], only one obot moves, while the est of the team of small-sized obots foms an equilateal tiangle of localization beacons in ode to update thei pose estimates Anothe implementation of coopeative localization is descibed in [] and [9] In this appoach a team of obots moves though the fee space systematically mapping the envionment These appoaches have the following limitations: (a) Only one obot (o team) is allowed to move at any given time, and (b) The two obots (o teams) must maintain visual (o sona) contact at all times A Kalman filte-based implementation of a coopeative navigation schema is descibed in [] In this wok the effect of the oientation uncetainty in both the state popagation and the elative position measuements is ignoed esulting in a simplified distibuted algoithm In [], [] a Kalman filte pose estimato is pesented fo a goup of simultaneously moving obots The Kalman filte is decomposed into a numbe of smalle communicating filtes, one fo evey obot, pocessing senso data collected by its host obot It has been shown that when evey obot senses and communicates with its colleagues at all times, evey membe of the goup has less uncetainty about its position than the obot with the best (single) localization esults To the best of ou knowledge thee exist only two cases in the liteatue whee uncetainty popagation has been consideed in

2 the context of coopeative localization In [] the impovement in localization accuacy is computed afte only a single update step with espect to the pevious values of uncetainty In [] the authos have exploed the effect of diffeent obot tacke sensing modalities on the effectiveness of coopeative localization Statistical popeties wee deived fom simulated esults fo goups of obots of inceasing size N when only one obot moved at a time In the following sections we pesent the details of ou appoach fo estimating the uncetainty popagation duing coopeative localization in -D Ou initial fomulation is based on the algoithm descibed in [] III COOPERATIVE LOCALIZATION IN -D Conside the case of N obots moving andomly along one dimension (without loss of geneality let the dimension be the x axis) Each obot is equipped with odometic sensos that measue the velocity of the obot and a obot tacke senso that measues the elative position between any two obots, and possesses no othe sensing ability A Kalman Filte estimato is employed to combine the two senso measuements and povide an optimal position estimate fo each obot togethe with a position uncetainty estimate In the next subsections we povide an analytical expession fo the position uncetainty as it gows ove time A Motion Model Fo the case of a single obot moving in a -D envionment, its motion is descibed (in discete time) by the following equation x i (k +)x i (k) +V i (k)ffit () If a senso on boad the obot measues its velocity then the estimate fo this motion is given by ^x i (k +) ^x i (k) +V mi (k)ffit () with V mi (k) V i (k) w Vi (k) () whee w Vi is a zeo-mean white Gaussian pocess, the noise in the velocity measuement fo obot i, with covaiance q EfwV i g Since each obot caies its own popioceptive senso that measues the velocity of the vehicle, these measuements ae independent, ie Efw Vi w Vj g fo i j The eo in the position estimate is given by: ex i (k +) x i (k +) ^x i (k +) x i (k) ^x i (k) +(V i (k) V mi (k)) ffit ex i (k) +w Vi (k)ffit []ex i (k) +[ffit]w Vi (k) F i ex i (k) +G i w Vi (k) Expanding the pevious expession to the case of N obots, we have ex (k +) ex (k) w V (k) + ffit, ex N (k +) ex N (k) w VN (k) ex(k +) Φ e X(k) +GW (k), ex(k +) I e X(k) +ffitiw (k) () The same equation in continuous time is given by _ex(t) F e X(t) +G c W (t), _ex(t) e X(t) +IW (t) () B Measuement Model When one of the obots in the goup detects anothe obot and measues thei elative position, the measuement equation is: z ij (k +)x j (k +) x i (k +)+n ij (k +) () whee n ij (k +)is the noise associated with the elative position measuement This noise is assumed to be a zeo-mean white Gaussian pocess with known vaiance Efn ij (k +)g Note also that the measuements n ij ae independent, ie Efn ij n kl g fo ij kl The estimated measuement would be ^z ij (k +) ^x j (k +) ^x i (k +) () and the esidual (eo) fo this measuement is ez ij (k +) z ij (k +) ^z ij (k +) x j (k +) x i (k +) (^x j (k +) ^x i (k +))+n ij (k +) ex j (k +) ex i (k +)+n ij (k +) [ ::: ::: ::: ] ex (k +) ex i (k +) ex j (k +) ex N (k +) + n ij (k +) H ij e X(k +)+nij (k +) (8) If each of the obots measues its elative position with espect to all obots in the goup then the esulting N (N ) measuement eo equations can be witten in a compact fom as: ez (k ) ez N (k ) ez N (k ) ez N(N ) (k ) whee k k +and H ex N (k ) ex (k ) + n (k ) n N (k ) n N (k ) n N(N ) (k ) ez(k ) H e X(k )+N (k ) (9) H ::: ::: ::: :::,

3 By employing the pevious expession, the covaiance of the esidual (measuement eo) is given by: S Ef e Z e Z T g HP(k +k)h T + I () whee P(k +k) is the covaiance fo the position estimate at time k +using measuements up to time k and I is the (N (N )) identity matix Hee we have assumed that all elative position measuements between the obots have the same level of accuacy C Riccati Equation Since none of the obots in the goup eceives absolute positioning infomation, the system of the N obots is not obsevable and the covaiance matix will incease without bound In ode to detemine the ate of incease of the covaiance matix we fist wite the discete time popagation and update equations fo it: P(k +k) ΦP(kk)Φ T + GQG T P(kk) +Q d () whee we substituted Φ I fom Eq () The covaiance update equation in its invese fom is given by: P (k +k +) P (k +k) +H T R H o, in ode to simplify the notation (P(kk) +Q d ) + H T R H () P k+ (P k + q d I) + HT H () whee we substituted R I and Q d q d I qffit I This equation computes the invese covaiance of the system of N obots afte two consecutive steps of popagation and update Matix H T H is constant and it only depends on the numbe of obots N It can be computed as: H T H N ::: N ::: ::: N ::: N NI () whee ::: ::: ::: ::: ; N () Assume that at step k all obots know thei position with the same level of accuacy, that is P p I The covaiance matix P is symmetic with equal non-diagonal tems (hee all zeo) and also equal diagonal tems (hee all p ) We will pove that afte any numbe of steps the covaiance matix sustains this stuctue By substituting P p I in Eq () we have P (P + q d I) + HT H I + N q d + p I + N I + q d + p v I + u () By employing the elations in the Appendix the covaiance matix can be computed as: P v I u v (v + Nu ) v I + u () Note again that both the diagonal and non-diagonal elements of this matix ae equal between themselves (ie P ii P jj ; 8i; j, P ij P kl ; 8 i j; k l) Assume that afte a cetain numbe of popagation and update steps, at step k m the covaiance matix has still equal diagonal and equal non-diagonal elements That is P m v m I + u m (8) We will pove that the covaiance matix P m+ also has equal diagonal and non-diagonal elements By substituting in Eq () we have: P m+ (P m + q d I) + HT H ((v m + q d )I + u m ) + HT H + N I + v m + q d u m (v m + q d )(v m + q d + Nu m ) v m+ I + u m+ (9) By employing the elations in the Appendix the covaiance matix can be computed as: P m+ v m+ I v m+ I + u m+ u m+ v m+ (v m+ + Nu m+ ) We have poven the following: Lemma : The covaiance matix fo a goup of N obots with the same level of uncetainty fo thei popioceptive and exteoceptive measuements when pefoming coopeative localization is a matix with equal diagonal and equal nondiagonal tems A diect esult of the pevious lemma is the following: Coollay : A goup of N obots with the same level of uncetainty fo thei popioceptive and exteoceptive measuements expeience when pefoming coopeative localization the same level of positioning uncetainty and they shae the same amount of infomation

4 The amount of infomation shaed by two obots is captued in the coss-coelation tems (non-diagonal) of the covaiance matix At this point we employ Lemma to deive an analytical expession fo the ate of incease in the localization uncetainty fo the goup of obots Lemma : Fo a goup of N obots with the same level of uncetainty fo thei popioceptive q and exteoceptive measuements, when they pefom coopeative localization thei positioning uncetainty and coss-coelation tems gow linealy with espect to time ie 8 < P ij (t) : q N t + N q N t + N p p() + (N q ) N p p() q i j () N i j Hee we use the continuous time Riccati equation fo the popagation and update of the covaiance matix, that is: _P FP + PF T + G c QG T c PHT R HP () whee F, G c I, Q qi, R I, H T H (N )I and P vi +u Substituting in the pevious equation we have _P qi PHT HP qi (vi + u)(ni )(vi + u), _vi + _u q Nv I + v The last elation can be divided into two diffeential equations The fist one is: _v N v +v + q () It can be shown [] that the solution of this Riccati equation in steady state is given by q v(t) () N whee v c is a constant The second diffeential equation is _u v By ewitting Eq () as () v (t) (q _v(t)) () N and substituting in Eq (), fo u() we have u(t) Z t N v Z t dt (q _v(t ))dt N qt (v(t) p()) () N whee we have employed the initial condition v() p() The covaiance matix can now be witten as» q P(t) v(t)i + N t (v(t) p()) () N and afte sufficient time this would be» q q P(t) N I + N t q N N p() (8) whee we employed the esults fom Eq () Fom this last equation it is evident that afte sufficient time, the positioning uncetainty fo this goup of obots inceases linealy with time, ie _P(t) q N (9) Fom this last equation the following ae tue: ffl The ate of incease is popotional to the odometic uncetainty q of each obot and invesely popotional to the numbe N of obots ffl The ate of uncetainty incease in steady state does not depend on the accuacy (detemined by thei covaiance ) of the elative position measuements ffl The time fo the system to each steady state is detemined by the time constant of the system fi p () D Nq which depends on the odometic accuacy q of the obots, thei numbe N and the accuacy of the elative position measuements Inaccuate elative position measuements will delay the system eaching steady state On the othe hand lage teams of obots will quickly each steady state Finally, obots with vey pecise odometic infomation (small q) will depend less on the elative position measuements and moe on thei own odomety fo a longe peiod of time Up to this point, we have assumed that all obots have the same odometic uncetainty q i q; 8i and elative position measuement uncetainty i ; 8i This was done in ode to facilitate the pevious deivations and thus gain insight in the stuctue of the coopeative localization poblem Nevetheless, Eq (8) can be used to detemine the bounds fo the expected uncetainty gowth fo q max(q i ), max( i ) That is» q q P(t)» N I + N t q N N p() () IV SIMULATION RESULTS We pefomed a seies of expeiments in simulation to veify the pefomance of coopeative localization and the effect of the diffeent paametes (eg numbe of obots, initial uncetainty and senso uncetainty) on the incease in the uncetainty of the system The esults obtained validate the theoetical analysis pesented in the pevious section Figue pesents the actual (solid line) and theoetical (dashdotted line) covaiance values ove time fo diffeent numbes of obots As expected, afte the obots each steady state the values ae vey simila It is woth noting that as the numbe of obots inceases the ate of uncetainty incease educes following a law of diminishing etuns: the lage the goup the less value each additional obot adds to the localization The effect of the initial position uncetainty P () is pesented in Figue

5 Actual Covaiance Theoetical Value of Covaiance N Actual Covaiance Theoetical Value of Covaiance 8 9 P (m ) N P (m ) 8 N N N 8 9 t (sec) Fig Actual and theoetical covaiance values fo obot goups of diffeent size N 8 9 t (sec) Fig Actual and theoetical covaiance values fo a goup of N obots fo diffeent values of elative position measuement uncetainty P (m ) 8 Actual Covaiance Theoetical Value of Covaiance P() P()8 P() 8 9 t (sec) Fig Actual and theoetical covaiance values fo a goup of N obots fo diffeent values of initial uncetainty P () fo a goup of two obots; again, afte an initial peiod the theoetical and the simulated uncetainty agee Clealy, as pedicted by the theoetical analysis, the initial position uncetainty P () does not affect the ate of uncetainty incease The final significant esult is illustated in Figue : the quality of the obot tacke measuements does not influence the ate of position uncetainty incease In othe wods, in the case whee all the obots move simultaneously, coopeative localization enables the uncetainty of each obots position to decease invesely popotional to numbe of the obots in the goup but the accuacy of the obot tacke adds only an initial (constant) impovement Figue shows fo a goup of two obots that the simulated uncetainty matches the theoetically calculated values V CONCLUSIONS This pape pesented a theoetical analysis fo the popagation of position uncetainty fo a team of mobile obots moving in one dimension The most challenging case of localization was consideed based only on inte-obot obsevations (coopeative localization) and dead eckoning estimates Futhemoe, all obots moved simultaneously, in contast to pevious wok whee often the obots take tuns to move and to act as landmaks An analytical fomula was deived that expesses the uppe bound of the uncetainty accumulation as a function of time and the noise chaacteistics of the obot sensos The analysis pesented hee is a equied fist step fo the teatment of the moe geneal case of N obots moving simultaneously on flat teain The analytical solution pesented hee genealizes to the -D case unde cetain assumptions [] as an uppe bound of the accumulation of position uncetainty REFERENCES [] R Kuazume, S Nagata, and S Hiose, Coopeative positioning with multiple obots, in Poceedings of the IEEE Intenational Confeence in Robotics and Automation, Los Alamitos, CA, May 8-99, pp [] IM Rekleitis, G Dudek, and E Milios, Multi-obot exploation of an unknown envionment, efficiently educing the odomety eo, in Poceedings of the Fifteenth Intenational Joint Confeence on Atificial Intelligence (IJCAI 9), Nagoya, Japan, Aug -9 99, pp [] SI Roumeliotis and GA Bekey, Collective localization: A distibuted kalman filte appoach to localization of goups of mobile obots, in Poceeding of the IEEE Intenational Confeence on Robotics and Automation, San Fancisco, CA, Ap -8, pp 98 9 [] SI Roumeliotis, Analytical expessions fo multi-obot localization, Tech Rep, Compute Science and Engineeing Depatment, Univesity of Minnesota, Feb, epots/multi analysispdf [] R Kuazume, S Hiose, S Nagata, and N Sashida, Study on coopeative positioning system (basic pinciple and measuement expeiment), in Poceedings of the IEEE Intenational Confeence in Robotics and Automation, Minneapolis, MN, Ap -8 99, pp [] R Kuazume and S Hiose, Study on coopeative positioning system: optimum moving stategies fo CPS-III, in Poceedings of the IEEE Intenational Confeence in Robotics and Automation, Leuven, Belgium, May - 998, pp 89 9

6 [] R Kuazume and S Hiose, An expeimental study of a coopeative positioning system, Autonomous Robots, vol 8, no, pp, Jan [8] R Gabowski, L E Navao-Sement, C J J Paedis, and PK Khosla, Heteogenouse teams of modula obots fo mapping and exploation, Autonomous Robots, vol 8, no, pp 9 8, [9] IM Rekleitis, G Dudek, and E Milios, Multi-obot collaboation fo obust exploation, Annals of Mathematics and Atificial Intelligence, vol, no -, pp, [] AC Sandeson, A distibuted algoithm fo coopeative navigation among multiple mobile obots, Advanced Robotics, vol, no, pp 9, 998 [] SI Roumeliotis and GA Bekey, Distibuted multiobot localization, IEEE Tansactions on Robotics and Automation, vol 8, no, Oct [] IM Rekleitis, G Dudek, and E Milios, Multi-obot coopeative localization: A study of tade-offs between efficiency and accuacy, in IEEE/RSJ Intenational Confeence on Intelligent Robots and Systems, Lausanne, Switzeland, Sep-Oct APPENDIX Special Case of Matix Invesion: The invese of the N N symmetic matix is T T fi (fi + Nfl) fi + fl fl ::: fl fl fl fi + fl ::: fl fl fl fl ::: fi + fl fl fl fl ::: fl fi + fl fi I + fl () fi +(N )fl fl ::: fl fl fl fi +(N )fl ::: fl fl fl fl ::: fi +(N )fl fl fl fl ::: fl fi +(N )fl fi I fl fi (fi + Nfl) () This can be shown by computing the poduct TT and substituting fom the pevious expessions