Autonomous Navgaton and Map buldng Usng aser Range Sensors n Outdoor Applcatons Jose Guvant, Eduardo Nebot and Stephan Baker Australan Centre for Feld Robotcs Department of Mechancal and Mechatronc Engneerng The Unversty of Sydney, NSW 2006, Australa jguvant/nebot @mech.eng.usyd.edu.au Abstract Ths paper presents the desgn of a hgh accuracy outdoor navgaton system based on standard dead reckonng sensors and laser range and bearng nformaton. The data valdaton problem s addressed usng laser ntensty nformaton. The beacon desgn aspect and locaton of landmarks are also dscussed n relaton to desred accuracy and requred area of operaton. The results are mportant for Smultaneous ocalzaton and Map buldng applcatons, (SAM), snce the feature extracton and valdaton are resolved at the sensor level usng laser ntensty. Ths facltates the use of addtonal natural landmarks to mprove the accuracy of the localzaton algorthm. The modellng aspects to mplement SAM wth beacons and natural features are also presented. These results are of fundamental mportance because the mplementaton of the algorthm does not requre the surveyng of beacons. Furthermore we demonstrate that by usng natural landmarks hgh accurate localzaton can be acheved by only requrng the ntal estmate of the poston of the vehcle. The algorthms are valdated n outdoor envronments usng a standard utlty car retroftted wth the navgaton sensors and a 1 cm precson Knematc GPS used as ground truth. 1 Introducton Relable localzaton s an essental component of any autonomous vehcle. The basc navgaton loop s based on dead reckonng sensors that predct the vehcle hgh frequency manoeuvres and low frequency absolute sensors that bound the postonng errors [1]. For almost every land navgaton applcaton we can always fnd an approprate combnaton of dead reckonng sensors that can be used to obtan a reasonable predcton of the trajectory of the vehcle, [2],[3]. Wth external sensors the problem s more complcated. Although there are many dfferent types of external sensors, only few of them can be used n a partcular applcaton and the relablty wll be functon of the envronment of operaton, [4]. It s well known that wth the dfferent GPS mplementatons, poston fxes wth errors of the order of 1 cm. to 100 m. can be obtaned n real tme. Nevertheless ths accuracy cannot be guarantee all the tme n most workng envronments where partal satellte occluson and multpath effects can prevent normal GPS recever operaton. Smlar problems are experenced wth some other type of sensors such as Stereo Vson, Ultrasonc, aser and Radars. A sgnfcant amount of work has been devoted to the use of range and bearng sensors for localzaton purposes. Ultrasonc sensors have been wdely used n ndoor applcatons [5], but they are not adequate for most outdoor applcatons due to range lmtatons and bearng uncertantes.
Stereovson has been the object of research n many mportant research laboratores around the world. Recently n [6], stereoscopc omn drectonal systems were used n ndoor localzaton applcatons. Ths type of sensor s based on a concal mrror and a camera that returns a panoramc mage of the envronment surroundng the vehcle. Although a promsng technology, the complexty and ts poor dynamc range made ths technque stll not very relable for outdoor applcatons. Mllmeter Wave Radar [7], s an emergng technology that has enormous potental for obstacle detecton, map buldng and navgaton n ndoor and outdoor applcatons. The man drawback of ths technology s ts actual cost but ths s expected to change n the near future. Mllmeter Wave Radar had been used for localzaton purposes n [8] and n SAM applcatons n [9]. In ths case, specal beacons were desgned to ncrease the echo return ntensty such that smple threshold or more sophstcated polarzaton technques can be used to dscrmnate beacons from background at the sensor level. Range and bearng lasers have become one of the most attractve sensors for localzaton and map buldng purposes due to ther accuracy and low cost. Most common lasers provde range and bearng nformaton wth sub degree resoluton and accuraces of the order of 1-10 cm n 10-50 meter ranges. There are a number of works that addressed the localzaton usng pose nformaton [10], [11]. These works update the poston of the vehcle based on the determnaton of the transformaton between the pose of the robot and the laser measurements. aser has also been used to determne natural features n ndoor envronments. In [12] a comparson of the behavour monocular, trnocular and laser n localzaton applcatons s presented. One of the most dffcult problems for any beacon localzaton based algorthm s not feature extracton, but feature valdaton and data assocaton. That s to confrm that the extracted feature s a vald feature and to assocate t wth a known or estmated feature n the world map. Data assocaton s essental for the SAM problem. Ths problem has been addressed n prevous works usng redundant nformaton by lookng for stable features [9] or usng a combnaton of sensors such as n [13], where vson nformaton s used to valdate certan type of features extracted form laser nformaton. Ths work makes use of laser ntensty nformaton to recognze landmarks. It presents the characterzaton of the laser and desgn ssues for landmark detecton usng ths type of laser. It demonstrates that hgh accurate localzaton can be obtaned wth ths nformaton. A full SAM mplementaton usng beacon and beacons and natural features s presented. Analyss of absolute and relatve errors are also dcussed. The navgaton algorthm s mplemented n nformaton form. Ths algorthm becomes more attractve that the standard Kalman flter for applcaton where the external nformaton s avalable from dfferent sources and at dfferent tmes [1]. Ths paper begns n Secton 2 by descrbng the modellng aspects of the navgaton loop and the extenson to SAM. The characterzaton of the sensor s presented n Secton 3 and the nformaton flter n Secton 4. Fnally Secton 5 and 6 present the expermental results and conclusons 2 Navgaton loop The navgaton loop s based on encoders and range/ bearng nformaton provded by a laser sensor. The models for the process and observaton are non-lnear. The encoders provde velocty and steerng angle nformaton that s used wth a knematc model of the vehcle to predct poston and orentaton. The predcton s updated wth external range and bearng nformaton provded by a laser sensor.
r r r z(k)=(r,b) Modellng Aspect A smple knematc model s used for ths expermentaton. Ths model can be extended to consder other parameters such as wheel radus and slp angle that can have sgnfcant mportance n other applcatons [3]. The vehcle poston s represented n global coordnates as shown n Fgure 1. The steerng control α s defned n vehcle coordnate frame. The laser sensor s located n the front of the vehcle and returns range and bearng related to objects at dstances of up to 50 meters. Hgh ntensty reflecton can be obtaned by placng hgh reflectvty beacons n the area of operaton. These landmarks are labelled as B (=1..n) and measured wth respect to the vehcle coordnates (x l,y l ), that s zk ( ) = ( r, β, I), where r s the dstance from the beacon to the laser, β s the sensor bearng measured wth respect to the vehcle coordnate frame and I s the ntensty nformaton. y B B3 b y l l x f a B1 x Fgure 1 Vehcle coordnate system Consderng that the vehcle s controlled through a demanded velocty v c and steerng angle α the process model that predct the trajectory of the centre of the back axle s gven by x& c v c cos( φ ) y& c = vc sn ( φc) (1) & φ c v c tan ( α ) The laser s located n the front of the vehcle. To facltate the update stage, the knematc model of the vehcle s desgned to represent the trajectory of the centre of the laser. Based on Fgure 1 and 2, the translaton of the centre of the back axle can be gven P = P + a T + b T C φ φ + π 2 (2) Beng P and P C the poston of the laser and the centre of the back axle n global coordnates. The transformaton s defned by the orentaton angle, accordng to the followng vectoral expresson: ( cos ( ),sn ( )) T φ = φ φ The scalar representaton s (3)
( φ) cos( φ π 2) ( φ) sn ( φ π ) x = x + a cos + b + c y = y + a sn + b + c 2 (4) Encoder aser H Pc (yc, xc) y l x l b y x a Fnally the full state representaton can be wrtten vc vc cos( φ ) ( a sn ( φ) + b cos( φ) ) tan ( α) x& vc y& vc sn ( φ ) ( a cos ( φ) b sn ( φ) ) tan ( α) = + & φ v c tan ( α ) Fgure 2 Knematcs parameters (5) The velocty s generated wth an encoder located n the back left wheel. Ths velocty s translated to the centre of the axle wth the followng equaton: v c ν e = 1-tan ( α ) H (6) Where for ths car H = 0.75m, =2.83 m, b = 0.5 and a = + 0.95m. Fnally the dscrete model n global coordnates can be approxmated wth the followng set of equatons: ( φ ) xk ( 1) + tv c ( k 1) cos ( k 1) xk () v yk () c = ( a sn ( φ( k 1) ) + b cos ( φ( k 1) )) φ() k tan ( α( k 1) ) yk ( 1) + tv c ( k 1) sn ( φ( k 1) ) + v c ( k 1) ( a cos ( φ( k 1) ) b sn ( φ( k 1) )) tan ( α( k 1) ) v c ( k 1) tan ( α ( k 1) ) (7) where T s the samplng tme, that n our case s not constant. The process can then be wrtten as a nonlnear equaton
X( k) = f( X( k 1), u( k 1) + µ ( k 1)) + ω ( k 1) X( k) f( X( k 1), u( k 1)) + ω ( k 1) + ω ( k 1) u f f (8) where X(k-1) and u(k-1) are the estmate and nput at tme k-1 and µ ( k 1) and ω ( k 1) are process noses. The process nose s manly due to measurements error n the velocty and steerng nput nformaton. The model for ω ( ) s gven by: [ u ] u( k 1) ω ( k) = f ( X, u) µ ( k) where f fu = = u (9) ( x, y, φ ) s the gradent of f wth respect to the nput u = ( u1, u2) = ( v, α ) and ( k ) ( u, u ) 1 2 The equaton that relates the observaton wth the states s 2 2 ( x x) + ( y y ) z r = h( X, x, y) = ( y y z ) β atan φ π + ( x ) 2 x where z and [ xyφ,, ] are the observaton and state values respectvely, and (, ) (10) natural landmarks. The observaton equaton can be expressed n short form as zk ( ) = hxk ( ( )) + η( k) (11) wth f u k µ s Gaussan nose. x y are the postons of the beacons or η R ( k) η( k) = (12) ηβ ( k) The noses µ ( k) and η( k) are assumed to be Gaussan, temporally uncorrelated and zero mean, that s E[ µ ( k)] = E[ η( k)] = 0 (13) wth correspondng covarance T T E µ () µ ( j) = δjq, (), E αν η() η ( j) = δjrr, β() (14) Smultaneous ocalzaton and Map Buldng The localzaton and map buldng problem can also be approached wth ths combnaton of sensors. In ths case the estmated locaton of the features or beacon becomes part of the state vector. The vehcle start at an unknown poston wth a gven uncertanty and obtan measurements of the envronment relatve to ts poston. Ths nformaton s used to ncrementally buld and mantan a navgaton map and localze wth respect to ths map. The state vector s now gven by:
X Îx Þ v = Ï x ß V = (,, f ) ³ (,,..,, ) x x y R x = x y x y ³ R Ð à 1 1 n n 3 N (15) where x v and x are the states of the vehcle and actual landmarks. The landmarks can be natural features of specal desgned beacon located at unknown locaton. The dynamc model of the extended system that consders the new states can now be wrtten: V ( ) ( + 1) = V ( ) ( + 1) = ( ) x k f x k x k x k It can be seen that the dynamc of the states x s nvarant snce the landmarks are assumed to be statc. Then the Jacoban matrx for the extended system becomes Î f Þ F «Î J1 «Þ = Ï x ß V = T X Ï ß Ï T «I ß Ï«I ß Ð à Ð à J R R I R 3x3 3xN NxN 1 ³, «³, ³ (16) (17) The observatons obtaned wth a range and bearng devce are relatve to the vehcle poston. The observaton equaton s a functon of the state of the vehcle and the states representng the poston of the landmark: ( ) (, ) (, ) ( ) ( ) 2 Ë( y- y ) Û p a = h ( X) = atan f a Ì - + ( x x ) Ü Í - Ý 2 2 2 r = h X = x y - x y = x- x + y- y r (18) where (x,y) s the poston of the vehcle, (x,y ) the poston of the landmark numbered and Φ the orentaton of the car. Then the Jacoban matrx of the vector (r,α ) respect to the varables (x,y, Φ,x,y ) can be evaluated usng: Î h Þ Î r r h Ï X ß Ï ( xy,, f,{ x, y} ) ß Ï ß = Ï ß = X Ï ha ß Ï a ß Ï Ð X ßà ÏÐ ( x, y, f,{ x, y} ) ßà wth Þ (19)
hr 1 = ¼[ Dx, Dy,0,0,0,..., -Dx,-Dy,0,...,0,0] X D h Î Dy Dx Dy Dx Þ a = -,,-1,0,0,...,,-,0,...,0,0 2 2 2 2 X Ï Ð D D D D ß à ( ), ( ), ( ) ( ) 2 2 D x = x- x D y = y- y D = D x + Dy (20) These equatons can be used to buld and mantan a navgaton map of the envronment and to track the poston of the vehcle. 3 Range/Bearng/Intensty laser nformaton Ths secton presents the descrpton of the laser and the beacon desgn aspects. The laser used n ths experment s the MS200 model manufactured by SICK. It can return up to 361 range values spaced 0.5 degrees. The current verson returns ntensty nformaton wth eght dfferent levels of magntude. Ths nformaton s used to detect beacons. The laser returns ntensty nformaton only from surfaces wth hgh reflectvty. Ths nformaton s extremely relable and becomes of fundamental mportance for navgaton purposes. The beacon desgn s of fundamental mportance for the successful operaton of the system. In a gven area of operaton, the accuracy of the navgaton system wll be a functon of the sze, shape and type of materal of the reflector. In order to optmally desgn the reflector t s essental to characterze the laser beam. A set of experments was desgned to obtan the laser parameters. A retro reflectve tape (1.5x15cm) was radally moved at a constant dstance R n steps of 5mm perpendcular to the laser beam. The Intensty output of the scanner was recorder for dfferent radus. The results correspondng to two dfferent radus are shown n fgures 3 and 4. level 2 level 1 R=5m Fgure 3. Intensty at 5m, beam 30mm, shadow 5 mm ( 5mm reflector)
level 2 level 1 R=10m Fgure 4. Intensty at 10m, beam 50mm, shadow 30 mm. ( 5mm reflector) Wth ths nformaton the angular resoluton of the scanner as well as the openng angle of the beam was evaluated. The characterzaton of the laser obtaned s shown n Fgure 5. The beam angle becomes approxmately 0.2 degrees. Ths determnes the mnmum area of a beacon that wll be able to return maxmum ntensty at a gven dstance. In our expermentaton we used standard damond grade reflectve tape. It was determned that the laser was able to detect beacons at dstances of over 35 meters usng reflectors wth an area of 900 cm 2. The sze and shape of the beacon also becomes mportant when hgh accuracy s requred. One of the problem s that at short ranges the landmarks wll be detected at dfferent bearng angles. Fgure 5 aser Characterstcs Ths problem s shown n Fgure 6 for a flat and cylndrcal reflector. It can be seen that dependng of the orentaton and poston of the vehcle the same beacon wll be detected a dfferent locatons. The beacon shape s also of mportance to be able to see the landmarks form dfferent vehcle orentatons. The cylnder shape shown n Fgure 6 becomes very attractve for vsblty purposes but t can generate dfferent range and bearng returns dependng on the poston of the vehcle. These problems make the observaton of the poston of landmarks less accurately. Fnally the V shape wth an angle of 40 degrees provded the best results as trade-off between vsblty and poston determnaton. For each applcaton the fnal selecton of the shape and sze of the landmarks wll depend on the number of landmarks, the requred accuracy and the area of operatons n relaton to the characterstc of the laser.
Aeff α α A0 Fgure 6 Dfferent type of Beacons Ths secton presented the man characterstcs of the laser scanner and addressed the beacon desgn problem. Ths nformaton s essental to evaluate the maxmum accuracy that can be obtaned wth ths navgaton system. 4 Informaton Flter In ths work we used the nformaton Flter, also known as nverse covarance flter [1], to mplement the navgaton algorthm. The nformaton flter s a Kalman flter that expresses the optmal estmate n terms of the nverse of the covarance matrx 1 Y ( j) = P ( j) (21) and the nformaton state vector 1 y( j) = P ( j) x( j). (22) Consder a lnear system represented by x( k) = F( k) x ( k 1) +ω( k), (23) where x (k) s the state vector at tme k, F (k) s the state transton matrx and ω ( k) s a whte process nose sequence wth T E[ ω( ) ω ( j)] = δ Q ( ). The observaton s modelled as j z( k) = H( k) x ( k) +η( k), (24) where z (k) s the observaton vector, (k) sequence wth E[ η( ) η ( j)] = δ R ( ). The nformaton flter can be wrtten as: T y ( k k) = y( k k 1) + ( k) (25) Y ( k k) = Y( k k 1) + I( k), (26) where 1 ( k) = H( k) R ( k) z( k) (27) j H s the observaton model and ( k) s the nformaton state contrbuton from the observaton z (k) and η s a whte observaton (measurement) nose 1 T I( k) = H( k) R ( k) H ( k) (28) s ts assocated nformaton matrx. The predctons are gven by: 1 y ( k k 1) = Y( k k 1) F( k) Y ( k 1 k 1) y( k 1 k 1) (29)
and [ 1 T ( 1) ( ) ( 1 1) ( ) ( ) Y k k = F k Y k k F k + Q k ] 1. (30) The update stage has the followng form: N y ( k k) = yˆ ( k k 1) + ( k) (31) j j= 1 N Y ( k k) = Yˆ ( k k 1) + I ( k), (32) j j= 1 where N s the total number external sensors. The nformaton flter has several advantages over the covarance form of the Kalman flter. It allows for the ntalsaton of the flter for the cases where P 0 1 s sngular. Furthermore, for mult-sensor systems, the computatonal requrement of the flter s less than those of the standard Kalman flter. The reason s that the nformaton flter requres the nverson of the nformaton matrx that s of the dmenson of the state vector, whle the standard form requres the nverson of the composte nnovaton covarance matrx whch s of the dmenson of the observaton vector. Also, as shown by equatons 25 and 26, the flter only requres addtons at the estmaton (update) stage. Ths property can be exploted for effcent data fuson for systems wth multple sources of nformaton. Ths wll be the case where more than one external sensor s avalable to update the dead reckonng nformaton. In our case the beneft are obtaned updatng the states n a sequental manner wth each landmark detected. Nonlnear Informaton Flter The predcton and observaton models for the vehcle under nvestgaton are non-lnear. For such system, a nonlnear nformaton flter can be used. Ths flter s equvalent to the Extended Kalman Flter and lnearses the nonlnear model around the nomnal state to obtan the best lnearsed estmates for the nonlnear system. Consder a nonlnear system represented by x( k) = f[ k, x ( k 1)] +ω( k) (33) wth the observaton model z( k) = h[ k, x ( k)] +η( k). (34) The nformaton contrbuton from an observaton for ths case s agan obtaned from equatons 27 and 28, substtutng H( k) = xh[ k, x( k k)] (35) and replacng z by ( h[ k, x( k k 1)] h[ k, x( k k 1)] x( k 1) ) z = z x k (36) where hx s the Jacoban of h wth respect to x. The nonlnear form of the nformaton flter s dentcal to ts lnear form. However, for the calculaton of the partal nformaton state vector (k) and ts assocated nformaton matrx I (k), equatons 35 and 36 must be used. The predcton equaton 29 s replaced by
y ( k k 1) = Y( k k 1) f[ k, x( k 1 k 1)]. (37) and the nverse covarance s updated wth: 1 T [ ] 1 Y( k k 1) = f ( k) Y ( k 1 k 1) f ( k) + Q ( k) (38) 5 Results x x The navgaton system was tested wth a utlty vehcle retroftted wth the sensors descrbed. The utlty car used for the experment s shown n Fgure 7. The laser and the GPS antenna are mounted n front of the vehcle. A map of the testng ste (landmarks postons) and a typcal car trajectory s shown n Fgure 8. The vehcle was drven at speed of up to 4 m/sec. The expermental runs were performed n the top level of the car park buldng of the unversty campus. Ths testng ste was chosen to maxmze the number of satellte n vew. A Knematc Glonass/GPS system of 1 cm accuracy was used to generate ground truth nformaton. The stars n the map represent potental natural landmarks and the crcles are the artfcal reflectve beacons. Although ths envronment s very rch wth respect to the number of natural landmarks, the data assocaton becomes very dffcult snce most of the landmarks are very close together. Under a small poston error the navgaton algorthm wll not be able to assocate the extracted features correctly. The ncluson of beacons becomes equvalent to the ntroducton of a dfferent type of landmark that s valdated at the sensor level. Ths wll make the data assocaton of the natural landmark possble wth the potental of a sgnfcant reducton of the localzaton error. Fgure 7. Utlty car used for the experments.
40 35 30 south < attude >North 25 20 15 10 5 0 5 10 20 10 0 10 20 West < ongtude > East Fgure 8 andmark Postons and a typcal trajectory ( attude and ongtude n meters ) Fgure 9 shows a typcal laser frame wth the vehcle postoned at (0,0). The lnes ndcate hgh ntensty reflecton and concde wth the reflectve beacons. 25 20 Y (n meters) 15 10 5 0 10 5 0 5 10 15 X (n meters) Fgure 9 A typcal laser frame
The data assocaton s then performed consderng the a-pror estmates and uncertantes n landmarks postons and the covarance of vehcle poston and orentaton. Navgaton usng beacon at known locatons The frst set of results corresponds to the localzaton algorthms usng the reflectve beacons at known locatons. The fnal trajectory wth the beacons used s presented n Fgure 10. 15 Vehcle Trajectory and andmarks "*" 10 5 0 5 lattude (meters) 10 15 20 25 30 35 40 25 20 15 10 5 0 5 10 longtude (meters) Fgure 10. Fnal estmaton usng artfcal landmarks Fgure 11 presents the 95 % confdence bounds of the estmated poston of the vehcle, contnuous lne, wth the true error, dotted lne. It can be seen that most of the errors are bounded by the 95 % confdence bounds estmated by the flter. It s also mportant to note that the localzer s able to estmate the poston of the vehcle wth and error of approxmate 6 centmetres. Ths s a very mportant achevement consderng the systematc errors present n the surveyng and detecton of the landmarks and vehcle model errors. A better representaton of the uncertanty n estmaton process can be obtaned consderng the complete covarance submatrx P xy. Fgure 12 presents the uncertanty n x and y consderng the off dagonal terms of P xy. The 2-D standard devaton errors are presented wth the ground truth provded by the GPS poston nformaton. It can be noted that the error regon reduces abruptly when the number of observatons ncrease, coordnate (-8.4,-1), that s ncreasng the number of beacons used n the update stage. Ths plot also presents the evoluton of the magntude of the uncertanty regons when no observatons are obtaned. Ths s due to the cumulatve effects of the model uncertanty.
Fnally, a subset of the trajectory presented n Fgure 13 when the vehcle s turnng, coordnate (-11,-33). At ths moment the model s expected to have some systematc errors due to slp and steerng nonlneartes. It can be seen that a strong correcton of few centmetres s performed by the flter n the update stage. Ths can be reduced usng a larger number of beacons or wth the addton of artfcal landmarks as wll be shown later. 0.35 Total Errors and 2 sgma devatons 0.3 0.25 Total Errors (n meters) 0.2 0.15 0.1 0.05 0 160 180 200 220 240 260 280 300 320 340 360 380 Tme Fgure 11 Standard devaton wth beacons 1 0 1 attude (meters) 2 3 4 5 6 8.4 8.2 8 7.8 7.6 7.4 7.2 7 6.8 ongtude (meters) Fgure 12, Poston estmates and varances
Vehcle Trajectory and andmarks "*" 31.4 31.6 31.8 32 lattude (meters) 32.2 32.4 32.6 32.8 33 33.2 33.4 15 14 13 12 11 10 9 8 7 longtude (meters) Fgure 13 Enhanced Trajectory. Navgaton usng SAM wth artfcal beacons The second expermental results correspond to SAM usng only beacons. In ths case t s not necessary to survey the poston of the beacons. Ths nformaton s obtaned whle the vehcle navgates. The system bulds a map of the envronment and localze tself. The accuracy of ths map s determned by the ntal uncertanty of the vehcle and the qualty of the combnaton of dead reckonng and external sensors. In ths expermental results an ntal uncertan of 10 cm n coordnates x and y was assumed. Fgure 14 shows the ntal part of the expermental run wth only few beacons detected. The actual trajectory s plotted as a contnuous lne whle the total GPS trajectory s drawn as a dotted lne. Fgure 15 presents the absolute error and the predcted standard devaton ( 2 σ bounds, 95 % confdence bounds ). These plots show that the bounds are consstent wth the actual error. It s also mportant to remark that the uncertanty n poston does not reduce below the ntal uncertanty. Ths s expected snce the laser nformaton s obtaned relatve to the vehcle poston. The only way the uncertanty can be reduced s by ncorporatng addtonal nformaton that s not correlated to the vehcle poston, such as GPS poston nformaton or recognzng a beacon wth known poston.
The laser range nnovaton sequence can be seen n Fgure 16. It remans whte and valdates the assumed statstc for the model and sensors. The landmark covarance estmaton s shown n Fgure 17. Ths fgure presents the varance of poston x and y and the estmated uncertanty of a selected group of landmarks. The ones wth oscllatory behavour correspond to the uncertanty of the vehcle. Ths s expected snce no external absolute nformaton s ncorporated by the flter. The orgnal uncertanty of a new landmark wll be a functon of the actual vehcle uncertanty and sensor nose. It can be seen that the landmark once created are started wth dfferent ntal covarances. Ths value s a functon of the current vehcle uncertanty and the qualty of the observaton. It then decrease wth tme to a value that wll not be smaller that the ntal uncertanty of the vehcle. It can also be apprecated from ths plot that the due to the correlaton of the map all landmarks are beng updated all the tme. Fnally Fgure 18 shows that snce we are stll usng the same number of beacons, there s no mprovement wth respect to the smoothness of the updates when compared to the absolute navgaton algorthm. There s a stll a strong correcton due to the falure of the vehcle s model, coordnate (-11,-33). Vehcle Trajectory and andmarks "*" 10 5 0 5 lattude (meters) 10 15 20 25 30 35 20 15 10 5 0 5 longtude (meters) Fgure 14. Intal part of the trajectory usng SAM wth beacons
0.9 Total Errors and 2 sgma devatons 0.8 0.7 Total Errors (n meters) 0.6 0.5 0.4 0.3 0.2 0.1 0 160 180 200 220 240 260 280 300 320 340 360 380 Tme Fgure 15 Absolute poston error and standard devaton. 0.5 Innovaton sequence & sgma and 2 sgma devatons for all beacons 0.4 0.3 0.2 Innovaton ( Meters ) 0.1 0 0.1 0.2 0.3 0.4 0.5 0 200 400 600 800 1000 1200 Observatons Fgure 16 Innovaton sequence SAM wth beacons
0.45 devaton of Xv,Yv, X1,Y1, X5,Y5, X10,Y10, 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 50 100 150 200 250 300 350 400 450 500 Fgure 17 Estmated devaton of poston and beacons 31.4 Vehcle Trajectory and andmarks "*" 31.6 31.8 32 lattude (meters) 32.2 32.4 32.6 32.8 33 33.2 33.4 15 14 13 12 11 10 9 8 7 longtude (meters) Fgure 18 Enhanced Trajectory
Navgaton usng SAM wth Natural Features The fnal expermental results correspond to SAM usng all the features avalable n the envronment. In ths case t s not requred to modfy the nfrastructure of the envronment wth the addton of beacons. The most relevant navgaton features are obtaned whle the vehcle navgates. The vehcle bulds a navgaton map of the envronment, mantans t and localzes tself. The accuracy of ths map s determned by the ntal uncertanty of the vehcle and the qualty of the combnaton of dead reckonng and external sensors nstalled n the vehcle and frequency of external observatons. In ths expermental results an ntal uncertan of 10 cm n coordnates x and y was also assumed. Fgure 19 shows the ntal part of the expermental run whle the system s stll ncorporatng new landmarks. The actual trajectory s drawn wth a contnuous lne whle the total GPS trajectory s plot as a dotted lne. Fgure 20 presents the absolute error wth the predcted standard devaton ( 2 σ bounds, 95 % confdence bounds ). These plots show that the bounds obtaned usng all landmarks are consstent wth the actual errors. It s also mportant to remark that the uncertanty n poston become sgnfcantly smaller than the SAM wth beacons only. Ths s due to a larger number of landmarks that ncorporate more nformaton to the flter. The uncertanty does not become smaller than the ntal uncertan. Ths s expected snce the laser nformaton s obtaned relatve to the vehcle poston. The laser range nnovaton sequence can be seen n Fgure 21. It remans whte and valdates the assumed statstc for the model and sensors. The landmark dentfcaton covarance s shown n Fgure 22. Ths fgure presents the varance of poston x and y wth the uncertanty of some selected landmarks. The ones wth oscllatory behavour correspond to the uncertanty of the vehcle. The landmarks are orgnally ncorporated wth an ntal uncertanty functon of vehcle and sensor covarances. The postons are then updated and ts uncertantes are reduced as shown n the Fgure. It can also be apprecated from ths plot that the due to the correlaton between landmarks and landmarks and vehcle s states, the landmark are beng updated all the tme even f they are not beng observed at the present tme. Fnally Fgure 23 shows that snce we are usng a larger number of features there s a consderable mprovement wth respect to the smoothness of the updates. Ths trajectory can be compared to Fgures 13 and 18 where a much smaller number of landmarks are beng used. Ths can be mportant for vehcle control purposes snce less demand wll be mposed on the control and actuators.
15 Vehcle Trajectory and andmarks "*" 10 5 0 5 lattude (meters) 10 15 20 25 30 35 40 25 20 15 10 5 0 5 10 longtude (meters) Fgure 19. Intal part of the trajectory usng SAM wth natural features and beacons 0.7 Total Errors and 2 sgma devatons 0.6 0.5 Total Errors (n meters) 0.4 0.3 0.2 0.1 0 160 180 200 220 240 260 280 300 320 340 360 380 Tme Fgure 20 Absolute poston error and standard devaton.
0.5 Innovaton sequence & sgma and 2 sgma devatons for all beacons 0.4 0.3 0.2 Innovaton ( Meters ) 0.1 0 0.1 0.2 0.3 0.4 0.5 0 500 1000 1500 2000 2500 Observatons Fgure 21 Innovaton sequence SAM wth natural features 0.3 devaton of Xv,Yv, X1,Y1, X5,Y5, X10,Y10, 0.25 0.2 0.15 0.1 0.05 0 50 100 150 200 250 300 350 400 450 500 Fgure 22 Estmated devaton of poston and selected features
Vehcle Trajectory and andmarks "*" 31.5 32 lattude (meters) 32.5 33 33.5 15 14 13 12 11 10 9 8 7 6 longtude (meters) Fgure 23 Enhanced Trajectory
6 Concluson Ths work presented the mplementaton of dfferent types of hgh accuracy navgaton algorthms for outdoor and ndoor applcatons. A characterzaton of a range/bearng/ntensty laser s also presented. Ths task s essental to desgn beacons for a partcular navgaton envronments. The modellng aspect and the desgn of navgaton algorthms are presented wth an mplementaton based on the Informaton Flter. Ths approach becomes more attractve than the standard Kalman flter form for the case where a large number of observaton are present. Sequental processng of the laser landmark nformaton becomes much more effcent snce t does not requre the re-evaluaton of the Kalman gan matrx. The modellng aspect has also been extended to consder Smultaneous ocalzaton and Map buldng (SAM). A full mplementaton of SAM usng beacons s also presented. Ths s an mportant contrbuton snce t does not requre any surveyng of the beacons. The actual results have shown that the algorthm can delver an accuracy n accordance to the ntal uncertanty of the vehcle. It s mportant to remark that the maps obtaned are relatve to the ntal poston and orentaton of the vehcle. In many applcaton ths wll be all that s needed to accomplsh a certan task. In case the absolute poston s requred to use external nformaton such as GPS, then the uncertanty needs to be ncorporated as shown n these two examples. It was also demonstrated that the algorthm successful buld and mantan a map for long runs. Ths expermental results presented a 3 km run and the algorthm remans stable. In fact after revstng the old landmarks the problem transform to the standard navgaton algorthm wth known beacon poston. Fnally SAM consderng all natural features s presented. It s demonstrated that t s not always necessary to use specally desgned beacon for navgaton purposes. In fact n ths case the only requrement for the algorthm was the ntal poston and uncertanty of the vehcle. Wth only ths nformaton the algorthm was able to estmate the poston of the vehcle wth cm accuracy. It s mportant to remarks that although n ths case the beacons were not requred, they can be of fundamental mportance for the data assocaton problem n cases were the dstance between landmarks s smaller than the poston error buld-up that wll eventually appear when explorng new areas. Ths wll always be a functon of the partcular applcaton. Although the Informaton flter mplementaton presented n ths paper s effcent for the navgaton problem t may be computatonally expensve for the SAM n the case where the number of landmarks become large. We are currently nvestgatng more effcent mplementatons of ths algorthm takng nto consderaton the sparseness of the matrx nvolved n SAM. References [1] Nebot E., Durrant-Whyte H., Hgh Integrty Navgaton Archtecture for Outdoor Autonomous Vehcles, Journal of Robotcs and Autonomous Systems, Vol. 26, February 1999, p 81-97. [2] Sukkareh S., Nebot E., Durrant-Whyte H., A Hgh Integrty IMU/GPS Navgaton oop for Autonomous and Vehcle applcatons, IEEE Transacton on Robotcs and Automaton, June1999, p 572-578. [3] Schedng S., Dssanayake, Nebot E., Durrant-Whyte H., An Experment n Autonomous Navgaton of an Underground Mnng Vehcle, IEEE Transacton on Robotcs and Automaton, Vol. 15, No 1, February 1999, p 85-95. [4] Nebot E., "Sensors used for autonomous navgaton", Advances n Intellgent Autonomous Systems, Chapter 7, pp. 135-156, ISBN 0-7923-5580-6, March 1999, Kluwer Academc Publshers, Dordrecht.
[5] Elfes A., Sonar based real-world mappng navgaton, IEEE J. Robotcs Automaton, 3(3), p 249-265, 1987. [6] Drocout C., Delahoche., Pegard C., Clerentn A., Moble Robot ocalzaton Based on an Omndrectonal Stereoscopc Vson Percepton System, Proc. of the 1999 IEEE Conference on Robotcs and Automaton, Detrot, USA, pp 1329-1334. [7] Clark S., H. Durrant-Whyte, "Autonomous land vehcle navgaton usng mllmeter wave radar", Int. Proc. of the IEEE Internatonal conference of Robotc and Automaton, Belgum, May 1998, p 3697-3792. [8] Durrant-Whyte H., An Autonomous Guded Vehcle for Cargo Handlng Applcatons, Int. Journal of Robotcs Research, 15(5): 407-441, 1996. [9] Clark S., Dssanayake G., Smultaneous ocalsaton and Map Buldng Usng Mllmetre Wave Radar to Extract Natural Features, Proc. of the 1998 IEEE Conference on Robotcs and Automaton, Detrot, USA 1999, pp 1316-1321. [10] Madhavan, R.; Dssanayake, M.; Durrant-Whyte, H.F.; Autonomous Underground Navgaton of an HD usng a combned ICP-EKF approach IEEE Conference on Robotcs and Automaton Proceedngs of the 1998 Belgum, p 3703-3708 [11] Jensfelt P., Chrstensen H., aser Based Pose Trackng, Proc. of the 1999 IEEE Conference on Robotcs and Automaton, Detrot, USA, pp 2994-2998. [12] Castellanos J., Martnez J., Nera J., Tardos J., Smultaneous Map Buldng and ocalzaton for Moble Robots: A multsensor Fuson Approach, Proc. of the 1998 IEEE Conference on Robotcs and Automaton, p 1244-1249. [13] Perez J., Castellanos J., Montel J., Nera J., Tardos J., Contnuous Moble Robot ocalzaton: Vson vs. aser, Proc. Of the 1999 IEEE Conference on Robotcs and Automaton, p 2917-2923. [14] Smth C., Feder J., eonard J., Multple Target Trackng wth navgaton Uncertanty, Internatonal conference on decson and Control, December 1999.