A slam precision enhancement method

Abstract

The application discloses a SLAM precision enhancement method, and belongs to the field of point cloud data positioning, and comprises the following steps: segmenting auxiliary road point clouds according to mileages, performing ICP registration on the auxiliary road point clouds and main road point clouds in sections, linearly distributing registration conversion to frame point clouds, and finally performing joint optimization on the initial trajectory of SLAM, so that the precision enhancement is realized. According to the technical scheme provided by the application, high-precision spatial data can be obtained without considering the closed-loop scanning conditions of obvious environmental characteristics and flexible scenes.

Description

Technical Field

[0001] This invention belongs to the field of point cloud data localization technology, and particularly relates to a method for enhancing SLAM accuracy. Background Technology

[0002] Traditional vehicle-borne laser scanning systems utilize GNSS (Global Navigation Satellite System) + IMU (Inertial Measurement Unit) to determine high-precision position and attitude, thereby acquiring high-precision laser point clouds of the surrounding environment. Compared to vehicle-borne systems, SLAM (Simultaneous Localization and Mapping)-based laser scanning systems offer advantages such as lighter weight, smaller size, and lower hardware integration costs. These systems, often simply referred to as SLAM systems, can be easily and flexibly applied to point cloud acquisition in small urban scenes, indoor environments, and other concealed areas. They are not heavily reliant on GNSS, leading to rapid adoption. However, the application of SLAM technology requires certain prerequisites, including distinct environmental features and flexible closed-loop scanning conditions within the scene. If these conditions are insufficient, the accuracy of the acquired spatial data will be compromised, potentially failing to meet mapping-grade application requirements. In urban auxiliary road scanning, the frequent closed-loop scanning conditions required by SLAM technology are often unsatisfactory because auxiliary roads are frequently obstructed by trees and cannot provide continuous and reliable GNSS signal support. Furthermore, the workload of auxiliary road scanning is often substantial, with a single scan covering several kilometers. Under these circumstances, the accuracy of the laser point cloud generated by existing SLAM technology is insufficient, resulting in trajectory drift.

[0003] Traditional methods for enhancing SLAM accuracy mainly include: ① Establishing control points in the field and using them to correct the SLAM trajectory; ② Based on existing high-precision point clouds, such as vehicle-mounted point clouds, airborne point clouds, or vertex data from tilted models, manually selecting corresponding points to rigidly correct the SLAM point cloud onto the existing data; ③ Using a segmented scanning method in the field to reduce scanning time each time (e.g., 20 minutes each time), aiming for reliable accuracy in a short period and ensuring a large overlap between adjacent segmented scans, followed by automated stitching of multiple scans; ④ Increasing the hardware configuration of SLAM, such as configuring a higher-level inertial system (IMU) and adding more lasers, in order to improve the reliability of SLAM.

[0004] However, traditional accuracy enhancement methods have the following shortcomings: Method ① The workload of setting up field control points is very large and time-consuming. In actual operation, it has been found that the time spent on field control is basically the same as the scanning time, which is equivalent to doubling the workload; Method ② Using known high-precision point clouds for rigid registration cannot improve the accuracy of SLAM because the accuracy loss during the scanning process of the SLAM system is linear, that is, the internal accuracy is inconsistent, and rigid transformation methods cannot be used; Method ③ Segmented scanning increases the workload, and large overlap also increases the workload, making subsequent data management complex. In actual operation, it has been found that the workload of segmented scanning is more than twice that of a single scan, and it cannot fundamentally solve the problem of SLAM accuracy loss. Method ④ The solution of adding high-performance hardware first increases the system integration cost, and also increases the size and weight of the system, and it can only solve the accuracy problem to a certain extent, but cannot completely eliminate the problem of SLAM accuracy loss. Summary of the Invention

[0005] This invention proposes a method for enhancing SLAM accuracy to address the technical problems existing in the prior art.

[0006] To achieve the above objectives, the present invention provides a method for enhancing SLAM accuracy, comprising:

[0007] Obtain the point cloud of the auxiliary road and the point cloud of the main road, and segment the point cloud of the auxiliary road to obtain the segmented point cloud;

[0008] The segmented point cloud is registered with the main road point cloud in three dimensions to obtain the transformation matrix.

[0009] The transformation matrix is ​​then jointly optimized with the initial SLAM trajectory.

[0010] Preferably, the process of segmenting the auxiliary road point cloud includes:

[0011] Based on SLAM mileage, the auxiliary road point cloud is segmented to obtain segmented point clouds; wherein, the segmented point clouds include: front segmented point clouds and rear segmented point clouds.

[0012] Preferably, before obtaining the transformation matrix, the following steps are taken: Solving for R,t to minimize E(R,t):

[0013]

[0014] Among them, X i P represents the coordinates of the auxiliary road point cloud. i N represents the coordinates of the main road point cloud. p R represents the number of point clouds, R is the 3D rotation between two point clouds, and t is the 3D translation between two point clouds.

[0015] Preferably, the process of performing 3D point cloud registration between the segmented point cloud and the main road point cloud includes:

[0016] Based on the satellite positioning signal of the auxiliary road point cloud, the transformation matrix of the previous segment point cloud is used as the initial value of the subsequent segment point cloud, and flexible ICP registration is performed with the main road point cloud until the ICP registration of all segment point clouds is completed.

[0017] Preferably, the process of performing flexible ICP registration with the main road point cloud includes:

[0018] Obtain the adjacent transformation matrix of the segmented point cloud and the point cloud position of the current frame in the adjacent segmented point cloud. Based on the point cloud position, interpolate the adjacent transformation matrix to obtain the transformation matrix of the current frame, where the current frame is the point cloud of each frame between the adjacent segmented point clouds.

[0019] Preferably, the current frame transformation matrix includes: position offset and rotation offset.

[0020] Preferably, the process of obtaining the position offset includes:

[0021] Two segmented point clouds corresponding to the position are obtained. Based on the two segmented point clouds, the transformation coefficients of the current frame are obtained. Based on the transformation coefficients, the transformation matrix of the two segmented point clouds is linearly interpolated to obtain the linear transformation translation amount, which is the position offset.

[0022] Preferably, the process of obtaining the rotation offset includes:

[0023] The adjacent transformation matrices are converted into adjacent quaternions, and spherical interpolation is performed on the adjacent quaternions to obtain the rotation amount of the current frame, which is the rotation offset.

[0024] Preferably, the process of jointly optimizing the transformation matrix and the SLAM initial trajectory includes:

[0025] Obtain the coordinate transformation coefficients and identity matrix from the initial SLAM trajectory to the transformation matrix. Based on the coordinate transformation coefficients and identity matrix, obtain the current frame pose in the transformation matrix coordinate system. Perform flexible ICP registration on the current frame to obtain the registration pose. Based on the registration pose, perform joint optimization with the initial SLAM trajectory.

[0026] Compared with the prior art, the present invention has the following advantages and technical effects:

[0027] This invention provides a method for enhancing SLAM accuracy. It involves segmenting the auxiliary road point cloud by mileage, performing ICP registration on each segment with the main road point cloud, linearly distributing the registration transformation across each frame of the point cloud, and finally jointly optimizing it with the initial SLAM trajectory to achieve enhanced accuracy. The technical solution provided by this invention can acquire high-precision spatial data even without considering closed-loop scanning conditions with obvious environmental features and flexible scenarios. Attached Figure Description

[0028] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0029] Figure 1 This is a flowchart of a method according to an embodiment of the present invention;

[0030] Figure 2 This is a schematic diagram of segmented "flexible registration" according to an embodiment of the present invention;

[0031] Figure 3 This is a schematic diagram of spherical linear interpolation according to an embodiment of the present invention. Detailed Implementation

[0032] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0033] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0034] Example 1

[0035] The basic principle of this embodiment is to use existing high-precision third-party point clouds as anchor points (called point cloud anchors) to enhance the accuracy of the SLAM system's trajectory. Point cloud anchors include fixed-site point clouds, vehicle-mounted point clouds, airborne point clouds, and vertex data from tilted models. These point clouds generally have high absolute accuracy and good internal consistency. Before implementing this method, the point cloud anchors need to be denoised and homogenized. Generally, downsampling is performed at a point spacing of about 5cm, based on spatial distance. There are many general algorithms for point cloud denoising and homogenization, and the open-source software CloudCompare can also be used for processing.

[0036] This embodiment uses the high-precision point cloud generated by the vehicle-mounted laser scanning system on the main road as the point cloud anchor point, and provides how to use the technology of this embodiment to enhance accuracy in other situations.

[0037] like Figure 1 As shown, this embodiment provides a SLAM accuracy enhancement method, which can be described as follows: the auxiliary road point cloud is segmented according to mileage, and ICP (Iterative Closest Point) registration is performed between each segment and the main road point cloud. The registration transformation is then linearly distributed to the point cloud in each frame, and finally optimized in conjunction with the SLAM initial trajectory to achieve accuracy enhancement.

[0038] Set up points With Dianyunji The transformation relationship T(R,t) between point sets can be obtained by solving R,t to minimize E(R,t), where R is the three-dimensional rotation between two point clouds and t is the three-dimensional translation between two point clouds.

[0039]

[0040] Based on the above method, the auxiliary road point cloud can be segmented, and the transformation relationship between each segment and the main road point cloud can be obtained. Specifically, the auxiliary road point cloud is segmented according to SLAM mileage (e.g., 20 meters). Generally, the initial position of the auxiliary road point cloud has good GNSS signal, so the initial segment overlaps well with the main road point cloud. After ICP registration, a small transformation matrix T(R,t) can be obtained. As the GNSS signal deteriorates, the SLAM drift increases, and the corresponding T(R,t) will also be larger. Excessive offset will cause ICP registration failure. Therefore, the T(R,t) of the previous segment can be used as the initial value for the subsequent segment before ICP registration. However, if the overlap between the main and auxiliary road point clouds is too small, resulting in a low ICP registration score, the T(R,t) of the current segment is discarded, and the T(R,t) of the previous segment is directly used. This process is continued until all segmented ICP registrations are completed, at which point a series of T(R,t) values ​​are obtained. i (R,t).

[0041] If we directly divide the above into segments T i When (R,t) is applied to the entire segment, it inevitably leads to misalignment of the breaks, such as... Figure 2 The diagram illustrates segmented "flexible registration." The method used in this embodiment is "flexible fusion," also known as "semi-rigid fusion." Let the transformation matrices obtained by adjacent segments through ICP be T1 and T2, respectively. Then, interpolation is performed based on the current frame's position within T1 to T2 to obtain the current frame's transformation T. i This transformation will not result in discontinuities or misalignments, but it needs to be jointly optimized with the original pose in the SLAM trajectory to ensure both a smooth transformation and compliance with SLAM trajectory calculation.

[0042] Transformation T of the current frame iThis includes six degrees of freedom: position offset t and rotation offset R. Position offset can be directly interpolated using linear interpolation, while rotation offset requires spherical linear interpolation (SLEP). Let the mileage ranges of the two auxiliary road point clouds to be transformed be (…). Figure 2 (with frame number F as the mileage value) are respectively and The transform coefficients of the current frame i are:

[0043]

[0044] The position offset of the current frame is taken as the translation amount of the following linear transformation (i.e., the last column vector of the 4×4 matrix):

[0045]

[0046] The rotation offset of the current frame requires the matrix to be transformed into a quaternion before spherical interpolation. Let T1 and T2 be transformed into quaternions Q1 and Q2, respectively. Figure 3 As shown, this is a schematic diagram of spherical linear interpolation.

[0047] Then the rotation amount Qi of the current frame i is:

[0048]

[0049] in

[0050] θ=cos -1 (Q1,Q2) (5)

[0051] Let T be the coordinate transformation from the SLAM trajectory to the point cloud anchor point (the point cloud acquired by the vehicle-mounted laser scanning system). gw (If the SLAM system incorporates RTK (Real-time kinematic), it can directly provide a world coordinate system consistent with the vehicle's point cloud coordinate system, then T) gw (If it is the identity matrix, otherwise it is also the matrix to be determined). Let the pose of the i-th frame of SLAM be T. wi Then, the pose of the i-th frame converted to the point cloud anchor point coordinate system is: T gi =T gw T wi The pose of the i-th frame after flexible registration is: T′ gi =T gi T i The error term can then be expressed as:

[0052]

[0053] Solving the above equation using optimization theory (many excellent open-source libraries can be used to solve this optimization problem, such as iSAM, GTSAM, G2O, Ceres, etc.) yields the frame-by-frame pose T′ under the constraints of flexible transformation and SLAM itself. gi The SLAM point cloud is then unfolded in the new pose, resulting in the final point cloud with enhanced accuracy. This final point cloud takes into account the transformation relationship with the anchor points of the point cloud and conforms to the constraints of the SLAM algorithm itself, thus achieving internal consistency and improved accuracy, meeting the requirements.

[0054] Under normal circumstances, the above scheme has high robustness. However, under severe occlusion that prevents the acquisition of good GNSS support for a long time, the far-end SLAM trajectory may drift significantly, such as by several meters or tens of meters. A manual assistance method can be added, manually selecting pairs of points with the same name from the SLAM point cloud and the point cloud anchor points, such as identical tree trunks or streetlights. The SLAM trajectory is then coarsely registered using these pairs of points, following the formula (3) above. The process can then proceed to the steps outlined in this embodiment.

[0055] The above description is merely a preferred embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for enhancing SLAM accuracy, characterized in that, Includes the following steps: Obtain the point cloud of the auxiliary road and the point cloud of the main road, and segment the point cloud of the auxiliary road to obtain the segmented point cloud; The segmented point cloud is registered with the main road point cloud in three dimensions to obtain the transformation matrix. The process of performing 3D point cloud registration between the segmented point cloud and the main road point cloud includes: Based on the satellite positioning signal of the auxiliary road point cloud, the transformation matrix of the front segment point cloud is used as the initial value of the rear segment point cloud, and flexible ICP registration is performed with the main road point cloud until the flexible ICP registration of all segment point clouds is completed. The process of performing flexible ICP registration with the main road point cloud includes: Obtain the adjacent transformation matrix of the segmented point cloud and the point cloud position of the current frame in the adjacent segmented point cloud. Based on the point cloud position, interpolate the adjacent transformation matrix to obtain the transformation matrix of the current frame, where the current frame is the point cloud of each frame between the adjacent segmented point clouds. The transformation matrix is ​​jointly optimized with the SLAM initial trajectory; The process of jointly optimizing the transformation matrix and the SLAM initial trajectory includes: Let the pose of the i-th frame of SLAM be... Then, the pose of the i-th frame converted to the point cloud anchor point coordinate system is: The pose of the i-th frame after flexible registration is: The error term can then be expressed as: Solving the above equation using optimization theory yields the frame-by-frame pose under the constraints of flexible transformation and SLAM itself. Then, the SLAM point cloud is unfolded in the new pose, which is the result point cloud after accuracy enhancement. The result point cloud takes into account the transformation relationship with the point cloud anchor point and conforms to the constraints of the SLAM algorithm itself.

2. The SLAM accuracy enhancement method according to claim 1, characterized in that, The process of segmenting the auxiliary road point cloud includes: Based on SLAM mileage, the auxiliary road point cloud is segmented to obtain segmented point clouds; wherein, the segmented point clouds include: front segmented point clouds and rear segmented point clouds.

3. The SLAM accuracy enhancement method according to claim 2, characterized in that, Before obtaining the transformation matrix, the following steps are involved: solving... To minimize E(R,t): in, Represents the coordinates of the auxiliary road point cloud. Represents the point cloud coordinates of the main road. Indicates the number of point clouds. The three-dimensional rotation between two cloud points. This represents the three-dimensional translation between two cloud points.

4. The SLAM accuracy enhancement method according to claim 1, characterized in that, The current frame transformation matrix includes: position offset and rotation offset.

5. The SLAM accuracy enhancement method according to claim 4, characterized in that, The process of obtaining the position offset includes: Two segmented point clouds corresponding to the position are obtained. Based on the two segmented point clouds, the transformation coefficients of the current frame are obtained. Based on the transformation coefficients, the transformation matrix of the two segmented point clouds is linearly interpolated to obtain the linear transformation translation amount, which is the position offset.

6. The SLAM accuracy enhancement method according to claim 4, characterized in that, The process of obtaining the rotation offset includes: The adjacent transformation matrices are converted into adjacent quaternions, and spherical interpolation is performed on the adjacent quaternions to obtain the rotation amount of the current frame, which is the rotation offset.

Images