CN114216452A - Robot positioning method and device - Google Patents
Robot positioning method and device Download PDFInfo
- Publication number
- CN114216452A CN114216452A CN202111481096.4A CN202111481096A CN114216452A CN 114216452 A CN114216452 A CN 114216452A CN 202111481096 A CN202111481096 A CN 202111481096A CN 114216452 A CN114216452 A CN 114216452A
- Authority
- CN
- China
- Prior art keywords
- matching
- pose
- map
- layer
- point
- 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 51
- 238000010586 diagram Methods 0.000 claims abstract description 59
- 101100134058 Caenorhabditis elegans nth-1 gene Proteins 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 13
- 238000005070 sampling Methods 0.000 claims description 11
- 230000003247 decreasing effect Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 description 8
- 238000012986 modification Methods 0.000 description 5
- 230000004048 modification Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000012163 sequencing technique Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- DMSMPAJRVJJAGA-UHFFFAOYSA-N benzo[d]isothiazol-3-one Chemical compound C1=CC=C2C(=O)NSC2=C1 DMSMPAJRVJJAGA-UHFFFAOYSA-N 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
Images
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
- G01C15/002—Active optical surveying means
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention relates to the technical field of robots, in particular to a robot positioning method, which comprises the following steps: the method comprises the steps of acquiring a laser point diagram collected by a robot, wherein the laser point diagram comprises a plurality of laser points and semantic values of each laser point in the plurality of laser points; matching the laser point diagram with a current grid map of the robot, and obtaining a matching result; and if the matching result meets the matching condition, updating the current grid map according to the laser point diagram, and determining the current position of the robot. The method quickly corrects the positioning of the robot, so that the robot has strong adaptability to the field environment, the indoor positioning accuracy of the robot is improved, and the method also has the characteristic of low cost.
Description
Technical Field
The invention relates to the technical field of robots, in particular to a positioning method and device of a robot.
Background
The positioning method of the robot generally adopts a positioning mode based on laser radar data, and particularly relates to an indoor service robot. However, most of the lidar data is distance information, and when the environment is similar or changes, the positioning result of the robot is easy to shift, even the positioning is lost, which causes a problem of low positioning accuracy of the robot.
Disclosure of Invention
The embodiment of the application provides a positioning method and device of a robot, solves the technical problem that the positioning precision of the robot is low in the prior art, realizes quick correction of the positioning of the robot, enables the adaptability of the robot to the field environment to be strong, improves the indoor positioning precision of the robot, and has the technical effect of low cost.
In a first aspect, an embodiment of the present invention provides a positioning method for a robot, including:
the method comprises the steps of acquiring a laser point diagram collected by a robot, wherein the laser point diagram comprises a plurality of laser points and semantic values of each laser point in the plurality of laser points;
matching the laser point diagram with a current grid map of the robot, and obtaining a matching result;
and if the matching result meets the matching condition, updating the current grid map according to the laser point diagram, and determining the current position of the robot.
Preferably, the matching the laser spot diagram with the current grid map of the robot and obtaining a matching result includes:
after the current grid map is downsampled into N layers of grid sub-maps, matching the laser point diagram with the N layers of grid sub-maps to obtain an N layer of pose map set, wherein the resolution of the N layers of grid sub-maps is reduced in sequence, and N is larger than or equal to 2;
up-sampling each frame of the pose image set of the Nth layer, and obtaining a sampled pose image set of the Nth layer;
and matching the sampled pose atlas at the Nth layer with the grid sub-map at the Nth-1 st layer to obtain a pose atlas at the Nth-1 st layer, then up-sampling each frame of pose atlas in the pose atlas at the Nth-1 st layer to obtain a sampled pose atlas at the Nth-1 st layer, and repeatedly executing the operation until the grid sub-map at the first layer in the grid sub-map at the Nth layer is matched to obtain a final pose atlas, wherein the matching result is the final pose atlas.
Preferably, the matching the laser point diagram with the grid sub-map of the nth layer and obtaining the pose atlas of the nth layer includes:
taking each pixel point in the nth layer of grid sub-map as a rotation point, matching the nth layer of grid sub-map with the laser point map after rotating the nth layer of grid sub-map once according to a first preset rotation angle to obtain a matching pose map and a projection point set on the matching pose map, and obtaining the matching probability of the matching pose map according to the projection point set on the matching pose map, wherein the projection point set on the matching pose map is a set of points projected by the laser points in the nth layer of grid sub-map;
after the nth-layer grid sub-map rotates by one circle, obtaining a matching pose atlas of each pixel point in the nth-layer grid sub-map, and after obtaining the matching pose atlas, obtaining a matching pose atlas total set of the nth-layer grid sub-map, wherein each frame of matching pose atlas in the matching pose atlas total set is ordered according to the sequence of the matching probability of the matching pose atlas which is sequentially decreased;
and taking the matching pose graph corresponding to the maximum matching probability of the matching pose graph in the matching pose graph total set as a starting point, and taking a first preset number of matching pose graphs from the matching pose graph total set to form the pose graph set of the Nth layer.
Preferably, the obtaining the matching probability of the matching pose graph according to the projection point set on the matching pose graph includes:
and obtaining the matching probability according to the probability value of each projection point in the projection point set on the matching pose graph, wherein the probability value is the relation probability value between the semantic value of the laser point corresponding to each projection point and the semantic value of the map point in the Nth layer of grid sub-map.
Preferably, the matching the sampled pose atlas at the nth layer with the grid sub-map at the N-1 st layer to obtain the pose atlas at the N-1 st layer includes:
for each frame of sampled pose graph of the Nth layer in the sampled pose graph sets of the N layers, taking each pixel point of the reference point of the sampled pose graph of the Nth layer as a rotation point, rotating the sampled pose graph of the Nth layer once within a set rotation range according to a second preset rotation angle to obtain a rotation pose graph and a projection point set on the rotation pose graph, and then obtaining the matching probability of the rotation pose graph according to the projection point set on the rotation pose graph, wherein the projection point set on the rotation pose graph is a set of points projected by the sampled pose graph of the Nth layer in the grid sub-map of the N-1 layer;
after the sampled pose graph of the Nth layer rotates in the set rotation range, obtaining a rotation pose graph set of each pixel point of the reference point of the sampled pose graph of the Nth layer, and after obtaining the rotation pose graph set, obtaining a rotation pose graph total set of the sampled pose graph sets of the Nth layer, wherein each frame of rotation pose graph of the rotation pose graph total set is ordered according to the sequence of the sequentially decreasing matching probability of the rotation pose graph;
and taking the rotary pose graph corresponding to the maximum matching probability of the rotary pose graph in the rotary pose graph total set as a starting point, and taking a second preset number of rotary pose graphs from the rotary pose graph total set to form the pose graph set of the N-1 layer.
Preferably, if the matching result satisfies a matching condition, updating the current grid map according to the laser spot map, and determining the current position of the robot includes:
and if the matching probability of the matching result is not less than a first matching threshold, updating the current grid map according to the laser point diagram, and determining the current position of the robot.
Preferably, if the matching result satisfies a matching condition, updating the current grid map according to the laser spot map, and determining the current position of the robot includes:
and if the matching probability of the matching result is smaller than the first matching threshold and not smaller than a second matching threshold, and the matching probability of the projection point specified in the matching result is within the laser point matching range, updating the current grid map according to the laser point map, and determining the current position of the robot, wherein the second matching threshold is smaller than the first matching threshold.
Based on the same inventive concept, in a second aspect, the present invention further provides a positioning device for a robot, comprising:
the acquisition module is used for acquiring a laser point diagram acquired by a robot, wherein the laser point diagram comprises a plurality of laser points and a semantic value of each laser point in the plurality of laser points;
the matching module is used for matching the laser point diagram with the current grid map of the robot and obtaining a matching result;
and the execution module is used for updating the current grid map according to the laser point map and determining the current position of the robot if the matching result meets the matching condition.
Based on the same inventive concept, in a third aspect, the invention provides a robot comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps of the positioning method of the robot when executing the program.
Based on the same inventive concept, in a fourth aspect, the present invention provides a computer-readable storage medium having stored thereon a computer program, which, when executed by a processor, performs the steps of the positioning method of the robot.
One or more technical solutions in the embodiments of the present invention have at least the following technical effects or advantages:
in the embodiment of the invention, a laser point diagram collected by a robot is obtained firstly, wherein the laser point diagram comprises a plurality of laser points and semantic values of each laser point in the plurality of laser points; and matching the laser point diagram with the current grid map of the robot to obtain a matching result. And then, judging matching, and if the matching result meets the matching condition, updating the current grid map according to the laser point diagram and determining the current position of the robot. The positioning method of the embodiment of the invention quickly corrects the positioning of the robot, so that the robot has strong adaptability to the field environment, the indoor positioning accuracy of the robot is improved, and the method also has the characteristic of low cost.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. Also, like reference numerals are used to refer to like parts throughout the drawings. In the drawings:
fig. 1 shows a flow chart illustrating the steps of a positioning method of a robot in an embodiment of the present invention;
fig. 2 shows a block schematic diagram of a positioning device of a robot in an embodiment of the invention;
fig. 3 shows a schematic structural diagram of a robot in an embodiment of the present invention.
Detailed Description
Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.
Example one
A first embodiment of the present invention provides a positioning method for a robot, which is applied to a robot as shown in fig. 1. The following describes in detail specific implementation steps of the positioning method for a robot provided in this embodiment with reference to fig. 1:
firstly, executing step S101, and acquiring a laser point diagram collected by a robot, wherein the laser point diagram comprises a plurality of laser points and a semantic value of each laser point in the plurality of laser points;
specifically, when the robot is in a certain space, a laser spot diagram of the space acquired by the robot is acquired, and the laser spot diagram comprises a plurality of laser spots. When each laser point is acquired, not only the position information of the laser point but also the semantic value of the laser point is acquired. The semantic value of the laser point represents the concrete meaning of the laser. For example, the semantic value of a laser point is 1, and the real object represented by the laser point is unknown. The semantic value of the laser point is 2, and the real object represented by the laser point is a wall surface. The semantic value of the laser point is 3, and the object represented by the laser point is a door. The semantic value of the laser point is 4, and the represented object is a cabinet.
Then, executing step 102, matching the laser point diagram with the current grid map of the robot, and obtaining a matching result;
specifically, after a current grid map is down-sampled into N layers of grid sub-maps, matching a laser point diagram with the N layers of grid sub-maps to obtain an N layer of pose map set, wherein the resolution of the N layers of grid sub-maps is sequentially reduced, and N is larger than or equal to 2.
The downsampling of the current grid map into N-layer grid sub-maps specifically includes: and dividing the current grid map into N layers of grid maps with sequentially reduced resolutions. For example, the current grid map is down-sampled into a first-layer grid map, a second-layer grid sub-map, and a third-layer grid sub-map, the resolution of the first-layer grid sub-map being 800 × 800, the resolution of the second-layer grid sub-map being 400 × 400, and the resolution of the third-layer grid sub-map being 100 × 100.
And after the current grid map is down-sampled into N layers of grid sub-maps, matching a laser point diagram with the N layer of grid sub-maps, and obtaining a pose map set of the N layer. The specific process of obtaining the pose atlas of the Nth layer is as follows:
the method comprises the steps of firstly, taking each pixel point in an Nth-layer grid sub-map as a rotating point, matching the Nth-layer grid sub-map with a laser point map after rotating the Nth-layer grid sub-map once according to a first preset rotating angle to obtain a matching pose map and a projection point set on the matching pose map, and obtaining the matching probability of the matching pose map according to the projection point set on the matching pose map, wherein the projection point set on the matching pose map is a set of points projected by a plurality of laser points in the Nth-layer grid sub-map, and the first preset rotating angle is set according to actual requirements.
And secondly, after the nth-layer grid sub-map rotates by one circle, obtaining a matching pose map set of each pixel point in the nth-layer grid sub-map, and after obtaining the matching pose map set, obtaining a matching pose map total set of the nth-layer grid sub-map, wherein each frame of matching pose map in the matching pose map total set is ordered according to the sequence of the matching probability of the matching pose maps which is sequentially decreased.
And taking the matching pose graph corresponding to the maximum matching probability of the matching pose graph in the matching pose graph total set as a starting point, and taking a first preset number of matching pose graphs from the matching pose graph total set to form a pose graph set of the Nth layer. Wherein, the first preset number is set according to actual requirements.
For example, it is assumed that the nth grid sub-map is the third grid sub-map, the resolution of the third grid sub-map is 100 × 100, and the resolution of 100 × 100 indicates that the third grid sub-map has 100 × 100 — 10000 pixels. And taking each pixel point of the grid map of the third layer as a rotation point for matching. For each rotation point, after the third-layer grid sub-map rotates once at a first preset rotation angle of 30 degrees in a clockwise direction from the east, matching the third-layer grid sub-map with a laser point map to obtain a frame of matching pose map and a projection point set of the matching pose map; and obtaining the matching probability of the matching pose image according to the projection point set of the matching pose image.
Rotating the third-layer grid sub-map by 30 degrees again, wherein the third-layer grid sub-map is rotated by 60 degrees, and matching the third-layer grid sub-map rotated by 30 degrees again with the laser point map to obtain a frame of matching pose map and a projection point set of the matching pose map; and obtaining the matching probability of the matching pose image according to the projection point set of the matching pose image.
Rotating the third-layer grid sub-map by 30 degrees again, wherein the third-layer grid sub-map is rotated by 90 degrees, and matching the third-layer grid sub-map rotated by 30 degrees again with the laser point map to obtain a frame of matching pose map and a projection point set of the matching pose map; and obtaining the matching probability of the matching pose image according to the projection point set of the matching pose image. And by parity of reasoning, continuously rotating the third-layer grid map, and continuously matching the third-layer grid sub map with the laser dot map.
After the third-layer grid sub-map is rotated by 360 degrees (namely, one circle), a matching pose atlas of the rotation point is obtained. Because the first preset rotation angle is 30 degrees, one frame of matching pose image is obtained, and the rotation points together obtain 12 frames of matching pose images at 360 degrees/30 degrees.
And obtaining a matching pose atlas of each rotation point according to the process of each rotation point. And obtaining a matching pose graph set of each rotation point, and obtaining a matching pose graph total set of the third layer of grid sub-maps, namely 12 multiplied by 10000 which is 120000 frames of matching pose graphs.
When each frame of matching pose image is obtained, the matching probability of each frame of matching pose image is also obtained. And sequencing each frame of matching pose graphs in the matching pose graph total set according to the sequence of sequentially decreasing matching probability, and taking the matching pose graph corresponding to the maximum matching pose graph in the matching pose graph total set as a starting point. For example, the matching probability of the matching pose map of the 1 st frame is 0.99, the matching probability of the matching pose map of the 2 nd frame is 0.98, the matching probability of the matching pose map of the 3 rd frame is 0.95.
And (4) extracting the matching pose images of the previous 30 frames from the matching pose image total set, namely extracting the matching pose images of the 1 st frame to the 30 th frame to form a pose image set of a third layer.
The matching probability of each frame of matching pose graph is obtained according to the probability value of each projection point in the projection point set on the matching pose graph, wherein the probability value is the relation probability value between the semantic value of the laser point corresponding to each projection point and the semantic value of the map point in the N-th layer raster sub-map.
Specifically, taking an example of matching a laser spot diagram with an nth-layer raster sub-map, when a frame of matching pose diagram is obtained, a projection point set exists on the matching pose diagram, where the projection point set on the matching pose diagram is a set of points projected by a plurality of laser spots of the laser spot diagram in the nth-layer raster sub-map. Each proxel in the set of proxels represents a point at which a laser point is projected in the nth layer grid sub-map. And the probability value of each projection point is the relation probability between the semantic value of the laser point corresponding to each projection point and the semantic value of the map point in the N-th layer grid sub-map.
The probability values of the projection points are set as follows:
the semantic value of the laser point corresponding to the projection point and the semantic value of the map point in the Nth layer of grid sub-map are unknown, and the probability value of the projection point is 1;
the semantic value of the laser point corresponding to the projection point is unknown, and if the semantic value of the map point in the nth layer of grid sub-map is not unknown, the probability value of the projection point is 0.5;
if the semantic value of the laser point corresponding to the projection point is not unknown and the semantic value of the map point in the N-th layer of grid sub-map is unknown, the probability value of the projection point is 0.9;
the semantic value of the laser point corresponding to the projection point is not unknown, the semantic value of the map point in the N-th layer of grid sub-map is not unknown, and the semantic value of the laser point corresponding to the projection point is the same as the semantic value of the map point in the N-th layer of grid sub-map, so that the probability value of the projection point is 5;
and if the semantic value of the laser point corresponding to the projection point is not unknown, the semantic value of the map point in the N-th layer of grid sub-map is not unknown, and the semantic value of the laser point corresponding to the projection point is different from the semantic value of the map point in the N-th layer of grid sub-map, the probability value of the projection point is 0.2.
After the probability value of each projection point in the projection point set of the matching pose image is determined, the probability values of the projection points are added to obtain a sum S, and the sum S is divided by the number n of the projection points in the projection point set to obtain the matching probability of the matching pose image.
After the pose atlas of the Nth layer is obtained, each frame of pose atlas in the pose atlas of the Nth layer is subjected to up-sampling, and a sampled pose atlas of the Nth layer is obtained.
Specifically, the pose map of each frame of the nth layer in the pose map set of the nth layer represents the orientation of one reference point. For example, the nth grid sub-map is a third grid sub-map, the resolution of the third grid sub-map is 100 × 100, and a frame of the third layer of the pose graph represents a pose graph of a 3 rd orientation of a 1000 th pixel point in the third grid sub-map, where the 1000 th pixel point is a reference point, the 3 rd orientation is an orientation when the reference point is a rotation point, and the third grid sub-map is rotated three times in a clockwise direction from the east to the south by a first preset rotation angle of 30 °, that is, the orientation is directed to 90 °. When the pose graph of the third layer is up-sampled to the grid sub-map of the second layer (the resolution of the grid sub-map of the second layer is 400 x 400), one pixel point of the pose graph of the third layer is changed into four pixel points, namely the reference point of the pose graph of the third layer is changed into four pixel points, and the pose graph of the third layer becomes a sampled pose graph of the third layer after up-sampling. And (4) upsampling all 30 frames of displacement images of the third layer to obtain a sampled pose image set of the third layer.
And then matching the sampled pose atlas of the Nth layer with the grid sub-map of the (N-1) th layer to obtain a pose atlas of the (N-1) th layer, then up-sampling each frame of the pose atlas in the pose atlas of the (N-1) th layer to obtain a sampled pose atlas of the (N-1) th layer, and repeatedly executing the operation until the grid sub-map of the first layer in the grid sub-map of the N layers is matched and a final pose is obtained, wherein the matching result is the final pose.
Specifically, the specific process of matching the sampled pose atlas of the nth layer with the grid sub-map of the (N-1) th layer to obtain the pose atlas of the (N-1) th layer is that, for each sampled pose atlas of the nth layer in the sampled pose atlas sets of the N layers:
the method comprises the steps that firstly, each pixel point of a reference point of a sampled pose graph of the Nth layer is taken as a rotation point, the sampled pose graph of the Nth layer is rotated once within a set rotation range according to a second preset rotation angle to obtain a rotation pose graph and a projection point set on the rotation pose graph, and then the matching probability of the rotation pose graph is obtained according to the projection point set on the rotation pose graph, wherein the projection point set on the rotation pose graph is a set of points projected by the sampled pose graph of the Nth layer in a grid sub-map of the (N-1) th layer;
secondly, after the sampled pose graph of the Nth layer rotates in a set rotation range, obtaining a rotation pose graph set of each pixel point of the reference point of the sampled pose graph of the Nth layer, and obtaining a rotation pose graph total set of the sampled pose graph sets of the Nth layer after obtaining the rotation pose graph set, wherein each frame of rotation pose graph of the rotation pose graph total set is ordered according to the sequence of the sequentially decreasing matching probability of the rotation pose graphs;
and thirdly, taking the rotary pose graph corresponding to the maximum matching probability of the rotary pose graph in the rotary pose graph total set as a starting point, and taking out a second preset number of rotary pose graphs from the rotary pose graph total set to form a pose graph set of the (N-1) th layer. Wherein the second preset number is set according to actual requirements.
Taking the nth grid sub-map as the third grid sub-map, the resolution of the third grid sub-map is 100 × 100, the nth-1 grid sub-map is the second grid sub-map, and the resolution of the second grid sub-map is 400 × 400 as an example.
As for each frame of the sampled pose image of the third layer in the sampled pose image set of the third layer, it can be known from the above up-sampling process that the reference points of the sampled pose image of the third layer include four pixel points, and then each pixel point of the reference points of the sampled pose image of the third layer is taken as a rotation point. For each rotation point of the sampled pose graph of the third layer of a frame, the orientation of the sampled pose graph of the third layer is assumed to be 90 ° to the south, and the orientation of the sampled pose graph of the third layer at the reference point thereof is ± 30 ° in the range of 60 ° to 120 °. After the sampled pose graph of the third layer rotates once within the range of +/-30 degrees of the orientation of the reference point of the third layer by a second preset rotation angle of 10 degrees, matching the sampled pose graph of the third layer with the grid sub-map of the second layer to obtain a frame of rotating pose graph (the rotating pose graph and the matching pose graph both represent the matched pose graph) and a projection point set of the rotating pose graph; and obtaining the matching probability of the rotary pose image according to the projection point set of the rotary pose image. The method for obtaining the matching probability of the rotation pose graph is the same as the method for obtaining the matching probability of the rotation pose graph, and is not described herein again.
And rotating the sampled pose graph of the third layer within the range of 60-120 degrees to obtain a rotating pose graph set of the rotating point, namely 6 frames of rotating pose graphs. After each pixel point of the reference point of the sampled pose graph of the third layer performs the above operation, a rotation pose atlas of the sampled pose graph of the third layer is obtained, that is, 6 × 4 is 24 frames of rotation pose graphs. And after each frame of the sampled pose graph of the third layer executes the operation, obtaining a total set of the rotation pose graphs of the sampled pose graph sets of the third layer.
When each frame of rotation pose image is obtained, the matching probability of each frame of rotation pose image is also obtained. And sequencing each frame of the rotation position map in the rotation position map total set according to the sequence of the sequentially decreasing matching probability, and taking the rotation position map corresponding to the maximum rotation position map in the rotation position map total set as a starting point. For example, the matching probability of the rotation pose map of the 1 st frame is 0.99, the matching probability of the rotation pose map of the 2 nd frame is 0.98, the matching probability of the rotation pose map of the 3 rd frame is 0.95.
And taking out the rotation pose images of the previous 15 frames from the rotation pose image total set, namely taking out the rotation pose images of the 1 st frame to the 15 th frame to form a pose image set of a second layer.
After the pose atlas of the second layer is obtained, each frame of pose atlas in the pose atlas of the second layer is subjected to up-sampling, and a sampled pose atlas of the second layer is obtained. And then, matching the sampled pose image set of the second layer with the grid sub-map of the first layer to obtain a final pose image, namely obtaining a matching result. The process of matching the sampled pose atlas of the second layer with the grid sub-map of the first layer is referred to as the process of matching the sampled pose atlas of the third layer with the grid sub-map of the second layer, and the two principles are the same.
Then, step S103 is executed, and if the matching result satisfies the matching condition, the current grid map is updated according to the laser spot map, and the current position of the robot is determined.
Specifically, if the matching probability of the matching result is not less than a first matching threshold, that is, the matching probability of the final pose map is not less than the first matching threshold, the current grid map is updated according to the laser point map, and the current position of the robot is determined. The first matching threshold is usually 0.8, and may also be set according to actual requirements.
And if the matching probability of the matching result is smaller than the first matching threshold and not smaller than the second matching threshold, and the matching probability of the designated projection point in the matching result is within the laser point matching range, updating the current grid map according to the laser point map, and determining the current position of the robot, wherein the second matching threshold is smaller than the first matching threshold. The second matching threshold is usually 0.5, and may be set according to actual requirements. The laser point matching range can also be set according to actual requirements.
The process of obtaining the matching probability of the designated projection points comprises the steps of obtaining a projection point set of a matching result (final pose graph), taking the projection points corresponding to the semantic value of the laser points in the matching result not less than the semantic threshold as the designated projection points, adding the probability values of all the designated projection points to obtain a sum, and dividing the sum by the number of the designated projection points to obtain the matching probability of the designated projection points. The semantic threshold is set according to actual requirements, for example, the semantic threshold is 2.
One or more technical solutions in the embodiments of the present invention have at least the following technical effects or advantages:
in the embodiment, a laser spot diagram acquired by a robot is acquired, wherein the laser spot diagram comprises a plurality of laser spots and a semantic value of each laser spot in the plurality of laser spots; and matching the laser point diagram with the current grid map of the robot to obtain a matching result. And then, judging matching, and if the matching result meets the matching condition, updating the current grid map according to the laser point diagram and determining the current position of the robot. The positioning method of the embodiment can quickly correct the positioning of the robot, so that the adaptability of the robot to the field environment is strong, the indoor positioning accuracy of the robot is improved, and the method also has the characteristic of low cost.
Example two
Based on the same inventive concept, a second embodiment of the present invention further provides a positioning apparatus for a robot, as shown in fig. 2, including:
the acquisition module 201 is configured to acquire a laser spot diagram acquired by a robot, where the laser spot diagram includes a plurality of laser spots and a semantic value of each laser spot in the plurality of laser spots;
the matching module 202 is configured to match the laser point diagram with the current grid map of the robot, and obtain a matching result;
and the execution module 203 is configured to update the current grid map according to the laser spot map and determine the current position of the robot if the matching result meets the matching condition.
As an alternative embodiment, the matching module 202, configured to match the laser point map with the current grid map of the robot and obtain a matching result, includes:
after the current grid map is downsampled into N layers of grid sub-maps, matching the laser point diagram with the N layers of grid sub-maps to obtain an N layer of pose map set, wherein the resolution of the N layers of grid sub-maps is reduced in sequence, and N is larger than or equal to 2;
up-sampling each frame of the pose image set of the Nth layer, and obtaining a sampled pose image set of the Nth layer;
and matching the sampled pose atlas at the Nth layer with the grid sub-map at the Nth-1 st layer to obtain a pose atlas at the Nth-1 st layer, then up-sampling each frame of pose atlas in the pose atlas at the Nth-1 st layer to obtain a sampled pose atlas at the Nth-1 st layer, and repeatedly executing the operation until the grid sub-map at the first layer in the grid sub-map at the Nth layer is matched to obtain a final pose atlas, wherein the matching result is the final pose atlas.
As an alternative embodiment, the matching module 202, configured to match the laser point map with the grid sub-map of the nth layer, and obtain a pose atlas of the nth layer, includes:
taking each pixel point in the nth layer of grid sub-map as a rotation point, matching the nth layer of grid sub-map with the laser point map after rotating the nth layer of grid sub-map once according to a first preset rotation angle to obtain a matching pose map and a projection point set on the matching pose map, and obtaining the matching probability of the matching pose map according to the projection point set on the matching pose map, wherein the projection point set on the matching pose map is a set of points projected by the laser points in the nth layer of grid sub-map;
after the nth-layer grid sub-map rotates by one circle, obtaining a matching pose atlas of each pixel point in the nth-layer grid sub-map, and after obtaining the matching pose atlas, obtaining a matching pose atlas total set of the nth-layer grid sub-map, wherein each frame of matching pose atlas in the matching pose atlas total set is ordered according to the sequence of the matching probability of the matching pose atlas which is sequentially decreased;
and taking the matching pose graph corresponding to the maximum matching probability of the matching pose graph in the matching pose graph total set as a starting point, and taking a first preset number of matching pose graphs from the matching pose graph total set to form the pose graph set of the Nth layer.
As an alternative embodiment, the matching module 202, configured to obtain the matching probability of the matching pose graph according to the set of projection points on the matching pose graph, includes:
and obtaining the matching probability according to the probability value of each projection point in the projection point set on the matching pose graph, wherein the probability value is the relation probability value between the semantic value of the laser point corresponding to each projection point and the semantic value of the map point in the Nth layer of grid sub-map.
As an alternative embodiment, the matching module 202 is configured to match the sampled pose atlas at the nth layer with the grid sub-map at the N-1 st layer to obtain the pose atlas at the N-1 st layer, and includes:
for each frame of sampled pose graph of the Nth layer in the sampled pose graph sets of the N layers, taking each pixel point of the reference point of the sampled pose graph of the Nth layer as a rotation point, rotating the sampled pose graph of the Nth layer once within a set rotation range according to a second preset rotation angle to obtain a rotation pose graph and a projection point set on the rotation pose graph, and then obtaining the matching probability of the rotation pose graph according to the projection point set on the rotation pose graph, wherein the projection point set on the rotation pose graph is a set of points projected by the sampled pose graph of the Nth layer in the grid sub-map of the N-1 layer;
after the sampled pose graph of the Nth layer rotates in the set rotation range, obtaining a rotation pose graph set of each pixel point of the reference point of the sampled pose graph of the Nth layer, and after obtaining the rotation pose graph set, obtaining a rotation pose graph total set of the sampled pose graph sets of the Nth layer, wherein each frame of rotation pose graph of the rotation pose graph total set is ordered according to the sequence of the sequentially decreasing matching probability of the rotation pose graph;
and taking the rotary pose graph corresponding to the maximum matching probability of the rotary pose graph in the rotary pose graph total set as a starting point, and taking a second preset number of rotary pose graphs from the rotary pose graph total set to form the pose graph set of the N-1 layer.
As an alternative embodiment, the executing module 203 is configured to, if the matching result meets the matching condition, update the current grid map according to the laser spot map, and determine the current position of the robot, and includes:
and if the matching probability of the matching result is not less than a first matching threshold, updating the current grid map according to the laser point diagram, and determining the current position of the robot.
As an alternative embodiment, the executing module 203 is configured to, if the matching result meets the matching condition, update the current grid map according to the laser spot map, and determine the current position of the robot, and includes:
and if the matching probability of the matching result is smaller than the first matching threshold and not smaller than a second matching threshold, and the matching probability of the projection point specified in the matching result is within the laser point matching range, updating the current grid map according to the laser point map, and determining the current position of the robot, wherein the second matching threshold is smaller than the first matching threshold.
Since the positioning device of the robot described in this embodiment is a device used for implementing the positioning method of the robot in the first embodiment of the present application, based on the positioning method of the robot described in the first embodiment of the present application, a person skilled in the art can understand the specific implementation of the positioning device of the robot in this embodiment and various variations thereof, and therefore, how to implement the method in the first embodiment of the present application by the positioning device of the robot is not described in detail herein. The device used by those skilled in the art to implement the positioning method of the robot in the first embodiment of the present application is within the scope of the present application.
EXAMPLE III
Based on the same inventive concept, the third embodiment of the present invention further provides a robot, as shown in fig. 3, including a memory 304, a processor 302, and a computer program stored on the memory 304 and executable on the processor 302, wherein the processor 302 implements the steps of any one of the positioning methods of the robot when executing the program.
Where in fig. 3 a bus architecture (represented by bus 300), bus 300 may include any number of interconnected buses and bridges, bus 300 linking together various circuits including one or more processors, represented by processor 302, and memory, represented by memory 304. The bus 300 may also link together various other circuits such as peripherals, voltage regulators, power management circuits, and the like, which are well known in the art, and therefore, will not be described any further herein. A bus interface 306 provides an interface between the bus 300 and the receiver 301 and transmitter 303. The receiver 301 and the transmitter 303 may be the same element, i.e., a transceiver, providing a means for communicating with various other apparatus over a transmission medium. The processor 302 is responsible for managing the bus 300 and general processing, and the memory 304 may be used for storing data used by the processor 302 in performing operations.
Example four
Based on the same inventive concept, a fourth embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, implements the steps of any one of the methods of the positioning method of the robot described in the first embodiment.
As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present invention is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. Therefore, it is intended that the appended claims be interpreted as including preferred embodiments and all such alterations and modifications as fall within the scope of the invention.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present invention without departing from the spirit and scope of the invention. Thus, if such modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include such modifications and variations.
Claims (10)
1. A method of positioning a robot, comprising:
the method comprises the steps of acquiring a laser point diagram collected by a robot, wherein the laser point diagram comprises a plurality of laser points and semantic values of each laser point in the plurality of laser points;
matching the laser point diagram with a current grid map of the robot, and obtaining a matching result;
and if the matching result meets the matching condition, updating the current grid map according to the laser point diagram, and determining the current position of the robot.
2. The method of claim 1, wherein the matching the laser spot map with the current grid map of the robot and obtaining a matching result comprises:
after the current grid map is downsampled into N layers of grid sub-maps, matching the laser point diagram with the N layers of grid sub-maps to obtain an N layer of pose map set, wherein the resolution of the N layers of grid sub-maps is reduced in sequence, and N is larger than or equal to 2;
up-sampling each frame of the pose image set of the Nth layer, and obtaining a sampled pose image set of the Nth layer;
and matching the sampled pose atlas at the Nth layer with the grid sub-map at the Nth-1 st layer to obtain a pose atlas at the Nth-1 st layer, then up-sampling each frame of pose atlas in the pose atlas at the Nth-1 st layer to obtain a sampled pose atlas at the Nth-1 st layer, and repeatedly executing the operation until the grid sub-map at the first layer in the grid sub-map at the Nth layer is matched to obtain a final pose atlas, wherein the matching result is the final pose atlas.
3. The method of claim 2, wherein matching the laser point map with an nth tier grid sub-map and obtaining an nth tier pose atlas comprises:
taking each pixel point in the nth layer of grid sub-map as a rotation point, matching the nth layer of grid sub-map with the laser point map after rotating the nth layer of grid sub-map once according to a first preset rotation angle to obtain a matching pose map and a projection point set on the matching pose map, and obtaining the matching probability of the matching pose map according to the projection point set on the matching pose map, wherein the projection point set on the matching pose map is a set of points projected by the laser points in the nth layer of grid sub-map;
after the nth-layer grid sub-map rotates by one circle, obtaining a matching pose atlas of each pixel point in the nth-layer grid sub-map, and after obtaining the matching pose atlas, obtaining a matching pose atlas total set of the nth-layer grid sub-map, wherein each frame of matching pose atlas in the matching pose atlas total set is ordered according to the sequence of the matching probability of the matching pose atlas which is sequentially decreased;
and taking the matching pose graph corresponding to the maximum matching probability of the matching pose graph in the matching pose graph total set as a starting point, and taking a first preset number of matching pose graphs from the matching pose graph total set to form the pose graph set of the Nth layer.
4. The method of claim 3, wherein the obtaining the matching probability of the matching pose graph from the set of projection points on the matching pose graph comprises:
and obtaining the matching probability according to the probability value of each projection point in the projection point set on the matching pose graph, wherein the probability value is the relation probability value between the semantic value of the laser point corresponding to each projection point and the semantic value of the map point in the Nth layer of grid sub-map.
5. The method of claim 2, wherein matching the sampled pose atlas of the nth layer with the grid sub-map of the N-1 st layer to obtain the pose atlas of the N-1 st layer comprises:
for each frame of sampled pose graph of the Nth layer in the sampled pose graph sets of the N layers, taking each pixel point of the reference point of the sampled pose graph of the Nth layer as a rotation point, rotating the sampled pose graph of the Nth layer once within a set rotation range according to a second preset rotation angle to obtain a rotation pose graph and a projection point set on the rotation pose graph, and then obtaining the matching probability of the rotation pose graph according to the projection point set on the rotation pose graph, wherein the projection point set on the rotation pose graph is a set of points projected by the sampled pose graph of the Nth layer in the grid sub-map of the N-1 layer;
after the sampled pose graph of the Nth layer rotates in the set rotation range, obtaining a rotation pose graph set of each pixel point of the reference point of the sampled pose graph of the Nth layer, and after obtaining the rotation pose graph set, obtaining a rotation pose graph total set of the sampled pose graph sets of the Nth layer, wherein each frame of rotation pose graph of the rotation pose graph total set is ordered according to the sequence of the sequentially decreasing matching probability of the rotation pose graph;
and taking the rotary pose graph corresponding to the maximum matching probability of the rotary pose graph in the rotary pose graph total set as a starting point, and taking a second preset number of rotary pose graphs from the rotary pose graph total set to form the pose graph set of the N-1 layer.
6. The method of claim 1, wherein if the matching result satisfies a matching condition, updating the current grid map and determining the current position of the robot according to the laser spot map comprises:
and if the matching probability of the matching result is not less than a first matching threshold, updating the current grid map according to the laser point diagram, and determining the current position of the robot.
7. The method of claim 6, wherein if the matching result satisfies a matching condition, updating the current grid map and determining the current position of the robot according to the laser spot map comprises:
and if the matching probability of the matching result is smaller than the first matching threshold and not smaller than a second matching threshold, and the matching probability of the projection point specified in the matching result is within the laser point matching range, updating the current grid map according to the laser point map, and determining the current position of the robot, wherein the second matching threshold is smaller than the first matching threshold.
8. A positioning device for a robot, comprising:
the acquisition module is used for acquiring a laser point diagram acquired by a robot, wherein the laser point diagram comprises a plurality of laser points and a semantic value of each laser point in the plurality of laser points;
the matching module is used for matching the laser point diagram with the current grid map of the robot and obtaining a matching result;
and the execution module is used for updating the current grid map according to the laser point map and determining the current position of the robot if the matching result meets the matching condition.
9. A robot comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor, when executing the program, carries out the method steps of any of claims 1-7.
10. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the method steps of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111481096.4A CN114216452B (en) | 2021-12-06 | 2021-12-06 | Positioning method and device for robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111481096.4A CN114216452B (en) | 2021-12-06 | 2021-12-06 | Positioning method and device for robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114216452A true CN114216452A (en) | 2022-03-22 |
| CN114216452B CN114216452B (en) | 2024-03-19 |
Family
ID=80700023
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111481096.4A Active CN114216452B (en) | 2021-12-06 | 2021-12-06 | Positioning method and device for robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114216452B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200226782A1 (en) * | 2018-05-18 | 2020-07-16 | Boe Technology Group Co., Ltd. | Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database |
| CN112179330A (en) * | 2020-09-14 | 2021-01-05 | 浙江大华技术股份有限公司 | Pose determination method and device of mobile equipment |
| CN112258618A (en) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map |
| CN113375683A (en) * | 2021-06-10 | 2021-09-10 | 亿嘉和科技股份有限公司 | Real-time updating method for robot environment map |
| CN113483747A (en) * | 2021-06-25 | 2021-10-08 | 武汉科技大学 | Improved AMCL (advanced metering library) positioning method based on semantic map with corner information and robot |
-
2021
- 2021-12-06 CN CN202111481096.4A patent/CN114216452B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200226782A1 (en) * | 2018-05-18 | 2020-07-16 | Boe Technology Group Co., Ltd. | Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database |
| CN112179330A (en) * | 2020-09-14 | 2021-01-05 | 浙江大华技术股份有限公司 | Pose determination method and device of mobile equipment |
| CN112258618A (en) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map |
| CN113375683A (en) * | 2021-06-10 | 2021-09-10 | 亿嘉和科技股份有限公司 | Real-time updating method for robot environment map |
| CN113483747A (en) * | 2021-06-25 | 2021-10-08 | 武汉科技大学 | Improved AMCL (advanced metering library) positioning method based on semantic map with corner information and robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114216452B (en) | 2024-03-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20220245952A1 (en) | Parking spot detection method and parking spot detection system | |
| US11354238B2 (en) | Method and device for determining memory size | |
| CN111476343B (en) | Method and apparatus for utilizing masking parameters | |
| CN112017205B (en) | Automatic calibration method and system for space positions of laser radar and camera sensor | |
| CN110599586A (en) | Semi-dense scene reconstruction method and device, electronic equipment and storage medium | |
| CN114036253B (en) | High-precision map data processing method, device, electronic equipment and medium | |
| CN112198878B (en) | Instant map construction method and device, robot and storage medium | |
| CN109073390B (en) | Positioning method and device, electronic equipment and readable storage medium | |
| CN114943952A (en) | Method, system, device and medium for obstacle fusion under multi-camera overlapped view field | |
| CN114240805B (en) | Multi-angle SAR dynamic imaging detection method and device | |
| CN115326051A (en) | Positioning method and device based on dynamic scene, robot and medium | |
| CN115436920A (en) | Laser radar calibration method and related equipment | |
| CN112362059A (en) | Method, apparatus, computer device and medium for positioning mobile carrier | |
| CN112967393B (en) | Correction method and device for vehicle movement track, electronic equipment and storage medium | |
| CN112215887A (en) | Pose determination method and device, storage medium and mobile robot | |
| CN114216452B (en) | Positioning method and device for robot | |
| CN115019150B (en) | Target detection fixed point model establishing method and device and readable storage medium | |
| CN116481513A (en) | Map generation method and device and electronic equipment | |
| CN115731408A (en) | Training method and device of image processing model, electronic equipment and storage medium | |
| CN110880003A (en) | Image matching method and device, storage medium and automobile | |
| CN113513983B (en) | Precision detection method and device, electronic equipment and medium | |
| CN112801193B (en) | Positioning data processing method and device, electronic equipment and medium | |
| CN115183786A (en) | Training method and device of sensor error prediction model for automatic driving | |
| CN112556702A (en) | Height correction method for vehicle moving track and related device | |
| CN115619958B (en) | Target aerial view generation method and device, electronic device and storage medium |
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 |