CN115727836B - Fusion positioning method and system based on laser reflector and odometer - Google Patents
Fusion positioning method and system based on laser reflector and odometer Download PDFInfo
- Publication number
- CN115727836B CN115727836B CN202211471520.1A CN202211471520A CN115727836B CN 115727836 B CN115727836 B CN 115727836B CN 202211471520 A CN202211471520 A CN 202211471520A CN 115727836 B CN115727836 B CN 115727836B
- Authority
- CN
- China
- Prior art keywords
- odometer
- positioning
- laser
- matching
- preset threshold
- 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 63
- 230000004927 fusion Effects 0.000 title claims abstract description 53
- 238000006073 displacement reaction Methods 0.000 claims abstract description 37
- 238000005259 measurement Methods 0.000 claims abstract description 29
- 238000012545 processing Methods 0.000 claims description 12
- 101150009249 MAP2 gene Proteins 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 4
- 101150020133 MAP6 gene Proteins 0.000 claims description 3
- 101100456045 Schizosaccharomyces pombe (strain 972 / ATCC 24843) map3 gene Proteins 0.000 claims description 2
- 238000012216 screening Methods 0.000 claims description 2
- 238000004590 computer program Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 101150014751 map4 gene Proteins 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000011430 maximum method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Image Processing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention discloses a fusion positioning method and a system based on a laser reflector and an odometer, wherein the method comprises the following steps: acquiring map data and laser point cloud data; matching the point cloud data with the map data to obtain a globally positioned position p0; the odometer starts to track the position from the globally positioned position p0, and accumulates tracking displacement to obtain the position p1 predicted by the positioning of the odometer; based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2; based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position. The invention can improve the positioning precision and stability.
Description
Technical Field
The invention relates to a fusion positioning method and system based on a laser reflector and an odometer, and belongs to the technical field of robots.
Background
The current relative positioning method of the robot comprises SLAM positioning, UWB positioning, two-dimensional code and visual positioning and the like, and the positioning has different characteristics.
The two-dimensional code and vision positioning method is a method which is applied more in the relative positioning field, has moderate precision and stability, has a plurality of excellent algorithms which are sufficiently sophisticated, can achieve high positioning precision, and has the defects that the method has large calculated amount and is difficult to realize positioning with higher frequency.
The laser reflector positioning is a scheme which is cheap, easy to realize, high in precision and strong in stability, most of the existing laser reflector positioning methods sample a V-shaped reflector for relative positioning, the method is used for purposely designing the shape into a V shape and positioning by utilizing a middle corner point in order to easily find a specific point on the reflector, the effect is obvious, but the processing difficulty of a marker is high, the cost is high, and the risk of false identification exists.
The existing positioning method based on the laser reflector has the defects of large calculated amount, low precision and poor stability in the positioning of the robot after the displacement.
Disclosure of Invention
The invention aims to overcome the defects in the prior art and provides a fusion positioning method and system based on a laser reflector and an odometer, which can improve positioning accuracy and stability. In order to achieve the above purpose, the invention is realized by adopting the following technical scheme:
in a first aspect, the invention provides a fusion positioning method based on a laser reflector and an odometer, comprising the following steps:
acquiring map data and laser point cloud data;
Matching the point cloud data with the map data to obtain a globally positioned position p0;
the odometer starts to track the position from the globally positioned position p0, and accumulates tracking displacement to obtain the position p1 predicted by the positioning of the odometer;
Based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
Based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position.
With reference to the first aspect, further, the acquiring map data includes:
loading the map picture into a gray level map M1, screening the M1 according to pixels, adopting a floating point array F </SUB > to represent the M1, wherein the elements in the array are in one-to-one correspondence with the pixels in the M1, if the gray level value of the pixels is less than 100, the corresponding element value in the array is set to be 0.99, otherwise, the value of the corresponding element in the array is set to be 0;
Restoring the floating point array F </SUB > into a grid Map according to the length and width of M1, wherein the grid element value is of floating point type and corresponds to the value in F </SUB >;
processing the grid Map using an expansion algorithm, for each grid p in the Map, if the value of p is not 0.99, finding the grid calculation distance d closest to p with a value of 0.99, updating the value c=0.99 of p (1-e k·d);
after the robot is started, maps are processed from the transverse direction and the longitudinal direction respectively by using a sliding window maximum value method, and the lengths of the sliding windows are 2, 4,8, 16, 32 and 64 in sequence to respectively obtain Map2, map3, map4, map5, map6 and Map7.
With reference to the first aspect, further, the acquiring point cloud data includes:
Obtaining a frame of laser point cloud Scan from a robot laser radar, performing grid filtering on the point cloud, and ensuring that only one point is reserved in each grid to obtain a filtered point cloud Scan_;
The point clouds were rotated by 0 degrees, 1 degree, and 359 degrees, respectively, to obtain 360 sets of point clouds scan_0, scan_1, scan_359.
With reference to the first aspect, further, the obtaining the globally located position p0 includes:
In the most blurred Map7, multiplying the original resolution by the maximum sliding window length 64 to obtain 3.2 meters as a step length, and performing violence matching by using Scan_0 and Scan_1, respectively, wherein Scan_359 is used for scoring the point cloud and the position thereof by using a rule of adding the values of grids where each point falls after the point cloud is inserted into a designated position;
Selecting the point cloud with the highest score, opening a window at the position of the highest score, wherein the size of the window is 3.2 x 2 = 6.4m, taking 3.2/2 = 1.6m as a step length, and performing violent matching by using the Map6 of the next level, wherein the Map is matched until Map2 according to the rule, and the obtained position is the global positioning position p0.
With reference to the first aspect, further, the obtaining the position p1 predicted by the positioning of the odometer includes:
Aligning the odometer with the global positioning, so that the odometer starts to track the position from the position p0 of the global positioning;
And in each time step of position tracking, calculating the accumulated displacement of the odometer, and adding the displacement and the last positioning result to obtain the position p1 of the positioning prediction of the odometer.
With reference to the first aspect, further, the obtaining the matching ratio score between the laser matching measurement position p2 and the positions p1 and p2 includes:
A window is arranged around the position p1 predicted by the positioning of the odometer, the size of the window is in direct proportion to the accumulated displacement of the odometer,
In the window, according to the position step length of 0.05m and the angle step length of 0.017 radian, performing laser data violent matching to obtain a matching result p2 with the highest score, and outputting a matching rate score between the positions p1 and p 2.
With reference to the first aspect, further, the preset fusion positioning method includes:
the weight of the preset mileage meter measured value is k1[ k10, k11, k12], and the weight of the laser matching measured value is k2[ k20, k21, k22];
judgment 1: judging that the matching rate score is larger than a preset threshold epsilon 1, if the matching rate score is larger than the preset threshold epsilon 1, the laser matching measurement position p2 is reliable, otherwise, the laser matching measurement position p2 is unreliable;
judging 2: performing deviation on the displacement of the laser matching measurement position p2 and the position p0 of the last global positioning to obtain a vector a0 (dx 1, dy 1), decomposing the vector a0 into a right-hand Cartesian coordinate system with the direction of the p0 as the x axis to obtain a transformed vector a1 (dx 2, dy 2), and judging whether dy2 is smaller than a preset threshold epsilon 2;
judging 3: if dy2 is smaller than a preset threshold epsilon 2, judging whether the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3 or not;
Executing judgment 1, wherein the matching rate score is smaller than or equal to a preset threshold epsilon 1, the laser matching measurement position p2 is unreliable, executing judgment 2 and judgment 3, accumulating tracking displacement by the odometer, and using the position p1 predicted by the odometer positioning as a positioning result;
based on the result that the matching rate score is larger than the preset threshold epsilon 1, executing judgment 2, wherein dy2 is larger than or equal to the preset threshold epsilon 2, using the weight k1 and the weight k2 to calculate a weighted average value of p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and the odometer, executing no judgment 3, accumulating tracking displacement by the odometer, and outputting a positioning result;
Based on the judgment that the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3, increasing weights of k10 and k11 in the weight k1 to obtain the weight k1', positively correlating the increasing proportion with d, using the weight k1' and the weight k2 to carry out weighted average on the positions p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and the odometer, accumulating tracking displacement by the odometer, and outputting a positioning result.
With reference to the first aspect, further comprising pose correction:
If the matching rate score is smaller than a preset threshold epsilon 4, a window of 2m x 2m is opened on the position p1, and the point cloud data and the map data are matched to obtain a pose position p3 and a matching rate score2 between the output positions p1 and p 3;
if the matching rate score2 is larger than the preset threshold epsilon 5, the new pose is reliable, p3 is used as a fusion positioning result to be output, and the odometer is aligned.
In a second aspect, the present invention provides a fused positioning system based on a laser reflector and an odometer, comprising:
the acquisition module is used for: the method comprises the steps of acquiring map data and laser point cloud data;
the primary positioning module: the method comprises the steps of matching point cloud data with map data to obtain a globally positioned position p0;
The odometer positioning module: the position tracking is performed from the global positioning position p0 by the odometer, and the position p1 predicted by the odometer positioning is obtained by accumulating tracking displacement;
And the laser positioning module is used for: the method comprises the steps of carrying out local laser positioning on a position p1 predicted based on odometer positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
Fusion positioning module: and the method is used for obtaining a final positioning position by adopting a preset fusion positioning method based on the matching rate score, the positions p0, p1 and p 2.
In a third aspect, the present invention provides a computer readable storage medium storing one or more programs, characterized in that the one or more programs comprise instructions, which when executed by a computing device, cause the computing device to perform any of the methods of the first aspect.
Compared with the prior art, the fusion positioning method and system based on the laser reflector and the odometer provided by the embodiment of the invention have the beneficial effects that:
The method comprises the steps of obtaining map data and laser point cloud data; matching the point cloud data with the map data to obtain a globally positioned position p0; the odometer starts to track the position from the globally positioned position p0, and accumulates tracking displacement to obtain the position p1 predicted by the positioning of the odometer; based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2; based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position. The fusion positioning method provided by the invention has the advantages of high calculation precision, strong stability, less influence from environment and more stability in the positioning of the robot after the displacement.
Drawings
Fig. 1 is a flowchart of a fusion positioning method based on a laser reflector and an odometer according to embodiment 1 of the present invention.
Detailed Description
The invention is further described below with reference to the accompanying drawings. The following examples are only for more clearly illustrating the technical aspects of the present invention, and are not intended to limit the scope of the present invention.
Embodiment one:
as shown in fig. 1, a fusion positioning method based on a laser reflector and an odometer includes:
acquiring map data and laser point cloud data;
Matching the point cloud data with the map data to obtain a globally positioned position p0;
the odometer starts to track the position from the globally positioned position p0, and accumulates tracking displacement to obtain the position p1 predicted by the positioning of the odometer;
Based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
Based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position.
The method comprises the following specific steps:
Step 1: map data is acquired.
Step 1.1: and (5) loading a map.
Loading the map picture into a gray level map M1, filtering the map picture according to pixels, adopting a floating point array F </SUB > to represent the M1, wherein the elements in the array are in one-to-one correspondence with the pixels in the M1, if the gray level value of the pixels is less than 100, the corresponding element value in the array is set to 0.99, otherwise, the value of the corresponding element in the array is set to 0.
And restoring the floating point array F </SUB > into a grid Map according to the length and width of M1, wherein the grid element value is of a floating point type and corresponds to the value in F </SUB >.
The grid Map is processed using an expansion algorithm, and for each grid p in the Map, if the value of p is not 0.99, the grid calculation distance d closest to p is found with a value of 0.99, and the value of p c=0.99 is updated (1-e k·d).
Step 1.2: and (5) starting up and repositioning.
After the robot is started, maps are processed from the transverse direction and the longitudinal direction respectively by using a sliding window maximum value method.
Sliding window maximum method: a window size is selected, slid along a linear data structure, and all values within the window are set to the maximum value within the window range.
For example, the original data is 00000 10000, the sliding window length is 2, and 00001 1100 0 is obtained after sliding.
In the case of the two-dimensional data of this embodiment, for example, the data before the sliding window maximum processing is:
The data obtained after transverse and longitudinal processing of the maximum value of the sliding window with the length of 2 are as follows:
Firstly, selecting the sliding window length as 2, and carrying out sliding window maximum value processing on the data to obtain a Map2. And then selecting the sliding window length 4, and carrying out sliding window maximum processing on Map2 data to obtain a Map layer Map3. And selecting the sliding window lengths of 8, 16, 32 and 64, and performing sliding window processing on the data to obtain Map4, map5, map6 and Map7.
Step 2: laser point cloud data is acquired.
And obtaining a frame of laser point cloud Scan from the robot laser radar, and performing grid filtering on the point cloud to ensure that only one point is reserved in each grid to obtain a filtered point cloud Scan_. In this embodiment, a 5cm grid was used to filter the laser point cloud.
The point clouds were rotated by 0 degrees, 1 degree, and 359 degrees, respectively, to obtain 360 sets of point clouds scan_0, scan_1, scan_359.
Step 3: and matching the point cloud data with the map data to obtain a globally positioned position p0.
In the most blurred Map7, 3.2 meters of step size is obtained by multiplying the original resolution by the maximum sliding window length 64, and scan_0, scan_1 and scan_359 are used for violence matching respectively, so that after the point cloud is inserted into a designated position, the sum of the values of the grids where each point falls is used for scoring the point cloud and the position thereof.
Selecting the point cloud with the highest score, opening a window at the position of the highest score, wherein the size of the window is 3.2 x 2 = 6.4m, taking 3.2/2 = 1.6m as a step length, and performing violent matching by using the Map6 of the next level, wherein the Map is matched until Map2 according to the rule, and the obtained position is the global positioning position p0.
Step 4: the odometer starts to track the position from the globally positioned position p0, and the accumulated tracking displacement obtains the position p1 predicted by the odometer positioning.
The odometer and global positioning are aligned such that the odometer performs position tracking starting from the position p0 of the global positioning.
And in each time step of position tracking, calculating the accumulated displacement of the odometer, and adding the displacement and the last positioning result to obtain the position p1 of the positioning prediction of the odometer.
Step 5: and (3) based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2.
And opening a window around the position p1 predicted by the positioning of the odometer, wherein the size of the window is in direct proportion to the accumulated displacement of the odometer, performing violent matching of laser data according to the window with the position step length of 0.05m and the angle step length of 0.017 radian to obtain a matching result p2 with the highest score, and outputting a matching rate score between the positions p1 and p 2.
Step 6: based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position.
The weights of the preset odometer measurement values are k1[ k10, k11, k12], and the weights of the laser matching measurement values are k2[ k20, k21, k22].
Judgment 1: judging that the matching rate score is larger than a preset threshold epsilon 1, if the matching rate score is larger than the preset threshold epsilon 1, the laser matching measurement position p2 is reliable, otherwise, the laser matching measurement position p2 is unreliable;
judging 2: performing deviation on the displacement of the laser matching measurement position p2 and the position p0 of the last global positioning to obtain a vector a0 (dx 1, dy 1), decomposing the vector a0 into a right-hand Cartesian coordinate system with the direction of the p0 as the x axis to obtain a transformed vector a1 (dx 2, dy 2), and judging whether dy2 is smaller than a preset threshold epsilon 2;
judging 3: if dy2 is smaller than the preset threshold epsilon 2, judging whether the distance d between the positions p1 and p2 is larger than the preset threshold epsilon 3.
And executing judgment 1, wherein the matching rate score is smaller than or equal to a preset threshold epsilon 1, the laser matching measurement position p2 is unreliable, executing judgment 2 and judgment 3, accumulating tracking displacement by the odometer, and using the position p1 predicted by the odometer positioning as a positioning result.
Based on the result that the matching rate score is larger than the preset threshold epsilon 1, the judgment 2 is executed, dy2 is larger than or equal to the preset threshold epsilon 2, the weight k1 and the weight k2 are used for carrying out weighted average on the p1 and the p2 to obtain a fusion positioning position, the fusion positioning position and the odometer are aligned again, the judgment 3 is not executed, the odometer accumulates tracking displacement, and the positioning result is output.
Based on the judgment that the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3, increasing weights of k10 and k11 in the weight k1 to obtain the weight k1', positively correlating the increasing proportion with d, using the weight k1' and the weight k2 to carry out weighted average on the positions p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and the odometer, accumulating tracking displacement by the odometer, and outputting a positioning result.
The method also comprises the following steps of:
If the matching rate score is smaller than a preset threshold epsilon 4, a window of 2m x 2m is opened on the position p1, and the point cloud data and the map data are matched to obtain a pose position p3 and a matching rate score2 between the output positions p1 and p 3;
if the matching rate score2 is larger than the preset threshold epsilon 5, the new pose is reliable, p3 is used as a fusion positioning result to be output, and the odometer is aligned.
The fusion positioning method provided by the invention has the advantages of high calculation precision, strong stability, less influence from environment and more stability in the positioning of the robot after the displacement.
Embodiment two:
the embodiment of the invention provides a positioning system based on a laser reflector and an odometer, which comprises the following components:
the acquisition module is used for: the method comprises the steps of acquiring map data and laser point cloud data;
the primary positioning module: the method comprises the steps of matching point cloud data with map data to obtain a globally positioned position p0;
The odometer positioning module: the position tracking is performed from the global positioning position p0 by the odometer, and the position p1 predicted by the odometer positioning is obtained by accumulating tracking displacement;
And the laser positioning module is used for: the method comprises the steps of carrying out local laser positioning on a position p1 predicted based on odometer positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
Fusion positioning module: and the method is used for obtaining a final positioning position by adopting a preset fusion positioning method based on the matching rate score, the positions p0, p1 and p 2.
Embodiment III:
an embodiment of the invention provides a computer-readable storage medium storing one or more programs, characterized in that the one or more programs comprise instructions, which when executed by a computing device, cause the computing device to perform a method according to embodiment one.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. 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.
The foregoing is merely a preferred embodiment of the present invention, and it should be noted that modifications and variations could be made by those skilled in the art without departing from the technical principles of the present invention, and such modifications and variations should also be regarded as being within the scope of the invention.
Claims (8)
1. The fusion positioning method based on the laser reflector and the odometer is characterized by comprising the following steps of:
acquiring map data and laser point cloud data;
Matching the point cloud data with the map data to obtain a globally positioned position p0;
the odometer starts to track the position from the globally positioned position p0, and accumulates tracking displacement to obtain the position p1 predicted by the positioning of the odometer;
Based on the position p1 predicted by the odometer positioning, performing local laser positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
based on the matching rate score, the positions p0, p1 and p2, a preset fusion positioning method is adopted to obtain a final positioning position;
the preset fusion positioning method comprises the following steps:
the weight of the preset mileage meter measured value is k1[ k10, k11, k12], and the weight of the laser matching measured value is k2[ k20, k21, k22];
judgment 1: judging that the matching rate score is larger than a preset threshold epsilon 1, if the matching rate score is larger than the preset threshold epsilon 1, the laser matching measurement position p2 is reliable, otherwise, the laser matching measurement position p2 is unreliable;
judging 2: performing deviation on the displacement of the laser matching measurement position p2 and the position p0 of the last global positioning to obtain a vector a0 (dx 1, dy 1), decomposing the vector a0 into a right-hand Cartesian coordinate system with the direction of the p0 as the x axis to obtain a transformed vector a1 (dx 2, dy 2), and judging whether dy2 is smaller than a preset threshold epsilon 2;
judging 3: if dy2 is smaller than a preset threshold epsilon 2, judging whether the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3 or not;
Executing judgment 1, wherein the matching rate score is smaller than or equal to a preset threshold epsilon 1, the laser matching measurement position p2 is unreliable, executing judgment 2 and judgment 3, accumulating tracking displacement by the odometer, and using the position p1 predicted by the odometer positioning as a positioning result;
based on the result that the matching rate score is larger than the preset threshold epsilon 1, executing judgment 2, wherein dy2 is larger than or equal to the preset threshold epsilon 2, using the weight k1 and the weight k2 to calculate a weighted average value of p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and the odometer, executing no judgment 3, accumulating tracking displacement by the odometer, and outputting a positioning result;
Based on the judgment that the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3, increasing weights of k10 and k11 in the weight k1 to obtain a weight k1', positively correlating the increased proportion with d, using the weight k1' and the weight k2 to carry out weighted average on the positions p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and an odometer, accumulating tracking displacement by the odometer, and outputting a positioning result;
The method also comprises the following steps of:
If the matching rate score is smaller than a preset threshold epsilon 4, a window of 2m x 2m is opened on the position p1, and the point cloud data and the map data are matched to obtain a pose position p3 and a matching rate score2 between the output positions p1 and p 3;
if the matching rate score2 is larger than the preset threshold epsilon 5, the new pose is reliable, p3 is used as a fusion positioning result to be output, and the odometer is aligned.
2. The fusion positioning method based on the laser reflector and the odometer according to claim 1, wherein the acquiring map data comprises:
loading the map picture into a gray level map M1, screening the M1 according to pixels, adopting a floating point array F </SUB > to represent the M1, wherein the elements in the array are in one-to-one correspondence with the pixels in the M1, if the gray level value of the pixels is less than 100, the corresponding element value in the array is set to be 0.99, otherwise, the value of the corresponding element in the array is set to be 0;
Restoring the floating point array F </SUB > into a grid Map according to the length and width of M1, wherein the grid element value is of floating point type and corresponds to the value in F </SUB >;
Processing the grid Map using an expansion algorithm, for each grid p in the Map, if the value of p is not 0.99, finding the grid calculation distance d' closest to p with a value of 0.99, updating the value c=0.99 of p (1-e k·d');
after the robot is started, maps are processed from the transverse direction and the longitudinal direction respectively by using a sliding window maximum value method, and the lengths of the sliding windows are 2, 4,8, 16, 32 and 64 in sequence to respectively obtain Map2, map3, map4, map5, map6 and Map7.
3. The fusion positioning method based on the laser reflector and the odometer according to claim 2, wherein obtaining the point cloud data comprises:
Obtaining a frame of laser point cloud Scan from a robot laser radar, performing grid filtering on the point cloud, and ensuring that only one point is reserved in each grid to obtain a filtered point cloud Scan_;
The point clouds were rotated by 0 degrees, 1 degree, and 359 degrees, respectively, to obtain 360 sets of point clouds scan_0, scan_1, scan_359.
4. The fusion positioning method based on the laser reflector and the odometer according to claim 3, wherein the obtaining the global positioning position p0 includes:
In the most blurred Map7, multiplying the original resolution by the maximum sliding window length 64 to obtain 3.2 meters as a step length, and performing violence matching by using Scan_0 and Scan_1, respectively, wherein Scan_359 is used for scoring the point cloud and the position thereof by using a rule of adding the values of grids where each point falls after the point cloud is inserted into a designated position;
Selecting the point cloud with the highest score, opening a window at the position of the highest score, wherein the size of the window is 3.2 x 2 = 6.4m, taking 3.2/2 = 1.6m as a step length, and performing violent matching by using the Map6 of the next level, wherein the Map is matched until Map2 according to the rule, and the obtained position is the global positioning position p0.
5. The fusion positioning method based on the laser reflector and the odometer according to claim 1, wherein the obtaining the position p1 predicted by the odometer positioning comprises:
Aligning the odometer with the global positioning, so that the odometer starts to track the position from the position p0 of the global positioning;
And in each time step of position tracking, calculating the accumulated displacement of the odometer, and adding the displacement and the last positioning result to obtain the position p1 of the positioning prediction of the odometer.
6. The fusion positioning method based on the laser reflector and the odometer according to claim 1, wherein the obtaining the laser matching measurement position p2 and the matching ratio score between the positions p1 and p2 comprises:
A window is arranged around the position p1 predicted by the positioning of the odometer, the size of the window is in direct proportion to the accumulated displacement of the odometer,
In the window, according to the position step length of 0.05m and the angle step length of 0.017 radian, performing laser data violent matching to obtain a matching result p2 with the highest score, and outputting a matching rate score between the positions p1 and p 2.
7. Fusion positioning system based on laser reflector and odometer, characterized by comprising:
the acquisition module is used for: the method comprises the steps of acquiring map data and laser point cloud data;
the primary positioning module: the method comprises the steps of matching point cloud data with map data to obtain a globally positioned position p0;
The odometer positioning module: the position tracking is performed from the global positioning position p0 by the odometer, and the position p1 predicted by the odometer positioning is obtained by accumulating tracking displacement;
And the laser positioning module is used for: the method comprises the steps of carrying out local laser positioning on a position p1 predicted based on odometer positioning to obtain a laser matching measurement position p2 and a matching rate score between the positions p1 and p 2;
Fusion positioning module: the method is used for obtaining a final positioning position by adopting a preset fusion positioning method based on the matching rate score, the positions p0, p1 and p 2;
the preset fusion positioning method comprises the following steps:
the weight of the preset mileage meter measured value is k1[ k10, k11, k12], and the weight of the laser matching measured value is k2[ k20, k21, k22];
judgment 1: judging that the matching rate score is larger than a preset threshold epsilon 1, if the matching rate score is larger than the preset threshold epsilon 1, the laser matching measurement position p2 is reliable, otherwise, the laser matching measurement position p2 is unreliable;
judging 2: performing deviation on the displacement of the laser matching measurement position p2 and the position p0 of the last global positioning to obtain a vector a0 (dx 1, dy 1), decomposing the vector a0 into a right-hand Cartesian coordinate system with the direction of the p0 as the x axis to obtain a transformed vector a1 (dx 2, dy 2), and judging whether dy2 is smaller than a preset threshold epsilon 2;
judging 3: if dy2 is smaller than a preset threshold epsilon 2, judging whether the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3 or not;
Executing judgment 1, wherein the matching rate score is smaller than or equal to a preset threshold epsilon 1, the laser matching measurement position p2 is unreliable, executing judgment 2 and judgment 3, accumulating tracking displacement by the odometer, and using the position p1 predicted by the odometer positioning as a positioning result;
based on the result that the matching rate score is larger than the preset threshold epsilon 1, executing judgment 2, wherein dy2 is larger than or equal to the preset threshold epsilon 2, using the weight k1 and the weight k2 to calculate a weighted average value of p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and the odometer, executing no judgment 3, accumulating tracking displacement by the odometer, and outputting a positioning result;
Based on the judgment that the distance d between the positions p1 and p2 is larger than a preset threshold epsilon 3, increasing weights of k10 and k11 in the weight k1 to obtain a weight k1', positively correlating the increased proportion with d, using the weight k1' and the weight k2 to carry out weighted average on the positions p1 and p2 to obtain a fusion positioning position, realigning the fusion positioning position and an odometer, accumulating tracking displacement by the odometer, and outputting a positioning result;
The method also comprises the following steps of:
If the matching rate score is smaller than a preset threshold epsilon 4, a window of 2m x 2m is opened on the position p1, and the point cloud data and the map data are matched to obtain a pose position p3 and a matching rate score2 between the output positions p1 and p 3;
if the matching rate score2 is larger than the preset threshold epsilon 5, the new pose is reliable, p3 is used as a fusion positioning result to be output, and the odometer is aligned.
8. A computer readable storage medium storing one or more programs, wherein the one or more programs comprise instructions, which when executed by a computing device, cause the computing device to perform any of the methods of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211471520.1A CN115727836B (en) | 2022-11-23 | 2022-11-23 | Fusion positioning method and system based on laser reflector and odometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211471520.1A CN115727836B (en) | 2022-11-23 | 2022-11-23 | Fusion positioning method and system based on laser reflector and odometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115727836A CN115727836A (en) | 2023-03-03 |
| CN115727836B true CN115727836B (en) | 2024-07-16 |
Family
ID=85297635
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211471520.1A Active CN115727836B (en) | 2022-11-23 | 2022-11-23 | Fusion positioning method and system based on laser reflector and odometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115727836B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116935016B (en) * | 2023-07-28 | 2024-03-08 | 上海济目科技有限公司 | Integrated tunnel construction site risk identification and laser positioning method |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112987061A (en) * | 2021-02-08 | 2021-06-18 | 福州大学 | Fuzzy fusion positioning method based on GPS and laser radar |
| CN114296096A (en) * | 2021-12-23 | 2022-04-08 | 深圳优地科技有限公司 | Robot positioning method, system and terminal |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019040997A1 (en) * | 2017-09-04 | 2019-03-07 | Commonwealth Scientific And Industrial Research Organisation | Method and system for use in performing localisation |
| CN110243380B (en) * | 2019-06-26 | 2020-11-24 | 华中科技大学 | Map matching method based on multi-sensor data and angle feature recognition |
| CN112965076B (en) * | 2021-01-28 | 2024-05-24 | 上海思岚科技有限公司 | Multi-radar positioning system and method for robot |
| CN113050116B (en) * | 2021-03-05 | 2024-02-27 | 深圳市优必选科技股份有限公司 | Robot positioning method, apparatus, robot, and readable storage medium |
| CN113376650B (en) * | 2021-08-09 | 2021-11-23 | 浙江华睿科技股份有限公司 | Mobile robot positioning method and device, electronic equipment and storage medium |
| CN114383611A (en) * | 2021-12-30 | 2022-04-22 | 华南智能机器人创新研究院 | Multi-machine cooperative laser SLAM method, device and system for mobile robot |
| CN114370871A (en) * | 2022-01-13 | 2022-04-19 | 华南理工大学 | Close coupling optimization method for visible light positioning and laser radar inertial odometer |
| CN115375861A (en) * | 2022-08-18 | 2022-11-22 | 江苏徐工工程机械研究院有限公司 | Three-dimensional mapping method, device and storage medium for unmanned mining area |
-
2022
- 2022-11-23 CN CN202211471520.1A patent/CN115727836B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112987061A (en) * | 2021-02-08 | 2021-06-18 | 福州大学 | Fuzzy fusion positioning method based on GPS and laser radar |
| CN114296096A (en) * | 2021-12-23 | 2022-04-08 | 深圳优地科技有限公司 | Robot positioning method, system and terminal |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115727836A (en) | 2023-03-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107038717B (en) | A method of 3D point cloud registration error is automatically analyzed based on three-dimensional grid | |
| EP3731181A1 (en) | Method and apparatus for point cloud registration, server and computer readable medium | |
| CN109144056A (en) | The global method for self-locating and equipment of mobile robot | |
| CN111324848B (en) | Vehicle-mounted track data optimization method of mobile laser radar measurement system | |
| CN114636993A (en) | External parameter calibration method, device and equipment for laser radar and IMU | |
| US20140142891A1 (en) | Generaton of map data | |
| CN115727836B (en) | Fusion positioning method and system based on laser reflector and odometer | |
| CN110763239B (en) | Filtering combined laser SLAM mapping method and device | |
| CN102169581A (en) | Feature vector-based fast and high-precision robustness matching method | |
| CN111915657B (en) | Point cloud registration method and device, electronic equipment and storage medium | |
| CN113947636B (en) | Laser SLAM positioning system and method based on deep learning | |
| CN111707997A (en) | Radar target tracking method and device, electronic equipment and storage medium | |
| CN112578357A (en) | Radar calibration parameter correction method and device, electronic equipment and road side equipment | |
| CN113066127A (en) | Visual inertial odometer method and system for calibrating equipment parameters on line | |
| CN112861870A (en) | Pointer instrument image correction method, system and storage medium | |
| CN114137562B (en) | Multi-target tracking method based on improved global nearest neighbor | |
| CN115457152A (en) | External parameter calibration method and device, electronic equipment and storage medium | |
| CN112258582B (en) | Camera attitude calibration method and device based on road scene recognition | |
| CN111366921B (en) | Dual-station radar cross positioning method, system and medium based on distance weighted fusion | |
| CN111951341A (en) | Closed loop detection improvement method based on RGB-D SLAM | |
| Song et al. | MF-LIO: integrating multi-feature LiDAR inertial odometry with FPFH loop closure in SLAM | |
| CN109919998B (en) | Satellite attitude determination method and device and terminal equipment | |
| CN114485683A (en) | Local fragment map matching method and device based on Monte Carlo optimization | |
| CN117292118B (en) | Radar-guided photoelectric tracking coordinate compensation method, radar-guided photoelectric tracking coordinate compensation device, electronic equipment and medium | |
| CN115294485B (en) | Municipal engineering measurement positioning method and system |
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 |