CN117606481A - Mirror detection method and device based on laser intensity - Google Patents
Mirror detection method and device based on laser intensity Download PDFInfo
- Publication number
- CN117606481A CN117606481A CN202311554011.XA CN202311554011A CN117606481A CN 117606481 A CN117606481 A CN 117606481A CN 202311554011 A CN202311554011 A CN 202311554011A CN 117606481 A CN117606481 A CN 117606481A
- Authority
- CN
- China
- Prior art keywords
- mirror
- radar data
- mirror surface
- radar
- initial
- 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.)
- Pending
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 26
- 238000000034 method Methods 0.000 claims abstract description 32
- 238000013507 mapping Methods 0.000 claims abstract description 19
- 238000012545 processing Methods 0.000 claims abstract description 19
- 238000010276 construction Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000005259 measurement Methods 0.000 claims description 8
- 238000012216 screening Methods 0.000 claims description 6
- 238000013480 data collection Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 2
- 239000000203 mixture Substances 0.000 claims description 2
- 239000002245 particle Substances 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000005457 optimization Methods 0.000 abstract description 10
- 238000010586 diagram Methods 0.000 description 6
- 239000011521 glass Substances 0.000 description 6
- 239000000463 material Substances 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Automation & Control Theory (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Analysis (AREA)
- Theoretical Computer Science (AREA)
- General Engineering & Computer Science (AREA)
- Software Systems (AREA)
- Databases & Information Systems (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention provides a mirror detection method and device based on laser intensity, wherein the method comprises the following steps: after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, initial radar data of the environment to be mapped are obtained, and the pose data of the robot are updated based on a preset pose updating algorithm; judging whether a mirror surface exists in the environment at the moment according to the initial radar data, if the mirror surface exists in the environment, performing point cloud processing on the initial radar data to extract a mirror surface model, and if the mirror surface does not exist in the environment, directly performing grid construction based on the initial radar data; updating a radar coordinate mirror model according to the robot pose data; and setting the occupation area of specular reflection output as unknown according to the specular model, and then carrying out grid mapping. The invention can solve the technical problems of limited detection field and easy false detection of other objects in the environment in the traditional mirror surface identification and mapping optimization method.
Description
Technical Field
The invention belongs to the technical field of laser navigation, and particularly relates to a mirror detection method and system based on laser intensity.
Background
The laser radar mapping is an advanced mapping technical scheme applied to service, industrial, medical and rescue robots at present. Although the laser radar loaded by the robot has better mapping effect in a conventional environment, the finally constructed map is easily interfered by high-reflectivity objects such as glass, a shiny or semitransparent surface and a mirror. Highly reflective material surfaces such as glass, shiny or translucent surfaces and mirrors can cause the laser beam emitted by the lidar to return erroneous distance information, which can prevent such highly reflective materials from being properly built in the map.
In the prior art, mirror identification and map construction optimization are mainly performed in two technical directions: multi-sensor methods and single-sensor methods. The multi-sensor method is that a laser radar and a sonar sensor are fused, and the sonar sensor can make up part of defects of the laser radar when the laser radar is used for high-reflection materials. The single sensor method is to search an edge mirror frame of a mirror surface by using a three-dimensional radar, and then perform mapping optimization according to the characteristics of the mirror surface.
However, the two methods have some disadvantages, for example, when the laser radar and the sonar sensor are used together, the single-line sonar sensor is used, the laser radar can accurately measure the visual field of the glass or the mirror surface, and in addition, the sonar sensor can be used for detecting other substances in the environment by mistake. On the other hand, when a three-dimensional radar is used to find a glass or mirror, it is assumed that each glass or mirror has an edge mirror frame, and this assumption may result in the glass or mirror from the floor to the ceiling, etc. not being detected and identified because they do not have an edge mirror frame.
Disclosure of Invention
In order to solve the problems, the invention provides a mirror detection method and a mirror detection device based on laser intensity, which can perform processing modeling by utilizing radar data collected by a two-dimensional laser radar and perform grid mapping according to the obtained mirror model.
In a first aspect, an embodiment of the present invention provides a method for detecting a mirror surface based on laser intensity, including the steps of:
s1: after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, initial radar data of the environment to be mapped are obtained, and the pose data of the robot are updated based on a preset pose updating algorithm;
s2: judging whether a mirror surface exists in the environment at the moment according to the initial radar data, and if the mirror surface exists in the environment, performing point cloud processing on the initial radar data to extract a mirror surface model; if no mirror surface exists in the environment, directly carrying out grid mapping based on the initial radar data;
s3: updating a radar coordinate mirror model according to the robot pose data;
s4: and setting the occupation area of specular reflection output as unknown according to the specular model, and then carrying out grid mapping.
In a second aspect, the present application further proposes a mirror detection device based on laser intensity, the device specifically including:
the data collection module is used for obtaining initial radar data of the environment to be mapped after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, and updating the pose data of the robot based on a preset pose updating algorithm;
the model construction module is used for judging whether a mirror surface exists in the environment at the moment according to the initial radar data, and analyzing and processing the initial radar data to extract a mirror surface model if the mirror surface exists in the environment; if no mirror surface exists in the environment, directly carrying out grid mapping based on the initial radar data;
the model updating module is used for updating the mirror surface model according to the robot pose data;
and the image building module is used for performing grid image building after setting the occupied area of specular reflection output as unknown according to the specular model.
Through the application, the following steps are adopted: after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, initial radar data of the environment to be mapped are obtained, and the pose data of the robot are updated based on a preset pose updating algorithm; judging whether a mirror surface exists in the environment at the moment according to the initial radar data, and if the mirror surface exists in the environment, performing point cloud processing on the initial radar data to extract a mirror surface model; if no mirror surface exists in the environment, directly carrying out grid mapping based on the initial radar data; updating a radar coordinate mirror model according to the robot pose data; and setting the occupation area of specular reflection output as unknown according to the specular model, and then carrying out grid mapping. The technical problems that the detection field of view is limited and other objects in the environment are easy to be detected by mistake in the traditional mirror face identification and mapping optimization method are solved. The point cloud and grid map construction is optimized in the moving process of the robot, so that the influence of the mirror on the navigation of the mobile robot is reduced, the safety of a navigation system is improved, and the navigation capacity and response speed of the mobile robot are also greatly improved.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application, illustrate and explain the application and are not to be construed as limiting the application.
FIG. 1 is a schematic flow chart of a mirror detection method based on laser intensity according to an embodiment of the present application;
FIG. 2 is a schematic flow chart of obtaining a mirror model according to an embodiment of the present application;
FIG. 3 is a schematic diagram of a mirror detection device based on laser intensity according to an embodiment of the present application;
fig. 4 is a photograph of a site environment of a laser radar detection map to be built according to an embodiment of the present application;
FIG. 5 is a grid map optimization effect diagram after mirror recognition and map construction optimization of the field environment shown in FIG. 4, according to an embodiment of the present application, where the left side is an effect diagram before optimization and the right side is an effect diagram after optimization;
FIG. 6 is point cloud data prior to optimization using mirror identification provided in accordance with an embodiment of the present application;
fig. 7 is optimized point cloud data using mirror identification according to an embodiment of the present application.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are only some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, are intended to fall within the scope of the present invention.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this disclosure belongs. Although any methods and materials similar or equivalent to those described herein can also be used in the practice or testing of the present disclosure, the exemplary methods and materials are now described.
It should be understood that the terms "comprises" and "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It is also to be understood that the terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
It should be further understood that the term "and/or" as used in the present specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
Referring to fig. 1, fig. 1 is a schematic diagram of a mirror detection method based on laser intensity according to an embodiment of the present invention, where mirror detection and mapping optimization within a certain spatial range are illustrated. In this embodiment, the mirror detection method based on the laser intensity includes the steps of:
s1: after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, initial radar data of the environment to be mapped are obtained, and the pose data of the robot are updated based on a preset pose updating algorithm;
s2: judging whether a mirror surface exists in the environment at the moment according to the initial radar data, and if the mirror surface exists in the environment, performing point cloud processing on the initial radar data to extract a mirror surface model; if no mirror surface exists in the environment, directly carrying out grid mapping based on the initial radar data;
s3: updating a radar coordinate mirror model according to the robot pose data;
s4: and setting the occupation area of specular reflection output as unknown according to the specular model, and then carrying out grid mapping.
Preferably, the preset pose updating algorithm in step S1 shown in fig. 1 adopts a Gmapping open source Slam algorithm based on an RBPF particle filter algorithm.
Specifically, as shown in fig. 2, in step S2, whether a mirror exists in the environment at this time is determined according to the initial radar data, and if the mirror exists in the environment, the point cloud processing is performed on the initial radar data to extract a mirror model, and specifically the method further includes the following steps:
s21: setting a laser intensity threshold T, traversing all initial radar data, and when the data characteristics of the laser intensity larger than the laser intensity threshold T appear in the initial radar data, proving that a mirror surface exists in the environment at the moment, and acquiring radar point cloud data meeting the data characteristics;
s22: carrying out planarization assumption on the mirror surface, and calculating to obtain a linear equation where the mirror surface is positioned by using the radar point cloud data at the moment t;
s23: traversing all initial radar data at the time t, screening and obtaining a mirror plane straight line symmetrical point set according to a preset symmetrical judgment condition, and storing the intersection point of the symmetrical point farthest from the origin of the radar coordinate system and the line connecting the origin of the radar coordinate system with the mirror plane straight line in a point queue to be processed.
The symmetry point farthest from the point is s i,t Calculating the origin p of the point and the radar coordinate system 0 The line (slope is k) i ) Intersection point of straight line with mirror surfaceThe formula is as follows:
if the initial radar data cannot obtain a corresponding mirror plane straight line symmetry point set through screening, directly carrying out grid construction;
s24: according to the intersection point data stored in the point queue to be processed, traversing and calculating the distance between every two continuous intersection points, judging that the two intersection points are on the same line segment when the distance between the two selected continuous intersection points is smaller than a preset line segment judgment threshold D, and finally taking the intersection point line segment with the longest length as a mirror surface line segment;
s25: and processing the mirror surface line segment to obtain a mirror surface model.
Preferably, the preset line segment judgment threshold D is set according to the actually used angular resolution of the lidar and the required size of the map, and in this embodiment, the preset line segment judgment threshold D is set as follows: 0.015m.
Specifically, the preset symmetry judgment condition in step S23 specifically further includes the following steps:
s231: calculating two radar data points s which are not symmetrically judged in all the initial radar data at the moment t i,t Sum s j,t Distance d to mirror line i,t And d j,t The radar data points s i,t Sum s j,t The distance from the radar data point to the mirror surface straight line does not take absolute value, and the slope k of the straight line where the connecting line of the radar data points is located is obtained after the two radar data points are connected c The formula involved is as follows:
θ i ＝θ+iΔθ
X i,t ＝R i,t ·cos(θ i )
Y i,t ＝R i,t ·sin(θ i )
where i is the index of the laser beam of the eligible radar data points, [ X ] i,t ,Y i,t ]For time t the frame of radar data points s i,t Cartesian coordinates, [ X ] j,t ,Y j,t ]For time t the frame of radar data points s j,t Cartesian coordinates of R i,t For radar data point distance measurements, θ i For the angle at which the radar data point is located, θ is the initial angle of the initial radar data, Δθ is the angular resolution of the initial radar data, then there is
b M ＝Y i,t -X i,t ·k M
Wherein the origin of the radar coordinate is p 0 P in radar coordinate system 0 To s i,t The linear equation of (2) is y=k i x, the straight line of the mirror surface is y=k M x+b M Then has
S232: setting a minimum slope threshold k min And a highest slope threshold k max If k M ·k c >k min ||k M ·k c <k max If not, the symmetry judgment of the two radar data points at the current stage is skipped, and the symmetry judgment of the two radar data points which are not subjected to the symmetry judgment in the initial radar data at the next group t is continued;
s233: if d i,t ·d j,t >0, judging that the two radar data points are not symmetrical points when the two radar data points are on the same side of the mirror surface, and continuing to perform symmetrical judgment of the two radar data points which are not subjected to symmetrical judgment in all the initial radar data at the next group of t moments by skipping the symmetrical judgment of the current two radar data points;
s234: setting a threshold d min If |d j,t |<d min |||d i,t |<d min Judging that the two radar data points are positioned on the straight line of the mirror surface, judging that the current two radar data points are not symmetrical points, and continuing to carry out symmetrical judgment of the two radar data points which are not subjected to symmetrical judgment in all the initial radar data at the next group of t moments by skipping the symmetrical judgment of the current two radar data points;
s235: and determining a pair of radar data points which can pass through the screening process as a group of symmetrical points, putting the symmetrical points into the mirror plane straight line symmetrical point set, and then continuing to carry out symmetrical judgment on two radar data points which are not subjected to symmetrical judgment in all initial radar data at the next group of t moments until all the initial radar data are subjected to one round of symmetrical judgment.
Specifically, in step S25, the processing the mirror line segment to obtain a mirror model further includes: the method comprises the steps of storing the angle and distance mixture from the nearest point of a mirror surface straight line to a radar coordinate origin to the robot coordinate origin as a mean vector, calculating a covariance matrix through two straight line parameters and two end point parameters, and independently storing the head and tail coordinates of a mirror surface line segment, wherein the related formulas are as follows:
wherein the method comprises the steps ofFor the x and y coordinates of the point of the mirror line closest to the radar origin of coordinates,/for the point of the mirror line closest to the radar origin of coordinates>Is the angle of the nearest point of the mirror line from the radar origin to the origin of the robot coordinate system, +.>Is the distance from the nearest point of the mirror line to the radar origin of coordinates to the robot origin of coordinates, +.>Is the mean vector of the mirror line segment relative to the robot radar coordinate system,>is the covariance matrix of the specular prediction, σ α Sum sigma ρ The measurement noise of the covariance matrix angle and the distance is delta x, delta y and delta theta, and the registration result of the ICP algorithm in the Gapplying open source Slam algorithm is shown. Preferably, the covariance matrix angle versus distanceThe measurement noise may be set to: sigma (sigma) α 2-3 degrees and sigma ρ 0.04m to 0.1m.
Specifically, in step S3 shown in fig. 1, updating the mirror model according to the pose data of the robot further includes the following steps:
s31: based on the robot pose data [ x ] t ,y t ,θ t ]The acquired mirror model is converted into a world coordinate system, and a standard EKF is used for updating a mirror mean vector and a covariance matrix, and the related formulas are as follows:
wherein,and->Is jacobian matrix of the mirror surface model relative to the pose information and the linear measurement value of the robot, P t Is the covariance matrix of the robot pose, < +.>Is a specular mean vector, ">Is the angle from the nearest point of the mirror line to the radar origin to the origin of the robot coordinate system.
S32: and updating the mirror line segment based on the updated mirror mean vector and covariance matrix.
Specifically, in step S32, updating the mirror line segment based on the updated mirror mean vector and covariance matrix further includes the following specific steps:
s321: acquiring updated calibration mirror surface straight lines based on the updated mirror surface mean value vector and covariance matrix, and acquiring two updated actual mirror surface straight lines and an actual mirror surface straight line corresponding point queue generated by detecting the same mirror surface straight line by two adjacent frames in the radar detection process;
s322: respectively selecting a to-be-selected endpoint on the calibration mirror surface straight line, which is closest to all data point distances in a point array corresponding to the actual mirror surface straight line, at two sides of an intersection point of the actual mirror surface straight line and the calibration mirror surface straight line, and performing the same processing on the other actual mirror surface straight line to finally obtain a to-be-selected endpoint set on the calibration mirror surface straight line;
s323: and selecting two points with the farthest distance from the to-be-selected end point set as two mirror line segment end points on the calibration mirror straight line to obtain an updated mirror line segment, wherein the related formula is as follows:
wherein,is the head and tail coordinates of the updated mirror line segment, p 1 ,p 2 Calibrating the point on the mirror straight line after updating;
s324: transforming the head and tail coordinates of the mirror line segment from a world coordinate system to a radar coordinate system, calculating the intersection point of the laser beam and the mirror line segment through an angle index in the initial radar data, and eliminating other points which cannot be intersected in the initial radar data so as to effectively reduce the calculated amount, wherein the related formula is as follows:
n min ＝min(n 1 ,n 2 )
n max ＝max(n 1 ,n 2 )
wherein [ X ] start ,Y start ],[X end ,Y end ]Is the two-dimensional coordinates of the head and the tail of the mirror line segment, theta is the initial angle of the initial radar data, delta theta is the angular resolution of the initial radar data, and n min ,n max Is the index of the head and tail coordinates of the mirror line segment in the initial radar data, n 1 And n 2 No physical meaning is provided for the intermediate mathematical variables in the calculation process;
s325: judging whether an intersection point exists between the laser and the mirror line segment in an initial radar data index corresponding to the head-tail coordinates of the mirror line segment, if so, modifying the radar data into the intersection point coordinates, if so, selecting the intersection point closest to the origin of the radar coordinates, and if not, not processing.
Fig. 3 is a schematic block diagram of a mirror detection device based on laser intensity according to an embodiment of the present invention. As shown in fig. 3, the present invention also provides a device for detecting mirror optimized point cloud data and grid mapping based on laser intensity information, corresponding to the above method for detecting a mirror based on laser intensity. The device comprises: the system comprises a data collection module, a model construction module, a model updating module and a mapping module.
The data collection module is used for obtaining initial radar data of the environment to be mapped after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, and updating the pose data of the robot based on a preset pose updating algorithm.
And the model construction module is used for judging whether a mirror surface exists in the environment at the moment according to the initial radar data, analyzing and processing the initial radar data to extract a mirror surface model if the mirror surface exists in the environment, and directly carrying out grid construction on the basis of the initial radar data if the mirror surface does not exist in the environment.
And the model updating module is used for updating the mirror model according to the robot pose data.
And the image building module is used for performing grid image building after setting the occupied area of specular reflection output as unknown according to the specular model.
Those of ordinary skill in the art will appreciate that the various illustrative modules and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of functionality in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the system embodiments described above are merely illustrative. For example, each module is divided into only one logic function, and there may be another division manner in actual implementation. For example, multiple units or components may be combined or may be integrated into another system, or some features may be omitted, or not performed.
The steps in the method of the embodiment of the invention can be sequentially adjusted, combined and deleted according to actual needs. The modules in the system of the embodiment of the invention can be combined, divided and deleted according to actual needs. In addition, each functional module in the embodiment of the present invention may be integrated in one processing module, or each module may exist alone physically, or two or more modules may be integrated in one module.
The foregoing describes in detail preferred embodiments of the present invention. It should be understood that numerous modifications and variations can be made in accordance with the concepts of the invention without requiring creative effort by one of ordinary skill in the art. Therefore, all technical solutions which can be obtained by logic analysis, reasoning or limited experiments based on the prior art by a person skilled in the art according to the inventive concept shall be within the scope of protection defined by the claims.
Claims (10)
1. The mirror detection method based on the laser intensity is characterized by comprising the following steps:
s1: after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, initial radar data of the environment to be mapped are obtained, and the pose data of the robot are updated in a preset pose updating algorithm;
s2: judging whether a mirror surface exists in the environment at the moment according to the initial radar data, if the mirror surface exists in the environment, performing point cloud processing on the initial radar data to extract a mirror surface model, and if the mirror surface does not exist in the environment, directly performing grid construction based on the initial radar data;
s3: updating a radar coordinate mirror model according to the robot pose data;
s4: and setting the occupation area of specular reflection output as unknown according to the specular model, and then carrying out grid mapping.
2. The laser intensity-based mirror detection method according to claim 1, wherein the preset pose updating algorithm comprises: the Gmapping open source Slam algorithm based on RBPF particle filter algorithm is used.
3. The method according to claim 1, wherein the determining whether a mirror exists in the environment at this time based on the initial radar data, and if the mirror exists in the environment, performing a point cloud process on the initial radar data to extract a mirror model, comprises:
s21: setting a laser intensity threshold T, traversing all initial radar data, and when the data characteristics of the laser intensity larger than the laser intensity threshold T appear in the initial radar data, proving that a mirror surface exists in the environment at the moment, and acquiring radar point cloud data meeting the data characteristics;
s22: carrying out planarization assumption on the mirror surface, and calculating to obtain a linear equation where the mirror surface is positioned by using the radar point cloud data at the moment t;
s23: traversing all initial radar data at the time t, screening and obtaining a mirror plane straight line symmetric point set according to preset symmetric judgment conditions, storing intersection points of symmetric points farthest from the origin of a radar coordinate system and a mirror plane straight line, which are connected with the origin of the radar coordinate system, in a point queue to be processed, and directly carrying out grid construction if the initial radar data cannot obtain a corresponding mirror plane straight line symmetric point set after screening;
s24: according to the intersection point data stored in the point queue to be processed, traversing and calculating the distance between every two continuous intersection points, judging that the two intersection points are on the same line segment when the distance between the two selected continuous intersection points is smaller than a preset line segment judgment threshold D, and finally taking the intersection point line segment with the longest length as a mirror surface line segment;
s25: and processing the mirror surface line segment to obtain a mirror surface model.
4. A laser intensity-based mirror detection method according to claim 3, wherein the preset line segment judgment threshold D is set according to the angular resolution of the laser radar used and the size of the map to be built, and is set as follows: 0.015m.
5. A method for detecting a mirror surface based on laser intensity according to claim 3, wherein the preset symmetry judgment conditions include:
s231: calculating two radar data points s which are not symmetrically judged in all the initial radar data at the moment t i,t Sum s j,t Distance d to mirror line i,t And d j,t The radar data points s i,t Sum s j,t The distance from the radar data point to the mirror surface straight line does not take absolute value, and the slope k of the straight line where the connecting line of the radar data points is located is obtained after the two radar data points are connected c The formula involved is as follows:
θ i ＝θ+iΔθ
X i,t ＝R i,t ·cos(θ i )
Y i,t ＝R i,t ·sin(θ i )
where i is the index of the laser beam of the eligible radar data points, [ X ] i,t ,Y i,t ]For time t the frame of radar data points s i,t Cartesian coordinates, [ X ] j,t ,Y j,t ]For time t the frame of radar data points s k,t Cartesian coordinates of R i,t For radar data point distance measurements, θ i For the angle at which the radar data point is located, θ is the initial angle of the initial radar data, Δθ is the angular resolution of the initial radar data, then there is
b M ＝Y i,t -X i,t ·k M
Wherein the origin of the radar coordinate is p 0 P in radar coordinate system 0 To s i,t The linear equation of (2) is y=k i x, the straight line of the mirror surface is y=k M x+b M Then has
S232: setting a minimum slope threshold k min And a highest slope threshold k max If k M ·k c >k min ||k M ·k c <k max If the current two radar data points are not symmetrical points, the symmetry judgment of the current two radar data points is skipped, and the process is continuedThe symmetry judgment of two radar data points which are not subjected to symmetry judgment in all the initial radar data at the next group of t moments is performed;
s233: if d i,t ·d j,t >0, judging that the two radar data points are not symmetrical points when the two radar data points are on the same side of the mirror surface, and continuing to perform symmetrical judgment of the two radar data points which are not subjected to symmetrical judgment in all the initial radar data at the next group of t moments by skipping the symmetrical judgment of the current two radar data points;
s234: setting a threshold d min If |d j,t |<d min |||d i,t |<d min Judging that the two radar data points are positioned on the straight line of the mirror surface, judging that the current two radar data points are not symmetrical points, and continuing to carry out symmetrical judgment of the two radar data points which are not subjected to symmetrical judgment in all the initial radar data at the next group of t moments by skipping the symmetrical judgment of the current two radar data points;
s235: and determining a pair of radar data points which can pass through the screening process as a group of symmetrical points, putting the symmetrical points into the mirror plane straight line symmetrical point set, and then continuing to carry out symmetrical judgment on two radar data points which are not subjected to symmetrical judgment in all initial radar data at the next group of t moments until all the initial radar data are subjected to one round of symmetrical judgment.
6. A method of mirror detection based on laser intensity according to claim 3, wherein said processing said mirror segments to obtain a mirror model comprises: the method comprises the steps of storing the angle and distance mixture from the nearest point of a mirror surface straight line to a radar coordinate origin to the robot coordinate origin as a mean vector, calculating a covariance matrix through two straight line parameters and two end point parameters, and independently storing the head and tail coordinates of a mirror surface line segment, wherein the related formulas are as follows:
wherein the method comprises the steps ofFor the x and y coordinates of the point of the mirror line closest to the radar origin of coordinates,/for the point of the mirror line closest to the radar origin of coordinates>Is the angle of the nearest point of the mirror line from the radar origin to the origin of the robot coordinate system, +.>Is the distance from the nearest point of the mirror line to the radar origin of coordinates to the robot origin of coordinates, +.>Is the mean vector of the mirror line segment relative to the robot radar coordinate system,is the covariance matrix of the specular prediction, σ α Sum sigma ρ The measurement noise of the covariance matrix angle and the distance is delta x, delta y and delta theta, and the registration result of the ICP algorithm in the Gapplying open source Slam algorithm is shown.
7. The laser intensity-based mirror detection method according to claim 5, wherein the covariance matrix angle-to-distance measurement noise is set as: sigma (sigma) α 2-3 degrees and sigma ρ 0.04m to 0.1m.
8. The laser intensity-based mirror detection method according to claim 1, wherein the updating the mirror model based on the robot pose data comprises:
s31: based on the instituteRobot pose data [ x ] t ,y t ,θ t ]The acquired mirror model is converted into a world coordinate system, and a standard EKF is used for updating a mirror mean vector and a covariance matrix, and the related formulas are as follows:
wherein,and->Is jacobian matrix of the mirror surface model relative to the pose information and the linear measurement value of the robot, P t Is the covariance matrix of the robot pose, < +.>Is a specular mean vector, ">The angle from the nearest point of the mirror surface straight line to the radar coordinate origin to the robot coordinate system origin;
s32: and updating the mirror line segment based on the updated mirror mean vector and covariance matrix.
9. The method of claim 8, wherein updating the mirror line segment based on the updated mirror mean vector and covariance matrix comprises:
s321: acquiring updated calibration mirror surface straight lines based on the updated mirror surface mean value vector and covariance matrix, and acquiring two updated actual mirror surface straight lines and an actual mirror surface straight line corresponding point queue generated by detecting the same mirror surface straight line by two adjacent frames in the radar detection process;
s322: respectively selecting a to-be-selected endpoint on the calibration mirror surface straight line, which is closest to all data point distances in a point array corresponding to the actual mirror surface straight line, at two sides of an intersection point of the actual mirror surface straight line and the calibration mirror surface straight line, and performing the same processing on the other actual mirror surface straight line to finally obtain a to-be-selected endpoint set on the calibration mirror surface straight line;
s323: and selecting two points with the farthest distance from the to-be-selected end point set as two mirror line segment end points on the calibration mirror straight line to obtain an updated mirror line segment, wherein the related formula is as follows:
wherein,is the head and tail coordinates of the updated mirror line segment, p 1 ,p 2 Calibrating the point on the mirror straight line after updating;
s324: transforming the head and tail coordinates of the mirror line segment from a world coordinate system to a radar coordinate system, calculating the intersection point of the laser beam and the mirror line segment through an angle index in the initial radar data, and excluding other points which cannot be intersected in the initial radar data, wherein the related formula is as follows:
n min ＝min(n 1 ,n 2 )
n max ＝max(n 1 ,n 2 )
wherein [ X ] start ,Y start ],[X end ,Y end ]Is the two-dimensional coordinates of the head and the tail of the mirror line segment, theta is the initial angle of the initial radar data, delta theta is the angular resolution of the initial radar data, and n min ,n max Is the index of the head and tail coordinates of the mirror line segment in the initial radar data, n 1 And n 2 No physical meaning is provided for the intermediate mathematical variables in the calculation process;
s325: judging whether an intersection point exists between the laser and the mirror line segment in an initial radar data index corresponding to the head-tail coordinates of the mirror line segment, if so, modifying the radar data into the intersection point coordinates, if so, selecting the intersection point closest to the origin of the radar coordinates, and if not, not processing.
10. A laser intensity-based mirror detection apparatus, comprising:
the data collection module is used for obtaining initial radar data of the environment to be mapped after the two-dimensional laser radar emits laser beams to irradiate the surrounding environment, and updating the pose data of the robot based on a preset pose updating algorithm;
the model construction module is used for judging whether a mirror surface exists in the environment at the moment according to the initial radar data, analyzing and processing the initial radar data to extract a mirror surface model if the mirror surface exists in the environment, and directly carrying out grid construction on the basis of the initial radar data if the mirror surface does not exist in the environment;
the model updating module is used for updating the mirror model according to the robot pose data;
and the image building module is used for performing grid image building after setting the occupied area of specular reflection output as unknown according to the specular model.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311554011.XA CN117606481A (en) | 2023-11-20 | 2023-11-20 | Mirror detection method and device based on laser intensity |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311554011.XA CN117606481A (en) | 2023-11-20 | 2023-11-20 | Mirror detection method and device based on laser intensity |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117606481A true CN117606481A (en) | 2024-02-27 |
Family
ID=89948863
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311554011.XA Pending CN117606481A (en) | 2023-11-20 | 2023-11-20 | Mirror detection method and device based on laser intensity |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117606481A (en) |
-
2023
- 2023-11-20 CN CN202311554011.XA patent/CN117606481A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102210715B1 (en) | Method, apparatus and device for determining lane lines in road | |
| CN108152831B (en) | Laser radar obstacle identification method and system | |
| US10024965B2 (en) | Generating 3-dimensional maps of a scene using passive and active measurements | |
| CN103424112B (en) | A kind of motion carrier vision navigation method auxiliary based on laser plane | |
| Zhou et al. | T-LOAM: Truncated least squares LiDAR-only odometry and mapping in real time | |
| US20200372664A1 (en) | Vehicle positioning method and system based on laser device | |
| CN110503040B (en) | Obstacle detection method and device | |
| CN112513679B (en) | Target identification method and device | |
| KR101964100B1 (en) | Object detection apparatus based on neural network learning and method of the same | |
| CN108303096B (en) | Vision-assisted laser positioning system and method | |
| KR20180041176A (en) | METHOD, DEVICE, STORAGE MEDIUM AND DEVICE | |
| JP6906471B2 (en) | Target information estimation device, program and method for estimating the direction of a target from a point cloud | |
| CN113378760A (en) | Training target detection model and method and device for detecting target | |
| KR101804681B1 (en) | A human detecting apparatus and method using a low-resolution 2d lidar sensor | |
| JP2008309671A (en) | Object recognition method and device | |
| CN113902782A (en) | Rapid registration method and system for three-dimensional point cloud of obstacles around excavator | |
| CN108873014A (en) | Mirror surface detection method and device based on laser radar | |
| JP2024133190A (en) | Self-location estimation device | |
| JP2024056032A (en) | Data structure, storage medium, and storage device | |
| CN118463965A (en) | Positioning accuracy evaluation method and device and vehicle | |
| CN112699748B (en) | Human-vehicle distance estimation method based on YOLO and RGB image | |
| EP3088983A1 (en) | Moving object controller, landmark, and program | |
| CN111542828A (en) | Line recognition method, line recognition device, line recognition system, and computer storage medium | |
| JP2023164502A (en) | Stationary object data generator, method for control, program, and storage medium | |
| Dekan et al. | Versatile approach to probabilistic modeling of Hokuyo UTM-30LX |
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 |