Research on PF-SLAM Indoor Pedestrian Localization Algorithm Based on Feature Point Map

Save this PDF as:

Size: px
Start display at page:

Download "Research on PF-SLAM Indoor Pedestrian Localization Algorithm Based on Feature Point Map"

Transcription

6 Micromaches 218, 9, In order to extract straight-le walkg phase with better learity from position data correspondg to trajectory derived from dead-reckong, a feature pot Micromaches 218, 9, x FOR PEER REVIEW 6 19 extraction method based on position formation was proposed. In experiment, mean all samplg pots 1 s was taken accordg to Equation (2). On one h, it avoids error some f f samplg pots avoids impact feature detection; on or h, difference adjacent data is creased, which is easy Xi Y to observe 1be1i furr treated. The samplg frequency i= i= (2) was set to 1 Hz this article s experiments. X m = () t, Ym = () t f f In formula, X m is mean value X abscissa i Y all samplg pots 1 s at time t. i i=1 i=1 X i is abscissa all samplg X m pots = (t), 1 s. Y m Y = (t) (2) f m is mean value ordate all f samplg pots 1 s at time t. Y i is abscissa all samplg pots 1 s. f is samplg In frequency. formula, X m is mean value abscissa all samplg pots 1 s at time t. X i is abscissa Afterwards, all samplg adjacent pots average 1 s. samplg Y m is mean pot value is calculated ordate accordg all to samplg Equation pots (3) to obta 1 s at time t. connection Y i is abscissa angle, all samplg Figure 4a pots is obtaed. 1 s. f is After samplg data frequency. is smood to obta Figure Afterwards, 4b, adjacent has average a static samplg terval pot before is startg calculated to accordg walk to measure to Equation (3) drift to obta gyro s connection constant value. angle, Therefore, Figure 4a first is obtaed. segment After figure data is is drift smood measurement to obta phase Figure 4b, is not cluded has a calculation static terval as shown before by startg part to walk side to measure blue rectangle drift Figure gyro s 4b. It constant can be clearly value. Therefore, seen from first figure segment that four-segment figure is straight-le drift measurement walkg phase, phase order is not to cluded obta a better fittg calculation effect, as took shown out by mid-section part side data blue rectangle straight-le Figure 4b. It walkg can be clearly phase seen performed from figure straight-le that fittg four-segment as shown straight-le by part walkg side phase, yellow rectangle. order to obta a better fittg effect, took out mid-section data straight-le walkg phase performed 1i i straight-le fittg as shown by part side yellow rectangle. i Ym Y m θ =arctan 1 ( i i ) θ i = X Y mi arctan Y X m i 1 m (3) i Xm X i m i 1 (3) -π < θ π π < θ i π 1In formula, ( i i i i 1In formula, ( X Xm, i m, Y Ym) i m ( ) X i 1 ( Xm, Y m, Ym i 1 ) m are ) are coordates adjacent two mean coordates adjacent two mean samplg pots; θ i i samplg pots; is connectg θ is angle connectg angle adjacent mean adjacent samplg mean pots. samplg pots. f f Angle[ ] Angle[ ] Measurg pots (a) Measurg pots (b) Figure Figure (a) (a) Unsmood Unsmoodangle angleformation (b) (b) smood smood angle angle formation. formation. Use first-order le fittg straight-le walkg phase extracted by Equation (4) Use first-order le fittg straight-le walkg phase extracted by Equation (4) obta fitted straight le: obta fitted straight le: n ˆb = XY ˆ i i i nxy = n i=1 X iy i nxy i=1 b = 22n 1 1n X2 i nx2 â = YX i i nx = ˆbX (4) p = â + ˆbq (4) aˆ = Y bx ˆ p= aˆ + bq ˆ

7 Micromaches 218, 9, x FOR PEER REVIEW 7 19 where (, ) X Y are coordates mean position pot for extracted fittg part; Micromachesi218, i 9, (, ) X Y is mean coordate all samplg pots representg fitable parts extracted; where (X i, Y i ) are coordates mean position pot for extracted fittg part; ( X, Y ) is n is mean total coordate number all samplg samplg pots pots representg fitable part fitable extracted. ˆb parts extracted; is coefficient n is total number one-time term samplg for pots fitted straight fitable le. part â extracted. is coefficient ˆb is coefficient lear constant one-time term; term for p is fitted straight-le le. expression. â is coefficient lear constant term; p is straight-le expression. The feature pots extraction method based on location formation is is shown Figure. The blue le segment is straight le fitted by s straight-le walkg phase. The fittg adjacent straight-le extension le to pot is is one required for this study. The feature pots are shown by red circle figure. In order to obta more accurate feature pots, it it was necessary to to design design experiment to to take take multiple multiple values values for for same same feature feature pot pot obta obta average average value value as as fal feature fal feature pot data pot data order to order construct to construct a more a accurate more accurate feature pot feature map. pot map. Y Position/m -4-6 Startg pot Feature pots Pedestrian true track Fittg le Extendg le Figure. Feature pots extraction based on location formation Build Ma Path Feature Pot Map Most terior buildgs are made up corridors rooms. In this paper, we call corridor paths ma ma door door paths, paths, which which reflects reflects general general situation situation terior terior a buildg. a The buildg. feature The pots feature on pots ma on path ma are easier path are to observe easier to observe extract. The extract. variability The variability layout layout room is too room largeis too feature large pot feature extraction pot extraction is difficult. is Therefore, difficult. Therefore, feature pot feature extraction pot extraction construction construction updatg updatg feature pot feature map proposed pot map proposed this paper were this aimed paper at were aimed at turng potturng that occurs pot onthat occurs ma path. on ma path. Based on short-time high precision ertial navigation system that has advantages autonomy to design experiments, that is, s have autonomous obstacle avoidance, autonomously select roads that y can travel, can dependently design route-walkg characteristics. Each time experimenter started from same startg pot walked along ma path door buildg, experiment showed that shorter walkg time, time, fewer fewer number number turns, turns, higher higher precision precision INS output INS trajectory. output The trajectory. trajectory The trajectory same color same Figure color 6 was Figure an experimental 6 was an path, experimental feature path, pots were feature extracted pots accordg were extracted to a feature accordg pot to extraction a feature pot method extraction based on method position based formation on position formation are represented are by circles represented by same circles color. When same designg color. When experimental designg path, experimental it was ensured path, that it was tersection ensured that each tersection path passed ateach leastpath twice, passed fally at least twice, average value fally was taken average as value required was feature taken as pot. As required each path feature started pot. from As each same path startg started pot, from suchsame as startg purplepot, dots such Figure as 6, purple paths dots were put Figure toger 6, paths combed were put with toger extracted combed feature pots with to build extracted a feature feature pot pots map that to build coulda reflect feature pot ma map path that could buildg. reflect ma path buildg.

8 Micromaches 218, 9, Micromaches 218, 9, FOR PEER REVIEW Micromaches 218, Micromaches 9, x FOR PEER 218, REVIEW 9, x FOR PEER REVIEW Y Position/m -4-6 Y Position/m Position/m Figure 6. Feature Figure pots Feature map. pots map. In order to verify accuracy feature pot map, maps were matched with In order to In verify order to accuracy verify accuracy feature pot feature map, pot maps map, were maps matched were with matched with experimentally selected door architectural map. The matchg result is shown Figure 7. From experimentally experimentally selected door selected architectural door architectural map. The matchg map. map. The The result matchg is shown result result is shown is Figure shown 7. Figure From Figure 7. From 7. From figure, it can be seen that extracted feature pots can be seen as cross pot route figure, it figure, can figure, be itseen canit be can that seen be seen that extracted that extracted feature extracted feature pots feature pots can be pots can seen becan as seen a be cross aseen a cross pot as a pot cross pot route route with route with allowable error range, constructed feature pot map can reflect approximate with allowable with allowable error allowable error range, range, error range, constructed constructed constructed feature feature pot pot feature map map can pot reflect canmap reflect can approximate reflect approximate path path terior buildg. The details two zoomed- feature pots are as shown left path terior path terior buildg. terior buildg. The buildg. details The details The details two zoomed- two zoomed- two feature zoomed- feature pots feature pots are as are pots shown as shown are as shown left left sideleft side picture correspondg to pots B, respectively. At pot A, passes side picture side picture correspondg picture correspondg to pots to pots A to Apots B, respectively. B, A B, respectively. At pot At pot A, At a A, pot a A, a passes passes through passes through tersection twice. Each time extracted feature pots are shown different through through tersection tersection twice. twice. Each Each twice. time time Each extracted time feature extracted pots feature are shown pots are shown different color different circles, color circles, average two feature pots is output to obta feature pots required by color circles, color average circles, average two feature pots two feature is output pots to obta is output to feature obta pots feature required pots by required this article, by this article, as shown black Figure 7. Pot is same as pot A: this article, as this shown article, as black shown black Figure 7. Pot Figure B is7. Pot same B is as pot same A: as pot A: passes passes tersection three times. passes tersection passes tersection three times. three times pot zoom A pot zoom A pot zoom pot zoom 18 2 B pot zoom B pot zoom Y Position/m -1 Y Y Position/m A A B B Position/m Figure 7. Matchg feature pot maps with terior buildg plans details two Figure 7. Matchg Figure feature Matchg pot feature maps with pot terior maps with buildg terior plans buildg plans details details two two zoomed- feature pots. zoomed- feature zoomed- pots. feature pots. The feature pot map not only contas extracted feature pot formation, but also The feature The pot feature map not pot only map contas not only contas extracted feature extracted pot feature formation, pot formation, but also but also cludes The feature position potformation map not only contas each pot on extracted ma feature path, reby pot formation, distguishg but also wher cludes cludes cludes position formation position formation each pot on each pot ma on path, reby ma path, distguishg reby distguishg wher wher positionturng formation pot occurs each pot on onma ma path path, or side reby distguishg house, which wher provides ideas for turng pot turng occurs pot on occurs ma on path or ma side path or house, side which house, provides which ideas provides for ideas for followg turng pot new occurs feature onpot ma extraction. path or That sideis, this house, position which formation provides ideas constitutes for followg ma path new followg new followg feature new pot feature extraction. pot That extraction. is, this That position is, this formation position formation constitutes constitutes ma path ma path database, feature pot extraction. extracted That new is, this turng position pot formation is matched constitutes with ma path madatabase path database, one by one. If database, database, extracted new extracted turng new pot turng is matched pot with is matched ma with path database ma path one database by one. If one by one. If matchg distance is less than given threshold, it is determed that turng pot occurs on matchg distance matchg is less distance than a is given less than threshold, a given it threshold, is determed it is that determed turng that pot turng occurs pot on occurs on ma path. Judgg by Equation (): ma path. Judgg ma path. by Equation Judgg by (): Equation ():

9 Micromaches 218, 9, extracted new turng pot is matched with ma path database one by one. If matchg distance is less than a given threshold, it is determed that turng pot occurs on ma path. Judgg by Equation (): ( ) d n i (x T = n T x i ) 2 ( l + y n T y i 2 l) { ( ) 1 d l n n i T = T Th () ors In formula, ( x n T, yn T) is coordate nth turn pot extracted; ( x i l, y i l) is coordate ith position ma path database; ( d n i T) is distance between extracted nth turn pot ith position pot ma path database. Th is threshold value, which is determed for turng pot state, specific value needs to be determed accordg to actual situation experimental site. l n T = 1 dicates extracted nth turn pot that occurs on ma path. l n T = dicates that extracted nth turn pot is not on ma path. 4. Design Particle Filter-Simultaneous Localization Map Buildg (PF-SLAM) Indoor Pedestrian Positiong System Based on Feature Pot Map 4.1. SLAM Problem Description Simultaneous localization mappg, abbreviated as SLAM, means that when a movg object is completely unfamiliar to its environment, its location is not determed, such as emergency rescue missions, for example, fire rescue. In this case, by creatg a surroundg environment map n usg created environment map to help user to achieve ir own positiong, SLAM maly solves two major problems. These two problems are mutually premised teract with each or: first, an environment map is constructed. This is achieved by collectg formation about surroundg environment through various sensors worn on carrier. However, this formation is ten large unorganized, feature formation needs to be extracted from this formation aggregated to an environment map. Second, user s positiong determes location user constructed environment map. The SLAM idea is core idea entire positiong system. The ma implementation steps SLAM algorithm can be summarized followg steps: (1) Feature pots are extracted, ma path feature pot map is constructed. The INS system obtas formation location, accuracy which is determed by accuracy sensors samplg time. The feature pots door ma path are constructed by extractg feature pots from straight-le fittg straight walkg phase through more accurate ma path attitude formation obtaed. (2) Location prediction. The headg angle difference is corrected detected straight-le walk state, reby suppressg accumulation headg angle error. The s position posture are predicted by corrected headg particle filter algorithm based on dead reckong. (3) Feature pot matchg. Determe wher observed position is a turng pot. If yes, match feature pot update particle weight particle distribution. If it cannot be matched with current feature pot database, flection pot on ma path is extracted as a new feature pot added to feature pot map. (4) Correction. The position is corrected accordg to feature pot matchg result, detected turng pot on ma path, which when unmatched is used to exp update feature pot map formation.

10 Micromaches 218, 9, PF-SLAM System Model The SLAM system a practical project is a non-lear system where noise does not follow Gaussian distribution. If method based on extended Kalman filter-slam (EKF-SLAM) is used to estimate non-lear non-gaussian state, lear errors will evitably be troduced to system, particle filterg based on Monte Carlo sequential importance samplg (SIS) is suitable for processg non-lear non-gaussian systems. It is an effective method for processg SLAM systems. Therefore, SLAM method based on particle filterg was used this paper. Particle filterg is implemented by non-parametric Monte Carlo simulation method for Bayesian filterg. The prior a posteriori formation is described sample form stead function. The SLAM problem needs to be solved by calculatg jot posterior probability density distribution pose feature pot map: P(x 1:k, θ z 1:k, u 1:k ) (6) In formula, x 1:k = [x 1, x 2,, x k ] T represents walkg path up to time k; θ = [x 1, y 1, x 2, y 2,, x n, y n ] T dicates position feature pot feature pot map; z k is observation, note z 1:k = [z 1, z 2,, z k ] T represents observation sequence up to time k; u k is control put, note u 1:k = [u 1, u 2,, u k ] T represents control put sequence up to time k. The core idea particle filterg SLAM is to factorize jot posterior probability distribution to path part map part with path as condition. P(x 1:k, θ z 1:k, u 1:k ) = P(θ x 1:k,z 1:k, u 1:k )P(x 1:k z 1:k,u 1:k ) (7) If s entire path x 1:k is given, map θ can be calculated analytically. Therefore, posterior probability s pose P(x 1:k z 1:k,u 1:k ) can be estimated by particle filterg method. Based on this, map θ can be recursively calculated by Kalman filter. The particle importance weights are: w (i) k x (i) P(z k k )w (i) k 1 (8) where w (i) k is weight particle at moment;w (i) k 1 is weight particle at previous xk moment; P(z (i) k ) is posterior probability position. Perform particle weight normalization: w (i) k = w(i) k N w (i) k i=1 (9) where N is total number particles. Doucet [22] showed that variance weights creases with time, which leads to degeneracy problem that prevails particle filterg. An appropriate measure degradation is adoption effective populations N e f f. The smaller N e f f dicates that degradation is more serious. N e f f can be calculated by followg equation [12]: 1 N e f f = (1) N (w (i) k )2 i=1 The re-samplg method is ten used to solve degeneracy phenomenon, re-samplg is performed when number effective particles is less than a given number. Given a valid particle