Multi-sensor fusion SLAM method suitable for dynamic rain and fog environment

Abstract

The invention discloses a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment. A point cloud preprocessing module, an environment judgment and decision module, a data association module, an odometer module and an optimization module are included. The point cloud preprocessing module removes multipath false points, outlier noisy points and dynamic targets and estimates the radar self-speed; the environment judgment and decision module judges environment conditions and makes corresponding decisions; the data association module associates the 4D millimeter wave radar point cloud with the image feature information; the odometer module runs a self-speed pre-integral, a 4D millimeter-wave radar odometer and a visual odometer in parallel; and the optimization module constructs loopback factors through loopback detection, performs factor graph optimization in combination with each constraint factor, and outputs an accurate pose and a 3D point cloud map. According to the invention, data of multiple sensors such as the 4D millimeter wave radar, the IMU and the camera are fused, the problem that the traditional SLAM is limited in the rain and fog environment is solved, and the method is suitable for accurate positioning and 3D mapping of an intelligent automobile or robot in the rain, fog, smoke and other environments.

Description

Technical Field

[0001] The present invention relates to the technical field of timely positioning and map construction of robots or intelligent vehicles, and more specifically, to a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments. Background Art

[0002] In recent years, the demand for complex environment perception technology in the SLAM field has been increasing. This is mainly due to the need for special robots to accurately understand and adapt to complex and changing environments when performing high-risk and difficult tasks, and the need for smart cars to achieve accurate environmental perception in bad weather. In harsh environments, such as rain, fog, and dust, traditional perception systems based on lidar or vision will be greatly limited. 4D millimeter-wave radar is a new type of sensor that has only begun to be recognized by people in recent years. It has powerful environmental perception capabilities and has robust detection effects even in extreme weather. It is suitable for robots to perceive the environment in complex scenes. 4D millimeter-wave radar can provide a variety of information such as distance, azimuth, elevation, and speed. Due to its unique advantages, it is currently attracting much attention in the field of autonomous driving. Compared with lidar, 4D millimeter-wave radar has stronger penetration and still performs well in harsh environments. Compared with traditional millimeter-wave radar, 4D millimeter-wave radar provides more dimensional information, as well as a larger range and field of view.

[0003] In the current SLAM field, LiDAR and vision are still the dominant factors, and there are very few SLAM methods based on the fusion of 4D millimeter-wave radar and visual inertial navigation. In the face of severe weather such as rain, snow, haze, and dust, SLAM methods based on LiDAR are often strongly interfered and their perception capabilities are greatly reduced. In the face of degraded scenes such as insufficient features and weak textures, SLAM methods based on visual sensors often cannot effectively extract feature information, resulting in inaccurate positioning and ineffective mapping. The multi-sensor fusion SLAM method based on 4D millimeter-wave radar and visual inertial navigation can utilize the strong detectability of 4D millimeter-wave radar and the data complementarity between multiple sensors such as vision, and can be effectively applied to robots or smart cars for precise positioning and 3D mapping in rainy and foggy environments. Summary of the invention

[0004] In order to deal with the environmental perception problem in the above-mentioned harsh scenes, the present invention provides a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments, comprising a point cloud preprocessing module (1), an environmental judgment and decision module (2), a data association module (3), an odometer module (4), and an optimization module (5);

[0005] The point cloud preprocessing module (1) comprises:

[0006] (1.1) The adaptive DBSCAN multidimensional clustering denoising algorithm combines the spatial density and RCS features of the point cloud to dynamically adjust the threshold and remove multipath false points and outlier noise points in the 4D millimeter wave radar point cloud;

[0007] (1.2) Combine IMU pre-integration to screen out dynamic targets;

[0008] (1.3) Estimate radar frame self-velocity using the spatial distribution and radial velocity of static targets;

[0009] The environment judgment and decision module (2) comprises: judging the current environment condition according to the image features, making image processing decisions and visual odometer switch decisions, so as to avoid excessive errors introduced by the visual odometer in a strong rain and fog environment;

[0010] The data association module (3) comprises: associating the 4D millimeter wave radar key frame point cloud with the image key frame feature points, giving the image key frame feature points depth information and projecting them into 3D points;

[0011] The odometer module (4) comprises: running a self-speed pre-integration, a 4D millimeter wave radar odometer and a visual odometer in parallel, and constructing a self-speed pre-integration factor, a 4D millimeter wave radar odometer factor and a visual odometer factor respectively;

[0012] The optimization module (5) includes: detecting the loop status of the visual odometer and constructing a loop factor; combining the loop factor, the various factors of the odometer module (4) and the radar key frame state node to construct a sliding window factor graph, considering the residuals of various factors and solving the optimal posture and outputting a point cloud map.

[0013] Optionally, the point cloud preprocessing module (1) (1.1) combines the point cloud spatial density and RCS features to remove multipath false points and outlier noise points in the 4D millimeter wave radar point cloud, comprising the following steps:

[0014] S1: Input radar point cloud frame and count all target points in the current radar point cloud frame;

[0015] S2: Construct a four-dimensional vector containing spatial position information and RCS information of each target point;

[0016] S3: Determine the minimum number of neighborhood samples MinPts and parameter K value in the DBSCAN algorithm parameters through the dimension of statistical information;

[0017] S4: Count the Mahalanobis distances between each target point and all other target points, and the Mahalanobis distances between each target and its K nearest neighbor targets;

[0018] S5: Use the maximum likelihood estimation method to estimate the Poisson distribution parameters of all target K distances, calculate the expected value, and thus determine the neighborhood distance e value;

[0019] S6: Substitute the minimum number of neighborhood samples MinPts described in step S3 and the neighborhood distance e value described in step S4 into the DBSCAN algorithm, filter the input current radar point cloud frame, and remove multipath false points and outlier noise points.

[0020] Optionally, the dynamic target screening in (1.2) of the point cloud preprocessing module (1) comprises the following steps:

[0021] a: Combine IMU measurement data and motion model to obtain preliminary radar self-velocity, and solve the radar self-velocity components in all target directions;

[0022] b: Calculate the absolute radial velocity of all targets by combining the radar self-velocity component described in step a and the relative radial velocity of the target measured by the 4D millimeter wave radar;

[0023] c: Set the speed threshold based on the preliminary radar self-speed described in step a;

[0024] d: Combining the absolute radial speeds of all targets described in step b and the speed threshold set in step c, all target points are divided into dynamic targets and static targets, dynamic targets are removed and static targets are retained.

[0025] Optionally, the environment judgment and decision module (2) comprises the following steps:

[0026] A: Input continuous image frames;

[0027] B: Extract the channel minimum value of all pixels in each image, construct a grayscale image and divide it into grids;

[0028] C: Count the pixels with larger grayscale values ​​in each grid of the grayscale image and calculate the density of the grids they occupy; calculate the mean value of the grid density, and filter out grids with density significantly lower than the mean, and count the number of remaining grids;

[0029] D: Classify the image according to the number of remaining grids. If the current image is classified as normal, jump to the next image processing, otherwise jump to step E;

[0030] E: Use guided filtering to quickly remove haze from the image;

[0031] F: Perform step B and step C again on the dehazed image;

[0032] G: Classify the current image as a weak rain / fog / smoke image or a strong rain / fog / smoke image according to the number of remaining grids outputted in step F;

[0033] H: After all consecutive image frames are classified, the image classification results are counted to determine the current environmental conditions. When the environmental conditions are judged to be normal or weak rain / fog / smoke, the visual odometry is kept on; when the environmental conditions are judged to be strong rain / fog / smoke, the visual odometry is turned off to avoid introducing too many errors.

[0034] Optionally, the data association module (3) comprises the following steps:

[0035] Step 1: Align the radar keyframe and image frame timestamps, select the image keyframe and extract ORB features;

[0036] Step 2: Input image keyframes and set image feature thresholds;

[0037] Step 3: Judge according to the threshold. When the number of features does not meet the threshold, the current image key frame processing will be terminated, the next image key frame will be selected and the process will be skipped to step 2. When the number of features meets the threshold, the process will be skipped to step 4.

[0038] Step 4: Align the current image keyframe and the radar keyframe coordinate system to the same coordinate system;

[0039] Step 5: Project the image feature points and radar point cloud to the unit circle and store them in the kd-tree;

[0040] Step 6: Search the nearest neighbor point cloud of each feature point and read the distance information from each point cloud to the radar coordinate origin, and calculate the average;

[0041] Step 7: Assign the obtained mean value as the depth value to the corresponding ORB feature point, and project the ORB feature point into a 3D point.

[0042] Optionally, the odometer module (4) performs a self-velocity pre-integration:

[0043] Combined with the radar self-velocity obtained in (1.3) of the point cloud preprocessing module (1), the relative pose transformation dynamically updated between radar key frames is calculated:

[0044]

[0045] Where i and j represent adjacent radar key frames, △t is the time interval between radar point cloud frames between radar key frames, and the radar point cloud frame self-velocity vector is is the debiased angular acceleration measured by the IMU, η ω ,η vRepresents IMU noise, k represents the kth IMU frame or radar point cloud frame between the i-th frame and the j-th radar key frame; by combining with the IMU noise model in this way, the self-velocity pre-integration factor will be provided for the factor graph optimization in the optimization module.

[0046] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0047] (1) The present invention proposes a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments. It innovatively combines the advantages of high detection capability and strong penetration of 4D millimeter-wave radar, as well as the data complementarity between 4D millimeter-wave radar, IMU and camera. It addresses the problems of poor positioning accuracy and difficulty in mapping of traditional SLAM methods in harsh environments such as rain, fog, smoke and dust, and solves the needs of high-precision positioning and mapping of robots or smart cars in special environments.

[0048] (2) The present invention proposes a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments. A point cloud processing flow is designed for 4D millimeter-wave radar point clouds in complex scenes. By combining the multi-dimensional clustering denoising method of RCS features and dynamic target screening, the problem of many outlier noise points in 4D millimeter-wave radar point clouds and difficulty in removing them is solved, and the introduction of errors is avoided. At the same time, the dynamic point cloud is removed and the clean static point cloud is retained, reducing the errors in pose estimation and mapping.

[0049] (3) The present invention proposes a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments, and designs a visual-based environment judgment and decision-making module to analyze whether the current environment is a normal environment, a weak rain and fog environment, or a strong rain and fog environment, and make image processing and visual odometer switch decisions for various environments. This design can effectively utilize visual information in normal environments and weak rain and fog environments to supplement the 4D millimeter wave radar perception environment, and can avoid introducing too many errors when the visual ability is limited in a strong rain and fog environment.

[0050] (4) The present invention proposes a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments. It targets the speed measurement characteristics of 4D millimeter-wave radar and estimates the radar self-velocity. The estimated radar self-velocity is introduced in the IMU pre-integration process to reduce the complexity of IMU pre-integration. At the same time, the radar self-velocity is introduced as a state quantity in the back-end graph optimization process to improve the back-end graph optimization accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 The present invention is a system block diagram of a multi-sensor fusion SLAM method suitable for dynamic rain and fog environments.

[0052] Figure 2It is a strategy diagram of an adaptive DBSCAN multi-dimensional clustering algorithm combining point cloud spatial density and RCS features in a point cloud preprocessing module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment of the present invention.

[0053] Figure 3 It is a strategy diagram of an environment judgment and decision-making module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment of the present invention.

[0054] Figure 4 The present invention discloses a strategy diagram of a data association module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment.

[0055] Figure 5 The invention discloses a sliding window factor graph of an optimization module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment. DETAILED DESCRIPTION

[0056] In order to make the purpose, advantages and technical solutions of the present invention clearer and more specific, further detailed description is given below in conjunction with specific embodiments of the present invention and corresponding drawings.

[0057] Figure 1 This is a general block diagram of a multi-sensor fusion SLAM method suitable for dynamic rain and fog environment of the present invention. The sensors used include 4D millimeter wave radar, IMU, and monocular camera. The modules include point cloud preprocessing module, environment judgment and decision module, data association module, odometer module, and optimization module. The point cloud preprocessing module includes the following steps:

[0058] S1: Removal of multipath false points and outlier noise points: According to the distribution of random outlier noise points and multipath false points in the 4D millimeter wave radar point cloud in the scene, the adaptive DBSCAN clustering algorithm combining position and RCS features dynamically adjusts the threshold to divide the inner and outer points, and removes outlier noise points and multipath false points;

[0059] S2: Dynamic target screening: According to the initial value of radar self-velocity provided by IMU pre-integration and the radial velocity of each point cloud detected by radar, the absolute velocity of each point cloud is solved by combining the spatial distribution of the point cloud in the radar coordinate system, and the velocity threshold is set to filter out dynamic targets;

[0060] S3: Self-velocity estimation: Based on the radial velocity vector of the retained static point cloud, an equation is constructed by combining the spatial position relationship between the origin of the radar coordinate system and the static point cloud to solve the radar self-velocity.

[0061] Figure 2It is a strategy diagram of an adaptive DBSCAN multidimensional clustering algorithm combining point cloud spatial density and RCS features in a point cloud preprocessing module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment of the present invention, that is, the content of step S1, and the specific implementation method is as follows:

[0062] S11: input radar point cloud frame and count all target points in the current radar point cloud frame;

[0063] S12: Construct a four-dimensional vector containing the spatial position information and RCS information of each target point, expressed as

[0064]

[0065] in Represent the x, y, z, and rcs dimension values ​​of each target point respectively;

[0066] S13: Based on the dimension of statistical information, the minimum number of neighborhood samples MinPts and the parameter K value in the DBSCAN algorithm parameters are determined in the following way:

[0067]

[0068] S14: The Mahalanobis distances between each target and all other targets, and the Mahalanobis distances between each target and the K nearest targets are calculated as follows:

[0069]

[0070] Where S is the covariance matrix, X i ,X j The vectors representing the i-th and j-th point clouds;

[0071] S15: Use the maximum likelihood estimation method to estimate the parameters of the Poisson distribution of all k distances, find the expected value, and thus determine the neighborhood distance e value:

[0072]

[0073] S16: Substitute the minimum number of neighborhood samples MinPts and the neighborhood distance e value determined above into the DBSCAN algorithm, filter the input current radar point cloud frame, and remove multipath false points and outlier noise points.

[0074] As a preferred embodiment of the present invention, the step S2 of dynamic target screening in the point cloud preprocessing module is specifically implemented as follows:

[0075] S21: Get the initial radar self-velocity v by pre-integrating the IMU measurement data c , the relationship between the radial velocity of the point cloud is constructed as

[0076]

[0077] where v cx , v cy , v cz They are v c The velocity components in the x, y, and z directions, It represents the radial velocity of the radar relative to the target point, and r is the radial distance from the target point to the radar;

[0078] S22: Relative radial velocity of the target point measured by 4D millimeter wave data The absolute radial velocity of the target point is obtained by calculation

[0079]

[0080] S23: Combine the absolute radial velocity obtained in step S22 Set the speed threshold to

[0081]

[0082] S24: Based on the absolute radial velocity of the target point The speed threshold set in step S23 divides all target points into dynamic points and static points, and the division method is as follows:

[0083]

[0084] Finally, the dynamic points are removed and only the static points are retained.

[0085] As a preferred embodiment of the present invention, the step S3 of the point cloud preprocessing is self-velocity estimation, and the specific implementation is as follows:

[0086] S31: Construct the motion equation of the point cloud radial velocity and the radar self-velocity as follows:

[0087]

[0088] where v r is the radar velocity, v i Target point radial velocity, P i Target point coordinates;

[0089] S32: Through the selected n point clouds, the above equation is constructed as

[0090]

[0091] Where V pis the matrix constructed by the velocity vectors of n point clouds, and H is the matrix constructed by the unit direction vector from the radar coordinate origin to the target point;

[0092] S33: Solve the above equation using the least squares method:

[0093] v r =(H T H) -1 H T V p (11) By solving this equation, we can get the estimated radar self-velocity v r .

[0094] Figure 3 It is a strategy diagram of an environment judgment and decision module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment of the present invention, wherein the environment judgment and decision module specifically comprises the following steps:

[0095] A: input continuous image frames (in this embodiment, 5 continuous image frames are selected);

[0096] B: Extract the minimum value of each image channel to construct a grayscale image, and divide it into a grid (in this embodiment, the image resolution is 720×640 and the image is evenly divided into 9×8 grids);

[0097] C: Count the pixel blocks with grayscale values ​​greater than 10 in each grid of the grayscale image, and calculate their density in the grid, then calculate the mean of the grid density, and filter out the grids with density lower than 25% of the mean in the current grayscale image, and count the number of remaining grids;

[0098] D: Perform image judgment. If the number of remaining grids is greater than the threshold (50% of the total number of grids is selected in this embodiment), jump to step E; if the number of remaining grids is less than the threshold, classify the current image as normal and jump to the next image processing.

[0099] E: Use guided filtering to quickly remove haze from the image;

[0100] F: Perform step B and step C on the dehazed image;

[0101] G: The grayscale image outputted in step F is judged again. If the number of remaining grids is greater than the threshold, the current image is classified as weak rain / fog / smoke; if the number of remaining grids is greater than the threshold, the current image is classified as strong rain / fog / smoke.

[0102] H: After all continuous image frames are processed, the image frame classification is counted and the environment is judged. When the number of normal image frames is greater than or equal to the threshold (3 image frames are selected in this embodiment), the current environment is judged to be a normal environment and the visual odometer is kept turned on; if the number of weak rain / fog / smoke image frames is greater than or equal to the threshold, the current environment is judged to be a weak rain / fog / smoke environment and the visual odometer is kept turned on; if the number of strong rain / fog / smoke image frames is greater than or equal to the threshold, the current environment is judged to be a strong rain / fog / smoke environment and the visual odometer is turned off.

[0103] Figure 4 It is a strategy diagram of a data association module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment of the present invention, which specifically includes the following steps:

[0104] a: the image key frame determined in step b during input image preprocessing;

[0105] b: Set the image feature count threshold to 0.15% of the total number of image pixels;

[0106] c: Judge according to the threshold. If the number of feature points in a certain image frame is less than the threshold, the next image frame is processed. If the number of feature points is greater than the threshold, the subsequent processing flow continues.

[0107] d: Align the timestamp and coordinate system of the current image frame and the radar point cloud frame;

[0108] e: Project the image ORB feature points and radar point cloud to the unit circle and store them in the kd-tree;

[0109] f: Search the three nearest point clouds of each ORB feature point and read the depth information (the distance from the point cloud to the radar coordinate origin) to obtain the mean depth;

[0110] g: Assign the calculated depth mean to the corresponding ORB feature point and project the ORB feature point into a 3D point.

[0111] As a preferred embodiment of the present invention, the self-velocity pre-integration in the odometer module is provided by introducing the radar self-velocity vector obtained by point cloud preprocessing into the IMU pre-integration to provide self-velocity pre-integration constraints for the factor graph optimization of the optimization module. The specific implementation is as follows:

[0112] For the adjacent i-th and j-th radar key frames, the pose of the j-th radar frame is estimated by combining the IMU motion model and the self-velocity vector of the radar point cloud frame separated by time between the key frames. get:

[0113]

[0114] in is the angular acceleration measured by IMU, η ω ,η v represents IMU noise, and k represents the kth radar point cloud frame between the i-th frame and the j-th radar key frame. The relative pose transformation between the i-th frame and the j-th radar key frame is calculated by the following formula:

[0115]

[0116] By combining this method with the IMU noise model, the inter-frame self-velocity pre-integration constraint of the radar key frame is obtained, and the self-velocity pre-integration factor is provided for the factor graph optimization in the optimization module.

[0117] As a preferred embodiment of the present invention, the radar odometer of the odometer module adopts the normal distribution (NDT) registration method to estimate the pose transformation between key frames of the radar point cloud. The specific implementation is as follows:

[0118] A dense subgraph is constructed from radar point cloud keyframes using a sliding window method. The sliding window size is selected as 10 frames. If the overlap rate from the latest point cloud frame to the current keyframe is less than a threshold (90% is selected in this embodiment), the current frame is selected as a new keyframe, the new keyframe is added to the subgraph, and the nodes associated with it will be added to the pose graph of the backend. When the number of keyframes in the subgraph exceeds the window size, the oldest keyframe will be removed from the sliding window. The subgraph is divided into grid cells, and the radar points in each cell are modeled as a local normal distribution. When calculating the mean and covariance of the normal distribution, the probability density function of each point is included according to its measurement uncertainty, the latest 4D radar point cloud is aligned with the radar subgraph, and the relative pose transformation is calculated by minimizing the NDT cost function.

[0119] As a preferred embodiment of the present invention, the specific implementation of the visual odometer in the odometer module is as follows:

[0120] The feature matching relationship between the image key frames is used to simultaneously associate the 3D point cloud frame matching, and the objective function is constructed through nonlinear optimization. Multiple iterations are performed to minimize the objective function, thereby estimating the relative pose transformation between the two key frame point clouds. The objective function is constructed in the following way:

[0121]

[0122] Figure 5The invention discloses a sliding window factor graph of an optimization module of a multi-sensor fusion SLAM method suitable for a dynamic rain and fog environment, wherein the constraints include a radar odometer factor, a visual odometer factor, a self-velocity pre-integration factor, and a loopback factor. Specifically, a sliding window factor graph containing 20 pose nodes is constructed with a fixed window, and as a new radar keyframe is inserted and a new constraint factor is introduced, the old radar keyframe is marginalized by a fixed lag smoothing method. The latest radar keyframe and the edge frame are constrained by a unary edge, while other keyframes are constrained by a binary edge including a radar odometer factor, a visual odometer factor, a self-velocity pre-integration factor, and a loopback factor. The radar keyframe is managed by a keyframe selection mechanism, that is, the radar keyframe selection method in the radar odometer; the visual odometer is managed by a feature point threshold (0.15% of the total number of image pixels), and when the feature point number of a frame is less than the threshold, the current frame is judged to be a visually degraded image frame, and the corresponding visual odometer factor is discarded to avoid error introduction; the loopback factor is managed by a bag of words model, and after confirming the loopback, the loopback factor is added to the backend factor graph to perform a global optimization. The radar self-velocity obtained by point cloud preprocessing in the odometer module is introduced into the factor graph optimization as an additional state quantity of each node. The marginalized radar keyframes will be used to construct a 3D point cloud map, and finally a global 3D point cloud map of the environment and an optimized accurate pose will be obtained.

[0123] The above description is only a preferred embodiment of the present invention, so that those skilled in the art can understand the present invention. However, it should be clear that this is not intended to limit the present invention. All other embodiments obtained by those of ordinary skill in the art without making any creative work, any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention, should fall within the scope of protection of the present invention.

Claims

1. A multi-sensor fusion SLAM method suitable for dynamic rain and fog environment, characterized in that: It includes a point cloud preprocessing module (1), an environment judgment and decision module (2), a data association module (3), an odometer module (4), and an optimization module (5); The point cloud preprocessing module (1) comprises: (1.1) The adaptive DBSCAN multidimensional clustering denoising algorithm combines the spatial density and RCS features of the point cloud to dynamically adjust the threshold and remove multipath false points and outlier noise points in the 4D millimeter wave radar point cloud; (1.2) Combine IMU motion model to filter out dynamic targets; (1.3) Estimate radar frame self-velocity using the spatial distribution and radial velocity of static targets; The environment judgment and decision module (2) includes: judging the current environment conditions according to the image features, making image defogging processing and visual odometer switch decisions, and avoiding excessive errors introduced by the visual odometer in a heavy rain / fog / smoke environment; The data association module (3) comprises: associating the 4D millimeter wave radar key frame point cloud with the image key frame feature points, giving the image key frame feature points depth information and projecting them into 3D points; The odometer module (4) comprises: running a self-speed pre-integration, a 4D millimeter wave radar odometer and a visual odometer in parallel, and constructing a self-speed pre-integration factor, a 4D millimeter wave radar odometer factor and a visual odometer factor respectively; The optimization module (5) includes: detecting the loop status of the visual odometer and constructing a loop factor; combining the loop factor, the various factors of the odometer module (4) and the radar key frame state node to construct a sliding window factor graph, considering the residuals of various factors and solving the optimal posture and outputting a point cloud map.

2. A multi-sensor fusion SLAM method suitable for dynamic rain and fog environment according to claim 1, characterized in that: The point cloud preprocessing module (1) (1.1) combines the point cloud spatial density and RCS features to remove multipath false points and outlier noise points of the 4D millimeter wave radar point cloud, and comprises the following steps: S1: Input radar point cloud frame and count all target points in the current radar point cloud frame; S2: Construct a four-dimensional vector containing spatial position information and RCS information of each target point; S3: Determine the minimum number of neighborhood samples MinPts and parameter K value in the DBSCAN algorithm parameters through the dimension of statistical information; S4: Count the Mahalanobis distances between each target point and all other target points, and the Mahalanobis distances between each target and its K nearest neighbor targets; S5: Use the maximum likelihood estimation method to estimate the Poisson distribution parameters of all target K distances, calculate the expected value, and thus determine the neighborhood distance e value; S6: Substitute the minimum number of neighborhood samples MinPts described in step S3 and the neighborhood distance e value described in step S4 into the DBSCAN algorithm, filter the input current radar point cloud frame, and remove multipath false points and outlier noise points.

3. A multi-sensor fusion SLAM method suitable for dynamic rain and fog environment according to claim 1, characterized in that: The dynamic target screening (1.2) in the point cloud preprocessing module (1) comprises the following steps: a: Combine IMU measurement data and motion model to obtain preliminary radar self-velocity, and solve the radar self-velocity components in all target directions; b: Calculate the absolute radial velocity of all targets by combining the radar self-velocity component described in step a and the relative radial velocity of the target measured by the 4D millimeter wave radar; c: Set the speed threshold based on the preliminary radar self-speed described in step a; d: Combining the absolute radial speeds of all targets described in step b and the speed threshold set in step c, all target points are divided into dynamic targets and static targets, dynamic targets are removed and static targets are retained.

4. The multi-sensor fusion SLAM method suitable for dynamic rain and fog environment according to claim 1, characterized in that: The environmental judgment and decision module (2) comprises the following steps: A: Input continuous image frames; B: Extract the channel minimum value of all pixels in each image, construct a grayscale image and divide it into grids; C: Count the pixels with larger grayscale values ​​in each grid of the grayscale image and calculate the density of the grids they occupy; calculate the mean value of the grid density, and filter out grids with density significantly lower than the mean, and count the number of remaining grids; D: Classify the image according to the number of remaining grids. If the current image is classified as normal, jump to the next image processing, otherwise jump to step E; E: Use guided filtering to quickly remove haze from the image; F: Perform step B and step C again on the dehazed image; G: Classify the current image as a weak rain / fog / smoke image or a strong rain / fog / smoke image according to the number of remaining grids outputted in step F; H: After all consecutive image frames are classified, the image classification results are counted to determine the current environmental conditions. When the environmental conditions are judged to be normal or weak rain / fog / smoke, the visual odometry is kept on; when the environmental conditions are judged to be strong rain / fog / smoke, the visual odometry is turned off to avoid introducing too many errors.

5. The multi-sensor fusion SLAM method suitable for dynamic rain and fog environment according to claim 1, characterized in that: The data association module (3) comprises the following steps: Step 1: Align the radar keyframe and image frame timestamps, select the image keyframe and extract ORB features; Step 2: Input image keyframes and set image feature thresholds; Step 3: Make a judgment based on the threshold. When the number of features does not meet the threshold, the current image key frame processing will be terminated, the next image key frame will be selected and jump to step 2; When the number of features meets the threshold, jump to step 4; Step 4: Align the current image keyframe and the radar keyframe coordinate system to the same coordinate system; Step 5: Project the image feature points and radar point cloud to the unit circle and store them in the kd-tree; Step 6: Search the nearest neighbor point cloud of each feature point and read the distance information from each point cloud to the radar coordinate origin, and calculate the average; Step 7: Assign the obtained mean value as the depth value to the corresponding ORB feature point, and project the ORB feature point into a 3D point.

6. The multi-sensor fusion SLAM method suitable for dynamic rain and fog environment according to claim 1, characterized in that: The odometer module (4) is self-speed pre-integrated: Combined with the radar self-velocity obtained in (1.3) of the point cloud preprocessing module (1), the relative pose transformation dynamically updated between radar key frames is calculated: Where i and j represent adjacent radar key frames, △t is the time interval between radar point cloud frames between radar key frames, and the radar point cloud frame self-velocity vector is is the debiased angular acceleration measured by the IMU, η ω ,η v Represents IMU noise, k represents the kth IMU frame or radar point cloud frame between the i-th frame and the j-th radar key frame; by combining with the IMU noise model in this way, the self-velocity pre-integration factor will be provided for the factor graph optimization in the optimization module.

Images