Road Segmentation and Slope Estimation Method Based on Multi-Sensor Fusion

Abstract

The present invention belongs to the technical field of image processing, and particularly relates to a road segmentation and slope estimation method based on multi-sensor fusion. It includes: S1: registering the camera and the lidar; S2: generating a road segmentation result based on the output data of the registered camera and lidar; S3: obtaining a three-dimensional point cloud image of the road surface based on the road segmentation result, and realizing the slope estimation of the road surface based on the three-dimensional point cloud image. The present invention effectively reduces the error rate of road surface segmentation and the error of slope estimation through multi-sensor complementarity, and still maintains stable output under complex backgrounds such as vibration, rain, fog, and low light.

Description

Technical Field

[0001] The present invention belongs to the technical field of image processing, and particularly relates to a road segmentation and slope estimation method based on multi-sensor fusion. Background Art

[0002] The environmental perception of a single sensor has great limitations. The RGB camera is limited by lighting conditions and cannot perceive depth information, while lidar performs poorly in complex weather such as rain and snow. In the fields of intelligent transportation and autonomous driving, environmental perception, as a core technical link, its accuracy and reliability directly affect the quality of system decision-making. Currently, the limitations of the application of a single sensor are as follows:

[0003] The RGB camera realizes scene recognition by virtue of high-resolution texture information, but is easily interfered by lighting conditions. In a low-light environment at night, the signal-to-noise ratio of the image drops sharply, resulting in the failure of feature extraction; in a strong light direct or backlight scene, the overexposure / underexposure problem of the image causes the target edge to be blurred. At the same time, the RGB camera is essentially a two-dimensional imaging device, lacking the ability to directly obtain the depth information of the scene, and it is difficult to accurately model the distance of obstacles and the undulation of the road surface.

[0004] Although lidar can obtain high-precision three-dimensional point cloud data through the time-of-flight principle (TOF), its performance decays significantly under complex meteorological conditions. In rainy and snowy weather, Mie scattering occurs between the laser beam and suspended particles, resulting in the disorder of the echo signal, and the phenomenon of point cloud data jump and false alarm; in a sandy environment, the sensor window is polluted, resulting in a decrease in ranging accuracy, and even completely losing the perception ability in extreme cases.

[0005] In the prior art, the perception blind area of a single sensor has become a key bottleneck restricting the robustness of the autonomous driving system. Taking the ramp working condition as an example, the traditional vision scheme cannot accurately estimate the slope angle, and lidar is prone to sparse point cloud problems on a wet and slippery road surface. Both are difficult to meet the requirements of high-precision road surface segmentation and slope detection. Therefore, it is urgent to construct a complementary perception system through multi-sensor information fusion technology to adapt to complex road environments. Summary of the Invention

[0006] In view of this, the present invention aims to provide a road segmentation and slope estimation method based on multi-sensor fusion to solve the problems that the prior art cannot accurately estimate the slope angle and is difficult to meet the requirements of high-precision road surface segmentation and slope detection. The present invention effectively reduces the error rate of road surface segmentation and the error of slope estimation through multi-sensor complementarity, and still maintains stable output under complex backgrounds such as vibration, rain and fog, and low light.

[0007] To achieve the above object, the technical solution of the present invention is realized as follows:

[0008] A road segmentation and slope estimation method based on multi-sensor fusion, specifically including the following steps:

[0009] S1: Register the camera and the lidar;

[0010] S2: Generate a road segmentation result based on the output data of the registered camera and lidar;

[0011] S3: Obtain a three-dimensional point cloud image of the road surface based on the road segmentation result, and implement slope estimation of the road surface based on the three-dimensional point cloud image.

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

[0013] S11: Use ApproximateTimeSynchronizer to set an approximate time, match the timestamps of the output data of the camera and the lidar based on the approximate time, and achieve time synchronization of the camera and the lidar;

[0014] S12: Use a checkerboard as a planar target, and simultaneously collect data of the planar target using the camera and the lidar to obtain a camera image and a lidar point cloud correspondingly;

[0015] S13: Extract the feature points of the planar target from the camera image, and detect the black and white corner points of the planar target based on the Harris corner detection algorithm;

[0016] In the lidar point cloud, use the RANSIC plane fitting algorithm to fit the point cloud plane of the planar target, and calculate the transformation relationship between the point cloud plane and the plane where the black and white corner points are located through the following formula to achieve spatial synchronization of the camera and the lidar and complete the registration of the camera and the lidar:

[0017] ;

[0018]

[0019]

[0020] Among them, are respectively the angles of rotation of the point cloud around the x-axis, y-axis, and z-axis of the lidar coordinate system, R is the rotation matrix, T is the translation matrix, ( , , ) is the coordinate position of the point cloud before rotation corresponding to the camera coordinate system, ( , , ) is the coordinate position of the point cloud after rotation corresponding to the lidar coordinate system, ( , , )(respectively represent the translation vectors of the point cloud in the x, y, and z directions, ( , , )) respectively represent the matrices for the point cloud to rotate around the x, y, and z axes.

[0021] Furthermore, step S2 specifically includes the following steps:

[0022] S21: Preprocess the lidar point cloud and the camera image respectively, and correspondingly obtain a point cloud sparse map and a corrected image;

[0023] S22: Obtain a trained lightweight segmentation model, input the point cloud sparse map and the corrected image into the lightweight segmentation model for processing, obtain a point cloud confidence map and an image confidence map, and perform result-level fusion on the point cloud confidence map and the image confidence map to obtain a binary result;

[0024] S23: Construct a LiDAR confidence evaluation formula and a visual confidence evaluation formula, and perform weight assignment on the point cloud confidence map and the image confidence map according to the LiDAR confidence evaluation formula and the visual confidence evaluation formula to obtain a road segmentation result.

[0025] Furthermore, in step S21, the steps for preprocessing the lidar point cloud to obtain a point cloud sparse map include:

[0026] S21A1: Take the x-axis position, y-axis position, and z-axis position of each point in the lidar point cloud;

[0027] S21A2: Horizontally splice the rotation matrix and the translation vector to obtain a 4×3 transformation matrix, and expand the bottom of the transformation matrix to obtain a 4x4 homogeneous transformation matrix;

[0028] S21A3: After transforming each point in the lidar point cloud processed in step S21A1 through the transposed homogeneous transformation matrix, take the first 3 column elements of the transformed matrix as the point cloud transformation matrix, and use the elements of the point cloud transformation matrix as the new 3D coordinates corresponding to each point in the lidar point cloud;

[0029] The first column element of the point cloud transformation matrix corresponds to the x value of each point in the lidar point cloud, the second column element of the point cloud transformation matrix corresponds to the y value of each point in the lidar point cloud, and the third column element of the point cloud transformation matrix corresponds to the z value of each point in the lidar point cloud;

[0030] S21A4: Remove all points in the point cloud transformation matrix with negative x values. Take the first two column elements of the point cloud transformation matrix after removing the corresponding points as two-dimensional image coordinate values. Use the camera internal parameters and distortion parameters to convert the two-dimensional image coordinate values into a 2D image coordinate matrix, and round the values of the 2D image coordinate matrix;

[0031] S21A5: Based on the z values of each point in the point cloud transformation matrix and the 2D image coordinate matrix, construct a depth map, and perform normalization processing on the depth map so that the values of the pixel points included in the depth map are in the range of 0 to 255, obtaining a sparse point cloud map;

[0032] The steps for preprocessing the camera image to obtain a corrected image include:

[0033] S21B1: Based on checkerboard images taken from multiple angles, use the Zhang Zhengyou calibration algorithm to calculate the camera internal parameter matrix and distortion coefficients, and establish the mapping relationship between the camera coordinate system and the image coordinate system constructed using the camera images taken by the camera;

[0034] S21B2: Based on the calibration results of step S21B1, perform real-time correction on the camera image using bilinear interpolation to obtain a corrected image:

[0035] ;

[0036] ;

[0037] ;

[0038] where r is the Euclidean distance from the pixel point with two-dimensional coordinates in the camera image to the center of the camera image, is the two-dimensional coordinate of the corrected image, and are the radial distortion coefficients of the camera, and are the tangential distortion coefficients of the camera.

[0039] Furthermore, the lightweight segmentation model includes an encoder, a decoder, and a multi-scale evidence collection module. Input the corrected image and the sparse point cloud map into the encoder-decoder structure for feature extraction, and correspondingly obtain a first feature map and a second feature map. Input the first feature map and the second feature map into the multi-scale evidence collection module for processing, and obtain a binary result after result-level fusion;

[0040] The multi-scale evidence collection module includes a first branch and a second branch with the same network structure. The first branch includes a first sub-branch, a second sub-branch, and a third sub-branch. The first sub-branch, the second sub-branch, and the third sub-branch all contain a convolution layer, an upsampling layer, and a softplus activation layer connected in sequence. Among them, the convolution layer of the first sub-branch is a 1×1 convolution, the convolution layer of the second sub-branch is a 3×3 convolution, and the convolution layer of the third sub-branch is a 5×5 convolution.

[0041] Further, step S23 includes:

[0042] S231: Construct the LiDAR confidence evaluation formula and the visual confidence evaluation formula :

[0043] ;

[0044] ;

[0045] Among them, the fitting residual is the deviation between the predicted value and the true value output by the lightweight segmentation model, E is the probability map entropy value output by the encoder-decoder structure, L is the light intensity, is the maximum light intensity obtained according to prior knowledge;

[0046] S232: Calculate the final road surface through the following formula and use the final road surface as the road segmentation result:

[0047] .

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

[0049] S31: Reproject the road segmentation result back to the point cloud to obtain the three-dimensional point cloud image of the road surface;

[0050] S32: Segment the three-dimensional point cloud image, use the RANSIC algorithm to perform plane fitting on each section of the road surface, and calculate the relative slope angle of each section of the road surface according to the plane normal vector obtained after fitting each section of the road surface :

[0051] ;

[0052] Among them, is the plane normal vector obtained after performing plane fitting on the i-th section of the road surface, is the inverse cosine function.

[0053] Further, if the vehicle-mounted IMU provides the plane normal vector of the vehicle body itself, calculate the absolute slope angle of each section of the road surface through the following formula:

[0054] 。

[0055] Further, in step S31, the road segmentation result is reprojected back to the point cloud by the following formula:

[0056] ;

[0057] ;

[0058] where x and y are the calculated x and y values in the camera coordinate system corresponding to the z value of the current point, u and v are the pixel coordinates in the image coordinate system corresponding to the z value of the current point, P is the camera intrinsic matrix, z is the depth value of the current point, P(0, 0) and P(1, 1) are both the focal lengths of the camera, and P(0, 2) and P(1, 2) are both the principal point positions of the camera.

[0059] Compared with the prior art, the present invention can achieve the following beneficial effects:

[0060] (1) For the road segmentation and slope estimation method based on multi-sensor fusion according to the present invention, the complementarity of multi-sensors effectively reduces the error rate of road surface segmentation and the error of slope estimation; it still maintains stable output in complex environments such as vibration, rain, fog, and low light; it realizes low latency through low-error time synchronization and simplified Lidar input; it supports access to more sensors (such as millimeter-wave radar) to adapt to special scenarios.

[0061] (2) For the road segmentation and slope estimation method based on multi-sensor fusion according to the present invention, it first uses the fusion method of lidar and camera for slope estimation; innovatively transforms the point cloud into a point cloud sparse map, and uses the point cloud sparse map instead of the point cloud as the input of the road segmentation network (lightweight segmentation model), greatly reducing the computational complexity brought by multi-channel data input to the network; by obtaining the confidence degrees of the RGB image and the point cloud data and performing weighted fusion, it can effectively achieve complementary advantages and increase the accuracy of road segmentation. The present invention is of great significance for autonomous driving, robot navigation, intelligent transportation systems, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation to the present invention. In the drawings:

[0063] Figure 1 is a schematic flow chart of the road segmentation and slope estimation method based on multi-sensor fusion according to the embodiment of the present invention;

[0064] Figure 2 Schematic structural diagram of the road segmentation and slope estimation method based on multi-sensor fusion according to the embodiment of the present invention

[0065] Figure 3 Schematic diagram of the transformation from the lidar coordinate system to the camera coordinate system according to the embodiment of the present invention

[0066] Figure 4 Schematic network structure diagram of the lightweight segmentation model according to the embodiment of the present invention

[0067] Figure 5 Schematic diagram of the fusion of the point cloud road surface and the image road surface according to the embodiment of the present invention

[0068] Figure 6 Schematic diagram of the point cloud road surface after segmentation according to the embodiment of the present invention

[0069] Figure 7 Schematic diagram of the calculation of the absolute slope angle according to the embodiment of the present invention Detailed implementation manners

[0070] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, rather than limiting the present invention.

[0071] It should be noted that, without conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0072] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "center", "longitudinal", "lateral", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as limiting the present invention. In addition, the terms "first", "second", etc. are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined with "first", "second", etc. may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more.

[0073] In the description of the present invention, it should be noted that unless otherwise clearly specified and limited, the terms "installation", "connection", and "coupling" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two components. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific situations.

[0074] The present invention will be described in detail below with reference to the accompanying drawings and in conjunction with embodiments.

[0075] As Figure 1 - Figure 2 shown, the present invention proposes a road segmentation and slope estimation method based on multi-sensor fusion, which specifically includes the following steps: S1: Register the camera and the lidar; S2: Generate a road segmentation result based on the output data of the registered camera and lidar; S3: Obtain a three-dimensional point cloud image of the road surface based on the road segmentation result, and realize the slope estimation of the road surface based on the three-dimensional point cloud image.

[0076] The present invention realizes the efficient fusion of these two sensors in time and space by synchronously registering the lidar and camera data. The lidar can provide rich depth information, while the camera captures detailed image texture information. By combining these two types of information, the road surface can be more accurately identified and segmented. In the process of realizing road surface segmentation, first, the multi-source data is preprocessed and optimized to ensure the accurate alignment of the lidar and camera data in the same coordinate system. Then, using the texture features in the image, the road and other objects can be effectively distinguished, thereby extracting a clear road area. Next, the segmented road surface is back-projected and converted back to the three-dimensional space. This process can not only retain the geometric features of the road surface but also provide accurate basic data for subsequent three-dimensional point cloud processing. On this basis, a plane fitting analysis is performed on the three-dimensional point cloud to identify the plane structure of the road. Finally, by analyzing the plane fitting result, the slope angle of the road ahead can be calculated.

[0077] That is, the present invention first realizes the collaborative work of multi-sensors, including camera-lidar time synchronization, camera-lidar spatial registration, and camera-lidar joint calibration; secondly, road segmentation is performed based on the data aligned in time and space, including RGB images (camera images) and point cloud sparse maps, and the fusion of multi-source heterogeneous data adopts a result-level fusion strategy. Then, the two-dimensional "RGB image + point cloud sparse map" fusion segmentation result is back-projected into a three-dimensional point cloud to obtain a three-dimensional point cloud image of the road; finally, the road point cloud plane is fitted, the normal vector of the road plane is calculated, and the slope angle of the road ahead is obtained.

[0078] In some embodiments, step S1 specifically includes the following steps:

[0079] S11: Use the ApproximateTimeSynchronizer to set an approximate time, and based on the approximate time, match the timestamps of the output data of the camera and the lidar respectively to achieve time synchronization between the camera and the lidar;

[0080] S12: Use a checkerboard as a planar target, and use the camera and the lidar to collect data from the planar target simultaneously to obtain the camera image and the lidar point cloud respectively;

[0081] S13: Extract the feature points of the planar target from the camera image, and detect the black and white corner points of the planar target based on the Harris corner detection algorithm;

[0082] In the lidar point cloud, use the RANSIC plane fitting algorithm to fit the point cloud plane of the planar target, and calculate the transformation relationship between the point cloud plane and the plane where the black and white corner points are located through the following formula to achieve spatial synchronization between the camera and the lidar and complete the registration of the camera and the lidar:

[0083] ;

[0084]

[0085]

[0086] Where are respectively the angles of rotation of the point cloud around the x-axis, y-axis, and z-axis of the lidar coordinate system, R is the rotation matrix, T is the translation matrix, ( , , ) is the coordinate position of the point cloud before rotation in the camera coordinate system, ( , , ) is the coordinate position of the point cloud after rotation in the lidar coordinate system, ( , , ) respectively represent the translation vectors of the point cloud in the x-direction, y-direction, and z-direction, and ( , , ) respectively represent the matrices of the point cloud rotating around the x-axis, y-axis, and z-axis.

[0087] It should be noted that in order to prevent inconsistent scene information from being obtained between the lidar and the camera, which may lead to errors in the subsequent data fusion and segmentation processes, it is necessary to perform time and space synchronization processing on these two sensors (the camera and the lidar, that is, multi-sensors).

[0088] In terms of time synchronization, since there may be different delays in data acquisition by these two sensors, the images captured by the two sensors will be inconsistent in a high-speed motion scenario. Time synchronization can avoid data inconsistency caused by time differences, thereby improving the accuracy of subsequent fusion.

[0089] Adopt a soft synchronization method for time alignment. The tool used is the message filter message_filters of the ROS system. The message filter message_filters is similar to a message cache. When a message arrives at the message filter, it will not be output immediately, but at a later time point and under certain conditions. In the present invention, the nearest neighbor frame is found by looking for adjacent timestamps. The method adopted is to use ApproximateTimeSynchronizer (approximate time synchronizer) for time matching. Since the possibility that the timestamps of different sensors are the same when collecting data is close to zero, it is necessary to set a time threshold and use the approximate time synchronizer to match the messages of two sensors with a time error less than the given threshold. The proximity time can be customized, rather than the absolute time being exactly the same. In this way, time alignment can be performed through the approximate time synchronizer to achieve the purpose of synchronous callback output.

[0090] In terms of space synchronization, first perform camera feature extraction: Use a checkerboard as a planar target, collect the image of the planar target by the camera, extract the feature points of the planar target from the camera image, and detect the black and white corner points in the checkerboard through the Harris corner detection algorithm. The formula is as follows:

[0091] ;

[0092] where R is the response of the Harris corner detection algorithm and is used as a corner judgment flag bit, , is the determinant of the matrix, is the trace of the matrix, and are the change components in two orthogonal directions, and k is an empirical constant in the range (0.04, 0.06).

[0093] In terms of space synchronization, feature extraction of the lidar: Extract the three-dimensional points corresponding to the camera feature points from the lidar point cloud, and fit the plane of the calibration target through the RANSIC plane fitting algorithm.

[0094] Build the coordinate transformation relationship between the lidar and the camera, and use the least squares method to estimate the extrinsic parameters (rotation matrix and translation vector) between the camera and the lidar, which is achieved by minimizing the distance error between the reprojection error of the pixel points in the camera image and the point cloud of the lidar.

[0095] As Figure 3 shown, the coordinate transformation relationship between the lidar and the camera is as follows:

[0096] ;

[0097] ;

[0098] ;

[0099] Wherein, are respectively the angles of the point cloud rotating around the x-axis, y-axis and z-axis of the lidar coordinate system, R is the rotation matrix, T is the translation matrix, ( , , ) is the coordinate position of the point cloud before rotation corresponding to the camera coordinate system, ( , , ) is the coordinate position of the point cloud after rotation corresponding to the lidar coordinate system, ( , , ) respectively represent the translation vectors of the point cloud in the x, y and z directions, ( , , ) respectively represent the matrices of the point cloud rotating around the x-axis, y-axis and z-axis.

[0100] In some embodiments, step S2 specifically includes the following steps:

[0101] S21: Preprocess the lidar point cloud and the camera image respectively to obtain a point cloud sparse map and a corrected image;

[0102] S22: Obtain a trained lightweight segmentation model, input the point cloud sparse map and the corrected image into the lightweight segmentation model for processing, obtain a point cloud confidence map and an image confidence map, and perform result-level fusion on the point cloud confidence map and the image confidence map to obtain a binary result;

[0103] S23: Construct a LiDAR confidence evaluation formula and a visual confidence evaluation formula, and perform weight assignment on the point cloud confidence map and the image confidence map according to the LiDAR confidence evaluation formula and the visual confidence evaluation formula to obtain a road segmentation result.

[0104] It should be noted that in order to effectively estimate the slope of the road surface, the road area is segmented to exclude other interferences. Specifically: First, the data obtained by the camera and lidar are preprocessed respectively to reduce the computational complexity and improve the data quality; then a dual-branch structure network (lightweight segmentation model) is designed. By extracting and learning features from the RGB image (camera image) and the sparse point cloud map, a point cloud confidence map and an image confidence map are obtained; finally, the two confidences are fused at the result level, and finally a fused road surface segmentation result is generated.

[0105] Point cloud data preprocessing: Since the amount of point cloud data obtained by lidar is huge, for tasks with high real-time requirements such as autonomous driving, directly processing the point cloud data will result in slow processing speed. Therefore, by preprocessing the three-dimensional point cloud, it is converted from a three-dimensional form to a two-dimensional sparse point cloud map, reducing the computational complexity while retaining useful information.

[0106] The specific steps for preprocessing the lidar point cloud to obtain the sparse point cloud map include:

[0107] a. Preprocess the obtained lidar point cloud data. The common point cloud data has 4 bits, namely: the x-axis position, the y-axis position, the z-axis position, and the reflectivity. The reflectivity does not need to be reflected in the sparse point cloud map, so only the first three bits are taken during the data preprocessing.

[0108] b. Obtain the calibration information and construct the transformation matrix: Horizontally concatenate the rotation matrix and the translation vector to form a 4x3 transformation matrix. Expand it at the bottom to extend it to a 4x4 homogeneous transformation matrix to apply to 3D transformations.

[0109] The purpose of expanding the matrix is to enable it to be calculated with other matrices. To avoid the influence of the expanded values on matrix operations, 1 should be filled in the main diagonal position and 0 in the remaining positions. Here, the fourth row elements to be expanded are [0, 0, 0, 1].

[0110] c. Convert the point cloud coordinate system to the camera coordinate system: Apply the rotation and translation transformation to the point cloud using matrix multiplication, and transform each point of the lidar point cloud through the transposed homogeneous transformation matrix. Extract the first 3 columns of the transformed result to represent the new 3D coordinates.

[0111] d. Since the camera's view is forward, the point cloud behind naturally cannot be projected onto the camera. Therefore, remove the points in the point cloud that are behind the camera, that is, remove the points with negative X in the new 3D coordinates.

[0112] e. Convert the 3D points to 2D image coordinates using the camera intrinsic parameters and distortion parameters. Here, the rotation and translation are both zero because they have been incorporated in the previous steps. Then, convert the rotated point cloud to integer values by rounding.

[0113] f. Obtain the pixel values corresponding to the depth values based on the coordinate values of the third dimension in the transformed point cloud matrix. Generate a depth map of the same size as the image, and normalize this depth map so that its values are in the range of 0 to 255, i.e., generate the sparse point cloud map required by the network.

[0114] Preprocess the camera image:

[0115] Collect checkerboard images taken from multiple angles, and calculate the camera's intrinsic matrix and distortion coefficients (radial distortion and tangential distortion and ) through Zhang Zhengyou calibration algorithm, and establish the mapping relationship between the camera coordinate system and the image coordinate system.

[0116] Based on the calibration results (part of which is the camera's intrinsic matrix and distortion coefficients, and the other part is the mapping relationship between the camera coordinate system and the image coordinate system (established based on the images captured by the camera)), perform real-time correction on the camera image using bilinear interpolation:

[0117] [[ID=2)4]];

[0118] ;

[0119] ;

[0120] where r is the Euclidean distance from the pixel point with two-dimensional coordinates in the camera image to the center of the camera image, is the two-dimensional coordinate of the corrected image, and are the radial distortion coefficients of the camera, and are the tangential distortion coefficients of the camera.

[0121] Next, reduce the noise in the corrected image by smoothing the pixel values or using local neighborhood information, thereby retaining the main features and details of the corrected image. This denoising step helps subsequent image processing tasks and enables higher-precision image segmentation.

[0122] In some embodiments, the lightweight segmentation model includes an encoder, a decoder, and a multi-scale evidence collection module. The corrected image and the sparse point cloud map are input into the encoder-decoder structure for feature extraction, and the first feature map and the second feature map are obtained correspondingly. The first feature map and the second feature map are input into the multi-scale evidence collection module for processing, and a binary result is obtained after result-level fusion.

[0123] The multi-scale evidence collection module includes a first branch and a second branch with the same network structure. The first branch includes a first sub-branch, a second sub-branch, and a third sub-branch. The first sub-branch, the second sub-branch, and the third sub-branch all include a convolution, an upsampling layer, and a softplus activation layer connected in sequence. Among them, the convolution of the first sub-branch is a 1×1 convolution, the convolution of the second sub-branch is a 3×3 convolution, and the convolution of the third sub-branch is a 5×5 convolution.

[0124] Further, as Figure 4 shown, the lightweight segmentation model is designed based on the encoder-decoder structure. In the encoder part, a feature extraction network based on depthwise separable convolution optimized V3 (MobileNet-V3) is adopted. In addition, an attention mechanism CBAM module (Convolutional Block Attention Module) is introduced in the decoder to improve the recovery ability of road edge details. The encoder-decoder structure is the same as the U-net structure. The sparse point cloud map and the corrected RGB (corrected image) are input into the feature extraction network (parameters are not shared) in parallel to obtain corresponding feature maps. The two feature maps are respectively fed into the multi-scale evidence collection module to generate the road segmentation results of the point cloud and the image. The multi-scale evidence collection module consists of three parallel branches. In each branch, first, convolution is used to obtain feature maps of two channels with multiple scales. The convolutions in these branches are 1×1, 3×3, and 5×5 respectively. Then, an upsampling layer is used to enlarge the two-channel feature map to the size of the original image. Finally, a softplus activation layer is used in the branch to ensure that the output result is non-negative. The result-level fusion is represented by a binary image, that is, the road is 1 and the non-road is 0. The road area and the non-road area of the result-level fusion are determined by the binary result, and the road area is divided into a point cloud confidence map and an image confidence map based on the characteristics of the point cloud and the RGB image.

[0125] The confidence calculations of the road segmentation results of the point cloud and the image are respectively performed (the calculation method is as follows) to obtain the corresponding fusion weights, and then the final road segmentation result is calculated according to the respective fusion weights of the two.

[0126] 1) LiDAR confidence evaluation formula:

[0127] Calculate the LiDAR confidence according to the point cloud density (such as points / m²) and the fitting residual :

[0128] ;

[0129] Among them, the fitting residual is the deviation between the predicted value and the true value output by the lightweight segmentation model, and the fitting residual threshold is set according to user requirements.

[0130] 2) Visual confidence evaluation formula:

[0131] Based on the probability map entropy value output by the lightweight segmentation model and the illumination condition calculate the visual confidence :

[0132] ;

[0133] Among them, E is the probability map entropy value output by the encoder-decoder structure (the sum of the entropy values calculated from the first feature image and the second feature image respectively), L is the illumination intensity, is the maximum illumination value obtained according to prior knowledge.

[0134] As Figure 5 shown, determine the result-level fusion weight assignment according to the confidence calculation result, and the final road surface segmentation result calculation method is as follows:

[0135] .

[0136] In some embodiments, step S3 specifically includes the following steps:

[0137] S31: Reproject the road segmentation result back to the point cloud to obtain the three-dimensional point cloud image of the road surface;

[0138] S32: Segment the three-dimensional point cloud image, and use the RANSIC algorithm to perform plane fitting on each section of the road surface, and calculate the relative slope angle of each section of the road surface according to the plane normal vector obtained after fitting each section of the road surface :

[0139] ;

[0140] Among them, is the plane normal vector obtained after performing plane fitting on the i-th section of the road surface, is the inverse cosine function.

[0141] The slope estimation consists of three steps: the first step is point cloud back-projection, the second step is plane fitting, and the third step is slope estimation. The specific implementation process is as follows:

[0142] 1) Point cloud back-projection

[0143] As Figure 6 shown, the segmentation result of the final road surface is a two-dimensional mask, lacking depth information. Therefore, slope estimation cannot be performed yet. So, it is necessary to re-project it back into the point cloud and use the three-dimensional measurement values of the point cloud to estimate the slope of the road ahead. The specific process is as follows:

[0144] First, according to the projection matrix P of the given camera, the depth value z, and the pixel coordinates u, v (i.e., the pixel coordinates of the road surface after road segmentation), calculate the three-dimensional coordinates x and y in the camera coordinate system corresponding to the depth value z. The formula is:

[0145] ;

[0146] ;

[0147] where x and y are the calculated x value and y value in the camera coordinate system corresponding to the z value of the current point, u and v are the pixel coordinates in the image coordinate system corresponding to the z value of the current point, P is the camera internal parameter matrix, z is the depth value of the current point, P(0, 0) and P(1, 1) are both the focal lengths of the camera, and P(0, 2) and P(1, 2) are both the principal point positions of the camera.

[0148] Convert the 2D coordinate system of the road surface to the 3D coordinate system of the camera through the above formula. Then, stack all the pixel points of the calculated three-dimensional coordinates into a two-dimensional array to form points in the camera coordinate system. Invert the rotation matrix and translation matrix obtained through spatial synchronization, and transform the points in the obtained camera coordinate system with this matrix to the lidar coordinate system. Through the above method, generate the final three-dimensional point coordinates, and the returned result only contains the three coordinate components of x, y, and z.

[0149] In this way, the point cloud information only containing the road surface can be obtained. At the same time, the point cloud points of non-road surfaces can be effectively removed, reducing the complexity of fitting the road surface in subsequent slope estimation.

[0150] 2) Plane fitting + slope estimation

[0151] The three-dimensional point cloud image obtained through back-projection is segmented according to the distance d. For example , generally, the higher the segmentation accuracy, the more accurate the slope estimation. Use the RANSIC algorithm to perform plane fitting on each section of the road surface. The normal vector of the fitted plane is , and at this time, the relative slope angle of the slope ahead can be calculated :

[0152] .

[0153] As Figure 7As shown, if the vehicle-mounted IMU provides the normal vector of the vehicle body's own plane , the absolute slope angle of each section of the road surface is calculated by the following formula :

[0154] .

[0155] It should be understood that the various forms of the process shown above can be used, with steps reordered, added or deleted. For example, the steps described in the disclosure of the present invention can be executed in parallel, sequentially or in a different order, as long as the desired results of the technical solution disclosed in the present invention can be achieved, and no limitation is made herein.

[0156] The above specific embodiments do not constitute a limitation to the protection scope of the present invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention shall be included within the protection scope of the present invention.

Claims

1. A road segmentation and slope estimation method based on multi-sensor fusion, characterized in that, Specifically, it includes the following steps: S1: Register the camera and the lidar; S2: Generate a road segmentation result based on the output data of the registered camera and lidar; S21; Preprocess the lidar point cloud and the camera image respectively, and obtain a point cloud sparse map and a corrected image correspondingly; S22: Obtain a trained lightweight segmentation model, input the point cloud sparse map and the corrected image into the lightweight segmentation model for processing, obtain a point cloud confidence map and an image confidence map, and perform result-level fusion on the point cloud confidence map and the image confidence map to obtain a binary result; S23: Construct a LiDAR confidence evaluation formula and a visual confidence evaluation formula, and perform weight assignment on the point cloud confidence map and the image confidence map according to the LiDAR confidence evaluation formula and the visual confidence evaluation formula to obtain a road segmentation result; S231: Construct the LiDAR confidence evaluation formula and the visual confidence evaluation formula : ； ； Among them, the fitting residual is the deviation between the predicted value and the true value output by the lightweight segmentation model, E is the probability map entropy value output by the encoder-decoder structure, and L is the light intensity, which is the maximum light intensity obtained according to prior knowledge; S232: Calculate the final road surface through the following formula, and use the final road surface as the road segmentation result: ； S3: Obtain a three-dimensional point cloud image of the road surface based on the road segmentation result, and implement slope estimation of the road surface based on the three-dimensional point cloud image.

2. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 1, wherein: Step S1 specifically includes the following steps: S11: Use ApproximateTimeSynchronizer to set an approximate time, match the timestamps of the output data of the camera and the lidar based on the approximate time, and achieve time synchronization of the camera and the lidar; S12: Use a checkerboard as a planar target, and use the camera and the lidar to collect data from the planar target simultaneously, and obtain a camera image and a lidar point cloud correspondingly; S13: Extract the feature points of the planar target from the camera image, and detect the black and white corner points of the planar target based on the Harris corner detection algorithm; In the lidar point cloud, use the RANSIC plane fitting algorithm to fit the point cloud plane of the planar target, calculate the transformation relationship between the point cloud plane and the plane where the black and white corner points are located through the following formula, achieve spatial synchronization of the camera and the lidar, and complete the registration of the camera and the lidar: ； wherein, are respectively the angles of the point cloud rotating around the x-axis, y-axis, and z-axis of the lidar coordinate system, R is the rotation matrix, T is the translation matrix, ( , , ) is the coordinate position of the point cloud before rotation in the camera coordinate system, ( , , ) is the coordinate position of the point cloud after rotation in the lidar coordinate system, ( , , ) respectively represent the translation vectors of the point cloud in the x-direction, y-direction, and z-direction, ( , , ) respectively represent the matrices of the point cloud rotating around the x-axis, y-axis, and z-axis.

3. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 1, wherein: In step S21, the steps of preprocessing the lidar point cloud to obtain a point cloud sparse map include: S21A1: Take the x-axis position, y-axis position, and z-axis position of each point in the lidar point cloud; S21A2: Horizontally splice the rotation matrix and the translation vector to obtain a 4×3 transformation matrix, and expand the bottom of the transformation matrix to obtain a 4x4 homogeneous transformation matrix; The specific method of expanding the bottom of the transformation matrix is to fill 1 at the main diagonal position of the bottom of the transformation matrix, and fill 0 at the remaining positions; S21A3: After transforming each point in the lidar point cloud processed in step S21A1 through the transposed homogeneous transformation matrix, take the first 3 column elements of the transformed matrix as the point cloud transformation matrix, and use the elements of the point cloud transformation matrix as the new 3D coordinates corresponding to each point in the lidar point cloud; The first column element of the point cloud transformation matrix corresponds to the x value of each point in the lidar point cloud, the second column element of the point cloud transformation matrix corresponds to the y value of each point in the lidar point cloud, and the third column element of the point cloud transformation matrix corresponds to the z value of each point in the lidar point cloud; S21A4: Remove all points with negative x values in the point cloud transformation matrix. Take the first two column elements of the point cloud transformation matrix after removing the corresponding points as two-dimensional image coordinate values. Use the camera internal parameters and distortion parameters to convert the two-dimensional image coordinate values into a 2D image coordinate matrix, and round the values in the 2D image coordinate matrix; S21A5: Construct a depth map based on the z values of each point in the point cloud transformation matrix and the 2D image coordinate matrix, and perform normalization processing on the depth map so that the values of the pixel points included in the depth map are in the range of 0 to 255, obtaining a point cloud sparse map; The steps for preprocessing the camera image to obtain a corrected image include: S21B1: Based on checkerboard images taken from multiple angles, use the Zhang Zhengyou calibration algorithm to calculate the camera internal parameter matrix and distortion coefficients, and establish the mapping relationship between the camera coordinate system and the image coordinate system; S21B2: Based on the calibration results of step S21B1, use bilinear interpolation to perform real-time correction on the camera image to obtain a corrected image: ； ； ； where r is the Euclidean distance from the pixel point with two-dimensional coordinates in the camera image to the center of the camera image, is the two-dimensional coordinate of the corrected image, and are the radial distortion coefficients of the camera, and are the tangential distortion coefficients of the camera.

4. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 1, wherein: The lightweight segmentation model includes an encoder-decoder structure and a multi-scale evidence collection module. Input the corrected image and the point cloud sparse map into the encoder-decoder structure for feature extraction, and correspondingly obtain a first feature map and a second feature map. Input the first feature map and the second feature map into the multi-scale evidence collection module for processing, and obtain a binary result after result-level fusion; The multi-scale evidence collection module includes a first branch and a second branch with the same network structure. The first branch includes a first sub-branch, a second sub-branch, and a third sub-branch. The first sub-branch, the second sub-branch, and the third sub-branch all contain a convolution, an upsampling layer, and a softplus activation layer connected in sequence. Among them, the convolution of the first sub-branch is a 1×1 convolution, the convolution of the second sub-branch is a 3×3 convolution, and the convolution of the third sub-branch is a 5×5 convolution.

5. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 1, wherein: Step S3 specifically includes the following steps: S31: Reproject the road segmentation result back to the point cloud to obtain a three-dimensional point cloud image of the road surface; S32: Segment the three-dimensional point cloud image, use the RANSIC algorithm to perform plane fitting on each section of the road surface, and calculate the relative slope angle of each section of the road surface according to the plane normal vector obtained after fitting each section of the road surface : ； Among them, is the plane normal vector obtained after performing a plane fitting on the i-th road surface, is the arccosine function.

6. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 5, characterized in that: If the in-vehicle IMU provides the normal vector of the vehicle body's own plane , calculate the absolute slope angle of each section of the road surface through the following formula : 。 7. The method for road segmentation and slope estimation based on multi-sensor fusion according to claim 5, characterized in that: In step S31, reproject the road segmentation result back to the point cloud through the following formula: ； ； where x and y are the calculated x value and y value in the camera coordinate system corresponding to the z value of the current point, u and v are the pixel coordinates in the image coordinate system corresponding to the z value of the current point, P is the camera internal parameter matrix, z is the depth value of the current point, P(0, 0) and P(1, 1) are both the focal lengths of the camera, and P(0, 2) and P(1, 2) are both the principal point positions of the camera.

Images