CN117824667B - Fusion positioning method and medium based on two-dimensional code and laser - Google Patents
Fusion positioning method and medium based on two-dimensional code and laser Download PDFInfo
- Publication number
- CN117824667B CN117824667B CN202410251452.0A CN202410251452A CN117824667B CN 117824667 B CN117824667 B CN 117824667B CN 202410251452 A CN202410251452 A CN 202410251452A CN 117824667 B CN117824667 B CN 117824667B
- Authority
- CN
- China
- Prior art keywords
- dimensional code
- coordinates
- laser
- robot
- representing
- 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
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000004927 fusion Effects 0.000 title claims abstract description 24
- 238000005457 optimization Methods 0.000 claims abstract description 44
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 8
- 230000006870 function Effects 0.000 description 19
- 238000001514 detection method Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000004590 computer program Methods 0.000 description 3
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a fusion positioning method and medium based on two-dimension codes and laser. In the positioning method, any two-dimensional code is scanned on the travelling route of the robot, and a first positioning coordinate of the robot at any moment is obtained based on a calibration result of the two-dimensional code; for any two-dimensional code laser key frame, selecting a plurality of corresponding two-dimensional code data frames, and performing nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of the plurality of first positioning coordinates and the optimized speed of the robot; and for any two-dimensional code laser key frame, calculating coordinates by using time speed and time difference based on the optimized first positioning coordinates corresponding to the two-dimensional code data frame at the first moment corresponding to the laser key frame and the speed of the robot, so as to obtain the coordinates of the robot. Compared with the prior art, the accuracy of robot positioning is improved.
Description
Technical Field
The invention relates to the field of robot navigation and positioning, in particular to a fusion positioning method and medium based on two-dimension codes and lasers.
Background
In robot positioning, in order to cope with laser positioning failure in a high dynamic environment, the use of two-dimensional codes to assist in robot positioning is a common method, but the existing method has the defects of high deployment requirement, low robustness and the like. The common mode is that two-dimensional code combines wheeled odometer, and this kind of mode can lead to the precision to roll down when the wheel appears skidding, appears can't scan next two-dimensional code along the route even, leads to the lost problem of location. Some methods also combine laser navigation, but the existing method for fusing laser navigation is more used for secondary positioning, uses laser navigation to reach the vicinity of a site, and starts local positioning after scanning a two-dimensional code, so that high-precision site pickup is realized, and the laser navigation is easy to fail under the condition of environmental change, so that the laser navigation cannot reach the corresponding site. The method only uses a small amount of information of the two-dimensional code positioning, so that the positioning precision is not very high, and the possibility of positioning loss still exists when the map change is large and the laser navigation fails.
Disclosure of Invention
The application aims to solve the technical problem of providing a fusion positioning method and medium based on two-dimensional codes and lasers, and the method and medium have the characteristic of higher positioning precision.
In a first aspect, an embodiment provides a two-dimensional code and laser-based fusion positioning method, where the positioning method is implemented based on two-dimensional codes arranged on paths corresponding to a global environment map and is applied to a robot trolley positioning device; the positioning method comprises the following steps:
Scanning any two-dimensional code on a travel route of the robot, and obtaining the positioning coordinate of the robot at any moment as a first positioning coordinate based on the calibration result of the any two-dimensional code; the method for obtaining the calibration result comprises the following steps: scanning each two-dimensional code, and acquiring a two-dimensional code data frame for any scanned two-dimensional code, and simultaneously acquiring a laser data frame as a laser key frame; for any two-dimensional code data frame, acquiring the coordinates of the two-dimensional code data frame relative to the robot as a first coordinate, and for any laser key frame, acquiring the coordinates of the robot as a second coordinate, and performing nonlinear optimization on the coordinates of any two-dimensional code based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes to acquire the coordinates of all the two-dimensional codes;
When any two-dimensional code is scanned on a travel route, taking all obtained laser frames as laser key frames and marking the laser key frames as two-dimensional code laser key frames, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frames for any two-dimensional code laser key frames, and carrying out nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of a plurality of first positioning coordinates and the optimized speed of the robot;
For any two-dimensional code laser key frame, calculating coordinates as coordinates of the any two-dimensional code laser key frame by adopting time speed and time difference based on the optimized first positioning coordinates of the first moment corresponding to the two-dimensional code data frame of the first moment and the speed of the robot;
and obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of any two-dimensional code.
In an embodiment, the nonlinear optimization is performed on the coordinates of any two-dimensional code based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes to obtain the coordinates of all the two-dimensional codes, including:
，
Wherein, Representing a first coordinate at the moment i corresponding to the two-dimensional code j,/>Representing a second coordinate at the moment i corresponding to the two-dimensional code j,/>The Euclidean distance calculation is represented by I, the index of the moment is 1-1, the I is 1-1, the total number of the moment is represented by I, the index of the two-dimensional code is represented by J, the J is 1-1, the J is 1-J, and the total number of the two-dimensional code is represented by J; /(I)The coordinates representing the two-dimensional code j change with optimization; /(I)And representing the coordinate set of all the two-dimensional codes to be optimized.
In an embodiment, the scanning any two-dimensional code on the travel route of the robot, and obtaining the positioning coordinate of the robot at any time as the first positioning coordinate based on the calibration result of the any two-dimensional code, includes:
，
Wherein, Representing the first positioning coordinate at time i,/>The method comprises the steps of representing a first coordinate at a moment I, corresponding to a two-dimensional code J, wherein I represents a moment index, I is more than or equal to 1 and less than or equal to I, I represents a total number of moments, J represents a two-dimensional code index, J is more than or equal to 1 and less than or equal to J, and J represents a total number of the two-dimensional codes; /(I)The coordinates representing the two-dimensional code j change with optimization.
In an embodiment, for any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame, performing nonlinear optimization on a plurality of first positioning coordinates and speeds of the robot corresponding to the two-dimensional code data frames one to one, and obtaining an optimized set of the plurality of first positioning coordinates and the optimized speeds of the robot, where the method includes:
，
Wherein, Representing the speed of the robot corresponding to the plurality of two-dimensional code data frames,/>，/>Representing the linear velocity,/>Represents angular velocity; /(I)Representing a set of the plurality of first positioning coordinates, J representing indexes of the two-dimensional codes, J being larger than or equal to 1 and smaller than or equal to J, and J representing the total number of the two-dimensional codes; f is a cost function,/>，Indicating Euclidean distance calculation, I indicates index of time, I is not less than 1 and not more than I, I indicates total number of time, and I is not less than/>Representing the inverse of the covariance of the robot observations,/>The inverse number of covariance of the robot prediction is represented, and the accuracy of the actual robot model and the two-dimensional code sensor is determined; /(I)Indicating the displacement of the robot from instant i-1 to instant i,，/>Representing the time increment from instant i-1 to instant i,/>Representing the yaw angle of the robot at time i-1,/>Representing displacement in x-direction,/>Representing displacement in y-direction,/>Representing the increment of the yaw angle of the robot.
In an embodiment, for any one of the two-dimensional code laser key frames, based on the optimized first positioning coordinate of the first moment corresponding to the two-dimensional code data frame of the first moment corresponding to the two-dimensional code laser key frame and the speed of the robot, calculating the coordinate as the coordinate of the any one of the laser key frames by using the time speed and the time difference, and the method includes:
，
Wherein, Representing the coordinates of any two-dimensional code laser key frame,/>Representing the optimized first positioning coordinate of the first moment corresponding to the two-dimensional code data frame of the first moment corresponding to the arbitrary two-dimensional code laser key frame,/>Representing the yaw angle of the robot corresponding to the two-dimensional code data frame at the first moment corresponding to any two-dimensional code laser key frame,/>And the time difference from the moment of scanning the two-dimensional code to the moment of scanning the two-dimensional code laser key frame is represented.
In one embodiment, for any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame includes: and regarding the two-dimensional code moment, taking the two-dimensional code data frame at the moment before any one two-dimensional code laser key frame and the two-dimensional code data frame at the moment after any two-dimensional code laser key frame as the two-dimensional code data frame corresponding to any one laser key frame.
In an embodiment, the obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of the arbitrary two-dimensional code includes:
and taking the coordinates of any two-dimensional code laser key frame as the coordinates of the robot.
In an embodiment, the obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of the arbitrary two-dimensional code includes:
And taking the coordinates of any two-dimensional code laser key frame as residual errors, adding the residual errors into a nonlinear optimization function for optimization, and taking the coordinates of any two-dimensional code laser key frame after optimization as the coordinates of the robot.
In one embodiment, the optimizing the coordinates of the laser key frame of any two-dimensional code as a residual error by adding a nonlinear optimization function includes:
，
Wherein, Representing the coordinates of the optimized arbitrary two-dimensional code laser key frame,/>Representing the cost function of any two-dimensional code laser key frame,/>，Representing the laser point cloud scanned by the laser key frame of any two-dimensional code, wherein K represents the index of the points of the laser point cloud, K represents the total number of the points of the laser point cloud, and/(>Weight representing two-dimensional code observation,/>Representing a raster map bicubic interpolation function.
In a second aspect, an embodiment provides a computer readable storage medium, where a program is stored, where the stored program includes a two-dimensional code and laser-based fusion positioning method that can be loaded and processed by a processor in any of the above embodiments.
The beneficial effects of the invention are as follows:
In the positioning method, coordinates of all the two-dimensional codes are obtained through coordinate estimation of laser, and therefore calibration of all the two-dimensional codes is achieved. And positioning the robot at any moment based on the calibration result of each two-dimensional code. When any two-dimensional code is scanned on a travel route, all obtained laser frames are used as laser key frames and marked as two-dimensional code laser key frames, a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frames are selected for any two-dimensional code laser key frames, a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot are subjected to nonlinear optimization, so that more accurate coordinates are calculated through multi-frame fusion, the relative coordinates between the robot and the two-dimensional code are converted into the coordinates of the robot under a global coordinate system, and the positioning accuracy of the robot is improved.
Drawings
FIG. 1 is a flow diagram of a two-dimensional code and laser based fusion positioning method according to an embodiment of the application;
FIG. 2 is a schematic diagram of a key laser frame and a two-dimensional code data frame according to an embodiment of the present application;
FIG. 3 is a schematic diagram of positioning prior to nonlinear optimization of first positioning coordinates in accordance with one embodiment of the present application;
FIG. 4 is a schematic diagram of the non-linear optimization of the first positioning coordinates of the embodiment of FIG. 3 in accordance with the present application.
Detailed Description
The application will be described in further detail below with reference to the drawings by means of specific embodiments. Wherein like elements in different embodiments are numbered alike in association. In the following embodiments, numerous specific details are set forth in order to provide a better understanding of the present application. However, one skilled in the art will readily recognize that some of the features may be omitted, or replaced by other elements, materials, or methods in different situations. In some instances, related operations of the present application have not been shown or described in the specification in order to avoid obscuring the core portions of the present application, and may be unnecessary to persons skilled in the art from a detailed description of the related operations, which may be presented in the description and general knowledge of one skilled in the art.
Furthermore, the described features, operations, or characteristics of the description may be combined in any suitable manner in various embodiments. Also, various steps or acts in the method descriptions may be interchanged or modified in a manner apparent to those of ordinary skill in the art. Thus, the various orders in the description and drawings are for clarity of description of only certain embodiments, and are not meant to be required orders unless otherwise indicated.
The numbering of the components itself, e.g. "first", "second", etc., is used herein merely to distinguish between the described objects and does not have any sequential or technical meaning. The term "coupled" as used herein includes both direct and indirect coupling (coupling), unless otherwise indicated.
In order to facilitate the explanation of the inventive concept, a brief explanation of the robot positioning technique is provided below.
In the laser positioning of the robot, the calculation requirement of back-end loop detection is reduced by selecting the laser key frames, and under normal conditions, unnecessary laser frames are filtered through the angle threshold, the distance threshold and the time threshold, and the rest laser frames are used as the laser key frames, so that the positioning of the robot is realized based on the laser key frames, and the global pose of laser positioning is obtained.
In the current fusion positioning of the laser and the two-dimensional code, in one scheme, the global pose obtained through the two-dimensional code and the global pose obtained through laser positioning are respectively acquired, and then the global pose of the robot is corrected through the two global poses obtained through alignment to realize the positioning of the robot.
However, in the research of the applicant, the applicant finds that, in the above positioning method, to obtain a higher-precision positioning, it is necessary to ensure a high-precision calibration of the relative pose of the two-dimensional code sensor and the laser radar, and also to precisely synchronize the time stamps of the two-dimensional code sensor and the laser radar, so as to obtain the positioning of the robot in two coordinate systems at the same moment. However, in the actual scene, particularly when various sensors and user equipment need to be adapted, time synchronization with higher precision and calibration of relative pose are difficult to ensure, so that the precision of fusion of laser and two-dimensional code positioning is difficult to ensure, and the positioning accuracy is further difficult to improve.
In view of the above problems, an implementation of the present application provides a two-dimensional code and laser-based fusion positioning method, which is implemented based on two-dimensional codes arranged on paths corresponding to a global environment map, and can be applied to a robot trolley positioning device. In the positioning method, the coordinates of all the two-dimensional codes are obtained through the coordinate estimation of the laser, so that the calibration of all the two-dimensional codes is realized. And positioning the robot at any moment based on the calibration result of each two-dimensional code. When any two-dimensional code is scanned on a travel route, all obtained laser frames are used as laser key frames and marked as two-dimensional code laser key frames, a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frames are selected for any two-dimensional code laser key frames, a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot are subjected to nonlinear optimization, so that more accurate coordinates are calculated through multi-frame fusion, the relative coordinates between the robot and the two-dimensional code are converted into the coordinates of the robot under a global coordinate system, and the positioning accuracy of the robot is improved.
In order to clearly illustrate the positioning method, a specific environment to which the positioning method is applied is described first, in one embodiment, a global environment map can be constructed through SLAM, after a complete local environment map is constructed, two-dimensional codes are arranged on a path according to requirements, and the arrangement interval depends on the environment and the laser precision. In one embodiment, in order to ensure the running stability after the environment changes, a two-dimensional code is arranged every 5m-10 m. After the two-dimension codes are arranged, in a specific embodiment, the AGV trolley can be controlled to scan each arranged two-dimension code to achieve calibration of the two-dimension codes.
Based on the above application environment, please refer to fig. 1, a positioning method based on two-dimensional code and laser provided in an embodiment of the present application includes:
Step S10, any two-dimensional code is scanned on the travelling route of the robot, and the positioning coordinate of the robot at any moment is obtained as a first positioning coordinate based on the calibration result of the any two-dimensional code.
In one embodiment, the method for obtaining the calibration result includes: scanning each two-dimensional code, and acquiring a two-dimensional code data frame for any scanned two-dimensional code, and simultaneously acquiring a laser data frame as a laser key frame. And for any two-dimensional code data frame, acquiring the coordinate of the two-dimensional code data frame relative to the robot as a first coordinate. And for any two-dimensional code laser key frame, acquiring the coordinates of the robot as second coordinates. And carrying out nonlinear optimization on the coordinates of any two-dimensional code based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes to obtain the coordinates of all the two-dimensional codes.
In one embodiment, based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes, nonlinear optimization is performed on the coordinates of any two-dimensional code to obtain the coordinates of all the two-dimensional codes, including:
，
Wherein, Representing a first coordinate at the moment i corresponding to the two-dimensional code j,/>Representing a second coordinate at the moment i corresponding to the two-dimensional code j,/>The Euclidean distance calculation is represented by I, the index of the moment is 1-1, the I is 1-1, the total number of the moment is represented by I, the index of the two-dimensional code is represented by J, the J is 1-1, the J is 1-J, and the total number of the two-dimensional code is represented by J; /(I)The coordinates representing the two-dimensional code j change with optimization; /(I)And representing the coordinate set of all the two-dimensional codes to be optimized.
After the two-dimensional code is scanned, the coordinates of the two-dimensional code in the global coordinate system can be jointly optimized according to SLAM positioning and the scanned coordinates of the two-dimensional code, and the coordinates of the two-dimensional code can be calibrated well due to the fact that no environment changes at the moment due to the fact that the two-dimensional code is built in the step. From this, the arrangement of the environment is completed.
In one embodiment, in order to ensure calibration accuracy, the two-dimensional code can be traversed multiple times and multiple angles, so as to optimize more accurate coordinates.
In one embodiment, the robot may be an AGV cart. When the AGV runs and positions, the laser navigation program is formed by front-end odometer and rear-end loop detection. The front-end odometer obtains real-time positioning of the trolley under an odometer coordinate system, and meanwhile, laser is filtered by a key frame selector to obtain a laser key frame for loop detection. When the loop is detected by the trolley, the conversion relation from the odometer coordinate system to the global coordinate system can be optimized to eliminate the accumulated error of the odometer. When the two-dimensional code reader scans the two-dimensional code, the coordinates of the two-dimensional code relative to the trolley can be obtained, at the moment, the key frame selector can mark all laser frames with the labels of the two-dimensional code laser key frames, and the labels are transmitted to the rear end, so that the two-dimensional code laser key frames are not filtered, and loop detection is not performed.
In one embodiment, any two-dimensional code is scanned on a travel route of the robot, and based on a calibration result of the any two-dimensional code, a positioning coordinate of the robot at any moment is obtained as a first positioning coordinate, including:
，
Wherein, The first positioning coordinate at time i is indicated.
Therefore, the relative coordinates between the AGV trolley and the two-dimensional code can be converted into the coordinates of the trolley under the global coordinate system.
Step S20, for any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame, and performing nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of the plurality of first positioning coordinates and the optimized speed of the robot. When any two-dimensional code is scanned on a travel route, all obtained laser frames are used as laser key frames and marked as two-dimensional code laser key frames.
When the two-dimensional code is scanned, all laser frames are used as laser key frames and marked as the two-dimensional code laser key frames, so that settlement of coordinates of the two-dimensional code laser key frames is carried out. Referring to fig. 2, in a normal case, the laser frame rate is 20Hz, and the two-dimensional code camera frame rate is 100Hz, so that a plurality of two-dimensional code data frames are corresponding to one two-dimensional code laser key frame time.
In order to calculate a more accurate pose by multi-frame fusion, according to the mobility of an AGV trolley, assuming uniform motion in the time of any two-dimensional code laser key frames, the following steps are provided: in an embodiment, for any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame, performing nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of the plurality of first positioning coordinates and the optimized speed of the robot, including:
，
Wherein, Representing the speed of the robot corresponding to the plurality of two-dimensional code data frames,/>，/>Representing the linear velocity,/>Represents angular velocity; /(I)Representing a set of the plurality of first positioning coordinates; f is the cost function of the device,，/>Representing European distance calculation,/>Representing the inverse of the covariance of the robot observations,/>Reciprocal representing covariance of robot prediction, and determination of/(on the basis of accuracy of actual robot model and two-dimensional code sensor)Indicating the displacement of the robot from instant i-1 to instant i,，/>Representing the time increment from instant i-1 to instant i,/>The yaw angle of the robot at time i-1 is also a variable/>Angle part of/>Representing displacement in x-direction,/>Representing displacement in y-direction,/>Representing the increment of the yaw angle of the robot.
In one embodiment, for any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame includes: and regarding the two-dimensional code moment, taking the two-dimensional code data frame at the moment before any one two-dimensional code laser key frame and the two-dimensional code data frame at the moment after any two-dimensional code laser key frame as the two-dimensional code data frame corresponding to any one laser key frame. For example, referring to fig. 2, for the first two-dimensional code laser key frame, a second two-dimensional code data frame (two-dimensional code time, two-dimensional code data frame one time before the two-dimensional code laser key frame) and third and fourth two-dimensional code data frames (two-dimensional code time, two-dimensional code data frames two times after the two-dimensional code laser key frame) are taken as two-dimensional code data frames corresponding to the first two-dimensional code laser key frame. And taking a sixth two-dimensional code data frame (two-dimensional code time, one two-dimensional code data frame before the two-dimensional code laser key frame) and seventh and eighth two-dimensional code data frames (two-dimensional code time, two-dimensional code data frames after the two-dimensional code laser key frame) as two-dimensional code data frames corresponding to the second two-dimensional code laser key frame for the second two-dimensional code laser key frame.
Please refer to fig. 3, which illustrates the positioning before optimization. Please refer to fig. 4, which is an optimized positioning. By the above-described optimization method, the error of recognition can be reduced.
When the two-dimensional code laser key frame reaches the rear end, the key frame coordinate settlement is carried out, and the process carries out nonlinear optimization twice to obtain the global coordinate of the two-dimensional code laser key frame, so that even if the time stamps of the code reader and the laser are not particularly accurate and are not aligned, the AGV coordinate estimation with better precision can be ensured, the coordinate conversion requirements on the laser radar and the camera are reduced, the time synchronization requirement is reduced, and the positioning precision is improved.
Step S30, for any two-dimensional code laser key frame, calculating coordinates by using time speed and time difference based on the optimized first positioning coordinates of the first moment corresponding to the two-dimensional code data frame of the first moment and the speed of the robot, and taking the coordinates as the coordinates of the any two-dimensional code laser key frame.
The coordinates of the two-dimensional code laser key frame can be calculated by using the speed time difference by using the coordinates of the first moment corresponding to the two-dimensional code laser key frame as initial coordinates, and in one embodiment, the method comprises the following steps:
，
Wherein, Representing the coordinates of any two-dimensional code laser key frame,/>Representing the optimized first positioning coordinate of the first moment corresponding to the two-dimensional code data frame of the first moment corresponding to the arbitrary two-dimensional code laser key frame,/>Representing the yaw angle of the robot corresponding to the two-dimensional code data frame at the first moment corresponding to any two-dimensional code laser key frame,/>And the time difference from the moment of scanning the two-dimensional code to the moment of scanning the two-dimensional code laser key frame is represented.
And step S40, obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of any two-dimensional code.
In one embodiment, the coordinates of the laser key frame of any two-dimensional code are used as the coordinates of the robot.
However, the applicant finds that if the time stamps are accurate, the coordinates of the obtained two-dimensional code laser key frames are accurate, but the time stamps are difficult to adjust in the world physical environment, especially when the two-dimensional code laser key frames are suitable for different sensors and different vehicle types.
In view of this, in order to obtain a more accurate two-dimensional code laser key frame, a secondary nonlinear optimization is performed based on matching of a map and laser, including: and taking the coordinates of any two-dimensional code laser key frame as residual errors, adding a nonlinear optimization function to optimize, and taking the coordinates of any two-dimensional code laser key frame after optimization as the coordinates of the robot.
Performing bicubic interpolation on the grid map, optimizing the two-dimensional code laser key frames, and in order to better realize that the optimized result is not worse than before optimization even under the environment change, in one embodiment, adding the coordinates of any two-dimensional code laser key frame as residual errors into a nonlinear optimization function for optimization, wherein the method comprises the following steps:
，
Wherein, Representing the coordinates of the optimized arbitrary two-dimensional code laser key frame,/>Representing the cost function of any two-dimensional code laser key frame,/>，Representing the laser point cloud scanned by the laser key frame of any two-dimensional code, wherein K represents the index of the points of the laser point cloud, K represents the total number of the points of the laser point cloud, and/(>Weight representing two-dimensional code observation,/>And representing a bicubic interpolation function of the grid map, and obtaining a result of interpolation of the grid map at the position according to the input point cloud coordinates.
Based on the above embodiment, the initial pose calculated by the two-dimensional code is used as a residual error term to be added into a nonlinear optimization function for optimization, so that more coordinates can be adjusted only when the grid map and the two-dimensional code laser key frame are good (the characteristic value of the H matrix is large and the convex function property is met by comparison), and more accurate coordinates of the two-dimensional code laser key frame can be obtained. Due to the optimization of the laser, even if the coordinates calculated by the two-dimensional code are poor, higher precision can be ensured. Meanwhile, the cost function also ensures that even if the point cloud cannot be matched with the map, the two-dimensional code coordinates are guaranteed, and the robustness of the program is improved.
Based on the fusion positioning result of the two-dimensional code and the laser obtained by any one of the embodiments, the positioning result and other loop detection results can be subjected to nonlinear optimization to obtain the coordinates of the AGV mileage coordinate system relative to the global coordinate system, so that even if the environment changes greatly, no effective loop constraint exists, the pose estimation can be obtained accurately.
In one embodiment, through loop detection and coordinate settlement of the two-dimensional code laser key frames, some constraints can be obtained, and through the constraints, the transformation relation from the odometer coordinate system to the global coordinate system can be optimized, which is to optimize the coordinates of each laser key frame in the global coordinate system, and the method comprises the following steps:
，
Wherein, And representing the coordinates of all the laser key frames, including the two-dimensional code laser key frames. /(I)Representing a cost function,/>. The first residual block is the odometer constraint,/>Representing European transformation between two adjacent laser key frames, p and q each represent an index of a laser key frame, and x p and x q represent two adjacent laser key frames,/>Representing odometer constraints. The second residual block is an observation constraint comprising a constraint of loop detection and a constraint of a two-dimensional code laser key frame, and is a constraint of a/>European transformation of laser key frame to global coordinate system, m represents origin of global coordinate system,/>, andAnd representing the constraint of observation, namely a loop detection result or a two-dimensional code laser key frame calculation result. Omega pq represents covariance of the odometer, omega p represents covariance of loop detection, depending on laser and two-dimensional code sensor accuracy. Through optimization, the coordinate estimation of each laser key frame in the global map can be obtained. And taking the last frame of laser key frame as the coordinate transformation from the global map to the local map.
An embodiment of the present application provides a computer readable storage medium, where a program is stored, where the stored program includes a two-dimensional code and laser-based fusion positioning method that can be loaded and processed by a processor in any of the above embodiments.
Those skilled in the art will appreciate that all or part of the functions of the various methods in the above embodiments may be implemented by hardware, or may be implemented by a computer program. When all or part of the functions in the above embodiments are implemented by means of a computer program, the program may be stored in a computer readable storage medium, and the storage medium may include: read-only memory, random access memory, magnetic disk, optical disk, hard disk, etc., and the program is executed by a computer to realize the above-mentioned functions. For example, the program is stored in the memory of the device, and when the program in the memory is executed by the processor, all or part of the functions described above can be realized. In addition, when all or part of the functions in the above embodiments are implemented by means of a computer program, the program may be stored in a storage medium such as a server, another computer, a magnetic disk, an optical disk, a flash disk, or a removable hard disk, and the program in the above embodiments may be implemented by downloading or copying the program into a memory of a local device or updating a version of a system of the local device, and when the program in the memory is executed by a processor.
The foregoing description of the invention has been presented for purposes of illustration and description, and is not intended to be limiting. Several simple deductions, modifications or substitutions may also be made by a person skilled in the art to which the invention pertains, based on the idea of the invention.
Claims (8)
1. The fusion positioning method based on the two-dimensional code and the laser is characterized in that the positioning method is realized based on the two-dimensional code which is arranged on a path corresponding to a global environment map and is applied to a positioning device of a robot trolley; the positioning method comprises the following steps:
Scanning any two-dimensional code on a travel route of the robot, and obtaining the positioning coordinate of the robot at any moment as a first positioning coordinate based on the calibration result of the any two-dimensional code; the method for obtaining the calibration result comprises the following steps: scanning each two-dimensional code, and acquiring a two-dimensional code data frame for any scanned two-dimensional code, and simultaneously acquiring a laser data frame as a laser key frame; for any two-dimensional code data frame, acquiring the coordinates of the two-dimensional code data frame relative to the robot as a first coordinate, and for any laser key frame, acquiring the coordinates of the robot as a second coordinate, and performing nonlinear optimization on the coordinates of any two-dimensional code based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes to acquire the coordinates of all the two-dimensional codes;
When any two-dimensional code is scanned on a travel route, taking all obtained laser frames as laser key frames and marking the laser key frames as two-dimensional code laser key frames, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frames for any two-dimensional code laser key frames, and carrying out nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of a plurality of first positioning coordinates and the optimized speed of the robot;
For any two-dimensional code laser key frame, calculating coordinates as coordinates of the any two-dimensional code laser key frame by adopting time speed and time difference based on the optimized first positioning coordinates of the first moment corresponding to the two-dimensional code data frame of the first moment and the speed of the robot;
obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of any two-dimensional code;
For any two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame, performing nonlinear optimization on a plurality of first positioning coordinates corresponding to the two-dimensional code data frames one by one and the speed of the robot to obtain an optimized set of the plurality of first positioning coordinates and the optimized speed of the robot, wherein the method comprises the following steps:
，
Wherein, Representing the speed of the robot corresponding to the plurality of two-dimensional code data frames,/>，/>Representing the linear velocity,/>Represents angular velocity; /(I)Representing a set of the plurality of first positioning coordinates, J representing indexes of the two-dimensional codes, J being larger than or equal to 1 and smaller than or equal to J, and J representing the total number of the two-dimensional codes; f is a cost function,/>，/>Representing a second coordinate at the moment i corresponding to the two-dimensional code j,/>Indicating Euclidean distance calculation, I indicates index of time, I is not less than 1 and not more than I, I indicates total number of time, and I is not less than/>Representing the inverse of the covariance of the robot observations,/>The inverse number of covariance of the robot prediction is represented, and the accuracy of the actual robot model and the two-dimensional code sensor is determined; /(I)Representing the displacement of the robot from instant i-1 to instant i,/>，/>Representing the time increment from instant i-1 to instant i,/>Representing the yaw angle of the robot at time i-1,/>Representing displacement in x-direction,/>Representing displacement in y-direction,/>Representing an increment of a yaw angle of the robot;
the method for obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of any two-dimensional code comprises the following steps:
and taking the coordinates of any two-dimensional code laser key frame as the coordinates of the robot.
2. The method for positioning based on the fusion of two-dimensional codes and laser according to claim 1, wherein the nonlinear optimization is performed on the coordinates of any two-dimensional code based on the first coordinates and the corresponding second coordinates of all the two-dimensional codes to obtain the coordinates of all the two-dimensional codes, comprising:
，
Wherein, Representing a first coordinate at the moment i corresponding to the two-dimensional code j,/>Representing a second coordinate at the moment i corresponding to the two-dimensional code j,/>The Euclidean distance calculation is represented by I, the index of the moment is 1-1, the I is 1-1, the total number of the moment is represented by I, the index of the two-dimensional code is represented by J, the J is 1-1, the J is 1-J, and the total number of the two-dimensional code is represented by J; /(I)The coordinates representing the two-dimensional code j change with optimization; /(I)And representing the coordinate set of all the two-dimensional codes to be optimized.
3. The method for positioning a robot based on two-dimensional code and laser fusion according to claim 1, wherein the step of scanning any two-dimensional code on the travel path of the robot, and obtaining the positioning coordinates of the robot at any moment as the first positioning coordinates based on the calibration result of any two-dimensional code, comprises:
，
Wherein, Representing the first positioning coordinate at time i,/>The method comprises the steps of representing a first coordinate at a moment I, corresponding to a two-dimensional code J, wherein I represents a moment index, I is more than or equal to 1 and less than or equal to I, I represents a total number of moments, J represents a two-dimensional code index, J is more than or equal to 1 and less than or equal to J, and J represents a total number of the two-dimensional codes; /(I)The coordinates representing the two-dimensional code j change with optimization.
4. The two-dimensional code and laser-based fusion positioning method according to claim 1, wherein the calculating the coordinates of any one of the two-dimensional code laser key frames based on the optimized first positioning coordinates of the first moment corresponding to the two-dimensional code data frame of the first moment and the speed of the robot by using the time speed and the time difference comprises:
，
Wherein, Representing the coordinates of any two-dimensional code laser key frame,/>Representing the optimized first positioning coordinate of the first moment corresponding to the two-dimension code data frame of the first moment corresponding to the arbitrary two-dimension code laser key frame,Representing the yaw angle,/>, of the robot corresponding to the two-dimensional code data frame at the first moment corresponding to the laser key frame of any two-dimensional codeAnd the time difference from the moment of scanning the two-dimensional code to the moment of scanning the two-dimensional code laser key frame is represented.
5. The two-dimensional code and laser based fusion positioning method according to any one of claims 1 to 4, wherein for any one two-dimensional code laser key frame, selecting a plurality of two-dimensional code data frames corresponding to the two-dimensional code laser key frame comprises: and regarding the two-dimensional code moment, taking the two-dimensional code data frame at the moment before any one two-dimensional code laser key frame and the two-dimensional code data frame at the moment after any two-dimensional code laser key frame as the two-dimensional code data frame corresponding to any one laser key frame.
6. The two-dimensional code and laser-based fusion positioning method according to any one of claims 1 to 4, wherein the obtaining the coordinates of the corresponding robot based on the coordinates of the laser key frame of the any one of the two-dimensional codes comprises:
And taking the coordinates of any two-dimensional code laser key frame as residual errors, adding the residual errors into a nonlinear optimization function for optimization, and taking the coordinates of any two-dimensional code laser key frame after optimization as the coordinates of the robot.
7. The two-dimensional code and laser-based fusion positioning method according to claim 6, wherein the optimizing the coordinates of the laser key frame of any two-dimensional code as a residual error by adding a nonlinear optimization function comprises:
，
Wherein, Representing the coordinates of the optimized arbitrary two-dimensional code laser key frame,/>Representing the cost function of any two-dimensional code laser key frame,/>，Representing the laser point cloud scanned by the laser key frame of any two-dimensional code, wherein K represents the index of the points of the laser point cloud, K represents the total number of the points of the laser point cloud, and/(>Weight representing two-dimensional code observation,/>Representing a raster map bicubic interpolation function.
8. A computer-readable storage medium, wherein a program is stored on the storage medium, the stored program including the two-dimensional code and laser-based fusion positioning method according to any one of claims 1 to 7 that can be loaded and processed by a processor.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410251452.0A CN117824667B (en) | 2024-03-06 | 2024-03-06 | Fusion positioning method and medium based on two-dimensional code and laser |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410251452.0A CN117824667B (en) | 2024-03-06 | 2024-03-06 | Fusion positioning method and medium based on two-dimensional code and laser |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117824667A CN117824667A (en) | 2024-04-05 |
| CN117824667B true CN117824667B (en) | 2024-05-10 |
Family
ID=90524508
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410251452.0A Active CN117824667B (en) | 2024-03-06 | 2024-03-06 | Fusion positioning method and medium based on two-dimensional code and laser |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117824667B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117824666B (en) * | 2024-03-06 | 2024-05-10 | 成都睿芯行科技有限公司 | Two-dimensional code pair for fusion positioning, two-dimensional code calibration method and fusion positioning method |
Citations (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000057251A (en) * | 1998-08-17 | 2000-02-25 | Asahi Techno Glass Corp | Two-dimensional code and its formation, and device and method for reading two-dimensional code |
| CN108550318A (en) * | 2018-03-12 | 2018-09-18 | 浙江大华技术股份有限公司 | A kind of method and device of structure map |
| CN111337011A (en) * | 2019-12-10 | 2020-06-26 | 亿嘉和科技股份有限公司 | Indoor positioning method based on laser and two-dimensional code fusion |
| CN111982099A (en) * | 2019-05-21 | 2020-11-24 | 顺丰科技有限公司 | Robot hybrid positioning method, device, equipment and computer readable medium |
| CN112066982A (en) * | 2020-09-07 | 2020-12-11 | 成都睿芯行科技有限公司 | Industrial mobile robot positioning method in high dynamic environment |
| CN112197770A (en) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | Robot positioning method and positioning device thereof |
| CN112734852A (en) * | 2021-03-31 | 2021-04-30 | 浙江欣奕华智能科技有限公司 | Robot mapping method and device and computing equipment |
| CN112785702A (en) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | SLAM method based on tight coupling of 2D laser radar and binocular camera |
| CN113405544A (en) * | 2021-05-08 | 2021-09-17 | 中电海康集团有限公司 | Mapping and positioning method and system for mobile robot |
| CN114526739A (en) * | 2022-01-25 | 2022-05-24 | 中南大学 | Mobile robot indoor repositioning method, computer device and product |
| WO2022183785A1 (en) * | 2021-03-05 | 2022-09-09 | 深圳市优必选科技股份有限公司 | Robot positioning method and apparatus, robot, and readable storage medium |
| CN115061142A (en) * | 2022-06-15 | 2022-09-16 | 山东大学 | AGV positioning method and system based on laser radar |
| CN115774265A (en) * | 2023-02-15 | 2023-03-10 | 江苏集萃清联智控科技有限公司 | Two-dimensional code and laser radar fusion positioning method and device for industrial robot |
| CN116009525A (en) * | 2022-07-28 | 2023-04-25 | 杭州蓝芯科技有限公司 | Optimization method of two-dimensional code map and positioning method of mobile robot |
| CN116228870A (en) * | 2023-05-05 | 2023-06-06 | 山东省国土测绘院 | Mapping method and system based on two-dimensional code SLAM precision control |
| WO2024001649A1 (en) * | 2022-06-29 | 2024-01-04 | 深圳市海柔创新科技有限公司 | Robot positioning method, apparatus and computing readable storage medium |
| CN117830604A (en) * | 2024-03-06 | 2024-04-05 | 成都睿芯行科技有限公司 | Two-dimensional code anomaly detection method and medium for positioning |
| CN117824666A (en) * | 2024-03-06 | 2024-04-05 | 成都睿芯行科技有限公司 | Two-dimensional code pair for fusion positioning, two-dimensional code calibration method and fusion positioning method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114413909A (en) * | 2022-01-25 | 2022-04-29 | 台州学院 | Indoor mobile robot positioning method and system |
-
2024
- 2024-03-06 CN CN202410251452.0A patent/CN117824667B/en active Active
Patent Citations (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2000057251A (en) * | 1998-08-17 | 2000-02-25 | Asahi Techno Glass Corp | Two-dimensional code and its formation, and device and method for reading two-dimensional code |
| CN108550318A (en) * | 2018-03-12 | 2018-09-18 | 浙江大华技术股份有限公司 | A kind of method and device of structure map |
| CN111982099A (en) * | 2019-05-21 | 2020-11-24 | 顺丰科技有限公司 | Robot hybrid positioning method, device, equipment and computer readable medium |
| CN111337011A (en) * | 2019-12-10 | 2020-06-26 | 亿嘉和科技股份有限公司 | Indoor positioning method based on laser and two-dimensional code fusion |
| CN112066982A (en) * | 2020-09-07 | 2020-12-11 | 成都睿芯行科技有限公司 | Industrial mobile robot positioning method in high dynamic environment |
| CN112197770A (en) * | 2020-12-02 | 2021-01-08 | 北京欣奕华数字科技有限公司 | Robot positioning method and positioning device thereof |
| CN112785702A (en) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | SLAM method based on tight coupling of 2D laser radar and binocular camera |
| WO2022183785A1 (en) * | 2021-03-05 | 2022-09-09 | 深圳市优必选科技股份有限公司 | Robot positioning method and apparatus, robot, and readable storage medium |
| CN112734852A (en) * | 2021-03-31 | 2021-04-30 | 浙江欣奕华智能科技有限公司 | Robot mapping method and device and computing equipment |
| CN113405544A (en) * | 2021-05-08 | 2021-09-17 | 中电海康集团有限公司 | Mapping and positioning method and system for mobile robot |
| CN114526739A (en) * | 2022-01-25 | 2022-05-24 | 中南大学 | Mobile robot indoor repositioning method, computer device and product |
| CN115061142A (en) * | 2022-06-15 | 2022-09-16 | 山东大学 | AGV positioning method and system based on laser radar |
| WO2024001649A1 (en) * | 2022-06-29 | 2024-01-04 | 深圳市海柔创新科技有限公司 | Robot positioning method, apparatus and computing readable storage medium |
| CN117367419A (en) * | 2022-06-29 | 2024-01-09 | 深圳市海柔创新科技有限公司 | Robot positioning method, apparatus and computer readable storage medium |
| CN116009525A (en) * | 2022-07-28 | 2023-04-25 | 杭州蓝芯科技有限公司 | Optimization method of two-dimensional code map and positioning method of mobile robot |
| CN115774265A (en) * | 2023-02-15 | 2023-03-10 | 江苏集萃清联智控科技有限公司 | Two-dimensional code and laser radar fusion positioning method and device for industrial robot |
| CN116228870A (en) * | 2023-05-05 | 2023-06-06 | 山东省国土测绘院 | Mapping method and system based on two-dimensional code SLAM precision control |
| CN117830604A (en) * | 2024-03-06 | 2024-04-05 | 成都睿芯行科技有限公司 | Two-dimensional code anomaly detection method and medium for positioning |
| CN117824666A (en) * | 2024-03-06 | 2024-04-05 | 成都睿芯行科技有限公司 | Two-dimensional code pair for fusion positioning, two-dimensional code calibration method and fusion positioning method |
Non-Patent Citations (6)
| Title |
|---|
| LiDAR地图匹配与二维码融合的AGV室内定位与导航;周宗锟等;《测绘通报》;20210125(第1期);9-12、52 * |
| Location Sensing using QR codes via 2D camera for Automated Guided Vehicles;Jacqueline Lee Fang Ang等;《IEEE》;20201102;1-6 * |
| 基于二维码视觉与激光雷达融合的高精度定位算法;栾佳宁等;《计算机应用》;20201211;第41卷(第5期);1484-1491 * |
| 基于图优化的同时定位与地图创建综述;梁明杰;闵华清;罗荣华;;机器人;20130715(04);全文 * |
| 液压并联机器人力/位混合控制策略研究;陶晗;曲智勇;丛大成;;农业机械学报;20180918(09);全文 * |
| 激光雷达和视觉技术的机器人移动位置跟踪系统;何小英等;《激光杂志》;20230125;第44卷(第1期);232-237 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117824667A (en) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6760114B2 (en) | Information processing equipment, data management equipment, data management systems, methods, and programs | |
| US11519729B2 (en) | Vision-aided inertial navigation | |
| CN112197770B (en) | Robot positioning method and positioning device thereof | |
| CN112268559B (en) | Mobile measurement method for fusing SLAM technology in complex environment | |
| CN112734852B (en) | Robot mapping method and device and computing equipment | |
| US8698875B2 (en) | Estimation of panoramic camera orientation relative to a vehicle coordinate frame | |
| CN117824667B (en) | Fusion positioning method and medium based on two-dimensional code and laser | |
| US8259994B1 (en) | Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases | |
| CN111121754A (en) | Mobile robot positioning navigation method and device, mobile robot and storage medium | |
| CN107167826B (en) | Vehicle longitudinal positioning system and method based on variable grid image feature detection in automatic driving | |
| CN110189366B (en) | Laser coarse registration method and device, mobile terminal and storage medium | |
| CN117824666B (en) | Two-dimensional code pair for fusion positioning, two-dimensional code calibration method and fusion positioning method | |
| US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
| CN114526745A (en) | Drawing establishing method and system for tightly-coupled laser radar and inertial odometer | |
| CN109059907A (en) | Track data processing method, device, computer equipment and storage medium | |
| CN113933818A (en) | Method, device, storage medium and program product for calibrating laser radar external parameter | |
| US20200250849A1 (en) | Three-dimensional position estimation device and program | |
| Dimitrievski et al. | Robust matching of occupancy maps for odometry in autonomous vehicles | |
| Han et al. | Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map | |
| CN114429432B (en) | Multi-source information layered fusion method and device and storage medium | |
| CN113566817B (en) | Vehicle positioning method and device | |
| CN115962773A (en) | Method, device and equipment for synchronous positioning and map construction of mobile robot | |
| CN113379915B (en) | Driving scene construction method based on point cloud fusion | |
| Hu et al. | Accurate fiducial mapping for pose estimation using manifold optimization | |
| CN116858276B (en) | Map-assisted multi-sensor fusion positioning method and computer readable 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 |