CN110333513A - A kind of particle filter SLAM method merging least square method - Google Patents
A kind of particle filter SLAM method merging least square method Download PDFInfo
- Publication number
- CN110333513A CN110333513A CN201910621086.2A CN201910621086A CN110333513A CN 110333513 A CN110333513 A CN 110333513A CN 201910621086 A CN201910621086 A CN 201910621086A CN 110333513 A CN110333513 A CN 110333513A
- Authority
- CN
- China
- Prior art keywords
- thread
- robot
- particle
- map
- particle filter
- 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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
Abstract
The invention discloses a kind of particle filter SLAM methods for merging least square method, are related to robot and positions immediately and map structuring, solve the problems, such as that position data of the robot in map samples heavy dependence mileage and count.The present invention includes that thread I and II dual-thread of thread are executed to obtain position of the robot in map, and time-consuming for the particle filter algorithm as used in thread II, therefore thread II is repeatedly updated in t-1~t time interval.The purpose of Least squares matching algorithm in thread II is exactly the robot displaced posi calculated in t-1~t time, while the application has used the mode of local map, and local map is reset after the completion of particle filter system update in thread I.Its core is in each iterative process that the robot location being calculated using Least squares matching expands particle collection, when some particles can be effectively limited near true distribution by primary update when odometer error is larger in interval.
Description
Technical field
It is positioned immediately the present invention relates to robot and map structuring, and in particular to a kind of particle filter for merging least square method
Wave SLAM method.
Background technique
China is to promote to implement " scientific and technological Xingan " strategy, transfers societal forces and participates in safety in production tackling of key scientific and technical problems, further
Strengthen prevention containment severe and great casualty technology guarantee ability, carries out " mechanization is substituted, automation subtracts people " in key industry field
Scientific and technological strong peace special campaigns, emphasis are to reduce manual operation with mechanization production replacement manual work, with automation control, energetically
Improve enterprise safety operation technology guarantee ability.
In the positioning and map structuring of robot, the SLAM algorithm available one of particle filter is used under normal conditions
A preferable estimated result, but due to the heavy dependence that sampling process counts mileage, cause to count when robot mileage
When error is larger, proposal distribution and target distribution grave fault based on odometer, the scan matching of subsequent progress, weight computing
And etc. it is meaningless.And since the complexity of particle filter system itself is higher, when robot movement speed is too fast, t-1 to t
There has been large change in the period inner machine people position at moment, and algorithm is carrying out the estimation procedure at t-1 moment at this time,
This phenomenon can be such that the scan matching Process Precision under circumstances not known declines.The SLAM system for being widely used at present research is mostly special
It infuses in wheeled mobile robot, and requires the self-contained alignment sensor of robot, such as encoder, gyroscope etc., and thus count
Calculation obtains robot motion's mileage.But the precision of such algorithm heavy dependence odometer, when ground has hollow or wheel to float
When shifting, the sensor errors such as odometer are larger, and robot physical location does not change under limiting case, but mileage counts
According to having shown that the mobile a distance of robot, such case can not only make SLAM algorithm be unable to get the accurate position of robot
It sets, phenomena such as tearing, distorting can also occur for the grating map of creation.
Summary of the invention
It is an object of that present invention to provide a kind of particle filter SLAM method for merging least square method, this method was not needed
The technical issues of degree relies on mileage and counts as estimation initial value, and the application solves are as follows: position data of the robot in map
Sampling heavy dependence mileage counts.
The present invention is achieved through the following technical solutions:
A kind of particle filter SLAM method merging least square method, including thread I and thread II, the thread I are to adopt
Position of the robot in map is obtained with particle filter, thread II is that robot is obtained using Least squares matching in map
Position, specific steps include:
Step 1, when t=0, obtaining radar data obtains initial bit of the robot in map simultaneously for thread I and thread II
It sets, hereafter, robot starts to move；
Step 2, when robot moving distance is more than given threshold, into the t=1 moment, particle filter algorithm in thread I
Map inner machine people position is replaced using the sample that odometer motion model samples particle, and according to weight to grain
Son is ranked up；
Step 3, within t=0~t-1 period, the synchronous radar data that obtains of thread II obtains robot location x1', then
N number of particle that maximum weight in particle collection can be obtained, using least square method to x1The sample that ' sampling obtains replaces t=0~t-
1 period inner machine people position resets local map, the particle collection after being expanded, and the sum of particle becomes in particle collection
M+N；
Step 4, thread II is scanned matching, weight computing, resampling completion update to particle collection, wherein maximum weight
Position of the robot in map that represents of particle, that is, represent the position of robot at this time and environmental map data；
Step 5, thread II is ranked up particle collection, gives up the lesser N number of particle of weight, and total number of particles becomes M, due to
Thread I has been completed in t=1 moment map inner machine people's location updating, is reset in thread II to local map, to next t
=2 moment continued to execute step 2~5 when arriving, then thread II will do it multiple update in thread I a renewable time.
By long-term research and practice, the inventors of the present application found that in the positioning and map structuring of robot, by
In the heavy dependence that data robot shift position sampling process counts mileage, when ground has hollow or wheel to drift about
When, the sensor errors such as odometer are larger, and robot physical location does not change under limiting case, but mileage counts
Having shown that the mobile a distance of robot, such case can not only make SLAM algorithm be unable to get the accurate location of robot,
Phenomena such as tearing, distortion, can also occur for the grating map of creation.The application provides a kind of particle filter for merging least square method
SLAM method is executed including thread I and II dual-thread of thread to obtain position of the robot in map, since thread II is made
Time-consuming for particle filter algorithm, therefore thread II is repeatedly updated in t-1~t time interval.In thread II
The purpose of Least squares matching algorithm is exactly the robot displaced posi calculated in t-1~t time, while the application uses
The mode of local map, local map is reset after the completion of the particle filter system update in thread I.Its core is every time repeatedly
During generation, the robot location being calculated using Least squares matching expands particle collection, updates interval when primary
Some particles can be effectively limited near true distribution by interior odometer error when larger.
Further, the mode that the thread I initializes robot location includes: default initial machine people pose
Vector is (0,0,0)T, using the three-dimensional artificial environment of ROS, Gazebo and RVIZ, and use the laser radar mould in Gazebo
It is quasi- to obtain virtual laser data, according to first frame laser data initialization context map.
Further, the scan matching uses the scan matching algorithm based on possibility regional model.
Further, the weight computing and resampling include the weight computing mode such as following formula 3-22 institute of m-th of particle
Show:
Wherein,For robot observation probability value；The resampling is determined using adaptive resampling technology
When resampling steps occur, as shown in following formula 3-23:
Wherein Neff is otherwise known as number of effective particles, carries out resampling steps when Neff value is less than a certain given threshold value,
Otherwise algorithm iteration next time is directly carried out after the completion of weight computing.
Further, the observation probability valueIt is obtained by establishing observation model.
Further, it is described establish a possibility that observation model includes using laser radar regional model to observation information into
Row modeling.
Further, in order to obtain the observation probability value, observation model introduces measurement noise and random measurement noise.
Further, the measurement noise is indicated using Gaussian Profile, to the extreme coordinates of any one group of laser dataThe Euclidean distance of itself and obstacle article coordinate nearest in existing map is calculated, then the sensor as caused by measurement noise
Measure probability value such as following formula 3-8:
Further, in the random measurement noise, when laser sensor hits translucent glass or sonar sensor
Unfixed random noise can be all generated when receiving same wave band sound wave, this random noise is indicated with being uniformly distributed, such as following formula 3-
10:
Further, observation probability value p (zt|xt, m) calculation formula 3-10 it is as follows:
Wherein q indicates the observation probability of each laser data, zhitIndicate the probability threshold value that measurement noise occurs, zranD table
Show the probability threshold value that random noise occurs, zhitMuch larger than zrandAnd the two and be 1.
The present invention has the advantage that and the utility model has the advantages that
In each iterative process, the robot location being calculated using Least squares matching expands particle collection,
Effectively some particles can be limited near true distribution, be avoided excessive when odometer error is larger in primary update interval
Mileage is relied on to count as estimation initial value.
Detailed description of the invention
Attached drawing described herein is used to provide to further understand the embodiment of the present invention, constitutes one of the application
Point, do not constitute the restriction to the embodiment of the present invention.In the accompanying drawings:
Fig. 1 is flow chart of the invention.
Fig. 2 is that the particle filter of thread I in the present invention initializes map schematic diagram.
Fig. 3 is laser data schematic diagram to be matched in the present invention.
Fig. 4 is the sampling schematic diagram that population is 30 in the present invention.
Fig. 5 a is particle sampler distribution map in the present invention.
Fig. 5 b is scan matching particle distribution figure in the present invention.
Fig. 6 is robot motion's track schematic diagram in the present invention.
Fig. 7 a is the range error contrast schematic diagram of scan matching in the present invention.
Fig. 7 b is the angular error contrast schematic diagram of scan matching in the present invention.
Fig. 8 is the time shaft schematic diagram that thread I and thread II update map in the present invention.
Fig. 9 a is odometer drift schematic diagram in the present invention.
Fig. 9 b is odometer drift results figure in the present invention.
Figure 10 a is local map location updating error schematic diagram in the present invention.
Figure 10 b is that local map angle updates error schematic diagram in the present invention.
Specific embodiment
To make the objectives, technical solutions, and advantages of the present invention clearer, below with reference to embodiment and attached drawing, to this
Invention is described in further detail, and exemplary embodiment of the invention and its explanation for explaining only the invention, are not made
For limitation of the invention.
In the following description, a large amount of specific details are elaborated in order to provide a thorough understanding of the present invention.However, for this
Field those of ordinary skill it is evident that: the present invention need not be carried out using these specific details.In other instances, it is
The present invention that avoids confusion, does not specifically describe well known structure, circuit, material or method.
Throughout the specification, meaning is referred to " one embodiment ", " embodiment ", " example " or " example "
: a particular feature, structure, or characteristic described in conjunction with this embodiment or example is comprised at least one embodiment of the invention.
Therefore, the phrase " one embodiment " in each place appearance of the whole instruction, " embodiment ", " example " or " example "
It is not necessarily all referring to the same embodiment or example.Furthermore, it is possible in any suitable combination and or sub-portfolio by specific feature,
Structure or characteristic combines in one or more embodiment or examples.In addition, it should be understood by one skilled in the art that herein
The diagram of offer is provided to the purpose of explanation, and diagram is not necessarily drawn to scale.Term used herein " and/
Or " include the project that one or more correlations are listed any and all combinations.
In the description of the present invention, it is to be understood that, term "front", "rear", "left", "right", "upper", "lower", " perpendicular
Directly ", the orientation or positional relationship of the instructions such as "horizontal", "high", " low " "inner", "outside" is orientation based on the figure or position
Relationship is merely for convenience of description of the present invention and simplification of the description, rather than the device or element of indication or suggestion meaning must have
There is specific orientation, be constructed and operated in a specific orientation, therefore should not be understood as limiting the scope of the invention.
Embodiment
A kind of particle filter SLAM method merging least square method, including thread I and thread II, the thread I are to adopt
Position of the robot in map is obtained with particle filter, thread II is that robot is obtained using Least squares matching in map
Position, specific steps include:
Step 1, when t=0, obtaining radar data obtains initial bit of the robot in map simultaneously for thread I and thread II
It sets, hereafter, robot starts to move；
Step 2, when robot moving distance is more than given threshold, into the t=1 moment, particle filter algorithm in thread I
Map inner machine people position is replaced using the sample that odometer motion model samples particle, and according to weight to grain
Son is ranked up；
Step 3, within t=0~t-1 period, the synchronous radar data that obtains of thread II obtains robot location x1', then
N number of particle that maximum weight in particle collection can be obtained, using least square method to x1The sample that ' sampling obtains replaces t=0~t-
1 period inner machine people position resets local map, the particle collection after being expanded, and the sum of particle becomes in particle collection
M+N；
Step 4, thread II is scanned matching, weight computing, resampling completion update to particle collection, wherein maximum weight
Position of the robot in map that represents of particle, that is, represent the position of robot at this time and environmental map data；
Step 5, thread II is ranked up particle collection, gives up the lesser N number of particle of weight, and total number of particles becomes M, due to
Thread I has been completed in t=1 moment map inner machine people's location updating, is reset in thread II to local map, to next t
=2 moment continued to execute step 2~5 when arriving, then thread II will do it multiple update in thread I a renewable time.
By long-term research and practice, the inventors of the present application found that in the positioning and map structuring of robot, by
In the heavy dependence that data robot shift position sampling process counts mileage, when ground has hollow or wheel to drift about
When, the sensor errors such as odometer are larger, and robot physical location does not change under limiting case, but mileage counts
Having shown that the mobile a distance of robot, such case can not only make SLAM algorithm be unable to get the accurate location of robot,
Phenomena such as tearing, distortion, can also occur for the grating map of creation.The application provides a kind of particle filter for merging least square method
SLAM method is executed including thread I and II dual-thread of thread to obtain position of the robot in map, since thread II is made
Time-consuming for particle filter algorithm, therefore thread II is repeatedly updated in t-1~t time interval.In thread II
The purpose of Least squares matching algorithm is exactly the robot displaced posi calculated in t-1~t time, while the application uses
The mode of local map, local map is reset after the completion of the particle filter system update in thread I.Its core is every time repeatedly
During generation, the robot location being calculated using Least squares matching expands particle collection, updates interval when primary
Some particles can be effectively limited near true distribution by interior odometer error when larger.
It is counted for particle filter heavy dependence mileage, present applicant proposes a kind of particle filters for merging least square method
Wave SLAM method.Its core is in each iterative process that the robot location being calculated using Least squares matching is to grain
Subset is expanded, when some particles effectively can be limited in true distribution by primary update when odometer error is larger in interval
Near.Algorithm overall flow is as shown in Figure 1, include thread I and thread II, the thread I is to obtain machine using particle filter
Position of the people in map, thread II is that position of the robot in map is obtained using Least squares matching, due to thread II
Time-consuming for used particle filter algorithm, therefore thread II is repeatedly updated in t-1~t time interval.Thread II
The purpose of interior Least squares matching algorithm is exactly the robot displaced posi calculated in t-1~t time, while the application
The mode for having used local map, local map is reset after the completion of particle filter system update in thread I.Specifically execute step
It is rapid as follows:
1) it initializes, two threads are performed simultaneously initialization step when bringing into operation, and the time is denoted as t=0 at this time.Particle filter
Initialization mode is that the starting of SLAM algorithm is to start at the t=0 moment to execute initialization step, due to not receiving any biography at this time
Sensor data, therefore it is (0,0,0) that the application, which defaults initial machine people pose vector,T, there are two the purposes of initialization step, and one
It is that particle collection is initialized according to total number of particles, second is that according to first frame laser data initialization context map, this map is by conduct
The input of successive iterations algorithm for estimating, the map that initialization step obtains are expressed as m0.Particle content after the completion of initialization is
Formula 3-17, wherein the weight of each particle is identical, it is 1/M.
When known machine people position, according only to sensing data can to map be updated, when original stateLaser light can be calculated according to formula 3-10 for all K group laser datas in a frame
Position of the Shu Duandian under global coordinate system simultaneously initializes map；
In formula 3-10, q indicates the observation probability of each laser data, zhitIndicate the probability threshold value that measurement noise occurs,
zrandIndicate the probability threshold value that random noise occurs, under normal circumstances zhitMuch larger than zrandAnd the two and be 1, specifically, setting
Map resolution ratio is 5cm, and the map obtained using RVIZ observation initialization step is as shown in Fig. 2, wherein black region indicates machine
Device people's peripheral obstacle, white area indicate idle accessible.Least squares matching thread then uses robot initial position more
New map.
2) when robot moving distance is more than given threshold, into the t=1 moment, in thread I particle filter algorithm according to
Odometer information samples particle, and is ranked up according to weight to particle；
3) due within t=0~t-1 period, II synchronous computer device people position of thread, then in available thread
Robot location x1', N number of particle of maximum weight in particle collection is obtained, to x1The sample that ' sampling obtains replaces its inner machine people
Position, the particle collection after being expanded, particle collection sum become M+N.
4) matching, weight computing, resampling completion update are scanned to particle collection, wherein particle institute's generation of maximum weight
The robot location of table and map illustrate the position of robot at this time and environmental map data.
Scan matching.The selection of the overall estimation precision and number of particles of particle filter system is closely related, but for
SLAM algorithm not only includes robot position data in each particle, and also maintenance portion includes the grid of multiple a grids
Figure.Number of particles, which crosses conference, increases whole system operation time, and algorithm availability reduces.In order to reduce number of particles, optimize
Particle sampler is as a result, the application has used the scan matching algorithm based on possibility regional model.Its core concept is to swash
Light data is matched with existing map, further improves robot location, particularly, Fig. 3 i.e. illustrate one group to be matchedly
Figure and laser data.
In particle filter algorithm, according to laser radar data and current grid map to each grain before weight computing step
Robot position data in son optimizes.As shown in formula 3-20:
For particle distribution shown in Fig. 4, the result after scan matching is as shown in figure 5 a and 5b, it can be seen that sweeps
Particle totally starts to change towards at robot actual position after retouching matching step.
Further, since the particle of t moment is to be calculated by the particle at t-1 moment by sampling, particulate errors meeting
It accumulates at any time.In order to preferably describe the effect of scan matching, it is assumed herein that the sample without resampling, i.e., in particle collection
It will not delete, the operation such as substitution.Definition error and E, each particle inner machine people position of expression t moment and actual position
Deviation.Such as following formula:
WhereinRespectively represent the actual position value of robot.
Using the emulation mode in Fig. 6, record whether there is or not the overall error of scan matching process and available Fig. 7 a and Fig. 7 b,
Wherein B lines indicate that no-raster matching process occurs, and A lines indicate the sum of the deviations after scan matching process, it can be seen that sweep
Retouching matching algorithm greatly improves particle concentration robot position distribution situation, makes it closer to robot actual position.But
With the increase of timestamp, robot is constantly moved, regardless of whether it is scanned matching, a general robot that particle collection represents
It sets error be all continuously increased, for this solution it is required that resampling steps improve particle content, by the biggish particle of error
Give up, retains accurate particle.
Weight computing and resampling.Calculating with weight in standard particle filtering algorithm is slightly different, the power of m-th of particle
It is worth calculation as shown in following formula 3-22:
Probability value in above formulaExactly observation probability value.The application has used adaptive resampling skill simultaneously
Art determines when resampling steps occur, such as following formula 3-23:
Wherein Neff is otherwise known as number of effective particles, carries out resampling steps when Neff value is less than a certain given threshold value,
Otherwise algorithm iteration next time is directly carried out after the completion of weight computing.It is similar with the resampling steps in standard particle filtering,
When resampling occurs, each particle determines that the retained probability of this particle, small weight particle can be in weights according to the size of its weight
It is replaced in sampling process by the biggish particle of weight, guarantees that total number of particles does not change.
5) particle collection is ranked up, gives up the lesser N number of particle of weight, total number of particles becomes M.Since thread I is in t=
The update at 1 moment has been completed, and local map is reset in thread II, continues to execute 2~5 steps when next t=2 moment arrives
Suddenly.Specifically, thread II will do it repeatedly more in a renewable time of thread I shown in renewal time axis Fig. 8 of two threads
Newly.
In order to describe the effect of algorithm after merging, wheel slip process is replaced using special circumstances shown in Fig. 9 a, is made
It is mobile to metope with Keyboard Control robot, cause mileage to count constantly to accumulate and robot is true since wheel persistently rotate
Position does not change.Table 1 is the distribution situation of t moment particle in the case of describing this, and table 1 indicates standard when odometer drift
Particle distribution, does not consider angle-data temporarily, and the robot location at t-1 moment is (2.37,1.97), the robot position of t moment
Being set to (2.71.1.97) and taking number of particles is 30.
Table 1
As it can be seen from table 1 leading to particle collection inner machine people position point due to the scan matching step that the t-1 moment carries out
Cloth is more concentrated, and since drift phenomenon odometer error is larger in t-1~t moment, in the particle that t moment samples very
Difficulty, which is found, can directly result in positioning accuracy decline with particle similar in actual position, this phenomenon, and map is torn, such as Fig. 9 b
It is shown.For above situation, using the SLAM algorithm of fusion Least squares matching, total number of particles is set as 25, every time when sampling
The population of expansion is 5, and the particle distribution of t moment is as shown in table 2 below, and table 2 indicates that improving particle filter when odometer drift calculates
Method particle distribution, it can be seen that due to the effect of Least squares matching in thread II, 5 grains of serial number 26~30 after expansion
Son, when other particles can not indicate real machine people position, remains to obtain one there is no being counted to be sampled according to mileage
A preferable estimated result.
Table 2
Same control robot is moved along track described in Fig. 6, is recorded each local map and is updated result and and machine
Device people is true, and amount of movement compares, the direction x, the direction y, angle error image as as-shown-in figures 10 a and 10b, it can be seen that
As robot is mobile, local map updates location error may remain in 10cm substantially, and angular error substantially remains in 0.1
In radian, precision is higher.
Above-described specific embodiment has carried out further the purpose of the present invention, technical scheme and beneficial effects
It is described in detail, it should be understood that being not intended to limit the present invention the foregoing is merely a specific embodiment of the invention
Protection scope, all within the spirits and principles of the present invention, any modification, equivalent substitution, improvement and etc. done should all include
Within protection scope of the present invention.
Claims (10)
1. a kind of particle filter SLAM method for merging least square method, which is characterized in that including II dual-thread of thread I and thread
It executes to obtain position of the robot in map, the thread I is that position of the robot in map is obtained using particle filter
It sets, thread II is that position of the robot in map is obtained using Least squares matching, and specific steps include:
Step 1, when t=0, obtaining radar data obtains initial position of the robot in map simultaneously for thread I and thread II, this
Afterwards, robot starts to move；
Step 2, when robot moving distance is more than given threshold, into the t=1 moment, particle filter algorithm is used in thread I
The sample that odometer motion model samples particle replaces map inner machine people position, and according to weight to particle into
Row sequence；
Step 3, within t=0~t-1 period, the synchronous radar data that obtains of thread II obtains robot location x '1, then can be obtained
N number of particle of maximum weight in particle collection, using least square method to x '1It samples obtained sample and replaces t=0~t-1 time
Section inner machine people position resets local map, the particle collection after being expanded, and the sum of particle becomes M+N in particle collection；
Step 4, thread II is scanned matching, weight computing, resampling completion update to particle collection, wherein the grain of maximum weight
Position of the robot of filial generation table in map represents the position of robot at this time and environmental map data；
Step 5, thread II is ranked up particle collection, gives up the lesser N number of particle of weight, total number of particles becomes M, due to thread
I has completed in t=1 moment map inner machine people's location updating, resets in thread II to local map, when next t=2
It is carved into when coming and continues to execute step 2~5, then thread II will do it multiple update in thread I a renewable time.
2. a kind of particle filter SLAM method for merging least square method according to claim 1, which is characterized in that described
The mode that thread I initializes robot location includes: that default initial machine people pose vector is (0,0,0)T, use
The three-dimensional artificial environment of ROS, Gazebo and RVIZ, and virtual laser data are obtained using the laser radar simulation in Gazebo,
According to first frame laser data initialization context map.
3. a kind of particle filter SLAM method for merging least square method according to claim 1, which is characterized in that described
Scan matching uses the scan matching algorithm based on possibility regional model.
4. a kind of particle filter SLAM method for merging least square method according to claim 1, which is characterized in that described
Weight computing and resampling include the weight computing mode of m-th of particle as shown in following formula 3-22:
Wherein,For robot observation probability value；The resampling is adopted again using the decision of adaptive resampling technology
When sample step occurs, as shown in following formula 3-23:
Wherein Neff is otherwise known as number of effective particles, carries out resampling steps when Neff value is less than a certain given threshold value, otherwise
Algorithm iteration next time is directly carried out after the completion of weight computing.
5. a kind of particle filter SLAM method for merging least square method according to claim 4, which is characterized in that described
Observation probability valueIt is obtained by establishing observation model.
6. a kind of particle filter SLAM method for merging least square method according to claim 5, which is characterized in that described
Establishing observation model includes being modeled using regional model a possibility that laser radar to observation information.
7. a kind of particle filter SLAM method for merging least square method according to claim 6, which is characterized in that in order to
The observation probability value is obtained, observation model introduces measurement noise and random measurement noise.
8. a kind of particle filter SLAM method for merging least square method according to claim 7, which is characterized in that described
Measurement noise is indicated using Gaussian Profile, to the extreme coordinates of any one group of laser dataCalculate it with existing
The Euclidean distance of nearest obstacle article coordinate in figure, then such as following formula 3-8 of the sensor measurement probability value as caused by measurement noise:
9. a kind of particle filter SLAM method for merging least square method according to claim 8, which is characterized in that described
In random measurement noise, all when laser sensor hits translucent glass or sonar sensor receives same wave band sound wave
Unfixed random noise can be generated, this random noise is indicated with being uniformly distributed, such as following formula 3-10:
10. a kind of particle filter SLAM method for merging least square method according to claim 9, which is characterized in that see
Survey probability value p (zt|xt, m) calculation formula 3-10 it is as follows:
Wherein q indicates the observation probability of each laser data, zhitIndicate the probability threshold value that measurement noise occurs, zrandIndicate random
The probability threshold value that noise occurs, zhitMuch larger than zrandAnd the two and be 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910621086.2A CN110333513B (en) | 2019-07-10 | 2019-07-10 | Particle filter SLAM method fusing least square method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910621086.2A CN110333513B (en) | 2019-07-10 | 2019-07-10 | Particle filter SLAM method fusing least square method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110333513A true CN110333513A (en) | 2019-10-15 |
| CN110333513B CN110333513B (en) | 2023-01-10 |
Family
ID=68146230
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910621086.2A Active CN110333513B (en) | 2019-07-10 | 2019-07-10 | Particle filter SLAM method fusing least square method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110333513B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110887489A (en) * | 2019-11-22 | 2020-03-17 | 深圳晨芯时代科技有限公司 | AR robot-based SLAM algorithm experimental method |
| CN113032411A (en) * | 2021-03-02 | 2021-06-25 | 上海景吾智能科技有限公司 | Method, system and medium for updating local map based on existing map |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100809352B1 (en) * | 2006-11-16 | 2008-03-05 | 삼성전자주식회사 | Method and apparatus of pose estimation in a mobile robot based on particle filter |
| CN104197958A (en) * | 2014-08-27 | 2014-12-10 | 北京航空航天大学 | Speedometer calibration method based on laser velocimeter dead reckoning system |
| CN104503339A (en) * | 2015-01-05 | 2015-04-08 | 黑龙江工程学院 | Multi-resolution indoor three-dimensional scene reconstitution device and method based on laser radar and quadrotor |
| US20150276783A1 (en) * | 2014-03-31 | 2015-10-01 | Stmicroelectronics S.R.I. | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method |
| CN105631017A (en) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | Method and device for achieving offline coordinate calibrating and map building |
| CN105865449A (en) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | Laser and vision-based hybrid location method for mobile robot |
| WO2016162568A1 (en) * | 2015-04-10 | 2016-10-13 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
| CN106211072A (en) * | 2016-07-14 | 2016-12-07 | 广东工业大学 | The localization method of a kind of small-sized movable primary user and device |
| CN106595659A (en) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | Map merging method of unmanned aerial vehicle visual SLAM under city complex environment |
| CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
| CN107240133A (en) * | 2017-04-24 | 2017-10-10 | 西华大学 | A kind of stereoscopic vision mapping model method for building up |
| CN107356252A (en) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | A kind of Position Method for Indoor Robot for merging visual odometry and physics odometer |
| CN107607107A (en) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人（深圳）有限公司 | A kind of Slam method and apparatus based on prior information |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| CN109579824A (en) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode information |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109752725A (en) * | 2019-01-14 | 2019-05-14 | 天合光能股份有限公司 | A kind of low speed business machine people, positioning navigation method and Position Fixing Navigation System |
-
2019
- 2019-07-10 CN CN201910621086.2A patent/CN110333513B/en active Active
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100809352B1 (en) * | 2006-11-16 | 2008-03-05 | 삼성전자주식회사 | Method and apparatus of pose estimation in a mobile robot based on particle filter |
| US20150276783A1 (en) * | 2014-03-31 | 2015-10-01 | Stmicroelectronics S.R.I. | Positioning apparatus comprising an inertial sensor and inertial sensor temperature compensation method |
| CN104197958A (en) * | 2014-08-27 | 2014-12-10 | 北京航空航天大学 | Speedometer calibration method based on laser velocimeter dead reckoning system |
| CN104503339A (en) * | 2015-01-05 | 2015-04-08 | 黑龙江工程学院 | Multi-resolution indoor three-dimensional scene reconstitution device and method based on laser radar and quadrotor |
| WO2016162568A1 (en) * | 2015-04-10 | 2016-10-13 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
| CN105631017A (en) * | 2015-12-29 | 2016-06-01 | 福州华鹰重工机械有限公司 | Method and device for achieving offline coordinate calibrating and map building |
| CN105865449A (en) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | Laser and vision-based hybrid location method for mobile robot |
| CN106211072A (en) * | 2016-07-14 | 2016-12-07 | 广东工业大学 | The localization method of a kind of small-sized movable primary user and device |
| CN106595659A (en) * | 2016-11-03 | 2017-04-26 | 南京航空航天大学 | Map merging method of unmanned aerial vehicle visual SLAM under city complex environment |
| CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
| CN107240133A (en) * | 2017-04-24 | 2017-10-10 | 西华大学 | A kind of stereoscopic vision mapping model method for building up |
| CN107356252A (en) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | A kind of Position Method for Indoor Robot for merging visual odometry and physics odometer |
| CN107607107A (en) * | 2017-09-14 | 2018-01-19 | 斯坦德机器人（深圳）有限公司 | A kind of Slam method and apparatus based on prior information |
| CN108917759A (en) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | Mobile robot pose correct algorithm based on multi-level map match |
| CN109579824A (en) * | 2018-10-31 | 2019-04-05 | 重庆邮电大学 | A kind of adaptive Kano Meng Te localization method incorporating two-dimensional barcode information |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109752725A (en) * | 2019-01-14 | 2019-05-14 | 天合光能股份有限公司 | A kind of low speed business machine people, positioning navigation method and Position Fixing Navigation System |
Non-Patent Citations (3)
| Title |
|---|
| WEI WANG 等: ""Simultaneous localization and mapping embedded with particle filter algorithm"", 《2016 10TH EUROPEAN CONFERENCE ON ANTENNAS AND PROPAGATION (EUCAP)》 * |
| 曲丽萍: ""移动机器人同步定位与地图构建关键技术的研究"", 《中国博士论文全文数据库》 * |
| 赵立军: ""室内环境下同步定位与地图创建改进算法"", 《机器人》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110887489A (en) * | 2019-11-22 | 2020-03-17 | 深圳晨芯时代科技有限公司 | AR robot-based SLAM algorithm experimental method |
| CN113032411A (en) * | 2021-03-02 | 2021-06-25 | 上海景吾智能科技有限公司 | Method, system and medium for updating local map based on existing map |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110333513B (en) | 2023-01-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113781582B (en) | Synchronous positioning and map creation method based on laser radar and inertial navigation combined calibration | |
| CN109798896B (en) | Indoor robot positioning and mapping method and device | |
| CN105467838B (en) | A kind of synchronous superposition method under stochastic finite collection framework | |
| CN111324848B (en) | Vehicle-mounted track data optimization method of mobile laser radar measurement system | |
| CN107063264A (en) | A kind of robot map creating method suitable for extensive substation | |
| Pan et al. | Real-time collision detection and distance computation on point cloud sensor data | |
| CN113110455B (en) | Multi-robot collaborative exploration method, device and system for unknown initial state | |
| CN103901891A (en) | Dynamic particle tree SLAM algorithm based on hierarchical structure | |
| CN110333513A (en) | A kind of particle filter SLAM method merging least square method | |
| CN108763811A (en) | Dynamic data drives forest fire appealing prediction technique | |
| CN103839274B (en) | A kind of Extended target tracking based on geometric proportion relation | |
| CN110986956A (en) | Autonomous learning global positioning method based on improved Monte Carlo algorithm | |
| CN108492324A (en) | Aircraft method for tracing based on fully-connected network and Kalman filter | |
| CN114581619A (en) | Coal bunker modeling method based on three-dimensional positioning and two-dimensional mapping | |
| CN114119920A (en) | Three-dimensional point cloud map construction method and system | |
| CN108564600A (en) | Moving object attitude tracking method and device | |
| CN107967675A (en) | A kind of structuring point cloud denoising method based on adaptive projection Moving Least Squares | |
| Zhu et al. | Vdb-edt: An efficient euclidean distance transform algorithm based on vdb data structure | |
| CN110726416A (en) | Reinforced learning path planning method based on obstacle area expansion strategy | |
| Jiang et al. | 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments | |
| CN111761583B (en) | Intelligent robot motion positioning method and system | |
| CN111951341A (en) | Closed loop detection improvement method based on RGB-D SLAM | |
| CN116380039A (en) | Mobile robot navigation system based on solid-state laser radar and point cloud map | |
| CN114310872B (en) | Automatic vegetable-beating method for mechanical arm based on DGG point cloud segmentation network | |
| CN113656918B (en) | Four-rotor simulation test method applied to finished product overhead warehouse scene |
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 |