Laser mapping method, laser mapping device and electronic equipment

Abstract

The invention relates to a laser mapping method, a laser mapping device and electronic equipment, and the method comprises the steps: generating at least one laser speedometer track through collected laser point cloud data and wheel speed meter measurement data, determining a key frame in each laser speedometer track, taking the minimum calculation result of a first optimization function as a target, and carrying out the optimization of the key frame in the laser speedometer track. And optimizing the original poses of the key frames according to the first measurement information of the key frames to obtain first optimized poses, further removing abnormal values, and optimizing the original poses of the key frames through the first measurement information after the abnormal values are removed by taking the minimum calculation result of the second optimization function as a target to obtain second optimized poses. And finally, a point cloud map is constructed according to the second optimized pose of each key frame and the laser point cloud data, so that laser mapping is realized, the utilization rate of effective measurement values is improved, and the mapping accuracy and efficiency are improved.

Description

Technical Field

[0001] The present disclosure relates to the technical field of unmanned laser mapping, and in particular, to a laser mapping method, a laser mapping device, and an electronic device. Background Art

[0002] Laser mapping technology is one of the core technologies in the field of autonomous driving. The L4-level autonomous driving solution is based on a high-precision map, and the mapping accuracy will affect the online positioning accuracy. An inaccurate map may even endanger driving safety.

[0003] In existing mapping algorithms, the RTK (Real-Time Kinematic) sensor is the only global measurement source, which can provide globally consistent high-precision positioning information without cumulative error for the mapping algorithm. After receiving signals from multiple satellites, the RTK receiver will attempt to calculate the position of the current receiver and give the confidence level of the current positioning accuracy. Sometimes, the confidence level of the position calculation is not completely accurate, and there are often situations where the RTK confidence level is normal, but the actual positioning result is abnormal, that is, false positive outliers. The false positive RTK measurement values will bias the mapping and positioning trajectories.

[0004] The prior art usually deletes the measurement values at the positions where RTK false positives may occur by manual experience judgment. Such a manual deletion method relies heavily on the operator's manual experience, and manual deletion can only qualitatively delete the entire segment of RTK data. In this way, not all false positive RTK measurement values can be deleted, and true positive RTK measurement values may be mistakenly deleted; in addition, manual deletion requires returning to laser odometry mapping again. Therefore, the prior art often has problems such as the mapping trajectory being biased by RTK false positive measurement values, poor mapping quality, and frequent mapping rework. Summary of the Invention

[0005] In order to solve the above technical problems or at least partially solve the above technical problems, embodiments of the present disclosure provide a laser mapping method, a laser mapping device, and an electronic device, which solve the problems in the prior art such as the mapping trajectory being biased by RTK false positive measurement values, poor mapping quality, and frequent mapping rework.

[0006] In a first aspect, an embodiment of the present disclosure provides a laser mapping method, which includes:

[0007] Generating at least one laser odometry trajectory according to the collected laser point cloud data and wheel speedometer measurement data;

[0008] For each of the laser odometry trajectories, key frames in the laser odometry trajectory are determined, and with the goal of minimizing the calculation result of a first optimization function, the original poses of the key frames are optimized according to the first measurement information of each key frame to obtain first optimized poses; wherein, the first measurement information includes an odometry measurement value and a differential positioning measurement value, the first optimization function includes a second optimization function and a robust loss function, and the second optimization function includes a residual term between the original pose and the odometry measurement value, and a residual term between the original pose and the differential positioning measurement value;

[0009] Outliers in each first measurement information are removed according to the first optimized poses of each key frame;

[0010] With the goal of minimizing the calculation result of the second optimization function, the original poses of the key frames are optimized according to the first measurement information after removing outliers to obtain second optimized poses;

[0011] A point cloud map is constructed according to the second optimized poses of the key frames in each laser odometry trajectory and the laser point cloud data.

[0012] In a second aspect, an embodiment of the present disclosure further provides a laser mapping device, and the device includes:

[0013] An odometry trajectory determination module, configured to generate at least one laser odometry trajectory according to the acquired laser point cloud data and wheel speedometer measurement data;

[0014] A first optimization module, configured to, for each of the laser odometry trajectories, determine key frames in the laser odometry trajectory, and with the goal of minimizing the calculation result of a first optimization function, optimize the original poses of the key frames according to the first measurement information of each key frame to obtain first optimized poses; wherein, the first measurement information includes an odometry measurement value and a differential positioning measurement value, the first optimization function includes a second optimization function and a robust loss function, and the second optimization function includes a residual term between the original pose and the odometry measurement value, and a residual term between the original pose and the differential positioning measurement value;

[0015] A first outlier removal module, configured to remove outliers in each first measurement information according to the first optimized poses of each key frame;

[0016] A second optimization module, configured to, with the goal of minimizing the calculation result of the second optimization function, optimize the original poses of the key frames according to the first measurement information after removing outliers to obtain second optimized poses;

[0017] A mapping module, configured to construct a point cloud map according to the second optimized poses of the key frames in each laser odometry trajectory and the laser point cloud data.

[0018] In a third aspect, embodiments of the present disclosure further provide an electronic device, which includes: one or more processors; a storage device for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the laser mapping method as described above.

[0019] In a fourth aspect, embodiments of the present disclosure further provide a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the laser mapping method as described above.

[0020] A laser mapping method provided by an embodiment of the present disclosure generates at least one laser odometry trajectory through the collected laser point cloud data and wheel speedometer measurement data. For each laser odometry trajectory, key frames in the laser odometry trajectory are determined. With the goal of minimizing the calculation result of the first optimization function, the original poses of the key frames are optimized according to the first measurement information of each key frame to obtain the first optimized poses. Furthermore, outliers in each first measurement information are removed according to the first optimized positions of each key frame, and with the goal of minimizing the calculation result of the second optimization function, the original poses of each key frame are optimized through the first measurement information after removing outliers to obtain the second optimized poses. Finally, a point cloud map is constructed based on the second optimized poses of each key frame and the laser point cloud data, realizing laser mapping. This method can remove outliers through the result of the first round of optimization, and re-perform the second round of optimization according to the measurement information after removing outliers, improving the utilization rate of valid measurement values and the mapping accuracy, solving problems such as mapping trajectory deviation, unevenness, and inconsistency caused by outdoor occlusion or abnormal measurement values in indoor-outdoor switching scenarios. Moreover, it solves the problems of low efficiency and long time consumption caused by relying on manual deletion of outliers, reducing the dependence on manual mapping experience. In addition, this method can decouple the time-consuming laser odometry link from the outlier removal and other links, only need to execute the laser odometry link once, and there is no need to execute the laser odometry link again after removing outliers, further improving the mapping efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In combination with the accompanying drawings and referring to the following specific embodiments, the above and other features, advantages, and aspects of the various embodiments of the present disclosure will become more obvious. Throughout the drawings, the same or similar reference numerals denote the same or similar elements. It should be understood that the drawings are schematic, and the original and elements are not necessarily drawn to scale.

[0022] Figure 1 is a flowchart of a laser mapping method in an embodiment of the present disclosure;

[0023] Figure 2 is a schematic diagram of the process of the first-stage optimization in an embodiment of the present disclosure;

[0024] Figure 3 Schematic diagram of the process of multi-trajectory and multi-constraint pose optimization provided by an embodiment of the present disclosure;

[0025] Figure 4 Schematic diagram of the structure of a laser mapping device in an embodiment of the present disclosure;

[0026] Figure 5 Schematic diagram of the structure of an electronic device in an embodiment of the present disclosure. Detailed implementation manners

[0027] Embodiments of the present disclosure will be described in more detail with reference to the accompanying drawings. Although some embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. On the contrary, these embodiments are provided to more thoroughly and completely understand the present disclosure. It should be understood that the drawings and embodiments of the present disclosure are only for illustrative purposes and are not used to limit the protection scope of the present disclosure.

[0028] It should be noted that the concepts such as "first" and "second" mentioned in the present disclosure are only used to distinguish different devices, modules or units, and are not used to limit the order of functions executed by these devices, modules or units or their interdependent relationships.

[0029] The names of the messages or information exchanged between multiple devices in the embodiments of the present disclosure are only for illustrative purposes and are not used to limit the scope of these messages or information.

[0030] Before introducing the laser mapping method provided by the embodiments of the present disclosure in detail, the technical problems solved by this method will be described first.

[0031] In the prior art, the RTK sensor is the only global measurement source, which can provide globally consistent high-precision positioning information without cumulative error for the mapping algorithm. After receiving signals from multiple satellites, the RTK receiver will try to calculate the UTM (Universal Transverse Mercator) coordinate position of the current receiver and give the confidence level of the current positioning accuracy. Sometimes the confidence level of the position calculation is not completely accurate, and there are often situations where the RTK confidence level is normal, but the actual positioning result is abnormal. The embodiments of the present disclosure call this type of RTK data false positive outliers. The measured values of RTK include: position (x, y, z) and heading angle (yaw). The most common abnormal situation is that the RTK heading angle yaw and height z show false positives. The false positive RTK measured values will deviate the mapping and positioning trajectories.

[0032] In existing online mapping algorithms, in scenarios such as outdoor RTK occlusion, the accuracy of RTK measurement values will gradually deteriorate. To ensure the reliability of RTK filtering, after obtaining a measurement value file containing false positive RTK data, the prior art usually manually deletes the measurement values at positions where RTK false positives may occur through manual experience judgment.

[0033] This manual deletion method relies heavily on the operator's manual experience, and manual operation can only qualitatively delete the entire segment of RTK data, unable to quantitatively judge each RTK measurement value one by one. In this way, not all false positive RTK measurement values can be deleted, and many normal RTK measurement values will be mistakenly deleted. Without manual intervention, existing mapping algorithms are difficult to obtain accurate, consistent, and reliable mapping results. Moreover, there must be a large number of repeated mapping and rework operations in this process, resulting in low mapping efficiency.

[0034] Therefore, to solve the above problems, the embodiments of the present disclosure provide a laser mapping method, which can solve the problems in the prior art of relying on manual experience to delete jumpy and false positive RTK measurement values and manually coordinating multiple inconsistent measurement sources through repeated experiments. It can also solve the problems in the prior art of low mapping deployment efficiency, long time-consuming repeated mapping, and high rework cost.

[0035] Figure 1 It is a flowchart of a laser mapping method in the embodiments of the present disclosure. This method can be executed by a laser mapping device, which can be implemented in software and / or hardware, and the device can be configured in an electronic device. As Figure 1 shown, the method specifically may include the following steps:

[0036] S110. Generate at least one laser odometry trajectory according to the collected laser point cloud data and wheel speedometer measurement data.

[0037] Specifically, odometry mapping can be performed based on the laser point cloud data and wheel speedometer measurement data. Since there is no interference from constraints such as RTK measurement and loop measurement, high-precision laser odometry mapping can obtain a globally consistent and smooth laser odometry trajectory. Among them, by executing the laser odometry link in parallel, laser odometry trajectories of multiple routes can be obtained, that is, odometry mapping is performed on the laser point cloud data and wheel speedometer measurement data collected by each acquisition vehicle respectively.

[0038] After obtaining multiple laser odometry trajectories, to facilitate subsequent joint optimization of multiple trajectories, each laser odometry trajectory can be transformed into the same global coordinate system, such as the UTM coordinate system.

[0039] In a specific implementation, after generating at least one laser odometry trajectory, the following steps are further included:

[0040] Step 11: For each laser odometry trajectory, put the positive differential positioning measurement values of each frame in the laser odometry trajectory into the first set, and put the positioning anchor points with confidence greater than the set threshold of each frame into the first set;

[0041] Step 12: Put the original poses corresponding to each differential positioning measurement value or each positioning anchor point in the first set into the second set;

[0042] Step 13: Determine the transformation matrix between the local coordinate system and the global coordinate system corresponding to the laser odometry trajectory according to the first set and the second set, and convert the original poses of each frame in the laser odometry trajectory to the global coordinate system according to the transformation matrix.

[0043] Among them, in step 11, for each frame in the laser odometry trajectory, the differential positioning measurement value (i.e., RTK measurement value) corresponding to this frame can be obtained. These RTK measurement values may be positive points (such as RTK points outdoors) or negative points (such as RTK points indoors or outdoors with occlusion); it can be judged whether the RTK measurement values of each frame in the laser odometry trajectory are positive. If so, put the positive differential positioning measurement values into the first set.

[0044] And, positioning can be performed on the LO (Lidar Odometry) guided map or on the positioning map to obtain the positioning anchor point corresponding to each frame and the corresponding confidence. Further, it can be judged whether the confidence of the positioning anchor point corresponding to each frame in the laser odometry trajectory is greater than the set threshold. If so, put the positioning anchor points with confidence greater than the set threshold into the first set.

[0045] Further, in step 12, the original poses corresponding to each differential positioning measurement value or each positioning anchor point in the first set can be put into the second set. Among them, the original pose refers to the pose in the laser odometry trajectory obtained through odometry mapping.

[0046] Among them, the points in the first set are in the global coordinate system; the points in the second set are in the local coordinate system. The local coordinate system is constructed with the starting frame as the origin, and the pose in the local coordinate system refers to the relative pose with respect to the starting frame. For each laser odometry trajectory, the process of transforming it to the global coordinate system can be regarded as the process of minimizing the overall error between the points after transforming the points in the second set to the first set.

[0047] Assume that in the first set (anchor set) and the second set (odom set), there are several pairs of paired anchor coordinate points and odom coordinate points. The anchor coordinate points are P = {p1, p2,..., p n}, and the odom coordinate points are Q = {q1, q2,..., q n}, where p i and q i are coordinate points in three-dimensional space. The rotation matrix between the odom trajectory and the RTK trajectory is R, and the translation distance is t. Then, a least squares problem can be constructed:

[0048]

[0049] Based on the SVD (Singular Value Decomposition) method, the rotation R and translation t that minimize the above sum of squared errors can be obtained.

[0050] Therefore, in step 13, a least squares problem can be constructed to minimize the overall error between the points in the second set after being transformed to the first set. Then, the points in the first set and the second set are substituted into this least squares problem to obtain the transformation matrix between the local coordinate system and the global coordinate system corresponding to the lidar odometry trajectory. Among them, the transformation matrix includes a rotation matrix and a translation matrix.

[0051] After obtaining the transformation matrix, further, according to the transformation matrix, the original poses of each frame in the lidar odometry trajectory can be converted from the local coordinate system to the global coordinate system.

[0052] Among them, encapsulating the rotation R and translation t into the transformation matrix gives:

[0053]

[0054] Assume that the original pose of each frame in the lidar odometry trajectory includes translation amounts x, y, z, and rotation amounts roll, pitch, yaw. Encapsulating the 6-degree-of-freedom original pose x in the local coordinate system into the transformation matrix:

[0055]

[0056] Then, the process of transforming the local original pose x to the global coordinate system can be expressed as:

[0057]

[0058] In the formula, y is the original pose in the global coordinate system, R x , t xR and t are the rotation matrix and translation matrix corresponding to the original pose in the local coordinate system. y and y are the rotation matrix and translation matrix corresponding to the original pose in the global coordinate system.

[0059] Repeating the above process can transform the entire laser odometry trajectory to the global coordinate system, such as the UTM coordinate system. Since each laser odometry trajectory has a corresponding first set and second set, repeating this process can unify the original poses in multiple laser odometry trajectories to the global coordinate system.

[0060] Through the above steps 11 - 13, considering that each laser odometry trajectory is established in the corresponding local coordinate system, to facilitate subsequent interaction with other trajectories and alignment with the actual on-site RTK measurement values, the original pose in the laser odometry trajectory can be transformed to the global coordinate system to ensure the accuracy of optimization during subsequent single-trajectory optimization and multi-trajectory global joint optimization.

[0061] S120. For each laser odometry trajectory, determine the key frames in the laser odometry trajectory, aiming to minimize the calculation result of the first optimization function, and optimize the original poses of the key frames according to the first measurement information of each key frame to obtain the first optimized pose.

[0062] Among them, the first measurement information includes the odometer measurement value (odom measurement value) and the differential positioning measurement value, the first optimization function includes the second optimization function and the robust loss function, and the second optimization function includes the residual term between the original pose and the odometer measurement value, and the residual term between the original pose and the differential positioning measurement value.

[0063] Specifically, for each laser odometry trajectory, the key frames in the laser odometry trajectory can be determined first. For example, all frames can be used as key frames, or considering that the difference between frames is small when the data acquisition frequency is high, some frames can also be selected as key frames according to a preset interval when the data acquisition frequency is greater than the set frequency, otherwise, all frames are used as key frames.

[0064] In the embodiments of the present disclosure, according to the factor graph optimization theory, the key frames can be used as the optimization vertices of the factor graph, the odom measurement value is the binary constraint between the vertices, and the RTK measurement value is the unary constraint associated with the vertices. The vertex pose is represented by a 6-degree-of-freedom transformation matrix:

[0065]

[0066] Then the multi-constraint pose graph optimization problem can be expressed as the following least squares problem:

[0067]

[0068] In the formula, r k (χ k ) is the residual term of a certain constraint, and χ k is the set of key frames x k associated with this constraint. Ω k is the information matrix corresponding to this constraint, which is used to represent the weight size of this constraint in the optimization problem. K is the number of constraints. Ω k is the inverse matrix of the measurement noise covariance matrix ∑ k of this constraint. Therefore, it can be qualitatively obtained that the smaller the noise of a certain constraint, the smaller the noise variance, the smaller the covariance matrix, the larger the information matrix, and the greater the weight in the optimization. For a certain key frame x k , there may be various measurement constraints associated with it. Pose graph optimization is to uniformly adjust the poses of all key frames globally so that the sum of the squared residuals constructed by all constraints associated with all poses is minimized.

[0069] In the embodiments of the present disclosure, two rounds of optimization can be set. The main purpose of the first round of optimization is to eliminate RTK outliers, and the main purpose of the second round of optimization is to use normal RTK measurement values for pose graph optimization to obtain an accurate laser mapping result. Among them, according to the residual term between the original pose and the differential positioning measurement value (representing the RTK constraint), and the residual term between the original pose and the odometer measurement value (representing the odom constraint), a second optimization function can be constructed, and this second optimization function is used in the second round of optimization process; and a robust loss function is applied to the second optimization function to obtain a first optimization function, and this first optimization function is used in the first round of optimization process.

[0070] In the embodiments of the present disclosure, odom measurement is a binary constraint. The residual term between the original pose and the odometer measurement value (i.e., the odom constraint) can be expressed as:

[0071]

[0072] In the formula, x k and x j are the original poses of two key frames respectively, is the relative pose measurement between two key frames, that is, the odometer measurement value. Log(·) represents the logarithmic mapping that converts the transformation matrix into a 6-dimensional vector, represents the generalized subtraction, which represents finding the relative pose between two quantities. is the residual term of the odom constraint, and χ k = {x k , x j} is the set of key frames, and the covariance matrix ∑ kSet as a 6D diagonal matrix, and the diagonal stores the noise variances of the odom constraint in six directions: translation in x, y, z and rotation in roll, pitch, yaw.

[0073] In the embodiments of the present disclosure, RTK measurement is a unary constraint, and the residual term (i.e., RTK constraint) between the original pose and the differential positioning measurement value can be expressed as:

[0074]

[0075] In the formula, x k is the original pose of the key frame, is the RTK measurement value corresponding to the key frame x k , Log(·) represents the logarithmic mapping that converts the transformation matrix into a 6D vector, represents the generalized subtraction, which represents finding the relative pose between two quantities. is the residual term of the RTK constraint, χ k ={x k} is the set of key frames, and the covariance matrix ∑ k is set as a 6D diagonal matrix, and the diagonal stores the noise variances of the RTK constraint in six directions: translation in x, y, z and rotation in roll, pitch, yaw. Since there is no measurement in the roll and pitch directions for RTK, the noise variances in these two directions can be set to extremely large values.

[0076] Specifically, the residual term between the original pose and the odometer measurement value, as well as the residual term between the original pose and the differential positioning measurement value, can be combined to construct a second optimization function that describes the sum of squares of all residual terms associated with the original pose of the key frame:

[0077]

[0078] In the formula, K is equal to 2, r1(χ1) can be the residual term between the original pose and the differential positioning measurement value, and r2(χ2) can be the residual term between the original pose and the odometer measurement value. Minimizing this second optimization function is a least squares problem that minimizes the sum of squares of all residual terms associated with the original pose of the key frame.

[0079] Considering that there may be false positives in the RTK measurement values, at this time in , the difference between the RTK measurement value and the key frame pose x k is large, and the value of the residual is large. Also, the default noise of the RTK measurement is small and its weight in the optimization is large. To reduce the residual, the key frame pose x kwill be over-optimized to a wrong position (i.e., in order to obtain the smallest possible x k will be close to the false positive ), therefore, the false positive RTK measurement value may cause the above second optimization function to obtain an incorrect result, and the overall optimization result shows that the mapping trajectory is deviated and jumps. It can be considered that the false positive RTK measurement value is an outlier, and the false positive RTK measurement value is screened out through the first round of optimization.

[0080] To solve this problem, two rounds of optimization can be performed. The first round of optimization uses the function after applying the robust loss function to the second optimization function, where the robust loss function can specifically be the robust Cauchy kernel function. For example, the first optimization function is expressed as:

[0081]

[0082] In the formula, the robust Cauchy kernel function is expressed as:

[0083]

[0084] In the formula, δ is the control parameter of the robust Cauchy kernel function. The robust Cauchy kernel function can impose constraints on the residual chi-square r 2 When the residual chi-square is small, the function value ρ cauchy (r 2 ) is relatively close to the input value r 2 . When the residual chi-square increases, the robust Cauchy kernel function will rapidly decrease or flatten the rising amplitude (trend) of the function value ρ 2 (r cauchy ) as the input value r 2 increases, thereby reducing the negative impact of the residual chi-square r 2 with a large error on the overall least squares problem.

[0085] Compared with the original least squares problem, when the robust Cauchy kernel function is imposed on the residual chi-square, the optimization result will show the following characteristics: If the RTK measurement value is a true positive, then this value is originally relatively close to the original pose x k of the key frame. Therefore, the residual chi-square r 2 constructed by the two is small and is less affected by the robust Cauchy kernel function. The optimization result should be a value that is very close to the RTK measurement value ; If the RTK measurement value is a false positive, then this value is originally a value that is relatively different from the original pose x k of the key frame. The residual chi-square r constructed by the two2 will be relatively large, and thus be greatly affected by the robust Cauchy kernel function. At this time, ρ cauchy (r 2 ) has little impact / contribution to the optimization problem, the original pose x of the key frame k No over-optimization due to incorrect measurements, optimized results Should be a false positive RTK measurement value There is a big difference between the values.

[0086] Therefore, through the above first optimization function, the goal can be to minimize the calculation result (i.e. The odometer measurement value and differential positioning measurement value of each key frame are substituted into the first optimization function, and the original pose of each key frame is optimized in the first round to obtain the first optimized pose of each key frame, so that the false positive RTK measurement value can be eliminated according to the first optimized pose, and the second round of optimization can be performed according to the normal RTK measurement value.

[0087] S130: Eliminate abnormal values ​​in each first measurement information according to the first optimized posture of each key frame.

[0088] Among them, the outliers in the first measurement information include false positive RTK measurement values. Specifically, after the first round of optimization is completed, the false positive RTK measurement values ​​can be screened out from the first measurement information as outliers based on the residual chi-square between the first optimized pose and the differential positioning measurement value, and then removed. Exemplarily, the residual chi-square between the second optimized pose and the RTK measurement value is:

[0089]

[0090] The residual term involved in the formula is:

[0091]

[0092] Among them, according to the characteristics that the residual chi-square corresponding to the false positive RTK measurement value is relatively large, while the residual chi-square corresponding to the true positive RTK measurement value is relatively small, the chi-square threshold can be set in advance, and the residual chi-square of each frame can be judged one by one. If the residual chi-square is greater than the chi-square threshold, the RTK measurement value is considered to be a false positive and can be removed as an outlier.

[0093] In the embodiments of the present disclosure, taking into account the business demand of expanding new routes with old maps that often arises in practice, under this demand, the mapping of the new route must not only comply with odom constraints, RTK constraints, etc. on the new site, but also the maploc (map location) constraints from the old map in the area where the new route overlaps with the old route, so that there is no ghosting between the new route map and the old map in the overlapping area.

[0094] Among them, the maploc measurement is a unary constraint, and the residual term (i.e., the maploc constraint) between the original pose and the anchor point positioning measurement value is expressed as:

[0095]

[0096] In the formula, x k is the original pose of the key frame, is the maploc measurement value (anchor point positioning measurement value) corresponding to the key frame x k , Log(·) represents the logarithmic mapping that converts the transformation matrix into a 6D vector, represents the generalized subtraction, which represents finding the relative pose between two quantities. is the residual term of the maploc constraint, χ k ={x k} is the set of key frames, and the covariance matrix ∑ k is set as a 6D diagonal matrix, and the diagonal stores the noise variances of the maploc constraint in 6 directions of translation x, y, z and rotation roll, pitch, yaw.

[0097] Therefore, in order to achieve the joint alignment of the new and old maps, such as the joint alignment of the existing indoor map and the RTK sections of multiple exits, the method provided by the embodiments of the present disclosure can be applicable to both the joint mapping of multi-trajectory new scenarios and the expansion / updating of old maps in pure outdoor, indoor-outdoor scenarios, and the maploc constraint can also be introduced in the first-round optimization and the second-round optimization.

[0098] In an alternative embodiment, the method provided by the embodiments of the present disclosure further includes: determining the positioning anchor points of the key frames in the historical map in each laser odometry trajectory to obtain the anchor point positioning measurement values of the key frames;

[0099] The first measurement information further includes the anchor point positioning measurement values, and the second optimization function further includes the residual term between the original pose and the anchor point positioning measurement values.

[0100] Specifically, the new route data (i.e., the laser odometry trajectory) can be positioned on the old map, and the positioning method can be LO-guided positioning or map positioning to obtain the positioning anchor point maploc of the new route on the old map, that is, the anchor point positioning measurement values of each frame in the laser odometry trajectory.

[0101] In the process of constructing the second optimization function, the residual term between the original pose and the anchor point positioning measurement values can also be constructed and added to the second optimization function. For example, the second optimization function is expressed as:

[0102]

[0103] Wherein, K is equal to 3, r1(χ1) can be the residual term between the original pose and the differential positioning measurement value, r2(χ2) can be the residual term between the original pose and the odometer measurement value, and r3(χ3) can be the residual term between the original pose and the anchor point positioning measurement value.

[0104] By constructing a second optimization function including the residual term between the original pose and the anchor point positioning measurement value, the residual term between the original pose and the differential positioning measurement value, and the residual term between the original pose and the odometer measurement value, the residual term between the original pose and the anchor point positioning measurement value can also be included in the first optimization function.

[0105] Furthermore, in the first-round optimization process, the goal can be to minimize the calculation result of the first optimization function (i.e., ), substitute the odometer measurement value, differential positioning measurement value, and anchor point positioning measurement value of each key frame into the first optimization function, and perform the first-round optimization on the original pose of each key frame to obtain the first optimized pose of each key frame.

[0106] Through the above implementation, the maploc constraint can play a role in the optimization process to obtain the joint optimization result of multi-constraint measurements, which is applicable to both the joint mapping of multi-trajectory new scenarios and the extension / updating of old maps in pure outdoor and indoor-outdoor scenarios, realizing the joint alignment of new and old maps. It greatly reduces the mapping difficulty when coordinating the consistency of multiple measurement constraints in challenging scenarios, reduces the dependence on the operator's mapping experience, and improves the accuracy and consistency of the mapping algorithm. Figure 1 consistency.

[0107] After introducing the maploc constraint for the first-round optimization, correspondingly, during the process of removing outliers, in addition to removing false positive RTK measurement values, false positive maploc measurement values can also be removed.

[0108] In a specific implementation, removing outliers from each first measurement information according to the first optimized pose of each key frame includes:

[0109] For each key frame, determine the residual chi-square between the first optimized pose of the key frame and the differential positioning measurement value, and the residual chi-square between the first optimized pose and the anchor point positioning measurement value; determine the differential positioning measurement value and the anchor point positioning measurement value with a residual chi-square greater than the preset chi-square threshold as outliers, and remove the outliers.

[0110] Specifically, for each key frame, the residual chi-square between the first optimized pose and the differential positioning measurement value can be calculated, and the residual chi-square between the first optimized pose and the anchor point positioning measurement value can be calculated. Among them, the calculation of the residual chi-square can refer to the previous discussion.

[0111] Further, the differential positioning measurement values with a residual chi-square greater than a preset chi-square threshold and the anchor point positioning measurement values can be determined as false positive outliers and eliminated.

[0112] Through the above implementation, while eliminating the false positive RTK measurement values, the false positive maploc measurement values can also be eliminated, ensuring the reliability of the first measurement information participating in the second-round optimization, and further ensuring the accuracy of the second-round optimization.

[0113] S140. With the goal of minimizing the calculation result of the second optimization function, optimize the original poses of each key frame according to the first measurement information after eliminating outliers to obtain the second optimized poses.

[0114] In the embodiment of the present disclosure, after the first-round optimization and outlier elimination, the measurement values in the remaining first measurement information can be determined as true positives. Then, the remaining true positive measurement values can be added to the optimization problem, and the robust Cauchy kernel function can be cancelled to avoid the kernel function affecting the optimization result. Using the original poses of the key frames as the optimization vertices and performing pose graph optimization again, a mapping trajectory not affected by false positive measurements can be obtained.

[0115] Specifically, with the goal of minimizing the calculation result of the second optimization function (i.e., Input the odometer measurement values and differential positioning measurement values after eliminating outliers into the second optimization function, and optimize the original poses of each key frame to obtain the second optimized poses of each key frame.

[0116] Of course, if the second optimization function also includes a residual term between the original pose and the anchor point positioning measurement value, the anchor point positioning measurement values after eliminating outliers can also be input into the second optimization function together to optimize the original poses of each key frame, so that the measurement constraints from odom, RTK, and maploc can be coordinated in the multi-constraint pose graph optimization, ensuring that there is no ghosting between the new route and the old map in the new route mapping scenario.

[0117] S150. Construct a point cloud map according to the second optimized poses of the key frames in each laser odometer trajectory and the laser point cloud data.

[0118] Specifically, after the second-round optimization, the second optimized poses of the key frames in each laser odometer trajectory can be obtained, and a point cloud map can be constructed in combination with the point clouds corresponding to each key frame in the laser point cloud data.

[0119] In the embodiment of the present disclosure, the first-round optimization and the second-round optimization can be regarded as the first-stage optimization. Figure 2 It is a schematic diagram of the process of the first-stage optimization in the embodiment of the present disclosure, as shown in Figure 2As shown, compared with the prior art, the prior art needs to rely on manual experience to filter false positives and perform the laser odometry part, i.e., odometry mapping, multiple times after filtering, resulting in time-consuming work. In the embodiments of the present disclosure, false positives can be filtered fully automatically, and the laser odometry part only needs to be executed once, greatly reducing the working hours.

[0120] Reference Figure 2 , the process of the first-stage optimization is specifically as follows: First, perform the laser odometry part, that is, construct the laser odometry trajectory. Then, based on the LO factor (i.e., the original pose) in the laser odometry trajectory, the RTK factor (i.e., the differential positioning measurement value) in the RTK trajectory (constituted by the RTK measurement values of each frame), and the robust kernel function, perform the first-round optimization. After optimization, construct an error chi-square to filter the false positive RTK measurement values. Then, based on the LO factor and the normal RTK factor obtained after filtering, perform the second-round optimization to obtain the mapping anchor points. This process can decouple the laser odometry part from the optimization part. The laser odometry part with a long time consumption (such as 5 minutes / 1000 frames) only needs to be executed once, while the processing speed of the two-round optimization is relatively fast (it can reach 0.5 seconds / 1000 frames), which can greatly improve the mapping efficiency.

[0121] The laser mapping method provided in this embodiment generates at least one laser odometry trajectory through the collected laser point cloud data and wheel speedometer measurement data. For each laser odometry trajectory, determine the key frames in the laser odometry trajectory. With the goal of minimizing the calculation result of the first optimization function, optimize the original pose of each key frame according to the first measurement information of each key frame to obtain the first optimized pose. Then, eliminate the outliers in each first measurement information according to the first optimized position of each key frame. With the goal of minimizing the calculation result of the second optimization function, optimize the original pose of each key frame through the first measurement information after eliminating the outliers to obtain the second optimized pose. Finally, construct a point cloud map according to the second optimized pose of each key frame and the laser point cloud data, realizing laser mapping. This method can eliminate outliers through the result of the first-round optimization and re-perform the second-round optimization according to the measurement information after eliminating the outliers, improving the utilization rate of valid measurement values and the mapping accuracy, solving the problems such as the mapping trajectory being skewed, uneven, and inconsistent caused by abnormal measurement values in outdoor occlusion or indoor-outdoor switching scenarios. Moreover, it solves the problems of low efficiency and long time consumption caused by relying on manual deletion of outliers, reducing the dependence on manual mapping experience.

[0122] During the first stage optimization process, the optimization between each laser odometer track is independent of each other and does not affect each other. Considering that in practice a single track may return to a position that it has passed through, or that multiple tracks may have overlapping areas, in order to make the point cloud maps of these overlapping areas as consistent and aligned as possible without ghosting, in the disclosed embodiment, after the first stage optimization, loop detection can be performed within a single track and between multiple tracks, and a second stage optimization can be performed. The second stage optimization includes the third round of optimization and the fourth round of optimization. The second stage optimization can refer to the first stage optimization, the difference is that the second stage optimization also introduces relative pose measurement values ​​(obtained through loop detection), and the second stage optimization is performed on the basis of the first stage optimization results.

[0123] In a specific implementation, constructing a point cloud map according to the second optimized pose of the key frame in each laser odometer trajectory and the laser point cloud data includes the following steps:

[0124] Step 21: Detect loop frame pairs for each laser odometer trajectory, and determine the relative pose measurement value between two key frames in the loop frame pair;

[0125] Step 22: for each laser odometer trajectory, with the goal of minimizing the calculation result of the third optimization function, the second optimized pose of each key frame is optimized according to the second measurement information of each key frame in the laser odometer trajectory to obtain a third optimized pose; wherein the second measurement information includes odometer measurement values, differential positioning measurement values ​​and relative pose measurement values, the third optimization function includes a fourth optimization function and a robust loss function, and the fourth optimization function includes a residual term between the second optimized pose and the odometer measurement value, a residual term between the second optimized pose and the differential positioning measurement value, and a residual term between the second optimized pose and the relative pose measurement value;

[0126] Step 23: Eliminate abnormal values ​​in each second measurement information according to the third optimized posture of each key frame;

[0127] Step 24, with the goal of minimizing the calculation result of the fourth optimization function, optimizing the second optimized pose of each key frame according to the second measurement information after removing the abnormal value, to obtain a fourth optimized pose;

[0128] Step 25: construct a point cloud map according to the fourth optimized pose of the key frame in each laser odometer trajectory and the laser point cloud data.

[0129] Among them, in step 21, the optimization result of the first stage is relatively consistent with the true positive RTK measurement and maploc measurement. Under this premise, loop detection can be performed on the key frames in the multi-track.

[0130] Exemplarily, for a certain trajectory, the Euclidean distance between adjacent two points in the trajectory can be calculated. Starting from the starting point, the Euclidean distances between all adjacent points before each point are accumulated together and defined as the cumulative distance of this point (the cumulative distance is the distance of a certain point relative to the starting point, rather than the displacement between two points); and then loop detection is performed through the cumulative distance.

[0131] Regarding the above step 21, in one example, the detection of loop frame pairs for each laser odometry trajectory includes:

[0132] Determine the loop frame pairs within a single trajectory and the loop frame pairs between multiple trajectories according to the second optimized poses of the key frames in each laser odometry trajectory;

[0133] Among them, for any two key frames in the same laser odometry trajectory, the two key frames that meet the following conditions are determined as loop frame pairs: the cumulative distance difference between the two key frames is greater than a preset distance threshold, and the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold;

[0134] For any two key frames in different laser odometry trajectories, the two key frames that meet the following conditions are determined as loop frame pairs: the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold.

[0135] Specifically, a certain key frame frame_m1 in a laser odometry trajectory can be traversed first, and all key frames frame_m2 in multiple trajectories after this key frame can be traversed. If frame_m1 and frame_m2 belong to the same laser odometry trajectory, they are determined as loop frame pairs when the following conditions are met: 1. The cumulative distance difference between the two key frames is greater than a preset distance threshold; 2. The pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold. If frame_m1 and frame_m2 do not belong to the same laser odometry trajectory, they are determined as loop frame pairs when the following condition is met: the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold. Repeating the above process can obtain all the loop frame pairs within a single trajectory and all the loop frame pairs between multiple trajectories.

[0136] Through the above example, the loop frame pairs within a single trajectory and between multiple trajectories can be detected, which is convenient for reducing the ghosting degree within a single trajectory and between multiple trajectories subsequently, and thus improving the reliability of the map.

[0137] In step 21, after all loop frame pairs are determined, loop registration can be performed to determine the relative pose measurement value between two key frames in the loop frame pair. Exemplarily, for each loop frame pair, several frames around the first key frame in the loop frame pair can be retrieved, and these frames can be stitched together with the second optimized pose to obtain a local point cloud map centered on the first key frame, and this local point cloud map can be used as the target point cloud; moreover, the point cloud of the second key frame in the loop frame pair is retrieved as the source point cloud, and with the second optimized pose of the second key frame as the initial registration value, point cloud registration is performed on the target point cloud and the source point cloud, and the registration method can select methods such as gicp, ndt, and icp to obtain the more accurate global pose of the second key frame.

[0138] Furthermore, calculate the relative pose between the second optimized pose of the first key frame and the global pose of the second key frame, and this relative pose can be used as the relative pose measurement value between the first key frame and the second key frame. Repeat the above process to obtain the relative pose measurement values (i.e., loop measurement values) associated with all loop frame pairs.

[0139] In the embodiments of the present disclosure, loop measurement is a binary constraint, and the residual term (representing the loop constraint) between the second optimized pose and the relative pose measurement value can be expressed as:

[0140]

[0141] In the formula, x k and x j are respectively the second optimized poses of two loop candidate key frames, is the relative pose measurement between two candidate frames, Log(·) represents the logarithmic mapping that converts the transformation matrix into a 6-dimensional vector, represents the generalized subtraction, indicating finding the relative pose between two quantities. is the residual term of the loop constraint, χ k ={x k ,x j} is the set of loop candidate key frames, and the covariance matrix ∑ k is set as a 6-dimensional diagonal matrix, and the diagonal stores the noise variances of the loop constraint in 6 directions of translation x, y, z and rotation roll, pitch, yaw, etc.

[0142] Therefore, in order to reduce the ghosting between the inside of a single trajectory and multiple trajectories, a fourth optimization function can be constructed by combining loop constraints. Specifically, the optimization in the second stage is carried out based on the optimization result of the first stage (i.e., the second optimized pose). Therefore, the fourth optimization function includes the residual term between the second optimized pose and the odometer measurement value, the residual term between the second optimized pose and the differential positioning measurement value, and the residual term between the second optimized pose and the relative pose measurement value. That is, the fourth optimization function includes odom constraints, RTK constraints, and loop constraints. For example, the second optimization function is expressed as:

[0143]

[0144] In the formula, K is equal to 3. r1(χ1) can be the residual term between the second optimized pose and the differential positioning measurement value, r2(χ2) can be the residual term between the second optimized pose and the odometer measurement value, and r3(X3) can be the residual term between the second optimized pose and the relative pose measurement value.

[0145] To further eliminate potential false positive measurement values, the optimization in the second stage can be divided into two times (i.e., the third round of optimization and the fourth round of optimization). For the third round of optimization, a third optimization function can be constructed to further eliminate the outliers in the second measurement information through the third round of optimization, including false positive RTK measurement values and false positive loop measurement values. Among them, the third optimization function can be a function after applying a robust loss function to the fourth optimization function, and its expression form can refer to the first optimization function, which will not be elaborated here.

[0146] Specifically, in step 22, for each lidar odometry trajectory, with the goal of minimizing the calculation result of the third optimization function, the odometer measurement value, differential positioning measurement value, and relative pose measurement value of each key frame are substituted into the third optimization function to perform the third round of optimization on the second optimized pose of each key frame, and the third optimized pose of each key frame is obtained.

[0147] Furthermore, in step 23, based on the chi-square of the residual between the third optimized pose and the differential positioning measurement value, and the chi-square of the residual between the third optimized pose and the relative pose measurement value, false positive differential positioning measurement values and relative pose measurement values can be screened out as outliers and then eliminated.

[0148] To further ensure that there is no ghosting between the new route and the old map, maploc constraints can also be introduced in the second stage optimization to further ensure the accuracy of mapping.

[0149] In an alternative embodiment, the method provided by the embodiments of the present disclosure further includes: determining the positioning anchor points of the key frames in the historical map for each lidar odometry trajectory to obtain the anchor point positioning measurement values of the key frames;

[0150] The second measurement information further includes the anchor point positioning measurement value, and the fourth optimization function further includes a residual term between the original pose and the anchor point positioning measurement value.

[0151] Specifically, in the process of constructing the third optimization function, a residual term between the second optimized pose and the anchor point positioning measurement value can be constructed and added to the third optimization function. Further, in the third-round optimization process, with the goal of minimizing the calculation result of the third optimization function, the odometry measurement value, differential positioning measurement value, relative pose measurement value, and anchor point positioning measurement value of each key frame can be substituted into the third optimization function to perform the third-round optimization on the second optimized pose of each key frame.

[0152] Through the above implementation manners, the maploc constraint can be substituted into the optimization process of the second stage, further ensuring the joint alignment of the new and old maps and improving the accuracy of the map.

[0153] After introducing the maploc constraint for the third-round optimization, correspondingly, in the process of removing outliers, in addition to removing false positive RTK measurement values and loop measurement values, false positive maploc measurement values can also be removed.

[0154] Regarding step 23 above, in one example, removing outliers from the second measurement information according to the third optimized pose of each key frame includes:

[0155] For each key frame, determine the chi-square of the residual between the third optimized pose of the key frame and the differential positioning measurement value, the chi-square of the residual between the third optimized pose and the anchor point positioning measurement value, and the chi-square of the residual between the third optimized pose and the relative pose measurement value;

[0156] Determine the differential positioning measurement value, anchor point positioning measurement value, and relative pose measurement value with a chi-square of the residual greater than the preset chi-square threshold as outliers and remove the outliers.

[0157] Specifically, for each key frame, the chi-square of the residual between the third optimized pose and the differential positioning measurement value, the chi-square of the residual between the third optimized pose and the anchor point positioning measurement value, and the chi-square of the residual between the third optimized pose and the relative pose measurement value can be calculated; and then the differential positioning measurement value, anchor point positioning measurement value, and relative pose measurement value with a chi-square of the residual greater than the preset chi-square threshold are determined as false positive outliers and removed.

[0158] Through the above example, false positive maploc measurement values can be removed while removing false positive RTK measurement values and loop measurement values, further ensuring the reliability of the second measurement information participating in the fourth-round optimization, thereby ensuring the accuracy of the fourth-round optimization.

[0159] After the third round of optimization and outlier rejection, in step 24, it can be determined that the measurement values in the remaining second measurement information are true positives. Furthermore, the measurement values of the remaining true positives can be added to the optimization problem, and the robust Cauchy kernel function can be cancelled to avoid the influence of the kernel function on the optimization result. Using the original pose of the key frame as the optimization vertex, perform pose graph optimization again, and the mapping trajectory unaffected by false positive measurements can be obtained.

[0160] Specifically, with the goal of minimizing the calculation result of the fourth optimization function, the odometer measurement values, differential positioning measurement values, and relative pose measurement values after outlier rejection are input into the fourth optimization function to optimize the second optimized poses of each key frame, and the fourth optimized poses of each key frame are obtained.

[0161] Of course, if the fourth optimization function also includes the residual term between the original pose and the anchor point positioning measurement value, the anchor point positioning measurement values after outlier rejection can also be input into the fourth optimization function to optimize the original poses of each key frame, so that the measurement constraints from odom, RTK, loop, and maploc can be coordinated in the multi-constraint pose graph optimization, further ensuring that there is no ghosting between the new route and the old map in the new route mapping scenario.

[0162] Furthermore, in step 25, a point cloud map can be constructed according to the fourth optimized poses of each key frame and the point clouds corresponding to each key frame in the laser point cloud data.

[0163] Through the above steps 21 - 25, loop detection and the second stage of optimization can be performed after the first stage of optimization, realizing multi-trajectory loop detection in scenarios with RTK global priors such as pure outdoor and indoor-outdoor, and reducing map ghosting in the overlapping areas of multi-trajectories in various scenarios. Moreover, through two filtering processes, the filtering rate for RTK, maploc, and loop outliers can reach 100%, avoiding manual deletion of false positive measurement values, and at the same time avoiding the deviation of the mapping trajectory caused by false positive measurements, improving the utilization rate of effective measurement values.

[0164] Figure 3 It is a schematic diagram of the process of multi-trajectory and multi-constraint pose optimization provided by an embodiment of the present disclosure. This process can fully and automatically filter false positive measurement values, can meet the mapping requirements of multi-trajectory joint optimization, can meet the mapping requirements of expanding new routes on the old map, can reduce the dependence on manual experience, and can decouple the most time-consuming mapping link so that it only executes once, greatly improving the mapping deployment efficiency.

[0165] Specifically, this process can split the existing single-trajectory multi-constraint online mapping algorithm and divide it into a laser odometry part and a multi_pgo (Multi-trajectory Multi-constraint PoseGraph Optimization) part in terms of the process flow.

[0166] As Figure 3 shown, in the laser odometry part, mapping data (laser point cloud data and wheel speedometer measurement data) can be obtained. The mapping data can be segmented data collected from a long route, data collected from multiple routes, or data for expanding a new route from an old map, and the corresponding RTK measurement values and maploc measurement values can be obtained. Then, the laser point cloud data and wheel speedometer measurement data are input to construct a laser odometry trajectory, and trajectories 1 to n are obtained. Moreover, multiple trajectories can be transformed into a unified global coordinate system.

[0167] Furthermore, in the multi_pgo part, it can be divided into single-trajectory first-stage optimization, multi-trajectory loop detection, and multi-trajectory second-stage optimization.

[0168] For the single-trajectory first-stage optimization, a thread can be established for each trajectory, that is, trajectory 1 corresponds to thread 1, trajectory 2 corresponds to thread 2, and trajectory n corresponds to thread n, so that each trajectory can perform the single-trajectory first-stage optimization separately. Among them, the single-trajectory first-stage optimization includes: 1. The first round of optimization, using LO constraints (odom constraints), RTK constraints, maploc constraints, and applying a robust kernel function; 2. Removing outliers, calculating the chi-square of the pose error before and after optimization, and removing false positive RTK and maploc measurement values; 3. The second round of optimization, using LO constraints, RTK constraints, and maploc constraints.

[0169] For the multi-trajectory loop detection, it includes: 1. After the first round of optimization, multiple trajectories are all in the UTM coordinate system and the cumulative error has been eliminated; 2. Each trajectory traverses the key frames, and then traverses the key frames of other trajectories, avoiding repeated traversal, and performs loop detection; 3. For each pair of loop key frames, point cloud registration is performed to obtain loop measurement values, and this process can be multi-threaded.

[0170] For the multi-trajectory second-stage optimization, it includes: 1. The third round of optimization, using LO constraints, RTK constraints, maploc constraints, loop constraints, and applying a robust kernel function; 2. Removing outliers, calculating the chi-square of the pose error before and after optimization, and removing false positive RTK, loop, and maploc measurement values; 3. The fourth round of optimization, using LO constraints, RTK constraints, loop constraints, and maploc constraints. Finally, an accurate laser mapping result is obtained.

[0171] In the above multi-trajectory and multi-constraint pose optimization process, the time-consuming lidar odometry and loop detection steps that only need to be executed once are decoupled from the multi-constraint pose graph optimization step that can be executed multiple times with very little debugging time. If the user is not satisfied with the final mapping result, they can return and re-optimize in the second stage without having to execute the lidar odometry and loop detection steps again, greatly improving the mapping efficiency.

[0172] Moreover, considering the need for multi-trajectory mapping in actual project mapping deployment, due to the complex diversity of the actual operation routes of the project site and the presence of multiple vehicles for collecting mapping data, there are usually multiple mapping trajectories in the same site during mapping deployment, and there may be overlapping areas between multiple mapping trajectories. Existing mapping algorithms are single-trajectory mapping algorithms and cannot handle the problem of joint mapping of multiple trajectories. The above process can solve the problem of map ghosting in the overlapping areas of multiple trajectories.

[0173] In addition, considering the need to expand new routes on an old map in actual project mapping deployment, due to the development of project operations, there is often a need to add new operation routes in a project site that is already in normal operation. At this time, the map that has been stably operating should not be changed, and the overlapping area between the new operation route and the old map should be consistent. Although the LO guidance can solve this problem, it cannot be used simultaneously in multi-trajectory mapping. Therefore, the above multi-trajectory and multi-constraint pose optimization method can solve the overlapping problem caused by expanding new routes on an old map by introducing the maploc constraint.

[0174] In addition, considering the need to reduce the dependence on manual experience in actual project mapping deployment, there are mainly two types of dependence on manual experience in existing technologies. One is the dependence on the manual coordination of the positive distribution of multiple measurement sources to ensure the smoothness and consistency of the mapping trajectory; the other is the dependence on the manual design of the mapping data collection route to pre-emptively avoid potential mapping problems in specific scenarios. The above multi-trajectory and multi-constraint pose optimization method can solve the problems in existing technologies, such as relying on manual experience to delete jumpy and false positive RTK measurement values and repeatedly experimenting to manually coordinate multiple inconsistent measurement sources, and relying on manual experience to design the collection route to avoid potential mapping challenges.

[0175] In addition, considering that in the actual mapping deployment of projects, it is necessary to improve the mapping deployment efficiency. The existing technologies highly rely on manual experience for debugging, and the manual experience debugging process depends on the practical understanding of specific project scenarios. For example, only when there is a problem of trajectory deviation caused by false positives in RTK in the mapping algorithm, will the operator notice that the RTK measurement value at a certain position is a false positive, and then delete the false positive RTK measurement value, and then use the new RTK measurement value to reconstruct the map. Repeat the above process until all suspected false positive RTK measurement values are deleted. This process involves a large amount of repeated mapping and rework operations. The above multi-trajectory and multi-constraint pose optimization method can solve the problems in the existing technologies, such as low mapping deployment efficiency, long time-consuming repeated mapping, and high rework cost.

[0176] Figure 4 The following is a schematic structural diagram of a laser mapping device in an embodiment of the present disclosure. As Figure 4 shown: The device includes: an odometer trajectory determination module 410, a first optimization module 420, a first outlier rejection module 430, a second optimization module 440, and a mapping module 450, where:

[0177] The odometer trajectory determination module 410 is configured to generate at least one laser odometer trajectory according to the collected laser point cloud data and wheel speedometer measurement data;

[0178] The first optimization module 420 is configured to, for each of the laser odometer trajectories, determine key frames in the laser odometer trajectory, and optimize the original poses of the key frames with the goal of minimizing the calculation result of a first optimization function, and obtain first optimized poses according to the first measurement information of each key frame; wherein, the first measurement information includes odometer measurement values and differential positioning measurement values, and the first optimization function includes a second optimization function and a robust loss function, and the second optimization function includes a residual term between the original pose and the odometer measurement value, and a residual term between the original pose and the differential positioning measurement value;

[0179] The first outlier rejection module 430 is configured to reject outliers in each first measurement information according to the first optimized poses of each key frame;

[0180] The second optimization module 440 is configured to, with the goal of minimizing the calculation result of the second optimization function, optimize the original poses of each key frame according to the first measurement information after rejecting outliers, and obtain second optimized poses;

[0181] The mapping module 450 is configured to construct a point cloud map according to the second optimized poses of the key frames in each laser odometer trajectory and the laser point cloud data.

[0182] Optionally, the map building module 450 includes a loop detection module, a third optimization module, a second outlier rejection module, a fourth optimization module, and a final map composition module, where:

[0183] The loop detection module is used to detect loop frame pairs for each of the laser odometry trajectories and determine the relative pose measurement value between two key frames in the loop frame pair;

[0184] The third optimization module is used to optimize the second optimized pose of each key frame to obtain a third optimized pose for each laser odometry trajectory with the goal of minimizing the calculation result of a third optimization function. Wherein, the second measurement information includes an odometry measurement value, a differential positioning measurement value, and a relative pose measurement value, the third optimization function includes a fourth optimization function and a robust loss function, and the fourth optimization function includes a residual term between the second optimized pose and the odometry measurement value, a residual term between the second optimized pose and the differential positioning measurement value, and a residual term between the second optimized pose and the relative pose measurement value;

[0185] The second outlier rejection module is used to reject outliers in each second measurement information according to the third optimized pose of each key frame;

[0186] The fourth optimization module is used to optimize the second optimized pose of each key frame to obtain a fourth optimized pose with the goal of minimizing the calculation result of the fourth optimization function according to the second measurement information after rejecting outliers;

[0187] The final map composition module is used to construct a point cloud map according to the fourth optimized pose of the key frames in each laser odometry trajectory and the laser point cloud data.

[0188] Optionally, the loop detection module is specifically used to: determine loop frame pairs within a single trajectory and loop frame pairs between multiple trajectories according to the second optimized pose of the key frames in each laser odometry trajectory;

[0189] Among them, for any two key frames in the same laser odometry trajectory, two key frames that meet the following conditions are determined as a loop frame pair: the cumulative distance difference between the two key frames is greater than a preset distance threshold, and the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold; for any two key frames in different laser odometry trajectories, two key frames that meet the following conditions are determined as a loop frame pair: the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold.

[0190] Optionally, the above device further includes an anchor point positioning measurement first module, configured to determine the positioning anchor points of the key frames in the historical map in each of the laser odometry trajectories, and obtain the anchor point positioning measurement values of the key frames; the first measurement information further includes the anchor point positioning measurement values, and the second optimization function further includes a residual term between the original pose and the anchor point positioning measurement values.

[0191] Optionally, the first outlier rejection module is specifically configured to:

[0192] For each key frame, determine the chi-square of the residual between the first optimized pose of the key frame and the differential positioning measurement value, and the chi-square of the residual between the first optimized pose and the anchor point positioning measurement value; determine the differential positioning measurement value and the anchor point positioning measurement value with the chi-square of the residual greater than the preset chi-square threshold as outliers, and reject the outliers.

[0193] Optionally, the mapping module further includes an anchor point positioning measurement second module, configured to determine the positioning anchor points of the key frames in the historical map in each of the laser odometry trajectories, and obtain the anchor point positioning measurement values of the key frames; the second measurement information further includes the anchor point positioning measurement values, and the fourth optimization function further includes a residual term between the original pose and the anchor point positioning measurement values.

[0194] Optionally, the second outlier rejection module is specifically configured to:

[0195] For each key frame, determine the chi-square of the residual between the third optimized pose of the key frame and the differential positioning measurement value, the chi-square of the residual between the third optimized pose and the anchor point positioning measurement value, and the chi-square of the residual between the third optimized pose and the relative pose measurement value; determine the differential positioning measurement value, the anchor point positioning measurement value, and the relative pose measurement value with the chi-square of the residual greater than the preset chi-square threshold as outliers, and reject the outliers.

[0196] Optionally, the odometry trajectory determination module 410 is further configured to:

[0197] For each of the laser odometry trajectories, put the positive differential positioning measurement values of each frame in the laser odometry trajectory into a first set, and put the positioning anchor points with the confidence level of each frame greater than the set threshold into the first set; put the original poses corresponding to each differential positioning measurement value or each positioning anchor point in the first set into a second set; determine the transformation matrix between the local coordinate system and the global coordinate system corresponding to the laser odometry trajectory according to the first set and the second set, and convert the original poses of each frame in the laser odometry trajectory to the global coordinate system according to the transformation matrix.

[0198] The laser mapping device provided by the embodiments of the present disclosure can execute the steps in the laser mapping method provided by the method embodiments of the present disclosure. The implementation steps and beneficial effects are not elaborated here.

[0199] Figure 5 It is a schematic structural diagram of an electronic device in the embodiments of the present disclosure. Specifically, refer to Figure 5 which shows a schematic structural diagram of the electronic device 500 suitable for implementing the embodiments of the present disclosure. Figure 5 The electronic device shown is only an example and should not impose any limitations on the functions and usage scope of the embodiments of the present disclosure.

[0200] As Figure 5 shown, the electronic device 500 may include a processing device 501, a ROM 502, a RAM 503, a bus 504, an input / output (I / O) interface 505, an input device 506, an output device 507, a storage device 508, and a communication device 509. The processing device (such as a central processing unit, a graphics processing unit, etc.) 501 can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM) 502 or the program loaded from the storage device 508 into the random access memory (RAM) 503 to implement the method of the embodiments as described in the present disclosure. In the RAM 503, various programs and data required for the operation of the electronic device 500 are also stored. The processing device 501, the ROM 502, and the RAM 503 are connected to each other through the bus 504. The input / output (I / O) interface 505 is also connected to the bus 504.

[0201] Specifically, according to the embodiments of the present disclosure, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments of the present disclosure include a computer program product that includes a computer program carried on a non-transitory computer-readable medium. The computer program contains program codes for executing the method shown in the flowchart, thereby implementing the laser mapping method as described above. In such an embodiment, the computer program can be downloaded and installed from the network through the communication device 509, or installed from the storage device 508, or installed from the ROM 502. When the computer program is executed by the processing device 501, the above-mentioned functions defined in the method of the embodiments of the present disclosure are executed.

[0202] It should be noted that the above-mentioned computer-readable medium in the present disclosure can be a computer-readable signal medium, a computer-readable storage medium, or any combination of the two. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the above. More specific examples of a computer-readable storage medium can include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, a computer-readable storage medium can be any tangible medium that contains or stores a program, which can be used by or in combination with an instruction execution system, apparatus, or device. In the present disclosure, a computer-readable signal medium can include a data signal propagated in a baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium can also be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in combination with an instruction execution system, apparatus, or device. The program code contained on a computer-readable medium can be transmitted by any suitable medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0203] The above-mentioned computer-readable medium can be included in the above-mentioned electronic device; it can also exist separately and not be assembled into the electronic device. The above-mentioned computer-readable medium carries one or more programs, and when the above-mentioned one or more programs are executed by the electronic device, the electronic device executes the laser mapping method provided in any embodiment of the present disclosure.

[0204] Optionally, when the above-mentioned one or more programs are executed by the electronic device, the electronic device can also execute the other steps described in the above embodiments.

[0205] In the context of the present disclosure, a machine-readable medium can be a tangible medium that can contain or store a program for use by or in connection with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of a machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0206] The foregoing description is only a preferred embodiment of the present disclosure and an illustration of the applied technical principles. Those skilled in the art should understand that the scope of the disclosure involved in the present disclosure is not limited to the technical solution formed by the specific combination of the above technical features, and should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the above disclosure concept. For example, a technical solution formed by mutually replacing the above features with technical features (but not limited to) having similar functions disclosed in the present disclosure.

Claims

1. A laser mapping method, characterized in that, The method includes: Generating at least one laser odometry trajectory according to the collected laser point cloud data and wheel speedometer measurement data; For each of the laser odometry trajectories, determining key frames in the laser odometry trajectory, and optimizing the original poses of the key frames according to the first measurement information of each key frame with the goal of minimizing the calculation result of the first optimization function to obtain the first optimized pose; wherein, the first measurement information includes odometry measurement values and differential positioning measurement values, the first optimization function includes a second optimization function and a robust loss function, and the second optimization function includes a residual term between the original pose and the odometry measurement value, and a residual term between the original pose and the differential positioning measurement value; Eliminating outliers in each of the first measurement information according to the first optimized pose of each key frame; Optimizing the original poses of the key frames according to the first measurement information after eliminating outliers with the goal of minimizing the calculation result of the second optimization function to obtain the second optimized pose; Constructing a point cloud map according to the second optimized poses of the key frames in each of the laser odometry trajectories and the laser point cloud data.

2. The method according to claim 1, wherein The constructing a point cloud map according to the second optimized poses of the key frames in each of the laser odometry trajectories and the laser point cloud data includes: Detecting loop frame pairs for each of the laser odometry trajectories and determining the relative pose measurement value between two key frames in the loop frame pair; For each of the laser odometry trajectories, optimizing the second optimized poses of the key frames according to the second measurement information of the key frames in the laser odometry trajectory with the goal of minimizing the calculation result of the third optimization function to obtain the third optimized pose; wherein, the second measurement information includes odometry measurement values, differential positioning measurement values and relative pose measurement values, the third optimization function includes a fourth optimization function and a robust loss function, and the fourth optimization function includes a residual term between the second optimized pose and the odometry measurement value, a residual term between the second optimized pose and the differential positioning measurement value, and a residual term between the second optimized pose and the relative pose measurement value; Eliminating outliers in each of the second measurement information according to the third optimized pose of each key frame; Optimizing the second optimized poses of the key frames according to the second measurement information after eliminating outliers with the goal of minimizing the calculation result of the fourth optimization function to obtain the fourth optimized pose; Constructing a point cloud map according to the fourth optimized poses of the key frames in each of the laser odometry trajectories and the laser point cloud data.

3. The method according to claim 2, wherein The detecting loop frame pairs for each of the laser odometry trajectories includes: Determining loop frame pairs within a single trajectory and loop frame pairs between multiple trajectories according to the second optimized poses of the key frames in each of the laser odometry trajectories; Wherein, for any two key frames in the same laser odometry trajectory, two key frames that meet the following conditions are determined as loop frame pairs: the cumulative path difference between the two key frames is greater than a preset path threshold, and the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold. For any two key frames in different laser odometer trajectories, two key frames that meet the following conditions are determined as a loop closure frame pair: the pose difference between the second optimized poses of the two key frames is less than a preset horizontal distance threshold.

4. The method according to claim 1, wherein The method further includes: determining the positioning anchor points of the key frames in the historical map in each of the laser odometer trajectories to obtain the anchor point positioning measurement values of the key frames; The first measurement information further includes the anchor point positioning measurement values, and the second optimization function further includes a residual term between the original pose and the anchor point positioning measurement values.

5. The method according to claim 4, wherein The removing the outliers in each of the first measurement information according to the first optimized pose of each key frame includes: For each key frame, determining the chi-square of the residual between the first optimized pose of the key frame and the differential positioning measurement value, and the chi-square of the residual between the first optimized pose of the key frame and the anchor point positioning measurement value; Determining the differential positioning measurement value and the anchor point positioning measurement value with the chi-square of the residual greater than the preset chi-square threshold as outliers, and removing the outliers.

6. The method according to claim 2, wherein The method further includes: determining the positioning anchor points of the key frames in the historical map in each of the laser odometer trajectories to obtain the anchor point positioning measurement values of the key frames; The second measurement information further includes the anchor point positioning measurement values, and the fourth optimization function further includes a residual term between the original pose and the anchor point positioning measurement values.

7. The method according to claim 6, characterized in that, The removing the outliers in each of the second measurement information according to the third optimized pose of each key frame includes: For each key frame, determining the chi-square of the residual between the third optimized pose of the key frame and the differential positioning measurement value, the chi-square of the residual between the third optimized pose of the key frame and the anchor point positioning measurement value, and the chi-square of the residual between the third optimized pose of the key frame and the relative pose measurement value; Determining the differential positioning measurement value, the anchor point positioning measurement value, and the relative pose measurement value with the chi-square of the residual greater than the preset chi-square threshold as outliers, and removing the outliers.

8. The method according to claim 1, wherein After generating at least one laser odometer trajectory, it further includes: For each of the laser odometer trajectories, putting the positive differential positioning measurement values of each frame in the laser odometer trajectory into a first set, and putting the positioning anchor points with the confidence level of each frame greater than the set threshold into the first set; Putting the original poses corresponding to each of the differential positioning measurement values or each of the positioning anchor points in the first set into a second set; According to the first set and the second set, determining the transformation matrix between the local coordinate system and the global coordinate system corresponding to the laser odometer trajectory, and converting the original poses of each frame in the laser odometer trajectory to the global coordinate system according to the transformation matrix.

9. A laser mapping device, characterized in that, The device includes: An odometer trajectory determination module, configured to generate at least one laser odometer trajectory according to the collected laser point cloud data and wheel speedometer measurement data; The first optimization module is configured to determine key frames in each of the laser odometry trajectories, and optimize the original poses of the key frames according to the first measurement information of each key frame with the goal of minimizing the calculation result of the first optimization function, so as to obtain the first optimized poses. Wherein, the first measurement information includes an odometry measurement value and a differential positioning measurement value, the first optimization function includes a second optimization function and a robust loss function, and the second optimization function includes a residual term between the original pose and the odometry measurement value, and a residual term between the original pose and the differential positioning measurement value. The first outlier rejection module is configured to reject outliers in each of the first measurement information according to the first optimized poses of each key frame. The second optimization module is configured to optimize the original poses of each key frame according to the first measurement information after rejecting outliers with the goal of minimizing the calculation result of the second optimization function, so as to obtain the second optimized poses. The mapping module is configured to construct a point cloud map according to the second optimized poses of the key frames in each of the laser odometry trajectories and the laser point cloud data.

10. An electronic device, characterized in that, The electronic device includes: One or more processors; A storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1-8.

Images