CN111210475B - Map updating method and device - Google Patents
Map updating method and device Download PDFInfo
- Publication number
- CN111210475B CN111210475B CN202010315513.7A CN202010315513A CN111210475B CN 111210475 B CN111210475 B CN 111210475B CN 202010315513 A CN202010315513 A CN 202010315513A CN 111210475 B CN111210475 B CN 111210475B
- Authority
- CN
- China
- Prior art keywords
- laser
- data
- frame
- map
- voxel
- 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
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
Abstract
The application relates to the technical field of map updating, in particular to a map updating method and device, which respectively acquire each frame of laser data, wherein each frame of laser data comprises laser point data of a plurality of laser points; respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data; respectively converting the laser data of each frame into a preset global coordinate system according to the laser data of each frame and the corresponding pose, and obtaining the converted laser data of each frame; and respectively calculating the SDF value of each laser point in each frame of laser data according to each frame of laser data after conversion, and updating the map according to the SDF value of each laser point, so that the map updating efficiency can be improved, and the calculated amount during map updating can be reduced.
Description
Technical Field
The present application relates to the field of map updating technologies, and in particular, to a map updating method and apparatus.
Background
At present, in the field of logistics robots such as Automated Guided Vehicles (AGVs), in order to realize tasks such as positioning, navigation, path planning and the like of the robots, a grid map is usually required to be constructed, but since a scene usually contains dynamic objects such as people and vehicles, the map needs to be updated in real time to eliminate the influence of the dynamic objects on positioning and navigation.
In the prior art, a map can be generally represented as a grid map, the grid map represents three states of occupied, unoccupied and unknown states and represents the three states in a probability manner, however, in the calculation process, the grid probability of each grid needs to be calculated, and therefore, a large amount of calculation resources and memory need to be occupied, the second is a point cloud map, most of the point cloud map represents the real world directly through point clouds, but the memory occupied by the point clouds is many, and therefore, the problem of large calculation amount is caused, and the third is a voxel-based map.
Disclosure of Invention
The embodiment of the application provides a map updating method and device, so that the map updating efficiency is improved, and the calculated amount is reduced.
The embodiment of the application provides the following specific technical scheme:
a map updating method, comprising:
respectively acquiring each frame of laser data, wherein each frame of laser data comprises laser point data of a plurality of laser points;
respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data;
respectively converting the laser data of each frame into a preset global coordinate system according to the laser data of each frame and the corresponding pose, and obtaining the converted laser data of each frame;
and respectively calculating the SDF value of each laser point in each frame of laser data according to the converted frame of laser data, and updating the map according to the SDF value of each laser point.
Optionally, the respectively obtaining the laser data of each frame specifically includes:
respectively acquiring each frame of laser data to be processed, wherein each frame of laser data to be processed comprises the laser point data to be processed of a plurality of laser points;
respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of to-be-processed laser data;
respectively converting each pose to be processed into a coordinate system corresponding to first laser point data in any frame of laser data to be processed to obtain each converted pose to be processed, wherein the coordinate system is the coordinate system of the acquisition equipment;
and obtaining each frame of laser data according to each converted pose to be processed and the laser data to be processed.
Optionally, after the pose of the acquisition device corresponding to each laser point data in each frame of laser data is respectively obtained, the method further includes:
respectively calculating normal vectors of voxels corresponding to each laser data aiming at any frame of laser data, wherein the normal vectors represent distance values between each laser point and the corresponding voxel in each frame of laser data;
and respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function, and obtaining the optimized pose corresponding to each laser point data.
Optionally, updating the map according to the SDF value of each laser point specifically includes:
according to the SDF value of each laser point, each laser point in each frame of laser data is mapped to a map respectively, and the distribution of each laser point in each frame of laser data in the map is obtained, wherein when the SDF value is 0, the probability that the laser point is mapped to the corresponding voxel is highest, when the SDF value is not 0, the probability that the laser point is mapped to the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object;
and updating the map according to the distribution of each laser point in each frame of laser data in the map.
Optionally, further comprising:
calculating the voxel probability of each voxel in the map according to a preset period or when the update of the map is determined to be finished, wherein the voxel probability represents the probability that the SDF value is 0, and each laser point is mapped to a corresponding position in a corresponding global coordinate system;
and if the voxel probability is determined to be smaller than a preset voxel probability threshold, deleting the voxel from locally stored voxels, and updating the map.
A map updating apparatus, comprising:
the first acquisition module is used for respectively acquiring each frame of laser data, wherein each frame of laser data comprises laser point data of a plurality of laser points;
the second acquisition module is used for respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data;
the conversion module is used for respectively converting the laser data of each frame into a preset global coordinate system according to the laser data of each frame and the corresponding pose so as to obtain the converted laser data of each frame;
and the first updating module is used for calculating the SDF value of each laser point in each frame of laser data according to each frame of laser data after conversion and updating the map according to the SDF value of each laser point.
Optionally, the first obtaining module is specifically configured to:
respectively acquiring each frame of laser data to be processed, wherein each frame of laser data to be processed comprises the laser point data to be processed of a plurality of laser points;
respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of to-be-processed laser data;
respectively converting each pose to be processed into a coordinate system corresponding to first laser point data in any frame of laser data to be processed to obtain each converted pose to be processed, wherein the coordinate system is the coordinate system of the acquisition equipment;
and obtaining each frame of laser data according to each converted pose to be processed and the laser data to be processed.
Optionally, after the pose of the acquisition device corresponding to each laser point data in each frame of laser data is respectively obtained, the method further includes:
the first processing module is used for calculating a normal vector of a voxel corresponding to each laser data aiming at any frame of laser data, wherein the normal vector represents a distance value between each laser point in each frame of laser data and the corresponding voxel;
and the optimization module is used for respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function and obtaining the optimized pose corresponding to each laser point data.
Optionally, when the map is updated according to the SDF value of each laser point, the first updating module is specifically configured to:
according to the SDF value of each laser point, each laser point in each frame of laser data is mapped to a map respectively, and the distribution of each laser point in each frame of laser data in the map is obtained, wherein when the SDF value is 0, the probability that the laser point is mapped to the corresponding voxel is highest, when the SDF value is not 0, the probability that the laser point is mapped to the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object;
and updating the map according to the distribution of each laser point in each frame of laser data in the map.
Optionally, further comprising:
the second processing module is used for calculating the voxel probability of each voxel in the map according to a preset period or when the map updating is determined to be finished, wherein the voxel probability represents the probability that an SDF (software development framework) value is 0, and each laser point is mapped to a corresponding position in a corresponding global coordinate system;
and the second updating module is used for deleting the voxel from the locally stored voxel and updating the map if the voxel probability is determined to be smaller than a preset voxel probability threshold.
An electronic device comprises a memory, a processor and a computer program stored on the memory and operable on the processor, wherein the processor implements the steps of the map updating method when executing the program.
A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the above-mentioned map updating method.
In the embodiment of the application, each frame of laser data is obtained respectively, each frame of laser data comprises laser point data of a plurality of laser points, the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data is obtained respectively, each frame of laser data is converted into the preset global coordinate system according to each frame of laser data and the corresponding pose, each converted frame of laser data is obtained, the SDF value of each laser point in each frame of laser data is calculated according to each converted frame of laser data, and the map is updated according to the SDF value of each laser point, so that the SDF value is calculated according to each frame of laser data and the corresponding pose, the map is updated according to the SDF value, calculation resources and memory can be saved, and the update efficiency of the map is improved.
Drawings
FIG. 1 is a flow chart of a map updating method in an embodiment of the present application;
FIG. 2 is a flow chart of another map updating method according to an embodiment of the present application;
FIG. 3 is a schematic structural diagram of a map updating apparatus according to an embodiment of the present application;
fig. 4 is a schematic structural diagram of an electronic device in an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
At present, in the field of logistics robots such as Automated Guided Vehicles (AGVs), in order to realize tasks such as positioning, navigation, path planning and the like of the robots, a grid map is usually required to be constructed, but since a scene usually contains dynamic objects such as people and vehicles, the map needs to be updated in real time to eliminate the influence of the dynamic objects on positioning and navigation.
In the prior art, in the field of logistics robots, maps can be generally represented in several ways, the first is a grid map, the grid map is represented in three states of occupied state, unoccupied state and unknown state, and the three states are represented in a probabilistic manner, but in the calculation process, the probability of each grid needs to be calculated, and a large amount of calculation resources and memory are occupied, the second map is a point cloud map, most of which directly represents the real world with point clouds, but the memory occupied by the point clouds is many and is difficult to process in fusion and update, therefore, the map update through the point clouds is very large in calculation amount, the third map is a voxel-based map, such as a Truncated Signed Distance Function (TSDF) map, which can effectively reduce the calculation amount and the memory occupied by the TSDF map, but the TSDF map is difficult to update, once the dynamic object is occupied, the voxel still exists, and the dynamic object cannot be filtered out.
In the embodiment of the application, each frame of laser data is obtained, each frame of laser data comprises laser data of a plurality of laser points, the pose of an acquisition device corresponding to each laser point data in each frame of laser data is obtained, each frame of laser data is converted into a preset global coordinate system according to each frame of laser data and the corresponding pose, each converted laser data is obtained, a directed Distance Field (SDF) value of each converted laser data is calculated, and a map is updated according to the SDF value.
Based on the foregoing embodiment, referring to fig. 1, a flowchart of a map updating method in the embodiment of the present application specifically includes:
step 100: and respectively acquiring laser data of each frame.
Wherein each frame of laser data comprises laser point data of a plurality of laser points.
In the embodiment of the application, the laser data and the pose are collected through the collecting device, sent to the server, calculated by the server, and updated.
The collecting device in the embodiment of the present application may be, for example, a logistics robot, which is not limited in the embodiment of the present application.
Step 110: and respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data.
Step 120: and respectively converting each frame of laser data into a preset global coordinate system according to each frame of laser data and the corresponding pose, and obtaining each frame of laser data after conversion.
After obtaining each frame of laser data and the corresponding pose, converting each frame of laser data into a preset global coordinate system to obtain each converted frame of laser data, which can be calculated by the following formula, for example:
wherein the content of the first and second substances,
in order to convert the laser data into the laser data,
in order to deduce the pose according to the speed of the acquisition device,
is the laser data.
Step 130: and respectively calculating the SDF value of each laser point in each frame of laser data according to each frame of converted laser data, and updating the map according to the SDF value of each laser point.
In the embodiment of the present application, when calculating the SDF value of each laser point in each frame of laser data, first, the converted laser needs to be obtained, and each frame of laser data after conversion is obtained according to each frame of laser data and the corresponding pose, so that when obtaining each frame of laser data, the method specifically includes:
s1: and respectively acquiring the laser data to be processed of each frame.
Wherein, each frame of laser data to be processed comprises the data of the laser points to be processed of a plurality of laser points.
In the embodiment of the application, when logistics robots such as an AGV collect laser data, the robots are in motion, and the collected laser data can generate motion distortion, so that the collected laser data to be processed needs to be subjected to motion distortion removal, and the laser data after the motion distortion removal is obtained.
In the embodiment of the application, each frame of laser data to be processed is obtained, for example, a plurality of lasers can be sent through a collection device to obtain laser data, where the lasers include a plurality of laser points, and therefore the collected laser data includes laser point data of the plurality of laser points.
Each frame of laser data to be processed at least includes laser point data to be processed of a plurality of laser points, and the duration of each frame may be preset by a relevant worker, for example, each frame may be set to 1s, which is not limited in the embodiment of the present application.
Moreover, the laser points emitted by each frame of laser may also be preset by the relevant staff, for example, the laser points sent by each frame of laser data may be set to 360, and it can be further obtained that the acquisition device sends laser points 360 degrees and sends one laser point every 1 degree, which is not limited in the embodiment of the present application.
S2: and respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of the to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of the to-be-processed laser data.
In the embodiment of the application, for any frame of laser data to be processed, the pose of the front frame and the pose of the rear frame of laser data of any frame of laser data to be processed and the running speed of the acquisition equipment can be obtained through inter-frame matching of the laser data.
For example, assume that at time i, the pose of the acquisition device is
In the time period when the acquisition device sends each laser point, the pose of the acquisition device at the time of generating the laser data can be considered as
Wherein, in the step (A),
。
wherein the content of the first and second substances,
the time difference of sending each laser spot for the acquisition device,
to capture the time of the last laser spot of the laser emitted by the device,
to capture the time instant of the first laser spot of the laser light emitted by the device,
the running speed of the equipment is acquired.
In the embodiment of the present application, the operation speed of the acquisition device may be preset by a relevant worker, which is not limited herein.
Aiming at the to-be-processed laser point data corresponding to each laser point in any frame of to-be-processed laser data, the to-be-processed pose of the acquisition equipment corresponding to each to-be-processed laser point data can be obtained through interpolation. Can be expressed as:
wherein the content of the first and second substances,
the pose of the acquisition equipment corresponding to the jth laser point data to be processed is represented by n, j is represented by the jth laser point,
to acquire the corresponding pose of the device at the ith moment,
and generating the pose of the acquisition equipment at the moment of laser data generation.
The addition and subtraction symbols in the interpolation formula represent addition and subtraction on a lie algebra.
S3: and respectively converting each to-be-processed pose into a coordinate system corresponding to the first laser point data in any frame of to-be-processed laser data to obtain each converted to-be-processed pose.
Wherein the coordinate system is the coordinate system of the acquisition equipment.
In the embodiment of the application, for example, when the jth laser point is obtained, then each to-be-processed pose is converted into a coordinate system corresponding to the first laser point data in any frame of to-be-processed laser data, and the to-be-processed pose of the acquisition device corresponding to each converted laser point is generated.
S4: and obtaining the laser data of each frame according to the converted pose to be processed and the laser data to be processed.
In the embodiment of the application, after the converted positions and postures to be processed are obtained, new laser data are regenerated according to the converted positions and postures to be processed and the laser data to be processed, and the laser data of each frame are obtained.
After the pose corresponding to the acquisition equipment of each laser data point is obtained through the laser odometer, in order to further improve the accuracy of the pose, the method can also be used for correcting the obtained pose, and specifically comprises the following steps:
s1: and respectively calculating the normal vector of the voxel corresponding to each laser data aiming at any frame of laser data.
And the normal vector represents the distance value between each laser point and the corresponding voxel in each frame of laser data.
In the embodiment of the present application, the pose of the acquisition device corresponding to each laser Point data in each frame of laser data may be obtained through a laser odometer technology, for example, the pose of the acquisition device corresponding to each laser Point data in each frame of laser data may be obtained through a laser odometer provided by methods such as inter-frame matching (scan-match), Iterative Closest Point (ICP), cartographer, and sector.
After the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data is obtained, although the pose obtained by the laser odometer can be trusted, in order to further improve the accuracy of map updating, the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data can be corrected, and errors caused by the laser odometer are eliminated.
For the ith laser point, calculating the normal vector of the corresponding voxel: from the potential field of the SDF in the map, the normal vector of the voxel is computed, which can be expressed as:
wherein x, y and z are respectively the values of the coordinates corresponding to the laser point data,
the voxel coordinates to which the laser point is projected.
S2: and respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function, and obtaining the optimized pose corresponding to each laser point data.
In the embodiment of the application, after the normal vectors of the voxels corresponding to each laser point are obtained, an optimization objective function is constructed according to each normal vector to perform optimization solution, and the optimized pose corresponding to each laser point data is obtained.
When the framework optimization objective function is optimized and solved, for example, an efficient nonlinear optimization library (Ceres) or graph-based optimization (g 2o) may be used, which is not limited in the embodiment of the present application.
For example, the optimization objective function can be expressed as:
wherein the content of the first and second substances,
for optimizing the objective function, R and t are variables to be optimized, and the initial values thereof correspond to the acquisition equipment provided by the laser odometer
The pose of the robot is determined by the position of the robot,
as a laser spot
,
For the voxel position onto which the laser spot is projected
，
Is the resolution of the voxel.
And then R can be obtained according to the formula, wherein R is the orientation in the pose of the acquisition equipment, and t are the coordinates (x, y and z) corresponding to the laser point data of the laser point.
And when the converted frame of laser data and the optimized corresponding pose are obtained, the converted frame of laser data is obtained according to the frame of laser data and the corresponding pose, the SDF value of each laser point in the frame of laser data is calculated according to the converted frame of laser data, and the map is updated according to the SDF value of each laser point.
In the embodiment of the present application, updating the map according to the SDF value of each laser point specifically includes:
s1: and respectively mapping each laser point in each frame of laser data into a map according to the SDF value of each laser point, and obtaining the distribution of each laser point in each frame of laser data in the map.
When the SDF value is not 0, the probability that the laser point is mapped into the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object.
Specifically, when the SDF value of any one laser point is 0, the probability that the laser point is mapped to the corresponding voxel is highest, and voxels before and after the voxel are also mapped to voxels before and after the corresponding voxel, but the probability is lower with respect to the voxel to which the laser point is currently corresponding, and when the SDF value of any one laser point is not 0, for example, greater than 0 or less than 0, the probability that the laser point is mapped to the corresponding voxel is lowest.
In the embodiment of the present application, for each frame of laser data, the laser point data corresponding to each laser point obeys gaussian distribution and uniform distribution, and the laser point data can be expressed as:
wherein the content of the first and second substances,
the nth laser point of the current frame laser, Z is the true value, pi is the confidence rate,
is the variance of the nth laser spot,
is the minimum of the true value(s),
is the maximum value of the true value.
Therefore, for the SDF value of each laser spot, the SDF value represents the projection depth of the voxel corresponding to each laser spot along the light ray direction, and in this embodiment, the SDF value of each laser spot may be defined as:
wherein V is a voxel, i is the ith point,
in order to measure the depth value,
is a constant.
Further, for SDF also obeying gaussian and uniform distributions, the SDF value can be expressed as:
wherein the content of the first and second substances,
the SDF value for the nth laser spot.
S2: and updating the map according to the distribution of the laser points in the map in each frame of laser data.
In the embodiment of the present application, when the map is updated according to the distribution of each laser point in each frame of laser data in the map, the probability distribution of any frame of laser data may be represented as:
the distribution can be approximated as the product of a Beta distribution and a gaussian distribution, which can be expressed as:
wherein the content of the first and second substances,
is the average value of the errors and is,
is the variance of the error.
Since the map is composed of a plurality of frames of laser data, the distribution of the map can be expressed as:
wherein the content of the first and second substances,
likewise, the distribution can also be approximated as the product of a Beta distribution and a Gaussian distribution:
the voxel, i.e., the map, can be updated according to the above formula, but the formula for constructing and updating the map is not limited in the embodiment of the present application.
And after the SDF value corresponding to each laser point in each frame of laser data is obtained, mapping each laser point in each frame of laser data into a map, obtaining the distribution of each laser point in each frame of laser data in the map, and further updating the map according to the distribution.
Further, after the map is constructed and updated, since the dynamic object is in motion, the dynamic object can be filtered in real time, specifically including:
s1: and calculating the voxel probability of each voxel in the map according to a preset period or when the end of the map updating is determined.
The voxel probability represents the probability that the SDF value is 0, and the voxel represents the corresponding position of each laser point mapped to the corresponding global coordinate system.
In the embodiment of the application, after the map is updated, the dynamic objects can be filtered in real time, and also can be filtered in real time according to the preset period in the process of updating the map, so that the embodiment of the application is not limited.
The method specifically comprises the following steps: the voxel probability of each voxel in the map is calculated.
In the embodiment of the present application, the voxel probability can be expressed as
This is not limited in the embodiments of the present application.
S2: and if the voxel probability is determined to be smaller than the preset voxel probability threshold, deleting the voxel from the locally stored voxel, and updating the map.
In the embodiment of the present application,
the physical meaning of (1) is whether the laser point to be inserted into the frame is nearest to the surface in the voxel, i.e. the voxel probability of the voxel can be expressed as
，
The closer to 1, the closer the voxel is to the real surface,
closer to 0, the farther away the voxel is from the real surface.
Wherein the content of the first and second substances,
for voxel probability, a and b are parameters obtained, for example, parameters obtained by a sensor of the acquisition device, which is not limited in the embodiment of the present application.
If there is a dynamic object in the environment at the ith time instant, the SDF value set of the dynamic object can be expressed as:
after the map is inserted, its corresponding voxel distribution can be expressed as:
at the time j, at the position of the dynamic object, no laser spot is located at the position of the dynamic object at the time i, i.e. at the time j
。
Thus, after inserting the laser data into the map at time j, the laser data can be expressed as:
therefore, the voxel where the dynamic object is located is deleted, and the filtering of the dynamic object in the map is completed, but in some positions, the observation of the dynamic object is only 1 time, and the front and the back of the dynamic object are overlapped, and the dynamic object cannot be completely eliminated by the method, so that the voxel with a lower threshold can be removed by setting the probability threshold of the voxel, and then the filtering of the dynamic object is completed, and the accuracy of the map updating can be improved.
Therefore, in the embodiment of the present application, a probability threshold is preset for the voxel probability of the voxel, if it is determined that the voxel probability of the voxel is smaller than the preset probability threshold, the voxel is deleted from the locally stored voxel, if it is determined that the voxel probability of the voxel is greater than or equal to the preset probability threshold, the locally stored voxel is retained, the preset probability threshold may be preset by a relevant worker, for example, the probability threshold may be set to 0.95, and when the calculated voxel probability is smaller than the probability threshold 0.95, the voxel is deleted from the locally stored voxel, which is not limited in the embodiment of the present application.
In the embodiment of the application, after each frame of laser data to be processed is acquired, the pose to be processed of the acquisition device corresponding to each laser point data in any frame of laser data to be processed is determined according to the running speed of the acquisition device, each processing pose is converted into the coordinate system corresponding to the first laser point data in any frame of laser data to be processed, each converted pose to be processed is obtained, then each frame of laser data is obtained according to each converted pose to be processed and the laser data to be processed, after the pose of the acquisition device corresponding to each laser point data in each frame of laser data is acquired, the normal vector of the voxel corresponding to each laser data is calculated according to any frame of laser data, and the pose of each laser point data is optimized according to each normal vector and optimization objective function, and obtaining each optimized pose, then obtaining the SDF value corresponding to each laser point according to each frame of laser data and each optimized pose, and updating the map according to the SDF value, so that the laser data and the pose of the acquisition equipment corresponding to each laser point are corrected, and the SDF value is obtained according to the corrected laser data and the corrected pose, thereby saving resources, reducing the calculated amount and improving the map updating efficiency.
Further, when the update of the map is determined to be finished, or according to a preset period, the voxel probability of each voxel in the map is calculated, if the voxel probability is determined to be greater than a preset probability threshold, the voxel is deleted from the locally stored voxel, and the map is updated, so that the voxel smaller than the preset probability threshold in the map is deleted in time by setting the probability threshold, and dynamic objects in the map can be effectively filtered.
Based on the foregoing embodiment, the following describes the map updating method in the present embodiment in detail by taking the current frame and filtering out the dynamic object in the map when the map updating is finished, and refer to fig. 2, which is a flowchart of another map updating method in the present embodiment.
Step 200: and acquiring laser data to be processed of the current frame.
The current frame laser comprises a plurality of laser points, and the current frame laser data to be processed comprises laser point data to be processed of the plurality of laser points.
Step 201: and respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in the current frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at the current frame of to-be-processed laser data.
Step 202: and respectively converting each to-be-processed pose into a coordinate system corresponding to the first laser point data in the current frame to-be-processed laser data to obtain each converted to-be-processed pose.
Wherein the coordinate system is the coordinate system of the acquisition equipment.
Step 203: and obtaining the laser data of each frame according to the converted pose to be processed and the laser data to be processed.
Step 204: and respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in the current frame laser data through the laser mileometers.
Step 205: and calculating the normal vector of the voxel corresponding to the current frame laser data according to the current frame laser data.
The normal vector represents a distance value between each laser point and a corresponding voxel in the current frame laser data.
Step 206: and respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function, and obtaining the optimized pose corresponding to each laser point data.
Step 207: and respectively converting the current frame laser data into a preset global coordinate system according to the current frame laser data and the corresponding optimized pose, and obtaining the converted current frame laser data.
Step 208: and respectively calculating the SDF value of each laser point in the current frame laser data according to the converted current frame laser data.
Step 209: and respectively mapping each laser point in the current frame laser data to a map according to the SDF value of each laser point in the current frame to obtain the distribution of each laser point in the current frame laser data in the map.
When the SDF value is not 0, the probability that the laser point is mapped into the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object.
Step 210: and updating the map according to the distribution of the laser points in the map in each frame of laser data.
Step 211: the voxel probability of each voxel in the map is calculated.
The voxel probability represents the probability that the SDF value is 0, and the voxel represents the corresponding position of each laser point mapped to the corresponding global coordinate system.
Step 212: and (3) judging whether the voxel probability is smaller than a preset voxel probability threshold, if so, executing a step 213, and if not, executing a step 214.
Step 213: the voxels are retained and the map is updated.
Step 214: the voxel is deleted from the locally stored voxel and the map is updated.
In the embodiment of the application, firstly, the motion distortion of the acquired current frame laser data to be processed is removed to obtain the current frame laser data to be processed, then the pose of the acquisition equipment corresponding to each laser point data in the current frame laser data is respectively obtained through a laser odometer, the corresponding pose is optimized to obtain the corresponding optimized pose, the current frame laser data is converted according to the current frame laser data and the optimized pose to obtain the converted current frame laser data, the SDF value of each laser point in the current frame laser data is calculated, each laser point in the current frame laser data is respectively mapped into the map according to the SDF value of each laser point of the current frame to obtain the distribution of each laser point in the current frame laser data in the map, the map is updated according to the distribution, and the voxel probability of each voxel in the map is calculated after the map is updated, and dynamic objects in the map are filtered according to the voxel probability, so that the map updating accuracy can be improved, and the calculated amount can be reduced.
Based on the same inventive concept, the embodiment of the present application provides a map updating apparatus, which may be a hardware structure, a software module, or a hardware structure plus a software module. Based on the above embodiments, referring to fig. 3, a schematic structural diagram of a map updating apparatus in an embodiment of the present application specifically includes:
a first obtaining module 300, configured to obtain each frame of laser data, where each frame of laser data includes laser point data of multiple laser points;
a second obtaining module 310, configured to obtain a pose of the acquisition device corresponding to each laser point data in each frame of laser data respectively;
the conversion module 320 is configured to convert each frame of laser data into a preset global coordinate system according to each frame of laser data and a corresponding pose, and obtain each converted frame of laser data;
the first updating module 330 is configured to calculate an SDF value of each laser spot in each frame of laser data according to each frame of laser data after conversion, and update a map according to the SDF value of each laser spot.
Optionally, the first obtaining module 300 is specifically configured to:
respectively acquiring each frame of laser data to be processed, wherein each frame of laser data to be processed comprises the laser point data to be processed of a plurality of laser points;
respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of to-be-processed laser data;
respectively converting each pose to be processed into a coordinate system corresponding to first laser point data in any frame of laser data to be processed to obtain each converted pose to be processed, wherein the coordinate system is the coordinate system of the acquisition equipment;
and obtaining each frame of laser data according to each converted pose to be processed and the laser data to be processed.
Optionally, after the pose of the acquisition device corresponding to each laser point data in each frame of laser data is respectively obtained, the method further includes:
the first processing module 340 is configured to calculate, for any one frame of laser data, a normal vector of a voxel corresponding to each frame of laser data, where the normal vector represents a distance value between each laser point in each frame of laser data and the corresponding voxel;
and an optimizing module 350, configured to optimize the pose of each laser point data according to each normal vector and an optimization objective function, and obtain an optimized pose corresponding to each laser point data.
Optionally, when the map is updated according to the SDF value of each laser point, the first updating module 330 is specifically configured to:
according to the SDF value of each laser point, each laser point in each frame of laser data is mapped to a map respectively, and the distribution of each laser point in each frame of laser data in the map is obtained, wherein when the SDF value is 0, the probability that the laser point is mapped to the corresponding voxel is highest, when the SDF value is not 0, the probability that the laser point is mapped to the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object;
and updating the map according to the distribution of each laser point in each frame of laser data in the map.
Optionally, further comprising:
a second processing module 360, configured to calculate a voxel probability of each voxel in the map according to a preset period or when it is determined that the map update is finished, where the voxel probability represents a probability that an SDF value is 0, and the voxel represents that each laser point is mapped to a corresponding position in a corresponding global coordinate system;
a second updating module 370, configured to delete the voxel from the locally stored voxel and update the map if it is determined that the voxel probability is smaller than the preset voxel probability threshold.
Based on the above embodiments, referring to fig. 4, a schematic structural diagram of an electronic device in an embodiment of the present application is shown.
An embodiment of the present application provides an electronic device, which may include a processor 410 (CPU), a memory 420, an input device 430, an output device 440, and the like, wherein the input device 430 may include a keyboard, a mouse, a touch screen, and the like, and the output device 440 may include a display device, such as a liquid crystal display (L acquired crystal display, <tttransition = L "&tttl &ttt/t &tttcd), a Cathode Ray Tube (Cathode Ray Tube, CRT), and the like.
The processor 410 is configured to execute any one of the map updating methods according to the embodiment of the present application by calling the program instructions stored in the memory 420 according to the obtained program instructions.
Based on the above embodiments, in the embodiments of the present application, a computer-readable storage medium is provided, on which a computer program is stored, which, when executed by a processor, implements a map updating method in any of the above method embodiments.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to the application. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
It will be apparent to those skilled in the art that various changes and modifications may be made in the present application without departing from the spirit and scope of the application. Thus, if such modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is intended to include such modifications and variations as well.
Claims (10)
1. A map updating method, comprising:
respectively acquiring each frame of laser data, wherein each frame of laser data comprises laser point data of a plurality of laser points;
respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data;
respectively converting the laser data of each frame into a preset global coordinate system according to the laser data of each frame and the corresponding pose, and obtaining the converted laser data of each frame;
calculating the SDF value of the directed distance field of each laser point in each frame of laser data according to each frame of laser data after conversion, and updating the map according to the SDF value of each laser point, wherein updating the map according to the SDF value of each laser point specifically comprises: according to the SDF value of each laser point, each laser point in each frame of laser data is mapped to a map respectively, and the distribution of each laser point in each frame of laser data in the map is obtained, wherein when the SDF value is 0, the probability that the laser point is mapped to the corresponding voxel is highest, when the SDF value is not 0, the probability that the laser point is mapped to the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object; and updating the map according to the distribution of each laser point in each frame of laser data in the map.
2. The method of claim 1, wherein the separately acquiring each frame of laser data comprises:
respectively acquiring each frame of laser data to be processed, wherein each frame of laser data to be processed comprises the laser point data to be processed of a plurality of laser points;
respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of to-be-processed laser data;
respectively converting each pose to be processed into a coordinate system corresponding to first laser point data in any frame of laser data to be processed to obtain each converted pose to be processed, wherein the coordinate system is the coordinate system of the acquisition equipment;
and obtaining each frame of laser data according to each converted pose to be processed and the laser data to be processed.
3. The method of claim 1, wherein after obtaining the pose of the acquisition device corresponding to each laser point data in each frame of laser data, respectively, further comprising:
respectively calculating normal vectors of voxels corresponding to each laser data aiming at any frame of laser data, wherein the normal vectors represent distance values between each laser point and the corresponding voxel in each frame of laser data;
and respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function, and obtaining the optimized pose corresponding to each laser point data.
4. The method of claim 1, further comprising:
calculating the voxel probability of each voxel in the map according to a preset period or when the update of the map is determined to be finished, wherein the voxel probability represents the probability that the SDF value is 0, and each laser point is mapped to a corresponding position in a corresponding global coordinate system;
and if the voxel probability is determined to be smaller than a preset voxel probability threshold, deleting the voxel from locally stored voxels, and updating the map.
5. A map updating apparatus, comprising:
the first acquisition module is used for respectively acquiring each frame of laser data, wherein each frame of laser data comprises laser point data of a plurality of laser points;
the second acquisition module is used for respectively acquiring the pose of the acquisition equipment corresponding to each laser point data in each frame of laser data;
the conversion module is used for respectively converting the laser data of each frame into a preset global coordinate system according to the laser data of each frame and the corresponding pose so as to obtain the converted laser data of each frame;
a first updating module, configured to calculate, according to the converted frame of laser data, an SDF value of each laser point in the frame of laser data, and update a map according to the SDF value of each laser point, where when the map is updated according to the SDF value of each laser point, the first updating module is specifically configured to: according to the SDF value of each laser point, each laser point in each frame of laser data is mapped to a map respectively, and the distribution of each laser point in each frame of laser data in the map is obtained, wherein when the SDF value is 0, the probability that the laser point is mapped to the corresponding voxel is highest, when the SDF value is not 0, the probability that the laser point is mapped to the corresponding voxel is lowest, and the SDF value represents whether the laser data is the coordinates of the surface of the detected object; and updating the map according to the distribution of each laser point in each frame of laser data in the map.
6. The apparatus of claim 5, wherein the first obtaining module is specifically configured to:
respectively acquiring each frame of laser data to be processed, wherein each frame of laser data to be processed comprises the laser point data to be processed of a plurality of laser points;
respectively determining the to-be-processed pose of the acquisition equipment corresponding to each laser point data in any frame of to-be-processed laser data according to the running speed of the acquisition equipment aiming at any frame of to-be-processed laser data;
respectively converting each pose to be processed into a coordinate system corresponding to first laser point data in any frame of laser data to be processed to obtain each converted pose to be processed, wherein the coordinate system is the coordinate system of the acquisition equipment;
and obtaining each frame of laser data according to each converted pose to be processed and the laser data to be processed.
7. The apparatus of claim 5, wherein after obtaining the pose of the acquisition device corresponding to each laser point data in each frame of laser data, respectively, further comprises:
the first processing module is used for calculating a normal vector of a voxel corresponding to each laser data aiming at any frame of laser data, wherein the normal vector represents a distance value between each laser point in each frame of laser data and the corresponding voxel;
and the optimization module is used for respectively optimizing the pose of each laser point data according to each normal vector and the optimization objective function and obtaining the optimized pose corresponding to each laser point data.
8. The apparatus of claim 5, further comprising:
the second processing module is used for calculating the voxel probability of each voxel in the map according to a preset period or when the map updating is determined to be finished, wherein the voxel probability represents the probability that an SDF (software development framework) value is 0, and each laser point is mapped to a corresponding position in a corresponding global coordinate system;
and the second updating module is used for deleting the voxel from the locally stored voxel and updating the map if the voxel probability is determined to be smaller than a preset voxel probability threshold.
9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the steps of the method of any of claims 1-4 are implemented when the program is executed by the processor.
10. A computer-readable storage medium having stored thereon a computer program, characterized in that: the computer program when being executed by a processor realizes the steps of the method of any one of claims 1 to 4.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010315513.7A CN111210475B (en) | 2020-04-21 | 2020-04-21 | Map updating method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010315513.7A CN111210475B (en) | 2020-04-21 | 2020-04-21 | Map updating method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111210475A CN111210475A (en) | 2020-05-29 |
| CN111210475B true CN111210475B (en) | 2020-07-14 |
Family
ID=70784779
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010315513.7A Active CN111210475B (en) | 2020-04-21 | 2020-04-21 | Map updating method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111210475B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP4024349A1 (en) * | 2020-12-29 | 2022-07-06 | Waymo LLC | Three-dimensional point cloud labeling using distance field data |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114930391A (en) * | 2020-07-20 | 2022-08-19 | 深圳元戎启行科技有限公司 | Map updating method, map updating device, computer equipment and storage medium |
| CN112380312B (en) * | 2020-11-30 | 2022-08-05 | 北京智行者科技股份有限公司 | Laser map updating method based on grid detection, terminal and computer equipment |
| CN112987010A (en) * | 2021-01-28 | 2021-06-18 | 上海思岚科技有限公司 | System and method for multi-radar mapping of robot |
| CN113640822B (en) * | 2021-07-07 | 2023-08-18 | 华南理工大学 | High-precision map construction method based on non-map element filtering |
| CN115527034B (en) * | 2022-10-26 | 2023-08-01 | 北京亮道智能汽车技术有限公司 | Vehicle end point cloud dynamic and static segmentation method, device and medium |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9619933B2 (en) * | 2014-06-16 | 2017-04-11 | Occipital, Inc | Model and sizing information from smartphone acquired image sequences |
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN108320329B (en) * | 2018-02-02 | 2020-10-09 | 维坤智能科技（上海）有限公司 | 3D map creation method based on 3D laser |
| CN109737974B (en) * | 2018-12-14 | 2020-11-27 | 中国科学院深圳先进技术研究院 | 3D navigation semantic map updating method, device and equipment |
| CN110221603B (en) * | 2019-05-13 | 2020-08-14 | 浙江大学 | Remote obstacle detection method based on laser radar multi-frame point cloud fusion |
-
2020
- 2020-04-21 CN CN202010315513.7A patent/CN111210475B/en active Active
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP4024349A1 (en) * | 2020-12-29 | 2022-07-06 | Waymo LLC | Three-dimensional point cloud labeling using distance field data |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111210475A (en) | 2020-05-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111210475B (en) | Map updating method and device | |
| CN111325843B (en) | Real-time semantic map construction method based on semantic inverse depth filtering | |
| CN110009718B (en) | Three-dimensional high-precision map generation method and device | |
| US20190086543A1 (en) | Method And Apparatus For Tracking Obstacle | |
| CN111462207A (en) | RGB-D simultaneous positioning and map creation method integrating direct method and feature method | |
| CN111443359B (en) | Positioning method, device and equipment | |
| CN111429574A (en) | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion | |
| Wang et al. | Latte: accelerating lidar point cloud annotation via sensor fusion, one-click annotation, and tracking | |
| CN109635816B (en) | Lane line generation method, apparatus, device, and storage medium | |
| CN110900602B (en) | Positioning recovery method and device, robot and storage medium | |
| CN112904369B (en) | Robot repositioning method, apparatus, robot, and computer-readable storage medium | |
| JP2021175972A (en) | Method, apparatus, computing device, storage medium, and computer program for detecting environmental change | |
| CN115236644A (en) | Laser radar external parameter calibration method, device, equipment and storage medium | |
| CN115326051A (en) | Positioning method and device based on dynamic scene, robot and medium | |
| CN114066773B (en) | Dynamic object removal based on point cloud characteristics and Monte Carlo expansion method | |
| CN115861547B (en) | Model surface spline generating method based on projection | |
| CN112308917A (en) | Vision-based mobile robot positioning method | |
| CN113218408A (en) | Multi-sensor fusion 2Dslam method and system suitable for multiple terrains | |
| CN112630798B (en) | Method and apparatus for estimating ground | |
| CN116921932A (en) | Welding track recognition method, device, equipment and storage medium | |
| CN117031491A (en) | Map construction method and device, automatic navigation trolley and electronic equipment | |
| CN116358528A (en) | Map updating method, map updating device, self-mobile device and storage medium | |
| CN115619954A (en) | Sparse semantic map construction method, device, equipment and storage medium | |
| CN115937825A (en) | Robust lane line generation method and device under BEV (beam-based attitude vector) of on-line pitch angle estimation | |
| CN111208785B (en) | Motion control method and device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |