CN116840820A - Method and system for detecting 2D laser positioning loss and storage medium - Google Patents
Method and system for detecting 2D laser positioning loss and storage medium Download PDFInfo
- Publication number
- CN116840820A CN116840820A CN202311098384.0A CN202311098384A CN116840820A CN 116840820 A CN116840820 A CN 116840820A CN 202311098384 A CN202311098384 A CN 202311098384A CN 116840820 A CN116840820 A CN 116840820A
- Authority
- CN
- China
- Prior art keywords
- laser
- map
- points
- penetration
- positioning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000003860 storage Methods 0.000 title claims abstract description 15
- 230000035515 penetration Effects 0.000 claims abstract description 63
- 238000009826 distribution Methods 0.000 claims abstract description 7
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 230000001360 synchronised effect Effects 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 8
- 238000004519 manufacturing process Methods 0.000 description 7
- 230000006378 damage Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 3
- 208000027418 Wounds and injury Diseases 0.000 description 2
- 238000007667 floating Methods 0.000 description 2
- 208000014674 injury Diseases 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000000149 penetrating effect Effects 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The application provides a method and a system for detecting 2D laser positioning loss and a storage medium, wherein the method comprises the following steps: carrying out Gaussian distribution on map points to generate a likelihood Gaussian probability grid map, calculating the matching degree G of grids where each laser point is located and the nearest map point, and calculating a matching degree mean value G; when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points; counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points; and when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, a positioning loss judgment result is given, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained. Therefore, in a complex environment, whether the 2D laser positioning is lost or not can be accurately judged.
Description
Technical Field
The present application relates to laser positioning technologies, and in particular, to a method and system for detecting 2D laser positioning loss, and a storage medium.
Background
Industrial robots play an important role in automated production, and accurate positioning technology is crucial for accurate operation and efficient production of robots. Currently, indoor environments are often mapped, positioned, and navigated using 2D single line lasers. Although the single-line laser-based mapping technology is mature, the 2D laser positioning technology is easy to have the risk of positioning loss under special complex environments, such as environmental conditions of large-scale environmental changes, more people flow, more similar obstacles and the like.
And once the positioning is lost in the industrial environment, serious problems are caused. First, product damage is an important risk that a loss of robot positioning may result in misalignment or incorrect posture, and thus, failure to reach a specified location accurately. Secondly, lost positioning can deviate the robot motion path from the expected one, requiring additional time and operation to readjust the positioning, resulting in reduced production efficiency, prolonged production cycle, and even interruption problems such as production line downtime.
In addition, the risk of safety accidents still exists in the location loss, and the robot can not accurately discern work area boundary and barrier, has increased collision, unexpected contact or the risk of clamping injury, probably leads to personal injury, equipment damage or other incident, can reduce the user to robot system's confidence, influences product market competition and enterprise reputation.
Therefore, by which means it is accurately and reliably detected whether the 2D laser positioning of the industrial robot is lost is a problem to be solved by those skilled in the art.
Disclosure of Invention
Therefore, the main purpose of the present application is to provide a method, a system and a storage medium for detecting 2D laser positioning loss, so as to accurately determine whether the 2D laser positioning is lost in a complex environment.
To achieve the above object, according to a first aspect of the present application, there is provided a method for detecting 2D laser positioning loss, comprising the steps of:
step S100, carrying out Gaussian distribution on map points, generating a likelihood Gaussian probability grid map, calculating the matching degree G of grids where each laser point is located and the nearest map point, and calculating a matching degree mean value G;
step S200, when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points;
step S300, counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points;
step S400 provides a positioning loss judgment result when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
In a possibly preferred embodiment, the step of calculating the matching degree mean G in step S100 includes:
step S110 calculation
；
Wherein the method comprises the steps ofThe distance between the grid where the current laser point is located and the nearest map point;
step S120 adds all the calculated matching degrees G, and divides the sum by the total number of grids where the laser points are located, thereby obtaining a matching degree average G.
In a possibly preferred embodiment, the step of determining in step S200 whether the laser light penetrates the map includes:
step S210, dividing the map coordinate system into a plurality of areas according to the slope, taking the current position as an original point, taking each laser point as an end point, and constructing a laser ray by adopting a Bresenham scribing algorithm;
step S220 traverses all the vertical and horizontal coordinate positions on the laser ray in each region, judges whether coordinates overlapped with map points exist or not, and if so, judges that the map is penetrated.
In a possibly preferred embodiment, the step of traversing all the vertical and horizontal coordinate positions of the laser beam line in each region in step S220 includes:
step S221, judging the slope between the current laser point and the origin coordinate to determine the area of the map coordinate system where the laser ray is located;
step S222, according to the region of the map coordinate system where the laser rays are located, performing coordinate transformation to transform into a main region;
step S223 sets the increment of the comparison expression and the initial value of the comparison expression, and circularly traverses each abscissa on the laser ray to obtain the corresponding ordinate.
In a possibly preferred embodiment, the step of calculating the distance scale score L of the map point and the laser point at the penetration point position comprises:
step S230, setting the coordinates of map points at the positions of the penetration points as @、) The coordinates of the corresponding laser points are%) Calculating the distance between the two
；
Step S340 calculates the sum of dist of all the penetration point positions and divides it by the total number of penetration pointsObtaining a distance proportion score L
。
In a possibly preferred embodiment, the loss threshold determination condition in step S400 includes:
when the matching degree mean value G is more than 0.7, judging that the penetration ratio N is more than 0.25, and the distance ratio score L is more than 0.3;
when the matching degree mean value G < = 0.7, judging the penetration ratio N to be more than 0.18 and the distance ratio score L to be more than 0.2;
and judging that the current positioning is lost when any condition is met.
In a possibly preferred embodiment, the inversely proportional threshold judgment condition in step S400 includes:
when the penetration ratio N is less than 0.17 and the average distance M is more than 0.3, the current positioning is judged to be lost.
In a possibly preferred embodiment, the method for detecting 2D laser positioning loss further includes:
step S500 stops the loss of position determination when it is determined that the time stamps between the laser information and the positioning information are not synchronized.
In order to achieve the above object, there is also provided a system for detecting 2D laser positioning loss according to a second aspect of the present application, which includes:
the storage unit is used for storing a program comprising the method steps for detecting 2D laser positioning loss, so that the processing unit and the navigation unit can timely perform the adjustment;
the navigation unit is used for carrying out Gaussian distribution on the map points, generating a likelihood Gaussian probability grid map, acquiring a laser radar scanning frame, and determining the position of the laser points on the grid map and the current positioning position;
the processing unit is used for calculating the matching degree G of the grid where each laser point is located and the nearest map point of the grid, and calculating the matching degree mean value G; when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points; counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points; and then, when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, a positioning loss judgment result is given, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
In order to achieve the above object, corresponding to the above method, according to a third aspect of the present application, there is also provided a computer readable storage medium having stored thereon a computer program, wherein the computer program, when executed by a processor, implements the steps of the method for detecting 2D laser positioning loss as described in any of the above.
The method, the system and the storage medium for detecting 2D laser positioning loss can skillfully calculate the matching degree of laser and a map, judge whether the map is penetrated by the laser or not, and comprehensively evaluate whether the current positioning is lost or not by integrating the numerical values corresponding to the characteristics, so that whether the 2D laser positioning of the industrial robot is lost or not can be accurately and reliably detected in real time, the safety accident risk of the mobile robot is reduced, and the reliability of the user on the automatic production of the robot is improved.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 is a schematic diagram of steps of a method for detecting 2D laser positioning loss according to the present application;
FIG. 2 is a schematic diagram showing the correspondence between a laser scanning frame and a map in a normal positioning state;
fig. 3 is a conceptual diagram of dividing a coordinate system into 8 areas to reduce computing resources when determining whether laser penetrates a map in the method for detecting 2D laser positioning loss according to the present application;
FIG. 4 is a schematic diagram of all vertical and horizontal coordinate positions on a main area calculation laser line when judging whether laser penetrates a map in the method for detecting 2D laser positioning loss of the application;
FIG. 5 is a schematic diagram showing a method for detecting 2D laser positioning loss according to the present application, when determining whether laser penetrates a map, determining penetration according to whether there is a position overlapping the map on a laser line;
FIG. 6 is a logic diagram of determining whether a position is lost in the method for detecting 2D laser positioning loss according to the present application;
FIG. 7 is a schematic diagram illustrating that an example satisfies an inverse threshold judgment condition in the method for detecting 2D laser positioning loss according to the present application;
fig. 8 is a schematic structural diagram of a system for detecting 2D laser positioning loss according to the present application.
Detailed Description
In order that those skilled in the art can better understand the technical solutions of the present application, the following description will clearly and completely describe the specific technical solutions of the present application in conjunction with the embodiments to help those skilled in the art to further understand the present application. It will be apparent that the embodiments described herein are merely some, but not all embodiments of the application. It should be noted that embodiments of the present application and features of embodiments may be combined with each other by those of ordinary skill in the art without departing from the spirit of the present application and without conflicting with each other. All other embodiments, which are derived from the embodiments herein without creative effort for a person skilled in the art, shall fall within the disclosure and the protection scope of the present application.
Furthermore, the terms "first," "second," "S100," "S200," and the like in the description and in the claims and drawings are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the application described herein may be implemented in sequences other than those described herein. Also, the terms "comprising" and "having" and any variations thereof herein are intended to cover a non-exclusive inclusion. Unless specifically stated or limited otherwise, the terms "disposed," "configured," "mounted," "connected," "coupled" and "connected" are to be construed broadly, e.g., as being either permanently connected, removably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the terms in this case will be understood by those skilled in the art in view of the specific circumstances and in combination with the prior art.
In order to accurately judge whether the robot positioning is lost, the application comprehensively considers a plurality of indexes. Firstly, according to the matching degree of the current laser and the map, the positioning accuracy can be judged. I.e. a higher degree of matching indicates a good positioning and a lower degree of matching may mean a lost positioning.
Secondly, whether the positioning is lost is judged by checking whether the laser penetrates through the originally constructed map. If the laser is mostly penetrating the map or each laser point is a large distance from its nearest map, it may indicate a high probability of loss of positioning.
Further, consider that since the navigation map is a gaussian likelihood map, there are situations where the positioning is lost but the laser remains near the map but does not penetrate the map. For this reason, in this case, the inventors consider that the average distance of the overall laser point to the map can be calculated, such that a smaller average distance may mean that the positioning is better, while a larger average distance may imply that the positioning is lost.
Based on the above inventive concept, the present application provides a method for detecting 2D laser positioning loss, so as to accurately determine whether positioning loss occurs in 2D laser positioning in a complex environment, where, as shown in fig. 1 to 6, the method includes the steps of:
step S100, gaussian distribution is carried out on map points, a likelihood Gaussian probability grid map is generated, the matching degree G of grids where all laser points are located and the nearest map points are calculated, and the matching degree mean value G is calculated.
Specifically, in order to improve the judgment calculation efficiency, the matching degree g score of the grids possibly scanned by the laser points on the grid map can be calculated. If a Gaussian probability grid map is constructed, the received map points are divided into discrete grid cells, information about the position is stored in each grid cell, and then when the laser points fall in the grid, the matching degree g and the distance from the current grid to the nearest map point can be correspondingly known.
The example construction steps are: first determining the size of the grid map and the size of each grid, creating a blank grid map, and initializing the values in the grids. And setting the initial matching degree score as 1 at the corresponding grid position according to the received original map points. Finally, the distances of 4 grids around each map point grid from the map point grid are checked with each map point grid as the center. If the distance is smaller than the threshold value, the matching degree g of the positions of the expanded 4 grids is calculated. Then, 4 grids are inflated again centering on each new grid, and the distances of these grids to the map grid are calculated. If the distance is greater than the threshold, the grid exits the loop; if the distance is smaller than the threshold value, performing loop iteration, and calculating the matching degree g of each grid.
Wherein the step of calculating the matching degree g comprises the following steps:
step S110 calculation
；
Wherein the method comprises the steps ofThe distance between the grid where the current laser point is located and the nearest map point; through the steps, each grid near each map point can be known to have different score values, and the matching score of the current laser and the map can be more comprehensively analyzed.
Then, each time the laser scans a frame, the matching degree average value G can be calculated, which comprises the following steps:
step S120 is to add the matching degree G of the map points and the grids of all the calculated laser points in the current laser scanning frame, and divide the sum by the total number of grids of the laser points to obtain a matching degree mean value G.
In step S200, when it is determined that there is a laser penetration map, the map points and the laser points at the positions of the penetration points are recorded to calculate a distance ratio score L therebetween.
In this example, the step S200 of determining whether the laser light penetrates the map includes:
in step S210, the map coordinate system is divided into a plurality of areas according to the slope, the current position is taken as an original point, each laser point is taken as an end point, and a Bresenham scribing algorithm is adopted to construct laser rays.
Step S220 traverses all the vertical and horizontal coordinate positions on the laser ray in each region, judges whether coordinates overlapped with map points exist or not, and if so, judges that the map is penetrated.
Specifically, as shown in fig. 3 to 5, in order to determine whether the current laser spot penetrates the map, the present example preferably uses the idea of Bresenham's scribing algorithm to simulate the laser beam, wherein since a large number of laser spots need to be calculated, the calculation efficiency must be ensured, and for this reason, it is preferable to implement straight line drawing using only addition, subtraction and comparison operations in the present example, thereby greatly reducing the required calculation resources. As shown in fig. 3, the overall idea is to divide the coordinate system into 8 parts according to the slope, and calculate the position of the longitudinal and transverse coordinates on the laser beam line in each region.
For example, let the origin coordinates of the robot be%) The position coordinates of the end point laser point are%) And drawing a straight line by taking the origin of the robot as a starting point and the position of the laser spot as an end point. The idea is to traverse all the abscissa and find out each abscissa according to the linear equationOrdinate corresponding to the label(as shown in fig. 4, where each grid represents a pixel).
As can be seen from the figure 4 of the drawings,corresponding ordinate isRounding is used to round the ordinate values. To be used forFor example, fruitsThenEquivalent toIf (3)ThenEquivalent to. Will beBoth sides multiply by 2 at the same time to becomeWhereinEqual toThe equation becomes solved，（，) Thus can be used forReplaced byAs an initial value of the expression, only the judgment is neededThe comparison expression with 0 can be used to find the corresponding y value, at this time，All are integers, and complex calculation of floating point type is completely avoided.
Meanwhile, as can be seen from fig. 3, in the region 1 (main region), the slope is greater than 0 and less than 1, so that the ordinate corresponding to the adjacent abscissa either remains unchanged or is at most added with 1. From this it can be deduced that if it is satisfied thatThen describe the currentDerived from the eastern of the last moment, whichThe value remains unchanged. If the condition is not satisfied, then the current time is describedThe value being derived from the northeast direction of the last moment, whichThe value is directly added to 1.
Next considerThe corresponding ordinate isIf the previous pixel was derived from the northeast direction, a comparison is required(because inWhere the y value range isTo the point ofA watershed of 1.5), equivalent toIs also equivalent toIn increments of。
If the previous pixel was derived from the northeast direction, a comparison is requiredEquivalent toIs also equivalent toIn increments of。
Similarly, the rule can be found by deducing the rule until the end of the line segment, and if the previous position is derived from the east, the increment isIf the previous position was derived from northeast, the increment is:。
based on this concept, it can be actually found that the laser rays of different areas can be converted into the main area (area 1) and then calculated by the above scheme, thereby avoiding complex calculation of floating point type.
For this purpose, in step S220 of the present example, the preferred step of traversing all the vertical and horizontal coordinate positions on the laser beam line in each region includes:
step S221 determines the slope between the current laser spot and the origin coordinate to determine the region of the map coordinate system where the laser ray is located.
Step S222 performs coordinate transformation to transform into the main area according to the area of the map coordinate system where the laser beam is located.
Step S223 sets the increment of the comparison expression and the initial value of the comparison expression, and circularly traverses each abscissa on the laser ray to obtain the corresponding ordinate.
For example, a slope between the current laser point and the robot origin coordinates is first determined.
Wherein the method comprises the steps of，，；
If slope is less than 1, it is stated that the current laser spot is in region 1, no transform is needed.
If slope is greater than 1, it is sufficient to exchange the abscissa and ordinate to change to region 1, indicating that the current laser spot is in region 2.
If slope is less than 0, this indicates that the current laser spot is in regions 7 and 8, pairThe axis is subjected to mirror image transformation, i.eThe transition is made to region 1.
According to the above conception, other conditions show that the current laser point is in the 3, 4, 5 and 6 areas, the starting point and the end point of the linear are exchanged, namely the current point can be converted to the right area of the y axis, namely the current point is firstly judged to be in which area according to the slope, and the current point is converted to the area 1 according to the conversion criteria of different areas.
Thereafter, an increment of the comparison expression is set, and if the previous position is derived from the east, the increment is:
；
if the previous position was derived from northeast, the increment is:
；
setting an initial value of a comparison expression:
；
and circularly traversing each abscissa to obtain a corresponding y coordinate:
if it isThe representation being derived from the east, the y value being unchanged, plus the corresponding increment；
Otherwise, the representation is generated by northeast Fang Yan, the y value is incremented by one, plus the corresponding increment
Thus, all the vertical and horizontal coordinate positions on the laser ray can be calculated, at this time, only all the vertical and horizontal coordinate positions on the laser ray in each region are traversed, whether the coordinates overlapped with the map points exist or not is judged, if so, the map is regarded as being penetrated, and the judgment of whether the laser points penetrate the map points or not is finished
Further, after the determination of the map penetration is completed, a distance scale score L of the map point and the laser point at the penetration point position can be calculated, which includes the following exemplary steps:
step S230, setting the coordinates of map points at the positions of the penetration points as @、) The coordinates of the corresponding laser points are%) Calculating the distance between the two
；
Step S340 calculates the sum of dist of all the penetration point positions and divides it by the total number of penetration pointsObtaining a distance proportion score L
。
Step S300, the whole penetration ratio N of the grid map is counted, and the average distance M between all laser points and map points is calculated.
Specifically, after the map penetration determination is completed, it is known how many penetration points are in a frame of laser radar scanning frame, the ratio of the number of all laser points to the number of penetration points is counted, the overall penetration ratio N of the map can be calculated, and the calculation example of the average distance M between all laser points and the map point is as follows:
；
wherein the method comprises the steps ofRepresenting the position of each laser spot,the bit representing the nearest map point corresponding to each laser point, n represents the number of laser points, i.e. after summing the distances from each laser point to its nearest map point, dividing by the total number of all laser points in a frame of laser radar scanning frame.
Step S400 provides a positioning loss judgment result when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
Specifically, the overall judgment flow is as follows:
firstly, judging whether the current robot is on an illegal point (outside a map or on an obstacle), if so, losing the default positioning.
Next, if the robot is not at an illegal point, as shown in fig. 6, performing the loss threshold determination in step S400 includes:
judging condition 1: when the matching degree mean value G is more than 0.7, judging whether the penetration ratio N is more than 0.25, and the distance ratio score L is more than 0.3;
judging condition 2: when the matching degree mean value G < = 0.7, judging whether the penetration ratio N is more than 0.18 and the distance ratio score L is more than 0.2;
and when any judging condition is met, judging that the current positioning is lost. Otherwise, the position is not considered to be lost. By setting two groups of different threshold judgment and adjustment, the stringency of the positioning device can be adjusted according to different conditions, so that the situation of positioning loss can be detected more accurately.
On the other hand, considering that in practice there is also a case where the penetration ratio is small but the average distance of the laser from the map is large, there is a possibility that, empirically, there is a case where the positioning is lost.
For example, as shown in fig. 7, only about 8% of the laser light has penetrated the map, but the positioning has been lost at this time. It is also necessary to calculate the average distance of all laser points from the map to determine if the positioning is lost, based on such experience. For example, an average distance greater than 2 meters and less than 5 meters, then a loss of position is considered; but if the average distance is greater than 5 meters, the positioning is considered normal. This is because most of the laser light is completely blocked when the vehicle is surrounded by many people, resulting in a large distance from the laser light to the map.
The inversely proportional threshold judgment condition in step S400 includes:
when the penetration ratio N is less than 0.17 and the average distance M is more than 0.3, the current positioning is judged to be lost.
In addition, the above-mentioned threshold judgment condition parameters in the present example are merely examples, and those skilled in the art may also adjust the threshold judgment condition parameters according to actual situations, and thus the present application is not limited to the examples.
Further, in order to avoid false alarm problem, in the pre-selection example, a new judgment condition is introduced: when a loss of position is detected in several consecutive frames (e.g. 5 frames), the current position is considered to be lost and an error code is reported. Compared with the traditional single-frame judging mode, the continuous frame judgment can reduce the influence of environmental noise and instantaneous deviation, and improve the stability and accuracy of judgment, thereby improving the accuracy of positioning loss judgment.
Further, in order to reduce the occurrence rate of false alarm, improve the detection accuracy, and control the timestamp synchronization of each data, the method for detecting the 2D laser positioning loss further includes:
step S500 stops the loss of position determination when it is determined that the time stamps between the laser information and the positioning information are not synchronized. I.e. the above example of the judging step is performed when the time stamps between the laser information and the positioning information are synchronized.
On the other hand, referring to fig. 8, corresponding to the above method example, the present application further provides a system for detecting 2D laser positioning loss, which includes:
the storage unit is used for storing a program comprising the method steps for detecting 2D laser positioning loss, so that the processing unit and the navigation unit can timely perform the adjustment;
the navigation unit is used for carrying out Gaussian distribution on the map points, generating a likelihood Gaussian probability grid map, acquiring a laser radar scanning frame, and determining the position of the laser points on the grid map and the current positioning position;
the processing unit is used for calculating the matching degree G of the grid where each laser point is located and the nearest map point of the grid, and calculating the matching degree mean value G; when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points; counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points; and then, when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, a positioning loss judgment result is given, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
In another aspect, the present application also provides a computer readable storage medium having stored thereon a computer program, corresponding to the above method examples, wherein the computer program, when executed by a processor, implements the steps of the method for detecting 2D laser positioning loss as described in any of the above.
In summary, by the method, the system and the storage medium for detecting 2D laser positioning loss, provided by the application, the matching degree of laser and a map can be calculated skillfully, whether the phenomenon of laser penetrating the map exists or not is judged, and meanwhile, whether the current positioning is lost or not is comprehensively evaluated by integrating the numerical values corresponding to the characteristics, so that whether the problem of losing the 2D laser positioning of the industrial robot exists or not can be detected accurately and reliably in real time, the safety accident risk of the mobile robot is reduced, and the reliability of automatic production of the robot for a user is improved.
The preferred embodiments of the application disclosed above are intended only to assist in the explanation of the application. The preferred embodiments are not exhaustive or to limit the application to the precise form disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the application and the practical application, to thereby enable others skilled in the art to best understand and utilize the application. The application is to be limited only by the following claims and their full scope and equivalents, and any modifications, equivalents, improvements, etc., which fall within the spirit and principles of the application are intended to be included within the scope of the application.
It will be appreciated by those skilled in the art that the system, apparatus and their respective modules provided by the present application may be implemented entirely by logic programming method steps, in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc., except for implementing the system, apparatus and their respective modules provided by the present application in a purely computer readable program code. Therefore, the system, the apparatus, and the respective modules thereof provided by the present application may be regarded as one hardware component, and the modules included therein for implementing various programs may also be regarded as structures within the hardware component; modules for implementing various functions may also be regarded as being either software programs for implementing the methods or structures within hardware components.
Furthermore, all or part of the steps in implementing the methods of the embodiments described above may be implemented by a program, where the program is stored in a storage medium and includes several instructions for causing a single-chip microcomputer, chip or processor (processor) to execute all or part of the steps in the methods of the embodiments of the application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
In addition, any combination of various embodiments of the present application may be performed, so long as the concept of the embodiments of the present application is not violated, and the disclosure of the embodiments of the present application should also be considered.
Claims (10)
1. A method of detecting 2D laser positioning loss, the steps comprising:
step S100, carrying out Gaussian distribution on map points, generating a likelihood Gaussian probability grid map, calculating the matching degree G of grids where each laser point is located and the nearest map point, and calculating a matching degree mean value G;
step S200, when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points;
step S300, counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points;
step S400 provides a positioning loss judgment result when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
2. The method for detecting 2D laser positioning loss according to claim 1, wherein the step of calculating the matching degree mean G in step S100 includes:
step S110 calculation
；
Wherein the method comprises the steps ofThe distance between the grid where the current laser point is located and the nearest map point;
step S120 adds all the calculated matching degrees G, and divides the sum by the total number of grids where the laser points are located, thereby obtaining a matching degree average G.
3. The method for detecting 2D laser positioning loss according to claim 1, wherein the step of determining whether the laser light penetrates the map in step S200 includes:
step S210, dividing the map coordinate system into a plurality of areas according to the slope, taking the current position as an original point, taking each laser point as an end point, and constructing a laser ray by adopting a Bresenham scribing algorithm;
step S220 traverses all the vertical and horizontal coordinate positions on the laser ray in each region, judges whether coordinates overlapped with map points exist or not, and if so, judges that the map is penetrated.
4. A method for detecting loss of 2D laser positioning according to claim 3, wherein the step of traversing all the vertical and horizontal coordinate positions on the laser line in each region in step S220 includes:
step S221, judging the slope between the current laser point and the origin coordinate to determine the area of the map coordinate system where the laser ray is located;
step S222, according to the region of the map coordinate system where the laser rays are located, performing coordinate transformation to transform into a main region;
step S223 sets the increment of the comparison expression and the initial value of the comparison expression, and circularly traverses each abscissa on the laser ray to obtain the corresponding ordinate.
5. The method of detecting 2D laser positioning loss according to claim 1, wherein the step of calculating a distance scale score L of the map point and the laser point at the position of the penetration point comprises:
step S230, setting the coordinates of map points at the positions of the penetration points as @、/>) The coordinates of the corresponding laser spot are (+.>) Calculating the distance between the two
；
Step S340 calculates the sum of dist of all the penetration point positions and divides it by the total number of penetration pointsObtaining a distance proportion score L
。
6. The method for detecting 2D laser positioning loss according to claim 1, wherein the loss threshold judgment condition in step S400 includes:
when the matching degree mean value G is more than 0.7, judging that the penetration ratio N is more than 0.25, and the distance ratio score L is more than 0.3;
when the matching degree mean value G < = 0.7, judging the penetration ratio N to be more than 0.18 and the distance ratio score L to be more than 0.2;
and judging that the current positioning is lost when any condition is met.
7. The method for detecting 2D laser positioning loss according to claim 1, wherein the inverse threshold judgment condition in step S400 includes:
when the penetration ratio N is less than 0.17 and the average distance M is more than 0.3, the current positioning is judged to be lost.
8. The method of detecting 2D laser positioning loss according to claim 1, wherein the steps further comprise:
step S500 stops the loss of position determination when it is determined that the time stamps between the laser information and the positioning information are not synchronized.
9. A system for detecting 2D laser positioning loss, comprising:
a storage unit for storing a program comprising the method steps of detecting 2D laser positioning loss according to any one of claims 1 to 8 for the processing unit, the navigation unit timely retrieving and executing;
the navigation unit is used for carrying out Gaussian distribution on the map points, generating a likelihood Gaussian probability grid map, acquiring a laser radar scanning frame, and determining the position of the laser points on the grid map and the current positioning position;
the processing unit is used for calculating the matching degree G of the grid where each laser point is located and the nearest map point of the grid, and calculating the matching degree mean value G; when judging that the laser penetration map exists, recording map points and laser points at the positions of all penetration points to calculate a distance proportion score L between the map points and the laser points; counting the whole penetration ratio N of the grid map, and calculating the average distance M between all laser points and map points; and then, when the matching degree mean value G, the distance proportion score L and the penetration proportion N are detected to meet the loss threshold judgment condition, a positioning loss judgment result is given, otherwise, whether the penetration proportion N and the average distance M exceed the inverse proportion threshold judgment condition is judged, and the positioning loss judgment result is obtained.
10. A computer readable storage medium having stored thereon a computer program, wherein the computer program, when executed by a processor, implements the steps of the method of detecting 2D laser positioning loss as claimed in any of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311098384.0A CN116840820B (en) | 2023-08-29 | 2023-08-29 | Method and system for detecting 2D laser positioning loss and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311098384.0A CN116840820B (en) | 2023-08-29 | 2023-08-29 | Method and system for detecting 2D laser positioning loss and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116840820A true CN116840820A (en) | 2023-10-03 |
| CN116840820B CN116840820B (en) | 2023-11-24 |
Family
ID=88165533
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311098384.0A Active CN116840820B (en) | 2023-08-29 | 2023-08-29 | Method and system for detecting 2D laser positioning loss and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116840820B (en) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017152390A1 (en) * | 2016-03-09 | 2017-09-14 | 广州艾若博机器人科技有限公司 | Map construction method, and correction method and apparatus |
| CN110675307A (en) * | 2019-08-19 | 2020-01-10 | 杭州电子科技大学 | Implementation method of 3D sparse point cloud to 2D grid map based on VSLAM |
| CN111077495A (en) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| US20200241112A1 (en) * | 2019-01-29 | 2020-07-30 | Ubtech Robotics Corp Ltd | Localization method and robot using the same |
| CN111578932A (en) * | 2020-05-28 | 2020-08-25 | 长沙中联重科环境产业有限公司 | Image establishing method, device, medium and equipment based on multi-line laser radar |
| CN112711012A (en) * | 2020-12-18 | 2021-04-27 | 上海蔚建科技有限公司 | Global position initialization method and system of laser radar positioning system |
| WO2021104497A1 (en) * | 2019-11-29 | 2021-06-03 | 广州视源电子科技股份有限公司 | Positioning method and system based on laser radar, and storage medium and processor |
| US20210270612A1 (en) * | 2020-03-02 | 2021-09-02 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method, apparatus, computing device and computer-readable storage medium for positioning |
| CN113532472A (en) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | Method and system for detecting laser mapping odometer and integrated navigation positioning deviation |
| CN114440858A (en) * | 2022-01-25 | 2022-05-06 | 中国人民解放军总医院第一医学中心 | Mobile robot positioning loss detection method, system, equipment and storage medium |
| WO2022267285A1 (en) * | 2021-06-25 | 2022-12-29 | 深圳市优必选科技股份有限公司 | Robot pose determination method and apparatus, robot and storage medium |
| CN116125447A (en) * | 2023-02-22 | 2023-05-16 | 广州高新兴机器人有限公司 | Robot positioning recovery method, system, electronic equipment and storage medium |
-
2023
- 2023-08-29 CN CN202311098384.0A patent/CN116840820B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017152390A1 (en) * | 2016-03-09 | 2017-09-14 | 广州艾若博机器人科技有限公司 | Map construction method, and correction method and apparatus |
| US20200241112A1 (en) * | 2019-01-29 | 2020-07-30 | Ubtech Robotics Corp Ltd | Localization method and robot using the same |
| CN110675307A (en) * | 2019-08-19 | 2020-01-10 | 杭州电子科技大学 | Implementation method of 3D sparse point cloud to 2D grid map based on VSLAM |
| WO2021104497A1 (en) * | 2019-11-29 | 2021-06-03 | 广州视源电子科技股份有限公司 | Positioning method and system based on laser radar, and storage medium and processor |
| CN111077495A (en) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| US20210270612A1 (en) * | 2020-03-02 | 2021-09-02 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method, apparatus, computing device and computer-readable storage medium for positioning |
| CN113532472A (en) * | 2020-04-15 | 2021-10-22 | 北京智行者科技有限公司 | Method and system for detecting laser mapping odometer and integrated navigation positioning deviation |
| CN111578932A (en) * | 2020-05-28 | 2020-08-25 | 长沙中联重科环境产业有限公司 | Image establishing method, device, medium and equipment based on multi-line laser radar |
| CN112711012A (en) * | 2020-12-18 | 2021-04-27 | 上海蔚建科技有限公司 | Global position initialization method and system of laser radar positioning system |
| WO2022267285A1 (en) * | 2021-06-25 | 2022-12-29 | 深圳市优必选科技股份有限公司 | Robot pose determination method and apparatus, robot and storage medium |
| CN114440858A (en) * | 2022-01-25 | 2022-05-06 | 中国人民解放军总医院第一医学中心 | Mobile robot positioning loss detection method, system, equipment and storage medium |
| CN116125447A (en) * | 2023-02-22 | 2023-05-16 | 广州高新兴机器人有限公司 | Robot positioning recovery method, system, electronic equipment and storage medium |
Non-Patent Citations (2)
| Title |
|---|
| BINGQUAN SHEN ET AL.: "A novel strategy for global mapping of O-GlcNAc proteins and peptides using selective enzymatic deglycosylation, HILIC enrichment and mass spectrometry identification", TALANTA, pages 195 - 202 * |
| 王会 等: "激光雷达在无人车辆中的应用及关键技术分析", 激光与红外, vol. 48, no. 12, pages 1458 - 1467 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116840820B (en) | 2023-11-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108458715B (en) | Robot positioning initialization method based on laser map | |
| US11537142B2 (en) | Method for robot repositioning | |
| Zhang et al. | Weld line detection and tracking via spatial-temporal cascaded hidden Markov models and cross structured light | |
| CN111258320B (en) | Robot obstacle avoidance method and device, robot and readable storage medium | |
| CN110900602B (en) | Positioning recovery method and device, robot and storage medium | |
| WO2020042426A1 (en) | Method for detecting straight side by robot, and method for screening reference wall side by cleaning robot | |
| CN113432533B (en) | Robot positioning method and device, robot and storage medium | |
| CN111390439B (en) | Welding seam detection method and device, welding robot and storage medium | |
| CN111522022B (en) | Dynamic target detection method of robot based on laser radar | |
| CN111308500A (en) | Obstacle sensing method and device based on single-line laser radar and computer terminal | |
| CN116840820B (en) | Method and system for detecting 2D laser positioning loss and storage medium | |
| Li et al. | Multiple weld seam laser vision recognition method based on the IPCE algorithm | |
| CN113791617A (en) | Global path planning method based on physical dimension of fire-fighting robot | |
| CN111251335B (en) | High-precision mechanical arm collision detection method based on bounding box algorithm | |
| CN116958529A (en) | Robot remote control method, system and medium | |
| Rohde et al. | Vehicle pose estimation in cluttered urban environments using multilayer adaptive monte carlo localization | |
| CN116300876A (en) | Multi-agent unknown environment autonomous collaborative exploration method, system, device and storage medium | |
| CN117548824B (en) | Laser remote welding method for optical precision measurement robot | |
| CN116503409B (en) | Weld defect 3D point cloud detection method, equipment and medium | |
| CN114781073B (en) | Finite element grid geometric cleaning method, device and medium based on normal vector | |
| CN116699577B (en) | 2D laser positioning quality evaluation method and system and storage medium | |
| CN115508804A (en) | Wall surface detection method, robot and storage medium | |
| CN114596293B (en) | Method for operating workpiece based on AGV and AGV system | |
| CN116513168B (en) | Path planning method and device, electronic equipment and storage medium | |
| CN117367447A (en) | Navigation method, navigation device, electronic equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |