CN118149796B - Map construction method and device under degradation environment, electronic equipment and storage medium - Google Patents
Map construction method and device under degradation environment, electronic equipment and storage medium Download PDFInfo
- Publication number
- CN118149796B CN118149796B CN202410566387.0A CN202410566387A CN118149796B CN 118149796 B CN118149796 B CN 118149796B CN 202410566387 A CN202410566387 A CN 202410566387A CN 118149796 B CN118149796 B CN 118149796B
- Authority
- CN
- China
- Prior art keywords
- laser
- pose
- degradation
- map
- factor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 230000015556 catabolic process Effects 0.000 title claims abstract description 155
- 238000006731 degradation reaction Methods 0.000 title claims abstract description 155
- 238000010276 construction Methods 0.000 title claims abstract description 36
- 238000000034 method Methods 0.000 claims abstract description 57
- 230000010354 integration Effects 0.000 claims abstract description 44
- 238000001514 detection method Methods 0.000 claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 17
- 230000006870 function Effects 0.000 claims description 40
- 239000011159 matrix material Substances 0.000 claims description 34
- 238000005457 optimization Methods 0.000 claims description 34
- 238000012545 processing Methods 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 12
- 230000004927 fusion Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 description 32
- 238000004364 calculation method Methods 0.000 description 15
- 238000004891 communication Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 230000011664 signaling Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
The embodiment of the application provides a map construction method, a map construction device, electronic equipment and a storage medium in a degradation environment, wherein the method comprises the following steps: acquiring an intensity map generated by laser point cloud data and sensor data of inertial measurement equipment; performing geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose; obtaining laser intensity pose according to the intensity map; fusing the laser geometric pose and the laser intensity pose to obtain a fused pose; obtaining a pre-integration factor according to sensor data of the inertial measurement device; and optimizing the fused pose and the pre-integration factor to obtain map data. By implementing the embodiment of the application, the accurate construction of the map can be realized in the degradation environment, the interference of surrounding environment factors is avoided, the fault tolerance is high, the error is small, and the accuracy of the map can be improved.
Description
Technical Field
The application relates to the technical field of unmanned positioning and mapping, in particular to a map construction method, a map construction device, electronic equipment and a storage medium in a degradation environment.
Background
The real-time map construction technology is widely applied to the fields of automatic driving, unmanned aerial vehicle, robot navigation and the like at present, the technology can help equipment to determine the self position and the topography and barriers of the surrounding environment, and the application of the technology can generate extremely high economic benefits in ports, mines and working parks.
In view of the great economic value and social value that map construction technology can produce in numerous industries, the algorithm of the prior art has better scene effect with obvious characteristics, however, in degradation environment, such as entrance corridor, open field, i.e. the place with few surrounding environment references, unobvious characteristics and low brightness, there are many problems, for example, the degradation environment can cause the sensor to be in a failure state, if only one sensor is used for processing degradation problem, the error is larger; the geometric features are obvious in degradation scene failure; the error rate of degradation failure detection is higher only once, and layered detection and optimization can improve the track accuracy and the like, which can cause interference to the map construction accuracy, and cause map construction failure or low accuracy.
Disclosure of Invention
The embodiment of the application aims to provide a map construction method, a device, electronic equipment and a storage medium in a degradation environment, which can realize accurate construction of a map in the degradation environment, cannot be interfered by surrounding environment factors, has high fault tolerance and small error, and can improve the accuracy of the map.
In a first aspect, an embodiment of the present application provides a map construction method in a degradation environment, where the method includes:
acquiring an intensity map generated by laser point cloud data and sensor data of inertial measurement equipment;
Performing geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose;
Obtaining laser intensity pose according to the intensity map;
fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
Obtaining a pre-integration factor according to sensor data of the inertial measurement device;
And optimizing the fused pose and the pre-integration factor to obtain map data.
In the implementation process, the laser geometric pose is obtained through degradation detection of the laser points, then the laser geometric pose and the laser intensity pose are fused, and optimization is performed based on the pre-integration factor, so that accurate construction of the map can be realized in a degradation environment, interference of surrounding environment factors is avoided, the fault tolerance is high, the error is small, and the accuracy of the map can be improved.
Further, the step of performing geometric feature degradation detection on the laser points in the intensity graph to obtain the geometric pose of the laser includes:
obtaining a first residual error of the laser point;
Nonlinear optimization is carried out on the first residual error, and a homography matrix is obtained;
Obtaining a degradation characteristic threshold according to the homography matrix;
And obtaining the laser geometric pose according to the degradation characteristic threshold.
In the implementation process, the homography matrix is obtained according to the first residual error, and then the laser geometric pose is obtained according to the degradation characteristic threshold value, so that the laser geometric pose of the laser point can be simply and effectively extracted, the data processing flow is simplified, and the error is reduced.
Further, the step of obtaining the degradation characteristic threshold value according to the homography matrix includes:
constructing a hessian matrix according to the homography matrix to obtain a degradation characteristic vector;
constructing a degradation characteristic function according to the degradation characteristic vector;
And obtaining the degradation characteristic threshold according to the degradation characteristic function.
In the implementation process, the Heisen matrix is constructed according to the homography matrix to obtain the degradation characteristic vector, so that pose errors caused by the degradation environment can be avoided, and the anti-interference capability of the laser odometer on the degradation environment is improved.
Further, the step of obtaining the laser geometric pose according to the degradation characteristic threshold value includes:
judging whether the degradation characteristic threshold is larger than a first threshold or not;
and if not, carrying out nonlinear optimization on the degradation characteristic function to obtain the laser geometric pose.
In the implementation process, the degradation characteristic threshold value is compared with the first threshold value, and nonlinear optimization is performed on the degradation function, so that the geometric characteristics of the laser point can be accurately and rapidly obtained, and the accuracy is improved.
Further, the step of obtaining the first residual of the laser spot includes:
obtaining geometric characteristic information of the laser point cloud data;
Constructing a smoothness function according to the geometric feature information;
Obtaining the distance difference between any two laser points according to the smoothness function;
And judging the characteristics of the laser points according to the distance difference, obtaining the first residual error according to a plane point residual error function if the characteristics of the laser points are plane points, and obtaining the first residual error according to an edge point residual error function if the characteristics of the laser points are edge points.
In the implementation process, the characteristics of the laser points are judged according to the distance difference, and different residual functions are constructed according to the laser points with different characteristics, so that calculation errors can be effectively reduced.
Further, the laser geometric pose and the laser intensity pose are fused through the following formula, and the fused pose is obtained:
；
Wherein, As the pose after the fusion,For the laser geometric pose, H is the laser intensity pose,、Representing preset parameters, and the range is [0,1].
Further, the step of obtaining a pre-integration factor from sensor data of the inertial measurement device includes:
And carrying out pre-integration processing on the speed information, the position information and the rotation information in the sensor data to obtain the pre-integration factor.
In the implementation process, the pre-integration factor is obtained according to the speed information, the position information and the rotation information, so that the pre-integration factor can describe the characteristics of the sensor data in a multi-dimensional manner, and the utilization rate of the sensor data is improved.
Further, the step of optimizing the fused pose and the pre-integration factor to obtain map data includes:
Acquiring a laser odometer factor;
Comparing the laser odometer factor with the pre-integral factor to obtain a second residual error between the laser odometer factor and the pre-integral factor;
And carrying out optimization processing according to the second residual error to obtain map data.
In the implementation process, the laser odometer factor is compared with the pre-integral factor, so that the invalid laser odometer factor is screened out, the calculated amount generated in the optimization process is reduced, and the calculation efficiency and accuracy are improved.
Further, the step of optimizing according to the second residual error to obtain map data includes:
Comparing the second residual with a degradation threshold;
if the second residual error is larger than the degradation threshold, the laser radar fails in the degradation environment, a priori pose is obtained according to the pre-integration factor, and the priori pose and the laser odometer factor are optimized according to a factor graph, so that the map data are obtained;
and if the second residual error is smaller than the degradation threshold value, optimizing the pre-integration factor and the laser odometer factor according to the factor graph to obtain the map data.
In the implementation process, the second residual error is compared with the degradation threshold value, and the degradation environment detection result is fed back to the generation stage of the laser odometer factor, so that the influence of the degradation environment on pose is reduced, and the calculation error is further reduced.
In a second aspect, an embodiment of the present application further provides a map construction apparatus in a degraded environment, where the apparatus includes:
the acquisition module is used for acquiring an intensity graph generated by laser point cloud data and sensor data of the inertial measurement device;
The degradation detection module is used for carrying out geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose;
The data acquisition module is used for acquiring laser intensity pose according to the intensity map; the system is also used for obtaining a pre-integral factor according to sensor data of the inertial measurement device;
the fusion module is used for fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
and the optimization module is used for carrying out optimization processing on the fused pose and the pre-integration factor to obtain map data.
In the implementation process, the laser geometric pose is obtained through degradation detection of the laser points, then the laser geometric pose and the laser intensity pose are fused, and optimization is performed based on the pre-integration factor, so that accurate construction of the map can be realized in a degradation environment, interference of surrounding environment factors is avoided, the fault tolerance is high, the error is small, and the accuracy of the map can be improved.
In a third aspect, an electronic device provided in an embodiment of the present application includes: a memory, a processor and a computer program stored in the memory and executable on the processor, the processor implementing the steps of the method according to any one of the first aspects when the computer program is executed.
In a fourth aspect, an embodiment of the present application provides a computer readable storage medium, where instructions are stored, when the instructions are executed on a computer, to cause the computer to perform the method according to any one of the first aspects.
In a fifth aspect, embodiments of the present application provide a computer program product, which when run on a computer causes the computer to perform the method according to any of the first aspects.
Additional features and advantages of the disclosure will be set forth in the description which follows, or in part will be obvious from the description, or may be learned by practice of the techniques of the disclosure.
And can be implemented in accordance with the teachings of the specification, the following detailed description of the preferred embodiments of the application, taken in conjunction with the accompanying drawings.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and should not be construed as limiting the scope values, and other related drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flow chart of a map construction method in a degradation environment according to an embodiment of the present application;
fig. 2 is a schematic structural diagram of a map building device in a degradation environment according to an embodiment of the present application;
fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only to distinguish the description, and are not to be construed as indicating or implying relative importance.
The following describes in further detail the embodiments of the present application with reference to the drawings and examples. The following examples are illustrative of the application and are not intended to limit the scope of the application.
Example 1
Fig. 1 is a flow chart of a map construction method in a degradation environment according to an embodiment of the present application, as shown in fig. 1, where the method includes:
s1, acquiring an intensity graph generated by laser point cloud data and sensor data of inertial measurement equipment;
s2, performing geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose;
s3, obtaining laser intensity pose according to the intensity graph;
s4, fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
s5, obtaining a pre-integration factor according to sensor data of the inertial measurement device;
And S6, optimizing the fused pose and the pre-integration factor to obtain map data.
In the implementation process, the laser geometric pose is obtained through degradation detection of the laser points, then the laser geometric pose and the laser intensity pose are fused, and optimization is performed based on the pre-integration factor, so that accurate construction of the map can be realized in a degradation environment, interference of surrounding environment factors is avoided, the fault tolerance is high, the error is small, and the accuracy of the map can be improved.
In S1, performing point cloud depth projection on three-dimensional data of a laser radar, extracting an intensity map from the obtained point cloud depth map, wherein pixels in the point cloud depth map represent distance information and reflection intensity information, and the intensity information is extracted independently and corresponds to each pixel one by one to generate an intensity map.
The inertial measurement device in the embodiment of the application is an IMU, and consists of three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers detect acceleration signals of the object on the independent three axes of the carrier coordinate system, the gyroscopes detect angular velocity signals of the carrier relative to the navigation coordinate system, and after the signals are processed, the position and rotation information of the carrier can be calculated.
Further, S2 includes:
obtaining a first residual error of the laser spot;
nonlinear optimization is carried out on the first residual error, and a homography matrix is obtained;
Obtaining a degradation characteristic threshold value according to the homography matrix;
And obtaining the laser geometric pose according to the degradation characteristic threshold.
In the implementation process, the homography matrix is obtained according to the first residual error, and then the laser geometric pose is obtained according to the degradation characteristic threshold value, so that the laser geometric pose of the laser point can be simply and effectively extracted, the data processing flow is simplified, and the error is reduced.
After obtaining the residual error of the edge point and the residual error of the plane point (collectively called as a first residual error), performing nonlinear optimization solution by adopting an LM algorithm, and calculating a Heisen matrix:
Where H represents a homography matrix, T represents a transpose, and n represents an integer.
Further, the step of obtaining the degradation characteristic threshold value according to the homography matrix includes:
Constructing a hessian matrix according to the homography matrix to obtain a degradation characteristic vector;
Constructing a degradation characteristic function according to the degradation characteristic vector;
and obtaining a degradation characteristic threshold according to the degradation characteristic function.
In the implementation process, the Heisen matrix is constructed according to the homography matrix to obtain the degradation characteristic vector, so that pose errors caused by the degradation environment can be avoided, and the anti-interference capability of the laser odometer on the degradation environment is improved.
In the embodiment of the application, as the influence of the geometric texture of the degradation environment (one degradation characteristic of the degradation environment) on the translational attitude is larger, when the robot moves, if the change of the geometric information is smaller, the constraint of the geometric texture on the translation is easy to fail, so that the calculation of the translation direction is wrong. Therefore, it is necessary to set a degradation characteristic threshold value to determine whether a degradation environment is entered, as shown in the following formula:
Wherein, Representing eigenvectors represented by eigenvalues in the hessian matrix,Representing the eigenvector maximum represented by the eigenvalues in the hessian matrix,The eigenvector maximum represented by eigenvalues in the hessian matrix, V, represents the degradation eigenvalue.
Further, the step of obtaining the laser geometric pose according to the degradation characteristic threshold value comprises the following steps:
judging whether the degradation characteristic threshold value is larger than a first threshold value or not;
and if not, carrying out nonlinear optimization on the degradation characteristic function to obtain the laser geometric pose.
In the implementation process, the degradation characteristic threshold value is compared with the first threshold value, and nonlinear optimization is performed on the degradation function, so that the geometric characteristics of the laser point can be accurately and rapidly obtained, and the accuracy is improved.
Degradation characteristic thresholdRepresenting the severity of the degradation condition, when the degradation characteristic threshold value is larger than 20, representing the environment degradation degree to be larger, wherein the laser odometer factor only trusts the pose calculated by the intensity information, and when the degradation characteristic threshold value is smaller than 20, the pose is uniformly calculated by solving a nonlinear function to obtain the laser geometric pose of the robot。
Further, the step of obtaining a first residual of the laser spot comprises:
obtaining geometrical characteristic information of laser point cloud data;
constructing a smoothness function according to the geometric feature information;
obtaining the distance difference between any two laser points according to the smoothness function;
and judging the characteristics of the laser spot according to the distance difference, obtaining a first residual according to a plane point residual function if the characteristics of the laser spot are plane points, and obtaining the first residual according to an edge point residual function if the characteristics of the laser spot are edge points.
In the implementation process, the characteristics of the laser points are judged according to the distance difference, and different residual functions are constructed according to the laser points with different characteristics, so that calculation errors can be effectively reduced.
The application constructs a smoothness formula based on geometric feature information of laser point cloud data, sets a feature point judgment threshold, selects one laser point, calculates the distance difference between the laser point and surrounding laser points, and has the following expression:
wherein S refers to the number of adjacent points selected by calculating smoothness, Refers to the distance of the selected laser point to the lidar,Refers to selecting the distance from the jth laser spot around the laser spot to the laser spot,Refers to the first selected laser spot.
When the distance difference is greater than the threshold (10 in the embodiment of the present application), the edge point is the planar point, and the residual error of the edge point and the residual error of the planar point are calculated.
In S3, according to the reflection intensity h in the intensity graph, the SHIFT characteristic point is extracted, a scale space is constructed to select the laser point with the maximum local intensity and the minimum local intensity as the key point, then the key point is subjected to curve fitting and gradient histogram calculation to obtain the position information, scale information and direction information of the key point, the SHIFT characteristic point is generated according to the position information, the scale information and the direction information, and then the frame pose is calculated by adopting an interframe matching algorithm, so that the laser intensity pose of the robot is obtained。
Further, S4 includes fusing the laser geometric pose and the laser intensity pose by the following formula, to obtain a fused pose:
；
Wherein, As the pose after the fusion is carried out,Is the laser geometric pose, H is the laser intensity pose,、Representing preset parameters, and the range is [0,1].
Optionally, when a degraded environment at the robot is detected, thenAnd the automatic adjustment is 0, and the robot only trusts the pose calculated by the reflection intensity.
Further, S5 includes:
And carrying out pre-integration processing on the speed information, the position information and the rotation information in the sensor data to obtain a pre-integration factor.
In the implementation process, the pre-integration factor is obtained according to the speed information, the position information and the rotation information, so that the pre-integration factor can describe the characteristics of the sensor data in a multi-dimensional manner, and the utilization rate of the sensor data is improved.
In order to improve the calculation efficiency, the speed information, the position information and the rotation information in the sensor data are subjected to pre-integration processing to construct a pre-integration factor, and a specific formula is shown as follows, so that the change relation of the speed, the position and the rotation direction of the robot can be represented:
Wherein, Representing the speed, position and direction of rotation of the robot respectively,Meaning at time t toThe speed change over the transition time is such that,Meaning at time t toThe position change in the transformation time is such that,Meaning at time t toThe direction of rotation during the change time is changed,AndRepresenting measured rotational angular velocity and acceleration of the IMU due to the presence of random walk (whereinAndRandom walk of angular velocity and acceleration at time t and white noise (whereAndWhite noise for angular velocity and acceleration, respectively, at time t), the measured values will interfere,Is a gravitational constant.
Further, S6 includes:
Acquiring a laser odometer factor;
comparing the laser odometer factor with the pre-integral factor to obtain a second residual error between the laser odometer factor and the pre-integral factor;
And carrying out optimization processing according to the second residual error to obtain map data.
In the implementation process, the laser odometer factor which is invalid is screened out by comparing the laser odometer factor with the pre-integral factor, so that the calculated amount generated in the optimization process is reduced, and the calculation efficiency and accuracy are improved.
And setting a degradation sliding window, and judging whether the laser odometer in the sliding window fails.
Setting a sliding window as 10s, and comparing the laser odometer factor with the pre-integral factor to obtain the following formula:
Wherein j represents a serial number, n is the total pose number generated in the sliding window, Representing a degradation threshold value,Representing the degradation flag bit.
Further, the step of performing optimization processing according to the second residual error to obtain map data includes:
comparing the second residual with a degradation threshold;
If the second residual error is smaller than the degradation threshold, the laser radar fails in the degradation environment, the priori pose is obtained according to the pre-integration factor, and the priori pose and the laser odometer factor are optimized according to the factor graph, so that map data are obtained;
and if the second residual error is larger than the degradation threshold value, optimizing the pre-integration factor and the laser odometer factor according to the factor graph to obtain map data.
In the implementation process, the second residual error is compared with the degradation threshold value, and the degradation environment detection result is fed back to the generation stage of the laser odometer factor, so that the influence of the degradation environment on pose is reduced, and the calculation error is further reduced.
When the second residual error (the second residual error refers to residual error sum) of the laser odometer factor and the pre-integral factor is larger than a degradation threshold value in the sliding window, the laser radar has a failure in a degradation environment, so that a degradation mark position is set to be 1, and then only the pose obtained by pre-integration is used as a temporary pose and is used as a priori pose, and the laser odometer factor is further optimized through a factor graph.
When the second residual error of the laser odometer factor and the pre-integral factor is smaller than the degradation threshold value in the sliding window, the laser odometer and the pre-integral factor are smaller in difference, and the laser radar does not lose efficacy in the degradation environment, so that the degradation zone bit is set to 0, and the factor graph is adopted to optimize the pre-integral factor and the laser odometer factor, so that a more accurate map and track are obtained.
The method comprises the steps of optimizing a pre-integral factor and a laser mileage factor through a factor graph to obtain a globally consistent track and a map, wherein the factor graph is one of probability graphs, in the process of constructing the factor graph, mainly comprises factor nodes and variable nodes, in the application, the laser mileage factor is constructed as variable nodes, the pre-integral factor forms factor nodes among related variable nodes, finally, the factor nodes are optimized through a GTSAM library to obtain the globally consistent pose track, and because of accurate pose, observation information is more accurate, and the globally consistent map is obtained after the map is spliced. GTSAM is a c++ library for smoothing and mapping in the robotics and computer vision fields.
The method can solve the problems of SLAM positioning and mapping distortion in the degradation environment: 1. the application carries out twice detection, thereby avoiding calculation force loss and calculation error caused by single detection error; 2. performing primary detection on the degradation environment by using a feature vector represented by a feature value in the hessian matrix, so as to avoid pose errors caused by the degradation environment; 3. and the detection result of the degradation environment is fed back to the fusion of the laser geometric pose and the laser intensity pose, so that the anti-interference capability of the laser odometer on the degradation environment is improved, the integral calculation efficiency of the algorithm is improved, and the positioning and map building precision is improved.
Example two
In order to perform a corresponding method of the above embodiment to achieve the responsive function and technical effect, a map construction apparatus in a degradation environment is provided as shown in fig. 2, the apparatus comprising:
the acquisition module 1 is used for acquiring an intensity map generated by laser point cloud data and sensor data of inertial measurement equipment;
The degradation detection module 2 is used for carrying out geometric feature degradation detection on the laser points in the intensity graph to obtain the laser geometric pose;
The data acquisition module 3 is used for acquiring laser intensity pose according to the intensity graph; the method is also used for obtaining a pre-integral factor according to sensor data of the inertial measurement device;
the fusion module 4 is used for fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
and the optimization module 5 is used for carrying out optimization processing on the fused pose and the pre-integration factor to obtain map data.
In the implementation process, the laser geometric pose is obtained through degradation detection of the laser points, then the laser geometric pose and the laser intensity pose are fused, and optimization is performed based on the pre-integration factor, so that accurate construction of the map can be realized in a degradation environment, interference of surrounding environment factors is avoided, the fault tolerance is high, the error is small, and the accuracy of the map can be improved.
Further, the degradation detection module 2 is further configured to:
obtaining a first residual error of the laser spot;
nonlinear optimization is carried out on the first residual error, and a homography matrix is obtained;
Obtaining a degradation characteristic threshold value according to the homography matrix;
And obtaining the laser geometric pose according to the degradation characteristic threshold.
In the implementation process, the homography matrix is obtained according to the first residual error, and then the laser geometric pose is obtained according to the degradation characteristic threshold value, so that the laser geometric pose of the laser point can be simply and effectively extracted, the data processing flow is simplified, and the error is reduced.
Further, the degradation detection module 2 is further configured to:
Constructing a hessian matrix according to the homography matrix to obtain a degradation characteristic vector;
Constructing a degradation characteristic function according to the degradation characteristic vector;
and obtaining a degradation characteristic threshold according to the degradation characteristic function.
In the implementation process, the Heisen matrix is constructed according to the homography matrix to obtain the degradation characteristic vector, so that pose errors caused by the degradation environment can be avoided, and the anti-interference capability of the laser odometer on the degradation environment is improved.
Further, the degradation detection module 2 is further configured to:
judging whether the degradation characteristic threshold value is larger than a first threshold value or not;
and if not, carrying out nonlinear optimization on the degradation characteristic function to obtain the laser geometric pose.
In the implementation process, the degradation characteristic threshold value is compared with the first threshold value, and nonlinear optimization is performed on the degradation function, so that the geometric characteristics of the laser point can be accurately and rapidly obtained, and the accuracy is improved.
Further, the data obtaining module 3 is further configured to:
obtaining geometrical characteristic information of laser point cloud data;
constructing a smoothness function according to the geometric feature information;
obtaining the distance difference between any two laser points according to the smoothness function;
and judging the characteristics of the laser spot according to the distance difference, obtaining a first residual according to a plane point residual function if the characteristics of the laser spot are plane points, and obtaining the first residual according to an edge point residual function if the characteristics of the laser spot are edge points.
In the implementation process, the characteristics of the laser points are judged according to the distance difference, and different residual functions are constructed according to the laser points with different characteristics, so that calculation errors can be effectively reduced.
Further, the fusion module 4 is further configured to fuse the laser geometric pose and the laser intensity pose according to the following formula, so as to obtain the fused pose:
；
Wherein, As the pose after the fusion is carried out,Is the laser geometric pose, H is the laser intensity pose,、Representing preset parameters, and the range is [0,1].
Further, the data obtaining module 3 is further configured to:
And carrying out pre-integration processing on the speed information, the position information and the rotation information in the sensor data to obtain a pre-integration factor.
In the implementation process, the pre-integration factor is obtained according to the speed information, the position information and the rotation information, so that the pre-integration factor can describe the characteristics of the sensor data in a multi-dimensional manner, and the utilization rate of the sensor data is improved.
Further, the optimization module 5 is further configured to:
Acquiring a laser odometer factor;
comparing the laser odometer factor with the pre-integral factor to obtain a second residual error between the laser odometer factor and the pre-integral factor;
And carrying out optimization processing according to the second residual error to obtain map data.
In the implementation process, the laser odometer factor which is invalid is screened out by comparing the laser odometer factor with the pre-integral factor, so that the calculated amount generated in the optimization process is reduced, and the calculation efficiency and accuracy are improved.
Further, the optimization module 5 is further configured to:
comparing the second residual with a degradation threshold;
If the second residual error is smaller than the degradation threshold, the laser radar fails in the degradation environment, the priori pose is obtained according to the pre-integration factor, and the priori pose and the laser odometer factor are optimized according to the factor graph, so that map data are obtained;
and if the second residual error is larger than the degradation threshold value, optimizing the pre-integration factor and the laser odometer factor according to the factor graph to obtain map data.
In the implementation process, the second residual error is compared with the degradation threshold value, and the degradation environment detection result is fed back to the generation stage of the laser odometer factor, so that the influence of the degradation environment on pose is reduced, and the calculation error is further reduced.
The map construction apparatus under the degraded environment described above may implement the method of the first embodiment described above. The options in the first embodiment described above also apply to this embodiment, and are not described in detail here.
The rest of the embodiments of the present application may refer to the content of the first embodiment, and in this embodiment, no further description is given.
Example III
An embodiment of the present application provides an electronic device, including a memory and a processor, where the memory is configured to store a computer program, and the processor is configured to execute the computer program to cause the electronic device to execute the map construction method in the degradation environment of the first embodiment.
Alternatively, the electronic device may be a server.
Referring to fig. 3, fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the application. The electronic device may include a processor 31, a communication interface 32, a memory 33, and at least one communication bus 34. Wherein the communication bus 34 is used to enable direct connection communication of these components. The communication interface 32 of the device in the embodiment of the present application is used for performing signaling or data communication with other node devices. The processor 31 may be an integrated circuit chip with signal processing capabilities.
The processor 31 may be a general-purpose processor, including a central processing unit (Central Processing Unit, CPU), a network processor (Network Processor, NP), etc.; but may also be a Digital Signal Processor (DSP), application Specific Integrated Circuit (ASIC), an off-the-shelf programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic device, discrete hardware components. The disclosed methods, steps, and logic blocks in the embodiments of the present application may be implemented or performed. The general purpose processor may be a microprocessor or the processor 31 may be any conventional processor or the like.
The Memory 33 may be, but is not limited to, random access Memory (Random Access Memory, RAM), read Only Memory (ROM), programmable Read Only Memory (Programmable Read-Only Memory, PROM), erasable Read Only Memory (Erasable Programmable Read-Only Memory, EPROM), electrically erasable Read Only Memory (Electric Erasable Programmable Read-Only Memory, EEPROM), etc. The memory 33 has stored therein computer readable instructions which, when executed by the processor 31, enable the apparatus to perform the various steps described above in relation to the embodiment of the method of fig. 1.
Optionally, the electronic device may further include a storage controller, an input-output unit. The memory 33, the memory controller, the processor 31, the peripheral interface, and the input/output unit are electrically connected directly or indirectly to each other, so as to realize data transmission or interaction. For example, the components may be electrically coupled to each other via one or more communication buses 34. The processor 31 is arranged to execute executable modules stored in the memory 33, such as software functional modules or computer programs comprised by the device.
The input-output unit is used for providing the user with the creation task and creating the starting selectable period or the preset execution time for the task so as to realize the interaction between the user and the server. The input/output unit may be, but is not limited to, a mouse, a keyboard, and the like.
It will be appreciated that the configuration shown in fig. 3 is merely illustrative, and that the electronic device may also include more or fewer components than shown in fig. 3, or have a different configuration than shown in fig. 3. The components shown in fig. 3 may be implemented in hardware, software, or a combination thereof.
In addition, the embodiment of the present application further provides a computer readable storage medium storing a computer program, which when executed by a processor, implements the map construction method in the degradation environment of the first embodiment.
The present application also provides a computer program product which, when run on a computer, causes the computer to perform the method described in the method embodiments.
In the several embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. The apparatus embodiments described above are merely illustrative, for example, of the flowcharts and block diagrams in the figures that illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based devices which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, functional modules in the embodiments of the present application may be integrated together to form a single part, or each module may exist alone, or two or more modules may be integrated to form a single part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a usb disk, a removable hard disk, a ROM, a RAM, a magnetic disk, or an optical disk, etc.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and variations will be apparent to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application. It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures.
The above description is merely illustrative of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily think about variations or substitutions within the scope of the present application, and the application is intended to be covered by the scope of the present application. Therefore, the protection scope of the present application shall be defined by the protection scope of the claims.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
Claims (12)
1. A method of map construction in a degenerate environment, the method comprising:
acquiring an intensity map generated by laser point cloud data and sensor data of inertial measurement equipment;
Performing geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose;
Obtaining laser intensity pose according to the intensity map;
fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
Obtaining a pre-integration factor according to sensor data of the inertial measurement device;
And optimizing the fused pose and the pre-integration factor to obtain map data.
2. The method for constructing a map in a degenerate environment according to claim 1, wherein the step of performing geometric feature degradation detection on the laser points in the intensity map to obtain the geometric pose of the laser comprises:
obtaining a first residual error of the laser point;
Nonlinear optimization is carried out on the first residual error, and a homography matrix is obtained;
Obtaining a degradation characteristic threshold according to the homography matrix;
And obtaining the laser geometric pose according to the degradation characteristic threshold.
3. The map construction method in a degradation environment according to claim 2, wherein the step of obtaining a degradation characteristic threshold value from the homography matrix includes:
constructing a hessian matrix according to the homography matrix to obtain a degradation characteristic vector;
constructing a degradation characteristic function according to the degradation characteristic vector;
And obtaining the degradation characteristic threshold according to the degradation characteristic function.
4. A map construction method in a degenerate environment according to claim 3, wherein the step of obtaining the laser geometrical pose from the degenerate feature threshold comprises:
judging whether the degradation characteristic threshold is larger than a first threshold or not;
and if not, carrying out nonlinear optimization on the degradation characteristic function to obtain the laser geometric pose.
5. The map construction method in a degenerate environment according to claim 2, wherein the step of obtaining the first residual of the laser spot comprises:
obtaining geometric characteristic information of the laser point cloud data;
Constructing a smoothness function according to the geometric feature information;
Obtaining the distance difference between any two laser points according to the smoothness function;
And judging the characteristics of the laser points according to the distance difference, obtaining the first residual error according to a plane point residual error function if the characteristics of the laser points are plane points, and obtaining the first residual error according to an edge point residual error function if the characteristics of the laser points are edge points.
6. The map construction method under the degradation environment according to claim 1, wherein the laser geometric pose and the laser intensity pose are fused by the following formula, so as to obtain the fused pose:
；
Wherein, As the pose after the fusion,For the laser geometric pose, H is the laser intensity pose,、Representing preset parameters, and the range is [0,1].
7. The map construction method under a degraded environment according to claim 1, characterized in that the step of obtaining a pre-integration factor from sensor data of the inertial measurement device comprises:
And carrying out pre-integration processing on the speed information, the position information and the rotation information in the sensor data to obtain the pre-integration factor.
8. The map construction method under the degradation environment according to claim 1, wherein the step of optimizing the fused pose and the pre-integration factor to obtain map data includes:
Acquiring a laser odometer factor;
Comparing the laser odometer factor with the pre-integral factor to obtain a second residual error between the laser odometer factor and the pre-integral factor;
And carrying out optimization processing according to the second residual error to obtain map data.
9. The map construction method in a degradation environment according to claim 8, wherein the step of performing optimization processing according to the second residual error to obtain map data includes:
Comparing the second residual with a degradation threshold;
if the second residual error is larger than the degradation threshold, the laser radar fails in the degradation environment, a priori pose is obtained according to the pre-integration factor, and the priori pose and the laser odometer factor are optimized according to a factor graph, so that the map data are obtained;
and if the second residual error is smaller than the degradation threshold value, optimizing the pre-integration factor and the laser odometer factor according to the factor graph to obtain the map data.
10. A map construction apparatus in a degraded environment, the apparatus comprising:
the acquisition module is used for acquiring an intensity graph generated by laser point cloud data and sensor data of the inertial measurement device;
The degradation detection module is used for carrying out geometric feature degradation detection on the laser points in the intensity graph to obtain laser geometric pose;
The data acquisition module is used for acquiring laser intensity pose according to the intensity map; the system is also used for obtaining a pre-integral factor according to sensor data of the inertial measurement device;
the fusion module is used for fusing the laser geometric pose and the laser intensity pose to obtain a fused pose;
and the optimization module is used for carrying out optimization processing on the fused pose and the pre-integration factor to obtain map data.
11. An electronic device comprising a memory for storing a computer program and a processor that runs the computer program to cause the electronic device to perform the map construction method in a degenerate environment according to any one of claims 1 to 9.
12. A storage medium, characterized in that it stores a computer program which, when executed by a processor, implements the map construction method under a degraded environment according to any one of claims 1to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410566387.0A CN118149796B (en) | 2024-05-09 | 2024-05-09 | Map construction method and device under degradation environment, electronic equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410566387.0A CN118149796B (en) | 2024-05-09 | 2024-05-09 | Map construction method and device under degradation environment, electronic equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118149796A CN118149796A (en) | 2024-06-07 |
| CN118149796B true CN118149796B (en) | 2024-08-09 |
Family
ID=91290899
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410566387.0A Active CN118149796B (en) | 2024-05-09 | 2024-05-09 | Map construction method and device under degradation environment, electronic equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118149796B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115639570A (en) * | 2022-10-24 | 2023-01-24 | 中国科学技术大学 | Robot positioning and mapping method integrating laser intensity and point cloud geometric features |
| CN116679314A (en) * | 2023-05-31 | 2023-09-01 | 武汉大学 | Three-dimensional laser radar synchronous mapping and positioning method and system for fusion point cloud intensity |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021128297A1 (en) * | 2019-12-27 | 2021-07-01 | 深圳市大疆创新科技有限公司 | Method, system and device for constructing three-dimensional point cloud map |
-
2024
- 2024-05-09 CN CN202410566387.0A patent/CN118149796B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115639570A (en) * | 2022-10-24 | 2023-01-24 | 中国科学技术大学 | Robot positioning and mapping method integrating laser intensity and point cloud geometric features |
| CN116679314A (en) * | 2023-05-31 | 2023-09-01 | 武汉大学 | Three-dimensional laser radar synchronous mapping and positioning method and system for fusion point cloud intensity |
Also Published As
| Publication number | Publication date |
|---|---|
| CN118149796A (en) | 2024-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Jiao et al. | Robust odometry and mapping for multi-lidar systems with online extrinsic calibration | |
| CN112639502B (en) | Robot pose estimation | |
| CN112734852B (en) | Robot mapping method and device and computing equipment | |
| CN113376650B (en) | Mobile robot positioning method and device, electronic equipment and storage medium | |
| CN111089585A (en) | Mapping and positioning method based on sensor information fusion | |
| US10210411B2 (en) | Method and apparatus for establishing feature prediction accuracy | |
| US10379542B2 (en) | Location and mapping device and method | |
| Pan et al. | Gem: online globally consistent dense elevation mapping for unstructured terrain | |
| CN114111774B (en) | Vehicle positioning method, system, equipment and computer readable storage medium | |
| US11567501B2 (en) | Method and system for fusing occupancy maps | |
| CN113137968B (en) | Repositioning method and repositioning device based on multi-sensor fusion and electronic equipment | |
| Tang et al. | LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots | |
| CN117218350A (en) | SLAM implementation method and system based on solid-state radar | |
| Yu et al. | Tightly-coupled fusion of VINS and motion constraint for autonomous vehicle | |
| CN114018269B (en) | Positioning method, positioning device, electronic equipment, storage medium and automatic driving vehicle | |
| CN117392241B (en) | Sensor calibration method and device in automatic driving and electronic equipment | |
| Valente et al. | Evidential SLAM fusing 2D laser scanner and stereo camera | |
| CN112446842B (en) | Sparse and dense depth map-based image reconstruction method, system, equipment and medium | |
| CN118149796B (en) | Map construction method and device under degradation environment, electronic equipment and storage medium | |
| Fu et al. | Semantic Map-based Visual Localization with Consistency Guarantee | |
| Zhao et al. | Localization and mapping algorithm based on Lidar-IMU-Camera fusion | |
| Xia et al. | Bayesian Simultaneous Localization and Multi-Lane Tracking Using Onboard Sensors and a SD Map | |
| Kubertschak et al. | Fusion routine independent implementation of advanced driver assistance systems with polygonal environment models | |
| Badalkhani et al. | Effects of Moving Landmark's Speed on Multi-Robot Simultaneous Localization and Mapping in Dynamic Environments. | |
| CN110850856B (en) | Data processing method and device and robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 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 |