CN117570998A - Robot positioning method and system based on reflective column information - Google Patents
Robot positioning method and system based on reflective column information Download PDFInfo
- Publication number
- CN117570998A CN117570998A CN202410063577.0A CN202410063577A CN117570998A CN 117570998 A CN117570998 A CN 117570998A CN 202410063577 A CN202410063577 A CN 202410063577A CN 117570998 A CN117570998 A CN 117570998A
- Authority
- CN
- China
- Prior art keywords
- particle
- robot
- weight
- column
- pose
- 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
- 238000000034 method Methods 0.000 title claims abstract description 34
- 239000002245 particle Substances 0.000 claims abstract description 221
- 238000012952 Resampling Methods 0.000 claims abstract description 13
- 238000012935 Averaging Methods 0.000 claims abstract description 6
- 238000009826 distribution Methods 0.000 claims description 50
- 230000004888 barrier function Effects 0.000 claims description 9
- 238000005259 measurement Methods 0.000 description 16
- 238000004364 calculation method Methods 0.000 description 13
- 238000005070 sampling Methods 0.000 description 11
- 230000008569 process Effects 0.000 description 9
- 206010033307 Overweight Diseases 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000011423 initialization method Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 230000010076 replication Effects 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Abstract
The invention provides a robot positioning method and system based on reflective column information, and belongs to the technical field of robot navigation and positioning. When the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map; predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle; averaging the pose of all the resampled particles to obtain the current pose of the robot; the invention reduces the maximum particle number burden and the iteration times and improves the positioning efficiency.
Description
Technical Field
The invention relates to the technical field of robot navigation and positioning, in particular to a robot positioning method and system based on reflective column information.
Background
The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.
Currently, a robot positioning technique represented by an AMCL (Adaptive Monte Carlo Localization, adaptive monte carlo positioning) algorithm has made remarkable progress in the autonomous navigation field. However, the inventors have found that in practical applications, existing AMCL algorithms still have some limitations, including:
laser SLAM technology is widely used at present, however, an indoor mobile robot usually adopts a single-line laser radar, the radar can only provide single-plane information, and the generated two-dimensional grid map is similar in local or global due to the degradation of point cloud characteristics and the limited map information acquired by the single-line laser radar, and particle filtering can be limited by local information in similar terrains or structures, so that the robot is difficult to position, even positioning failure is caused, and the robot is influenced to execute tasks; meanwhile, the initialization time of the AMCL is long, resulting in lower efficiency of positioning.
Disclosure of Invention
In order to solve the defects of the prior art, the invention provides a robot positioning method and a system based on reflective column information, when the initial pose of a robot is not provided, but a two-dimensional grid map containing reflective column information is known, the identified nearest reflective column is utilized to initialize particles around the identified nearest reflective column and determine the distribution mode of the particles, so that the maximum particle number burden and the iteration number are reduced, and the positioning efficiency is improved.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
in a first aspect, the invention provides a robot positioning method based on reflective column information.
A robot positioning method based on reflective column information comprises the following steps:
obtaining a two-dimensional grid map containing reflective column information according to two-dimensional laser radar data of the environment where the robot is located;
when the initial pose of the robot is acquired, initializing a particle swarm around the initial pose according to the two-dimensional grid map; when the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map;
predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle;
and averaging the pose of all the resampled particles to obtain the current pose of the robot.
As a further limitation of the first aspect of the present invention, the initializing of the particle swarm around the position of the light reflecting column closest to the robot comprises:
acquiring world coordinates of a reflecting column closest to the robot, and obtaining an X-axis pose of any particle according to the X-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particle and the center of the reflecting column and the average distribution angle of the particle around the center of the reflecting column; obtaining the Y-axis pose of the particles according to the Y-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particles and the center of the reflecting column and the average distribution angle of the particles around the center of the reflecting column; obtaining the pose angle of the particles according to the included angle between the robot and the reflecting column and the average distribution angle of the particles around the center of the reflecting column;
each particle is initialized with a gaussian distribution with the distance from the nearest reflector to the robot as the mean value, at an average distribution angle around the center of the reflector.
As a further definition of the first aspect of the invention, calculating the weight of each particle comprises:
and calculating the grid map weight and the weight additionally brought by the reflective column information for any particle, and adding the grid map weight and the weight additionally brought by the reflective column information to obtain the final weight of the particle.
As a further limitation of the first aspect of the present invention, according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is located to the surrounding barrier, and two times of gaussian distribution standard deviation, the updated grid map weight of the current cycle is obtained;
and obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is positioned to the surrounding barrier and the standard deviation of four times of Gaussian distribution.
In a second aspect, the invention provides a robot positioning system based on reflective column information.
A robot positioning system based on reflective column information, comprising:
a map building module configured to: obtaining a two-dimensional grid map containing reflective column information according to two-dimensional laser radar data of the environment where the robot is located;
a particle swarm initialization module configured to: when the initial pose of the robot is acquired, initializing a particle swarm around the initial pose according to the two-dimensional grid map; when the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map;
a particle weight update module configured to: predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle;
a pose estimation module configured to: and averaging the pose of all the resampled particles to obtain the current pose of the robot.
As a further limitation of the second aspect of the present invention, in the particle swarm initialization module, the initialization of the particle swarm around the position of the light reflecting column closest to the robot includes:
acquiring world coordinates of a reflecting column closest to the robot, and obtaining an X-axis pose of any particle according to the X-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particle and the center of the reflecting column and the average distribution angle of the particle around the center of the reflecting column; obtaining the Y-axis pose of the particles according to the Y-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particles and the center of the reflecting column and the average distribution angle of the particles around the center of the reflecting column; obtaining the pose angle of the particles according to the included angle between the robot and the reflecting column and the average distribution angle of the particles around the center of the reflecting column;
each particle is initialized with a gaussian distribution with the distance from the nearest reflector to the robot as the mean value, at an average distribution angle around the center of the reflector.
As a further limitation of the second aspect of the present invention, in the particle weight updating module, calculating the weight of each particle includes:
and calculating the grid map weight and the weight additionally brought by the reflective column information for any particle, and adding the grid map weight and the weight additionally brought by the reflective column information to obtain the final weight of the particle.
As a further limitation of the second aspect of the present invention, according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is located to the surrounding barrier, and two times of gaussian distribution standard deviation, obtaining the updated grid map weight of the current cycle;
and obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is positioned to the surrounding barrier and the standard deviation of four times of Gaussian distribution.
In a third aspect, the present invention provides a computer readable storage medium having stored thereon a program which, when executed by a processor, implements the steps in the robot positioning method based on reflective column information according to the first aspect of the present invention.
In a fourth aspect, the present invention provides an electronic device, including a memory, a processor, and a program stored on the memory and executable on the processor, where the processor implements the steps in the method for positioning a robot based on information of a reflective column according to the first aspect of the present invention when the program is executed.
Compared with the prior art, the invention has the beneficial effects that:
1. the invention creatively provides a robot positioning strategy based on reflective column information, when the initial pose of a robot is not provided, but a two-dimensional grid map containing reflective column information is known, the identified nearest reflective column is utilized to initialize particles around the identified nearest reflective column and determine the distribution mode of the particles, so that the maximum particle number burden and the iteration times are reduced, and the positioning efficiency is improved.
2. The invention creatively provides a robot positioning strategy based on the reflective column information, adds extra weight brought by the reflective column information, combines the additional weight with the grid map weight, improves algorithm robustness, not only avoids the problem of positioning failure caused by congruent triangles when the reflective column is used for positioning, but also solves the problem of positioning failure of an AMCL algorithm under a similar grid map when only a two-dimensional grid map is used.
3. The invention creatively provides a robot positioning strategy based on reflective column information, which can adjust the particle weight updating mode according to different environments, and when facing a changeable and dynamic environment, the additional particle weight brought by the reflective column map can be increased, so that the effect of the two-dimensional grid map in the particle updating process is reduced, namely the weight is reduced and even set to zero, and further more accurate positioning is realized.
Additional aspects of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
FIG. 1 is a flowchart of an improved AMCL algorithm provided in embodiment 1 of the present invention;
FIG. 2 is a flow chart of a method for selecting and initializing particle swarm according to embodiment 1 of the present invention;
FIG. 3 is a schematic view of the positions of the particles and the robot according to embodiment 1 of the present invention;
fig. 4 is a schematic diagram of a reflective column sampling according to embodiment 1 of the present invention;
FIG. 5 is a flow chart of a likelihood domain model for laser measurement according to embodiment 1 of the present invention;
fig. 6 is a graph showing the change of particle weight according to embodiment 1 of the present invention.
Detailed Description
The invention will be further described with reference to the drawings and examples.
It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the invention. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
Embodiments of the invention and features of the embodiments may be combined with each other without conflict.
Example 1:
the embodiment 1 of the invention provides a robot positioning method based on reflective column information, which utilizes a sensor to sense environmental information to create a map in advance, and combines control input of a robot and sensor observation data to determine the accurate position and posture of the robot in the environmental map; specifically, in the invention, when the robot moves, the laser radar scans the environment and generates point cloud information, meanwhile, whether the current position scans the reflecting column can be identified through the intensity value of the scanned laser point cloud, after the reflecting column information is introduced, the special markers can provide additional positioning constraint, the positioning accuracy is improved, and the robot can estimate the current position and the gesture by matching the point cloud information with a map constructed in advance.
As shown in FIG. 1, the invention establishes a two-dimensional grid map through two-dimensional laser radar data, odometer data and IMU multi-sensor data before the robot performs positioning, and simultaneously places a plurality of reflective columns in the positioning environment, wherein the reflective columns generally have high reflectivity and are relatively easy to be detected by the laser radar carried by the robot in various environments, so that a reflective column map can be established. The two-dimensional grid map containing the reflective column information is obtained through the method and is used for robot positioning.
Then, the motion model in the improved AMCL algorithm adopts an odometer motion model, the observation model adopts a likelihood domain model, the likelihood domain model is substituted into the particle filtering algorithm, the positioning is started after the laser data are obtained,
s1: and initializing a particle swarm.
At the beginning, a set of particles is generated, each particle representing a possible initial position of the robot, the Monte Carlo positioning algorithm uses a set of M particlesRepresenting confidence->In the present invention, the initialization of the particle swarm is divided into various cases. As shown in fig. 2, if an initial pose of the robot is provided before the positioning is started, the particles will initialize around the pose, otherwise, whether the reflecting column is identified or not will be judged according to the received laser radar data. When the reflective column information is not scanned, the initialization particles in the set M still obey global sampling, namely are uniformly distributed in the map; when one or more reflection columns are scanned, analysis is performed according to one reflection column with the shortest distance from the robot, sampling of the reflection column is performed nearby, and two initialization methods assign the same initial weight to each particle, wherein Z t Indicating the measurement of the lidar at time t, landmark j Representing the currently identified retroreflective columns, m representing known map information.
The specific implementation of the reflective column sampling is as follows: when the reflective column information is identified, the particle initialization method is as follows:
s1.1: the particles are uniformly distributed around the center of the light reflecting column.
Knowing the position of the center of the scanned nearest reflector in world coordinate system asThe particles willEvenly distributed around the center, pose P i Can be expressed as:
（1）。
where i is the index of the particle, r is the distance between the particle and the center of the light reflecting column,is the average distribution angle of particles around the center of the reflective column, < >>Is the pose angle of the particle.
S1.2: calculating pose angle of particles。
As shown in FIG. 3, X W -Y W Representing the X and Y axes in the world coordinate system, X R -Y R When the laser radar scans the reflecting column, the included angle alpha between the robot and the reflecting column can be obtained according to the prior information of the map of the reflecting column, and the average distribution angle of particles around the center of the reflecting column isThe angle is in the range of [0 °,360 ]]Then in world coordinate system the pose angle of the particle +.>The method comprises the following steps:
（2）。
s1.3: and taking the distance from the light reflecting column to the robot as a mean value, and realizing Gaussian distribution of particles.
At an average distribution angle around the center of the light reflecting column, the particles are initialized using a gaussian distribution with the distance of the light reflecting column to the robot as the mean value, which means that we wish the distance distribution of the particlesThe distance from the robot around the reflecting column is spread out as an average distance. For particles i of the same distribution angle we define a distance from the light reflecting column to the robotThe relevant gaussian distribution is specifically expressed as:
（3）。
wherein,is particle->The distance between the light source and the reflecting column is calculated by the following steps:
（4）。
wherein,is the average distance from the reflecting column to the robot, r i Is the distance between particle i and the center of the reflective column, < >>Is the standard deviation of the Gaussian distribution, the width of the distribution is controlled, so that the distance distribution of the particles is equal to +.>Is the center (is the->The standard deviation shows a gaussian distribution. The initialization mode is helpful for ensuring that the distance between the position of the particle and the reflecting column is relatively reasonable, and the initialization effect of the algorithm is improved.
The reflection column map is constructed as a salient feature, and thus the absolute pose of the reflection column in the grid map is known. After the reflective column is detected, the robot is determined to be in a circular area with the distance r from the robot to the reflective column as a radius.
As shown in fig. 4, the real position of the robot is reduced in a dark gray area, and by increasing the particle coverage rate of the area, the particle density of the area is increased in the process of initializing particles, and the area represents that the initial particles of the real pose of the robot are increased, so that the probability of successful positioning can be improved, and the iteration times can be reduced.
In particular, robots have a large probability of localization within dark gray annular areas of relative distance estimation. In order to compensate for possible errors in depth estimation, the area is extended outwards by delta distance, and based on the strategy, the three annular areas are considered in the sampling distribution of particles, so that more accurate estimation of the real position of the robot is ensured.
The invention takes the reflective columns as landmarks, the absolute pose of which in the grid map is known, which provides a specific reference point for the robot, serving as a known fixed feature in the map. The known position information can be used for initializing an AMCL algorithm, and after the information of the reflection column is identified, more initialized particle swarm is carried out around the information, so that the positioning accuracy is improved, and the positioning can be assisted and the estimation range can be reduced;
the reflection column has higher visual recognition performance, and the robot can accurately detect and recognize the landmarks through sensors such as a laser radar or a camera, so that the perception capability of the robot on the environment is improved, the robot can more reliably acquire the information of the surrounding environment, accurate positioning is realized in a changeable environment, and the environment perception capability is improved;
in environments with similar structures or features, robots may face positioning challenges, and by adding reflective column map information, robots can more easily distinguish similar environments because reflective columns provide unique identifications, and by changing the particle weight calculation formula when AMCL, the positioning accuracy of robots in such environments is improved, positioning failure caused by similar triangles when positioning by the reflective columns is avoided, positioning failure under a similar grid map when a pure AMCL algorithm is also solved, and the recognition capability of similar environments is improved.
S2: and predicting the particle motion trail.
The odometer motion model uses relative motion information, which is measured by the internal odometer of the robot. Estimating pose of new moment according to motion model based on particlesAnd control->Imaginary state at generation time t>This step includes sampling from a state transition profile that characterizes how the state changes over time, namely:
（5）。
s3: and (5) calculating the weight of the particles.
The pose of the particles after S2 is known, the weight calculation of the particles mainly depends on a likelihood domain observation model, and the measurement model is defined as a conditional probability distributionWherein x is t Is the pose, z of the robot t Is a measurement at time t, while one measurement z t Comprises a series of measured values, with +.>In the lidar measurement, it represents a ranging value, and the specific process of weight update is shown in fig. 5.
The following is a general procedure for particle weight calculation: (1) coordinate conversion: taking the pose of the particle as a conversion center, converting a frame of filtered radar data coordinate to a two-dimensional grid map, and mapping laser data to a map coordinate system by taking the known pose of the particle into consideration; (2) calculating gaussian sampling probability: for each particle, calculating the distance from the grid where the laser point is positioned to surrounding obstacles by using all laser beams emitted from the particle point to the surrounding, modeling the measurement error of the laser beam by using Gaussian distribution, and calculating a probability value by comparing the actual occupation situation on a map with the expected occupation situation of the laser beam; (3) weight calculation: and summing likelihood domains of all the calculated laser beams to obtain the overall likelihood value, namely the weight, of the particle. The Gaussian sampling probability is generally used for representing the matching degree of the laser data and the map, and a specific calculation mode may involve factors such as measurement errors of laser beams, occupation conditions of the map and the like; (4) normalization: normalizing the weights of all particles to ensure that their sum is 1; (5) updating pose estimation: and selecting the particle swarm with the largest weight value, and calculating the average pose of the particle swarm as the estimated pose of the robot. The above procedure ensures that in the next resampling step, particles with higher weights will be more likely to be copied, thereby improving the estimation accuracy of the robot pose.
The reasons that the AMCL algorithm may fail in the face of similar environments, unknown initial pose, global positioning requirements, or robotic kidnapping problems mainly involve two key issues: local similarity and resampling bias. In the similar region, the matching degree of the laser data and the grid map is high, so that the initial particle weight is high. During the resampling phase, high-weight false pose particles may be retained, while true particles reflecting the correct pose of the robot may be discarded due to the lower weight. This may lead to a predominance of spurious particles in similar areas, such that the final positioning result is converged in similar areas.
The invention therefore improves the way weights are updated, requiring weight calculation in combination with reflective column information. As shown in FIG. 5, when a measured value is obtainedWhen the distance from the grid of the laser spot to the nearest barrier around is calculated, a Gaussian sampling probability q is calculated through the distance value, and the process is known to be a normal distribution and a normal distribution according to a likelihood domain observation modelEvenly distributed mixing to obtain likelihood results, for each distance measurement +.>The latest can be obtained after k times of circulationThe initial value of q is 1, and the specific calculation formula is as follows:
（6）。
wherein the function prob (dist, sigma hit ) The calculation is centered on 0,the standard deviation is sigma as the result of the k-1 th cycle hit About dist, which represents the Euclidean distance of the measured coordinates from the nearest point on the map, Z hit 、Z random For weighting parameters measured by the sensor, Z max For the maximum measurement distance of the sensor, the coordinate system conversion information tf is known, which includes the relation between the laser radar coordinate system and the robot local coordinate system, the pose of each particle is taken as the coordinate of the robot under the world coordinate system, the measurement data of the laser are mapped to the global coordinate space of the map, the distance dist between the end point of the laser measurement and the nearest obstacle of the map is calculated through the known map information, and the probability of the occurrence of the laser measurement data is larger when the distance is smaller, and the weight of the particle is larger.
Each particle isWeight of->Representation that it is used to measure z t Incorporating into particle concentration, z t Is the laser radar measurement at time t, by z t Dist can be calculated, the importance of which is to measure z t In particle->Probability below. As shown in FIG. 6, the interval radian represents the weight, the distribution of the turntable represents the particle distribution, and when no reflection column information is added, the data z is obtained for one frame according to the likelihood domain observation model t， Each distance measurement +.>All the calculation is needed, and the current particle can be obtained after the addition of k times of loops>The latest weight->The calculation mode of the particle weight in the algorithm is shown as follows:
=/>+/>*/>（7）。
an initial value of 0, wherein->For the weighted value in the likelihood domain model, dist is the distance from the grid of each laser point in a frame of laser measurement to the nearest obstacle around.
When the laser data observe the reflective column, the weight calculation mode of the particles needs to be added with one step of calculation, and the calculation is shown as the following formula:
=/>+/>*/>（8）。
wherein the method comprises the steps ofThe initial value is 0, as shown in FIG. 5, the grid map weight is calculated, the weight additionally brought by the reflective column information is calculated, the final weight of the particle is obtained by adding the grid map weight and the reflective column information, and the standard deviation sigma can be obtained by the changed formula when the reflective column data is scanned by laser hit When the coefficient of (2) is changed from 2 to 4, the value of the exponential function is larger than that of the original equation, the weight value of the particles is also larger, and the added weight is also larger.
As shown in FIG. 6, the weight distribution of the particles corresponds to a turntable divided into N parts, i.e., from w 1 To w n And 1. Each interval is the weight of one particle, and the larger the weight is, the larger the radian of the interval is. Each time from w 1 The left vertical line starts to rotate, obviously to w 1 The probability of (2) is 0-w 1 Random number in between, go to w 2 The probability of w is 1 To w 1 +w 2 Random number in between, go to w 3 The probability of w is 1 +w 2 To w 1 +w 2 +w 3 Random numbers in between, and so on. When the weight of a particle is large, the probability that the generated random number falls in the corresponding interval is increased, so that the repeated duplication of the large-weight particle and the rejection of the particle with small weight are realized.
When the information of the reflection column is not scanned, the weight of each particle is calculated by a new formula when the reflection column is recognized, assuming that the weight of each particle is as shown in (a) of fig. 6, and the weight is increased as shown in (b) of fig. 6Let w be 1 、w k-1 、w n For particles striking the point of the reflecting column, their weight arc interval becomes larger, i.e. the importance becomes larger, while the weight of other particles will change accordingly.
S4: and resampling the particle swarm.
After S3, the weight of each particle is calculated and updated, and at this time, the particle weight values in the particle group are different. The probability that the particles with high weight represent the real pose of the robot is larger, the probability that the particles with low weight are the wrong pose is larger, and the real pose can be far deviated. Therefore, the low-weight particles need to be discarded; in the rest of the particle groups in S3, the replication operation is performed on the particles with high weight, and after resampling, the number of the particles is unchanged.
More specifically, the resampling process is as follows:
(1) Selecting random numbers: selecting a random number from the uniform distribution>，/>Is in the range of (0, 1);
(2) Calculating a sampling pointer: use->Calculate sample pointer +.>Wherein->Can take a value from 1 to +.>；
(3) Resampling: by passing throughCalendar with a displayAccording to the calculated->To select the corresponding particles, in +.>Selecting a particle when the particle falls within a certain particle weight interval; in this way, high weighted particles will be more likely to be selected, preserving their information.
The key to this process is thatBy means of uniformly distributed random numbers +.>And a weight for each particle such that particles with higher weights are more easily selected. This achieves biased, weight-proportional sampling of the particles, effectively reducing variance.
S5: averaging the pose of each particle in the returned particle set to obtain the real pose of the mobile robot at the current moment, wherein the estimated pose is expressed as:
（9）。
example 2:
the embodiment 2 of the invention provides a robot positioning system based on reflective column information, which comprises:
a map building module configured to: obtaining a two-dimensional grid map containing reflective column information according to two-dimensional laser radar data of the environment where the robot is located;
a particle swarm initialization module configured to: when the initial pose of the robot is acquired, initializing a particle swarm around the initial pose according to the two-dimensional grid map; when the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map; the specific process is shown in the step S1 in embodiment 1, and will not be described here again;
a particle weight update module configured to: predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle; the specific process is shown in steps S2-S4 in embodiment 1, and will not be repeated here;
a pose estimation module configured to: the pose of all the resampled particles is averaged to obtain the current pose of the robot, and the specific process is shown in step S5 in embodiment 1, which is not repeated here.
Example 3:
embodiment 3 of the present invention provides a computer-readable storage medium having stored thereon a program which, when executed by a processor, implements the steps in the robot positioning method based on reflective column information according to embodiment 1 of the present invention.
Example 4:
embodiment 4 of the present invention provides an electronic device, including a memory, a processor, and a program stored on the memory and executable on the processor, where the processor implements steps in the positioning method of the robot based on the reflective column information according to embodiment 1 of the present invention when executing the program.
The above description is only of the preferred embodiments of the present invention and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. The robot positioning method based on the reflective column information is characterized by comprising the following steps of:
obtaining a two-dimensional grid map containing reflective column information according to two-dimensional laser radar data of the environment where the robot is located;
when the initial pose of the robot is acquired, initializing a particle swarm around the initial pose according to the two-dimensional grid map; when the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map;
predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle;
and averaging the pose of all the resampled particles to obtain the current pose of the robot.
2. The method for positioning a robot based on information of a reflection column according to claim 1, wherein,
initializing the particle swarm around the position of the light reflecting column closest to the robot, comprising:
acquiring world coordinates of a reflecting column closest to the robot, and obtaining an X-axis pose of any particle according to the X-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particle and the center of the reflecting column and the average distribution angle of the particle around the center of the reflecting column; obtaining the Y-axis pose of the particles according to the Y-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particles and the center of the reflecting column and the average distribution angle of the particles around the center of the reflecting column; obtaining the pose angle of the particles according to the included angle between the robot and the reflecting column and the average distribution angle of the particles around the center of the reflecting column;
each particle is initialized with a gaussian distribution with the distance from the nearest reflector to the robot as the mean value, at an average distribution angle around the center of the reflector.
3. The method for positioning a robot based on information of a reflection column according to claim 1 or 2, wherein,
calculating a weight for each particle, comprising:
and calculating the grid map weight and the weight additionally brought by the reflective column information for any particle, and adding the grid map weight and the weight additionally brought by the reflective column information to obtain the final weight of the particle.
4. The method for positioning a robot based on information of a reflection column according to claim 3,
obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is located to the surrounding barrier and the two times of Gaussian distribution standard variance;
and obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is positioned to the surrounding barrier and the standard deviation of four times of Gaussian distribution.
5. A robot positioning system based on reflective column information, comprising:
a map building module configured to: obtaining a two-dimensional grid map containing reflective column information according to two-dimensional laser radar data of the environment where the robot is located;
a particle swarm initialization module configured to: when the initial pose of the robot is acquired, initializing a particle swarm around the initial pose according to the two-dimensional grid map; when the initial pose of the robot is not acquired, initializing a particle swarm around a reflection column position closest to the robot according to a two-dimensional grid map;
a particle weight update module configured to: predicting particle motion trajectories according to odometer data of the robot to obtain pose of each particle, calculating weight of each particle, and resampling particle swarm according to the weight of each particle;
a pose estimation module configured to: and averaging the pose of all the resampled particles to obtain the current pose of the robot.
6. The robotic positioning system based on reflective column information of claim 5,
in the particle swarm initialization module, initializing the particle swarm around the position of the reflection column closest to the robot, including:
acquiring world coordinates of a reflecting column closest to the robot, and obtaining an X-axis pose of any particle according to the X-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particle and the center of the reflecting column and the average distribution angle of the particle around the center of the reflecting column; obtaining the Y-axis pose of the particles according to the Y-axis coordinate value of the world coordinate system of the reflecting column, the distance between the particles and the center of the reflecting column and the average distribution angle of the particles around the center of the reflecting column; obtaining the pose angle of the particles according to the included angle between the robot and the reflecting column and the average distribution angle of the particles around the center of the reflecting column;
each particle is initialized with a gaussian distribution with the distance from the nearest reflector to the robot as the mean value, at an average distribution angle around the center of the reflector.
7. The robotic positioning system based on reflective column information as set forth in claim 5 or 6, wherein,
in the particle weight updating module, calculating the weight of each particle includes:
and calculating the grid map weight and the weight additionally brought by the reflective column information for any particle, and adding the grid map weight and the weight additionally brought by the reflective column information to obtain the final weight of the particle.
8. The robotic positioning system based on reflective column information of claim 7,
obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is located to the surrounding barrier and the two times of Gaussian distribution standard variance;
and obtaining the updated grid map weight of the current cycle according to the weight of the previous cycle of each particle, the weight value in the likelihood domain model, the distance from the grid where the laser point is positioned to the surrounding barrier and the standard deviation of four times of Gaussian distribution.
9. A computer-readable storage medium, on which a program is stored, which program, when being executed by a processor, implements the steps in the robot positioning method based on reflective column information as claimed in any one of claims 1 to 4.
10. An electronic device comprising a memory, a processor and a program stored on the memory and executable on the processor, wherein the processor performs the steps in the method for positioning a robot based on information of a light reflecting column according to any one of claims 1-4 when the program is executed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410063577.0A CN117570998B (en) | 2024-01-17 | 2024-01-17 | Robot positioning method and system based on reflective column information |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410063577.0A CN117570998B (en) | 2024-01-17 | 2024-01-17 | Robot positioning method and system based on reflective column information |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117570998A true CN117570998A (en) | 2024-02-20 |
| CN117570998B CN117570998B (en) | 2024-04-02 |
Family
ID=89888565
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410063577.0A Active CN117570998B (en) | 2024-01-17 | 2024-01-17 | Robot positioning method and system based on reflective column information |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117570998B (en) |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103487047A (en) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | Improved particle filter-based mobile robot positioning method |
| CN106599368A (en) * | 2016-11-14 | 2017-04-26 | 浙江大学 | FastSLAM method based on particle proposal distribution improvement and adaptive particle resampling |
| CN109540149A (en) * | 2018-12-19 | 2019-03-29 | 北京交通大学 | A method of real-time tracing being carried out to indoor automobile using VeTrack system |
| CN110207707A (en) * | 2019-05-30 | 2019-09-06 | 四川长虹电器股份有限公司 | Quick initial alignment method and robot device based on particle filter |
| CN110530368A (en) * | 2019-08-22 | 2019-12-03 | 浙江大华技术股份有限公司 | A kind of robot localization method and apparatus |
| CN110986956A (en) * | 2019-12-23 | 2020-04-10 | 苏州寻迹智行机器人技术有限公司 | Autonomous learning global positioning method based on improved Monte Carlo algorithm |
| CN111307147A (en) * | 2020-03-06 | 2020-06-19 | 同济人工智能研究院(苏州)有限公司 | AGV high-precision positioning method integrating positioning reflector and laser characteristics |
| CN112068547A (en) * | 2020-08-05 | 2020-12-11 | 歌尔股份有限公司 | Robot positioning method and device based on AMCL and robot |
| CN112161634A (en) * | 2020-08-13 | 2021-01-01 | 盐城工学院 | RTB-based (real time bus-based) line control chassis map construction and simultaneous positioning method |
| CN112612862A (en) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Grid map positioning method based on point cloud registration |
| CN112762928A (en) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | ODOM and DM landmark combined mobile robot containing laser SLAM and navigation method |
| CN112764053A (en) * | 2020-12-29 | 2021-05-07 | 深圳市普渡科技有限公司 | Fusion positioning method, device, equipment and computer readable storage medium |
| CN113759928A (en) * | 2021-09-18 | 2021-12-07 | 东北大学 | Mobile robot high-precision positioning method for complex large-scale indoor scene |
| CN116012446A (en) * | 2022-12-19 | 2023-04-25 | 山东新一代信息产业技术研究院有限公司 | Reliable positioning method and device for mobile robot |
| CN117392215A (en) * | 2023-09-14 | 2024-01-12 | 江苏科技大学 | Mobile robot pose correction method based on improved AMCL and PL-ICP point cloud matching |
-
2024
- 2024-01-17 CN CN202410063577.0A patent/CN117570998B/en active Active
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103487047A (en) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | Improved particle filter-based mobile robot positioning method |
| CN106599368A (en) * | 2016-11-14 | 2017-04-26 | 浙江大学 | FastSLAM method based on particle proposal distribution improvement and adaptive particle resampling |
| CN109540149A (en) * | 2018-12-19 | 2019-03-29 | 北京交通大学 | A method of real-time tracing being carried out to indoor automobile using VeTrack system |
| CN110207707A (en) * | 2019-05-30 | 2019-09-06 | 四川长虹电器股份有限公司 | Quick initial alignment method and robot device based on particle filter |
| CN110530368A (en) * | 2019-08-22 | 2019-12-03 | 浙江大华技术股份有限公司 | A kind of robot localization method and apparatus |
| CN110986956A (en) * | 2019-12-23 | 2020-04-10 | 苏州寻迹智行机器人技术有限公司 | Autonomous learning global positioning method based on improved Monte Carlo algorithm |
| CN111307147A (en) * | 2020-03-06 | 2020-06-19 | 同济人工智能研究院(苏州)有限公司 | AGV high-precision positioning method integrating positioning reflector and laser characteristics |
| CN112068547A (en) * | 2020-08-05 | 2020-12-11 | 歌尔股份有限公司 | Robot positioning method and device based on AMCL and robot |
| CN112161634A (en) * | 2020-08-13 | 2021-01-01 | 盐城工学院 | RTB-based (real time bus-based) line control chassis map construction and simultaneous positioning method |
| CN112762928A (en) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | ODOM and DM landmark combined mobile robot containing laser SLAM and navigation method |
| CN112612862A (en) * | 2020-12-24 | 2021-04-06 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Grid map positioning method based on point cloud registration |
| CN112764053A (en) * | 2020-12-29 | 2021-05-07 | 深圳市普渡科技有限公司 | Fusion positioning method, device, equipment and computer readable storage medium |
| US20230400585A1 (en) * | 2020-12-29 | 2023-12-14 | Shenzhen Pudu Technology Co., Ltd. | Fusion positioning method and apparatus, device and computer-readable storage medium |
| CN113759928A (en) * | 2021-09-18 | 2021-12-07 | 东北大学 | Mobile robot high-precision positioning method for complex large-scale indoor scene |
| CN116012446A (en) * | 2022-12-19 | 2023-04-25 | 山东新一代信息产业技术研究院有限公司 | Reliable positioning method and device for mobile robot |
| CN117392215A (en) * | 2023-09-14 | 2024-01-12 | 江苏科技大学 | Mobile robot pose correction method based on improved AMCL and PL-ICP point cloud matching |
Non-Patent Citations (1)
| Title |
|---|
| 罗元;傅有力;程铁凤;: "基于改进Rao-Blackwellized粒子滤波器的同时定位与地图构建", 控制理论与应用, vol. 32, no. 02, 28 February 2015 (2015-02-28), pages 267 - 272 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117570998B (en) | 2024-04-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110645974B (en) | Mobile robot indoor map construction method fusing multiple sensors | |
| WO2021135645A1 (en) | Map updating method and device | |
| Kümmerle et al. | Large scale graph-based SLAM using aerial images as prior information | |
| WO2017028653A1 (en) | Method and system for automatically establishing map indoors by mobile robot | |
| US11506511B2 (en) | Method for determining the position of a vehicle | |
| CN110132284B (en) | Global positioning method based on depth information | |
| CN108332752B (en) | Indoor robot positioning method and device | |
| Bosse | ATLAS: a framework for large scale automated mapping and localization | |
| CN108628318B (en) | Congestion environment detection method and device, robot and storage medium | |
| CN111061276B (en) | Mobile robot repositioning method based on dynamic area division | |
| CN110895408B (en) | Autonomous positioning method and device and mobile robot | |
| US11047708B2 (en) | Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder | |
| CN113108773A (en) | Grid map construction method integrating laser and visual sensor | |
| CN112904358B (en) | Laser positioning method based on geometric information | |
| CN114119920A (en) | Three-dimensional point cloud map construction method and system | |
| CN111522022A (en) | Dynamic target detection method of robot based on laser radar | |
| CN113759928B (en) | Mobile robot high-precision positioning method for complex large-scale indoor scene | |
| Chen et al. | An optimal selection of sensors in multi-sensor fusion navigation with factor graph | |
| CN107966702B (en) | construction method and device of environment map | |
| CN117570998B (en) | Robot positioning method and system based on reflective column information | |
| WO2019183928A1 (en) | Indoor robot positioning method and robot | |
| CN113160280A (en) | Dynamic multi-target tracking method based on laser radar | |
| Murphy et al. | Using incomplete online metric maps for topological exploration with the gap navigation tree | |
| CN110794434B (en) | Pose determination method, device, equipment and storage medium | |
| CN112731337A (en) | Map construction method, device and equipment |
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 |