Multi-sensor fusion SLAM method and system for dynamic traffic scene

Abstract

The invention provides a multi-sensor fusion SLAM method and system for a dynamic traffic scene, belongs to the field of automatic driving perception and positioning, and effectively weakens the interference of a dynamic traffic target on the positioning precision and static map construction by combining the innovative design of dynamic feature point elimination and dynamic point cloud filtering. Besides, through multi-sensor fusion of a laser radar, a camera and an inertial measurement unit, an SLAM framework integrating laser inertial estimation, visual inertial estimation and cross-modal joint factor graph optimization is designed, so that the robustness of the system under the conditions of illumination variation, severe weather and high-speed motion can be remarkably improved; therefore, high-precision and high-reliability positioning and environment perception can be realized in a complex dynamic traffic environment.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving perception and localization, and relates to a multi-sensor fusion SLAM method and system for dynamic traffic scenarios, which is applicable to complex scenarios with a large number of dynamic traffic participants. Background Technology

[0002] With the rapid development of autonomous driving technology, real-time high-precision environmental perception and localization have become one of the key technologies of autonomous driving systems. Simultaneous Localization and Mapping (SLAM), as a core perception technology, can help vehicles achieve self-localization in unknown environments and simultaneously build environmental maps, providing accurate navigation information. It is an important means to ensure the safe and stable driving of vehicles.

[0003] Traditional SLAM methods rely on a single sensor for environmental perception and localization, such as LiDAR or cameras. However, cameras are prone to degradation under changing lighting conditions and occlusion, and LiDAR is susceptible to significant measurement errors in rainy, snowy, or high-speed vehicle scenarios. Therefore, single sensors often struggle to maintain stable localization and mapping capabilities in complex traffic environments over long periods. Furthermore, complex traffic environments involve numerous dynamic traffic participants, such as pedestrians, vehicles, and non-motorized vehicles, which significantly impact the localization accuracy and mapping performance of the SLAM system. Figure 1 The inconsistency can cause significant interference, and existing SLAM schemes lack effective dynamic point removal and processing mechanisms, resulting in inaccurate map updates and even positioning drift.

[0004] In summary, for complex and ever-changing dynamic traffic scenarios, accurately distinguishing between static and dynamic objects, eliminating the impact of dynamic objects on positioning accuracy and mapping results, and designing a robust and high-precision SLAM method by fusing multi-source data have become key to improving the environmental perception and localization capabilities of autonomous vehicles. Summary of the Invention

[0005] To eliminate the interference of dynamic objects on positioning accuracy and mapping effect, and to overcome the limitations of single sensors in complex environments, this invention proposes an improved dynamic feature point removal method and a dynamic point cloud filtering method. By integrating the complementary advantages of multiple source sensors such as lidar, camera, and inertial measurement unit (IMU), a multi-sensor fusion SLAM method for dynamic traffic scenarios is designed to improve the robustness and anti-interference capability of the SLAM system and obtain more accurate and reliable environmental perception and positioning information.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A multi-sensor fusion SLAM method for dynamic traffic scenarios includes the following steps: S1, preprocess the collected multi-source sensor data, and perform laser inertial motion estimation and visual inertial motion estimation based on the preprocessed multi-source sensor data; S2, Based on the laser inertial motion estimation and visual inertial motion estimation results, a factor graph model is constructed and optimized to obtain the optimal pose estimation of the system; S3, based on the optimized best pose estimation results, constructs a static map by filtering out dynamic point clouds.

[0007] Preferably, the multi-source sensor data includes triaxial acceleration and angular velocity data acquired by the IMU, continuous image sequences acquired by the camera, and environmental point clouds acquired by the lidar.

[0008] Preferably, the preprocessing of the collected multi-source sensor data specifically includes: The IMU mathematical model is used to pre-integrate the IMU data acquired by the IMU to obtain the changes in rotation, velocity, and displacement (ΔR, Δv, Δp) and their covariance matrix between keyframes, which describes the relative motion constraints between keyframes. Radial and tangential distortion of images are corrected using camera and distortion models; High-frequency IMU data is used to perform time-series correction on laser point clouds to compensate for motion distortion.

[0009] Preferably, laser inertial motion estimation specifically includes the following steps: S31, Dynamic point cloud recognition is performed on time-corrected laser point cloud data based on semantic segmentation algorithm; S32 extracts feature points from all point cloud data with dynamic labels based on multi-scale local smoothness; S33: In the feature point set, dynamic feature points are removed based on the aforementioned dynamic point identifiers; a buffer zone is further set for the neighboring region of the dynamic points to remove points that may be affected by dynamic targets, and only static corner points and planar points are retained for subsequent laser inertial estimation; S34: Laser inertial motion estimation based on feature point matching.

[0010] Preferably, dynamic point cloud recognition is performed on the time-corrected laser point cloud data based on a semantic segmentation algorithm, specifically including the following steps: S311: Convert the time-corrected laser point cloud data into a distance image representation using spherical projection; S312: Use a 2D convolutional neural network to perform semantic segmentation on distance images; S313: Back-project the image semantic segmentation results to the corresponding 3D point cloud, assign semantic labels to each point cloud point, identify the attribute fields of dynamic points, and optimize the semantic segmentation results using the GPU-accelerated k-nearest neighbor search method; S314: Write dynamic identifiers into the attribute fields of the identified dynamic points to mark and distinguish the dynamic points, and maintain consistency with the point cloud index.

[0011] Preferably, feature points are extracted from all point cloud data with dynamic labels based on multi-scale local smoothness, specifically including the following sub-steps: S321: For each point in the input point cloud, calculate the local smoothness in both the small and large neighborhoods; where the small neighborhood smoothness is used to capture local geometric abrupt changes to identify corner points, and the large neighborhood smoothness is used to characterize the overall flatness to extract planar points.

[0012] S322: Based on the smoothness of the small neighborhood, sort the point cloud by smoothness value from largest to smallest, and sequentially traverse and select several points greater than the corner threshold as the corner point set; based on the smoothness of the large neighborhood, sort the point cloud by smoothness value from smallest to largest, sequentially traverse and select points less than the preset plane point threshold as plane candidate points, and perform voxel filtering or uniform downsampling on the plane candidate points to obtain the final plane point set.

[0013] Preferably, laser inertial motion estimation is achieved based on feature point matching; specifically, it includes the following steps: S341: Select a series of key frames that are spatiotemporally adjacent to the current frame, transform the point cloud of the key frames to the global coordinate system, and then overlay them to form a local point cloud map containing corner points and planar points. S342: For the corner points extracted from the point cloud of the current frame, search for the set of neighboring corner points in the local corner point map, and construct the residual from the current frame corner point to the line by fitting a straight line; for the extracted planar points, search for the set of neighboring planar points in the local planar point map, and construct the residual from the current frame planar points to the plane by fitting a plane. S343: Combine the point-to-line residual and the point-to-plane residual to form a geometric residual, and construct a pose optimization objective function; during the optimization process, apply a robust loss function to each residual term to suppress the influence of outliers; use nonlinear least squares optimization algorithms such as GN or LM to iteratively solve the objective function, thereby obtaining the current frame pose estimation result of the laser inertial subsystem.

[0014] Preferably, visual-inertial motion estimation specifically includes the following steps: S41: Extracting visual feature points from the corrected image through optical flow tracing and corner detection; S42: A depth information estimation method based on local surface fitting is used to obtain depth information of visual feature points; S43: Visual inertial motion estimation is achieved based on sliding window optimization.

[0015] Preferably, the depth information estimation method based on local surface fitting to obtain visual feature point depth information specifically includes the following steps: S421: Fuse multiple frame point clouds from nearby times to obtain a dense global point cloud, then transform its coordinates from the world coordinate system to the camera coordinate system, and project the point cloud and visual feature points in the image onto a unit sphere centered on the camera optical center. S422: Search for the nearest neighbor laser points for each feature point on the unit sphere, and fit a local quadratic surface model using the linear least squares method based on the three-dimensional coordinates of these points in the camera coordinate system. S423: By solving for the intersection point of the visual ray and the local surface, the distance from the intersection point to the optical center of the camera is calculated, which is the depth value of the feature point.

[0016] A multi-sensor fusion SLAM system for dynamic traffic scenarios includes a preprocessing module, a pose estimation module, and a fusion module. The preprocessing module preprocesses the collected multi-source sensor data and performs laser inertial motion estimation and visual inertial motion estimation based on the preprocessed multi-source sensor data. The pose estimation module constructs a factor graph model based on the laser inertial motion estimation and visual inertial motion estimation results and performs optimization to obtain the optimal pose estimation of the system, i.e., the positioning result output. The fusion module constructs a static map by filtering out dynamic point clouds, based on the optimized best pose estimation results.

[0017] Compared with the prior art, the present invention has the following beneficial technical effects: This invention proposes a multi-sensor fusion SLAM method for dynamic traffic scenarios. By combining innovative designs of dynamic feature point removal and dynamic point cloud filtering, the interference of dynamic traffic targets on positioning accuracy and static map construction is effectively reduced. In addition, through multi-sensor fusion of LiDAR, camera and inertial measurement unit, a SLAM framework integrating laser inertial estimation, visual inertial estimation and cross-modal joint factor graph optimization is designed, which can significantly improve the robustness of the system under conditions of illumination change, severe weather and high-speed movement, thereby achieving high-precision and high-reliability positioning and environmental perception in complex dynamic traffic environments. Attached Figure Description

[0018] Figure 1 This is a flowchart of a multi-sensor fusion SLAM method for dynamic traffic scenarios in an embodiment of the present invention.

[0019] Figure 2 This is a flowchart of the dynamic feature point removal algorithm in an embodiment of the present invention.

[0020] Figure 3This is the factor graph optimization model in the embodiments of the present invention.

[0021] Figure 4 This is a flowchart of the dynamic point cloud filtering algorithm in an embodiment of the present invention.

[0022] Figure 5 This is a diagram showing the dynamic feature point removal effect of the SLAM method on the KITTI dataset in this embodiment of the invention. Figure 5 (a) shows the laser point cloud feature point extraction result of an intersection. Figure 5 (b) in the figure represents the result of dynamic feature point removal.

[0023] Figure 6 This is a comparison diagram of pose estimation trajectories of the SLAM method in the KITTI_07 sequence in an embodiment of the present invention.

[0024] Figure 7 This is a comparison chart of pose estimation errors of the SLAM method in the KITTI_07 sequence in this embodiment of the invention. Figure 7 (a) in the figure represents a comparison of the absolute trajectory error data of LVI-SAM and the SLAM method of this invention. Figure 7 (b) in the figure represents a comparison of the absolute trajectory errors of LVI-SAM and the SLAM method of the present invention.

[0025] Figure 8 This is a diagram showing the static map construction effect of the SLAM method at the intersection in the KITTI_07 sequence in an embodiment of the present invention. Figure 8 (a) in the image represents the crossroads portion of the original point cloud map. Figure 8 (b) in the image is the static point cloud map obtained after filtering out the dynamic point cloud.

[0026] Figure 9 This is a real-vehicle experimental data acquisition platform in this embodiment of the invention.

[0027] Figure 10 This is a diagram showing the dynamic feature point removal effect of the SLAM method on a real vehicle dataset in an embodiment of the present invention. Figure 10 (a) in the figure represents the result of directly extracting feature points from the original point cloud; Figure 10 (b) in the figure represents the result of removing dynamic feature points.

[0028] Figure 11 This is a comparison diagram of pose estimation trajectories of the SLAM method on a real vehicle dataset in an embodiment of the present invention.

[0029] Figure 12 This is a diagram showing the static map construction effect of the SLAM method at an intersection on a real vehicle dataset in an embodiment of the present invention. Detailed Implementation

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

[0031] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0032] like Figure 1 As shown, a multi-sensor fusion SLAM method for dynamic traffic scenarios includes the following steps: S1. The collected multi-source sensor data is preprocessed, and laser inertial motion estimation (LIO) and visual inertial motion estimation (VIO) are performed based on the preprocessed multi-source sensor data. S2, Based on the laser inertial motion estimation (LIO) and visual inertial motion estimation (VIO) results, a factor graph model is constructed and optimized to obtain the optimal pose estimation of the system, i.e. the localization result output, including the motion state variables of all key frames, including position and velocity. S3, based on the optimized best pose estimation results, constructs a static map by filtering out dynamic point clouds.

[0033] In a specific embodiment of this application, the multi-source sensor data includes triaxial acceleration and angular velocity data acquired by the IMU, continuous image sequences acquired by the camera, and environmental point clouds acquired by the lidar; Preprocessing the collected multi-source sensor data specifically includes: S21. The IMU mathematical model is used to pre-integrate the IMU data (three-axis acceleration and angular velocity data) acquired by the IMU to obtain the changes in rotation, velocity and displacement (ΔR, Δv, Δp) and their covariance matrix between key frames, which describes the relative motion constraints between key frames. S22: Correct radial and tangential distortion of images using camera and distortion models; S23: Use high-frequency IMU data to perform time-series correction on the laser point cloud to compensate for motion distortion.

[0034] Laser inertial motion estimation (LIO) includes laser inertial motion estimation and dynamic feature point removal. LIO is performed after dynamic feature point removal. The algorithm for dynamic feature point removal is as follows: Figure 2 As shown.

[0035] Laser inertial motion estimation specifically includes the following steps: S31, Dynamic point cloud recognition is performed on the time-corrected laser point cloud data based on a semantic segmentation algorithm. This specifically includes the following sub-steps: S311: Convert the time-corrected laser point cloud data into a distance image representation using spherical projection; S312: Use a 2D convolutional neural network to perform semantic segmentation on the distance image; in order to encode rich features, the horizontal direction of the distance image is downsampled while the vertical resolution remains unchanged. S313: Back-project the image semantic segmentation results to the corresponding 3D point cloud, assign semantic labels to each point cloud point, identify the attribute fields of dynamic points, and optimize the semantic segmentation results using the GPU-accelerated k-nearest neighbor search method; S314: Write dynamic identifiers (such as occupying the high bits of the intensity channel or custom semantic fields) into the attribute fields of the identified dynamic points to achieve annotation and differentiation of dynamic points and maintain consistency with the point cloud index.

[0036] S32, feature points are extracted from all point cloud data with dynamic labels based on multi-scale local smoothness. After removing dynamic point clouds from the original point cloud, the point cloud representing the vehicle disappears. During subsequent feature point extraction, point clouds at the boundary between the background and the vehicle are easily misidentified as corner points, causing odometer drift. Therefore, this invention first extracts feature points and then removes dynamic feature points with dynamic labels. Specifically, it includes the following sub-steps: S321: For each point in the input point cloud, calculate the local smoothness in both the small and large neighborhoods; where the small neighborhood smoothness is used to capture local geometric abrupt changes to identify corner points, and the large neighborhood smoothness is used to characterize the overall flatness to extract planar points.

[0037] S322: Based on the smoothness of the small neighborhood, sort the point cloud by smoothness value from largest to smallest, and sequentially traverse and select several points greater than the corner threshold as the corner point set; based on the smoothness of the large neighborhood, sort the point cloud by smoothness value from smallest to largest, sequentially traverse and select points less than the preset plane point threshold as plane candidate points, and perform voxel filtering or uniform downsampling on the plane candidate points to obtain the final plane point set.

[0038] S33: In the feature point set, dynamic feature points are removed based on the aforementioned dynamic point identifiers; a buffer zone is further set for the neighboring region of the dynamic points to remove points that may be affected by dynamic targets, and only static corner points and planar points are retained for subsequent laser inertial estimation.

[0039] S34: Laser inertial motion estimation based on feature point matching; specifically including the following sub-steps: S341: Select a series of key frames that are spatiotemporally adjacent to the current frame, transform the point clouds of the key frames to the global coordinate system, and then overlay them to form a local point cloud map containing corner points and planar points; during the overlay process, introduce a time decay factor for each key frame to gradually weaken the contribution of earlier key frames, thereby suppressing map pollution caused by dynamic targets and improving the timeliness and robustness of the local map.

[0040] S342: For the corner points extracted from the point cloud of the current frame, search for the set of neighboring corner points in the local corner point map, and construct the residual from the corner point to the line of the current frame by fitting a straight line; for the extracted planar points, search for the set of neighboring planar points in the local planar point map, and construct the residual from the planar point to the plane of the current frame by fitting a plane.

[0041] S343: Combine the point-to-line residual and the point-to-plane residual to form a geometric residual, and construct a pose optimization objective function; during the optimization process, apply a robust loss function to each residual term to suppress the influence of outliers; use nonlinear least squares optimization algorithms such as GN or LM to iteratively solve the objective function, thereby obtaining the current frame pose estimation result of the laser inertial subsystem.

[0042] In a specific embodiment of this application, visual inertial motion estimation (VIO) specifically includes the following steps: S41: Visual feature points are extracted from the corrected image through optical flow tracking and corner detection. Specifically, the KLT sparse optical flow method is first used to track feature points from the previous frame. If the number is insufficient, the Shi-Tomasi detection method is used to supplement the feature points.

[0043] S42: A depth information estimation method based on local surface fitting is used to obtain visual feature point depth information. This includes the following sub-steps: S421: Fuse multiple frame point clouds from nearby times to obtain a dense global point cloud, then transform its coordinates from the world coordinate system to the camera coordinate system, and project the point cloud and visual feature points in the image onto a unit sphere centered on the camera's optical center.

[0044] S422: Search for multiple nearest neighbor laser points (e.g., K=6) for each feature point on a unit sphere, and fit a local quadratic surface model using linear least squares based on the three-dimensional coordinates of these points in the camera coordinate system to more accurately describe the true geometry of the object surface.

[0045] S423: By solving for the intersection point of the visual ray and the local surface, the distance from the intersection point to the optical center of the camera is calculated, which is the depth value of the feature point.

[0046] S43: Implement visual inertial motion estimation based on sliding window optimization. This includes the following sub-steps: S431: Initialize the visual-inertial subsystem. When the laser inertial subsystem is available, its output estimation results are used to initialize the visual-inertial subsystem; when the laser inertial subsystem is unavailable, a pure visual structure recovery method is used, combined with IMU pre-integration for visual-inertial alignment, to achieve initialization.

[0047] S432: The sliding window method is used to optimize the motion state of the visual-inertial subsystem. In this optimization process, the state variables within the window are used as optimization variables. An objective function is constructed that includes IMU measurement residuals, visual measurement residuals, and marginalized prior residuals. The maximum posterior estimate is obtained by minimizing the Mahalanobis distance between the various residuals.

[0048] In a specific embodiment of the present invention, the construction and optimization solution of the factor graph model includes the following steps: S51: Generate loop closure detection factors based on visual loop closure detection and precise point cloud registration. This includes the following sub-steps: S511: Utilizing the current keyframe pose and feature information provided by visual inertial motion estimation, the bag-of-words position recognition method DBoW2 is driven to perform similarity retrieval on the keyframe image to obtain candidate loop closure frames. Based on visual geometric consistency, the candidate loop closures are initially verified to eliminate false loop closures. If the verification is successful, the point cloud data of several keyframes before and after the loop closure frame are extracted, and after voxel filtering, they are superimposed to construct a local point cloud map.

[0049] S512: Using the pose estimation result of the visual-inertial subsystem as the initial value, enhance the registration of the current keyframe point cloud with the local point cloud map, and simultaneously estimate the registration covariance during the registration process. If the registration covariance is lower than a set threshold, the visual-inertial pose is corrected according to the registration result, and a loop closure detection factor is generated by combining the loop closure frame pose; if the covariance exceeds the threshold, the corresponding loop closure factor is discarded.

[0050] S52: Construct a factor graph model. The factor graph model includes laser inertial odometry factors, IMU pre-integration factors, and loop closure detection factors based on registration covariance, such as... Figure 3 As shown. The laser inertial odometry factor is constructed from the laser inertial motion estimation results, the IMU pre-integration factor is constructed based on the inertial measurement unit data, and the loop closure detection factor is constructed from the pose constraints output by the loop closure detection module.

[0051] S53: Optimization and solution of the factor graph model. The factor graph model is solved by incremental nonlinear optimization using the GTSAM tool library. iSAM2 is used to update the newly added keyframes and constraint factors in real time, and relinearization is triggered when the estimation deviation is too large. The optimal pose estimation of the system is obtained while ensuring computational efficiency, which is the final localization result output.

[0052] In a specific embodiment of the present invention, a static map is constructed by filtering out dynamic point clouds. The dynamic point cloud filtering algorithm flow is as follows: Figure 4 As shown, the specific implementation steps are as follows: S61: Based on the optimized pose estimation results, the corresponding lidar point cloud frames are superimposed onto a unified coordinate system to obtain the original point cloud map.

[0053] S62: Analyze the original point cloud map using region-based pseudo-occupancy descriptors (R-POD) and filter potential dynamic regions by the significance of changes in occupancy status; regions with significant changes in occupancy status are identified as potential dynamic regions, while regions with stable occupancy status are identified as static regions.

[0054] S63: Segment the potential dynamic region using ground points. The steps include: S631: The R-VPF method, a vertical plane fitting method based on region partitioning, is used to fit the vertical plane below the ground plane and remove the vertical plane points.

[0055] S632: The R-GPF method, a ground plane fitting method based on region division, is used to segment ground points in the potential dynamic region.

[0056] S64: To avoid misidentifying static objects as dynamic targets, a secondary dynamic region identification step is introduced. Specifically, for the initially identified potential dynamic regions, geometric discrimination is first performed based on the point cloud height difference and centroid changes. Then, the motion trend of the region is determined by combining the consistency analysis of continuous frame trajectories, thereby achieving spatiotemporal joint dynamic region identification and improving discrimination accuracy and robustness.

[0057] S65: If the secondary recognition is still a dynamic area, only the ground points are restored to the static map, and the non-ground points are removed as dynamic points; if the secondary recognition is a static area, both the ground points and non-ground points are restored to the map to obtain the final static map.

[0058] Specific experimental example 1: Localization and mapping were performed using sensor (LiDAR, camera, IMU) data from the KITTI dataset. The data included LiDAR point clouds, image sequences, and triaxial acceleration and angular velocity data. The sensor data was preprocessed, specifically by pre-integrating the IMU data to correct radial and tangential distortions in the images, and by performing temporal correction on the LiDAR point clouds to compensate for motion distortions.

[0059] A semantic segmentation algorithm is used to identify dynamic point clouds and assign them dynamic labels. Corner points and planar points are extracted based on multi-scale local smoothness. Dynamic feature points and neighborhood buffer points are removed based on dynamic point labels, and only static feature points are retained. A time decay factor is introduced to construct a local point cloud map. A pose optimization objective function is constructed based on geometric residuals. Solving the objective function realizes laser inertial motion estimation.

[0060] Practical experience has shown that after removing dynamic point clouds from the original point cloud, the point cloud representing the vehicle disappears. When extracting feature points subsequently, the point cloud at the boundary between the background and the vehicle is easily misidentified as corner points, causing odometer drift. Therefore, this invention extracts feature points first and then removes dynamic feature points, rather than removing dynamic point clouds from the original point cloud first and then extracting feature points.

[0061] The effect of dynamic feature point removal is as follows Figure 5 As shown, Figure 5 (a) shows the laser point cloud feature point extraction results for an intersection. Pink points are planar points, green points are corner points, and the area enclosed by the red rectangle represents the feature points of moving vehicles. Figure 5 (b) shows the result of dynamic feature point removal, with the red rectangle and... Figure 5 Corresponding to (a) in the figure, it can be seen that the method proposed in this invention cleanly eliminates dynamic feature points.

[0062] For image data, visual feature points are extracted through optical flow tracking and corner detection; multi-frame point clouds from neighboring times are fused to obtain a dense global point cloud, and the point cloud and visual feature points are uniformly projected onto a unit sphere centered on the camera's optical center. Depth information of visual feature points is obtained based on a depth information estimation method using local surface fitting; the visual inertial subsystem is initialized, and the motion state of the visual inertial subsystem is optimized using the sliding window method.

[0063] Similarity retrieval is performed on keyframe images, and false loop closures are eliminated through geometric consistency verification. Point cloud data from several keyframes before and after the loop closure frame are extracted, voxel filtered, and then superimposed to construct a local point cloud map. Loop closure detection factors are generated based on visual inertial estimation and registration with the local point cloud of the loop closure frame. If the registration covariance is less than a threshold, a reliable loop closure factor is generated. Based on this, a factor graph model is constructed, including laser inertial odometry factors, IMU pre-integration factors, and loop closure detection factors. The iSAM2 incremental optimization framework is used to optimize and solve the factor graph model to obtain the positioning output.

[0064] The factor graph optimization model of this invention achieves globally consistent pose optimization through multi-source constraints (laser, IMU, loop closure), which not only ensures high accuracy of local motion estimation, but also eliminates long-term accumulated errors by relying on loop closure detection, thereby significantly improving the positioning accuracy and robustness of the SLAM system in complex dynamic environments.

[0065] This invention is compared with the mainstream SLAM method LVI-SAM, which integrates LiDAR visual IMU. Figure 6 The figure compares the pose estimation trajectories of the two SLAM methods in the KITTI_07 sequence. The dashed line represents the ground truth trajectory, the blue line represents the pose estimation trajectory output by LVI-SAM, and the green line represents the pose estimation trajectory output by the SLAM method of this invention. As can be seen from the figure, the green line is closer to the dashed line than the blue line, indicating that the pose estimation trajectory of the SLAM method of this invention has a higher degree of overlap with the ground truth trajectory. Therefore, the SLAM method of this invention has higher positioning accuracy.

[0066] Figure 7 This image compares the pose estimation errors of two SLAM methods in the KITTI_07 sequence. lvi.zip represents the absolute trajectory error data of LVI-SAM (corresponding to the blue graph), while dylvi.zip represents the absolute trajectory error (APE) data of the SLAM method of this invention (corresponding to the green graph). Figure 7 As can be seen from (a) in the figure, the absolute trajectory error of the SLAM method of the present invention is lower than that of LVI-SAM throughout the entire process. Figure 7As can be seen in (b), except for the minimum error, the absolute trajectory errors of the SLAM method of the present invention are smaller than those of LVI-SAM, and the suppression effect on the maximum error is more significant.

[0067] Based on the optimized pose estimation results, the corresponding LiDAR point cloud frames are superimposed onto a unified coordinate system to obtain the original point cloud map. The map is then analyzed using region-based pseudo-occupancy descriptors (R-POD), and potential dynamic regions are screened based on the significance of changes in occupancy status. Ground point segmentation is then performed on the potential dynamic regions. A recognition mechanism based on geometric feature discrimination and trajectory consistency analysis is used to perform secondary recognition on the potential dynamic regions. If the secondary recognition identifies the region as dynamic, only ground points are restored, and non-ground points are removed as dynamic points. Otherwise, both ground points and non-ground points are restored simultaneously to obtain the final static map.

[0068] Figure 8 This shows the effect of the SLAM method of the present invention in constructing a static map at an intersection in the KITTI_07 sequence. Figure 8 (a) in the image shows the intersection section in the original point cloud map, where you can see the ghost images of vehicles passing through the intersection. Figure 8 (b) is the static point cloud map obtained after filtering out the dynamic point cloud. It can be seen that the ghost images of vehicles on the road are effectively filtered out, indicating that the SLAM method of the present invention has a good static map construction effect.

[0069] Specific experimental example 2: The dynamic scene segments in the KITTI dataset are relatively short. Therefore, further real-vehicle experiments were conducted to verify the positioning accuracy and static map construction effect of the SLAM method of this invention during long-term driving in dynamic scenes.

[0070] A real-vehicle experimental data acquisition platform was built based on the Lynk & Co 02, such as... Figure 9 As shown, the platform is equipped with a 64-line LiDAR, a binocular camera, and an IMU, and collects various sensor data, including laser point clouds, image sequences, and three-axis acceleration and angular velocity, via an industrial control computer. Real trajectory data was acquired during the experiment using a VBOX 3iS, a GNSS-assisted inertial navigation system that provides high-precision positioning and attitude information.

[0071] The aforementioned real-vehicle experimental data acquisition platform was used to collect vehicle operation data and environmental data within the campus. In order to obtain a dataset in dynamic scenarios, data was collected at the time when students had just finished class, when there were many dynamic objects such as vehicles, pedestrians, and bicycles on campus.

[0072] Sensor data is preprocessed, and dynamic feature points and neighborhood buffer points are removed through dynamic point cloud recognition and feature point extraction. Laser inertial motion estimation is then performed based on this process. The dynamic feature point removal effect of the SLAM method in this invention on a real vehicle dataset is as follows: Figure 10 As shown, Figure 10 (a) shows the result of directly extracting feature points from the original point cloud, where pink points are surface points, green points are corner points, and the red box area in the figure represents a moving vehicle. Figure 10 (b) shows the result of dynamic feature point removal, with the red-boxed area and... Figure 10 Corresponding to the red box area in (a), it can be seen that the dynamic feature points representing the vehicle have been effectively removed.

[0073] Visual inertial motion estimation is performed using image and point cloud data. Based on this, a factor graph model is constructed, including laser inertial odometry factors, IMU pre-integration factors, and loop closure detection factors. This model is then optimized and solved to obtain the positioning output. This invention is compared with the mainstream SLAM method LVI-SAM, which fuses LiDAR visual IMU. Figure 11 The figure shows a comparison of pose estimation trajectories of two SLAM methods on a real-vehicle campus dataset. The blue line represents the ground truth trajectory, the green line represents the trajectory output by LVI-SAM, and the red line represents the trajectory output by the SLAM method of this invention. As can be seen from the figure, in some areas, satellite signal differences during ground truth trajectory acquisition lead to trajectory anomalies. In normal areas, the red line is closer to the blue line than the green line, meaning the output trajectory of the SLAM method of this invention is closer to the ground truth trajectory. Therefore, the SLAM method of this invention has better localization performance in dynamic scenes.

[0074] A static map is constructed based on the optimized pose estimation results. To intuitively understand the effect of the static map construction, a local map is displayed, such as... Figure 12 This is a static map construction result of the SLAM method of this invention at an intersection on a real vehicle dataset. The area is an intersection near a campus commercial street, and there are many dynamic objects in this area, such as... Figure 12 As shown in (a). The original map corresponding to this area is as follows: Figure 12 As shown in (b), there are multiple messy point cloud trajectories on the road in the image. These are "ghost images" left on the map by pedestrians. The SLAM method of this invention is used to filter out the "ghost images" to obtain a static map, as shown in the figure. Figure 12 As shown in (c), the "ghost shadows" left by pedestrians in the original map are effectively filtered out, while static objects such as buildings, roadside parking, and vegetation are retained in the static map, indicating that the SLAM method of this invention has a good static map construction effect on the campus dynamic dataset.

Claims

1. A multi-sensor fusion SLAM method for dynamic traffic scenarios, characterized in that, The method comprises the following steps: S1, preprocessing the collected multi-source sensor data, and performing laser inertial motion estimation and visual inertial motion estimation based on the preprocessed multi-source sensor data; S2, constructing a factor graph model based on the results of laser inertial motion estimation and visual inertial motion estimation and performing optimization solving to obtain optimal pose estimation of the system; S3, constructing a static map by filtering out dynamic point clouds based on the optimal pose estimation result after optimization.

2. The multi-sensor fusion SLAM method for dynamic traffic scene according to claim 1, wherein, The multi-source sensor data includes three-axis acceleration and angular velocity data collected by an IMU, a continuous image sequence obtained by a camera, and environment point clouds collected by a laser radar.

3. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 2, characterized in that, The preprocessing of the collected multi-source sensor data specifically comprises: using an IMU mathematical model to pre-integrate the IMU data collected by the IMU to obtain the changes (ΔR, Δv, Δp) of rotation, velocity, and displacement between key frames and covariance matrices thereof, and to describe the relative motion constraints between the key frames; correcting the radial distortion and tangential distortion of the image through a camera model and a distortion model; using high-frequency IMU data to perform timing correction on the laser point clouds to compensate for motion distortion.

4. The multi-sensor fusion SLAM method for dynamic traffic scenes according to claim 3, characterized in that, The laser inertial motion estimation specifically comprises the following steps: S31, identifying dynamic point clouds from the timing-corrected laser point cloud data based on a semantic segmentation algorithm; S32, extracting feature points from all point cloud data with dynamic identification based on multi-scale local smoothness; S33: In the feature point set, the dynamic feature points are removed based on the aforementioned dynamic point identification; a buffer zone is further set in the vicinity of the dynamic points to remove points that may be disturbed by dynamic targets, and only static corner points and plane points are retained for subsequent laser inertial estimation; S34: laser inertial motion estimation is realized based on feature point matching.

5. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 4, characterized in that, The identification of dynamic point clouds from the timing-corrected laser point cloud data based on a semantic segmentation algorithm specifically comprises the following steps: S311: convert the timing-corrected laser point cloud data into a distance image representation through spherical projection; S312: use a 2D convolutional neural network to perform semantic segmentation on the distance image; S313: project the image semantic segmentation result back to the corresponding three-dimensional point cloud, assign semantic labels to each point cloud point, identify the attribute field of the dynamic points, and use GPU-accelerated k-nearest neighbor search method to optimize the semantic segmentation result; S314: write dynamic identification in the attribute field of the identified dynamic points to realize labeling and differentiation of the dynamic points and maintain consistency with the point cloud index.

6. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 4, characterized in that, The extraction of feature points from all point cloud data with dynamic identification based on multi-scale local smoothness specifically comprises the following sub-steps: S321: for each point of the input point cloud, calculate the local smoothness in a small neighborhood and a large neighborhood, respectively; wherein the small neighborhood smoothness is used to capture local geometric mutations to identify corner points, and the large neighborhood smoothness is used to represent overall flatness to extract plane points; S322: According to the small neighborhood smoothness, the point cloud is sorted according to the smoothness value from large to small, and a plurality of points greater than the corner point threshold value are selected as the corner point set in turn; according to the large neighborhood smoothness, the point cloud is sorted according to the smoothness value from small to large, and the points less than the preset plane point threshold value are selected as the plane candidate points in turn, and the plane candidate points are filtered by voxels or uniformly down-sampled to obtain the final plane point set.

7. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 1, characterized in that, The laser inertial motion estimation is realized based on feature point matching; specifically comprising the following steps: S341: A series of key frames adjacent to the current frame in space-time are selected, the key frame point clouds are converted to the global coordinate system and then superimposed to form a local point cloud map containing corner points and plane points; S342: For the corner points extracted from the current frame point cloud, adjacent corner point sets are searched in the local corner point map, and a straight line is fitted to construct a straight line residual of the current frame corner point to the straight line; for the plane points extracted, adjacent plane point sets are searched in the local plane point map, and a plane is fitted to construct a plane residual of the current frame plane point to the plane; S343: The point-to-line residual and the point-to-plane residual are combined to form a geometric residual, and a pose optimization objective function is constructed; in the optimization process, a robust loss function is applied to each residual term to suppress the influence of outliers; a GN or LM nonlinear least squares optimization algorithm is used to iteratively solve the objective function, thereby obtaining the current frame pose estimation result of the laser inertial subsystem.

8. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 3, characterized in that, The visual inertial motion estimation specifically comprises the following steps: S41: Visual feature points are extracted from the rectified image through optical flow tracking and corner detection; S42: Depth information of the visual feature points is obtained based on a local surface fitting depth information estimation method; S43: Visual inertial motion estimation is realized based on sliding window optimization.

9. The multi-sensor fusion SLAM method for dynamic traffic scenarios according to claim 8, characterized in that, The depth information of the visual feature points is obtained based on the local surface fitting depth information estimation method, specifically comprising the following steps: S421: A plurality of frames of point clouds adjacent in time are fused to obtain a dense global point cloud, and then the coordinates of the global point cloud are converted from the world coordinate system to the camera coordinate system; the point clouds and the visual feature points in the image are projected onto a unit sphere with the camera optical center as the center; S422: A plurality of nearest neighbor laser points of each feature point are searched on the unit sphere, and a local quadratic surface model is fitted based on the three-dimensional coordinates of the points in the camera coordinate system; S423: The distance from the intersection point of the visual ray and the local surface to the camera optical center is calculated, which is the depth value of the feature point.

10. A multi-sensor fusion SLAM system for dynamic traffic scenarios, characterized in that, The system comprises a preprocessing module, a pose estimation module and a fusion module: The preprocessing module pre-processes the collected multi-source sensor data, and performs laser inertial motion estimation and visual inertial motion estimation based on the pre-processed multi-source sensor data; The pose estimation module constructs a factor graph model based on the laser inertial motion estimation and the visual inertial motion estimation result, and optimizes and solves the factor graph model to obtain the optimal pose estimation of the system, i.e. the positioning result output; The fusion module filters out dynamic point clouds based on the optimal pose estimation result after optimization to construct a static map.

Images