High-accuracy global localization filter for three ... - Semantic Scholar
5RERWLFD YROXPH SS ± &DPEULGJH 8QLYHUVLW\ 3UHVV GRL 6
High-accuracy global localization filter for three-dimensional environments )HUQDQGR 0DUWÕQ /XLV 0RUHQR 6DQWLDJR *DUULGR DQG 'RORUHV %ODQFR Carlos III University, Madrid, Spain 5HFHLYHG LQ )LQDO )RUP -XQH DFFHSWHG -XQH )LUVW SXEOLVKHG RQOLQH -XO\
SUMMARY 7KH ORFDOL]DWLRQ SUREOHP LQ PRELOH URERWLFV FDQ EH GHILQH DV WKH VHDUFK RI WKH URERW¶V FRRUGLQDWHV LQ D NQRZQ HQYLURQPHQW ,I WKHUH LV QR LQIRUPDWLRQ DERXW WKH LQLWLDO ORFDWLRQ ZH DUH WDONLQJ DERXW JOREDO ORFDOL]DWLRQ ,Q WKLV ZRUN ZH KDYH GHYHORSHG DQ DOJRULWKP WKDW VROYHV WKLV SUREOHP LQ D WKUHHGLPHQVLRQDO ' HQYLURQPHQW XVLQJ HYROXWLRQDU\ FRPSXWDWLRQ FRQFHSWV 7KH PHWKRG KDV EHHQ FDOOHG 5(/) ' DQG KDV PDQ\ IHDWXUHV WKDW PDNH LW YHU\ UREXVW DQG UHOLDEOH WKUHVKROGLQJ DQG GLVFDUGLQJ PHFKDQLVPV GLIIHUHQW FRVW IXQFWLRQV HIIHFWLYH FRQYHUJHQFH FULWHULD DQG VR RQ 7KH UHVXOWLQJ JOREDO ORFDOL]DWLRQ PRGXOH KDV EHHQ WHVWHG LQ QXPHURXV H[SHULPHQWV DQG WKH PRVW LPSRUWDQW LPSURYHPHQW REWDLQHG LV WKH DFFXUDF\ RI WKH PHWKRG DOORZLQJ LWV DSSOLFDWLRQ LQ PDQLSXODWLRQ WDVNV .(error, ind best@ ← min cost bestmem ← pop ind best conv conditions checking . . . end while end function 䉯 UHWXUQ EHVWPHP HUURU DQG SRSXODWLRQ 4. Evolutive Localization Filter 7KH ORFDOL]DWLRQ DOJRULWKP LV EDVHG RQ HYROXWLYH RSWLPL]DWLRQ WHFKQLTXHV 7KHVH WHFKQLTXHV DUH SUREDELOLVWLF EXW ZLWKRXW GHULYDWLYHV RU SUREDELOLW\ GHQVLW\ IXQFWLRQV WR HVWLPDWH WKH EHVW VROXWLRQ WR WKH ORFDOL]DWLRQ SUREOHP ,Q RXU HYROXWLYH DOJRULWKP WKHUH DUH HOHPHQWV WKDW FRUUHVSRQG WR SRVVLEOH VROXWLRQV DQG WKH ILWQHV IXQFWLRQ YDOXH UHSUHVHQWV WKH HUURU EHWZHHQ WKH UHDO DQG HVWLPDWHG GDWD 7KH VWRFKDVWLF VHDUFK RI WKH URERW¶V FRRUGLQDWHV LV GRQH XVLQJ WKH '( PHWKRG SURSRVHG E\ 6WRUQ DQG 3ULFH IRU JOREDO RSWLPL]DWLRQ SUREOHPV RYHU FRQWLQXRXV VSDFHV ZKLFK LV H[SODLQHG LQ WKLV VHFWLRQ ,Q RUGHU WR GR WKDW WKH UHDGHU FDQ VHH $OJRULWKP %HVLGHV D FRPSOHWH H[SODQDWLRQ RI WKH '( DOJRULWKP FDQ EH IRXQG LQ RXU SUHYLRXV ZRUN )LUVW RI DOO LW LV QHFHVVDU\ WR FRPPHQW RQ VRPH GHWDLOV DERXW WKH HQYLURQPHQW DQG WKH URERW¶V FKDUDFWHULVWLFV 7KH HQYLURQPHQW KDV EHHQ PRGHOHG JHRPHWULFDOO\ DV DQ RFFXSDQF\ JULG PDS LQ WKUHH GLPHQVLRQV DQG WKH URERW¶V SRVH WKH URERW¶V SRVH LV GHILQH DV WKH URERW¶V SRVLWLRQ DQG RULHQWDWLRQ x y z DQG yaw LV UHSUHVHQWHG ZLWK WKH FDUWHVLDQ FRRUGLQDWHV DQG WKH KRUL]RQWDO RULHQWDWLRQ ,Q PRVW RI RXU H[SHULPHQWV HDFK FHOO LV D FXEH RI . FP VLGH DQG WKH ZKROH PDS FRQWDLQV × × = , , FHOOV ,W UHSUHVHQWV D ' HQYLURQPHQW ZLWK D P DUHD DQG D KHLJKW RI P ,I ZH ZDQW WR GHWHUPLQH WKH URERW¶V ORFDOL]DWLRQ IRXU Ö GHILQLQ D VWDWH Ö y, Ö zÖ , θ FRRUGLQDWHV PXVW EH HVWLPDWHG x, VSDFH ZLWK GRI LQ D ' PDS 7KH VLPXODWHG HQYLURQPHQW DQG WKH ZKROH DOJRULWKP KDYH EHHQ LPSOHPHQWHG XVLQJ 0$7/$% ZKLFK LV D QXPHULFDO FRPSXWLQJ HQYLURQPHQW DQG WK JHQHUDWLRQ SURJUDPLQJ ODQJXDJH 'HYHORSHG E\ 0DWK:RUNV 0$7/$% DOORZV PDWUL[ PDQLSXODWLRQV SORWWLQJ RI IXQFWLRQV DQG GDWD LPSOHPHQWDWLRQ RI DOJRULWKPV FUHDWLRQ RI XVHU LQWHUIDFHV DQG LQWHUIDFLQJ ZLWK SURJUDPV ZULWWHQ LQ RWKHU ODQJXDJHV LQFOXGLQJ & & DQG )RUWUDQ 7KH VHDUFK VWDUWV ZLWK D SRSXODWLRQ RI NP FDQGLGDWHV WKH DOJRULWKP LV SRSXODWLRQEDVHG DQG NP UHSUHVHQWV WKH QXPEHU
KWWS MRXUQDOVFDPEULGJHRUJ
RI HOHPHQWV ZKLFK DUH LQWURGXFHG LQ WKH ORFDOL]DWLRQ PRGXOH DQG HYROYH ZLWK WLPH WR WKH EHVW VROXWLRQ (DFK FDQGLGDWH LV D SRVVLEOH VROXWLRQ WR WKH JOREDO ORFDOL]DWLRQ SUREOHP WKH URERW¶V SRVH ZLWK '2) 7KH LQLWLDO SRSXODWLRQ ZLOO EH FKRVHQ UDQGRPO\ )RU HDFK FDQGLGDWH LWV DVVRFLDWHG ILWQHV IXQFWLRQ LV FDOFXODWHG OLQH WR LQ $OJRULWKP 7KH ILWQHV IXQFWLRQ LV D NH\ FRPSRQHQW RI WKH PHWKRG ,W FRPSDUHV UHDO GDWD UHFHLYHG LQ D ODVHU UHDGLQJ ZLWK VLPXODWHG GDWD IURP D FDQGLGDWH VROXWLRQ 7KH HVWLPDWHG GDWD KDYH EHHQ VRUWHG WR FRQILJXU WKH REVHUYDWLRQ YHFWRU ZLWK YHUWLFDO VFDQV VHSDUDWHG E\ ◦ DQG KRUL]RQWDO UHDGLQJV VHSDUDWHG E\ ◦ 7KH PDLQ ORRS VWDUWV LQ OLQH ,I RQH RI WKH FRQYHUJHQFH FRQGLWLRQV LV VDWLVILHG WKH ORFDOL]DWLRQ SURFHVV HQGV VXFFHVVIXOO\ $QRWKHU ORRS ZKLFK FRQWDLQV WKH HYROXWLYH VHDUFK VWDUWV LQ OLQH ,W FRQVLVWV RI D JHQHUDWLRQ RI D QHZ SRSXODWLRQ IRU WKH QH[W JHQHUDWLRQ ,Q D VLQJOH LWHUDWLRQ WKH DOJRULWKP LV H[HFXWHG WR REWDLQ WKH QH[W FDQGLGDWHV HYROYLQJ ZLWK WLPH WR WKH FRUUHFW SRVH (DFK SDUW RI WKH PHFKDQLVP LV H[SODLQHG LQ WKH QH[W VXEVHFWLRQ 4.1. Mutation, crossover, and selection 7KH LQLWLDO SRSXODWLRQ LV SHUWXUEHG WR JHQHUDWH D YDULDWLRQ v DFFRUGLQJ WR WKH IROORZLQJ H[SUHVVLRQ v = xak + F xbk − xck , ZKHUH xak xbk DQG xck DUH SDUDPHWHU YHFWRUV FKRVHQ UDQGRPO\ IURP WKH SRSXODWLRQ DW LWHUDWLRQ k DQG DUH GLIIHUHQW IURP UXQQLQJ LQGH[ WKLV x LV QRW WKH FDUWHVLDQ FRRUGLQDWH EXW WKH HOHPHQW RI SRSXODWLRQ GHILQH E\ IRXU FRRUGLQDWHV F LV D UHDO DQG FRQVWDQW IDFWRU WKDW FRQWUROV WKH DPSOLILFDWLR RI GLIIHUHQWLDO YDULDWLRQV xbk − xck $Q H[DPSOH RI WKH SHUWXUEDWLRQ SURFHVV FDQ EH VHHQ LQ )LJ 7KHUH LV DQ LQLWLDO SDUDPHWHU YHFWRU xik DQG WKH SHUWXUEDWLRQ LV GRQH ZLWK WKUHH
High-accuracy global localization filter for 3D environments Z
F*(xkb – xkc ) x ka
x kb
x ki
Minimum v New parameter vector
xkc Y
X
)LJ &RORXU RQOLQH 1HZ SRSXODWLRQ PHPEHU JHQHUDWLRQ
UDQGRP YDULDEOHV DQG WKH FRQVWDQW IDFWRU F JHQHUDWLQJ WKH QHZ SDUDPHWHU YHFWRU v 7KH YDULDEOHV RI WKLV H[SUHVVLRQ FRXOG EH FKDQJHG LQ RUGHU WR REWDLQ D GLIIHUHQW SHUWXUEHG YHFWRU ,W LV SRVVLEOH WR VHOHFW WKH best SRSXODWLRQ YHFWRU LQVWHDG RI xak 7KH FXUUHQW LPSOHPHQWDWLRQ DOORZV ERWK SRVVLELOLWLHV UDQGRP PXWDWLRQ DQG PXWDWLRQ IURP WKH EHVW FDQGLGDWH 7KH ILUV FKRLFH LV PRUH UREXVW EHFDXVH LW PDLQWDLQV WKH SRSXODWLRQ GLYHUVLW\ DQG LV DSSOLHG LQ JHQHUDO VLWXDWLRQV +RZHYHU WKH PXWDWLRQV IURP WKH EHVW FDQGLGDWHV DUH IDVWHU DQG WKH\ FRXOG EH DSSOLHG LQ VRPH VLWXDWLRQV VXFK DV WKH URERW LV VLWXDWHG LQ D SODFH WKDW LV YHU\ HDV\ WR LGHQWLI\ RU WUDFNLQJ WDVNV DIWHU D VXFFHVVIXO ORFDOL]DWLRQ ,Q RUGHU WR LQFUHDVH WKH GLYHUVLW\ RI WKH QHZ JHQHUDWLRQ WKH FURVVRYHU LV LQWURGXFHG 'HQRWHG E\ uki = uki, , uki, , . . . , uki,D T WKH QHZ SDUDPHWHU YHFWRU LV uki,j
=
k vi,j k xi,j
k LI pi,j , @ IRU HDFK SDUDPHWHU j RI WKH SRSXODWLRQ PHPEHU i DW VWHS k DQG δ LV WKH FURVVRYHU SUREDELOLW\ DQG FRQVWLWXWHV WKH FURVVRYHU k DUH PDGH DQHZ IRU FRQWURO YDULDEOH 7KH UDQGRP YDOXHV pi,j HDFK WULDO YHFWRU i 7KH QHZ PHPEHU RI WKH SRSXODWLRQ uki LV FRPSDUHG WR xik WR GHFLGH ZKHWKHU RU QRW YHFWRU uki VKRXOG EHFRPH D PHPEHU RI JHQHUDWLRQ i + ,I WKH YHFWRU uki \LHOGV D EHWWHU YDOXH IRU WKH REMHFWLYH ILWQHV IXQFWLRQ WKDQ xik WKHQ LW LV UHSODFHG RWKHUZLVH WKH ROG YDOXH xik LV UHWDLQHG IRU WKH QHZ E\ uk+ i JHQHUDWLRQ 7KH JHQHUDO LGHDV RI WKH SUHYLRXV PHFKDQLVP PXWDWLRQ FURVVRYHU DQG VHOHFWLRQ DUH ZHOO NQRZQ DQG FDQ EH IRXQG LQ OLWHUDWXUH
4.2. Thresholding mechanism (YROXWLRQDU\ $OJRULWKPV ($V DQG LQ JHQHUDO SRSXODWLRQ EDVHG PHWKRGV KDYH EHFRPH YHU\ SRSXODU GXH WR LWV DSSOLFDELOLW\ DQG LPSOHPHQWDWLRQ VLPSOLFLW\ 2QH RI WKH PRVW LPSRUWDQW VKRUWFRPLQJV RI WKHVH PHWKRGV LV WKH SUHPDWXUH FRQYHUJHQFH DQG WKH ODFN RI UREXVWQHVV LQ QRLV\ RSWLPL]DWLRQ SUREOHPV ,I '( LV FRPSDUHG ZLWK RWKHU SRSXODWLRQ VHDUFK EDVHG PHWKRGV IRU H[DPSOH *HQHWLF $OJRULWKPV *$ LW VKRZV VRPH ZHDNQHVV 7KLV EHKDYLRU KDV EHHQ VWXGLHG
KWWS MRXUQDOVFDPEULGJHRUJ
E\ .ULQN et al 7KHUH DUH WZR GLIIHUHQW DVSHFWV ZLWK D QHJDWLYH LQIOXHQFH WKH '( PHWKRG LPSOHPHQWV D JUHHG\ VHDUFK VWUDWHJ\ DQG WKH '( PHFKDQLVPV IRU JHQHUDWLQJ QHZ SRWHQWLDO VROXWLRQV DUH OHVV VWRFKDVWLF WKDQ RWKHU ($V 7KHVH GLVDGYDQWDJHV DUH VLJQLILFDQ ZKHQ WKH GLIIHUHQFH EHWZHHQ WKH FDQGLGDWH VROXWLRQ ILWQHV YDOXH DQG WKH FXUUHQW SRSXODWLRQ HOHPHQW ILWQHV YDOXH LV VPDOOHU WKDQ WKH ILWQHV YDULDQFH RULJLQDWHG E\ WKH QRLVH 7KH LGHD RI WKUHVKROGLQJ LV WR UHGXFH WKH HDJHUQHVV RI WKH DOJRULWKP E\ UHMHFWLQJ WKRVH QHZ VROXWLRQV WKDW GR QRW LPSURYH WKH SUHYLRXV K\SRWKHVLV LQ D SUHVSHFLILH PDJQLWXGH τ 7KLV LGHD LV QRW QHZ DQG KDV DOUHDG\ EHHQ DSSOLHG WR HYROXWLRQDU\ FRPSXWDWLRQ SUREOHPV 7KH WKUHVKROG FDQ QRW EH D IL HG PDJQLWXGH EHFDXVH WKLV XQLW GHSHQGV RQ WKH QRLVH YDULDQFH DQG WKH ILWQHV GLVWDQFH WR WKH RSWLPDO ILWQHV YDOXH 7KH QRLVH YDULDQFH PLJKW EH HVWLPDWHG EXW LW LV QRW HDV\ WR HVWLPDWH WKH VHFRQG IDFWRU )LUVW WKH ILWQHV YDOXH GLIIHUHQFH EHWZHHQ WKH SRSXODWLRQ PHPEHU xik DQG WKH FDQGLGDWH PHPEHU uki LV FDOFXODWHG 7KHQ WKLV GLIIHUHQFH LV FRPSDUHG ZLWK D SUHGHILQH WKUHVKROG YDOXH τ LQ RUGHU WR GHWHUPLQH LI WKH LPSURYHPHQW VKRZQ E\ uki LV QRW FDXVHG E\ WKH QRLVH ,I WKLV FRQGLWLRQ LV PHW WKH WDUJHW YHFWRU xik LV UHSODFHG E\ uki LQ WKH QH[W JHQHUDWLRQ RWKHUZLVH xik LV NHSW LQWR WKH SRSXODWLRQ ,Q FRQFOXVLRQ LI WKH LPSURYHPHQW LQ WKH ILWQHV IXQFWLRQ LV ELJJHU WKDQ WKH YDULDQFH RU WKH VWDQGDUG GHYLDWLRQ GHSHQGLQJ RQ WKH VHOHFWHG XQLWV LW FDQ EH FRQVLGHUHG WKDW LW LV QRW FDXVHG E\ WKH QRLVH DQG WKH QHZ PHPEHU FDQ EH LQWURGXFHG LQ WKH SRSXODWLRQ 7KH DOJRULWKP FRQYHUJHQFH DQG UREXVWQHVV LQ QRLV\ RSWLPL]DWLRQ SUREOHPV DQG WKH FRPSDULVRQ EHWZHHQ WKH FXUUHQW PHWKRG DQG WKH LQLWLDO PHWKRG ZLWKRXW WKUHVKROGLQJ KDYH EHHQ SXEOLVKHG LQ RXU UHFHQW ZRUN ,Q RXU H[SHULPHQWV ZH KDYH FKRVHQ D YDOXH WKDW GHSHQGV RQ WKH VHQVRU QRLVH EHFDXVH WKH WKUHVKROGLQJ PHFKDQLVP WULHV WR DYRLG RSWLPL]DWLRQ LQ WKH QRLVH EDQG 7KH WKUHVKROG τ LV HTXDO WR SN × fik ZKHUH SN LV WKH VHQVRU QRLVH SHUFHQWDJH RYHU WKH GLVWDQFH ZHLJKWHG DQG fik LV WKH ILWQHV IXQFWLRQ YDOXH IRU WKH iWK SRSXODWLRQ PHPEHU LQ WKH kWK LWHUDWLRQ 7KLV YDOXH KDV EHHQ FKRVHQ HPSLULFDOO\ )RU H[DPSOH ZKHQ WKH VHQVRU QRLVH LV RYHU WKH GLVWDQFH ZHLJKWHG D VLPSOH DQG DGDSWLYH WKUHVKROG OHYHO RI τ = . × fik KDV EHHQ DGRSWHG WR UHMHFW WKH RIIVSULQJ VROXWLRQ JHQHUDWHG E\ fik ,Q VSLWH RI LWV VLPSOLFLW\ LW ZRUNV TXLWH HIILFLHQWO 7KLV VHOHFWLRQ PHFKDQLVP GHFUHDVHV FRQVLGHUDEO\ WKH HDJHUQHVV RI WKH '( DOJRULWKP DQG DOVR LWV VSHHG RI FRQYHUJHQFH $V D FRQVHTXHQFH RI WKUHVKROGLQJ WKH DOJRULWKP UHMHFWV D KLJK TXDQWLW\ RI QHZ VROXWLRQV DQG DFFHSWV RQO\ WKRVH VROXWLRQV WKDW SUHVHQW D FOHDU LPSURYHPHQW LQ WKH ILWQHV IXQFWLRQ 7KH VWRFKDVWLF UREXVWQHVV RI WKH DOJRULWKP LWV FRPSXWDWLRQDO FRVW DQG WKH LWHUDWLRQV WR FRQYHUJH SUHVHQW ZRUVH YDOXHV VR DQRWKHU PHFKDQLVP GLVFDUGLQJ PXVW EH LQFRUSRUDWHG WR UHGXFH WKLV QHJDWLYH LQIOXHQFH 4.3. Discarding mechanism 7KH XVH RI D WKUHVKROGLQJ EDQG WHQGV WR GHFUHDVH WKH FRQYHUJHQFH VSHHG RI WKH DOJRULWKP SDUWLFXODUO\ DW LQLWLDO VWDJHV GXH WR WKH UHMHFWLRQ RI RIIVSULQJ WKDW GRHV QRW LPSURYH HQRXJK WKH SUHYLRXV K\SRWKHVLV EHORZ WKH WKUHVKROG EDQG
High-accuracy global localization filter for 3D environments
$ GLVFDUGLQJ PHFKDQLVP KDV EHHQ LQWURGXFHG WR LQFUHDVH WKH VSHHG RI WKH DOJRULWKP ZKLOH PDLQWDLQLQJ WKH VWRFKDVWLF DGYDQWDJHV LQ WHUPV RI UREXVWQHVV RI WKUHVKROGLQJ 7KH LGHD LV WR GHWHUPLQH WKH ZRUVW ILWQHV LQGLYLGXDO RI WKH QHZ SRSXODWLRQ DQG VXEVWLWXWH LW E\ D QHZ VROXWLRQ FORVH WR D EHWWHU RQH ,Q RUGHU WR GR WKDW D SHUFHQWDJH RI HOHPHQWV WR EH GLVFDUGHG LV FKRVHQ QRW RQO\ RQH IRU H[DPSOH ,Q RUGHU WR DYRLG FRQFHQWUDWLQJ WKH GLVFDUGHG VROXWLRQV DURXQG WKH EHVW H[LVWLQJ LQGLYLGXDO RQH RI WKH PHPEHUV RI WKH SRSXODWLRQ ZLWK LWV ILWQHV YDOXH ORFDWHG LQ WKH ILUV KDOI RI WKH ILWQHV UDQNLQJ LV VHOHFWHG UDQGRPO\ 7KLV VHOHFWHG VROXWLRQ SOXV D UHODWLYHO\ VPDOO UDQGRP FRPSRQHQW LV DGRSWHG DV D QHZ RIIVSULQJ 7KH GLVFDUGLQJ PHFKDQLVP LV LPSRUWDQW DW HDUO\ VWDJHV RI WKH RSWLPL]DWLRQ SURFHVV ZKHUH HUURUV DUH LPSRUWDQW DQG LW LV QRW VR LQWHUHVWLQJ DW ILQD VWHSV ZKHUH WKH SRSXODWLRQ LV FORVH WR WKH VROXWLRQ 4.4. Convergence conditions 7KHUH DUH PDQ\ SRVVLELOLWLHV ZKHQ FKRRVLQJ D VXLWDEOH REMHFWLYH IXQFWLRQ WR VROYH WKH UHFXUVLYH RSWLPL]DWLRQ SUREOHP JLYHQ E\ (T $VVXPLQJ WKDW WKH VHQVRU PHDVXUHPHQWV DUH *DXVVLDQGLVWULEXWHG RQH RI WKH PRVW FRPPRQ FKRLFHV DV D EDVLV IRU WKH HVWLPDWH LV WKH /QRUP ,Q WKLV FDVH WKH REMHFWLYH IXQFWLRQ WR EH PLQLPL]HG E\ WKH HYROXWLYH DOJRULWKP IRU D SRLQW RI WKH VWDWH VSDFH LQFOXGHG LQ j WKH SRSXODWLRQ xt FRXOG EH JLYHQ E\ WKH IROORZLQJ H[SUHVVLRQ j j f xt
=
j Ns zt,i − zÖ t,i
σe
i=
+
j T j xt − xÖt P − xt − xÖt ,
ZKHUH zt,i LV D VLQJOH PHDVXUHPHQW FRQWDLQHG LQ D UHDGLQJ j JLYHQ E\ WKH ' ODVHU DW LQVWDQW t zÖ t,i LV WKH H[SHFWHG REVHUYDWLRQ IRU WKDW PHDVXUHPHQW LI WKH URERW ZDV VLWXDWHG j LQ xt xÖt LV WKH HVWLPDWHG SRVH LI LW H[LVWV LQ WLPH t P − LV WKH FRYDULDQFH PDWUL[ RI WKH VWDWH HUURU DQG σe LV WKH REVHUYDWLRQ HUURU YDULDQFH :H KDYH FRQVLGHUHG WKDW WKH ODVHU HUURU LV *DXVVLDQGLVWULEXWHG RYHU WKH ZHLJKWHG GLVWDQFH 7KH VHFRQG WHUP RI WKH H[SUHVVLRQ GHSHQGV RQ WKH HVWLPDWHG SRVH xÖt ZKLFK GRHV QRW H[LVW DW WKH EHJLQQLQJ DQG FRXOG QRW EH XQLTXH EHFDXVH WKH URERW FRXOG VWDUW IURP GLIIHUHQW SRVHV ,W LV VWUDLJKWIRUZDUG WKDW WKH VHFRQG WHUP RI WKH ULJKW SDUW FRXOG EH HVWLPDWHG DV D IXQFWLRQ RI WKH GLVWDQFH EHWZHHQ WKH FDQGLGDWH SRVH DQG DOO WKH YLDEOH RQHV EXW LW ZLOO FRPSOLFDWH WRR PXFK WKH ILWQHV IXQFWLRQ HYDOXDWLRQ DGGLQJ DOVR D VWDWLVWLFDO FRPSRQHQW ,Q RUGHU WR DFFHOHUDWH WKH FDOFXODWLRQ ZH ZLOO QRW LQFOXGH LQ WKH ILWQHV IXQFWLRQ WKH LQIRUPDWLRQ JLYHQ E\ WKH GLVWDQFH EHWZHHQ WKH FDQGLGDWH SRVH DQG WKH HVWLPDWHG SRVH XQWLO WKH DOJRULWKP KDV FRQYHUJHG WR RQH VLQJOH SRVH LW LV FRQVLGHUHG WKDW WKH DOJRULWKP KDV FRQYHUJHG ZKHQ DOO FDQGLGDWH SRVHV DUH LQ D VSKHUH RI FRQVWDQW UDWLR DURXQG WKH EHVW RQH ([FOXGLQJ WKLV WHUP WKH ILWQHV IXQFWLRQ EHIRUH FRQYHUJHQFH ZLOO EH JLYHQ E\ WKH IROORZLQJ H[SUHVVLRQ j j f xt
=
j Ns zt,i − zÖ t,i i=
KWWS MRXUQDOVFDPEULGJHRUJ
σe
s νt,i , i= σe
N
=
j
ZKHUH νt,i = zt,i − zÖ t,i UHSUHVHQWV WKH GLVFUHSDQF\ EHWZHHQ WKH REVHUYHG DQG SUHGLFWHG VHQVRULDO GDWD ,W ZRXOG EH LQWHUHVWLQJ IRU XV WR ILQ DQ H[SHFWHG YDOXH IRU WKH FRVW IXQFWLRQ ,Q RUGHU WR GR WKLV ZH KDYH QRWLFHG WKDW WKHUH DUH WZR HIIHFWV WKDW LQIOXHQF WKH FRVW IXQFWLRQ WKH PHDVXUHPHQW QRLVH DQG WKH HVWLPDWLRQ HUURU ,I ZH REWDLQ D SHUIHFW HVWLPDWLRQ WKH VHFRQG HIIHFW FDQ EH HOLPLQDWHG EXW DQ HUURU LQWURGXFHG E\ WKH QRLVH ZLOO DOZD\V H[LVW +HQFH LW LV SRVVLEOH WR HVWLPDWH WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ ZKHQ LW LV FORVH WR WKH WUXH YDOXH E f :H KDYH WR UHPDUN WKDW LW LV WKH ILWQHV IXQFWLRQ YDOXH LQ WKH UHDO SRVLWLRQ s ,I ZH REVHUYH WKH WHUP N i= νt,i /σe RI WKH ODVW H[SUHVVLRQ /σe DUH UDQGRP YDULDEOHV ZLWK D WKH FRPSRQHQWV νt,i VWDQGDUG 1RUPDO GLVWULEXWLRQ N , DQG WKH VXP IROORZV D &KL6TXDUH SUREDELOLW\ GLVWULEXWLRQ ZLWK Ns '2) 7KLV SUREDELOLW\ GLVWULEXWLRQ ZKLFK LV ZHOO NQRZQ DQG WDEXODWHG KDV DQ DYHUDJH RI Ns DQG D YDULDQFH RI Ns / 7KHUHIRUH WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ WKDW ZH DUH WU\LQJ WR PLQLPL]H LV E>f @ =
+∞ −∞
f ν p ν dν = Ns /.
7KLV H[SUHVVLRQ WHOOV XV WKDW HYHQ LI WKH SRVH WKDW ZH DUH HYDOXDWLQJ ZDV WKH FRUUHFW SRVH RI WKH URERW DQG GXH WR PHDVXUHPHQW HUURUV WKDW RFFXU LQ WKH VHQVRU GXULQJ WKH HQYLURQPHQW SHUFHSWLRQ WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ LV Ns / 7KH GHWHUPLQDWLRQ RI D VWRSSLQJ FRQGLWLRQ IRU WKH DOJRULWKP EULQJV XV EDFN WR ZKDW ZDV GLVFXVVHG DERYH EHFDXVH RQFH ZH NQRZ WKH EHVW H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ LW LV SRVVLEOH WR HVWDEOLVK D VWRSSLQJ FRQGLWLRQ EDVHG RQ VRPH RI WKH VWDWLVWLFDO SDUDPHWHUV DVVRFLDWHG ZLWK WKLV REMHFWLYH IXQFWLRQ $V ZH KDYH FRQFOXGHG WKDW RXU FRVW IXQFWLRQ FDQ EH DSSUR[LPDWHG E\ D &KL6TXDUH ZLWK Ns '2) LW LV VWUDLJKWIRUZDUG WR DVVRFLDWH WKH REMHFWLYH IXQFWLRQ YDOXH ZLWK D JLYHQ SUREDELOLW\ ,Q RWKHU ZRUGV DQ REMHFWLYH IXQFWLRQ YDOXH f−p ZLWK SUREDELOLW\ − p PHDQV WKDW WKH RSWLPXP YDOXH KDV D SUREDELOLW\ − p RI EHLQJ EHORZ f−p 7KHVH YDOXHV DUH RIWHQ IRXQG WDEXODWHG LQ VWDWLVWLFDO OLWHUDWXUH IRU VRPH W\SLFDO TXDQWLOHV DQG D JLYHQ QXPEHU RI '2) %HVLGHV ZH KDYH FKRVHQ RWKHU VLPSOH FULWHULD WKDW KDYH EHHQ XVHG H[SHULPHQWDOO\ LQ FHUWDLQ VLWXDWLRQV 7KLV FULWHULD GR QRW HQVXUH FRQYHUJHQFH EXW PD\ OHDG WR JRRG UHVXOWV LQ OHVV WLPH ,I RQH RI WKH IROORZLQJ FRQYHUJHQFH FRQGLWLRQV LV VDWLVILHG WKH ORFDOL]DWLRQ SURFHVV ILQLVKHV r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH ILWQHV
IXQFWLRQ YDOXH RI WKH EHVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH ILWQHV IXQFWLRQ YDOXH RI WKH ZRUVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH GLIIHUHQFH EHWZHHQ WKH ILWQHV IXQFWLRQ YDOXH RI WKH EHVW HVWLPDWLRQ DQG WKH ILWQHV IXQFWLRQ YDOXH RI WKH ZRUVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW
High-accuracy global localization filter for 3D environments 4.5. Fitness function 7KH FRVW IXQFWLRQ FRPSDUHV ODVHU GDWD REWDLQHG IURP WKH URERW WUXH SRVH WR ODVHU GDWD IURP WKH SRVH HVWLPDWHV 7KH ORFDOL]DWLRQ DOJRULWKP PLQLPL]HV WKH HUURU DQG HYROYHV WR WKH WUXH VROXWLRQ 7KH VHQVRU FRQVLGHUHG LQ WKLV SDSHU LV D VLPXODWHG ODVHU WKDW ZRUNV LQ YHUWLFDO VFDQV VHSDUDWHG E\ ◦ DQG HDFK YHUWLFDO VFDQ FRQWDLQV KRUL]RQWDO UHDGLQJV VHSDUDWHG E\ ◦ 7KH ORFDOL]DWLRQ PHWKRG DSSOLHV WKH ILWQHV IXQFWLRQ DOLJQLQJ WKH FXUUHQW VFDQ HVWLPDWHG SRVH ZLWK WKH UHIHUHQFH VFDQ UHDO SRVH 7KH SRSXODWLRQ HYROYHV WR WKH FRUUHFW ORFDWLRQ EHFDXVH WKH PDWFKLQJ HUURU LV PLQLPL]HG 7KH PDWFKLQJ PHWKRG DSSOLHG LQ WKLV SDSHU DYRLGV VHDUFKLQJ IRU SRLQW DVVRFLDWLRQV E\ VLPSO\ PDWFKLQJ SRLQWV ZLWK WKH VDPH EHDULQJ ,W LV FDOOHG 3RODU 6FDQ 0DWFKLQJ 360 DQG WKH GLIIHUHQW RSWLRQV LPSOHPHQWHG DUH H[SODLQHG EHORZ 7KH QDWXUDO FKRLFH IRU WKH FRVW IXQFWLRQ LV WKH VXP RI WKH VTXDUHG HUURUV IXQFWLRQ /QRUP $V H[SODLQHG LQ WKH SUHYLRXV VHFWLRQ LI WKH REVHUYDWLRQ YHFWRU DW WLPH t LV zt = z,t , . . . , zp,t T WKH SUHGLFWHG REVHUYDWLRQV DFFRUGLQJ WR WKH HVWLPDWHG URERW¶V SRVH DUH zÖ t = Öz,t , . . . , zÖ p,t T DQG WKH REVHUYDWLRQ HUURU YDULDQFH LV σe 7KHQ WKH SHQDOW\ IXQFWLRQ j IRU D VLQJOH SRLQW xt RI WKH SRSXODWLRQ FDQ EH VWDWHG DV
L
j xt
− xÖt = zt − zÖ t T zt − zÖ t =
j Ns zt,i − zÖ t,i i=
σe
.
)ROORZLQJ DUH VRPH RI WKH IDFWRUV LQ JOREDO ORFDOL]DWLRQ WKDW PDNH WKLV ILWQHV IXQFWLRQ GLIILFXO WR PDQDJH r 7KH UDQJH DQG DFFXUDF\ RI WKH VHQVRU DQG WKH QXPEHU
RI VHQVRUV OLPLW WKH SRVVLELOLW\ RI GLVWLQJXLVKLQJ EHWZHHQ GLIIHUHQW SRVHV OHDGLQJ WKH ILWQHV IXQFWLRQ WR D KLJK QXPEHU RI JOREDO PD[LPD r 7KH JHRPHWULFDO VLPLODULWLHV LQ WKH HQYLURQPHQW GXH WR WKH UHSHWLWLRQ RI WKH VSDFH GLVWULEXWLRQ RULJLQDWHV WKH SUHVHQFH RI D KLJK QXPEHU RI SRVVLEOH URERW¶V SRVH VROXWLRQV WR WKH PHDQ VTXDUH ORVV IXQFWLRQ 7KH ORVV IXQFWLRQ GHILQH LQ WKLV ZD\ SURYLGHV XV ZLWK DQ RSWLPL]DWLRQ PHFKDQLVP WKDW REWDLQV DQ XQVWDEOH SDUDPHWHU VROXWLRQ DQG E\ H[WHQVLRQ DQ XQVWDEOH ILOWH 7KH HYROXWLYH ILOWH FDQ PRYH IURP RQH ORFDO PLQLPXP WR DQRWKHU RULJLQDWLQJ DEUXSW FKDQJHV LQ WKH SRVH HVWLPDWH 7KLV SUREOHP FRPHV IURP WKH IDFW WKDW ZH DUH QRW XVLQJ DOO WKH LQIRUPDWLRQ ZH NQRZ DERXW WKH V\VWHP LQ WKH ORVV IXQFWLRQ ,Q IDFW ZH RQO\ XVH WKH REVHUYDWLRQ PRGHO WR SUHGLFW WKH VHQVRU PHDVXUHPHQWV DW HDFK SRVLWLRQ DQG WKHVH SUHGLFWHG PHDVXUHPHQWV WRJHWKHU ZLWK WKH DFWXDO PHDVXUHPHQWV DUH LQWURGXFHG LQ WKH ORVV IXQFWLRQ WR HYDOXDWH WKH URERW¶V SRVH HVWLPDWH 7KH LQVWDELOLW\ RULJLQDWHG E\ WKH H[LVWHQFH RI PXOWLSOH VROXWLRQV WR WKH ORVV IXQFWLRQ FDQQRW EH VROYHG E\ FRQVLGHULQJ RQO\ WKH DYDLODEOH VHQVRU REVHUYDWLRQV %HVLGHV WKH OHDVW VTXDUHV PHWKRG LV QRW FRPSOHWHO\ VDWLVIDFWRU\ ZKHQ WKH QRLVH PRGHO LV QRW H[DFWO\ NQRZQ RU WKH PRGHO LV FRQWDPLQDWHG ZLWK RWKHU SUREDELOLW\ GLVWULEXWLRQV 7KH DEVROXWH HUURU /QRUP PD\ EH DQ DSSURSULDWH PHDVXUH LQ VRPH VLWXDWLRQV )RU H[DPSOH LW SUHVHQWV D
KWWS MRXUQDOVFDPEULGJHRUJ
EHWWHU SHUIRUPDQFH ZLWK KLJK HUURUV RULJLQDWHG LQ RXWOLHUV RU FRQWDPLQDWHG PHDVXUHV ,I WKH /QRUP LV QRW GHULYDEOH WKHQ LW LV QHFHVVDU\ WR DSSO\ OLQHDU SURJUDPLQJ PHWKRGV WR REWDLQ D VROXWLRQ WR WKH RSWLPL]DWLRQ SUREOHP %DVHG RQ WKH 0RQWH &DUOR VWXGLHV WKH XVH RI WKH /QRUP KDV EHHQ UHFRPPHQGHG ZKHQ WKH HUURUV IROORZ RQH RI WKHVH GLVWULEXWLRQV /DSODFH &DXFK\ PL[WXUH RI QRUPDO DQG XQLIRUP DQG FRQWDPLQDWHG QRUPDO ,I WKH ILWQHV IXQFWLRQ VHOHFWHG LV WKH /QRUP WKH IXQFWLRQ WR PLQLPL]H LV JLYHQ E\ WKH QH[W HTXDWLRQ
L
j xt
− xÖt =|zt − zÖ t | =
j Ns zt,i − zÖ t,i i=
σe
=
Ns |νt,i | i=
σe
,
j
ZKHUH νt,i = zt,i − zÖ t,i UHSUHVHQWV WKH GLVFUHSDQF\ EHWZHHQ WKH REVHUYHG DQG WKH SUHGLFWHG YDOXHV RI WKH VHQVRU GDWD
5. The Localization Method :H KDYH GHYHORSHG DQ LPSURYHPHQW RI WKH (/) DOJRULWKP WKDW VROYHV WKH JOREDO ORFDOL]DWLRQ SUREOHP LQ D UREXVW DQG HIILFLHQ ZD\ XVLQJ ' VHQVRU GDWD $ VLPXODWLRQ RI WKH ORFDOL]DWLRQ PHWKRG LPSOHPHQWHG LQ WKH URERW FDQ EH VHHQ LQ $OJRULWKP 7KH URERW LV ORFDWHG DW D SODFH LQ WKH HQYLURQPHQW 7KLV ORFDWLRQ LV UHSUHVHQWHG E\ WKUHH VSDWLDO FRRUGLQDWHV DQG WKH RULHQWDWLRQ LQ WKH KRUL]RQWDO SODQH ,W LV LPSRUWDQW WR UHPDUN WKDW RXU PHWKRG ZRUNV LQ WKUHH GLPHQVLRQV EXW WKHUH DUH FDVHV ZKHUH ZH FDQ DVVXPH WKH K\SRWKHVLV WKDW WKH ORFDOL]DWLRQ FRXOG EH VROYHG LQ WZR GLPHQVLRQV )RU LQVWDQFH ZKHQ WKH HQYLURQPHQW LV SODQH DQG WKH URERW KDV D IL HG KHLJKW ZH FDQ FRQVLGHU WKH z FRRUGLQDWH DV D FRQVWDQW YDOXH :H GR QRW KDYH DQ\ LQIRUPDWLRQ DERXW WKH ORFDWLRQ RI WKH URERW WKXV WKH SODFH FDQ EH FRQVLGHUHG UDQGRPO\ 7KH PRELOH URERW UHFHLYHV WKH LQIRUPDWLRQ LQ D ' ODVHU UHDGLQJ 2XU PHWKRG LV QRZ ZRUNLQJ ZLWK VLPXODWHG GDWD DQG WKH UHDGLQJ LV GRQH LQ OLQH RI WKH DOJRULWKP IURP WKH UHDO ORFDWLRQ RI WKH URERW $IWHU WKDW DQ LQLWLDO SRSXODWLRQ LV JHQHUDWHG FRYHULQJ WKH PDS UDQGRPO\ OLQH 7KDW SRSXODWLRQ FRQWDLQV NP SRVVLEOH FDQGLGDWHV :KHQ WKH ODVHU UHDGLQJ KDV EHHQ GRQH WKH PDLQ ORRS VWDUWV OLQH ZRUNLQJ LQ WKH IROORZLQJ ZD\ WKH URERW WULHV WR ORFDWH LWVHOI RQ WKH PDS ZH KDYH FDOOHG LW step DQG LQ RUGHU WR GR WKDW WKH HYROXWLRQDU\ DOJRULWKP OLQH LV H[HFXWHG UHWXUQLQJ WKH HVWLPDWHG VROXWLRQ WKH HUURU DQG WKH ILQD SRSXODWLRQ 7KH FRQFHSW step ZLOO EH XVHG VHYHUDO WLPHV DQG ZH ZLOO H[SODLQ ZKDW LW PHDQV LQ RXU FRQWH[W $ step FDQ EH UHJDUGHG DV WKH PRPHQW LQ WLPH DW ZKLFK WKH URERW XVHV D VLQJOH ' ODVHU VFDQ WR ORFDWH :KHQ LW UHFHLYHV DQRWKHU VFDQ DQG PRWLRQ LQIRUPDWLRQ D QHZ step VWDUWV ,Q RWKHU ZRUGV ZH VD\ WKH URERW LV ORFDWHG LQ D VLQJOH step ZKHQ WKH LW XVHV LQIRUPDWLRQ IURP D VLQJOH ' ODVHU VFDQ ZLWKRXW FRQVLGHULQJ PRYHPHQW LQIRUPDWLRQ ,I WKH URERW PRYHV WR DQRWKHU VLWH DQG WDNHV DQRWKHU VFDQ LW XVHV PRUH LQIRUPDWLRQ WR ORFDWH WKH VHFRQG ODVHU VFDQ DQG WKH PRYHPHQW LQIRUPDWLRQ ,Q WKDW FDVH ZH VD\ LW LV LQ WKH VHFRQG step
High-accuracy global localization filter for 3D environments
Algorithm 2 /RFDOL]DWLRQ ' 3$5$0(7(56 ,1752'8&7,21 real dist d ← dist est d . . . . . . . . . 䉯 /DVHU VLPXODWLRQ pop ← start pop . . . . . . .. dir desp ← NULL while dir desp = end do >bestmem, error, pop@ ← alg genet d . . . . . . . . . . move robot ← VALUE 䉯 URERW¶V GLVSODFHPHQW bestmem ← bestmem + move robot real position ← real position + move robot ∗ + error pos real dist d ← dist est d . . . . . . . . . 䉯 /DVHU VLPXO for i = NP do pop i ← pop i + move robot ∗ + error pos ∗ random end for end while 2QFH WKH ORFDOL]DWLRQ SURFHVV LQ D step KDV ILQLVKHG WKH URERW VWDUWV PRYLQJ 7KH URERW ORFDWLRQ DQG WKH EHVW HVWLPDWLRQ DUH PRYHG DFFRUGLQJ WR WKDW GLVSODFHPHQW ZLWK DQ DGGHG HUURU OLQHV DQG WR LQWHJUDWH WKH PRYHPHQW LQIRUPDWLRQ 7KH VHQVRU PDNHV D QHZ ODVHU UHDGLQJ OLQH DQG UHFHLYHV RGRPHWU\ GDWD 7KHQ WKH SRVVLEOH ORFDWLRQV DUH GLVSODFHG DFFRUGLQJ WR WKH RGRPHWU\ ZLWK DQ HUURU WRR OLQHV ± DQG WKH ORFDOL]DWLRQ SURFHVV LQ WKH VHFRQG step VWDUWV EXW QRZ ZLWK WKH UHVXOWV RI WKH ILUV RQH LQ WKH LQLWLDO SRSXODWLRQ ,W LV REYLRXV WKDW WKH ORFDOL]DWLRQ SURFHVV LV HDVLHU LQ WKLV FDVH EHFDXVH WKHUH LV PRUH LQIRUPDWLRQ :KHQ WKH VHFRQG step ILQLVKHV WKH WKLUG step VWDUWV DQG VR RQ 7KH DOJRULWKP LV UXQQLQJ FRQWLQXRXVO\ ZKLOH WKH URERW PRYHV ZLWKLQ WKH HQYLURQPHQW :H KDYH VLPXODWHG WKDW WKH URERW KDV FRPSOHWHG LWV WDVN LQWURGXFLQJ DQ end LQ OLQH $FFRUGLQJ WR $OJRULWKP WKH ORRS ILQLVKH ZKHQ DQ end LV LQWURGXFHG LQ WKH PRYHPHQW YDULDEOH ZKLFK PHDQV WKH URERW LV WXUQHG RII RU WKH PRYHPHQW LV RYHU DQG WKH URERW LV VXFFHVVIXOO\ ORFDOL]HG ,W LV QHFHVVDU\ WR GHILQ VHYHUDO SDUDPHWHUV EHIRUH WKH H[HFXWLRQ 7KH PRVW LPSRUWDQW RQHV ZLWK D FULWLFDO LQIOXHQF RQ WKH EHKDYLRU RI WKH DOJRULWKP DUH OLVWHG EHORZ r 3RSXODWLRQ 6L]H
,W LV DOVR QHFHVVDU\ WR LQWURGXFH WKH LQLWLDO QXPEHU RI HOHPHQWV ,W LV FULWLFDO DQG GHSHQGV RQ WKH HQYLURQPHQW VL]H :KHQ WKH URERW LV ORFDOL]HG WKH QHHGHG QXPEHU LV VPDOOHU DQG ZH FDQ UHGXFH LW ZLWK D SRVLWLYH LQIOXHQF LQ WKH FRPSXWDWLRQDO FRVW $QRWKHU SUREOHP LV KRZ WR GHWHUPLQH WKH SRSXODWLRQ QXPEHU IRU D JLYHQ HQYLURQPHQW 2Q WKH RQH KDQG WKH QXPEHU GHSHQGV RQ WKH PDS VL]H 2Q WKH RWKHU KDQG WKHUH LV DQRWKHU LPSRUWDQW HOHPHQW WKH V\PPHWULHV 7KH V\PPHWULHV RULJLQDWHG E\ WKH URERW¶V SRVLWLRQ DQG LWV SHUFHSWLRQ FDSDELOLWLHV FDQ PXOWLSO\ WKH SRWHQWLDO QXPEHU RI HTXLYDOHQW PD[LPD 7KLV QXPEHU FDQ YDU\ IURP RQH LQ WKH PRVW IDYRUDEOH FDVH WR PRUH WKDQ DV ZH KDYH REVHUYHG LQ RXU H[SHULPHQWV 7KH UHTXLUHG SRSXODWLRQ QXPEHU FDQ FKDQJH GUDPDWLFDOO\ EHFDXVH D FHUWDLQ QXPEHU RI SRSXODWLRQ HOHPHQWV DUH UHTXLUHG WR PDLQWDLQ DQ K\SRWKHVLV XQWLO QHZ GDWD DUH SHUFHLYHG ,W LV FRPPRQO\ NQRZQ WKDW QRQOLQHDU SRSXODWLRQEDVHG JOREDO RSWLPL]DWLRQ DOJRULWKPV FDQ HDVLO\ DQG SUHPDWXUHO\
KWWS MRXUQDOVFDPEULGJHRUJ
FRQYHUJH WR SRLQW HVWLPDWHV WKDW DUH QRW JOREDOO\ RSWLPDO ,W FDQ EH RULJLQDWHG E\ D ODFN RI SRSXODWLRQ GLYHUVLW\ WKH VORZQHVV RI WKH VHDUFK DOJRULWKP RU WKH H[LVWHQFH RI D ORFDO PLQLPD LQ D PXOWLPRGDO REMHFWLYH IXQFWLRQ 7KH ILUV WZR SUREOHPV FDQ EH DGGUHVVHG E\ LQFUHDVLQJ WKH SRSXODWLRQ VL]H EXW WKH WKLUG RQH LV PRUH GLIILFXOW )LUVW WKH IXQFWLRQ FDQ EH PXOWLPRGDO VR WKH DOJRULWKP ZLOO QRW FRQYHUJH XQWLO WKH PXOWLPRGDOLW\ GLVDSSHDUV 6HFRQG LI WKH QRLVH OHYHO ULVHV D ILFWLWLRX PXOWLPRGDOLW\ LQGXFHG E\ WKH QRLVH OHYHO FDQ DSSHDU r 0XWDWLRQ $PSOLILFDWLR )DFWRU F DQG &URVVRYHU δ F LV D UHDO DQG FRQVWDQW IDFWRU WKDW FRQWUROV WKH DPSOLILFDWLR RI GLIIHUHQWLDO YDULDWLRQV LQ WKH SHUWXUEDWLRQ RI WKH SDUDPHWHU YHFWRU PXWDWLRQ ,Q WKLV SDSHU WKH H[SHULPHQWV DUH GRQH ZLWK F = . 7KH YDOXH RI δ FRQWUROV WKH FURVVRYHU DQG LV IL HG WR . H[SHULPHQWDOO\ %RWK YDULDEOHV ZHUH H[SODLQHG LQ 6XEVHFWLRQ :H KDYH DOVR DGRSWHG DQ DGDSWLYH DGMXVWPHQW RI WKH SHUWXUEDWLRQ DPSOLILFDWLR IDFWRU F 7KLV PHFKDQLVP WULHV WR PDLQWDLQ D KLJK DPSOLILFDWLR IDFWRU ZKLOH WKH SRSXODWLRQ KDV QRW FRQYHUJHG WR WKH SURPLVLQJ DUHDV D ZLGH VFRSH VHDUFK LV UHTXLUHG DQG WR OLPLW WKH DOJRULWKP VHDUFK VFRSH ZKHQ WKH SRSXODWLRQ VHW LV GLVWULEXWHG LQ WKH PRVW IHDVLEOH DUHDV r 8SSHU /LPLW RI ,WHUDWLRQV 7KHUH LV DOVR DQ XSSHU OLPLW IRU WKH QXPEHU RI LWHUDWLRQV SHU step ZKLFK LV DQ DGGHG IHDWXUH GHVLJQHG WR WUDFN WKH URERW IDVWHU ZKHQ LW LV QRW QHFHVVDU\ WR ORFDOL]H LW LQ D VLQJOH RQH ,W LV IDVWHU EHFDXVH ZH XVH PRUH LQIRUPDWLRQ VHYHUDO ODVHU UHDGLQJV DQG RGRPHWU\ LQIRUPDWLRQ DQG LW FDQ EH FKDQJHG WR REWDLQ GLIIHUHQW UHVXOWV )RU H[DPSOH LWHUDWLRQV SHU step PDNH WKH DOJRULWKP JR IDVWHU EXW ZH QHHG PRUH steps WR REWDLQ D JRRG ORFDOL]DWLRQ 1HYHUWKHOHVV LWHUDWLRQV PDNH WKH DOJRULWKP JR VORZO\ EXW ZH FDQ ORFDOL]H WKH URERW LQ D VLQJOH step r 7UDFNLQJ 2QFH WKH ORFDOL]DWLRQ SURFHVV KDV ILQLVKH LQ D VDWLVIDFWRU\ ZD\ WKH URERW NQRZV H[DFWO\ LWV SRVLWLRQ 2XU SUREOHP LV QRZ FRQYHUWHG LQWR WKH WUDFNLQJ SUREOHP ,Q RWKHU ZRUGV WKH URERW ZLOO QHHG WR EH FRQWLQXRXVO\ ORFDOL]HG XSGDWLQJ LWV SRVH DIWHU VPDOO FKDQJHV RI SRVLWLRQ ,Q RUGHU WR GR WKDW RQOLQH WKH SRSXODWLRQ QXPEHU LV UHGXFHG GUDVWLFDOO\ ZKHQ WKH ILWQHV IXQFWLRQ RI WKH EHVW
High-accuracy global localization filter for 3D environments
)LJ &RORXU RQOLQH 7KUHHGLPHQVLRQDO PDS
HVWLPDWLRQ LV XQGHU D WKUHVKROG ZKLFK PHDQV WKDW WKH URERW LV ORFDOL]HG :H KDYH FKRVHQ DV WKH SRSXODWLRQ VL]H DIWHU WKDW 7KLV UHGXFWLRQ LPSOLHV DQ LPSUHVVLYH DGYDQFH LQ WKH FRPSXWDWLRQDO FRVW ,Q HDFK PRWLRQ F\FOH WKH URERW FDQ QRZ XSGDWH LWV SRVH DFFXUDWHO\ DQG TXLFNO\ 6. Experimental Results $OO H[SHULPHQWV KDYH EHHQ GHYHORSHG LQ D VLPXODWHG LQGRRU HQYLURQPHQW ODERUDWRULHV FRUULGRUV DQG RIILFH RI WKH &DUORV ,,, 8QLYHUVLW\ RI 0DGULG $Q H[DPSOH RI D ' PDS FDQ EH IRXQG LQ )LJ 7KH PHDVXUHPHQW PRGHO LV WKH VDPH WKDW ZDV H[SODLQHG LQ 6HFWLRQ . 6.1. Effectiveness of the algorithm ,Q WKH ILUV H[SHULPHQWV ZH KDYH WHVWHG WKH FDSDFLW\ RI RXU PHWKRG WR VROYH WKH ORFDOL]DWLRQ SUREOHP ,Q RUGHU WR GR WKDW ZH PXVW WDFNOH WKH SUREOHP RI ORFDOL]LQJ WKH URERW LQ D UDQGRP SODFH RI WKH HQYLURQPHQW RQO\ ZLWK WKH LQIRUPDWLRQ RI VHQVRUV DW WKLV SRLQW ' GDWD REWDLQHG ZLWK WKH ODVHU UDQJH ILQGH DQG PRYHPHQW LQIRUPDWLRQ )LUVW ZH ZLOO FRQVLGHU WKDW WKH URERW LV VWLOO 7KH ZD\ WR FKHFN LW FRQVLVWV RQ UXQQLQJ WKH DOJRULWKP D QXPEHU RI WLPHV VLWXDWLQJ WKH URERW DW GLIIHUHQW SODFHV DQG REWDLQLQJ WKH SHUFHQWDJH RI VXFFHVV 7KH JOREDO ORFDOL]DWLRQ SURFHVV VWDUWV ZLWK WKH URERW VLWXDWHG LQ D UDQGRP SODFH RI WKH HQYLURQPHQW DQG WZR GLIIHUHQW FDVHV DQG YDULDEOHV GHILQ WKH SURFHVV WKH HVWLPDWH PDWFKHV WKH UHDO SRVH WKH YDULDEOH true LV LQFUHPHQWHG E\ RQH RU WKH HVWLPDWH DQG WKH UHDO SRVH GR QRW FRLQFLGH WKH YDULDEOH false LV LQFUHPHQWHG E\ RQH $IWHU WKDW D QHZ UDQGRP SODFH LV JHQHUDWHG DQG WKH SUHYLRXV SURFHVV ZKLFK LV GHILQH DV D VLQJOH WULDO LV UHSHDWHG 7KH WRWDO QXPEHU RI WULDOV LV total 7KH SHUFHQWDJH RI VXFFHVV LV HTXDO
KWWS MRXUQDOVFDPEULGJHRUJ
WR true GLYLGHG E\ total :H KDYH FKRVHQ DV WKH QXPEHU RI HOHPHQWV RI WKH SRSXODWLRQ 7KH ODVHU UDQJH ILQGH KDV DQ HUURU RI RYHU WKH GLVWDQFH ZHLJKWHG :H KDYH QRW FRQVLGHUHG WKH FRQYHUJHQFH RI WKH DOJRULWKP LQ WKLV H[SHULPHQW DQG ZH OHW WKH DOJRULWKP UXQ D KLJK QXPEHU RI LWHUDWLRQV WR VHH ZKHWKHU LW FDQ REWDLQ WKH VROXWLRQ RU QRW 7KH VXFFHVV RI UXQQLQJ WKH SURJUDP XQGHU WKHVH FRQGLWLRQV LV 7KLV SHUFHQWDJH GHSHQGV RQ WKH HQYLURQPHQW DQG RXU PDS FRQWDLQV PDQ\ VLPLODU SODFHV ZKRVH FRUUHVSRQGHQW PHDVXUHPHQWV DUH WKH VDPH 7KHUHIRUH LW LV QRW SRVVLEOH WR GLVWLQJXLVK EHWZHHQ VHYHUDO GLIIHUHQW ORFDWLRQV EHFDXVH WKH VHQVRU PHDVXUHPHQWV DUH WKH VDPH )RU H[DPSOH LW RFFXUV ZKHQ WKH URERW LV LQVLGH DQ RIILFH 7KDW LV ZK\ WKH DOJRULWKP GRHV QRW VXFFHHG LQ VRPH FDVHV DQG WKH ORFDOL]DWLRQ PHWKRG FRQYHUJHV WR DOO SRVVLEOH VROXWLRQV FRUUHFW SODFH DQG VLPLODU ORFDWLRQV :H FRQVLGHU LW DV D IDLOXUH EHFDXVH LW GRHV QRW FRQYHUJH WR D VLQJOH VROXWLRQ 7KH DOJRULWKP UHWXUQV RQH VROXWLRQ EXW LW FRXOG EH D ZURQJ SODFH ,W FDQ DOVR EH H[SODLQHG XVLQJ WKH KXPDQ EHLQJV SHUFHSWLRQ ,I \RX DUH LQ DQ RIILF FRUQHU ORRNLQJ DW WKH ZDOO LW LV QRW SRVVLEOH WR NQRZ LQ ZKLFK RIILF \RX DUH ,W FDQ EH FRQFOXGHG WKDW WKH LQIRUPDWLRQ LV QRW JRRG HQRXJK WR DSSO\ ORFDOL]DWLRQ WHFKQLTXHV LQ WKHVH FDVHV 7KLV LV D SURPLVLQJ UHVXOW EHFDXVH ZH KDYH QRW LQFOXGHG WKH PRYHPHQW RI WKH URERW XVLQJ VLQJOH step WR ORFDOL]H WKH URERW ZLWK LQIRUPDWLRQ RI RQH ' ODVHU VFDQ DQG QR PRWLRQ LQIRUPDWLRQ EXW LW GRHV QRW UHIOHF D UHDO VLWXDWLRQ 7KH SXUSRVH RI WKH H[SHULPHQW LV WR FKHFN WKH FDSDELOLW\ RI RXU PHWKRG WR VROYH WKH JOREDO ORFDOL]DWLRQ SUREOHP 7KHUHIRUH WKH 5(/)' DOJRULWKP LV VXLWDEOH IRU VROYLQJ WKLV $Q H[DPSOH RI WKH LQIRUPDWLRQ WKDW WKH URERW ZRXOG UHFHLYH LV VKRZQ LQ )LJ ZKHUH WKH ' HQYLURQPHQW WKH ODVHU UHDGLQJV DQG WKH URERW DUH UHSUHVHQWHG
High-accuracy global localization filter for 3D environments
)LJ &RORXU RQOLQH ([DPSOH RI ORFDOL]DWLRQ LQ D FRUULGRU
)LJ &RORXU RQOLQH 6WHSV WR ORFDOL]DWLRQ YV LWHUDWLRQVVWHS (UURU = FRQYHUJHQFH FULWHULD ZRUVW PHPEHU ILWQHV YDOXH VPDOOHU WKDQ × Ns / $OJRULWKP RSWLRQV UDQGRP PXWDWLRQ ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ Ns =
+RZHYHU ZH ZLOO QRW ILQ WKLV VLWXDWLRQ LQ UHDO OLIH EHFDXVH WKH URERW ZLOO QRW EH VWLOO :H KDYH DOVR WHVWHG WKH FDSDFLW\ RI RXU PHWKRG XQGHU PRYHPHQW PDNLQJ GLIIHUHQW H[SHULPHQWV ZLWK FKDQJHV LQ WKH PD[LPXP LWHUDWLRQV SHU step 2Q RQH KDQG ZLWK D KLJK QXPEHU RI LWHUDWLRQV SHU step ZH ZLOO EH FORVHU WR WKH VROXWLRQ LQ RQH VLQJOH step EXW ZLWK D ORZHU YHORFLW\ EHFDXVH ZH QHHG PRUH WLPH WR GHDO ZLWK D VLQJOH ODVHU UHDGLQJ $V D UHPLQGHU WKH URERW XVHV WKH ILUV ODVHU UHDGLQJ WR ORFDWH ZLWK D JLYHQ QXPEHU RI LWHUDWLRQV DQG WKHQ WKH VHFRQG ODVHU UHDGLQJ IURP DQRWKHU ORFDWLRQ LV LQWURGXFHG LQ WKH ORFDOL]DWLRQ PRGXOH LQFOXGLQJ PRWLRQ LQIRUPDWLRQ 2Q WKH RWKHU KDQG ZLWK D ORZ QXPEHU RI LWHUDWLRQV WKH URERW ZLOO QHHG OHVV WLPH WR H[HFXWH D step 7KH QXPEHU RI steps WR ORFDOL]DWLRQ DJDLQVW WKH LWHUDWLRQV SHU VWHS LV VKRZQ LQ )LJ 7KH ODVHU UDQJH ILQGH KDV DQ HUURU RI RYHU WKH GLVWDQFH ZHLJKWHG DQG WKH SRSXODWLRQ VL]H LV 7KH FRQYHUJHQFH FULWHULD WR FRQFOXGH WKDW WKH URERW LV VXFFHVVIXOO\ ORFDOL]HG LV WKDW WKH ZRUVW PHPEHU ILWQHV YDOXH LV ORZHU WKDQ × ∗ Ns / 7KH DYHUDJH QXPEHU RI steps DIWHU VLWXDWLQJ WKH URERW DW GLIIHUHQW SODFHV LV VKRZQ LQ WKH ILJXUH ,QWHUHVWLQJ UHVXOWV FDQ EH FRQFOXGHG REVHUYLQJ )LJ ,W QHHGV PRUH steps WR VXFFHHG ZLWK D PD[LPXP RI 10 LWHUDWLRQV SHU VWHS ORZHU OLPLW FKRVHQ 7KHUHIRUH ZH REWDLQ EHWWHU UHVXOWV LQ RWKHU FDVHV 7KH WLPH QHHGHG LW GHSHQGV RQ WKH QXPEHU RI LWHUDWLRQV WR H[HFXWH WKH ORFDOL]DWLRQ DOJRULWKP LQ D step LV OHVV EXW ZH GR QRW NQRZ WKH VLWXDWLRQ RI WKH URERW XQWLO HLJKW steps ,W PHDQV WKDW WKH QXPEHU RI ODVHU UHDGLQJV XVHG DQG DOVR WKH GLVWDQFH IURP WKH LQLWLDO ORFDWLRQ WR WKH FXUUHQW RQH LV ELJJHU 7KLV FRXOG EH TXLWH GDQJHURXV IRU WKH URERW EHFDXVH LW JRHV D ORQJ ZD\ EHIRUH EHLQJ SURSHUO\
KWWS MRXUQDOVFDPEULGJHRUJ
ORFDOL]HG RQ WKH PDS ,I WKH QXPEHU RI LWHUDWLRQV SHU step LV WKH URERW ZLOO QHHG RQH VLQJOH step +RZHYHU WKH WLPH QHHGHG FRXOG EH D SUREOHP LI WKH WLPH SHU step LV D FRQVWUDLQW ,W FDQ EH FRQFOXGHG WKDW GLIIHUHQW YDOXHV FDQ EH FKRVHQ GHSHQGLQJ RQ URERW UHTXLUHPHQWV )RU H[DPSOH JRRG UHVXOWV DUH REWDLQHG ZLWK LWHUDWLRQV SHU step 7KH URERW LV VXFFHVVIXOO\ ORFDOL]HG TXLFNO\ LQ WZR VWHSV XVLQJ D ORZ QXPEHU RI LWHUDWLRQV 9DKGDW et al. VKRZ D VLPLODU H[SHULPHQW LPSOHPHQWLQJ WKUHH GLIIHUHQW PHWKRGV '( SDUWLFOH VZDUP RSWLPL]DWLRQ DQG 0RQWH &DUOR +RZHYHU WKH QXPEHU RI LWHUDWLRQV SHU step LV VXUSULVLQJO\ ORZ 7KH\ QHHG WZR RU WKUHH LWHUDWLRQV LQ HDFK time-step DQG DERXW steps IRU UREXVW FRQYHUJHQFH 7KH H[SHULPHQW LV GLIIHUHQW IURP RXUV EHFDXVH LW GRHV QRW OLPLW WKH QXPEHU RI steps EXW WKH QXPEHU RI LWHUDWLRQV ,W FDQ EH EHFDXVH LW LV QRW GDQJHURXV IRU WKHLU URERW DQG LW FDQ ZDLW steps XQWLO FRQYHUJHQFH ,W LV D GLIIHUHQW UHVXOW WKDW FRPSOHPHQWV RXU H[SHULPHQWV 7KH\ FRPSDUH FRQYHUJHQFH UDWHV FRQYHUJHQFH WLPHV DQG LQGLYLGXDOV QHHGHG IRU FRQYHUJHQFH DQG WKHLU FRQFOXVLRQ LV WKDW '( FRQYHUJHV IDVWHU PRUH UREXVWO\ DQG ZLWK IHZHU LQGLYLGXDOV 7KH SHUFHQWDJH RI VXFFHVV UXQQLQJ WKH SURJUDP XQGHU WKHVH FRQGLWLRQV LV KLJKHU 7KLV LV D ORJLF UHVXOW EHFDXVH ZH KDYH PRUH LQIRUPDWLRQ VHYHUDO ODVHU UHDGLQJV DQG PRYHPHQW RGRPHWU\ LQIRUPDWLRQ ,W PHDQV WKDW WKH DOJRULWKP UHWXUQV WR WKH FRUUHFW ORFDWLRQ LQ WKH PDMRULW\ RI FDVHV D YHU\ SURPLVLQJ UHVXOW FRQVLGHULQJ WKDW WKH SODFHV DUH UDQGRP DQG WKHUH DUH PDQ\ V\PPHWULHV LQ WKH PDS 6.2. Accuracy of the method 7KH ORFDOL]DWLRQ SURFHVV UHVXOWV LQ GLIIHUHQW SRLQWV RI WKH HQYLURQPHQW FDQ EH VHHQ LQ 7DEOH ,, 7KH FDUWHVLDQ FRRUGLQDWH RI WKH SRLQW ZH ZDQW WR ORFDOL]H LQ WKH PDS LV x DQG xÖ LV WKH HVWLPDWLRQ REWDLQHG 7KLV FDQ EH H[WHQGHG WR y yÖ z DQG zÖ 7KH RULHQWDWLRQ LQ WKH KRUL]RQWDO SODQH WLOW LV θ DQG θÖ LV WKH HVWLPDWLRQ REWDLQHG 7KH VHQVRU QRLVH OHYHO LV D *DXVVLDQ GLVWULEXWLRQ RYHU WKH ODVHU GLVWDQFH ZLWK D VWDQGDUG GHYLDWLRQ RI 7KH SRVLWLRQ HUURU ep LV WKH GLVWDQFH LQ PLOOLPHWHUV EHWZHHQ WKH HVWLPDWHG YDOXH DQG WKH UHDO SRVLWLRQ RI WKH URERW 7KH RULHQWDWLRQ HUURU eθ LV WKH GLIIHUHQFH EHWZHHQ θ DQG θÖ LQ GHJUHHV :H FDQ FRQFOXGH WKDW WKH DFFXUDF\ RI WKH ORFDOL]DWLRQ SURFHVV LV LPSURYHG VLJQLILFDQWO FRPSDUHG ZLWK RXU SUHYLRXV ZRUNV LQ ' PDSV DQG ZH EHOLHYH LW LV WKH PRVW LPSRUWDQW DGYDQFH WKDW ZH KDYH REWDLQHG ,I ZH REVHUYH WKH SRVLWLRQ HUURU FROXPQ ep WKH DYHUDJH HUURU LV HTXDO WR . PP DQG WKH VDPH PHDVXUHPHQW ZLWK RXU SUHYLRXV DOJRULWKP XVLQJ ' VHQVRU GDWD ZDV HTXDO WR FP DV FDQ EH FKHFNHG LQ UHI
High-accuracy global localization filter for 3D environments
7DEOH , (UURUV ORFDOL]LQJ WKH URERW DW GLIIHUHQW SRLQWV 7KH FDUWHVLDQ FRRUGLQDWHV DUH LQ FHOOV DQG WKH HUURUV LQ PLOOLPHWHUV DQG GHJUHHV
x
y
z
θ
xÖ
yÖ
zÖ
θÖ
ep PP
eθ GHJ
> @ 7KH PDLQ UHDVRQ IRU WKLV UHGXFWLRQ LV WKH LQFUHDVH LQ WKH DPRXQW RI LQIRUPDWLRQ 7KH ' ODVHU UHDGLQJ FRQWDLQV QLQH ELGLPHQVLRQDO VFDQV 7KXV WKH LQIRUPDWLRQ LV PXOWLSOLHG E\ QLQH ,W LV SRVVLEOH WR FRPSDUH WKH DFFXUDF\ UHVXOWV ZLWK WKRVH REWDLQHG E\ RWKHU JURXSV ZRUNLQJ RQ ORFDOL]DWLRQ LQ ' HQYLURQPHQWV 7KH DFFXUDF\ DFKLHYHG E\ .XPPHUOH et al. LV DERXW D IHZ FHQWLPHWHUV 6H et al.¶V DYHUDJH (XFOLGHDQ WUDQVODWLRQ HUURU LV HTXDO WR FP DQG WKH DYHUDJH URWDWLRQ HUURU LV R +RZHYHU WKH\ PHQWLRQ WKDW LW FDQ EH UHGXFHG E\ XVLQJ D KLJKHU LPDJH UHVROXWLRQ +R et al. ORFDOL]H WKH URERW ZLWKLQ P RI WUXH SRVLWLRQ DQG R IRU RULHQWDWLRQ 7KHLU EHVW UHVXOWV DUH FP DQG . R 7KH ORFDOL]DWLRQ HUURU PHDQ REWDLQHG E\ 5R\HU et al. LV HTXDO WR . FP DQG .R 7KH UHVXOWV REWDLQHG LQ WKLV SDSHU DUH PRUH DFFXUDWH WKDQ WKRVH DSSURDFKHV )XUWKHUPRUH WKLV LPSURYHPHQW KDV DQ LPSRUWDQW FRQVHTXHQFH IRU WKH FRPSXWDWLRQDO FRVW :H ZRUN ZLWK D ' ODVHU VFDQQHU DQG LW KDV D QHJDWLYH LQIOXHQF RQ WKH FRPSXWDWLRQDO FRVW +RZHYHU WKH DFFXUDF\ LV KLJKHU DQG ZH QHHG OHVV LWHUDWLRQV WR REWDLQ VDWLVIDFWRU\ UHVXOWV 7KH QHJDWLYH LQIOXHQF RI WKH ' ODVHU GDWD FDQ EH SDUWO\ FRPSHQVDWHG ZLWK WKH SRVLWLYH LQIOXHQF RI WKH DFFXUDF\ LPSURYHPHQW DOORZLQJ XV WR XVH WKH DOJRULWKP IDVWHU ZLWK UHOD[DWLRQ LQ WKH DFFXUDF\ RU FRQYHUJHQFH UHTXLUHPHQWV ,I ZH REVHUYH WKH RULHQWDWLRQ HUURU ZH VHH WKDW LWV YDOXH LV YHU\ VPDOO 7KH RULHQWDWLRQ HUURU DYHUDJH ZDV HTXDO WR .◦ IRU WKH UHVXOWV VKRZQ LQ 7DEOH , 7KHUHIRUH LW FDQ EH FRQFOXGHG WKDW WKH PHWKRG SUHVHQWV JRRG IHDWXUHV LQ WKLV DVSHFW 7KH LQIOXHQF RI WKH VHQVRU QRLVH OHYHO FDQ EH REVHUYHG LQ )LJ ZKHUH WKH ORFDOL]DWLRQ HUURU LQ FHQWLPHWHU LV UHSUHVHQWHG YHUVXV WKH ODVHU QRLVH OHYHO VWDQGDUG GHYLDWLRQ RI D *DXVVLDQ GLVWULEXWLRQ RYHU WKH ODVHU GLVWDQFH LQ SHUFHQWDJH RI WKH ZHLJKWHG GLVWDQFH :H REWDLQ DFFHSWDEOH UHVXOWV HYHQ ZLWK D KLJK QRLVH ,W LV D JUHDW UHVXOW WKDW OHDGV XV WR FRQFOXGH WKDW WKH DOJRULWKP SUHVHQWV JRRG SHUIRUPDQFH LQ WKLV DVSHFW 6.3. Thresholding and discarding effect $ WKUHVKROG UHMHFWLRQ EDQG KDV EHHQ DGGHG WR DYRLG WKH SUHPDWXUH HOLPLQDWLRQ RI VROXWLRQV 7KLV PHFKDQLVP GHFUHDVHV WKH HDJHUQHVV RI WKH DOJRULWKP OHWWLQJ LW WR HOLPLQDWH D FDQGLGDWH VROXWLRQ IURP WKH VHW RQO\ ZKHQ WKH RIIVSULQJ FDQGLGDWH LV VLJQLILFDQWO EHWWHU IURP D
KWWS MRXUQDOVFDPEULGJHRUJ
)LJ &RORXU RQOLQH (UURU YV VHQVRU QRLVH OHYHO
VWDWLVWLFDO SRLQW RI YLHZ $ GLVFDUGLQJ RSHUDWRU ZDV DOVR DGGHG WR DFFHOHUDWH WKH FRQYHUJHQFH RI WKH DOJRULWKP E\ HOLPLQDWLQJ D YHU\ ORZ SHUFHQWDJH RI WKH ZRUVW SRSXODWLRQ LQGLYLGXDOV DW HDFK LWHUDWLRQ RI WKH DOJRULWKP DQG UH VDPSOLQJ WKH LQGLYLGXDO FDQGLGDWH LQ WKH YLFLQLW\ RI D EHWWHU FDQGLGDWH VHOHFWHG UDQGRPO\ EHWZHHQ WKH EHVW HOHPHQWV RI WKH SRSXODWLRQ $Q LPSRUWDQW HIIHFW RI WKH XVH RI D WKUHVKROG UHMHFWLRQ OHYHO LV WKH GHFUHDVLQJ RI WKH FRQYHUJHQFH VSHHG DV VKRZQ LQ )LJ 7KH ODWHVW LPSURYHPHQW RFFXUV LQ LWHUDWLRQ QXPEHU LQ WKH WKUHVKROGLQJ RSWLRQ IRU WKH EHVW HVWLPDWLRQ ZKLOH LW LV SURGXFHG DURXQG LWHUDWLRQ QXPEHU LQ WKH RWKHU FDVH ZLWKRXW WKUHVKROGLQJ 7KH ILWQHV IXQFWLRQ LV WKH /QRUP (T DQG WKH REVHUYDWLRQ PHDVXUHPHQWV DUH LQ FHOOV 7KH XVH RI D GLVFDUGLQJ PHFKDQLVP DFFHOHUDWHV WKH FRQYHUJHQFH RI WKH DOJRULWKP ZLWK D WKUHVKROG OHYHO 6WXG\LQJ WKH FDVH ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ LW FDQ EH FRQFOXGHG WKDW WKH FRQYHUJHQFH LV UHDFKHG HDUOLHU 7KH DOJRULWKP ZLWK GLVFDUGLQJ LV HYHQ IDVWHU WKDQ WKH EDVLF YHUVLRQ ZKLOH PDLQWDLQLQJ WKH FDSDELOLW\ WR DYRLG SUHPDWXUH FRQYHUJHQFH LQVLGH WKH QRLVH EDQG DV FDQ EH VHHQ LQ )LJ ZKLFK VKRZV WKH VROXWLRQ RI WKH DOJRULWKP LQ D KLJKO\ V\PPHWULFDO HQYLURQPHQW ,Q WKLV W\SH RI HQYLURQPHQW LW LV QRW SRVVLEOH WR GLVWLQJXLVK EHWZHHQ DOO URRPV EHFDXVH WKH REVHUYDWLRQV DUH WKH VDPH IRU DOO RI WKHP ,I WKH DOJRULWKP LV RSWLPL]HG LQ WKH QRLVH EDQG
High-accuracy global localization filter for 3D environments
Best fitness function value (divided by 10,000)
Best fitness function value vs. iterations 0.8
Basic version Thresholding Thresholding with discarding
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
50
100
150
200
250
300
350
400
Iterations
)LJ %HVW ILWQHV HYROXWLRQ YDOXH YV LWHUDWLRQV 7KUHH GLIIHUHQW RSWLRQV ± ZLWKRXW WKUHVKROGLQJ ZLWK WKUHVKROGLQJ DQG ZLWK D WUHVKROGLQJ EDQG ± ZLWK RI SRSXODWLRQ GLVFDUGLQJ
LW ZLOO HYHQWXDOO\ FRQYHUJH LQWR D URRP JLYLQJ XV D ZURQJ UHVXOW ,W ZLOO ILQDOO FRQYHUJH WR RQH VROXWLRQ HOLPLQDWLQJ DOO RWKHU URRPV DQG FRQFHQWUDWLQJ DOO WKH SRSXODWLRQ DURXQG WKH RSWLPDO VROXWLRQ ,W FDQ EH REVHUYHG WKDW WKH HVWLPDWHG VROXWLRQ SRLQW LQ WKH FHQWHU ZLWK FRRUGLQDWHV GRHV QRW FRLQFLGH ZLWK WKH UHDO VROXWLRQ EXW WKHUH DUH VWLOO FDQGLGDWHV LQ DOO URRPV ,W LV D ORJLFDO UHVXOW WKDW LQGLFDWHV WKDW WKH DOJRULWKP DYRLGV WKH SUHPDWXUH HOLPLQDWLRQ RI VROXWLRQV
,V LV DOVR LPSRUWDQW WR VD\ WKDW WKH WKUHVKROGLQJ PHFKDQLVP GRHV QRW DOZD\V PDNH WKH DOJRULWKP FRQYHUJH VORZHU 7KHUH DUH FDVHV ZKHUH WKH QRLVH PDNHV WKH DOJRULWKP HYROYH WRZDUG HUURQHRXV VROXWLRQV $OWKRXJK WKH QXPEHU RI FDQGLGDWHV WKDW HYROYH LV VPDOOHU WKH LPSURYHPHQWV DUH PRUH VLJQLILFDQW DQG WKH ELJ LQIOXHQF RI WKH EHVW FDQGLGDWHV RQ WKH SRSXODWLRQ LPSURYHV WKH FRQYHUJHQFH VSHHG ,W RFFXUV LQ VLWXDWLRQV ZKHUH QRLVH SOD\V D YHU\ LPSRUWDQW UROH DV FDQ EH VHHQ LQ )LJ ZKLFK VKRZV WKH HYROXWLRQ RI WKH EHVW ILWQHV IXQFWLRQ YDOXH LQ D SDUWLFXODU FDVH XQGHU WKH LQIOXHQF RI DQ LPSRUWDQW QRLVH ZH
)LJ &RORXU RQOLQH $OJRULWKP VROXWLRQ LQ D V\PPHWULFDO HQYLURQPHQW ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ
KWWS MRXUQDOVFDPEULGJHRUJ
High-accuracy global localization filter for 3D environments
Best fitness function value vs. iterations
4
14
x 10
Basic version Thresholding Thresholding with discarding
Best fitness function value
12
10
8
6
4
2
0
10
20
30
40
50
60
70
80
90
100
Iterations )LJ %HVW ILWQHV IXQFWLRQ YDOXH YV LWHUDWLRQV 7KUHH GLIIHUHQW RSWLRQV ZLWKRXW WKUHVKROGLQJ ZLWK WKUHVKROGLQJ DQG ZLWK D WUHVKROGLQJ EDQG ZLWK RI SRSXODWLRQ GLVFDUGLQJ 1RLV\ FDVH RYHU GLVWDQFH
XVH D UDQJH ODVHU ILQGH DV VHQVRU DQG WKH QRLVH LQWURGXFHG LV HTXDO WR RYHU WKH GLVWDQFH ZHLJKWHG ,W FDQ EH REVHUYHG WKDW WKUHVKROGLQJ LQFUHDVHV WKH FRQYHUJHQFH VSHHG LQ WKLV FDVH EHLQJ HYHQ FORVH WR WKUHVKROGLQJ ZLWK GLVFDUGLQJ RSWLRQ
6.4. Fitness function options 7KHUH DUH VHYHUDO RSWLRQV ZKHQ FKRRVLQJ DQ DSSURSULDWH FRVW IXQFWLRQ IRU RXU SDUWLFXODU SUREOHP 7KH /QRUP DQG WKH /QRUP KDYH EHHQ VWXGLHG LQ WKLV SDSHU DQG WKH UHVXOWV DUH SUHVHQWHG EHORZ ,Q RUGHU WR WHVW WKH DOJRULWKP FKDUDFWHULVWLFV LQ WKLV DVSHFW D VLPXODWHG HQYLURQPHQW KDV EHHQ FRQVLGHUHG )LJ 7KLV HQYLURQPHQW LV VLPLODU WR PDQ\ RIILF LQGRRU DUHDV ZKHUH DOO RIILFH DUH ORFDWHG DORQJ WKH FHQWUDO FRUULGRU 7KLV WHVW HQYLURQPHQW ZLOO EH XVHG WR YHULI\ WKH DFFXUDF\ DQG UREXVWQHVV SURSHUWLHV RI WKH HYROXWLYH ORFDOL]DWLRQ ILOWH DOJRULWKP XQGHU WZR GLIIHUHQW FRVW IXQFWLRQV DQG WKH HIIHFW RQ WKH DOJRULWKP FRQYHUJHQFH RI D FRQWDPLQDWLRQ LQ WKH QRLVH HUURU $ *DXVVLDQ REVHUYDWLRQ HUURU RI ]HUR PHDQ DQG VWDQGDUG GHYLDWLRQ σ HTXDO WR RI WKH PHDVXUHG GLVWDQFH KDV EHHQ FRQVLGHUHG 7KH ILUV WHVW WULHV WR GHWHUPLQH WKH FDSDELOLW\ RI WKH DOJRULWKP WR ORFDOL]H WKH URERW ZKHQ LW LV ORFDWHG LQ RQH RI WKH GLVWLQJXLVKDEOH URRPV 7KH URERW¶V SRVH LV YDULDEOH DQG WKHUH LV QR FRQWDPLQDWHG QRLVH 7KH SRVLWLRQ HUURUV LQ FHQWLPHWHUV DUH SUHVHQWHG LQ 7DEOH ,, FROXPQV ZLWK VXELQGH[ NC 7KH VHFRQG WHVW WULHV WR GHWHUPLQH WKH FDSDELOLW\ RI WKH DOJRULWKP WR ORFDOL]H WKH URERW ZKHQ WKHUH H[LVWV D FRQWDPLQDWHG *DXVVLDQ QRLVH 7KLV EHKDYLRU KDV DOUHDG\ EHHQ VWXGLHG LQ RXU SUHYLRXV ZRUN 7KLV VLWXDWLRQ KDSSHQV ZKHQ WKHUH DUH PRELOH REMHWV RU XQH[SHFWHG REVWDFOHV ,Q RUGHU WR WHVW WKLV VLWXDWLRQ WKH URERW KDV EHHQ ORFDWHG LQ RQH RI WKH GLVWLQJXLVKDEOH URRPV DQG WKH QRUPDO *DXVVLDQ QRLVH KDV EHHQ FRQWDPLQDWHG ZLWK D XQLIRUP GLVWULEXWLRQ EHWZHHQ WKH DQG RI WKH VHQVRU PHDVXUHPHQW 7KH UHVXOWV ZLWK D
KWWS MRXUQDOVFDPEULGJHRUJ
RI FRQWDPLQDWLRQ FDQ EH REVHUYHG LQ 7DEOH ,, FROXPQV ZLWK VXELQGH[ C &RPSDULQJ WKH UHVXOWV SUHVHQWHG LQ WKH QR FRQWDPLQDWHG FDVH WKHUH LV QR FOHDU HYLGHQFH WR FKRRVH RQH RSWLRQ DV LQ ERWK FDVHV WKH HUURUV SUHVHQW FRPSDUDEOH YDOXHV 2QO\ WKH SRVLWLRQ HUURU LV FRQVLGHUHG EHFDXVH WKH UHVXOWV ZHUH QHJOLJLEOH ZLWK WKH RULHQWDWLRQ HUURU +RZHYHU LI ZH VWXG\ WKH FDVH ZLWK XQLIRUP FRQWDPLQDWLRQ WKH UHVXOWV DUH PXFK PRUH LQWHUHVWLQJ 7KH HUURU HVWLPDWLQJ WKH SRVH XVLQJ WKH /QRUP WKH DYHUDJH HUURU LV HTXDO WR . FP LV VLJQLILFDQWO ORZHU WKDQ XVLQJ WKH /QRUP WKH DYHUDJH HUURU LV . FP 7KLV LV D ORJLFDO UHVXOW EHFDXVH WKH FRQWDPLQDWLRQ LQWURGXFHG KDV D ELJJHU LQIOXHQF LQ WKH SHQDOL]DWLRQ RI WKH FRVW IXQFWLRQ YDOXH XVLQJ WKH VTXDUH HUURU EHLQJ PXFK PRUH UREXVW ZLWK WKH ILUV RUGHU RQH 0RUHRYHU LI ZH ORRN DW WKH WHUPV LQ SDUHQWKHVHV ZKLFK FRUUHVSRQG WR WKH SRSXODWLRQ VL]H XVHG LQ HDFK FDVH ZH ILQ DQRWKHU LQWHUHVWLQJ FRQFOXVLRQ ,W LV YHU\ RIWHQ QHFHVVDU\ WR XVH D ODUJHU SRSXODWLRQ WR VXFFHHG ZLWK D VPDOOHU VL]H WKH URERW ZDV XQDEOH WR ORFDWH LQ FDVHV RI FRQWDPLQDWLRQ ZKHQ XVLQJ WKH /QRUP ZKLFK DGYHUVHO\ DIIHFWV WKH FRPSXWDWLRQDO FRVW 7KHUHIRUH ZH REWDLQ EHWWHU VROXWLRQV ZLWK WKH FRVW IXQFWLRQ JLYHQ E\ WKH /QRUP LQ FDVHV ZKHUH WKHUH LV D VLJQLILFDQ FRQWDPLQDWLRQ FDVHV ZLWK PDQ\ G\QDPLF REMHFWV SHGHVWULDQV DQG VR RQ
6.5. Complexity and computational cost $ ILQD LVVXH DGGUHVVHG LQ RXU H[SHULPHQWV FRQFHUQV WKH UXQQLQJ WLPH RI WKH 5(/)' DOJRULWKP 7KH FRPSXWDWLRQDO FRVW RI DQ\ DOJRULWKP WKDW ZRUNV LQ ' PDSV LV YHU\ KLJK DQG WKLV LV RQH RI WKH ELJJHVW SUREOHPV IRXQG E\ UHVHDUFKHUV LQ WKLV ILHOG 7KH DEVROXWH WLPH GHSHQGV RQ VHYHUDO IDFWRUV WKH FRPSXWHU SODWIRUP WKH REVHUYDWLRQ SUHGLFWLRQ PRGHO DQG WKH VHQVRU GDWD DQG WKH SRSXODWLRQ DQG LWHUDWLRQV QXPEHU
High-accuracy global localization filter for 3D environments 7DEOH ,, /QRUP DQG /QRUP HVWLPDWLRQ SRVLWLRQ HUURUV IURP GLIIHUHQW ORFDWLRQV 7ZR GLIIHUHQW VLWXDWLRQV QR FRQWDPLQDWLRQ 1& DQG RI FRQWDPLQDWLRQ & (UURUV LQ FHQWLPHWHUV DQG ORFDWLRQV x y z θ LQ FHOOV (DFK FHOO VL]H LV D FXEH RI FP VLGH 3RSXODWLRQ VL]H UHTXLUHG LV LQ SDUHQWKHVHV 7UXH 3RVH $YHUDJH
eP L,N C
eP L,N C
eP L,C
eP L,C
7DEOH ,,, &RPSXWDWLRQ WLPH IRU D IL HG QXPEHU RI LWHUDWLRQV 3RSXODWLRQLWHUDWLRQV
7LPH V
7DEOH ,9 &RPSXWDWLRQ WLPH IRU D IL HG SRSXODWLRQ RI HOHPHQWV 3RSXODWLRQLWHUDWLRQV
7LPH V
7KH DOJRULWKP FRPSOH[LW\ LV O n ZKLFK PHDQV WKDW LW JURZV OLQHDUO\ ZLWK WKH SRSXODWLRQ VL]H DQG WKH SHUFHSWLYH VHQVRU VL]H ,W GRHV QRW GHSHQG RQ WKH URERW¶V GHJUHHV RI IUHHGRP ZKLFK FDQ EH HDVLO\ H[SDQGHG LI QHFHVVDU\ 7DEOH ,,, VKRZV WKH WLPH UHTXLUHG IRU 5(/)' WR XSGDWH WKH HVWLPDWH YDOXH XVLQJ × UDQJH ODVHU GDWD LWHUDWLRQVVWHS DQG GLIIHUHQW SRSXODWLRQ VL]HV 7DEOH ,9 VKRZV WKH WLPH UHTXLUHG IRU WKH 5(/)' DOJRULWKP WR XSGDWH WKH URERW¶V SRVH HVWLPDWH XVLQJ D IL HG SRSXODWLRQ VL]H DQG FKDQJLQJ LWHUDWLRQV SHU VWHS QXPEHU $QDO\]LQJ WKH ORFDOL]DWLRQ PHWKRG GHVFULEHG LQ $OJRULWKP WKH ERWWOHQHFN IURP D FRPSXWDWLRQDO SRLQW RI YLHZ LV WKH IXQFWLRQ QDPHG DV dist est d ,W FRQVLVWV RI HVWLPDWLQJ WKH ' ODVHU VFDQ FRPSRVHG RI × ODVHU EHDPV IURP D SRVH HVWLPDWH LQ WKH VLPXODWHG HQYLURQPHQW DQG LW KDV WR EH FDOFXODWHG IRU HDFK SRSXODWLRQ PHPEHU 7KH UHDGLQJ LV XVHG E\ WKH ILWQHV IXQFWLRQ WR FDOFXODWH WKH HUURU $ FULWLFDO SRLQW LQ DQ\ JOREDO ORFDOL]DWLRQ DOJRULWKP LV WKH YDULDWLRQ LQ WKH FRPSXWDWLRQDO UHTXLUHPHQWV ZLWK HQYLURQPHQW GLPHQVLRQV 7KH 5(/)' DOJRULWKP UHTXLUHPHQWV LQ RXU H[SHULPHQWV QRW RQO\ GHSHQG RQ WKH VL]H
KWWS MRXUQDOVFDPEULGJHRUJ
RI WKH HQYLURQPHQW EXW DOVR RQ WKH FKDUDFWHULVWLFV RI WKH SODFH ZKHUH WKH URERW LV ORFDWHG $V DQ H[DPSOH LQ WKH H[SHULPHQWV GRQH LQ RXU ODERUDWRU\ WHVW VLWH D P × P HQYLURQPHQW WKH DOJRULWKP UHTXLUHV DW OHDVW VDPSOHV ZKHQ LW LV LQ WKH FRUULGRU DQG ZKHQ LW LV LQ RQH RI WKH RIILFHV 7KH FRPSXWDWLRQDO FRVW LV KLJKHU ZKHQ ZH XVH WKH DOJRULWKP LQ WKUHH GLPHQVLRQV EXW WKH LPSURYHPHQW RI WKH DFFXUDF\ LV DQ LPSRUWDQW UHVXOW ,I ZH ZRUN ZLWK D PRELOH PDQLSXODWRU DQG WDVNV ZLWK VPDOO REMHFWV LW ZLOO EH LQGLVSHQVDEOH WR REWDLQ DQ DFFXUDF\ RI PLOOLPHWHUV ,I WKH URERW GRHV QRW QHHG WKLV OHYHO RI DFFXUDF\ ZH ZLOO REWDLQ D UHGXFWLRQ LQ FRPSXWDWLRQDO FRVW ZLWK OHVV UHVWULFWLYH FRQYHUJHQFH FRQGLWLRQV ,W LV QRW HDV\ WR FRPSDUH RXU DOJRULWKP WR RWKHU PHWKRGV 6H et al.¶V JOREDO ORFDOL]DWLRQ FRPSXWDWLRQDO WLPHV DUH . V DQG . V IRU WZR GLIIHUHQW PHWKRGV LPSOHPHQWHG 7KH\ VKRZ WKDW WKH 6,)7 IHDWXUH H[WUDFWLRQ LV HIILFLHQ IURP D FRP SXWDWLRQDO SRLQW RI YLHZ +RZHYHU WKHLU PHWKRG LV YLVLRQ EDVHG DQG WKH FRPSXWDWLRQDO SUREOHPV WKDW WKH\ QHHG WR VROYH DUH GLIIHUHQW %HVLGHV WKH\ GR QRW IRFXV RQ REWDLQLQJ WKH PD[ LPXP DFFXUDF\ DQG WKHLU PDS VL]H LV P RXU PDS VL]H LV P +R et al.¶V DOJRULWKP QHHGV V WR REWDLQ WKH JOREDO ORFDOL]DWLRQ VROXWLRQ 7KLV WLPH LV VLPLODU WR WKH WLPH REWDLQHG LQ RXU UHVXOWV /LQJHPDQQ et al.¶V PHWKRG FDQ EH XVHG RQOLQH EHFDXVH WKH\ QHHG . V WR FRPSXWH D WUDMHFWRU\ FRPSRVHG RI VFDQV +RZHYHU LW LV QRW HDV\ WR FRPSDUH WKHLU PHWKRG WR RXU DOJRULWKP EHFDXVH WKHLU WLPHV DUH UHIHUUHG WR WKH WUDFNLQJ SUREOHP )LQDOO\ WKH WLPH QHHGHG WR REWDLQ PHDVXUHPHQWV FDQ EH WKH PRVW LPSRUWDQW RQH LQ PDQ\ FDVHV DQG LW KDV WR EH FRQVLGHUHG LQ UHDO DSSOLFDWLRQV 7KH WLPH GHOD\V RI XOWUDVRQLF WUDQVPLWWHUUHFHLYHU PRGXOHV DUH DSSUR[ LPDWHO\ HTXDO WR . V LQ WKH SDSHU SXEOLVKHG E\ 7VDL et al
7. Conclusions 7KH ODVW YHUVLRQ RI RXU JOREDO ORFDOL]DWLRQ PHWKRG EDVHG RQ HYROXWLRQDU\ FRQFHSWV LV SUHVHQWHG LQ WKLV SDSHU 7KH DELOLW\ RI WKH DOJRULWKP WR VXFFHVVIXOO\ SHUIRUP LWV WDVNV LV GHPRQVWUDWHG DQG WKH LQIOXHQF RI YDULRXV LPSURYHPHQWV VXFK DV WKUHVKROGLQJ DQG GLVFDUGLQJ LV DOVR GLVFXVVHG $V ZH KDYH GHPRQVWUDWHG E\ WKH UHVXOWV REWDLQHG LQ WKH WHVWV FDUULHG RXW WKLV QHZ YHUVLRQ LV DEOH WR LPSURYH WKH DFFXUDF\ LQ DQ RUGHU RI PDJQLWXGH LQ FRPSDULVRQ ZLWK WKH
High-accuracy global localization filter for 3D environments UHVXOWV REWDLQHG ZLWK WKH RULJLQDO PHWKRG LQ ' PDSV 7KH LQFUHDVH LQ WKH DPRXQW RI LQIRUPDWLRQ DOORZV XV WR REWDLQ DQ DFFXUDF\ RI PLOOLPHWHUV ZKLFK LV EHORZ WKH DFFXUDF\ RI WKH ODVHU PDQLSXODWLRQ WDVNV ZLWK D PRELOH PDQLSXODWRU 7KLV IHDWXUH LV RQH RI WKH PRVW LPSRUWDQW FRQFOXVLRQV RI WKH SUHVHQWHG ZRUN ,W LV QHFHVVDU\ WR HPSKDVL]H WKDW WKH LQFUHPHQW LQ WKH FRPSXWDWLRQDO FRVW GXH WR WKH LQFUHDVH LQ WKH DPRXQW RI KDQGOHG LQIRUPDWLRQ FDQ EH OLPLWHG UHOD[LQJ WKH DFFXUDF\ DQG FRQYHUJHQFH UHTXLUHPHQWV 7KH FRPSXWDWLRQDO FRVW LV DQ LPSRUWDQW OLPLWDWLRQ WR EH DGGUHVVHG EHFDXVH LW LV D FRPPRQ SUREOHP LQ WKLV W\SH RI PHWKRGV ,W FDQ EH VHHQ LQ WKH SUHYLRXV VHFWLRQ ZKHUH WKH FRPSXWDWLRQDO FRVW LV FRPSDUHG ZLWK WKRVH REWDLQHG E\ RWKHU PHWKRGV %HVLGHV WKH SRSXODWLRQ UHGXFWLRQ DIWHU FRQYHUJHQFH PDNHV LW SRVVLEOH WR XVH WKH DOJRULWKP RQOLQH 'XH WR WKH VWRFKDVWLF QDWXUH RI WKH VHDUFK DOJRULWKP IRU WKH URERW¶V EHVW SRVH HVWLPDWH WKH DOJRULWKP LV DEOH WR FRSH ZLWK D KLJK OHYHO RI VHQVRU QRLVH ZLWK ORZ GHJUDGDWLRQ RI HVWLPDWLRQ UHVXOWV $ WKUHVKROGLQJ PHFKDQLVP KDV EHHQ DOVR LPSOHPHQWHG 7KH FRQVHTXHQFH LV WKH FDSDELOLW\ RI WKH 5(/)' WR DYRLG WKH SUHPDWXUH FRQYHUJHQFH WRZDUG VRPH DUHDV QRW VHOHFWLQJ WKH QHZ SRSXODWLRQ PHPEHUV WKDW FDQ EH RULJLQDWHG E\ WKH QRLVH 7KH LQWURGXFWLRQ RI D WKUHVKROG OHYHO KDV DQ LPSRUWDQW GLVDGYDQWDJH WKH GHFUHDVH RI WKH FRQYHUJHQFH VSHHG ,W ZDV WKHUHIRUH QHFHVVDU\ WR GHYHORS D GLVFDUGLQJ PHFKDQLVP WR LPSURYH WKH FRQYHUJHQFH VSHHG ZKLOH PDLQWDLQLQJ WKH SRVLWLYH DVSHFWV RI WKUHVKROGLQJ 7KH FRQYHUJHQFH VSHHG DIWHU GLVFDUGLQJ LV HYHQ KLJKHU WKDQ ZLWK WKH RULJLQDO PHWKRG $ IXUWKHU VWXG\ WR FRPSDUH WKH /QRUP DQG WKH / QRUP DV D FRVW IXQFWLRQ KDV EHHQ GRQH LQ WKLV ZRUN :H FDQ FRQFOXGH WKDW ZLWK D *DXVVLDQ QRLVH WKH EHKDYLRU LV VLPLODU LQ ERWK FDVHV ZKLOH WKH XVH RI WKH ILUVWRUGH HUURU LV XVHIXO LQ FHUWDLQ VLWXDWLRQV VXFK DV WKRVH ZKHUH WKHUH DUH G\QDPLF REMHFWV SHRSOH RXWOLHUV DQG VR RQ ,Q DGGLWLRQ WKH GHYHORSHG DOJRULWKP KDV PDQ\ RWKHU FKDUDFWHULVWLFV WKDW PDNH LWV XVH YHU\ LQWHUHVWLQJ ,W FDQ GHDO ZLWK QRQOLQHDU VWDWH VSDFH G\QDPLFV DQG QRLVH GLVWULEXWLRQV LW GRHV QRW UHTXLUH DQ\ DVVXPSWLRQV RQ WKH VKDSH RI WKH SRVWHULRU GHQVLW\ WKH FRPSXWDWLRQDO UHVRXUFHV IRFXV RQ WKH PRVW UHOHYDQW DUHDV DQG VR RQ
References 0 $JUDZDO DQG . .RQROLJH ³5HDO7LPH /RFDOL]DWLRQ LQ 2XWGRRU (QYLURQPHQWV XVLQJ 6WHUHR 9LVLRQ DQG ,QH[SHQVLYH *SV´ Proceedings of the International Conference on Pattern Recognition (ICPR’06) +RQJ .RQJ $XJ ± : %XUJDUG ' )R[ ' +HQQLQJ DQG 7 6FKPLGW ³(VWLPDWLQJ WKH $EVROXWH 3RVLWLRQ RI D 0RELOH 5RERW 8VLQJ 3RVLWLRQ 3UREDELOLW\ *ULGV´ Proceedings of the Fourteenth National Conference on Artificial Intelligence (AAAI-96) 3RUWODQG 2UHJRQ $XJ – ' 0 &ROH DQG 3 0 1HZPDQ ³8VLQJ /DVHU 5DQJH 'DWD IRU ' 6ODP LQ 2XWGRRU (QYLURQPHQWV´ Proccedings of IEEE International Conference on Robotics and Automation (ICRA’06) 2UODQGR )ORULGD 0D\ $ 'LRVL DQG / .OHHPDQ ³/DVHU 6FDQ 0DWFKLQJ LQ 3RODU &RRUGLQDWHV ZLWK $SSOLFDWLRQ WR 6ODP´ Proceedings of the
KWWS MRXUQDOVFDPEULGJHRUJ
International Conference on Intelligent Robots and Systems (IROS’05) $OEHUWD &DQDGD $XJ ± $ 'RXFHW ³2Q 6TXHQWLDO 6LPXODWLRQ%DVHG 0HWKRGV IRU %D\HVLDQ )LOWHULQJ´ 7HFKQLFDO 5HSRUW &8('),1)(1*75 'HSDUWPHQW RI (QJLQHHULQJ &DPEULGJH 8QLYHUVLW\ &DPEULGJH 8. ' )R[ : %XUJDUG DQG 6 7KUXQ ³0DUNRY ORFDOL]DWLRQ IRU PRELOH URERWV LQ G\QDPLF HQYLURQPHQWV´ J. Artif. Intell. Res. 11 ± ' ( *ROGEHUJ Genetic Algorithm in Search, Optimization and Machine Learning $GGLVRQ :HVOH\ 1 +R DQG 5 -DUYLV ³9LVLRQ%DVHG *OREDO /RFDOLVDWLRQ 8VLQJ D ' (QYLURQPHQWDO 0RGHO &UHDWHG E\ D /DVHU 5DQJH 6FDQQHU´ Proceedings of the International Conference on Intelligent Robots and Systems (IROS’08) $FURSROLV &RQYHQWLRQ &HQWHU 1LFH )UDQFH 6HS 3 -HQVIHOW ³$SSURDFKHV WR PRELOH URERW ORFDOL]DWLRQ LQ LQGRRU HQYLURQPHQWV´ PhD thesis 5R\DO ,QVWLWXWH RI 7HFKQRORJ\ 6ZHGHQ 7 .ULQN % )LOLSLF DQG * % )RJHO ³1RLV\ 2SWLPL]DWLRQ 3UREOHPV ± D 3DUWLFXODU &KDOOHQJH IRU 'LIIHUHQWLDO (YROXWLRQ"´ Proceedings of the Congress on Evolutionary Computation (CEC’04) 6DQ 'LHJR &DOLIRUQLD -XO\ ± 5 .XPPHUOH 5 7ULHEHO 3 3IDII DQG : %XUJDUG ³0RQWH &DUOR ORFDOL]DWLRQ LQ RXWGRRU WHUUDLQV XVLQJ PXOWLOHYHO VXUIDFH PDSV´ J. Field Robot. 42 ± / & /DL 7 / /HH + 7 )DQ DQG & - :X ³$ 1RQOLQHDU 3URJUDPPLQJ 0HWKRG IRU G /RFDOL]DWLRQ RI 0RELOH 5RERWV´ International Conference on Advanced Robotics 6HDWWOH :DVKLQJWRQ -XO ± . /LQJHPDQQ $ 1XFKWHU - +HUW]EHUJ DQG + 6XUPDQQ ³+LJKVSHHG ODVHU ORFDOL]DWLRQ IRU PRELOH URERWV´ Robot. Auton. Syst. 51 ± ' * /RZH ³2EMHFW 5HFRJQLWLRQ IURP /RFDO 6FDOH,QYDULDQW )HDWXUHV´ Seventh International Conference on Computer Vision (ICCV’99) .HUN\UD &RUIX *UHHFH 6HS ± 6 0DUNRQ ' 9 $UQROG 7 %DFN 7 %HLHOVWHLQ DQG + * %H\HU ³7KUHVKROGLQJD 6HOHFWLRQ 2SHUDWRU IRU 1RLV\ (V´ Proceedings of the Congress on Evolutionary Computation (CEC’01) 6HRXO .RUHD ) 0DUWÕQ & *RQ]DOH] / 0RUHQR DQG ' %ODQFR ³$FFHOHUDWHG /RFDOL]DWLRQ LQ 1RLV\ ' (QYLURQPHQWV 8VLQJ 'LIIHUHQWLDO (YROXWLRQ´ Proceedings of International Conference on Genetic and Evolutionary Methods /DV 9HJDV 1HYDGD -XO ± ) 0DUWÕQ / 0RUHQR 6 *DUULGR DQG ' %ODQFR ³/RFDOL]DWLRQ LQ ' (QYLURQPHQWV 8VLQJ 'LIIHUHQWLDO (YROXWLRQ´ International Symposium on Intelligent Signal Processing (WISP’2009) %XGDSHVW +XQJDU\ $XJ ± ) 0DUWÕQ 0 / 0X×QR] 6 *DUULGR ' %ODQFR DQG / 0RUHQR ³/QRUP *OREDO /RFDOL]DWLRQ %DVHG RQ D 'LIIHUHQWLDO (YROXWLRQ )LOWHU´ International Symposium on Intelligent Signal Processing (WISP’2009) %XGDSHVW +XQJDU\ $XJ ± / 0RUHQR 6 *DUULGR DQG 0 / 0X×QR] ³(YROXWLRQDU\ ILOWH IRU UREXVW PRELOH URERW ORFDOL]DWLRQ´ Robot. Auton. Syst. 54 ± $ 1XFKWHU . /LQJHPDQQ DQG - +HUW]EHUJ ³' VODP ± ' PDSSLQJ RXWGRRU HQYLURQPHQWV´ J. Field Robot. 24 ± ( 5R\HU 0 /KXLOOLHU 0 'KRPH DQG -0 /DYHVW ³0RQRFXODU YLVLRQ IRU PRELOH URERW ORFDOL]DWLRQ DQG DXWRQRPRXV QDYLJDWLRQ´ Int. J. Comput. Vis. 74 ± 6 6H ' * /RZH DQG - - /LWWOH ³9LVLRQEDVHG JOREDO ORFDOL]DWLRQ DQG PDSSLQJ IRU PRELOH URERWV´ IEEE Trans. Robot. 21 0 6ULGKDUDQ * .XKOPDQQ DQG 3 6WRQH ³3UDFWLFDO 9LVLRQ %DVHG 0RQWH &DUOR /RFDOL]DWLRQ RQ D /HJJHG 5RERW´ International Conference on Robotics and Automation
(ICRA’05) %DUFHORQD 6SDLQ YRO $SU ± SS 5 6WRUQ DQG . 3ULFH ³'LIIHUHQWLDO HYROXWLRQ ± D VLPSOH DQG HIILFLHQ +HXULVWLF IRU JOREDO RSWLPL]DWLRQ RYHU FRQWLQXRXV VSDFHV´ J. of Glob. Optim. 11 ± 'HF 6 7KUXQ ' +DKQHO ' )HUJXVRQ 0 0RQWHPHUOR 5 7ULHEHO : %XUJDUG & %DNHU = 2PRKXQGUR 6 7KD\HU DQG : :KLWWDNHU ³$ 6\VWHP IRU 9ROXPHWULF 5RERWLF 0DSSLQJ RI $EDQGRQHG 0LQHV´ Proceedings of the International Conference on Robotics and Automation (ICRA’03) 0D\ ± 5 7ULHEHO 3 3IDII DQG : %XUJDUG ³0XOWL/HYHO 6XUIDFH 0DSV IRU 2XWGRRU 7HUUDLQ 0DSSLQJ DQG /RRS &ORVLQJ´ Proceedings
KWWS MRXUQDOVFDPEULGJHRUJ
High-accuracy global localization filter for 3D environments of the International Conference on Intelligent Robots and Systems (IROS’06) %HLMLQJ &KLQD 2FW && 7VDL ++ /LQ DQG 6: /DL ³0XOWLVHQVRU ' SRVWXUH GHWHUPLQDWLRQ RI D PRELOH URERW XVLQJ LQHUWLDO DQG XOWUDVRQLF VHQVRUV´ J. Intell. Robot. Syst. 42 ± $ 5 9DKGDW 1 1 $VKUDIRGGLQ DQG 6 6 *KLGDU\ ³0RELOH 5RERW *OREDO /RFDOL]DWLRQ XVLQJ 'LIIHUHQWLDO (YROXWLRQ DQG 3DUWLFOH 6ZDUP 2SWLPL]DWLRQ´ Proceedings of the Congress on Evolutionary Computation (CEC’07) 6HS ± - :ROI : %XUJDUG DQG + %XUNKDUGW ³5REXVW YLVLRQ EDVHG ORFDOL]DWLRQ E\ FRPELQLQJ DQ LPDJH UHWULHYDO V\VWHP ZLWK PRQWH FDUOR ORFDOL]DWLRQ´ IEEE Trans. Robot. 21
High-accuracy global localization filter for three-dimensional environments )HUQDQGR 0DUWÕQ /XLV 0RUHQR 6DQWLDJR *DUULGR DQG 'RORUHV %ODQFR Carlos III University, Madrid, Spain 5HFHLYHG LQ )LQDO )RUP -XQH DFFHSWHG -XQH )LUVW SXEOLVKHG RQOLQH -XO\
SUMMARY 7KH ORFDOL]DWLRQ SUREOHP LQ PRELOH URERWLFV FDQ EH GHILQH DV WKH VHDUFK RI WKH URERW¶V FRRUGLQDWHV LQ D NQRZQ HQYLURQPHQW ,I WKHUH LV QR LQIRUPDWLRQ DERXW WKH LQLWLDO ORFDWLRQ ZH DUH WDONLQJ DERXW JOREDO ORFDOL]DWLRQ ,Q WKLV ZRUN ZH KDYH GHYHORSHG DQ DOJRULWKP WKDW VROYHV WKLV SUREOHP LQ D WKUHHGLPHQVLRQDO ' HQYLURQPHQW XVLQJ HYROXWLRQDU\ FRPSXWDWLRQ FRQFHSWV 7KH PHWKRG KDV EHHQ FDOOHG 5(/) ' DQG KDV PDQ\ IHDWXUHV WKDW PDNH LW YHU\ UREXVW DQG UHOLDEOH WKUHVKROGLQJ DQG GLVFDUGLQJ PHFKDQLVPV GLIIHUHQW FRVW IXQFWLRQV HIIHFWLYH FRQYHUJHQFH FULWHULD DQG VR RQ 7KH UHVXOWLQJ JOREDO ORFDOL]DWLRQ PRGXOH KDV EHHQ WHVWHG LQ QXPHURXV H[SHULPHQWV DQG WKH PRVW LPSRUWDQW LPSURYHPHQW REWDLQHG LV WKH DFFXUDF\ RI WKH PHWKRG DOORZLQJ LWV DSSOLFDWLRQ LQ PDQLSXODWLRQ WDVNV .(error, ind best@ ← min cost bestmem ← pop ind best conv conditions checking . . . end while end function 䉯 UHWXUQ EHVWPHP HUURU DQG SRSXODWLRQ 4. Evolutive Localization Filter 7KH ORFDOL]DWLRQ DOJRULWKP LV EDVHG RQ HYROXWLYH RSWLPL]DWLRQ WHFKQLTXHV 7KHVH WHFKQLTXHV DUH SUREDELOLVWLF EXW ZLWKRXW GHULYDWLYHV RU SUREDELOLW\ GHQVLW\ IXQFWLRQV WR HVWLPDWH WKH EHVW VROXWLRQ WR WKH ORFDOL]DWLRQ SUREOHP ,Q RXU HYROXWLYH DOJRULWKP WKHUH DUH HOHPHQWV WKDW FRUUHVSRQG WR SRVVLEOH VROXWLRQV DQG WKH ILWQHV IXQFWLRQ YDOXH UHSUHVHQWV WKH HUURU EHWZHHQ WKH UHDO DQG HVWLPDWHG GDWD 7KH VWRFKDVWLF VHDUFK RI WKH URERW¶V FRRUGLQDWHV LV GRQH XVLQJ WKH '( PHWKRG SURSRVHG E\ 6WRUQ DQG 3ULFH IRU JOREDO RSWLPL]DWLRQ SUREOHPV RYHU FRQWLQXRXV VSDFHV ZKLFK LV H[SODLQHG LQ WKLV VHFWLRQ ,Q RUGHU WR GR WKDW WKH UHDGHU FDQ VHH $OJRULWKP %HVLGHV D FRPSOHWH H[SODQDWLRQ RI WKH '( DOJRULWKP FDQ EH IRXQG LQ RXU SUHYLRXV ZRUN )LUVW RI DOO LW LV QHFHVVDU\ WR FRPPHQW RQ VRPH GHWDLOV DERXW WKH HQYLURQPHQW DQG WKH URERW¶V FKDUDFWHULVWLFV 7KH HQYLURQPHQW KDV EHHQ PRGHOHG JHRPHWULFDOO\ DV DQ RFFXSDQF\ JULG PDS LQ WKUHH GLPHQVLRQV DQG WKH URERW¶V SRVH WKH URERW¶V SRVH LV GHILQH DV WKH URERW¶V SRVLWLRQ DQG RULHQWDWLRQ x y z DQG yaw LV UHSUHVHQWHG ZLWK WKH FDUWHVLDQ FRRUGLQDWHV DQG WKH KRUL]RQWDO RULHQWDWLRQ ,Q PRVW RI RXU H[SHULPHQWV HDFK FHOO LV D FXEH RI . FP VLGH DQG WKH ZKROH PDS FRQWDLQV × × = , , FHOOV ,W UHSUHVHQWV D ' HQYLURQPHQW ZLWK D P DUHD DQG D KHLJKW RI P ,I ZH ZDQW WR GHWHUPLQH WKH URERW¶V ORFDOL]DWLRQ IRXU Ö GHILQLQ D VWDWH Ö y, Ö zÖ , θ FRRUGLQDWHV PXVW EH HVWLPDWHG x, VSDFH ZLWK GRI LQ D ' PDS 7KH VLPXODWHG HQYLURQPHQW DQG WKH ZKROH DOJRULWKP KDYH EHHQ LPSOHPHQWHG XVLQJ 0$7/$% ZKLFK LV D QXPHULFDO FRPSXWLQJ HQYLURQPHQW DQG WK JHQHUDWLRQ SURJUDPLQJ ODQJXDJH 'HYHORSHG E\ 0DWK:RUNV 0$7/$% DOORZV PDWUL[ PDQLSXODWLRQV SORWWLQJ RI IXQFWLRQV DQG GDWD LPSOHPHQWDWLRQ RI DOJRULWKPV FUHDWLRQ RI XVHU LQWHUIDFHV DQG LQWHUIDFLQJ ZLWK SURJUDPV ZULWWHQ LQ RWKHU ODQJXDJHV LQFOXGLQJ & & DQG )RUWUDQ 7KH VHDUFK VWDUWV ZLWK D SRSXODWLRQ RI NP FDQGLGDWHV WKH DOJRULWKP LV SRSXODWLRQEDVHG DQG NP UHSUHVHQWV WKH QXPEHU
KWWS MRXUQDOVFDPEULGJHRUJ
RI HOHPHQWV ZKLFK DUH LQWURGXFHG LQ WKH ORFDOL]DWLRQ PRGXOH DQG HYROYH ZLWK WLPH WR WKH EHVW VROXWLRQ (DFK FDQGLGDWH LV D SRVVLEOH VROXWLRQ WR WKH JOREDO ORFDOL]DWLRQ SUREOHP WKH URERW¶V SRVH ZLWK '2) 7KH LQLWLDO SRSXODWLRQ ZLOO EH FKRVHQ UDQGRPO\ )RU HDFK FDQGLGDWH LWV DVVRFLDWHG ILWQHV IXQFWLRQ LV FDOFXODWHG OLQH WR LQ $OJRULWKP 7KH ILWQHV IXQFWLRQ LV D NH\ FRPSRQHQW RI WKH PHWKRG ,W FRPSDUHV UHDO GDWD UHFHLYHG LQ D ODVHU UHDGLQJ ZLWK VLPXODWHG GDWD IURP D FDQGLGDWH VROXWLRQ 7KH HVWLPDWHG GDWD KDYH EHHQ VRUWHG WR FRQILJXU WKH REVHUYDWLRQ YHFWRU ZLWK YHUWLFDO VFDQV VHSDUDWHG E\ ◦ DQG KRUL]RQWDO UHDGLQJV VHSDUDWHG E\ ◦ 7KH PDLQ ORRS VWDUWV LQ OLQH ,I RQH RI WKH FRQYHUJHQFH FRQGLWLRQV LV VDWLVILHG WKH ORFDOL]DWLRQ SURFHVV HQGV VXFFHVVIXOO\ $QRWKHU ORRS ZKLFK FRQWDLQV WKH HYROXWLYH VHDUFK VWDUWV LQ OLQH ,W FRQVLVWV RI D JHQHUDWLRQ RI D QHZ SRSXODWLRQ IRU WKH QH[W JHQHUDWLRQ ,Q D VLQJOH LWHUDWLRQ WKH DOJRULWKP LV H[HFXWHG WR REWDLQ WKH QH[W FDQGLGDWHV HYROYLQJ ZLWK WLPH WR WKH FRUUHFW SRVH (DFK SDUW RI WKH PHFKDQLVP LV H[SODLQHG LQ WKH QH[W VXEVHFWLRQ 4.1. Mutation, crossover, and selection 7KH LQLWLDO SRSXODWLRQ LV SHUWXUEHG WR JHQHUDWH D YDULDWLRQ v DFFRUGLQJ WR WKH IROORZLQJ H[SUHVVLRQ v = xak + F xbk − xck , ZKHUH xak xbk DQG xck DUH SDUDPHWHU YHFWRUV FKRVHQ UDQGRPO\ IURP WKH SRSXODWLRQ DW LWHUDWLRQ k DQG DUH GLIIHUHQW IURP UXQQLQJ LQGH[ WKLV x LV QRW WKH FDUWHVLDQ FRRUGLQDWH EXW WKH HOHPHQW RI SRSXODWLRQ GHILQH E\ IRXU FRRUGLQDWHV F LV D UHDO DQG FRQVWDQW IDFWRU WKDW FRQWUROV WKH DPSOLILFDWLR RI GLIIHUHQWLDO YDULDWLRQV xbk − xck $Q H[DPSOH RI WKH SHUWXUEDWLRQ SURFHVV FDQ EH VHHQ LQ )LJ 7KHUH LV DQ LQLWLDO SDUDPHWHU YHFWRU xik DQG WKH SHUWXUEDWLRQ LV GRQH ZLWK WKUHH
High-accuracy global localization filter for 3D environments Z
F*(xkb – xkc ) x ka
x kb
x ki
Minimum v New parameter vector
xkc Y
X
)LJ &RORXU RQOLQH 1HZ SRSXODWLRQ PHPEHU JHQHUDWLRQ
UDQGRP YDULDEOHV DQG WKH FRQVWDQW IDFWRU F JHQHUDWLQJ WKH QHZ SDUDPHWHU YHFWRU v 7KH YDULDEOHV RI WKLV H[SUHVVLRQ FRXOG EH FKDQJHG LQ RUGHU WR REWDLQ D GLIIHUHQW SHUWXUEHG YHFWRU ,W LV SRVVLEOH WR VHOHFW WKH best SRSXODWLRQ YHFWRU LQVWHDG RI xak 7KH FXUUHQW LPSOHPHQWDWLRQ DOORZV ERWK SRVVLELOLWLHV UDQGRP PXWDWLRQ DQG PXWDWLRQ IURP WKH EHVW FDQGLGDWH 7KH ILUV FKRLFH LV PRUH UREXVW EHFDXVH LW PDLQWDLQV WKH SRSXODWLRQ GLYHUVLW\ DQG LV DSSOLHG LQ JHQHUDO VLWXDWLRQV +RZHYHU WKH PXWDWLRQV IURP WKH EHVW FDQGLGDWHV DUH IDVWHU DQG WKH\ FRXOG EH DSSOLHG LQ VRPH VLWXDWLRQV VXFK DV WKH URERW LV VLWXDWHG LQ D SODFH WKDW LV YHU\ HDV\ WR LGHQWLI\ RU WUDFNLQJ WDVNV DIWHU D VXFFHVVIXO ORFDOL]DWLRQ ,Q RUGHU WR LQFUHDVH WKH GLYHUVLW\ RI WKH QHZ JHQHUDWLRQ WKH FURVVRYHU LV LQWURGXFHG 'HQRWHG E\ uki = uki, , uki, , . . . , uki,D T WKH QHZ SDUDPHWHU YHFWRU LV uki,j
=
k vi,j k xi,j
k LI pi,j , @ IRU HDFK SDUDPHWHU j RI WKH SRSXODWLRQ PHPEHU i DW VWHS k DQG δ LV WKH FURVVRYHU SUREDELOLW\ DQG FRQVWLWXWHV WKH FURVVRYHU k DUH PDGH DQHZ IRU FRQWURO YDULDEOH 7KH UDQGRP YDOXHV pi,j HDFK WULDO YHFWRU i 7KH QHZ PHPEHU RI WKH SRSXODWLRQ uki LV FRPSDUHG WR xik WR GHFLGH ZKHWKHU RU QRW YHFWRU uki VKRXOG EHFRPH D PHPEHU RI JHQHUDWLRQ i + ,I WKH YHFWRU uki \LHOGV D EHWWHU YDOXH IRU WKH REMHFWLYH ILWQHV IXQFWLRQ WKDQ xik WKHQ LW LV UHSODFHG RWKHUZLVH WKH ROG YDOXH xik LV UHWDLQHG IRU WKH QHZ E\ uk+ i JHQHUDWLRQ 7KH JHQHUDO LGHDV RI WKH SUHYLRXV PHFKDQLVP PXWDWLRQ FURVVRYHU DQG VHOHFWLRQ DUH ZHOO NQRZQ DQG FDQ EH IRXQG LQ OLWHUDWXUH
4.2. Thresholding mechanism (YROXWLRQDU\ $OJRULWKPV ($V DQG LQ JHQHUDO SRSXODWLRQ EDVHG PHWKRGV KDYH EHFRPH YHU\ SRSXODU GXH WR LWV DSSOLFDELOLW\ DQG LPSOHPHQWDWLRQ VLPSOLFLW\ 2QH RI WKH PRVW LPSRUWDQW VKRUWFRPLQJV RI WKHVH PHWKRGV LV WKH SUHPDWXUH FRQYHUJHQFH DQG WKH ODFN RI UREXVWQHVV LQ QRLV\ RSWLPL]DWLRQ SUREOHPV ,I '( LV FRPSDUHG ZLWK RWKHU SRSXODWLRQ VHDUFK EDVHG PHWKRGV IRU H[DPSOH *HQHWLF $OJRULWKPV *$ LW VKRZV VRPH ZHDNQHVV 7KLV EHKDYLRU KDV EHHQ VWXGLHG
KWWS MRXUQDOVFDPEULGJHRUJ
E\ .ULQN et al 7KHUH DUH WZR GLIIHUHQW DVSHFWV ZLWK D QHJDWLYH LQIOXHQFH WKH '( PHWKRG LPSOHPHQWV D JUHHG\ VHDUFK VWUDWHJ\ DQG WKH '( PHFKDQLVPV IRU JHQHUDWLQJ QHZ SRWHQWLDO VROXWLRQV DUH OHVV VWRFKDVWLF WKDQ RWKHU ($V 7KHVH GLVDGYDQWDJHV DUH VLJQLILFDQ ZKHQ WKH GLIIHUHQFH EHWZHHQ WKH FDQGLGDWH VROXWLRQ ILWQHV YDOXH DQG WKH FXUUHQW SRSXODWLRQ HOHPHQW ILWQHV YDOXH LV VPDOOHU WKDQ WKH ILWQHV YDULDQFH RULJLQDWHG E\ WKH QRLVH 7KH LGHD RI WKUHVKROGLQJ LV WR UHGXFH WKH HDJHUQHVV RI WKH DOJRULWKP E\ UHMHFWLQJ WKRVH QHZ VROXWLRQV WKDW GR QRW LPSURYH WKH SUHYLRXV K\SRWKHVLV LQ D SUHVSHFLILH PDJQLWXGH τ 7KLV LGHD LV QRW QHZ DQG KDV DOUHDG\ EHHQ DSSOLHG WR HYROXWLRQDU\ FRPSXWDWLRQ SUREOHPV 7KH WKUHVKROG FDQ QRW EH D IL HG PDJQLWXGH EHFDXVH WKLV XQLW GHSHQGV RQ WKH QRLVH YDULDQFH DQG WKH ILWQHV GLVWDQFH WR WKH RSWLPDO ILWQHV YDOXH 7KH QRLVH YDULDQFH PLJKW EH HVWLPDWHG EXW LW LV QRW HDV\ WR HVWLPDWH WKH VHFRQG IDFWRU )LUVW WKH ILWQHV YDOXH GLIIHUHQFH EHWZHHQ WKH SRSXODWLRQ PHPEHU xik DQG WKH FDQGLGDWH PHPEHU uki LV FDOFXODWHG 7KHQ WKLV GLIIHUHQFH LV FRPSDUHG ZLWK D SUHGHILQH WKUHVKROG YDOXH τ LQ RUGHU WR GHWHUPLQH LI WKH LPSURYHPHQW VKRZQ E\ uki LV QRW FDXVHG E\ WKH QRLVH ,I WKLV FRQGLWLRQ LV PHW WKH WDUJHW YHFWRU xik LV UHSODFHG E\ uki LQ WKH QH[W JHQHUDWLRQ RWKHUZLVH xik LV NHSW LQWR WKH SRSXODWLRQ ,Q FRQFOXVLRQ LI WKH LPSURYHPHQW LQ WKH ILWQHV IXQFWLRQ LV ELJJHU WKDQ WKH YDULDQFH RU WKH VWDQGDUG GHYLDWLRQ GHSHQGLQJ RQ WKH VHOHFWHG XQLWV LW FDQ EH FRQVLGHUHG WKDW LW LV QRW FDXVHG E\ WKH QRLVH DQG WKH QHZ PHPEHU FDQ EH LQWURGXFHG LQ WKH SRSXODWLRQ 7KH DOJRULWKP FRQYHUJHQFH DQG UREXVWQHVV LQ QRLV\ RSWLPL]DWLRQ SUREOHPV DQG WKH FRPSDULVRQ EHWZHHQ WKH FXUUHQW PHWKRG DQG WKH LQLWLDO PHWKRG ZLWKRXW WKUHVKROGLQJ KDYH EHHQ SXEOLVKHG LQ RXU UHFHQW ZRUN ,Q RXU H[SHULPHQWV ZH KDYH FKRVHQ D YDOXH WKDW GHSHQGV RQ WKH VHQVRU QRLVH EHFDXVH WKH WKUHVKROGLQJ PHFKDQLVP WULHV WR DYRLG RSWLPL]DWLRQ LQ WKH QRLVH EDQG 7KH WKUHVKROG τ LV HTXDO WR SN × fik ZKHUH SN LV WKH VHQVRU QRLVH SHUFHQWDJH RYHU WKH GLVWDQFH ZHLJKWHG DQG fik LV WKH ILWQHV IXQFWLRQ YDOXH IRU WKH iWK SRSXODWLRQ PHPEHU LQ WKH kWK LWHUDWLRQ 7KLV YDOXH KDV EHHQ FKRVHQ HPSLULFDOO\ )RU H[DPSOH ZKHQ WKH VHQVRU QRLVH LV RYHU WKH GLVWDQFH ZHLJKWHG D VLPSOH DQG DGDSWLYH WKUHVKROG OHYHO RI τ = . × fik KDV EHHQ DGRSWHG WR UHMHFW WKH RIIVSULQJ VROXWLRQ JHQHUDWHG E\ fik ,Q VSLWH RI LWV VLPSOLFLW\ LW ZRUNV TXLWH HIILFLHQWO 7KLV VHOHFWLRQ PHFKDQLVP GHFUHDVHV FRQVLGHUDEO\ WKH HDJHUQHVV RI WKH '( DOJRULWKP DQG DOVR LWV VSHHG RI FRQYHUJHQFH $V D FRQVHTXHQFH RI WKUHVKROGLQJ WKH DOJRULWKP UHMHFWV D KLJK TXDQWLW\ RI QHZ VROXWLRQV DQG DFFHSWV RQO\ WKRVH VROXWLRQV WKDW SUHVHQW D FOHDU LPSURYHPHQW LQ WKH ILWQHV IXQFWLRQ 7KH VWRFKDVWLF UREXVWQHVV RI WKH DOJRULWKP LWV FRPSXWDWLRQDO FRVW DQG WKH LWHUDWLRQV WR FRQYHUJH SUHVHQW ZRUVH YDOXHV VR DQRWKHU PHFKDQLVP GLVFDUGLQJ PXVW EH LQFRUSRUDWHG WR UHGXFH WKLV QHJDWLYH LQIOXHQFH 4.3. Discarding mechanism 7KH XVH RI D WKUHVKROGLQJ EDQG WHQGV WR GHFUHDVH WKH FRQYHUJHQFH VSHHG RI WKH DOJRULWKP SDUWLFXODUO\ DW LQLWLDO VWDJHV GXH WR WKH UHMHFWLRQ RI RIIVSULQJ WKDW GRHV QRW LPSURYH HQRXJK WKH SUHYLRXV K\SRWKHVLV EHORZ WKH WKUHVKROG EDQG
High-accuracy global localization filter for 3D environments
$ GLVFDUGLQJ PHFKDQLVP KDV EHHQ LQWURGXFHG WR LQFUHDVH WKH VSHHG RI WKH DOJRULWKP ZKLOH PDLQWDLQLQJ WKH VWRFKDVWLF DGYDQWDJHV LQ WHUPV RI UREXVWQHVV RI WKUHVKROGLQJ 7KH LGHD LV WR GHWHUPLQH WKH ZRUVW ILWQHV LQGLYLGXDO RI WKH QHZ SRSXODWLRQ DQG VXEVWLWXWH LW E\ D QHZ VROXWLRQ FORVH WR D EHWWHU RQH ,Q RUGHU WR GR WKDW D SHUFHQWDJH RI HOHPHQWV WR EH GLVFDUGHG LV FKRVHQ QRW RQO\ RQH IRU H[DPSOH ,Q RUGHU WR DYRLG FRQFHQWUDWLQJ WKH GLVFDUGHG VROXWLRQV DURXQG WKH EHVW H[LVWLQJ LQGLYLGXDO RQH RI WKH PHPEHUV RI WKH SRSXODWLRQ ZLWK LWV ILWQHV YDOXH ORFDWHG LQ WKH ILUV KDOI RI WKH ILWQHV UDQNLQJ LV VHOHFWHG UDQGRPO\ 7KLV VHOHFWHG VROXWLRQ SOXV D UHODWLYHO\ VPDOO UDQGRP FRPSRQHQW LV DGRSWHG DV D QHZ RIIVSULQJ 7KH GLVFDUGLQJ PHFKDQLVP LV LPSRUWDQW DW HDUO\ VWDJHV RI WKH RSWLPL]DWLRQ SURFHVV ZKHUH HUURUV DUH LPSRUWDQW DQG LW LV QRW VR LQWHUHVWLQJ DW ILQD VWHSV ZKHUH WKH SRSXODWLRQ LV FORVH WR WKH VROXWLRQ 4.4. Convergence conditions 7KHUH DUH PDQ\ SRVVLELOLWLHV ZKHQ FKRRVLQJ D VXLWDEOH REMHFWLYH IXQFWLRQ WR VROYH WKH UHFXUVLYH RSWLPL]DWLRQ SUREOHP JLYHQ E\ (T $VVXPLQJ WKDW WKH VHQVRU PHDVXUHPHQWV DUH *DXVVLDQGLVWULEXWHG RQH RI WKH PRVW FRPPRQ FKRLFHV DV D EDVLV IRU WKH HVWLPDWH LV WKH /QRUP ,Q WKLV FDVH WKH REMHFWLYH IXQFWLRQ WR EH PLQLPL]HG E\ WKH HYROXWLYH DOJRULWKP IRU D SRLQW RI WKH VWDWH VSDFH LQFOXGHG LQ j WKH SRSXODWLRQ xt FRXOG EH JLYHQ E\ WKH IROORZLQJ H[SUHVVLRQ j j f xt
=
j Ns zt,i − zÖ t,i
σe
i=
+
j T j xt − xÖt P − xt − xÖt ,
ZKHUH zt,i LV D VLQJOH PHDVXUHPHQW FRQWDLQHG LQ D UHDGLQJ j JLYHQ E\ WKH ' ODVHU DW LQVWDQW t zÖ t,i LV WKH H[SHFWHG REVHUYDWLRQ IRU WKDW PHDVXUHPHQW LI WKH URERW ZDV VLWXDWHG j LQ xt xÖt LV WKH HVWLPDWHG SRVH LI LW H[LVWV LQ WLPH t P − LV WKH FRYDULDQFH PDWUL[ RI WKH VWDWH HUURU DQG σe LV WKH REVHUYDWLRQ HUURU YDULDQFH :H KDYH FRQVLGHUHG WKDW WKH ODVHU HUURU LV *DXVVLDQGLVWULEXWHG RYHU WKH ZHLJKWHG GLVWDQFH 7KH VHFRQG WHUP RI WKH H[SUHVVLRQ GHSHQGV RQ WKH HVWLPDWHG SRVH xÖt ZKLFK GRHV QRW H[LVW DW WKH EHJLQQLQJ DQG FRXOG QRW EH XQLTXH EHFDXVH WKH URERW FRXOG VWDUW IURP GLIIHUHQW SRVHV ,W LV VWUDLJKWIRUZDUG WKDW WKH VHFRQG WHUP RI WKH ULJKW SDUW FRXOG EH HVWLPDWHG DV D IXQFWLRQ RI WKH GLVWDQFH EHWZHHQ WKH FDQGLGDWH SRVH DQG DOO WKH YLDEOH RQHV EXW LW ZLOO FRPSOLFDWH WRR PXFK WKH ILWQHV IXQFWLRQ HYDOXDWLRQ DGGLQJ DOVR D VWDWLVWLFDO FRPSRQHQW ,Q RUGHU WR DFFHOHUDWH WKH FDOFXODWLRQ ZH ZLOO QRW LQFOXGH LQ WKH ILWQHV IXQFWLRQ WKH LQIRUPDWLRQ JLYHQ E\ WKH GLVWDQFH EHWZHHQ WKH FDQGLGDWH SRVH DQG WKH HVWLPDWHG SRVH XQWLO WKH DOJRULWKP KDV FRQYHUJHG WR RQH VLQJOH SRVH LW LV FRQVLGHUHG WKDW WKH DOJRULWKP KDV FRQYHUJHG ZKHQ DOO FDQGLGDWH SRVHV DUH LQ D VSKHUH RI FRQVWDQW UDWLR DURXQG WKH EHVW RQH ([FOXGLQJ WKLV WHUP WKH ILWQHV IXQFWLRQ EHIRUH FRQYHUJHQFH ZLOO EH JLYHQ E\ WKH IROORZLQJ H[SUHVVLRQ j j f xt
=
j Ns zt,i − zÖ t,i i=
KWWS MRXUQDOVFDPEULGJHRUJ
σe
s νt,i , i= σe
N
=
j
ZKHUH νt,i = zt,i − zÖ t,i UHSUHVHQWV WKH GLVFUHSDQF\ EHWZHHQ WKH REVHUYHG DQG SUHGLFWHG VHQVRULDO GDWD ,W ZRXOG EH LQWHUHVWLQJ IRU XV WR ILQ DQ H[SHFWHG YDOXH IRU WKH FRVW IXQFWLRQ ,Q RUGHU WR GR WKLV ZH KDYH QRWLFHG WKDW WKHUH DUH WZR HIIHFWV WKDW LQIOXHQF WKH FRVW IXQFWLRQ WKH PHDVXUHPHQW QRLVH DQG WKH HVWLPDWLRQ HUURU ,I ZH REWDLQ D SHUIHFW HVWLPDWLRQ WKH VHFRQG HIIHFW FDQ EH HOLPLQDWHG EXW DQ HUURU LQWURGXFHG E\ WKH QRLVH ZLOO DOZD\V H[LVW +HQFH LW LV SRVVLEOH WR HVWLPDWH WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ ZKHQ LW LV FORVH WR WKH WUXH YDOXH E f :H KDYH WR UHPDUN WKDW LW LV WKH ILWQHV IXQFWLRQ YDOXH LQ WKH UHDO SRVLWLRQ s ,I ZH REVHUYH WKH WHUP N i= νt,i /σe RI WKH ODVW H[SUHVVLRQ /σe DUH UDQGRP YDULDEOHV ZLWK D WKH FRPSRQHQWV νt,i VWDQGDUG 1RUPDO GLVWULEXWLRQ N , DQG WKH VXP IROORZV D &KL6TXDUH SUREDELOLW\ GLVWULEXWLRQ ZLWK Ns '2) 7KLV SUREDELOLW\ GLVWULEXWLRQ ZKLFK LV ZHOO NQRZQ DQG WDEXODWHG KDV DQ DYHUDJH RI Ns DQG D YDULDQFH RI Ns / 7KHUHIRUH WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ WKDW ZH DUH WU\LQJ WR PLQLPL]H LV E>f @ =
+∞ −∞
f ν p ν dν = Ns /.
7KLV H[SUHVVLRQ WHOOV XV WKDW HYHQ LI WKH SRVH WKDW ZH DUH HYDOXDWLQJ ZDV WKH FRUUHFW SRVH RI WKH URERW DQG GXH WR PHDVXUHPHQW HUURUV WKDW RFFXU LQ WKH VHQVRU GXULQJ WKH HQYLURQPHQW SHUFHSWLRQ WKH H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ LV Ns / 7KH GHWHUPLQDWLRQ RI D VWRSSLQJ FRQGLWLRQ IRU WKH DOJRULWKP EULQJV XV EDFN WR ZKDW ZDV GLVFXVVHG DERYH EHFDXVH RQFH ZH NQRZ WKH EHVW H[SHFWHG YDOXH RI WKH REMHFWLYH IXQFWLRQ LW LV SRVVLEOH WR HVWDEOLVK D VWRSSLQJ FRQGLWLRQ EDVHG RQ VRPH RI WKH VWDWLVWLFDO SDUDPHWHUV DVVRFLDWHG ZLWK WKLV REMHFWLYH IXQFWLRQ $V ZH KDYH FRQFOXGHG WKDW RXU FRVW IXQFWLRQ FDQ EH DSSUR[LPDWHG E\ D &KL6TXDUH ZLWK Ns '2) LW LV VWUDLJKWIRUZDUG WR DVVRFLDWH WKH REMHFWLYH IXQFWLRQ YDOXH ZLWK D JLYHQ SUREDELOLW\ ,Q RWKHU ZRUGV DQ REMHFWLYH IXQFWLRQ YDOXH f−p ZLWK SUREDELOLW\ − p PHDQV WKDW WKH RSWLPXP YDOXH KDV D SUREDELOLW\ − p RI EHLQJ EHORZ f−p 7KHVH YDOXHV DUH RIWHQ IRXQG WDEXODWHG LQ VWDWLVWLFDO OLWHUDWXUH IRU VRPH W\SLFDO TXDQWLOHV DQG D JLYHQ QXPEHU RI '2) %HVLGHV ZH KDYH FKRVHQ RWKHU VLPSOH FULWHULD WKDW KDYH EHHQ XVHG H[SHULPHQWDOO\ LQ FHUWDLQ VLWXDWLRQV 7KLV FULWHULD GR QRW HQVXUH FRQYHUJHQFH EXW PD\ OHDG WR JRRG UHVXOWV LQ OHVV WLPH ,I RQH RI WKH IROORZLQJ FRQYHUJHQFH FRQGLWLRQV LV VDWLVILHG WKH ORFDOL]DWLRQ SURFHVV ILQLVKHV r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH ILWQHV
IXQFWLRQ YDOXH RI WKH EHVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH ILWQHV IXQFWLRQ YDOXH RI WKH ZRUVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW r 1XPEHU RI LWHUDWLRQV ZLWKRXW FKDQJHV LQ WKH GLIIHUHQFH EHWZHHQ WKH ILWQHV IXQFWLRQ YDOXH RI WKH EHVW HVWLPDWLRQ DQG WKH ILWQHV IXQFWLRQ YDOXH RI WKH ZRUVW HVWLPDWLRQ LV ELJJHU WKDQ D FRQVWDQW
High-accuracy global localization filter for 3D environments 4.5. Fitness function 7KH FRVW IXQFWLRQ FRPSDUHV ODVHU GDWD REWDLQHG IURP WKH URERW WUXH SRVH WR ODVHU GDWD IURP WKH SRVH HVWLPDWHV 7KH ORFDOL]DWLRQ DOJRULWKP PLQLPL]HV WKH HUURU DQG HYROYHV WR WKH WUXH VROXWLRQ 7KH VHQVRU FRQVLGHUHG LQ WKLV SDSHU LV D VLPXODWHG ODVHU WKDW ZRUNV LQ YHUWLFDO VFDQV VHSDUDWHG E\ ◦ DQG HDFK YHUWLFDO VFDQ FRQWDLQV KRUL]RQWDO UHDGLQJV VHSDUDWHG E\ ◦ 7KH ORFDOL]DWLRQ PHWKRG DSSOLHV WKH ILWQHV IXQFWLRQ DOLJQLQJ WKH FXUUHQW VFDQ HVWLPDWHG SRVH ZLWK WKH UHIHUHQFH VFDQ UHDO SRVH 7KH SRSXODWLRQ HYROYHV WR WKH FRUUHFW ORFDWLRQ EHFDXVH WKH PDWFKLQJ HUURU LV PLQLPL]HG 7KH PDWFKLQJ PHWKRG DSSOLHG LQ WKLV SDSHU DYRLGV VHDUFKLQJ IRU SRLQW DVVRFLDWLRQV E\ VLPSO\ PDWFKLQJ SRLQWV ZLWK WKH VDPH EHDULQJ ,W LV FDOOHG 3RODU 6FDQ 0DWFKLQJ 360 DQG WKH GLIIHUHQW RSWLRQV LPSOHPHQWHG DUH H[SODLQHG EHORZ 7KH QDWXUDO FKRLFH IRU WKH FRVW IXQFWLRQ LV WKH VXP RI WKH VTXDUHG HUURUV IXQFWLRQ /QRUP $V H[SODLQHG LQ WKH SUHYLRXV VHFWLRQ LI WKH REVHUYDWLRQ YHFWRU DW WLPH t LV zt = z,t , . . . , zp,t T WKH SUHGLFWHG REVHUYDWLRQV DFFRUGLQJ WR WKH HVWLPDWHG URERW¶V SRVH DUH zÖ t = Öz,t , . . . , zÖ p,t T DQG WKH REVHUYDWLRQ HUURU YDULDQFH LV σe 7KHQ WKH SHQDOW\ IXQFWLRQ j IRU D VLQJOH SRLQW xt RI WKH SRSXODWLRQ FDQ EH VWDWHG DV
L
j xt
− xÖt = zt − zÖ t T zt − zÖ t =
j Ns zt,i − zÖ t,i i=
σe
.
)ROORZLQJ DUH VRPH RI WKH IDFWRUV LQ JOREDO ORFDOL]DWLRQ WKDW PDNH WKLV ILWQHV IXQFWLRQ GLIILFXO WR PDQDJH r 7KH UDQJH DQG DFFXUDF\ RI WKH VHQVRU DQG WKH QXPEHU
RI VHQVRUV OLPLW WKH SRVVLELOLW\ RI GLVWLQJXLVKLQJ EHWZHHQ GLIIHUHQW SRVHV OHDGLQJ WKH ILWQHV IXQFWLRQ WR D KLJK QXPEHU RI JOREDO PD[LPD r 7KH JHRPHWULFDO VLPLODULWLHV LQ WKH HQYLURQPHQW GXH WR WKH UHSHWLWLRQ RI WKH VSDFH GLVWULEXWLRQ RULJLQDWHV WKH SUHVHQFH RI D KLJK QXPEHU RI SRVVLEOH URERW¶V SRVH VROXWLRQV WR WKH PHDQ VTXDUH ORVV IXQFWLRQ 7KH ORVV IXQFWLRQ GHILQH LQ WKLV ZD\ SURYLGHV XV ZLWK DQ RSWLPL]DWLRQ PHFKDQLVP WKDW REWDLQV DQ XQVWDEOH SDUDPHWHU VROXWLRQ DQG E\ H[WHQVLRQ DQ XQVWDEOH ILOWH 7KH HYROXWLYH ILOWH FDQ PRYH IURP RQH ORFDO PLQLPXP WR DQRWKHU RULJLQDWLQJ DEUXSW FKDQJHV LQ WKH SRVH HVWLPDWH 7KLV SUREOHP FRPHV IURP WKH IDFW WKDW ZH DUH QRW XVLQJ DOO WKH LQIRUPDWLRQ ZH NQRZ DERXW WKH V\VWHP LQ WKH ORVV IXQFWLRQ ,Q IDFW ZH RQO\ XVH WKH REVHUYDWLRQ PRGHO WR SUHGLFW WKH VHQVRU PHDVXUHPHQWV DW HDFK SRVLWLRQ DQG WKHVH SUHGLFWHG PHDVXUHPHQWV WRJHWKHU ZLWK WKH DFWXDO PHDVXUHPHQWV DUH LQWURGXFHG LQ WKH ORVV IXQFWLRQ WR HYDOXDWH WKH URERW¶V SRVH HVWLPDWH 7KH LQVWDELOLW\ RULJLQDWHG E\ WKH H[LVWHQFH RI PXOWLSOH VROXWLRQV WR WKH ORVV IXQFWLRQ FDQQRW EH VROYHG E\ FRQVLGHULQJ RQO\ WKH DYDLODEOH VHQVRU REVHUYDWLRQV %HVLGHV WKH OHDVW VTXDUHV PHWKRG LV QRW FRPSOHWHO\ VDWLVIDFWRU\ ZKHQ WKH QRLVH PRGHO LV QRW H[DFWO\ NQRZQ RU WKH PRGHO LV FRQWDPLQDWHG ZLWK RWKHU SUREDELOLW\ GLVWULEXWLRQV 7KH DEVROXWH HUURU /QRUP PD\ EH DQ DSSURSULDWH PHDVXUH LQ VRPH VLWXDWLRQV )RU H[DPSOH LW SUHVHQWV D
KWWS MRXUQDOVFDPEULGJHRUJ
EHWWHU SHUIRUPDQFH ZLWK KLJK HUURUV RULJLQDWHG LQ RXWOLHUV RU FRQWDPLQDWHG PHDVXUHV ,I WKH /QRUP LV QRW GHULYDEOH WKHQ LW LV QHFHVVDU\ WR DSSO\ OLQHDU SURJUDPLQJ PHWKRGV WR REWDLQ D VROXWLRQ WR WKH RSWLPL]DWLRQ SUREOHP %DVHG RQ WKH 0RQWH &DUOR VWXGLHV WKH XVH RI WKH /QRUP KDV EHHQ UHFRPPHQGHG ZKHQ WKH HUURUV IROORZ RQH RI WKHVH GLVWULEXWLRQV /DSODFH &DXFK\ PL[WXUH RI QRUPDO DQG XQLIRUP DQG FRQWDPLQDWHG QRUPDO ,I WKH ILWQHV IXQFWLRQ VHOHFWHG LV WKH /QRUP WKH IXQFWLRQ WR PLQLPL]H LV JLYHQ E\ WKH QH[W HTXDWLRQ
L
j xt
− xÖt =|zt − zÖ t | =
j Ns zt,i − zÖ t,i i=
σe
=
Ns |νt,i | i=
σe
,
j
ZKHUH νt,i = zt,i − zÖ t,i UHSUHVHQWV WKH GLVFUHSDQF\ EHWZHHQ WKH REVHUYHG DQG WKH SUHGLFWHG YDOXHV RI WKH VHQVRU GDWD
5. The Localization Method :H KDYH GHYHORSHG DQ LPSURYHPHQW RI WKH (/) DOJRULWKP WKDW VROYHV WKH JOREDO ORFDOL]DWLRQ SUREOHP LQ D UREXVW DQG HIILFLHQ ZD\ XVLQJ ' VHQVRU GDWD $ VLPXODWLRQ RI WKH ORFDOL]DWLRQ PHWKRG LPSOHPHQWHG LQ WKH URERW FDQ EH VHHQ LQ $OJRULWKP 7KH URERW LV ORFDWHG DW D SODFH LQ WKH HQYLURQPHQW 7KLV ORFDWLRQ LV UHSUHVHQWHG E\ WKUHH VSDWLDO FRRUGLQDWHV DQG WKH RULHQWDWLRQ LQ WKH KRUL]RQWDO SODQH ,W LV LPSRUWDQW WR UHPDUN WKDW RXU PHWKRG ZRUNV LQ WKUHH GLPHQVLRQV EXW WKHUH DUH FDVHV ZKHUH ZH FDQ DVVXPH WKH K\SRWKHVLV WKDW WKH ORFDOL]DWLRQ FRXOG EH VROYHG LQ WZR GLPHQVLRQV )RU LQVWDQFH ZKHQ WKH HQYLURQPHQW LV SODQH DQG WKH URERW KDV D IL HG KHLJKW ZH FDQ FRQVLGHU WKH z FRRUGLQDWH DV D FRQVWDQW YDOXH :H GR QRW KDYH DQ\ LQIRUPDWLRQ DERXW WKH ORFDWLRQ RI WKH URERW WKXV WKH SODFH FDQ EH FRQVLGHUHG UDQGRPO\ 7KH PRELOH URERW UHFHLYHV WKH LQIRUPDWLRQ LQ D ' ODVHU UHDGLQJ 2XU PHWKRG LV QRZ ZRUNLQJ ZLWK VLPXODWHG GDWD DQG WKH UHDGLQJ LV GRQH LQ OLQH RI WKH DOJRULWKP IURP WKH UHDO ORFDWLRQ RI WKH URERW $IWHU WKDW DQ LQLWLDO SRSXODWLRQ LV JHQHUDWHG FRYHULQJ WKH PDS UDQGRPO\ OLQH 7KDW SRSXODWLRQ FRQWDLQV NP SRVVLEOH FDQGLGDWHV :KHQ WKH ODVHU UHDGLQJ KDV EHHQ GRQH WKH PDLQ ORRS VWDUWV OLQH ZRUNLQJ LQ WKH IROORZLQJ ZD\ WKH URERW WULHV WR ORFDWH LWVHOI RQ WKH PDS ZH KDYH FDOOHG LW step DQG LQ RUGHU WR GR WKDW WKH HYROXWLRQDU\ DOJRULWKP OLQH LV H[HFXWHG UHWXUQLQJ WKH HVWLPDWHG VROXWLRQ WKH HUURU DQG WKH ILQD SRSXODWLRQ 7KH FRQFHSW step ZLOO EH XVHG VHYHUDO WLPHV DQG ZH ZLOO H[SODLQ ZKDW LW PHDQV LQ RXU FRQWH[W $ step FDQ EH UHJDUGHG DV WKH PRPHQW LQ WLPH DW ZKLFK WKH URERW XVHV D VLQJOH ' ODVHU VFDQ WR ORFDWH :KHQ LW UHFHLYHV DQRWKHU VFDQ DQG PRWLRQ LQIRUPDWLRQ D QHZ step VWDUWV ,Q RWKHU ZRUGV ZH VD\ WKH URERW LV ORFDWHG LQ D VLQJOH step ZKHQ WKH LW XVHV LQIRUPDWLRQ IURP D VLQJOH ' ODVHU VFDQ ZLWKRXW FRQVLGHULQJ PRYHPHQW LQIRUPDWLRQ ,I WKH URERW PRYHV WR DQRWKHU VLWH DQG WDNHV DQRWKHU VFDQ LW XVHV PRUH LQIRUPDWLRQ WR ORFDWH WKH VHFRQG ODVHU VFDQ DQG WKH PRYHPHQW LQIRUPDWLRQ ,Q WKDW FDVH ZH VD\ LW LV LQ WKH VHFRQG step
High-accuracy global localization filter for 3D environments
Algorithm 2 /RFDOL]DWLRQ ' 3$5$0(7(56 ,1752'8&7,21 real dist d ← dist est d . . . . . . . . . 䉯 /DVHU VLPXODWLRQ pop ← start pop . . . . . . .. dir desp ← NULL while dir desp = end do >bestmem, error, pop@ ← alg genet d . . . . . . . . . . move robot ← VALUE 䉯 URERW¶V GLVSODFHPHQW bestmem ← bestmem + move robot real position ← real position + move robot ∗ + error pos real dist d ← dist est d . . . . . . . . . 䉯 /DVHU VLPXO for i = NP do pop i ← pop i + move robot ∗ + error pos ∗ random end for end while 2QFH WKH ORFDOL]DWLRQ SURFHVV LQ D step KDV ILQLVKHG WKH URERW VWDUWV PRYLQJ 7KH URERW ORFDWLRQ DQG WKH EHVW HVWLPDWLRQ DUH PRYHG DFFRUGLQJ WR WKDW GLVSODFHPHQW ZLWK DQ DGGHG HUURU OLQHV DQG WR LQWHJUDWH WKH PRYHPHQW LQIRUPDWLRQ 7KH VHQVRU PDNHV D QHZ ODVHU UHDGLQJ OLQH DQG UHFHLYHV RGRPHWU\ GDWD 7KHQ WKH SRVVLEOH ORFDWLRQV DUH GLVSODFHG DFFRUGLQJ WR WKH RGRPHWU\ ZLWK DQ HUURU WRR OLQHV ± DQG WKH ORFDOL]DWLRQ SURFHVV LQ WKH VHFRQG step VWDUWV EXW QRZ ZLWK WKH UHVXOWV RI WKH ILUV RQH LQ WKH LQLWLDO SRSXODWLRQ ,W LV REYLRXV WKDW WKH ORFDOL]DWLRQ SURFHVV LV HDVLHU LQ WKLV FDVH EHFDXVH WKHUH LV PRUH LQIRUPDWLRQ :KHQ WKH VHFRQG step ILQLVKHV WKH WKLUG step VWDUWV DQG VR RQ 7KH DOJRULWKP LV UXQQLQJ FRQWLQXRXVO\ ZKLOH WKH URERW PRYHV ZLWKLQ WKH HQYLURQPHQW :H KDYH VLPXODWHG WKDW WKH URERW KDV FRPSOHWHG LWV WDVN LQWURGXFLQJ DQ end LQ OLQH $FFRUGLQJ WR $OJRULWKP WKH ORRS ILQLVKH ZKHQ DQ end LV LQWURGXFHG LQ WKH PRYHPHQW YDULDEOH ZKLFK PHDQV WKH URERW LV WXUQHG RII RU WKH PRYHPHQW LV RYHU DQG WKH URERW LV VXFFHVVIXOO\ ORFDOL]HG ,W LV QHFHVVDU\ WR GHILQ VHYHUDO SDUDPHWHUV EHIRUH WKH H[HFXWLRQ 7KH PRVW LPSRUWDQW RQHV ZLWK D FULWLFDO LQIOXHQF RQ WKH EHKDYLRU RI WKH DOJRULWKP DUH OLVWHG EHORZ r 3RSXODWLRQ 6L]H
,W LV DOVR QHFHVVDU\ WR LQWURGXFH WKH LQLWLDO QXPEHU RI HOHPHQWV ,W LV FULWLFDO DQG GHSHQGV RQ WKH HQYLURQPHQW VL]H :KHQ WKH URERW LV ORFDOL]HG WKH QHHGHG QXPEHU LV VPDOOHU DQG ZH FDQ UHGXFH LW ZLWK D SRVLWLYH LQIOXHQF LQ WKH FRPSXWDWLRQDO FRVW $QRWKHU SUREOHP LV KRZ WR GHWHUPLQH WKH SRSXODWLRQ QXPEHU IRU D JLYHQ HQYLURQPHQW 2Q WKH RQH KDQG WKH QXPEHU GHSHQGV RQ WKH PDS VL]H 2Q WKH RWKHU KDQG WKHUH LV DQRWKHU LPSRUWDQW HOHPHQW WKH V\PPHWULHV 7KH V\PPHWULHV RULJLQDWHG E\ WKH URERW¶V SRVLWLRQ DQG LWV SHUFHSWLRQ FDSDELOLWLHV FDQ PXOWLSO\ WKH SRWHQWLDO QXPEHU RI HTXLYDOHQW PD[LPD 7KLV QXPEHU FDQ YDU\ IURP RQH LQ WKH PRVW IDYRUDEOH FDVH WR PRUH WKDQ DV ZH KDYH REVHUYHG LQ RXU H[SHULPHQWV 7KH UHTXLUHG SRSXODWLRQ QXPEHU FDQ FKDQJH GUDPDWLFDOO\ EHFDXVH D FHUWDLQ QXPEHU RI SRSXODWLRQ HOHPHQWV DUH UHTXLUHG WR PDLQWDLQ DQ K\SRWKHVLV XQWLO QHZ GDWD DUH SHUFHLYHG ,W LV FRPPRQO\ NQRZQ WKDW QRQOLQHDU SRSXODWLRQEDVHG JOREDO RSWLPL]DWLRQ DOJRULWKPV FDQ HDVLO\ DQG SUHPDWXUHO\
KWWS MRXUQDOVFDPEULGJHRUJ
FRQYHUJH WR SRLQW HVWLPDWHV WKDW DUH QRW JOREDOO\ RSWLPDO ,W FDQ EH RULJLQDWHG E\ D ODFN RI SRSXODWLRQ GLYHUVLW\ WKH VORZQHVV RI WKH VHDUFK DOJRULWKP RU WKH H[LVWHQFH RI D ORFDO PLQLPD LQ D PXOWLPRGDO REMHFWLYH IXQFWLRQ 7KH ILUV WZR SUREOHPV FDQ EH DGGUHVVHG E\ LQFUHDVLQJ WKH SRSXODWLRQ VL]H EXW WKH WKLUG RQH LV PRUH GLIILFXOW )LUVW WKH IXQFWLRQ FDQ EH PXOWLPRGDO VR WKH DOJRULWKP ZLOO QRW FRQYHUJH XQWLO WKH PXOWLPRGDOLW\ GLVDSSHDUV 6HFRQG LI WKH QRLVH OHYHO ULVHV D ILFWLWLRX PXOWLPRGDOLW\ LQGXFHG E\ WKH QRLVH OHYHO FDQ DSSHDU r 0XWDWLRQ $PSOLILFDWLR )DFWRU F DQG &URVVRYHU δ F LV D UHDO DQG FRQVWDQW IDFWRU WKDW FRQWUROV WKH DPSOLILFDWLR RI GLIIHUHQWLDO YDULDWLRQV LQ WKH SHUWXUEDWLRQ RI WKH SDUDPHWHU YHFWRU PXWDWLRQ ,Q WKLV SDSHU WKH H[SHULPHQWV DUH GRQH ZLWK F = . 7KH YDOXH RI δ FRQWUROV WKH FURVVRYHU DQG LV IL HG WR . H[SHULPHQWDOO\ %RWK YDULDEOHV ZHUH H[SODLQHG LQ 6XEVHFWLRQ :H KDYH DOVR DGRSWHG DQ DGDSWLYH DGMXVWPHQW RI WKH SHUWXUEDWLRQ DPSOLILFDWLR IDFWRU F 7KLV PHFKDQLVP WULHV WR PDLQWDLQ D KLJK DPSOLILFDWLR IDFWRU ZKLOH WKH SRSXODWLRQ KDV QRW FRQYHUJHG WR WKH SURPLVLQJ DUHDV D ZLGH VFRSH VHDUFK LV UHTXLUHG DQG WR OLPLW WKH DOJRULWKP VHDUFK VFRSH ZKHQ WKH SRSXODWLRQ VHW LV GLVWULEXWHG LQ WKH PRVW IHDVLEOH DUHDV r 8SSHU /LPLW RI ,WHUDWLRQV 7KHUH LV DOVR DQ XSSHU OLPLW IRU WKH QXPEHU RI LWHUDWLRQV SHU step ZKLFK LV DQ DGGHG IHDWXUH GHVLJQHG WR WUDFN WKH URERW IDVWHU ZKHQ LW LV QRW QHFHVVDU\ WR ORFDOL]H LW LQ D VLQJOH RQH ,W LV IDVWHU EHFDXVH ZH XVH PRUH LQIRUPDWLRQ VHYHUDO ODVHU UHDGLQJV DQG RGRPHWU\ LQIRUPDWLRQ DQG LW FDQ EH FKDQJHG WR REWDLQ GLIIHUHQW UHVXOWV )RU H[DPSOH LWHUDWLRQV SHU step PDNH WKH DOJRULWKP JR IDVWHU EXW ZH QHHG PRUH steps WR REWDLQ D JRRG ORFDOL]DWLRQ 1HYHUWKHOHVV LWHUDWLRQV PDNH WKH DOJRULWKP JR VORZO\ EXW ZH FDQ ORFDOL]H WKH URERW LQ D VLQJOH step r 7UDFNLQJ 2QFH WKH ORFDOL]DWLRQ SURFHVV KDV ILQLVKH LQ D VDWLVIDFWRU\ ZD\ WKH URERW NQRZV H[DFWO\ LWV SRVLWLRQ 2XU SUREOHP LV QRZ FRQYHUWHG LQWR WKH WUDFNLQJ SUREOHP ,Q RWKHU ZRUGV WKH URERW ZLOO QHHG WR EH FRQWLQXRXVO\ ORFDOL]HG XSGDWLQJ LWV SRVH DIWHU VPDOO FKDQJHV RI SRVLWLRQ ,Q RUGHU WR GR WKDW RQOLQH WKH SRSXODWLRQ QXPEHU LV UHGXFHG GUDVWLFDOO\ ZKHQ WKH ILWQHV IXQFWLRQ RI WKH EHVW
High-accuracy global localization filter for 3D environments
)LJ &RORXU RQOLQH 7KUHHGLPHQVLRQDO PDS
HVWLPDWLRQ LV XQGHU D WKUHVKROG ZKLFK PHDQV WKDW WKH URERW LV ORFDOL]HG :H KDYH FKRVHQ DV WKH SRSXODWLRQ VL]H DIWHU WKDW 7KLV UHGXFWLRQ LPSOLHV DQ LPSUHVVLYH DGYDQFH LQ WKH FRPSXWDWLRQDO FRVW ,Q HDFK PRWLRQ F\FOH WKH URERW FDQ QRZ XSGDWH LWV SRVH DFFXUDWHO\ DQG TXLFNO\ 6. Experimental Results $OO H[SHULPHQWV KDYH EHHQ GHYHORSHG LQ D VLPXODWHG LQGRRU HQYLURQPHQW ODERUDWRULHV FRUULGRUV DQG RIILFH RI WKH &DUORV ,,, 8QLYHUVLW\ RI 0DGULG $Q H[DPSOH RI D ' PDS FDQ EH IRXQG LQ )LJ 7KH PHDVXUHPHQW PRGHO LV WKH VDPH WKDW ZDV H[SODLQHG LQ 6HFWLRQ . 6.1. Effectiveness of the algorithm ,Q WKH ILUV H[SHULPHQWV ZH KDYH WHVWHG WKH FDSDFLW\ RI RXU PHWKRG WR VROYH WKH ORFDOL]DWLRQ SUREOHP ,Q RUGHU WR GR WKDW ZH PXVW WDFNOH WKH SUREOHP RI ORFDOL]LQJ WKH URERW LQ D UDQGRP SODFH RI WKH HQYLURQPHQW RQO\ ZLWK WKH LQIRUPDWLRQ RI VHQVRUV DW WKLV SRLQW ' GDWD REWDLQHG ZLWK WKH ODVHU UDQJH ILQGH DQG PRYHPHQW LQIRUPDWLRQ )LUVW ZH ZLOO FRQVLGHU WKDW WKH URERW LV VWLOO 7KH ZD\ WR FKHFN LW FRQVLVWV RQ UXQQLQJ WKH DOJRULWKP D QXPEHU RI WLPHV VLWXDWLQJ WKH URERW DW GLIIHUHQW SODFHV DQG REWDLQLQJ WKH SHUFHQWDJH RI VXFFHVV 7KH JOREDO ORFDOL]DWLRQ SURFHVV VWDUWV ZLWK WKH URERW VLWXDWHG LQ D UDQGRP SODFH RI WKH HQYLURQPHQW DQG WZR GLIIHUHQW FDVHV DQG YDULDEOHV GHILQ WKH SURFHVV WKH HVWLPDWH PDWFKHV WKH UHDO SRVH WKH YDULDEOH true LV LQFUHPHQWHG E\ RQH RU WKH HVWLPDWH DQG WKH UHDO SRVH GR QRW FRLQFLGH WKH YDULDEOH false LV LQFUHPHQWHG E\ RQH $IWHU WKDW D QHZ UDQGRP SODFH LV JHQHUDWHG DQG WKH SUHYLRXV SURFHVV ZKLFK LV GHILQH DV D VLQJOH WULDO LV UHSHDWHG 7KH WRWDO QXPEHU RI WULDOV LV total 7KH SHUFHQWDJH RI VXFFHVV LV HTXDO
KWWS MRXUQDOVFDPEULGJHRUJ
WR true GLYLGHG E\ total :H KDYH FKRVHQ DV WKH QXPEHU RI HOHPHQWV RI WKH SRSXODWLRQ 7KH ODVHU UDQJH ILQGH KDV DQ HUURU RI RYHU WKH GLVWDQFH ZHLJKWHG :H KDYH QRW FRQVLGHUHG WKH FRQYHUJHQFH RI WKH DOJRULWKP LQ WKLV H[SHULPHQW DQG ZH OHW WKH DOJRULWKP UXQ D KLJK QXPEHU RI LWHUDWLRQV WR VHH ZKHWKHU LW FDQ REWDLQ WKH VROXWLRQ RU QRW 7KH VXFFHVV RI UXQQLQJ WKH SURJUDP XQGHU WKHVH FRQGLWLRQV LV 7KLV SHUFHQWDJH GHSHQGV RQ WKH HQYLURQPHQW DQG RXU PDS FRQWDLQV PDQ\ VLPLODU SODFHV ZKRVH FRUUHVSRQGHQW PHDVXUHPHQWV DUH WKH VDPH 7KHUHIRUH LW LV QRW SRVVLEOH WR GLVWLQJXLVK EHWZHHQ VHYHUDO GLIIHUHQW ORFDWLRQV EHFDXVH WKH VHQVRU PHDVXUHPHQWV DUH WKH VDPH )RU H[DPSOH LW RFFXUV ZKHQ WKH URERW LV LQVLGH DQ RIILFH 7KDW LV ZK\ WKH DOJRULWKP GRHV QRW VXFFHHG LQ VRPH FDVHV DQG WKH ORFDOL]DWLRQ PHWKRG FRQYHUJHV WR DOO SRVVLEOH VROXWLRQV FRUUHFW SODFH DQG VLPLODU ORFDWLRQV :H FRQVLGHU LW DV D IDLOXUH EHFDXVH LW GRHV QRW FRQYHUJH WR D VLQJOH VROXWLRQ 7KH DOJRULWKP UHWXUQV RQH VROXWLRQ EXW LW FRXOG EH D ZURQJ SODFH ,W FDQ DOVR EH H[SODLQHG XVLQJ WKH KXPDQ EHLQJV SHUFHSWLRQ ,I \RX DUH LQ DQ RIILF FRUQHU ORRNLQJ DW WKH ZDOO LW LV QRW SRVVLEOH WR NQRZ LQ ZKLFK RIILF \RX DUH ,W FDQ EH FRQFOXGHG WKDW WKH LQIRUPDWLRQ LV QRW JRRG HQRXJK WR DSSO\ ORFDOL]DWLRQ WHFKQLTXHV LQ WKHVH FDVHV 7KLV LV D SURPLVLQJ UHVXOW EHFDXVH ZH KDYH QRW LQFOXGHG WKH PRYHPHQW RI WKH URERW XVLQJ VLQJOH step WR ORFDOL]H WKH URERW ZLWK LQIRUPDWLRQ RI RQH ' ODVHU VFDQ DQG QR PRWLRQ LQIRUPDWLRQ EXW LW GRHV QRW UHIOHF D UHDO VLWXDWLRQ 7KH SXUSRVH RI WKH H[SHULPHQW LV WR FKHFN WKH FDSDELOLW\ RI RXU PHWKRG WR VROYH WKH JOREDO ORFDOL]DWLRQ SUREOHP 7KHUHIRUH WKH 5(/)' DOJRULWKP LV VXLWDEOH IRU VROYLQJ WKLV $Q H[DPSOH RI WKH LQIRUPDWLRQ WKDW WKH URERW ZRXOG UHFHLYH LV VKRZQ LQ )LJ ZKHUH WKH ' HQYLURQPHQW WKH ODVHU UHDGLQJV DQG WKH URERW DUH UHSUHVHQWHG
High-accuracy global localization filter for 3D environments
)LJ &RORXU RQOLQH ([DPSOH RI ORFDOL]DWLRQ LQ D FRUULGRU
)LJ &RORXU RQOLQH 6WHSV WR ORFDOL]DWLRQ YV LWHUDWLRQVVWHS (UURU = FRQYHUJHQFH FULWHULD ZRUVW PHPEHU ILWQHV YDOXH VPDOOHU WKDQ × Ns / $OJRULWKP RSWLRQV UDQGRP PXWDWLRQ ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ Ns =
+RZHYHU ZH ZLOO QRW ILQ WKLV VLWXDWLRQ LQ UHDO OLIH EHFDXVH WKH URERW ZLOO QRW EH VWLOO :H KDYH DOVR WHVWHG WKH FDSDFLW\ RI RXU PHWKRG XQGHU PRYHPHQW PDNLQJ GLIIHUHQW H[SHULPHQWV ZLWK FKDQJHV LQ WKH PD[LPXP LWHUDWLRQV SHU step 2Q RQH KDQG ZLWK D KLJK QXPEHU RI LWHUDWLRQV SHU step ZH ZLOO EH FORVHU WR WKH VROXWLRQ LQ RQH VLQJOH step EXW ZLWK D ORZHU YHORFLW\ EHFDXVH ZH QHHG PRUH WLPH WR GHDO ZLWK D VLQJOH ODVHU UHDGLQJ $V D UHPLQGHU WKH URERW XVHV WKH ILUV ODVHU UHDGLQJ WR ORFDWH ZLWK D JLYHQ QXPEHU RI LWHUDWLRQV DQG WKHQ WKH VHFRQG ODVHU UHDGLQJ IURP DQRWKHU ORFDWLRQ LV LQWURGXFHG LQ WKH ORFDOL]DWLRQ PRGXOH LQFOXGLQJ PRWLRQ LQIRUPDWLRQ 2Q WKH RWKHU KDQG ZLWK D ORZ QXPEHU RI LWHUDWLRQV WKH URERW ZLOO QHHG OHVV WLPH WR H[HFXWH D step 7KH QXPEHU RI steps WR ORFDOL]DWLRQ DJDLQVW WKH LWHUDWLRQV SHU VWHS LV VKRZQ LQ )LJ 7KH ODVHU UDQJH ILQGH KDV DQ HUURU RI RYHU WKH GLVWDQFH ZHLJKWHG DQG WKH SRSXODWLRQ VL]H LV 7KH FRQYHUJHQFH FULWHULD WR FRQFOXGH WKDW WKH URERW LV VXFFHVVIXOO\ ORFDOL]HG LV WKDW WKH ZRUVW PHPEHU ILWQHV YDOXH LV ORZHU WKDQ × ∗ Ns / 7KH DYHUDJH QXPEHU RI steps DIWHU VLWXDWLQJ WKH URERW DW GLIIHUHQW SODFHV LV VKRZQ LQ WKH ILJXUH ,QWHUHVWLQJ UHVXOWV FDQ EH FRQFOXGHG REVHUYLQJ )LJ ,W QHHGV PRUH steps WR VXFFHHG ZLWK D PD[LPXP RI 10 LWHUDWLRQV SHU VWHS ORZHU OLPLW FKRVHQ 7KHUHIRUH ZH REWDLQ EHWWHU UHVXOWV LQ RWKHU FDVHV 7KH WLPH QHHGHG LW GHSHQGV RQ WKH QXPEHU RI LWHUDWLRQV WR H[HFXWH WKH ORFDOL]DWLRQ DOJRULWKP LQ D step LV OHVV EXW ZH GR QRW NQRZ WKH VLWXDWLRQ RI WKH URERW XQWLO HLJKW steps ,W PHDQV WKDW WKH QXPEHU RI ODVHU UHDGLQJV XVHG DQG DOVR WKH GLVWDQFH IURP WKH LQLWLDO ORFDWLRQ WR WKH FXUUHQW RQH LV ELJJHU 7KLV FRXOG EH TXLWH GDQJHURXV IRU WKH URERW EHFDXVH LW JRHV D ORQJ ZD\ EHIRUH EHLQJ SURSHUO\
KWWS MRXUQDOVFDPEULGJHRUJ
ORFDOL]HG RQ WKH PDS ,I WKH QXPEHU RI LWHUDWLRQV SHU step LV WKH URERW ZLOO QHHG RQH VLQJOH step +RZHYHU WKH WLPH QHHGHG FRXOG EH D SUREOHP LI WKH WLPH SHU step LV D FRQVWUDLQW ,W FDQ EH FRQFOXGHG WKDW GLIIHUHQW YDOXHV FDQ EH FKRVHQ GHSHQGLQJ RQ URERW UHTXLUHPHQWV )RU H[DPSOH JRRG UHVXOWV DUH REWDLQHG ZLWK LWHUDWLRQV SHU step 7KH URERW LV VXFFHVVIXOO\ ORFDOL]HG TXLFNO\ LQ WZR VWHSV XVLQJ D ORZ QXPEHU RI LWHUDWLRQV 9DKGDW et al. VKRZ D VLPLODU H[SHULPHQW LPSOHPHQWLQJ WKUHH GLIIHUHQW PHWKRGV '( SDUWLFOH VZDUP RSWLPL]DWLRQ DQG 0RQWH &DUOR +RZHYHU WKH QXPEHU RI LWHUDWLRQV SHU step LV VXUSULVLQJO\ ORZ 7KH\ QHHG WZR RU WKUHH LWHUDWLRQV LQ HDFK time-step DQG DERXW steps IRU UREXVW FRQYHUJHQFH 7KH H[SHULPHQW LV GLIIHUHQW IURP RXUV EHFDXVH LW GRHV QRW OLPLW WKH QXPEHU RI steps EXW WKH QXPEHU RI LWHUDWLRQV ,W FDQ EH EHFDXVH LW LV QRW GDQJHURXV IRU WKHLU URERW DQG LW FDQ ZDLW steps XQWLO FRQYHUJHQFH ,W LV D GLIIHUHQW UHVXOW WKDW FRPSOHPHQWV RXU H[SHULPHQWV 7KH\ FRPSDUH FRQYHUJHQFH UDWHV FRQYHUJHQFH WLPHV DQG LQGLYLGXDOV QHHGHG IRU FRQYHUJHQFH DQG WKHLU FRQFOXVLRQ LV WKDW '( FRQYHUJHV IDVWHU PRUH UREXVWO\ DQG ZLWK IHZHU LQGLYLGXDOV 7KH SHUFHQWDJH RI VXFFHVV UXQQLQJ WKH SURJUDP XQGHU WKHVH FRQGLWLRQV LV KLJKHU 7KLV LV D ORJLF UHVXOW EHFDXVH ZH KDYH PRUH LQIRUPDWLRQ VHYHUDO ODVHU UHDGLQJV DQG PRYHPHQW RGRPHWU\ LQIRUPDWLRQ ,W PHDQV WKDW WKH DOJRULWKP UHWXUQV WR WKH FRUUHFW ORFDWLRQ LQ WKH PDMRULW\ RI FDVHV D YHU\ SURPLVLQJ UHVXOW FRQVLGHULQJ WKDW WKH SODFHV DUH UDQGRP DQG WKHUH DUH PDQ\ V\PPHWULHV LQ WKH PDS 6.2. Accuracy of the method 7KH ORFDOL]DWLRQ SURFHVV UHVXOWV LQ GLIIHUHQW SRLQWV RI WKH HQYLURQPHQW FDQ EH VHHQ LQ 7DEOH ,, 7KH FDUWHVLDQ FRRUGLQDWH RI WKH SRLQW ZH ZDQW WR ORFDOL]H LQ WKH PDS LV x DQG xÖ LV WKH HVWLPDWLRQ REWDLQHG 7KLV FDQ EH H[WHQGHG WR y yÖ z DQG zÖ 7KH RULHQWDWLRQ LQ WKH KRUL]RQWDO SODQH WLOW LV θ DQG θÖ LV WKH HVWLPDWLRQ REWDLQHG 7KH VHQVRU QRLVH OHYHO LV D *DXVVLDQ GLVWULEXWLRQ RYHU WKH ODVHU GLVWDQFH ZLWK D VWDQGDUG GHYLDWLRQ RI 7KH SRVLWLRQ HUURU ep LV WKH GLVWDQFH LQ PLOOLPHWHUV EHWZHHQ WKH HVWLPDWHG YDOXH DQG WKH UHDO SRVLWLRQ RI WKH URERW 7KH RULHQWDWLRQ HUURU eθ LV WKH GLIIHUHQFH EHWZHHQ θ DQG θÖ LQ GHJUHHV :H FDQ FRQFOXGH WKDW WKH DFFXUDF\ RI WKH ORFDOL]DWLRQ SURFHVV LV LPSURYHG VLJQLILFDQWO FRPSDUHG ZLWK RXU SUHYLRXV ZRUNV LQ ' PDSV DQG ZH EHOLHYH LW LV WKH PRVW LPSRUWDQW DGYDQFH WKDW ZH KDYH REWDLQHG ,I ZH REVHUYH WKH SRVLWLRQ HUURU FROXPQ ep WKH DYHUDJH HUURU LV HTXDO WR . PP DQG WKH VDPH PHDVXUHPHQW ZLWK RXU SUHYLRXV DOJRULWKP XVLQJ ' VHQVRU GDWD ZDV HTXDO WR FP DV FDQ EH FKHFNHG LQ UHI
High-accuracy global localization filter for 3D environments
7DEOH , (UURUV ORFDOL]LQJ WKH URERW DW GLIIHUHQW SRLQWV 7KH FDUWHVLDQ FRRUGLQDWHV DUH LQ FHOOV DQG WKH HUURUV LQ PLOOLPHWHUV DQG GHJUHHV
x
y
z
θ
xÖ
yÖ
zÖ
θÖ
ep PP
eθ GHJ
> @ 7KH PDLQ UHDVRQ IRU WKLV UHGXFWLRQ LV WKH LQFUHDVH LQ WKH DPRXQW RI LQIRUPDWLRQ 7KH ' ODVHU UHDGLQJ FRQWDLQV QLQH ELGLPHQVLRQDO VFDQV 7KXV WKH LQIRUPDWLRQ LV PXOWLSOLHG E\ QLQH ,W LV SRVVLEOH WR FRPSDUH WKH DFFXUDF\ UHVXOWV ZLWK WKRVH REWDLQHG E\ RWKHU JURXSV ZRUNLQJ RQ ORFDOL]DWLRQ LQ ' HQYLURQPHQWV 7KH DFFXUDF\ DFKLHYHG E\ .XPPHUOH et al. LV DERXW D IHZ FHQWLPHWHUV 6H et al.¶V DYHUDJH (XFOLGHDQ WUDQVODWLRQ HUURU LV HTXDO WR FP DQG WKH DYHUDJH URWDWLRQ HUURU LV R +RZHYHU WKH\ PHQWLRQ WKDW LW FDQ EH UHGXFHG E\ XVLQJ D KLJKHU LPDJH UHVROXWLRQ +R et al. ORFDOL]H WKH URERW ZLWKLQ P RI WUXH SRVLWLRQ DQG R IRU RULHQWDWLRQ 7KHLU EHVW UHVXOWV DUH FP DQG . R 7KH ORFDOL]DWLRQ HUURU PHDQ REWDLQHG E\ 5R\HU et al. LV HTXDO WR . FP DQG .R 7KH UHVXOWV REWDLQHG LQ WKLV SDSHU DUH PRUH DFFXUDWH WKDQ WKRVH DSSURDFKHV )XUWKHUPRUH WKLV LPSURYHPHQW KDV DQ LPSRUWDQW FRQVHTXHQFH IRU WKH FRPSXWDWLRQDO FRVW :H ZRUN ZLWK D ' ODVHU VFDQQHU DQG LW KDV D QHJDWLYH LQIOXHQF RQ WKH FRPSXWDWLRQDO FRVW +RZHYHU WKH DFFXUDF\ LV KLJKHU DQG ZH QHHG OHVV LWHUDWLRQV WR REWDLQ VDWLVIDFWRU\ UHVXOWV 7KH QHJDWLYH LQIOXHQF RI WKH ' ODVHU GDWD FDQ EH SDUWO\ FRPSHQVDWHG ZLWK WKH SRVLWLYH LQIOXHQF RI WKH DFFXUDF\ LPSURYHPHQW DOORZLQJ XV WR XVH WKH DOJRULWKP IDVWHU ZLWK UHOD[DWLRQ LQ WKH DFFXUDF\ RU FRQYHUJHQFH UHTXLUHPHQWV ,I ZH REVHUYH WKH RULHQWDWLRQ HUURU ZH VHH WKDW LWV YDOXH LV YHU\ VPDOO 7KH RULHQWDWLRQ HUURU DYHUDJH ZDV HTXDO WR .◦ IRU WKH UHVXOWV VKRZQ LQ 7DEOH , 7KHUHIRUH LW FDQ EH FRQFOXGHG WKDW WKH PHWKRG SUHVHQWV JRRG IHDWXUHV LQ WKLV DVSHFW 7KH LQIOXHQF RI WKH VHQVRU QRLVH OHYHO FDQ EH REVHUYHG LQ )LJ ZKHUH WKH ORFDOL]DWLRQ HUURU LQ FHQWLPHWHU LV UHSUHVHQWHG YHUVXV WKH ODVHU QRLVH OHYHO VWDQGDUG GHYLDWLRQ RI D *DXVVLDQ GLVWULEXWLRQ RYHU WKH ODVHU GLVWDQFH LQ SHUFHQWDJH RI WKH ZHLJKWHG GLVWDQFH :H REWDLQ DFFHSWDEOH UHVXOWV HYHQ ZLWK D KLJK QRLVH ,W LV D JUHDW UHVXOW WKDW OHDGV XV WR FRQFOXGH WKDW WKH DOJRULWKP SUHVHQWV JRRG SHUIRUPDQFH LQ WKLV DVSHFW 6.3. Thresholding and discarding effect $ WKUHVKROG UHMHFWLRQ EDQG KDV EHHQ DGGHG WR DYRLG WKH SUHPDWXUH HOLPLQDWLRQ RI VROXWLRQV 7KLV PHFKDQLVP GHFUHDVHV WKH HDJHUQHVV RI WKH DOJRULWKP OHWWLQJ LW WR HOLPLQDWH D FDQGLGDWH VROXWLRQ IURP WKH VHW RQO\ ZKHQ WKH RIIVSULQJ FDQGLGDWH LV VLJQLILFDQWO EHWWHU IURP D
KWWS MRXUQDOVFDPEULGJHRUJ
)LJ &RORXU RQOLQH (UURU YV VHQVRU QRLVH OHYHO
VWDWLVWLFDO SRLQW RI YLHZ $ GLVFDUGLQJ RSHUDWRU ZDV DOVR DGGHG WR DFFHOHUDWH WKH FRQYHUJHQFH RI WKH DOJRULWKP E\ HOLPLQDWLQJ D YHU\ ORZ SHUFHQWDJH RI WKH ZRUVW SRSXODWLRQ LQGLYLGXDOV DW HDFK LWHUDWLRQ RI WKH DOJRULWKP DQG UH VDPSOLQJ WKH LQGLYLGXDO FDQGLGDWH LQ WKH YLFLQLW\ RI D EHWWHU FDQGLGDWH VHOHFWHG UDQGRPO\ EHWZHHQ WKH EHVW HOHPHQWV RI WKH SRSXODWLRQ $Q LPSRUWDQW HIIHFW RI WKH XVH RI D WKUHVKROG UHMHFWLRQ OHYHO LV WKH GHFUHDVLQJ RI WKH FRQYHUJHQFH VSHHG DV VKRZQ LQ )LJ 7KH ODWHVW LPSURYHPHQW RFFXUV LQ LWHUDWLRQ QXPEHU LQ WKH WKUHVKROGLQJ RSWLRQ IRU WKH EHVW HVWLPDWLRQ ZKLOH LW LV SURGXFHG DURXQG LWHUDWLRQ QXPEHU LQ WKH RWKHU FDVH ZLWKRXW WKUHVKROGLQJ 7KH ILWQHV IXQFWLRQ LV WKH /QRUP (T DQG WKH REVHUYDWLRQ PHDVXUHPHQWV DUH LQ FHOOV 7KH XVH RI D GLVFDUGLQJ PHFKDQLVP DFFHOHUDWHV WKH FRQYHUJHQFH RI WKH DOJRULWKP ZLWK D WKUHVKROG OHYHO 6WXG\LQJ WKH FDVH ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ LW FDQ EH FRQFOXGHG WKDW WKH FRQYHUJHQFH LV UHDFKHG HDUOLHU 7KH DOJRULWKP ZLWK GLVFDUGLQJ LV HYHQ IDVWHU WKDQ WKH EDVLF YHUVLRQ ZKLOH PDLQWDLQLQJ WKH FDSDELOLW\ WR DYRLG SUHPDWXUH FRQYHUJHQFH LQVLGH WKH QRLVH EDQG DV FDQ EH VHHQ LQ )LJ ZKLFK VKRZV WKH VROXWLRQ RI WKH DOJRULWKP LQ D KLJKO\ V\PPHWULFDO HQYLURQPHQW ,Q WKLV W\SH RI HQYLURQPHQW LW LV QRW SRVVLEOH WR GLVWLQJXLVK EHWZHHQ DOO URRPV EHFDXVH WKH REVHUYDWLRQV DUH WKH VDPH IRU DOO RI WKHP ,I WKH DOJRULWKP LV RSWLPL]HG LQ WKH QRLVH EDQG
High-accuracy global localization filter for 3D environments
Best fitness function value (divided by 10,000)
Best fitness function value vs. iterations 0.8
Basic version Thresholding Thresholding with discarding
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
50
100
150
200
250
300
350
400
Iterations
)LJ %HVW ILWQHV HYROXWLRQ YDOXH YV LWHUDWLRQV 7KUHH GLIIHUHQW RSWLRQV ± ZLWKRXW WKUHVKROGLQJ ZLWK WKUHVKROGLQJ DQG ZLWK D WUHVKROGLQJ EDQG ± ZLWK RI SRSXODWLRQ GLVFDUGLQJ
LW ZLOO HYHQWXDOO\ FRQYHUJH LQWR D URRP JLYLQJ XV D ZURQJ UHVXOW ,W ZLOO ILQDOO FRQYHUJH WR RQH VROXWLRQ HOLPLQDWLQJ DOO RWKHU URRPV DQG FRQFHQWUDWLQJ DOO WKH SRSXODWLRQ DURXQG WKH RSWLPDO VROXWLRQ ,W FDQ EH REVHUYHG WKDW WKH HVWLPDWHG VROXWLRQ SRLQW LQ WKH FHQWHU ZLWK FRRUGLQDWHV GRHV QRW FRLQFLGH ZLWK WKH UHDO VROXWLRQ EXW WKHUH DUH VWLOO FDQGLGDWHV LQ DOO URRPV ,W LV D ORJLFDO UHVXOW WKDW LQGLFDWHV WKDW WKH DOJRULWKP DYRLGV WKH SUHPDWXUH HOLPLQDWLRQ RI VROXWLRQV
,V LV DOVR LPSRUWDQW WR VD\ WKDW WKH WKUHVKROGLQJ PHFKDQLVP GRHV QRW DOZD\V PDNH WKH DOJRULWKP FRQYHUJH VORZHU 7KHUH DUH FDVHV ZKHUH WKH QRLVH PDNHV WKH DOJRULWKP HYROYH WRZDUG HUURQHRXV VROXWLRQV $OWKRXJK WKH QXPEHU RI FDQGLGDWHV WKDW HYROYH LV VPDOOHU WKH LPSURYHPHQWV DUH PRUH VLJQLILFDQW DQG WKH ELJ LQIOXHQF RI WKH EHVW FDQGLGDWHV RQ WKH SRSXODWLRQ LPSURYHV WKH FRQYHUJHQFH VSHHG ,W RFFXUV LQ VLWXDWLRQV ZKHUH QRLVH SOD\V D YHU\ LPSRUWDQW UROH DV FDQ EH VHHQ LQ )LJ ZKLFK VKRZV WKH HYROXWLRQ RI WKH EHVW ILWQHV IXQFWLRQ YDOXH LQ D SDUWLFXODU FDVH XQGHU WKH LQIOXHQF RI DQ LPSRUWDQW QRLVH ZH
)LJ &RORXU RQOLQH $OJRULWKP VROXWLRQ LQ D V\PPHWULFDO HQYLURQPHQW ZLWK WKUHVKROGLQJ DQG GLVFDUGLQJ
KWWS MRXUQDOVFDPEULGJHRUJ
High-accuracy global localization filter for 3D environments
Best fitness function value vs. iterations
4
14
x 10
Basic version Thresholding Thresholding with discarding
Best fitness function value
12
10
8
6
4
2
0
10
20
30
40
50
60
70
80
90
100
Iterations )LJ %HVW ILWQHV IXQFWLRQ YDOXH YV LWHUDWLRQV 7KUHH GLIIHUHQW RSWLRQV ZLWKRXW WKUHVKROGLQJ ZLWK WKUHVKROGLQJ DQG ZLWK D WUHVKROGLQJ EDQG ZLWK RI SRSXODWLRQ GLVFDUGLQJ 1RLV\ FDVH RYHU GLVWDQFH
XVH D UDQJH ODVHU ILQGH DV VHQVRU DQG WKH QRLVH LQWURGXFHG LV HTXDO WR RYHU WKH GLVWDQFH ZHLJKWHG ,W FDQ EH REVHUYHG WKDW WKUHVKROGLQJ LQFUHDVHV WKH FRQYHUJHQFH VSHHG LQ WKLV FDVH EHLQJ HYHQ FORVH WR WKUHVKROGLQJ ZLWK GLVFDUGLQJ RSWLRQ
6.4. Fitness function options 7KHUH DUH VHYHUDO RSWLRQV ZKHQ FKRRVLQJ DQ DSSURSULDWH FRVW IXQFWLRQ IRU RXU SDUWLFXODU SUREOHP 7KH /QRUP DQG WKH /QRUP KDYH EHHQ VWXGLHG LQ WKLV SDSHU DQG WKH UHVXOWV DUH SUHVHQWHG EHORZ ,Q RUGHU WR WHVW WKH DOJRULWKP FKDUDFWHULVWLFV LQ WKLV DVSHFW D VLPXODWHG HQYLURQPHQW KDV EHHQ FRQVLGHUHG )LJ 7KLV HQYLURQPHQW LV VLPLODU WR PDQ\ RIILF LQGRRU DUHDV ZKHUH DOO RIILFH DUH ORFDWHG DORQJ WKH FHQWUDO FRUULGRU 7KLV WHVW HQYLURQPHQW ZLOO EH XVHG WR YHULI\ WKH DFFXUDF\ DQG UREXVWQHVV SURSHUWLHV RI WKH HYROXWLYH ORFDOL]DWLRQ ILOWH DOJRULWKP XQGHU WZR GLIIHUHQW FRVW IXQFWLRQV DQG WKH HIIHFW RQ WKH DOJRULWKP FRQYHUJHQFH RI D FRQWDPLQDWLRQ LQ WKH QRLVH HUURU $ *DXVVLDQ REVHUYDWLRQ HUURU RI ]HUR PHDQ DQG VWDQGDUG GHYLDWLRQ σ HTXDO WR RI WKH PHDVXUHG GLVWDQFH KDV EHHQ FRQVLGHUHG 7KH ILUV WHVW WULHV WR GHWHUPLQH WKH FDSDELOLW\ RI WKH DOJRULWKP WR ORFDOL]H WKH URERW ZKHQ LW LV ORFDWHG LQ RQH RI WKH GLVWLQJXLVKDEOH URRPV 7KH URERW¶V SRVH LV YDULDEOH DQG WKHUH LV QR FRQWDPLQDWHG QRLVH 7KH SRVLWLRQ HUURUV LQ FHQWLPHWHUV DUH SUHVHQWHG LQ 7DEOH ,, FROXPQV ZLWK VXELQGH[ NC 7KH VHFRQG WHVW WULHV WR GHWHUPLQH WKH FDSDELOLW\ RI WKH DOJRULWKP WR ORFDOL]H WKH URERW ZKHQ WKHUH H[LVWV D FRQWDPLQDWHG *DXVVLDQ QRLVH 7KLV EHKDYLRU KDV DOUHDG\ EHHQ VWXGLHG LQ RXU SUHYLRXV ZRUN 7KLV VLWXDWLRQ KDSSHQV ZKHQ WKHUH DUH PRELOH REMHWV RU XQH[SHFWHG REVWDFOHV ,Q RUGHU WR WHVW WKLV VLWXDWLRQ WKH URERW KDV EHHQ ORFDWHG LQ RQH RI WKH GLVWLQJXLVKDEOH URRPV DQG WKH QRUPDO *DXVVLDQ QRLVH KDV EHHQ FRQWDPLQDWHG ZLWK D XQLIRUP GLVWULEXWLRQ EHWZHHQ WKH DQG RI WKH VHQVRU PHDVXUHPHQW 7KH UHVXOWV ZLWK D
KWWS MRXUQDOVFDPEULGJHRUJ
RI FRQWDPLQDWLRQ FDQ EH REVHUYHG LQ 7DEOH ,, FROXPQV ZLWK VXELQGH[ C &RPSDULQJ WKH UHVXOWV SUHVHQWHG LQ WKH QR FRQWDPLQDWHG FDVH WKHUH LV QR FOHDU HYLGHQFH WR FKRRVH RQH RSWLRQ DV LQ ERWK FDVHV WKH HUURUV SUHVHQW FRPSDUDEOH YDOXHV 2QO\ WKH SRVLWLRQ HUURU LV FRQVLGHUHG EHFDXVH WKH UHVXOWV ZHUH QHJOLJLEOH ZLWK WKH RULHQWDWLRQ HUURU +RZHYHU LI ZH VWXG\ WKH FDVH ZLWK XQLIRUP FRQWDPLQDWLRQ WKH UHVXOWV DUH PXFK PRUH LQWHUHVWLQJ 7KH HUURU HVWLPDWLQJ WKH SRVH XVLQJ WKH /QRUP WKH DYHUDJH HUURU LV HTXDO WR . FP LV VLJQLILFDQWO ORZHU WKDQ XVLQJ WKH /QRUP WKH DYHUDJH HUURU LV . FP 7KLV LV D ORJLFDO UHVXOW EHFDXVH WKH FRQWDPLQDWLRQ LQWURGXFHG KDV D ELJJHU LQIOXHQF LQ WKH SHQDOL]DWLRQ RI WKH FRVW IXQFWLRQ YDOXH XVLQJ WKH VTXDUH HUURU EHLQJ PXFK PRUH UREXVW ZLWK WKH ILUV RUGHU RQH 0RUHRYHU LI ZH ORRN DW WKH WHUPV LQ SDUHQWKHVHV ZKLFK FRUUHVSRQG WR WKH SRSXODWLRQ VL]H XVHG LQ HDFK FDVH ZH ILQ DQRWKHU LQWHUHVWLQJ FRQFOXVLRQ ,W LV YHU\ RIWHQ QHFHVVDU\ WR XVH D ODUJHU SRSXODWLRQ WR VXFFHHG ZLWK D VPDOOHU VL]H WKH URERW ZDV XQDEOH WR ORFDWH LQ FDVHV RI FRQWDPLQDWLRQ ZKHQ XVLQJ WKH /QRUP ZKLFK DGYHUVHO\ DIIHFWV WKH FRPSXWDWLRQDO FRVW 7KHUHIRUH ZH REWDLQ EHWWHU VROXWLRQV ZLWK WKH FRVW IXQFWLRQ JLYHQ E\ WKH /QRUP LQ FDVHV ZKHUH WKHUH LV D VLJQLILFDQ FRQWDPLQDWLRQ FDVHV ZLWK PDQ\ G\QDPLF REMHFWV SHGHVWULDQV DQG VR RQ
6.5. Complexity and computational cost $ ILQD LVVXH DGGUHVVHG LQ RXU H[SHULPHQWV FRQFHUQV WKH UXQQLQJ WLPH RI WKH 5(/)' DOJRULWKP 7KH FRPSXWDWLRQDO FRVW RI DQ\ DOJRULWKP WKDW ZRUNV LQ ' PDSV LV YHU\ KLJK DQG WKLV LV RQH RI WKH ELJJHVW SUREOHPV IRXQG E\ UHVHDUFKHUV LQ WKLV ILHOG 7KH DEVROXWH WLPH GHSHQGV RQ VHYHUDO IDFWRUV WKH FRPSXWHU SODWIRUP WKH REVHUYDWLRQ SUHGLFWLRQ PRGHO DQG WKH VHQVRU GDWD DQG WKH SRSXODWLRQ DQG LWHUDWLRQV QXPEHU
High-accuracy global localization filter for 3D environments 7DEOH ,, /QRUP DQG /QRUP HVWLPDWLRQ SRVLWLRQ HUURUV IURP GLIIHUHQW ORFDWLRQV 7ZR GLIIHUHQW VLWXDWLRQV QR FRQWDPLQDWLRQ 1& DQG RI FRQWDPLQDWLRQ & (UURUV LQ FHQWLPHWHUV DQG ORFDWLRQV x y z θ LQ FHOOV (DFK FHOO VL]H LV D FXEH RI FP VLGH 3RSXODWLRQ VL]H UHTXLUHG LV LQ SDUHQWKHVHV 7UXH 3RVH $YHUDJH
eP L,N C
eP L,N C
eP L,C
eP L,C
7DEOH ,,, &RPSXWDWLRQ WLPH IRU D IL HG QXPEHU RI LWHUDWLRQV 3RSXODWLRQLWHUDWLRQV
7LPH V
7DEOH ,9 &RPSXWDWLRQ WLPH IRU D IL HG SRSXODWLRQ RI HOHPHQWV 3RSXODWLRQLWHUDWLRQV
7LPH V
7KH DOJRULWKP FRPSOH[LW\ LV O n ZKLFK PHDQV WKDW LW JURZV OLQHDUO\ ZLWK WKH SRSXODWLRQ VL]H DQG WKH SHUFHSWLYH VHQVRU VL]H ,W GRHV QRW GHSHQG RQ WKH URERW¶V GHJUHHV RI IUHHGRP ZKLFK FDQ EH HDVLO\ H[SDQGHG LI QHFHVVDU\ 7DEOH ,,, VKRZV WKH WLPH UHTXLUHG IRU 5(/)' WR XSGDWH WKH HVWLPDWH YDOXH XVLQJ × UDQJH ODVHU GDWD LWHUDWLRQVVWHS DQG GLIIHUHQW SRSXODWLRQ VL]HV 7DEOH ,9 VKRZV WKH WLPH UHTXLUHG IRU WKH 5(/)' DOJRULWKP WR XSGDWH WKH URERW¶V SRVH HVWLPDWH XVLQJ D IL HG SRSXODWLRQ VL]H DQG FKDQJLQJ LWHUDWLRQV SHU VWHS QXPEHU $QDO\]LQJ WKH ORFDOL]DWLRQ PHWKRG GHVFULEHG LQ $OJRULWKP WKH ERWWOHQHFN IURP D FRPSXWDWLRQDO SRLQW RI YLHZ LV WKH IXQFWLRQ QDPHG DV dist est d ,W FRQVLVWV RI HVWLPDWLQJ WKH ' ODVHU VFDQ FRPSRVHG RI × ODVHU EHDPV IURP D SRVH HVWLPDWH LQ WKH VLPXODWHG HQYLURQPHQW DQG LW KDV WR EH FDOFXODWHG IRU HDFK SRSXODWLRQ PHPEHU 7KH UHDGLQJ LV XVHG E\ WKH ILWQHV IXQFWLRQ WR FDOFXODWH WKH HUURU $ FULWLFDO SRLQW LQ DQ\ JOREDO ORFDOL]DWLRQ DOJRULWKP LV WKH YDULDWLRQ LQ WKH FRPSXWDWLRQDO UHTXLUHPHQWV ZLWK HQYLURQPHQW GLPHQVLRQV 7KH 5(/)' DOJRULWKP UHTXLUHPHQWV LQ RXU H[SHULPHQWV QRW RQO\ GHSHQG RQ WKH VL]H
KWWS MRXUQDOVFDPEULGJHRUJ
RI WKH HQYLURQPHQW EXW DOVR RQ WKH FKDUDFWHULVWLFV RI WKH SODFH ZKHUH WKH URERW LV ORFDWHG $V DQ H[DPSOH LQ WKH H[SHULPHQWV GRQH LQ RXU ODERUDWRU\ WHVW VLWH D P × P HQYLURQPHQW WKH DOJRULWKP UHTXLUHV DW OHDVW VDPSOHV ZKHQ LW LV LQ WKH FRUULGRU DQG ZKHQ LW LV LQ RQH RI WKH RIILFHV 7KH FRPSXWDWLRQDO FRVW LV KLJKHU ZKHQ ZH XVH WKH DOJRULWKP LQ WKUHH GLPHQVLRQV EXW WKH LPSURYHPHQW RI WKH DFFXUDF\ LV DQ LPSRUWDQW UHVXOW ,I ZH ZRUN ZLWK D PRELOH PDQLSXODWRU DQG WDVNV ZLWK VPDOO REMHFWV LW ZLOO EH LQGLVSHQVDEOH WR REWDLQ DQ DFFXUDF\ RI PLOOLPHWHUV ,I WKH URERW GRHV QRW QHHG WKLV OHYHO RI DFFXUDF\ ZH ZLOO REWDLQ D UHGXFWLRQ LQ FRPSXWDWLRQDO FRVW ZLWK OHVV UHVWULFWLYH FRQYHUJHQFH FRQGLWLRQV ,W LV QRW HDV\ WR FRPSDUH RXU DOJRULWKP WR RWKHU PHWKRGV 6H et al.¶V JOREDO ORFDOL]DWLRQ FRPSXWDWLRQDO WLPHV DUH . V DQG . V IRU WZR GLIIHUHQW PHWKRGV LPSOHPHQWHG 7KH\ VKRZ WKDW WKH 6,)7 IHDWXUH H[WUDFWLRQ LV HIILFLHQ IURP D FRP SXWDWLRQDO SRLQW RI YLHZ +RZHYHU WKHLU PHWKRG LV YLVLRQ EDVHG DQG WKH FRPSXWDWLRQDO SUREOHPV WKDW WKH\ QHHG WR VROYH DUH GLIIHUHQW %HVLGHV WKH\ GR QRW IRFXV RQ REWDLQLQJ WKH PD[ LPXP DFFXUDF\ DQG WKHLU PDS VL]H LV P RXU PDS VL]H LV P +R et al.¶V DOJRULWKP QHHGV V WR REWDLQ WKH JOREDO ORFDOL]DWLRQ VROXWLRQ 7KLV WLPH LV VLPLODU WR WKH WLPH REWDLQHG LQ RXU UHVXOWV /LQJHPDQQ et al.¶V PHWKRG FDQ EH XVHG RQOLQH EHFDXVH WKH\ QHHG . V WR FRPSXWH D WUDMHFWRU\ FRPSRVHG RI VFDQV +RZHYHU LW LV QRW HDV\ WR FRPSDUH WKHLU PHWKRG WR RXU DOJRULWKP EHFDXVH WKHLU WLPHV DUH UHIHUUHG WR WKH WUDFNLQJ SUREOHP )LQDOO\ WKH WLPH QHHGHG WR REWDLQ PHDVXUHPHQWV FDQ EH WKH PRVW LPSRUWDQW RQH LQ PDQ\ FDVHV DQG LW KDV WR EH FRQVLGHUHG LQ UHDO DSSOLFDWLRQV 7KH WLPH GHOD\V RI XOWUDVRQLF WUDQVPLWWHUUHFHLYHU PRGXOHV DUH DSSUR[ LPDWHO\ HTXDO WR . V LQ WKH SDSHU SXEOLVKHG E\ 7VDL et al
7. Conclusions 7KH ODVW YHUVLRQ RI RXU JOREDO ORFDOL]DWLRQ PHWKRG EDVHG RQ HYROXWLRQDU\ FRQFHSWV LV SUHVHQWHG LQ WKLV SDSHU 7KH DELOLW\ RI WKH DOJRULWKP WR VXFFHVVIXOO\ SHUIRUP LWV WDVNV LV GHPRQVWUDWHG DQG WKH LQIOXHQF RI YDULRXV LPSURYHPHQWV VXFK DV WKUHVKROGLQJ DQG GLVFDUGLQJ LV DOVR GLVFXVVHG $V ZH KDYH GHPRQVWUDWHG E\ WKH UHVXOWV REWDLQHG LQ WKH WHVWV FDUULHG RXW WKLV QHZ YHUVLRQ LV DEOH WR LPSURYH WKH DFFXUDF\ LQ DQ RUGHU RI PDJQLWXGH LQ FRPSDULVRQ ZLWK WKH
High-accuracy global localization filter for 3D environments UHVXOWV REWDLQHG ZLWK WKH RULJLQDO PHWKRG LQ ' PDSV 7KH LQFUHDVH LQ WKH DPRXQW RI LQIRUPDWLRQ DOORZV XV WR REWDLQ DQ DFFXUDF\ RI PLOOLPHWHUV ZKLFK LV EHORZ WKH DFFXUDF\ RI WKH ODVHU PDQLSXODWLRQ WDVNV ZLWK D PRELOH PDQLSXODWRU 7KLV IHDWXUH LV RQH RI WKH PRVW LPSRUWDQW FRQFOXVLRQV RI WKH SUHVHQWHG ZRUN ,W LV QHFHVVDU\ WR HPSKDVL]H WKDW WKH LQFUHPHQW LQ WKH FRPSXWDWLRQDO FRVW GXH WR WKH LQFUHDVH LQ WKH DPRXQW RI KDQGOHG LQIRUPDWLRQ FDQ EH OLPLWHG UHOD[LQJ WKH DFFXUDF\ DQG FRQYHUJHQFH UHTXLUHPHQWV 7KH FRPSXWDWLRQDO FRVW LV DQ LPSRUWDQW OLPLWDWLRQ WR EH DGGUHVVHG EHFDXVH LW LV D FRPPRQ SUREOHP LQ WKLV W\SH RI PHWKRGV ,W FDQ EH VHHQ LQ WKH SUHYLRXV VHFWLRQ ZKHUH WKH FRPSXWDWLRQDO FRVW LV FRPSDUHG ZLWK WKRVH REWDLQHG E\ RWKHU PHWKRGV %HVLGHV WKH SRSXODWLRQ UHGXFWLRQ DIWHU FRQYHUJHQFH PDNHV LW SRVVLEOH WR XVH WKH DOJRULWKP RQOLQH 'XH WR WKH VWRFKDVWLF QDWXUH RI WKH VHDUFK DOJRULWKP IRU WKH URERW¶V EHVW SRVH HVWLPDWH WKH DOJRULWKP LV DEOH WR FRSH ZLWK D KLJK OHYHO RI VHQVRU QRLVH ZLWK ORZ GHJUDGDWLRQ RI HVWLPDWLRQ UHVXOWV $ WKUHVKROGLQJ PHFKDQLVP KDV EHHQ DOVR LPSOHPHQWHG 7KH FRQVHTXHQFH LV WKH FDSDELOLW\ RI WKH 5(/)' WR DYRLG WKH SUHPDWXUH FRQYHUJHQFH WRZDUG VRPH DUHDV QRW VHOHFWLQJ WKH QHZ SRSXODWLRQ PHPEHUV WKDW FDQ EH RULJLQDWHG E\ WKH QRLVH 7KH LQWURGXFWLRQ RI D WKUHVKROG OHYHO KDV DQ LPSRUWDQW GLVDGYDQWDJH WKH GHFUHDVH RI WKH FRQYHUJHQFH VSHHG ,W ZDV WKHUHIRUH QHFHVVDU\ WR GHYHORS D GLVFDUGLQJ PHFKDQLVP WR LPSURYH WKH FRQYHUJHQFH VSHHG ZKLOH PDLQWDLQLQJ WKH SRVLWLYH DVSHFWV RI WKUHVKROGLQJ 7KH FRQYHUJHQFH VSHHG DIWHU GLVFDUGLQJ LV HYHQ KLJKHU WKDQ ZLWK WKH RULJLQDO PHWKRG $ IXUWKHU VWXG\ WR FRPSDUH WKH /QRUP DQG WKH / QRUP DV D FRVW IXQFWLRQ KDV EHHQ GRQH LQ WKLV ZRUN :H FDQ FRQFOXGH WKDW ZLWK D *DXVVLDQ QRLVH WKH EHKDYLRU LV VLPLODU LQ ERWK FDVHV ZKLOH WKH XVH RI WKH ILUVWRUGH HUURU LV XVHIXO LQ FHUWDLQ VLWXDWLRQV VXFK DV WKRVH ZKHUH WKHUH DUH G\QDPLF REMHFWV SHRSOH RXWOLHUV DQG VR RQ ,Q DGGLWLRQ WKH GHYHORSHG DOJRULWKP KDV PDQ\ RWKHU FKDUDFWHULVWLFV WKDW PDNH LWV XVH YHU\ LQWHUHVWLQJ ,W FDQ GHDO ZLWK QRQOLQHDU VWDWH VSDFH G\QDPLFV DQG QRLVH GLVWULEXWLRQV LW GRHV QRW UHTXLUH DQ\ DVVXPSWLRQV RQ WKH VKDSH RI WKH SRVWHULRU GHQVLW\ WKH FRPSXWDWLRQDO UHVRXUFHV IRFXV RQ WKH PRVW UHOHYDQW DUHDV DQG VR RQ
References 0 $JUDZDO DQG . .RQROLJH ³5HDO7LPH /RFDOL]DWLRQ LQ 2XWGRRU (QYLURQPHQWV XVLQJ 6WHUHR 9LVLRQ DQG ,QH[SHQVLYH *SV´ Proceedings of the International Conference on Pattern Recognition (ICPR’06) +RQJ .RQJ $XJ ± : %XUJDUG ' )R[ ' +HQQLQJ DQG 7 6FKPLGW ³(VWLPDWLQJ WKH $EVROXWH 3RVLWLRQ RI D 0RELOH 5RERW 8VLQJ 3RVLWLRQ 3UREDELOLW\ *ULGV´ Proceedings of the Fourteenth National Conference on Artificial Intelligence (AAAI-96) 3RUWODQG 2UHJRQ $XJ – ' 0 &ROH DQG 3 0 1HZPDQ ³8VLQJ /DVHU 5DQJH 'DWD IRU ' 6ODP LQ 2XWGRRU (QYLURQPHQWV´ Proccedings of IEEE International Conference on Robotics and Automation (ICRA’06) 2UODQGR )ORULGD 0D\ $ 'LRVL DQG / .OHHPDQ ³/DVHU 6FDQ 0DWFKLQJ LQ 3RODU &RRUGLQDWHV ZLWK $SSOLFDWLRQ WR 6ODP´ Proceedings of the
KWWS MRXUQDOVFDPEULGJHRUJ
International Conference on Intelligent Robots and Systems (IROS’05) $OEHUWD &DQDGD $XJ ± $ 'RXFHW ³2Q 6TXHQWLDO 6LPXODWLRQ%DVHG 0HWKRGV IRU %D\HVLDQ )LOWHULQJ´ 7HFKQLFDO 5HSRUW &8('),1)(1*75 'HSDUWPHQW RI (QJLQHHULQJ &DPEULGJH 8QLYHUVLW\ &DPEULGJH 8. ' )R[ : %XUJDUG DQG 6 7KUXQ ³0DUNRY ORFDOL]DWLRQ IRU PRELOH URERWV LQ G\QDPLF HQYLURQPHQWV´ J. Artif. Intell. Res. 11 ± ' ( *ROGEHUJ Genetic Algorithm in Search, Optimization and Machine Learning $GGLVRQ :HVOH\ 1 +R DQG 5 -DUYLV ³9LVLRQ%DVHG *OREDO /RFDOLVDWLRQ 8VLQJ D ' (QYLURQPHQWDO 0RGHO &UHDWHG E\ D /DVHU 5DQJH 6FDQQHU´ Proceedings of the International Conference on Intelligent Robots and Systems (IROS’08) $FURSROLV &RQYHQWLRQ &HQWHU 1LFH )UDQFH 6HS 3 -HQVIHOW ³$SSURDFKHV WR PRELOH URERW ORFDOL]DWLRQ LQ LQGRRU HQYLURQPHQWV´ PhD thesis 5R\DO ,QVWLWXWH RI 7HFKQRORJ\ 6ZHGHQ 7 .ULQN % )LOLSLF DQG * % )RJHO ³1RLV\ 2SWLPL]DWLRQ 3UREOHPV ± D 3DUWLFXODU &KDOOHQJH IRU 'LIIHUHQWLDO (YROXWLRQ"´ Proceedings of the Congress on Evolutionary Computation (CEC’04) 6DQ 'LHJR &DOLIRUQLD -XO\ ± 5 .XPPHUOH 5 7ULHEHO 3 3IDII DQG : %XUJDUG ³0RQWH &DUOR ORFDOL]DWLRQ LQ RXWGRRU WHUUDLQV XVLQJ PXOWLOHYHO VXUIDFH PDSV´ J. Field Robot. 42 ± / & /DL 7 / /HH + 7 )DQ DQG & - :X ³$ 1RQOLQHDU 3URJUDPPLQJ 0HWKRG IRU G /RFDOL]DWLRQ RI 0RELOH 5RERWV´ International Conference on Advanced Robotics 6HDWWOH :DVKLQJWRQ -XO ± . /LQJHPDQQ $ 1XFKWHU - +HUW]EHUJ DQG + 6XUPDQQ ³+LJKVSHHG ODVHU ORFDOL]DWLRQ IRU PRELOH URERWV´ Robot. Auton. Syst. 51 ± ' * /RZH ³2EMHFW 5HFRJQLWLRQ IURP /RFDO 6FDOH,QYDULDQW )HDWXUHV´ Seventh International Conference on Computer Vision (ICCV’99) .HUN\UD &RUIX *UHHFH 6HS ± 6 0DUNRQ ' 9 $UQROG 7 %DFN 7 %HLHOVWHLQ DQG + * %H\HU ³7KUHVKROGLQJD 6HOHFWLRQ 2SHUDWRU IRU 1RLV\ (V´ Proceedings of the Congress on Evolutionary Computation (CEC’01) 6HRXO .RUHD ) 0DUWÕQ & *RQ]DOH] / 0RUHQR DQG ' %ODQFR ³$FFHOHUDWHG /RFDOL]DWLRQ LQ 1RLV\ ' (QYLURQPHQWV 8VLQJ 'LIIHUHQWLDO (YROXWLRQ´ Proceedings of International Conference on Genetic and Evolutionary Methods /DV 9HJDV 1HYDGD -XO ± ) 0DUWÕQ / 0RUHQR 6 *DUULGR DQG ' %ODQFR ³/RFDOL]DWLRQ LQ ' (QYLURQPHQWV 8VLQJ 'LIIHUHQWLDO (YROXWLRQ´ International Symposium on Intelligent Signal Processing (WISP’2009) %XGDSHVW +XQJDU\ $XJ ± ) 0DUWÕQ 0 / 0X×QR] 6 *DUULGR ' %ODQFR DQG / 0RUHQR ³/QRUP *OREDO /RFDOL]DWLRQ %DVHG RQ D 'LIIHUHQWLDO (YROXWLRQ )LOWHU´ International Symposium on Intelligent Signal Processing (WISP’2009) %XGDSHVW +XQJDU\ $XJ ± / 0RUHQR 6 *DUULGR DQG 0 / 0X×QR] ³(YROXWLRQDU\ ILOWH IRU UREXVW PRELOH URERW ORFDOL]DWLRQ´ Robot. Auton. Syst. 54 ± $ 1XFKWHU . /LQJHPDQQ DQG - +HUW]EHUJ ³' VODP ± ' PDSSLQJ RXWGRRU HQYLURQPHQWV´ J. Field Robot. 24 ± ( 5R\HU 0 /KXLOOLHU 0 'KRPH DQG -0 /DYHVW ³0RQRFXODU YLVLRQ IRU PRELOH URERW ORFDOL]DWLRQ DQG DXWRQRPRXV QDYLJDWLRQ´ Int. J. Comput. Vis. 74 ± 6 6H ' * /RZH DQG - - /LWWOH ³9LVLRQEDVHG JOREDO ORFDOL]DWLRQ DQG PDSSLQJ IRU PRELOH URERWV´ IEEE Trans. Robot. 21 0 6ULGKDUDQ * .XKOPDQQ DQG 3 6WRQH ³3UDFWLFDO 9LVLRQ %DVHG 0RQWH &DUOR /RFDOL]DWLRQ RQ D /HJJHG 5RERW´ International Conference on Robotics and Automation
(ICRA’05) %DUFHORQD 6SDLQ YRO $SU ± SS 5 6WRUQ DQG . 3ULFH ³'LIIHUHQWLDO HYROXWLRQ ± D VLPSOH DQG HIILFLHQ +HXULVWLF IRU JOREDO RSWLPL]DWLRQ RYHU FRQWLQXRXV VSDFHV´ J. of Glob. Optim. 11 ± 'HF 6 7KUXQ ' +DKQHO ' )HUJXVRQ 0 0RQWHPHUOR 5 7ULHEHO : %XUJDUG & %DNHU = 2PRKXQGUR 6 7KD\HU DQG : :KLWWDNHU ³$ 6\VWHP IRU 9ROXPHWULF 5RERWLF 0DSSLQJ RI $EDQGRQHG 0LQHV´ Proceedings of the International Conference on Robotics and Automation (ICRA’03) 0D\ ± 5 7ULHEHO 3 3IDII DQG : %XUJDUG ³0XOWL/HYHO 6XUIDFH 0DSV IRU 2XWGRRU 7HUUDLQ 0DSSLQJ DQG /RRS &ORVLQJ´ Proceedings
KWWS MRXUQDOVFDPEULGJHRUJ
High-accuracy global localization filter for 3D environments of the International Conference on Intelligent Robots and Systems (IROS’06) %HLMLQJ &KLQD 2FW && 7VDL ++ /LQ DQG 6: /DL ³0XOWLVHQVRU ' SRVWXUH GHWHUPLQDWLRQ RI D PRELOH URERW XVLQJ LQHUWLDO DQG XOWUDVRQLF VHQVRUV´ J. Intell. Robot. Syst. 42 ± $ 5 9DKGDW 1 1 $VKUDIRGGLQ DQG 6 6 *KLGDU\ ³0RELOH 5RERW *OREDO /RFDOL]DWLRQ XVLQJ 'LIIHUHQWLDO (YROXWLRQ DQG 3DUWLFOH 6ZDUP 2SWLPL]DWLRQ´ Proceedings of the Congress on Evolutionary Computation (CEC’07) 6HS ± - :ROI : %XUJDUG DQG + %XUNKDUGW ³5REXVW YLVLRQ EDVHG ORFDOL]DWLRQ E\ FRPELQLQJ DQ LPDJH UHWULHYDO V\VWHP ZLWK PRQWH FDUOR ORFDOL]DWLRQ´ IEEE Trans. Robot. 21
