Deterining the Robot-to-Robot Relative Pose Using Range-only Measureents Xun S Zhou and Stergios I Roueliotis Abstract In this paper we address the proble of deterining the relative pose of pairs robots that ove on a plane while easuring the distance to each other We show that the iniu nuber of distance easureents required for the degrees of freedo robot-to-robot transforation to becoe locally observable is Furtherore, we prove that the axiu nuber of possible solutions in this case is, while a iniu of distance easureents is necessary in order to uniquely deterine the robots relative pose Finally, we present efficient algoriths for coputing all possible solutions and evaluate the validity of our theoretical results both in siulation and experientally I INTRODUCTION AND RELATED WORK In order to solve distributed estiation probles such as cooperative localization, apping, and tracking, robots first need to deterine their relative position and orientation (pose) This initial calibration process is necessary for coordinating a robot tea and registering easureents to the sae frae of reference Since the accuracy of the relative (robot-to-robot) transforation can significantly affect the quality of a sensor fusion task (eg, tracking a target using observations fro ultiple sensors), it needs to be deterined precisely Mobile robots that ove on a plane and use distance and bearing sensors (eg, laser scanners, stereo caeras) can uniquely deterine their relative pose by processing distance and relative bearing easureents ] However, due to cost, power, and processing constraints, robots often have to rely on sensors that provide only distance easureents In these cases alternative algoriths and otion strategies are necessary in order to deterine the unknown robot-to-robot transforation Most current research on applications of range sensing has focused on designing algoriths that process distance easureents to deterine only the position of each node in a static network of sensors ], or the position and orientation of a obile robot when static beacons are deployed within an area of interest ] In the case of networks of sensors, a variety of algoriths based on convex optiization ] and Multi Diensional Scaling (MDS) ], have been eployed to localize the sensor nodes Additionally, distributed approaches that reduce the counication requireents and better balance the coputational load aong sensors have also received significant attention in the related literature (eg, ], 7]) In all these cases, the objective is to deterine only the position of the sensor nodes with respect to anchor This work was supported by the University of Minnesota (DTC), the NASA Mars Technology Progra (MTP-), and the National Science Foundation (EIA-8, IIS-8) Xun S Zhou and Stergios I Roueliotis are with Departent of Coputer Science and Engineering, University of Minnesota, Minneapolis, MN, USA {zhou stergios}@csunedu R (t ) R (t ) R (t ) R (t ) R (t ) R (t ) R (t ) R (t ) Fig A siplistic approach to deterine an initial estiate for the robot-to-robot relative transforation using distance easureents The dark (light) triangles denote the location of robot R (R ), and t i, t ij, i, j {, } indicate the tie step(s) that a robot reains at a certain location nodes that can globally localize via GPS easureents Siilarly, in the case of obile robots the ephasis is on using distance easureents to localize robots with respect to static beacons ], for exaple, and not on coputing the relative pose of the robots However, deterining the robot-to-robot transforation is a prerequisite for efficiently coordinating the otion of teas of robots and expressing easureents in a coon frae of reference The proble we are interested in is that of directly coputing the degrees-of-freedo (dof) transforation between cooperating robots using distance easureents Specifically, we consider pairs of robots equipped with odoetric sensors for tracking their otion and a range sensor for easuring the distance to each other In this case, if no prior inforation about their relative position and orientation is available, a huan operator will need to anually easure the transforation between the two robots before they can be deployed to perfor their assigned task This tedious process, however, liits the accuracy of the robot-to-robot transforation and increases the cost of deploying large teas of robots due to the tie and effort required A straightforward approach to autoating this initial calibration process is for the robots to ove randoly, collect distance easureents, and then copute their relative transforation using an iterative least squares algorith The proble in this case is that any iterative process applied to iniize the non-linear, in the unknown variables, cost function relies on the existence of an accurate initial estiate in order to converge to the correct solution Additionally, since the iniu nuber of range easureents necessary is not known a priori, a conservative strategy would require the robots to spend excessive tie and energy easuring The extension to the proble of ultiple robot teas is straightforward once a solution to the pair-wise proble is deterined
their distance nuerous ties Instead it would be beneficial for the robots to follow a two-step process: (i) Eploy a non-iterative algorith to process the iniu nuber of distance easureents required to copute an initial estiate of their relative transforation (ii) Apply iterative least squares to refine this initial estiate using additional range easureents This second step can be repeated until the user-specified level of accuracy is reached A siplistic ethod to copute an initial estiate for the dof transforation would require the robots to follow a sequence of coordinated otions and easure distances to each other at certain locations and tie instants Specifically, as shown in Fig, if robot R reains static while R easures its distance to R at different locations (tie instants t, t, and t ), the position of R with respect to R can be uniquely deterined In order to also copute their relative orientation, robot R will need to ove to a new location and reain again static till robot R records another distance easureents (tie instants t, t, and t ) and triangulates the new relative position of robot R Using these inferred relative position easureents and knowing the direction of otion of R (coputed fro its own odoetry), the relative orientation between the robots can be uniquely deterined The ain drawback of this approach is that it requires tight coordination between the robots for perforing the sequence of necessary otions and recording the distance easureents at the appropriate locations Additionally, this initial calibration phase delays the onset of the actual robot task which can be detriental in tie-critical situations involving large robot teas In this paper, we address this proble by developing noniterative algoriths for coputing the initial estiate of the dof robot-to-robot transforation without restricting their otion Specifically, we prove that when robots ove randoly and collect distance easureents at different locations, the axiu nuber of possible solutions is (cf Lea, Section II) When range easureents are available, we show that there can exist no ore than solutions (cf Lea, Section III) Furtherore, in Section IV (cf Lea ) we prove that the iniu nuber of distance easureents necessary in order to uniquely deterine the relative pose of the robots is (instead of based on the siplistic ethod outlined in Fig ) Efficient algoriths for coputing all possible solutions for the cases described above are presented Additionally, we provide a novel linear algorith for deterining the unique solution (ie, when range easureents are available) that iniizes the nuerical error in the coputed transforation (cf Section IV-B) In Section V, we describe the iterative least squares algorith that uses additional range easureents to refine the initial estiate for the unknown robot-to-robot transforation Finally, in Section VI, we present siulation and experiental results that verify the validity of our theoretical analysis II DETERMINING THE RELATIVE POSE FROM DISTANCE MEASUREMENTS: AT MOST SOLUTIONS Consider two robots R and R whose initial poses are indicated by the fraes of reference {} and {} respectively (cf Fig ) The two robots ove randoly {} ρ =d θ φ {} p p {} {} p {} d d p {} Fig The trajectories of robots R and R The odd (even) nubered fraes of reference depict the consecutive poses of robot R (R ) d ij, i {,,n }, j {,,n} denotes the distance between the two robots when aligned to fraes {i} and {j} respectively through a sequence of poses {}, {},,{n } for R, and {}, {},,{n} for R and easure their distance d ij, i {,,n }, j {,,n} at each of these locations Additionally, the robots are equipped with odoetric sensors for estiating their poses with respect to their initial fraes of reference That is, robot R estiates the position vectors p,, p n and the angles φ,, φ n necessary for deterining the rotational atrices C,, n C Siilarly the quantities p,, p n and φ,, φ n (and hence C,, nc) are estiated by robot R fro its own odoetry Our goal is to use the odoetry-based estiates and the n distance easureents to deterine the axiu nuber of solutions for the dof relative transforation between the two robots, ie, their relative position p and orientation φ = φ, or equivalently θ and φ, with ] cθ p = ρ sθ, C = cφ ] sφ sφ cφ Note that ρ = d is easured and consider known We first address the case when n = distance easureents (d, d, and d ) are available and prove the following lea: Lea : Given distance easureents between the two robots at different locations, the axiu nuber of possible solutions for the dof robot-to-robot transforation is We proceed by substituting the geoetric relations for the position vectors p, p (cf Fig ) () p = C T ( p + C p p ) () p = C T ( p + C p p ) () in the following expressions for the distance easureents d and d, respectively: d = p T p, d = p T p () Without loss of generality, we assue that only one of the robots records range easureents at each location If both robots easure the sae distance, the two easureents can be cobined to provide a ore accurate estiate of their distance Fro here on we use the concatenated fors cα and sα to denote the sin and cos functions of a real nuber α
After rearranging ters and substituting ρ = d for p T p, these can be written as: (d ρ p T p p T p )= ( p p ) T C p p T p () (d ρ p T p p T p )= ( p p ) T C p p T p () Note that the quantities on the left-hand side of these last two equations are known (easured or estiated), while the unknown variables θ and φ (ebedded in p and C) only appear in the right-hand side expressions Eqs () and () for a syste of non-linear equations in the unknowns θ and φ Applying standard nuerical techniques, such as Newton-Raphson 8], for solving this syste has a nuber of drawbacks Firstly, iterative processes often require a large nuber of steps before converging to a solution Secondly, in order for the algorith to converge to the correct answer, initial estiates close to the true values of the unknown variables need to be specified In practice, however, no such inforation is available; the only prior knowledge we have for θ and φ is that they lie within the interval, π) Furtherore, in the particular case where only n =distance easureents are available, the total nuber of solutions that need to be deterined is To copute all possible roots, the initial estiates for the unknowns θ and φ will need to span a wide range of values within the -diensional region, π), π) Such procedure would require a large nuber of initializations of the iterative process with no guarantees that all solutions will be coputed Instead we hereafter describe an eliination process to reove the quantities cθ, sφ, cφ fro the expression in Eqs () and () which results in a th order polynoial in the unknown variable y = sθ; all solutions of this polynoial can be deterined through efficient algoriths The idea behind this approach is siilar to the Gaussian Eliination in linear systes of equations Due to space liitations only the ain steps of this process are shown while reassignent of variables is used to preserve the clarity of presentation By substituting the displaceent estiates (known fro odoetry) for the two robots: ] ] ] ] a p =, a a p =, a b p = b b p = b in Eqs () and (), we have: (ρa cθ + ρa sθ a )cφ +(ρa sθ ρa cθ a 7 )sφ = a + ρ(a cθ + a sθ) (7) (ρb cθ + ρb sθ b )cφ +(ρb sθ ρb cθ b 7 )sφ = b + ρ(b cθ + b sθ) (8) with a (d ρ p T p p T p ) a a a + a a, a 7 a a a a b (d ρ p T p p T p ) b b b + b b, b 7 b b b b Eqs (7) and (8) can be written in a atrix for as: ] ] ] u v cφ w = v sφ w u (9) where u ρa cθ + ρa sθ a, v ρa sθ ρa cθ a 7 u ρb cθ + ρb sθ b, v ρb sθ ρb cθ b 7 w a + ρ(a cθ + a sθ), w b + ρ(b cθ + b sθ) Note that Eq (9), is linear in the unknowns cφ and sφ Solving for these two variables we have: ] cφ = ] v w v w () sφ det u w u w where, det = u v u v Substituting the above expressions for cφ and sφ in the trigonoetric constraint sφ +cφ =, results in a single equation in the variables cθ and sθ (v w v w ) +(u w u w ) =(u v u v ) (v + u )w +(v + u )w (v v + u u )w w =(u v u v ) () As described in 9], the ters v +u, v +u, v v +u u, and u v u v are all linear in cθ and sθ, while w, w, w w are all quadratic in the sae quantities Hence Eq () is a rd order polynoial in x cθ and y sθ, and can be written in the following sipler for: f = 9 x + 8 x y + 7 xy + x + xy + x+ y + y + y + = () where the constants,, 9 are functions of known quantities 9] The final step in the eliination process is to invoke the trigonoetric constraint f = x + y = () to eliinate x fro Eq () by using the Sylvester Resultant ] Specifically, by ultiplying Eq () with x, and Eq () with x and x and rewriting all these equations in a atrix for, we have: s s s s s s s s x x y y y where s y + y + y + s 7 y + y + s 8 y +, s 9 x x = () For the polynoials in Eqs () and () to have coon roots, the deterinant of the Sylvester atrix above ust be equal to zero It can be shown 9] that the deterinant is a th order polynoial in the single variable y: g = y + n y + n y + n y + n y + n y + n () where the constants n,,n are functions of the known quantities,, 9 Therefore, the axiu nuber of possible solutions, including coplex roots, is There are any standard ethods to copute the roots of a single variable polynoial ] Our approach is based on
the eigen-decoposition of the copanion atrix ]: n n n While this ethod will deterine all roots of the polynoial, only the real ones are of practical interest since they have a geoetric interpretation Once y is known, x is deterined by coputing the null space of the atrix in Eq () To prove our clai that there exist at ost solutions for (θ, φ), we need to show that for every solution of y (cf Eq ()), only one solution for x can be found (cf Eq ()) To do this, we need to use Groebner bases ] One base, g, is exactly the sae as the polynoial in Eq (), and g has the for g = x + k y + k y + k y + k y + k y + k where the constants k,,k are functions of the known quantities 9] Therefore, for every value of y there is only one solution of x corresponding to it In fact we can draw the sae conclusion without coputing the Groebner basis All we need to do is to show that the leading ter of g, LT(g ) is linear in x This can be easily seen by using the definition of a Groebner basis A set {g,,g s } I is a Groebner basis of an ideal I if and only if the leading ter of any eleent of I is divisible by one of the LT(g i ) We can construct one eleent q of the ideal I =< f,f > by setting q = f ( 9 x + 8 y + )f =( 7 9 )xy + (lower order ters) Since the leading ter LT(q)=( 7 9 )xy ust be divisible by LT(g ), the degree of x in LT(g ) has to be Equivalently, g is linear in x Hence, the total nuber of distinct solutions for (x, y) reains Finally, φ is uniquely deterined by back substitution of x = cθ and y = sθ in Eq () The total nuber of real roots in each case will depend on the robot trajectories A situation where real solutions exist is shown in Fig III DETERMINING THE RELATIVE POSE FROM DISTANCE MEASUREMENTS: AT MOST SOLUTIONS Consider now the case where the robots R and R continue their paths shown in Fig and ove to the new poses {7} and {8}, respectively, where they record an additional distance easureent d 78 We will prove the following: Lea : Given distance easureents between the two robots at different locations, the axiu nuber of possible solutions for the dof robot-to-robot transforation is We proceed in a siilar anner as for the case of distance easureents Specifically, the new position estiates for the two robots at the locations where they record their th distance easureent p 7 = e e ], p 8 = e e ] are related through the geoetric constraint (analogous to Eq ()): 7 p 8 = 7C T ( p + C p 8 p 7 ) () Substituting in the expression for the new distance easureent d 78 = 7 p T 8 7 p 8, results in the following equation: (d 78 ρ p T 8 p 8 p T 7 p 7 )= ( p p 7 ) T C p 8 p T p 7 (7) Following the sae algebraic process as in the previous section we have: (ρe cθ + ρe sθ e )cφ+(ρe sθ ρe cθ e 7 )sφ = e + ρ(e cθ + e sθ) (8) where e,e 7 are defined as before Rearranging Eqs (7), (8), and (8) in a atrix for, we have: u v w u v w cφ sφ = (9) u v w where the u i s, v i s, and w i s, i =,,, are functions of sθ, cθ, and known (easured or estiated) quantities For the above syste to have non-zero solutions, the deterinant of the coefficient atrix ust vanish, ie, (u v u v )w +(v u v u )w +(u v u v )w = Note that the ters u v u v, v u v u, and u v u v are again all linear in x cθ and y sθ and so are w, w, and w, which akes the above polynoial quadratic in x and y 9] Following the sae eliination procedure as in Section II, we arrive at a th order polynoial in y n y + n y + n y + n y + n = () where n,,n are known constants 9] In this case the axiu nuber of possible solutions for y is Once y is deterined, back-substitution allows us to deterine x Finally, sφ and cφ are retrieved by coputing the null space vector of the coefficient atrix in Eq (9) IV DETERMINING THE RELATIVE POSE FROM DISTANCE MEASUREMENTS: UNIQUE SOLUTION We now treat the case where the robots R and R ove again and arrive at the locations {9} and {}, respectively At that point, they record their th distance easureent d 9, and also have available the additional estiates for their positions p 9 and p We will first prove that in this case at ost one solution exists (Section IV-A) and then propose an efficient and robust algorith for coputing its value (Section IV-B) A Unique solution Lea : Given distance easureents between the robots at different locations, there exists at ost one solution for the dof robot-to-robot transforation Following the sae procedure as in Section II, we arrive at the following equations ( of these are the sae ones as
in Eq (9) and the th one is coputed in a siilar anner using the latest distance easureent): u i cφ + v i sφ = w i, i = () where the u i s, v i s, and w i s, are functions of cθ, sθ, and known constants 9] Choosing out of any of these equations, and using the eliination process detailed in Section II, we can derive polynoial equations each of th order in the unknown variable y = sθ: ξ,j y + + ξ,j y + ξ,j = () where the ξ i,j s, i, j =, are functions of easured and estiated quantities By rewriting these polynoials in atrix for, we have: ξ, ξ, y = ξ, ξ, y ξ, ξ, () Solving this linear syste of equations for the vector y = y y ] T, allows us to uniquely deterine the value of the unknown y Once y = sθ is uniquely deterined, the reaining unknowns, sθ, cφ, and sφ, can be coputed via back-substitution as in the previous two cases 9] B Efficient Coputation of the Unique Solution The approach for coputing the unique solution presented in the previous section, requires to repeat the eliination procedure of Section II ties In addition to being tie consuing, this ethod ay result in incorrect values for the robot-to-robot transforation or even fail due to the accuulation of nuerical errors In this section, we present an alternative approach based on a linear algorith that efficiently coputes the unique solution given distance easureents As described in Sections II, III, and IV-A, for each of the last distance easureents, d,,d 9,, we can write an equation siilar to Eq (7), repeated below after rearranging ters and renaing the known quantities α i,j s: α 7,j cφ + α,j sφ + α,j cθ + α,j sθ α,j c(θ φ) α,j s(θ φ)+α,j =, j = The unknowns in these equations are cφ, sφ, cθ, sθ, c(θ φ), s(θ φ) Rewriting the in a atrix for, we have cφ sφ α 7, α, cθ sθ = α 7, α, c(θ φ) Ax = s(θ φ) where A is the 7 coefficient atrix (known), and x is the unknown vector we want to solve for Once we have coputed the three vectors r, s, and t that span the null space of A, x can be written as: x = λ r + λ s + λ t () for soe scalars λ, λ, λ To deterine their values, we use the trigonoetric identities c φ + s φ =,c θ + s θ =,c (θ φ)+s (θ φ) = cθcφ + sθsφ = c(θ φ), sθcφ cθsφ = s(θ φ) () and λ r 7 + λ s 7 + λ t 7 = () where r 7, s 7, and t 7 denote the 7 th scalar eleents of vectors r, s, and t, respectively Substituting the corresponding eleents of x fro Eq (), in the constraints (), and eliinating λ using Eq (), we obtain the following syste of equations: λ β, β, λ λ λ β, β, λ = λ where β i,j s, i, j = and ε i s are functions of known quantities 9] This syste can be solved to uniquely deterine the unknown vector λ λ λ λ λ λ ] T Given the values of λ and λ, λ is coputed fro Eq () At this point the vector x (cf Eq ()) is uniquely deterined The unknown robot-to-robot transforation can be retrieved fro the first eleents of x V DETERMINING THE RELATIVE POSE FROM MORE THAN DISTANCE MEASUREMENTS When ore than distance easureents are available to the robots, their relative pose can be coputed with higher accuracy To do so we follow a two step procedure: (i) We process of these distance easureents to copute an initial estiate for the dof transforation (cf Section IV- B), (ii) We use this initial estiate in a weighted least squares algorith that processes all distance easureents available We hereafter describe the second step of this process Assue that the robots have recorded n distance easureents, which are used to for a syste of n nonlinear equations equivalent to Eq (7) Rearranging ters, these can be written in a copact for as ε ε h(x, u) = (7) where x = θ, φ] T is the vector of unknowns, and u = p T p T z T ] T is the vector of the known quantities: p = p T φ p T n φ n ] T estiated by robot R, p = p T φ p T n φ n ] T estiated by robot R, and z =d d (n )(n) ] T the distances easured by the robots Since p, p, and z are estiated or easured independently, the covariance atrix P of u has a block diagonal structure: P = P P R where P = E p p T ] (P = E p p T ])isthecovariance atrix for p ( p), and R = σd ij I n n is the covariance atrix for the noise in the distance easureents d ij, with σ dij denoting the standard deviation in each of the
TABLE I Given the estiate û of u (fro the robots odoetry and MEASUREMENTS CASE the recorded distance easureents) and the initial estiate Algorith IV-A Algorith IV-B ˆx of x coputed based on the ethod of the previous θ ean error (rad) 7 φ ean error (rad) section, the weighted least squares algorith coputes the θ error std (rad) 8 new estiate for x through the following iterative process: φ error std (rad) 9 97 ˆx κ+ = ˆx κ H T x (H u PH T u ) H x ] H T x (H u PH T u ) h(ˆx κ, û) where H x = h x x=ˆx κ, H u = h u Robot trajectories observed fro the overhead caera u=û 9 are the Jacobians of the nonlinear function h evaluated using 7 the current estiates for x and u The detailed expressions for these atrices are presented in 9] 8 At this point a coent is necessary on observability In order for the iterative least squares process to converge, the robot-to-robot transforation needs to be observable There exists a large nuber of singular configurations where the atrix H x looses rank (eg, when p = p and p = robot trajectory robot trajectory p ) In these cases, the robots will need to ove to distance easureent new locations and acquire additional range easureents A detailed study of the observability of the syste along with a list of cases when it becoes unobservable is presented Fig The trajectories of the two robots and the locations where distance in 9] easureents were recorded VI SIMULATION AND EXPERIMENTAL RESULTS ) Siulations: The purpose of our siulations is to verify the validity of the presented algoriths, and deonstrate the accuracy and robustness of the ethod presented in Section IV-B (vs that of Section IV-A) for coputing the relative pose of the two robots using distance easureents In our siulations we randoly generated trajectories for the robots and coputed the distances between the at distinct points The trajectories and distance easureents were generated as follows: (i) The two robots start at initial positions apart fro each other and record their first distance easureent (ii) Each robot rotates and oves approxiately towards a direction selected randoly fro a unifor distribution over, π) (iii) The robots record their distance easureent at the current position Steps (ii) and (iii) were repeated until distance easureents were collected While oving, the robots estiate their position and orientation independently based on their odoetric easureents In this case, the standard deviation of the noise in the robots linear and rotational velocity easureents was set to /sec and rad/sec, respectively The standard deviation of the noise in the distance easureents was c The solutions for the relative pose of the two robots were coputed using,, and distance easureents An exaple of the possible robot-to-robot transforations when distance easureents are available is shown in Fig In order to evaluate the accuracy of the ethod of Section IV-B copared to that of Section IV-A, we have coputed the ean error and the standard deviation for the unknown quantities θ, φ over trials These results are shownintablei As evident, both the ean value and the standard deviation of the error in the estiates coputed using Algorith IV- B, are significantly saller than those fro Algorith IV- A Furtherore, for higher values of easureent noise, Algorith IV-A fails to copute the correct solution in % of the cases copared to a % failure rate for Algorith IV- B Failure in this context is considered the case when the error in the estiate is larger than σ, where σ is obtained fro the diagonal eleents of the covariance atrix H T x (H u PH T u ) H x ] coputed subsequently using the iterative weighted least squares algorith ) Experients: For our experients we deployed two identical Pioneer II robots within an area of (cf Fig ) The robots estiated their poses with respect to their initial locations using linear and rotational velocity easureents fro their wheel-encoders An overhead caera ounted on the ceiling of the roo was used to provide ground truth for evaluating the errors in the coputed estiates Additionally, using the position easureents fro the caera, we were able to copute the distances between the and control their accuracy by adding noise in these easureents We have tested the algoriths presented in this work for the cases where - distance easureents were available to the robots In this experient, the standard deviation of the noise in the distance easureents was set to σ= The solutions with,, and distance easureents are shown in the first three coluns of Table II Note that for the case of or distance easureents, only the solutions which are closest to the true value (last colun, coputed using the caera) are included Finally, using as initial estiate the value of the relative pose coputed fro Algorith IV-B and distance easureents, we have tested the iterative least squares algorith for the case of distance easureents The coputed estiates in this case are shown in the th colun of Table II VII CONCLUSIONS AND FUTURE WORK In this paper, we presented efficient algoriths for solving the relative pose proble for pairs of robots oving on a plane using only robot-to-robot distance easureents Non-
robot trajectory robot trajectory robot trajectory robot trajectory robot trajectory robot trajectory (a) Solution (b) Solution (c) Solution robot trajectory robot trajectory robot trajectory robot trajectory robot trajectory robot trajectory (d) Solution (e) Solution (f) Solution Fig An experient with real solutions Solution is the true robot configuration The distance easureents are depicted by circles centered at robot R with radii equal to the distance to robot R at each location TABLE II RESULTS WITH,,, DISTANCE MEASUREMENTS No eas Ca θ (rad) -99-99 -99-898 -9 φ (rad) 8 8 8 8 iterative algoriths for coputing the initial estiate of the dof transforation were presented for the cases when, and distance easureents were available We have shown that the axiu nuber of solutions for the above cases are,, and respectively (ie, at least distance easureents are required to uniquely solve for the initial relative robot pose) Furtherore, we presented a novel linear algorith for coputing the unique solution that is robust to nuerical errors Finally, an iterative weighted least squares algorith was used to further refine the initial relative pose estiate provided by the non-iterative algorith Our approach does not require any robot coordination or specific otion strategies, thus increasing the flexibility of robot control One future extension of this work is the analysis of the relative robot transforation in D In this case, the transforation that we need to solve for has dof which akes the proble significantly ore challenging REFERENCES ] X S Zhou and S I Roueliotis, Multi-robot SLAM with unknown initial correspondence: The robot rendezvous case, in Proceedings of IEEE International Conference on Intelligent Robots and Systes, Beijing, China, Oct 9 -, pp 78 79 ] P Bahl and V Padanabhan, Radar: An in-building RF-based user location and tracking syste, in Proceedings of the IEEE INFOCOM, Tel Aviv, Israel, March, pp 77 78 ] J Djugash, S Singh, and P I Corke, Further results with localization and apping using range fro radio, in International Conference on Field and Service Robotics (FSR ), July ] L Doherty, K S J Pister, and L E Ghaoui, Convex position estiation in wireless sensor networks, in IEEE INFOCOM Proceedings Twentieth Annual Joint Conference of IEEE Coputer and Counications Societies, Anchorage, AK, April, pp ] Y Shang, W Rul, Y Zhang, and M P J Froherz, Localization fro ere connectivity, in Proceedings of the th ACM International Syposiu on Mobile Ad Hoc Networking and Coputing, Annapolis, MD, June, pp ] A Savvides, H Park, and M B Srivastava, The bits and flops of the n-hop ultilateration priitive for node localization probles, in Intl Workshop on Sensor Nets and Apps, Atlanta, GA, Sep, pp 7] J A Costa, N Patwari, and A O Hero, Distributed weightedultidiensional scaling for node localization in sensor networks, ACM Transactions on Sensor Networks, vol, no, pp 9, Feb 8] W Press, S Teukolsky, W Vetterling, and B Flannery, Nuerical Recipes in C Cabridge University Press, 988 9] X S Zhou and S I Roueliotis, Deterining the robot-torobot relative pose using range-only easureents, University of Minnesota, Minneapolis, MN, Tech Rep, May Online] Available: wwwcsunedu/ zhou/paper/distonlypdf ] A G Akritas, Sylvester s For of Resultant and the Matrix- Triangularization Subresultant PRS Method New York: Springer- Verlag, 99, pp ] D Nistér, An efficient solution to the five-point relative pose proble, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol, no, pp 7 77, June ] A Edelan and H Murakai, Polynoial roots fro copanion atrix eigenvalues, Matheatics of Coputation, vol, pp 7 77, 99 ] D Cox, J Little, and D O Shea, Ideals, Varieties, and Algoriths, nd ed Springer