CN114593738B - Robot global positioning method and system based on octree search reflective column - Google Patents
Robot global positioning method and system based on octree search reflective column Download PDFInfo
- Publication number
- CN114593738B CN114593738B CN202210496440.5A CN202210496440A CN114593738B CN 114593738 B CN114593738 B CN 114593738B CN 202210496440 A CN202210496440 A CN 202210496440A CN 114593738 B CN114593738 B CN 114593738B
- Authority
- CN
- China
- Prior art keywords
- column
- reflective
- octree
- robot
- 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.)
- Active
Links
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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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
- 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
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Electromagnetism (AREA)
- Bioinformatics & Cheminformatics (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Evolutionary Biology (AREA)
- Computer Networks & Wireless Communication (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Manipulator (AREA)
Abstract
The invention provides a robot global positioning method and system based on octree search reflective columns, which belongs to the technical field of robot positioning, wherein reflective columns in a reflective column global map are traversed once, reflective column triangles meeting requirements are extracted, and the reflective column triangles are stored in an octree data structure; traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using an octree; calculating the current robot pose by using a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by using all the reflective columns which are successfully matched, otherwise, judging that the robot pose is wrong; the invention can effectively improve the speed and accuracy of the global positioning of the robot based on the reflective columns.
Description
Technical Field
The invention relates to the technical field of robot positioning, in particular to a robot global positioning method and system based on octree search reflective columns.
Background
The statements in this section merely provide background information related to the present disclosure and may not constitute prior art.
The rapid development of production automation and intelligence technology is promoted by the application of industrial AGVs, automatic forklifts, intelligent robots and the like, and the positioning technology based on two-dimensional laser radars is one of the key technologies applied to indoor mobile robots. Firstly, under the condition that the characteristics of the natural environment are obvious and various, a grid map of the natural environment is constructed, and technologies such as AMCL (adaptive Monte Carlo localization), CSM (relative Scan matching) and the like are used for matching laser data with the grid map so as to determine the pose of the robot; secondly, under the condition of complicated natural environment, the reflecting columns are artificially placed, and a reflecting column map is constructed to position the mobile robot.
When a reflection column map is used for global positioning of a mobile robot, the traditional method needs to traverse reflection column triangles formed by reflection columns one by one and match the reflection column triangles formed by the reflection columns observed by the current laser radar, so that the time consumed by one-time global positioning is longer along with the increase of the number of the reflection columns in the map, the problem of matching errors possibly occurs, and the requirements of rapidness and accuracy in positioning of the mobile robot cannot be met.
Disclosure of Invention
In order to solve the defects of the prior art, the invention provides a robot global positioning method and system based on octree search reflective columns, which can effectively improve the speed and accuracy of the robot global positioning based on the reflective columns.
In order to achieve the purpose, the invention adopts the following technical scheme:
the invention provides a robot global positioning method based on octree search reflective columns.
A robot global positioning method based on octree search reflective columns comprises the following steps:
traversing the reflective columns in the reflective column global map once, extracting the reflective column triangles meeting the requirements, and storing the reflective column triangles in an octree data structure;
traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using octree;
and calculating the current robot pose by utilizing a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by utilizing all the reflective columns successfully matched, and otherwise, judging that the robot pose is wrong.
As an optional implementation manner, the light reflecting column triangle meeting the requirement includes:
for any one reflecting column triangle, the length of any one side is not more than the maximum distance of the reflecting column observed by the laser radar, and the length difference between any two sides is more than or equal to a preset value.
As an optional implementation manner, storing into the octree data structure includes:
establishing a cube as a root node of an octree by using the maximum distance of a laser radar observation reflecting column as the side length;
dividing the cube into 8 sub-cubes according to a fixed rule and marking;
when the light reflecting column triangle is inserted, X, Y coordinates corresponding to the side lengths of three sides of the light reflecting column triangle and the coordinates of the Z axis are respectively used for calculating which sub cube the light reflecting column triangle should be located in, if other light reflecting column triangles exist in the sub cube, the sub cube is continuously divided and labeled, and the process is repeated until the sub cube does not contain other light reflecting column triangles, and the insertion is finished.
As an optional implementation manner, traversing the currently observed retroreflective bar triangles formed by the retroreflective bar local map, and quickly searching the same retroreflective bar triangles in the global map by using an octree, includes:
extracting reflective columns from the current laser frame to form an observed local map, and performing accurate global positioning only when the number of the observed reflective columns is more than 3;
the method comprises the steps of obtaining a light reflecting column triangle from a local map according to the sequence, calculating the side lengths of three sides of the light reflecting column triangle, determining a position point of the light reflecting column triangle in an octree, and determining a cube as a search range by taking the position point as a center.
As an alternative implementation manner, assuming that the coordinates of the triangular position point of the light reflection column are (S _ lm, S _ mn, S _ nl), the maximum coordinates of the search range of the cube are: (S _ lm + match _ error, S _ mn + match _ error, S _ nl + match _ error), the minimum coordinate of the search range is: (S _ lm-match _ error, S _ mn-match _ error, S _ nl-match _ error), where the match _ error is an allowable error when searching for a reflection column triangle.
As an optional implementation manner, after the search range is determined, searching is started from a root node of an octree, firstly, 8 child nodes of the root node are traversed, and Cmax and Cmin are used for representing the maximum and minimum coordinates of the current child node respectively to judge whether the child nodes are intersected with the SearchBox;
if any of the conditions cmax. x < bmin. x, cmax. y < bmin. y, cmax. z < bmin. z, cmin. x > bmax. x, cmin.y > bmax.y, and cmin.z > bmax.z is satisfied, then the child node does not intersect SearchBox, skips the child node and traverses the next child node;
otherwise, the child node intersects with the SearchBox, whether the current child node is a leaf node is continuously judged, if not, the child node of the current child node is traversed until the current child node becomes the leaf node, then whether a reflection column triangle is contained in the leaf node is judged, the coordinate of the reflection column triangle is located in the SearchBox, if yes, the reflection column triangle is recorded, and if not, the traversing operation is continued until the traversing is completed.
As an optional implementation manner, verifying whether the currently observed other glistenings are matched with the global map according to the current robot pose includes:
finding out global reflective columns corresponding to the local reflective columns l, m and n according to the result of searching the triangular reflective column by the octree;
calculating the pose of the robot in the global map according to the corresponding relation and a trilateral positioning algorithm, calculating the global coordinates of other local reflective columns except l, m and n according to the pose of the robot, respectively searching the reflective columns closest to the global map, and if the closest distance is less than the allowed matching error, successfully matching;
if all the local reflective columns are matched with the global reflective columns successfully finally, the current robot is correct in pose, and otherwise, the pose is wrong; if the pose is correct, the pose of the mobile robot is calculated again by using all the successfully matched reflecting columns, and the final pose of the robot is determined.
The invention provides a robot global positioning system based on octree search reflective columns.
The utility model provides a robot global positioning system based on octree search reflection of light post which characterized in that:
the method comprises the following steps:
a reflective pillar traversal storage module configured to: traversing the reflective columns in the reflective column global map once, extracting the reflective column triangles meeting the requirements, and storing the reflective column triangles in an octree data structure;
a reflective pillar triangle search module configured to: traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using an octree;
a robot global positioning module configured to: and calculating the current robot pose by using a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by using all the reflective columns which are successfully matched, and otherwise, judging that the robot pose is wrong.
Compared with the prior art, the invention has the beneficial effects that:
1. according to the robot global positioning method and system based on octree search reflective columns, the reflective column global map is stored in the octree, the reflective column triangles composed of different reflective columns are obtained from the global map, and isosceles or overlong-side reflective column triangles are removed according to the side lengths of the three sides of the reflective column triangles, so that the subsequent algorithm calculation amount is reduced, and the search accuracy is improved.
2. According to the robot global positioning method and system based on octree search reflective columns, reflective column triangles in a local map are searched in octree, the reflective column triangles are extracted from the local map, a search area is set according to the side length of three sides of the reflective column triangles, and the time complexity of an algorithm can be effectively reduced by utilizing octree structure search.
3. According to the octree search reflective column-based robot global positioning method and system, the octree search result is utilized to verify whether the rest reflective columns in the local map are matched with the reflective columns of the global map, so that the position and posture reliability of the robot is further improved.
Advantages of 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 incorporated in and constitute a part of this specification, are included to provide a further understanding of the invention, and are included to illustrate an exemplary embodiment of the invention and not to limit the invention.
Fig. 1 is a schematic flowchart of a robot global positioning method based on octree search for reflective columns according to embodiment 1 of the present invention.
Fig. 2 is a schematic flow chart of storing a reflector map using octree according to embodiment 1 of the present invention.
Fig. 3 is a schematic diagram of inserting the light-reflecting pillar into the octree in a triangular shape according to embodiment 1 of the present invention.
Fig. 4 is a schematic flow chart of a method for searching for a triangular reflective pillar based on an octree according to embodiment 1 of the present invention.
Fig. 5 is a schematic diagram of a verification process of octree search results provided in embodiment 1 of the present invention.
Detailed Description
The invention is further described with reference to the following figures 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.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the invention. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, and it should be understood that when the terms "comprises" and/or "comprising" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof, unless the context clearly indicates otherwise.
The embodiments and features of the embodiments of the present invention may be combined with each other without conflict.
Example 1:
as shown in fig. 1, embodiment 1 of the present invention provides a robot global positioning method based on octree search of reflective columns, which includes the following steps:
s1: and traversing the reflective columns in the reflective column global map once, extracting the reflective column triangles meeting the requirements, and storing the reflective column triangles into the octree data structure.
S2: and traversing the reflective column triangles formed by the currently observed reflective column local map, and quickly searching the same reflective column triangles in the global map by using the octree.
S3: and according to the result of the octree search, calculating the current robot pose by using the traditional trilateration algorithm, and checking whether the other reflective columns observed currently are matched with the global map or not according to the pose.
S1 includes:
according to the serial numbers of the reflective columns in the global map, all the reflective columns are traversed sequentially, every three reflective columns form a reflective column triangle, and the same three reflective columns can only form one reflective column triangle.
If any one side exceeds the maximum distance Lmax of the reflecting columns observed by the laser radar, two reflecting columns forming the side cannot be observed simultaneously, and therefore the reflecting column triangle is not stored in an octree;
if the difference between any two sides is smaller than the allowable error delta ɛ, the light-reflecting column triangle is determined to be an equilateral light-reflecting column triangle, and the light-reflecting column triangle may be inversely matched due to the two equal sides during positioning, so that the light-reflecting column triangle is not stored in the octree. If the light-reflecting column triangle passes the screening, inserting the light-reflecting column triangle into the octree according to the coordinate { S _ ij, S _ jk, S _ ki } formed by the three side lengths of the light-reflecting column triangle, and repeating the steps until all the light-reflecting column triangles are inserted. The flow of storing the retro-reflective bar map using octree is shown in fig. 2.
A schematic diagram of inserting a light-reflecting cylindrical triangle into an octree according to coordinates formed by three side lengths of the light-reflecting cylindrical triangle is shown in fig. 3, a cube is established as a root node of the octree by taking Lmax as the side length, the cube is divided into 8 sub-cubes according to a fixed rule and is labeled, when the light-reflecting cylindrical triangle is inserted, the three side lengths of the three sides correspond to coordinates of x, y and z axes respectively, so that the position of the light-reflecting cylindrical triangle in the sub-cube is calculated, if other light-reflecting cylindrical triangles exist in the sub-cube, the sub-cube is continuously divided and labeled, and the process is repeated until the sub-cube does not contain other light-reflecting cylindrical triangles, and the insertion is completed.
S2 includes:
firstly, extracting reflective columns from a current laser frame to form an observed local map, and performing accurate global positioning only when the number of the observed reflective columns is more than 3;
then, a light-reflecting column triangle is obtained from the local map according to the sequence, the serial numbers of the light-reflecting columns contained in the light-reflecting column triangle in the local map are l, m and n, the side length of three sides of the light-reflecting column triangle is calculated, the position point B { S _ lm, S _ mn and S _ nl } of the light-reflecting column triangle in the octree is determined, a cube SearchBox is determined by taking the point B as the center, the maximum and minimum coordinates of the cube SearchBox are Bmax and Bmin respectively, and the concrete steps are as follows:
Bmax={S_lm+match_error，S_mn+match_error，S_nl +match_error}；
Bmin={S_lm - match_error，S_mn - match_error，S_nl - match_error}；
wherein, match _ error is an allowable error when searching the triangle of the reflection column.
After the SearchBox is determined, searching is started from a root node of an octree, firstly 8 child nodes of the root node are traversed, Cmax and Cmin are used for representing the maximum and minimum coordinates of the current child node respectively, whether the child nodes are intersected with the SearchBox is judged, if any one condition of Cmax.x < Bmin.x, Cmax.y < Bmin.y, Cmax.z < Bmin.z, Cmin.x > Bmax.x, Cmin.y > Bmax.y and Cmin.z > Bmax.z is met, the child nodes are not intersected with the SearchBox, the child nodes are skipped, and the next child node is traversed; otherwise, the child node is intersected with the SearchBox, whether the current child node is a leaf node or not is continuously judged, if not, the child node of the current child node is traversed until the current child node becomes the leaf node, whether a reflection column triangle is contained in the leaf node or not is judged, the coordinate of the reflection column triangle is located in the SearchBox, if yes, the reflection column triangle is recorded, and if not, the traversing operation is continued until the traversing is completed.
In S3, the method includes:
as shown in fig. 4 and 5, global glistens i, j, k corresponding to local glistens l, m, n are found according to the result of searching the glisten triangle by the octree, then the Pose position of the robot in the global map is calculated according to the corresponding relation and the traditional trilateral positioning algorithm, then the global coordinates of the other local glistens except for l, m, n are calculated according to the Pose position of the robot, and the glistens closest to the local glistens are respectively found in the global map, the closest distance is represented by min _ disparity, and if the min _ distance is smaller than the allowed matching error match _ error, the matching is successful.
If all the local reflective columns are matched with the global reflective columns successfully finally, the current robot Pose position is correct, otherwise, the Pose position is incorrect, and if the Pose position is correct, all the reflective columns which are matched successfully are used for calculating the Pose position of the mobile robot again to determine the final Pose position.
Example 2:
the embodiment 2 of the invention provides a robot global positioning system based on octree search reflective columns, which comprises:
a reflective pillar traversal storage module configured to: traversing the reflective columns in the reflective column global map once, extracting the reflective column triangles meeting the requirements, and storing the reflective column triangles in an octree data structure;
a reflective pillar triangle search module configured to: traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using octree;
a robot global positioning module configured to: and calculating the current robot pose by utilizing a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by utilizing all the reflective columns successfully matched, and otherwise, judging that the robot pose is wrong.
The reflective column traverses the memory module, and more particularly, comprises:
according to the serial numbers of the reflective columns in the global map, all the reflective columns are traversed sequentially, every three reflective columns form a reflective column triangle, and the same three reflective columns can only form one reflective column triangle.
The method comprises the steps that the serial numbers of reflecting columns contained in a currently traversed reflecting column triangle are set as i, j and k, the distance between every two reflecting columns is calculated, namely the side length S _ ij, S _ jk and S _ ki of the three sides of the reflecting column triangle, if any one side exceeds the maximum distance Lmax of a laser radar observation reflecting column, two reflecting columns forming the side cannot be observed at the same time, and therefore the reflecting column triangle is not stored in an octree;
if the difference between any two sides is smaller than the allowable error delta ɛ, the reflecting column triangle is determined to be an equilateral reflecting column triangle, and the reflecting column triangle may be inversely matched due to the equal two sides during positioning, so that the reflecting column triangle is not stored in the octree. If the reflecting column triangle passes the screening, inserting the reflecting column triangle into the octree according to the coordinates { S _ ij, S _ jk, S _ ki } formed by the three side lengths of the reflecting column triangle, and repeating the steps until all the reflecting column triangles are inserted.
Establishing a cube as a root node of an octree by taking Lmax as a side length, dividing the cube into 8 sub-cubes according to a fixed rule and marking, and when inserting a light-reflecting column triangle, respectively corresponding coordinates of x, y and z axes by the side lengths of three sides of the cube, thereby calculating that the light-reflecting column triangle should be positioned in the sub-cube, if other light-reflecting column triangles already exist in the sub-cube, continuing to divide the sub-cube and marking, and so on until the sub-cube does not contain other light-reflecting column triangles, and completing the insertion.
The reflective pillar triangle search module, more specifically, includes:
firstly, extracting reflective columns from a current laser frame to form an observed local map, and performing accurate global positioning only when the number of the observed reflective columns is more than 3;
then, a reflection column triangle is obtained from the local map in sequence, the serial numbers of the reflection columns contained in the reflection column triangle in the local map are l, m and n, the side length of three sides of the reflection column triangle is calculated, the position point B { S _ lm, S _ mn and S _ nl } of the reflection column triangle in the octree is determined, a cube SearchBox is determined by taking the point B as the center, and the maximum coordinate and the minimum coordinate of the cube SearchBox are Bmax and Bmin respectively.
Bmax={S_lm+match_error，S_mn+match_error，S_nl +match_error}；
Bmin={S_lm - match_error，S_mn - match_error，S_nl - match_error}；
Wherein, match _ error is the allowable error when searching the triangle of the reflection column.
After the SearchBox is determined, searching is started from a root node of an octree, firstly 8 child nodes of the root node are traversed, Cmax and Cmin are used for representing the maximum and minimum coordinates of the current child node respectively, whether the child nodes are intersected with the SearchBox is judged, if any one condition of Cmax.x < Bmin.x, Cmax.y < Bmin.y, Cmax.z < Bmin.z, Cmin.x > Bmax.x, Cmin.y > Bmax.y and Cmin.z > Bmax.z is met, the child nodes are not intersected with the SearchBox, the child nodes are skipped, and the next child node is traversed; otherwise, the child node is intersected with the SearchBox, whether the current child node is a leaf node or not is continuously judged, if not, the child node of the current child node is traversed until the current child node becomes the leaf node, whether a reflection column triangle is contained in the leaf node or not is judged, the coordinate of the reflection column triangle is located in the SearchBox, if yes, the reflection column triangle is recorded, and if not, the traversing operation is continued until the traversing is completed.
The robot global positioning module, more specifically, comprises:
and finding global reflective columns i, j and k corresponding to the local reflective columns l, m and n according to the result of searching the triangles of the reflective columns by the octree, then calculating the Pose position of the robot in the global map according to the corresponding relation and the traditional trilateral positioning algorithm, calculating the global coordinates of the other local reflective columns except the local reflective columns l, m and n according to the Pose position of the robot, respectively searching the reflective columns which are closest to the global reflective columns in the global map, wherein the closest distance is expressed by min _ disparity, and if the min _ distance is smaller than the allowed matching error match _ error, the matching is successful.
If all the local reflective columns are matched with the global reflective columns successfully finally, the current robot Pose position is correct, otherwise, the Pose position is incorrect, and if the Pose position is correct, all the reflective columns which are matched successfully are used for calculating the Pose position of the mobile robot again to determine the final Pose position.
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 a hardware embodiment, a 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, optical storage, and the like) having computer-usable program code embodied therein.
The present invention has been 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 flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations 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.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by a computer program, which can be stored in a computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), or the like.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. A robot global positioning method based on octree search reflective columns is characterized in that:
the method comprises the following steps:
traversing the reflective columns in the reflective column global map once, extracting reflective column triangles meeting requirements, and storing the reflective column triangles in an octree data structure;
traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using an octree;
calculating the current robot pose by utilizing a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by utilizing all the reflective columns which are successfully matched, otherwise, judging that the robot pose is wrong;
reflective column triangle-shaped of satisfied demand includes:
for any one reflecting column triangle, the length of any one side is not more than the maximum distance of the reflecting column observed by the laser radar, and the length difference between any two sides is more than or equal to a preset value;
storing into an octree data structure, comprising:
establishing a cube as a root node of an octree by using the laser radar to observe the maximum distance of the reflecting column as the side length;
dividing the cube into 8 sub-cubes according to a fixed rule and marking;
when the light reflecting column triangle is inserted, the coordinates of X, Y corresponding to the side length of three sides of the light reflecting column triangle and the Z axis are respectively used for calculating the sub cube in which the light reflecting column triangle is located, if other light reflecting column triangles already exist in the sub cube, the sub cube is continuously divided and labeled, and the process is repeated until the sub cube does not contain other light reflecting column triangles, and the insertion is finished.
2. The octree-based global positioning method for robots to search for retroreflective bars according to claim 1, wherein:
traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using an octree, wherein the method comprises the following steps of:
extracting reflective columns from the current laser frame to form an observed local map, and performing accurate global positioning only when the number of the observed reflective columns is more than 3;
and acquiring a light-reflecting column triangle from the local map according to the sequence, calculating the side lengths of three sides of the light-reflecting column triangle, determining a position point of the light-reflecting column triangle in the octree, and determining a cube as a search range by taking the position point as a center.
3. The octree search delineator column based robot global positioning method of claim 2, characterized in that:
assuming that the coordinates of the triangular position points of the reflective columns are (S _ lm, S _ mn, S _ nl), the maximum coordinates of the search range of the cube are: (S _ lm + match _ error, S _ mn + match _ error, S _ nl + match _ error), the minimum coordinate of the search range is: (S _ lm-match _ error, S _ mn-match _ error, S _ nl-match _ error), where the match _ error is an allowable error when searching for a reflection column triangle.
4. The octree-search reflector-based robot global positioning method according to claim 2, wherein:
after the search range is determined, searching is started from a root node of the octree, firstly, 8 child nodes of the root node are traversed, Cmax and Cmin are used for respectively representing the maximum and minimum coordinates of the current child node, and whether the child nodes are intersected with the SearchBox is judged;
if any of the conditions cmax. x < bmin. x, cmax. y < bmin. y, cmax. z < bmin. z, cmin. x > bmax. x, cmin.y > bmax.y, and cmin.z > bmax.z is satisfied, then the child node does not intersect SearchBox, skips the child node and traverses the next child node;
otherwise, the child node intersects with the SearchBox, whether the current child node is a leaf node or not is continuously judged, if not, the child node of the current child node is traversed until the current child node becomes the leaf node, then whether a reflection column triangle is contained in the leaf node or not is judged, the coordinate of the reflection column triangle is located in the SearchBox, if yes, the reflection column triangle is recorded, and if not, the traversing operation is continued until the traversing is completed;
wherein, the SearchBox is a cube determined by taking the position point of the reflective column triangle in the octree as the center.
5. The octree-based global positioning method for robots to search for retroreflective bars according to claim 1, wherein:
whether the other reflective columns observed currently are matched with the global map or not is verified according to the pose of the current robot, and the method comprises the following steps:
finding out global reflective columns corresponding to the local reflective columns l, m and n according to the result of searching the triangular reflective column by the octree;
calculating the pose of the robot in the global map according to the corresponding relation and a trilateral positioning algorithm, calculating the global coordinates of other local reflective columns except l, m and n according to the pose of the robot, respectively searching the reflective columns closest to the global map, and if the closest distance is less than the allowed matching error, successfully matching;
if all the local reflective columns are matched with the global reflective columns successfully finally, the current robot is correct in pose, and otherwise, the pose is wrong; and if the pose is correct, calculating the pose of the mobile robot again by using all the successfully matched reflective columns, and determining the final pose of the robot.
6. The utility model provides a robot global positioning system based on octree search reflection of light post which characterized in that:
the method comprises the following steps:
a reflective post traversal storage module configured to: traversing the reflective columns in the reflective column global map once, extracting reflective column triangles meeting requirements, and storing the reflective column triangles in an octree data structure;
a reflective pillar triangle search module configured to: traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using octree;
a robot global positioning module configured to: calculating the current robot pose by utilizing a trilateral positioning algorithm according to the result of the octree search, checking whether the other reflective columns observed currently are matched with the global map or not according to the current robot pose, if so, updating the robot pose by utilizing all the reflective columns which are successfully matched, otherwise, judging that the robot pose is wrong;
reflective column triangle-shaped of satisfied demand includes:
for any one reflecting column triangle, the length of any one side is not more than the maximum distance of the laser radar observation reflecting column, and the length difference between any two sides is more than or equal to a preset value;
storing into an octree data structure, comprising:
establishing a cube as a root node of an octree by using the maximum distance of a laser radar observation reflecting column as the side length;
dividing the cube into 8 sub-cubes according to a fixed rule and marking;
when the light reflecting column triangle is inserted, X, Y coordinates corresponding to the side lengths of three sides of the light reflecting column triangle and the coordinates of the Z axis are respectively used for calculating which sub cube the light reflecting column triangle should be located in, if other light reflecting column triangles exist in the sub cube, the sub cube is continuously divided and labeled, and the process is repeated until the sub cube does not contain other light reflecting column triangles, and the insertion is finished.
7. The octree search reflector-based robotic global positioning system of claim 6, wherein:
traversing the reflecting column triangles formed by the currently observed reflecting column local map, and quickly searching the same reflecting column triangles in the global map by using octree, wherein the method comprises the following steps:
extracting reflective columns from the current laser frame to form an observed local map, and performing accurate global positioning only when the number of the observed reflective columns is more than 3;
the method comprises the steps of obtaining a light reflecting column triangle from a local map according to the sequence, calculating the side lengths of three sides of the light reflecting column triangle, determining a position point of the light reflecting column triangle in an octree, and determining a cube as a search range by taking the position point as a center.
8. The octree search reflector-based robotic global positioning system of claim 6, wherein:
whether the other reflective columns observed currently are matched with the global map or not is verified according to the pose of the current robot, and the method comprises the following steps:
finding global light reflecting columns corresponding to the local light reflecting columns l, m and n according to the result of searching the triangles of the light reflecting columns by the octree;
calculating the pose of the robot in the global map according to the corresponding relation and a trilateral positioning algorithm, calculating the global coordinates of other local reflective columns except l, m and n according to the pose of the robot, respectively searching the reflective columns closest to the global map, and if the closest distance is less than the allowed matching error, successfully matching;
if all the local reflective columns are matched with the global reflective columns successfully finally, the current robot is correct in pose, and otherwise, the pose is wrong; if the pose is correct, the pose of the mobile robot is calculated again by using all the successfully matched reflecting columns, and the final pose of the robot is determined.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210496440.5A CN114593738B (en) | 2022-05-09 | 2022-05-09 | Robot global positioning method and system based on octree search reflective column |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210496440.5A CN114593738B (en) | 2022-05-09 | 2022-05-09 | Robot global positioning method and system based on octree search reflective column |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114593738A CN114593738A (en) | 2022-06-07 |
| CN114593738B true CN114593738B (en) | 2022-07-26 |
Family
ID=81811653
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210496440.5A Active CN114593738B (en) | 2022-05-09 | 2022-05-09 | Robot global positioning method and system based on octree search reflective column |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114593738B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110738726A (en) * | 2019-09-27 | 2020-01-31 | 华南理工大学 | robot vision-guided three-dimensional object reconstruction method based on octree |
| KR102083909B1 (en) * | 2018-10-23 | 2020-03-04 | 주식회사 모빌테크 | Automatic extraction method for lane data information for autonomous driving vehicles based on point cloud map |
| CN111060092A (en) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | Rapid matching method of reflective columns |
| CN111596299A (en) * | 2020-05-19 | 2020-08-28 | 三一机器人科技有限公司 | Light reflection column tracking and positioning method and device and electronic equipment |
| CN112629522A (en) * | 2020-12-31 | 2021-04-09 | 山东大学 | AGV positioning method and system with reflector and laser SLAM integrated |
| CN113223078A (en) * | 2021-05-27 | 2021-08-06 | 深圳数马电子技术有限公司 | Matching method and device of mark points, computer equipment and storage medium |
| CN113720324A (en) * | 2021-08-30 | 2021-11-30 | 上海大学 | Octree map construction method and system |
| CN114004869A (en) * | 2021-09-18 | 2022-02-01 | 浙江工业大学 | Positioning method based on 3D point cloud registration |
| CN114371462A (en) * | 2021-12-06 | 2022-04-19 | 广东嘉腾机器人自动化有限公司 | Laser radar positioning method, device and medium |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6330200B2 (en) * | 2013-10-22 | 2018-05-30 | 株式会社国際電気通信基礎技術研究所 | SOUND SOURCE POSITION ESTIMATION DEVICE, MOBILE BODY, AND MOBILE BODY CONTROL METHOD |
| US20180310025A1 (en) * | 2017-04-24 | 2018-10-25 | Nokia Technologies Oy | Method and technical equipment for encoding media content |
| CN109697753B (en) * | 2018-12-10 | 2023-10-03 | 智灵飞（北京）科技有限公司 | Unmanned aerial vehicle three-dimensional reconstruction method based on RGB-D SLAM and unmanned aerial vehicle |
| US20200393268A1 (en) * | 2019-06-17 | 2020-12-17 | DeepMap Inc. | Storing normals in compressed octrees representing high definition maps for autonomous vehicles |
-
2022
- 2022-05-09 CN CN202210496440.5A patent/CN114593738B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102083909B1 (en) * | 2018-10-23 | 2020-03-04 | 주식회사 모빌테크 | Automatic extraction method for lane data information for autonomous driving vehicles based on point cloud map |
| CN110738726A (en) * | 2019-09-27 | 2020-01-31 | 华南理工大学 | robot vision-guided three-dimensional object reconstruction method based on octree |
| CN111060092A (en) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | Rapid matching method of reflective columns |
| CN111596299A (en) * | 2020-05-19 | 2020-08-28 | 三一机器人科技有限公司 | Light reflection column tracking and positioning method and device and electronic equipment |
| CN112629522A (en) * | 2020-12-31 | 2021-04-09 | 山东大学 | AGV positioning method and system with reflector and laser SLAM integrated |
| CN113223078A (en) * | 2021-05-27 | 2021-08-06 | 深圳数马电子技术有限公司 | Matching method and device of mark points, computer equipment and storage medium |
| CN113720324A (en) * | 2021-08-30 | 2021-11-30 | 上海大学 | Octree map construction method and system |
| CN114004869A (en) * | 2021-09-18 | 2022-02-01 | 浙江工业大学 | Positioning method based on 3D point cloud registration |
| CN114371462A (en) * | 2021-12-06 | 2022-04-19 | 广东嘉腾机器人自动化有限公司 | Laser radar positioning method, device and medium |
Non-Patent Citations (2)
| Title |
|---|
| Shanshan Hu ; Dan Li ; Gujie Tang ; Xiangrong Xu.A 3D Semantic Visual SLAM in Dynamic Scenes.《2021 6th IEEE International Conference on Advanced Robotics and Mechatronics》.2021,全文. * |
| 融合反光柱的2D激光SLAM和高精度定位系统;周凯月，张建伟;《现代计算机》;20200420(第11期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114593738A (en) | 2022-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109724612B (en) | AGV path planning method and device based on topological map | |
| CN110220521B (en) | High-precision map generation method and device | |
| JP3962361B2 (en) | Phase determining device, decomposable shape generating device, structural mesh generating device, phase determining method, decomposable shape generating method, computer executable program for executing the phase determining method, and decomposable shape generating method Computer executable program and structured mesh generation system | |
| CN111222418B (en) | Crowdsourcing data rapid fusion optimization method for multiple road segments of lane line | |
| CN108444490B (en) | Robot path planning method based on depth fusion of visible view and A-x algorithm | |
| US5999881A (en) | Automated path planning | |
| CN108305289B (en) | Three-dimensional model symmetry characteristic detection method and system based on least square method | |
| JP2002073130A (en) | Planning method for gross motion path of robot and its controller | |
| CN112257762A (en) | Road network matching method and system among different-source high-precision maps | |
| CN114877905A (en) | Inform-RRT path planning method for bidirectional dynamic growth | |
| CN113311829A (en) | Multi-robot path planning method based on dynamic time window conflict search | |
| Garcia et al. | GPU-based dynamic search on adaptive resolution grids | |
| CN115048704A (en) | Revit-based bridge substructure modeling method, system and device | |
| CN114593738B (en) | Robot global positioning method and system based on octree search reflective column | |
| CN111815684B (en) | Space multivariate feature registration optimization method and device based on unified residual error model | |
| CN113034515A (en) | Bounding box tree-based polygon clipping method, electronic device and storage medium | |
| CN116958316A (en) | Topology map generation method, device, computer equipment and storage medium | |
| CN115167476B (en) | Unmanned system path planning method based on deep reinforcement learning | |
| CN107134007A (en) | The three-dimensional fast modeling method of traffic infrastructure | |
| CN115063560A (en) | Method and system for tracking three-dimensional target in motion state in real time | |
| CN116186867A (en) | Automatic modeling method and terminal for pipeline wall bushing, wing ring and wall penetrating hole | |
| CN113609691A (en) | Intersection modeling processing method for intelligent traffic simulation | |
| CN111323036B (en) | Method and system for intelligently optimizing path of stock yard, electronic equipment and storage medium | |
| CN117557585B (en) | Land parcel segmentation method, system, electronic equipment and storage medium | |
| CN110942513B (en) | Space filling method and device for three-dimensional grid model |
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 |