An Efficient Least-Squares Trilateration Algorithm for Mobile Robot Localization

Transcription

1 he IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems October -5, St. Lous, USA An Effcent Least-Squares rlateraton Algorthm for Moble Robot Localzaton Yu Zhou, Member, IEEE Abstract A novel effcent trlateraton algorthm s resented to estmate the oston of a target object, such as a moble robot, n a D or D sace. he roosed algorthm s derved from a nonlnear least-squares formulaton, and rovdes an otmal oston estmate from a number (greater than or equal to the dmenson of the envronment) of reference onts and corresondng dstance measurements. Usng standard lnear algebra technques, the roosed algorthm has low comutatonal comlety and hgh oeratonal robustness. Error analyss has been conducted through smulatons on reresentatve eamles. he results show that the roosed algorthm has lower systematc error and uncertanty n oston estmaton when dealng wth erroneous nuts, comared wth reresentatve closed-form methods. I. IRODUCIO hs aer resents a novel effcent trlateraton algorthm whch facltates the self-localzaton of autonomous moble robots n D and D envronments. A. rlateraton Prncle rlateraton refers to ostonng an object based on the measured dstances between the object and multle reference onts at known ostons [,]. (Peole tend to call t multlateraton when more than three reference onts are used to oston the object. However, multlateraton has been used to name another rocess of oston estmaton based on the measured dfferences n the dstances between the object and three or more reference onts [].) In rncle, trlateraton locates an object by solve a system of equatons n the form of ( ) ( ) r, () where denotes the unknown oston of the object, the known oston of the th reference ont, and r the known dstance between and. Equaton () reresents a crcle n or a shere n, centered at wth a radus of r. Solvng a system of () s equvalent to fndng the ntersecton ont/onts of a set of crcles n or sheres n. In realty, trlateraton error arses due to the naccuracy n measurng dstances and mang reference onts, and s largely affected by the geometrcal arrangement of the Manuscrt receved on March,. Yu Zhou s wth the Deartment of Mechancal Engneerng, State Unversty of ew York at Stony Brook, Stony Brook, Y74, USA (hone: 6-6-8; fa: ; e-mal: yuzhou@notes.cc. sunysb.edu). reference onts and the object [4]. As a result, the nvolved crcles or sheres may not ntersect at the actual oston of the object, or even may not ntersect at all. hus, t s necessary to determne a best aromaton. B. Revew of Estng Algorthms hough straghtforward n concet, the trlateraton roblem s far from trval to solve, due to the nonlnearty of () and the errors n and r. A number of algorthms have been roosed to solve the trlateraton roblem, ncludng both closed-form and numercal solutons. o determne the D oston of an object based on the dstance measurements from three reference onts, Fang rovded a closed-form soluton by referencng to the base lane defned by the three reference onts [5]. A smlar formulaton was resented by Zegert and Mze [6]. Indeendent of the choce of any artcular frame of reference, Manolaks derved a more general closed-form soluton [4]. Hs work shows that the ostonng error s affected by the rangng errors, the geometrcal arrangement of the object and reference onts, and the nonlnearty of the algorthm. A few tyos n [4] were fed by Rao [7]. Recently homas and Ros roosed an alternatve closedform soluton usng Cayley-Menger determnants whch are related to the geometry of the tetrahedra formed by the object and three reference onts []. In a more general contet, Cooe resented a closed-form soluton for determnng the ntersecton onts of n sheres n n based on Gaussan elmnaton [8]. In general, closed-form solutons have low comutatonal comlety when the soluton of () ests. hey also facltate the theoretcal analyss of the algorthm erformance [,4]. However, n general closed-form solutons do not accommodate the stuaton that the nvolved sheres (crcles) do not ntersect at one ont,.e. no soluton ests for (). Moreover, estng closed-form solutons only solve for the ntersecton onts of n sheres n n. hey do not aly to determnng the ntersecton ont of >n sheres n n, where small errors n dstance measurng and reference ont mang can easly cause the nvolved sheres to fal to ntersect at one ont. In order to determne the hyscally estng locaton of the target object, even f no ntersecton ont ests, t s always necessary to determne a best aromaton whch mnmzes the resduals of () n some arorate form. umercal methods are n general necessary n order to rovde such an estmate, as ndcated n [,8] //$5. IEEE 474

2 Foy resented a numercal algorthm called aylor-seres estmaton whch solves the smultaneous set of algebrac oston equatons by teratvely mrovng an ntal guess wth local lnear least-sum-squared-error correctons []. Incororatng the dstance measurement errors, adv et al. comared three statstcal methods, a lnear least-squares estmator, an teratvely reweghted least-squares estmator and a nonlnear least-squares technque, and showed that n general, the nonlnear least-squares method erforms the best []. Hu and ang gave a geometrc elanaton of the otmal result n least-squares-based trlateraton, whch s the ont of tangency between the hyerellsod, determned by the standard devaton of the ostonng error, and the ntersecton of the hyersurfaces, determned by the constrants among the measurements []. Cooe also suggested a nonlnear least-squares method to obtan the aromate soluton, whch mnmzes the sum of the dfference between the measured and estmated dstances [8]. In another work, Pent et al. defned a robablstc model of the dstance measurement error and used the etended Kalman flterng to solve the trlateraton roblem []. In general, numercal methods are avalable to rovde an otmal estmaton of the oston of a target object, n artcular when no soluton ests for (). Moreover, numercal methods are n general not lmted to dealng wth n sheres n n. In fact, more accurate oston estmate s eected as the number of nvolved reference onts ncreases. However, comared wth closed-form solutons, numercal methods n general have hgher comutatonal comlety, and closed-form erformance analyss s n general not avalable. Many numercal methods lnearze the trlateraton roblem [,,], whch ntroduces etra errors nto oston estmaton. Many numercal methods nvolve a searchng rocess, such as ewton s method and the steeest descent method, whch teratvely mroves an ntal guess towards a converged oston estmate [8-]. However, most of these search algorthms are senstve to the choce of the ntal guess, and a global convergence towards the desrable oston estmate s not guaranteed. C. Overvew of the Proosed Algorthm Addressng the above-lsted ssues of estng trlateraton algorthms wth an emhass on moble robotcs, we roose an alternatve algorthm whch estmates the oston of a target object, such as a moble robot, based on the smultaneous dstance measurements from multle reference onts, by solvng a nonlnear least-squares formulaton of trlateraton usng standard lnear algebra technques. he roosed algorthm rovdes an otmal oston estmate of the ntersecton ont of n sheres n n (where n= or ), whch s not lmted to solvng for the ntersecton onts of eact n sheres n n. he roosed algorthm does not deend on the technques whch tend to be affected by algebrac sngulartes, such as matr nverson, and hence has hgh oeratonal robustness. hough not n closed form, the roosed algorthm has a low comutatonal comlety. he layout of ths aer s as follows. In Secton II, we wll derve and elan the roosed algorthm n detal. In Secton III, we wll analyze the erformance of the roosed algorthm through smulatons wth reresentatve eamles. Secton IV wll summarze ths work. II. PROPOSED RILAERAIO ALGORIHM A. onlnear Least-Squares Formulaton he goal of the roosed trlateraton algorthm s to estmate the oston of a moble robot based on the smultaneous dstance measurements from multle reference onts at known ostons. In order to obtan an otmal oston estmate of the robot from merfect dstance measurement and reference mang, we target our algorthm to solve the general nonlnear least-squares trlateraton formulaton. hat s, we defne an otmal aromaton of the oston of the nvolved moble robot n n ( n can be ether or corresondng to a D or D envronment, wth the global frame of reference attached to the envronment.) as ot arg mn S( ), () where ) [( ) ( ) r ] S(, denotes an estmate of the robot oston, the re-maed oston of the th reference ont, r the measured dstance between and, and the number of reference onts used to determne. Here, we are not constraned to the case of =n. Instead, we are gong to gve a soluton to the general case of n (and + ). Equaton () resents a nonlnear otmzaton roblem. Search-based otmzaton algorthms are commonly used to solve ths category of roblems, ncludng both local otmzaton algorthms, e.g. the steeest descent method and the ewton-rahson method, and global otmzaton algorthms, e.g. the smulated annealng and the genetc algorthm []. he steeest descent method and the ewton-rahson method n general converge to a local mnmum n the vcnty of the ntal guess, and the global otmzaton deends on the choce of the ntal guess. Meanwhle, for the method of smulated annealng and the genetc algorthm, n order to reach the global mnmum, the varous algorthm arameters and decson crtera of these methods need to be tuned to ft wth the secfc roblem. he lack of general, systematc methods for a moble robot to automatcally generate the ntal guess and choose algorthm-secfc arameters and crtera onboard to guarantee the global convergence n oston estmaton causes nconvenence n usng these search-based algorthms. In addton, the relatvely hgh tme comlety of the smulated annealng method and the genetc algorthm makes them not sutable for real-tme alcatons. We here derve an algorthm to solve the least-squares formulaton of trlateraton n () usng standard lnear algebra technques, whch guarantees globally otmal oston estmates and has low comutatonal comlety. 475

3 B. Dervaton We notce that, gven and r, solvng () s equvalent to solvng S ( ) a B [ ( ) c, () where a ( r ), r B [ ( ) I and c. o smlfy (), we ntroduce a lnear transform q c, (4) and obtan an equaton contanng no quadratc term of q ( a Bc cc c) { Bq[cc ( c c) q} qq q. (5) Defne f a Bc cc c and D B cc ( c c) I, we rewrte (5) as f [ D ( q q) q. (6) Moreover, we notce that n fact D ( q q) I cc, (7) whch s an nn symmetrc matr and does not contan q. Defnng H D ( q q) I, we obtan from (6) f Hq. (8) Equaton (8) s a lnear system of n equatons of the unknown n-dmensonal vector q. If H s full-rank (nvertble), q can be calculated easly as q H f or usng numercal methods such as Gaussan elmnaton [4]. However, t may haen that H s not full-rank. In fact, wthout makng more general roof, we have verfed by symbolc comutaton that the H constructed from an arbtrary set of =n ndeendent reference onts n (where n=) or (where n=) has a rank of n-, though the H constructed from >n n general has a rank of n. Moreover, when all are at the same heght,.e. wth the same value of, y or z, H has a zero row and a zero column and hence a rank of n-. In these cases, (8) does not reresent a system of n ndeendent lnear equatons, and hence we cannot unquely determne q from only (8). Instead, addtonal constrants need to be found to construct a new system of n ndeendent equatons so that the secfc soluton can be obtaned. Here we roose a unfed soluton rocedure for rank(h)=n and n-. Frst, denotng the kth comonent of f as f k and the kth row of H as h k, we construct a n- dmensonal vector f'=[f - f n,, f n- - f n ] and a (n-)n matr H'=[ h -h n,, h n- -h n ], and obtan from (8) f' H' q. () et, usng orthogonal decomoston [4], we obtan H' QU, () where Q s a (n-)(n-) orthogonal matr and U a (n-)n uer dagonal matr. hen, re-multlyng the both sdes of () by Q, we obtan Q f 'Uq. () Rewrtng () n ts scalar form, we have for the D case v uq uq uq, () v uq uq where v k denotes the kth comonent of Q f', u kj the (k,j) entry of U, and q k the kth comonent of q. From (), we obtan uv v uu u q ( ) ( ) q uu u uu u. () v u q q u u ow we have unknowns but equatons, and therefore need one more ndeendent equaton to solve for q. In fact, one vald constrant s q q q q q, (4) where q q can be obtaned from D q q H as q q r c c. (5) Substtutng () nto (4), we obtan uv v uu u v u [( ) ( ) q q q q ] [ ] q uu u uu u u u, (6) whch s a quadratc equaton of q and can be solved n closed form. Substtutng the resultng q nto (), we can obtan q and q. Smlarly, for the D case, we obtan from () v u q q. (7) u u In the D case, the constrant (4) becomes q q q q. (8) Substtutng (7) nto (8), we obtan v u [ q q q ] q, () u u whch s a quadratc equaton of q and can be solved n closed form. Substtutng the resultng q nto (7), we can obtan q. et, substtutng the resultng q nto (4), we obtan. he above rocess generally results n two canddates of, due to the dualty of (6) and (). However, only one of the two canddates s the true. o ck the correct one, the judgng crteron s usually very smle, such as that s known on one secfc sde of the base lane (or base lne) defned by the reference onts, or that current estmate of should be close enough to last. C. Summary Followng the above dervaton, the roosed trlateraton algorthm s summarzed as Algorthm. As ndcated n the dervaton, the roosed trlateraton algorthm rovdes an otmal estmaton of the locaton of a moble robot based on ts dstances from reference onts, where can be any nteger greater than or equal to n a D envronment or n a D envronment. Usng standard lnear algebra technques, the roosed algorthm s hghly tractable and has low comutatonal comlety. Wthout deendng on the technques whch tend to be affected by 476

4 algebrac sngulartes, such as matr nverson, the roosed algorthm also has hgh oeratonal robustness. III. ERROR AALYSIS he nut to the algorthm ncludes the maed ostons of the reference onts,, and the measured dstances between the robot and reference onts, r. In ractce, errors arse n due to naccurate mang of the reference onts, whch haens n both manual and robotc mang rocesses; and errors arse n r due to merfect dstance measurement of the range sensors. hese nut errors wll cause outut errors n the estmaton of the robot oston. A. Performance Indces Algorthm : rlateraton n n (n{,}) Inut: A set of reference onts { +, n}, and the corresondng set of dstances between and the unknown oston {r +, n}. Outut:. ) Calculate a, B, c, f, f', H, H', Q and U. ) Calculate q q from (5). ) For D trlateraton, calculate q from (6); for D trlateraton, calculate q from (). 4) For D trlateraton, calculate q and q from (); for D trlateraton, calculate q from (7). 5) Calculate from (4). 6) Choose one of the two canddates of. 7) Return. We defne r, r r and. Denotng the actual value and random error of the measurement as and resectvely, assumng that the nut errors are zero-mean random errors,.e. E()=, and followng a smlar dervaton as [4], we can obtan the mean vector E( ) and varance matr var( ) of the outut error as ( ) E[ ( )] vec[var( )], () ( ) ( ) var[ ( )] var( ), () where vec(m) denotes the vector created from a matr M by stackng ts columns. Equatons () and () show that the mean and varance of the outut error are drectly related to the varance of the nut error. In artcular, to evaluate the mact of the error of,, on, we assume that the comonents of are zero-mean random varables and uncorrelated from one another wth the same standard devaton for each coordnate, and, smlar to [,4], defne two erformance ndces, the normalzed total bas B whch reresents the systematc estmaton error, and the normalzed total standard devaton error S whch reresents the uncertanty of oston estmaton B S E( ) vec( I), () r[var( )] r( ), () where v denotes the norm of a vector v, and r(m) denotes the trace of a matr M. We notce that B and S are ndeendent of. Smlarly, to evaluate the mact of the error of r, r, on, we assume that the comonents of r are zero-mean random varables and uncorrelated from one another wth the same standard devaton r, and defne two erformance ndces corresondngly as E[ ( )] Br vec( I), (4) r r S r r{var[ ( )]} r( ). (5) r r r We notce that B r and S r are ndeendent of r. We have tested B, S, B r and S r through smulatons. he roosed trlateraton algorthm rovdes an otmal aromaton of the ntersecton ont of crcles n and sheres n. Wthout loss of generalty, our error analyss focuses on. A smlar trend can be found for. hrough reresentatve eamles, we test the roosed algorthm wth reference onts at frst and then wth 4 reference onts. he roosed trlateraton algorthm has been rogrammed n Matlab. ested on a Dell Lattude D6 lato comuter wth a.66 GHz Intel Core CPU, the average runnng tme for the algorthm s <.6 second. hs means that the roosed algorthm s hghly sutable for real-tme trlateraton tasks. B. rlateraton n wth Reference Ponts Followng the reresentatve eamles n [,4], we eamne a -reference case n n whch the XY coordnates of the reference onts form an equlateral trangle nscrbed n a crcle centered at the orgn of the frame of reference wth a radus of. he reference onts are located at [ 5 5 ], [ ] and [5 5 ]. We also assume that a moble robot moves across a square data acquston regon defned as {[,y,z] z=8, - 4,y4}. o evaluate the mact of on, we set wth dfferent values ( {,,,4,5,6,7,8,,}), run the smulaton wth samles for each, and calculate B and S across the above data acquston regon. he resultng varatons of B and S are consstent across the range of, as ndcated by () and (). Fgures and show the varaton of B and S for a reresentatve =7. We observe that both B and S ncreases as the robot moves away from the center of the base trangle defned by the reference onts. In artcular, at the center of the data acquston regon where =[,,8], we obtan 477

5 y y.8 B =.57 and S =.6; whle at the edge of the data acquston regon where =[-4,4,8], we obtan B =.7 and S =.87. Comared wth the results reorted n [,4] whch were generated from the eactly same smulaton settng, our algorthm has lower S values ([] reorts that S 7 (S =.4) when =[,,8], and S (S =.86) when =[-4,4,8].), whch means that the roosed trlateraton algorthm has a reduced uncertanty n oston estmaton when usng merfectly maed reference onts. o evaluate the mact of r on, we set r wth dfferent values ( r {,,,4,5,6,7,8,,}), run ormalzed Bas B the smulaton wth samles for each r, and calculate B r and S r across the above data acquston regon. he resultng varatons of B r and S r are consstent across the range of r, as ndcated by (4) and (5). Moreover, the smulaton results show that B r and S r have smlar trends of varaton to those of B and S resectvely, and ther values are very close to B and S resectvely. In artcular, for r =7, at =[,,8], we obtan B r =.54 and S r =.; Fg.. ormalzed total bas B obtaned from the -reference eamle ormalzed Standard Devaton S Fg.. ormalzed total standard devaton S obtaned from the - reference eamle.5. whle at =[-4,4,8], we obtan B r =. and S r =.78. hey are very close to the values of B and S at the same onts. For ths reason, we do not resent the fgures for B r and S r secfcally. Comared wth the results reorted n [,4] whch were generated from the eactly same smulaton settng, our algorthm has sgnfcant lower B r values ([] reorts that the mamum B r on the edge of the same data acquston regon s about. whle our result s..), whch means that the roosed trlateraton algorthm has a reduced systematc error n oston estmaton when usng erroneous dstance measurements. C. rlateraton n wth 4 Reference Ponts o test the erformance of the roosed trlateraton algorthm wth >n reference onts, we eamne a 4- reference case n n whch the XY coordnates of the 4 reference onts form a square nscrbed n the same crcle as n the -reference eamle (centered at the orgn of the frame of reference wth a radus of ). he reference onts are located at [ 5 5 ], [ 5 5 ], 5 ] [5 and [5 5 ] 4. Same as the -reference eamle, we also assume that the moble robot moves across a square data acquston regon defned as {[,y,z] z=8, -4,y4}. Fgures and 4 show the varatons of B and S (taken at =7). Smlar to the -referecne case, both B and S ncreases as the robot moves away from the center of base square defned by the reference onts. In artcular, at the center of the data acquston regon where =[,,8], we obtan B =.77 and S =8.7; whle at the edge of the data acquston regon where =[-4,4,8], we obtan B =. and S =.4. Comared wth the - reference case, we notce that there s an ncrease n the mnmum B due to the addton of another merfectly maed reference ont. However, the mamum B decreases. Moreover, S becomes lower, whch means that the uncertanty n oston estmaton, when usng merfectly maed reference onts, wll decrease by referrng to more reference onts. he varaton of S r s very close to that of S. In artcular, for r =7, at =[,,8], we obtan S r =8.; whle at =[-4,4,8], we obtan S r =.. For ths reason, we do not resent the fgures for S r secfcally. However, the varaton of B r (Fg.5) s sgnfcantly dfferent from that of B. In artcular, for r =7, at =[,,8], we obtan B r =.4; at =[-4,4,8], we obtan B r =.77. Comared wth those of the -reference case, both B r and S r are sgnfcantly lower, whch means that both the systematc error and the uncertanty n oston estmaton, when usng erroneous dstance measurements, wll decrease by combnng more dstance measurements. 478

6 y y y ormalzed Bas B IV. COCLUSIO.8.85 hs aer resents an effcent trlateraton algorthm whch estmates the oston of a target object, e.g. a moble Fg.. ormalzed total bas B obtaned from the 4-reference eamle ormalzed Standard Devaton S Fg.4. ormalzed total standard devaton S obtaned from the 4- reference eamle ormalzed Bas B r Fg.5. ormalzed total bas B r obtaned from the 4-reference eamle robot, based on the smultaneous dstance measurements from multle reference onts. Solvng the nonlnear leastsquares formulaton of trlateraton, the roosed algorthm rovdes an otmal oston estmate of the ntersecton ont of n sheres n n (n= for D envronments and n= for D envronments), not lmted to solvng for the ntersecton onts of eact n sheres n n. Usng standard lnear algebra technques, the roosed algorthm, though not n the closed form, has low comutatonal comlety and s hghly alcable to real-tme alcatons. Wthout deendng on the technques whch tend to be affected by algebrac sngulartes, such as matr nverson, the roosed algorthm has hgh oeratonal robustness. he smulaton results show that the algorthm s hghly effectve, wth lower systematc bas and estmaton uncertanty than reresentatve closed-form methods, when dealng wth erroneous nuts of dstance measurements and reference onts. he smulatons also show that ntroducng more reference onts and corresondng dstance measurements nto the trlateraton rocess wll n general reduce the estmaton uncertanty. hough targetng the alcatons n moble robotcs, t s our belef that the roosed trlateraton algorthm s alcable to any rangng-based object localzaton tasks n varous envronments and scenaros. REFERECES [] J. Borensten, H. R. Everett, L. Feng, D. Wehe, Moble robot ostonng: sensors and technques, Journal of Robotc Systems, 4(4): -4, 7. [] F. homas, L. Ros, Revstng trlateraton for robot localzaton, IEEE ransactons on Robotcs, (): -, 5. [] Multlateraton, htt://en.wkeda.org/wk/multlateraton. [4] D. E. Manolaks, Effcent soluton and erformance analyss of -D oston estmaton by trlateraton, IEEE ransactons on Aerosace and Electronc Systems, (4): -48, 6. [5] B. Fang, rlateraton and etenson to global ostonng system navgaton, Journal of Gudance, (6): 75-77, 86. [6] J. Zegert, C. D. Mze, he laser ball bar: a new nstrument for machne tool metrology, Precson Engneerng, 6(4): [7] S. K. Rao, Comments on effcent soluton and erformance analyss of -D oston estmaton by trlateraton, IEEE ransactons on Aerosace and Electronc Systems, 4(): 68, 8. [8] I. D. Cooe, Relable comutaton of the onts of ntersecton of n sheres n R n, he Australan & ew Zealand Industral and Aled Mathematcs Journal, 4(E): C46 C477,. [] W. H. Foy, Poston-locaton solutons by aylor-seres estmaton, IEEE ransactons on Aerosace and Electroncs Systems, AES- (): 87-4, 76. [] W. avd, W. S. Murhy Jr., W. Hereman, Statstcal methods n surveyng by trlateraton, Comutatonal Statstcs & Data Analyss, 7: -7, 8. [] W. C. Hu, W. H. ang, Automated least-squares adjustment of trangulaton-trlateraton fgures, Journal of Surveyng Engneerng, -4,. [] M. Pent, M. A. Srto, E. urco, Method for ostonng GSM moble statons usng absolute tme delay measurements, Electroncs Letters, (4): -, 7. [] J. C. Sall, Introducton to Stochastc Search and Otmzaton: Estmaton, Smulaton, and Control, John Wley & Sons, 5. [4] G. H. Golub, C. F. Van Loan, Matr Comutatons, rd Edton, he Johns Hokns Unversty Press, 6. 47