6h Inernaonal Conference on Elecronc, Mechancal, Informaon and Managemen (EMIM 206) Parcle Fler Based Robo Self-localzaon Usng RGBD Cues and Wheel Odomery Measuremens Enyang Gao, a*, Zhaohua Chen and Qzhuhu Gao School of nformaon and conrol engneerng, Shenyang Janzhu Unversy, Shenyang, Chna a arnold00@sna.com Keywords: Moble robo; Self-localzaon; RGBD; Wheel odomery; Parcle fler Absrac. Moble robo localzaon n he GPS dened envronmens s ncreasngly exerng fundamenal roles n a wde range of applcaons such as SFM and SLAM. However, he radonal sngle sensor based posonng mehods are eher unrelable or naccurae n he long erm. Ths paper presens a novel movng agen localzng approach ha combnes boh RGBD cues and wheel odomery measuremens whn he parcle fler based probablsc framework. Unlke he radonal RGBD localzaon mehods whch are compuaonally expensve and non-robus, we ook advanage of wheel odomery measuremens as he pror nformaon or say he nal values durng he RBGD pose opmzaon process. Addonally, he opmal pose derved from vsual sensor s, n urn, able o deermne he relably of he wheel odomery npus. Ths verfyng process s consderably useful n he presence of wheel slp. Expermenal resuls valdae ha our approach s effecve and relable n wheel robo localzaon. Inroducon Moble robo localzaon deermnes he process of he locaon n unknown envronmens and s he core oward he realzaon of auomac moble robo navgaon ably. In recen years, he use of parcle fler algorhm [, 2] has become a ho opc n he robo auonomous posonng. Wh respec o varous ypes of sensors, posonng mehods can be dvded no dscrepan caegores such as he vsual based, laser based, wheel based, ec. A presen, sensors, such as odomeer sensor, ulrasonc sensor, laser sensor and vsual sensor, are wdely used. Ulrasonc sensor and laser sensor due o her sngle sensng mode and long nducon perod have bascally been aken as auxlary posonng sensors; odomeer s prmarly desgned on he bass of he wheeled moble robo localzaon; esmaes he movng agen dsance process by means of ncremenally negrang he wheel encoder daa n some specal crcumsances (smooh ground, opography nequaly). However, he encoder values obaned are by and large naccurae, and he posonng errors could accumulae n he long run. In hs paper, he deph and vsual cues are adoped. Complemenary o RGB sensor whch can unquely provde he world pon color nformaon, deph sensor s able o sense he addonal deph nformaon. Combnng color and deph cues, he feaure pon exracon me s sgnfcanly reduced. Bu compared wh he exracon nformaon speed, odomeer s sll slghly fased. For beer use of he nformaon awareness by he sensor, he arge poson of he moble robo accuraely esmae under he ndoor envronmen, usng parcle flerng fuson odomeer and deph percepon of envronmenal nformaon by vsual sensor can realze he ndependen poson of moble robo, and s verfed by expermens[3, 4, 5]. Sensor Model Odomeer Model and Posonng Prncple. Ths paper chooses wo wheels dfferenal drvng wheeled moble robo. Odomeer sensor perodc read pulse number of phooelecrc encoder, whch s nsalled on he moor shaf. Accordng o each read pulse, can denfy he curren poson of robo hrough dsance and angle of he pulse [6, 7]. Odomeer can deec drvng wheel changed angle whn some me hrough he phooelecrc encoder be nsalled on he moor shaf. Assume ha he drvng wheel radus are r, he resoluon 206. The auhors - Publshed by Alans Press 523
of encoder s w lne, he reducon rao of deceleraon moor s sc, encoder oupu pulse n n me for he wheel movng dsance of ds : 2nr ds () w sc Assume ha he moble robo drvng wheel of lef and rgh movng dsance ds L and ds R n me respecvely. And he drvng wheel spacng dsance s l, hus robo walk he dsance and angle n me as shown below: dsl dsr s 2 dsr dw l L The moble robo s raecory calculaon formula: (2) n xn x0 s cos 0 n yn y0 s sn 0 n n 0 0 (3) The formula (3), s show he robo movng dsance from o me, show he robo change n drecon from o me, x, y, show he robo locaon n me samplng. Movng raecory s shown n Fg.. Fgure. Movng raecory Deph Vson Sensor Model. Ths paper use he deph vson sensor based on knec model. Deph vson sensor can sense abundan envronmenal nformaon n order o esmae precson of moble robo posonng [8]. Through deph vson sensor o oban cues ha wh deph nformaon. Namely he color mage and s correspondng he deph mage. Feaure Exracon and Processng Based on RGBD Fas Machng Algorhm Based on A Revsed Four-Pon Coplanar. The core dea of four-pon coplanar fas machng algorhm: he known 3D pon se P and offlne processng of he 3D pon se, hen o calculae he normal vecor of each pon n pon se P; he offlne daa conans he 3D 524
coordnaes ( x, y, z) of he each pon n he pon se P he deph nformaon d of each pon, and he normal vecor n of each pon. Then real-me access o a frame mage correspondng o he 3D pon se Q, he pon se Q wll be carred ou n accordance wh he pon se P. The machng sage: frsly, can be seleced ha he four-pon approxmae coplanar base se B randomly, hen can be fnd ha all he four pon se U of approxmae congruen wh he base se B n pon se Q, nex he rgd ransformaon T can be calculaed beween he se B and he se U. Then he rgd ransformaon T apply o he whole se P and look for he bes rgd T ha mee he hreshold condon. Fnally, he rgd ransformaon ransformaon T can descrbe he space poson relaon [9] beween wo 3D space pon se P and Q. Four-pon coplanar fas machng algorhm flow char s shown n Fg. 2. Four-pon coplanar fas machng algorhm 3D pon se P can be obaned hrough scan an obec anywhere based on knec 3D pon se P preprocessng(calculae he normal vecor and he deph) 3D pon se Q can be obaned hrough scan anoher frame based on knec(q wll be processed accordng he se P) Four-pon approxmae coplanar base se B can be seleced from pon se P randomly I can be fnd ha all he approxmae congruen wh four-pon se Calculae he rgd ransformaon T beween B and U The rgd ransformaon T apply o he se P and fnd he bes rgd ransformaon mee he hreshold condon T descrbe he relaonshp of he locaon of relave movemen beween P and Q Fgure 2. Four pon coplanar fas machng algorhm flow char RGBD Cues Processng. Vsual odomeer echnology based on knec sensor s he core o acheve posonng echnology. Flow char shown n Fg. 3, manly ncludes hree lnks: Image Feaure Exracon and Machng. Vsual odomeer echnology based on knec sensor s he mage wh deph nformaon s processed, n oher word, he hree-dmensonal mage correspondng 3D pon se can be feaure exracon and machng. Ths arcle uses he four-pon coplanar fas machng algorhm o make mage exracon and machng. Accordng he rules o exrac he feaure pon se from a frame mage, he nex sep s o mach he nex frame mage accordng exrac he feaure pons. Accuracy degree of feaure machng wll drecly affec accuracy of he camera self moon esmaon. Usng he leas square mehod as feaure pon s sandard of smlary. Specfc operaon mehod s calculae he rgd ransformaon hrough feaure pon se, and he 3D pon se apply he rgd ransformaon. Then usng he leas square mehod o measure smlary beween wo 3D pon se, hus ge he bes rgd ransformaon, so ha can be accuraely esmae ha he relaonshp of poson of he camera movemen. Prelmnary Moon Esmaon. The 3D coordnaes beween wo frames, hrough exracon and machng of feaure pon proec o he ground and pon ses n map and hen o ge he moon parameers n he camera self. Camera moon parameers can express as he rgd ransformaon (r, ), represen ranslaon marx, r represens he roaon marx. Then he relaonshp beween he feaure pon s coordnaes and he movemen parameers can be descrbed by formula: 525
Q rp (4) In formula (4) P and Q represen he 2D coordnaes of feaure pon se ha he curren frame and he nex frame mage respecvely. And r represens he roaon marx, represen ranslaon marx. Usng Four-pon coplanar fas machng algorhm o mage machng for moon parameers, n oher word, ha s he roaon and ranslaon marx beween wo frame poson relaons, and can be moon esmaon by roaon and ranslaon marx on he camera self. Read color mage wh deph nformaon based on knec RGB Deph mage Four-pon coplanar fas machng algorhm for mage feaure exracon and machng Prelmnary esmae of moon posure ICP algorhm for posure opmzaon Pose(x,y,z,roll,pch, yaw) Fgure 3. Flow char of vsual odomeer based on Knec Movemen Posure Opmzaon. Four-pon coplanar fas machng algorhm s adoped o esmaon nally can ge a coarse machng values, raher han he opmal poson esmaon, so he pose esmaon s no accurae us on he bass of rough machng[0]. Se a hreshold n vsual odomeer based on Knec sensor. Accordng o he acual machng accuracy o deermne wheher or no o use he ICP algorhm furher precse machng, hus realze movemen pose esmaon opmzaon. Parcle Fler Localzaon Algorhm Parcle fler posonng deas s a probablsc localzaon mehod based on Mone Carlo mehod, hrough he sensory nformaon from he sensors recursve esmae pose probably densy dsrbuon of he sae space and hen o realze posonng. The key hough of Parcle fler algorhm s o us a random sample of n wh weghs express he credbly Bel(l) of he pose of he robo self, and he sample space can express as S={ s =,2,,n}. The dscree sae sample collecon can represen he relably of he robo acual locaon. Each samplng consss of he robo s poson l = ( x, y, ) and he weghs p, denoed by s =( l, p ). Whch he probably of p sad robo s locaed n he locaon, and mee he condons of N p. Parcle fler posonng s wo processes ha based on boh he moon model s 526
updang and he model percepon updaes. Forecas Updae Based on he Odomeer. Ths paper chooses wo wheels dfferenal drvng wheeled moble robo, se momen robo posure l = ( x, y, ) n he coordnae sysem, l a he me +, hus o esablsh he moon model of he robo s as follow: x x cos 0 y y sn 0 u v (5) 0 T The formula (5), u ( s, ) s he model npu of odomeer, s, s he dsplacemen of he moble robo and he urnng angle n (, +) me. And v s he npu nose subec o Gaussan whe nose n he process of dsrbuon. Robo compleed he predcon process of parcle collecon by he moon model of he robo: q p( l l, u ) Bel ( l ) (6) The formula (6), p( l l, u ) s he moon model, q s he updaed samplng dsrbuon. The Updaed Percepon Based on he Deph Vsual. The parcle se can make weghs agan by percepon model n parcle fler mehod. The percepon daa s hrough exrac feaures of frame poson o gan feaure vecor n he locang mehod based on deph vsual. Machng success beween wo frames correspondng feaure pon se ha he same obec locaed n dfferen poson s he premse of solvng mage moon ransformaon model parameers. Assume ha he robo oban he acual observaon envronmen model s he mage I n he curren poson. Then he updaed samplng p v s wegh s: N ( zv lv ) d( I k, I )( D ds( I, I ))( (, )) (7) N The formula (7), he machng facor s p z l ), and represen he machng degree v ( v v beween he curren observaon model and afer he updaed samplng. z v show he acual observaon of he camera. d I k, I ) show he lne dsance beween samplng mages and he poson of samplng ( s. show prese maxmum relave angle., ) show he relave k ( angle beween he advanced samplng and samplng s. Percepon Updae of Fuson Heerogeneous Sensor Informaon. Moble robo can make he predcon and updae of parcle hrough he moon model, hen can updae parcle hrough probably model of deph vsual sensor[6]. The mporan facor of measure heerogeneous sensor nformaon s p ( z l ), and represens he machng degree beween he curren observaon and movemen updaed samplng, hus can more accurae and relable esmae he poson of moble robo n me. The Expermenal Resuls and Analyss Se up he Expermenal Plaform. An expermen was conduced o assess he accuracy of robo localzaon esmaon whn a ypcal envronmen n a lab room. The auhor se up he expermenal plaform as shown n Fg. 4, he compuer use Iner Core 7 four core and egh hread processor, 4GB memory, run on Lnux based on 64 b ubunu 4.04 sysem. 527
Fgure 4. The expermenal plaform Accuracy of he Traecory Esmaon. In hs secon, we wll presen our resuls on he accuracy of our robo sysem. The mehod s farly accurae and hey are show n Table. We ake 30 pons n he raecory and evaluae he accuracy of he raecory esmaon. The accuracy of he robo raecory esmaon s affeced by velocy. Wh he ncrease of he robo's movemen speed, he accuracy of raecory esmaon s reduced. Ths can be explaned as, wh he ncrease of he knec energy, he error of posonng esmaon by wheel has decreased. Table Accuracy evaluaon of raecory wh respec o velocy speed Translaon RMSE Roaon RMSE 0.05m/s 0.53m 4.23 0.5 m/s 0.9m 4.54 0.30m/s 0.2m 5.87 Concluson Ths paper proposes a parcle fler based robo self-localzaon mehod usng RGBD cues and wheel odomery measuremens. In an envronmen whch has been prevously explored, hs approach maches he pon cloud proec o he ground o he map. As a nex sep, we plan o add laser sensor o ge more accurae robo moon raecores. References [] Luo F, Du B, Fan Z. Moble robo localzaon based on parcle fler[c]//inellgen Conrol and Auomaon (WCICA), 204 h World Congress on. IEEE, 204: 3089-3093. [2] Wang Z, Tan J, Sun Z. Error Facor and Mahemacal Model of Posonng wh Odomeer Wheel [J]. Advances n Mechancal Engneerng, 205, 7(): 30598. [3] Tsa G J, Chang K W, Chu C H, e al. he Performance Analyss of AN Indoor Moble Mappng Sysem wh Rgb-D Sensor [J]. The Inernaonal Archves of Phoogrammery, Remoe Sensng and Spaal Informaon Scences, 205, 40(): 83. [4] Ager D, Mra N J, Cohen-Or D. 4-pons congruen ses for robus parwse surface regsraon[c]//acm Transacons on Graphcs (TOG). ACM, 2008, 27(3): 85. [5] Ch W, Zhang W, Gu J F, e al. A vson-based moble robo localzaon mehod [C]//Robocs and Bommecs (ROBIO), 203 IEEE Inernaonal Conference on. IEEE, 203: 2703-2708. 528
[6] Chen Z L. Deph Camera-Asssed Indoor Localzaon Enhancemen [J]. 203. [7] L J, Cheng C K, and Jang T Y. Wavele de-nosng of paral dscharge sgnals based on genec adapve hreshold esmaon [J]. IEEE Transacons on Delecrcs and Elecrcal Insulaon, 202, 9(2): 543-549. [8] Sefan W, Chen K W, Guo H B, e al. Wavele-based de-nosng of posron emsson omography scans [J]. Journal of Scenfc Compung, 202, 50(3): 665-677. [9] Zhu R., Zhou Z. A real-me arculaed human moon rackng usng r-axs neral/magnec sensors package [J]. IEEE Transacons on Neural Sysems and Rehablaon Engneerng, 2004, 2(2): 295-302. [0] Manouds N, Sahak T. Pxel-based and regon-based mage fuson schemes usng ICA bases [J]. Informaon Fuson, 2007, 8(2), 3-42. 529