A method and system for map construction with collaborative multi-lidar

Abstract

The present invention belongs to the technical field of map construction, and more specifically, relates to a map construction method and system for multi-lidar collaboration. The method includes the following steps: multiple lidars respectively acquire the original point cloud data of the same mapping area, and solve the initial external parameters between the multiple lidars according to the original point cloud data; extract features from the original point cloud data to obtain feature g'; perform feature matching on the original point cloud data according to feature g' to obtain the initialized front-end pose; optimize the front-end pose to obtain the globally optimized pose and the globally optimized map; optimize and calibrate the external parameters of the multiple lidars according to the globally optimized pose and the initial external parameters to output the optimized external parameters; update the initial external parameters according to the optimized external parameters. The present invention improves the calibration accuracy of the external parameters for multi-lidar collaborative mapping, and at the same time reduces the cost of the system.

Description

Technical Field

[0001] The present invention belongs to the technical field of map construction, and more specifically, relates to a map construction method and system for collaborative multi-lidar. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) technology generally refers to a robot or autonomous moving device moving in an unknown environment, using the sensors it carries to observe the surrounding environment, obtaining its own pose and surrounding environment information, and constructing a map of its surrounding environment. With the rapid development of the unmanned driving and robot industries, the application frequency of SLAM is getting higher and higher, and the application scenarios are also becoming more and more diverse.

[0003] Currently, the most commonly used sensor for SLAM technology is lidar, which can obtain the point cloud information of the environment. The point cloud information includes the three-dimensional spatial coordinates of the laser points, the reflection intensity, and the laser acquisition time. Traditional SLAM generally only uses one lidar, and it can only increase the density of the point cloud and the mapping accuracy by increasing the number of lines of the lidar. In this way, a single lidar is easily affected by the weather environment or other failures, resulting in the inability to construct a map. Therefore, its robustness is not high enough, the mapping effect is not stable, and there are field of view blind spots. In addition, high-line-number lidars are costly.

[0004] To solve the above problems, in the existing technical solutions, multiple small lidars are also used to collaborate in mapping to improve the environmental perception ability of the system and the stability of the system. However, multiple lidars need to be effectively calibrated for external parameters to complete effective data fusion, thereby improving the accuracy of mapping. However, the current external parameter calibration of multiple lidars is not accurate enough, and the external parameters cannot be optimized in real time during the mapping process. Summary of the Invention

[0005] The present invention aims to overcome at least one of the above-mentioned deficiencies in the prior art, and provides a map construction method and system for collaborative multi-lidar, which can improve the accuracy of external parameter calibration of multi-lidar.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is:

[0007] Provide a map construction method for collaborative multi-lidar, including the following steps:

[0008] S1: Multiple lidars respectively obtain the original point cloud data of the same mapping area, preprocess the original point cloud data, and solve the initial values of the external parameters between multiple lidars based on the preprocessed point cloud data;

[0009] S2: Extract features from the preprocessed point cloud data to obtain edge features and plane features, and perform secondary extraction on the plane features to obtain point-plane feature g';

[0010] S3: Based on the key frames and the sliding window, use the point-plane feature g' to perform feature matching on the preprocessed point cloud data to obtain the initialized front-end pose;

[0011] S4: Based on bundle adjustment, locally optimize the front-end pose to obtain the locally optimized pose; optimize the global pose to obtain the globally optimized pose and the globally optimized map; optimize and calibrate the extrinsic parameters of multiple lidars according to the globally optimized pose and the initial value of the extrinsic parameters, output the optimized extrinsic parameters, and return the optimized extrinsic parameters to step S1 to update the initial value of the extrinsic parameters;

[0012] S5: Repeat steps S1 to S4 continuously, and output multiple globally optimized maps, and construct a SLAM map according to the multiple globally optimized maps.

[0013] In this solution, the initial value of the extrinsic parameters is first solved from the original point cloud data, and then after the front-end pose is optimized, the initial value of the extrinsic parameters is used for optimized calibration, so as to output the optimized extrinsic parameters, realize online extrinsic parameter calibration, output stable and accurate extrinsic parameter results, and improve the accuracy of multi-lidar fusion mapping.

[0014] Preferably, step S1 specifically includes the following steps:

[0015] S11: Preprocess the original point cloud data;

[0016] S12: Use the hand-eye calibration method to solve the preprocessed point cloud data in step S11 to obtain the initial value of the extrinsic parameters between multiple lidars.

[0017] Preferably, the feature extraction of the original point cloud data in step S2 specifically includes the following steps:

[0018] S21: Extract the edge feature and the plane feature g by calculating the plane smoothness in the preprocessed point cloud data;

[0019] S22: Perform secondary plane extraction on the plane feature g to obtain the point-plane feature g'.

[0020] Preferably, the secondary plane extraction of the plane feature g in step S22 to obtain the point-plane feature g' specifically includes the following steps:

[0021] S221: Calculate the point cloud normal vector of the plane feature g, denoted as N;

[0022] S222: Applying a region growing algorithm to calculate the point cloud normal vector N to obtain a plurality of plane clustered point sets D, where D = {d1, ..., dn};

[0023] S223: traverse all elements of point set D;

[0024] S224: Let the normal vector of the current plane cluster be The fitted plane equation is Where p is the original point cloud, d is the point in the plane cluster, and T represents the transpose;

[0025] S225: Solving Plane Equations and Overdetermined Equations at Each Point in the Cluster Get n and d, where ||n||=1;

[0026] S225: Traverse all elements in the plane cluster D and substitute them into the plane equation to calculate

[0027] S226: Determine whether y is less than y'. If so, set g'=g'∪pi and output the point surface feature g'. Otherwise, return to step S221.

[0028] Preferably, in the above step S3, based on the keyframe and the sliding window, the pre-processed point cloud data is matched using the point-surface feature g′ to obtain the initialized front-end pose, which specifically includes the following steps:

[0029] S31: extract the current frame of the pre-processed point cloud data and mark it as the next frame Ti+1;

[0030] S32: traverse all key frames of the pre-processed point cloud data;

[0031] S33: Use the next frame Ti+1 to transform the edge features And the plane feature gi+1 is transformed into the world coordinate system, and we get with g′i+1;

[0032] S34: Exploitation Calculating edge features The residual redge from the effective feature point pl in the plane feature to the nearest straight line of the sliding window is calculated using g'i+1; the residual redge from the effective feature point pl in the plane feature to the nearest plane of the sliding window is calculated using g'i+1;

[0033] S35: Solve to minimize the objective function T * i+1=argminTi+1{∑redge+∑rplane};

[0034] S36: Output the matched key frame T *i + 1, and the front-end pose is obtained.

[0035] Preferably, step S4 described above specifically includes the following steps:

[0036] S41: Use bundle adjustment to locally optimize the front-end pose, and use a sliding window for adjustment to obtain a locally optimized pose;

[0037] S42: Adopt the ISAM2 method to optimize the global pose based on the factor graph to obtain a globally optimized pose and a globally optimized map;

[0038] S43: Use the globally optimized pose to further optimize and calibrate the initial external parameters to obtain optimized external parameters. If the calibration result does not converge, keep the initial value of the external parameters unchanged. If the calibration result converges, replace the initial value of the external parameters with the optimized external parameters.

[0039] Preferably, step S43 described above specifically includes the following steps:

[0040] S431: Obtain a set of key frames of multiple lidars;

[0041] S432: Use the set of key frames to construct a local map at the current moment;

[0042] S433: Calculate and statistically analyze the residuals of all feature points in the set of key frames;

[0043] S434: Calculate and statistically analyze the effective plane residuals of all feature points in the set of key frames;

[0044] S435: Calculate the eigenvalues when the sub-objective function fi = ∑redge + ∑rplane is linearly approximated for the first time;

[0045] S436: Update the sub-objective function to fT according to the eigenvalues;

[0046] S437: Optimize the objective function T * AB = arg minTAB fT, and finally output the optimized T*AB, which is the optimized external parameter.

[0047] Preferably, in step S41 described above, using bundle adjustment to locally optimize the front-end pose and using a sliding window for adjustment to obtain a locally optimized pose specifically includes the following steps:

[0048] S411: Structure the features of the preprocessed point cloud data through voxel division;

[0049] S412: Set the sliding window size to m, k = 3;

[0050] S413: Put the features structured in step S411 into voxels;

[0051] S414: Calculate the covariance of the features within the voxels;

[0052] S415: Determine whether k is greater than m. If so, proceed to step 42. If not, wait until the sliding window capacity is full and then return to step S413.

[0053] Preferably, the above step S42 adopts the ISAM2 method to optimize the global pose based on the factor graph, and obtain the globally optimized pose and the globally optimized map, which specifically includes the following steps:

[0054] S421: Solve the optimal pose within the sliding window to obtain the key frame set;

[0055] S422: Traverse the poses within the sliding window and update them to the key frame set;

[0056] S423: Construct a factor graph;

[0057] S424: Marginalize some key frames in the key frame set in step 422 and publish them to the factor graph, construct the odometry factor and perform loop closure detection;

[0058] S425: Use the remaining key frames to construct a copy of the current sliding window;

[0059] S426: Marginalize the voxels and features corresponding to the key frames in the voxels to obtain the globally optimized pose and the globally optimized map.

[0060] This solution also provides a map construction system for multi-lidar collaboration, including a data acquisition system, a front-end odometry, a back-end optimization system, and an online extrinsic parameter calibration system;

[0061] The data acquisition system is used to obtain the original point cloud data of the same mapping area and send it to the front-end odometry;

[0062] The front-end odometry is used to solve the initial values of the extrinsic parameters between multiple lidars using the original point cloud data, extract features from the original power data to obtain the point-plane features g′, perform feature matching on the original point cloud data according to the point-plane features g′, obtain the initialized front-end pose and input it into the back-end optimization system;

[0063] The back-end optimization system is used to optimize the front-end pose to obtain the globally optimized pose and the globally optimized map, and output the SLAM map;

[0064] The online extrinsic parameter calibration system is used to optimize and calibrate the extrinsic parameters of multiple lidars according to the globally optimized pose and the initial values of the extrinsic parameters, output the optimized extrinsic parameters, and return the optimized extrinsic parameters to the front-end odometry.

[0065] Compared with the prior art, the beneficial effects are as follows:

[0066] On the one hand, the present invention constructs a map through the cooperation of multiple lidars, which can enhance the robustness of map construction, avoid the field of view blind area problem of a single dense lidar, improve the map construction effect, and the cost of multiple sparse lidars is lower than that of a single dense lidar; on the other hand, the initial value of the external parameter is solved from the original point cloud data, and then after the front-end pose optimization is completed, the initial value of the external parameter is used for optimization calibration, so as to output the optimized external parameter, realize online external parameter calibration, output a stable and accurate external parameter result, and improve the accuracy of multi-lidar fusion map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 is the overall flow schematic block diagram of the map construction method for multi-lidar cooperation in Embodiment 1 of the present invention;

[0068] Figure 2 is the online external parameter calibration and optimization flow schematic block diagram of the map construction method for multi-lidar cooperation in Embodiment 2 of the present invention;

[0069] Figure 3 is the overall connection schematic block diagram of the map construction system for multi-lidar cooperation in Embodiment 3 of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0070] The drawings are only for illustrative purposes and should not be construed as limiting the present invention; for better illustrating this embodiment, some components in the drawings will be omitted, enlarged or reduced, and do not represent the dimensions of the actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted. The positional relationships described in the drawings are only for illustrative purposes and should not be construed as limiting the present invention.

[0071] In the drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "long", "short", etc. indicating the orientation or positional relationship, they are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, so the terms describing the positional relationship in the drawings are only for illustrative purposes and should not be construed as limiting the present patent. For those of ordinary skill in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0072] The technical solutions of the present invention will be further specifically described below through specific embodiments and in conjunction with the drawings:

[0073] Embodiment 1:

[0074] As shown Figure 1 in the first embodiment of a map construction method for multi-lidar collaboration, which includes the following steps:

[0075] S1: Two lidars respectively acquire the original point cloud data of the same mapping area, preprocess the original point cloud data, and solve the initial value of the external parameters between the two lidars based on the preprocessed point cloud data;

[0076] S2: Extract features from the preprocessed point cloud data to obtain edge features and plane features, and perform secondary extraction on the plane features to obtain the point-plane feature g';

[0077] S3: Based on key frames and a sliding window, use the point-plane feature g' to perform feature matching on the preprocessed point cloud data to obtain the initialized front-end pose;

[0078] S4: Locally optimize the front-end pose based on bundle adjustment to obtain the locally optimized pose; optimize the global pose to obtain the globally optimized pose and the globally optimized map; optimize and calibrate the external parameters of multiple lidars according to the globally optimized pose and the initial value of the external parameters, output the optimized external parameters, and return the optimized external parameters to step S1 to update the initial value of the external parameters;

[0079] S5: Repeat steps S1 to S4, continuously output multiple globally optimized maps, and construct a SLAM map based on the multiple globally optimized maps.

[0080] The two lidars in this embodiment include a main lidar and an auxiliary lidar, which facilitates the discrimination of the data of the two lidars in the subsequent calculation process.

[0081] Specifically, step S1 in this embodiment includes the following steps:

[0082] S11: Preprocess the original point cloud data; specifically, the preprocessing includes filtering the original point cloud data to remove some noise points to avoid the influence of noise points on operations such as feature extraction; at the same time, downsample the original point cloud data to reduce the computational amount in the subsequent processing process, thereby improving the speed of map construction;

[0083] S12: Solve the preprocessed point cloud data using the hand-eye calibration method to obtain the initial value of the external parameters between the two lidars. In this embodiment, a linear interpolation method based on Euclidean transformation is used to perform motion alignment on the two lidars to obtain multiple sets of pose sequences and equations after motion alignment, so that the timestamps on the key frame poses of the two lidars are coherent.

[0084] In step S2 of this embodiment, the feature extraction of the original point cloud data specifically includes the following steps:

[0085] S21: Extract edge features and plane features g by calculating the plane smoothness in the preprocessed point cloud data and plane feature g;

[0086] S22: Perform secondary plane extraction on the plane feature g to obtain the point-plane feature g'. Performing secondary plane extraction can select strong plane features such as the bottom surface, curved surfaces with low curvature, or planes for aggregation to obtain the feature g' that is also a strong plane feature. It records effective plane constraints, reduces the number of plane feature point clouds, and using it for matching between key frames can effectively improve the accuracy and efficiency of key frame matching; for uneven weak plane features such as leaves, they will be used for feature matching in subsequent sliding windows.

[0087] In step S22 of this embodiment, performing secondary plane extraction on the plane feature g to obtain the feature g' specifically includes the following steps:

[0088] S221: Calculate the point cloud normal vector of the plane feature g, denoted as N;

[0089] S222: Apply the region growing algorithm to calculate the point cloud normal vector N to obtain two plane clustering point sets D, where D = {d1,..., dn};

[0090] S223: Traverse all elements in the point set D;

[0091] S224: Let the normal vector of the current plane clustering be The fitted plane equation is where p is the original point cloud, d is the point in the plane clustering, and T represents the transpose;

[0092] S225: Solve the overdetermined equation constructed by the plane equation and each point in the clustering to obtain n and d, where ||n|| = 1;

[0093] S225: Traverse all elements in the plane clustering D and substitute them into the plane equation to calculate S226: Judge whether y is less than y'. If so, let g' = g' U pi, and output the point-plane feature g'. If not, return to step S221.

[0094] In step S3 of this embodiment, based on the key frame and the sliding window, using the point-plane feature g' to perform feature matching on the original point cloud data to obtain the initialized front-end pose specifically includes the following steps:

[0095] S31: Extract the current frame of the preprocessed point cloud data and mark it as the next frame Ti+1;

[0096] S32: Traverse all key frames of the preprocessed point cloud data;

[0097] S33: Use the next frame Ti+1 to transform the edge feature i+1 and the plane feature gi+1 to the world coordinate system to obtain ’i+1 and g’i+1;

[0098] S34: Use ’i+1 to calculate the residual redge of the effective feature points pl in the edge feature i+1 to the nearest straight line in the sliding window; Use g’i+1 to calculate the residual redge of the effective feature points pl in the plane feature to the nearest plane in the sliding window;

[0099] S35: Solve the minimization objective function T * i+1 = argminTi+1{∑redge + ∑rplane};

[0100] S36: Output the matched key frame T * i+1, that is, obtain the front-end pose.

[0101] Step S4 in this embodiment specifically includes the following steps:

[0102] S41: Use bundle adjustment to locally optimize the front-end pose, and use the sliding window to adjust to obtain the locally optimized pose;

[0103] S42: Adopt the ISAM2 method to optimize the global pose based on the factor graph to obtain the globally optimized pose and the globally optimized map;

[0104] S43: Use the globally optimized pose and the local map to further optimize and calibrate the initial external parameters to obtain the optimized external parameters. If the calibration result does not converge, keep the initial value of the external parameters unchanged. If the calibration result converges, replace the initial value of the external parameters with the optimized external parameters.

[0105] Step S43 in this embodiment specifically includes the following steps:

[0106] S431: Obtain the key frame sets of two lidars;

[0107] S432: Use the key frame sets to construct the local map at the current moment;

[0108] S433: Calculate and statistically analyze the residuals of all feature points in the key frame sets;

[0109] S434: Calculate and statistically analyze the effective plane residuals of all feature points in the key frame sets;

[0110] S435: Calculate the eigenvalue when the sub-objective function \(f_i=\sum r_{edge}+\sum r_{plane}\) is linearly approximated for the first time;

[0111] S436: Update the sub-objective function to \(f_T\) according to the eigenvalue;

[0112] S437: Optimize the objective function \(T\) according to the updated sub-objective function \(f_T\) * \(AB = \arg\min_{T_{AB}} f_T\) for optimization, and finally output the optimized \(T^*_{AB}\), which is the optimized external parameter.

[0113] Where \(f_i=\sum r_{edge}+\sum r_{plane}\), which is the sub-objective function of the feature point cloud of each frame of the auxiliary lidar to the local map.

[0114] This is because there may be drift errors in the motion estimation during the initialization of the external parameter calibration process, and there may be rigid body changes between the two lidars, which may affect the acquisition results. The online external parameter calibration method in this embodiment does not require the assistance of other auxiliary sensors. It directly optimizes the initial value of the external parameter by using multiple fixed lidars and the obtained local map. After multiple iterations, a reliable, stable, and accurate external parameter calibration result can be obtained, which can accurately represent the positional relationship between multiple radars. At the same time, the accurate external parameter calibration result can also be fed back to the data acquisition and mapping processes to reduce the possible errors during data acquisition, thereby improving the accuracy of mapping.

[0115] In step S41 of this embodiment, bundle adjustment is used to locally optimize the front-end pose, and a sliding window is used for adjustment. The specific steps for obtaining the locally optimized pose include the following steps:

[0116] S411: Structure the features of the preprocessed point cloud data through voxel division;

[0117] S412: Set the sliding window size to \(m\), \(k = 3\);

[0118] S413: Put the features structured in step S411 into the voxels;

[0119] S414: Calculate the covariance of the features within the voxels;

[0120] S415: Determine whether \(k\) is greater than \(m\). If so, go to step 42. If not, wait until the sliding window capacity is full and then return to step S413.

[0121] In step S42 of this embodiment, the ISAM2 method is used to optimize the global pose based on the factor graph to obtain the globally optimized pose and the globally optimized map. The specific steps include the following steps:

[0122] S421: Solve for the optimal pose within the sliding window and obtain the set of key frames;

[0123] S422: Traverse the poses within the sliding window and update them to the set of key frames;

[0124] S423: Construct a factor graph;

[0125] S424: Marginalize some of the key frames in the set of key frames in step 422 and publish them to the factor graph, construct an odometry factor and perform loop closure detection;

[0126] S425: Use the remaining key frames to construct a copy of the current sliding window;

[0127] S426: Marginalize the voxels and features corresponding to the key frames in the voxels to obtain the globally optimized pose and the globally optimized map.

[0128] It should be noted that the number of lidars in this embodiment is two only for reference and should not be construed as a limitation of this solution. In the actual implementation process, of course, the number of lidars can be increased according to the actual situation, and the mapping process is the same as that of this embodiment, which will not be elaborated here.

[0129] In this embodiment, on the one hand, the collaborative mapping by multiple lidars can enhance the robustness of mapping, avoid the field of view blind spot problem of a single dense lidar, improve the mapping effect, and the cost of two sparse lidars is lower than that of a single dense lidar; on the other hand, the initial value of the external parameter is solved from the raw point cloud data, and then after the front-end pose optimization is completed, the initial value of the external parameter is used for optimization calibration, so as to output the optimized external parameter to achieve online external parameter calibration, output a stable and accurate external parameter result, improve the accuracy of multi-lidar fusion mapping, and avoid the problem that the external parameter calibration result of multi-lidar fusion mapping is not accurate enough and subsequent optimization cannot be performed.

[0130] Embodiment 2:

[0131] As Figure 2 shown is the second embodiment of a multi-lidar collaborative mapping method. The difference between this embodiment and Embodiment 1 is only that in step S2 of this embodiment, when extracting features from the raw point cloud data, the feature point clouds will be divided into strong features and weak features according to the magnitude of the plane smoothness in the raw point cloud data. In this embodiment, four strong and weak features are first extracted, namely strong edge features, weak edge features, strong plane features and weak plane features, and then the strong edge features and weak edge features are combined into edge features, and the strong plane features and weak plane features are combined into plane features. Subsequently, the plane feature g is extracted twice to obtain the feature g'. The weak plane features in this embodiment will be added to the sliding window for feature matching to increase the accuracy of feature matching.

[0132] Embodiment 3:

[0133] As Figure 3 shown is an embodiment of a map construction system with multi-lidar collaboration for implementing the above Embodiment 1 or Embodiment 2, including a data acquisition system, a front-end odometer, a back-end optimization system, and an online extrinsic parameter calibration system; the data acquisition system is communicatively electrically connected to the front-end odometer, the front-end odometer is communicatively electrically connected to the back-end optimization system, the back-end optimization system is communicatively connected to the online extrinsic parameter calibration system, and the online extrinsic parameter calibration system is also communicatively connected to the data acquisition system.

[0134] The data acquisition system is used to acquire the original point cloud data of the same mapping area and send it to the front-end odometer;

[0135] The front-end odometer is used to solve the initial values of the extrinsic parameters between multiple lidars by using the original point cloud data, extract features from the original power data to obtain the point-plane feature g′, perform feature matching on the original point cloud data according to the point-plane feature g′ to obtain the initialized front-end pose, and input it into the back-end optimization system;

[0136] The back-end optimization system is used to optimize the front-end pose to obtain the globally optimized pose and the globally optimized map, and output the SLAM map;

[0137] The online extrinsic parameter calibration system is used to optimize and calibrate the extrinsic parameters of multiple lidars according to the globally optimized pose and the initial values of the extrinsic parameters, output the optimized extrinsic parameters, and return the optimized extrinsic parameters to the front-end odometer; in this embodiment, the optimized extrinsic parameters are also returned to the data acquisition system to improve the accuracy of data acquisition.

[0138] The present invention is described with reference to the flowcharts or block diagrams of the methods, devices (systems), and computer program products of the embodiments of the present application. It should be understood that each process or block in the flowchart or block diagram, as well as the combination of processes or blocks in the flowchart or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0139] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limitations on the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

[0140] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, rather than limitations on the implementation manners of the present invention. For those of ordinary skill in the art, other different forms of changes or alterations can be made based on the above description. It is not necessary and impossible to enumerate all the implementation manners here. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the claims of the present invention.

Claims

1. A map construction method for multi-lidar collaboration, characterized in that It includes the following steps: S1: Multiple lidars respectively acquire the original point cloud data of the same mapping area, preprocess the original point cloud data, and solve the initial external parameters between multiple lidars based on the preprocessed point cloud data; S2: Extract features from the preprocessed point cloud data to obtain edge features and plane features, and perform secondary extraction on the plane features to obtain point-plane features S3: Based on the key frames and the sliding window, use the point-plane features to perform feature matching on the preprocessed point cloud data to obtain an initialized front-end pose; S4: Locally optimize the front-end pose based on bundle adjustment to obtain the locally optimized pose; optimize the global pose to obtain the globally optimized pose and the globally optimized map; Optimize and calibrate the external parameters of multiple lidars according to the globally optimized pose and the initial external parameters, output the optimized external parameters, and return the optimized external parameters to step S1 to update the initial external parameters; S5: Repeat steps S1 to S4, continuously output multiple globally optimized maps, and construct a SLAM map based on the multiple globally optimized maps.

2. The method for constructing a map by multi-lidar collaboration according to claim 1, characterized in that, Step S1 specifically includes the following steps: S11: Preprocess the original point cloud data; S12: Use the hand-eye calibration method to solve the preprocessed point cloud data in step S11 to obtain the initial external parameters between multiple lidars.

3. A method for map construction with multi-lidar collaboration according to claim 2, characterized in that, The specific steps of feature extraction from the original point cloud data in step S2 are as follows: S21: Extract edge features by calculating the planar smoothness of the preprocessed point cloud data and planar features S22: Perform secondary plane extraction on the planar feature to obtain the point-plane feature 4. A method for map construction with collaborative multi-lidar, according to claim 3, characterized in that The planar feature described in step S22 is subjected to secondary planar extraction to obtain a point-plane feature Specifically, it includes the following steps: S221: Calculate the planar feature of the point cloud normal vector, denoted as N; S222: Apply the region growing algorithm to calculate the point cloud normal vector N to obtain multiple plane clustering point sets D, where D = {d1,..., dn}; S223: Traverse all elements in the point set D; S224: Set the normal vector of the current plane clustering as The fitted plane equation is where p is the original point cloud, d is the point in the plane clustering, and T represents the transpose; S225: Solve the overdetermined equations constructed by the plane equation and each point in the clustering Obtain n and d, where ‖n‖ = 1; S225: Traverse all elements in the planar cluster D and substitute them into the plane equation for calculation to obtain S226: Determine whether y is less than y'. If so, then let and output the point-plane feature Otherwise, return to step S221.

5. A method for map construction with collaborative multi-lidar, according to claim 4, characterized in that In step S3, based on the key frames and the sliding window, using the point-plane features Performing feature matching on the preprocessed point cloud data to obtain the initialized front-end pose specifically includes the following steps: S31: Extract the current frame of the preprocessed point cloud data and mark it as the next frame Ti+1; S32: Traverse all key frames of the preprocessed point cloud data; S33: Use the next frame Ti+1 to transform the edge feature and the plane feature into the world coordinate system to obtain and S34: Using to calculate the residual redge of the valid feature point pl in the edge feature to the nearest straight line of the sliding window; using to calculate the residual redge of the valid feature point pl in the plane feature to the nearest plane of the sliding window; S35: Solve for minimizing the objective function T * i + 1 = argminTi + 1{∑redge + ∑rplane}; S36: Output the key frame T after matching * i + 1, thus obtaining the front-end pose.

6. A method for map construction by collaborative multi-lidar, according to any one of claims 1 to 5, characterized in that Step S4 specifically includes the following steps: S41: Locally optimize the front-end pose using bundle adjustment and adjust it using a sliding window to obtain the locally optimized pose; S42: Use the ISAM2 method to optimize the global pose based on the factor graph to obtain the globally optimized pose and the globally optimized map; S43: Further optimize and calibrate the initial external parameters using the globally optimized pose to obtain the optimized external parameters. If the calibration result does not converge, keep the initial external parameters unchanged. If the calibration result converges, replace the initial external parameters with the optimized external parameters.

7. A method for map construction with multi-lidar collaboration according to claim 6, characterized in that, The specific steps of locally optimizing the front-end pose using bundle adjustment and adjusting it using a sliding window to obtain the locally optimized pose in step S41 are as follows: S411: Structure the features of the preprocessed point cloud data through voxel division; S412: Set the sliding window size to m and k = 3; S413: Put the features structured in step S411 into voxels; S414: Calculate the covariance of the features within the voxel; S415: Determine whether k is greater than m. If so, go to step 42. If not, wait until the sliding window capacity is full and then return to step S413.

8. A method for map construction with collaborative multi-lidar, according to claim 7, characterized in that The specific steps of using the ISAM2 method to optimize the global pose based on the factor graph to obtain the globally optimized pose and the globally optimized map in step S42 are as follows: S421: Solve the optimal pose within the sliding window to obtain the key frame set; S422: Traverse the poses within the sliding window and update them to the key frame set; S423: Construct a factor graph; S424: Marginalize some of the key frames in the key frame set in step S422, publish them to the factor graph, construct the odometry factor, and perform loop detection; S425: Use the remaining key frames to construct a copy of the current sliding window; S426: Marginalize the voxels and features corresponding to the key frames in the voxels to obtain the globally optimized pose and the globally optimized map.

9. A method for map construction by multi-lidar collaboration according to claim 8, characterized in that, Step S43 specifically includes the following steps: S431: Obtain the key frame sets of multiple lidars; S432: Use the key frame sets to construct a local map at the current moment; S433: Calculate and statistically analyze the residuals of all feature points in the key frame sets; S434: Calculate and statistically analyze the effective plane residuals of all feature points in the key frame sets; S435: Calculate the eigenvalues when the sub-objective function fi = ∑redge + ∑rplane is linearly approximated for the first time; S436: Update the sub-objective function to fT according to the eigenvalues; S437: Optimize the objective function T according to the updated sub-objective function fT * AB = arg minTAB fT, and finally output the optimized T*AB, which is the optimized external parameter.

10. A system for implementing the map construction method for multi-lidar collaboration according to any one of claims 1 to 9, characterized in that it includes a data acquisition system, a front-end odometry, a back-end optimization system, and an online extrinsic parameter calibration system; The described data acquisition system is used to obtain the original point cloud data of the same mapping area and send it to the front-end odometer; the front-end odometer is used to solve the initial external parameters between multiple lidars by using the original point cloud data, and perform feature extraction on the original power data to obtain point-plane features According to the point-plane features Perform feature matching on the original point cloud data to obtain the initialized front-end pose and input it into the back-end optimization system; the back-end optimization system is used to optimize the front-end pose to obtain the globally optimized pose and the globally optimized map, and output the SLAM map; The online extrinsic parameter calibration system is used to optimize and calibrate the extrinsic parameters of multiple lidars according to the globally optimized pose and the initial extrinsic parameter values, output the optimized extrinsic parameters, and return the optimized extrinsic parameters to the front-end odometry.

Images