CN106767821A - A kind of map match localization method and system based on particle filter - Google Patents
A kind of map match localization method and system based on particle filter Download PDFInfo
- Publication number
- CN106767821A CN106767821A CN201611131151.6A CN201611131151A CN106767821A CN 106767821 A CN106767821 A CN 106767821A CN 201611131151 A CN201611131151 A CN 201611131151A CN 106767821 A CN106767821 A CN 106767821A
- Authority
- CN
- China
- Prior art keywords
- particle
- cloud
- renewal
- current location
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
The invention provides a kind of map match localization method based on particle filter and system, method is：It is linear road feasible zone and intersection region that the feasible zone of indoor environment map is disassembled, and path to being included in region carries out path number；Obtain the particle cloud of the current location that the result based on PDR is generated, and the path number residing for the particle cloud of current location；The error of current location, the variance of current location, step-length, the evaluated error of step-length, course angle and the course angle of particle cloud is obtained, the renewal of particle state is carried out, the particle cloud after being updated；According to path number, region residing for the particle in the particle cloud after updating is judged, the weight of each particle, obtains positioning result in the particle cloud after adjustment renewal.Present invention employs the method for particle filter, indoor environment map is based on positioning solution and its error sets up particle cloud, then by way of particle weights are adjusted and update particle cloud, obtain positioning solution, realize indoor positioning, improve positioning precision.
Description
Technical field
The present invention relates to indoor positioning field, more particularly to a kind of map match localization method based on particle filter and it is
System.
Background technology
Continuous pursuit with the development and people of wireless location technology to quality of the life, location requirement is extended to from outdoor
It is indoor.Indoor Location Information gradually plays the part of important role in daily life.Outdoor mainly provides service by GNSS, and indoor
Then blocked by wall, typically GNSS signal is very weak indoors or cannot receive, it is impossible to which indoor positioning service is provided, thus it is suitable
It is widely studied and pays close attention to for indoor localization method.
Develop today like a raging fire in location-based service (Location Based Service, LBS), in prior art
In, generally using the indoor positioning technologies of the mobile phone PDR based on sensing data, PDR is referred to as pedestrian's dead reckoning, mainly
Calculate the position of the people for obtaining indoor walking, the algorithm is that the step number walked according to pedestrian, step-length, direction measure and unite
Meter, extrapolates the information such as pedestrian's run trace and position.And pedestrian's run trace and position are surveyed in the one-dimensional interior space
Measure.The data that we obtain all are obtained in the one-dimensional interior space, that is, what is obtained is all one-dimensional data, algorithm letter
It is single, it is easy to calculating to compare.But during being positioned with PDR, by the angular integral that PDR is used is by gyroscope
The angular speed integration of output, and the inexpensive gyroscope used for common equipments such as mobile phones is to temperature, environment etc. is very quick
Sense, i.e., itself output error is larger.Therefore Output speed has a more obvious drift deviation after starting gyroscope, with
The increase of positioning number of times, i.e. the extension of positioning time, this drift error is also increasing to the error that integration angle is brought, and leads
Positioning result is caused constantly to dissipate.This characteristic cause its it is inevasible in utilization error is gradually increased, for indoor ring
Border, it is meant that positioning solution may be displaced to it is architectural, cause positioning it is inaccurate.
Therefore, defect of the prior art is to carry out indoor positioning by PDR technologies, and PDR self-views are by integrating
Come, cause when being positioned with PDR self-views, with the increase of positioning number of times, error is increasing, and then causes
Positioning is inaccurate.
The content of the invention
For above-mentioned technical problem, the present invention provides a kind of map match localization method and system based on particle filter,
The method for employing particle filter, is based on indoor environment map positioning solution and its error sets up particle cloud, then by particle
Weight adjustment updates the mode of particle cloud, obtains positioning solution, realizes indoor positioning, improves positioning precision；Avoid by PDR
Algorithm carries out indoor positioning, because its error is big, causes the problem of Position location accuracy difference.
In order to solve the above technical problems, the technical scheme that the present invention is provided is：
In a first aspect, the present invention provides a kind of map match localization method based on particle filter, including：
Step S1, obtains indoor environment map, and the feasible zone of the indoor environment map is disassembled as linear road is feasible
Domain and intersection region, and line label is entered in path to being included in the linear road feasible zone and intersection region, per paths pair
Answer a path number；
The particle cloud of step S2, the particle cloud of the current location that result of the acquisition based on PDR is generated, and the current location
Residing path number；
Step S3, the current location of the particle cloud, the variance of current location, step-length, step-length are obtained by PDR algorithms
The error of evaluated error, course angle and course angle, carries out the renewal of the particle state of current location, the particle after being updated
Cloud；
Step S4, according to the path number, judges region residing for the particle in the particle cloud after the renewal：
When the path number is individual paths, judge that the particle in the particle cloud after the renewal is located at the straight line
In road feasible zone；
When the path number is mulitpath, judge that the particle in the particle cloud after the renewal is located at the intersection
In region；
Step S5, region residing for the particle in the particle cloud after the renewal adjusts the particle cloud after the renewal
In each particle weight, calculate the weighted mean and variance of each particle weights in the particle cloud after the renewal, determined
Position result.
Map match localization method based on particle filter of the invention, its technical scheme is：Indoor environment map is obtained,
It is linear road feasible zone and intersection region that the feasible zone of the indoor environment map is disassembled, and feasible to the linear road
Line label is entered in the path included in domain and intersection region, one path number of correspondence per paths；Then the knot based on PDR is obtained
The particle cloud of the current location of fruit generation, and the path number residing for the particle cloud of the current location；
Then the current location of the particle cloud, the variance of current location, step-length, step-length is obtained by PDR algorithms to estimate
The error of meter error, course angle and course angle, carries out the renewal of the particle state of current location, the particle cloud after being updated；
Then according to the path number, region residing for the particle in the particle cloud after the renewal is judged：When the path number is
During individual paths, judge that the particle in the particle cloud after the renewal is located in the linear road feasible zone；When the path
When numbering is mulitpath, judge that the particle in the particle cloud after the renewal is located in the intersection region；
It is every in the particle cloud after the adjustment renewal finally according to region residing for the particle in the particle cloud after the renewal
The weight of individual particle, calculates the weighted mean and variance of each particle weights in the particle cloud after the renewal, obtains positioning knot
Really.
Map match localization method based on particle filter of the invention, the method for employing particle filter, by indoor ring
Condition figure is based on positioning solution and its error sets up particle cloud, then by way of particle weights are adjusted and update particle cloud, obtains
Positioning solution really, realizes indoor positioning, improves positioning precision.
Further, the step S5, specially：
Region residing for particle in the particle cloud after the renewal, adjusts each grain in the particle cloud after the renewal
The weight of son；
The particle in particle cloud after the renewal is in the linear road feasible zone, will be positioned at the linear road
Particle weights in the feasible zone of road put 1, otherwise set to 0；
The particle in particle cloud after the renewal is in the cross-domain, and the cross-domain is by a plurality of straight line path
Intersect to form, the particle weights on straight line path that will be located in the cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after the renewal are calculated, positioning result is obtained.
Further, the step S3, by the current location of the PDR algorithms acquisition particle cloud, the side of current location
The error of difference, step-length, the evaluated error of step-length, course angle and course angle, carries out the renewal of the particle state of current location, specifically
For：
Current location, the variance of current location, step-length, the estimation mistake of step-length of the particle cloud are obtained by PDR algorithms
The error of difference, course angle and course angle；
The variance of current location, current location according to the particle cloud, certain amount is obtained by dimensional gaussian distribution
Particle, the particle represents current location；
The evaluated error of the step-length and the error of the course angle are calculated by Gaussian noise algorithm, error is obtained；
According to the step-length and course angle, using the step-length as average, using the error as variance evaluation, height is obtained
This value；
The Gauss value is carried out the renewal of position as new step-length to each particle in the particle.
Further, also include：
Step S6, according to the positioning result, updates the corresponding path number of the positioning result.
Second aspect, the present invention provides a kind of map match alignment system based on particle filter, including：
Map pretreatment module, for obtaining indoor environment map, by the feasible zone of the indoor environment map disassemble for
Linear road feasible zone and intersection region, and rower is entered in path to being included in the linear road feasible zone and intersection region
Number, one path number of correspondence per paths；
Particle cloud generation module, the particle cloud for obtaining the current location that the result based on PDR is generated, and it is described current
Path number residing for the particle cloud of position；
Particle update module, for obtained by PDR algorithms the current location of the particle cloud, the variance of current location,
The error of step-length, the evaluated error of step-length, course angle and course angle, carries out the renewal of the particle state of current location, obtains more
Particle cloud after new；
Passage zone judge module, for according to the path number, judging the particle in the particle cloud after the renewal
Residing region：
When the path number is individual paths, judge that the particle in the particle cloud after the renewal is located at the straight line
In road feasible zone；
When the path number is mulitpath, judge that the particle in the particle cloud after the renewal is located at the intersection
In region；
Locating module, for region residing for the particle in the particle cloud after the renewal, after adjusting the renewal
The weight of each particle in particle cloud, calculates the weighted mean and variance of each particle weights in the particle cloud after the renewal,
Obtain positioning result.
Map match alignment system based on particle filter of the invention, its technical scheme is：First pass through map pretreatment
Module, for obtaining indoor environment map, it is linear road feasible zone and friendship that the feasible zone of the indoor environment map is disassembled
Fork region, and line label is entered in path to being included in the linear road feasible zone and intersection region, the correspondence one per paths
Path number；Then by particle cloud generation module, the particle cloud of the current location that the result based on PDR is generated, and institute are obtained
State the path number residing for the particle cloud of current location；
Then by particle update module, by the current location of the PDR algorithms acquisition particle cloud, the side of current location
The error of difference, step-length, the evaluated error of step-length, course angle and course angle, carries out the renewal of the particle state of current location, obtains
Particle cloud after renewal；Then by passage zone judge module, according to the path number, the particle after the renewal is judged
Region residing for particle in cloud：When the path number is individual paths, the particle in the particle cloud after the renewal is judged
In the linear road feasible zone；When the path number is mulitpath, in the particle cloud after the judgement renewal
Particle be located at the intersection region in；
Finally by locating module, region residing for the particle in the particle cloud after the renewal adjusts the renewal
The weight of each particle in particle cloud afterwards, calculates weighted mean and the side of each particle weights in the particle cloud after the renewal
Difference, obtains positioning result.
Map match alignment system based on particle filter of the invention, the method for employing particle filter, by indoor ring
Condition figure is based on positioning solution and its error sets up particle cloud, then by way of particle weights are adjusted and update particle cloud, obtains
Positioning solution really, realizes indoor positioning, improves positioning precision.
Further, the locating module, specifically for：
Region residing for particle in the particle cloud after the renewal, adjusts each grain in the particle cloud after the renewal
The weight of son；
The particle in particle cloud after the renewal is in the linear road feasible zone, will be positioned at the linear road
Particle weights in the feasible zone of road put 1, otherwise set to 0；
The particle in particle cloud after the renewal is in the cross-domain, and the cross-domain is by a plurality of straight line path
Intersect to form, the particle weights on straight line path that will be located in the cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after the renewal are calculated, positioning result is obtained.
Further, the particle update module, specifically for：
Current location, the variance of current location, step-length, the estimation mistake of step-length of the particle cloud are obtained by PDR algorithms
The error of difference, course angle and course angle；
The variance of current location, current location according to the particle cloud, certain amount is obtained by dimensional gaussian distribution
Particle, the particle represents current location；
The evaluated error of the step-length and the error of the course angle are calculated by Gaussian noise algorithm, error is obtained；
According to the step-length and course angle, using the step-length as average, using the error as variance evaluation, height is obtained
This value；
The Gauss value is carried out the renewal of position as new step-length to each particle in the particle.
Further, also it is used for including path number update module：
According to the positioning result, the corresponding path number of the positioning result is updated.
Brief description of the drawings
In order to illustrate more clearly of the specific embodiment of the invention or technical scheme of the prior art, below will be to specific
The accompanying drawing to be used needed for implementation method or description of the prior art is briefly described.
Fig. 1 shows a kind of map match localization method based on particle filter that first embodiment of the invention is provided
Flow chart；
Fig. 2 shows a kind of map match alignment system based on particle filter that second embodiment of the invention is provided
Schematic diagram.
Specific embodiment
The embodiment of technical solution of the present invention is described in detail below in conjunction with accompanying drawing.Following examples are only used for
Technical scheme is clearly illustrated, therefore is intended only as example, and protection of the invention can not be limited with this
Scope.
Embodiment one
Fig. 1 shows a kind of map match localization method based on particle filter that first embodiment of the invention is provided
Flow chart；As shown in figure 1, the embodiment of the present invention one provides a kind of map match localization method based on particle filter, including：
Step S1, obtains indoor environment map, the feasible zone of indoor environment map is disassembled as linear road feasible zone and
Intersection region, and line label is entered in path to being included in linear road feasible zone and intersection region, one road of correspondence per paths
Number in footpath；
Step S2, obtains the particle cloud of the current location that the result based on PDR is generated, and residing for the particle cloud of current location
Path number；
Step S3, the estimation of the current location of particle cloud, the variance of current location, step-length, step-length is obtained by PDR algorithms
The error of error, course angle and course angle, carries out the renewal of the particle state of current location, the particle cloud after being updated；
Step S4, according to path number, judges region residing for the particle in the particle cloud after updating：
When path number is individual paths, judge that the particle in the particle cloud after updating is located at linear road feasible zone
It is interior；
When path number is mulitpath, judge that the particle in the particle cloud after updating is located in intersection region；
Step S5, region residing for the particle in the particle cloud after renewal, adjustment update after particle cloud in each grain
The weight of son, calculates the weighted mean and variance of each particle weights in the particle cloud after updating, and obtains positioning result.
Map match localization method based on particle filter of the invention, its technical scheme is：Indoor environment map is obtained,
It is linear road feasible zone and intersection region that the feasible zone of indoor environment map is disassembled, and to linear road feasible zone and intersection
Line label is entered in the path included in region, one path number of correspondence per paths；Then obtain what the result based on PDR was generated
The particle cloud of current location, and the path number residing for the particle cloud of current location；
Then current location, the variance of current location, step-length, the estimation mistake of step-length of particle cloud are obtained by PDR algorithms
The error of difference, course angle and course angle, carries out the renewal of the particle state of current location, the particle cloud after being updated；Then
According to path number, region residing for the particle in the particle cloud after updating is judged：When path number is individual paths, judge more
The particle in particle cloud after new is located in linear road feasible zone；When path number is mulitpath, after judging to update
Particle in particle cloud is located in intersection region；
Finally according to region residing for the particle in the particle cloud after renewal, each particle in the particle cloud after adjustment renewal
Weight, calculates the weighted mean and variance of each particle weights in the particle cloud after updating, and obtains positioning result.
Map match localization method based on particle filter of the invention, based on the feasible zone in indoor environment map and non-
Feasible zone, linear road feasible zone and intersection region are divided into by feasible zone, can be walked in the feasible domain representation room of linear road
Region, intersection region represents the region walked that indoor linear road intersects；Then to linear road feasible zone and the zone of intersection
The road included in domain is marked, one path number of correspondence per paths；
Then during indoor positioning is carried out, the method for employing particle filter is built based on positioning solution and its error
Vertical particle cloud, is then based on the data of PDR algorithms acquisition, including the current location of particle cloud, the variance of current location, step-length,
The error of the evaluated error, course angle and course angle of step-length, the particle state to current location is updated, after being updated
Particle cloud, i.e., the particle cloud of new position；Area residing for the particle in the particle cloud of new position is judged then according to path number
Domain, carries out the adjustment of the particle weights of new position, is finally computed weighted mean and variance, obtains positioning result, and it is right to realize
The positioning of current location.By the method, positioning precision is improve.Meanwhile, during positioning, particle cloud is carried out constantly
Renewal, make positioning more accurate.
Specifically, step S5, specially：
Region residing for particle in the particle cloud after renewal, the power of each particle in the particle cloud after adjustment renewal
Weight；
The particle in particle cloud after renewal is in linear road feasible zone, will be located in linear road feasible zone
Particle weights put 1, otherwise set to 0；
The particle in particle cloud after renewal is in cross-domain, and cross-domain is intersected to form by a plurality of straight line path, will
Particle weights on the straight line path in cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after updating are calculated, positioning result is obtained.
In position fixing process, particle cloud is also being constantly updated, ineligible particle (not in linear road feasible zone
Interior particle or the not particle on the straight line path that cross-domain is included) removal, retain credible particle, i.e., when particle be located at it is straight
During line region, based on current location, and current location deviation, step-length and step error, deflection and direction angle error are entered
Row particle state updates, and the particle cloud after being updated, the particle weights that particle is still on this paths are put 1, otherwise set to 0,
Average acquisition positioning result, and path number is updated according to positioning result；
If update after position be located at cross-domain, path number be designated as cross-domain where mulitpath number, work as road
When footpath numbering is mulitpath, illustrate particle in cross-domain, then when newly-generated particle weights are adjusted, positioned at elder generation
The weight of preceding record path is 1, and remaining sets to 0.
By way of adjusting particle weights, make the particle in particle cloud more accurate, be that positioning does standby next time, improve
The degree of accuracy for positioning next time.
Specifically, step S3, current location, the variance of current location, step-length, the step of particle cloud are obtained by PDR algorithms
The error of evaluated error, course angle and course angle long, carries out the renewal of the particle state of current location, specially：
Current location, the variance of current location, step-length, the evaluated error of step-length, the boat of particle cloud are obtained by PDR algorithms
To angle and the error of course angle；
The variance of current location, current location according to particle cloud, a number of grain is obtained by dimensional gaussian distribution
Son, particle represents current location；
By the evaluated error and the error of course angle of Gaussian noise algorithm material calculation, error is obtained；
According to step-length and course angle, using step-length as average, using error as variance evaluation, Gauss value is obtained；
Gauss value is carried out the renewal of position as new step-length to each particle in particle.
It is the position location and its error foundation obtained according to particle filter to set up particle cloud, can be given by PDR
The step-length of current location, the evaluated error of step-length, course angle, the evaluated error of course angle, then each particle in particle cloud
All updated using step-length and angle containing random error, specially：Each particle is used with step-length as average, error is variance
The step-length that the Gauss value of estimation is used as renewal, particle state is updated, the particle cloud after being updated.To particle cloud
In particle state be updated, the state of more positive corpusc(u)le makes the position representated by the particle in particle cloud more accurate, and then make
Positioning is more accurate, improves positioning precision.
Wherein, error can be the evaluated error of step-length and the error of course angle, and the two errors are used in combination with two
First Gaussian noise unifies form of noise, the error that obtains；Error can also be the evaluated error of step-length and the error of course angle
Each do the superposition of gaussian random migration noise, the error for obtaining.
Specifically, also include：
Step S6, according to positioning result, updates the corresponding path number of positioning result.
According to positioning result, the corresponding path number of positioning result is updated, the new path number of acquisition is actually used
In weighed value adjusting next time, that is during position next time, weighed using the path number after current renewal
The adjustment of weight, and then positioned, it is ensured that the real-time of position fixing process, make positioning more accurate.
Specifically, when being positioned with PDR, particle state is updated, does following explanation：
When being positioned with PDR, exactly safeguard that variable allows domain (can walking path region), i.e., it is next
Moment, the region (numbering) that particle may be in is for example current to allow domain for No. 2 lines (path number), then next particle is more
After new, what its weights was in No. two regions is just placed in 1, is otherwise just 0, is then tried to achieve according to particle weights and distribution current true
Real storage passage zone, if current true path region is located at cross-domain, then this allows domain to be changing to 2,3,4
(assuming that cross-domain is 2,3,4 three lines intersect), next particle update after be exactly that to be located at the particle weights in 234 regions be 1, region
Weight in addition is just 0.
Embodiment two
Fig. 2 shows a kind of map match alignment system based on particle filter that second embodiment of the invention is provided
Schematic diagram, as shown in Fig. 2 the embodiment of the present invention two provides a kind of map match alignment system 10 based on particle filter, including：
Map pretreatment module 101, for obtaining indoor environment map, the feasible zone of indoor environment map is disassembled as straight
Drawing lines road feasible zone and intersection region, and line label, every are entered in path to being included in linear road feasible zone and intersection region
Path one path number of correspondence；
Particle cloud generation module 102, the particle cloud for obtaining the current location that the result based on PDR is generated, and currently
Path number residing for the particle cloud of position；
Particle update module 103, current location, the variance of current location, step for obtaining particle cloud by PDR algorithms
The error of length, the evaluated error of step-length, course angle and course angle, carries out the renewal of the particle state of current location, is updated
Particle cloud afterwards；
Passage zone judge module 104, for according to path number, judging area residing for the particle in the particle cloud after updating
Domain：
When path number is individual paths, judge that the particle in the particle cloud after updating is located at linear road feasible zone
It is interior；
When path number is mulitpath, judge that the particle in the particle cloud after updating is located in intersection region；
Locating module 105, for region residing for the particle in the particle cloud after renewal, the particle cloud after adjustment renewal
In each particle weight, calculate update after particle cloud in each particle weights weighted mean and variance, obtain positioning knot
Really.
Map match alignment system 10 based on particle filter of the invention, its technical scheme is：Map is first passed through to locate in advance
Reason module 101, for obtaining indoor environment map, it is linear road feasible zone and friendship that the feasible zone of indoor environment map is disassembled
Fork region, and line label is entered in path to being included in linear road feasible zone and intersection region, one path of correspondence per paths
Numbering；Then by particle cloud generation module 102, the particle cloud of the current location that the result based on PDR is generated is obtained, and currently
Path number residing for the particle cloud of position；
Then by particle update module 103, by the current location of PDR algorithms acquisition particle cloud, the side of current location
The error of difference, step-length, the evaluated error of step-length, course angle and course angle, carries out the renewal of the particle state of current location, obtains
Particle cloud after renewal；Then by passage zone judge module 104, according to path number, in the particle cloud after judgement renewal
Particle residing for region：When path number is individual paths, judge that the particle in the particle cloud after updating is located at linear road
In feasible zone；When path number is mulitpath, judge that the particle in the particle cloud after updating is located in intersection region；
Finally by locating module 105, region residing for the particle in the particle cloud after renewal, adjustment update after grain
The weight of each particle in sub- cloud, calculates the weighted mean and variance of each particle weights in the particle cloud after updating, and is determined
Position result.
Map match alignment system 10 based on particle filter of the invention, based on the feasible zone in indoor environment map and
Non- feasible zone, linear road feasible zone and intersection region are divided into by feasible zone, and linear road can walk in feasible domain representation room
Region, intersection region represents the region walked that indoor linear road intersects；Then to linear road feasible zone and intersection
The road included in region is marked, one path number of correspondence per paths；
Then during indoor positioning is carried out, the method for employing particle filter is built based on positioning solution and its error
Vertical particle cloud, is then based on the data of PDR algorithms acquisition, including the current location of particle cloud, the variance of current location, step-length,
The error of the evaluated error, course angle and course angle of step-length, the particle state to current location is updated, after being updated
Particle cloud, i.e., the particle cloud of new position；Area residing for the particle in the particle cloud of new position is judged then according to path number
Domain, carries out the adjustment of the particle weights of new position, obtains positioning result, realizes the positioning to current location.By the method,
Improve positioning precision.Meanwhile, during positioning, particle cloud is constantly updated, make positioning more accurate.
Specifically, locating module 105, specifically for：
Region residing for particle in the particle cloud after renewal, the power of each particle in the particle cloud after adjustment renewal
Weight；
The particle in particle cloud after renewal is in linear road feasible zone, will be located in linear road feasible zone
Particle weights put 1, otherwise set to 0；
The particle in particle cloud after renewal is in cross-domain, and cross-domain is intersected to form by a plurality of straight line path, will
Particle weights on the straight line path in cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after updating are calculated, positioning result is obtained.
In position fixing process, particle cloud is also being constantly updated, ineligible particle (not in linear road feasible zone
Interior particle or the not particle on the straight line path that cross-domain is included) removal, retain credible particle, i.e., when particle be located at it is straight
During line region, based on current location, and current location deviation, step-length and step error, deflection and direction angle error are entered
Row particle state updates, and the particle cloud after being updated, the particle weights that particle is still on this paths are put 1, otherwise set to 0,
Average acquisition positioning result, and path number is updated according to positioning result；
If update after position be located at cross-domain, path number be designated as cross-domain where mulitpath number, work as road
When footpath numbering is mulitpath, illustrate particle in cross-domain, then when newly-generated particle weights are adjusted, positioned at elder generation
The weight of preceding record path is 1, and remaining sets to 0.
By way of adjusting particle weights, make the particle in particle cloud more accurate, be that positioning does standby next time, improve
The degree of accuracy for positioning next time.
Specifically, particle update module 103, specifically for：
Current location, the variance of current location, step-length, the evaluated error of step-length, the boat of particle cloud are obtained by PDR algorithms
To angle and the error of course angle；
The variance of current location, current location according to particle cloud, a number of grain is obtained by dimensional gaussian distribution
Son, particle represents current location；
By the evaluated error and the error of course angle of Gaussian noise algorithm material calculation, error is obtained；
According to step-length and course angle, using step-length as average, using error as variance evaluation, Gauss value is obtained；
Gauss value is carried out the renewal of position as new step-length to each particle in particle.
It is the position location and its error foundation obtained according to particle filter to set up particle cloud, can be given by PDR
The step-length of current location, the evaluated error of step-length, course angle, the evaluated error of course angle, then each particle in particle cloud
All updated using step-length and angle containing random error, specially：Each particle is used with step-length as average, error is variance
The step-length that the Gauss value of estimation is used as renewal, particle state is updated, the particle cloud after being updated.To particle cloud
In particle state be updated, the state of more positive corpusc(u)le makes the position representated by the particle in particle cloud more accurate, and then make
Positioning is more accurate, improves positioning precision.
Wherein, error can be the evaluated error of step-length and the error of course angle, and the two errors are used in combination with two
First Gaussian noise unifies form of noise, the error that obtains；Error can also be the evaluated error of step-length and the error of course angle
Each do the superposition of gaussian random migration noise, the error for obtaining.
Specifically, also it is used for including path number update module 106：
According to positioning result, the corresponding path number of positioning result is updated.
According to positioning result, the corresponding path number of positioning result is updated, the new path number of acquisition is actually used
In weighed value adjusting next time, that is during position next time, weighed using the path number after current renewal
The adjustment of weight, and then positioned, it is ensured that the real-time of position fixing process, make positioning more accurate.
Specifically, when being positioned with PDR, particle state is updated, does following explanation：
When being positioned with PDR, exactly safeguard that variable allows domain (can walking path region), i.e., it is next
Moment, the region (numbering) that particle may be in is for example current to allow domain for No. 2 lines (path number), then next particle is more
After new, what its weights was in No. two regions is just placed in 1, is otherwise just 0, is then tried to achieve according to particle weights and distribution current true
Real storage passage zone, if current true path region is located at cross-domain, then this allows domain to be changing to 2,3,4
(assuming that cross-domain is 2,3,4 three lines intersect), next particle update after be exactly that to be located at the particle weights in 234 regions be 1, region
Weight in addition is just 0.
Beneficial effects of the present invention are：
1. independence in the inertial sensor short time is kept, advantage high is positioned, meanwhile, it is capable to suppress error value product well
It is tired, improve the precision and stability of location algorithm.
2. particle filter is a kind of important means of filtering, the characteristics of due to its imparametrization, breaks away from understanding linear by no means
Stochastic variable must is fulfilled for the restriction of Gaussian Profile during filtering problem, for analysis nonlinear dynamic system provides one kind effectively
Solution.
3. map feasible zone makes simple, smaller using particle filter operand, more effective auxiliary adjustment PDR algorithms
Positioning result.
Finally it should be noted that：Various embodiments above is merely illustrative of the technical solution of the present invention, rather than its limitations；To the greatest extent
Pipe has been described in detail with reference to foregoing embodiments to the present invention, it will be understood by those within the art that：Its according to
The technical scheme described in foregoing embodiments can so be modified, or which part or all technical characteristic are entered
Row equivalent；And these modifications or replacement, the essence of appropriate technical solution is departed from various embodiments of the present invention technology
The scope of scheme, it all should cover in the middle of the scope of claim of the invention and specification.
Claims (8)
1. a kind of map match localization method based on particle filter, it is characterised in that including：
Step S1, obtains indoor environment map, the feasible zone of the indoor environment map is disassembled as linear road feasible zone and
Intersection region, and line label is entered in path to being included in the linear road feasible zone and intersection region, the correspondence one per paths
Individual path number；
Step S2, obtains the particle cloud of the current location that the result based on PDR is generated, and residing for the particle cloud of the current location
Path number；
Step S3, the estimation of the current location of the particle cloud, the variance of current location, step-length, step-length is obtained by PDR algorithms
The error of error, course angle and course angle, carries out the renewal of the particle state of current location, the particle cloud after being updated；
Step S4, according to the path number, judges region residing for the particle in the particle cloud after the renewal：
When the path number is individual paths, judge that the particle in the particle cloud after the renewal is located at the linear road
In feasible zone；
When the path number is mulitpath, judge that the particle in the particle cloud after the renewal is located at the intersection region
It is interior；
Step S5, region residing for the particle in the particle cloud after the renewal adjusts every in the particle cloud after the renewal
The weight of individual particle, calculates the weighted mean and variance of each particle weights in the particle cloud after the renewal, obtains positioning knot
Really.
2. the map match localization method based on particle filter according to claim 1, it is characterised in that
The step S5, specially：
Region residing for particle in the particle cloud after the renewal, each particle in the particle cloud after the adjustment renewal
Weight
The particle in particle cloud after the renewal is in the linear road feasible zone, will can positioned at the linear road
Particle weights in row domain put 1, otherwise set to 0；
The particle in particle cloud after the renewal is in the cross-domain, and the cross-domain is intersected by a plurality of straight line path
Formed, the particle weights on straight line path that will be located in the cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after the renewal are calculated, positioning result is obtained.
3. the map match localization method based on particle filter according to claim 1, it is characterised in that
The step S3, the current location of the particle cloud, the variance of current location, step-length, step-length are obtained by PDR algorithms
The error of evaluated error, course angle and course angle, carries out the renewal of the particle state of current location, specially：
Current location, the variance of current location, step-length, the evaluated error of step-length, the boat of the particle cloud are obtained by PDR algorithms
To angle and the error of course angle；
The variance of current location, current location according to the particle cloud, a number of grain is obtained by dimensional gaussian distribution
Son, the particle represents current location；
The evaluated error of the step-length and the error of the course angle are calculated by Gaussian noise algorithm, error is obtained；
According to the step-length and course angle, using the step-length as average, using the error as variance evaluation, Gauss is obtained
Value；
The Gauss value is carried out the renewal of position as new step-length to each particle in the particle.
4. the map match localization method based on particle filter according to claim 1, it is characterised in that
Also include：
Step S6, according to the positioning result, updates the corresponding path number of the positioning result.
5. a kind of map match alignment system based on particle filter, it is characterised in that including：
Map pretreatment module, for obtaining indoor environment map, it is straight line that the feasible zone of the indoor environment map is disassembled
Road feasible zone and intersection region, and line label is entered in path to being included in the linear road feasible zone and intersection region, often
Paths one path number of correspondence；
Particle cloud generation module, the particle cloud for obtaining the current location that the result based on PDR is generated, and the current location
Particle cloud residing for path number；
Particle update module, for obtained by PDR algorithms the current location of the particle cloud, the variance of current location, step-length,
The error of the evaluated error, course angle and course angle of step-length, carries out the renewal of the particle state of current location, after being updated
Particle cloud；
Passage zone judge module, for according to the path number, judging residing for the particle in the particle cloud after the renewal
Region：
When the path number is individual paths, judge that the particle in the particle cloud after the renewal is located at the linear road
In feasible zone；
When the path number is mulitpath, judge that the particle in the particle cloud after the renewal is located at the intersection region
It is interior；
Locating module, for region residing for the particle in the particle cloud after the renewal, adjusts the particle after the renewal
The weight of each particle in cloud, calculates the weighted mean and variance of each particle weights in the particle cloud after the renewal, obtains
Positioning result.
6. the map match alignment system based on particle filter according to claim 5, it is characterised in that
The locating module, specifically for：
Region residing for particle in the particle cloud after the renewal, each particle in the particle cloud after the adjustment renewal
Weight
The particle in particle cloud after the renewal is in the linear road feasible zone, will can positioned at the linear road
Particle weights in row domain put 1, otherwise set to 0；
The particle in particle cloud after the renewal is in the cross-domain, and the cross-domain is intersected by a plurality of straight line path
Formed, the particle weights on straight line path that will be located in the cross-domain put 1, otherwise set to 0；
The weighted mean and variance of each particle weights in the particle cloud after the renewal are calculated, positioning result is obtained.
7. the map match alignment system based on particle filter according to claim 5, it is characterised in that
The particle update module, specifically for：
Current location, the variance of current location, step-length, the evaluated error of step-length, the boat of the particle cloud are obtained by PDR algorithms
To angle and the error of course angle；
The variance of current location, current location according to the particle cloud, a number of grain is obtained by dimensional gaussian distribution
Son, the particle represents current location；
The evaluated error of the step-length and the error of the course angle are calculated by Gaussian noise algorithm, error is obtained；
According to the step-length and course angle, using the step-length as average, using the error as variance evaluation, Gauss is obtained
Value；
The Gauss value is carried out the renewal of position as new step-length to each particle in the particle.
8. the map match alignment system based on particle filter according to claim 5, it is characterised in that
Also include path number update module, be used for：
According to the positioning result, the corresponding path number of the positioning result is updated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611131151.6A CN106767821B (en) | 2016-12-09 | 2016-12-09 | map matching positioning method and system based on particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611131151.6A CN106767821B (en) | 2016-12-09 | 2016-12-09 | map matching positioning method and system based on particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106767821A true CN106767821A (en) | 2017-05-31 |
| CN106767821B CN106767821B (en) | 2019-12-17 |
Family
ID=58879741
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611131151.6A Active CN106767821B (en) | 2016-12-09 | 2016-12-09 | map matching positioning method and system based on particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106767821B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108632761A (en) * | 2018-04-20 | 2018-10-09 | 西安交通大学 | A kind of indoor orientation method based on particle filter algorithm |
| CN109870716A (en) * | 2017-12-01 | 2019-06-11 | 北京京东尚科信息技术有限公司 | Localization method and positioning device and computer readable storage medium |
| CN110530359A (en) * | 2019-09-10 | 2019-12-03 | 中国计量大学 | It is a kind of to be corrected based on bluetooth and map match seeks an indoor orientation method |
| CN110763236A (en) * | 2019-10-28 | 2020-02-07 | 杭州电子科技大学 | Map matching positioning optimization method based on least square |
| CN110896520A (en) * | 2018-09-12 | 2020-03-20 | 北京聚利科技股份有限公司 | Path identification method and system, road side identification equipment and vehicle-mounted equipment |
| CN111061276A (en) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method based on dynamic area division |
| CN114577210A (en) * | 2022-02-24 | 2022-06-03 | 哈尔滨工程大学 | Cross-region detection algorithm based on map information matrix |
| US11797906B2 (en) | 2019-12-18 | 2023-10-24 | Industrial Technology Research Institute | State estimation and sensor fusion switching methods for autonomous vehicles |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103791906A (en) * | 2014-02-21 | 2014-05-14 | 南京北大工道创新有限公司 | Indoor positioning position correction method based on indoor positioning device |
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
-
2016
- 2016-12-09 CN CN201611131151.6A patent/CN106767821B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103791906A (en) * | 2014-02-21 | 2014-05-14 | 南京北大工道创新有限公司 | Indoor positioning position correction method based on indoor positioning device |
| CN104061934A (en) * | 2014-06-10 | 2014-09-24 | 哈尔滨工业大学 | Pedestrian indoor position tracking method based on inertial sensor |
Non-Patent Citations (2)
| Title |
|---|
| 冯成涛等: "基于粒子滤波的室内地图匹配算法", 《第五届中国卫星导航学术年会电子文集》 * |
| 蒲书祥: "基于粒子滤波的个人导航系统算法研究", 《中国优秀硕士学位论文全文数据库•信息科技辑》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109870716A (en) * | 2017-12-01 | 2019-06-11 | 北京京东尚科信息技术有限公司 | Localization method and positioning device and computer readable storage medium |
| CN108632761A (en) * | 2018-04-20 | 2018-10-09 | 西安交通大学 | A kind of indoor orientation method based on particle filter algorithm |
| CN110896520A (en) * | 2018-09-12 | 2020-03-20 | 北京聚利科技股份有限公司 | Path identification method and system, road side identification equipment and vehicle-mounted equipment |
| CN110896520B (en) * | 2018-09-12 | 2021-02-26 | 北京聚利科技有限公司 | Path identification method and system, road side identification equipment and vehicle-mounted equipment |
| CN110530359A (en) * | 2019-09-10 | 2019-12-03 | 中国计量大学 | It is a kind of to be corrected based on bluetooth and map match seeks an indoor orientation method |
| CN110763236A (en) * | 2019-10-28 | 2020-02-07 | 杭州电子科技大学 | Map matching positioning optimization method based on least square |
| US11797906B2 (en) | 2019-12-18 | 2023-10-24 | Industrial Technology Research Institute | State estimation and sensor fusion switching methods for autonomous vehicles |
| CN111061276A (en) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method based on dynamic area division |
| CN111061276B (en) * | 2019-12-31 | 2022-07-26 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method based on dynamic area division |
| CN114577210A (en) * | 2022-02-24 | 2022-06-03 | 哈尔滨工程大学 | Cross-region detection algorithm based on map information matrix |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106767821B (en) | 2019-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106767821A (en) | A kind of map match localization method and system based on particle filter | |
| CN106406320A (en) | Robot path planning method and robot planning route | |
| US9989650B2 (en) | Position in urban canyons | |
| CN102163060B (en) | Early warning method for collision avoidance of helicopter in training flight | |
| CN106197406B (en) | A kind of fusion method based on inertial navigation and RSSI wireless location | |
| CN108020813B (en) | Positioning method, positioning device and electronic equipment | |
| CN103827632B (en) | Position detection device and position detection method | |
| CN106840163A (en) | A kind of indoor orientation method and system | |
| CN109883423A (en) | Localization method, system, equipment and storage medium based on Kalman filtering | |
| CN105571636B (en) | One kind is used to position mesh calibration method and measuring apparatus | |
| CN109807911A (en) | Based on GNSS, UWB, IMU, laser radar, code-disc the multi-environment joint positioning method of outdoor patrol robot | |
| CN106443578A (en) | Indoor positioning base station coordinate calibration method | |
| CN107110652A (en) | Handle space characteristics | |
| CN108303043A (en) | Plant leaf area index detection method and system combined of multi-sensor information | |
| CN104216405B (en) | The air navigation aid and equipment of field robot | |
| CN104243580A (en) | Personnel comprehensive locating method and system applied to water conservancy construction site | |
| CN108235251A (en) | Wireless sensor network node locating method and device under being assisted based on unmanned plane | |
| CN107607904A (en) | A kind of multiple target homogeneity recognition methods based on Angle Information | |
| CN108801267A (en) | It is a kind of to merge the indoor without anchor point localization method of multisensor | |
| US10495500B2 (en) | Flow measuring instrument comprising a wind velocity sensor | |
| CN108139458A (en) | For determining the method, apparatus and system in indoor orientation | |
| Binjammaz et al. | Context-aware GPS integrity monitoring for intelligent transport systems | |
| CN101266153B (en) | Mapping engineering top total station accuracy assessment method | |
| CN106249198B (en) | Position Method for Indoor Robot based on multiple spot RFID combination ultrasonic wave | |
| CN106441295A (en) | Walker marching direction determination method and device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |