CN111061287A - Mobile robot repositioning method based on particle self-convergence - Google Patents
Mobile robot repositioning method based on particle self-convergence Download PDFInfo
- Publication number
- CN111061287A CN111061287A CN201911403712.7A CN201911403712A CN111061287A CN 111061287 A CN111061287 A CN 111061287A CN 201911403712 A CN201911403712 A CN 201911403712A CN 111061287 A CN111061287 A CN 111061287A
- Authority
- CN
- China
- Prior art keywords
- grid
- particle
- particles
- pose
- particle set
- 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
Images
Landscapes
- Manipulator (AREA)
Abstract
The invention is suitable for the technical field of robot positioning, and provides a mobile robot repositioning method based on particle self-convergence, which comprises the following steps: s1, generating a grid particle set _ a based on the grid map, wherein the grid particle set _ a is composed of grid particles, and the grid particles are grid points pi(xi,yi) The optimal pose of (1); s2, updating the particles in the grid particle set _ a and the corresponding scores; s3, resampling the particles in the grid particle set _ a, copying the resampled particles to a new particle set _ b, enabling the weights of the particles in the particle set _ b to be equal, S4, calculating the pose standard deviations of all the particles in the particle set _ b, detecting whether the pose standard deviations of all the particles are smaller than a preset threshold, if the detection result is yes, taking the average pose of all the particles in the set _ b in the particle set as the current repositioning pose of the robot, and if the detection result is no, executing the step S2. After the robot loses the pose, the pose of the robot can be quickly and accurately recovered, namely, the robot is repositioned without manual intervention.
Description
Technical Field
The invention belongs to the technical field of robot positioning, and provides a mobile robot repositioning method based on particle self-convergence.
Background
With the development of society and the advancement of technology, mobile robots are increasingly involved in human daily lives, such as cleaning robots in homes, transfer robots in factories, and meal delivery robots in restaurants. The mobile robot needs to accurately know the position of the mobile robot when the mobile robot wants to realize the functions, and sensors commonly used for positioning and navigation of the mobile robot comprise a magnetic navigation sensor, a camera, a laser radar and the like. The navigation path of magnetic navigation is single, expansion and change are not facilitated, and the use limitation is large. The positioning and navigation method based on vision is greatly influenced by light source conditions, has poor stability and precision, and cannot enable the mobile robot to perform stable and precise operation. The positioning and navigation method based on the laser radar can be divided into a positioning and navigation method based on a reflector and a positioning and navigation method based on a contour, wherein the positioning and navigation method based on the reflector needs to arrange a large number of road signs in the environment, and the application scene is more limited. The positioning and navigation method based on the contour is more and more widely applied to positioning and navigation of the mobile robot due to the flexible path and no need of arranging artificial marks. However, in the existing contour-based positioning and navigation method, when the mobile robot has situations of slipping drift, artificial movement, restart or shutdown, etc., the robot positioning fails and needs to be repositioned, so that the repositioning problem of the mobile robot is very important in the contour-based positioning and navigation application.
The existing relocation problem solutions mainly include the following three types: 1) the robot is returned to the initial position, and after the positioning fails, the robot is manually pushed to the initial position to be restarted; 2) comparing the map of the robot positioning navigation with the real environment, and manually marking the pose of the robot; however, these solutions have certain problems in practical application: the above schemes all require manual intervention, and the efficiency and accuracy of relocation cannot be guaranteed.
Disclosure of Invention
The embodiment of the invention provides a mobile robot repositioning method based on particle self-convergence, which can automatically, quickly and accurately recover the pose of a robot under the condition that the pose of the mobile robot is lost due to restarting, slipping or other reasons.
The invention is realized in such a way that a mobile robot repositioning method based on particle self-convergence specifically comprises the following steps:
s1, generating a grid particle set _ a based on the grid map, wherein the grid particle set _ a is composed of grid particles, and the grid particles are grid points pi(xi,yi) The optimal pose of (1);
s2, updating the particles in the grid particle set _ a and the corresponding scores;
s3, resampling the particles in the grid particle set a, copying the resampled particles to a new particle set b, wherein the weights of the particles in the particle set b are equal,
s4, calculating the pose standard deviations of all the particles in the set _ b of the particle set, detecting whether the pose standard deviations of all the particles are smaller than a preset threshold, if so, taking the average pose of all the particles in the set _ b of the particle set as the current repositioning pose of the robot, and if not, executing the step S2.
Further, the method for generating the set _ a of grid particles specifically includes the following steps:
s11, taking a grid point p on the grid map at intervals of a set distance delta di(xi,yi)；
S12, detecting whether the grid where the grid-taking mesh point is located is occupied by an obstacle;
s13, if the detection result is negative, obtaining the lattice point pi(xi,yi) If the detection result is yes, taking the next grid point on the grid map based on the set distance delta d, and executing step S12;
and S14, traversing all grid points on the grid map, and outputting a grid particle set _ a.
Further, the grid points pi(xi,yi) The method for acquiring the optimal pose specifically comprises the following steps:
from 0 degrees to 360 degrees, each set angular step Δ θ is taken at an angle θjAngle thetajAnd grid point pi(xi,yi) Form a pose pi,j(xi,yi,θj)；
Pose p pair using likelihood domain modeli,j(xi,yi,θj) Scoring and traversing the grid points pi(xi,yi) All angles and the highest scoring pose are lattice points pi(xi,yi) The optimal pose of (1).
Further, the method for updating particles in the grid particle set _ a specifically includes the following steps:
s21, selecting one particle p in the grid particle set _ a in sequencei(xi,yi,θi) And in the particle pi(xi,yi,θi) Generating k Gaussian random poses around;
s22 calculating particle p by using likelihood domain modeli(xi,yi,θi) And scoring the k random pose sets to obtain the highest score wmaxAnd corresponding pose pmaxLet the current pose equal to the pose p with the highest scorei＝pmaxThe weight of the current pose is equal to the highest score, wi＝wmax；
And S23, traversing all the particles in the particle grid particle set _ a, and finishing the updating and scoring in the grid particle set _ a.
Further, the particle resampling process is specifically as follows:
s31, carrying out descending order arrangement on the weight of the particles in the grid particle set;
s32, calculating the number copy _ count of the particles to be copied based on the particle copy ratio, setting the particle copy ratio as r, calculating the number copy _ count of the particles to be copied as r set _ count of the current grid particle set;
s33, constructing an empty particle set _ b, copying the top copy _ count particles to the particle set _ b, and setting the weight of all the particles in the particle set _ b as 1/copy _ count;
and S34, replacing the grid particle set _ a with the particle set _ b.
The mobile robot repositioning method based on particle self-convergence provided by the invention has the following beneficial effects: after the robot loses the pose, the pose of the robot can be quickly and accurately recovered, namely, the robot is repositioned without manual intervention.
Drawings
Fig. 1 is a flowchart of a method for repositioning a mobile robot based on particle self-convergence according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Fig. 1 is a flowchart of a method for repositioning a mobile robot based on particle self-convergence according to an embodiment of the present invention, where the method specifically includes the following steps:
s1, generating a grid particle set _ a based on the grid map, wherein the grid particle set _ a is composed of grid particles, and the grid particles are grid points pi(xi,yi) The optimal pose of (1);
before generating the grid particle set, an occupancy grid map needs to be constructed, and the method for generating the grid particle set _ a specifically includes the following steps:
s11, taking a grid point p on the grid map at intervals of a set distance delta di(xi,yi)；
S12, detecting whether the grid where the grid-taking mesh point is located is occupied by an obstacle;
s13, if the detection result is negative, obtaining the lattice point pi(xi,yi) If the detection result is yes, taking the next grid point on the grid map based on the set distance delta d, and executing the stepStep S12, traversing all grid points on the grid map, and outputting a grid particle set _ a;
in an embodiment of the invention, the grid points pi(xi,yi) The method for acquiring the optimal pose specifically comprises the following steps:
from 0 degrees to 360 degrees, each set angular step Δ θ is taken at an angle θjAngle thetajAnd grid point pi(xi,yi) Form a pose pi,j(xi,yi,θj)；
Pose p pair using likelihood domain modeli,j(xi,yi,θj) Scoring and traversing the grid points pi(xi,yi) All angles and the highest scoring pose are lattice points pi(xi,yi) The optimal pose of (1).
S2, updating the particles in the grid particle set _ a and the corresponding scores;
in the embodiment of the present invention, the method for updating particles in the grid particle set _ a specifically includes the following steps:
s21, selecting one particle p in the grid particle set _ a in sequencei(xi,yi,θi) And in the particle pi(xi,yi,θi) Generating k Gaussian random poses around;
s22 calculating particle p by using likelihood domain modeli(xi,yi,θi) And scoring the k random pose sets to obtain the highest score wmaxAnd corresponding pose pmaxLet the current pose equal to the pose p with the highest scorei＝pmaxThe weight of the current pose is equal to the highest score, wi＝wmax；
And S23, traversing all the particles in the particle grid particle set _ a, and finishing the updating and scoring in the grid particle set _ a.
S3, resampling the particles in the grid particle set _ a, copying the resampled particles to a new particle set _ b, wherein the weights of the particles in the particle set _ b are equal, and the particle resampling process specifically comprises the following steps:
s31, carrying out descending order arrangement on the weight of the particles in the grid particle set;
s32, calculating the number copy _ count of the particles to be copied based on the particle copy ratio, setting the particle copy ratio as r, calculating the number copy _ count of the particles to be copied as r set _ count of the current grid particle set;
s33, constructing an empty particle set _ b, copying the top copy _ count particles to the particle set _ b, and setting the weight of all the particles in the particle set _ b as 1/copy _ count;
and S34, replacing the grid particle set _ a with the particle set _ b.
S4, calculating the pose standard deviations of all the particles in the particle set _ b, detecting whether the pose standard deviations of all the particles are smaller than a preset threshold, if so, taking the average pose of all the particles in the particle set _ b as the current repositioning pose of the robot, and if not, executing the step S2.
And calculating the standard deviation of the poses of all the particles in the new particle set, wherein the calculation formula is as follows:
where n is the number of particles in the set of particles set b,
respectively, the mean of the three components of all particle poses in the set of particles set _ b. If the standard deviation of all the particle poses in the particle set _ b meets the set condition, that is:
wherein ξ (x), ξ (y) and ξ (theta) are threshold values set in advance, the current average pose is output
As a movementOtherwise, returning to the step S2 to continue calculating.
In the embodiment of the invention, the likelihood domain model calculation steps are as follows:
sequentially selecting a scanning point z from a laser radar scanning framet(θt,dt)，(θt,dt) Mapping the scanning points to a global coordinate system (namely a grid map system) for the angles and the distances of the scanning points:
wherein (x, y, theta) is the current pose of the particle, (x)t,yt) Is the global coordinate of the scanned point, i.e. the coordinate scanned into the grid map.
Calculating (x)t,yt) And calculating the score p on the current pose of the particle according to the distance dist from the nearest obstacle:
wherein z ishit、zranddomAnd zmaxRespectively representing different parts of the mixed weight of the ranging error, respectively representing measurement noise, unexplained random measurements and measurement failures, sigmahitAnd traversing all scanning points on the scanning frame of the laser radar to measure the standard deviation of the noise, and accumulating to obtain the score p of the current pose.
The mobile robot repositioning method based on particle self-convergence provided by the invention has the following beneficial effects: after the robot loses the pose, the pose of the robot can be quickly and accurately recovered, namely, the robot is repositioned without manual intervention.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.
Claims (5)
1. A mobile robot repositioning method based on particle self-convergence is characterized by specifically comprising the following steps:
s1, generating a grid particle set _ a based on the grid map, wherein the grid particle set _ a is composed of grid particles, and the grid particles are grid points pi(xi,yi) The optimal pose of (1);
s2, updating the particles in the grid particle set _ a and the corresponding scores;
s3, resampling the particles in the grid particle set a, copying the resampled particles to a new particle set b, wherein the weights of the particles in the particle set b are equal,
s4, calculating the pose standard deviations of all the particles in the set _ b of the particle set, detecting whether the pose standard deviations of all the particles are smaller than a preset threshold, if so, taking the average pose of all the particles in the set _ b of the particle set as the current repositioning pose of the robot, and if not, executing the step S2.
2. The method for repositioning a mobile robot based on particle self-convergence according to claim 1, wherein the method for generating the grid particle set _ a specifically comprises the following steps:
s11, taking a grid point p on the grid map at intervals of a set distance delta di(xi,yi)；
S12, detecting whether the grid where the grid-taking mesh point is located is occupied by an obstacle;
s13, if the detection result is negative, obtaining the lattice point pi(xi,yi) If the detection result is yes, taking the next grid point on the grid map based on the set distance delta d, and executing step S12;
and S14, traversing all grid points on the grid map, and outputting a grid particle set _ a.
3. The method for repositioning mobile robot based on particle self-convergence according to claim 2, wherein the grid points pi(xi,yi) The method for acquiring the optimal pose specifically comprises the following steps:
from 0 degrees to 360 degrees, each set angular step Δ θ is taken at an angle θjAngle thetajAnd grid point pi(xi,yi) Form a pose pi,j(xi,yi,θj)；
Pose p pair using likelihood domain modeli,j(xi,yi,θj) Scoring and traversing the grid points pi(xi,yi) All angles and the highest scoring pose are lattice points pi(xi,yi) The optimal pose of (1).
4. The method for repositioning a mobile robot based on particle self-convergence according to claim 1, wherein the method for updating the particles in the grid particle set _ a specifically comprises the following steps:
s21, selecting one particle p in the grid particle set _ a in sequencei(xi,yi,θi) And in the particle pi(xi,yi,θi) Generating k Gaussian random poses around;
s22 calculating particle p by using likelihood domain modeli(xi,yi,θi) And scoring the k random pose sets to obtain the highest score wmaxAnd corresponding pose pmaxLet the current pose equal to the pose p with the highest scorei＝pmaxThe weight of the current pose is equal to the highest score, wi＝wmax；
And S23, traversing all the particles in the particle grid particle set _ a, and finishing the updating and scoring in the grid particle set _ a.
5. The method for repositioning a mobile robot based on particle self-convergence according to claim 1, wherein the particle resampling process is as follows:
s31, carrying out descending order arrangement on the weight of the particles in the grid particle set;
s32, calculating the number copy _ count of the particles to be copied based on the particle copy ratio, setting the particle copy ratio as r, calculating the number copy _ count of the particles to be copied as r set _ count of the current grid particle set;
s33, constructing an empty particle set _ b, copying the top copy _ count particles to the particle set _ b, and setting the weight of all the particles in the particle set _ b as 1/copy _ count;
and S34, replacing the grid particle set _ a with the particle set _ b.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911403712.7A CN111061287B (en) | 2019-12-31 | 2019-12-31 | Mobile robot repositioning method based on particle self-convergence |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911403712.7A CN111061287B (en) | 2019-12-31 | 2019-12-31 | Mobile robot repositioning method based on particle self-convergence |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111061287A true CN111061287A (en) | 2020-04-24 |
| CN111061287B CN111061287B (en) | 2022-05-27 |
Family
ID=70305203
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911403712.7A Active CN111061287B (en) | 2019-12-31 | 2019-12-31 | Mobile robot repositioning method based on particle self-convergence |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111061287B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111539994A (en) * | 2020-04-28 | 2020-08-14 | 武汉科技大学 | Particle filter repositioning method based on semantic likelihood estimation |
| CN112014854A (en) * | 2020-08-31 | 2020-12-01 | 华通科技有限公司 | Positioning method based on particle swarm positioning algorithm |
| CN112797981A (en) * | 2020-12-24 | 2021-05-14 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Particle filter positioning method based on normal distribution |
Citations (7)
| 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 |
| US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
| 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 |
| CN107462892A (en) * | 2017-07-28 | 2017-12-12 | 深圳普思英察科技有限公司 | Mobile robot synchronous superposition method based on more sonacs |
| CN109556611A (en) * | 2018-11-30 | 2019-04-02 | 广州高新兴机器人有限公司 | A kind of fusion and positioning method based on figure optimization and particle filter |
| CN109798896A (en) * | 2019-01-21 | 2019-05-24 | 东南大学 | A kind of positioning of Indoor Robot with build drawing method and device |
| CN110567441A (en) * | 2019-07-29 | 2019-12-13 | 广东星舆科技有限公司 | Particle filter-based positioning method, positioning device, mapping and positioning method |
-
2019
- 2019-12-31 CN CN201911403712.7A patent/CN111061287B/en active Active
Patent Citations (7)
| 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 |
| US20140350839A1 (en) * | 2013-05-23 | 2014-11-27 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
| 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 |
| CN107462892A (en) * | 2017-07-28 | 2017-12-12 | 深圳普思英察科技有限公司 | Mobile robot synchronous superposition method based on more sonacs |
| CN109556611A (en) * | 2018-11-30 | 2019-04-02 | 广州高新兴机器人有限公司 | A kind of fusion and positioning method based on figure optimization and particle filter |
| CN109798896A (en) * | 2019-01-21 | 2019-05-24 | 东南大学 | A kind of positioning of Indoor Robot with build drawing method and device |
| CN110567441A (en) * | 2019-07-29 | 2019-12-13 | 广东星舆科技有限公司 | Particle filter-based positioning method, positioning device, mapping and positioning method |
Non-Patent Citations (1)
| Title |
|---|
| 黄鹤，等: "粒子滤波与ORB特征检测结合的移动机器人定位算法", 《传感器与微系统》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111539994A (en) * | 2020-04-28 | 2020-08-14 | 武汉科技大学 | Particle filter repositioning method based on semantic likelihood estimation |
| CN111539994B (en) * | 2020-04-28 | 2023-04-18 | 武汉科技大学 | Particle filter repositioning method based on semantic likelihood estimation |
| CN112014854A (en) * | 2020-08-31 | 2020-12-01 | 华通科技有限公司 | Positioning method based on particle swarm positioning algorithm |
| CN112797981A (en) * | 2020-12-24 | 2021-05-14 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Particle filter positioning method based on normal distribution |
| CN112797981B (en) * | 2020-12-24 | 2024-04-05 | 长三角哈特机器人产业技术研究院 | Particle filter positioning method based on normal distribution |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111061287B (en) | 2022-05-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111061287B (en) | Mobile robot repositioning method based on particle self-convergence | |
| CN111076733B (en) | Robot indoor map building method and system based on vision and laser slam | |
| WO2020224305A1 (en) | Method and apparatus for device positioning, and device | |
| CN108180917B (en) | Top map construction method based on pose graph optimization | |
| US8831778B2 (en) | Method of accurate mapping with mobile robots | |
| CN109613550B (en) | Laser radar map construction and positioning method based on reflector | |
| CN111060092B (en) | Rapid matching method of reflective columns | |
| CN111273304B (en) | Natural positioning method and system for fusion reflecting column | |
| CN111982099B (en) | Robot hybrid positioning method, device, equipment and computer readable medium | |
| CN111060888A (en) | Mobile robot repositioning method fusing ICP and likelihood domain model | |
| WO2023273169A1 (en) | Vision and laser-fused 2.5d map construction method | |
| WO2020199565A1 (en) | Street lamp pole-based vehicle posture correction method and device | |
| WO2019136613A1 (en) | Indoor locating method and device for robot | |
| CN103512548A (en) | Range measurement apparatus and range measurement method | |
| WO2022111723A1 (en) | Road edge detection method and robot | |
| CN110986956A (en) | Autonomous learning global positioning method based on improved Monte Carlo algorithm | |
| CN108332752A (en) | The method and device of robot indoor positioning | |
| KR20200065875A (en) | Method and system for recognizing lane using landmark | |
| KR101272422B1 (en) | Device and method for locationing using laser scanner and landmark matching | |
| JP2009128192A (en) | Object recognition device and robot device | |
| CN106296797A (en) | A kind of spatial digitizer characteristic point modeling data processing method | |
| CN115790539B (en) | Cooperative target underwater photogrammetry method | |
| CN105809685A (en) | Single-concentric circle image-based camera calibration method | |
| US20240085448A1 (en) | Speed measurement method and apparatus based on multiple cameras | |
| CN113758422B (en) | Method for numbering and identifying divergent multi-line laser stripes |
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 |