Map updating method of full-life-cycle SLAM (simultaneous localization and mapping) system
Through the map update method of the full life cycle SLAM system, 3D lidar is used to perform real-time scanning and loopback detection of prior maps, which solves the problem of map update in complex dynamic environments of the SLAM system and improves the positioning and navigation capabilities of autonomous robots.
Patent Information
- Application Number
- CN202411085135.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-08
- Publication Date
- 2024-11-15
AI Technical Summary
The SLAM system is difficult to update maps in complex dynamic environments, resulting in limited positioning and navigation capabilities of autonomous robots.
Using the map update method of the full life cycle SLAM system, loopback detection and registration is performed through real-time scanning and prior maps of 3D lidar, low-dynamic objects and semi-static objects are identified, point clouds are added and deleted, and local updates of maps are realized. .
Effectively filter out dynamic points, improve map creation effect, reduce the calculation cost of real-time SLAM, ensure the update and maintenance of environmental maps, and are suitable for complex dynamic environments.
Smart Images

Figure CN118960720A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of unmanned driving positioning and navigation, and more specifically, to a multi-sensor fusion SLAM and map updating method. Background Art
[0002] With the continuous development of science and technology and the gradual expansion of the fields involved in mobile robots, state estimation, positioning and mapping are the basic premise of a complete intelligent mobile robot, which is also necessary for many other functions such as feedback control, obstacle avoidance and planning. At present, the SLAM system has good robustness and efficiency in static environments, and the positioning and mapping effects are relatively mature, which can achieve the desired effect. However, in the real world, most environments are complex and dynamic. When there are moving objects in the scene, the accuracy of algorithms such as feature matching, pose estimation, loop detection and map construction based on dynamic objects may decrease. In a SLAM system constructed in a single period, the global map only reflects the map information of that period. However, the environmental information in the real world is complex and dynamic. In this case, the global map is difficult to transform and update. Therefore, how to update the environment in the global map is one of the key issues to improve the positioning and navigation capabilities of autonomous robots.
[0003] In the complex dynamic outdoor environment, the robustness of the SLAM system is reduced. The real environment is constantly changing, and a single SLAM system has difficulty updating the map of dynamic objects. Summary of the invention
[0004] An object of the present invention is to solve at least the above problems and / or disadvantages and to provide at least the advantages which will be described hereinafter.
[0005] In order to achieve these purposes and other advantages of the present invention, a map updating method of a full life cycle SLAM system is provided, comprising:
[0006] S1, using the environmental map of the current area obtained based on the SLAM system as a priori map;
[0007] S2. In the current period, the current area is scanned in real time by a 3D laser radar to obtain a real-time scanning frame;
[0008] S3, performing real-time loop detection and registration on the real-time scanning frame obtained by S2 and the prior map of S1, and comparing the low-dynamic objects and semi-static objects identified in the prior map and the real-time scanning frame to determine whether to add and / or delete the corresponding point cloud in the prior map, thereby completing the local update of the prior map at different times;
[0009] S4. Return to S1 until the update operation of all areas in the environment map is completed.
[0010] Preferably, in S3, the process of real-time loop detection and registration includes:
[0011] S31, after receiving new real-time scanning frame data, performing global registration between the real-time scanning frame and the prior map;
[0012] S32, by constructing the iKD-Tree structure, the point cloud data set in the prior map and real-time scanning frame data is pre-screened, the unmatched point cloud is directly eliminated through the loop factor and marking mechanism, and then the potential updated point cloud cluster set screened at time n is obtained through calculation processing {Add n , Del n}, Add in collection n is the potential increase in point cloud clusters, Del n is a potential point cloud cluster to be eliminated;
[0013] S33, will update the point cloud cluster set in the prior map {Add n ,Del n}.
[0014] Preferably, in S31, the global registration includes: performing initial alignment based on context scanning, and determining whether there is a point cloud of more than 85% of the prior map based on a radius search of posture proximity, and if so, the registration is completed.
[0015] Preferably, in S32, the point set {Add n , Del n} is obtained as follows:
[0016] S320, after receiving the new real-time scanning frame data, the point cloud layout of the prior map and the real-time scanning frame is analyzed, and the overlapping area point cloud cluster R of the two is obtained by the following formula: n :
[0017] R n =S n ∩M n
[0018] In the above formula, S n represents the prior map point cloud cluster, M n Represents a point cloud cluster of a real-time scan frame;
[0019] S320, Potentially add point cloud cluster Add n Obtained by the following formula:
[0020] Add n =S n -R n
[0021] Potentially eliminated point cloud cluster Deln Obtained by the following formula:
[0022]
[0023] In the above formula, Range(R n ) represents the range of the scanned frame corresponding to the prior map,
[0024] Preferably, it further includes: introducing a scanning perspective line to potentially eliminate point cloud clusters Del n Conduct secondary screening.
[0025] Preferably, the scanning perspective line is a straight line starting from the center of the real-time scanning frame, and the direction is determined by Add n The voxel block determines the construction equation of the scanning perspective line as follows:
[0026]
[0027] In the above formula, x, y, and z represent the x, y, and z coordinates of the point cloud to be added or deleted in space, respectively. ot ,y ot 、z ot They represent the x, y, and z coordinates of the point cloud with the earliest timestamp in Add in space. a ,y a 、z a They represent the x, y, and z coordinates of the point cloud in Add in space respectively.
[0028] Preferably, based on the construction equation of the scanning perspective line, the set of potential elimination point cloud clusters Del is traversed n to check whether the voxel block is located within the vector <p ot , p a >If the positive extension line of the defined scanning perspective line is on the positive direction, then the voxel block is considered to be occluded and the point cloud cluster Del is removed from the set n Remove it from the list, and then get the potential deleted point cloud set Del n ′;
[0029] Among them, p ot The vector representing the point cloud with the earliest timestamp, representing the range of the perspective line extended by the vector of the point cloud in ADD.
[0030] Preferably, in S3, before the global registration of the real-time scan frame with the priori map, it also includes online removal of dynamic point cloud from the current SLAM process.
[0031] The present invention has at least the following beneficial effects:
[0032] The map updating method of the full life cycle SLAM system of the present invention, wherein the SLAM of the prior map can effectively filter out dynamic points to achieve better mapping effect, and the subsequent map updating method can greatly reduce the calculation cost of real-time SLAM in unfamiliar environments, and at the same time update and maintain the environmental map.
[0033] The present invention can be used in indoor workshops or outdoor scenic spots, industrial parks and other scenes, and solves the problem that it is difficult for the SLAM system to include the latest information in maps in dynamic environments.
[0034] Other advantages, objectives and features of the present invention will be embodied in part through the following description, and in part will be understood by those skilled in the art through study and practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 It is the overall framework diagram of the map updating method in the present invention;
[0036] Figure 2 Schematic diagram of laser radar motion distortion in the present invention;
[0037] Figure 3 The ID of the laser line scanned at the same position within one frame of point cloud time in the present invention;
[0038] Figure 4 It is a diagram of incremental update of the three-dimensional view on the tree of the present invention;
[0039] Figure 5 For Figure 4 Illustration after inserting the point and tree and rebalancing;
[0040] Figure 6 An overview diagram of the point cloud updating system of the present invention;
[0041] Figure 7 A priori map of a period before the present invention;
[0042] Figure 8 This is the point cloud map after the map of the present invention is updated. DETAILED DESCRIPTION
[0043] The present invention is further described in detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.
[0044] The present invention proposes a universal lifelong positioning and mapping framework (i.e., a full-lifecycle SLAM system), which tracks changes in the scene and maintains an up-to-date map through a map update method of the full-lifecycle SLAM system, so as to accurately and robustly estimate the positioning. Specifically, the map update method of the full-lifecycle SLAM system uses the global map created by the SLAM system in the previous period as input, and the current real-time SLAM system performs feature matching with it to build a local map, obtains semi-static object point cloud information, and thus updates the map. Then, the trajectories are associated to build a global map in the same coordinate system.
[0045] In order to achieve these purposes and other advantages of the present invention, a map updating method of a full life cycle SLAM system is provided, comprising:
[0046] S1. Use the SLAM system composed of IMU and 3D laser radar physical entities from a previous period to collect and process physical data of the environment and build a map of this environment, which we call a priori map;
[0047] In S1, the SLAM system comprises:
[0048] S10. Use IMU to perform IMU pre-integration algorithm. The IMU pre-integration method includes: S101. IMU accumulates IMU measurement values between two frames to obtain an IMU pre-integration measurement value; S102. Obtain the relative posture change between the two frames; S103. IMU gives the laser odometer an initial posture.
[0049] S11, use 3D laser radar for scanning;
[0050] S12, point cloud data dedistortion and feature extraction;
[0051] In S12, the methods of dedistortion and feature extraction of point cloud data include: S121, removing invalid points and abnormal points in the original point cloud by point cloud filtering, and using radius filtering to filter out outlier point data to save time cost; S122, using voxel filtering to downsample the point cloud, which can significantly reduce the number of points while avoiding affecting the geometric structure of the point cloud; S123, obtaining t through the original data of IMU k and t k+1 The angular velocity and acceleration at each moment between the two laser radar frames can be used to obtain the rotation matrix, thereby performing rotation and translation transformation on the point cloud data between each two laser radar frames, and converting all the point clouds to t k S124: After the original point cloud data is distorted, it is necessary to extract the point surface features in the point cloud data for subsequent matching.
[0052] S13, point cloud registration;
[0053] S14, loop detection algorithm builds a global prior map.
[0054] S2. Perform a real-time 3D lidar scan of the same environment in the current period to obtain a current real-time scan frame; wherein the current period refers to the period when the map needs to be updated; the same environment refers to the maintenance of fixed environment information and maps.
[0055] S3, the current frame and the prior map are subjected to real-time loop detection and registration, and maps of different periods are compared to identify low-dynamic objects and semi-static objects, and to add and delete point clouds;
[0056] In S3, the real-time map updating method process includes: S31, inputting the a priori current environment map and the real-time scanning frame; S32, performing online removal of dynamic point clouds for the current SLAM process; S33, performing feature matching between the real-time point cloud and the a priori point cloud as in S13; S34, performing loop detection between the real-time point cloud and the a priori point cloud; S35, performing trajectory association between the real-time scan and the a priori map, using the anchor node-based cyclic factor to eliminate drift and estimate the offset between the two; S36, performing semi-static object detection based on the a priori map, and adding and deleting point clouds, wherein the point cloud addition is to search for sufficient point clouds within the radius r meters of the prior map during the current real-time scanning process; the point cloud deletion is to search for insufficient point clouds within the radius r meters of the prior map during the current real-time scanning process; S37, restoration of occluded objects.
[0057] Embodiment 1:
[0058] The present invention provides a map updating method for a full life cycle SLAM system, such as Figure 1 shown.
[0059] The framework based on the full life cycle SLAM system is as follows Figure 1 As shown in the figure, the early SLAM system is called the Past SLAM system, and the current real-time SLAM system is the Now SLAM system. The pose, point data, and trajectory of each key frame processed in real time by the Now SLAM system are matched and registered with those of the Past period and used as the input of the map update system. The key frame point cloud data and the corresponding pose information are then used to align and associate the trajectories. The associated trajectories and point cloud information are in the same coordinate system. The corresponding global map is then used to identify semi-static objects, and divide them into point clouds that need to be added and point clouds that need to be removed, and the point cloud information of the map is updated.
[0060] Step 1: Sensor data input: Get the raw data of multi-line laser radar and inertial sensor IMU (Inertial Measurement Unit). These data are collected from the current carrier in real time.
[0061] Step 2: IMU pre-integration module: Use the integration between multiple frames of information from the IMU sensor to obtain the predicted posture for subsequent processes.
[0062]
[0063] Among them, w represents the world coordinate system, b represents the carrier coordinate system, and Δt is the time difference between two consecutive frames of IMU measurement data. t k and t k+1 The position change from the world coordinate system to the carrier coordinate system at the moment, Represents the coordinate transformation matrix from the world coordinate system to the carrier coordinate system at time t, represents the velocity of the carrier at time k and time k+1, It can be seen from the above formula that when the initial state changes, the entire integral needs to be recalculated, which will greatly affect the calculation efficiency. At the same time, it can be seen from the above formula that R i The change will cause the integral to be repeated, which can be done by left multiplying both sides of the above formula Can get,
[0064]
[0065] in,
[0066]
[0067] That is, the optimized pre-integrated value is only related to the acceleration and angular velocity measurements at different times, but has nothing to do with the initial state value, as shown in the formula, for example It is only related to the bias of the gyroscope, accelerometer and angular velocity meter, and the bias is also the variable to be optimized, which will result in a new bias for each optimization. It is necessary to update the bias and perform pre-integration again, which will greatly increase the computational complexity. The pre-integration term is used to perform Taylor expansion on the bias.
[0068]
[0069] in, Represents the pre-integrated term α on the random walk b a Find the derivative, Represents the pre-integrated term α on the random walk b w Find the derivative, represents the pre-integrated term β on the random walk b a Find the derivative, represents the pre-integrated term β on the random walk b w Find the derivative, represents the pre-integrated term γ for the random walk b w Find the derivative.
[0070] Due to the existence of noise, the pre-integration calculated by IMU at each moment has errors. The analysis of the errors is as follows:
[0071]
[0072] By performing median integration on both sides of the above equation, we can obtain the following state relationship:
[0073]
[0074] Through the above relationship, we can get the discrete state recursive relationship:
[0075] x k+1 =Fx k +VQ
[0076] Among them, F represents the state transfer matrix, V represents the noise propagation matrix, and Q represents the noise matrix.
[0077] Since it is very complicated to solve the Jacobian matrix of bias by pre-integration, the initial Jacobian matrix is set equal to I, and then the Jacobian matrix is updated using state recursion.
[0078] J k+1 =FJ k
[0079] The corresponding optimization variables are extracted from the Jacobian matrix, and the bias derivative part can be updated by pre-integration solution.
[0080] Step 3: Dedistortion and feature extraction of point cloud data: A frame of data from a lidar sensor is not the data at a certain moment, but all the data generated within a cycle, and the data is timestamped. If the lidar sensor or its carrier does not move within this cycle, then the point cloud information will not have motion distortion. However, in the real world, lidar sensors are often in motion. If point cloud data with motion distortion is used for alignment, it may lead to a decrease in accuracy. Therefore, it is necessary to dedistort the point cloud data and perform motion compensation. Motion compensation is to compensate all point clouds to a certain moment, use the IMU information and the laser odometer to obtain the relative rotation matrix of the point cloud information at a certain moment, and use the matrix to transfer the point cloud information to the moment when the lidar scan starts to make motion compensation. For example Figure 2As shown, the red solid line is the real object shape, and the green dotted line is the object geometry measured after motion distortion.
[0081] like Figure 3 , t k to k+1 There is a frame of point cloud data between each frame, and different moments correspond to different IDs of scanning laser lines. k to k+1 All point cloud data between t k In the coordinate system of the moment, it is necessary to apply the IMU pre-integration described in step 2 to obtain the rotation matrix to rotate and translate the point cloud data between the two frames of lidar.
[0082] After the original point cloud data is distorted, it is necessary to extract the point and surface features in the point cloud data for subsequent matching. This paper adopts the LOAM feature extraction method to extract corner points and plane features by calculating the curvature of points in some areas of the point cloud data. When the point is on the edge of a static building, the curvature will be very large and must be considered as a corner point. Therefore, through a certain threshold, the curvature exceeding this threshold is considered a corner point, and the curvature below this threshold is considered a plane feature.
[0083] Step 4: Point cloud registration: In the previous steps, we have filtered out some dynamic point clouds. The point cloud features at this time are called static features. The feature point set at the kth moment is The former is an edge feature, while the latter is a plane feature. At the same time, the new key frame is defined as when When it arrives, use the feature set of the previous n+1 key frames and the initial pose estimate provided by IMU pre-integration {T k-n ,…,T k} Conduct local map M k The construction of the present invention k The definition of is as follows:
[0084]
[0085]
[0086] In the formula and They are the features of the key frame at the kth moment from the initial pose estimation to the world coordinate system. and It is the local edge feature map and the local plane feature map in the world coordinate system.
[0087] Then, the scan-submap matching method is used to and as well as and The matching of edge feature points is to find the shortest distance between an edge feature point and the line formed by edge feature points of another frame. Find a line in to find the shortest distance. Select a point i, In the KD-Tree, the nearest neighbor search is used to select the point j closest to i, and the nearest neighbor search of KD-Tree is used to select the point j closest to i. Select the nearest point l in the scan line adjacent to point j; for plane points, the points are on a plane, so plane point matching requires the distance from the point to the plane, that is, Find a point i in the KD-Tree and use the nearest neighbor search in Select the point j closest to i, and select the nearest point l with the same laser scanning beam as point j, and then find the nearest point m on the scanning line adjacent to point j. These three points will be three non-collinear points and can form a valid plane.
[0088] Therefore, constraints between points and lines and between points and planes are established, and the relative pose is optimized by minimizing these two constraints. The initial edge feature points and plane feature points are selected in the LOAM selection method. If the point spacing between the current frame and the previous frame is too large, it will easily cause errors in the point cloud matching, resulting in a decrease in accuracy. At the same time, the frame matching frame calculation and frame matching global map method used by LOAM will affect the optimization results when the previous and next frames change greatly. Therefore, the frame-local map matching method is adopted to increase the robustness of the registration.
[0089] After the local map is built, a feature point needs to be selected in the current frame, and then five similar feature points are selected in the local map using the KD-Tree nearest neighbor search, and the covariance matrix M of these five points is calculated, and the eigenvalue of M is recorded as V, and the eigenvector is recorded as E. The numerical rule of the eigenvalue V can be used to determine whether there is a matching edge or matching surface, as well as the direction of the matching edge and the direction of the matching plane.
[0090] If it is an edge feature, that is, the feature points are distributed in a line segment, then the eigenvalues in V will be one large and two small, that is, one eigenvalue is obviously larger than the other two eigenvalues. In this case, there is a matching edge, and the eigenvector corresponding to the larger eigenvalue of V is the direction of the edge. Therefore, the other two edge features corresponding to the edge feature points of the current frame can be quickly found.
[0091] If it is a planar feature, that is, the feature points are distributed on a plane, then the eigenvalues in V will be one small and two large, that is, one eigenvalue is obviously smaller than the other two eigenvalues. In this case, there is a matching plane, and the eigenvector corresponding to the smaller eigenvalue in V is the direction of the plane. Therefore, the other three plane features corresponding to the plane feature points of the current frame can be quickly found.
[0092] For the matched feature point set, the LM method is used to minimize D e With D h Solve for relative pose
[0093] Step 5: Map update: The real-time LiDAR scanning frame is jointly optimized with the prior map obtained through the above steps, and the frame is aligned with the prior map through loop detection to achieve map update of the point cloud data.
[0094] Specifically, after comprehensive comparison of various data structures, the iKD-Tree data structure is selected for point cloud data management. Because the KD-Tree structure commonly used in robotic applications is "static", in which the tree is built from scratch using all points, which contradicts the fact that data is usually acquired sequentially in actual robotic applications. In this case, it is usually very inefficient and time-consuming to merge the new data frame into the existing data frame by rebuilding the entire data tree from scratch. Therefore, KD-Tree is usually updated at a low frequency, or rebuilt only on new points. In order to adapt to the nature of sequential data acquisition, this structure can solve the problem of depth imbalance and decreased search efficiency caused by the traditional KD-Tree structure in the presence of a large amount of data additions and deletions, effectively eliminating redundant operations when rebuilding the entire tree, and saving a lot of calculations. Supports incremental insertion and deletion, supports self-adjustment of the tree structure, and always maintains dynamic balance. Figure 4-Figure 5 Incremental updates and rebalancing on the tree from a three-dimensional view are shown. Figure 4 Represents the existing KD-Tree (black dots) and new dots (red triangles) to be inserted, and the blue cubes represent the spaces that need to be rebalanced (i.e. branches). Figure 5 KD-Tree after insertion point and tree rebalancing. The blue cube represents the space after rebalancing, while the rest of the tree remains unchanged.
[0095] Specifically, the point cloud of the offline prior map is used as the basis, and the iKD-Tree method is used to efficiently detect the current scanned point cloud and propose the point cloud data to be updated. Figure 6 shown.
[0096] The iKD-Tree algorithm can accurately and quickly identify newly added point cloud data, thereby detecting newly appeared objects and calculating the point clouds that need to be deleted. Secondly, in order to maintain the accuracy of the data, special attention will be paid to those stable point clouds that are blocked, and they will be removed from the point clouds to be deleted and retained. Finally, the detected point cloud data is integrated and updated to the prior map.
[0097] Specifically, when updating the point cloud map, we focus on identifying the differences between the current scan frame and the existing map, which includes discovering newly added point clouds and confirming missing point clouds. Through comparative analysis, we locate new elements that have not been recorded in the map and point clouds that no longer exist in the current scan, thereby determining the changes of objects in the environment.
[0098] The update detection process is divided into two stages: First, the iKD-Tree structure is constructed to pre-screen the point cloud data, and the unmatched point clouds are directly eliminated through the loop factor and marking mechanism (the marking mechanism is a mechanism of the ikd-tree data structure), narrowing the matching range and searching whether there are enough point clouds of the prior map within a radius of r meters. Subsequently, after receiving the new real-time frame data, the existing 3D map and the point cloud layout of the real-time frame are deeply analyzed, and the overlapping area of the two is calculated to achieve efficient and accurate update detection. The following formula:
[0099] R n =S n ∩M n
[0100] R n is the overlapping part of the real-time frame and the prior map. The real-time frame S n In addition to the intersection point cloud cluster, it is determined whether the remaining point cloud clusters are likely to be updated. The calculation method is as follows
[0101] Add n =S n -R n
[0102] After determining the updated part, you need to filter and delete the point cloud Del n , delete the point cloud cluster corresponding to the actual object from the map. n The calculation method is as follows:
[0103]
[0104] Where Range(R n ) represents the range of the scan frame corresponding to the prior map. After the above two equations, we can basically get a set of potential updated point cloud clusters {Add n ,Del n}, this set represents the point cloud that needs to be updated at time n.
[0105] Specifically, when performing global registration of the real-time frame with the prior map, the initial alignment is performed using context-based scanning and further refinement is performed using radius search based on pose proximity. After the registration is completed, in order to improve efficiency and avoid reinitializing the data structure at each comparison, the system first grids the prior map to build a basic grid structure. After that, all point cloud data registered to the map use this pre-established grid as a reference to build the corresponding point cloud structure, thereby achieving efficient management and comparison of point cloud data.
[0106] After completing the iKD-Tree structure analysis of the prior environment map and the real-time scanning point cloud, the two point cloud data sets are processed by set operations. n ,Del n}, click Add n It is a potential increase in point cloud, Del n is the potential culling point cloud. n Further screening and judgment is required because the point set may also contain obscured map point clouds. Therefore, to further filter the obscured point clouds, only Del n In addition, when constructing a voxel grid collection of a 3D map and scanned point cloud, the content contained is the index sequence of the 3D map grid, rather than the specific coordinate points in the point cloud. This means that each element represents a grid area, rather than the location information of a single point.
[0107] The scan perspective line is a straight line starting from the center of the current scan frame, and its direction is determined by Add n The voxel block is determined by the following equation:
[0108]
[0109] After determining the scanning perspective line equation, the next step is to traverse the set Del n to check whether they are located in the vector <p ot ,p a >The positive extension line of the defined scanning perspective line. According to this definition, the scanning line is used for corresponding detection.
[0110] If the detection finds that a voxel block is located on the positive extension line of the scan perspective line, it will be identified as an occluded voxel block and removed from the set Del n After this scanning perspective line perspective process, a new potential deletion point cloud set Del will be obtained.n This approach ensures that occluded points can still be processed and will not affect the overall performance of the map.
[0111] From this example, it can be seen that it has the following characteristics compared with the prior art:
[0112] 1. Under the premise of low computing power, the system can update the map online;
[0113] 2. Use the ikd-tree data structure to process point clouds in real time, and perform dynamic and efficient addition and deletion of point clouds;
[0114] 3. A collection idea of grid management is proposed to achieve the management and comparison of point cloud data;
[0115] 4. In the process of updating the map, the method of scanning the perspective extension line is introduced to restore the obscured objects.
[0116] Embodiment 2:
[0117] The prior map input and the real-time scan frame input are used to update the detection using the initial alignment based on context scanning and the radius search based on posture proximity, and the point cloud to be updated is applied to the prior map.
[0118] like Figure 7 This is a priori map constructed at a certain period in the past. Point A in the map contained a temporary building at that time. Due to later planning arrangements, it was demolished, so it needed to be deleted from the original priori map. Figure 8 Global update map is in Figure 7 Based on the global map updated online, a good mapping of the real environment and demolished buildings was made, and it was found that the point cloud was added at B, reflecting the updated relationship of the real green belt.
[0119] The above solution is only an illustration of a preferred embodiment, but is not limited thereto. When implementing the present invention, appropriate replacement and / or modification can be performed according to user needs.
[0120] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and the embodiments. It can be fully applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily realized. Therefore, without departing from the general concept defined by the claims and equivalent scope, the present invention is not limited to the specific details and the illustrations shown and described here.
Claims
1. A map updating method for a full life cycle SLAM system, characterized in that: include: S1, using the environmental map of the current area obtained based on the SLAM system as a priori map; S2. In the current period, the current area is scanned in real time by a 3D laser radar to obtain a real-time scanning frame; S3, performing real-time loop detection and registration on the real-time scanning frame obtained by S2 and the prior map of S1, and comparing the low-dynamic objects and semi-static objects identified in the prior map and the real-time scanning frame to determine whether to add and / or delete the corresponding point cloud in the prior map, thereby completing the local update of the prior map at different times; S4. Return to S1 until the update operation of all areas in the environment map is completed.
2. The map updating method of the full life cycle SLAM system as claimed in claim 1, characterized in that: In S3, the process of real-time loop detection and registration includes: S31, after receiving new real-time scanning frame data, performing global registration between the real-time scanning frame and the prior map; S32, by constructing the iKD-Tree structure, the point cloud data set in the prior map and real-time scanning frame data is pre-screened, the unmatched point cloud is directly eliminated through the loop factor and marking mechanism, and then the potential updated point cloud cluster set screened at time n is obtained through calculation processing {Add n , Del n }, Add in collection n is a potential increase in point cloud clusters, Del n is a potential point cloud cluster to be eliminated; S33, will update the point cloud cluster set in the prior map {Add n ,Del n }.
3. The map updating method of the full life cycle SLAM system as claimed in claim 2, characterized in that: In S31, the global registration includes: performing initial alignment based on context scanning, and determining whether there is a point cloud of more than 85% of the prior map based on a radius search of posture proximity, and if so, the registration is completed.
4. The map updating method of the full life cycle SLAM system as claimed in claim 2, characterized in that: In S32, the point set {Add n , Del n } is obtained as follows: S320, after receiving the new real-time scanning frame data, the point cloud layout of the prior map and the real-time scanning frame is analyzed, and the overlapping area point cloud cluster R of the two is obtained by the following formula: n : R n =S n ∩M n In the above formula, S n represents the prior map point cloud cluster, M n Represents a point cloud cluster of a real-time scan frame; S320, Potentially add point cloud cluster Add n Obtained by the following formula: Add n =S n -R n Potentially eliminated point cloud cluster Del n Obtained by the following formula: In the above formula, Range(R n ) represents the range of the scanned frame corresponding to the prior map.
5. The map updating method of the full life cycle SLAM system as claimed in claim 4, characterized in that: Also includes: Introduce scanning perspective lines to potentially eliminate point cloud clusters Del n Conduct secondary screening.
6. The map updating method of the full life cycle SLAM system as claimed in claim 5, characterized in that: The scan perspective line is a straight line starting from the center of the real-time scan frame, and its direction is determined by Add n The voxel block determines the construction equation of the scanning perspective line as follows: In the above formula, x, y, and z represent the x, y, and z coordinates of the point cloud to be added or deleted in space, respectively. ot ,y ot 、z ot They represent the x, y, and z coordinates of the point cloud with the earliest timestamp in Add in space. a ,y a 、z a They represent the x, y, and z coordinates of the point cloud in Add in space respectively.
7. The map updating method of the full life cycle SLAM system as claimed in claim 6, characterized in that: Based on the construction equation of the scanning perspective line, the traversal set potentially eliminates the point cloud cluster Del n to check whether the voxel block is located within the vector <p ot , p a >If the positive extension line of the defined scanning perspective line is on the positive direction, then the voxel block is considered to be occluded and the point cloud cluster Del is removed from the set n Remove it from the list, and then get the potential deleted point cloud set Del n ′; Among them, p ot The vector representing the point cloud with the earliest timestamp, representing the range of the perspective line extended by the vector of the point cloud in ADD.
8. The map updating method of the full life cycle SLAM system as claimed in claim 2, characterized in that: In S3, before the global registration of the real-time scanning frame with the prior map, the current SLAM process also includes online removal of dynamic point clouds.
Citation Information
Cited By
Positioning method based on global static map and local dynamic map and application
CN119573706A
Dynamic map generation method based on laser radar
CN120143182A
SLAM method and device under semi-dynamic environment change
CN120252690A
Open road high-precision positioning method based on multi-source fusion
CN120779411A
