Dynamic environment semantic slam method based on monocular vision and lidar fusion

Abstract

The application relates to a dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion and belongs to the technical field of robots. The method comprises the following steps: 1) performing instance segmentation on a point cloud, separating unknown motion state, pure static road markers and ground attribute point clouds, identifying semi-static and dynamic road marker point clouds through preliminary pose estimation and Kalman filtering, and then performing accurate pose estimation; 2) performing image instance segmentation and enhancement, and extracting SuperPoint feature points, projecting dynamic road markers to segmented images, then distinguishing static and non-static points, tracking static features by adopting multiple tracking strategies, and optimizing a pose by utilizing a local map; 3) performing heterogeneous sensor pose fusion; 4) combining two loop detection results to output an optimized trajectory; and 5) constructing a global pure static semantic map. The application considers the influence of a dynamic environment, and improves the positioning precision and mapping quality of a system in a dynamic and complex environment.

Description

Technical Field

[0001] The invention belongs to the technical field of robotics and relates to a dynamic environment semantic SLAM method based on the fusion of monocular vision and LiDAR. Background Art

[0002] Simultaneous Localization and Mapping (SLAM), the foundation for autonomous robot navigation, has made significant progress over the past few decades. However, traditional SLAM methods, primarily designed for static environments, still have numerous limitations when faced with the complex real world, making it difficult to meet the needs of autonomous robot navigation in dynamic scenarios.

[0003] First, traditional visual SLAM systems rely on static feature points in the environment for localization and mapping. However, in dynamic environments, the presence of feature points on moving objects interferes with feature matching accuracy, easily causing visual odometry drift and resulting in a decrease in localization accuracy. This inaccurate pose estimation directly impacts the accuracy of subsequent visual-LiDAR fusion, causing the fused pose to shift. Second, maps created by traditional laser SLAM are mostly composed of simple geometric elements and lack a description of the categories and attributes of objects in the environment. This lack of semantic representation severely limits the usefulness of maps in high-level applications such as robotic navigation and human-computer interaction. Furthermore, dynamic objects generate a large number of false trajectories and motion blur during the SLAM 3D mapping process, causing map inconsistencies and the appearance of "ghosting." Therefore, achieving robust localization and high-quality mapping in dynamic scenes is a key issue that needs to be addressed. Integrating semantic information with the SLAM process to construct semantic maps is an essential requirement for improving the environmental perception and understanding capabilities of SLAM systems.

[0004] In summary, facing complex dynamic environments, the existing SLAM systems have the following limitations:

[0005] 1) Traditional visual SLAM relies on static feature points for positioning and mapping. In dynamic environments, moving objects affect feature matching accuracy, which in turn causes visual odometry drift, further affecting the accuracy of visual-LiDAR fusion and causing the fused pose to shift.

[0006] 2) The lack of effective methods to deeply integrate rich visual semantic information with laser point clouds makes it difficult to construct accurate, semantic, purely static three-dimensional environmental map models, which restricts the robot's high-level understanding of the environment. Summary of the Invention

[0007] Therefore, the present application aims to provide a dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, solve the problems of inaccurate pose estimation and lack of semantic understanding of the existing SLAM system in a dynamic environment, and improve the positioning accuracy and mapping quality of the system in a dynamic and complex environment by considering the influence of the dynamic environment.

[0008] To achieve the above-mentioned purpose, the present application provides the following technical solutions:

[0009] A dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, specifically comprising the following steps:

[0010] S1: LiDAR (Laser Radar) odometry: acquire time-synchronized panoramic images, perform semantic segmentation on the panoramic images, map the pixel coordinates in the panoramic images to the 3D point cloud coordinate system with the corresponding timestamp through the LiDAR and image extrinsic calibration parameters, and then perform instance segmentation on the 3D point cloud to separate the unknown motion state, pure static road marker point cloud and ground attribute point cloud; identify the semi-static road marker point cloud and dynamic road marker point cloud through preliminary pose estimation and Kalman filtering, and then perform accurate pose estimation;

[0011] S2: Visual odometry: perform instance segmentation and data augmentation on 2D images, and extract SuperPoint feature points from the original images and augmented images; project the dynamic road markers identified in the LiDAR point cloud onto the corresponding image plane through the calibration parameters between the LiDAR and the image, and determine the instance category to which the projection points belong by using the instance segmentation results of the image, and then distinguish static and non-static feature points; track static features using multiple tracking strategies (motion model, reference key frame, SuperGlue strategy, repositioning, etc.), and optimize the pose using local map optimization;

[0012] S3: Fusion strategy: convert the pose estimated by the visual odometry to the LiDAR coordinate system, so that the pose estimates of the visual odometry and the LiDAR odometry are in the same coordinate system; then, use a decision-level fusion algorithm to fuse the poses at the odometry level;

[0013] S4: Loop detection: integrate the loop detection results of the LiDAR SLAM and the visual SLAM, and output the optimized trajectory estimate after loop correction;

[0014] S5: Mapping: filter pure static semantic point cloud information, combine optimized pose information, and construct a globally consistent pure static semantic map.

[0015] Further, step S1 specifically comprises the following steps:

[0016] S101: Collect LiDAR point cloud data and panoramic camera image for time synchronization, use DeepLabV3 model for semantic segmentation of panoramic image, identify road, vehicle, pedestrian and pole and other semantic categories; then, through the external parameter calibration parameters of LiDAR and image, the semantics of pixel coordinates in the panoramic image are mapped to the 3D point cloud coordinate system with the corresponding timestamp, and semantic information is given to each 3D point;

[0017] S102: For 3D point cloud with semantic information, DBSCAN clustering is performed on point cloud with the same semantic label, and instance segmentation of 3D point cloud is realized through clustering; and according to the semantic information of instance segmentation, the instance is divided into unknown motion state road sign, pure static road sign and ground attribute point cloud;

[0018] S103: In the preliminary pose estimation stage, only the horizontal position change Δx and Δy and the change Δθ of the yaw angle are estimated, that is, Δt = [Δx, Δy, Δθ] T , where Δt represents the change of the preliminary estimated attitude; when the number of pure static road signs is greater than that of unknown motion state road signs, only pure static road signs are used, otherwise pure static road signs and unknown motion state road signs are selected for preliminary pose estimation;

[0019] S104: For the road signs selected in the k-1th frame and the kth frame, the corresponding relationship between the road signs is established through the consistency of semantic information and the distance threshold of road sign gravity center between the two consecutive frames; after the paired road signs are processed in the vertical direction, Graham's Scan algorithm is used to construct two-dimensional convex hull for each layer of point cloud; based on the integral difference of the corner function curve and the Hausdorff distance, a comprehensive evaluation index is constructed to evaluate the convex hull similarity, so as to select the most similar convex hull feature pair; a target function with the convex hull overlap area as the optimization objective is constructed, and the optimized particle swarm algorithm is applied to solve the optimal solution Δt * = [Δx * ,Δy * ,Δθ * ] T , where Δt * represents the optimal solution of the change of the preliminary estimated attitude, Δx * , Δy * , and Δθ * represent the optimal solution of the horizontal position change and the yaw angle change, respectively;

[0020] S105: For the road sign at time k, a road sign dynamic state model based on the road sign center is constructed, and the state vector of the model includes the displacement vector (x, y) of the road sign center and the velocity vector The road sign center in the LiDAR coordinate system at the current time k is mapped through the preliminary pose estimation result Δt *= [Δx * , Δy * , Δθ * ] T Convert to LiDAR coordinate system at time k-1, and take the converted landmark position as the observation value of Kalman filtering;

[0021] S106: Construct the state transition equation of Kalman filtering based on the two-dimensional uniform turning model, which is used to predict the state of the displacement vector (x, y) and the velocity vector of the landmark center;

[0022] S107: Estimate the state of the landmark at time k through the Kalman filter by comprehensively considering the predicted state and the observation value of the landmark center; for landmarks with the same semantics, compare the filtering estimation values of the landmark at time k-1 and time k; if the position change and the velocity change are both less than the preset adaptive threshold, mark the landmark as a semi-static landmark; otherwise, mark it as a dynamic landmark;

[0023] S108: The pose accurate estimation is divided into two stages of horizontal direction and vertical direction attitude estimation; for the horizontal direction attitude estimation, only pure static and semi-static landmarks are used to solve, so as to avoid the negative influence of dynamic landmarks; and the Scan-to-Local Submap registration method is used to match the current frame with the landmarks in the local map to realize the initial pairing; then the horizontal direction attitude is estimated based on the estimation method of step S104;

[0024] S109: For the vertical direction attitude estimation, the point cloud with ground semantic information is extracted from the LiDAR data after instance segmentation; these point clouds are divided into forward and backward two parts, and RANSAC algorithm is used for plane fitting respectively to obtain the plane model and normal vector;

[0025] S120: Estimate the pitch angle of the vehicle relative to the initial attitude based on the normal vector of the fitted plane; combine the obtained yaw angle information and horizontal position, and estimate the position of the vehicle in the z direction through the established space geometry model, finally complete the complete 6DOF pose estimation, and obtain the attitude transformation of the LiDAR odometer, wherein R L represents the rotation matrix part of T L , and t L represents the translation vector part of T L .

[0026] Further, step S2 specifically comprises the following steps:

[0027] S201: For the front-view camera, instance segmentation is performed on the original 2D image using YOLOv8 Segment to realize target detection and pixel-level segmentation of unknown moving objects, and the accurate pixel coordinate information is obtained;

[0028] S202: The original gray image I(x c ,y c ) is subjected to data enhancement by inverse color transformation, and a new enhanced image I'(x c ,y c ) is generated through pixel inversion operation I'(x c ,y c ) = 255-I(x c ,y c );

[0029] S203: The original gray image I(x c ,y c ) and the enhanced image I'(x c ,y c ) are subjected to SuperPoint convolutional neural network to extract SuperPoint feature points;

[0030] S204: The identified dynamic road marker point cloud in the LiDAR point cloud is projected onto the camera image plane detected by YOLO v8 Segmentation to realize alignment of camera and LiDAR instance segmentation; the projection process includes the following two coordinate transformations: (1) conversion from LiDAR coordinate system to camera coordinate system: P cam =T CL ·P LiDAR , where P cam represents the coordinates in the camera coordinate system after conversion, T CL represents the conversion matrix from the LiDAR coordinate system to the camera coordinate system obtained by calibration, and P LiDAR represents the coordinates of the point cloud in the LiDAR coordinate system identified as dynamic road markers by Kalman filtering for motion state tracking; (2) conversion from camera coordinate system to pixel coordinate system: p = K·P cam , where K is the camera intrinsic matrix, and p is the final pixel coordinate;

[0031] S205: Based on the projection transformation of step S204, the LiDAR point cloud of the dynamic target is accurately projected onto the camera image plane and matched with the YOLO v8 detection result; the SuperPoint feature points located in the dynamic area are filtered through the spatial correspondence relationship and marked as dynamic feature points, and only the static feature points outside the area are retained for subsequent visual odometry calculation;

[0032] S206: In the monocular initialization stage, the feature matching is completed by using the static SuperPoint feature points and the SuperGlue algorithm; based on the matching result, the fundamental matrix is calculated and the 3D points are obtained through feature point triangulation, and the monocular initialization is completed through BA optimization;

[0033] S207: In the tracking strategy stage of the feature points, the system adopts a three-level tracking strategy: first, the constant speed model is used to predict the pose; if it fails, the reference key frame tracking is used; then, the SuperGlue-based tracking method is used to solve the pose; when the tracking is lost, the repositioning is performed in the key frame database through the bag-of-words model;

[0034] S208: After the camera pose is determined, the key frames and their map points having the co-view relationship with the current frame are selected from the local map to construct the local map;

[0035] S209: Based on the correspondence relationship between the 3D points in the local map and the 2D static SuperPoint feature points of the current frame, the camera pose is optimized by minimizing the re-projection error, so as to obtain the estimated pose matrix where R V represents the rotation matrix part of T V , and t V represents the translation vector part of T V .

[0036] Further, step S3 specifically includes the following steps:

[0037] S301: The LiDAR and the camera are calibrated by using the LiDAR Camera Calibrator tool of MATLAB to obtain the external parameter conversion relationship between them; then, the estimated value of the visual odometry is converted from the camera coordinate system to the LiDAR coordinate system by using the external parameter, and the accompanying matrix V of R is obtained, so that the unified representation of the two odometry estimates in the same coordinate system is realized;

[0038] S302: For the fusion of the decision-level odometry translation, considering that the monocular visual odometry has the problem of scale uncertainty, while the LiDAR odometry can provide accurate absolute scale information, therefore, the translation of the LiDAR odometry is directly used as the translation result after fusion, that is, the translation vector

[0039] S303: For the fusion of the decision-level rotation matrix, the state prediction is performed according to the accompanying matrix of the visual odometry at time k; the state prediction equation is: where k is the discrete time point, is the rotation matrix predicted according to the state transition equation; is the matrix for state transition, here where I3 represents the unit matrix; is the noise term in the state transition equation, which is assumed to conform to a normal distribution with a mean of zero and a covariance matrix is expressed as

[0040] At time k, the observation of the current state by the LiDAR odometer satisfies the following measurement equation: where, is the rotation matrix in the LiDAR odometer pose state, which is taken as the observation; is the observation model matrix, which maps the state space to the observation space, here is the observation noise in the observation process, which is assumed to conform to a normal distribution with a mean of zero and a covariance matrix Calculate the Kalman gain:

[0041] where, represents the Kalman gain, represents the state covariance matrix; update the state estimate as: S304: combine the rotation matrix

[0042] and the translation vector into a complete 6-DOF pose Further, step S4 specifically includes the following steps:

[0043] S401: use a parallel detection method to simultaneously run two loop detection threads for the laser radar and vision;

[0044] S402: in the laser radar loop detection thread, only use pure static LinK3D features to perform loop detection based on the BoW3D algorithm;

[0045] S403: in the visual loop detection thread, use the loop detection mechanism of ORB-SLAM2 to perform loop detection of the visual scene based on ORB features;

[0046] S404: based on the detected loop relationship, perform global pose optimization and correct the accumulated trajectory error.

[0047] Further, step S5 specifically includes the following steps:

[0048]

[0049] ​S501: based on the result of semantic segmentation, the point cloud data is screened, the point cloud corresponding to the dynamic object and the semi-static object is removed, and the pure static environment information is retained;

[0050] S502: using the fused pose information, the screened pure static point cloud with semantic labels is uniformly converted to a global coordinate system; on this basis, a pure static map with global consistency and semantic attributes is constructed.

[0051] The beneficial effects of the present application are:

[0052] (1) The present application realizes the identification of dynamic targets through LiDAR, and projects the identification results to the visual image for feature point screening, and then combines the decision level fusion algorithm to optimize the pose estimation, thereby effectively improving the positioning accuracy in complex dynamic environment.

[0053] (2) The present application realizes instance segmentation on LiDAR point cloud, and screens out pure static point cloud information, and constructs a globally consistent pure static semantic map using the fused pose information. Compared with the traditional geometric map, the semantic map constructed by the present application not only effectively solves the ghost problem in dynamic environment and improves the mapping quality, but also retains the semantic information of the scene, thereby laying a foundation for intelligent decision and environment understanding of the robot.

[0054] Other advantages, objects and features of the present application will be set forth in part in the following specification, and in part will become apparent to those skilled in the art from the following, or can be learned from the practice of the present application. The objects and other advantages of the present application can be realized and obtained by the following description. BRIEF DESCRIPTION OF DRAWINGS

[0055] In order to make the objects, technical solutions and advantages of the present application clearer, the preferred detailed description of the present application will be made below in combination with the drawings, wherein:

[0056] Figure 1 The flow chart of the present application based on monocular vision and LiDAR fusion dynamic environment semantic SLAM method;

[0057] Figure 2 The original panoramic image data example;

[0058] Figure 3 The processed result example of DeepLabV3 semantic segmentation;

[0059] Figure 4 The 16-line LiDAR point cloud semantic segmentation result example;

[0060] Figure 5The recognition result of the dynamic and static attributes of the SuperPoint feature point of the application;

[0061] Figure 6 The physical map of the UGV experimental verification platform carrying a panoramic camera and a 16-line LiDAR;

[0062] Figure 7 The estimated trajectory comparison of the algorithm of the application and the comparative algorithm on the CQU 01 data set;

[0063] Figure 8 The estimated trajectory comparison of the algorithm of the application and the comparative algorithm on the CQU 02 data set;

[0064] Figure 9 The pure static attribute global consistent map established by the algorithm of the application on the CQU 02 data set. DETAILED DESCRIPTION

[0065] The present application will be described in more detail by the following specific examples. Other advantages and effects of the present application can be easily understood by those skilled in the art from this disclosure. The present application can also be implemented or applied in other different specific embodiments, and various modifications or changes can be made to the details in this specification based on different views and applications without departing from the spirit of the present application. It should be noted that the drawings provided in the following examples only illustrate the basic concept of the present application in a schematic manner, and the following examples and features in the examples can be combined with each other without conflict.

[0066] Please refer to Figures 1-9 The application provides a dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, which specifically comprises the following steps:

[0067] Step 1: LiDAR odometry: obtain time-synchronized panoramic images, perform semantic segmentation on the panoramic images, map the pixel coordinates in the panoramic images to the 3D point cloud coordinate system corresponding to the time stamp through the LiDAR and image extrinsic calibration parameters, and then perform instance segmentation on the 3D point cloud to separate unknown motion state, pure static road marker point cloud and ground attribute point cloud. Through preliminary pose estimation and Kalman filtering, semi-static road marker point cloud and dynamic road marker point cloud are identified, and then accurate pose estimation is performed.

[0068] Step 1 specifically comprises the following steps:

[0069] 1-1: For time-synchronized acquisition of LiDAR point cloud data and panoramic camera images, use the DeepLabV3 model to perform semantic segmentation on the panoramic image, identifying semantic categories such as roads, vehicles, pedestrians, and poles. Then, through the external calibration parameters of the LiDAR and the image, the semantics of the pixel coordinates in the panoramic image are mapped to the 3D point cloud coordinate system with the corresponding timestamp, giving each 3D point semantic information. Figure 2 is an example of original panoramic image data, Figure 3 is an example of DeepLabV3 semantic segmentation processing results, Figure 4 is an example of 16-line LiDAR point cloud semantic segmentation results.

[0070] 1-2: For 3D point cloud with semantic information, DBSCAN clustering is performed on point clouds with the same semantic label to achieve instance segmentation of 3D point cloud. According to the semantic information of instance segmentation, instances are divided into unknown motion state landmarks, pure static landmarks, and ground attribute point clouds.

[0071] 1-3: In the preliminary pose estimation stage, only the horizontal position change Δx and Δy, and the change in yaw angle Δθ are estimated, i.e. Δt = [Δx, Δy, Δθ] T , where Δt represents the change in the preliminary estimated pose; when the number of pure static landmarks is greater than that of unknown motion state landmarks, only pure static landmarks are used for preliminary pose estimation, otherwise pure static landmarks and unknown motion state landmarks are selected for preliminary pose estimation.

[0072] 1-4: For the landmarks selected in the k-1th frame and the kth frame, the correspondence relationship between the landmarks in the two consecutive frames is established through the consistency of semantic information and the distance threshold of landmark barycenter. After hierarchical processing of the paired landmarks in the vertical direction, Graham's Scan algorithm is used to construct a two-dimensional convex hull for each layer of point cloud. Based on the integral difference of the corner function curve and the Hausdorff distance, a comprehensive evaluation index is constructed to evaluate the convex hull similarity, so as to select the most similar convex hull feature pair. A target function is constructed with the convex hull overlap area as the optimization objective, and an optimized particle swarm algorithm is applied to solve the optimal solution Δt * = [Δx * ,Δy * ,Δθ * ] T , where Δt * represents the optimal solution of the change in the preliminary estimated pose, Δx * , Δy * , and Δθ * represent the optimal solutions of the horizontal position change and the yaw angle change, respectively.

[0073] 1-5: For the road mark at time k, a road mark dynamic state model based on the road mark center is constructed, and the state vector of the model contains the displacement vector (x, y) and the velocity vector of the road mark center The road mark center in the LiDAR coordinate system at the current time k is converted to the LiDAR coordinate system at time k-1 through the preliminary pose estimation result Δt * = [Δx * , Δy * , Δθ * ] T and the converted road mark position is taken as the observation value of the Kalman filter.

[0074] 1-6: The state transition equation of the Kalman filter is constructed based on the two-dimensional uniform turning model, which is used to predict the state of the displacement vector (x, y) and the velocity vector of the road mark center .

[0075] 1-7: Through the Kalman filter, the predicted state and the observation value of the road mark center are comprehensively considered to estimate the state of the road mark at time k. For road marks with the same semantics, the filter estimation values of the road mark at time k-1 and at time k are compared. If the position change and the velocity change are both less than the preset adaptive threshold, the road mark is marked as a semi-static road mark; otherwise, it is marked as a dynamic road mark.

[0076] 1-8: The pose accurate estimation is divided into two stages of horizontal direction and vertical direction pose estimation. For the horizontal direction pose estimation, only pure static and semi-static road marks are used for solving to avoid the negative influence of dynamic road marks. And the Scan-to-Local Submap registration method is used to match the current frame with the road marks in the local map to realize the initial pairing. Then the horizontal direction pose is estimated based on the estimation method described in 1-4.

[0077] 1-9: For the vertical direction pose estimation, the point clouds with ground semantic information are extracted from the LiDAR data after instance segmentation. These point clouds are divided into forward and backward parts, and the RANSAC algorithm is used for plane fitting respectively to obtain the plane model and the normal vector.

[0078] 1-10: Based on the normal vector of the fitted plane, the pitch angle of the vehicle relative to the initial pose is estimated. Combined with the obtained yaw angle information and horizontal position, the position amount of the vehicle in the z direction is estimated through the established space geometry model, and finally the complete 6DOF pose estimation is completed to obtain the pose transformation of the LiDAR odometer , where R L represents the rotation matrix part of T L , and t L represents the translation vector part of T L .

[0079] Step 2: Visual odometry: instance segmentation and data augmentation are performed on the 2D image, and SuperPoint feature points are extracted from the original image and the augmented image. By the calibration parameters between LiDAR and image, the dynamic road markers identified in the LiDAR point cloud are projected onto the corresponding image plane, and the instance segmentation results of the image are used to determine the instance category to which the projected points belong, thereby distinguishing static and non-static feature points. Multiple tracking strategies (motion model, reference key frame, SuperGlue strategy, relocalization, etc.) are used to track static features, and local map optimization is used to optimize the pose.

[0080] Step 2 specifically includes the following steps:

[0081] 2-1: For the front-view camera, YOLOv8 Segment is used for instance segmentation on the original 2D image to achieve target detection and pixel-level segmentation of unknown moving objects, and accurate pixel coordinate information is obtained.

[0082] 2-2: Data augmentation is performed on the original grayscale image I(x c ,y c ) using inverse color transformation, and a new augmented image I'(x c ,y c ) is generated through pixel inversion operation I'(x c ,y c ) = 255 - I(x c ,y c ).

[0083] 2-3: SuperPoint convolutional neural network is used to extract SuperPoint feature points from the original grayscale image I(x c ,y c ) and the augmented image I'(x c ,y c ).

[0084] 2-4: The identified dynamic road marker point cloud in the LiDAR point cloud is projected onto the camera image plane detected by YOLO v8 Segmentation to align the camera and LiDAR instance segmentation. The projection process includes the following two coordinate transformations:

[0085] (1) The conversion from LiDAR coordinate system to camera coordinate system is as follows: P cam = T CL ·P LiDAR , where P cam represents the coordinates in the camera coordinate system after conversion, T CL represents the conversion matrix from the LiDAR coordinate system to the camera coordinate system obtained by calibration, and P LiDARrepresents coordinates of the point cloud recognized as a dynamic road sign by motion state tracking through Kalman filtering in a LiDAR coordinate system.

[0086] (2) Conversion of the camera coordinate system to the pixel coordinate system: p = K P cam , wherein K is an intrinsic parameter matrix of the camera, and p is the final pixel coordinate.

[0087] 2-5: Based on the above projection transformation, the LiDAR point cloud of the dynamic target is accurately projected onto the camera image plane, and is matched with the YOLOv8 detection result. The SuperPoint feature points located in the dynamic region are screened through the spatial correspondence, and are marked as dynamic feature points. Only the static feature points outside the region are reserved for subsequent visual odometry calculation. Figure 5 is an example of the recognition result of the dynamic and static attributes of the SuperPoint feature points of the application.

[0088] 2-6: In the monocular initialization stage, the static SuperPoint feature points are used to complete feature matching through the SuperGlue algorithm. Based on the matching result, the fundamental matrix is calculated and the feature point triangulation is completed to obtain 3D points, and the monocular initialization is completed through the BA optimization.

[0089] 2-7: In the tracking strategy stage of the feature points, the system adopts a three-level tracking strategy: first, the constant speed model is used to predict the pose; if it fails, the reference key frame tracking is used; finally, the SuperGlue-based tracking method is used to solve the pose. When the tracking is lost, the repositioning is performed in the key frame database through the bag-of-words model.

[0090] 2-8: After the camera pose is determined, the key frames and their map points having the co-view relationship with the current frame are selected from the local map to construct the local map.

[0091] 2-9: Based on the correspondence relationship between the 3D points in the local map and the 2D static SuperPoint feature points of the current frame, the camera pose is optimized by minimizing the re-projection error, so as to obtain the estimated pose matrix , wherein R V represents the rotation matrix part of T V , and t V represents the translation vector part of T V .

[0092] Step 3: Fusion strategy: the pose estimated by the visual odometry is converted to the LiDAR coordinate system, so that the pose estimates of the visual odometry and the LiDAR odometry are in the same coordinate system. Then, the decision level fusion algorithm is used to fuse the poses at the odometry level.

[0093] Step 3 specifically includes the following steps:

[0094] 3-1: The LiDAR and camera are calibrated by using the LiDAR Camera Calibrator tool of MATLAB to obtain the extrinsic transformation relationship between them. Then, the estimated value of the visual odometry is converted from the camera coordinate system to the LiDAR coordinate system using the extrinsic parameter, and the R V is obtained. The unified representation of the two kinds of odometry estimates in the same coordinate system is realized.

[0095] 3-2: For the fusion of the decision-level odometry translation, the scale uncertainty problem of the monocular visual odometry is considered, and the LiDAR odometry can provide accurate absolute scale information. Therefore, the translation of the LiDAR odometry is directly used as the translation result after fusion, that is, the translation vector

[0096] 3-3: For the fusion of the decision-level rotation matrix, the state prediction is performed according to the companion matrix of the visual odometry at time k The state prediction equation is: where k is a discrete time point, is the rotation matrix predicted according to the state transition equation; is the matrix used for state transition, here where I3 represents the unit matrix; is the noise term in the state transition equation, which is assumed to conform to the normal distribution with mean zero and covariance matrix

[0097] At time k, the observation of the current state by the LiDAR odometry satisfies the following measurement equation: where is the rotation matrix in the LiDAR odometry pose state, which is taken as the observation; is the observation model matrix, which maps the state space to the observation space, here is the observation noise in the observation process, which is assumed to conform to the normal distribution with mean zero and covariance matrix : The Kalman gain is calculated as: where represents the Kalman gain, represents the state covariance matrix. The state estimate is updated as:

[0098] 3-4: The rotation matrix and the translation vector are combined into a complete 6-DOF pose

[0099] Step 4: Loop closure detection: integrate the loop closure results of LiDAR SLAM and visual SLAM, and output the optimized trajectory estimation after loop closure correction.

[0100] Step 4 specifically includes the following steps:

[0101] 4-1: Use parallel detection mode, simultaneously run two loop closure detection threads of laser radar and vision.

[0102] 4-2: In the laser radar loop closure detection thread, based on the BoW3D algorithm, only pure static LinK3D features are used for loop closure detection.

[0103] 4-3: In the visual loop closure detection thread, the loop closure detection mechanism of ORB-SLAM2 is used, and ORB features are used for loop closure detection of visual scenes.

[0104] 4-4: Based on the detected loop closure relationship, global pose optimization is performed, and the accumulated trajectory error is corrected.

[0105] Step 5: Mapping: screen pure static semantic point cloud information, combine optimized pose information, and construct a globally consistent pure static semantic map.

[0106] Step 5 specifically includes the following steps:

[0107] 5-1: Based on the results of the aforementioned semantic segmentation, screen the point cloud data, remove the point cloud corresponding to dynamic and semi-static objects, and retain pure static environmental information.

[0108] 5-2: Based on the fused pose information, uniformly convert the pure static point cloud with semantic labels to the global coordinate system. On this basis, a globally consistent pure static map with semantic attributes is constructed.

[0109] Comparative experiment:

[0110] In this experiment, the method of the present application is compared with a plurality of existing SLAM algorithms (including FLOAM: WANG H, WANG C, CHEN C L, et al. F-LOAM: Fast LiDAR Odometry and Mapping [C] / / 2021 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). 2021: 4390-4396; Light-LOAM: YI S, LYU Y, HUA L, et al. Light-LOAM: A Lightweight LiDAR Odometry and Mapping Based on Graph-Matching [J]. IEEE Robotics and Automation Letters, 2023, 9(4): 3219-3226; LOAM-Velodyne: ZHANG J, SINGH S. LOAM: Lidar Odometry and Mapping in Real-time [C] / / Robotics: Science and Systems. 2014: 1-9; SD-SLAM: SD-SLAM: A semantic SLAM approach for dynamic scenes based on LiDAR point clouds [J]. Big Data Research, 2024, 36: 100463) for comparison. In order to ensure the reliability and accuracy of the evaluation, the Huachu CGI-610 GNSS system is used as the ground truth (GT) reference system in this experiment, Figure 6 The physical diagram of the UGV experimental verification platform carrying a panoramic camera, a 16-line LiDAR and a GNSS.

[0111] The experimental evaluation adopts two dimensions of quantity and quality, and is verified on two data sets CQU01 and CQU02. From the quantitative evaluation point of view, this experiment adopts three key indicators to measure the performance: average error of estimated position (AEEP), which is used to evaluate the average deviation between the estimated position of the algorithm and the GNSS ground truth; standard deviation of estimated position error (SDEPE), which reflects the stability and consistency of position estimation; position error of trajectory endpoint (PETE), which evaluates the influence of cumulative error on the final positioning accuracy.

[0112] Table 1 Comparison of positioning accuracy of the present application and comparative algorithms

[0113]

[0114]

[0115] Quantitative analysis results show that, as shown in Table 1, the three evaluation indexes of the method proposed in the application are significantly better than the benchmark algorithm on the two data sets of CQU01 and CQU02. Specifically, on the CQU01 data set, the method of the application realizes an average estimated position error (AEEP) of 0.512 m, a standard deviation of estimated position error (SDEPE) of 0.229 m, and a position error of trajectory endpoint (PETE) of 0.754 m. After introducing the loop detection mechanism, the algorithm performance is significantly improved: the AEEP is reduced to 0.341 m, the SDEPE is reduced to 0.153 m, and the PETE is optimized to 0.502 m.

[0116] The qualitative evaluation results strongly verify the effectiveness of the method. To visually display the algorithm performance, the experiment selects representative experimental results for comparative analysis. Figure 7 and Figure 8 respectively show the estimated trajectories of the method of the application and a plurality of classical SLAM algorithms on the CQU01 and CQU02 data sets. The results show that the trajectory generated by the algorithm of the application maintains the highest consistency with the GNSS reference trajectory, fully embodying the positioning accuracy advantage of the method. In addition, the application further explores the ability of the proposed method to reconstruct a pure static semantic map in a complex dynamic environment. Figure 9 A globally consistent static semantic map based on the CQU02 data set is shown. The rich semantic attribute information intuitively reflects the robustness of the algorithm in extracting pure static structures and eliminating dynamic interference in dynamic scenes.

[0117] In summary, the method proposed in the application can effectively handle the positioning and mapping problems in dynamic scenes, significantly improving the positioning accuracy and mapping quality of the system in complex dynamic environments. The experimental results show that the method has obvious advantages in handling dynamic targets, maintaining high positioning accuracy, and constructing high-quality semantic maps.

[0118] Finally, it should be explained that the above examples are only used to illustrate the technical solutions of the application and not to limit it. Although the application has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the application can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should be covered in the scope of the claims of the application.

Claims

1. A dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, characterized in that: The method specifically comprises the following steps: S1: LiDAR odometry: Acquires time-synchronized panoramic images, performs semantic segmentation on the panoramic images, and uses LiDAR and image extrinsic calibration parameters to map the pixel coordinates in the panoramic images to the 3D point cloud coordinate system of the corresponding timestamp. This allows for instance segmentation of the 3D point cloud, distinguishing between unknown motion states, purely static landmark point clouds, and ground attribute point clouds. Initial pose estimation and Kalman filtering are used to identify semi-static and dynamic landmark point clouds, leading to precise pose estimation. S2: Visual Odometry: Performs instance segmentation and data augmentation on 2D images, and extracts SuperPoint feature points from both the original and enhanced images. Using calibration parameters between the LiDAR and image, dynamic landmarks identified in the LiDAR point cloud are projected onto the corresponding image plane. The instance segmentation results of the image are used to determine the instance category to which the projected points belong, thereby distinguishing between static and non-static feature points. Static features are tracked using multiple tracking strategies, and the pose is optimized using a local map. S3: Fusion strategy: The pose estimated by the visual odometry is converted to the LiDAR coordinate system so that the pose estimates of the visual odometry and LiDAR odometry are in the same coordinate system; then, the pose is fused using a decision-level fusion algorithm at the odometry level; S4: Loop closure detection: Integrate the loop closure detection results of LiDAR SLAM and visual SLAM, and output the optimized trajectory estimate after loop closure correction; S5: Mapping: Filter the purely static semantic point cloud information and combine it with the optimized posture information to build a globally consistent purely static semantic map.

2. The dynamic environment semantic SLAM method according to claim 1, wherein Step S1 specifically includes the following steps: S101: For time-synchronized acquisition of LiDAR point cloud data and panoramic images, the DeepLabV3 model is used to perform semantic segmentation on the panoramic image and identify semantic categories. Then, using extrinsic calibration parameters of the LiDAR and image, the semantics of the pixel coordinates in the panoramic image are mapped to the 3D point cloud coordinate system of the corresponding timestamp, assigning semantic information to each 3D point. S102: For 3D point clouds with semantic information, DBSCAN clustering is performed on point clouds with the same semantic labels to achieve instance segmentation of the 3D point cloud. Instances are then divided into unknown motion state landmarks, purely static landmarks, and ground attribute point clouds based on the semantic information of instance segmentation. S103: In the initial pose estimation stage, only the horizontal position changes Δx and Δy, and the yaw angle change Δθ are estimated, that is, Δt = [Δx, Δy, Δθ] T , where Δt represents the change in posture during the preliminary estimation stage; when the number of pure static landmarks is greater than the number of unknown motion state landmarks, only pure static landmarks are used; otherwise, pure static landmarks and unknown motion state landmarks are used for preliminary pose estimation; S104: For the landmarks selected in the k-1th frame and the kth frame, a landmark correspondence is established between the two consecutive frames based on the consistency of semantic information and the landmark center of gravity distance threshold; after vertical layering of the paired landmarks, a two-dimensional convex hull is constructed for each layer of point cloud using the Graham's Scan algorithm; a comprehensive evaluation index is constructed based on the integral difference of the corner function curve and the Hausdorff distance to evaluate the convex hull similarity, thereby screening the most similar convex hull feature pairs; an objective function is constructed with the convex hull overlap area as the optimization target, and an optimized particle swarm algorithm is applied to solve the optimal solution Δt that maximizes the overlap area. * =[Δx * ,Δy * ,Δθ * ] T , where Δt * represents the optimal solution for the change in posture in the initial estimation stage, Δx * ,Δy * ,Δθ * They represent the optimal solutions for the horizontal position change and the yaw angle change respectively; S105: For the landmark at time k, a landmark dynamic state model based on the landmark center is constructed. The state vector of the model includes the displacement vector (x, y) and velocity vector of the landmark center. The center of the landmark in the LiDAR coordinate system at the current k moment is estimated by the preliminary pose estimation result Δt * =[Δx * ,Δy * ,Δθ * ] T Convert to the LiDAR coordinate system at time k-1 and use the converted landmark position as the observation value of the Kalman filter; S106: Construct the state transition equation of the Kalman filter based on the two-dimensional uniform turning model. This equation is used to predict the displacement vector (x, y) and velocity vector of the center of the landmark. Status; S107: Using a Kalman filter, the predicted state and observed value of the landmark center are comprehensively considered to estimate the state of the landmark at time k. For landmarks with the same semantics, the filtered estimated values ​​at time k-1 and time k are compared. If both the position change and the speed change are less than a preset adaptive threshold, the landmark is marked as a semi-static landmark; otherwise, it is marked as a dynamic landmark. S108: Accurate pose estimation is divided into two stages: horizontal and vertical pose estimation. For horizontal pose estimation, only purely static and semi-static landmarks are used for solution. The Scan-to-Local Submap registration method is used to match the current frame with landmarks in the local map to achieve initial pairing. Then, the horizontal pose is estimated based on the estimation method in step S104. S109: For vertical pose estimation, extract point clouds with ground semantic information from the instance-segmented LiDAR data; divide these point clouds into forward and backward parts, and use the RANSAC algorithm to perform plane fitting on each part to obtain the plane model and normal vector; S110: Estimate the vehicle's pitch angle relative to the initial attitude based on the normal vector of the fitted plane; combine the obtained yaw angle information and horizontal position, and estimate the vehicle's position in the z direction through the established spatial geometric model, finally completing the complete 6DOF pose estimation and obtaining the attitude transformation matrix of the LiDAR odometer where R L Indicates T L The rotation matrix part, t L Indicates T L The translation vector portion of .

3. The dynamic environment semantic SLAM method according to claim 2, wherein Step S2 specifically includes the following steps: S201: For the front-view camera, YOLOv8 Segment is used to perform instance segmentation on the original 2D image to achieve target detection and pixel-level segmentation of unknown moving objects and obtain their precise pixel coordinate information; S202: The original grayscale image I(x c ,y c ) uses inverse color transformation to perform data enhancement, and performs pixel inversion operation I′(x c ,y c )=255-I(x c ,y c ) generates a new enhanced image I′(x c ,y c ); S203: The original grayscale image I(x c ,y c ) and enhanced image I′(x c ,y c )Use SuperPoint convolutional neural network to extract SuperPoint feature points; S204: Project the identified dynamic landmark point cloud in the LiDAR point cloud onto the camera image plane detected by YOLO v8 Segmentation to achieve alignment between the camera and LiDAR instance segmentation; the projection process includes the following two steps of coordinate transformation: (1) Conversion from LiDAR coordinate system to camera coordinate system: P cam =T CL ·P LiDAR , where P cam Represents the coordinates in the camera coordinate system after transformation, T CL represents the transformation matrix from the LiDAR coordinate system to the camera coordinate system obtained by calibration, P LiDAR represents the coordinates of the point cloud identified as a dynamic landmark in the LiDAR coordinate system after motion tracking by Kalman filtering; (2) Conversion from the camera coordinate system to the pixel coordinate system: p = K·P cam , where K is the camera intrinsic parameter matrix and p is the final pixel coordinate; S205: Based on the projection transformation in step S204, the LiDAR point cloud of the dynamic target is accurately projected onto the camera image plane and matched with the YOLO v8 detection results; the SuperPoint feature points located in the dynamic area are filtered through spatial correspondence and marked as dynamic feature points, and only the static feature points outside the area are retained for subsequent visual odometry calculation; S206: In the monocular initialization stage, static SuperPoint feature points are used to complete feature matching through the SuperGlue algorithm; based on the matching results, the basic matrix is ​​calculated and the feature points are triangulated to obtain 3D points, and monocular initialization is completed through BA optimization; S207: In the feature point tracking strategy phase, the system adopts a three-level tracking strategy: first, the constant velocity model is used to predict the pose; if this fails, reference keyframe tracking is used; then the pose is solved using a SuperGlue-based tracking method; if tracking is lost, relocalization is performed in the keyframe database using a bag-of-words model; S208: After determining the camera pose, select key frames and their map points that have a common view relationship with the current frame from the local map to construct a local map; S209: Based on the correspondence between the 3D points in the local map and the 2D static SuperPoint feature points of the current frame, the camera pose is optimized by minimizing the reprojection error to obtain the estimated pose matrix where R V Indicates T V The rotation matrix part, t V Indicates T V The translation vector portion of .

4. The dynamic environment semantic SLAM method according to claim 3, wherein Step S3 specifically includes the following steps: S301: Calibrate the LiDAR and camera external parameters to obtain the external parameter conversion relationship between them; then, use this external parameter to convert the estimated value of the visual odometry from the camera coordinate system to the LiDAR coordinate system to obtain R V The adjoint matrix R C , achieving unified representation of the two odometry estimates in the same coordinate system; S302: For the fusion of decision-level odometry translation, directly use the translation of the LiDAR odometry as the fused translation result, that is, the fused translation vector S303: Fusion of the decision-making level rotation matrix, based on the adjoint matrix of the visual odometry at time k Perform state prediction; the state prediction equation is: Where k is a discrete time point, is the rotation matrix predicted by the state transfer equation; is the matrix for state transition, where Where I3 represents the unit matrix; is the noise term in the state transfer equation, assuming that it has a mean of zero and a covariance matrix of Expressed as At time k, the LiDAR odometry's observation of the current state satisfies the following measurement equation: Where, is the rotation matrix in the LiDAR odometry pose state, which is used as the observation; is the observation model matrix, which maps the state space to the observation space. Here is the observation noise in the observation process, assuming that it has a mean of zero and a covariance matrix of The normal distribution is: Calculate the Kalman gain: Where, represents the Kalman gain, represents the state covariance matrix; the updated state estimate is: S304: Rotate the matrix and translation vectors Combined into a complete 6-DOF pose 5. The dynamic environment semantic SLAM method according to claim 1, wherein Step S4 specifically includes the following steps: S401: Use parallel detection to run two loop detection threads, one for lidar and one for vision, simultaneously. S402: In the lidar loop detection thread, loop detection is performed based on the BoW3D algorithm using only pure static LinK3D features. S403: In the visual loop detection thread, the loop detection mechanism of ORB-SLAM2 is used to perform loop detection of the visual scene based on ORB features; S404: Based on the detected loop closure relationship, perform global pose optimization and correct the accumulated trajectory error.

6. The dynamic environment semantic SLAM method according to claim 1, wherein Step S5 specifically includes the following steps: S501: Based on the results of semantic segmentation, the point cloud data is filtered to remove point clouds corresponding to dynamic objects and semi-static objects, and the purely static environment information is retained; S502: Using the fused pose information, the filtered pure static point clouds with semantic labels are uniformly converted to the global coordinate system; on this basis, a globally consistent pure static map with semantic attributes is constructed.

Images