CN109556611B - Fusion positioning method based on graph optimization and particle filtering - Google Patents
Fusion positioning method based on graph optimization and particle filtering Download PDFInfo
- Publication number
- CN109556611B CN109556611B CN201811459729.XA CN201811459729A CN109556611B CN 109556611 B CN109556611 B CN 109556611B CN 201811459729 A CN201811459729 A CN 201811459729A CN 109556611 B CN109556611 B CN 109556611B
- Authority
- CN
- China
- Prior art keywords
- particles
- particle
- map
- pose
- robot
- 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.)
- Active
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
Abstract
The invention belongs to the field of synchronous positioning and map building (SLAM) of a mobile robot, and particularly relates to a fusion positioning method based on map optimization and particle filtering. Generating a sub map with rich local information based on a map optimization method, and screening particles generated by sampling in the global map according to the sub map, so that the particle distribution area is reduced to a great extent, and the convergence speed of the particles in the repositioning process is accelerated; secondly, clustering the screened particles, and generating new particles by re-randomly sampling according to a clustering result, so that the particle set can well cover a real pose, and the robot can rapidly and accurately complete relocation in a large map.
Description
Technical Field
The invention belongs to the field of synchronous positioning and map building (SLAM) of mobile robots, and particularly relates to a fusion positioning method based on graph optimization and particle filtering.
Background
The omnidirectional mobile robot can realize the movement in any direction and can be widely applied to the fields of military, industry, household service and the like. Simultaneous localization and map creation (SLAM) of a mobile robot is a hot research problem in the robot field, which is a precondition and basis for self-service task planning and path planning of the mobile robot. The SLAM problem of a robot is that in an unknown environment, a mobile robot needs to establish an environment map and locate itself on the map at the same time. This process is similar to a person walking into a completely unfamiliar environment, knowing the environment and determining their location based only on observations of the surrounding environment and estimates of their own motion, without carrying any equipment that can determine location and orientation. SLAM is essentially a system state estimation problem (including the current pose of the robot and all map feature locations, etc.). From this point of view, the solving method can be roughly classified into Kalman filter-based method, particle filter-based method, and graph optimization-based method 3. In the existing scheme, a particle filter-based positioning method comprises the following steps: and sampling the pose of the robot through a motion model to generate a large number of particles, updating the weight of the particles according to the observation result of the sensor, resampling, and continuously iterating to make the particles converge. The prior art has the following disadvantages: in a large map, the repositioning convergence speed based on particle filtering is slow, and if the particle set does not well cover the real pose, the particles cannot be converged to the correct pose finally.
Disclosure of Invention
In order to solve the technical defects in the prior art, the invention provides a fusion positioning method based on graph optimization and particle filtering, and the relocation of a robot in a large map can be quickly converged to a correct pose through the fusion positioning of the graph optimization and the particle filtering.
The invention is realized by the following technical scheme:
a fusion positioning method based on graph optimization and particle filtering comprises the following steps:
s1: generating a sub-map based on a map optimization method;
s2: globally sampling to generate particle sets, and randomly sampling all free areas in a map to generate particle sets representing the pose of the robot;
s3: screening particles;
s4: clustering the particles;
s5: resampling particles;
s6: updating the set of particles according to the motion model;
s7: updating the particle weight;
s8: resampling the particles according to the weight of the particles, and resetting the weight;
s9: and jumping to S6 until the particles converge, and obtaining the weighted average of the converged particles as the final positioning result.
Further, the step S1 of generating the sub-map based on the graph optimization method further includes,
1) the robot rotates a circle in situ, and the robot pose x in the motion process is recorded according to the robot motion model1:tAnd observed value z under the pose1:tThe laser point cloud data is the observed value;
2) constructing a graph, taking the pose of the robot as a vertex, namely a variable needing to be optimized, and taking the constraints of a robot motion model and an observation model as edges, wherein an objective function is as follows:
in the formula x0To an initial pose, Ω0As initial pose covariance, g (u)t,xt-1) As a model of robot motion utTo control the amount, RtAs the covariance of the motion noise, h (m)t,xt) For observation of the model, mtAs a map feature, QtTo observe the noise covariance;
3) optimizing the constructed graph by taking the vertex as an optimization variable so as to obtain an optimized robot pose;
4) and generating a sub-map according to the optimized pose of the robot and the laser point cloud data.
Further, the optimization map in step 3) further includes: adjusting the pose x of the robot at each moment1:tIt is made to satisfy the constraints of the edges as much as possible, i.e. to minimize the objective function J.
Further, the particle screening in step S3 further includes mapping the sub-map in S1 to the global map according to the pose of each particle, retaining the particles with high matching degree, and removing the particles with low matching degree.
Further, the clustering of the particles in step S4 further includes clustering the particles retained in S3 based on a hierarchical clustering algorithm, and the clustering result is denoted as X ═ X1,X2,...,XnWhere n is the number of categories.
Further, the resampling the particles in step S5 further includes averaging the clustered particles in step S4 to obtain average values
Based on the Gaussian distribution
Sampling near the middle pose generates a new set of particles, noted as:
wherein the particle weight is initialized to
Further, the updating the particle set in step S6 further includes updating the particle state according to the motion model, that is, updating the particle state
In the formula utTo control quantity, vtIs noise.
Further, the updating of the particle weight in step S7 further includes mapping the current laser point cloud data to a global map according to the pose of each particle, and updating the particle weight according to the matching degree, that is, the particles with high matching degree obtain a large weight, and the particles with low matching degree obtain a small weight; the normalized weights are:
further, the step S8 resets the weight to
The present invention also includes a non-volatile storage medium comprising one or more computer instructions that when executed perform the fusion positioning method described above.
Compared with the prior art, the invention has at least the following beneficial effects or advantages: according to the scheme, the robot can rapidly and accurately complete relocation in the geodetic map through a fusion positioning method based on graph optimization and particle filtering. Firstly, generating a sub-map with rich local information based on a map optimization method, and screening particles generated by sampling in a global map according to the sub-map, so that the distribution area of the particles is reduced to a great extent, and the convergence rate of the particles in the repositioning process is accelerated; secondly, clustering the screened particles, and generating new particles by re-randomly sampling according to a clustering result, so that the particle set can well cover a real pose, and the robot can rapidly and accurately complete relocation in a large map.
Detailed Description
The technical solutions in the embodiments of the present invention are clearly and completely described below, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention relates to a fusion positioning method based on graph optimization and particle filtering, which specifically comprises the following steps:
step 1: and generating the sub-map based on a map optimization method.
And (4) the robot rotates in place for a circle, and the current pose and the laser point cloud data of the robot are recorded every time one frame of laser data is received. And constructing a graph with the pose of the robot as a vertex and the pose relationship as an edge. And the laser point cloud data under each pose is observed quantity. And optimizing the constructed map by taking the peak as an optimization variable so as to obtain an optimized robot pose, and finally generating a sub-map according to the robot pose and the laser point cloud data.
The robot rotates a circle in situ, and the robot pose x in the motion process is recorded according to the robot motion model1:tAnd observed value z under the pose1:t. The laser point cloud data is the observed value.
Firstly, a graph is constructed, the pose of the robot is taken as a vertex, namely a variable needing to be optimized, and the constraint of a robot motion model and an observation model is taken as an edge, so that the target function is as follows:
in the formula x0To an initial pose, Ω0As initial pose covariance, g (u)t,xt-1) As a model of robot motion utTo control the amount, RtAs the covariance of the motion noise, h (m)t,xt) For observation of the model, mtAs a map feature, QtTo observe the noise covariance.
Then optimizing the graph, namely adjusting the pose x of the robot at each moment1:tIt is made to satisfy the constraints of the edges as much as possible, i.e. to minimize the objective function J. And finally, generating a sub-map by the optimized pose of the robot and the corresponding laser point cloud data.
Step 2: global sampling of a particle subset
Random sampling of all free areas in the map generates a set of particles that represent the robot pose.
Step 3: particle screening
According to the pose of each particle, the sub-map in Step1 is mapped to the global map, the particles with high matching degree are reserved, and the particles with low matching degree are removed
Step 4: particle clustering
Based on a hierarchical clustering algorithm, clustering is carried out on the particles reserved in Step3, and the clustering result is recorded as X ═ X1,X2,...,XnWhere n is the number of categories.
Step 5: particle resampling
Respectively averaging the particles clustered in Step4 to obtain
Based on the Gaussian distribution
Sampling near the middle pose to generate a new set of particles, which is recorded as
Wherein the particle weight is initialized to
Step 6: updating the particle set:
updating the state of the particles according to a motion model, i.e.
In the formula utTo control quantity, vtIs noise.
Step 7: updating particle weights
And mapping the current laser point cloud data to a global map according to the pose of each particle, and updating the weight of the particles according to the matching degree, namely obtaining a large weight by the particles with high matching degree and obtaining a small weight by the particles with low matching degree. Then the normalized weight is
Step 8: resampling the particles according to their weights and resetting the weights to
Step 9: and jumping to Step6 until the particles converge, and obtaining the weighted average of the converged particles as the final positioning result.
The present invention also provides a non-volatile storage medium comprising one or more computer instructions that, when executed, implement the above-described fused positioning method.
The above-mentioned embodiments are provided to further explain the objects, technical solutions and advantages of the present invention in detail, and it should be understood that the above-mentioned embodiments are only examples of the present invention and are not intended to limit the scope of the present invention. Any modification, equivalent replacement, improvement and the like made without departing from the spirit and scope of the invention are also within the protection scope of the invention.
Claims (9)
1. A fusion positioning method based on graph optimization and particle filtering is characterized by comprising the following steps:
s1: generating a sub-map based on a map optimization method;
s2: globally sampling to generate particle sets, and randomly sampling all free areas in a map to generate particle sets representing the pose of the robot;
s3: screening particles;
s4: clustering the particles;
s5: resampling particles;
s6: updating the set of particles according to the motion model;
s7: updating the particle weight;
s8: resampling the particles according to the weight of the particles, and resetting the weight;
s9: jumping to S6 until the particles converge, and obtaining the weighted average of the converged particles as the final positioning result;
the sub-map is generated based on the graph optimization method in step S1, including,
1) the robot rotates a circle in situ, and the robot pose x in the motion process is recorded according to the robot motion model1:tAnd observed value z under the pose1:tThe laser point cloud data is the observed value;
2) constructing a graph, taking the pose of the robot as a vertex, namely a variable needing to be optimized, and taking the constraints of a robot motion model and an observation model as edges, wherein an objective function is as follows:
in the formula x0To an initial pose, Ω0As initial pose covariance, g (u)t,xt-1) As a model of robot motion utTo control the amount, RtAs the covariance of the motion noise, h (m)t,xt) For observation of the model, mtAs a map feature, QtTo observe the noise covariance;
3) optimizing the constructed graph by taking the vertex as an optimization variable so as to obtain an optimized robot pose;
4) and generating a sub-map according to the optimized pose of the robot and the laser point cloud data.
2. The method for fusion localization based on graph optimization and particle filtering according to claim 1, wherein the optimization graph of step 3) further comprises: adjusting the pose x of the robot at each moment1:tIt is made to satisfy the constraints of the edges as much as possible, i.e. to minimize the objective function J.
3. The method of fusion localization based on graph optimization and particle filtering according to claim 2, wherein the particle filtering in step S3 further comprises mapping the sub-map in S1 to the global map according to the pose of each particle.
4. The fusion positioning method based on graph optimization and particle filtering according to claim 3, wherein the clustering of the particles in step S4 further comprises clustering the particles retained in S3 based on a hierarchical clustering algorithm, and the clustering result is denoted as X ═ X { (X)1,X2,...,XnWhere n is the number of categories.
5. The fusion localization method according to claim 4, wherein the resampling of the particles in step S5 further comprises averaging the clustered particles in S4 to obtain the average values
Based on the Gaussian distribution
Sampling near the middle pose generates a new set of particles, noted as:
wherein the particle weight is initialized to
6. The fusion localization method based on graph optimization and particle filtering according to claim 5, wherein the updating the particle set in step S6 further comprises updating the particle state according to a motion model
In the formula utTo control quantity, vtIs noise.
7. The fusion positioning method based on graph optimization and particle filtering according to claim 6, wherein the updating the particle weights in step S7 further comprises mapping the current laser point cloud data to a global map according to the pose of each particle, and updating the particle weights according to the matching degree, i.e. the particles with high matching degree obtain a large weight and the particles with low matching degree obtain a small weight; the normalized weights are:
8. the fusion localization method based on graph optimization and particle filtering of claim 7, wherein the step S8 resets the weight to
9. A non-volatile storage medium comprising one or more computer instructions which, when executed, implement the method of any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811459729.XA CN109556611B (en) | 2018-11-30 | 2018-11-30 | Fusion positioning method based on graph optimization and particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811459729.XA CN109556611B (en) | 2018-11-30 | 2018-11-30 | Fusion positioning method based on graph optimization and particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109556611A CN109556611A (en) | 2019-04-02 |
| CN109556611B true CN109556611B (en) | 2020-11-10 |
Family
ID=65868342
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811459729.XA Active CN109556611B (en) | 2018-11-30 | 2018-11-30 | Fusion positioning method based on graph optimization and particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109556611B (en) |
Families Citing this family (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110954113B (en) * | 2019-05-30 | 2021-10-15 | 北京初速度科技有限公司 | Vehicle pose correction method and device |
| CN110263905B (en) * | 2019-05-31 | 2021-03-02 | 上海电力学院 | Robot positioning and mapping method and device based on firefly optimized particle filtering |
| CN110260855B (en) * | 2019-06-24 | 2021-06-29 | 北京壹氢科技有限公司 | Indoor pedestrian navigation positioning method integrating pedestrian dead reckoning, geomagnetic information and indoor map information |
| CN110645998B (en) * | 2019-09-10 | 2023-03-24 | 上海交通大学 | Dynamic object-free map segmentation establishing method based on laser point cloud |
| CN110749901B (en) * | 2019-10-12 | 2022-03-18 | 劢微机器人科技(深圳)有限公司 | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium |
| CN110598804B (en) * | 2019-10-14 | 2023-05-09 | 安徽理工大学 | Improved FastSLAM method based on clustering and membrane calculation |
| CN111061287B (en) * | 2019-12-31 | 2022-05-27 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method based on particle self-convergence |
| CN111061276B (en) * | 2019-12-31 | 2022-07-26 | 芜湖哈特机器人产业技术研究院有限公司 | Mobile robot repositioning method based on dynamic area division |
| CN111369640B (en) * | 2020-02-28 | 2024-03-26 | 广州高新兴机器人有限公司 | Multi-robot mapping method, system, computer storage medium and electronic equipment |
| CN112097772B (en) * | 2020-08-20 | 2022-06-28 | 深圳市优必选科技股份有限公司 | Robot and map construction method and device thereof |
| CN112070201A (en) * | 2020-08-26 | 2020-12-11 | 成都睿芯行科技有限公司 | Method for increasing mapping speed based on Gmapping |
| CN114323035A (en) * | 2020-09-30 | 2022-04-12 | 华为技术有限公司 | Positioning method, device and system |
| CN114485607B (en) * | 2021-12-02 | 2023-11-10 | 陕西欧卡电子智能科技有限公司 | Method, operation equipment, device and storage medium for determining motion trail |
| TWM637241U (en) * | 2022-07-22 | 2023-02-01 | 杜宇威 | Computing apparatus and model generation system |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107680133A (en) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103901895B (en) * | 2014-04-18 | 2014-10-29 | 江苏久祥汽车电器集团有限公司 | Target positioning method based on unscented FastSLAM algorithm and matching optimization and robot |
| US10075818B2 (en) * | 2016-09-13 | 2018-09-11 | Google Llc | Systems and methods for graph-based localization and mapping |
| CN107246873A (en) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | A kind of method of the mobile robot simultaneous localization and mapping based on improved particle filter |
| CN108303710B (en) * | 2018-06-12 | 2018-11-02 | 江苏中科院智能科学技术应用研究院 | Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar |
-
2018
- 2018-11-30 CN CN201811459729.XA patent/CN109556611B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107680133A (en) * | 2017-09-15 | 2018-02-09 | 重庆邮电大学 | A kind of mobile robot visual SLAM methods based on improvement closed loop detection algorithm |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109556611A (en) | 2019-04-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109556611B (en) | Fusion positioning method based on graph optimization and particle filtering | |
| Xuexi et al. | SLAM algorithm analysis of mobile robot based on lidar | |
| Kümmerle et al. | Monte carlo localization in outdoor terrains using multilevel surface maps | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN109798896B (en) | Indoor robot positioning and mapping method and device | |
| CN109144056A (en) | The global method for self-locating and equipment of mobile robot | |
| CN102395192B (en) | Method and device for locating wireless sensor terminal | |
| Yu et al. | Probabilistic path planning for cooperative target tracking using aerial and ground vehicles | |
| CN112882056B (en) | Mobile robot synchronous positioning and map construction method based on laser radar | |
| CN111061276B (en) | Mobile robot repositioning method based on dynamic area division | |
| Geng et al. | UAV surveillance mission planning with gimbaled sensors | |
| CN113483747A (en) | Improved AMCL (advanced metering library) positioning method based on semantic map with corner information and robot | |
| Maddern et al. | Continuous appearance-based trajectory SLAM | |
| CN108426582B (en) | Indoor three-dimensional map matching method for pedestrians | |
| Leung et al. | An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping | |
| Li et al. | A new RSS fingerprinting-based location discovery method under sparse reference point conditions | |
| Yang et al. | Vision-based localization and mapping for an autonomous mower | |
| CN114066773B (en) | Dynamic object removal based on point cloud characteristics and Monte Carlo expansion method | |
| Dengler et al. | Online object-oriented semantic mapping and map updating | |
| CN117501322A (en) | Generating a map of physical space from point cloud data | |
| CN109931940A (en) | A kind of robot localization method for evaluating confidence based on monocular vision | |
| CN112857379A (en) | Improved Gmapping-SLAM map updating method and system | |
| CN113810846B (en) | Indoor positioning method based on WiFi and IMU fusion | |
| Kwon et al. | A new feature commonly observed from air and ground for outdoor localization with elevation map built by aerial mapping system | |
| Li et al. | An SLAM Algorithm Based on Laser Radar and Vision Fusion with Loop Detection Optimization |
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 | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
| PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A fusion localization method based on graph optimization and particle filter Effective date of registration: 20210615 Granted publication date: 20201110 Pledgee: Bank of China Limited by Share Ltd. Guangzhou Tianhe branch Pledgor: GUANGZHOU GOSUNCN ROBOT Co.,Ltd. Registration number: Y2021440000203 |