Indoor Simultaneous Localization And Mapping Based On FastSLAM

Transcription

1 Inernaional Core Journal of Engineering Vol.4 No ISSN: Indoor Simulaneous Localizaion And Mapping Based On FasSLAM Xingming Zhu 1, a, Hong Song 1, a, Xisong Gan 2, a 1Sichuan Universiy of Science &Engineering, Yibin , China 2Jiangling Holdings Limied Developmen Cener, China a @qq.com Absrac Aiming a he disadvanage of he radiional FasSLAM algorihm ha he proposed sampling disribuion only considers he robo moion model, a more confiden lidar observaion daa is inroduced ino he sampling disribuion of pose esimaion by daa fusion, which effecively reduces he cumulaive error of pose esimaion. Then an adapive resampling mehod is inroduced o reduce he calculaion. The SLAM precision is increased wih he number of paricles. The improved algorihm is esed by MATLAB simulaion. In addiion, a dynamic global pah planning soluion based on saic A * algorihm of ROS navigaion pacage and DWA algorihm is designed. Keywords RBPF; simulaneous localizaion and map building; pah planning. 1. Inroducion The research of mobile robo usually has o solve hree basic problems: firsly, he robo esimaes is posiion and pose; secondly, i expresses he environmen model by special represenaion mehod based on he unfamiliar scene informaion obained by he sensor; hirdly, i calculaes a safe driving pah from he saring poin o he arge poin independenly. Simple summary is: self-posiioning, map building and pah planning, SLAM echnology, ha is, simulaneous posiioning and map building. They are he basis for mobile robos o navigae in unnown siuaions and complee ass a he same ime. The descripion of he self-localizaion problem is o calculae he posiion coordinaes and orienaion angles of he robo sysem in he global map according o he sensor informaion. Map building refers o he inegraion of informaion acquired by sysem sensors ino a specified represenaion o complee he real environmen modeling, which includes various deecable obsacles in he environmen. Pah planning is o use a specific algorihm in he wor environmen o find a safe roue from he saring poin o he arge poin. The design crieria of pah planning algorihms are usually based on hree poins: he leas cos, he shores pah and he leas ime. In he research and applicaion of mobile service robos, he robo navigaion heory sysem based on prior environmenal informaion has been developed more maure. Bu for he dynamic environmen and unnown environmen, he echnology developmen of auonomous navigaion conrol is sill very young, and is sill in he exploraory experimenal sage. There is sill a long way o go o solve he problems of sysem modeling and pah planning under uncerain environmenal informaion. In his paper, he research on mobile service robo echnology is mainly based on he indoor environmen such as residenial and office, relying on he informaion obained by robo sensors o achieve posiioning, map consrucion, pah planning and oher navigaion wor. 100

2 Inernaional Core Journal of Engineering Vol.4 No ISSN: Improved FasSLAM algorihm based on RBPF Theoreical research shows ha he closer he proposed disribuion is o he arge disribuion, he beer he effec of paricle filer is. The proposed disribuion of paricle sampling based on Rao-Blacwellized paricle filer FasSLAM 1.0 is he moion model of he robo, wihou aing ino accoun he laes observaions, ha is, no roos. According o he laes observaion daa, he robo pose is updaed in real ime, so he deviaion beween he robo pose represened by each paricle and he acual robo pose is larger. When he noise error of he robo moion model is very large, many paricles sampled from he proposed disribuion will deviae from he acual posiion and aiude of he robo, which will lead o a large deviaion beween he final map and he acual environmen. If he arge disribuion can be sampled direcly, hen he bes filering effec can be obained. And here is no need for resampling seps. In pracice, i is ofen difficul o sample direcly from he arge disribuion. In order o mae he proposed disribuion closer o he arge disribuion, he proposed disribuion can be improved by inroducing he -momen observaion ino he proposed disribuion. When he robo's observaion sensor accuracy is higher han he robo's conrol accuracy, he sysem improvemen effec will be differen. I is ofen obvious[1]. The paricle represenaion of he proposed improved paricle filer SLAM algorihm is he same as ha of he Fas SLAM 1.0 algorihm based on paricle filer. The complee updae process is as follows: Pose sampling In he sampling sage, he laes sysem observaion informaion is fused ino he robo moion model, and he proposed disribuion is expressed as follows: z z x p( x x, u, z, c ) (1) 1: 1 1: 1: 1: The following improvemens are made o he proposed disribuion: p( x x, u, z, c ) 1: 1 1: 1: 1: p( z m, x, c ) p( m x, z, c ) dm p( x x, u ) c c 1: 1 1: 1 1: 1 c 1 The firs order Taylor expansion is used o approximae he observaion funcion H: c c, 1 m c c, 1 x h( m, x ) h(, x ) H ( m ) H ( x x) (3) The following parameers are expressed as follows: z h( c, 1, ) x x g( x 1, u) Hm m h( m, ) c c x H h( m, x ) x x, mc c, 1 x x c x x, mc c, 1 H m and H x are Jacobian marices wih respec o he observaion funcion h, and are he values of he derivaives of H wih respec o mc and a he expeced values of heir parameers. Under his approximaion, Gauss, which saisfies he expeced sampling and disribuion, can be obained: x ( H [ Q ] H [ R ] ) (5) T x x x T 1 x x x H [ Q ]( z z) x (6) m c, 1 m Q =H H + Q (7) (2) (4) 101

3 Inernaional Core Journal of Engineering Vol.4 No ISSN: Among hem, represens he covariance of he moion noise, and represens he covariance of he observed noise[2]. Feaure esimaion updae c sill represens he correlaion variable a ime, n represens he nh characerisic, and when c n indicaes ha he sysem has no observed he characerisic n a ime, here are: When c n R n, n, n, 1 n, Q (, ) (, ) (8), he sysem observed he characerisic n a ime and updaed he environmenal characerisics of he K paricle: p( m x, z, c ) p( z x, m, c ) p( m x, z, c ) (9) c 1: 1: c c 1: 1 1: 1 1: 1 The measuremen funcion H is firs-order Taylor expansion, and approximae expression is as follows: A his poin: c c, 1 m c c, 1 x h( m, x ) h(, x ) H ( m ) H ( x x ) p( x x, u, z, c ) 1: 1 1: 1: 1: m c c, 1 z H ( m ) x is a non-free variable, so he 1 1 exp ( z z Hm ( mc, 1 )) ( (, 1 )) c Q z z Hm mc c 2 1 ( 1, 1 )[, 1 ] ( mc c c mc c, 1 ) 2 (11) Feaure updaing is achieved by exending Calman filering: K c, m c, 1 (10) ( I K H ) (12) K H Q T 1 c, 1 ( ) m (13) K c, c, 1 K ( z z ) (14) The new paricles afer he esimaion of new feaures are added o he emporary paricle se. Resampling: To execue (1) (2) processes for all paricles, a emporary se of M paricles is obained. Deermining resampling imporance coefficien: w p( x z, u, c ) (,,, ) (,, ) 1: 1: 1: p x z1: u1: c1: x1: 1 p x1: 1 z 1: 1 u 1: 1 c 1: 1 p( x x, u ) p( z m, x, c ) (15) 1 c p( m x, z, c, u ) dm dx c 1: 1 1: 1 1: 1 1: 1 c is he normalizaion consan, and finally he imporance coefficien is deduced as follows: T 1 w 2 L exp ( z z ) [ L ] ( z z ), (16) 2 L H Q H H H R T T x T x m c, 1 m T By normalizing he imporance coefficien of resampling, and hen resampling, a new paricle se wih pose and map informaion can be obained.

4 Inernaional Core Journal of Engineering Vol.4 No ISSN: Adapive resampling For paricle filer, he weigh variance of paricles generaed by sequenial imporance sampling mehod will increase wih ime, causing paricle degradaion.. "Paricle degeneraion" refers o he increasing variance of he weighs of he paricle se afer many imes of mediaion, in which only a few paricles have larger weighs and mos of he oher paricles have smaller weighs. A his ime, a large number of calculaions are used o updae he nearly useless paricles wih small weighs. Theory has proved ha paricle degeneraion is unavoidable, which grealy wases compuaional resources. The resampling process effecively prevens he paricle degradaion process, and more high-weigh paricles are chosen o replace low-weigh paricles. However, frequen resampling will lead o he problem of paricle depleion, ha is, he paricles wih high weighs are replicaed many imes, hose wih low weighs are ignored, and he paricles are concenraed near a few poins wih large poserior probabiliy value, hus losing he diversiy of paricles[3]. Grisei also uses an adapive resampling algorihm o measure he degree of paricle degradaion by calculaing he effecive paricle number. The smaller he value, he greaer he variance of N eff he weigh of he paricle, he more serious he degree of paricle degradaion[4]. On he conrary, i shows ha he beer he paricle diversiy: he approximae expression of is as follows: In he formula, N represens he number of paricles: N eff N ( i) 2 ( w ) i1 N eff N eff 1 (17) i w is he normalized weigh of paricles. N eff N/2 is usually se o he ime sysem o re-sampling process. Through a large number of experimens, i is found ha his mehod can effecively reduce he ris of paricle degradaion. Because he number of resampling decreases, i is only execued when needed. By inroducing he adapive resampling mehod, he number of resampling and he complexiy of he algorihm can be effecively reduced, and he robusness of he algorihm can be improved. 4. Simulaion verificaion In order o verify he effeciveness of he proposed algorihm, he improved FasSLAM algorihm based on raser map is implemened on MATLAB simulaion plaform. Fig. 2 is a simulaed indoor environmen. The experimenal environmen is a labyrinh of approximaely 21 m * 23 m wih 10 rasers per meer. As shown in Figure 2, he robo complees boh posiioning and incremenal raser mapping in he environmen, where blac lines represen he inerior walls. Fig. 1 Simulaed experimenal environmen 103

5 Inernaional Core Journal of Engineering Vol.4 No ISSN: Figure 1 depics he incremenal creaion of raser maps on he iniial map during he mobile robo movemen. Fig. 2 Drawing process The purple-red diamond represens he robo, he dar blue represens he grid map creaed by laser scanning, he red represens he corridor boundary (obsacles), and he sy blue represens he boundary of laser scanning a his ime. In he experimen, he number of paricles is 30, and he number of effecive paricles is 15. This process compleely simulaes he consrucion of a mobile robo wih a laser rangefinder scanner in a real environmen.fig. 3 (a) and (b) represen robo pose errors in he radiional Fas SLAM algorihm based on raser maps and in he improved Fas SLAM algorihm based on raser maps, respecively. Fig. 3(a) Fig. 3(b) Fig. 3 Comparison of pose esimaion error 104

6 Inernaional Core Journal of Engineering Vol.4 No ISSN: From he Fig.3 (a), i can be seen ha he robo posiion error is approximaely beween 0-6 cm and he yaw angle error is beween rad. The robo posiioning effec is poor because he grid map precision is relaed o he robo posiioning. When he robo posiioning is no accurae, i will cause map errors. The difference is large. From he Fig.3 (b), i can be seen ha he robo posiion error is approximaely beween 0-2 cm, and he yaw angle error is approximaely beween rad. Compared wih he radiional algorihm, he robo posiioning effec is beer, so he esablished map is closer o Figure Robo indoor SLAM experimen In his experimen, he raspberry pie uses he Ubunu mae operaing sysem, and he ROS version used in he Ubunu mae is Kineic. In he experimen, wo compuers are used. The raspberry pie 3B is used as he airborne PC conrol robo. I is responsible for he implemenaion of SLAM algorihm and navigaion. The airborne PC communicaes wih X-2 experimenal plaform and laser radar hrough serial por and USB inerface. As a remoe conrol PC, noeboo compuer conrols and moniors he experimenal process of robo hrough virual machine and rviz visualizaion. The communicaion mechanism beween he robo plaform and he PC conrol plaform is as follows: Se up he VMware newor mode o bridge he WiFi newor card from windows. Ener he robo Ubunu sysem and access he same WiFi wih PC windows. In PC virual machine ubunu, wo compuers need o be configured in a ROS environmen o run ROS MASTER on he raspberry pie hos. Therefore, he environmen variables ROS_MASTER_URI in raspberry pie and virual machine are configured as hp:/(robo ip): 11311, and heir ROS_HOSTNAME is configured as heir respecive I. P. Using SSH service o realize remoe login raspberry pie 3B Ubunu sysem, you can wrie, compile and run ROS code hrough a noeboo. The real environmen seleced in his paper is 427 Laboraory of Arificial Inelligence Experimen Cener of Sichuan Universiy of Ligh Chemical Technology. The lengh and widh of he laboraory are 8 meers each. The lengh and widh of he corridor are 12 meers and 3 meers respecively. The oal area needed o be mapped is 100 square meers. There are wo poins o pay special aenion o before building a map. When he robo moves hrough he eyboard, i avoids he emergence of sudden sop and acceleraion o avoid collision beween he robo and obsacles, resuling in pose drif. Roae a cerain angle remoely a a complex posiion wih more feaure poins o obain a larger scanning angle of he lidar, so as o mae he mapping operaion more adequae and reduce he probabiliy of he area no being scanned in he map. Fig. 4 Posure change relaion of robos a adjacen ime 105

7 Inernaional Core Journal of Engineering Vol.4 No ISSN: Fig. 5 (a) Fig. 5 (b) As shown in Fig. 6, for he resul of he map creaion experimen using SLAM, he graph (a) is he resul of using only he robo moion model, and he graph (b) is he resul of inroducing high-precision observaional measuremen o correc he pose while considering he moion model in he proposed disribuion. Considering he cumulaive error caused by mechanical noise, when he mapping process is scanned o he repeiive area, he generaed map drifs obviously. When he new proposed disribuion is improved and repeaed experimens are carried ou under he same condiions, he resuls of he map consrucion are very clear and idy, and also ally wih he acual scene. In he FasSLAM algorihm based on paricle filer, robo localizaion is realized by paricle mainenance. During he SLAM process, he paricles represening he map are measured coninuously. The map displayed each ime is he highes weigh among all he paricles represening he map. Through he coninuous modificaion in he whole SLAM process, he final weigh is obained. The posiion and pose esimaion of he larges paricle complees he closed loop o realize he map consrucion, so he posiioning accuracy of he sysem direcly deermines he mapping consrucion effec. As a resul, he more paricles he sysem mainains, he higher he posiioning accuracy of he robo will be, and he map consrucion will be more accurae. Bu in pracice, he hardware condiions and he scene complexiy will affec he posiioning accuracy. The influence of excessive paricle number is ha he sysem calculaion load is oo high, which will lead o poor or even failure in he final mapping. Therefore, under differen condiions, we need o choose he mos appropriae paricle number range hrough experimens. Fig. 6 Differen paricle mapping experimens As shown above, he map is consruced when he number of paricles is 20, 40, 100, and 30 respecively. The green box in he map is he laboraory inerior, he red arrow is he des placed horizonally close o each oher, he blac do represens various small diameer obsacles such as chair legs, and he yellow arrow represens he wall, which is compared wih he real scene. I can be found ha when he number of paricles is 20, he dess placed side by side (a he red arrow) produce greaer separaion, and he map conour is somewha disored and unclear. When he number of paricles is 40 and 100, alhough he map sharpness is improved, here is sill a close des in he map separaion phenomenon, only when he number of paricles is 30, he consrucion of he des. The boundary of he map boundary is clear wihou scaering, and he des is close o he acual scene. 106

8 Inernaional Core Journal of Engineering Vol.4 No ISSN: References [1] Paine N, Mehling J S, Holley J, e al. Acuaor conrol for he NASA JSC Valyrie humanoid robo: A decoupled dynamics approach for orque conrol of series elasic robos[j]. Journal of Field Roboics, 2015, 32(3): [2] Choi T, Do H, Kim G, e al. An example of performing ar wih robo[c]//ubiquious Robos and Ambien Inelligence (URAI), h Inernaional Conference on. IEEE, 2016: [3] Subramanian M B, Sudhagar K, Rajarajeswari G. Design of navigaion conrol archiecure for an auonomous mobile robo agen[j]. Indian Journal of Science and Technology, 2016, 9(10). [4] Lin P, Abney K, Beey G A. Robo ehics: he ehical and social implicaions of roboics[m]. The MIT Press,