We are IntechOpen, the world s leading publisher of Open Access books Built by scientists, for scientists. International authors and editors

Transcription

1 We ae IntechOpen, the wold s leadng publshe of Open Access books Bult by scentsts, fo scentsts 3, , M Open access books avalable Intenatonal authos and edtos Downloads Ou authos ae among the 151 Countes delveed to TOP 1% most cted scentsts 12.2% Contbutos fom top 500 unvestes Selecton of ou books ndexed n the Book Ctaton Index n Web of Scence Coe Collecton (BKCI) Inteested n publshng wth us? Contact book.depatment@ntechopen.com Numbes dsplayed above ae based on latest data collected. Fo moe nfomaton vst

2 Featue extacton: technques fo landmak based navgaton system X Featue extacton: technques fo landmak based navgaton system Molaletsa Namoshe 1,2, Oduetse Matsebe 1,2 and Nkgatho Tlale 1 1 Depatment of Mechatoncs and Mco Manufactung, Cente fo Scentfc and Industal Reseach, 2 Depatment of Mechancal Engneeng, Tshwane Unvesty of Technology, Petoa, South Afca 1. Intoducton A obot s sad to be fully autonomous f t s able to buld a navgaton map. The map s a epesentaton of a obot suoundngs modelled as 2D geometc featues extacted fom a poxmty senso lke lase. It povdes succnct space descpton that s convenent fo envonment mappng va data assocaton. In most cases these envonments ae not known po, hence maps needs to be geneated automatcally. Ths makes featue based SLAM algothms attactve and a non tval poblems. These maps play a pvotal ole n obotcs snce they suppot vaous tasks such as msson plannng and localzaton. Fo decades, the latte has eceved ntense scutny fom the obotc communty. The emegence of stochastc map poposed by semnal papes of (Smth et al., 1986; Moutale et al., 1989a; Moutale et al., 1989b & Smth et al., 1985), howeve, saw the bth of jont posteo estmaton. Ths s a complex poblem of jontly estmatng the obot s pose and the map of the envonment consstently (Wllams S.B et al., 2000) and effcently. The emegence of new sensos systems whch can povde nfomaton at hgh ates such as wheel encodes, lase scannes and sometmes cameas made ths possble. The poblem has been eseach unde the name Smultaneous Localzaton and Mappng (SLAM) (Duant-Whyte, H et al Pat I and II) fom ts ncepton. That s, to localze a moble obot, geometc featues/ landmaks (2D) ae geneated fom a lase scanne by measung the depth to these obstacles. In offce lke set up, pont (fom table legs), lne (walls) and cone (cone fomng walls) featues makes up a epeated ecognsable patten fomed by a the lase data. These landmaks o featues can be extacted and used fo navgaton puposes. A obot s pecepton of ts poston elatve to these landmaks nceases, mpovng ts ablty to accomplsh a task. In SLAM, featue locatons, obot pose estmates as well featue to obot pose coelatons statstcs ae stochastcally mantaned nsde an Extended Kalman flte nceasng the complexty of the pocess (Thope & Duant-Whyte, 2001). It s also mpotant to note that, though a SLAM poblem has the same attbutes as estmaton and tackng poblems, t s not fully obsevable but detectable. Ths has a huge mplcaton n the soluton of SLAM poblem. Theefoe, t s mpotant to develop obust extacton algothms of geometc featues fom senso data to ad a obot navgaton system.

3 348 Senso Fuson and Its Applcatons Accuate and elable maps geneated autonomously guaantees mpoved localzaton especally n GPS dened suoundngs lke ndoo (Hough, P.V.C, 1959). The use of odomety s not suffcent fo poston estmaton due unbounded poston eos. Theefoe, snce offce lke envonments conssts of plana sufaces, a 2D space model s adequate to descbe the obot suoundngs because objects ae pedomnantly staght lne segments and ght angle cones. Concdentally, lne segments and cone epesentaton ae the two most popula methods fo ndoo modellng fom a lase angefnde. The focus n ths pape howeve s cone extacton methods. A numbe of lne and cone extacton technques fst tansfom scan data nto Catesan space then a lnea egesson method o cone extacton algothm s appled. Some algothms employ Hugh tansfom (Hough, P.V.C, 1959). & (Duda, R. O, 1972) a popula tool fo lne detecton fom scan data due to ts obustness to nose and mssng data. It woks n senso measuement space. Howeve, the computatonal cost assocated to ts votng mechansm endes eal-tme mplementaton mpossble. On the othe hand, an ealy wok by (Cowley, J, 1989) paved the way to subsequent lne extacton methods fom a ange senso. In the wok, a pocess fo extactng lne segments fom adjacent co-lnea ange measuements was pesented. The Kalman flte update equatons wee developed to pemt the coespondence of a lne segment to the model to be appled as a coecton to estmated poston. The appoach was ecently extended by (Pfste, S.T et al. 2003), fst povdng an accuate means to ft a lne segment to a set of uncetan ponts va maxmum lkelhood fomalsm. Then weghts wee deved fom senso nose models such that each pont s nfluence on the ft s accodng to ts uncetanty. Anothe nteestng wok s one by (Roumelots & Bekey, 2000), whee two Extended Kalman fltes ae used to extact lnes fom the scan data. In the algothm, one Kalman flte s used to tack the lne segments whle the othe estmates lne paametes. The combnaton of the two fltes makes t possble to detect edges and staght lne segments wthn the senso feld of vew. Thee ae many featues types one can extact fom a lase senso, and ae dependent on the obstacles found n the oom. If the oom has cha and table, one would be tempted to extact pont featues fom the legs. Sze, shape and textue of objects contbute to the type of featue to extact fom the senso. The use of genealsed algothms s not uncommon,.e. algothms whch extact lnes fom wall, pont featues fom table legs and acs to categose ccula objects (Mendes, & Nunes, 2004). The paametes that dstngush each extacted featue makes up the map o state estmate. The key to a successful obot pose estmaton les n ts ablty to effectvely extact useful nfomaton about ts locaton fom obsevatons (L & Jlkov, 2003). Theefoe we poposed an mpoved cone detecton method to educe computatonal cost and mpoved obustness. The pape s stuctued as follows; secton 2 deals wth featue extacton, secton 3 dscuss the EKF-SLAM pocess. Secton 4 s esult and analyss, whle secton 5 coves concluson and futue wok. 2. Featue Extacton Featue extacton foms the lowe pat of the two layeed pocedue of featue detecton. The top te s the data segmentaton pocess, whch ceates clustes of ponts deemed to ognate fom the same obstacle. It goups measuements of a scan nto seveal clustes accodng to the dstances between consecutve scans. These segments sectos then ae fed to

4 Featue extacton: technques fo landmak based navgaton system 349 the featue extacton algothms, whee featues lke cones o lnes ae consdeed. These featues ae well defned enttes whch ae ecognsable and can be epeatedly detected. In ths pape, eal lase data fom the senso onboad a obot s pocessed to extact cone lke featues, common n most ndoo envonments. A obot used fo ths expement s called Mee-Cat and was developed n house, depcted by Fgue 1 below. Fg. 1. Mee-Cat moble platfom equpped wth Sck lase scanne. The obot has an upght boad at the top used fo tackng puposes va anothe lase senso. 2.1 Cone Extacton Most cone detecton algothms utlses a sldng wndow technque (Spnello, L, 2007) o pckng out the ends ponts of a lne segment as a cones, e.g. slght-and- Mege (Pfste, S.T et al. 2003). Ths s nomally whee two lne segments meet. Although, an algothm by (Ensele, T, 2001) s a Splt and Mege pocedue and t detemne cones lkewse, t has a slght vaaton n data pocessng. The followng subsectons dscuses methods of cone extacton, to be used by an ndoo navgaton system Sldng wndow cone detecto The sldng wndow technque has thee man pats; vectos detemnaton fom thee ponts (Catesan ponts), Angle check between the vectos, and the backwad check when a cones angle s satsfed. Fstly the sze of a wndow s detemned by pe-settng a mdpont poston. That s, a wndow secto sze of 11 sample scans has mdpont at 6 th sample data, 13 at 7 th, and 15 at 8 th and so on. The wndow s boken nto two vectos ( v and vj ), such that fo an 11 sample sze wndow, the fst and the eleventh samples ae temnal ponts of these vectos. Theefoe, the algothm assumes a cone f the vectos foms a tangula shape wth the mdpont sample beng one of ts vetexes. An teatve seach fo a cone angle s caed out by sldng the wndow step by step ove the ente scan. If condtons ae met a cone s noted at mdpont. That s, an up bound fo the angle

5 350 Senso Fuson and Its Applcatons between the vectos as well as the mnmum allowable opposte dstance c as shown n fgue 2b below ae set po. A cone s nomally descbed by angles less than 120 degees, whle the sepaaton dstance s tghtly elated to the angula esoluton of the lase angefnde. The dstance c s set to vey small values; computatons geate than ths value ae passed as cones. If a cone s detected, an nwad seach s conducted. Ths s done by checkng fo a cone angle volaton/ exstence between the 2 nd and 10 th, 3 d and 9 th, and so on, fo sample secto of 11 data ponts. Ths s fom the assumpton that a lnea ft can be pefomed on the vectos. The seachng outne of ths method aleady demand hgh computaton speed, theefoe nwad seach wll undoubtedly ncease the complexty. Fg. 2. (a), Sldng wndow technque. (b) Shows how two vectos cented at the mdpont ae deved f a cone f found. The temnal ponts ae at the fst and the eleventh pont gven that the mdpont of the secto s 6. The angle s calculated usng cosne ule, that s, 1 cos ( v. vj / ( v vj )). (1) Usng the above methods one uns nto the poblem of mappng outles as cones. Ths has huge mplcaton n eal tme mplementaton because computaton complexty of the SLAM pocess s quadatc the numbe of landmaks mapped. The outles o ghost landmaks coupt the EKF SLAM pocess Splt and Mege Lase senso poduces ange scans whch descbes a 2D slce of the envonment. Each ange pont s specfed n pola coodnates system whose ogn s the locaton of the senso on boad the obot. Scan data fom a lase ange fnde has almost neglgble angula uncetanty, and the nose on ange measuement s assumed to follow Gaussans dstbuton popetes. Data segments ognatng fom the same object can be epesented by a lne. And tadtonally, staght lnes ae epesented by the followng paametes y mx c (2) whee c and m s the y -ntecept and slope of a lne espectvely. The shotcomng wth ths epesentaton s that vetcal lnes eque nfnte m (gadent).

6 Featue extacton: technques fo landmak based navgaton system 351 Fg. 3. As the lne become vetcal, the slope appoaches nfnty. If objects n an envonment can be epesented by polygonal shapes, then lne fttng s a sutable choce to appoxmate objects shapes. Dung data segmentaton, clustes ae fomed, and a cluste can be epesented by a set of lnes, defned as follows: T C { l [ P, P, m, b] : 0 n} (3) f whee P and Pf ae espectvely the Catesan coodnates of the ntal and the end of a lne. Whle m and b ae the paametes of an th lne. A method poposed by [14] s used to seach fo a beakng pont of a cluste, whch occus at the maxmum pependcula dstance to a lne. The pocess stats by connectng the fst and last data ponts of a cluste by a staght lne ( Ax By C 0 ), whee A y f y ; B x f x ; C ( By f Ax f ). Then fo all data ponts between the exteme ponts, a pependcula dstance d to the lne s calculated. Such that d, k Ax By C k A k B 2 2. (4) If a toleance value s volated by the d then a beak pont s detemned, ths s done ecusvely untl the pont befoe last. The fnal step s to detemne staght lne paametes,.e. an othogonal egesson method (Mathpages ) s appled to detemne lnea ft that mnmzes quadatc eo. The pocess s gaphcally epesented by the fgue below

7 352 Senso Fuson and Its Applcatons Fg. 4. Recusve lne fttng To mtgate the nfnte slope poblem, a pola epesentaton o Hessen fom s used. In the method, each pont n the Catesan coodnate space adds a snusod n the (, ) space. Ths s shown the fgue 5 below. Fg. 5. Mappng between the Catesan space and the pola Space. The pola fom used to epesent lnes s gven as follows x cos( ) y sn( ) (5) by and s the angle between the x axs and the nomal of the lne as shown n whee 0 s the pependcula dstance of the lne to the ogn. The angle s bounded the fgue 6 below.

8 Featue extacton: technques fo landmak based navgaton system 353 Fg. 6. Fttng lne paametes. d s the fttng eo we wsh to mnmze. A lne s expessed n pola coodnates ( and ). ( x, y ) s the Catesan coodnates of a pont on the lne. Usng the above epesentaton, the splt-and-mege algothm ecusvely subdvdes scan data nto sets of collnea ponts, appoxmated as lnes n total least squae sense. The algothm detemnes cones by two man computatons, the lne extacton and collecton of endponts as cones. Intally, scanned data s clusteed nto sectos assumed to come fom the same objects. The numbe of data ponts wthn a cetan cluste as well as an dentfcaton of that cluste s stoed. Clustes ae then passed to a lne fttng algothm (Lu & Mlos, 1994). When we pefom a egesson ft of a staght lne to a set of ( x, y ) data ponts we typcally mnmze the sum of squaes of the "vetcal" dstance between the data ponts and the lne (Mathpages ).Theefoe, the am of the lnea egesson method s to mnmze the mean squaed eo of { cos sn( )} 2 (6) 2 d x y such that ( x, y ) ae the nputs ponts n Catesan coodnates. The soluton to the lne paametes can be found by takng the fst devatve of the equaton 6 above wth espect to and espectvely. We assume that

9 354 Senso Fuson and Its Applcatons 2 2 d d 0 and 0 Lne paametes can be detemned by the followng tan(2 ) 2 ( y y )( x x ) m m 2 2 [( ym y ) ( xm x ) ] 2 ( ym y )( xm x ) 0.5a tan [( ym y ) ( xm x ) ] (7) (8) f we assume that the Centod s on the lne then can be computed usng equaton 4 as: whee x cos( ) y sn( ) (9) m x 1 m N and y m 1 N m x y (10) ae ( x m, y m ) ae Catesan coodnates of the Centod, and N s the numbe of ponts n the secto scan we wsh to ft lne paamete to. Fg. 7. Fttng lnes to a lase scan. A lne has moe than fou sample ponts.

10 Featue extacton: technques fo landmak based navgaton system 355 Dung the lne fttng pocess, futhe splttng postons wthn a cluste ae detemned by computng pependcula dstance of each pont to the ftted lne. As shown by fgue 6. A pont whee the pependcula dstance s geate than the toleance value s maked as a canddate splttng poston. The pocess s teatvely done untl the whole cluste scan s made up of lnea sectons as depcted by fgue 7 above. The next pocedue s collecton of endponts, whch s jonng ponts of lnes closest to each othe. Ths s how cone postons ae detemned fom splt and mege algothm. The fgue below shows extacted cones defned at postons whee two lne meet. These postons (cones) ae maked n pnk. Fg. 8. Splttng poston taken as cones (pnk maks) vewed fom successve obot postons. The fst and second extacton shows 5 cones. Inteestngly, n the second extacton a cone s noted at a new poston, In SLAM, the map has total of 6 landmaks n the state vecto nstead of 5. The assocaton algothm wll not assocate the cones; hence a new featue s mapped couptng the map. The splt and mege cone detecto bngs up many possble cones locatons. Ths has a hgh pobablty of couptng the map because some cones ae ghosts. Thee s also the ssue of computaton buden bought about by the numbe of landmaks n the map. The standad EKF-SLAM eques tme quadatc n the numbe of featues n the map (Thun, S et al. 2002).Ths computatonal buden estcts EKF-SLAM to medum szed envonments wth no moe than a few hunded featues.

11 356 Senso Fuson and Its Applcatons Poposed Method We popose an extenson to the sldng wndow technque, to solve the computatonal cost poblem and mpove the obustness of the algothm. We stat by defnng the lmtng bounds fo both angle and the opposte dstance c. The fst assumpton we make s that a cone s detemned by angles between 70 to 110. To detemne the coespondng lowe and uppe bound of the opposte dstance c we use the mnus cosne ule. Followng an explanaton n secton 2.1.1, lengths vectos of ae detemned by takng the modulus of v and vj such that a v and b v j. Usng the cosne ule, whch s bascally an extenson of the Pythagoas ule as the angle nceases/ deceases fom the ctcal angle (90), the mnus cosne functon s deved as: c a b abf 2 ( ) whee c ( a b ) f ( ) 2ab whee f ( ) s mnus cosne. The lmts of opeatng bounds fo c can be nfeed fom the output of f ( ) at coespondng bound angles. That s, s dectly popoton to dstance c. Acute angles gve negatve esults because the squae of c s less than the sum of squaes of a andb. The fgue 9 below shows the angle-to-sdes assocaton as well as the coespondng f ( ) esults as the angle gows fom acuteness to obtuseness. (11)

12 Featue extacton: technques fo landmak based navgaton system 357 Fg. 9. The elaton of the sde lengths of a tangle as the angle nceases. Usng mnus cosne functon, an ndect elatonshp s deduced as the angle s nceased fom acute to obtuse. The f ( ) functon ndectly has nfomaton about the mnmum and maxmum allowable opposte dstance. Fom expement ths was found to be wthn [ ]. That s, any output wthn ths egon was consdeed a cone. Fo example, at 90 angle c a b, outputtng zeo fo f ( ) functon. As the angle nceases, acuteness ends and obtuseness stats, the elaton between 2 c and a b 2 2 s evesed. The man am of ths algothm s to dstngush between legtmate cones and those that ae not (outles). Cone algothms usng sldng wndow technque ae susceptble to mappng outle as cones. Ths can be shown pctoal by the fgue below

13 358 Senso Fuson and Its Applcatons Fg. 10. Outle cone mappng s the change n angle as the algothm checks consecutvely fo a cone angle whee between ponts. That s, f thee ae 15 ponts n the wndow and cone condtons ae met, cone check pocess wll be done. The pocedue checks fo cone condton volaton/ acceptance between the 2 nd & 14 th, 3 d & 13 th, and lastly between the 4 th & 12 th data ponts as potayed n fgue 10 above. If does not volate the pe-set condton,.e. (cone angles 120) then a cone s noted. c s the opposte dstance between checkng ponts. Because ths paamete s set to vey small values, almost all outle cone angle checks wll pass the condton. Ths s because the dstances ae nomally lage than the set toleance, hence meetng the condton. The algothm we popose uses a smple and effect check, t shfts the mdpont and checks fo the peset condtons. Fgue 11 below shows how ths s mplemented Fg. 11. Shftng the md-pont to a next sample pont (e.g. the 7 th poston fo a 11 sample sze wndow) wthn the wndow angles ae almost equal, because the angula As depcted by fgue 11 above, and esoluton of the lase senso s almost neglgble. Hence, shftng the Md-pont wll almost gve the same cone angles,.e. wll fall wth the f ( ) bounds. Lkewse, f a Md

14 Featue extacton: technques fo landmak based navgaton system 359 pont concdes wth the outle poston, and cone condtons ae met,.e. and c (o f ( ) condtons) ae satsfes evokng the check pocedue. Shftng a mdpont gves a esults depcted by fgue 12 below. Fg. 12. If a Md-pont s shfted to the next consecutve poston, the pont wll almost cetanly be n-lne wth othe pont fomng an obtuse tangle. Evdently, the cone check pocedue depcted above wll volate the cone condtons. We expect angle to be close to 180 and the output of f ( ) functon to be almost 1, whch s outsde the bounds set. Hence we dsegad the cone fndngs at the Md-pont as ghost,.e. the Md-pont concde wth an outle pont. The fgue below shows an EKF SLAM pocess whch uses the standad cone method, and mappng an outle as cone. Fg. 13. Mappng outles as cones lagely due to the lmtng bounds set. Most angle and opposte dstances pass the cone test bounds.

15 360 Senso Fuson and Its Applcatons Fg. 14. A pseudo code fo the poposed cone extacto.

16 Featue extacton: technques fo landmak based navgaton system 361 A pseudo code n the fgue s able to dstngush outle fom legtmate cone postons. Ths s has a sgnfcant mplcaton n eal tme mplementaton especally when one maps lage envonments. EKF-SLAM s complexty s quadatc the numbe of landmaks n the map. If thee ae outles mapped, not only wll they dstot the map but ncease the computatonal complexty. Usng the poposed algothm, outles ae dentfed and dscaded as ghost cones. The fgue below shows a mappng esult when the two algothms ae used to map the same aea Fg. 15. Compason between the two algothms (mappng the same aea) 3. EKF-SLAM The algothm developed n the pevous chapte fom pat of the EKF-SLAM algothms. In ths secton we dscuss the man pats of ths pocess. The EKF-SLAM pocess conssts of a ecusve, thee-stage pocedue compsng pedcton, obsevaton and update steps. The EKF estmates the pose of the obot made up of the poston (, ), x y and oentaton togethe wth the estmates of the postons of the N envonmental featues f, whee 1 N, usng obsevatons fom a senso onboad the obot (Wllams, S.B et al. 2001). SLAM consdes that all landmaks ae statonay; hence the state tanston model fo the th featue s gven by: x ( k) x ( k 1) x f, f, f, It s mpotant to note that the evoluton model fo featues does have any uncetanty snce the featues ae consdeed statc. x (12) 3.1 Pocess Model Implementaton of EKF-SLAM eques that the undelyng state and measuement models to be developed. Ths secton descbes the pocess models necessay fo ths pupose Dead-Reckoned Odomety Measuements Sometmes a navgaton system wll be gven a dead eckoned odomety poston as nput wthout ecouse to the contol sgnals that wee nvolved. The dead eckoned postons can

17 362 Senso Fuson and Its Applcatons be conveted nto a contol nput fo use n the coe navgaton system. It would be a bad dea to smply use a dead-eckoned odomety estmate as a dect measuement of state n a Kalman Flte (Newman, P, 2006). Fg. 16. Odomety alone s not deal fo poston estmaton because of accumulaton of eos. The top left fgue shows an eve nceasng 2 bound aound the obot s poston. x (1), x (2), x (3), x ( k) of dead eckoned postons, we need to Gven a sequence fgue out a way n whch these postons could be used to fom a contol nput nto a navgaton system. Ths s gven by: u ( k) x ( k 1) x ( k) o o o Ths s equvalent to gong back along x ( k 1) 0 and fowad along x ( k) 0 small contol vecto ( k) 0 (13). Ths gves a u deved fom two successve dead eckoned poses. Equaton 13 subtacts out the common dead-eckoned goss eo (Newman, P, 2006). The plant model fo a obot usng a dead eckoned poston as a contol nput s thus gven by: X ( k) f ( X ( k 1), u( k)) X ( k) X ( k 1) u ( k) o and ae composton tansfomatons whch allows us to expess obot pose descbed n one coodnate fame, n anothe altenatve coodnate fame. These composton tansfomatons ae gven below: x x cos y x x y x sn y 1 2 sn cos (14) (15) (16) x1 cos1 y1 sn1 x1 x1 sn1 y1 cos 1 1 (17)

18 Featue extacton: technques fo landmak based navgaton system Measuement Model Ths secton descbes a senso model used togethe wth the above pocess models fo the mplementaton of EKF-SLAM. Assume that the obot s equpped wth an extenal senso capable of measung the ange and beang to statc featues n the envonment. The measuement model s thus gven by: ( k) z( k) h( X ( k), x, y ) h( k) ( k) (18) ( x, y ) ae the coodnates of the poston of the obot at tme k. ( k ) 2 2 x x y y 1 y y tan x x th featue n the envonment. ( k) h (19) (20) X s the ( x, y ) s the senso nose assumed to be tempoally uncoelated, zeo mean and Gaussan wth standad devaton. ( k ) and ( k) the ange and beang espectvely to the vehcle pose. ae th featue n the envonment elatve to the ( k h ) The stength (covaance) of the obsevaton nose s denoted R. R dag 2 2 (21) (22) 3.3 EKF-SLAM Steps Ths secton pesents the thee-stage ecusve EKF-SLAM pocess compsng pedcton, obsevaton and update steps. Fgue 17 below summases the EKF - SLAM pocess descbed hee.

19 364 Senso Fuson and Its Applcatons x 0; P 0 Map ntalzaton [ z, R ] GetLaseSensoMeasuemet 0 0 If ( z 0! =0) End Fo k = 1: NumbeSteps (=N) x 0 0,P0 0 AugmentMap( x0 0; P0 0, z0, R 0) x R, kk 1,Q k GetOdometyMeasuement x k k 1,P k k 1 EKF _ P edct( xk 1 k 1; Pk 1 k 1, x Rk k 1) [ z, R ] GetLaseSensoMeasuemet k k H DoDataAssocaton( x,p, z, R ) k k k 1 k k 1 k k x k k,pk k EKF _ Update( xk k 1 ; Pk k 1, z k, Rk, Hk ) {If a featue exsts n the map} x k k,p k k AugmentMap( xk k 1 ; Pk k 1, z k, Rk, Hk ) {If t s a new featue} If ( z k = =0) end end Fg. 17. EKF- SLAM pseudo code x k k,p k k = x k k 1,P k k Map Intalzaton The selecton of a base efeence B to ntalse the stochastc map at tme step 0 s mpotant. One way s to select as base efeence the obot s poston at step 0. The advantage n choosng ths base efeence s that t pemts ntalsng the map wth pefect knowledge of the base locaton (Castellanos, J.A et al. 2006). X B B 0 X 0 (23) P P B B 0 0 (24) Ths avods futue states of the vehcle s uncetanty eachng values below ts ntal settngs, snce negatve values make no sense. If at any tme thee s a need to compute the vehcle locaton o the map featue wth espect to any othe efeence, the appopate tansfomatons can be appled. At any tme, the map can also be tansfomed to use a

20 Featue extacton: technques fo landmak based navgaton system 365 featue as base efeence, agan usng the appopate tansfomatons (Castellanos, J.A et al. 2006) Pedcton usng Dead-Reckoned Odomety Measuement as nputs The pedcton stage s acheved by a composton tansfomaton of the last estmate wth a small contol vecto calculated fom two successve dead eckoned poses. X ( k k 1) X ( k 1 k 1) u ( k) (25) o The state eo covaance of the obot state ( k k 1) P s computed as follows: T P ( k k 1) J ( X, u ) P ( k 1 k 1) J ( X, u ) J ( X, u ) U ( k) J ( X, u ) (26) T 1 o 1 o 2 o O 1 o J (, ) 1 X u o s the Jacoban of equaton (16) wth espect to the obot pose, J (, ) 2 X u o s the Jacoban of equaton (16) wth espect to the contol nput, o equatons (12), the above Jacobans ae calculated as follows: J x, x x x 1 2 x 1 0 x2 sn1 y2 cos1 J1 x1, x2 0 1 x2 cos1 y2 sn J J x, x x 1 x 1 2 x X and u. Based on (27) (28) (29) cos1 sn1 0 x, x sn cos 0 (30) Obsevaton Assume that at a cetan tme k an onboad senso makes measuements (ange and beang) to m featues n the envonment. Ths can be epesented as: z ( k) [ z.. z ] (31) m 1 m

21 366 Senso Fuson and Its Applcatons Update th The update pocess s caed out teatvely evey k step of the flte. If at a gven tme step no obsevatons ae avalable then the best estmate at tme k s smply the pedcton X ( k k 1) state estmate can now be updated usng the optmal gan matx W ( k). If an obsevaton s made of an exstng featue n the map, the. Ths gan matx povdes a weghted sum of the pedcton and obsevaton. It s computed usng the nnovaton covaance S ( k), the state eo covaance P ( k k 1) and the Jacobans of the obsevaton model (equaton 18), H ( ). k whee S ( k) s gven by: 1 W ( k) P( k k 1) H ( k) S ( k), (32) S( k) H ( k) P( k k 1) H T ( k) R ( k) (33) R( k) s the obsevaton covaance. Ths nfomaton s then used to compute the state update ( k k) state eo covaance P ( k k). X as well as the updated X ( k k ) X ( k k 1) W ( k ) ( k ) (34) P ( k k ) P ( k k 1) W ( k ) S ( k ) W ( k ) T (35) The nnovaton, v ( k) s the dscepancy between the actual obsevaton, ( k) pedcted obsevaton, z ( k k 1). whee z ( k k 1) s gven as: z and the v( k) z( k) z ( k k 1), (36) z( k k 1) h X ( k k 1), x, y (37) X ( k k 1) s the pedcted pose of the obot and x, ) s the poston of the obseved map featue. ( y 3.4 Incopoatng new featues Unde SLAM the system detects new featues at the begnnng of the msson and when explong new aeas. Once these featues become elable and stable they ae ncopoated nto the map becomng pat of the state vecto. A featue ntalsaton functon y s used fo ths pupose. It takes the old state vecto, X ( ) and the obsevaton to the new k k

22 Featue extacton: technques fo landmak based navgaton system 367 featue, z ( k) as aguments and etuns a new, longe state vecto wth the new featue at ts end (Newman 2006). * ( k k) ( k k), ( k) X y X z (338) X ( k k) y sn( ) X (39) * ( k k) x cos( ) Whee the coodnates of the new featue ae gven by the functon g : x cos( ) g g 1 y sn( ) g (40) 2 ( x, y and ae the and ae the ange and beang to the new featue espectvely. ) estmated poston and oentaton of the obot at tme k. The augmented state vecto contanng both the state of the vehcle and the state of all featue locatons s denoted: X k k X k x x (41) * T T T ( ) [ ( ) f,1.. f, N ] We also need to tansfom the covaance matx P when addng a new featue. The gadent fo the new featue tansfomaton s used fo ths pupose: x cos( ) g g 1 y sn( ) g (42) 2 The complete augmented state covaance matx s then gven by: whee Y x. z s gven by: k k P( k k) 0 0 R * T P( ) Yx, z Yx, z, (43) Y x, z Inxn 0 [ Gx zeos( nstates n)] G nx2 z (44)

23 368 Senso Fuson and Its Applcatons whee nstates and n ae the lengths of the state and obot state vectos espectvely. X g X G (45) g1 g1 g1 x y GX g2 g2 g2 0 1 cos( ) x y 1 0 sn( ) (46) z g z G (47) g1 g1 cos( ) sn( ) Gz g2 g2 sn( ) cos( ) (48) 3.5 Data assocaton In pactce, featues have smla popetes whch make them good landmaks but often make them dffcult to dstngush one fom the othe. When ths happen the poblem of data assocaton has to be addessed. Assume that at tme k, an onboad senso obtans a set of measuements z ( k) of m envonment featues E ( 1,..., m). Data Assocaton conssts of detemnng the ogn of each measuement, n tems of map featues F j, j 1,..., n. The esults s a hypothess: matchng each measuement ( k) ndcates that the measuement ( k) H... k j1 j2 j3 jm, (49) z wth ts coespondng map featue. F ( 0) j j z does not come fom any featue n the map. Fgue 2 below summases the data assocaton pocess descbed hee. Seveal technques have been poposed to addess ths ssue and moe nfomaton on some these technques can be found n (Castellanos, J.A et al. 2006) and (Coope, A.J, 2005). Of nteest n ths chapte s the smple data assocaton poblem of fndng the coespondence of each measuement to a map featue. Hence the Indvdual Compatblty Neaest Neghbou Method wll be descbed.

24 Featue extacton: technques fo landmak based navgaton system Indvdual Compatblty The IC consdes ndvdual compatblty between a measuement and map featue. Ths dea s based on a pedcton of the measuement that we would expect each map featue to geneate, and a measue of the dscepancy between a pedcted measuement and an actual measuement made by the senso. The pedcted measuement s then gven by: z ( k k 1) h( X ( k k 1), x, y ) (50) j j j The dscepancy between the obsevaton ( k) z ( k k 1) s gven by the nnovaton tem ( k) j j z and the pedcted measuement v : ( k) z ( k) z ( k k 1) (51) j j The covaance of the nnovaton tem s then gven as: T Sj ( k) H ( k) P( k k 1) H ( k) R ( k) (52) H ( k) s made up of H, whch s the Jacoban of the obsevaton model wth espect to the obot states and H obseved map featue. Fj, the gadent Jacoban of the obsevaton model wth espect to the H ( k) H H Fj 00 (53) Zeos n equaton (53) above epesents un-obseved map featues. To deduce a coespondence between a measuement and a map featue, Mahalanobs dstance s used to detemne compatblty, and t s gven by: D ( k) v ( k) S ( k) v ( k) (54) 2 T 1 j j j j The measuement and a map featue can be consdeed compatble f the Mahalanobs dstance satsfes: 2 2 D j ( k ) d, 1 (55) Whee d dm( v j ) and 1 s the desed level of confdence usually taken to be 95 %. The esult of ths execse s a subset of map featues that ae compatble wth a patcula measuement. Ths s the bass of a popula data assocaton algothm temed Indvdual

25 370 Senso Fuson and Its Applcatons Compatblty Neaest Neghbou. Of the map featues that satsfy IC, ICNN chooses one wth the smallest Mahalanobs dstance (Castellanos, J.A et al. 2006). 3.6 Consstency of EKF-SLAM EKF-SLAM consstency o lack of was dscussed n (Castellanos, J.A et al. 2006), (Newman, P.M. (1999), (Coope, A.J, 2005), and (Castellanos, J.A et al. 2006), It s a non-lnea poblem hence t s necessay to check f t s consstent o not. Ths can be done by analysng the eos. The flte s sad to be unbased f the Expectaton of the actual state estmaton eo, X satsfes the followng equaton: ( k) whee the actual state estmaton eo s gven by: E[ X ] 0 (56) T E X ( k) X ( k) P ( k k 1) (57) X ( k) X ( k) X ( k k 1) (58) P ( k k 1) s the state eo covaance. Equaton (57) means that the actual mean squae eo matches the state covaance. When the gound tuth soluton fo the state vaables s avalable, a ch-squaed test can be appled on the nomalsed estmaton eo squaed to check fo flte consstency. ( ) T 1 ( 1) X k P k k X ( k ) whee DOF s equal to the state dmenson dmx( k) (59) 2 d,1 d and 1 s the desed confdence level. In most cases gound tuth s not avalable, and consstency of the estmaton s checked usng only measuements that satsfy the nnovaton test: v ( k ) S v ( k ) (60) T 1 2 j j j d,1 Snce the nnovaton tem depends on the data assocaton hypothess, ths pocess becomes ctcal n mantanng a consstent estmaton of the envonment map. 4. Result and Analyss Fgue 19 below shows offlne EKF SLAM esults usng data logged by a obot. The expement was conducted nsde a oom of 900 cm x 720cm dmenson wth a few obstacles. Usng an EKF-SLAM algothm whch takes data nfomaton (cones locatons & odomety); a map of the oom was developed. Cone featues wee extacted fom the lase data. To ntalze the mappng pocess, the obot s statng poston was taken efeence. In fgue 19 below, the top left cone s a map dawn usng odomety; pedctably the map s skewed because of accumulaton of eos. The top mddle pctue s an envonment dawn usng EKF SLAM map (cones locatons). The cones wee extacted usng an algothm we poposed, amed at solvng the possblty of mappng false cones. When a cone s e

26 Featue extacton: technques fo landmak based navgaton system 371 obseved a Kalman flte update s done. Ths mpoves the oveall poston estmates of the obot as well as the landmak. Consequently, ths causes the confdence ellpse dawn aound the map (obot poston and cones) to educe n sze (bottom left pctue). Fg. 18. In fgue 8, two consecutve cone extacton pocess fom the splt and mege algothm maps one cone wongly, whle n contast ou cone extacton algothm pcks out the same two cones and coectly assocates them. Fg. 19. EKF-SLAM smulaton esults showng map econstucton (top ght) of an offce space dawn fom senso data logged by the Mee Cat. When a cone s detected, ts poston s mapped and a 2 confdence ellpse s dawn aound the featue poston. As the numbe of obsevaton of the same featue ncease the confdence ellpse collapses (top ght). The bottom ght pctue depct x coodnate estmaton eo (blue) between 2 bounds (ed). Peceptual nfeence Expectedly, as the obot evsts ts pevous poston, thee s a majo decease n the ellpse, ndcatng obot s hgh peceptual nfeence of ts poston. The fa top ght pctue shows a educton n ellpses aound obot poston. The estmaton eo s wth the 2, ndcatng consstent esults, bottom ght pctue. Dung the expement, an exta lase senso was

27 372 Senso Fuson and Its Applcatons use to tack the obot poston, ths povded absolute obot poston. An ntal scan of the envonment (backgound) was taken po by the extenal senso. A smple matchng s then caed out to detemne the pose of the obot n the backgound afte exploaton. Fgue 7 below shows that as the obot close the loop, the estmated path and the tue ae almost dentcal, mpovng the whole map n the pocess. 5 4 SLAM Abolute Poston SLAM vs Abolute Poston 3 temnaton poston [m] stat [m] Fg. 20. The fgue depcts that as the obot evsts ts pevous exploed egons; ts postonal pecepton s hgh. Ths means mpoved localzaton and mappng,.e. mpoved SLAM output. 5. Concluson and futue wok In ths pape we dscussed the esults of an EKF SLAM usng eal data logged and computed offlne. One of the most mpotant pats of the SLAM pocess s to accuately map the envonment the obot s explong and localze n t. To acheve ths howeve, s depended on the pecse acquement of featues extacted fom the extenal senso. We looked at cone detecton methods and we poposed an mpoved veson of the method dscussed n secton It tansped that methods found n the lteatue suffe fom hgh computatonal cost. Addtonally, thee ae susceptble to mappng ghost cones because of undelyng technques, whch allows many computatons to pass as cones. Ths has a majo mplcaton on the soluton of SLAM; t can lead to coupted map and ncease computatonal cost. Ths s because EKF-SLAM s computatonal complexty s quadatc the numbe of landmaks n the map, ths nceased computatonal buden can peclude eal

28 Featue extacton: technques fo landmak based navgaton system 373 tme opeaton. The cone detecto we developed educes the chance of mappng dummy cones and has mpoved computaton cost. Ths offlne smulaton wth eal data has allowed us to test and valdate ou algothms. The next step wll be to test algothm pefomance n a eal tme. Fo lage ndoo envonments, one would employ a ty a egesson method to ft lne to scan data. Ths s because codos wll have numeous possble cones whle t wll take a few lnes to descbe the same space. 6. Refeence Baley, T and Duant-Whyte, H. (2006), Smultaneous Localsaton and Mappng (SLAM): Pat II State of the At. Tm. Robotcs and Automaton Magazne, Septembe. Castellanos, J.A., Nea, J., and Tad os, J.D. (2004) Lmts to the consstency of EKF-based SLAM. In IFAC Symposum on Intellgent Autonomous Vehcles. Castellanos, J.A.; Nea, J.; Tados, J.D. (2006). Map Buldng and SLAM Algothms, Autonomous Moble Robots: Sensng, Contol, Decson Makng and Applcatons, Lews, F.L. & Ge, S.S. (eds), 1st edn, pp , CRC, , New Yok, USA Colle, J, Ramez-Seano, A (2009)., "Envonment Classfcaton fo Indoo/Outdoo Robotc Mappng," cv, Canadan Confeence on Compute and Robot Vson, pp Coope, A.J. (2005). A Compason of Data Assocaton Technques fo Smultaneous Localsaton and Mappng, Mastes Thess, Massachusets Insttute of Technology Cowley, J. (1989). Wold modelng and poston estmaton fo a moble obot usng ultasound angng. In Poc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA). Duda, R. O. and Hat, P. E. (1972) "Use of the Hough Tansfomaton to Detect Lnes and Cuves n Pctues," Comm. ACM, Vol. 15, pp ,Januay. Duant-Whyte, H and Baley, T. (2006). Smultaneous Localzaton and Mappng (SLAM): Pat I The Essental Algothms, Robotcs and Automaton Magazne. Ensele, T. (2001) "Localzaton n ndoo envonments usng a panoamc lase ange fnde," Ph.D. dssetaton, Techncal Unvesty of München, Septembe. Hough,P.V.C., Machne Analyss of Bubble Chambe Pctues. (1959). Poc. Int. Conf. Hgh Enegy Acceleatos and Instumentaton. L, X. R. and Jlkov, V. P. (2003). Suvey of Maneuveng Taget Tackng.Pat I: Dynamc Models. IEEE Tans. Aeospace and Electonc Systems, AES-39(4): , Octobe. Lu, F. and Mlos, E.E..(1994). Robot pose estmaton n unknown envonments by matchng 2D ange scans. In Poc. of the IEEE Compute Socety Conf. on Compute Vson and Patten Recognton (CVPR), pages Mathpages, Pependcula egesson of a lne ( ) Mendes, A., and Nunes, U. (2004)"Stuaton-based mult-taget detecton and tackng wth lase scanne n outdoo sem-stuctued envonment", IEEE/RSJ Int. Conf. on Systems and Robotcs, pp Moutale, P. and Chatla, R. (1989a). An expemental system fo ncemental envonment modellng by an autonomous moble obot. In ISER. Moutale, P. and Chatla, R. (1989b). Stochastc multsensoy data fuson fo moble obot locaton and envonment modellng. In ISRR ).

29 374 Senso Fuson and Its Applcatons Newman, P.M. (1999). On the stuctue and soluton of the smultaneous localzaton and mappng poblem. PhD Thess, Unvesty of Sydney. Newman, P. (2006) EKF Based Navgaton and SLAM, SLAM Summe School. Pfste, S.T., Roumelots, S.I., and Budck, J.W. (2003). Weghted lne fttng algothms fo moble obot map buldng and effcent data epesentaton. In Poc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA). Roumelots S.I. and Bekey G.A. (2000). SEGMENTS: A Layeed, Dual-Kalman flte Algothm fo Indoo Featue Extacton. In Poc. IEEE/RSJ Intenatonal Confeence on Intellgent Robots and Systems, Takamatsu, Japan, Oct Nov. 5, pp Smth, R., Self, M. & Cheesman, P. (1985). On the epesentaton and estmaton of spatal uncetanty. SRI TR 4760 & Smth, R., Self, M. & Cheesman, P. (1986). Estmatng uncetan spatal elatonshps n obotcs, Poceedngs of the 2nd Annual Confeence on Uncetanty n Atfcal Intellgence, (UAI-86), pp , Elseve Scence Publshng Company, Inc., New Yok, NY. Spnello, L. (2007). Cone extacto, Insttute of Robotcs and Intellgent Systems, Autonomous Systems Lab, ETH Züch Thope, C. and Duant-Whyte, H. (2001). Feld obots. In ISRR. Thun, S., Kolle, D., Ghahmaan, Z., and Duant-Whyte, H. (2002) Slam updates eque constant tme. Tech. ep., School of Compute Scence, Canege Mellon Unvesty Wllams S.B., Newman P., Dssanayake, M.W.M.G., and Duant-Whyte, H. (2000.). Autonomous undewate smultaneous localsaton and map buldng. Poceedngs of IEEE Intenatonal Confeence on Robotcs and Automaton, San Fancsco, USA, pp , Wllams, S.B.; Newman, P.; Rosenblatt, J.; Dssanayake, G. & Duant-Whyte, H. (2001). Autonomous undewate navgaton and contol, Robotca, vol. 19, no. 5, pp

30 Senso Fuson and ts Applcatons Edted by Cza Thomas ISBN Had cove, 488 pages Publshe Scyo Publshed onlne 16, August, 2010 Publshed n pnt edton August, 2010 Ths book ams to exploe the latest pactces and eseach woks n the aea of senso fuson. The book ntends to povde a collecton of novel deas, theoes, and solutons elated to the eseach aeas n the feld of senso fuson. Ths book s a unque, compehensve, and up-to-date esouce fo senso fuson systems desgnes. Ths book s appopate fo use as an uppe dvson undegaduate o gaduate level text book. It should also be of nteest to eseaches, who need to pocess and ntepet the senso data n most scentfc and engneeng felds. The ntal chaptes n ths book povde a geneal ovevew of senso fuson. The late chaptes focus mostly on the applcatons of senso fuson. Much of ths wok has been publshed n efeeed jounals and confeence poceedngs and these papes have been modfed and edted fo content and style. Wth contbutons fom the wold's leadng fuson eseaches and academcans, ths book has 22 chaptes coveng the fundamental theoy and cuttng-edge developments that ae dvng ths feld. How to efeence In ode to coectly efeence ths scholaly wok, feel fee to copy and paste the followng: Molaletsa Namoshe, Oudetse Matsebe and Nkgatho Tlale (2010). Cone Featue Extacton: Technques fo Landmak Based Navgaton Systems, Senso Fuson and ts Applcatons, Cza Thomas (Ed.), ISBN: , InTech, Avalable fom: InTech Euope Unvesty Campus STeP R Slavka Kautzeka 83/A Rjeka, Coata Phone: +385 (51) Fax: +385 (51) InTech Chna Unt 405, Offce Block, Hotel Equatoal Shangha No.65, Yan An Road (West), Shangha, , Chna Phone: Fax: