A rain day vehicle speed and vehicle distance decision method based on environment perception

Abstract

This invention discloses a method for determining vehicle speed and distance in rainy weather based on environmental perception, belonging to the field of autonomous driving technology. It addresses the problems of poor traffic safety and low traffic efficiency inherent in existing methods during heavy rainfall. This invention calculates safe vehicle speed and distance based on a physical model and real-time estimated environmental parameters, providing a quantitative scientific basis for vehicle control that conforms to the dynamic limits of rainy weather. This significantly reduces the risk of accidents and, while ensuring vehicle safety, adjusts vehicle speed according to the current environmental conditions, thereby ensuring traffic efficiency. A degradation strategy driven by internal performance indicators is designed to dynamically adjust the fusion strategy based on sensor health status, effectively combating the complex environment of dynamically changing rainfall intensity and ensuring that the core functions of the system do not collapse when some sensors temporarily fail due to extreme weather. This method can be applied to speed and distance decisions in autonomous driving systems during rainy weather.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to a method for determining vehicle speed and distance in rainy weather based on environmental perception. Background Technology

[0002] The proper functioning of high-level autonomous driving systems heavily relies on their accurate perception of the surrounding environment and precise vehicle self-localization. Currently, the mainstream solution is multi-sensor fusion, which typically combines LiDAR, cameras, and inertial measurement units for accurate environmental perception and self-localization. Under clear weather conditions and ideal sensor operating conditions, centimeter-level positioning accuracy and reliable environmental perception can be achieved through tightly coupled or loosely coupled algorithms.

[0003] However, sensor performance is severely compromised during rainfall, especially heavy rainfall. For cameras, raindrops and water films adhering to the lens cause image blurring, reduced contrast, and introduce non-uniform distortion, significantly impacting vision-based feature extraction and matching. For lidar, the emitted laser beam experiences severe scattering and attenuation when penetrating rain, resulting in numerous random noise points in the point cloud formed by raindrop reflections, significantly shortening the effective detection range and sparsening the point cloud. This weather-induced interference is dynamic, unstructured, and varies in intensity, posing a significant challenge to traditional multi-sensor fusion algorithms. Loosely coupled methods with fixed weights cannot reflect the dramatic fluctuations in the reliability of each sensor in real time, easily leading to divergent fusion results; while tightly coupled optimization is prone to getting trapped in local optima when faced with a large number of abnormal observations (noise), producing incorrect pose estimations. Therefore, in heavy rainfall, the non-uniform degradation of sensor performance leads to inaccurate multi-sensor fusion and failure of environmental perception. Autonomous driving systems relying on conventional sensors for environmental perception and self-localization suffer from poor traffic safety and low traffic efficiency.

[0004] In existing technologies, some solutions attempt to introduce additional dedicated weather sensors to directly measure environmental parameters and statically adjust the perception algorithm accordingly. This approach not only increases the hardware cost and complexity of the system, but more importantly, the relationship between the "weather parameters" measured by external sensors and the "perception interference" experienced by cameras and LiDAR is not a simple linear one. Static adjustments often have limited effectiveness and cannot cope with complex situations such as localized, sudden heavy rainfall. Therefore, how to build a method that enables internal self-evaluation, adaptive fusion, and intelligent decision-making without adding additional dedicated sensors—that is, utilizing the vehicle's standard perception suite (IMU, LiDAR, camera) combined with algorithmic improvements—to maintain the continuity and safety of core functions (i.e., system "resilience") under extreme rainfall conditions, has become a key technological bottleneck that urgently needs to be overcome for the current deployment of autonomous driving. Summary of the Invention

[0005] The purpose of this invention is to address the problems of poor traffic safety and low traffic efficiency in existing methods during heavy rainfall. It proposes a rain-based vehicle speed and distance decision-making method based on environmental perception to achieve high-precision positioning, accurate environmental perception, and intelligent optimization of vehicle speed and distance when driving in rainy weather, thereby improving the safety and efficiency of vehicle driving and enhancing the resilience of the road traffic system.

[0006] The technical solution adopted by the present invention to solve the above-mentioned technical problems is: a method for determining vehicle speed and distance in rainy weather based on environmental perception, the method specifically including the following steps:

[0007] Step 1: Collect data synchronously using the IMU, LiDAR and camera deployed on the autonomous vehicle, and then process the collected current frame image data and point cloud data to obtain the processed current frame image data and point cloud data.

[0008] Step 2: Calculate the visual interference factor of the processed current frame image, perform distortion correction on the processed current frame image based on the visual interference factor, and then process the distortion-corrected current frame image to obtain the enhanced current frame image.

[0009] And perform FAST corner detection on the current frame enhanced image to obtain the feature points in the current frame enhanced image;

[0010] Step 3: Calculate the relative rotation between the previous frame augmented image and the current frame augmented image based on the IMU measurement values, and then predict the position of the feature points in the previous frame augmented image in the current frame augmented image based on the relative rotation between the previous frame augmented image and the current frame augmented image.

[0011] Then, a fine match is performed based on the prediction results, and the fine match results are subjected to distortion removal and normalization to obtain the normalized result.

[0012] Step 4: Add the enhanced image of the current frame to the keyframe window, and establish a residual function based on the normalization result in Step 3. Then, solve the established residual function to obtain the visual inertial pose of the current frame. And calculate visual confidence. ;

[0013] Step 5: Perform motion compensation on the processed point cloud data based on IMU measurement data;

[0014] Step 6: Calculate the lidar rainfall interference factor based on the motion-compensated point cloud data of the current frame. The motion-compensated point cloud data is then filtered and denoised to obtain the filtered and denoised point cloud data.

[0015] The edge point set and planar point set of the current frame are extracted from the filtered and denoised point cloud data, and the inertial pose of the lidar is estimated based on the edge point set and planar point set of the current frame. LiDAR matching error ;

[0016] Step 7: Visual inertial pose of the current frame and lidar inertial pose Adaptive fusion processing is performed to obtain the fused pose. ;

[0017] Step 8: Utilize the fused pose The filtered and denoised point cloud data of the current frame is transformed into the world coordinate system, and then the transformed point cloud is divided into different clusters by the clustering algorithm DBSCAN, where each cluster of point cloud represents an obstacle.

[0018] The point cloud of static obstacles is incrementally fused into the global map. Then, the SORT multi-object tracking algorithm is used to track the trajectory of each dynamic obstacle, outputting a list of dynamic obstacles. The positions of the dynamic obstacles are marked as temporarily occupied in the constructed static environment map, resulting in an updated static environment map. ;

[0019] Step 9: Based on visual interference factors LiDAR interference factor Visual confidence Matching error with lidar An input vector is constructed and used as the input to a two-layer fully connected neural network. The equivalent rainfall intensity is then output through the two-layer fully connected neural network. ;

[0020] Based on the static environment map Determine the sensing line of sight and calculate the optimal safe speed based on the sensing line of sight and the equivalent rainfall intensity;

[0021] Step 10: Based on the optimal safe speed Calculate the minimum safe vehicle spacing ;

[0022] Step 11: Control the movement of the autonomous vehicle based on the calculated optimal safe speed and minimum safe distance. When the next data collection time is reached, return to step 1.

[0023] Furthermore, the processing of the acquired image data and point cloud data to obtain processed image data and point cloud data specifically involves:

[0024] The image data is subjected to brightness equalization and noise reduction; invalid points are filtered from the point cloud data to obtain the remaining point cloud data after filtering.

[0025] Furthermore, the specific process of step two is as follows:

[0026] Step 2: Record the processed current frame image as... Calculate image Visual interference factors :

[0027]

[0028] in, Representing an image The average gradient of the inner pixel, This represents a reference value for the average pixel gradient obtained under clear conditions.

[0029] Representing an image The number of FAST corner points extracted from the internal data. This represents the reference value for the number of FAST corner points obtained under clear conditions;

[0030] Representing an image Compared with the processed previous frame image Structural similarity index;

[0031] , and Indicates the normalized weighting coefficient;

[0032] Step 22: Based on visual interference factors For images Perform distortion correction to obtain the current frame image after distortion correction;

[0033] in, Representing an image The pixels in , , and It is the camera distortion coefficient. and It is the rainfall impact coefficient. , This represents the pixels in the current frame image after distortion correction.

[0034] Steps 2 and 3: Perform guided filtering on the distortion-corrected current frame image to obtain the enhanced current frame image. ;

[0035] Step 24: Enhance the image of the current frame. FAST corner detection is performed to obtain feature points in the enhanced image of the current frame; during FAST corner detection, a detection threshold is set. according to Sure:

[0036]

[0037] in, Indicates the baseline threshold. It is the adjustment coefficient. This indicates the detection threshold.

[0038] Furthermore, the specific process of step three is as follows:

[0039] Step 3: 1. Enhance the image based on the feature points in the previous frame. In addition to the relative rotation between the previous and current enhanced images, the feature points in the previous enhanced image are predicted. Location in the current frame of the enhanced image:

[0040]

[0041] in, Indicates feature points in the previous frame of the enhanced image. Predicted location in the current frame augmented image Indicates relative rotation. This represents the inverse depth estimation optimized by the sliding window. For camera projection function, It is the back projection function;

[0042] Step 3.2: Identify the image in the previous enhanced frame. Using the image patch centered on it as a reference template, and determining the image within the current frame augmented image with... A preset search window is set as the center. Within this window, the region that best matches the reference template is determined, and the center position of the determined best-matching region is taken as the reference template. The corresponding pixel in the current frame augmented image ;

[0043] Then, the camera intrinsic parameter matrix and distortion coefficients are used to enhance the pixels in the current frame image. Perform distortion correction and transform the result to a normalized plane to obtain normalized plane coordinates. .

[0044] Furthermore, the specific process of step four is as follows:

[0045] Step 41: Construct a state vector including all variables to be optimized. :

[0046]

[0047] Among them, superscript Indicates transpose. These represent the first and second elements within the sliding window, respectively. Each keyframe and the state of the current frame image Represents the first observation within the sliding window. The inverse depth of each landmark;

[0048]

[0049] in, Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the vehicle's attitude in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the accelerometer has zero bias; Indicates the first data acquisition within the sliding window. At frame rate, the gyroscope has zero bias;

[0050] Step 42: Utilize the first [part of the sliding window] Frame and the Pre-integrate all IMU measurements between frames to obtain the first frame within the sliding window. Frame and the Relative rotation of vehicles between frames Relative velocity change and relative position change ;

[0051] The first in the sliding window Frame and the IMU residuals between frames Defined as:

[0052]

[0053] in, Indicates posture Rotation matrix for transformation to IMU coordinate system Indicates posture Rotation matrix for transformation to IMU coordinate system; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; This is the gravity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. Frame and the The time difference between frames; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system; This represents mapping the rotation matrix to the Lie algebra space;

[0054] Step 4.3 Visual Reprojection Residue Defined as:

[0055]

[0056] in, Indicates the first The first road sign point Normalized planar coordinates on the frame, Indicates the first The first road sign point Normalized planar coordinates on the frame;

[0057] Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time. Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time;

[0058] Indicates the first The inverse depth of each landmark point, with the superscript -1 indicating the inverse. Indicates the first Frame and the Frame to the first Visual reprojection residuals of each road sign point;

[0059] Step 4: Establish the residual function by minimizing the sum of the Mahalanobis distances of all IMU residuals and visual reprojection residuals.

[0060]

[0061] in, It is the set of each pair of adjacent frames in the sliding window. Represents the covariance matrix of IMU observations;

[0062] This represents the total number of landmarks observed within the sliding window. and Indicates the landmark observed within the sliding window. A pair of image frames, Indicates the number of observations within the sliding window. Road signs The set of each pair of image frames, For robust kernel functions, The covariance matrix of visual observation;

[0063] This indicates the calculation of the L2 norm;

[0064] Steps four and five: Use the Gauss-Newton method to optimize the established residual function and obtain the state vector. According to the state vector The position and pose of the current frame augmented image are used to obtain the visual inertial pose of the current frame augmented image. ;

[0065] Step 46: Calculate visual confidence. :

[0066]

[0067] in, For the first The visual reprojection residual of the image frame, The total number of effective residuals, This is the normalization coefficient.

[0068] Furthermore, the specific process of step five is as follows:

[0069] Calculate the point cloud after processing the current frame relative to the scan start time based on IMU measurement data. rotation matrix Translation vector ;

[0070] For the point cloud data after processing the current frame, the first... Points , will point Corrected to time:

[0071]

[0072] in, Indicates a point The result of the correction.

[0073] Furthermore, the specific process of step six is ​​as follows:

[0074] Step 61: For the points after motion compensation Select points from the motion-compensated point cloud of the current frame that are similar to... Calculate the 20 nearest neighbors of the point. Average distance to nearest neighbor :

[0075]

[0076] in, Indicates the selected and The neighboring One point;

[0077] calculate mean and standard deviation ; and then according to and Calculate the rainfall interference factor of lidar :

[0078]

[0079] in, It represents the number of points where the reflection intensity is below a preset threshold. It is the total number of points after motion compensation;

[0080] Step 62: Calculate the adaptive denoising threshold related to the lidar rainfall interference factor. :

[0081]

[0082] according to After motion compensation, the points are filtered and denoised, i.e., all points that satisfy the condition are retained. The point will be retained as the first Each point is recorded as ;

[0083] Step 63: Extract edge points and planar points from the filtered and denoised point cloud; specifically:

[0084] Searching for... neighbor set According to the nearest neighbor set Calculation points Local curvature :

[0085]

[0086] in, For the nearest neighbor set The number of points in Point Distance to the origin of the lidar sensor Represents the set of neighboring points any point in it;

[0087] The curvature values ​​of all points in the filtered and denoised point cloud Sort by size from largest to smallest and select the first one. The points corresponding to the curvature of a position constitute the set of edge feature points. Select the last one The points corresponding to the curvature of a point constitute a set of planar feature points. ;

[0088] Step 64: Fuse the edge feature points of the most recent N frames to obtain an edge point map. The planar feature points in the most recent N frames are fused to obtain a planar point map. ;

[0089] For any edge point in the current frame ,Will Projected onto edge point map Then, a map of the edge points is obtained. Zhongyu The two nearest neighbors Calculation points arrive and The perpendicular distance of the line L formed by the line L :

[0090]

[0091] in, express and The distance;

[0092] For any plane point in the current frame ,Will Projected onto a plane point map Then, a point map on the plane is obtained. Midpoint The three nearest neighbors Calculation points From point The perpendicular distance of the plane Π :

[0093]

[0094] Step 65: Establish the projection transformation matrix from the current frame to the local map. Objective function:

[0095]

[0096] The objective function is solved iteratively using the LM algorithm to obtain the projection transformation matrix. ,Will The inertial pose of the lidar in the current frame And record the final distance value of each edge point in the current frame. and the final distance value of each plane point ;

[0097] Step 66: Calculate the lidar matching error :

[0098]

[0099] in, This indicates the number of edge points in the point cloud of the current frame. This indicates the number of planar points in the point cloud of the current frame.

[0100] Furthermore, the specific process of step seven is as follows:

[0101] Step 71, and Transform them to the IMU coordinate system respectively to obtain the transformation results. and ;

[0102] Step 72: Use an adaptive Kalman filter to... and Perform fusion processing to obtain the fused pose. ;Specifically:

[0103] Step 721, according to Initialize state vector , These represent position, velocity, attitude, accelerometer zero bias, and gyroscope zero bias, respectively.

[0104] according to Initialize the observations, ;

[0105] Step 722: Calculate the one-step prediction result of the state. One-step prediction results of the error covariance matrix And then according to and Update the state and error covariance matrices:

[0106]

[0107] in, Represents the observation noise covariance matrix. , This is a preset scaling factor related to visual observation. It is the identity matrix. For the observation matrix, For Kalman gain, This represents the estimated state vector. This represents the estimated error covariance matrix;

[0108] Kalman filtering involves multiple iterations, with the position and orientation estimated in the state vector from the last iteration serving as the final fused pose. .

[0109] Furthermore, in step nine, the specific process of calculating the optimal safe speed is as follows:

[0110] Step 91: Calculate the road surface friction coefficient ;

[0111] (1) When wheel slip ratio is detected When the preset threshold is exceeded or the anti-lock braking system is activated, the road surface friction coefficient is calculated as follows:

[0112]

[0113] in, Indicates the instantaneous adhesion coefficient. , This indicates the real-time monitored longitudinal acceleration of the vehicle. Indicates the basic coefficient of friction. Indicates the weighting coefficient;

[0114] (2) When wheel slip ratio is detected When the preset threshold is not exceeded and the anti-lock braking system is not activated, the road surface friction coefficient is calculated as follows:

[0115]

[0116] Step 92: Calculate the meteorological sight distance :

[0117]

[0118] in, This is the maximum visibility under clear weather conditions. It is the attenuation coefficient. Indicates equivalent rainfall intensity;

[0119] Calculate the effective safe line of sight :

[0120]

[0121] in, Indicates the safety factor. Indicates perceived line-of-sight distance;

[0122] Step 93: Based on the effective safe line of sight Calculate the maximum speed :

[0123]

[0124] in, Indicates based on effective safe line of sight The calculated maximum speed, Indicates the maximum deceleration. , It is the efficiency coefficient of the braking system;

[0125] The maximum speed that ensures safe parking. for:

[0126]

[0127] The optimal safe speed for:

[0128] .

[0129] Furthermore, the specific process of step ten is as follows:

[0130] Step 11: Define a safe distance model that includes reaction distance:

[0131]

[0132] in, It is the system's equivalent reaction time. Indicates the speed of the vehicle in front. This indicates the safe distance calculated based on the reaction distance;

[0133] Step 12: Calculate the perception advantage compensation distance :

[0134]

[0135] in, Indicates the perceived advantage time;

[0136] Perceived advantage time and overall system health Proportional:

[0137]

[0138] in, It is the time constant of maximum advantage;

[0139] Overall system health for:

[0140]

[0141] in, and These are the weighting coefficients. It is the maximum reasonable threshold for lidar matching error;

[0142] Then the final minimum safe vehicle spacing for:

[0143]

[0144] in, It is the safety margin when at rest.

[0145] The beneficial effects of this invention are:

[0146] 1. Excellent environmental adaptability: Real-time and quantitative assessment of the health status of each sensor and dynamic adjustment of the fusion strategy effectively combats the complex environment of dynamically changing rainfall intensity, fundamentally solving the problem of mismatch in traditional fixed parameter algorithms under severe weather conditions.

[0147] 2. Scientific decision-making for safety: It abandons the simple strategy based on rules or fixed deceleration ratios, and instead calculates safe speed and distance based on physical models and real-time estimated environmental parameters (friction coefficient, safe sight distance). This provides a quantitative scientific basis for vehicle control that conforms to the dynamic limits of rainy weather, significantly reducing the risk of accidents. While ensuring the safety of vehicle passage, it can adjust the vehicle speed according to the current environmental conditions, thereby ensuring traffic efficiency.

[0148] 3. Strong system resilience: The design is based on a degradation strategy driven by internal performance indicators to ensure that the core functions of the system do not collapse when some sensors temporarily fail due to extreme weather. It can still maintain usable positioning and perception capabilities, reserving valuable time and operational basis for the driver to drive safely.

[0149] 4. Low cost and easy deployment: It fully utilizes the standard sensor hardware of existing autonomous vehicles, without the need to install any dedicated weather sensors. All core enhancement functions are implemented through software algorithms, possessing extremely high commercial potential and market promotion value. Attached Figure Description

[0150] Figure 1 This is a flowchart of a rain-time vehicle speed and distance decision-making method based on environmental perception, according to the present invention. Detailed Implementation

[0151] The autonomous driving system is equipped with sensors such as IMU, LiDAR, and cameras. Each sensor is connected to the vehicle's central computing platform through a high-bandwidth, low-latency in-vehicle network (such as in-vehicle Ethernet), and the output of each sensor is processed on the vehicle's central computing platform.

[0152] Specific implementation method one: Combining Figure 1 This embodiment describes a method for determining vehicle speed and following distance in rainy weather based on environmental perception. The method specifically includes the following steps:

[0153] Step 1: Simultaneously collect data using the IMU (Inertial Measurement Unit), LiDAR, and cameras deployed on the autonomous vehicle. Then, process the collected image data and point cloud data to obtain the processed image data and point cloud data.

[0154] Specifically, the image data is subjected to brightness equalization and noise reduction; invalid points are filtered from the point cloud data to obtain the remaining point cloud data after filtering.

[0155] The following is a detailed explanation of the invalid point filtering method for point cloud data: Since reflections from the vehicle's own components will generate self-vehicle points, and signal attenuation will generate points with excessive errors or low signal-to-noise ratios, the minimum and maximum effective distances are determined based on the LiDAR installation location, vehicle geometry, sensor performance, and current safety perception requirements, in order to filter out points that are less than the minimum effective distance and greater than the maximum effective distance.

[0156] Step 2: Calculate the visual interference factor of the processed current frame image, perform distortion correction on the processed current frame image based on the visual interference factor, and then process the distortion-corrected current frame image to obtain the enhanced current frame image.

[0157] And perform FAST corner detection on the current frame enhanced image to obtain the feature points in the current frame enhanced image;

[0158] Specifically:

[0159] Step 2: Record the processed current frame image as... Calculate image Visual interference factors :

[0160]

[0161] in, Representing an image The average gradient of the inner pixel, Representing an image The number of FAST corner points extracted from the internal data. This represents a reference value for the average pixel gradient obtained under clear conditions. This represents the reference value for the number of FAST corner points obtained under clear conditions. Representing an image Compared with the processed previous frame image The structural similarity index, , and This represents the normalized weight coefficients, and ;

[0162] , Indicates no interference. This indicates that the interference is extremely serious;

[0163] Step 22: Lens water film can cause additional, non-uniform distortion. Therefore, this invention introduces a method similar to the one used in standard camera distortion models. The relevant dynamic terms, i.e., based on visual interference factors For images Perform distortion correction to obtain the current frame image after distortion correction;

[0164] in, Representing an image The pixels in , , and It is the camera distortion coefficient. and It is the rainfall impact coefficient (obtained through laboratory simulation of rainfall environment calibration, used to dynamically adjust the distortion correction intensity). , This represents the pixels in the current frame image after distortion correction. The corresponding correction value;

[0165] exist As the diameter increases, the method of the present invention can automatically enhance the correction force for radial and tangential distortions;

[0166] Steps 2 and 3: Perform guided filtering on the distortion-corrected current frame image to obtain the enhanced current frame image. Guided filtering can smooth noise while preserving edges;

[0167] Step 24: Enhance the image of the current frame. FAST corner detection is performed to obtain feature points in the current frame augmented image (to predict the positions of these feature points in the next frame augmented image); during FAST corner detection, a detection threshold is set. according to Sure:

[0168]

[0169] in, This represents the baseline threshold (obtained through laboratory simulation of a sunny environment). It is an adjustment coefficient (an empirical value, ranging from 0 to 1). This indicates the detection threshold.

[0170] according to Determining a detection threshold ensures that only the most contrasting and reliable feature points are retained when the image is blurred, avoiding the introduction of a large number of noise points.

[0171] IMU pre-integration technology is achieved by integrating high-frequency accelerometer and gyroscope data. Through IMU pre-integration technology, the relative motion change of the vehicle between two adjacent frames in the inertial measurement unit (IMU) coordinate system can be calculated.

[0172] Step 3: Calculate the relative rotation between the previous frame augmented image and the current frame augmented image based on the IMU measurement values, and then predict the position of the feature points in the previous frame augmented image in the current frame augmented image based on the relative rotation between the previous frame augmented image and the current frame augmented image.

[0173] Then, a fine match is performed based on the prediction results, and the fine match results are subjected to distortion removal and normalization to obtain the normalized result.

[0174] Any feature point in the above enhanced image To illustrate, for example:

[0175] Step 3: 1. Enhance the image based on the feature points in the previous frame. In addition to the relative rotation between the previous and current enhanced images, the feature points in the previous enhanced image are predicted. Location in the current frame of the enhanced image:

[0176]

[0177] in, Indicates feature points in the previous frame of the enhanced image. Predicted location in the current frame augmented image Indicates relative rotation. This represents the inverse depth estimation optimized by the sliding window. For camera projection function, It is the back projection function;

[0178] Step 3.2: Identify the image in the previous enhanced frame. Using the image patch centered on it as a reference template, and determining the image within the current frame augmented image with... A preset search window is set as the center. Within this window, the region that best matches the reference template is determined, and the center position of the determined best-matching region is taken as the reference template. The corresponding pixel in the current frame augmented image ;

[0179] Then, the camera intrinsic parameter matrix and distortion coefficients are used to enhance the pixels in the current frame image. Perform distortion correction and transform the result to a normalized plane to obtain normalized plane coordinates. .

[0180] This method significantly compresses the search range and substantially improves the success rate and accuracy of feature tracking in low-texture or motion-blurred regions. We have thus obtained stable feature points and their normalized plane observations for consecutive keyframe inter-frame matching, and utilized IMU pre-integration to provide prior information on inter-frame motion. These visual observation data will serve as input for subsequent tightly coupled nonlinear optimization.

[0181] Step 4: Add the enhanced image of the current frame to the keyframe window, and establish a residual function based on the normalization result in Step 3. Then, solve the established residual function to obtain the visual inertial pose of the current frame. And calculate visual confidence. ;

[0182] It should be noted that the feature points in this invention refer to a single frame image. Feature points observed by multiple keyframes are called landmark points. The specific process of step four is as follows:

[0183] Step 41: Construct a state vector including all variables to be optimized. :

[0184]

[0185] Among them, superscript Indicates transpose. These represent the first and second elements within the sliding window, respectively. Each keyframe and the state of the current frame image Represents the first observation within the sliding window. The inverse depth of each landmark (i.e., the reciprocal of the depth of a landmark relative to the coordinate system of the first keyframe camera observed);

[0186]

[0187] in, Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the vehicle's attitude in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the accelerometer has zero bias; Indicates the first data acquisition within the sliding window. At frame rate, the gyroscope has zero bias;

[0188] Step 42: Utilize the first [part of the sliding window] Frame and the Pre-integrate all IMU measurements between frames to obtain the first frame within the sliding window. Frame and the Relative rotation of vehicles between frames Relative velocity change and relative position change ;

[0189] Assuming the first [window] within the sliding window Frame and the A frame is a pair of adjacent keyframes within a sliding window, and the first keyframe within the sliding window... Frame and the IMU residuals between frames Defined as:

[0190]

[0191] in, Indicates posture Rotation matrix for transformation to IMU coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the vehicle's attitude in the world coordinate system; Indicates posture Rotation matrix for transformation to IMU coordinate system; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; This indicates mapping the rotation matrix to the Lie algebra space. This is the gravity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. Frame and the The time difference between frames; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system;

[0192] The IMU residuals were compared with the first-order residuals predicted from the state variables. Frame and the Relative motion of frames (based on) , , , , , and gravity ) and the observed IMU pre-integral quantity The differences between them;

[0193] Step 4.3, for the first time that the same landmark point is observed... Frame and the Frame, visual reprojection residual Defined as:

[0194]

[0195] in, Indicates the first The first road sign point Normalized planar coordinates on the frame, Indicates the first The first road sign point Normalized planar coordinates on the frame, Indicates the first The inverse depth of each landmark point Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time (determined based on the position and orientation in the state variables). Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time, with a superscript of -1 indicating inverse calculation. Indicates the first Frame and the Frame to the first Visual reprojection residuals of each road sign point;

[0196] Step 4: Establish the residual function by minimizing the sum of the Mahalanobis distances of all IMU residuals and visual reprojection residuals.

[0197]

[0198] in, It is the set of each pair of adjacent frames in the sliding window. This represents the total number of landmarks observed within the sliding window. and Indicates the landmark observed within the sliding window. A pair of image frames, Indicates the number of observations within the sliding window. Road signs The set of each pair of image frames, This is a robust kernel function used to suppress the effects of false matches (outside points). The covariance matrix of visual observations. Represents the covariance matrix of IMU observations. This indicates the calculation of the L2 norm;

[0199] Steps four and five: Use the Gauss-Newton method to optimize the established residual function and obtain the state vector. According to the state vector The position and pose of the current frame augmented image are used to obtain the visual inertial pose of the current frame augmented image. ;

[0200] Step 46: Calculate visual confidence. :

[0201]

[0202] in, For the first The visual reprojection residuals of image frames (each landmark corresponds to several pairs of image frames, and the total number of image frame pairs corresponding to all landmarks is...) ), The total number of effective residuals, This is the normalization coefficient.

[0203] When the image is clear and the feature matching is accurate, the reprojection error is small. When rainfall causes image blurring and increases mismatches, the reprojection error increases. The value decreased.

[0204] Step 5: Perform motion compensation on the processed point cloud data based on IMU measurement data, specifically as follows:

[0205] Calculate the point cloud after processing the current frame relative to the scan start time based on IMU measurement data. rotation matrix Translation vector , This indicates the time when the point cloud data for the current frame was collected;

[0206] Then, motion compensation is performed on each point in the point cloud data processed in the current frame. For example, for the point cloud data processed in the current frame... Points , will point Corrected to time:

[0207]

[0208] in, Indicates a point The result of the correction.

[0209] Step 6: Calculate the lidar rainfall interference factor based on the motion-compensated point cloud data of the current frame. The motion-compensated point cloud data is then filtered and denoised to obtain the filtered and denoised point cloud data.

[0210] The edge point set and planar point set of the current frame are extracted from the filtered and denoised point cloud data, and the inertial pose of the lidar is estimated based on the edge point set and planar point set of the current frame. LiDAR matching error ;

[0211] Specifically:

[0212] Step 61: For the points after motion compensation Using data structures such as KD-Tree, points are selected from the motion-compensated point cloud of the current frame that are similar to... Calculate the 20 nearest neighbors of the point. Average distance to nearest neighbor :

[0213]

[0214] in, Indicates the selected and The neighboring One point;

[0215] On a uniform real surface, Smaller and more stable; while discrete rain noise points are usually far from the real structure. The value will be significantly larger.

[0216] calculate mean and standard deviation ; and then according to and Calculate the rainfall interference factor of lidar :

[0217]

[0218] in, It is the number of points where the reflection intensity is lower than a preset threshold (raindrops typically have low reflectivity). It is the total number of points after motion compensation;

[0219] , A higher value indicates more significant rainfall noise. This factor comprehensively assesses the dispersion of the point cloud. Its low reflectivity and other characteristics can effectively distinguish between rainfall disturbances and normal environmental structural changes.

[0220] Step 62: Calculate the adaptive denoising threshold related to the lidar rainfall interference factor. :

[0221]

[0222] When rainfall is heavy (i.e.) (when large), threshold Automatically raise the threshold for more aggressive filtering; lower the threshold when the weather is good to avoid accidentally deleting real details.

[0223] according to After motion compensation, the points are filtered and denoised, i.e., all points that satisfy the condition are retained. The point will be retained as the first Each point is recorded as The point cloud has been significantly freed from rain noise and mainly contains points representing the structure of the real environment, providing a clean data source for high-precision feature extraction;

[0224] Step 63: Extract edge points (located on object contours with high curvature) and planar points (located on flat surfaces with low curvature) from the filtered and denoised point cloud; specifically:

[0225] In the filtered and denoised point cloud, KD-Tree search is used. neighbor set According to the nearest neighbor set Calculation points Local curvature :

[0226]

[0227] in, For the nearest neighbor set The number of points in Point Distance to the origin of the lidar sensor Represents the set of neighboring points any point in it;

[0228] At sharp edges or corners, neighboring points are distributed across different surfaces, resulting in a larger average distance, i.e., higher curvature; in flat areas, neighboring points are concentrated, resulting in a smaller average distance and lower curvature. Normalization factor (Distance from the point to the sensor origin) is used to reduce the influence of distance on curvature calculation, making similar features at different distances comparable.

[0229] The curvature values ​​of all points in the filtered and denoised point cloud Sort by size from largest to smallest and select the first one. The points corresponding to the curvature of a position constitute the set of edge feature points. Select the last one The points corresponding to the curvature of a point constitute a set of planar feature points. ;

[0230] It should be noted that, to ensure the uniformity of feature distribution, the azimuth angle of the LiDAR can be divided into multiple sectors. Within each sector, several points with the largest curvature are independently selected to output the set of edge points. Within each sector, several points with the minimum curvature are independently selected to output a set of planar points. Uniformity can be ensured by uniformly selecting points within each sector. These feature points are extracted from the filtered and denoised point cloud, are less affected by noise, and can stably represent the key geometric structures of the environment.

[0231] Step 64: Fuse the edge feature points of the most recent N frames (in this invention, N is 10) to obtain an edge point map. The planar feature points in the most recent N frames are fused to obtain a planar point map. ;

[0232] For any edge point in the current frame ,Will Projected onto edge point map Then, a map of the edge points is obtained. Zhongyu The two nearest neighbors Calculation points arrive and The perpendicular distance of the line L formed by the line L :

[0233]

[0234] in, express and The distance;

[0235] For any plane point in the current frame ,Will Projected onto a plane point map Then, a point map on the plane is obtained. Midpoint The three nearest neighbors Calculation points From point The perpendicular distance of the plane Π :

[0236]

[0237] Step 65: Establish the projection transformation matrix from the current frame to the local map. Objective function:

[0238]

[0239] The objective function is solved iteratively using the LM algorithm to obtain the projection transformation matrix. ,Will The inertial pose of the lidar in the current frame And record the final distance value of each edge point in the current frame. and the final distance value of each plane point ;

[0240] Step 66: Calculate the lidar matching error :

[0241]

[0242] in, This indicates the number of edge points in the point cloud of the current frame. Indicates the number of planar points in the point cloud of the current frame;

[0243] When the point cloud quality is high and the environmental features are rich and clear, the matching is accurate. and The value is very small. A value less than 0.05m indicates reliable lidar sensing. When rainfall increases noise, masks effective features, or reduces the repeatability of environmental features (e.g., in long, straight corridors), the matching error increases. The value increased.

[0244] Step 7: Visual inertial pose of the current frame and lidar inertial pose Adaptive fusion processing is performed to obtain the fused pose. ;

[0245] Specifically:

[0246] Step 71: Utilize pre-calibrated sensor extrinsic parameters (Camera to IMU) and (LiDAR to IMU), will and Transform them to the IMU coordinate system respectively to obtain the transformation results. and ;

[0247] Since there may be slight delays in the VIS and LIS processing pipelines, linear interpolation is used to align them to the same fusion time in the IMU coordinate system.

[0248] Step 72: Apply an adaptive Kalman filter (AKF) to... and Perform fusion processing to obtain the fused pose. ;Specifically:

[0249] Step 721: Initialize the state vector , These represent position, velocity, attitude, accelerometer zero bias, and gyroscope zero bias, respectively. The position and attitude information in the middle comes from , The velocity information is obtained through differential estimation;

[0250] Initial observations ;

[0251] Step 722: Calculate the one-step prediction result of the state. One-step prediction results of the error covariance matrix The process noise covariance matrix in the prediction process , These are preset scaling factors related to the motion model. It is the identity matrix; then according to and Update the state and error covariance matrices:

[0252]

[0253] in, Represents the observation noise covariance matrix. , This is a preset scaling factor related to visual observation. It is the identity matrix. For the observation matrix, For Kalman gain, This represents the estimated state vector. This represents the estimated error covariance matrix;

[0254] Kalman gain The essence is the degree of trust in observation. The larger the value, the more confident one is in the observed value. (VIS) to correct the predicted value (LIS). According to the gain formula: when VIS is reliable but LIS is unreliable, this leads to As the value increases, the filter places more trust in the VIS observations, requiring significant corrections to the LIS predictions. This leads to problems when the VIS is unreliable while the LIS is reliable. As the value decreases, the filter places more trust in the LIS predictions and almost ignores the VIS observations. When both are reliable or neither is reliable, Take the middle value to achieve balanced integration.

[0255] Kalman filtering involves multiple iterations, with the position and orientation estimated in the state vector from the last iteration serving as the final fused pose. This pose remains stable even when rainfall causes fluctuations in sensor performance;

[0256] Step 8: Utilize the fused pose The filtered and denoised point cloud data of the current frame is transformed into the world coordinate system, and then the transformed point cloud is divided into different clusters by the DBSCAN clustering algorithm based on Euclidean distance. Each cluster of point cloud represents an obstacle.

[0257] The truncated signed distance function (TSDF) fusion method is used to incrementally fuse the point cloud of static obstacles into the global map. Then, the SORT multi-object tracking algorithm based on Kalman filtering is used to track the trajectory of each dynamic obstacle, outputting a list of dynamic obstacles. The positions of the dynamic obstacles are marked as temporarily occupied in the constructed static environment map, resulting in an updated static environment map. ;

[0258] Step 9: Based on visual interference factors LiDAR interference factor Visual confidence Matching error with lidar An input vector is constructed and used as the input to a two-layer fully connected neural network. The equivalent rainfall intensity is then output through the two-layer fully connected neural network. ;

[0259] Based on the static environment map Determine the sensing line of sight and calculate the optimal safe speed based on the sensing line of sight and the equivalent rainfall intensity; specifically:

[0260] Step 91: Calculate the road surface friction coefficient ;

[0261] (1) When wheel slip ratio is detected When the preset threshold is exceeded or the anti-lock braking system (ABS) is activated, the road surface friction coefficient is calculated as follows:

[0262]

[0263] in, Indicates the instantaneous adhesion coefficient. , This indicates the real-time monitored longitudinal acceleration of the vehicle. Indicates the basic coefficient of friction;

[0264] (2) When wheel slip ratio is detected When the preset threshold is not exceeded and the anti-lock braking system (ABS) is not activated, the road surface friction coefficient is calculated as follows:

[0265]

[0266] It should be noted that the basic coefficient of friction The method for obtaining the coefficient of friction is as follows: using a semantic segmentation network to identify the current road surface material (asphalt, cement) and surface condition (dry, wet, waterlogged), and then searching a preset empirical lookup table based on the identification results to obtain the basic friction coefficient. .

[0267] Step 92: Calculate the meteorological sight distance :

[0268]

[0269] in, This is the maximum visibility under clear weather conditions. The attenuation coefficient was determined experimentally. Indicates equivalent rainfall intensity. The larger the diameter, the shorter the weather visibility.

[0270] Calculate the effective safe line of sight :

[0271]

[0272] in, Indicates the safety factor. Indicates perceived line-of-sight distance;

[0273] Take the smaller of the meteorological line of sight and the sensing line of sight, and multiply the smaller of them by a safety factor less than 1. To obtain an effective safe line of sight, a safety margin is reserved for system estimation errors and driver or controller reaction delays;

[0274] The method for obtaining the perceived line of sight distance is: in a static environment map. In the process, a ray is projected from the vehicle's current position along the center line of the current lane or the planned path. The distance from the point where the ray first intersects a grid marked "occupied" on the map to the vehicle is the perceived line-of-sight distance. It represents the passable distance of the vehicle to the nearest static obstacle in its current position.

[0275] Step 93: Ignore air resistance and proceed according to the effective safe line of sight. Calculate the maximum speed :

[0276]

[0277] in, Indicates based on effective safe line of sight The calculated maximum speed, Indicates the maximum deceleration. , It is the efficiency coefficient of the braking system;

[0278] The maximum speed that ensures safe parking. for:

[0279]

[0280] The optimal safe speed for:

[0281]

[0282] When the sensor estimates that the road conditions are slippery ( (reduced) or decreased visibility ( When (decreased), the calculated It automatically lowers, providing quantified speed recommendations for vehicle control that are consistent with the current environmental dynamic limits.

[0283] Step 10: Based on the optimal safe speed Calculate the minimum safe vehicle spacing ;

[0284] Based on the traditional following distance safety model, this invention innovatively introduces "perceptual advantage compensation" to make the decision adaptive. Specifically:

[0285] Step 11: Define a safe distance model that includes reaction distance:

[0286]

[0287] in, It is the system's equivalent response time (including the delays in the perception, decision-making, and execution links). Indicates the speed of the vehicle in front. This indicates the safe distance calculated based on the reaction distance;

[0288] Step 12: The perception sensors (especially LiDAR) of an autonomous driving system may detect hazards ahead (such as sudden deceleration of the vehicle in front or road obstacles) earlier than a human driver. This invention quantifies this advantage into an equivalent warning distance compensation, that is, calculates the perception advantage compensation distance. :

[0289]

[0290] in, The perceived advantage time indicates that the system, on average, anticipates the difference between itself and a human driver. The same danger was identified in seconds;

[0291] The perceived advantage time is not a fixed value, but rather depends on the overall health of the system. Proportional:

[0292]

[0293] in, It is the time constant of maximum advantage;

[0294] System health The overall health of the system is obtained by integrating core perception performance indicators. for:

[0295]

[0296] in, and These are the weighting coefficients. It is the maximum reasonable threshold for lidar matching error;

[0297] when High and When low, When the system's perception tends towards 1, it considers itself "healthy" and enjoys its maximum advantage during that time; when perception deteriorates significantly due to weather conditions, Reduced, the advantage time shortened;

[0298] Then the final minimum safe vehicle spacing for:

[0299]

[0300] in, This is the safety margin when stationary (taken as 2 meters).

[0301] Ensure the distance after compensation is not less than This mechanism intelligently adjusts the aggressiveness of the following strategy. When the system's own perception is good (i.e., When perception is high, it can calculate smaller but still safer vehicle spacing, improving road traffic efficiency; when perception performance is reduced due to weather (i.e., When the temperature is low, the system automatically adopts a more conservative strategy and increases the safety margin, which forms an intelligent feedback loop from perceived quality to decision parameters.

[0302] After calculating the safe speed and safe distance, the following plan can be made:

[0303] 1. Integrated path and velocity planning. The resilient navigation and human-machine interaction unit receives all upstream information: robust pose. As the starting point for planning, a static environment map is used for obstacle avoidance, dynamic obstacle trajectories serve as movement constraints, and the optimal safe speed is determined. As a hard constraint on the upper speed limit for generating the trajectory, the minimum safe vehicle spacing As a distance constraint that must be maintained between the vehicle and the vehicle ahead, the planner, while satisfying all these constraints, solves in real time a safe, comfortable, and dynamically feasible local trajectory, and sends it to the vehicle's drive-by-wire chassis for execution.

[0304] 2. Multimodal Human-Machine Interaction and Warning. The vehicle's surrounding environment is clearly rendered in a top-down view on the in-vehicle central control screen: a static map (such as road boundaries and lane lines) and dynamic obstacles (highlighted and displayed with velocity vectors). A "recommended safe speed" is displayed in real-time in a prominent location on the screen (such as the instrument panel area) using large numbers and a dynamic progress bar. "and recommended minimum vehicle distance" Meanwhile, the current system health status is visually displayed in the corner using icons (such as dashboard icons) or color bars (green / yellow / red). .

[0305] 3. Voice Warning: Triggers tiered voice prompts (volume related to urgency) in the following situations: including Level 1 prompts (informative): when the driver manually operates the vehicle or the current autonomous driving control command causes the vehicle speed to exceed [a certain threshold]. At that time, a prompt appeared saying, "The road is slippery in rainy weather. It is recommended to control your speed at [[ "Below 1 km / h"; Level 2 warning (warning): When the actual following distance is consistently less than 1 km / h. At that time, a message appeared saying "The following distance is too close, please maintain at least [ ]" "Safe distance of meters"; Level 3 prompt (alertness): When the system health status... When the temperature is below the medium threshold, the system will display the message: "Caution: Severe weather conditions and limited perception. Please drive with caution."

[0306] It should be noted that a degradation strategy is required in special circumstances:

[0307] Determining and executing sensor degradation strategies. Real-time monitoring in the system background. and This is the "ballast" that ensures the system does not crash under extreme conditions and maintains minimum safety functions.

[0308] Demotion criteria: If continuous Frames (10 frames, approximately 0.3 seconds) below the extremely low threshold If so, it is determined that the camera's function is essentially lost. continuous Frames above the extremely high threshold If the effective point density is below the threshold (0.4m), the lidar function is basically lost.

[0309] Graceful degradation execution: Once a visual failure is determined, the system immediately forces the observation noise covariance matrix in the adaptive Kalman filter to be reduced. Setting it to a value approaching infinity results in a Kalman gain. The value tends towards 0. The system completely ignores unreliable values. The algorithm smoothly degenerates into a pure lidar inertial odometry (LIS) mode. Simultaneously, it attempts to extract structured features such as lane lines and road edges from the lidar point cloud, providing additional heading constraints for the pure LIS mode. If lidar failure is detected, the system forcibly adjusts the process noise covariance matrix. Setting it to a value approaching infinity results in a Kalman gain. The system tends towards 1. It completely trusts visual observation. The system smoothly degrades to a pure visual-inertial odometry (VIS). Simultaneously, it automatically activates a pre-trained lightweight monocular depth estimation network. This network takes the rain-adapted image output from the front-end vision processing module as input and infers a dense depth map in real time. This depth map provides relative distance information for each pixel, injecting crucial geometric priors into the pure vision system. Subsequently, by combining the depth map with the corrected image using a camera model, a pseudo-3D point cloud of the scene can be reconstructed, or it can be directly used to assist in obstacle segmentation. This mechanism partially compensates for the loss of real-time, dense 3D environmental perception capabilities due to LiDAR failure, allowing the degraded system to maintain basic spatial understanding and providing indispensable environmental geometric information for subsequent conservative path planning.

[0310] Degradation Warning and Interaction: Upon triggering a degradation, the system immediately issues the highest level of clear warning via voice and screen: "Warning: Camera function is limited, the system has switched to LiDAR-dominated mode, please remain vigilant" and "Warning: LiDAR function is limited, the system has switched to vision-dominated mode, please drive cautiously and prepare to take over immediately".

[0311] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for determining vehicle speed and following distance in rainy weather based on environmental perception, characterized in that, The method specifically includes the following steps: Step 1: Collect data synchronously using the IMU, LiDAR and camera deployed on the autonomous vehicle, and then process the collected current frame image data and point cloud data to obtain the processed current frame image data and point cloud data. Step 2: Calculate the visual interference factor of the processed current frame image, perform distortion correction on the processed current frame image based on the visual interference factor, and then process the distortion-corrected current frame image to obtain the enhanced current frame image. And perform FAST corner detection on the current frame enhanced image to obtain the feature points in the current frame enhanced image; Step 3: Calculate the relative rotation between the previous frame augmented image and the current frame augmented image based on the IMU measurement values, and then predict the position of the feature points in the previous frame augmented image in the current frame augmented image based on the relative rotation between the previous frame augmented image and the current frame augmented image. Then, a fine match is performed based on the prediction results, and the fine match results are subjected to distortion removal and normalization to obtain the normalized result. Step 4: Add the enhanced image of the current frame to the keyframe window, and establish a residual function based on the normalization result in Step 3. Then, solve the established residual function to obtain the visual inertial pose of the current frame. And calculate visual confidence. ; Step 5: Perform motion compensation on the processed point cloud data based on IMU measurement data; Step 6: Calculate the lidar rainfall interference factor based on the motion-compensated point cloud data of the current frame. The motion-compensated point cloud data is then filtered and denoised to obtain the filtered and denoised point cloud data. The edge point set and planar point set of the current frame are extracted from the filtered and denoised point cloud data, and the inertial pose of the lidar is estimated based on the edge point set and planar point set of the current frame. LiDAR matching error ; Step 7: Visual inertial pose of the current frame and lidar inertial pose Adaptive fusion processing is performed to obtain the fused pose. ; Step 8: Utilize the fused pose The filtered and denoised point cloud data of the current frame is transformed into the world coordinate system, and then the transformed point cloud is divided into different clusters by the clustering algorithm DBSCAN, where each cluster of point cloud represents an obstacle. The point cloud of static obstacles is incrementally fused into the global map. Then, the SORT multi-object tracking algorithm is used to track the trajectory of each dynamic obstacle, outputting a list of dynamic obstacles. The positions of the dynamic obstacles are marked as temporarily occupied in the constructed static environment map, resulting in an updated static environment map. ; Step 9: Based on visual interference factors LiDAR interference factor Visual confidence Matching error with lidar An input vector is constructed and used as the input to a two-layer fully connected neural network. The equivalent rainfall intensity is then output through the two-layer fully connected neural network. ; Based on the static environment map Determine the sensing line of sight and calculate the optimal safe speed based on the sensing line of sight and the equivalent rainfall intensity; Step 10: Based on the optimal safe speed Calculate the minimum safe vehicle spacing ; Step 11: Control the movement of the autonomous vehicle based on the calculated optimal safe speed and minimum safe distance. When the next data collection time is reached, return to step 1.

2. The method for determining vehicle speed and distance in rainy weather based on environmental perception as described in claim 1, characterized in that, The process of processing the acquired image data and point cloud data yields the processed image data and point cloud data, specifically as follows: The image data is subjected to brightness equalization and noise reduction; invalid points are filtered from the point cloud data to obtain the remaining point cloud data after filtering.

3. The method for determining vehicle speed and distance in rainy weather based on environmental perception as described in claim 2, characterized in that, The specific process of step two is as follows: Step 2: Record the processed current frame image as... Calculate the image Visual interference factors : in, Representing an image The average gradient of the inner pixel, This represents a reference value for the average pixel gradient obtained under clear conditions. Representing an image The number of FAST corner points extracted from the internal data. This represents the reference value for the number of FAST corner points obtained under clear conditions; Representing an image Compared with the processed previous frame image Structural similarity index; , and Indicates the normalized weighting coefficient; Step 22: Based on visual interference factors For images Perform distortion correction to obtain the current frame image after distortion correction; in, Representing an image The pixels in , , and It is the camera distortion coefficient. and It is the rainfall impact coefficient; in, This represents the pixels in the current frame image after distortion correction. Steps 2 and 3: Perform guided filtering on the distortion-corrected current frame image to obtain the enhanced current frame image. ; Step 24: Enhance the image of the current frame. FAST corner detection is performed to obtain feature points in the enhanced image of the current frame; during FAST corner detection, a detection threshold is set. according to Sure: in, Indicates the baseline threshold. It is the adjustment coefficient. This indicates the detection threshold.

4. The method for determining vehicle speed and distance in rainy weather based on environmental perception according to claim 3, characterized in that, The specific process of step three is as follows: Step 3:

1. Enhance the image based on the feature points in the previous frame. In addition to the relative rotation between the previous and current enhanced images, the feature points in the previous enhanced image are predicted. Location in the current frame of the enhanced image: in, Indicates feature points in the previous enhanced image frame Predicted location in the current frame augmented image Indicates relative rotation. This represents the inverse depth estimation optimized by the sliding window. For camera projection function, It is the back projection function; Step 3.2: Identify the image in the previous enhanced frame. Using the image patch centered on it as a reference template, and determining the image within the current frame augmented image with... A preset search window is set as the center. Within this window, the region that best matches the reference template is determined, and the center position of the determined best-matching region is used as the reference template. The corresponding pixel in the current frame augmented image ; Then, the camera intrinsic parameter matrix and distortion coefficients are used to enhance the pixels in the current frame image. Perform distortion correction and transform the result to a normalized plane to obtain normalized plane coordinates. .

5. The method for determining vehicle speed and distance in rainy weather based on environmental perception according to claim 4, characterized in that, The specific process of step four is as follows: Step 41: Construct a state vector including all variables to be optimized. : Among them, superscript Indicates transpose. These represent the first and second elements within the sliding window, respectively. Each keyframe and the state of the current frame image Represents the first observation within the sliding window. The inverse depth of each landmark; in, Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system; Indicates the number of samples collected within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. At frame rate, the vehicle's attitude in the world coordinate system; Indicates the number of samples collected within the sliding window. At frame rate, the accelerometer has zero bias; Indicates the first data acquisition within the sliding window. At frame rate, the gyroscope has zero bias; Step 4.2: Utilize the first [part of the sliding window] Frame and the Pre-integrate all IMU measurements between frames to obtain the first frame within the sliding window. Frame and the Relative rotation of vehicles between frames Relative velocity change and relative position change ; The first in the sliding window Frame and the IMU residuals between frames Defined as: in, Indicates posture Rotation matrix for transformation to IMU coordinate system, Indicates posture Rotation matrix for transformation to IMU coordinate system; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional velocity vector in the world coordinate system; This is the gravity vector in the world coordinate system; Indicates the first data acquisition within the sliding window. Frame and the The time difference between frames; Indicates the first data acquisition within the sliding window. At frame time, the vehicle's three-dimensional position coordinates in the world coordinate system; This represents mapping the rotation matrix to the Lie algebra space; Step 4.3 Visual Reprojection Residue Defined as: in, Indicates the first The first road sign point Normalized planar coordinates on the frame, Indicates the first The first road sign point Normalized planar coordinates on the frame; Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time. Indicates the first data acquisition within the sliding window. The vehicle's pose at frame time; Indicates the first The inverse depth of each landmark point, with the superscript -1 indicating the inverse. Indicates the first Frame and the Frame to the first Visual reprojection residuals of each road sign point; Step 4: Establish the residual function by minimizing the sum of the Mahalanobis distances of all IMU residuals and visual reprojection residuals. in, It is the set of each pair of adjacent frames in the sliding window. Represents the covariance matrix of IMU observations; This represents the total number of landmarks observed within the sliding window. and Indicates the landmark observed within the sliding window. A pair of image frames, Indicates the number of observations within the sliding window. Road sign The set of each pair of image frames, For robust kernel functions, Let be the covariance matrix of visual observations; This indicates the calculation of the L2 norm; Steps four and five: Use the Gauss-Newton method to optimize the established residual function and obtain the state vector. According to the state vector The position and pose of the current frame augmented image are used to obtain the visual inertial pose of the current frame augmented image. ; Step 46: Calculate visual confidence. : in, For the first The visual reprojection residual of the image frame, The total number of effective residuals, This is the normalization coefficient.

6. The method for determining vehicle speed and distance in rainy weather based on environmental perception according to claim 5, characterized in that, The specific process of step five is as follows: Calculate the point cloud after processing the current frame relative to the scan start time based on IMU measurement data. rotation matrix Translation vector ; For the point cloud data after processing the current frame, the first... Points , will point Corrected to time: in, Indicates a point The result of the correction.

7. The method for determining vehicle speed and distance in rainy weather based on environmental perception according to claim 6, characterized in that, The specific process of step six is ​​as follows: Step 61: For the points after motion compensation Select points from the motion-compensated point cloud of the current frame that are similar to... Calculate the 20 nearest neighbors of the point. Average distance to nearest neighbor : in, Indicates the selected and The neighboring One point; calculate mean and standard deviation ; and then according to and Calculate the rainfall interference factor of lidar : in, It represents the number of points where the reflection intensity is below a preset threshold. It is the total number of points after motion compensation; Step 62: Calculate the adaptive denoising threshold related to the lidar rainfall interference factor. : according to After motion compensation, the points are filtered and denoised, i.e., all points that satisfy the condition are retained. The point will be retained as the first Each point is recorded as ; Step 63: Extract edge points and planar points from the filtered and denoised point cloud; specifically: Searching for... Neighbor set According to the nearest neighbor set Calculation points Local curvature : in, For the nearest neighbor set The number of points in Point Distance to the origin of the lidar sensor Represents the set of neighboring points any point in it; The curvature values ​​of all points in the filtered and denoised point cloud Sort by size from largest to smallest and select the first one. The points corresponding to the curvature of a position constitute the set of edge feature points. Select the last one The points corresponding to the curvature of a point constitute a set of planar feature points. ; Step 64: Fuse the edge feature points of the most recent N frames to obtain an edge point map. The planar feature points in the most recent N frames are fused to obtain a planar point map. ; For any edge point in the current frame ,Will Projected onto edge point map Then, a map of the edge points is obtained. Zhongyu The two nearest neighbors Calculation points arrive and The perpendicular distance of the line L formed by the line L : in, express and The distance; For any plane point in the current frame ,Will Projected onto a plane point map Then, a point map on the plane is obtained. Midpoint The three nearest neighbors Calculation points From point The perpendicular distance of the plane Π : Step 65: Establish the projection transformation matrix from the current frame to the local map. Objective function: The objective function is solved iteratively using the LM algorithm to obtain the projection transformation matrix. ,Will The inertial pose of the lidar in the current frame And record the final distance value of each edge point in the current frame. and the final distance value of each plane point ; Step 66: Calculate the lidar matching error : in, This indicates the number of edge points in the point cloud of the current frame. This indicates the number of planar points in the point cloud of the current frame.

8. The method for determining vehicle speed and distance in rainy weather based on environmental perception according to claim 1, characterized in that, The specific process of step seven is as follows: Step 71, and Transform them to the IMU coordinate system respectively to obtain the transformation results. and ; Step 72: Use an adaptive Kalman filter to... and Perform fusion processing to obtain the fused pose. ;Specifically: Step 721, according to Initialize state vector , These represent position, velocity, attitude, accelerometer zero bias, and gyroscope zero bias, respectively. according to Initialize observations: Step 722: Calculate the one-step prediction result of the state. One-step prediction results of the error covariance matrix And then according to and Update the state and error covariance matrices: in, For the observation matrix, For Kalman gain, This represents the estimated state vector. This represents the estimated error covariance matrix. Represents the observation noise covariance matrix; in, This is a preset scaling factor related to visual observation. It is the identity matrix; The Kalman filter undergoes multiple iterations, using the position and orientation in the state vector estimated in the last iteration as the final fused pose. .

9. A method for determining vehicle speed and distance in rainy weather based on environmental perception, as described in claim 8, is characterized in that... In step nine, the specific process for calculating the optimal safe speed is as follows: Step 91: Calculate the road surface friction coefficient ; (1) When wheel slip ratio is detected When the preset threshold is exceeded or the anti-lock braking system is activated, the road surface friction coefficient is calculated as follows: in, Indicates the instantaneous adhesion coefficient. Indicates the basic coefficient of friction. Indicates the weighting coefficient; in, This represents the real-time monitored longitudinal acceleration of the vehicle. (2) When wheel slip ratio is detected When the preset threshold is not exceeded and the anti-lock braking system is not activated, the road surface friction coefficient is calculated as follows: Step 92: Calculate the meteorological sight distance : in, This is the maximum visibility under clear weather conditions. It is the attenuation coefficient. Indicates equivalent rainfall intensity; Calculate the effective safe line of sight : in, Indicates the safety factor. Indicates perceived line-of-sight distance; Step 93: Based on the effective safe line of sight Calculate the maximum speed : in, Indicates based on effective safe line of sight The calculated maximum speed, Indicates the maximum deceleration; in, It is the efficiency coefficient of the braking system; The maximum speed that ensures safe parking. for: The optimal safe speed for: 。 10. A method for determining vehicle speed and distance in rainy weather based on environmental perception, as described in claim 9, is characterized in that... The specific process of step ten is as follows: Step 11: Define a safe distance model that includes reaction distance: in, It is the system's equivalent reaction time. Indicates the speed of the vehicle in front. This indicates the safe distance calculated based on the reaction distance; Step 12: Calculate the perception advantage compensation distance : in, Indicates the perceived advantage time; Perceived advantage time and overall system health Proportional: in, It is the time constant of maximum advantage; Overall system health for: in, and These are the weighting coefficients. It is the maximum reasonable threshold for lidar matching error; Then the final minimum safe vehicle spacing for: in, It is the safety margin when at rest.

Images