Front road gradient estimation method based on multivariate information fusion

Abstract

The invention discloses a front road gradient estimation method based on multivariate information fusion. Relates to the technical field of intelligent driving, in particular to a front road slope estimation method based on multivariate information fusion. According to the method, road segmentation is performed through an improved Patchwork + + model, background and interference point clouds are eliminated, ground effective point cloud data information is extracted, and a road slope value is calculated based on the ground point cloud data information. The method comprises the following steps: performing time synchronization and space registration on a camera and a laser radar to obtain a multi-source information data set; the method comprises the following steps: constructing and improving a Patchwork + + model to obtain an improved Patchwork + + model: processing a multi-source information data set through the improved Patchwork + + model to obtain a coordinate # imgabs0 # of a ground point cloud; and calculating a road slope value according to the coordinate # imgabs1 # of the ground point cloud.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and particularly to a method for estimating the slope of the road ahead based on multi-source information fusion in the technical field. Background Art

[0002] As a core technology for intelligent driving environment perception, the accuracy of road slope estimation directly determines vehicle power distribution, energy recovery efficiency, and driving safety margins. Early slope analysis relied on manual measurement or topographic map calculation, which had problems such as low efficiency, strong subjectivity, and insufficient accuracy. With the development of intelligent driving technology, vehicles need to obtain road slopes in real time to optimize power distribution and braking control. To improve accuracy and robustness, heterogeneous data fusion solutions based on cameras and lidar have been studied in recent years, solving the perception blind spots of traditional methods in complex scenarios and improving road slope estimation capabilities.

[0003] Existing slope estimation schemes include methods based on dynamic models and Kalman filtering, machine learning and data-driven methods, geographic information systems and remote sensing technology methods, and multi-sensor fusion methods. The following is a comparison of several schemes: For the method based on dynamic models and Kalman filtering, a state space model is established through the vehicle longitudinal dynamics equation, and the Kalman filter is used to iteratively optimize the slope estimation value. It has strong real-time performance, but parameter drift leads to error accumulation, and the accuracy decreases under non-steady state conditions; For machine learning and data-driven methods, a feedforward neural network is used to process multi-source sensor data, combined with an extended state observer for dynamic correction, with strong adaptive capabilities, but a large amount of labeled data is required for training, resulting in high costs; For geographic information systems and remote sensing technology methods, the regional slope is calculated through spatial analysis based on a digital elevation model, with high efficiency, but poor real-time performance and not suitable for dynamic vehicle control; For multi-sensor fusion methods, such as fusing lidar and cameras, ground features are extracted through point cloud segmentation and image semantic analysis, with cross-modal complementarity, improved segmentation accuracy in complex scenarios, effectively solving traditional blind spot problems such as snow cover, and providing a basis for slope-friction coefficient mapping.

[0004] In the doctoral dissertation of the University of Chinese Academy of Sciences, "Research on Key Technologies of Multi-Sensor 3D Environment Perception System for Autonomous Driving" (Wang Jiarong), a multi-sensor 3D object detection method MCF3D (Multi-Stage Complementary Fusion for Multi-sensor 3D Object Detection) model based on multi-stage complementary fusion is proposed, which can be used to fuse 3D point clouds and RGB images; the MCF3D model includes data-level fusion, feature-level fusion, and decision-level fusion.

[0005] The multi-sensor fusion solution is significantly superior to other solutions in terms of accuracy, environmental robustness, and functionality. Based on multi-sensor data fusion, the present invention proposes a method for estimating the slope of the road ahead, effectively solving the pain points existing in traditional slope estimation solutions, and being more sufficient, efficient, and accurate in estimating the slope of the road ahead compared with existing fusion solutions. Summary of the Invention

[0006] Aiming at the problems of large error, low accuracy, high training cost, and poor real-time performance of existing slope estimation methods, the present invention proposes a method for estimating the slope of the road ahead based on multi-source information fusion. The road is segmented through an improved Patchwork++ model, the background and interfering point clouds are excluded, the effective ground point cloud data information is extracted, and the road slope value is calculated based on the ground point cloud data information.

[0007] The method includes the following steps: S1. Synchronize the time and spatially register the camera and lidar to obtain a multi-source information data set; S2. Construct and improve the Patchwork++ model to obtain an improved Patchwork++ model: S21. Improve the Reflection Noise Removal (RNR) module in the Patchwork++ model: Introduce the BiSeNet V2 module and the lossless fine fusion module into the reflection noise removal module; S22. After the Cuboid Zoning Module (CZM) of the Patchwork++ model, introduce the Inverse Perspective Mapping (IPM) module; S23. Introduce a comprehensive weight into the Regional Vertical Plane Fitting (R-VPF) module of the Patchwork++ model; Introduce the Histogram of Oriented Gradients (HOG) module for feature extraction and the Support Vector Machine (SVM) module for classification into the geometric constraint filtering module of the Regional Vertical Plane Fitting (R-VPF) module of the Patchwork++ model; S24. Only retain the TGR time reduction mechanism in the Time Ground Recovery (TGR) module of the Patchwork++ model, replace the binary judgment module with the Gradient Boosting Decision Tree (GBDT) module, and introduce a joint feature construction module before the Gradient Boosting Decision Tree (GBDT) module; S25. In the Adaptive Ground Likelihood Estimation (A-GLE) module of the Patchwork++ model, replace the single threshold judgment module with an energy function optimization module, and introduce a dynamic filtering and anti-noise structure after the energy function optimization module; S26. Introduce a spatial recovery enhancement module after the Adaptive Ground Likelihood Estimation (A-GLE) module in the Patchwork++ model; S3. Process the multi-source information dataset through the improved Patchwork++ model to obtain the coordinates of the ground point cloud. ; S4. Calculate the road slope value according to the coordinates of the ground point cloud. , calculate the road slope value.

[0008] Furthermore, the time synchronization and spatial registration of the camera and lidar are specifically as follows: On the computer side, drive the camera and lidar sensors based on the ROS platform, and unify the sensor sampling moments through an external trigger signal to achieve time synchronization; Independently calibrate and jointly calibrate the lidar and camera, calculate the internal parameter matrix of each sensor and the external parameter matrix between the sensors; Align the 3D point cloud of the lidar and the pixels of the camera RGB image to achieve spatial registration.

[0009] Furthermore, the role of the BiSeNet V2 module is: segment the RGB image to obtain the road surface and non-road surface probability maps; The role of the lossless fine fusion module is: project the 3D point cloud onto the RGB image plane and suppress the intensity of high-reflection points in the non-ground area.

[0010] Furthermore, the comprehensive weight is specifically: , and both represent undetermined coefficients, , represents the reflection intensity weight, represents the distance weight; The role of the comprehensive weight is: point cloud reflection weight distribution and probability sampling optimization.

[0011] Furthermore, the joint feature construction module sequentially passes through image superpixel segmentation, point cloud geometric feature extraction, and joint feature construction from input to output.

[0012] Furthermore, the energy function optimization module constructs a CRF four-element energy function; The CRF four-element energy function includes a unary term, a flatness constraint term, a smooth term, and a reflection intensity constraint term; The dynamic filtering and anti-noise structure passes through the formula: Dynamically adjust the reflection intensity variance threshold , where represents the th adjusted reflection intensity variance threshold, represents the critical proportion threshold of the waterlogging / snowy area mask area, represents the control threshold, represents the proportion of the waterlogging area, represents the waterlogging area, represents the complete area.

[0013] Furthermore, the spatial restoration enhancement module is specifically as follows: First, detect the low-density area, and then implement cross-modal point cloud completion through GAN.

[0014] Furthermore, the calculation of the road slope value according to the coordinates of the ground point cloud is implemented through the RANSAC algorithm, specifically as follows: S801. Initialize the RANSAC parameters; S802. Randomly sample and fit the plane equation: , where represents the normal vector , represents the constant term of the plane equation; S803. Calculate the road slope value according to the formula: Calculate the road slope value.

[0015] The beneficial effects of the method of the present invention are as follows: (1) The method of the present invention uses the improved Patchwork++ model to process 3D point clouds and RGB images simultaneously. The improved Patchwork++ model has a segmentation technology with high precision, low computational complexity, and strong anti-noise ability, breaking through the technical bottleneck that the existing Patchwork++ model cannot efficiently process 3D point clouds and RGB images simultaneously. Moreover, the method of the present invention has strong real-time performance and is applicable to dynamic vehicle control.

[0016] (2) The method of the present invention uses RANSAC plane fitting for slope estimation, significantly improving the accuracy and robustness of slope estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is the flowchart of the method described in Embodiment 1 of the present invention; Figure 2 is the schematic diagram of the process of the improved Patchwork++ model for processing multi-source information data sets described in Embodiment 1 of the present invention; Figure 3 is the schematic diagram of the comparison of the structures before and after the improvement of the regional vertical plane fitting R-VPF module described in Embodiment 1 of the present invention; Figure 4 is the schematic diagram of the comparison of the structures before and after the improvement of the time ground restoration TGR module described in Embodiment 1 of the present invention; Figure 5 is the schematic diagram of the comparison of the structures before and after the improvement of the adaptive ground likelihood estimation A-GLE module described in Embodiment 1 of the present invention; Figure 6 is the flowchart of the method for calculating the road slope value through the RANSAC algorithm described in Embodiment 1 of the present invention; Figure 7Schematic diagram of data-level fusion described in Embodiment 2 of the present invention. Detailed implementation manners

[0018] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0019] This embodiment provides a method for estimating the slope of the front road based on multi-source information fusion. The flowchart of the method is as Figure 1 shown, and the method includes the following steps: S1. Synchronize the time and perform spatial registration of the camera and the lidar to obtain a multi-source information dataset; The following uses a specific example to introduce the relevant operations in step S1: The synchronization of the time and spatial registration of the camera and the lidar is specifically as follows: On the computer side, drive the camera and the lidar sensor based on the ROS platform, and adopt an external trigger method to realize the time sampling synchronization of the monocular camera and the lidar sensor in the embedded intelligent control board; Independently calibrate and jointly calibrate the lidar and the camera, calculate the internal parameter matrix of each sensor and the external parameter matrix between the sensors; align the 3D point cloud of the lidar and the pixels of the RGB image of the camera to achieve the spatial registration of heterogeneous sensors.

[0020] The spatial registration of the heterogeneous sensors includes the following steps: S101. Define four commonly used coordinate systems: the world coordinate system , the camera coordinate system , the image coordinate system and the pixel coordinate system , and calibrate the camera; S102. Establish the conversion matrix between each pixel point in the RGB image and the surface point of the spatial object, so that the RGB image and the 3D point cloud are represented in the same coordinate system; S103. Given any point, find the corresponding image pixel point according to the internal and external parameter matrices to complete the spatial alignment and registration of the 3D point cloud of the lidar to the 2D pixels of the RGB image of the camera. Specifically, it is realized through the following formulas (1) and (2): (1) (2) Wherein, represents the 3D point cloud coordinate in the lidar coordinate system, represents the corresponding to The camera coordinates, denote the extrinsic parameter matrix for transforming from the LiDAR coordinate system to the camera coordinate system, which consists of the rotation matrix and the translation matrix and is composed of denotes the 2D projection matrix that transforms the 3D point cloud in the camera coordinate system into an RGB image in the camera coordinate system.

[0021] S2. Construct and improve the Patchwork++ model to obtain an improved Patchwork++ model: S21. Improve the PNR module for removing reflection noise in the Patchwork++ model: Introduce the BiSeNet V2 module and the lossless fine fusion module into the reflection noise removal module; S22. After the CZM module for spatial partitioning in the Patchwork++ model, introduce the inverse perspective transformation IPM module; S23. Introduce comprehensive weights into the R-VPF module for regional vertical plane fitting in the Patchwork++ model; Introduce the feature extraction HOG module and the classifier SVM module into the geometric constraint filtering module of the R-VPF module for regional vertical plane fitting in the Patchwork++ model; S24. In the TGR module for time ground recovery in the Patchwork++ model, only retain the TGR time reduction mechanism, replace the binary judgment module with the gradient boosting decision tree GBDT module, and introduce a joint feature construction module before the gradient boosting decision tree GBDT module; S25. In the A-GLE module for adaptive ground likelihood estimation in the Patchwork++ model, replace the single threshold judgment module with an energy function optimization module, and introduce a dynamic filtering and anti-noise structure after the energy function optimization module; S26. Introduce a spatial recovery enhancement module after the A-GLE module for adaptive ground likelihood estimation in the Patchwork++ model.

[0022] Introduce the relevant operations in step S2 with a specific example: For example, Figure 2 As shown, the improved reflection noise removal module is named the multi-modal joint denoising module.

[0023] In the original RNR module for removing reflection noise in Patchwork++ (eliminating noise points based on the LIDAR reflection model), parallelly add the RGB semantic segmentation model: the BiSeNet V2 module, and perform pixel (lightweight)-level classification on the RGB image through the BiSeNet V2 module to output a road surface / non-road surface probability map.

[0024] The BiSeNet V2 module first uniformly scales the image to a fixed resolution, maintains the aspect ratio to avoid distortion, and normalizes it according to the mean and variance of ImageNet. Secondly, it adjusts the output layer classification to a binary classification (road surface / non-road surface) and outputs a single-channel probability map. Then, it conducts model training and fine-tuning, outputs an RGB image, and outputs a single-channel probability map (the value range ) represents the road surface probability. At the same time, it binarizes the probability map with a default threshold of 0.5 and uses DenseCRF (Dense Conditional Random Field) to refine the boundaries, combining color similarity and spatial smoothness.

[0025] Then, cross-modal noise suppression is performed: The lossless fine fusion module is used to project the 3D point cloud onto the RGB image plane, align the pixel and point cloud coordinates, and perform intensity suppression on the point cloud in the non-road surface area with a reflection intensity > 0.8 to avoid high-reflection noise interfering with ground detection. Calculate the variance or curvature of the local point cloud surface normal to quantify geometric irregularities.

[0026] Perform Canny edge detection on the corresponding image patch and count the proportion of edge pixels.

[0027] Weightedly fuse roughness and edge density to identify outliers.

[0028] As Figure 2 shown, the spatial partitioning CZM module and the IPM module are combined and named the cross-modal concentric circle partitioning module.

[0029] The spatial partitioning CZM module of the Patchwork++ model (divides the point cloud into multiple fan-shaped regions) defines the coordinate system and sets parameters. A polar coordinate system is established with the vehicle as the center, and the BEV space is divided into three annuli: the inner annulus (inner annulus radius ), the middle annulus (middle annulus radius ), and the outer annulus (outer annulus radius ), and partitions them at fixed angular intervals to enhance direction perception, unify the grid resolution of the RGB image and the 3D point cloud, and increase the height limit of the ground point cloud. In this embodiment, , , ; Next, perform point cloud annulus partitioning: Through formulas (3) and (4), convert the lidar point cloud from the Cartesian coordinate system to the vehicle coordinate system, project it onto the BEV plane ( where, represents the height of the BEV plane), and remove non-ground points whose height exceeds the ground range.

[0030] (3) (4) Among them, , successively represent the inner ring, the middle ring, and the outer ring, represents the center coordinate of the ring in the BEV plane, represents the horizontal azimuth angle of the point cloud in the ring relative to the vehicle's due front (positive X-axis direction); According to the radius the point cloud is divided into the inner, middle, and outer rings, and each ring belt is divided into fan-shaped blocks to enhance the direction perception ability; After CZM partitioning, the RGB image is converted into a BEV view through IPM (Inverse Perspective Mapping): Define BEV control points, and through the corresponding points of the image plane and the BEV plane , among which, represents the BEV control point coordinates. Through formula (5), the homography matrix H is calculated and the matrix accuracy is verified; (5) Use the homography matrix H to perform perspective transformation on the image to generate a BEV view.

[0031] Finally, perform adaptive grid partitioning: Design the grid size, partition the point cloud within each ring belt according to the grid size, and count the point cloud attributes within the grid; Divide the image BEV into blocks according to the same grid size, and extract local features (color histogram, texture, edge density, etc.) to ensure that the image grid corresponds one-to-one with the point cloud grid.

[0032] As Figure 2 and 3 shown, the improved regional vertical plane fitting R-VPF module is named the multi-modal plane fitting module. First, perform weight assignment (comprehensive weight , reflection intensity weight and distance weight ): Normalize the point cloud reflection intensity (Intensity). The higher the reflection intensity, the lower the weight. Points closer to the vehicle are given higher weights. The comprehensive weight is as shown in formula (6): (6) Among them, and both represent undetermined coefficients, , represents the reflection intensity weight, and the reflection intensity weight is as shown in formula (7): (7) (8) represents the maximum value of the point cloud reflection intensity represents the distance weight; the distance weight is shown in formula (8).

[0033] Perform probability sampling according to the weight distribution, calculate the initial plane equation through the RANSAC algorithm, optimize the plane parameters using the weighted least squares method, and retain the ground with the highest score as the final ground model through repeated sampling and scoring functions; Then, perform HOG feature verification on the out-of-plane point cloud: after aligning the point cloud with the image, convert the image to grayscale and enhance the contrast using adaptive histogram equalization; calculate the horizontal and vertical gradients through the Sobel filter , calculate the gradient magnitude of each pixel according to formulas (9) and (10) and direction , discretize the direction into 9 intervals (each interval); (9) (10) Divide the image block into 8×8 cells, calculate the gradient histogram in 9 directions for each cell, form blocks of 2×2 cells, perform L2 normalization on the histogram, and output the HOG feature vector generated for each image block.

[0034] Use the SVM classifier to train the HOG features of obstacles (vehicles, pedestrians) and non-obstacles (ground, vegetation), calculate the cosine similarity between the current HOG feature and the obstacle template, if the similarity is greater than the threshold, determine it as an obstacle; otherwise mark it as noise or unknown object (outlier), and adjust the similarity threshold according to the average brightness of the image block, and use higher resolution image blocks for small targets at a long distance.

[0035] Such as Figure 2 and Figure 4 shown, the improved Time Ground Recovery TGR module is named: Local Ground Joint Verification Module.

[0036] Based on retaining the TGR time reduction mechanism, the method of the present invention adds the following multi-modal modeling and classification capabilities.

[0037] First, perform image superpixel segmentation and feature extraction: With the point cloud projection position as the center, crop the local image patch to cover the area around the point cloud. Use the SLIC (Simple Linear Iterative Clustering) algorithm to segment the image patch into 50 - 100 superpixels, balance color and spatial similarity, and extract color features, texture features, shape features, position features, etc.; Next, extract the point cloud geometric features: The extracted point cloud geometric features include: height residual, local roughness, reflection intensity, and density. Obtain the point cloud spatial distribution based on the point cloud geometric features: the average distance between the point cloud and the nearest adjacent point cloud and the local surface curvature; Construct a joint feature vector, map the point cloud points to the corresponding superpixels, extract all the image features of the superpixels, perform feature stitching, normalize the numerical features according to the mean and variance through Z - score normalization, and normalize the color histogram according to the sum through histogram normalization; Adopt the Gradient Boosting Decision Tree GBDT module to replace the binary judgment in the original scheme, judge whether each point is a real ground point, and synchronously output the ground material type. If the confidence of the GBDT classification result is low, still rely on the flatness statistics of TGR for secondary verification and remove non - ground points. TGR solves the misjudgment of temporary rough ground, and GBDT solves the misjudgment related to materials.

[0038] As Figure 2 and 5 shown, the Adaptive - Ground Likelihood Estimation A - GLE module is named the Adaptive Parameter Optimization module.

[0039] First, initialize the A - GLE statistics, and then construct the CRF four - element energy function through the energy function optimization module: Calculate each energy term according to the features of the current frame, and obtain the CRF four - element energy function by weighted summation. The CRF four - element energy function consists of four parts: unary term, flatness constraint term, smooth term, and reflection intensity constraint term, replacing the single - threshold judgment in the original scheme.

[0040] The unary term fuses the RGB semantic segmentation confidence (CNN probability map), replacing the pure geometric confidence in the original scheme; The flatness constraint term is used to enforce the geometric flatness of the ground in the lane line area, and only acts on the pixel area covered by the lane line mask, imposing a quadratic penalty on the curvature difference between adjacent pixels. The greater the curvature difference, the higher the energy penalty, and the weight parameter is dynamically adjusted according to the scene; The smooth term is used to ensure the spatial continuity of the labels of adjacent pixels. If the colors of adjacent pixels are similar, force their labels to be the same, weighted by a Gaussian kernel function. The smaller the color difference, the stronger the smooth constraint; The reflection intensity constraint term is used to suppress the misdetection of lane lines in abnormal reflection areas. The variance of the reflection intensity is calculated for each superpixel region. If it exceeds the reflection intensity variance threshold , a very large energy penalty is imposed to prohibit this area from being marked as a lane line. When water accumulation / snow is detected, the threshold is automatically relaxed to allow a higher reflection variance.

[0041] Secondly, a new dynamic filtering and anti-noise structure is added, and the parameter dynamic adjustment stage is entered.

[0042] When the curvature variance of the lane line area shows an upward trend within N consecutive frames, it is increased proportionally to enhance the flatness constraint. At the same time, a Kalman filter is added to smooth the weight change to avoid parameter mutations.

[0043] When the coverage area of the water accumulation / snow mask exceeds the preset ratio, according to the proportion of the water accumulation area , the reflection intensity variance threshold is adjusted according to formula (11) : (11) where represents the reflection intensity variance threshold for the th adjustment, represents the critical ratio threshold of the water accumulation / snow mask area, represents the control threshold, which is a parameter (relaxation sensitivity coefficient) that controls the relaxation amplitude of the threshold adjustment, represents the proportion of the water accumulation area, represents the area of the water accumulation area, represents the area of the complete area.

[0044] To avoid drastic jitter of parameters due to single-frame noise, a time consistency protection mechanism is added. The mean and standard deviation of historical parameter values are recorded. If the parameter change in the current frame exceeds the historical mean (standard deviation range): ±3σ, where σ represents the standard deviation, then the historical mean is used to replace the current value, and exponential moving average (EMA) filtering is performed on and .

[0045] Finally, the final lane line detection result and terrain state are output, and the parameter change curve and detection performance index are recorded for offline analysis.

[0046] Patchwork++ does not have a point cloud completion function. To solve this problem, the present invention introduces a spatial recovery enhancement module.

[0047] As Figure 2 shown, first, sparse area detection is performed. The point cloud is divided into voxel grids of 0.2m×0.2m, the number of point clouds in each voxel is calculated, and the density lower than the threshold is marked Voxels are used to perform morphological dilation and merging on adjacent low-density voxels to form candidate regions ; Segment the road surface area of the image to obtain a mask , project the point cloud into the image coordinate system through calibration parameters, and extract the corresponding image patches ; Calculate the SSIM similarity between and typical road surface textures. If SSIM > 0.7 and it is located within , it is determined as a region to be filled, and output: a set of target regions that are sparse and texture-matched { }; Otherwise, it is determined as an irreparable area

[0048] Secondly, through cross-modal GAN point cloud completion, the generator G inputs the sparse point cloud and the corresponding image patches , extracts geometric features through PointNet++ , and then extracts texture features through ResNet-50 , generates hybrid features through a cross-modal attention module , and generates a dense point cloud based on the FoldingNet network ; The discriminator D inputs the dense point cloud , judges the rationality of the global scene layout, the accuracy of the mid-range object contour, and the authenticity of the local surface details, calculates the loss function, and realizes the optimization of the generated point cloud

[0049] S3. Process the multi-source information dataset through the improved Patchwork++ model to obtain the coordinates of the ground point cloud .

[0050] Introduce the relevant operations of step S3 with a specific example Input the multi-source information dataset into the improved Patchwork++ model. The improved Patchwork++ model processes the point cloud and RGB image simultaneously, constructs a concentric cylindrical coordinate system in the BEV space, divides the point cloud into an inner ring, a middle ring, and an outer ring, synchronously converts the RGB image into a BEV view through inverse perspective mapping (IPM), divides the image patches according to the same annulus, performs adaptive grid division on the RGB image for each annulus, excludes the background and interfering point clouds, extracts the effective ground point cloud data information, and obtains the coordinates of the ground point cloud .

[0051] S4. Calculate the road slope value according to the coordinates of the ground point cloud .

[0052] Introduce the relevant operations of step S3 with a specific example As shown Figure 6 below, calculating the road slope value based on the coordinates of the ground point cloud is achieved through the RANSAC algorithm, specifically as follows: S401. Based on the coordinates of the ground point cloud , organize the point cloud data into an N×3 matrix; S402. Initialize the RANSAC parameters: set the maximum number of iterations, inlier threshold, and minimum inlier ratio; S403. Randomly sample and calculate the plane model: randomly select 3 non - collinear points and calculate the plane equation; S404. Through inlier detection and model evaluation: calculate the distance from the inliers to the plane , mark the inliers, record the current number of inliers, and update the optimal plane parameters and inlier set if it exceeds the historical best value; Repeat steps S403 - S404 until the maximum number of iterations is reached or the minimum inlier ratio is satisfied, end the iteration, and obtain the finally fitted plane equation (12) as: (12) where, represents the normal vector , represents the constant term of the plane equation, and the positive or negative sign determines the offset of the plane relative to the origin; S505. The slope is the angle between the normal vector and the vertical direction (z - axis) (with the unit vector of the vertical direction as as a reference), and the calculation formula is as formula (13): (13) The calculation result needs to be converted to an angle, and the conversion formula is as formula (14): (14) S506. Visualize the fitted plane and the original point cloud to ensure the rationality of the fitting result, calculate the average error of the inliers, and the calculation formula of the average error is (15): (15) where, represents the th inlier, represents the total number of inliers.

[0053] If the average error is much larger than the inlier threshold, adjust the RANSAC parameters and judge the rationality of the slope.

[0054] Example 2 This embodiment further limits Embodiment 1. In this embodiment, the lossless fine fusion module uses the data-level fusion module in the MCF3D (Multi-Stage Complementary Fusion for Multi-sensor 3D Object Detection) model to implement the fusion of 3D point clouds and RGB images.

[0055] As Figure 7 shown, first, perform adaptive image patch allocation method fusion. Set the template size according to the angular resolution of the image and lidar, then perform registration to obtain registered points and unregistered points, find the registered point with the closest distance for the unregistered points, classify it into the corresponding image patch, combine each point with the matching image patch, and expand it into a piece of data; Secondly, adopt RGB-I data-level fusion, obtain the reflection intensity information and the external parameter matrix of the two sensors to achieve coordinate system unification, and project the 3D point cloud in the camera coordinate system onto the two-dimensional image of the camera according to the internal parameter matrix. Determine the corresponding position of each lidar 3D point in the camera image, and now add the reflection intensity RGB-I of the corresponding lidar 3D point as its fourth channel; Finally, cut the three-dimensional point cloud into layers according to height and project it onto a top-down two-dimensional grid. Each grid calculates the average height, height difference, and average reflection intensity to obtain a global view similar to a "top-down map", which is convenient for subsequent detection of large-range targets.

Claims

1. A method for estimating the slope of the road ahead based on multi-source information fusion, characterized in that, The method includes the following steps: S1. Synchronize the time and perform spatial registration of the camera and lidar to obtain a multi-source information dataset; S2. Construct and improve the Patchwork++ model to obtain an improved Patchwork++ model: S21. Improve the Reflection Noise Removal (RNR) module in the Patchwork++ model: introduce the BiSeNet V2 module and the lossless fine fusion module into the reflection noise removal module; S22. After the Cuboid Zone Model (CZM) module for spatial division in the Patchwork++ model, introduce the Inverse Perspective Mapping (IPM) module; S23. Introduce a comprehensive weight into the Region-based Vertical Plane Fitting (R-VPF) module in the Patchwork++ model; Introduce the Histogram of Oriented Gradients (HOG) module for feature extraction and the Support Vector Machine (SVM) module for classification into the geometric constraint filtering module of the Region-based Vertical Plane Fitting (R-VPF) module in the Patchwork++ model; S24. In the Time Ground Recovery (TGR) module of the Patchwork++ model, only retain the TGR time reduction mechanism, replace the binary judgment module with the Gradient Boosting Decision Tree (GBDT) module, and introduce a joint feature construction module before the Gradient Boosting Decision Tree (GBDT) module; S25. In the Adaptive Ground Likelihood Estimation (A-GLE) module of the Patchwork++ model, replace the single threshold judgment module with an energy function optimization module, and introduce a dynamic filtering and anti-noise structure after the energy function optimization module; S26. Introduce a spatial recovery enhancement module after the Adaptive Ground Likelihood Estimation (A-GLE) module in the Patchwork++ model; S3. Process the multi-source information dataset through the improved Patchwork++ model to obtain the coordinates of the ground point cloud ; S4. Calculate the road slope value based on the coordinates of the ground point cloud , and calculate the road slope value.

2. The method for estimating the slope of the road ahead based on multi-source information fusion according to claim 1, characterized in that, The specific operation of synchronizing the time and performing spatial registration of the camera and lidar is as follows: drive the camera and lidar sensors based on the ROS platform at the computer end, and unify the sensor sampling moments through an external trigger signal to achieve time synchronization; Independently calibrate and jointly calibrate the lidar and the camera, calculate the internal parameter matrix of each sensor and the external parameter matrix between the sensors; align the spatial positions of the lidar 3D point cloud and the camera RGB image pixels to achieve spatial registration.

3. The method for estimating the slope of the road ahead based on multi-source information fusion according to claim 2, wherein The function of the BiSeNet V2 module is: segment the RGB image to obtain the road surface and non-road surface probability maps; The function of the lossless fine fusion module is: project the 3D point cloud onto the RGB image plane and suppress the intensity of high-reflection points in the non-ground area.

4. The method for estimating the slope of the road ahead based on multi-source information fusion according to claim 3, characterized in that, The specific comprehensive weight is as follows: , and both represent undetermined coefficients, , represents the reflection intensity weight, represents the distance weight; the function of the comprehensive weight is: point cloud reflection weight allocation and probability sampling optimization.

5. The method for estimating the slope of the road ahead based on multi-source information fusion according to claim 4, wherein, The joint feature construction module sequentially passes through image superpixel segmentation, point cloud geometric feature extraction, and joint feature construction from input to output.

6. The method for estimating the slope of the road ahead based on multi-source information fusion according to claim 5, characterized in that, The energy function optimization module constructs a Conditional Random Field (CRF) four-element energy function; the CRF four-element energy function includes a unary term, a flatness constraint term, a smooth term, and a reflection intensity constraint term; The dynamic filtering and noise reduction structure is based on the formula: dynamically adjusts the variance threshold of the reflection intensity , where represents the variance threshold of the reflection intensity for the th adjustment, represents the critical proportion threshold of the waterlogging / snowy area mask, represents the control threshold, represents the proportion of the waterlogging area, represents the area of the waterlogging area, represents the area of the complete area.

7. A method for estimating the slope of the road ahead based on multi-source information fusion according to claim 6, characterized in that The spatial recovery enhancement module is specifically: first detect the low-density area, and then use a Generative Adversarial Network (GAN) to achieve cross-modal point cloud completion.

8. A method for estimating the slope of the road ahead based on multi-source information fusion according to claim 7, characterized in that, The coordinates based on the ground point cloud , calculating the road slope value is achieved through the RANSAC algorithm, specifically as follows: S801. Initialize the RANSAC parameters; S802. Random sampling and fitting of the plane equation: , where represents the normal vector , represents the constant term of the plane equation; S803. Calculate the road slope value according to the formula: ​

Images