CN110763243B - Sliding map updating method and device - Google Patents
Sliding map updating method and device Download PDFInfo
- Publication number
- CN110763243B CN110763243B CN201810848241.XA CN201810848241A CN110763243B CN 110763243 B CN110763243 B CN 110763243B CN 201810848241 A CN201810848241 A CN 201810848241A CN 110763243 B CN110763243 B CN 110763243B
- Authority
- CN
- China
- Prior art keywords
- moment
- point cloud
- laser radar
- sliding map
- map
- 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
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The embodiment of the application provides a sliding map updating method and device, and the sliding map is updated according to the accumulated distance dimension by adding dimension information of accumulated distance dimension to point cloud features, so that the point cloud features near recently observed areas in the sliding map can be reserved, and the effectiveness of the point cloud features in the sliding map is guaranteed; redundant point cloud features which are not observed for a long time in the sliding map can be deleted, and the simplification and consistency of the sliding map are guaranteed. Furthermore, the sliding map updating method provided by the embodiment of the application is essentially an updating method based on the spatial distance, so that the sliding map can be ensured not to shrink to the current observation range due to the fact that the laser radar moves for a long time and is still.
Description
Technical Field
The application relates to the technical field of machine vision, in particular to a sliding map updating method and device.
Background
The lidar odometer is an algorithm for obtaining motion estimation of a lidar from continuous lidar observation data (i.e. observed point cloud features) by using a feature matching algorithm, wherein the motion estimation refers to a relative pose between two adjacent moments. In order to obtain accurate real-time motion estimation of a laser radar, a commonly used method for realizing a laser radar odometer is as follows: maintaining a Map, and matching (Scan to Map) the observed point cloud characteristics with the point cloud characteristics in the Map in the movement process of the laser radar to obtain the difference between the point cloud characteristics and the point cloud characteristics, so as to obtain the movement estimation of the laser radar. In the implementation method of the laser radar odometer, the way of maintaining the map by the laser radar odometer has a great influence on the performance.
In one approach, the maintenance map may be updated in a global map approach: after new point cloud characteristics are observed, motion estimation and a new global pose of the laser radar are obtained through a characteristic matching algorithm, wherein the global pose refers to the pose relative to a global coordinate system at a certain moment; based on the global pose and the feature association information of the matching algorithm, the point cloud features bearing the new environment information are merged into a global map. The method does not delete the point cloud characteristics of the map, so that the size of the map can be continuously increased, the calculated amount and the storage amount are increased along with the increase of the map, and the real-time performance cannot be guaranteed at the later stage. In a smaller environment, when the motion track closed loop accumulation error of the laser radar odometer based on a global map mode is smaller, the function of global SLAM (Simultaneous Localization And Mapping) can be realized; however, in a larger environment, when the global motion trajectory closed loop accumulated error is larger, the motion estimation jump is easy to occur due to the inconsistency of the map point cloud characteristics at the closed loop.
In another mode, the maintenance map can also be updated in a sliding map mode: the sliding map is a map display mode in which the sliding map slides on a global coordinate system with the movement of the laser radar or the carrier carrying the laser radar as a center. After effective motion estimation, deleting the area with smaller information quantity provided for motion estimation in a certain mode, and then selecting and adding a new area from the scanning data to the map, thereby realizing iterative update.
Disclosure of Invention
The embodiment of the application provides a sliding map updating method and device, and aims to solve the problems that the effective area of a sliding map is difficult to screen and the sliding map is contracted possibly in the sliding map updating process.
Specifically, the embodiment of the application is realized by the following technical scheme:
in a first aspect of the embodiments of the present application, a sliding map updating method is provided, including:
acquiring a sliding map at the K-1 moment and a point cloud characteristic observed by a laser radar at the K moment;
updating the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment to obtain the sliding map at the K moment; wherein the update operation is:
correlating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1;
adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment;
aiming at each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1;
calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
In a second aspect of the embodiments of the present application, a sliding map updating apparatus is provided, which has a function of implementing the method provided in the first aspect. The functions can be realized by hardware, and the functions can also be realized by executing corresponding software by hardware. The hardware or software includes one or more modules or units corresponding to the above functions.
In one implementation, the apparatus may include:
the acquisition module is used for acquiring a sliding map at the K-1 moment and point cloud characteristics observed by the laser radar at the K moment;
the updating module is used for executing updating operation on the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment to obtain the sliding map at the K moment; wherein the update operation is: correlating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1; adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment; aiming at each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1; calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
In another implementation, the apparatus may include a processor, a memory, and a bus, where the processor and the memory are connected to each other through the bus; the memory stores machine-readable instructions, and the processor executes the method provided by the first aspect of the embodiments of the present application by calling the machine-readable instructions.
According to the method and the device, the dimension information of 'accumulated distance dimension' is newly added to the point cloud features, and the sliding map is updated according to the 'accumulated distance dimension', so that the point cloud features near the recently observed area in the sliding map can be reserved, and the effectiveness of the point cloud features in the sliding map is guaranteed; redundant point cloud features which are not observed for a long time in the sliding map can be deleted, and the simplification and consistency of the sliding map are guaranteed. Furthermore, the sliding map updating method provided by the embodiment of the application is essentially an updating method based on the spatial distance, so that the sliding map can be ensured not to shrink to the current observation range due to the fact that the laser radar moves for a long time and is still.
Drawings
Fig. 1 is a schematic diagram of an application scenario provided in an embodiment of the present application;
FIG. 2 is a flow chart of the operation of the apparatus for operating a lidar odometer provided by an embodiment of the present application at each segment;
fig. 3 is a flowchart of a sliding map updating method provided in an embodiment of the present application;
fig. 4 is a schematic diagram of motion estimation hopping provided by an embodiment of the present application;
fig. 5 is a block diagram of device modules according to an embodiment of the present application.
Detailed Description
If the lidar odometer is realized by matching the observed point cloud characteristics with a map maintained by the odometer to obtain the motion estimation of the lidar, wherein the map maintained by the lidar odometer can be a global map or a sliding map, then when the motion estimation of the lidar is calculated based on the newly observed point cloud characteristics, only part of the area in the global map can provide reference information required by the motion estimation due to the limitation of the observation range and the view angle of the lidar, namely the global map has quite a lot of redundant information. Compared with the global map, the laser radar odometer based on the sliding map has the advantages that the calculated amount and the memory amount are smaller and more stable, the precision loss is small, the problem of map feature inconsistency can be reduced or avoided, and the method is a more superior implementation mode.
The sliding map maintained by the laser radar odometer is a map with a limited size, which takes a laser radar (or a carrier carrying the laser radar) as a center and slides on a global coordinate system (also called a world coordinate system) along with the movement of the laser radar or the carrier, and can be used for estimating the movement estimation of the laser radar. In order to obtain accurate real-time motion estimation of the laser radar, the sliding map needs to be updated, including updating some dimension information of point cloud features in the map, and adding and deleting the point cloud features in the map. The point cloud features are mainly added in two areas, namely a newly observed area and an area with sparse original features, so that the sliding map can represent the surrounding environment fully and uniformly. And two types of point cloud features, namely the point cloud features of an area which is difficult to observe by the laser radar at the current pose and the point cloud features which are inconsistent with the current pose of the laser radar due to the accumulation of motion track errors, are required to be deleted when the point cloud features are deleted, so that the continuity of motion estimation is ensured. The update algorithm of the sliding map directly determines the performance of the lidar odometer. Three possible sliding map update methods are listed below:
in a first possible sliding map updating method, when the distance between a map point cloud feature and a laser radar exceeds a certain threshold, the map point cloud feature is deleted, thereby ensuring that only close-range map point cloud features are maintained. The disadvantages of this method are: the distance threshold is difficult to set, the map range is too small due to the smaller distance threshold, sufficient map point cloud features cannot be guaranteed to be matched with the observation point cloud features, and the precision and the robustness of motion estimation are further influenced. The map range is too large due to a large distance threshold, the map point cloud feature quantity is increased, the matching speed is reduced, even in some annular scenes, the consistency of the map point cloud features is damaged due to track accumulation errors, and the problem of motion estimation jumping similar to that of an odometer using a global map occurs.
In a second possible sliding map updating method, any point cloud feature in the sliding map may include a timestamp associated with the last observed data, and when deleting the point cloud feature of the map, the point cloud feature with the timestamp longer than the current time is selected for deletion. The main disadvantages of this method are: when the laser radar is in a static state for a long time, the point cloud characteristics of the map can be contracted into a current observation area, the distribution is not uniform any more in space, and when the laser radar moves again, the motion estimation is easy to be inaccurate.
In a third possible sliding map update method, after each map expansion, if the number of map point cloud features exceeds a threshold, the map point cloud features are randomly pruned. The point cloud characteristics of the current observation area are continuously added from the new observation point cloud characteristics, and the areas which are not observed all the time are not supplemented by the new point cloud characteristics, so that the point cloud characteristics of the areas which are not observed any more are gradually deleted. The main disadvantages of this method are: the point cloud feature deletion rate with high randomness and high probability can ensure that a non-observation area is rapidly reduced, but the point cloud features of an observation area can be greatly deleted, so that the feature density of the area is reduced, and the precision of the next motion estimation is influenced; and the point cloud feature deletion with low probability cannot rapidly reduce the number of point cloud features in a non-observation area, so that the matching calculation amount is increased. In addition, when the laser radar is stationary for a long time, the point cloud features of the map shrink to the current observation area.
Therefore, the embodiment of the application provides a new sliding map updating scheme, which can at least solve the problems of difficulty in screening the effective area of the sliding map and shrinkage of the sliding map in the sliding map updating process.
The sliding map updating method provided by the embodiment of the application can be applied to equipment for operating the laser radar odometer, and the equipment has certain computing capacity and can be a personal computer, a server, a mobile phone and the like. The relative position between this equipment and lidar is not restricted in this application embodiment, as long as this equipment can communicate with lidar, for example this equipment can carry on same carrier with lidar, or this equipment can place on different carriers with lidar. The application scenario of the embodiment of the present application and the functions performed by the device operating the lidar odometer in the application scenario are first exemplified as follows.
For example, referring to fig. 1, a carrier (such as an unmanned vehicle, a robot, etc.) carrying a laser radar moves in an environment to be detected, and the laser radar scans the surrounding environment to obtain a point cloud feature; assuming that the point cloud feature m is observed by the laser radar at the moment K, only the coordinate of the point cloud feature m in the laser radar coordinate system R can be obtained according to the observation information, the global pose of the laser radar in the global coordinate system W needs to be further estimated, and then the global coordinate of the point cloud feature m can be calculated through coordinate transformation. To obtain a motion estimate for the lidar, the lidar may transmit the scanned point cloud features in real-time to a device operating the lidar odometer, triggering the device to perform the method shown in fig. 2:
the method shown in fig. 2 is only a segment executed during the operation of the device, and the device estimates the motion of the laser radar in real time by repeating the segment continuously. Taking the estimation of the relative pose of the lidar at time K with respect to time K-1 as an example, the steps performed by the apparatus operating the lidar odometer in each segment will be described:
step 201: and acquiring a sliding map at the moment K-1 and the point cloud characteristics observed by the laser radar at the moment K.
The point cloud features are multi-dimensional point data obtained by scanning the surrounding environment with a laser radar. The dimensions of the sliding map are determined by the information dimensions carried by the point cloud features. In the embodiment of the application, information carried by any point cloud feature in the sliding map can have the following seven dimensions: 1) the value of X; 2) y value: 3) the value of Z; 4) a normal vector X component; 5) a normal vector Y component; 6) a normal vector Z component; 7) the distance dimensions are accumulated. The information of the first 6 dimensions can be used for point cloud feature matching to estimate the relative pose of the laser radar and update the motion track of the laser radar; the 7 th dimension is a newly added dimension in the embodiment of the present application, is used to indicate an accumulated movement distance between the position where the laser radar is located and the position where the laser radar is located when each point cloud feature is observed by the laser radar for the last time, and may be used to update the sliding map, and how to update the sliding map according to the accumulated distance dimension of the point cloud feature will be described below, which will not be described in detail here.
In the embodiment of the application, each time corresponds to a frame of point cloud characteristics observed by the laser radar at the time, and the time interval between adjacent times (such as the K-1 time and the K time) can be adjusted according to the computing capacity of the device for operating the laser radar odometer. For example, in the case of insufficient computing power of the device, the time interval between adjacent time instants may be adjusted to be large, so that the relatively low-frequency lidar relative pose estimation may be realized through point cloud features of discontinuous frames.
Step 202: and preliminarily registering the point cloud characteristics observed by the laser radar at the K moment to a global coordinate system according to the global pose of the laser radar at the K-1 moment.
And preliminarily registering the point cloud characteristics observed at the moment K to a global coordinate system, namely calculating the rough position coordinates of the point cloud characteristics observed at the moment K in the global coordinate system. The point cloud characteristics observed by the laser radar are relative to a laser radar coordinate system, so the point cloud characteristics observed by the laser radar need to be registered in a global coordinate system; at the moment, the global pose of the laser radar at the moment K is unknown, and the global pose of the laser radar at the moment K-1 is calculated in the last round of motion estimation, so that the point cloud feature observed by the laser radar at the moment K can be added with the pose of the laser radar at the moment K-1 in the global coordinate system, the point cloud feature observed by the laser radar at the moment K is converted into the rough position coordinate of the point cloud feature observed at the moment K in the global coordinate system relative to the laser radar coordinate system, and the rough position coordinate of the point cloud feature observed at the moment K in the global coordinate system is obtained.
Here, the global pose of the lidar at time K-1 may be accumulated from the relative poses of the lidar at each time prior to time K with respect to the previous time (i.e., the relative pose at time K-1 with respect to time K-2, the relative pose at time K-2 with respect to time K-3, and the relative pose at time … … 1 with respect to time 0).
In one embodiment, after the device operating the lidar odometer obtains a single-frame point cloud feature from the lidar, if the movement speed of the lidar is high, the point cloud feature may be added with motion compensation to correct the point cloud data distortion caused by the movement, and then the corrected point cloud feature is preliminarily registered in a world coordinate system.
Step 203: and calculating the global position and the relative position of the laser radar at the K moment relative to the K-1 moment according to the point cloud characteristics observed at the K moment after the initial registration and the point cloud characteristics of the sliding map at the K-1 moment, and accurately registering the point cloud characteristics observed at the K moment to a global coordinate system according to the global position and the relative position of the laser radar at the K moment relative to the K-1 moment.
In one example, ICP (Iterative Closest Point matching algorithm) can be used to calculate the relative pose of the lidar, the process is briefly as follows:
1) taking the point cloud characteristics in the sliding map at the K-1 moment as an X1 point set, and taking the point cloud characteristics observed at the K moment as an X2 point set; calculating the corresponding near point of each point in the X2 point set in the X1 point set, wherein any point in the X2 point set and the corresponding near point in the X1 point set form a related point pair;
in the step 1), the searching speed of the nearest point can be accelerated by constructing a kd tree by using the point cloud characteristics of the sliding map at the moment K-1.
2) After obtaining the associated point pairs, deleting the associated point pairs with larger associated distance or larger normal vector difference;
the reason is that the correct associated point pair should be in a similar position in space, the distance between the two points constituting the associated point pair should be close, and the normal vectors of the two point cloud features should also be similar; if the distance is far or the difference of the normal vectors is large, it indicates that the two points do not represent the same object surface and should not be associated together.
2) Obtaining rigid body transformation which enables the average distance of the associated point pairs to be minimum, and obtaining translation parameters and rotation parameters;
3) obtaining a new transformation point set by using the translation parameter and the rotation parameter obtained in the previous step for the X2 point set;
4) and if the new transformation point set and the X1 point set meet that the average distance between the two point sets is less than a given threshold, stopping iterative computation, otherwise, taking the new transformation point set as a new X2 point set to continue iteration until the requirement is met.
And finally obtaining the translation parameter and the rotation parameter which meet the requirements, namely the global pose of the laser radar at the moment K. Then, the relative position of the laser radar at the time K relative to the time K-1 can be obtained by comparing the global positions of the laser radar at the time K and the global position of the laser radar at the time K-1; and correcting the rough position coordinates of the point cloud features observed at the K moment in a global coordinate system through the global pose of the laser radar at the K moment, so as to realize accurate registration.
Step 204: judging whether the calculated relative pose of the laser radar at the time K relative to the time K-1 is reasonable or not according to the motion model of the laser radar; if so, continue to step 205.
In one embodiment, the lidar and the lidar-mounted carrier are rigidly connected, so that the motion model of the carrier can be considered as the motion model of the lidar. Assuming that the carrier is a vehicle, the vehicle usually adopts a differential motion model in motion, and the motion of the vehicle is limited to only longitudinal motion in a plane. If the computed laser radar has an obvious vertical jump from the moment K-1 to the moment K, and the vertical jump is obviously out of the limit range of the differential motion model, the computed laser radar can be considered to have an unreasonable relative pose at the moment K relative to the moment K-1. Unreasonable estimation of the relative pose means that the point cloud features are not accurately matched, and if the sliding map is updated, newly added point cloud features may be in wrong positions, so that subsequent errors in calculation of the relative pose may be caused. Therefore, the relative pose calculation result of step 203 can be discarded, step 205 is not executed any more, the point cloud feature observed by the laser radar at the moment K +1 is waited to be received, and the relative pose of the laser radar at the moment K +1 relative to the moment K-1 is continuously calculated.
Step 205: and when the calculated relative pose of the laser radar at the time K relative to the time K-1 is determined to be reasonable, updating the sliding map at the time K-1 according to the point cloud characteristics observed at the time K after accurate registration.
Next, the procedure of performing the update operation on the sliding map at the time K-1 in step 205 will be described with reference to fig. 3:
step 301: and associating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1.
In an example, the implementation procedure of step 301 may refer to the implementation procedure of step 203, which includes: taking the point cloud characteristics in the sliding map at the K-1 moment as an X1 point set, and taking the point cloud characteristics observed at the K moment as an X2 point set; correlating the X1 point set with the X2 point set, namely calculating corresponding near points of each point in the X2 point set in the X1 point set to obtain a correlated point pair; and after obtaining the associated point pairs, deleting the associated point pairs with larger associated distance or larger normal vector difference. And finally, the point cloud features in the reserved association point pairs are the point cloud features associated.
Step 302: and adding the point cloud characteristics into a sliding map at the K-1 moment aiming at each point cloud characteristic which is not associated in the point cloud characteristics observed at the K moment.
As described above, in the sliding map provided in the embodiment of the present application, any map point cloud feature carries 7 dimensions, namely, an X value, a Y value, a Z value, a normal vector X component, a normal vector Y component, a normal vector Z component, and an accumulated distance dimension, where the accumulated distance dimension is used to represent an accumulated movement distance between a position where the laser radar is located when the point cloud feature is observed by the laser radar last time and a position where the laser radar is located at a current time. For example, if the laser radar observes a certain point cloud feature m at time K-1 and observes the point cloud feature m again at time K, the accumulated distance dimension value of the point cloud feature m at time K is 0. And assuming that the laser radar observes the cloud feature n at a certain point at the time K-1, but does not observe the cloud feature n at the time K, wherein the accumulated distance dimension value of the cloud feature n at the time K is the movement distance of the laser radar from the time K-1 to the time K. The time interval between adjacent moments is usually relatively small, so the straight-line distance from the position of the lidar at the moment K-1 to the position at the moment K can be used to represent the movement distance of the lidar from the moment K-1 to the moment K.
In step 302, there is a point cloud feature that is not associated in the point cloud features observed at the time K, which indicates that the sliding map at the time K-1 lacks area information corresponding to the point cloud feature, and therefore the point cloud feature that is not associated needs to be added into the sliding map; meanwhile, the point cloud feature is observed at the moment K, so that the value of the accumulated distance dimension of the point cloud feature is set to be zero.
Step 303: and for each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1.
Here, there is a point cloud feature associated in the point cloud features observed at the time K, which indicates that the region corresponding to the point cloud feature is being observed by the laser radar, and therefore, the point cloud feature needs to be retained in the sliding map at the time K-1, and the value of the accumulated distance dimension of the point cloud feature is reset to zero.
Step 304: calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
Here, the existence of the point cloud feature in the sliding map at the time K-1 is not associated, which indicates that the area corresponding to the point cloud feature is not observed by the laser radar at the time K. On the premise of no closed-loop repeated observation, the accumulated error of the global motion track of the laser radar can be considered to be in direct proportion to the accumulated motion distance of the laser radar, and when a certain point cloud feature is observed for the last time, the distance between the position of the laser radar and the position of the laser radar at the current moment is larger, the point cloud feature is not suitable for being matched with the point cloud feature observed in the future in a sliding map; the deletion of such point cloud features from the sliding map can be accomplished via step 304. In one example, the implementation of step 304 is as follows:
1) acquiring the movement distance of the laser radar from the K-1 moment to the K moment;
here, the movement distance of the lidar from the time K-1 to the time K can be calculated according to the relative pose of the lidar at the time K relative to the time K-1 obtained in step 203.
2) For each point cloud feature not associated in the sliding map at the time K-1, performing the following processing:
a. adding the value of the accumulated distance dimension of the point cloud feature to the movement distance of the laser radar from the K-1 moment to the K moment to obtain a sum value, wherein the sum value represents the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud feature is observed by the laser radar for the last time;
b. judging whether the sum is larger than a first set threshold value;
c. if the sum is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the K-1 moment;
d. if the sum value is smaller than a first set threshold value, the point cloud feature is reserved in the sliding map at the moment K-1, and the value of the accumulated distance dimension of the point cloud feature is updated to be the sum value.
Optionally, in the implementation process of step 304, after obtaining the movement distance of the laser radar from the time K-1 to the time K, the device operating the laser radar odometer may first determine whether the movement distance of the laser radar from the time K-1 to the time K is smaller than a second set threshold; if yes, the calculated movement distance of the laser radar from the K-1 moment to the K moment is considered to be caused by the matching error, the laser radar does not move, therefore, the movement distance of the laser radar from the K-1 moment to the K moment is forcibly set to be zero, and then the step 2 is executed; otherwise, directly executing step 2. Therefore, the problem that the consistency of the point cloud characteristics of the sliding map is damaged and the motion estimation jumping occurs due to the motion track accumulated error can be solved. Referring to fig. 4, a carrier carrying the laser radar walks around a large obstacle (such as a tall building), and due to an accumulated error of motion trajectory estimation, a difference between an estimated motion trajectory (a dotted line) and a real motion trajectory (a solid line) increases with an increase in motion distance, and a map feature (black five-pointed star) and an observation feature (white five-pointed star) of the same feature are inconsistent, that is, the motion estimation jumps.
Finally, through the steps 301 and 304, the sliding map at the time K can be obtained.
To this end, the flow shown in fig. 2 and 3 is completed.
As can be seen from the above process, in the embodiment of the application, by adding dimension information of "accumulated distance dimension" to the point cloud features and updating the sliding map according to the "accumulated distance dimension", the point cloud features near the recently observed area in the sliding map can be retained, and the validity of the point cloud features in the sliding map is ensured; redundant point cloud features which are not observed for a long time in the sliding map can be deleted, and the simplification and consistency of the sliding map are guaranteed. Furthermore, the sliding map updating method provided by the embodiment of the application is essentially an updating method based on the spatial distance, so that the sliding map can be ensured not to shrink to the current observation range due to the fact that the laser radar moves for a long time and is still.
The method provided by the embodiment of the application is described above. The following describes the apparatus provided in the embodiments of the present application.
Referring to fig. 5, fig. 5 is a functional block diagram of a sliding map updating apparatus according to an embodiment of the present application. As shown in fig. 5, the apparatus includes:
the acquisition module 501 is configured to acquire a sliding map at the time K-1 and a point cloud feature observed by a laser radar at the time K;
the updating module 502 is configured to perform an updating operation on the sliding map at the time K-1 according to the point cloud characteristics observed at the time K, so as to obtain the sliding map at the time K; wherein the update operation is: correlating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1; adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment; aiming at each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1; calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
In one embodiment, the dimension information carried by the point cloud features in the sliding map comprises an accumulated distance dimension, and the accumulated distance dimension is used for representing the accumulated movement distance between the position where the laser radar is located and the position where the laser radar is located when each point cloud feature is observed by the laser radar for the last time;
the updating module 502 is configured to obtain a movement distance from the time K-1 to the time K of the laser radar; for each point cloud feature not associated in the sliding map at the time K-1, performing the following processing: adding the value of the accumulated distance dimension of the point cloud feature to the movement distance of the laser radar from the K-1 moment to the K moment to obtain a sum value, wherein the sum value represents the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud feature is observed by the laser radar for the last time; judging whether the sum is larger than a first set threshold value or not; if the sum is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the K-1 moment; if the sum value is smaller than a first set threshold value, the point cloud feature is reserved in the sliding map at the moment K-1, and the value of the accumulated distance dimension of the point cloud feature is updated to be the sum value.
In one embodiment, the updating module 502 is configured to, for each point cloud feature that is not associated in the point cloud features observed at time K, add the point cloud feature to the sliding map at time K-1, and set a value of an accumulated distance dimension of the point cloud feature to zero; and for each point cloud feature associated in the sliding map at the moment K-1, reserving the point cloud feature in the sliding map at the moment K-1 and resetting the value of the accumulated distance dimension of the point cloud feature to zero.
In one embodiment, the updating module 502 is further configured to, after obtaining the movement distance of the lidar from the time K-1 to the time K, determine whether the movement distance of the lidar from the time K-1 to the time K is smaller than a second set threshold before performing the processing for each point cloud feature that is not associated in the sliding map at the time K-1; and if so, setting the movement distance of the laser radar from the K-1 moment to the K moment to be zero.
In one embodiment, the updating module 502 is further configured to preliminarily register, to a global coordinate system, a point cloud feature observed by the lidar at a time K according to a global pose of the lidar at the time K-1; calculating the global position and the relative position of the laser radar at the K moment relative to the K-1 moment according to the point cloud characteristics observed at the K moment after the initial registration and the point cloud characteristics of the sliding map at the K-1 moment, and accurately registering the point cloud characteristics observed at the K moment to a global coordinate system according to the global position and the relative position of the laser radar at the K moment relative to the K-1 moment; judging whether the calculated relative pose of the laser radar at the time K relative to the time K-1 is reasonable or not according to a motion model of the laser radar; and if the sliding map is reasonable, updating the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment after accurate registration.
It should be noted that the division of the unit in the embodiment of the present application is schematic, and is only a logic function division, and there may be another division manner in actual implementation. The functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The implementation process of the functions and actions of each unit in the above device is specifically described in the implementation process of the corresponding step in the above method, and is not described herein again.
The description of the apparatus shown in fig. 5 is thus completed.
The embodiment of the application also provides a sliding map updating device, which comprises a processor, a memory and a bus, wherein the processor and the memory are mutually connected through the bus; the memory stores machine readable instructions which the processor invokes to implement the methods shown in fig. 2 and 3.
Further, a machine-readable storage medium is provided in embodiments of the present application, which stores machine-readable instructions that, when invoked and executed by a processor, cause the processor to implement the methods shown in fig. 2 and 3.
The above description is only exemplary of the present application and should not be taken as limiting the present application, as any modification, equivalent replacement, or improvement made within the spirit and principle of the present application should be included in the scope of protection of the present application.
Claims (10)
1. A sliding map updating method, comprising:
acquiring a sliding map at the K-1 moment and a point cloud characteristic observed by a laser radar at the K moment;
updating the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment to obtain the sliding map at the K moment; wherein the update operation is:
correlating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1;
adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment;
aiming at each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1;
calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
2. The method of claim 1, wherein dimension information carried by the point cloud features in the sliding map comprises an accumulated distance dimension, and the accumulated distance dimension is used for representing an accumulated movement distance between a position where the laser radar is located and a position where the laser radar is located at the current moment when each point cloud feature is observed by the laser radar for the last time;
calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; if the accumulated movement distance is larger than a first set threshold, deleting the point cloud feature from the sliding map at the moment K-1, wherein the method comprises the following steps:
acquiring the movement distance of the laser radar from the K-1 moment to the K moment;
for each point cloud feature not associated in the sliding map at the time K-1, performing the following processing:
adding the value of the accumulated distance dimension of the point cloud feature to the movement distance of the laser radar from the K-1 moment to the K moment to obtain a sum value, wherein the sum value represents the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud feature is observed by the laser radar for the last time;
judging whether the sum is larger than a first set threshold value or not;
if the sum is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the K-1 moment;
if the sum value is smaller than a first set threshold value, the point cloud feature is reserved in the sliding map at the moment K-1, and the value of the accumulated distance dimension of the point cloud feature is updated to be the sum value.
3. The method of claim 2, wherein adding the point cloud features to a sliding map at time K-1 for each of the observed point cloud features not associated with it comprises:
adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment, and setting the value of the accumulated distance dimension of the point cloud features as zero;
the method for keeping the point cloud characteristics in the sliding map at the time K-1 aiming at each point cloud characteristic associated in the sliding map at the time K-1 comprises the following steps:
and aiming at each point cloud feature associated in the sliding map at the moment K-1, reserving the point cloud feature in the sliding map at the moment K-1 and resetting the value of the accumulated distance dimension of the point cloud feature to zero.
4. The method of claim 2, wherein after obtaining the distance of movement of the lidar from time K-1 to time K, before performing the processing for each point cloud feature not associated in the sliding map at time K-1, the method further comprises:
judging whether the movement distance of the laser radar from the K-1 moment to the K moment is smaller than a second set threshold value or not;
and if so, setting the movement distance of the laser radar from the K-1 moment to the K moment to be zero.
5. The method of claim 1, wherein prior to performing an update operation on the sliding map at time K-1 based on the point cloud features observed at time K, the method further comprises:
preliminarily registering the point cloud characteristics observed by the laser radar at the K moment to a global coordinate system according to the global pose of the laser radar at the K-1 moment;
calculating the global position and the relative position of the laser radar at the K moment relative to the K-1 moment according to the point cloud characteristics observed at the K moment after the initial registration and the point cloud characteristics of the sliding map at the K-1 moment, and accurately registering the point cloud characteristics observed at the K moment to a global coordinate system according to the global position and the relative position of the laser radar at the K moment relative to the K-1 moment;
judging whether the calculated relative pose of the laser radar at the time K relative to the time K-1 is reasonable or not according to a motion model of the laser radar;
and if the sliding map is reasonable, updating the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment after accurate registration.
6. A sliding map updating apparatus, comprising:
the acquisition module is used for acquiring a sliding map at the K-1 moment and point cloud characteristics observed by the laser radar at the K moment;
the updating module is used for executing updating operation on the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment to obtain the sliding map at the K moment; wherein the update operation is: correlating the point cloud characteristics observed at the moment K with the point cloud characteristics of the sliding map at the moment K-1; adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment; aiming at each point cloud feature associated in the sliding map at the moment K-1, keeping the point cloud feature in the sliding map at the moment K-1; calculating the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud characteristics are observed by the laser radar for the last time aiming at each point cloud characteristic which is not associated in the sliding map at the K-1 moment; and if the accumulated movement distance is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the moment K-1.
7. The device as claimed in claim 6, wherein the dimension information carried by the point cloud features in the sliding map includes a cumulative distance dimension, and the cumulative distance dimension is used for representing a cumulative moving distance between the position of the lidar when each point cloud feature is observed by the lidar last time and the position of the lidar at the current moment;
the updating module is used for acquiring the movement distance of the laser radar from the K-1 moment to the K moment; for each point cloud feature not associated in the sliding map at the time K-1, performing the following processing: adding the value of the accumulated distance dimension of the point cloud feature to the movement distance of the laser radar from the K-1 moment to the K moment to obtain a sum value, wherein the sum value represents the accumulated movement distance between the position of the laser radar and the position of the laser radar at the K moment when the point cloud feature is observed by the laser radar for the last time; judging whether the sum is larger than a first set threshold value or not; if the sum is larger than a first set threshold value, deleting the point cloud feature from the sliding map at the K-1 moment; if the sum value is smaller than a first set threshold value, the point cloud feature is reserved in the sliding map at the moment K-1, and the value of the accumulated distance dimension of the point cloud feature is updated to be the sum value.
8. The apparatus of claim 7,
the updating module is used for adding the point cloud features into a sliding map at the K-1 moment aiming at each point cloud feature which is not associated in the point cloud features observed at the K moment, and setting the value of the accumulated distance dimension of the point cloud features to be zero; and for each point cloud feature associated in the sliding map at the moment K-1, reserving the point cloud feature in the sliding map at the moment K-1 and resetting the value of the accumulated distance dimension of the point cloud feature to zero.
9. The apparatus of claim 7,
the updating module is further used for judging whether the movement distance of the laser radar from the K-1 moment to the K moment is smaller than a second set threshold value or not after the movement distance of the laser radar from the K-1 moment to the K moment is obtained and before the processing is executed for each point cloud feature which is not associated in the sliding map at the K-1 moment; and if so, setting the movement distance of the laser radar from the K-1 moment to the K moment to be zero.
10. The apparatus of claim 6,
the updating module is also used for preliminarily registering the point cloud characteristics observed by the laser radar at the K moment to a global coordinate system according to the global pose of the laser radar at the K-1 moment; calculating the global position and the relative position of the laser radar at the K moment relative to the K-1 moment according to the point cloud characteristics observed at the K moment after the initial registration and the point cloud characteristics of the sliding map at the K-1 moment, and accurately registering the point cloud characteristics observed at the K moment to a global coordinate system according to the global position and the relative position of the laser radar at the K moment relative to the K-1 moment; judging whether the calculated relative pose of the laser radar at the time K relative to the time K-1 is reasonable or not according to a motion model of the laser radar; and if the sliding map is reasonable, updating the sliding map at the K-1 moment according to the point cloud characteristics observed at the K moment after accurate registration.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810848241.XA CN110763243B (en) | 2018-07-27 | 2018-07-27 | Sliding map updating method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810848241.XA CN110763243B (en) | 2018-07-27 | 2018-07-27 | Sliding map updating method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110763243A CN110763243A (en) | 2020-02-07 |
| CN110763243B true CN110763243B (en) | 2021-08-24 |
Family
ID=69328610
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810848241.XA Active CN110763243B (en) | 2018-07-27 | 2018-07-27 | Sliding map updating method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110763243B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113776544A (en) * | 2020-06-10 | 2021-12-10 | 杭州海康威视数字技术股份有限公司 | Point cloud map updating method and device, electronic equipment and positioning system |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105547305A (en) * | 2015-12-04 | 2016-05-04 | 北京布科思科技有限公司 | Pose solving method based on wireless positioning and laser map matching |
| 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 |
| CN106599108A (en) * | 2016-11-30 | 2017-04-26 | 浙江大学 | Method for constructing multi-mode environmental map in three-dimensional environment |
| CN106840176A (en) * | 2016-12-28 | 2017-06-13 | 济宁中科先进技术研究院有限公司 | GPS space-time datas increment road network real-time update and path matching system |
| CN107741745A (en) * | 2017-09-19 | 2018-02-27 | 浙江大学 | It is a kind of to realize mobile robot autonomous positioning and the method for map structuring |
| CN107764270A (en) * | 2017-10-19 | 2018-03-06 | 武汉工控仪器仪表有限公司 | A kind of laser scan type indoor map generation and updating device and method |
| CN108007453A (en) * | 2017-12-11 | 2018-05-08 | 北京奇虎科技有限公司 | Map updating method, device and electronic equipment based on a cloud |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104359430B (en) * | 2014-10-14 | 2017-07-11 | 华南农业大学 | A kind of dynamic paddy field flatness detecting device and method based on laser ranging |
| CN106556843A (en) * | 2015-09-28 | 2017-04-05 | 东莞前沿技术研究院 | Dimensional topography mapping system and mapping method |
| GB2550567A (en) * | 2016-05-20 | 2017-11-29 | Nokia Technologies Oy | Point Cloud Matching Method |
| CN107918753B (en) * | 2016-10-10 | 2019-02-22 | 腾讯科技（深圳）有限公司 | Processing Method of Point-clouds and device |
| US10422639B2 (en) * | 2016-12-30 | 2019-09-24 | DeepMap Inc. | Enrichment of point cloud data for high-definition maps for autonomous vehicles |
-
2018
- 2018-07-27 CN CN201810848241.XA patent/CN110763243B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
| CN105547305A (en) * | 2015-12-04 | 2016-05-04 | 北京布科思科技有限公司 | Pose solving method based on wireless positioning and laser map matching |
| CN106599108A (en) * | 2016-11-30 | 2017-04-26 | 浙江大学 | Method for constructing multi-mode environmental map in three-dimensional environment |
| CN106840176A (en) * | 2016-12-28 | 2017-06-13 | 济宁中科先进技术研究院有限公司 | GPS space-time datas increment road network real-time update and path matching system |
| CN107741745A (en) * | 2017-09-19 | 2018-02-27 | 浙江大学 | It is a kind of to realize mobile robot autonomous positioning and the method for map structuring |
| CN107764270A (en) * | 2017-10-19 | 2018-03-06 | 武汉工控仪器仪表有限公司 | A kind of laser scan type indoor map generation and updating device and method |
| CN108007453A (en) * | 2017-12-11 | 2018-05-08 | 北京奇虎科技有限公司 | Map updating method, device and electronic equipment based on a cloud |
Non-Patent Citations (3)
| Title |
|---|
| LOAM:Lidar Odometry and Mapping in Real-time;Ji Zhang;《Conference:Robotics and System Conference》;20140731;第1-9页 * |
| 基于激光测距仪的移动机器人同时定位与地图构建研究;高强;《中国优秀硕士学位论文全文数据库》;20170315;第1-75页 * |
| 机器人认知地图创建关键技术研究;梁明杰;《中国博士学位论文全文数据库》;20141115;第1-147页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110763243A (en) | 2020-02-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112132893B (en) | Visual SLAM method suitable for indoor dynamic environment | |
| CN109974693B (en) | Unmanned aerial vehicle positioning method and device, computer equipment and storage medium | |
| CN112179330B (en) | Pose determination method and device of mobile equipment | |
| CN110631554B (en) | Robot posture determining method and device, robot and readable storage medium | |
| CN109816696A (en) | A kind of robot localization and build drawing method, computer installation and computer readable storage medium | |
| CN111968229A (en) | High-precision map making method and device | |
| CN112414403B (en) | Robot positioning and attitude determining method, equipment and storage medium | |
| CN103218827B (en) | The contour tracing method of segmentation and figure matching and correlation is combined in Shape-based interpolation transmission | |
| CN111862214B (en) | Computer equipment positioning method, device, computer equipment and storage medium | |
| CN111144213A (en) | Object detection method and related equipment | |
| CN113409459A (en) | Method, device and equipment for producing high-precision map and computer storage medium | |
| CN116449384A (en) | Radar inertial tight coupling positioning mapping method based on solid-state laser radar | |
| CN113096181B (en) | Method and device for determining equipment pose, storage medium and electronic device | |
| CN113066127A (en) | Visual inertial odometer method and system for calibrating equipment parameters on line | |
| CN113298910A (en) | Method, apparatus and storage medium for generating traffic sign line map | |
| CN110763243B (en) | Sliding map updating method and device | |
| CN113984068A (en) | Positioning method, positioning apparatus, and computer-readable storage medium | |
| CN113763548B (en) | Vision-laser radar coupling-based lean texture tunnel modeling method and system | |
| CN112308917A (en) | Vision-based mobile robot positioning method | |
| CN115619954A (en) | Sparse semantic map construction method, device, equipment and storage medium | |
| CN113885044A (en) | SLAM mapping positioning method, system and device in complex environment of excavator | |
| CN113126117A (en) | Method for determining absolute scale of SFM map and electronic equipment | |
| CN116337072A (en) | Construction method, construction equipment and readable storage medium for engineering machinery | |
| CN108090961B (en) | Rapid adjustment method in three-dimensional laser point cloud imaging | |
| CN117746388A (en) | Point cloud labeling method and device, electronic equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |