Previewing method for terrain in front of emergency rescue vehicle under geometric feature degradation scene

Abstract

A method for previewing the terrain in front of an emergency rescue vehicle in a geometric feature degradation scene relates to the technical field of road surface recognition, realizes accurate alignment of LiDAR point cloud and IMU data through timestamp synchronization and linear interpolation, and combines a spherical projection model and a degradation perception complementary feature selection algorithm to realize the previewing of the terrain in front of the emergency rescue vehicle. Converting the three-dimensional point cloud into a robust intensity image and extracting gradient significant features; based on a double-observation secondary filter frame, a motion state is predicted by utilizing IMU forward propagation, a point cloud geometric residual error and an intensity image luminosity residual error are synchronously fused, timestamp deviation is compensated through back propagation, and a multi-source observation model under a global coordinate system is constructed. Finally, the error state is iteratively optimized to realize collaborative output of the high-precision odometer and the three-dimensional terrain map, and the problems of data asynchronism, feature degradation and dynamic interference in a complex scene are effectively solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of road surface recognition, and in particular to a front-vehicle terrain preview method in a geometric feature degradation scenario. BACKGROUND

[0002] The front-vehicle terrain elevation as a suspension preview input plays a significant role in improving the high maneuverability and smoothness of the emergency rescue vehicle. However, when the vehicle passes through rugged and uneven unstructured roads, it cannot obtain terrain input with high precision, resulting in low vehicle speed and poor smoothness, and the vehicle cannot successfully complete the rescue mission. If the vehicle can obtain effective elevation information of the front terrain during driving, specifically the 3D geometric information of the terrain, the effective elevation can be extracted as the preview input of the vehicle to achieve early control of the suspension and improve the smoothness of the vehicle.

[0003] Current terrain mapping methods mainly include single sensor mapping and multi-sensor fusion mapping. Single sensors often have limitations when dealing with these challenges, such as LiDAR, which is prone to feature matching failure in environments lacking significant geometric features, leading to a sharp decline in pose estimation accuracy. Multi-sensor fusion mapping mainly includes LiDAR-inertial odometry frameworks of loose coupling and tight coupling. The loose coupling method processes LiDAR point clouds and inertial measurement unit data independently and only performs pose fusion in the back end, which reduces the computational complexity but fails to effectively model the dynamic relationship between system state variables, making it difficult to suppress the coupling effect of new scan poses and other state errors.

[0004] Existing LiDAR-inertial odometry systems are mainly divided into two categories: loose coupling and tight coupling. The loose coupling algorithm mainly uses the measurement value of the inertial measurement unit to eliminate the motion distortion in the LiDAR point cloud and provides motion prior data for pose estimation. In the tight coupling framework, the error of each sensor is estimated and compensated in real time by obtaining data from multiple sensors, and the updated error sensor data is used for each data update to obtain more accurate state estimation. In contrast, the tight coupling framework deeply fuses Lidar and IMU data, achieving collaborative optimization of multi-source errors in forward state estimation, backward distortion correction, and tight coupling filtering optimization, and showing higher mapping accuracy and robustness. Although the tight coupling architecture has significant advantages in theory, it still faces many challenges in practical applications. In particular, it is difficult to achieve accurate extraction of point cloud features in a structural feature degradation scenario, resulting in large cumulative mapping errors. SUMMARY

[0005] The present application provides a method that fuses laser radar multi-modal feature extraction and IMU pre-integration information to improve the robustness of vehicle pose estimation and environment modeling.

[0006] The technical solution adopted by this invention to overcome its technical problems is: A method for terrain prediction in front of an emergency rescue vehicle in a scenario with degraded geometric features, characterized by comprising: S1. Install lidar, IMU, and cameras on the vehicle; S2. For the current The point cloud data from the lidar at any given time is preprocessed to obtain the preprocessed point cloud dataset. ; S3. Regarding the current situation Raw data from the IMU at any given time Preprocessing is performed to obtain preprocessed IMU data. ; S4. Based on the preprocessed point cloud dataset Obtain the intensity feature set ; S5. Based on the current image captured by the camera. Color image and intensity feature set at time of moment Obtain the intensity Jacobian matrix ; S6. Using the intensity Jacobian matrix Obtain the optimal estimate of the state vector. ; S7. Based on the optimal estimate of the state vector The optimal estimate after joint update is obtained ; S8. Based on the optimal estimate Get a global map that is updated in real time .

[0007] Preferably, in step S1, a lidar is installed on the top of the vehicle at a height of 1.5m above the ground. The lidar emits radar waves that are horizontally pointed in front of the vehicle, and a camera captures images of the front of the vehicle.

[0008] Furthermore, step S2 includes the following steps: S2-1. Obtain the current status via lidar. Original point cloud dataset at time step , ,in For the present The first moment Data from the original points, , For the present The number of original point clouds at time t, in the point cloud spherical coordinates, the th The X-axis coordinates of the original points are: Its Y-axis coordinate is Its Z-axis coordinate is ; S2-2. The first Data from the original points Linear interpolation is used to compensate for vehicle motion, resulting in the compensated first... Data from the original points ; S2-3. Call the ApproximateTimeSynchronizer function in the PCL library to calculate the compensated first... Data from the original points Synchronize with the IMU's hardware timestamp completion time to obtain the preprocessed [number]th [time]. Data from the original points ,all The preprocessed point cloud dataset consists of data from the original points after preprocessing. , .

[0009] Furthermore, regarding the current situation Raw data from the IMU at any given time The raw data, obtained through handheld calibration and the pre-calibrated rotation matrix, is transformed into the lidar coordinate system to obtain pre-processed IMU data. .

[0010] Furthermore, step S4 includes the following steps: S4-1. Calculate using the ICP registration function in the PCL library. The first time after preprocessing Data from the original points and The first moment Data from the original points pose transformation matrix Accessing the pose transformation matrix via matrix index The translation vector is obtained by extracting the first three rows of the fourth column. Calculate the translation vector Euclidean norm When Euclidean norm When the value is greater than or equal to 0.5m, the first pre-processed part is determined. Data from the original points For keyframe point cloud data ; S4-2. Keyframe point cloud data The normal vector of the plane is obtained by using RANSAC plane fitting. The pcl::computePointCloudCurvature function in the PCL library is called to process the keyframe point cloud data. By performing discrete differentiation, its curvature value can be calculated. , , For the number of keyframe point cloud data, all Each curvature value constitutes a curvature set. , Set curvature threshold When the curvature value Greater than the curvature threshold Discard it when the curvature value is... Less than or equal to the curvature threshold This is retained to obtain the optimized curvature set. ; S4-3. Through formula Calculation yields the first Keyframe point cloud data Direction angle Through formula Calculation yields the first Keyframe point cloud data Angle of elevation Through formula Calculation yields the first Spatial distance from each keyframe point to the LiDAR , No. Pixel coordinates of keyframe points in the intensity image , , Calling the cv::Mat::at() function in the OpenCV library will... Laser reflection intensity in the raw point cloud data at time [time] Assigned to pixel coordinates All laser reflection intensities Keyframes constitute pixel images ; S4-4. Pixel Image Median filtering is used to obtain the processed image. The processed image Gaussian filtering is used to obtain the processed image. ; S4-5. Call the cv::Sobel() function in the OpenCV library to process the image. The 3×3 Sobel operator is calculated pixel by pixel to obtain the processed image. No. Horizontal gradient of each pixel and vertical gradient Through formula The processed image is calculated. The The modulus of each pixel Through formula The processed image is calculated. No. Gradient direction angle of each pixel ; S4-6. When the gradient direction angle When the gradient direction angle is greater than 180°, Subtract 180° to obtain the compressed direction angle. Through formula Calculate the direction interval index In the formula To round up, ; S4-7. Construct a one-dimensional array of length 8. The array of the first The elements are , , The initial value is 0, and the index is determined by the direction interval. The value will be the processed image No. The modulus of each pixel Save the corresponding number element In the middle, the processed image The modulus of all pixels is placed into an array. After the corresponding elements, the array Sum all the moduli for each element to obtain the updated array. , The Middle The elements are ; S4-8. The optimized curvature set is calculated using the numpy.exp() function in the NumPy library. curvature weighting factor Through formula Calculate the weighted number of... element Through formula Calculate the sum of histograms ; S4-9. Through formula Calculation yields the first The probability of the direction of each element Through formula Calculate probability entropy value If the entropy value If less than or equal to 0.5, then retain the first value. element ,if If the value is greater than 0.5, then discard the first one. element ; S4-10. Extract one-dimensional arrays respectively The largest modulus stored among all the elements retained in the middle, the first The largest module length and its corresponding directional interval index Forming a binary pair All the binary pairs constitute the intensity feature set. .

[0011] Furthermore, step S5 includes the following steps: S5-1. Through formula Calculate the set of intensity features Central direction interval index actual direction angle In the processed image Extract gradient direction angles greater than or equal to and less than or equal to From the extracted pixels, select the pixel with the largest modulus; the coordinates of this pixel are... , This is the x-coordinate of the pixel. This is the ordinate of the pixel. S5-2. Call the OpenCV library function cv2.cvtColor() to display the current image captured by the camera. Color image in BGR format at time Convert to grayscale image ; S5-3. Using coordinates Center point in the processed image Extract a 5x5 local window area The local window region is calculated by calling the cv::Mat::at() function in OpenCV. Inner gradient magnitude weighted intensity value The coordinates are adjusted using the cv::calcOpticalFlowPyrLK() function in OpenCV. Perform optical flow tracking and matching with grayscale images The corresponding pixel in the image has the following coordinates: wherein is the horizontal coordinate of the pixel point, is the vertical coordinate of the pixel point; S5-4. The cv::Mat.at() function in OpenCV is called to read the grayscale image The gray value of the pixel point with coordinates The module length of the first pixel point of the processed image is normalized to obtain the normalized weight factor The weighted intensity residual error is calculated by the formula , wherein is the projection function, , , , , is the X-axis coordinate of the pixel point with the point cloud spherical coordinate , is the Y-axis coordinate thereof, is the Z-axis coordinate thereof; S5-5. The pcl::projectPointToPlane() function is called to project the data of the original point corresponding to the pixel point with the point cloud spherical coordinate in the original point cloud data set at the current time to the global map, to obtain the projected data The point-to-plane distance residual error is calculated by the formula , wherein is the transpose, The point-to-plane distance residual error is extremely obtained by the ICP registration algorithm of point-to-plane ; S5-6. A 3x3 Gaussian kernel, a standard deviation is adopted, and the cv::GaussianBlur function of the OpenCV library is called to perform smoothing processing on the processed image to obtain the Gaussian smoothed image A 3x3 sliding window is adopted, the neighborhood mean value of each pixel is calculated by the cv::blur function of the OpenCV library , the standard deviation in the same window is calculated by the cv::meanStdDev function of the OpenCV library , and the formula ​​​​​The image after brightness normalization was calculated. The Sobel operator is used to normalize the brightness of the image. Perform horizontal and vertical convolutions to obtain its horizontal gradient. and vertical gradient , horizontal gradient with vertical gradient Perform a stitching operation to obtain the image gradient term. ; S5-7. Through formula Calculate the spherical projection scaling parameters In the formula The horizontal resolution of the lidar is given by the formula. Calculate the spherical projection scaling parameters In the formula The vertical resolution of the lidar is given by the formula. The projected differential Jacobian matrix is ​​calculated. In the formula ; S5-8. Call the EkfLocalizationNode class in the robot_localization package of ROS to process the current... Raw data from the IMU at any given time Solve the problem to obtain the position vector in the global coordinate system. Through formula The displacement vector is calculated. The `EkfLocalizationNode` class in the `robot_localization` package of ROS is called to process the preprocessed IMU data. Solving for the rotation matrix in the global coordinate system yields the result. Through formula The displacement vector is calculated. , the displacement vector Substituting into the cross product matrix in Lie algebra, we obtain the constructed cross product matrix. , rotate matrix cross product matrix The pose differential term is obtained by combining the matrix blocks using a matrix block splicing method. ; S5-9. Through formula The intensity Jacobian matrix was calculated. .

[0012] Preferably, the horizontal resolution of the lidar in steps S5-6 Vertical resolution of LiDAR .

[0013] Furthermore, step S6 includes the following steps: S6-1. Point-to-surface distance residual With weighted intensity residual Vertical stacking yields the residual vector , Geometric Jacobian matrix With intensity Jacobian matrix Vertical stacking yields the residual vector , ; S6-2. Establish the observation noise covariance matrix , ,in The standard deviation of the geometric residuals. Set an initial value for the standard deviation of the strength residual. , , set the initial value Noise parameters of the IMU IMU set time interval The input is fed into the error state-space equation, and the solution is used to obtain the updated state prediction covariance matrix. ; S6-3. Through formula The multi-source residual fusion matrix was calculated. In the formula To observe the noise covariance matrix The inverse matrix, To update the state prediction covariance matrix The inverse matrix; S6-4. Preprocess the IMU data and time interval The state transition Jacobian matrix is ​​calculated by inputting it into the inertial navigation error propagation equation. Extract preprocessed IMU data acceleration values ​​in and angular velocity value angular velocity value The attitude is calculated by inputting the quaternion update equation into the inertial navigation state recursive equation. , acceleration value and posture The velocity is calculated by inputting it into the inertial navigation velocity update equation of the inertial navigation state recursion equation. speed The position is calculated by inputting the inertial navigation state recursive equation into the inertial navigation position update equation. To obtain the prior state ; S6-5. Through formula Calculation yields the first The latest state after error correction in the next iteration In the formula For the first The latest state after error correction in the next iteration, the initial state , This is the IMU noise gain matrix. For the IMU noise vector, For error propagation, ; S6-6. Through formula The state transition Jacobi was calculated. In the formula For manifold subtraction operators, For the identity matrix, use the formula The optimal estimate of the state vector is calculated. In the formula For manifold addition, , The updated position For the updated speed, This is the updated stance.

[0014] Furthermore, step S7 includes the following steps: S7-1. Call the `integrateMeasurement()` function of the `PreintegratedImuMeasurements` class in the GTSAM library to process the pre-processed IMU data. Perform pre-integration and output the current result. Relative pose transformation at any time and zero bias estimate ; S7-2. Relative pose transformation is performed using the Sophus::SO3d::log() function in the Sophus library. and zero bias estimate Perform vector operations to obtain the IMU residuals. ; S7-3. Residual vector IMU residuals Vertical stacking yields the residual vector , The residual vector is calculated by calling the Eigen library. IMU residuals fusion matrix ; S7-4. Through formula The optimal estimate after joint update is calculated. wherein , is the jointly updated position, is the jointly updated velocity, is the jointly updated pose; S7-5. If the updated bias estimate is calculated by the formula and the updated bias estimate replaces the bias estimate in step S7-1, and steps S7-2 to S7-4 are repeated, wherein is the L2 norm, is the bias estimate at time t.

[0015] Further, step S8 includes the following steps: S8-1. The jointly updated position and the jointly updated pose are input into the pcl::transformPointCloud() function of the PCL library, and the function is used to convert the key frame point cloud data to the global coordinate system to obtain the map ; S8-2. The pcl::VoxelGrid::setLeafSize(res, res, res) function of the PCL library is called to downsample the map to obtain the downsampled point cloud ; S8-3. The pcl::PassThrough function of the PCL library is called to perform straight-through filtering on the downsampled point cloud to obtain the filtered point cloud ; S8-4. The pcl::KdTreeFLANN function of the PCL library is used to construct the KD tree of the filtered point cloud , and the pcl::IterativeClosestPoint function is called to perform ICP registration of the filtered point cloud with the global map through the KD tree to obtain the registered point cloud ; S8-5. The pcl::concatenatePointClouds function of the PCL library is called to merge the registered point cloud into the global map to obtain the merged map ; S8-6. The merged map​ The pcl::StatisticalOutlierRemoval function in the PCL library is used to remove statistical outliers to obtain a real-time updated global map .

[0016] The present application has the advantages that: high-precision environment perception and real-time mapping are realized, accurate alignment of LiDAR point cloud and IMU data is realized through timestamp synchronization and linear interpolation, three-dimensional point cloud is converted into an intensity image by using a spherical projection model, and robust features are extracted by combining degenerate perception and complementary feature selection algorithms. Based on an iterative extended Kalman filter (IEKF) framework, a multi-source observation model is constructed by fusing point cloud geometric feature residuals and intensity image photometric residuals. State estimation is iteratively optimized through forward propagation and backward propagation to realize collaborative update of odometer output and terrain maps. The problems of sensor data asynchronicity and feature degradation in complex environments are solved, and the method has the characteristics of high fusion accuracy, strong real-time performance and good environmental adaptability, thereby providing reliable foundation support for unmanned navigation and path planning. DETAILED DESCRIPTION

[0017] The present application will be further described below.

[0018] A front-terrain pre-look method for an emergency rescue vehicle in a geometric feature degradation scenario, characterized in comprising: S1. Installing a laser radar, an IMU and a camera on the vehicle.

[0019] S2. Preprocessing point cloud data of the laser radar at the current time to obtain a preprocessed point cloud data set .

[0020] S3. Preprocessing original data of the IMU at the current time to obtain preprocessed IMU data .

[0021] S4. Obtaining an intensity feature set from the preprocessed point cloud data set .

[0022] S5. Obtaining an intensity Jacobian matrix from the color image captured by the camera at the current time and the intensity feature set .

[0023] S6. Obtaining an optimal estimation value of a state vector using the intensity Jacobian matrix .

[0024] S7. Based on the optimal estimate of the state vector The optimal estimate after joint update is obtained .

[0025] S8. Based on the optimal estimate Get a global map that is updated in real time .

[0026] It integrates multi-sensor fusion technology and SLAM technology to overcome the problem of low mapping accuracy caused by the degradation of structured features in front of the vehicle, thereby achieving the goal of improving the accuracy and robustness of terrain mapping.

[0027] In one embodiment of the present invention, in step S1, a lidar is installed on the top of the vehicle at a height of 1.5m above the ground. The radar waves emitted by the lidar are horizontally pointed forward of the vehicle, and a camera captures images of the area in front of the vehicle. This ensures that the lidar's scanning range covers the terrain in front of the vehicle, enabling the acquisition of high-precision 3D point cloud data of the terrain. The IMU is mounted on a rigid bracket near the vehicle's center of gravity to reduce vibration interference and ensure parallelism with the lidar coordinate system.

[0028] In one embodiment of the present invention, step S2 includes the following steps: S2-1. Obtain the current status via lidar. Original point cloud dataset at time step , ,in For the present The first moment Data from the original points, , For the present The number of original point clouds at time t, in the point cloud spherical coordinates, the th The X-axis coordinates of the original points are: Its Y-axis coordinate is Its Z-axis coordinate is .

[0029] S2-2. The first Data from the original points Linear interpolation is used to compensate for vehicle motion, resulting in the compensated first... Data from the original points .

[0030] S2-3. Call the ApproximateTimeSynchronizer function in the PCL library to calculate the compensated first... Data from the original points Synchronize with the IMU's hardware timestamp completion time to obtain the preprocessed [number]th [time]. Data from the original points ,all The data of the preprocessed original points constitute a preprocessed point cloud data set , .

[0031] In an embodiment of the present application, the original data of the IMU at the current time is converted to the coordinate system of the laser radar through a pre-designated rotation matrix obtained by a handheld calibration method, to obtain preprocessed IMU data . .

[0032] In an embodiment of the present application, step S4 comprises the following steps: S4-1. Using the ICP registration function in the PCL library to calculate the pose transformation matrix of the preprocessed original data of the point at the current time and the preprocessed original data of the point at the current time . Access the fourth column of the first three rows of the pose transformation matrix through matrix indexing, to extract a translation vector . Calculate the Euclidean norm of the translation vector . When the Euclidean norm is greater than or equal to 0.5 m, it is determined that the set feature change is large enough, i.e., the preprocessed original data of the point is marked as key frame point cloud data .

[0033] S4-2. Using RANSAC plane fitting to obtain the normal vector of the plane , calling the pcl:computePointCloudCurvature function in the PCL library to perform discrete differential operation on the key frame point cloud data , to calculate the curvature value thereof , , , is the number of key frame point cloud data, and all curvature values constitute a curvature set , , a curvature threshold is set , when the curvature value is greater than the curvature threshold , it is discarded, and when the curvature value is less than or equal to the curvature threshold , it is retained, to obtain an optimized curvature set .

[0034] S4-3. The direction angle (rotation angle around the z axis, ranging from 0°-360°) of the first key frame point cloud data is calculated by the formula . S4-4. The pixel image is obtained by assigning the laser reflection intensity in the original point cloud data at the time of , , .

[0035] S4-4. The pixel image is obtained by assigning the laser reflection intensity in the original point cloud data at the time of .

[0036] S4-5. The 3x3 Sobel operator is calculated for each pixel of the processed image .

[0037] S4-6. When the gradient direction angle​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​ greater than 180° will be converted to the gradient direction angle Subtract 180° to get the compressed direction angle , the direction interval index is calculated by the formula , where is the ceiling function, .

[0038] S4-7. Construct a one-dimensional array of length 8, the first element of which is , , , , The initial value is 0, and the magnitude of the first pixel point of the processed image is saved in the corresponding first element , , , , , , , , , , , ,

[0039] S4-8. The curvature weight factor of the optimized curvature set is calculated using the numpy.exp() function in the NumPy library, the adjustment coefficient R=50, the weighted first element is calculated by the formula , the histogram sum is calculated by the formula .

[0040] S4-9. The direction probability of the first element is calculated by the formula , the entropy value of the probability is calculated by the formula , , , , , , , , , .​​​

[0041] S4-10. Extract one-dimensional arrays respectively The largest modulus stored among all the elements retained in the middle, the first The largest modulus and its corresponding directional interval index Forming a binary pair All the binary pairs constitute the intensity feature set. .

[0042] In one embodiment of the present invention, step S5 includes the following steps: S5-1. Through formula Calculate the set of intensity features Central direction interval index actual direction angle In the processed image Extract gradient direction angles greater than or equal to and less than or equal to From the extracted pixels, select the pixel with the largest modulus; the coordinates of this pixel are... , This is the x-coordinate of the pixel. This is the ordinate of the pixel.

[0043] S5-2. Call the OpenCV library function cv2.cvtColor() to display the current image captured by the camera. Color image in BGR format at time Convert to grayscale image .

[0044] S5-3. Using coordinates Center point in the processed image Extract a 5x5 local window area The local window region is calculated by calling the cv::Mat::at() function in OpenCV. Inner gradient magnitude weighted intensity value The coordinates are adjusted using the cv::calcOpticalFlowPyrLK() function in OpenCV. Perform optical flow tracking and matching with grayscale images The corresponding pixel in the image has the following coordinates: ,in This is the x-coordinate of the pixel. This is the ordinate of the pixel.

[0045] S5-4. Use the cv::Mat.at() function in OpenCV to read grayscale images. Coordinates are grayscale value of pixels For the processed image The The modulus of each pixel Normalization is performed to obtain the normalized weight factors. Through formula The weighted intensity residuals were calculated. In the formula For projection function, , , , For the point cloud spherical coordinates are The X-axis coordinates of the pixels. Its Y-axis coordinate is Its Z-axis coordinate.

[0046] S5-5. Calling the pcl::projectPointToPlane() function will return the point cloud spherical coordinates to... The pixels in the current Original point cloud dataset at time step Data corresponding to the original points Projected onto the global map, To obtain the projected data Through formula The point-to-surface distance residual was calculated. In the formula To transpose, the point-to-surface distance residuals are... The geometric Jacobian matrix is ​​obtained through the point-to-surface ICP registration algorithm. .

[0047] S5-6. Use a 3×3 Gaussian kernel and standard deviation. The cv::GaussianBlur function from the OpenCV library is called to process the image. Smoothing is performed to obtain the Gaussian smoothed image. For the Gaussian smoothed image A 3×3 sliding window is used, and the neighborhood mean of each pixel is calculated using the cv::blur function from the OpenCV library. The standard deviation within the same window is calculated using the cv::meanStdDev function from the OpenCV library. Through formula The image after brightness normalization was calculated. The Sobel operator is used to normalize the brightness of the image. The horizontal and vertical convolutions are performed to obtain horizontal and vertical gradients . The horizontal and vertical gradients are spliced to obtain an image gradient term . .

[0048] S5-7. The spherical projection scaling parameter is calculated by the formula , wherein is the horizontal resolution of the laser radar, and the spherical projection scaling parameter is calculated by the formula , wherein is the vertical resolution of the laser radar, and the projection differential Jacobian matrix is calculated by the formula , wherein . Preferably, takes the value 1024, takes the value 100, the horizontal field of view of the laser radar is 360° (i.e. radians), and the vertical field of view is 30° (i.e. radians).

[0049] S5-8. The EkfLocalizationNode class in the robot_localization package of ROS is called to solve the original data of the IMU at the current time to obtain a position vector in the global coordinate system , the displacement vector is calculated by the formula , the EkfLocalizationNode class in the robot_localization package of ROS is called to solve the preprocessed IMU data to obtain a rotation matrix in the global coordinate system , the displacement vector is calculated by the formula , the displacement vector is substituted into the cross product matrix in the Lie algebra to obtain the constructed cross product matrix , the rotation matrix and the cross product matrix are combined by the matrix block splicing method to obtain the pose differential term .

[0050] S5-9. The intensity Jacobian matrix is calculated by the formula .

[0051] In this embodiment, preferably, the horizontal resolution of the lidar in steps S5-6 is... Vertical resolution of LiDAR .

[0052] In one embodiment of the present invention, step S6 includes the following steps: S6-1. Point-to-surface distance residuals With weighted intensity residual Vertical stacking yields the residual vector , Geometric Jacobian matrix With intensity Jacobian matrix Vertical stacking yields the residual vector , .

[0053] S6-2. Establish the observation noise covariance matrix , ,in The standard deviation of the geometric residuals. Set an initial value for the standard deviation of the strength residual. , , set the initial value Noise parameters of the IMU IMU set time interval The input is fed into the error state-space equation, and the solution is used to obtain the updated state prediction covariance matrix. In this embodiment, , .

[0054] S6-3. Through formula The multi-source residual fusion matrix was calculated. In the formula To observe the noise covariance matrix The inverse matrix, To update the state prediction covariance matrix The inverse matrix.

[0055] S6-4. Preprocess the IMU data and time interval The state transition Jacobian matrix is ​​calculated by inputting it into the inertial navigation error propagation equation. Extract preprocessed IMU data acceleration values ​​in and angular velocity value angular velocity value The attitude is calculated by inputting the quaternion update equation into the inertial navigation state recursive equation. , acceleration value and posture The velocity is calculated by inputting it into the inertial navigation velocity update equation of the inertial navigation state recursion equation. speed The position is calculated by inputting the inertial navigation state recursive equation into the inertial navigation position update equation. To obtain the prior state .

[0056] S6-5. Through formula Calculation yields the first The latest state after error correction in the next iteration In the formula For the first The latest state after error correction in the next iteration, the initial state , This is the IMU noise gain matrix (predefined during algorithm initialization, no real-time solution required). This is the IMU noise vector (which is an inherent parameter of the IMU sensor). For error propagation, .

[0057] S6-6. Through formula The state transition Jacobi was calculated. In the formula For manifold subtraction operators, For the identity matrix, use the formula The optimal estimate of the state vector is calculated. In the formula For manifold addition, , The updated position For the updated speed, This is the updated stance.

[0058] In one embodiment of the present invention, step S7 includes the following steps: S7-1. Call the `integrateMeasurement()` function of the `PreintegratedImuMeasurements` class in the GTSAM library to process the pre-processed IMU data. Perform pre-integration and output the current result. Relative pose transformation at any time and zero bias estimate .

[0059] S7-2. Relative pose transformation is performed using the Sophus::SO3d::log() function in the Sophus library. and zero bias estimate Perform vector operation to get IMU residual .

[0060] S7-3. Stack the residual vector with the IMU residual vertically to get the residual vector , , and call the Eigen library to calculate the residual vector . .

[0061] S7-4. Calculate the optimal estimation value after joint update by the formula , wherein , is the position after joint update, is the velocity after joint update, is the attitude after joint update.

[0062] S7-5. If , calculate the updated bias estimation value by the formula , replace the bias estimation value in step S7-1 with the updated bias estimation value , and repeat steps S7-2 to S7-4, wherein is the L2 norm, is the bias estimation value at the time t.

[0063] In an embodiment of the present application, step S8 comprises the following steps: S8-1. Input the position after joint update and the attitude after joint update into the pcl::transformPointCloud() function of the PCL library, and use the function to convert the key frame point cloud data to the global coordinate system to get the map .

[0064] S8-2. Call the pcl::VoxelGrid::setLeafSize(res, res, res) function in the PCL library to down-sample the map to get the down-sampled point cloud .

[0065] S8-3. Call the pcl::PassThrough function in the PCL library to process the down-sampled point cloud ​​Straight-through filtering, the specific setting filtering range is based on the joint updated position and the joint updated pose The 5m region in front, remove irrelevant noise points, get the filtered point cloud .

[0066] S8-4. Use the pcl::KdTreeFLANN function in the PCL library to construct the KD tree of the filtered point cloud , call the pcl::IterativeClosestPoint function to perform ICP registration of the filtered point cloud with the global map, use the intensity feature set as an additional constraint to improve registration accuracy, and get the registered point cloud .

[0067] S8-5. Call the pcl::concatenatePointClouds function in the PCL library to merge the registered point cloud into the global map, and get the merged map .

[0068] S8-6. Merge the merged map into the global map, and get the merged map .

[0069] Finally, it should be noted that: the above only for the preferred embodiments of the present application, and not for limiting the present application, although the foregoing embodiments of the present application are described in detail, for those skilled in the art, it still can be modified, or part of the technical features of the equivalent replacement. Any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application, should be included in the protection scope of the present application.

Claims

1. A method for pre-estimating the terrain in front of an emergency rescue vehicle in a scene with degraded geometric features, characterized in that, Comprising: S1. installing a laser radar, an IMU and a camera on a vehicle; S2. Preprocess the point cloud data of the current laser radar at the moment, to obtain a preprocessed point cloud data set ; S3. pre-process the raw data of the IMU at the current time instant to obtain pre-processed IMU data ;​​ S4. The pre-processed point cloud dataset obtaining the intensity feature set ; S5. obtaining a color image and a set of intensity features of the current moment from the camera obtaining an intensity Jacobian matrix obtaining an intensity Jacobian matrix ; S6. Utilize the intensity Jacobian matrix Obtain the optimal estimate of the state vector ; S7. Optimal estimate value of state vector Obtain joint updated optimal estimate value ; S8. According to the optimal estimate value Obtaining a real-time updated global map .

2. The method of claim 1, wherein the method further comprises: determining a distance between the emergency rescue vehicle and the object; and determining a distance between the emergency rescue vehicle and the object. In step S1, the laser radar is installed on the top of the vehicle at a distance of 1.5m from the ground, and the radar waves emitted by the laser radar are horizontally directed to the front of the vehicle, and the camera captures the front of the vehicle.

3. The method of claim 1, wherein, Step S2 includes the following steps: S2-1. Obtain a current time raw point cloud data set through laser radar , , wherein is the data of the i-th raw point at the current , is the number of raw point clouds at the current time, the X-axis coordinate of the i-th raw point in the point cloud spherical coordinate is , the Y-axis coordinate is , and the Z-axis coordinate is ;​​​ S2-2. The data of the first original point is compensated for vehicle motion using linear interpolation to obtain the compensated data of the first original point ; S2-3. Call ApproximateTimeSynchronizer function in PCL library to compensate the data of the i-th original point with the hardware timestamp of IMU to get the data of the i-th original point after preprocessing with the hardware timestamp of IMU , and the data of all i-th original points after preprocessing form the preprocessed point cloud dataset . .​ 4. The method of claim 1, wherein the method further comprises: To the current Raw data of the IMU at the current moment The raw data is converted to the coordinate system of the laser radar by a pre-calibration rotation matrix obtained by a handheld calibration method, to obtain pre-processed IMU data .

5. The method of claim 3, wherein, Step S4 includes the following steps: S4-1. Calculate using the ICP registration function in the PCL library the preprocessed data of the i-th original point at time t the preprocessed data of the i-th original point at time t the preprocessed data of the i-th original point at time t the pose transformation matrix of the i-th original point at time t access the translation vector by matrix indexing the first three rows of the fourth column of the pose transformation matrix calculate the Euclidean norm of the translation vector when the Euclidean norm is greater than or equal to 0.5m, determine that the preprocessed data of the i-th original point is key frame point cloud data ;​​​​​​​ S4-2. Key frame point cloud data Using RANSAC plane fitting to obtain the normal vector of the plane , call the pcl: : computePointCloudCurvature function in the PCL library to perform discrete differential operation on the key frame point cloud data , and calculate the curvature value , , is the number of key frame point cloud data, and all curvature values constitute a curvature set , , set a curvature threshold , discard the curvature value when it is greater than the curvature threshold , and retain the curvature value when it is less than or equal to the curvature threshold , to obtain an optimized curvature set ; S4-3. Through formula Calculation yields the first Keyframe point cloud data Direction angle Through formula Calculation yields the first Keyframe point cloud data Angle of elevation Through formula Calculation yields the first Spatial distance from each keyframe point to the LiDAR , No. Pixel coordinates of keyframe points in the intensity image , , Calling the cv::Mat::at() function in the OpenCV library will... Laser reflection intensity in the raw point cloud data at time [time] Assigned to pixel coordinates All laser reflection intensities Keyframes constitute pixel images ; S4-4. To the pixel image Using median filtering, the processed image is obtained , the processed image is obtained Using Gaussian filtering, the processed image is obtained ; S4-5. Call the cv::Sobel() function in the OpenCV library to process the image. The 3×3 Sobel operator is calculated pixel by pixel to obtain the processed image. No. Horizontal gradient of each pixel and vertical gradient Through formula The processed image is calculated. The The modulus of each pixel Through formula The processed image is calculated. No. Gradient direction angle of each pixel ; S4-6. When the gradient direction angle is greater than 180°, the gradient direction angle is subtracted by 180° to obtain the compressed direction angle , and the direction interval index is calculated by the formula , where is the upward rounding, ; S4-7. Construct a one-dimensional array of length 8 , the first element of which is , , initialized to 0, and the magnitude of the first pixel of the processed image is saved in the corresponding first element of the array In this way, the magnitudes of all the pixels of the processed image are saved in the corresponding elements of the array After this, the array is updated by adding all the magnitudes saved in each element , , the first element of which is ;​ S4-8. The optimized curvature set is calculated using the numpy.exp() function in the NumPy library. curvature weighting factor Through formula Calculate the weighted number of... element Through formula Calculate the sum of histograms ; S4-9. Through formula Calculation yields the first The probability of the direction of each element Through formula Calculate probability entropy value If the entropy value If less than or equal to 0.5, then retain the first value. element ,if If the value is greater than 0.5, then discard the first one. element ; S4-10. Extract one-dimensional array respectively the maximum modulus length saved in each element reserved in the middle the maximum modulus length and its corresponding direction interval index constitute a two-tuple All two-tuples constitute the intensity feature set .

6. The method of claim 5, wherein, Step S5 includes the following steps: S5-1. Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula Calculate the intensity feature set by the formula S5-2. Call the OpenCV library function cv2.cvtColor() function to convert the color image in BGR format captured by the camera at the current time point into a grayscale image ;​​ S5-3. Using coordinates Center point in the processed image Extract a 5x5 local window area The local window region is calculated by calling the cv::Mat::at() function in OpenCV. Inner gradient magnitude weighted intensity value The coordinates are adjusted using the cv::calcOpticalFlowPyrLK() function in OpenCV. Perform optical flow tracking and matching with grayscale images The corresponding pixel in the image has the following coordinates: ,in This is the x-coordinate of the pixel. This is the ordinate of the pixel. S5-4. Use the cv::Mat.at() function in OpenCV to read grayscale images. Coordinates are grayscale value of pixels For the processed image The The modulus of each pixel Normalization is performed to obtain the normalized weight factors. Through formula The weighted intensity residuals were calculated. In the formula For projection function, , , , For the point cloud spherical coordinates are The X-axis coordinates of the pixels. Its Y-axis coordinate is Its Z-axis coordinate; S5-5. Calling the pcl::projectPointToPlane() function will return the point cloud spherical coordinates to... The pixels in the current Original point cloud dataset at time step Data corresponding to the original points Projected onto the global map, To obtain the projected data Through formula The point-to-surface distance residual was calculated. In the formula To transpose, the point-to-surface distance residuals are... The geometric Jacobian matrix is ​​obtained through the point-to-surface ICP registration algorithm. ; S5-6. Use a 3×3 Gaussian kernel and standard deviation. The cv::GaussianBlur function from the OpenCV library is called to process the image. Smoothing is performed to obtain the Gaussian smoothed image. For the Gaussian smoothed image A 3×3 sliding window is used, and the neighborhood mean of each pixel is calculated using the cv::blur function from the OpenCV library. The standard deviation within the same window is calculated using the cv::meanStdDev function from the OpenCV library. Through formula The image after brightness normalization was calculated. The Sobel operator is used to normalize the brightness of the image. Perform horizontal and vertical convolutions to obtain its horizontal gradient. and vertical gradient , horizontal gradient with vertical gradient Perform a stitching operation to obtain the image gradient term. ; S5-7. The spherical projection scaling parameter is calculated by the formula S5-8. The horizontal resolution of the LiDAR is calculated by the formula S5-9. The spherical projection scaling parameter is calculated by the formula S5-10. The vertical resolution of the LiDAR is calculated by the formula S5-11. The projection differential Jacobian matrix is calculated by the formula S5-12. The projection differential Jacobian matrix is calculated by the formula​​​​ S5-8. Call the EkfLocalizationNode class in the robot_localization package of ROS to process the current... Raw data from the IMU at any given time Solve the problem to obtain the position vector in the global coordinate system. Through formula The displacement vector is calculated. The `EkfLocalizationNode` class in the `robot_localization` package of ROS is called to process the preprocessed IMU data. Solving for the rotation matrix in the global coordinate system yields the result. Through formula The displacement vector is calculated. , the displacement vector Substituting into the cross product matrix in Lie algebra, we obtain the constructed cross product matrix. , rotate matrix cross product matrix The pose differential term is obtained by combining the matrix blocks using a matrix block splicing method. ; S5-9. Compute the intensity Jacobian matrix by the formula .

7. The method of claim 6, wherein the method further comprises: determining a distance between the emergency rescue vehicle and the object; and determining a distance between the emergency rescue vehicle and the object. Horizontal resolution of the lidar in step S5-6 Vertical resolution of the lidar .

8. The method of claim 6, wherein the method further comprises: In step S6 includes the following steps: S6-1. Stacking the point-to-plane distance residuals with the weighted intensity residuals to get a residual vector , stacking the geometric Jacobian matrix with the intensity Jacobian matrix to get a residual vector , ; S6-2. Establish observation noise covariance matrix , , wherein is the geometric residual standard deviation, is the intensity residual standard deviation, set the initial value , , the initial value , the noise parameter of the IMU itself , the time interval set by the IMU is input into the error state space equation, and the updated state prediction covariance matrix is solved. S6-3. The formula The multi-source residual fusion matrix is calculated , wherein is an inverse matrix of an observation noise covariance matrix , and is an inverse matrix of an update state prediction covariance matrix . S6-4. The preprocessed IMU data and time interval is input into the inertial navigation error propagation equation to calculate a state transition Jacobian matrix , the acceleration value and the angular velocity value in the preprocessed IMU data are extracted, the angular velocity value is input into the quaternion update equation of the inertial navigation state recursion equation to calculate a pose , the acceleration value and the pose are input into the inertial navigation velocity update equation of the inertial navigation state recursion equation to calculate a velocity , the velocity is input into the inertial navigation position update equation of the inertial navigation state recursion equation to calculate a position , and an a priori state is obtained; S6-5. The latest state after the error correction of the mth iteration is calculated by the formula The latest state after the error correction of the mth iteration is calculated by the formula , wherein is the latest state after the error correction of the mth iteration, and the initial state is , , is an IMU noise gain matrix, is an IMU noise vector, is an error propagation amount, ;​ S6-6. The optimal estimate of the state vector is calculated by the formula is the manifold subtraction operator, is the identity matrix, and the optimal estimate of the state vector is calculated by the formula is the manifold addition operator, , is the updated position, is the updated velocity, is the updated attitude.​​​​ 9. The method of claim 5, wherein, Step S7 includes the following steps: S7-1. Call integrateMeasurement() function of PreintegratedImuMeasurements class in GTSAM library to integrate the preprocessed IMU data Pre-integration is performed, and the output is obtained the relative pose transformation at the current time and the bias estimate ; S7-2. Perform a vector operation on the relative pose transform by the Sophus::SO3d::log() function in the Sophus library and zero bias estimates Perform a vector operation on the relative pose transform by the Sophus::SO3d::log() function in the Sophus library ; S7-3. The residual vector with the IMU residual The vertical stack results in a residual vector , , the Eigen library is called to compute the residual vector with the IMU residual The fusion matrix ; S7-4. The optimal estimate value after joint update is calculated by formula wherein , is the position after joint update, is the velocity after joint update, is the attitude after joint update;​ S7-5. If then the updated zero bias estimate is calculated by the formula and the updated zero bias estimate replaces the zero bias estimate in step S7-1 and steps S7-2 through S7-4 are repeated, where is the L2 norm, is the zero bias estimate at time .

10. The method of claim 9, wherein, Step S8 includes the following steps: S8-1. Combine the updated positions and the jointly updated stance The input was passed to the pcl::transformPointCloud() function in the PCL library, and this function was used to transform the keyframe point cloud data. Transform to the global coordinate system to obtain the map. ; S8-2. Call the pcl::VoxelGrid::setLeafSize(res, res, res) function in the PCL library to down-sample the map to obtain the down-sampled point cloud ; S8-3. Call the pcl: :PassThrough function in the PCL library to down-sample the point cloud Pass through filtering to obtain the filtered point cloud ; S8-4. Construct the filtered point cloud using the pcl::KdTreeFLANN function in the PCL library , call the pcl::IterativeClosestPoint function to perform ICP registration of the filtered point cloud with the global map through the KD tree, and obtain the registered point cloud ; S8-5. Call the pcl::concatenatePointClouds function in the PCL library to concatenate the registered point clouds into the global map to obtain the merged map ; S8-6. The merged map is updated The pcl::StatisticalOutlierRemoval function in the PCL library is used to remove statistical outliers to obtain a real-time updated global map .

Images