Estimating the Odometry Error of a Mobile Robot during Navigation

Transcription

1 Estatng the Odoetry Error of a Moble Robot durng Navgaton Agostno Martnell and Roland Segwart Autonoous Systes Lab Swss Federal Insttute of Technology Lausanne (EPFL) CH-5 Lausanne, Swtzerland e-al: agostno.artnell, Abstract. Ths paper addresses the proble of the odoetry error estaton durng the robot navgaton. The robot s equpped wth an external sensor (lke laser range fnder). Concernng the systeatc error an augented Kalan Flter s ntroduced. Ths flter estates a vector state contanng the robot confguraton and the paraeters characterzng the systeatc coponent of the odoetry error. It uses encoder readngs as nputs and the readngs fro the external sensor as observatons. The estaton of the non-systeatc coponent s carred out through another Kalan Flter where the observatons are obtaned by two subsequent robot confguratons provded by the prevous augented Kalan Flter. Both synchronous and dfferental drve systes are consdered. Key Words: Robot Navgaton, Kalan Flter, Odoetry, Autocalbraton Introducton Deternng the odoetry errors of a oble robot s very portant both n order to reduce the, and to know the accuracy of the state confguraton estated by usng encoder data. Odoetry errors can be both systeatc and non-systeatc. In a seres of papers Borensten and collaborators [3,, 5,, 7,, ] nvestgated on possble sources of both knd of errors. A revew of all the types of these sources s gven n []. In the work by Borensten and Feng [7], a calbraton technque called UMBark test has been developed to calbrate for systeatc errors of a oble robot wth a dfferental drve. Larsen et al. [, ] suggested an algorth that uses the robot s sensors to autoatcally calbrate the robot as t operates. In partcular, they ntroduced an augented Kalan flter (AKF ) whch sultaneously estates the robot confguraton and the paraeters characterzng the systeatc odoetry error. Ths flter uses encoder readngs as nputs and vson easureents as observatons. They referred to a oble robot wth a dfferental drve syste. Many nvestgatons have been carred out on the odoetry error fro a theoretcal pont of vew. Wang [7] and Chong and Kleean [9] analyzed the non-systeatc errors and coputed the odoetry covarance atrx Q for specal knd of the robot trajectory. Kelly [] presented the general soluton for lnearzed systeatc error propagaton for any trajectory and any error odel. Martnell [] derved general forulas for the covarance atrx and also suggested a strategy to estate the odel paraeters for a oble robot wth a synchronous drve syste. Ths strategy s based on the evaluaton of the ean values of soe quanttes (called observables) whch depend on the odel paraeters and on the chosen robot oton. In ths paper we suggest a ethod to estate both systeatc and non-systeatc odoetry error of a oble robot, durng navgaton. Concernng the systeatc coponent, we adopt the sae AKF ntroduced by Larsen et al. [, ] by consderng also the case of a synchronous drve. Concernng the non-systeatc paraeters, we ntroduce a new flter (the Observable Flter, OF) where the state to be estated contans the paraeters characterzng the non-systeatc error and the observatons are provded by the observables as defned n [] and whch can be evaluated by knowng two subsequent robot confguratons. In Secton we ntroduce the odels adopted to characterze the odoetry error for oble robot wth both synchronous and dfferental drve. In Secton 3 we suarze the AKF ntroduced by Larsen et al. [, ]. The new flter (OF) s presented n Sect.. In Secton 5 we show soe results obtaned through sulatons. Fnally, soe conlcusons are gven n Sect. The odoetry error odel We consder two dfferent drve syste: synchronous and dfferental. Concernng the forer we adopt the sae odel ntroduced n [] whereas for the latter we adopt a sple odel slar to the one ntroduced by Chong- Kleean ([9]). In the next subsectons we brefly suarze these odoetry error odels. Ths work has been supported by the European project RECSYS (Real-Te Ebedded Control of Moble Systes wth Dstrbuted Sensng)

2 . Synchronous Drve In the synchronous drve syste each wheel s capable of beng drven and steered. Let denote wth δρ and δθ respectvely the robot translaton and rotaton n the th te step wth respect to a global world-coordnate frae. Because of the odoetry errors these values dffer fro the encoder readngs. The odel here adopted assues that δρ and δθ are rando varables, uncorrelated, wth gaussan dstrbuton. In partcular ther ean values are gven by the encoder readngs corrected for the systeatc error. It s assued that the systeatc errors (both n translaton and rotaton) ncrease lnearly wth the dstance traveled by the robot. Therefore, δρ = δ ρ δρ e δθ = δθ e + E θ δρ e () where δρ e and δθe are the encoder readngs respectvely for the robot translaton and rotaton, and δ ρ and E θ characterze the systeatc errors. Fnally, t s also assued that the varances ncrease lnearly wth the dstance traveled by the robot. We therefore can wrte: δρ = δρ + ν ρ δθ = δθ + ν θ ν ρ N(,K ρ δρ e ) ν θ N(,K θ δρ e ) () The odoetry error odel here presented s based on paraeters. Two of the ( δ ρ, E θ ) characterze the systeatc coponents whereas the other two (K ρ, K θ ) characterze the non-systeatc coponents. Clearly, these paraeters depend on the envronent where the robot oves.. Dfferental Drve A sple way to characterze the odoetry error for a oble robot wth a dfferental drve syste s obtaned by odelng separately the error n the translaton of each wheel [9]. The actual translaton of the rght/left wheel related to the th te step s assued to be a gaussan rando varable satsfyng the followng relaton: δρ R/L = δρ R/L + ν R/L δρ R/L = δρ er/l δ R/L ν R/L N(,K w δρ er/l ) (3) In other words, both δρ R and δρ L are assued gaussan rando varables, whose ean values are gven by the encoder readngs (respectvely δρ er and δρ el ) corrected for the systeatc errors (whch are assued to ncrease lnearly wth the dstance traveled by each wheel), and whose varances also ncrease lnearly wth the traveled dstance. Moreover, t s assued that δρ R and δρ L are uncorrelated. Wth respect to the Chong-Kleean odel, only one paraeter (K w ) s here adopted to characterze both the varances for the rght and left wheel. The robot translaton and rotaton are gven by the followng relatons: δρ = δρr + δρ R δθ = δρr δρ L () dδ d where d s the estated dstance between the wheels and δ d characterzes the uncertanty on ths estaton. Clearly, the robot translaton and rotaton are correlated accordngly to the equatons (3-). The odoetry error odel here proposed s based on paraeters. Three of the ( δ R, δ L and δ d ) characterze the systeatc coponents whereas the last one (K w ) characterzes the non-systeatc coponents. In Sect. 3 and we suggest a strategy to sultaneously estate all these paraeters durng the robot navgaton. 3 Systeatc Paraeters Estaton durng Navgaton In order to estate the paraeters characterzng the systeatc error (both for synchronous and dfferental drve) we adopt the sae AKF ntroduced by Larsen et al. [, ] for the dfferental drve. Ths flter estates a state (the augented state) contanng the robot confguraton and the systeatc paraeters, through an extended Kalan flter (EKF). Let denote wth X the robot confguraton (X = [x, y, θ] T )andwthx a the augented state. We have, respectvely for the synchronous and dfferental drve X a =[x, y, θ, δ ρ,e θ ] T X a =[x, y, θ, δ R,δ L,δ d ] T The state X evolves accordngly to the dynacal equaton X + = f(x,u )whereu =[δρ,δθ ] T for the synchronous drve and U =[δρ R,δρL ]T for the dfferental drve. The observaton at the th te step depends on the current robot confguraton and t s assued to be affected by an error w wth a gaussan dstrbuton, zero-ean and covarance atrx R =<w w T >

3 z = h(x,w ) (5) The dynacal equaton for the augented state X a s gven by the equaton: X a+ = f a (X a,u ) () The functon f a, restrctly to the frst three coponents, s obtaned drectly fro the functon f ncludng the dependence on the systeatc paraeters n the nput U ; concernng the last coponents (two for the synchronous drve and three for the dfferental drve) f a s the dentty functon snce there s no evoluton n te for the error paraeters. In order to obtan the EKF equatons for the augented state (.e. the equatons of the AKF ), t s necessary to copute the Jacoban F a of the functon f a wth respect to X a and the Jacoban G a of the functon f a wth respect to the vector ν, whchs[ν ρ,ν θ ] T n the synchronous drve (eq. ()) and [ν R,ν L ] T n the dfferental drve (eq. (3)): F a = Xa f a Xa( ),U G a = ν f a Xa( ),U where X a ( ) s the state estated at the prevous te step and U s the ean value of the vector U prevously defned. The coputaton of these atrx can be found n [, ] for the dfferental drve and can be easly carred out for the synchronous drve. Once these atrx are known t s possble to pleent the AKF by applyng the standard equatons of the EKF ([, 3]). Non-Systeatc Paraeters Estaton durng navgaton The non-systeatc paraeters cannot be evaluated followng the prevous ethod. Indeed, by ncludng n the augented state the non-systeatc paraeters, the Kalan gan related to these paraeters s null. The dea we suggest here s based on the observables defned n []. The observables are rando varables related to a gven robot oton whose propertes (ean value and varance) depend on the paraeters characterzng the odoetry error and on the robot trajectory n the odoetry reference frae. It s possble to evaluate the observable ean value only by knowng the actual ntal and fnal confguraton. We buld another kalan flter where the state to be estated contans the non-systeatc paraeters and the observaton are drectly provded by the observable ean value estaton obtaned fro two subsequent robot confguraton estatons obtaned fro the AKF. Let denote wth K the vector contanng the non-systeatc paraeters. The dynacal and observatonal equatons are: K +j = f K (K )=K z+j Obs = Obs (K +j )+w+j Obs (7) (we use + j nstead of + to reark that the frequency of ths second flter s not necessarly the sae of the prevous one). Obs (K +j ) s the ean value of the chosen observable coputed wth the non-systeatc paraeters at the ( + j) th te step, and w+j Obs s a zero-ean rando varable whose covarance atrx contans both the covarance atrx of the chosen observable and the error n the robot confguraton estated by the AKF (.e. the atrx P a ( ) andp a (+j +j)), snce the observable ean value s estated fro two subsequent robot confguraton estatons obtaned fro the AKF and these estatons are affected by the error gven by the atrx P a. In order to ntroduce the adopted observable we defne the followng quanttes. Let X e, Y e and θ e the dsplaceents respectvely n the x-axs, y-axs and orentaton between the (+j) th and th te step as evaluated by the odoetry corrected for the systeatc errors by usng the systeatc paraeters estated by the AKF at the ( + j) th te step. Moreover, we denote wth X, Y and θ the sae dsplaceents as evaluated by the AKF. The observable we adopt s: z Obs = [ ( X X e ) +( Y Y e ), ( θ θ e ) ] T The ean value of the second coponent of ths observable can be coputed wthout approxaton for any trajectory followed by the robot between the ( + j) th and th te step []. Concernng the frst coponent the sae property holds only for the synchronous drve. However, even n ths case we show here the result obtaned by approxatng the trajectory by an arc of crcuference for the sake of splcty [5]. In the next subsecton we copute the ean value of ths observable for the synchronous drve. Concernng the dfferental drve we adopt a spler observable consstng only of the second coponent of the prevous observable, z Obs =( θ θ e ). ()

4 . Synchronous Drve It s possble to defne the robot trajectory by gvng the orentaton as a functon of the curve length. In the synchronous drve both the orentaton and the curve length are drectly estated by the odoetry. We obtan for the ncreents n the orentaton and translaton between the ( + j) th and th te step respectvely θ e = +j k= δθe k and ρe = +j k= δρe k. Moreover, we obtan for the ean value of the observable n (), ([5]) ( <z Obs >= Obs (K) = [K ρ ρ e +(δ ρ ρ e ) R{F (z)} cos( θe ) ( θ e ) where F (z) = z +e z z and z = K θ ρ e + covarance atrx. It can be carred out followng slar coputaton as descrbed n []. ), K θ ρ e ] T (9) ( E θ + θe ρ e ) ρ e. We do not report here the coputaton of the. Dfferental Drve Fro the equatons (3-) t s easy to obtan the ean value and the varance of the observable z Obs =( θ θ e ) ([]): <z Obs >= Obs (K) = K w( ρ er + ρ el ) (d δ d ) Cov Obs = K w ( ρ er d δd + ρ el) where ρ er = +j δρ er k and ρ el = +j δρ el. k k= k= () The state estated at the th te step by the OF s, respectvely for the synchronous and dfferental drve: K =[K ρ,k θ ] T K = K w () The equatons of the flter are the equatons of the EKF. Clearly, the atrx F = K f K s the dentty and the atrx G s the zero-atrx snce the dynacal equaton n (7) s not affected by any error. The atrx H (.e. the jacoban of the observatonal equaton wth respect to the state estated by the flter) s, respectvely for the synchronous and dfferental drve, the jacoban of the functon n equaton (9) and n equaton () wth respect to the state K n (). Fnally, the atrx R (.e. the error atrx of the observable when the state K s known) s gven by the su of the covarance atrx of the observable (Cov Obs ) plus the error atrx whch takes nto account the errors n the used confguraton estatons both at ( + j) th and th te step ( [ ] [ ] X z+j Obs [P ( + j + j)+p ( )] X z+j Obs T, where the atrx P s the subatrx of Pa contanng the covarance of the robot confguraton X). Observe that n the ost of cases the functon Obs (K) s lnear n K (second coponent n the synchronous drve and n the dfferental drve). However, the kalan flter s stll not optal snce the dstrbuton of w Obs s not gaussan. 5 Results We sulate a oble robot ovng n an envronent consstng of a square wth sde easure. Therefore, the ap conssts of four straght lne and t s a pror known. The external sensor s sulated through a functon whch provdes the dstance of the ap lnes fro the actual robot confguraton. In partcular, at each te step, 3 dstances are provded yeldng a deg angular resoluton. An error source s ntroduced by addng at each dstance a gaussan rando varable, zero-ean, and whose varance s equal to (3c). The rando varables correspondng to dfferent dstances are ndependent. The errors n the encoder readngs are obtaned by ntroducng gaussan rando varables accordngly to the odels descrbed n the sectons. and.. The AKF ntroduced n secton 3 s used to estate the robot confguraton (x, y, θ) and the systeatc paraeters (δ ρ and E θ for the synchronous drve and δ R, δ L and δ d for the dfferental drve). The systeatc paraeters are ntalzed n order to have a null systeatc error (δ ρ =,E θ =andδ R = δ L = δ d = ). The non-systeatc paraeters are ntalzed at a value whch dffers fro the actual one by a factor (we both consdered the cases of saller and larger ntal value obtanng slar results). Table shows the values of the adopted actual paraeters. We sulated the sae robot oton (a crcuference wth radus equal to 5) tes. The length of each robot oton s about 3. The error on the estated robot confguraton at each te step s about c for

5 δ ρ =. E θ = deg Kρ =(.e 3) K θ = deg δ R =. δ L =.9 δ d =. K w =(.5e ) Table. The actual systeatc and non-systeatc odel paraeters for the synchronous and dfferental drve the poston and deg for the orentaton (and ths s consstent wth the experental results obtaned n our laboratory []). Fnally, the flter frequency s set to the sae value ( cycle per c) for both the AKF and the OF..5 x..5 δ ρ.5.5 E θ δ ρ E θ (a) (b) (c) (d) K ρ x K θ K ρ 5 5 K θ (e) (f) (g) (h) Fg.. Sulaton results for the synchronous drve. The unts adopted to represent the odel paraeters are rad for angle and c for length Fg. shows the results related to the synchronous drve. Fg. a and b dsplay the ean values of δ ρ and E θ at each te step (δ ρ and E θ ). These ean values are plotted vs the traveled dstance (n ). These values are obtaned fro the sulated robot oton (for nstance, concernng the forer, δ ρ = s= δ ρ s ). Fg. c and d dsplay the accuracy on the prevous paraeter estatons (n ) (for nstance δρ δ ρ, where δ ρ = s= (δ ρ s δ ρ ) ). Fgures e-h show the results related to the non-systeatc paraeters. The frequency of the OF s the sae as for the AKF (.e. j = n the equatons (7-9)). Fg. e and f show the obtaned ean values of K ρ and K θ at each te step. Fg. g and h show the accuracy on the prevous estated paraeters n. Fg. show the results related to the dfferental drve. We plot the sae quanttes as n the prevous case. Fg. a-f concern the systeatc paraeters and fg. g and h concern the non-systeatc paraeter K w. We can conclude that t s possble to reach good accuracy on the paraeter estaton by ovng the oble robot along qute short dstances (3) Conclusons and Future Research A new flter, the OF, was ntroduced for the estaton of the non-systeatc odoetry error durng the robot navgaton. Ths flter s based on the Observables (ntroduced n a prevous work []) whch provde the observatons for an EKF whch estates a state contanng the paraeters characterzng the non-systeatc odoetry error. When ths new flter s used together wth the AKF (ntroduced by Larsen et al. [, ] and here extended to the case of a oble robot wth a synchronous drve) the sultaneous estaton of the systeatc and non-systeatc odoetry error can be carred out durng the robot navgaton. Both cases of synchronous and dfferental drve were consdered and the perforance of the proposed ethod was successfully tested through sulatons. We are pleentng the proposed strategy on a real oble platfor.

6 δ L. δ R.5.9 δ R δ d (a) (b) (c) (d) δ L δ d..... K W K W (e) (f) (g) (h) Fg.. Sulaton results for the dfferental drve. The unts adopted to represent the odel paraeters are rad for angle and c for length References. K.O.Arras, N.Toats, B.T.Jensen and R.Segwart, Multsensor on-the-fly localzaton: Precson and relablty for applcatons, Robotcs and Autonoous Systes 3, pp. 3 3,.. Y.Bar-Shalo, T.E.Fortann,, Trackng and data assocaton, atheatcs n scence and engneerng, Vol 79, Acadec Press, New York, Borensten J., Internal Correcton of Dead-reckonng Errors wth the Sart Encoder Traler, Internatonal Conference on Intellgent Robots and Systes, vol., pp. 7 3, 99.. Borensten J., The CLAPPER: A dual-drve Moble Robot wth Internal Correcton of Dead-reckonng Errors, Internatonal Conference on Robotcs and Autoaton, vol. 3, pp , Borensten J., Feng L., Correcton of systeatc odoetry errors n oble robots, Internatonal Conference on Intellgent Robots and Systes, vol. 3, pp , Borensten J., Feng L., Measureent and correcton of systeatc odoetry errors n oble robots, IEEE Transactons on Robotcs and Autoaton, vol., pp. 9, Borensten J., Feng L., UMBark - A ethod for easurng, coparng and correctng dead-reckonng errors n oble robots, Techncal Report UM-MEAM-9-, Unversty of Mchgan.. Borensten J., Experental results fro nternal odoetry error correcton wth the OnMate oble robot, IEEE Transactons on Robotcs and Autoaton, vol., pp , Chong K.S., Kleean L., Accurate Odoetry and Error Modellng for a Moble Robot, Internatonal Conference on Robotcs and Autoaton, vol., pp. 73 7, Kelly A, General Soluton for Lnearzed Systeatc Error Propagaton n Vehcle Odoetry, Internatonal Conference on Intelgent Robot and Systes (IROS) Mau, Hawa, USA, Oct. 9 - Nov. 3, pag 93-95,. T.D. Larsen, Optal Fuson of Sensors, PhD thess, Departent of Autoaton, Techncal Unversty of Denark, Sept. 99. T.D. Larsen, M. Bak, N.A. Andersen and O. Ravn, Locaton Estaton for Autonoously Guded Vehcle usng an Augented Kalan Flter to Autocalbrate the Odoetry, FUSION9 Spe Conference Las Vegas, USA, July J.J. Leonard, H.F. Durrant-Whyte, Drected Sonar Sensng for Moble Robot Navgaton, Kluwer Acadec Publshers, Dordrecht, 99.. Martnell A, The odoetry error of a oble robot wth a synchronous drve syste, IEEE Trans. on Robotcs and Autoaton Vol, NO. 3 June, pp Martnell A, Evaluatng the Odoetry Error of a Moble Robot, Internatonal Conference on Intelgent Robot and Systes (IROS) Lausanne, Swtzerland, Septeber 3 - October,, Lausanne, Swtzerland, pp Papouls A., Probablty, Rando Varables, and Stochastc Process McGRAW-HILL INTERNATIONAL EDITIONS, Wang C.M., Locaton estaton and uncertanty analyss for oble robots, Internatonal Conference on Robotcs and Autoaton, pp. 3 35, 9.. Zhejun F., Borensten J., Wehe D., Koren Y., Experental evaluaton of an Encoder Traler for dead-reckonng n tracked oble robots, IEEE Internatonal Syposu on Intellgent Control, pp , 995. Ths artcle was processed usng the TEX acro package wth ECMR3 style