2019 2nd World Conference on Mechanical Engineering and Intelligent Manufacturing (WCMEIM)

$*9ORFDOL]DWLRQEDVHGRQRGRPHWU\DQG/L'$5

6LER4XDQ -LDQZX&KHQ
6RXWK&KLQD8QLYHUVLW\RI7HFKQRORJ\ 6RXWK&KLQD8QLYHUVLW\RI7HFKQRORJ\
*XDQJ]KRX&KLQD *XDQJ]KRX&KLQD
HPDLOTXDQVLER#VFXWHGXFQ HPDLO#TTFRP

Abstract—/RFDOL]DWLRQ LV WKH NH\ WR $*9 DXWRQRPRXV RI WKH REVHUYHG OLNHOLKRRG IXQFWLRQ DQG WKH ILOWHU PD\ QRW
QDYLJDWLRQ7KHPRWLRQPRGHODQGWKHREVHUYDWLRQPRGHORIWKH FROOHFW HIIHFWLYH SDUWLFOHV >@ VR WKH DFFXUDF\ RI WKH
$*9 DUH HVWDEOLVKHG EDVHG RQ RGRPHWU\ DQG /L'$5 ORFDOL]DWLRQ RI WKH PRELOH URERW FDQQRW EH JXDUDQWHHG
UHVSHFWLYHO\ ,W LV SURSRVHG WKDW WKH HVWLPDWHG SRVLWLRQ RI WKH $OWKRXJKPDQ\LPSURYHG0&/DOJRULWKPV>@LPSURYHWKH
$*9LVREWDLQHGE\WKHRGRPHWU\WKHQWKHDFFXUDWHORFDWLRQLV DFFXUDF\ RI PRELOH URERW ORFDOL]DWLRQ WKH\ LQFUHDVH WKH
TXLFNO\ REWDLQHG E\ PDWFKLQJ REVHUYHG LQIRUPDWLRQ 7KH FRPSXWDWLRQDOFRPSOH[LW\DQGDUHQRWHDV\WRLPSOHPHQW,Q
FXPXODWLYH HUURU RI WKH PRWLRQ PRGHO DQG WKH PHDVXUHPHQW WKLVSDSHUWKHPRWLRQPRGHODQGREVHUYDWLRQPRGHORIIRXU
QRLVH RI WKH REVHUYDWLRQ PRGHO DUH FRQVLGHUHG WKH ODWWHU LV
ZKHHO $*9 DUH HVWDEOLVKHG %DVHG RQ WKH RGRPHWU\ DQG
VXEMHFWHGWRSUREDELOLW\ILOWHULQJWRFRUUHFWWKHRGRPHWU\LQWKH
/L'$5VFDQQLQJLQIRUPDWLRQUHDOWLPHORFDOL]DWLRQDQGJULG
6/$0SURFHVV$QLPSURYHG0RQWH&DUORORFDOL]DWLRQPHWKRG
LVSURSRVHGZKLFKLQFUHDVHVWKHVFUHHQLQJSDVVUDWHRIHIIHFWLYH
PDS FRQVWUXFWLRQ DUH UHDOL]HG $Q LPSURYHG DOJRULWKP LV
SDUWLFOHV LQ WKH LQLWLDO ORFDOL]DWLRQ SURFHVV DQG LPSURYHV WKH SURSRVHGIRUWKHSUREOHPRI0&/DOJRULWKPWRLQFUHDVHWKH
HIILFLHQF\DQGDFFXUDF\RIORFDOL]DWLRQ  ORFDOL]DWLRQVSHHG

Keywords: AGV; odometry; LiDAR; Monte Carlo algorithm;

,, 027,21$1'2%6(59$7,2102'(/62)$*9
gradational dilation map 7KH$*9PRWLRQSODWIRUPLQWKLVSDSHULVEDVHGRQ526
IRU QDYLJDWLRQ DQG FRQWURO 7KH FRPSXWDWLRQDO SURFHVV RI
, ,1752'8&7,21  SHUFHLYHG HQYLURQPHQW GDWD DQG QDYLJDWLRQ SODQQLQJ LV
:LWK WKH DGYDQFHPHQW RI PDQXIDFWXULQJ VWUDWHJLHV WKH KDQGOHGE\WKH$*9RQERDUGLQGXVWULDOFRPSXWHU,QWHUQDO
PDQXIDFWXULQJ PHWKRGV DUH JUDGXDOO\ GHYHORSLQJ WRZDUGV VHQVRUV HQFRGHUV DQG H[WHUQDO VHQVRUV /L'$5 DUH
FXVWRPHU FXVWRPL]DWLRQ DQG IOH[LEOH PDQXIDFWXULQJ 7KH HTXLSSHGRQWKH$*9ZKHHOVDQGSODWIRUPUHVSHFWLYHO\VR
SURGXFWLRQ OLQHV DQG ZDUHKRXVH LWHPV WHQG WR EH VPDOO DQG WKH$*9FROOHFWVWKHH[WHUQDOHQYLURQPHQWDOLQIRUPDWLRQDQG
YDULHG EDWFKHV ZKLFK UHTXLUHV FRQYH\RUV WR FRQYH\ JRRGV DWWKHVDPHWLPHPDNHVLWVRZQSRVHDQGPRWLRQHVWLPDWLRQ
IOH[LEO\DQGLQWHOOLJHQWO\IRUDGDSWLQJWRIUHTXHQWFKDQJHVRI SODQQLQJWKHRSWLPDOSDWKLGHQWLI\LQJDQGDYRLGLQJREVWDFOHV
PDQXIDFWXULQJHQYLURQPHQW>@$XWRPDWHG*XLGHG9HKLFOHV DVWRDXWRQRPRXVQDYLJDWLQJ 
$*9V ZKLFK DGDSWV WR WKLV QHHG KDYH EHFRPH WKH PDLQ 7KH$*9SRVHDQGPRWLRQHVWLPDWLRQUHOLHVRQDPRWLRQ
ORJLVWLFV HTXLSPHQW DQG WKHUH LV DQ LQFUHDVLQJ QXPEHU RI PRGHO LH D ZKHHOHG RGRPHWU\ GHULYHG IURP WKH LQYHUVH
$*9 DSSOLFDWLRQV ZLWK LQWHOOLJHQW DXWRQRPRXV QDYLJDWLRQ NLQHPDWLFVRIWKH$*97KHZKHHOVSHHGLVFDOFXODWHGIURP
WKDWGRQRWUHTXLUHPDQXDOLQWHUYHQWLRQDQGDQ\SUHVHWWUDFNV WKHSXOVHVLJQDOPHDVXUHGE\WKHHQFRGHU$FFRUGLQJWRWKH
RUVLJQV$XWRQRPRXVQDYLJDWLRQ$*9LVDQLQGRRUPRELOH
ZKHHOVSHHGDQGWKHWUDLQVWUXFWXUHWKHSDUWLDOYHORFLW\vxvy
URERW $FFXUDWH ORFDOL]DWLRQ LV WKH NH\ WR UHDOL]H URERWLF
LQWHOOLJHQWDXWRQRPRXVQDYLJDWLRQ7KHPDLQDSSURDFKLVUHDO RIWKH$*9DORQJWKHORQJLWXGLQDO;D[LVDQGWKHODWHUDO<
WLPH ORFDOL]DWLRQ DQG PDS FRQVWUXFWLRQ EDVHG RQ YDULRXV D[LVDQGWKHDQJXODUYHORFLW\ȦzDURXQGWKH=D[LVLQWKHZRUOG
PRWLRQ SHUFHSWLRQ DQG HQYLURQPHQWDZDUH VHQVRU GDWD 7KH FRRUGLQDWH V\VWHP DUH REWDLQHG WKURXJK WKH LQYHUVH
0RQWH&DUOR/RFDOL]DWLRQ0&/DOJRULWKP>@LVZLGHO\XVHG NLQHPDWLFVDQDO\VLVWKHUHODWLRQVKLSVEHWZHHQWKHURWDWLRQDO
LQ WKH UHVHDUFK RI PRELOH URERW VHOIORFDOL]DWLRQ WHFKQRORJ\ VSHHGV RI HDFK ZKHHO DQG WKH $*9 PRWLRQ SDUDPHWHUV DUH
EHFDXVHLWFDQHIIHFWLYHO\GHDOZLWKQRQOLQHDUSUREOHPVDQG GHULYHG VR WKH XQLTXH VROXWLRQ RI WKH PRWLRQ PRGHO LV
KDV WKH FKDUDFWHULVWLFV RI SDUDOOHOL]DWLRQ DQG HDV\ REWDLQHG%\LQWHJUDWLQJZLWKUHVSHFWWRWLPHZKHHORGRPHWU\
LPSOHPHQWDWLRQ +RZHYHU WKH WUDGLWLRQDO 0&/ DOJRULWKP GDWDLQFOXGLQJSODWIRUPSRVLWLRQxy GLUHFWLRQșDQGVSHHG
VXIIHUV IURP SUREOHPV VXFK DV SDUWLFOH GHJUDGDWLRQ DQG LVREWDLQHGRYHUWLPH$WDQ\WLPHWKHRGRPHWU\LQIRUPDWLRQ
SDUWLFOHGHSOHWLRQ2QWKHRQHKDQG0&/UHSODFHVVDPSOLQJ RIWKHIRXUZKHHO$*9SODWIRUPLQWKHFRRUGLQDWHV\VWHPLV
IURP WKH SRVWHULRU GLVWULEXWLRQ E\ VDPSOLQJ IURP WKH SULRU ‫ݔ‬ሺ௡ሻ ൌ ‫ݔ‬ሺ௡ିଵሻ ൅ ο‫ݔ‬ሺ௡ሻ ή ܿ‫ߠݏ݋‬ሺ௡ିଵሻ െ ο‫ݕ‬ሺ௡ሻ ή ‫ߠ݊݅ݏ‬ሺ௡ିଵሻ
GLVWULEXWLRQ DQG XVHV WKH REVHUYDWLRQ OLNHOLKRRG WR HVWLPDWH ቐ‫ݕ‬ሺ௡ሻ ൌ ‫ݕ‬ሺ௡ିଵሻ ൅ ο‫ݔ‬ሺ௡ሻ ή ‫ߠ݊݅ݏ‬ሺ௡ିଵሻ ൅ ο‫ݕ‬ሺ௡ሻ ή ܿ‫ߠݏ݋‬ሺ௡ିଵሻ   
WKH VDPSOLQJ LPSRUWDQFH 7KH FXUUHQW REVHUYDWLRQ ߠሺ௡ሻ ൌ ߠሺ௡ିଵሻ ൅ οߠሺ௡ሻ
LQIRUPDWLRQ LV QRW FRQVLGHUHG LQ SULRU SDUWLFOH SURSDJDWLRQ
DQGWKHSUHGLFWHGSDUWLFOHVHWPD\EHGLVWULEXWHGDWWKHWDLORI
WKHREVHUYHGOLNHOLKRRGIXQFWLRQUHVXOWLQJLQGHJUDGDWLRQRI
WKH SDUWLFOH VHW 2Q WKH RWKHU KDQG DFFXUDWH HQYLURQPHQWDO
SHUFHSWLRQVHQVRUVVXFKDV/L'$5PD\FDXVHWKHVKDUSVKDSH

978-1-7281-5045-1/19/$31.00 ©2019 IEEE 483

DOI 10.1109/WCMEIM48965.2019.00102

Authorized licensed use limited to: University of Wollongong. Downloaded on May 30,2020 at 16:41:23 UTC from IEEE Xplore. Restrictions apply.
 ο‫ݔ‬ሺ௡ሻ 2Q WKH RQH KDQG WKH $*9 SHUFHLYHV WKH HQYLURQPHQW
‫ݒۓ‬௫ሺ௡ሻ ൌ
ۖ ܶ DFTXLUHVPHDVXUHPHQWGDWDDQGPDWFKHVZLWKWKHEDFNJURXQG
ο‫ݕ‬ሺ௡ሻ UDVWHUPDSWRDFKLHYHORFDOL]DWLRQ2QWKHRWKHUKDQGWKHPDS
  ‫ݒ‬௬ሺ௡ሻ ൌ  
‫۔‬ ܶ LVXSGDWHGE\REVHUYDWLRQGDWDDQGWKHG\QDPLFREVWDFOHVQRW
ۖ οߠሺ௡ሻ LQ WKH RULJLQDO PDS DUH SHUFHLYHG DQG DGGHG WR WKH PDS
‫߱ە‬௭ሺ௡ሻ ൌ ܶ +RZHYHU LI WKH $*9 SRVLWLRQ LV FDOFXODWHG RQO\ E\ WKH
T LVWKHHQFRGHUPHDVXUHPHQWSHULRGLQIRUPXOD  REVHUYDWLRQPRGHOWKHDFWXDOSRVLWLRQLQDODUJHDQGFRPSOH[
7KH SHUFHSWLRQ RI WKH HQYLURQPHQW LV GHULYHG IURP WKH SURGXFWLRQ VFHQH LV D ORRNLQJ IRU D QHHGOH LQ WKH RFHDQ
REVHUYDWLRQDO PRGHOV /L'$5 LV D VHQVRU WKDW TXLFNO\ SURFHVV +HQFH LW LV SURSRVHG WKDW WKH $*9 HVWLPDWHG
PHDVXUHVWKHSURILOHGLVWDQFHRIREMHFWVLQDQHQYLURQPHQWLW SRVLWLRQLVREWDLQHGE\WKHRGRPHWU\RQWKLVEDVLVWKURXJK
LV PRXQWHGRQ WKH XSSHUSDUW RI WKH $*9SODWIRUP DQG LWV PDWFKLQJWKHREVHUYDWLRQLQIRUPDWLRQWKH$*9ORFDOL]DWLRQ
VFDQQLQJDQJOHUDQJHLV“GHJUHHVang_max aang_min LV REWDLQHG DQG WKH VSHHG DQG DFFXUDF\ RI $*9 UHDOWLPH
$*9 PRYHPHQW REWDLQV UDGDU VFDQ VHULHV GLVWDQFH GDWD ORFDOL]DWLRQ DQG PDS FRQVWUXFWLRQ 6/$0 LV JUHDWO\
ZKHUHWKH ‫݃ݎ‬଴ ǡ ‫݃ݎ‬ଵ ǡ ǥ ˈ‫݃ݎ‬௡ିଵ WKHkWKGDWDLVFRQYHUWHGLQWR LPSURYHG 
WKHUDGDUFRRUGLQDWHV\VWHP  ,QWKHPDSFRQVWUXFWLRQSURFHVVWKHQRLVHRIWKHOLGDUFDQ
̴ܽ݊݃݉ܽ‫ ݔ‬െ ̴ܽ݊݃݉݅݊ EH DSSUR[LPDWHG DV *DXVVLDQ QRLVH WKH SUREDELOLW\EDVHG
‫ݔ‬௅௞ ൌ ‫݃ݎ‬௞ ൈ …‘•ሾሺ݇ െ ͳሻ ൈ ൅ ̴݈ܽ݊݃݁݉݅݊ሿ PHWKRG FDQ EH XVHG DV WKH HIIHFWLYH ILOWHU WKHUH LV D
൞ ݊  
̴ܽ݊݃݉ܽ‫ ݔ‬െ ̴ܽ݊݃݉݅݊ FXPXODWLYH HUURU LQ WKH RGRPHWHU WKH RGRPHWHU FRUUHFWLRQ
‫ݕ‬௅௞ ൌ ‫݃ݎ‬௞ ൈ •‹ሾሺ݇ െ ͳሻ ൈ ൅ ݈ܽ݊݃݁ ̴݉݅݊ሿ
݊ QHHGVWREHSHUIRUPHGZKLOHFRQVWUXFWLQJWKHPDS
7KHWUDQVIRUPDWLRQPDWUL[ ௥௟ܶEHWZHHQ/L'$5FRRUGLQDWH
V\VWHPDQGWKH$*9FRRUGLQDWHV\VWHPLVGHWHUPLQHGE\WKH 7KH$*9SRVLWLRQFRQWDLQVWKUHHYDULDEOHV[\șDQG
LQVWDOODWLRQSRVLWLRQDQGDWWLWXGHRIWKH/L'$5RQWKH$*9 DUH DOO FRQWLQXRXV SDUDPHWHUV WKDW DUH GLIILFXOW WR VROYH
7KHYDOXHVLQWKHXQLILHGFRRUGLQDWHV\VWHPFDQEHREWDLQHG GLUHFWO\ E\ D FRPSXWDWLRQ IRUPXOD 7KH DERYHPHQWLRQHG
E\WKHPDWUL[WUDQVIRUPDWLRQ  PHWKRG RI XVLQJ REVHUYDWLRQDO PRGHO DQG PDS PDWFKLQJ WR
UHDOL]H DFFXUDWH $*9 SRVLWLRQ LV RQO\ RQH LGHD EHFDXVH LW
,,, *5,'0$3$1'$*9/2&$/,=$7,21 FDQQRW EH FDOFXODWHG GLUHFWO\ HYHQ LI WKH REVHUYDWLRQ GDWD
7KHPRWLRQPRGHODQGWKHREVHUYDWLRQPRGHOSURYLGHD GRHVQRWFRQWDLQQRLVH$QHIIHFWLYHVROXWLRQLVWRVDPSOHLQ
WKHRUHWLFDOEDVLVIRUWKHPRWLRQDQGSHUFHSWXDOFDOFXODWLRQRI D FRQWLQXRXV YLDEOH VSDFH E\ HYDOXDWLQJ WKH VDPSOHV
WKH$*9%XWWKHRGRPHWU\EDVHGRQWKHHQFRGHULQIRUPDWLRQ UHPRYLQJ WKH VDPSOHV ZLWK SRRU FRQIRUPLW\ DQG WKHQ
UHIOHFWVRQO\LGHDO$*9
VPRWLRQVWDWHEHFDXVHLWLVQRWIURP UHVDPSOLQJ FRQWLQXRXVO\ LWHUDWLRQ ORRSLQJ WR QDUURZ WKH
FORVHGORRSVHUYR,QIDFWWKHZKHHOVKDYHPDQXIDFWXUHDQG VHDUFK UHVXOWLQJ LQ WKH KLJKHVW PDWFKLQJ SRVLWLRQ HVWLPDWH
LQVWDOODWLRQ HUURU DQG WKH ORDG RI WKH $*9 DQG WKH JURXQG 7KLVLVWKHFRUHLGHDRI0RQWH&DUORORFDOL]DWLRQPHWKRG>@
FRQGLWLRQVGXULQJWKHUXQQLQJRI$*9ZLOOFDXVHWKHZKHHOV¶ ZKLFK LV HVVHQWLDOO\ D SDUWLFOH ILOWHU PHWKRG DQG LWV
GHIRUPDWLRQ DQG VOLGLQJ 7KXV FRPSDUHG ZLWK WKH DFWXDO DOJRULWKPLFSURFHVVLVVKRZQLQ)LJXUH
GLVSODFHPHQWRIWKH$*9WKHUHPXVWEHWKHRGRPHWU\HUURU ,IWKHSRVLWLRQDQGSRVHRIWKH$*9LVQRWNQRZQDWDOO
DQGZLWKWKHLQFUHDVHRI$*9UXQQLQJGLVWDQFHWKHHUURUZLOO WKDW LV ZLWKRXW DQ\ UHIHUHQFH WKH $*9 KDV WKH VDPH
EH DFFXPXODWHG PRUH DQG PRUH VHULRXV VR LW VKRXOG EH SUREDELOLW\LQDOOSRVVLEOHORFDWLRQVLHWKHJULGSRLQWVWKDW
HOLPLQDWHGLQWLPH  DUH QRW RFFXSLHG LQ WKH VSDFH :H VHW D VHULHV RI SDUWLFOHV
,I XVLQJ WKH REVHUYDWLRQ LQIRUPDWLRQ WR DFKLHYH WKH ZKLFK HYHQO\ GLVWULEXWH LQ WKH VSDFH DQG HDFK SDUWLFOH
ORFDOL]DWLRQ FRUUHFWLRQ LW LV QHFHVVDU\ WR FRPSDUH WKH UHSUHVHQWVWKHSRVVLEOHVWDWHSDUDPHWHUVRIWKHURERWVLPXODWH
REVHUYDWLRQLQIRUPDWLRQZLWKDNQRZQPRGHOZLWKFHUWDLQ WKH SDUWLFOH PRWLRQ DFFRUGLQJ WR WKH LQFUHPHQW RI WKH URERW
DFFXUDF\WKDWFDQPDWFKWKHREVHUYDWLRQGDWD,QWKLVDUWLFOHD PRWLRQREWDLQHGE\WKHRGRPHWU\WKHQFRPSDUHWKHREVHUYHG
PRVW HIILFLHQW ZD\  RFFXS\LQJ JULG PDS LV XVHG ZKLFK GDWDRIWKHURERWLQWKHSDUWLFOHVWDWHZLWKNQRZQHQYLURQPHQW
GLVFUHWL]HV WKH FRQWLQXRXV WZRGLPHQVLRQDO VSDFH LQWR JULG PDSDFFRUGLQJWRWKHPDWFKLQJUDWHEHWZHHQWKHERWKJLYH
SRLQWVRIDVSHFLILFUHVROXWLRQKHUHWKHJULGUHVROXWLRQLVWKH WKHSDUWLFOHDVFRUHWKDWUHSUHVHQWVWKHOLNHOLKRRGRIWKH$*9
VDPHDVWKHUDGDUVFDQUHVROXWLRQDQGWKHYDOXHRIHDFKSRLQW DWWKHORFDWLRQIXUWKHUE\ILOWHULQJRXWWKHSDUWLFOHVZLWKORZ
UHSUHVHQWV WKH SUREDELOLW\ WKDW WKH VSDFH LV RFFXSLHG 7KH VFRUHV DQG RQ WKH EDVLV RI WKH *DXVVLDQ SUREDELOLW\
HQYLURQPHQWWKUHHGLPHQVLRQDOSRLQWFORXGLVREWDLQHGIURP GLVWULEXWLRQ VRPH QHZ SDUWLFOHV DUH JHQHUDWHG DURXQG WKH
WKH /L'$5 VFDQQLQJ LQ ZKLFK WKH x y WZRGLPHQVLRQDO UHPDLQLQJ SDUWLFOHV 7KHQ WKH DERYH SURFHVV LV UHSHDWHG
LQIRUPDWLRQ LV XVHG IRU JURXQG QDYLJDWLRQ DQG ORFDOL]DWLRQ FRQWLQXRXVO\ DQG ILQDOO\ ZKHQ D FHUWDLQ SDUWLFOH PDWFKLQJ
DQGWKHWKLUGGLPHQVLRQDOHQYLURQPHQWLQIRUPDWLRQLVXVHGIRU UDWHH[FHHGVDVHWWKUHVKROGWKHPDWFKLQJHQGVDQGWKHUHVXOW
REVWDFOHDYRLGDQFHDWKHLJKWRQO\WKHWDNLQJWKHLQIRUPDWLRQ LVRXWSXW
EHORZ$*9DQGLWVORDGKHLJKWVRDVWRUHGXFHWKHFDOFXODWLRQ 7KRXJK WKH ZKHHO RGRPHWU\ REWDLQHG E\ WKH HQFRGHU
DPRXQW 7KH WKUHHGLPHQVLRQDO SRLQW FORXG LV WKUHVKROGHG PHQWLRQHG DERYH KDV DQ HUURU LW SURYLGHV WKH LPSRUWDQW
DQGWKHWKUHVKROGHGWKUHHGLPHQVLRQDOSRLQWFORXGLVPDSSHG LQIRUPDWLRQRIPRWLRQLQFUHPHQWIRU0RQWH&DUORORFDOL]DWLRQ
LQWRWKHFRUUHVSRQGLQJWZRGLPHQVLRQDOJULGPDS PHWKRG ZKLFK LV WKH PDLQ EDVLV IRU VLPXODWLQJ SDUWLFOH
PRWLRQEHFDXVHLWPDNHVWKHUHVDPSOLQJSURFHVVDGLUHFWLRQDO
H[SORUDWLRQ WKXV HIIHFWLYHO\ UHGXFHV WKH UDQJH RI SDUWLFOH

484

Authorized licensed use limited to: University of Wollongong. Downloaded on May 30,2020 at 16:41:23 UTC from IEEE Xplore. Restrictions apply.
VHDUFK7KHUHIRUHZLWKRGRPHWU\LQIRUPDWLRQLWLVSRVVLEOH DUHUHSOHQLVKHGLQWKHUHJLRQLWLVOLNHO\WKDWWKHWUXHJOREDO
WRUHGXFHWKHQXPEHURILWHUDWLRQVDQGLPSURYHWKHHIILFLHQF\ RSWLPDOSRVLWLRQLVLJQRUHG 
RI$*9ORFDOL]DWLRQZKLOHWKHRXWSXWVRIPDQ\PLVPDWFKHV
QRQUHDO SRVLWLRQV EXW ZLWK KLJK PDWFKLQJ UDWH LH ORFDO
RSWLPDOSDUWLFOHVDUHDYRLGHG


)LJXUH,PSURYHGHIIHFWLYHSDUWLFOHV

7KHUHIRUHDQLPSURYHGPHWKRGLVSURSRVHGDWWKHLQLWLDO
LWHUDWLRQRIWKHDOJRULWKPWKHUHLVDORZRYHUDOOPDWFKLQJUDWH
PDNLQJ DQ DSSURSULDWH GLODWLRQ SURFHVVLQJ RI WKH PDS FDQ
ODUJHO\DYRLGWKHSUREOHPRIIDOVHHOLPLQDWLRQFDXVHGE\VPDOO
GHYLDWLRQRISDUWLFOHSRVLWLRQ2QWKHRWKHUKDQGWKHGLODWLRQ
PDSLPSURYHVWKHRYHUDOOPDWFKLQJRIWKHVDPSOHSDUWLFOHV
DOVR LQFOXGHV WKH ORFDOO\ RSWLPDO EXW QRQJOREDO RSWLPDO
SDUWLFOHVVRWKHGLODWLRQGHJUHHRIWKHPDWFKLQJPDSVKRXOG
EHJUDGXDOO\UHGXFHGDVWKHPDWFKLQJUDWHLQFUHDVHV,QWKLV
ZD\ZKLOHUHWDLQLQJWKHHIIHFWLYHSDUWLFOHVLQWKHHDUO\VWDJH
D VHULHV RI QRQJOREDO RSWLPDO SDUWLFOHV FDQ EH JUDGXDOO\
HOLPLQDWHG LQ WKH ODWHU VWDJH ILQDOO\ WKH JOREDO RSWLPDO
HVWLPDWHRIWKHUHDOSRVLWLRQLVREWDLQHG7KHVWHSVDUH
 %HIRUHWKHLQLWLDOORFDOL]DWLRQJHQHUDWLQJGLODWLRQPDS
)LJXUH0RQWH&DUOR/RFDOL]DWLRQ0HWKRG ݉ܽ‫݌‬଴ ǃ ݉ܽ‫݌‬ଵ ǃ ݉ܽ‫݌‬ଶ « ݉ܽ‫݌‬௡ ZLWK GLODWLRQ FRHIILFLHQW
RI ݇଴ǡ ݇ଵǡǥǥǡ ݇୬ …‘””‡•’‘†‹‰Ž› ZKHUH ݇଴ ൌ ͳ  PHDQV
,9 ,03529('0217(//$52/2&$/,=$7,21$/*25,7+0
QRQGLODWLRQSURFHVVLQJ ݇ଵ ൏ ݇ଶ ൏«൏ ݇௡ DQGVHWWLQJWKH
8VLQJ0RQWH&DUORORFDOL]DWLRQPHWKRGWRDFKLHYH$*9 FRUUHVSRQGLQJPDWFKLQJWKUHVKROGV ‫ݍ‬ଵ ǡ ‫ݍ‬ଶ ǡ«‫ݍ‬௡ ˗ 
ORFDOL]DWLRQFDQEHGLYLGHGLQWRWZRFDVHV
 6DPSOLQJ LQ D IHDVLEOH VSDFH WR REWDLQ D VDPSOH RI
A. Initial localization SDUWLFOHVZLWKVXIILFLHQWGHQVLW\
:KHQWKH$*9LVMXVWVWDUWHGWKHRGRPHWU\LV%XWWKH
6LPXODWLQJWKHSDUWLFOHPRWLRQDQGPDWFKLQJREVHUYLQJ
$*9LQLWLDOL]DWLRQSRVLWLRQLVQRWQHFHVVDULO\WKHFRRUGLQDWH
PRGHOZLWK ݉ܽ‫݌‬௡  WRFDOFXODWHWKHSDUWLFOHVFRUHV
RULJLQRIJULGPDSWKXVWKHVWDUWXSSURFHVVUHTXLUHVXQLIRUP
DQG ZLGHO\ GLVWULEXWHG SDUWLFOH VDPSOHV WR HQVXUH WKDW WKH (OLPLQDWLQJWKHSDUWLFOHVZLWKDVFRUHOHVVWKDQ ‫ݍ‬௡ Ǣ LI
IHDVLEOHSRVLWLRQVLQWKHPDSDUHDOOFRYHUHGLQWKHSDUWLFOHV WKH PHDQ VFRUH RI WKH UHVLGXDO SDUWLFOHV LV EHORZ WKH VHW
DQGE\LQFUHDVLQJWKHQXPEHURISDUWLFOHVWRHQVXUHVXIILFLHQW WKUHVKROG WKH PDS UHPDLQV XQFKDQJHG WKHQ UHVDPSOLQJ
GHQVLW\ WKHQ LWHUDWLYHO\ LWHUDWLQJ XQWLO WKH JOREDO RSWLPDO UHSHDW VWHS  RU HOVH WDNLQJ ݉ܽ‫݌‬௡ିଵ DV WKH PDWFKLQJ PDS
SDUWLFOHLVREWDLQHG  WKHQUHVDPSOLQJDQGUHSHDWLQJVWHS
B. Odometry correction in motion process  :KHQ WKH ILQDO UHVDPSOHG SDUWLFOHV PDWFKLQJ ݉ܽ‫݌‬଴ 
$IWHU WKH LQLWLDO ORFDOL]DWLRQ WKH IROORZXS PRYHPHQW DQGHOLPLQDWLQJLQYDOLGSDUWLFOHVWKHPHDQVFRUHRIUHPDLQLQJ
SURFHVV RI $*9 WKDW KDV EHHQ DFFXUDWHO\ SRVLWLRQHG LV SDUWLFOHVLVQRWOHVVWKDQ‫ݍ‬଴ HQGLQJWKHFDOFXODWLRQDQGRXWSXW
JHQHUDOO\FRQWLQXRXV(YHQLIWKHRGRPHWU\KDVDQHUURUWKH WKHRSWLPDOSDUWLFOHSRVLWLRQ
HUURU ZLOO QRW EH ODUJH LQ D VKRUW PHDVXUHPHQW SHULRG 7RVLPSOLI\WKHFDOFXODWLRQWKH ‫ݍ‬଴ ǡ ‫ݍ‬ଵ ǡ ǥ ǡ ‫ݍ‬௡  DERYHFDQ
7KHUHIRUHDWWKDWWLPH0RQWH&DUORPHWKRGLVPDLQO\XVHGWR EHWDNHQDVHTXDOYDOXHV
FRUUHFW WKH RGRPHWU\ ORFDOL]DWLRQ WKXV WKH SDUWLFOH
GLVWULEXWLRQ UDQJH GRHV QRW QHHG WR EH YHU\ ODUJH DQG WKH 7KHSVHXGRFRGHRILPSURYHGORFDOL]DWLRQDOJRULWKPLVDV
SDUWLFOHQXPEHUFDQEHDSSURSULDWHO\UHGXFHG IROORZV
7KHUHLVDSUREOHPLQLQLWLDOL]LQJORFDOL]DWLRQSURFHVVRI &DOFXODWLQJLQLWLDOL]DWLRQ
WKH H[LVWLQJ 0RQWH &DUOR ORFDOL]DWLRQ DOJRULWKP WKDW LV QRW 0DSBH[SHQG 0DSB,QLWPDSBUDZ*HQHUDWLQJD
FRQGXFLYHWRIDVWDSSURDFKLQJWKHUHDOSRVLWLRQ$VVKRZQLQ JUDGLHQWGLODWLRQPDS
)LJZKHQDQHIIHFWLYHSDUWLFOHLVFORVHWRWKHUHDOSRVLWLRQ &XUUHQWBSDUWLFDO 3DUWLFOHB,QLWLDOPDSBUDZHYHQO\
WKHPDWFKLQJUDWHPD\EHYHU\ORZGXHWRWKHVOLJKWSRVLWLRQ LQLWLDOL]LQJWKHSDUWLFOH
RU SRVWXUH GHYLDWLRQ WKHQ LV HOLPLQDWHG LQ WKH VFRULQJ
6HOHFWLQJWKHJUDGLHQWGLODWLRQPDSWRPDWFK
VFUHHQLQJ SURFHVV ,I LQ VXEVHTXHQW UHVDPSOLQJ QR SDUWLFOHV
)RUPDSLQPDSBH[SHQG

485

Authorized licensed use limited to: University of Wollongong. Downloaded on May 30,2020 at 16:41:23 UTC from IEEE Xplore. Restrictions apply.
 :KLOHDJYHUDJHBYDOXH” WKUHVKROGLWHUDWLYH PDS LV UHGXFHG DQG PDQ\ PLVPDWFKHG SDUWLFOHV DQG ORFDO
FDOFXODWLQJXQWLOWKHPDWFKGHJUHHLVJUHDWHUWKDQWKH RSWLPDOSDUWLFOHVDUHTXLFNO\HOLPLQDWHGVRWKDWWKHUHVDPSOHG
WKUHVKROG SDUWLFOHVFDQEHJHQHUDWHGPRUHDURXQGWKHHIIHFWLYHSDUWLFOHV
/RRSH[HFXWLQJ0RQWH&DUORORFDOL]DWLRQDOJRULWKP WKHHIIHFWLYHSDUWLFOHUDWHLQFUHDVHVUDSLGO\IURPVWHSWRVWHS
&XUUHQWBSDUWLFDO 3DUWLFOHB0RWLRQFXUUHQWBSDUWLFDO WKHHIIHFWLYHSDUWLFOHUDWHLQFUHDVHGE\DPLQLPXPRI
WKHPD[LPXPRIDQGHYHQWXDOO\VWDELOL]HGQHDUWKH
RGRPBLQFUH
JOREDORSWLPDOSDUWLFOH 
0DWFKBYDOXH 
3DUWLFOH9DOXHB&DOFXODWLRQFXUUHQWBSDUWLFDOVFDQ 9 &21&/86,216
&XUUHQWBSDUWLFDO 3DUWLFOHB)LOWHUFXUUHQWBSDUWLFDO %DVHG RQ WKH ZKHHO RGRPHWU\ WR HVWDEOLVK WKH PRWLRQ
PDWFKBYDOXHWKUHVKROG PRGHORI$*9DQGEDVHGRQWKH/L'$5WRHVWDEOLVKWKH$*9
$JYHUDJHBYDOXH  REVHUYDWLRQ PRGHO WKH HVWLPDWHG SRVLWLRQ RI WKH $*9 LV
$YHUDJH9DOXHB&DOFXODWHFXUUHQWBSDUWLFDOPDSVFDQ REWDLQHG IURP WKH RGRPHWU\ 2Q WKLV EDVLV DFFXUDWH $*9
2XWSXWUHVXOWV ORFDOL]DWLRQFDQEHREWDLQHGWKURXJKREVHUYDWLRQLQIRUPDWLRQ
3RVHBRIBURERW %HVW3DUWLFOHB)LQGFXUUHQWBSDUWLFDO PDWFKLQJZKLFKFDQDYRLGWKHQHHGOHLQDKD\VWDFNSURFHVV
 RIFDOFXODWLQJ$*9SRVLWLRQRQO\WKURXJKREVHUYDWLRQPRGHO
,QRUGHUWRHYDOXDWHWKHH[HFXWLRQHIILFLHQF\RIWKHLQLWLDO $OWKRXJK WKH RGRPHWU\ REWDLQHG IURP WKH HQFRGHUV KDV
ORFDOL]DWLRQ DOJRULWKP D P P PDS LV GHVLJQHG LQ WKH VRPH HUURU LW SURYLGHV WKH LPSRUWDQW LQIRUPDWLRQ RI
YLUWXDOHQYLURQPHQWIRUDOJRULWKPWHVWLQJ6HWWKHQXPEHURI PRYHPHQWLQFUHPHQWIRUWKH0RQWH&DUORORFDOL]DWLRQPHWKRG
SDUWLFOHVLQWKHLQLWLDOL]DWLRQDQGDGRSW$GDSWLYH0RQWH ZKLFKPDNHVWKHUHVDPSOLQJSURFHVVDGLUHFWLRQDOH[SORUDWLRQ
&DUOR /RFDOL]DWLRQ $0&/ WR UHVDPSOH ZKLFK PDNHV WKH HIIHFWLYHO\QDUURZLQJWKHSDUWLFOHVHDUFKUDQJHDQGLPSURYLQJ
QXPEHU RI VDPSOH SDUWLFOHV DXWRPDWLF DGMXVWPHQW 7KH WKH$*9ORFDOL]DWLRQHIILFLHQF\DQGDYRLGLQJPLVPDWFKLQJ
SDUWLFOHV ZLWKLQ WKH UHFWDQJXODU UDQJH RI “P LQ WKH DFWXDO
SRVLWLRQRIWKHYLUWXDOURERWDUHGHILQHGDVHIIHFWLYHSDUWLFOHV 7KH LPSURYHG PHWKRG RI JUDGDWLRQDO GLODWLRQ PDS
DQG WKH SDUWLFOH VWDWH FKDQJHV EHIRUH DQG DIWHU WKH SURSRVHG LQ WKH SDSHU FDQ RYHUFRPH WKH SUREOHP WKDW WKH
LPSURYHPHQW DUH UHFRUGHG 7KH VWDWH RI WKH SDUWLFOHV LQ WKH 0RQWH &DUOR SDUWLFOH ILOWHU ORFDOL]DWLRQ DOJRULWKP LV QRW
LWHUDWLYHSURFHVVLVVKRZQLQ7DEOH FRQGXFLYHWRIDVWDSSUR[LPDWLQJWKHUHDOSRVLWLRQGXULQJWKH
LQLWLDO ORFDOL]DWLRQ ,Q WKH LQLWLDO LWHUDWLRQ ZKHQ WKH RYHUDOO
7$%/(  ,03529('67$7,67,&$/7$%/(  PDWFKLQJUDWHLVORZWKHLPSURYHGPHWKRGFDQODUJHO\DYRLG
3UHLPSURYHPHQW0RQWH ,PSURYHG0RQWH&DUOR
IDOVH HOLPLQDWLRQ ZKLFK LV FDXVHG JHQHUDOO\ E\ WKH VPDOO
1XPEHU &DUORORFDOL]DWLRQDOJRULWKP ORFDOL]DWLRQDOJRULWKP SRVLWLRQDO GHYLDWLRQ RI WKH SDUWLFOHV DQG PDNHV WKH VDPSOH
RI 1XPEHURI
3URSRUWLRQ
1XPEHURI
3URSRUWLRQ SDUWLFOHV WR LPSURYH RYHUDOO PDWFKLQJ UDWH ODWHU ZKLOH
LWHUDWLRQV HIIHFWLYH HIIHFWLYH
SDUWLFOHV

SDUWLFOHV
 UHWDLQLQJHIIHFWLYHSDUWLFOHVDVHULHVRIQRQJOREDOO\RSWLPDO
     SDUWLFOHV DUH JUDGXDOO\ HOLPLQDWHG DQG WKH JOREDO RSWLPDO
     HVWLPDWLRQRIWKHUHDOSRVLWLRQLVREWDLQHGTXLFNO\
    
5()(5(1&(6
    
>@$<DGDY6&-D\VZDO³0RGHOOLQJRIIOH[LEOHPDQXIDFWXULQJV\VWHPD
     UHYLHZ´,QW-3URG5HVYRO瀃瀃濁 
     >@ ) 'HOODHUW ' )R[ : %XUJDUG HW DO ³0RQWH FDUOR ORFDOL]DWLRQ IRU
     PRELOHURERWV´,&5$SS
     >@<6RQJ)&6XQ4濟澳濟濼澿³7KHLPSURYHGWUDFHOHVVSDUWLFOHILOWHULQJRIWKH
     XQWUDFHDEOHSDUWLFOHLQWKH0RQWH&DUORORFDOL]DWLRQDOJRULWKP´$FWD
$XWRPDW6LQLFDYROSSLQ&KLQHVH
    
>@/=KDQJ5=DSDWD3/HSLQD\³6HOIDGDSWLYH0RQWH&DUORORFDOL]DWLRQ
    
IRUPRELOHURERWVXVLQJUDQJHILQGHUV´5RERWLFDYROSS
     
     >@&+&KLHQ:<:DQJ--RHWDO³(QKDQFHG0RQWH&DUORORFDOL]DWLRQ
LQFRUSRUDWLQJ D PHFKDQLVP IRU SUHYHQWLQJ SUHPDWXUH FRQYHUJHQFH´
澳 5RERWLFDYROSS
,WFDQEHVHHQIURPWKHGDWDLQWKHWDEOHWKDWWKHLPSURYHG >@ : ;LDR\X / , &DLKRQJ 6 /L HW DO ³2Q $GDSWLYH 0RQWH &DUOR
ORFDOL]DWLRQDOJRULWKPUHWDLQVPRUHHIIHFWLYHSDUWLFOHVGXHWR /RFDOL]DWLRQ $OJRULWKP IRU WKH 0RELOH5RERW %DVHG RQ 526´ WK
WKHGLODWLRQRIWKHPDSLQWKHHDUO\VWDJHEXWDOVRUHWDLQVPDQ\ &KLQHVH&RQWURO&RQI,(((SS
PLVPDWFKHGSDUWLFOHVDWWKHVDPHWLPHVRWKHSURSRUWLRQRI >@ ) 3 &DR 4 < )DQ ³5HVHDUFK RQ 5HDOWLPH 3RVLWLRQLQJ %DVHG RQ
HIIHFWLYH SDUWLFOHV LV QRW KLJK +RZHYHU DV WKH LWHUDWLRQ $GDSWLYH0RQWH&DUOR$OJRULWKP´&RPS(QJYRO1R
SURJUHVVHV DQG WKH GHJUHH RI PDS GLODWLRQ GHFUHDVHV WKH SSLQ&KLQHVH
VXEVHTXHQW VFUHHQLQJ HIILFLHQF\ LV VLJQLILFDQWO\ LQFUHDVHG >@$<LOPD]+7HPHOWDV³6HOIDGDSWLYH0RQWH&DUORPHWKRGIRULQGRRU
ORFDOL]DWLRQRIVPDUW$*9VXVLQJ/,'$5GDWD´5RERW$XWRQ6\VW
ZKHQPRUHHIIHFWLYHSDUWLFOHVDUHUHWDLQHG$WWKHEHJLQQLQJ 
RI WKH ILIWK LWHUDWLRQ WKH GHJUHH RI GLODWLRQRI WKH PDWFKLQJ
澳

486

Authorized licensed use limited to: University of Wollongong. Downloaded on May 30,2020 at 16:41:23 UTC from IEEE Xplore. Restrictions apply.