Dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion

Abstract

The invention 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 point clouds to obtain unknown motion state point clouds, pure static road sign point clouds and ground attribute point clouds, identifying semi-static and dynamic road sign point clouds through initial pose estimation and Kalman filtering, and then performing accurate pose estimation; 2) segmenting and enhancing the image instance, extracting SuperPoint feature points, projecting a dynamic road sign to the segmented image so as to distinguish static and non-static points, tracking static features by adopting a plurality of tracking strategies, and optimizing the pose by utilizing a local map; 3) carrying out heterogeneous sensor attitude fusion; 4) outputting an optimized track by combining two loopback detection results; and 5) constructing a global pure static semantic map. According to the method, the dynamic environment influence is considered, and the positioning precision and the mapping quality of the system in the dynamic complex environment are improved.

Description

Technical Field

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

[0002] SLAM (Simultaneous Localization and Mapping), as the basis for realizing autonomous navigation of robots, has made great progress in the past few decades. However, traditional SLAM methods are mainly designed for static environments. When facing the complex real world, there are still many limitations, and it is difficult to meet the autonomous navigation needs of robots in dynamic scenes.

[0003] First, the traditional visual SLAM system relies on static feature points in the environment to achieve positioning and mapping. However, in a dynamic environment, the presence of feature points on moving objects interferes with the accuracy of feature matching, easily causing visual odometer drift, resulting in a decrease in positioning accuracy. This inaccuracy in attitude estimation will directly affect the subsequent visual-LiDAR fusion accuracy, causing the fusion pose to shift. Secondly, the maps built by traditional laser SLAM are mostly composed of simple geometric elements, lacking descriptions of object categories and attributes in the environment. This lack of semantic representation severely limits the use value of maps in high-level applications such as robot navigation and human-computer interaction. Furthermore, dynamic targets generate a large number of false trajectories and motion blur in the SLAM 3D mapping process, causing map inconsistency and the appearance of "ghosting". Therefore, how to achieve robust positioning and high-quality map construction in dynamic scenes is a key issue that needs to be solved urgently; how to combine semantic information with the SLAM process and build a semantic map is an inevitable requirement for improving the environmental perception and understanding capabilities of the SLAM system.

[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 a dynamic environment, moving objects affect the accuracy of feature matching, which in turn causes the visual odometer to drift, further affecting the accuracy of visual-LiDAR fusion and causing the fusion 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 build an accurate, semantic, purely static three-dimensional environmental map model, which restricts the robot's high-level understanding of the environment. Summary of the invention

[0007] In view of this, the purpose of the present invention is to provide a dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, to solve the problems of inaccurate pose estimation and lack of semantic understanding of existing SLAM systems in dynamic environments, and to improve the positioning accuracy and mapping quality of the system in dynamic and complex environments by considering the impact of dynamic environments.

[0008] In order to achieve the above object, the present invention 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) odometer: 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 of the corresponding timestamp through the external calibration parameters of LiDAR and the image, and then perform instance segmentation on the 3D point cloud to separate unknown motion states, purely static landmark point clouds, and ground attribute point clouds; identify semi-static landmark point clouds and dynamic landmark point clouds through preliminary pose estimation and Kalman filtering, and then perform precise pose estimation;

[0011] S2: Visual odometry: Perform instance segmentation and data enhancement on 2D images, and extract SuperPoint feature points from the original image and enhanced image; project the dynamic landmarks identified in the LiDAR point cloud onto the corresponding image plane through the calibration parameters between LiDAR and image, and use the instance segmentation results of the image to determine the instance category to which the projected point belongs, and then distinguish between static and non-static feature points; use a variety of tracking strategies (motion model, reference keyframe, SuperGlue strategy, relocalization, etc.) to track static features, and use local maps to optimize the pose;

[0012] S3: Fusion strategy: transform the attitude estimated by the visual odometry into the LiDAR coordinate system so that the attitude 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 attitude at the odometry level;

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

[0014] S5: Mapping: Filter the pure static semantic point cloud information and combine it with the optimized posture information to build a globally consistent pure static semantic map.

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

[0016] S101: For the time-synchronous collection of LiDAR point cloud data and panoramic camera images, the DeepLabV3 model is used to perform semantic segmentation on the panoramic image to identify semantic categories such as roads, vehicles, pedestrians, and poles; then, through the external calibration parameters of LiDAR and images, the semantics of the pixel coordinates in the panoramic image are mapped to the 3D point cloud coordinate system of the corresponding timestamp, and semantic information is given to each 3D point;

[0017] S102: For the 3D point cloud with semantic information, DBSCAN clustering is performed on the point clouds with the same semantic label, and instance segmentation of the 3D point cloud is achieved through clustering; and the instances are divided into unknown motion state landmarks, purely static landmarks, and ground attribute point clouds according to the semantic information of instance segmentation;

[0018] 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 in the preliminary estimation stage; when the number of pure static landmarks is greater than the unknown motion state landmarks, only pure static landmarks are used, otherwise pure static landmarks and unknown motion state landmarks are used for preliminary posture estimation;

[0019] S104: For the landmarks selected in the k-1th frame and the kth frame, a landmark correspondence relationship is established between two consecutive frames through the consistency of semantic information and the landmark center of gravity distance threshold; after the paired landmarks are layered in the vertical direction, 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 selecting the most similar convex hull feature pairs; an objective function with the convex hull overlap area as the optimization target is constructed, and an optimized particle swarm algorithm is used 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;

[0020] S105: For the road sign at time k, a road sign dynamic state model based on the road sign center is constructed. The state vector of the model includes the displacement vector (x, y) and the velocity vector of the road sign center. The center of the landmark in the LiDAR coordinate system at the current time k 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;

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

[0022] S107: using a Kalman filter, comprehensively considering the predicted state and observed value of the center of the landmark, estimating the state of the landmark at time k; for landmarks with the same semantics, comparing the filtered estimated values ​​at time k-1 and time k; if the position change and speed change are both less than a preset adaptive threshold, marking the landmark as a semi-static landmark; otherwise, marking it as a dynamic landmark;

[0023] S108: The precise pose estimation is divided into two stages: horizontal and vertical pose estimation. For the horizontal pose estimation, only pure static and semi-static landmarks are used to solve the problem, so as to avoid the negative impact of dynamic landmarks. The Scan-to-Local Submap registration method is used to match the current frame with the landmarks in the local map to achieve initial pairing. Then, the horizontal pose is estimated based on the estimation method of step S104.

[0024] S109: For vertical posture estimation, extract point clouds with ground semantic information from the LiDAR data after instance segmentation; divide the point clouds into two parts, forward and backward, and use the RANSAC algorithm to perform plane fitting to obtain plane models and normal vectors;

[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 spatial geometric model, and finally complete the complete 6DOF pose estimation to obtain the attitude transformation of the LiDAR odometer Where R L Indicates T L The rotation matrix part, t L Indicates T L The translation vector portion of .

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

[0027] 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;

[0028] S202: The original grayscale image I(x c ,y c ) uses inverse color transformation for data enhancement, through pixel inversion operation I′(x c ,y c )=255-I(x c ,y c ) generates a new enhanced image I′(x c ,y c );

[0029] S203: The original grayscale image I(x c ,y c ) and the enhanced image I′(x c ,y c )Use SuperPoint convolutional neural network to extract SuperPoint feature points;

[0030] 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 Kalman filtering for motion tracking; (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;

[0031] S205: Based on the projection transformation of step S204, the LiDAR point cloud of the dynamic target is accurately projected to 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 they are marked as dynamic feature points, and only the static feature points outside the area are retained for subsequent visual odometer calculation;

[0032] 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;

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

[0034] S208: After determining the camera position and posture, select key frames and their map points that have a co-viewing relationship with the current frame from the local map to construct a local map;

[0035] 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 .

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

[0037] S301: Use the LiDAR Camera Calibrator tool of MATLAB to 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 odometer from the camera coordinate system to the LiDAR coordinate system to obtain R V The adjoint matrix Achieve unified representation of two odometer estimates in the same coordinate system;

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

[0039] S303: Fusion of the decision-level rotation matrix, based on the adjoint matrix of the visual odometer at time k Perform state prediction; the state prediction equation is: Where k is a discrete time point, is the rotation matrix predicted from the state transfer equation; is the matrix for state transition, here 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

[0040] At time k, the LiDAR odometer's observation of the current state satisfies the following measurement equation: In the formula, is the rotation matrix in the LiDAR odometer posture 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 of is:

[0041] Calculate the Kalman gain: In the formula, represents the Kalman gain, represents the state covariance matrix; the updated state estimate is:

[0042] S304: Rotate the matrix and translation vector Combined into a complete 6-DOF pose

[0043] Further, step S4 specifically includes the following steps:

[0044] S401: Use parallel detection to run two loop detection threads, lidar and vision, at the same time;

[0045] S402: In the lidar loop detection thread, loop detection is performed based on the BoW3D algorithm using only pure static LinK3D features;

[0046] S403: In the visual loop detection thread, the loop detection mechanism of ORB-SLAM2 is adopted to perform loop detection of the visual scene based on ORB features;

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

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

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

[0050] 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.

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

[0052] (1) The present invention realizes the recognition of dynamic targets through LiDAR, and projects the recognition results into the visual image for feature point screening, and then combines the decision-level fusion algorithm to optimize the posture estimation, effectively improving the positioning accuracy in complex dynamic environments.

[0053] (2) The present invention performs instance segmentation on the LiDAR point cloud, filters out pure static point cloud information, and uses the fused posture information to construct a globally consistent pure static semantic map. Compared with traditional geometric maps, the semantic map constructed by the present invention not only effectively solves the ghost problem in dynamic environments and improves the quality of map construction, but also retains the semantic information of the scene, laying the foundation for the robot's intelligent decision-making and environmental understanding.

[0054] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0056] Figure 1 It is a flow chart of the dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion of the present invention;

[0057] Figure 2 is an example of original panoramic image data;

[0058] Figure 3 This is an example of the result of DeepLabV3 semantic segmentation processing;

[0059] Figure 4 This is an example of the semantic segmentation result of 16-line LiDAR point cloud;

[0060] Figure 5It is the recognition result of the dynamic and static attributes of the SuperPoint feature point of the present invention;

[0061] Figure 6 This is a physical picture of the UGV experimental verification platform equipped with a panoramic camera and 16-line LiDAR;

[0062] Figure 7 Comparison of the estimated trajectories of the algorithm of the present invention and the comparison algorithm on the CQU 01 dataset;

[0063] Figure 8 Comparison of the estimated trajectories of the algorithm of the present invention and the comparison algorithm on the CQU 02 dataset;

[0064] Fig. 9 It is a globally consistent map of purely static attributes established by the algorithm of the present invention on the CQU 02 dataset. DETAILED DESCRIPTION

[0065] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0066] See also Figures 1 to 9 The present invention provides a dynamic environment semantic SLAM method based on monocular vision and LiDAR fusion, which specifically includes the following steps:

[0067] Step 1: LiDAR odometer: 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 of the corresponding timestamp through the external calibration parameters of LiDAR and the image, and then perform instance segmentation on the 3D point cloud to separate unknown motion states, purely static landmark point clouds, and ground attribute point clouds. Use preliminary pose estimation and Kalman filtering to identify semi-static landmark point clouds and dynamic landmark point clouds, and then perform precise pose estimation.

[0068] Step 1 specifically includes the following steps:

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

[0070] 1-2: For 3D point clouds with semantic information, DBSCAN clustering is performed on point clouds with the same semantic labels, and instance segmentation of 3D point clouds is achieved through clustering. And based on the semantic information of instance segmentation, the instances are divided into unknown motion state landmarks, purely static landmarks, and ground attribute point clouds.

[0071] 1-3: 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 in the preliminary estimation stage; when the number of pure static landmarks is greater than the 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.

[0072] 1-4: For the landmarks selected in the k-1th frame and the kth frame, the landmark correspondence is established between the two consecutive frames through the consistency of semantic information and the landmark center of gravity distance threshold. After the paired landmarks are layered in the vertical direction, the Graham's Scan algorithm is used to construct a two-dimensional convex hull for each layer of point cloud. 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 out the most similar convex hull feature pairs. Construct an objective function with the convex hull overlap area as the optimization target, and apply the optimized particle swarm algorithm 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;

[0073] 1-5: 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 time k 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.

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

[0075] 1-7: The Kalman filter is used to estimate the state of the landmark at time k by comprehensively considering the predicted state and observed value of the landmark center. 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 the preset adaptive threshold, the landmark is marked as a semi-static landmark; otherwise, it is marked as a dynamic landmark.

[0076] 1-8: 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 to solve the problem to avoid the negative impact of dynamic landmarks. The Scan-to-Local Submap registration method is used to match the current frame with the landmarks in the local map to achieve initial pairing. Then, the horizontal pose is estimated based on the estimation method described in 1-4.

[0077] 1-9: For vertical pose estimation, extract point clouds with ground semantic information from instance segmented LiDAR data. Divide these point clouds into two parts, forward and backward, and use the RANSAC algorithm to perform plane fitting to obtain the plane model and normal vector.

[0078] 1-10: Estimate the pitch angle of the vehicle relative to the initial attitude based on the normal vector of the fitted plane. Combined with the obtained yaw angle information and horizontal position, the vehicle's position in the z direction is estimated through the established spatial geometry model, and finally the complete 6DOF pose estimation is completed to obtain the attitude transformation of the LiDAR odometer. Where R L Indicates T L The rotation matrix part, t L Indicates T L The translation vector portion of .

[0079] Step 2: Visual odometry: Perform instance segmentation and data enhancement on the 2D image, and extract SuperPoint feature points from the original image and the enhanced image. Through the calibration parameters between LiDAR and the image, the dynamic landmarks 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 between static and non-static feature points. A variety of tracking strategies (motion model, reference keyframe, SuperGlue strategy, relocalization, etc.) are used to track static features, and the local map 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 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.

[0082] 2-2: For the original grayscale image I(x c ,y c ) uses inverse color transformation for data enhancement, through pixel inversion operation I′(x c ,y c )=255-I(x c ,y c ) generates a new enhanced image I′(x c ,y c ).

[0083] 2-3: For the original grayscale image I(x c ,y c ) and the enhanced image I′(x c ,y c )Use SuperPoint convolutional neural network to extract SuperPoint feature points.

[0084] 2-4: Project the identified dynamic landmark point cloud in the LiDAR point cloud 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 steps of coordinate transformation:

[0085] (1) The conversion from LiDAR coordinate system to camera coordinate system is as follows: 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 LiDARRepresents the coordinates of the point cloud identified as a dynamic landmark through motion state tracking by Kalman filtering in the LiDAR coordinate system.

[0086] (2) Conversion from camera coordinate system to pixel coordinate system: p = K·P cam , where K is the camera intrinsic parameter matrix 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 to the camera image plane and matched with the YOLOv8 detection result. The SuperPoint feature points located in the dynamic area are filtered through the spatial correspondence relationship and marked as dynamic feature points. Only the static feature points outside the area are retained for subsequent visual odometer calculation. Figure 5 This is an example of the recognition result of the dynamic and static attributes of the SuperPoint feature point of the present invention.

[0088] 2-6: 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.

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

[0090] 2-8: After determining the camera pose, select the key frames and their map points that have a co-viewing relationship with the current frame from the local map to construct a local map.

[0091] 2-9: 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 .

[0092] Step 3: 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 LiDAR odometry are in the same coordinate system. Then, a decision-level fusion algorithm is used at the odometry level to fuse the poses.

[0093] Step 3 specifically includes the following steps:

[0094] 3-1: Use the LiDAR Camera Calibrator tool in MATLAB to calibrate the LiDAR and camera external parameters and obtain the external parameter conversion relationship between them. Then, use this external parameter to convert the estimated value of the visual odometer from the camera coordinate system to the LiDAR coordinate system to obtain R V The adjoint matrix Achieve unified representation of two odometer estimates in the same coordinate system.

[0095] 3-2: Fusion of decision-level odometer translation: Considering the scale uncertainty of monocular visual odometer, and LiDAR odometer can provide accurate absolute scale information, the translation of LiDAR odometer is directly used as the translation result after fusion, that is, the fused translation vector

[0096] 3-3: Fusion of the decision-level rotation matrix, based on the adjoint matrix of the visual odometer at time k Perform state prediction. The state prediction equation is: Where k is a discrete time point, is the rotation matrix predicted from the state transfer equation; is the matrix for state transition, here 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

[0097] At time k, the LiDAR odometer's observation of the current state satisfies the following measurement equation: In the formula, is the rotation matrix in the LiDAR odometer posture 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 of is: Calculate the Kalman gain: In the formula, represents the Kalman gain, represents the state covariance matrix. The updated state estimate is:

[0098] 3-4: Rotate the matrix and translation vector Combined into a complete 6-DOF pose

[0099] Step 4: 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.

[0100] Step 4 specifically includes the following steps:

[0101] 4-1: Use parallel detection to run two loop detection threads, lidar and vision, at the same time.

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

[0103] 4-3: In the visual loop detection thread, the loop detection mechanism of ORB-SLAM2 is adopted to perform loop detection of visual scenes based on ORB features.

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

[0105] Step 5: Map construction: Filter the pure static semantic point cloud information and combine it with the optimized posture information to build 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, the point cloud data is screened, the point clouds corresponding to dynamic objects and semi-static objects are removed, and the purely static environment information is retained.

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

[0109] Comparative experiment:

[0110] In this experiment, the method of the present invention is compared with a variety of existing SLAM algorithms (including FLOAM: WANG H, WANGC, CHEN CL, 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 In order to ensure the reliability and accuracy of the evaluation, this experiment uses the Huace CGI-610GNSS system as the ground truth (GT) reference system. Figure 6 This is a physical picture of the UGV experimental verification platform equipped with a panoramic camera, 16-line LiDAR and GNSS.

[0111] The experimental evaluation adopts both quantitative and qualitative dimensions and is verified on two datasets, CQU01 and CQU02. From the perspective of quantitative evaluation, this experiment uses three key indicators for performance measurement: the average error of the estimated position (AEEP), which is used to evaluate the average deviation between the algorithm's estimated position and the GNSS true value; the standard deviation of the estimated position error (SDEPE), which reflects the stability and consistency of the position estimate; and the position error at the end of the trajectory (PETE), which evaluates the impact of the cumulative error on the final positioning accuracy.

[0112] Table 1 Comparison of positioning accuracy between the present invention and the comparative algorithm

[0113]

[0114]

[0115] The quantitative analysis results show that, as shown in Table 1, the proposed method is significantly better than the baseline algorithm in the three evaluation indicators on the two datasets CQU01 and CQU02. Specifically, on the CQU01 dataset, the proposed method achieves an average estimated position error (AEEP) of 0.512m, a standard deviation of the estimated position error (SDEPE) of 0.229m, and a trajectory endpoint position error (PETE) of 0.754m. After further introducing the loop detection mechanism, the algorithm performance is significantly improved: AEEP is reduced to 0.341m, SDEPE is reduced to 0.153m, and PETE is optimized to 0.502m.

[0116] The qualitative evaluation results strongly verify the effectiveness of this method. In order to intuitively demonstrate the performance of the algorithm, this experiment selected representative experimental results for comparative analysis. Figure 7 and Figure 8 The estimated trajectory comparison between the proposed method and multiple classic SLAM algorithms on the CQU 01 and CQU 02 datasets is shown. The results show that the trajectory generated by the proposed algorithm maintains the highest consistency with the GNSS reference trajectory, which fully demonstrates the positioning accuracy advantage of the proposed method. In addition, the present invention further explores the ability of the proposed method to reconstruct a purely static semantic map in a complex dynamic environment. Fig. 9 The globally consistent static semantic map constructed based on the CQU02 dataset is presented. 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 this paper can effectively handle the positioning and mapping problems in dynamic scenes, and significantly improve the positioning accuracy and mapping quality of the system in complex dynamic environments. Experimental results show that this method has obvious advantages in handling dynamic targets, maintaining high positioning accuracy, and building high-quality semantic maps.

[0118] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

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 odometer: 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 of the corresponding timestamp through the external calibration parameters of LiDAR and the image, and then perform instance segmentation on the 3D point cloud to separate unknown motion states, purely static landmark point clouds, and ground attribute point clouds; identify semi-static landmark point clouds and dynamic landmark point clouds through preliminary pose estimation and Kalman filtering, and then perform precise pose estimation; S2: Visual odometry: Perform instance segmentation and data enhancement on 2D images, and extract SuperPoint feature points from the original image and enhanced image; project the dynamic landmarks identified in the LiDAR point cloud onto the corresponding image plane through the calibration parameters between LiDAR and image, and use the instance segmentation results of the image to determine the instance category to which the projected points belong, and then distinguish between static and non-static feature points; use multiple tracking strategies to track static features, and use local maps to optimize the pose; S3: Fusion strategy: transform the attitude estimated by the visual odometry into the LiDAR coordinate system so that the attitude 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 attitude at the odometry level; S4: Loop detection: Integrate the loop detection results of LiDAR SLAM and visual SLAM, and output the optimized trajectory estimation after loop correction; S5: Mapping: Filter the pure static semantic point cloud information and combine it with the optimized posture information to build a globally consistent pure static semantic map.

2. The dynamic environment semantic SLAM method according to claim 1, characterized in that: Step S1 specifically includes the following steps: S101: For the time-synchronous collection of LiDAR point cloud data and panoramic images, the DeepLabV3 model is used to perform semantic segmentation on the panoramic image and identify the semantic categories; then, through the external calibration parameters of LiDAR and the image, the semantics of the pixel coordinates in the panoramic image are mapped to the 3D point cloud coordinate system of the corresponding timestamp, and semantic information is given to each 3D point; S102: For the 3D point cloud with semantic information, DBSCAN clustering is performed on the point clouds with the same semantic label, and instance segmentation of the 3D point cloud is achieved through clustering; and the instances are divided into unknown motion state landmarks, purely static landmarks, and ground attribute point clouds according to 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 in the preliminary estimation stage; when the number of pure static landmarks is greater than the unknown motion state landmarks, only pure static landmarks are used, otherwise pure static landmarks and unknown motion state landmarks are used for preliminary posture estimation; S104: For the landmarks selected in the k-1th frame and the kth frame, a landmark correspondence relationship is established between two consecutive frames through the consistency of semantic information and the landmark center of gravity distance threshold; after the paired landmarks are layered in the vertical direction, 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 selecting the most similar convex hull feature pairs; an objective function with the convex hull overlap area as the optimization target is constructed, and an optimized particle swarm algorithm is used 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 road sign at time k, a road sign dynamic state model based on the road sign center is constructed. The state vector of the model includes the displacement vector (x, y) and the velocity vector of the road sign center. The center of the landmark in the LiDAR coordinate system at the current time k 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 transfer equation of Kalman filter based on the two-dimensional uniform turning model, which is used to predict the displacement vector (x, y) and velocity vector of the center of the road sign Status; S107: using a Kalman filter, comprehensively considering the predicted state and observed value of the center of the landmark, estimating the state of the landmark at time k; for landmarks with the same semantics, comparing the filtered estimated values ​​at time k-1 and time k; if the position change and speed change are both less than a preset adaptive threshold, marking the landmark as a semi-static landmark; otherwise, marking it as a dynamic landmark; S108: accurate pose estimation is divided into two stages: horizontal and vertical pose estimation. For horizontal pose estimation, only pure static and semi-static landmarks are used for solution. The Scan-to-Local Submap registration method is used to match the current frame with the landmarks in the local map to achieve initial pairing. Then, the horizontal pose is estimated based on the estimation method of step S104. S109: For vertical posture estimation, extract point clouds with ground semantic information from the LiDAR data after instance segmentation; divide the point clouds into two parts, forward and backward, and use the RANSAC algorithm to perform plane fitting to obtain plane models and normal vectors; 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 spatial geometric model, and finally complete the complete 6DOF pose estimation to obtain 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, characterized in that: 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 for data enhancement, through 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 the 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 Kalman filtering for motion tracking; (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 of step S204, the LiDAR point cloud of the dynamic target is accurately projected to 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 they are marked as dynamic feature points, and only the static feature points outside the area are retained for subsequent visual odometer 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 tracking strategy stage of feature points, the system adopts a three-level tracking strategy: first, the constant velocity model is used to predict the pose; if it fails, reference keyframe tracking is used; then the SuperGlue-based tracking method is used to solve the pose; when tracking is lost, repositioning is performed in the keyframe database through the bag-of-words model; S208: After determining the camera position and posture, select key frames and their map points that have a co-viewing 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, characterized in that: Step S3 specifically includes the following steps: S301: Calibrate the external parameters of LiDAR and camera to obtain the external parameter conversion relationship between them; then, use this external parameter to convert the estimated value of the visual odometer from the camera coordinate system to the LiDAR coordinate system to obtain R V The adjoint matrix R C , achieving unified representation of two odometer estimates in the same coordinate system; S302: For the fusion of the decision-level odometer translation, the translation of the LiDAR odometer is directly used as the fused translation result, that is, the fused translation vector S303: Fusion of the decision-level rotation matrix, based on the adjoint matrix of the visual odometer at time k Perform state prediction; the state prediction equation is: Where k is a discrete time point, is the rotation matrix predicted from the state transfer equation; is the matrix for state transition, here 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 odometer's observation of the current state satisfies the following measurement equation: In the formula, is the rotation matrix in the LiDAR odometer posture 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 of is: Calculate the Kalman gain: In the formula, 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, characterized in that: Step S4 specifically includes the following steps: S401: Use parallel detection to run two loop detection threads, lidar and vision, at the same time; 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 adopted to perform loop detection of the visual scene based on the ORB feature; 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, characterized in that: Step S5 specifically includes the following steps: S501: Based on the result of semantic segmentation, the point cloud data is screened, point clouds corresponding to dynamic objects and semi-static objects are removed, and pure 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