Road gradient estimation and vehicle body pitch angle prediction method based on multiple sensors

Abstract

The invention discloses a road gradient estimation and vehicle body pitch angle prediction method based on multiple sensors, which realizes omnibearing perception of a road environment by fusing data of different sensors such as an inertial measurement unit (IMU), a global positioning system (GPS), a camera and a laser radar. Through effective processing and analysis of multi-sensor data, the precision and robustness of road slope estimation are improved. Meanwhile, an unscented Kalman filter (UKF) is utilized to effectively fuse the pitch angle actually measured by the IMU and the predicted pitch angle, and a more accurate vehicle body pitch angle predicted value is obtained. In addition, the change trend of the difference between the predicted pitch angle and the IMU actually measured pitch angle is observed, and a fluctuation threshold value is set to pre-judge whether a ramp appears in front or not. According to the method, the high-precision road slope estimated value and the vehicle body pitch angle predicted value are obtained, the navigation path can be optimized, and the driving safety and comfort are improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation and navigation, and particularly to a method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors. Background Art

[0002] In the fields of autonomous driving and intelligent transportation, accurate road slope estimation and vehicle body pitch angle prediction are of crucial significance for aspects such as safe driving, assisted driving, and path planning of vehicles. However, existing road slope estimation and vehicle body pitch angle prediction methods have many deficiencies. There are mainly two traditional road slope estimation methods: one is to estimate the slope of the road where the vehicle is located through a dynamic model, and the other is to rely on a single sensor to estimate the current road slope. But both of these methods have obvious defects. On the one hand, they are vulnerable to interference from noise or environmental factors, resulting in inaccurate slope estimation; on the other hand, in the face of bad weather or complex road conditions, the performance of a single sensor will significantly decline, and its robustness is poor, which may lead to misjudgment of the road conditions by the driver and the navigation, and then increase the accident risk.

[0003] In addition, the prediction of the vehicle body pitch angle is closely related to road slope estimation. Accurate prediction of the vehicle body pitch angle is directly related to the vehicle's perception of the road slope. It can provide more accurate attitude information for the vehicle, help optimize the navigation path, improve driving safety and comfort, and at the same time help the vehicle adjust the dynamic distribution of braking force by predicting changes in the pitch angle to achieve the purpose of saving energy costs. Therefore, there is an urgent need for a method that can more accurately estimate the road slope and predict the vehicle body pitch angle, ensuring driving economy and comfort while assisting the driver to more accurately perceive the road conditions and reducing the risk of accidents. Summary of the Invention

[0004] Object of the Invention: To solve the problems mentioned in the background art, the present invention discloses a method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors. By effectively fusing the measured pitch angle and the predicted pitch angle using the Unscented Kalman Filter (UKF), a more accurate predicted value of the vehicle body pitch angle is obtained; combined with the change trend of the difference between the predicted pitch angle and the IMU-measured pitch angle, a fluctuation threshold is set to predict the ramp.

[0005] Technical Solution:

[0006] The present invention discloses a method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors, and the method includes the following steps:

[0007] S1 Obtain a dataset of the road to be measured, screen the road slope data and display it visually, and integrate the data into the point cloud coordinate system;

[0008] S2 calculates the included angle of the 3D target detection frame coordinate matrix within the forward limited vision range of the vehicle, and filters the target detection frames;

[0009] S3 calculates the coordinates of the center intersection point of the bottom plane of the detection frame. Based on the center intersection point of the bottom plane of the vehicle and the center intersection points of the bottom planes of each target in front of the vehicle, combined with the initial body pitch angle of the vehicle, it calculates the corresponding road slope estimation value, that is, the predicted pitch angle;

[0010] S4 designs a forward ramp prediction strategy based on the initial body pitch angle and the road slope estimation value, sets a fluctuation threshold according to the change trend of the difference between the predicted pitch angle and the measured pitch angle, and predicts whether there is a ramp ahead;

[0011] S5 If there is a ramp, the unscented Kalman filter UKF is used to fuse the measured pitch angle and the predicted pitch angle to achieve accurate prediction of the body pitch angle.

[0012] Furthermore, the specific steps of S1 are as follows:

[0013] Adopt the road ramp test data of the public dataset, screen out the road data with slopes, and the obtained data includes road image data, lidar point cloud data, GPS and IMU data. Use the ROS framework to publish the road data information and visualize it through the rivz tool, and integrate all data into the point cloud coordinate system.

[0014] Furthermore, the calculation of the included angle of the 3D target detection frame coordinate matrix is specifically as follows:

[0015] Calculate the coordinate matrix of the 3D target detection frame:

[0016] Obtain the key information of the target vehicle from the public dataset: height, width, length, position coordinates (tx, ty, tz), and the heading angle around the y-axis;

[0017] To transform the three-dimensional position and orientation of the target object into the overall camera coordinate system, define a rotation matrix R around the y-axis, and use the rotation matrix R to transform the initial corner point coordinate matrix W corners , and (tx, ty, tz) is the translation position of the target relative to the origin, to obtain the corner point coordinate matrix W of the 3D target detection frame in the camera coordinate system corners_3d_cam2 , by considering the relative position and attitude relationship between the camera and the lidar, transform the 3D target detection frame coordinate matrix W corners_3d_cam2 from the camera coordinate system to the point cloud coordinate system, denoted as W corners_3d_velos , for subsequent data analysis and visualization;

[0018] Calculate the included angle between the 3D target detection frame and the positive direction of the x-axis of the origin:

[0019] Let the coordinates of the 4 corner points on the front of the i-th 3D object detection box be C i1 , C i2 , C i3 , C i4 . Each corner point coordinate is a three-dimensional vector C ij = [x ij , y ij , z ij T , j = 1, 2, 3, 4. Thus, the center coordinate of the front of the detection box is:

[0020]

[0021] According to the center coordinate of the front of the 3D object detection box, calculate the angle θ i with the positive x-axis direction of the origin.

[0022] Furthermore, the screening of the object detection box requires that the angle θ i of the 3D object detection box coordinate matrix satisfies the requirement of -30° ≤ θ i ≤ 30°, and the maximum number of detected box screenings is 3.

[0023] Furthermore, the specific steps of S3 are as follows:

[0024] Select the target point: Abandon the centroid of the 3D object detection box and select the intersection point of the bottom plane centers as the target point;

[0025] Calculate the coordinates of the intersection point of the bottom plane centers:

[0026] Let the 4 corner coordinates of the bottom plane of the i-th 3D object detection box be P i0 , P i1 , P i2 , P i3 . Each corner point coordinate is a three-dimensional vector. Use the least squares method to solve and calculate the coordinates N i of the intersection point of the bottom plane centers of the 3D object detection box as: N i = P i0 + t·d 02 , where t is a scalar and d 02 is a defined vector. The center coordinate of the vehicle bottom plane is marked as EGOCAR_POINT;

[0027] Estimate the road slope value:

[0028] Calculate the slope between the vehicle and each target in front. Given that the intersection point of the vehicle bottom plane centers is EGOCAR_POINT and the intersection points of the bottom plane centers of each target in front of the vehicle are N i (i = 0, 1, 2,...), that is, the slope calculation formula is as follows:​

[0029]

[0030] Where Δx, Δy, Δz are the three-dimensional (x, y, z) coordinate differences between EGOCAR_POINT and Ni;

[0031] Calculate the relative slope using the weight ratio, and set the slope value of the corresponding target detection box to be grad1, grad2, ..., grad n , n is the number of target detection frames, and different weights are used to calculate the relative slope grad according to the distance between the front target detection frame and the vehicle:

[0032]

[0033] Combined with the initial pitch angle of the vehicle, calculate the slope value of the corresponding road:

[0034] pred_pitch=grad+initial_pitch

[0035] Among them, grad is the relative slope, and initial_pitch is the initial pitch angle of the vehicle on the road section.

[0036] Furthermore, the initial vehicle body pitch angle initial_pitch is obtained in the following steps:

[0037] According to the coordinates of the intersection point of the bottom plane of the 3D target detection frame, obtain the coordinates of the intersection point of the bottom plane of the vehicle EGOCAR_POINT, and read the pitch angle value pitch of the first frame in the IMU data;

[0038] Predict the slope value of the first frame: filter out the targets in the front field of view of the vehicle, calculate the coordinates N of the bottom plane center intersection of each target and the slope value gradient, check whether the slope values ​​have different signs, filter out the slope values ​​with different signs, and take the average slope as the slope prediction value pred_pitch_value of the first frame;

[0039] Calculate the initial vehicle pitch angle initial_pitch: Use the pitch angle value of the first frame in the measured data minus the slope prediction value of the first frame as the initial vehicle pitch angle prediction value on the road section. The calculation formula is as follows:

[0040] initial_pitch=pitch-pred_pitch_value.

[0041] Furthermore, the forward ramp prediction strategy is as follows:

[0042] The defined road slope estimation value consists of the relative slope grad and the initial body pitch angle. The vehicle obtains grad by weighted summing the slopes between the vehicle itself and each target object. When the target in front and the vehicle are in different road sections, the value of grad will mutate, resulting in abnormal prediction values of the pitch angle. Moreover, the initial body pitch angle changes every time the vehicle enters a ramp section;

[0043] Given that the predicted pitch angle is pred_pitch and the measured pitch angle is pitch, let the prediction error value error_pitch be the absolute value of the difference between the predicted pitch angle and the measured pitch angle in each frame, expressed as:

[0044] error_pitch = |pred_pitch - pitch|

[0045] Preset a fluctuation threshold, traverse all frame images. If the value of error_pitch is greater than the fluctuation threshold, feedback the information "About to enter a ramp ahead" to the vehicle, then change the initial body pitch angle initial_pitch of the vehicle, and correct the predicted pitch angle pred_pitch.

[0046] Furthermore, the specific steps of S5 are as follows:

[0047] Define the state transition function: x k+1 = f(x k , Δt) = x k

[0048] Among them, x k represents the true pitch angle value in the ideal state at the k-th moment, and Δt is the time step;

[0049] Define the observation function: z k = h(x k ) = x k

[0050] Among them, z k represents the fused observation value of the measured pitch angle value and the predicted pitch angle value at the k-th moment. The observation function directly takes the state as the observation value;

[0051] Fuse the measured pitch angle value and the predicted pitch angle value by weighted average, predict the state at the next moment according to the state transition function f(x k , Δt), incorporate the fused measurement value as the observation value into the state estimation, and update the state and covariance matrix to obtain a more accurate predicted pitch angle value.

[0052] Furthermore, the method is completed based on a body pitch angle prediction system, including:

[0053] Data Preparation and Tool Function Definition Module: Used to initialize data information and define functions for calculating the central intersection of the bottom plane of the 3D object detection box, calculating the slope, and filtering targets within the vehicle's front field of view;

[0054] 3D Object Detection Box Calculation Module: Calculates the 3D object detection box based on the rotation matrix R and the yaw angle, and converts it to the point cloud coordinate system;

[0055] Initial Body Pitch Angle Calculation Module: Used to estimate the road slope of the first frame, and then obtain the initial body pitch angle of this section based on the measured body pitch angle of the first frame;

[0056] Body Pitch Angle Prediction Module Based on Front Targets: Used to calculate the slope between the vehicle and each front target, sum them up with weights to obtain the relative slope, and calculate the predicted value of the body pitch angle in combination with the initial body pitch angle of the vehicle in this section;

[0057] Front Ramp Anticipation Module: Used to calculate the prediction error value based on the measured pitch angle value and the predicted pitch angle value in the dataset, set a fluctuation threshold, compare the error value with the fluctuation threshold, and if the error exceeds the fluctuation threshold, it is determined that the vehicle is about to enter the next ramp, then update the initial pitch angle and correct the predicted pitch angle;

[0058] Unscented Kalman Filter Fusion Module: Used to fuse the measured pitch angle of the IMU and the predicted pitch angle using the unscented Kalman filter to achieve accurate prediction of the body pitch angle.

[0059] Beneficial Effects:

[0060] 1. The dataset of the present invention includes road image data, lidar point cloud data, GPS and IMU data, with multi-sensor deep fusion, and an all-round perception method using the advantages of multi-sensors, which overcomes the limitations of single sensors or simple combinations being easily affected by the environment and noise interference, and ensures the fitting degree and robustness of subsequent fusion predictions.

[0061] 2. The present invention designs a ramp anticipation strategy, which anticipates whether there is a ramp ahead by observing the change trend of the difference between the predicted pitch angle and the measured pitch angle, and sets a fluctuation threshold. When the error exceeds the threshold, the initial pitch angle and the predicted value are adjusted in a timely manner. This strategy enables the system to quickly respond to road condition changes and assist the driver in further reducing the risk of accidents.

[0062] 3. The present invention abandons the commonly used centroid and selects the central intersection of the bottom plane of the 3D object detection box as the target point. This selected point is more consistent with the true road slope, avoiding errors caused by differences in the central position due to diverse target types, and further improving the accuracy of slope estimation.

[0063] 4. The present invention introduces an unscented Kalman filter to fuse the measured pitch angle and the predicted pitch angle in the dataset, achieving accurate prediction of the vehicle body pitch angle and improving the robustness and anti-interference ability of the prediction method. Description of the Drawings

[0064] Figure 1 is the flowchart of the road slope estimation method in the present invention;

[0065] Figure 2 is the visualization diagram of the target to be measured in the camera and point cloud coordinate systems in the present invention;

[0066] Figure 3 is the schematic diagram of the eight corner points of the 3D target detection box in the present invention;

[0067] Figure 4 is the schematic diagram of the road slope estimation method in the present invention;

[0068] Figure 5 is the flowchart of the vehicle body pitch angle prediction method in the present invention;

[0069] Figure 6 is the schematic diagram of the vehicle body pitch angle prediction process in the present invention. Detailed Embodiments

[0070] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. 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.

[0071] As Figure 1 shown, the present invention discloses a multi-sensor-based road slope estimation and vehicle body pitch angle prediction method, and the method steps are as follows:

[0072] S1 Obtain the dataset of the road to be measured, screen the road data with slopes and make it visually displayed, and integrate the data into the point cloud coordinate system;

[0073] Data source screening: In this embodiment, the road ramp test data of the large public dataset KITTI is selected. The KITTI dataset is currently the largest computer vision algorithm evaluation dataset in the field of autonomous driving, which contains rich road scene information and provides sufficient data support for road slope estimation. The data with road slopes are screened out from the KITTI dataset, and these data can better simulate the actual driving scenarios and help improve the accuracy of slope estimation;

[0074] Multi-sensor data information: The acquired data includes road image data, lidar point cloud data, GPS and IMU data;

[0075] Visualization data processing: Use the Robot Operating System (ROS) framework to publish the road data information, and then perform visual display through the rivz tool to intuitively observe the data content. Figure 2 It is the visible view of the measured target in the camera coordinate system and the point cloud coordinate system.

[0076] S2 calculates the included angle of the 3D target detection box coordinate matrix within the front limited field of view of the vehicle and filters the target detection box;

[0077] First, calculate the coordinate matrix of the 3D target detection box:

[0078] The tracklets file is a data file for three-dimensional multi-object tracking in the KITTI dataset. Read the tracklets file: The tracklets file can parallelize the targets in different frames to form the trajectory information of the targets and annotate the multi-dimensional information of the targets; To obtain the 3D detection box position coordinate information of the target, read the key information of the measured target in the tracklets file, such as height (h), width (w), length (l), position (tx, ty, tz), and yaw angle around the y-axis;

[0079] The 8 corner point coordinates of the initial 3D target detection box are represented by a 3×8 matrix W corners Meanwhile, (tx, ty, tz) represents the position information of the target relative to the origin of the vehicle coordinate system. To convert the three-dimensional position and orientation of the target object to the overall camera coordinate system, a rotation matrix R around the y-axis is defined as follows:

[0080]

[0081] Let the corner point coordinate matrix of the initial 3D target detection box be W corners :

[0082]

[0083] Among them, x_corners, y_corners, and z_corners are respectively expressed as:

[0084] x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]

[0085] y_corners = [0, 0, 0, 0, -h, -h, -h, -h]

[0086] z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]

[0087] Use the rotation matrix R to transform the initial corner coordinate matrix W corners and (tx, ty, tz) is the translation position of the target relative to the origin, obtaining the corner coordinate matrix W of the 3D object detection box in the camera coordinate system corners_3d_cam2 , and its specific calculation formula is:

[0088]

[0089] Convert the 3D object detection box coordinate matrix to the point cloud coordinate system: By considering the relative position and attitude relationship between the camera and the lidar, convert the 3D object detection box coordinate matrix W corners_3d_cam2 from the camera coordinate system to the point cloud coordinate system, denoted as W corners_3d_velos for subsequent data analysis and visualization.

[0090] The specific steps to filter the targets within the limited field of view in front of the vehicle are as follows:

[0091] Calculate the center coordinate of the front of the 3D object detection box: Figure 3 is a schematic diagram of the eight corners of the 3D object detection box. As can be seen from the figure, the 4 corners (0, 1, 4, 5) are the 4 vertices of the front of the 3D object detection box. Then, according to the 3D object detection box coordinate matrix W corners_3d_velos , take the average of the coordinates of the 4 vertices on the front of the detection box to obtain the center coordinate of the front of the detection box;

[0092] Let the coordinates of the 4 corners of the front of the i-th 3D object detection box be C i1 , C i2 , C i3 , C i4 , and the coordinate of each corner is a three-dimensional vector C ij = [x ij , y ij , z ij T , j = 1, 2, 3, 4. Thus, the center coordinate of the front of the detection box is:

[0093]

[0094] Calculate the angle θ i between the center coordinate and the positive x-axis direction of the origin

[0095]

[0096] where and ​They are respectively the x and y components of

[0097] According to the calculated angle θ between the center coordinate and the positive x-axis direction of the origin i , filter out the target detection frames that satisfy -30° ≤ θ i ≤ 30°, and limit the maximum number to 3 in the same frame of image. This filtering method will affect the estimation accuracy of the road slope; by setting the range at ±30°, it can ensure that the front targets are in the road to be measured, effectively avoid the interference of other targets on both sides of the road, and improve the road slope estimation accuracy. The maximum number of detection frames is 3 because it can avoid excessive calculation while ensuring the estimation accuracy.

[0098] S3 Calculate the coordinates of the intersection point of the bottom plane centers of the detection frames. Based on the intersection point of the bottom plane centers of the vehicle itself and each target in front of the vehicle, combined with the initial body pitch angle of the vehicle, calculate the corresponding road slope estimation value, that is, the predicted pitch angle;

[0099] Select the target point: Abandon the centroid of the 3D target detection frame and select the intersection point of the bottom plane centers as the target point. When calculating the road slope estimation, two target points need to be in the same plane, and the intersection point of the bottom plane centers is more appropriate for the true slope of the road. If the center point of the 3D target detection frame is selected, due to the diversity of target types and the differences in the center positions of different targets, a large error will be generated.

[0100] Calculate the coordinates of the intersection point of the bottom plane centers:

[0101] Let the four corner coordinates of the bottom plane of the i-th 3D target detection frame be P i0 , P i1 , P i2 , P i3 , and each corner point coordinate is a three-dimensional vector. Using the least squares method to solve, the center coordinate of the bottom plane of the detection frame is N i ;

[0102] First, define the vector as:

[0103] Secondly, let A = [d 02 , -d 13 T , b = P i1 - P i0 , then Ax = b;

[0104] Then, use the least squares method to solve:

[0105]

[0106] Among them, t and s are scalars, indicating along the vectors d 02 and d​13 Scale factor;

[0107] Finally, calculate the coordinates N of the center intersection point of the bottom plane of the 3D target detection box i as: N i = P i0 + t·d 02

[0108] where the coordinates of the center of the bottom plane of the vehicle are named EGOCAR_POINT.

[0109] Calculate the slope between the vehicle and each target in front. Given that the center intersection point of the bottom plane of the vehicle is EGOCAR_POINT, and the center intersection points of the bottom planes of each target in front of the vehicle are N i (i = 0, 1, 2,...), that is, the slope calculation formula is as follows:

[0110]

[0111] where Δx, Δy, Δz are the three-dimensional (x, y, z) coordinate differences between EGOCAR_POINT and N i ;

[0112] Find the relative slope with the weight ratio. Let the slope values of the corresponding target detection boxes be grad1, grad2,..., grad n (n is the number of target detection boxes), and calculate with different weights according to the distance of the front target detection box from the vehicle. The calculation formula for the relative slope grad is:

[0113]

[0114] Estimate the road slope. The relative slope grad is obtained based on the slopes between the vehicle and each target object in this frame, representing the slope value that the vehicle will change in the next moment. Then, combined with the initial pitch angle initial_pitch of the vehicle itself on this section of the road, calculate the slope value of the corresponding road. Therefore, the calculation formula for road slope estimation is:

[0115] pred_pitch = grad + initial_pitch

[0116] where grad is the relative slope and initial_pitch is the initial body pitch angle of the vehicle on this section of the road.

[0117] The specific steps for calculating the initial body pitch angle of this section of the road are as follows:

[0118] Obtain relevant data: Calculate based on the center intersection coordinates of the bottom plane of the 3D target detection box to obtain the center intersection coordinates EGOCAR_POINT of the bottom plane of the vehicle, and read the pitch value of the first frame in the IMU data;

[0119] Predict the slope value of the first frame: First, filter out the targets within the forward field of view of the vehicle, then calculate the center intersection coordinates N and slope value gradient of the bottom plane center of each target, check whether the slope values have different signs, filter out the slope values with different signs (different signs indicate that the target and the vehicle are on different road sections), and take the average slope as the slope prediction value pred_pitch_value of the first frame;

[0120] Calculate the initial body pitch initial_pitch: Use the pitch value of the first frame in the IMU data minus the slope prediction value of the first frame as the initial body pitch prediction value on this road section. The calculation formula is as follows: initial_pitch = pitch - pred_pitch_value

[0121] S4 Design a forward ramp prediction strategy based on the initial body pitch and the estimated road slope value to obtain the predicted pitch angle, set a fluctuation threshold according to the change trend of the difference between the predicted pitch angle and the measured pitch angle, and predict whether there is a ramp ahead. The specific process is as Figure 5 shown;

[0122] Design of forward ramp prediction strategy

[0123] When the vehicle is driving slowly, the influence of other factors on the pitch angle can be greatly reduced. Therefore, it is defined that the predicted body pitch angle (i.e., the estimated road slope value) is composed of the relative slope grad and the initial body pitch. Analyze Figure 4 in (a) and (d), it can be seen that when the vehicle and the forward target to be measured are both on the first ramp or the second ramp, the estimated relative slope value should match the actual slope. From Figure 4 in (b) and (c), it can be seen that when the forward target appears in a different road section relative to the vehicle, the relative slope value will mutate, resulting in an abnormal predicted pitch angle, then it is determined that there is a ramp ahead.

[0124] Given that the predicted pitch angle is pred_pitch and the IMU measured pitch angle is pitch, let the prediction error value error_pitch be the absolute value of the difference between the predicted pitch angle and the IMU measured pitch angle in each frame, expressed as: error_pitch = |pred_pitch - pitch|

[0125] Preset a fluctuation threshold, traverse all frame images. If the error_pitch value is greater than the fluctuation threshold, feedback the information "About to enter a ramp ahead" to the vehicle, then change the initial body pitch angle initial_pitch of the vehicle and correct the predicted pitch angle pred_pitch;

[0126] Among them, by observing the change trend of the difference between the predicted pitch angle and the pitch angle measured by the IMU, setting the fluctuation threshold to predict whether there is a ramp ahead has important significance in vehicle navigation and path planning, which helps the subsequent research on autonomous driving.

[0127] S5 If there is a ramp, use the Unscented Kalman Filter (UKF) to fuse the measured pitch angle and the predicted pitch angle to achieve accurate prediction of the body pitch angle;

[0128] Define the state transition function and the observation function: the state transition function fx(x, dt), where x represents the current pitch angle state and dt is the time step, that is, the pitch angle of the vehicle body remains unchanged within one time step; the observation function hx(x), which means directly taking the system state as the observation value;

[0129] Determine the state dimension and the observation dimension: Set both the state dimension dim_x and the observation dimension dim_z to 1, that is, only consider the pitch angle as a variable for the system state and the observation value;

[0130] Set the unscented transformation parameters: Use the MerweScaledSigmaPoints class to generate the Sigma points required for the unscented transformation, where n = dim_x, alpha = 0.1, beta = 2.0, and kappa = 1.0;

[0131] Initialize the Unscented Kalman Filter (UKF): Create a UKF object and set the initial state ukf.x, the initial covariance ukf.P, the measurement noise covariance ukf.R, and the process noise covariance ukf.Q;

[0132] Process each frame of data according to the UKF: Obtain the IMU measurement value z_measured and the predicted value z_predicted of the pitch angle, perform UKF prediction, perform weighted averaging on the measurement value and the predicted value to obtain the fused measurement value combined_pitch, put combined_pitch as the observation value into the state estimation, update the state and the covariance matrix, and obtain a more accurate predicted value of the pitch angle fused_pitch, which improves the robustness and anti-interference ability of the prediction method;

[0133] The specific mathematical expressions are as follows:

[0134] Define the state transition function: x k+1= f(x k , Δt) = x k

[0135] where x k represents the true pitch angle value in the ideal state at the k-th moment, and Δt is the time step;

[0136] Define the observation function: z k = h(x k ) = x k

[0137] where z k represents the fused observation value of the measured pitch angle value and the predicted pitch angle value at the k-th moment, and the observation function directly takes the state as the observation value;

[0138] Fuse the measured pitch angle value of the IMU and the predicted pitch angle value by weighted average:

[0139] z combined,k = 0.5 × z imu,k + 0.5 × z pred,k

[0140] where z combined,k is the fused measurement value at the k-th moment, z imu,k is the IMU measurement value at the k-th moment, and z pred,k is the predicted value at the k-th moment;

[0141] Predict using the unscented Kalman filter: Predict the state at the next moment according to the state transition function f(x k , Δt). Since it is assumed that the pitch angle remains unchanged, the predicted state is equal to the current state. The calculation steps are as follows:

[0142] Generate Sigma points. According to the current state x k and the covariance matrix P k , use the MerweScaledSigmaPoints class to generate a set of Sigma points (i = 0, 1,..., 2n, n is the state dimension);

[0143] Propagate the Sigma points through the state transition function:

[0144] Observed state:

[0145] Predicted covariance:

[0146] where W i m and W i cis the weight of the Sigma points, and Q is the process noise covariance matrix.

[0147] Update the state and covariance matrix using the unscented Kalman filter: Incorporate the fused measurement value as the observation value into the state estimation, and update the state and covariance matrix to obtain a more accurate predicted value of the pitch angle. The calculation steps are as follows:

[0148] Propagate the predicted Sigma points through the observation function: γ i,k+1|k = h(x i,k+1|k )

[0149] Predict the observation value:

[0150] Calculate the observation covariance and cross-covariance:

[0151]

[0152] where R is the measurement noise covariance matrix;

[0153] Calculate the Kalman gain:

[0154] Update the state and covariance:

[0155]

[0156] As Figure 6 shown, the specific process of predicting the body pitch angle in this embodiment is as follows:

[0157] The entire prediction process is completed based on the body pitch angle prediction system, including

[0158] Data preparation and tool function definition module: Used to initialize data information and define functions for calculating the center intersection point of the bottom plane of the 3D target detection box, calculating the slope, and filtering targets within the field of view in front of the vehicle;

[0159] 3D target detection box calculation module: Calculate the 3D target detection box according to the rotation matrix R and the rotation angle yaw, and convert it to the point cloud coordinate system;

[0160] Initial body pitch angle calculation module, used to estimate the road slope of the first frame, and then obtain the initial body pitch angle of this section according to the measured body pitch angle of the first frame;

[0161] Body pitch angle prediction module based on the front target: Used to calculate the slope between the vehicle and each front target, sum them up with weights to obtain the relative slope, and calculate the predicted value of the body pitch angle in combination with the initial body pitch angle of the vehicle in this section;

[0162] Front ramp prediction module: It is used to calculate the prediction error value based on the measured pitch angle value and the predicted pitch angle value in the dataset, set the fluctuation threshold, compare the error value with the fluctuation threshold for judgment. If the error exceeds the fluctuation threshold, it is determined that the vehicle is about to enter the next ramp, and then the initial pitch angle is updated and the predicted pitch angle is corrected.

[0163] Unscented Kalman filter fusion module: It is used to fuse the measured pitch angle of the IMU and the predicted pitch angle using the unscented Kalman filter to achieve accurate prediction of the vehicle body pitch angle.

[0164] The server obtains data on sloped roads selected from KITTI (including image data, lidar point cloud data, GPS, and IMU data corresponding to the ramp). These data are published using the Robot Operating System (ROS) framework and visualized through the rivz tool to intuitively present the data characteristics and distribution.

[0165] Based on the above road test data, the server calculates the coordinates of the 3D object detection box in the point cloud coordinate system, thereby obtaining the position information of the 3D object detection box of each detected object (such as pedestrians, cars, trucks, etc.) in the point cloud coordinate system in the same frame of image. Among them, instead of using the common centroid, the center intersection point of the bottom plane of the 3D object detection box is selected as the target point to avoid errors caused by differences in the center positions of different objects due to the diversity of object types.

[0166] The server determines whether the currently processed frame of image is the first frame. If it is determined to be the first frame, the server obtains the slope value of the road in this frame based on the road slope estimation method, and at the same time reads the measured pitch angle value of the IMU at this moment. By calculating the difference between the two, the initial body pitch angle of the vehicle on this section of the road is obtained.

[0167] If the current frame is not the first frame, the server combines the initial pitch angle calculated in the early stage and calculates the slope between the vehicle and each target to predict the pitch angle value in real time. The relative position relationship and motion state between the vehicle and the target need to be fully considered during the prediction process.

[0168] The server calculates the prediction error value based on the measured pitch angle value of the IMU and the predicted pitch value, sets the fluctuation threshold by observing the change trend of the error value, and thus predicts whether there is a ramp ahead.

[0169] The server compares and judges the calculated error value with the preset fluctuation threshold. If the error value is greater than the fluctuation threshold, it feeds back the information that there is a ramp in front of the vehicle to the system, and at the same time changes the initial pitch angle value and corrects the predicted pitch angle value; if the error value is not greater than the fluctuation threshold, the initial pitch angle value and the predicted pitch angle value remain unchanged.

[0170] Finally, the server uses an unscented Kalman filter to fuse the measured pitch angle value of the IMU and the predicted pitch angle value. Among them, the measured pitch angle value and the predicted pitch angle value of the IMU are weighted and averaged for fusion, and the fused measurement value is used as the observation value and incorporated into the state estimation to update the state and covariance matrix, so as to obtain a more accurate predicted pitch angle value, which can effectively improve the robustness and anti-interference ability of the prediction method.

[0171] The above description of the embodiments enables those skilled in the art to implement or use the present invention. Various modifications to the embodiments will be obvious to those skilled in the art. The general principles of the present invention can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention should not be limited to the embodiments shown herein, but should cover the widest scope consistent with the principles and novel features disclosed in the present invention.

Claims

1. A method for road slope estimation and vehicle body pitch angle prediction based on multi-sensors, characterized in that, The method includes the following steps: S1: Obtain the road dataset to be measured, screen the slope road data and visualize it, and integrate the data into the point cloud coordinate system; S2: Calculate the included angle of the 3D target detection box coordinate matrix within the limited field of view in front of the vehicle, and screen the target detection boxes; S3: Calculate the coordinates of the center intersection point of the bottom plane of the detection box. Based on the center intersection point of the bottom plane of the vehicle and the center intersection points of the bottom planes of each target in front of the vehicle, combined with the initial body pitch angle of the vehicle, calculate the corresponding road slope estimation value, that is, the predicted pitch angle; S4: Design a forward ramp prediction strategy based on the initial body pitch angle and the road slope estimation value, set a fluctuation threshold according to the change trend of the difference between the predicted pitch angle and the measured pitch angle, and predict whether there is a ramp ahead; S5: If there is a ramp, use the Unscented Kalman Filter (UKF) to fuse the measured pitch angle and the predicted pitch angle to achieve accurate prediction of the body pitch angle.

2. The method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors according to claim 1, wherein The specific steps of S1 are as follows: Use the road ramp test data of the public dataset to screen out the road data with slopes. The obtained data includes road image data, lidar point cloud data, GPS and IMU data. Use the ROS framework to publish the road data information and visualize it through the rivz tool, and integrate all data into the point cloud coordinate system.

3. The method for road slope estimation and body pitch angle prediction based on multi-sensors according to claim 2, wherein The calculation of the included angle of the 3D target detection box coordinate matrix is specifically as follows: Calculate the coordinate matrix of the 3D target detection box: Obtain the key information of the target vehicle from the public dataset: height, width, length, position coordinates (tx, ty, tz), and the heading angle around the y-axis; To transform the three-dimensional position and orientation of the target object into the overall camera coordinate system, a rotation matrix R about the y-axis is defined, and the initial corner point coordinate matrix W is transformed using the rotation matrix R corners Let (tx, ty, tz) be the translation position of the target relative to the origin. The corner point coordinate matrix W of the 3D target detection box in the camera coordinate system is obtained corners_3d_cam2 By considering the relative position and attitude relationship between the camera and the lidar, the 3D target detection box coordinate matrix W corners_3d_cam2 is transformed from the camera coordinate system to the point cloud coordinate system, denoted as W corners_3d_velos for subsequent data analysis and visualization; Calculate the included angle between the 3D target detection box and the positive direction of the x-axis of the origin: Let the coordinates of the 4 corner points on the front of the i-th 3D object detection box be C i1 , C i2 , C i3 , C i4 , and the coordinate of each corner point is a three-dimensional vector C ij = [x ij , y ij , z ij T , j = 1, 2, 3, 4. Thus, the center coordinate of the front of the detection box is:​ According to the center coordinates of the front of the 3D object detection box Calculate the angle θ with the positive x-axis direction of the origin i .

4. The method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors according to claim 3, characterized in that The screening of the target detection box requires the angle θ of the 3D target detection box coordinate matrix i The angle satisfies the requirement of -30° ≤ θ i ≤ 30°, and the maximum number of detected box screenings is 3 at most.

5. The method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors according to claim 1, characterized in that, The specific steps of S3 are as follows: Select the target point: Abandon the centroid of the 3D target detection box and select the center intersection point of the bottom plane as the target point; Calculate the coordinates of the center intersection point of the bottom plane: Let the four corner coordinates of the bottom plane of the i-th 3D object detection box be P i0 , P i1 , P i2 , P i3 . Each corner point coordinate is a three-dimensional vector. Solve it using the least squares method, and calculate the center intersection coordinate N i of the bottom plane of the 3D object detection box as: N i = P i0 + t·d 02 , where t is a scalar, and d 02 is a defined vector. The center coordinate of the bottom plane of this vehicle is marked as EGOCAR_POINT; Estimate the road slope value: Calculate the slope between the vehicle and each target ahead. It is known that the center intersection point of the vehicle's bottom plane is EGOCAR_POINT, and the center intersection point of the bottom plane of each target ahead of the vehicle is N i (i = 0, 1, 2,...), that is, the slope calculation formula is as follows: where Δx, Δy, Δz are the three-dimensional (x, y, z) coordinate differences between EGOCAR_POINT and N i ; Calculate the relative slope based on the weight ratio. Let the slope values corresponding to the target detection boxes be grad1, grad2,..., grad n , ， where n is the number of target detection boxes, and different weights are used for calculation according to the distance of the front target detection box from the vehicle to obtain the relative slope grad: Combined with the initial body pitch angle of the vehicle, calculate the slope value of the corresponding road: pred_pitch = grad + initial_pitch Wherein, grad is the relative slope, and initial_pitch is the initial body pitch angle of the vehicle on this section of the road.

6. The method for estimating road slope and predicting vehicle body pitch angle based on multi-sensors according to claim 5, characterized in that, The steps for obtaining the initial body pitch angle initial_pitch are as follows: According to the calculation of the coordinates of the center intersection point of the bottom plane of the 3D target detection box, obtain the coordinates of the center intersection point of the bottom plane of the vehicle EGOCAR_POINT, and read the pitch angle value pitch of the first frame in the IMU data; Predict the slope value of the first frame: Screen out the targets within the field of view in front of the vehicle, calculate the coordinates N and slope value gradient of the center intersection point of the bottom plane of each target, check whether there are opposite-sign slope values, filter out the opposite-sign slope values, and take the slope mean as the slope prediction value pred_pitch_value of the first frame; Calculate the initial body pitch angle initial_pitch: Use the pitch angle value of the first frame in the measured data minus the slope prediction value of the first frame as the predicted value of the initial body pitch angle on this section of the road. The calculation formula is as follows: initial_pitch = pitch - pred_pitch_value。 7. The method for road slope estimation and vehicle body pitch angle prediction based on multi-sensors according to claim 1, wherein The specific forward ramp prediction strategy is as follows: It is defined that the road slope estimation value consists of the relative slope grad and the initial body pitch angle. The vehicle obtains grad by weighted summation of the slopes between the vehicle itself and each target object. When the forward target and the vehicle are in different road sections, the value of grad will have a mutation, resulting in an abnormal predicted pitch angle value, and the initial body pitch angle will change every time the vehicle enters a ramp; Given that the predicted pitch angle is pred_pitch and the measured pitch angle is pitch, let the prediction error value error_pitch be the absolute value of the difference between the predicted pitch angle and the measured pitch angle in each frame, expressed as: error_pitch = |pred_pitch - pitch| A fluctuation threshold is preset in advance. Traverse all frame images. If the value of error_pitch is greater than the fluctuation threshold, feedback the information "The vehicle is about to enter a ramp" to the vehicle, then change the initial body pitch angle initial_pitch of the vehicle, and correct the predicted pitch angle pred_pitch.

8. The method for road slope estimation and vehicle body pitch angle prediction based on multi-sensors according to claim 1, wherein The specific steps of S5 are as follows: Define the state transition function: x k+1 = f(x k , Δt) = x k Among them, xk represents the true pitch angle value in the ideal state at the k-th moment, and Δt is the time step; Define the observation function: z k = h(x k ) = x k Among them, zk represents the fused observation value of the measured pitch angle value and the predicted pitch angle value at the k-th moment, and the observation function directly takes the state as the observation value; The weighted average is used to fuse the measured pitch angle value and the predicted pitch angle value. According to the state transition function f(x k ,Δt), the state at the next moment is predicted. The fused measurement value is used as the observed value and incorporated into the state estimation to update the state and covariance matrix, obtaining a more accurate predicted pitch angle value.

9. The method for estimating road slope and predicting vehicle body pitch angle based on multiple sensors according to any one of claims 1-8, characterized in that, The method is completed based on the body pitch angle prediction system, including: Data preparation and tool function definition module: used to initialize data information, and define functions for calculating the center intersection point of the bottom plane of the 3D target detection box, calculating the slope, and screening the targets within the forward field of view of the vehicle; 3D target detection box calculation module: calculates the 3D target detection box according to the rotation matrix R and the rotation angle yaw, and converts it to the point cloud coordinate system; Initial body pitch angle calculation module, used to estimate the road slope of the first frame, and then obtain the initial body pitch angle of this section according to the measured body pitch angle of the first frame; Body pitch angle prediction module based on forward targets: used to calculate the slopes between the vehicle and each forward target, and obtain the relative slope by weighted summation, and calculate the predicted body pitch angle value in combination with the initial body pitch angle of the vehicle in this section; Forward ramp prediction module: used to calculate the prediction error value according to the measured pitch angle value and the predicted pitch angle value in the dataset, set the fluctuation threshold, compare the error value with the fluctuation threshold for judgment. If the error exceeds the fluctuation threshold, it is determined that the vehicle is about to enter the next ramp, then update the initial pitch angle and correct the predicted pitch angle; Unscented Kalman filter fusion module: used to fuse the IMU measured pitch angle and the predicted pitch angle using the unscented Kalman filter to achieve accurate prediction of the body pitch angle.

Images