Instant positioning and map construction method and device, computer device and storage medium

Abstract

The application relates to a real-time positioning and map construction method and device, computer equipment and a storage medium. The method comprises the following steps: acquiring point cloud data frames from a laser radar and inertial measurement data from an inertial measurement unit; acquiring an initial pose according to the point cloud data frames and the inertial measurement data; inputting the point cloud data frames and the initial pose into a sliding window; optimizing the point cloud data frames and the initial pose in the sliding window; fusing the point cloud data frames out of the sliding window into a plurality of first sub-maps; acquiring a first pose; constructing a plurality of second sub-maps according to the plurality of first sub-maps and acquiring a second pose; taking each second sub-map as a node unit to perform loop optimization; constructing a global map according to the second sub-maps after loop optimization and acquiring a global pose. The application can improve the real-time positioning and map construction efficiency.

Description

Technical Field

[0001] This invention relates to the field of real-time positioning and mapping, and more particularly to a method, apparatus, computer device, and storage medium for real-time positioning and mapping. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) has important applications in fields such as 3D reconstruction, geographic surveying, and robotics.

[0003] The key to SLAM is registering the continuous point clouds acquired by LiDAR into a consistent 3D coordinate system. Traditional methods include several approaches. One is using the Iterative Closest Point (ICP) algorithm and its variants for pairwise registration. In this method, small errors between adjacent point clouds accumulate, inevitably leading to trajectory drift and final map distortion. Another approach is graph optimization, which optimizes the pose of all point clouds simultaneously by minimizing the sum of the relative pose errors between any two point clouds. This method only considers the robot's relative pose constraints and neglects the global consistency of the point cloud map. Therefore, accumulated errors in the map may only decrease but not be completely eliminated; if erroneous cycles are detected or incorrect relative poses are added, map distortion can become even more severe.

[0004] It is evident that there is currently no effective method to address SLAM errors. Summary of the Invention

[0005] In order to solve the above-mentioned technical problems, or at least partially solve the above-mentioned technical problems, the present invention provides a real-time positioning and map building method, apparatus, computer equipment and storage medium.

[0006] In a first aspect, the present invention provides a real-time positioning and map building method, the method comprising:

[0007] Acquire point cloud data frames from the lidar and inertial measurement data from the inertial measurement unit;

[0008] The initial pose is obtained based on the point cloud data frame and the inertial measurement data;

[0009] The point cloud data frame and the initial pose are sent into a sliding window, the point cloud data frame and the initial pose within the sliding window are optimized, the point cloud data frame exiting the sliding window is fused into multiple first sub-maps, and the first pose is obtained.

[0010] Based on multiple first sub-maps, construct multiple second sub-maps and obtain the second pose;

[0011] Using each second sub-map as a node unit, perform loop closure optimization, construct a global map based on the loop closure optimized second sub-map, and obtain the global pose.

[0012] Optionally, the step of sending the point cloud data frame and the initial pose into a sliding window, optimizing the point cloud data frame and the initial pose within the sliding window, fusing the point cloud data frame exiting the sliding window into multiple first sub-maps, and obtaining the first pose includes:

[0013] Continuously move the sliding window, and each time the sliding window is moved, obtain one of the first sub-maps;

[0014] Each time the sliding window is moved, a first sub-map is obtained, including:

[0015] Move the sliding window so that the optimized point cloud data frame and initial pose are removed from the sliding window, and the unoptimized point cloud data frame and initial pose are brought into the sliding window.

[0016] The point cloud data frame and initial pose entering the sliding window are optimized to obtain the first pose.

[0017] The point cloud data frames from the sliding window are merged into a first sub-map.

[0018] Optionally, the optimization of the point cloud data frame and initial pose entering the sliding window is performed in the following manner:

[0019]

[0020] in, Indicates optimization, where T is the initial pose, M is the number of planes detected, w is the number of point cloud data frames entering the sliding window, and N is the number of frames. ij N represents the number of points corresponding to the i-th plane in the j-th frame. i Let n be the total number of points in the i-th plane. i and q i Let n be the plane parameter of the i-th plane. i Let q be the normal vector of the i-th plane. i Let i be the center point of the i-th plane. p is the transpose of the normal vector of the i-th plane. ijk Let i be the k-th point in the j-th frame corresponding to the i-th plane, where i,j,k = 1, 2, 3, ...

[0021] Optionally, the point cloud data frames exiting the sliding window are fused into a first sub-map in the following manner:

[0022]

[0023] T L1 =T11

[0024] Among them, S L1 For the first sub-map after fusion, m is the number of point cloud data frames in the sliding window, and T is the number of frames. L1 T11 is the first pose after optimization of the first initial pose. i For the i-th first pose, F i Let i be the i-th frame to be merged, where i = 1, 2, 3, ...

[0025] Optionally, constructing multiple second sub-maps based on multiple first sub-maps includes:

[0026] Based on the rotation angle between adjacent point cloud data frames, the point cloud data frames are divided into a first type of data frame and a second type of data frame.

[0027] Based on the classification of the point cloud data frames, the point cloud data frames in multiple global coordinate systems are merged into multiple second sub-maps.

[0028] Optionally, dividing the point cloud data frames into a first type of data frame and a second type of data frame based on the rotation angle between adjacent point cloud data frames includes:

[0029] Obtain the rotation angle of the current point cloud data frame relative to the previous point cloud data frame;

[0030] If the rotation angle is greater than or equal to the preset angle, then the current point cloud data frame is taken as the first type of data frame;

[0031] If the rotation angle is less than the preset angle, then the current point cloud data frame is used as the second type of data frame.

[0032] Optionally, the step of fusing point cloud data frames in multiple global coordinate systems into multiple second sub-maps based on the classification of the point cloud data frames includes:

[0033] If the type of the (N+1)th point cloud data frame is the same as the type of the Nth point cloud data frame, then the (N+1)th point cloud data frame will be merged with the current base frame.

[0034] If the type of the (N+1)th point cloud data frame is different from the type of the Nth point cloud data frame, then:

[0035] If the Nth point cloud data frame is fused, then the current base frame after fusing the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0036] If the Nth point cloud data frame is not merged, then the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0037] Where N = 1, 2, 3...

[0038] Optionally, before performing loop closure optimization using each of the second sub-maps as a node unit, the method further includes:

[0039] Perform internal optimizations on each of the second sub-maps;

[0040] The second sub-map was internally optimized as follows:

[0041]

[0042] in, Indicates optimization, N′ i Let i be the number of points in the first submap. p is the transpose of the normal vector of the plane corresponding to the j-th point in the i-th frame in the (i+1)-th frame. i,j For the i-th point in the first sub-map, q i+1,j Let the plane be the plane corresponding to the j-th point in the (i+1)-th point cloud frame. Let q be the transpose of the normal vector of the j-th point in frame i corresponding to the plane in frame i-1. i-1,j Let T1 be the plane corresponding to the j-th point in the (i-1)-th point cloud frame, where T1 is the first pose, and i,j = 1, 2, 3, ...

[0043] Optionally, the step of performing loop closure optimization with each of the second sub-maps as a node unit, and constructing a global map based on the loop closure optimized second sub-maps, includes:

[0044] Using the second sub-map as a node unit, candidate loops are obtained within a preset radius around the node unit;

[0045] Perform set registration between the second sub-map corresponding to the node unit and each of the candidate loops;

[0046] The candidate loop with the best set registration result is used as the reference map;

[0047] Determine whether the reference map is an erroneous loop;

[0048] If the reference map contains an incorrect loop, then the reference map containing the incorrect loop is filtered out, and the second sub-map corresponding to the node unit is added to the global base map to update the global base map.

[0049] If the reference map is not an erroneous loop, then the second sub-map corresponding to the node unit and the reference map are added to the global base map to update the global base map;

[0050] The map after all the second sub-maps are added to the global base map is called the global map.

[0051] Optionally, the step of obtaining candidate loop closures within a preset radius around the second sub-map as a node unit includes:

[0052] Multiple loop maps are acquired within a preset radius around the node unit;

[0053] Obtain the first and second loop indexes for each loop map;

[0054] If the first loop closure index and the second loop closure index meet the loop closure condition, then the loop closure map is confirmed as a candidate loop closure;

[0055] The first loop closure index is used to indicate the proximity of a node unit to a corresponding point on the loop closure map, and the second loop closure index is used to indicate the coverage between the point cloud of the node unit and the point cloud of the loop closure map.

[0056] The loop closure condition is: the first loop closure index is less than the first preset threshold, and the second loop closure index is greater than the second preset threshold.

[0057] Optionally, determine if the reference map is an incorrect loop by obtaining the line process item l as follows. ij :

[0058]

[0059] f(T2 i T2 j ,X)=∑||T2 i p′-T 2j q′|| 2 ≈∑||T2 i p′-T2 j Xp′|| 2

[0060] When the line process item l ij When the value is less than the loopback threshold, the reference map is considered an erroneous loopback.

[0061] in, Indicates optimization, T2 i For the pose of the i-th second submap, T2 i+1 The pose of the (i+1)th second submap, R iT2 represents the relative poses of the i-th and (i+1)-th second sub-maps. j For the pose of the j-th second submap, T2 ij Let be the relative pose of the i-th and j-th second sub-maps, μ be the weight, X be the estimated relative pose transformation, p′ be a point in the i-th second sub-map, q′ be the corresponding point in the j-th second sub-map, and l be the weight. ij For the i-th and j-th second sub-maps, there are line process items.

[0062] Secondly, a real-time positioning and mapping device is provided, the device comprising:

[0063] The acquisition unit is used to acquire point cloud data frames from the lidar and inertial measurement data from the inertial measurement unit;

[0064] Odometry, used to obtain initial pose based on the point cloud data frame and the inertial measurement data;

[0065] A local processing unit is used to send the point cloud data frame and the initial pose into a sliding window, optimize the point cloud data frame and the initial pose within the sliding window, fuse the point cloud data frame exiting the sliding window into multiple first sub-maps, and obtain the first pose.

[0066] The sub-map refinement unit is used to construct multiple second sub-maps based on multiple first sub-maps and obtain a second pose;

[0067] The loop closure detection unit is used to perform loop closure optimization with each second sub-map as a node unit, construct a global map based on the loop closure optimized second sub-map, and obtain the global pose.

[0068] Thirdly, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the method as described in any of the preceding claims.

[0069] Fourthly, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the preceding claims.

[0070] This invention provides a real-time localization and mapping (BALM) method, apparatus, computer device, and storage medium. The method includes: acquiring point cloud data frames from a lidar system and inertial measurement data from an inertial measurement unit (IMU); obtaining an initial pose based on the point cloud data frames and the IMU data; feeding the point cloud data frames and the initial pose into a sliding window; optimizing the point cloud data frames and the initial pose within the sliding window; fusing the point cloud data frames exiting the sliding window into multiple first sub-maps and obtaining a first pose; constructing multiple second sub-maps based on the multiple first sub-maps and obtaining a second pose; performing loop closure optimization using each second sub-map as a node unit; constructing a global map based on the loop closure optimized second sub-maps; and obtaining a global pose. In this embodiment of the invention, a three-layer sub-map is constructed based on the point cloud data frames and IMU data. Particularly in generating multiple first sub-maps, the use of a sliding window allows the BALM to match the operating frequency of the odometry, improving system efficiency. In the method of this invention, each sub-map is obtained by merging / fusing multiple point cloud data frames. Compared to a single frame of point cloud data, this results in richer features and more robust registration for each sub-map. Furthermore, merging multiple point cloud data frames into multiple sub-maps reduces the number of corresponding poses, decreases the optimization scale, and improves system efficiency. Attached Figure Description

[0071] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0072] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0073] Figure 1 The diagram shown illustrates the application environment of the real-time positioning and map building method according to an embodiment of the present invention.

[0074] Figure 2 The diagram shown is a flowchart illustrating the real-time positioning and map building method according to an embodiment of the present invention.

[0075] Figure 3 The diagram shown is a schematic of a sliding window according to an embodiment of the present invention;

[0076] Figure 4 The diagram shown is a schematic diagram of the second sub-map formed by merging the first sub-map at a large corner according to an embodiment of the present invention;

[0077] Figure 5 The diagram shown is a schematic diagram of the second sub-map formed by merging the first sub-map at a small corner according to an embodiment of the present invention;

[0078] Figure 6 The diagram shown is a schematic diagram of loop closure detection in the second sub-map, which serves as a node unit, according to an embodiment of the present invention.

[0079] Figure 7 The diagram shows an application scenario of the real-time positioning and map building method according to an embodiment of the present invention.

[0080] Figure 8 The diagram shown is a structural block diagram of the real-time positioning and map building device according to an embodiment of the present invention;

[0081] Figure 9 The diagram shown is an internal structural diagram of a computer device in an embodiment of the present invention. Detailed Implementation

[0082] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0083] Figure 1 This is an application environment diagram of a real-time localization and mapping method in one embodiment. (Refer to...) Figure 1 This real-time positioning and mapping method is applied to a real-time positioning and mapping system. The method includes a terminal 110 and / or a server 120. The terminal 110 and server 120 are connected via a network. The terminal 110 can be a desktop terminal or a mobile terminal; the mobile terminal can be at least one of a mobile phone, tablet computer, or laptop computer. The server 120 can be a standalone server or a server cluster consisting of multiple servers.

[0084] The real-time positioning and map building method of the present invention is applied to terminal 110 and / or server 120.

[0085] In this embodiment of the invention, the terminal 110 and / or the server 120 can both be installed on mobile devices such as robots, drones, and exploration vehicles.

[0086] Figure 2 The diagram shown is a flowchart of a real-time positioning and map building method according to an embodiment of the present invention. Figure 2 As shown, the method includes:

[0087] Step 210: Acquire point cloud data frames from the lidar and inertial measurement data from the inertial measurement unit;

[0088] An inertial measurement unit can be called an IMU unit, and inertial measurement data can also be called IMU data.

[0089] The data collected by the lidar is in the form of point clouds. In this embodiment of the invention, the point cloud data is processed in data frames. To obtain more robust initial pose estimation, this embodiment of the invention also uses IMU data from the IMU unit to address the degradation problem of lidar in complex environments.

[0090] Point cloud data frames can be referenced Figure 7 As shown in (a), a large number of point clouds constitute a data frame, which is called a point cloud data frame.

[0091] Step 220: Obtain the initial pose based on the point cloud data frame and the inertial measurement data;

[0092] The initial pose is obtained by odometry based on the point cloud data frame and the IMU data.

[0093] Step 230: Send the point cloud data frame and the initial pose into the sliding window, optimize the point cloud data frame and the initial pose in the sliding window, merge the point cloud data frame out of the sliding window into multiple first sub-maps, and obtain the first pose.

[0094] Step 240: Based on the multiple first sub-maps, construct multiple second sub-maps and obtain the second pose;

[0095] Step 250: Perform loop closure optimization on each second sub-map as a node unit, construct a global map based on the loop closure optimized second sub-map, and obtain the global pose.

[0096] In the method of this embodiment, a three-layer sub-map is constructed based on point cloud data frames and IMU data. Specifically, when generating multiple first sub-maps, a sliding window is used, which allows the BALM (Balanced Alignment Model) to match the odometry's operating frequency, improving system efficiency. In the method of this embodiment, each sub-map is obtained by merging / fusing multiple point cloud data frames. Compared to a single frame of point cloud data, this results in richer features and more robust registration for each sub-map. Furthermore, merging multiple point cloud data frames into multiple sub-maps reduces the number of corresponding poses, decreasing the optimization scale and improving system efficiency.

[0097] In this embodiment of the invention, step 230, which involves sending the point cloud data frame and the initial pose into a sliding window, optimizing the point cloud data frame and the initial pose within the sliding window, fusing the point cloud data frame exiting the sliding window into multiple first sub-maps, and obtaining the first pose, includes:

[0098] Continuously move the sliding window, and each time the sliding window is moved, obtain one of the first sub-maps;

[0099] Each time the sliding window is moved, a first sub-map is obtained, including:

[0100] Move the sliding window so that the optimized point cloud data frame and initial pose are removed from the sliding window, and the unoptimized point cloud data frame and initial pose are brought into the sliding window.

[0101] The point cloud data frame and initial pose entering the sliding window are optimized to obtain the first pose.

[0102] The point cloud data frames from the sliding window are merged into a first sub-map.

[0103] In this embodiment of the invention, the optimization of the point cloud data frame and initial pose entering the sliding window is performed in the following manner:

[0104]

[0105] in, Indicates optimization, where T is the initial pose, M is the number of planes detected, w is the number of point cloud data frames entering the sliding window, and N is the number of frames. ij N represents the number of points corresponding to the i-th plane in the j-th frame. i Let n be the total number of points in the i-th plane. i and q i Let n be the plane parameter of the i-th plane. i Let q be the normal vector of the i-th plane. i Let i be the center point of the i-th plane. p is the transpose of the normal vector of the i-th plane. ijk Let i be the k-th point in the j-th frame corresponding to the i-th plane, where i,j,k = 1, 2, 3, ...

[0106] In this embodiment of the invention, the optimization of the point cloud data frame and initial pose entering the sliding window is performed by a local processing unit, also known as a local BA unit.

[0107] The local BA unit receives the point cloud data frame and initial pose from the odometry output and feeds them into a sliding window. The LiDAR Bundle Adjustment (BALM) technique is then used to jointly optimize the point cloud within the window. BALM can improve map consistency by minimizing the distance from points to the plane.

[0108] Since the time complexity of BALM technology is exponentially related to the number of frames N, it cannot be applied to real-time SLAM for large-scale point clouds. Therefore, in this embodiment of the invention, by using a sliding window with an appropriate window width, BALM can be matched to the operating frequency of the odometry, thereby enabling the application of BALM technology to real-time SLAM.

[0109] Figure 3 The diagram shown is a schematic representation of a sliding window according to an embodiment of the present invention. Figure 3 As shown, the sliding window 300 can be continuously slid. The width of the sliding window can be set according to needs; generally, a wider window provides higher precision but lower efficiency.

[0110] Figure 3 In the middle, the data in the sliding window 300 moves from left to right, 310 is the data about to enter the sliding window, and 320 is the data that will be merged into the first sub-map after leaving the sliding window. Figure 3 The illustrated embodiment includes three first sub-maps.

[0111] In this embodiment of the invention, the point cloud data frames exiting the sliding window are fused into a first sub-map in the following manner:

[0112]

[0113] T L1 =T11

[0114] Among them, S L1 For the first sub-map after fusion, m is the number of point cloud data frames in the sliding window, and T is the number of frames. L1 T11 is the first pose after optimization of the first initial pose. i For the i-th first pose, F i Let i be the i-th frame to be merged, where i = 1, 2, 3, ...

[0115] In this embodiment of the invention, step 240, which involves constructing multiple second sub-maps based on multiple first sub-maps, includes:

[0116] Based on the rotation angle between adjacent point cloud data frames, the point cloud data frames are divided into a first type of data frame and a second type of data frame.

[0117] Based on the classification of the point cloud data frames, the point cloud data frames in multiple global coordinate systems are merged into multiple second sub-maps.

[0118] In this embodiment of the invention, dividing the point cloud data frames into a first type of data frame and a second type of data frame based on the rotation angle between adjacent point cloud data frames includes:

[0119] Obtain the rotation angle of the current point cloud data frame relative to the previous point cloud data frame;

[0120] If the rotation angle is greater than or equal to the preset angle, then the current point cloud data frame is taken as the first type of data frame;

[0121] If the rotation angle is less than the preset angle, then the current point cloud data frame is used as the second type of data frame.

[0122] In this embodiment of the invention, the step of fusing point cloud data frames in multiple global coordinate systems into multiple second sub-maps based on the classification of the point cloud data frames includes:

[0123] If the type of the (N+1)th point cloud data frame is the same as the type of the Nth point cloud data frame, then the (N+1)th point cloud data frame will be merged with the current base frame.

[0124] If the type of the (N+1)th point cloud data frame is different from the type of the Nth point cloud data frame, then:

[0125] If the Nth point cloud data frame is fused, then the current base frame after fusing the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0126] If the Nth point cloud data frame is not merged, then the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0127] Where N = 1, 2, 3...

[0128] When robots, drones, and exploration vehicles move, the lidar that collects data also moves. Due to the movement of the data acquisition equipment, there is a relative rotation angle between the collected data frames. In this embodiment of the invention, the first type of data frame and the second type of data frame are distinguished by their rotation angle. That is, the first type of data frame has a larger rotation angle, also known as a large rotation angle, and the second type of data frame has a smaller rotation angle, also known as a small rotation angle.

[0129] If there is a large rotation angle between two adjacent first sub-maps, it means a significant change in the field of view, which is prone to errors during map construction and optimization. In this embodiment of the invention, multiple consecutive first sub-maps with large rotation angles are merged into one second sub-map, while multiple consecutive first sub-maps with small rotation angles are merged into another second sub-map.

[0130] In this embodiment of the invention, during system initialization, the first sub-map can be used as the base frame, and the base frame is continuously updated as the first sub-map is merged.

[0131] For example, multiple consecutive first submaps, A, B, C, D, e, f, g, h, i, J, K, L, M, n, p, q, where uppercase letters represent first submaps with large turns and lowercase letters represent first submaps with small turns.

[0132] During the merging process, A is used as the base frame, and B, C, and D are all the first sub-maps with large turns. Therefore, A, B, C, and D are merged into the first second sub-map.

[0133] e is the first sub-map at the small corner, and its type is different from D. Therefore, e is used as the base frame, and e, f, g, h, and i are merged into the second sub-map. Similarly, after using J as the base frame, J, K, L, and M are merged into the third sub-map; after using n as the base frame, n, p, and q are merged into the fourth sub-map.

[0134] Figure 4 The diagram shown is a schematic representation of the second sub-map formed by merging the first sub-map at a large turning point according to an embodiment of the present invention. Figure 5 The diagram shown is a schematic diagram of the second sub-map formed by merging the first sub-map at a small corner according to an embodiment of the present invention.

[0135] In this embodiment of the invention, before step 250, before performing loop closure optimization using each of the second sub-maps as node units, the method further includes:

[0136] Perform internal optimizations on each of the second sub-maps;

[0137] The second sub-map was internally optimized as follows:

[0138]

[0139] in, Indicates optimization, N′ i Let i be the number of points in the first submap. p is the transpose of the normal vector of the plane corresponding to the j-th point in the i-th frame in the (i+1)-th frame. i,j For the i-th point in the first sub-map, q i+1,jLet the plane be the plane corresponding to the j-th point in the (i+1)-th point cloud frame. Let q be the transpose of the normal vector of the j-th point in frame i corresponding to the plane in frame i-1. i-1,j Let T1 be the plane corresponding to the j-th point in the (i-1)-th point cloud frame, where T1 is the first pose, and i,j = 1, 2, 3, ...

[0140] Since the second sub-map in this embodiment of the invention integrates multiple first sub-maps, the parameters in the above formula include points in the first sub-map, the number of points, etc.

[0141] In this embodiment of the invention, the optimization problem within the second sub-map is constructed as a standard least squares problem, where the residual is defined as the Euclidean distance from a point on the plane to its corresponding plane. By optimizing the second sub-map, errors can be reduced, subsequent computational load can be decreased, thereby improving system efficiency.

[0142] In this embodiment of the invention, step 240, which involves performing loop closure optimization using each of the second sub-maps as a node unit and constructing a global map based on the loop closure optimized second sub-maps, includes:

[0143] Using the second sub-map as a node unit, candidate loops are obtained within a preset radius around the node unit;

[0144] Perform set registration between the second sub-map corresponding to the node unit and each of the candidate loops;

[0145] The candidate loop with the best set registration result is used as the reference map;

[0146] Determine whether the reference map is an erroneous loop;

[0147] If the reference map contains an incorrect loop, then the reference map containing the incorrect loop is filtered out, and the second sub-map corresponding to the node unit is added to the global base map to update the global base map.

[0148] If the reference map is not an erroneous loop, then the second sub-map corresponding to the node unit and the reference map are added to the global base map to update the global base map;

[0149] The map after all the second sub-maps are added to the global base map is called the global map.

[0150] In this invention, the step of obtaining candidate loop closures within a preset radius around the second sub-map as a node unit includes:

[0151] Multiple loop maps are acquired within a preset radius around the node unit;

[0152] Obtain the first and second loop indexes for each loop map;

[0153] If the first loop closure index and the second loop closure index meet the loop closure condition, then the loop closure map is confirmed as a candidate loop closure;

[0154] The first loop closure index is used to indicate the proximity of a node unit to a corresponding point on the loop closure map, and the second loop closure index is used to indicate the coverage between the point cloud of the node unit and the point cloud of the loop closure map.

[0155] The loop closure condition is: the first loop closure index is less than the first preset threshold, and the second loop closure index is greater than the second preset threshold.

[0156] refer to Figure 6 As shown, Figure 6 The diagram shown is a schematic of loop closure detection in the second sub-map, which serves as a node unit, according to an embodiment of the present invention. The blue circle represents the preset radius.

[0157] The second submap corresponding to each node unit will be geometrically registered with each candidate loop in a coarse-to-fine manner. The first loop metric can be the RMSE metric, and the second loop metric can be the Fitness metric.

[0158] The smaller the first loop metric RMSE, the better; the larger the second loop metric Fitness, the better. In order to balance efficiency and quality, the loop conditions in this invention can be set to RMSE < 10 and Fitness > 0.1.

[0159] In this embodiment of the invention, performing set registration between the second sub-map corresponding to the node unit and each candidate loop can include coarse registration and fine registration.

[0160] In this embodiment of the invention, a coarse registration can be performed using the RANSAC method based on Scan Context features, followed by further refinement of the pose transformation using Generalized-ICP. Scan Context is a descriptor for loop closure detection in 3D LiDAR point clouds. Its core idea is to convert the 3D point cloud into a ring image, or "context graph," to capture the geometric structure of the scene and perform similarity matching in a simple way.

[0161] RANSAC (RANdom Simple Consensus) is a robust algorithm commonly used to estimate model parameters from data containing a large number of outliers. Its core idea is to repeatedly randomly sample subsets from the data, estimate the model, and find the largest set of interior points based on a certain consistency criterion. In other words, it involves multiple random matching attempts and selecting the one with the best registration result. Generalized-ICP is a high-precision and robust algorithm that incorporates local geometric information about points, i.e., covariance, into the traditional ICP algorithm.

[0162] In this embodiment of the invention, by performing two registrations, both the overall geometric structure and the local geometric structure of the scene can be taken into account.

[0163] In this embodiment of the invention, to determine whether the reference map is an erroneous loop, the linear process item l is obtained in the following manner. ij :

[0164]

[0165] f(T2 i T2 j ,X)=∑||T2 i p′-T 2j q′|| 2 ≈∑||T2 i p′-T2 j Xp′|| 2

[0166] When the line process item l ij When the value is less than the loopback threshold, the reference map is considered an erroneous loopback.

[0167] in, Indicates optimization, T2 i For the pose of the i-th second submap, T2 i+1 The pose of the (i+1)th second submap, R i T2 represents the relative poses of the i-th and (i+1)-th second sub-maps. j For the pose of the j-th second submap, T2 ij Let be the relative pose of the i-th and j-th second sub-maps, μ be the weight, X be the estimated relative pose transformation, p′ be a point in the i-th second sub-map, q′ be the corresponding point in the j-th second sub-map, and l be the weight. ij For the i-th and j-th second sub-maps, there are line process items.

[0168] In this embodiment of the invention, as many loops as possible are retained, even if l ij →1; When the loop closure matching error is large, l should be reduced. ij Hedging. Setting lij The threshold is 0.25; loops smaller than this threshold are discarded.

[0169] Since erroneous loops can severely impact the generated map, this embodiment of the invention eliminates erroneous loops to reduce errors.

[0170] In this embodiment of the invention, after outputting the global map, the output pose is the global pose.

[0171] Figure 7 The diagram shows an application scenario of the real-time positioning and map building method according to an embodiment of the present invention.

[0172] Figure 7 (a) is the original point cloud data frame acquired by the lidar. Figure 7 (b) Acquired initial pose and point cloud data frame. Figure 7 (c) is the first sub-map. Figure 7 (d) and Figure 7 (e) is the generated second sub-map. Figure 7 (f) shows the loop closure detection and optimization within the second sub-map. Figure 7 (g) is the local map.

[0173] The method of this invention can simultaneously achieve high accuracy and online real-time generation of a global map through multi-level sub-maps. In addition, the multiple point cloud data frames and multiple sub-maps of this invention are fused to construct a sub-map, which can improve the accuracy and robustness of pose graph optimization.

[0174] like Figure 8 As shown, the present invention also provides an instant positioning and map building device, the device comprising:

[0175] The acquisition unit 810 is used to acquire point cloud data frames from the lidar and inertial measurement data from the inertial measurement unit;

[0176] Odometer 820 is used to obtain the initial pose based on the point cloud data frame and the inertial measurement data;

[0177] The local processing unit 830 is used to send the point cloud data frame and the initial pose into a sliding window, optimize the point cloud data frame and the initial pose within the sliding window, fuse the point cloud data frame exiting the sliding window into multiple first sub-maps, and obtain the first pose.

[0178] The sub-map refinement unit 840 is used to construct multiple second sub-maps based on multiple first sub-maps and obtain a second pose;

[0179] The loop closure detection unit 850 is used to perform loop closure optimization with each second sub-map as a node unit, construct a global map based on the loop closure optimized second sub-map, and obtain the global pose.

[0180] In this embodiment of the invention, the local processing unit 830 is further configured to:

[0181] Continuously move the sliding window, and each time the sliding window is moved, obtain one of the first sub-maps;

[0182] Each time the sliding window is moved, a first sub-map is obtained, including:

[0183] Move the sliding window so that the optimized point cloud data frame and initial pose are removed from the sliding window, and the unoptimized point cloud data frame and initial pose are brought into the sliding window.

[0184] The point cloud data frame and initial pose entering the sliding window are optimized to obtain the first pose.

[0185] The point cloud data frames from the sliding window are merged into a first sub-map.

[0186] In this embodiment of the invention, the local processing unit 830 is further configured to optimize the point cloud data frame and initial pose entering the sliding window in the following manner:

[0187]

[0188] in, Indicates optimization, where T is the initial pose, M is the number of planes detected, w is the number of point cloud data frames entering the sliding window, and N is the number of frames. ij N represents the number of points corresponding to the i-th plane in the j-th frame. i Let n be the total number of points in the i-th plane. i and q i Let n be the plane parameter of the i-th plane. i Let q be the normal vector of the i-th plane. i Let i be the center point of the i-th plane. p is the transpose of the normal vector of the i-th plane. ijk Let i be the k-th point in the j-th frame corresponding to the i-th plane, where i,j,k = 1, 2, 3, ...

[0189] In this embodiment of the invention, the local processing unit 830 is further configured to fuse the point cloud data frames exiting the sliding window into a first sub-map in the following manner:

[0190]

[0191] TL1 =T11

[0192] Among them, S L1 For the first sub-map after fusion, m is the number of point cloud data frames in the sliding window, and T is the number of frames. L1 T11 is the first pose after optimization of the first initial pose. i For the i-th first pose, F i Let i be the i-th frame to be merged, where i = 1, 2, 3, ...

[0193] In this embodiment of the invention, the sub-map refinement unit 840 is further configured to:

[0194] Based on the rotation angle between adjacent point cloud data frames, the point cloud data frames are divided into a first type of data frame and a second type of data frame.

[0195] Based on the classification of the point cloud data frames, the point cloud data frames in multiple global coordinate systems are merged into multiple second sub-maps.

[0196] In this embodiment of the invention, the sub-map refinement unit 840 is further configured to:

[0197] Obtain the rotation angle of the current point cloud data frame relative to the previous point cloud data frame;

[0198] If the rotation angle is greater than or equal to the preset angle, then the current point cloud data frame is taken as the first type of data frame;

[0199] If the rotation angle is less than the preset angle, then the current point cloud data frame is regarded as the second type of data frame.

[0200] In this embodiment of the invention, the sub-map refinement unit 840 is further configured to:

[0201] If the type of the (N+1)th point cloud data frame is the same as the type of the Nth point cloud data frame, then the (N+1)th point cloud data frame will be merged with the current base frame.

[0202] If the type of the (N+1)th point cloud data frame is different from the type of the Nth point cloud data frame, then:

[0203] If the Nth point cloud data frame is fused, then the current base frame after fusing the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0204] If the Nth point cloud data frame is not merged, then the Nth point cloud data frame is used as a second sub-map, and the (N+1)th point cloud data frame is used as the next base frame.

[0205] Where N = 1, 2, 3...

[0206] In this embodiment of the invention, the loop closure detection unit 850 is further configured to:

[0207] Perform internal optimizations on each of the second sub-maps;

[0208] In this embodiment of the invention, the loop closure detection unit 850 is further configured to perform internal optimization of the second sub-map in the following manner:

[0209]

[0210] in, Indicates optimization, N′ i Let i be the number of points in the first submap. p is the transpose of the normal vector of the plane corresponding to the j-th point in the i-th frame in the (i+1)-th frame. i,j For the i-th point in the first sub-map, q i+1,j Let the plane be the plane corresponding to the j-th point in the (i+1)-th point cloud frame. Let q be the transpose of the normal vector of the j-th point in frame i corresponding to the plane in frame i-1. i-1,j Let T1 be the plane corresponding to the j-th point in the (i-1)-th point cloud frame, where T1 is the first pose, and i,j = 1, 2, 3, ...

[0211] In this embodiment of the invention, the loop closure detection unit 850 is further configured to:

[0212] Using the second sub-map as a node unit, candidate loops are obtained within a preset radius around the node unit;

[0213] Perform set registration between the second sub-map corresponding to the node unit and each of the candidate loops;

[0214] The candidate loop with the best set registration result is used as the reference map;

[0215] Determine whether the reference map is an erroneous loop;

[0216] If the reference map contains an incorrect loop, then the reference map containing the incorrect loop is filtered out, and the second sub-map corresponding to the node unit is added to the global base map to update the global base map.

[0217] If the reference map is not an erroneous loop, then the second sub-map corresponding to the node unit and the reference map are added to the global base map to update the global base map;

[0218] The map after all the second sub-maps are added to the global base map is called the global map.

[0219] In this embodiment of the invention, the loop closure detection unit 850 is further configured to:

[0220] Multiple loop maps are acquired within a preset radius around the node unit;

[0221] Obtain the first and second loop indexes for each loop map;

[0222] If the first loop closure index and the second loop closure index meet the loop closure condition, then the loop closure map is confirmed as a candidate loop closure;

[0223] The first loop closure index is used to indicate the proximity of a node unit to a corresponding point on the loop closure map, and the second loop closure index is used to indicate the coverage between the point cloud of the node unit and the point cloud of the loop closure map.

[0224] The loop closure condition is: the first loop closure index is less than the first preset threshold, and the second loop closure index is greater than the second preset threshold.

[0225] In this embodiment of the invention, the loop closure detection unit 850 is further configured to obtain the linear process item l in the following manner. ij Determine if the reference map contains an incorrect loop:

[0226]

[0227] f(T2 i T2 j ,X)=∑||T2 i p′-T 2j q′|| 2 ≈∑||T2 i p′-T2 j Xp′|| 2

[0228] When the line process item l ij When the value is less than the loopback threshold, the reference map is considered an erroneous loopback.

[0229] in, Indicates optimization, T2 i For the pose of the i-th second submap, T2 i+1 The pose of the (i+1)th second submap, R i T2 represents the relative poses of the i-th and (i+1)-th second sub-maps. j For the pose of the j-th second submap, T2 ij Let be the relative pose of the i-th and j-th second sub-maps, μ be the weight, X be the estimated relative pose transformation, p′ be a point in the i-th second sub-map, q′ be the corresponding point in the j-th second sub-map, and l be the weight. ijFor the i-th and j-th second sub-maps, there are line process items.

[0230] The method of this invention can simultaneously achieve high accuracy and online real-time generation of a global map through multi-level sub-maps. In addition, the multiple point cloud data frames and multiple sub-maps of this invention are fused to construct a sub-map, which can improve the accuracy and robustness of pose graph optimization.

[0231] This invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the following method: The method includes: acquiring point cloud data frames from a lidar and inertial measurement data from an inertial measurement unit; acquiring an initial pose based on the point cloud data frames and the inertial measurement data; sending the point cloud data frames and the initial pose into a sliding window; optimizing the point cloud data frames and the initial pose within the sliding window; fusing the point cloud data frames exiting the sliding window into multiple first sub-maps and acquiring a first pose; constructing multiple second sub-maps based on the multiple first sub-maps and acquiring a second pose; performing loop closure optimization with each second sub-map as a node unit; constructing a global map based on the loop closure optimized second sub-maps and acquiring a global pose.

[0232] This invention also provides a computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program implements the following method: The method includes: acquiring point cloud data frames from a lidar and inertial measurement data from an inertial measurement unit; acquiring an initial pose based on the point cloud data frames and the inertial measurement data; sending the point cloud data frames and the initial pose into a sliding window; optimizing the point cloud data frames and the initial pose within the sliding window; fusing the point cloud data frames exiting the sliding window into multiple first sub-maps and acquiring a first pose; constructing multiple second sub-maps based on the multiple first sub-maps and acquiring a second pose; performing loop closure optimization with each second sub-map as a node unit; constructing a global map based on the loop closure optimized second sub-maps and acquiring a global pose.

[0233] The aforementioned real-time positioning and mapping method achieves the beneficial effect of solving the technical problems raised in the background section.

[0234] Figure 2 This is a flowchart illustrating a real-time localization and map building method in one embodiment. It should be understood that, although... Figure 2The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 2 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0235] Figure 9 An internal structural diagram of a computer device in one embodiment is shown. Specifically, this computer device may be… Figure 1 Server 120 in the middle. For example... Figure 9 As shown, the computer device includes a processor, memory, network interface, input device, and display screen connected via a system bus. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores an operating system and may also store computer programs. When executed by the processor, these programs enable the processor to implement a real-time localization and map building method. The internal memory may also store computer programs, which, when executed by the processor, enable the processor to implement the real-time localization and map building method. The display screen can be an LCD screen or an e-ink screen. The input device can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.

[0236] Those skilled in the art will understand that Figure 9 The structure shown is merely a block diagram of a portion of the structure related to the present invention and does not constitute a limitation on the computer device to which the present invention is applied. A specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0237] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0238] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0239] The above description is merely a specific embodiment of the present invention, enabling those skilled in the art to understand or implement the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the present invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A method of simultaneous localization and mapping, characterized by, The method comprises: acquiring a point cloud data frame from a laser radar and inertial measurement data from an inertial measurement unit; acquiring an initial pose according to the point cloud data frame and the inertial measurement data; feeding the point cloud data frame and the initial pose into a sliding window, optimizing the point cloud data frame and the initial pose in the sliding window, fusing the point cloud data frame out of the sliding window into a plurality of first sub-maps, and acquiring a first pose; constructing a plurality of second sub-maps according to the plurality of first sub-maps and acquiring a second pose; taking each of the second sub-maps as a node unit to perform loop optimization, constructing a global map according to the second sub-maps after loop optimization, and acquiring a global pose; Before the taking each of the second sub-maps as a node unit to perform loop optimization, the method further comprises: performing internal optimization on each of the second sub-maps; performing internal optimization on the second sub-maps in the following manner: wherein, represents an optimization, N' i is the number of points of the i-th first submap, is the transpose of the normal vector of the j-th point of the i-th frame to the plane corresponding in the i+1-th frame, p i,j is the j-th point of the i-th first submap, q i+1,j is the plane corresponding to the j-th point in the i+1-th point cloud frame, is the transpose of the normal vector of the j-th point of the i-th frame to the plane corresponding in the i-1-th frame, q i-1,j is the plane corresponding to the j-th point in the i-1-th point cloud frame, T1 is the first pose, i, j = 1, 2, 3,....

2. The method of claim 1, wherein, The feeding the point cloud data frame and the initial pose into a sliding window, optimizing the point cloud data frame and the initial pose in the sliding window, fusing the point cloud data frame out of the sliding window into a plurality of first sub-maps, and acquiring a first pose comprises: continuously moving the sliding window, and acquiring one of the first sub-maps each time the sliding window is moved; The acquiring one of the first sub-maps each time the sliding window is moved comprises: moving the sliding window so that the optimized point cloud data frame and initial pose out of the sliding window, and the unoptimized point cloud data frame and initial pose enter the sliding window, optimizing the point cloud data frame and initial pose entering the sliding window to acquire the first pose, fusing the point cloud data frame out of the sliding window into one of the first sub-maps.

3. The method of claim 2, wherein, The optimizing the point cloud data frame and initial pose entering the sliding window is in the following manner: wherein, represents optimization, T is an initial pose, M is the number of detected planes, w is the number of frames of point cloud data entering the sliding window, N ij is the number of points of the i-th plane in the j-th frame, N i is the total number of points of the i-th plane, n i and q i are plane parameters of the i-th plane, n i is a normal vector of the i-th plane, q i is a center point of the i-th plane, is a transpose of the normal vector of the i-th plane, p ijk is the k-th point of the i-th plane in the j-th frame, i, j, k = 1, 2, 3, … 4. The method of claim 2, wherein, The fusing the point cloud data frame out of the sliding window into one of the first sub-maps is in the following manner: T L1 = T11 wherein S L1 is the first submap after fusion, m is the frame number of the point cloud data frame out of the sliding window, L1 is the initial pose, T11 is the first pose after optimization of the first initial pose, T1 i is the i-th first pose, F i is the i-th frame to be fused, i = 1, 2, 3,...

5. The method of claim 1, wherein, The constructing a plurality of second sub-maps according to the plurality of first sub-maps comprises: dividing the point cloud data frames into first type data frames and second type data frames according to the rotation angle between adjacent point cloud data frames; fusing a plurality of point cloud data frames in a global coordinate system into a plurality of second sub-maps according to the classification of the point cloud data frames.

6. The method of claim 5, wherein, The dividing the point cloud data frames into first type data frames and second type data frames according to the rotation angle between adjacent point cloud data frames comprises: acquiring the rotation angle of a current point cloud data frame relative to a previous point cloud data frame; if the rotation angle is greater than or equal to a preset angle, the current point cloud data frame is taken as the first type data frame; if the rotation angle is less than the preset angle, the current point cloud data frame is taken as the second type data frame.

7. The method of claim 5, wherein, The fusing a plurality of point cloud data frames in a global coordinate system into a plurality of second sub-maps according to the classification of the point cloud data frames comprises: If the type of the N+1th point cloud data frame is the same as the type of the Nth point cloud data frame, the N+1th point cloud data frame is fused with the current base frame; If the type of the N+1th point cloud data frame is not the same as the type of the Nth point cloud data frame, then: If the Nth point cloud data frame is fused, the current base frame after the Nth point cloud data frame is fused is taken as a second sub-map, and the N+1th point cloud data frame is taken as a next base frame, If the Nth point cloud data frame is not fused, the Nth point cloud data frame is taken as a second sub-map, and the N+1th point cloud data frame is taken as the next base frame; Wherein, N=1, 2, 3…….

8. The method of claim 1, wherein, The second sub-map is taken as a node unit, loop optimization is performed, and a global map is constructed according to the second sub-map after loop optimization, comprising: A candidate loop is obtained within a preset radius around the node unit; The second sub-map corresponding to the node unit is set to register with each candidate loop; The candidate loop with the best set registration result is taken as a reference map; It is judged whether the reference map is an error loop; If the reference map is an error loop, the reference map of the error loop is filtered out, and the second sub-map corresponding to the node unit is added to the global base map to update the global base map, If the reference map is not an error loop, the second sub-map corresponding to the node unit and the reference map are added to the global base map to update the global base map; When all the second sub-maps are added to the global base map, the map after the global base map is taken as the global map.

9. The method of claim 8, wherein, The second sub-map is taken as a node unit, a candidate loop is obtained within a preset radius around the node unit, comprising: A plurality of loop maps are obtained within a preset radius around the node unit; A first loop index and a second loop index of each loop map are obtained; If the first loop index and the second loop index meet the loop condition, the loop map is confirmed as a candidate loop; The first loop index is used to indicate the proximity of the node unit and the corresponding points of the loop map, and the second loop index is used to indicate the coverage rate between the point cloud of the node unit and the point cloud of the loop map; The loop condition is that the first loop index is less than a first preset threshold, and the second loop index is greater than a second preset threshold.

10. The method of claim 8, wherein, determining whether the reference map is a false loop, the line progress item l is obtained in the following manner ij : When the line process item l ij is less than the back loop threshold, the reference map is an erroneous back loop; wherein, denotes the optimization, T2 i is the pose of the i-th second submap, T2 i+1 is the pose of the i+1-th second submap, R i is the relative pose between the i-th and i+1-th second submap, T2 j is the pose of the j-th second submap, T2 ij is the relative pose between the i-th and j-th second submap, μ is a weight, X is an estimated relative pose transform, p' is a point in the i-th second submap, q' is the corresponding point in the j-th second submap, l ij is the line progress term between the i-th and j-th second submap.

11. A simultaneous localization and mapping apparatus, comprising: The device comprises: A collection unit for obtaining point cloud data frames from a laser radar and inertial measurement data from an inertial measurement unit; An odometer for obtaining an initial pose according to the point cloud data frames and the inertial measurement data; A local processing unit for sending the point cloud data frames and the initial pose into a sliding window, optimizing the point cloud data frames and the initial pose in the sliding window, fusing the point cloud data frames out of the sliding window into a plurality of first sub-maps, and obtaining a first pose; A sub-map refinement unit for constructing a plurality of second sub-maps according to a plurality of first sub-maps, and obtaining a second pose; a loop detection unit, configured to perform loop optimization in a node unit of each of the second sub-maps, and construct a global map according to the second sub-maps after loop optimization, and obtain a global pose; the loop detection unit is further configured to: perform internal optimization on each of the second sub-maps; the loop detection unit is further configured to perform internal optimization on the second sub-maps in the following manner: wherein, represents an optimization, N' i is the number of points of the i-th first submap, is the transpose of the normal vector of the j-th point of the i-th frame to the plane corresponding in the i+1-th frame, p i,j is the j-th point of the i-th first submap, q i+1,j is the plane corresponding to the j-th point in the i+1-th point cloud frame, is the transpose of the normal vector of the j-th point of the i-th frame to the plane corresponding in the i-1-th frame, q i-1,j is the plane corresponding to the j-th point in the i-1-th point cloud frame, T1 is the first pose, i, j = 1, 2, 3,....

12. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, the processor implements the method in any one of claims 1-10 when executing the computer program.

13. A computer readable storage medium having stored thereon a computer program, characterized in that, the computer program is executed by the processor to implement the method in any one of claims 1-10.

Images