A highly robust method for extracting traversable areas in wild environments based on LiDAR
Through a highly robust method based on lidar, using an inertial measurement unit and a 3D-mininet convolutional neural network to extract passable areas in the field environment, combining RANSAC and Kalman filters, the extraction accuracy and The problem of insufficient robustness achieves high-precision field accessibility in the wild environment.
Patent Information
- Application Number
- CN202111592871.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-23
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2041-12-23
AI Technical Summary
The prior art is difficult to accurately extract passable areas in the wild environment, especially in night and unstructured environments. The camera sensor is sensitive to light changes and is not robust, and the noise influence of lidar when driving on rough roads is great, resulting in Inadequate extraction accuracy and robustness.
Using a highly robust method based on lidar, point cloud data is corrected through inertial measurement units, projected into two-dimensional depth map space, semantic segmentation is used using 3D-mininet convolutional neural network, and multi-section planes are performed by combining RANSAC and Kalman filters. Fit and obstacle tracking to improve the accuracy and robustness of extraction.
It significantly improves the extraction accuracy and robustness of passable areas in the wild environment, expands the application scenarios, reduces the environment's strict requirements on extraction methods, and adapts to non-deterministic obstacle detection in complex wild environments.
Smart Images

Figure CN114325634B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of unmanned vehicle intelligent driving, and more specifically, to a method for extracting traversable areas in a highly robust outdoor environment based on laser radar. Background Art
[0002] In the prior art, the extraction of passable areas based on camera sensors can only be used when the environmental conditions such as lighting are suitable. Camera sensors are not only demanding on the environment, but also relatively demanding on the environment and sensitive to changes in lighting. However, the field environment is complex and changeable, and it is difficult to identify at night. Based on the extraction of markers such as lane lines or boundary contours, only the passable areas in fixed structured environments can be identified and analyzed. The lack of fixed and regular lane lines or boundary contours in the field environment will affect the correctness of the extraction of passable areas. LiDAR is used to extract land passable areas or perform path planning, but the application of LiDAR in field scenes is more complex and irregular than that of flat urban roads, and some extraction algorithms based on regular road signs may fail. In addition, the noise caused by the bumps of the vehicle body driving on rugged roads will also affect the performance of the algorithm, especially the algorithm using camera sensors, and the camera imaging will produce visual blur due to shaking. Summary of the invention
[0003] In order to overcome the defects in the above-mentioned prior art, the present invention provides a highly robust method for extracting traversable areas in a wild environment based on laser radar, which effectively improves the accuracy and robustness of extracting traversable areas on the ground.
[0004] In order to solve the above technical problems, the technical solution adopted by the present invention is: a highly robust method for extracting traversable areas in a field environment based on laser radar, comprising the following steps:
[0005] S1. Point cloud horizontal correction: Use the inertial measurement unit to perform horizontal correction on the acquired LiDAR point cloud data to reduce the impact of the LiDAR and vehicle body tilt on the point cloud data;
[0006] S2. Point cloud coordinate system projection: Project the corrected 3D lidar point cloud into the 2D depth map space while retaining the 3D point cloud;
[0007] S3. Point cloud semantic segmentation: Use the convolutional neural network of 3D-mininet to perform semantic segmentation and extract potential traversable areas of interest, positive obstacles, negative obstacles, and areas of no interest from a complex large environment;
[0008] S4. Multi-segment plane fitting: Processing the potential plane point cloud obtained in step S3; for potential plane points, multi-segment plane fitting is performed using the RANSAC method to further obtain more accurate plane points;
[0009] S5. Clustering of non-planar points: Based on the planar points extracted in step S4, non-planar points within the drivable range are obtained and Euclidean clustering is performed on them;
[0010] S6. Correction of positive and negative obstacle labels: Process the potential positive and negative obstacles of interest obtained in steps S3 and S5; for potential positive and negative obstacle points, correct the misclassification in the convolutional neural network in combination with the plane model obtained in step S4;
[0011] S7. Missed obstacle completion: The obtained positive and negative obstacles are tracked using the Kalman filter, and at the same time, whether there are missed obstacles is determined based on the Kalman pre-check of the previous frame.
[0012] Aiming at the problem of extracting traversable areas on the ground when an autonomous robot is driving or exploring autonomously in a wild environment, the present invention proposes a method for extracting traversable areas on the ground based on a lidar sensor, which solves the problem of difficult perception due to low visibility at night in the wild and complex unstructured environment, realizes a high-precision traversable area extraction algorithm in a wild environment, and makes the algorithm highly robust.
[0013] In one embodiment, the inertial measurement unit is installed directly below the laser radar, and the local coordinate system of the inertial measurement unit is rotationally aligned with the coordinate system of the laser radar point cloud; and the laser radar and the inertial measurement unit are rigidly connected.
[0014] In one embodiment, in step S1, for each laser radar point cloud p(x, y, z, i), i is the reflection intensity, its xyz is taken, and then a coordinate vector [x, y, z] is formed; for the obtained rotation angle data, the following formula is used to combine it into a rotation matrix of three axes:
[0015]
[0016]
[0017]
[0018] Get the corresponding total rotation matrix:
[0019]
[0020] In the formula, θ, are the rotation angles around the x, y, and z axes respectively;
[0021] Multiply the coordinate vector by the rotation matrix to obtain the corrected lidar point cloud data.
[0022] In one embodiment, in the step S2, the three-dimensional point cloud data is converted into a two-dimensional depth map by spherical projection, specifically including: 1 ,y 1 ,z 1 ), project it onto a 2D image to obtain a new 2D coordinate representation. First, assume that the x-axis is the front view of the laser radar, and the point (x 1 ,y 1 ,z 1 ) and the origin form an angle with the xy plane, which is the pitch angle. 1 ,y 1 ,z 1 ) The projection on the xy plane and the line connecting the origin and the center line x or xz plane form a deflection angle yaw; the deflection angle and pitch angle of each point in the three-dimensional point cloud data are calculated by the following formula to realize the conversion of the point cloud into a two-dimensional depth map:
[0023] pitch=sin -1 (z 1 / r)=arcsin(z 1 / r)
[0024] yaw=tan -1 (y 1 / x 1 ) = arctan(y 1 / x 1 )
[0025] Where r is the distance from the three-dimensional point to the origin.
[0026] In one of the embodiments, the 3D-mininet network first learns 2D representations from the original points through novel projections to extract local and global information from 3D data; it is then input into a 2D fully convolutional neural network to produce 2D semantic segmentation; the obtained 2D semantic labels are then reprojected back to 3D space and enhanced through a post-processing module.
[0027] In one embodiment, the step S3 specifically includes:
[0028] S1. Point cloud network training data: Label the point cloud data and divide the data into four types of objects: non-interest areas, passable areas, positive obstacles, and negative obstacles;
[0029] S2. Data balancing: Use multi-class weighted cross entropy loss as regularization to mitigate the impact of extreme class imbalance. The corresponding weights of class imbalance regularization are calculated based on the statistical results of the training set. The cross entropy loss function L is expressed as:
[0030]
[0031] In the formula, M is the number of points, C is the number of categories, and y is c,m is a binary indicator of whether m belongs to class c, is the probability that the point m predicted by CNN belongs to category c, f t is the median of the frequency of occurrence of all category points, f c is the frequency of occurrence of points belonging to category c;
[0032] S3. Fully convolutional neural network: First, a W / 4×H / 4×C6 tensor is calculated through the projection learning module, and then the semantic segmentation result is obtained by running an efficient two-dimensional semantic segmentation CNN; the neural network module uses a fully convolutional neural network based on the MiniNetV2 architecture; the encoder of the network performs L1 depthwise separable convolution and L2 multi-dilated depthwise separable convolution; for the decoder, the network uses bilinear interpolation as an upsampling layer, performs L3 separable convolution at W / 4×H / 4 resolution and L4 separable convolution at W / 2×H / 2 resolution; finally, a two-dimensional semantic segmentation prediction is obtained at W×H resolution;
[0033] S4. Back-projection from 2D to 3D: The predicted 2D semantic segmentation labels are reprojected into 3D space; points that have not yet been projected into 3D space are projected without semantic labels, and for these points, the point labels of their neighboring 2D coordinates are assigned.
[0034] In one embodiment, the RANSAC method specifically includes:
[0035] Randomly sample K points in the limited data range; use the fitting method to build a plane model for these K points; calculate the distance from other points to the fitting model, and treat the points less than the set threshold as inliers, and then count the number of inliers; use the selected inliers to re-use the fitting method to build the plane model, and then re-count the inliers, repeat this step until the inliers increase speed converges, record the inliers and the model as an iteration; re-randomly sample K points for a new round of iterations; repeat the iterations multiple times, and select the model with the largest number of inliers from the existing models; after finding the model, use the model to search for inliers in all points in the area of interest, and if there are inliers containing semantic non-planar labels, correct them to planar labels.
[0036] In one embodiment, in step S5, the clustering method used is DBSACAN, which specifically includes the following steps:
[0037] S51. Detect a point p in the data that has not been checked. If the point p has not been processed, that is, it has not been classified into a cluster or marked as noise, then check the points whose distances among the adjacent points are less than the domain range. If the number of points included is not less than minP, establish a new cluster T and add all the points in it to the candidate set N;
[0038] S52. For all points q in the candidate set N that have not been processed, check the points whose distances among the adjacent points are less than the domain range. If there are at least minP points, add these points to N; if point q does not belong to any cluster, add point q to T;
[0039] S53. Repeat step S52 to continue checking unprocessed points in N until the current candidate set N is empty;
[0040] S54. Repeat steps S51 to S53 until all points are classified into a cluster or marked as noise.
[0041] In one embodiment, the Kalman filter used in step S7 specifically includes the following steps:
[0042] S71. Prediction process:
[0043]
[0044] P k =AP k-1 A T +Q
[0045] In the formula, Represents the physical quantity x at time k, μ k is the noise at time k, A is the transfer matrix, B is the noise matrix, P k is the prediction error at time k, Q is the covariance matrix;
[0046] S72. Update process:
[0047] G k =P k H T (HP k H T +R2) -1
[0048]
[0049] P k =(1-G k H)P k
[0050] In the formula, G k represents the Kalman gain, H is the scaling factor matrix, and R2 is the measurement noise mean matrix.
[0051] In one of the embodiments, all established Kalman filter prediction value centers xy are matched with the clusters obtained in step S5. If there are unprocessed clusters, their labels are updated, and the updated categories are based on the positions of the clusters relative to the nearby planes, with positive obstacles above the planes and negative obstacles below the planes. After matching and processing, all accounting results are used to update the Kalman filter.
[0052] Compared with the prior art, the beneficial effects are:
[0053] 1. The present invention uses a laser radar sensor, which has fewer environmental requirements than other methods that use sensors such as cameras to extract the passable area. Cameras are not only sensitive to lighting conditions, but also have relatively strict requirements on the environment. The shaking of the vehicle body has a greater impact and interference on them;
[0054] 2. The method proposed in the present invention expands the scope of application scenarios. Compared with other urbanized road applications, most of them can only be applied to relatively regular environments with lane lines. The experimental scenario of the present invention is a field environment with a complex background, which greatly expands the scope of application scenarios. In addition, the present invention will also process obstacles in the passable area, which is different from the previous focus on road segmentation and is more suitable for detecting the appearance of uncertain obstacles in the field environment;
[0055] 3. The proposed method for extracting the traversable area on the ground has high accuracy and robustness. The use of laser radar to collect data in the field also has some limitations, such as the radar itself and the tilt of the vehicle body, and the obstacles that are too small due to different scanning angles. The present invention uses an inertial measurement unit (IMU) to correct the horizontality of the laser radar, which greatly improves the accuracy of extracting the traversable area in the field;
[0056] 4. The present invention solves the problem that the laser radar data collected by the unmanned vehicle will change drastically in several consecutive frames due to the bumps of the unmanned vehicle equipment. When the unmanned vehicle is driving in the wild, there will be different degrees of bumps, which will cause the laser radar installed on the unmanned vehicle to scan too few points of certain obstacles at a certain moment, or the shape of the obstacles scanned is irregular compared to the previous moment, which will cause the semantic neural network and clustering to miss these obstacles, and change dramatically in continuous time frames, seriously affecting the stability and accuracy of the algorithm. Through the continuous tracking of obstacles by the Kalman filter, the missed detection of these instances can be effectively reduced. BRIEF DESCRIPTION OF THE DRAWINGS
[0057] Figure 1 It is a schematic diagram of the overall process of the present invention.
[0058] Figure 2It is a schematic diagram of the conversion of the three-dimensional point cloud to rangeimg in the present invention.
[0059] Figure 3 It is a neural network model based on 3D-mininet of the present invention.
[0060] Figure 4 It is a schematic diagram of the correction of the plane point label in the present invention. DETAILED DESCRIPTION
[0061] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The present invention is described in one of the embodiments in combination with the specific implementation methods. Among them, the drawings are only used for exemplary descriptions, and only schematic diagrams are shown, not physical drawings, and cannot be understood as limitations on this patent; in order to better illustrate the embodiments of the present invention, some parts of the drawings will be omitted, enlarged or reduced, and do not represent the size of the actual product; for those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.
[0062] In the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right" and the like indicate an orientation or positional relationship based on the orientation or positional relationship shown in the drawings, it is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, the terms describing the positional relationship in the drawings are only used for exemplary explanations and cannot be understood as a limitation of this patent. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances. In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present invention, the descriptions of "first", "second", etc. are only used for descriptive purposes, and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" can explicitly or implicitly include at least one of the features. In addition, the meaning of "and / or" appearing in the full text is to include three parallel schemes. Taking "A and / or B" as an example, it includes scheme A, or scheme B, or schemes that satisfy both A and B.
[0063] Embodiment 1:
[0064] like Figure 1 As shown, a highly robust method for extracting traversable areas in a field environment based on laser radar includes the following steps:
[0065] Step 1. Use the inertial measurement unit to perform horizontal correction on the acquired LiDAR point cloud data to reduce the impact of the LiDAR and vehicle body tilt on the point cloud data.
[0066] The inertial measurement unit can measure the three-axis attitude angle (or angular rate) and acceleration of an object, and is widely used in the field of autonomous driving. In the application of the present invention, the imu should be installed directly below the laser radar, and its local coordinate system should be rotationally aligned with the coordinate system of the laser radar point cloud. This installation method can avoid the coordinate system conversion calibration between the laser radar and the imu. At the same time, it should also be ensured that the laser radar and the imu are rigidly connected, otherwise the attitude transformation angle obtained by the imu needs to be corrected before it can be applied.
[0067] After ensuring the above conditions, the corresponding serial port software is used to read the values measured in the imu. The present invention only uses the three-axis attitude angle. Since the rotation of the imu coordinate system is guaranteed to be consistent with the laser radar coordinate system, it is only necessary to apply the read rotation angle to the laser point cloud. The application method is: for each laser radar point cloud p (x, y, z, i), i is the reflection intensity, take its xyz, and then form a coordinate vector [x, y, z]. For the obtained rotation angle (Euler angle) data, use the following formula to combine it into a rotation matrix of three axes:
[0068]
[0069]
[0070]
[0071] Get the corresponding total rotation matrix:
[0072]
[0073] In the formula, θ, are the rotation angles around the x, y, and z axes respectively;
[0074] Multiply the coordinate vector by the rotation matrix to obtain the corrected lidar point cloud data.
[0075] Step 2. Project the rectified 3D lidar point cloud into the 2D depth map space while preserving the 3D point cloud.
[0076] Due to the huge amount of data, sparsity and irregularity of 3D point cloud data, it is often challenging and time-consuming to directly process the original 3D data. Traditional 2D CNN cannot process it directly. It is necessary to convert the 3D point cloud data into a CNN-friendly data structure. By using spherical projection, the point cloud can be converted into rangeimg.
[0077] The specific conversion diagram is as follows Figure 2 shown.
[0078] for Figure 2 The left point (x 1 ,y 1 ,z 1 ), it needs to be projected onto a 2D image to obtain a new 2D coordinate representation. First, in the left figure, it is assumed that the X-axis is the front view of the lidar, that is, the direction in which the car is moving. The line connecting this point and the origin will form an angle with the xy plane (understood as the ground), which is the pitch angle (pitch). The projection of this point in the xy plane and the line connecting the origin form a deflection angle (yaw) with the center line x or xz plane. By using trigonometric functions, the pitch and yaw values of each point can be obtained. Since the origin is at the center of the image, these yaw and pitch values constitute the position of each pixel of the projected image. Therefore, by calculating the yaw and pitch angles of each point, the projected image can be completely formed. It can be concluded that the representation of the pitch and yaw angles is as follows:
[0079] pitch=sin -1 (z 1 / r)=arcsin(z 1 / r)
[0080] yaw=tan -1 (y 1 / x 1 ) = arctan(y 1 / x 1 )
[0081] Where r is the distance from the three-dimensional point to the origin.
[0082] Step 3. Perform semantic segmentation by referencing the convolutional neural network of 3D-mininet to extract potential traversable areas of interest, positive obstacles, negative obstacles, and areas of no interest from the complex large environment.
[0083] like Figure 3 As shown in the figure, the network of 3D-mininet is a new method for LIDAR semantic segmentation that combines 3D and 2D learning layers. It first learns a 2D representation from the original points through a novel projection to extract local and global information from the 3D data. This representation is input into a 2D fully convolutional neural network (FCNN) to produce 2D semantic segmentation. The obtained 2D semantic labels are then reprojected back to 3D space and enhanced by a post-processing module.
[0084] The step S3 specifically includes:
[0085] S1. Point cloud network training data: Label the point cloud data and divide the data into four types of objects: non-interest areas, passable areas, positive obstacles, and negative obstacles;
[0086] S2. Data balancing: Use multi-class weighted cross entropy loss as regularization to mitigate the impact of extreme class imbalance. The corresponding weights of class imbalance regularization are calculated based on the statistical results of the training set. The cross entropy loss function L is expressed as:
[0087]
[0088] In the formula, M is the number of points, C is the number of categories, and y is c,m is a binary indicator of whether m belongs to class c, is the probability that the point m predicted by CNN belongs to category c, f t is the median of the frequency of occurrence of all category points, f c is the frequency of occurrence of points belonging to category c;
[0089] S3. Fully convolutional neural network: First, a W / 4×H / 4×C6 tensor is calculated through the projection learning module, and then the semantic segmentation result is obtained by running an efficient two-dimensional semantic segmentation CNN; the neural network module uses a fully convolutional neural network based on the MiniNetV2 architecture; the encoder of the network performs L1 depthwise separable convolution and L2 multi-dilated depthwise separable convolution; for the decoder, the network uses bilinear interpolation as an upsampling layer, performs L3 separable convolution at W / 4×H / 4 resolution and L4 separable convolution at W / 2×H / 2 resolution; finally, a two-dimensional semantic segmentation prediction is obtained at W×H resolution;
[0090] S4. Back-projection from 2D to 3D: The predicted 2D semantic segmentation labels are reprojected into 3D space; points that have not yet been projected into 3D space are projected without semantic labels, and for these points, the point labels of their neighboring 2D coordinates are assigned.
[0091] Step 4. Figure 4 As shown, the potential plane point cloud of interest obtained in step 3 is processed; for potential plane points, the RANSAC method is used to perform multi-segment plane fitting to further obtain more accurate plane points.
[0092] The basic assumption of the RANSAC method is that the sample contains correct data (inliers, data that can be described by the model) and abnormal data (outliers, data that deviates far from the normal range and cannot adapt to the mathematical model), that is, the data set contains noise. These abnormal data may be caused by incorrect measurements, incorrect assumptions, incorrect calculations, etc. At the same time, RANSAC also assumes that, given a set of correct data, there is a method to calculate the model parameters that conform to these data. In the scenario of the present invention, outliers are non-planar points, inliers are planar points, and the model describing planar data is ax+by+cz+d=0. The way to apply RANSAC is to find inliers that conform to the model from the plane points of interest extracted from the network as the corrected plane points.
[0093] The RANSAC method specifically includes:
[0094] Randomly sample K (the minimum number of points required to fit the plane model) points in the limited data range; use the fitting method (such as the least squares method) to build a plane model for these K points; calculate the distance from other points to the fitting model, and treat the points less than the set threshold as inliers, and then count the number of inliers; use the selected inliers to re-use the fitting method to build the plane model, and then re-count the inliers, repeat this step until the inliers increase speed converges, record the inliers and the model as an iteration; re-randomly sample K points and perform a new round of iterations; repeat the iterations multiple times, and select the model with the largest number of inliers from the existing models; after finding the model, use the model to search for inliers in all points in the area of interest, and if there are inliers containing semantic non-plane labels, they will be corrected to plane labels. In the scenario of the present invention, the plane points of interest will first be divided into multiple segments (at least 2) according to the direction of vehicle travel, and then the multiple segments of plane points will be processed by the RANSAC algorithm to obtain their respective models, and then inliers will be found on their respective models for modification.
[0095] Step 5. Based on the planar points extracted in step 4, non-planar points within the drivable range are obtained and Euclidean clustering is performed on them.
[0096] Step 6. Process the potential positive obstacles and negative obstacles of interest obtained in steps 3 and 5; for potential positive and negative obstacle points, combine the plane model obtained in step 4 to correct the misclassification in the convolutional neural network.
[0097] All clusters obtained in step 5 will have labels obtained during network segmentation. At the same time, some of the labels are the connections between the ground and obstacles that cannot be distinguished in the neural network. Through the separation of non-planar points by the plane model and the convergence of Euclidean clustering, these points will be connected to the points with obstacle labels. At this time, all clusters can be traversed, and the ground labels contained in them are modified to the labels of the corresponding obstacle points. The obstacle labels already included in the cluster can also be used to filter out the boundary points of the passable area that are mistakenly clustered (these points will be connected to the grass, resulting in a large distance, which is difficult to distinguish in the neural network) to prevent over-processing.
[0098] Step 7. Use the Kalman filter to track the obtained positive and negative obstacles, and determine whether there are any missed obstacles based on the Kalman pre-check of the previous frame. Since the acquisition of field lidar data is accompanied by the influence of scanning angle and vehicle bumps, some objects or potholes that were more obvious in the previous frame will be missing or even incomplete in the current frame. Such point cloud clusters are difficult to be detected by deep learning networks and subsequent refined processing. Based on the principle that the surrounding objects are basically stable and only the observer changes greatly, the target tracking algorithm can be used to track the existing targets. The Kalman filter is one of the algorithms.
[0099] This embodiment proposes a highly robust method for extracting passable areas in a wild environment based on laser radar, which can be applied not only to normal daytime visible periods, but also to nighttime and periods with difficult visual conditions. First, point cloud data in a wild environment is obtained based on laser radar, and then the point cloud data is horizontally corrected using the posture change data obtained by the inertial measurement unit (imu) before entering the network training to reduce the impact of the laser radar and the vehicle body tilt on the point cloud data. Then, the corrected three-dimensional laser radar point cloud is projected into a two-dimensional depth image space, and then the laser radar point cloud is semantically segmented using the convolutional neural network of 3D-mininet through the two-dimensional depth map and the corrected three-dimensional laser radar point cloud. Through the deep learning network, points including positive and negative obstacles in the passable area of interest can be initially extracted under a relatively complex background, followed by the refined processing of the extracted points. The main refined processing includes post-optimization processing of the plane point cloud and the positive and negative obstacle point cloud. For the initially extracted plane point cloud, the RANSAC algorithm based on the plane model is used to fit it in segments, and then the plane points on different segments and the non-plane points above the plane are obtained. For the plane points, if there are other labels, they can be corrected. The non-plane points will be aggregated using Euclidean distance clustering, and then the existing labels on each aggregate will be corrected. Next, the stored targets in the Kalman filter are checked, the labels of the targets inside are corrected, and all the clustered targets are input into the Kalman filter to update its parameters for use in the next lidar point cloud frame, thereby improving the recognition rate of positive and negative obstacles in the surrounding field environment.
[0100] Example 2
[0101] The other steps of this embodiment are the same as those of embodiment 1, except that in step 5, the clustering method used is DBSACAN. The core measurement method of Euclidean distance clustering is the straight-line distance in three-dimensional space, and the calculation method is the bi-norm of the difference between xyz. The clustering method used in the present invention is DBSACAN, and the specific steps are as follows:
[0102] S51. Detect a point p in the data that has not been checked. If the point p has not been processed, that is, it has not been classified into a cluster or marked as noise, then check the points whose distances among the adjacent points are less than the domain range. If the number of points included is not less than minP, establish a new cluster T and add all the points in it to the candidate set N;
[0103] S52. For all points q in the candidate set N that have not been processed, check the points whose distances among the adjacent points are less than the domain range. If there are at least minP points, add these points to N; if point q does not belong to any cluster, add point q to T;
[0104] S53. Repeat step S52 to continue checking unprocessed points in N until the current candidate set N is empty;
[0105] S54. Repeat steps S51 to S53 until all points are classified into a cluster or marked as noise.
[0106] Example 3
[0107] The other steps of this embodiment are the same as those of embodiment 1, except that this embodiment discloses a detailed process of a specific Kalman filter. The specific process corresponding to the Kalman filter is as follows:
[0108] 1. Initialization phase: Initialize the state estimate and the estimated uncertainty.
[0109] 2. Calculate the state transfer matrix and transfer the initial state from the previous moment to the current moment.
[0110] 3. Calculate the covariance of the prior states.
[0111] 4. Calculate the Kalman gain.
[0112] 5. When measurements are received, the prior state is updated.
[0113] 6. After correcting the state estimate, update the prior error covariance.
[0114] 7. Enter the state estimation and measurement update for the next moment.
[0115] In this embodiment, the Kalman filter used in step 7 specifically includes the following steps:
[0116] S71. Prediction process:
[0117]
[0118] P k =AP k-1 A T +Q
[0119] In the formula, Represents the physical quantity x at time k, μ k is the noise at time k, A is the transfer matrix, B is the noise matrix, P k is the prediction error at time k, Q is the covariance matrix;
[0120] S72. Update process:
[0121] G k =P k H T (HP k H T +R2) -1
[0122]
[0123] P k =(1-G k H)P k
[0124] In the formula, G k represents the Kalman gain, H is the scaling factor matrix, and R2 is the measurement noise mean matrix.
[0125] During the operation of the algorithm, all the established Kalman filter prediction value centers xy are matched with the clusters obtained in step S5. If there are unprocessed clusters, their labels are updated, and the updated categories are based on the position of the clusters relative to the nearby planes. Above the planes are positive obstacles, and below the planes are negative obstacles. After matching and processing, all accounting results are used to update the Kalman filter.
[0126] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.
Claims
1. A highly robust method for extracting traversable areas in a field environment based on laser radar, characterized in that: The following steps are involved: S1. Point cloud horizontal correction: Use the inertial measurement unit to perform horizontal correction on the acquired LiDAR point cloud data to reduce the impact of the LiDAR and vehicle body tilt on the point cloud data; S2. Point cloud coordinate system projection: Project the corrected 3D lidar point cloud into the 2D depth map space while retaining the 3D point cloud; S3. Point cloud semantic segmentation: Use the convolutional neural network of 3D-mininet to perform semantic segmentation and extract potential traversable areas of interest, positive obstacles, negative obstacles, and areas of no interest from a complex large environment; S4. Multi-segment plane fitting: Processing the potential plane point cloud obtained in step S3; for potential plane points, multi-segment plane fitting is performed using the RANSAC method to further obtain more accurate plane points; the RANSAC method specifically includes: Randomly sample K points in the limited data range; use the fitting method to build a plane model for these K points; calculate the distance from other points to the fitting model, and treat the points less than the set threshold as inliers, and then count the number of inliers; use the selected inliers to re-use the fitting method to build a plane model, and then re-count the inliers, repeat this step until the inliers increase speed converges, record the inliers and the model as an iteration; re-randomly sample K points for a new round of iterations; repeat the iterations multiple times, and select the model with the largest number of inliers from the existing models; after finding the model, use the model to search for inliers in all points in the area of interest, and if there are inliers containing semantic non-planar labels, correct them to planar labels; S5. Clustering of non-planar points: Based on the planar points extracted in step S4, non-planar points within the drivable range are obtained and Euclidean clustering is performed on them; S6. Correction of positive and negative obstacle labels: Process the potential positive and negative obstacles of interest obtained in steps S3 and S5; for potential positive and negative obstacle points, correct the misclassification in the convolutional neural network in combination with the plane model obtained in step S4; S7. Missing obstacle completion: The obtained positive and negative obstacles are tracked using a Kalman filter, and at the same time, whether there are missed obstacles is determined based on the Kalman pre-check of the previous frame; the Kalman filter used in step S7 specifically includes the following steps: S71. Prediction process: P k =AP k-1 From T +Q In the formula, Represents the physical quantity x at time k, μ k is the noise at time k, A is the transfer matrix, B is the noise matrix, P k is the prediction error at time k, Q is the covariance matrix; S72. Update process: G k =P k H T (HP k H T +R2) -1 P k =(1-G k H)P k In the formula, G k represents the Kalman gain, H is the scaling factor matrix, and R2 is the measurement noise mean matrix; Perform center matching on all established Kalman filter prediction value centers xy and the clusters obtained in step S5. If there are unprocessed clusters, update their labels and update categories according to the positions of the clusters relative to the nearby planes. Above the planes are positive obstacles, and below the planes are negative obstacles. After matching and processing, use all obstacle results to update the Kalman filter.
2. The method for extracting traversable areas in a field environment based on laser radar with high robustness according to claim 1 is characterized in that: The inertial measurement unit is installed directly below the laser radar, and the local coordinate system of the inertial measurement unit is rotationally aligned with the coordinate system of the laser radar point cloud; and the laser radar and the inertial measurement unit are rigidly connected.
3. The method for extracting traversable areas in a field environment based on laser radar with high robustness according to claim 1 is characterized in that: In the step S1, for each laser radar point cloud p(x, y, z, i), i is the reflection intensity, its xyz is taken, and then a coordinate vector [x, y, z] is formed; for the obtained rotation angle data, the following formula is used to combine it into a rotation matrix of three axes: Get the corresponding total rotation matrix: In the formula, θ, are the rotation angles around the x, y, and z axes respectively; Multiply the coordinate vector by the rotation matrix to obtain the corrected lidar point cloud data.
4. The method for extracting traversable areas in a highly robust field environment based on laser radar according to claim 3 is characterized in that: In the step S2, the three-dimensional point cloud data is converted into a two-dimensional depth map through spherical projection, which specifically includes: projecting the point (x1, y1, z1) in the three-dimensional point cloud data onto a 2D image to obtain a new 2D coordinate representation. First, assuming that the x-axis is the front view of the laser radar, the line connecting the point (x1, y1, z1) and the origin forms an angle with the xy plane, that is, the pitch angle pitch; the projection of the point (x1, y1, z1) in the xy plane and the line connecting the origin form a deflection angle yaw with the center line x or xz plane; the deflection angle and pitch angle of each point in the three-dimensional point cloud data are calculated by the following formula to realize the conversion of the point cloud into a two-dimensional depth map: pitch=sin -1 (z1 / r)=arcsin(z1 / r) yawn:tan -1 (y1 / x1)=arctan(y1 / x1) Where r is the distance from the three-dimensional point to the origin.
5. The method for extracting traversable areas in a field environment with high robustness based on laser radar according to claim 4 is characterized in that: The 3D-mininet network first learns 2D representations from the original points through a novel projection to extract local and global information from the 3D data; it is then input into a 2D fully convolutional neural network to produce 2D semantic segmentation; the obtained 2D semantic labels are then reprojected back to 3D space and enhanced through a post-processing module.
6. The method for extracting traversable areas in a field environment with high robustness based on laser radar according to claim 5 is characterized in that: The step S3 specifically includes: S1. Point cloud network training data: Label the point cloud data and divide the data into four types of objects: non-interest areas, passable areas, positive obstacles, and negative obstacles; S2. Data balancing: Use multi-class weighted cross entropy loss as regularization to mitigate the impact of extreme class imbalance. The corresponding weights of class imbalance regularization are calculated based on the statistical results of the training set. The cross entropy loss function L is expressed as: In the formula, M is the number of points, C is the number of categories, and y is c,m is a binary indicator of whether m belongs to class c, is the probability that the point m predicted by CNN belongs to category c, f t is the median of the frequency of occurrence of all category points, f c is the frequency of occurrence of points belonging to category c; S3. Fully convolutional neural network: First, a W / 4×H / 4×C6 tensor is calculated through the projection learning module, and then the semantic segmentation result is obtained by running an efficient two-dimensional semantic segmentation CNN; the neural network module uses a fully convolutional neural network based on the MiniNetV2 architecture; the encoder of the network performs L1 depthwise separable convolution and L2 multi-dilated depthwise separable convolution; for the decoder, the network uses bilinear interpolation as an upsampling layer, performs L3 separable convolution at W / 4×H / 4 resolution and L4 separable convolution at W / 2×H / 2 resolution; finally, a two-dimensional semantic segmentation prediction is obtained at W×H resolution; S4. Back-projection from 2D to 3D: The predicted 2D semantic segmentation labels are reprojected into 3D space; points that have not yet been projected into 3D space are projected without semantic labels, and for these points, the point labels of their neighboring 2D coordinates are assigned.
7. The method for extracting traversable areas in a field environment with high robustness based on laser radar according to claim 1, characterized in that: In step S5, the clustering method used is DBSACAN, which specifically includes the following steps: S51. Detect a point p in the data that has not been checked. If the point p has not been processed, that is, it has not been classified into a cluster or marked as noise, then check the points whose distances among the adjacent points are less than the domain range. If the number of points included is not less than minP, establish a new cluster T and add all the points in it to the candidate set N; S52. For all points q in the candidate set N that have not been processed, check the points whose distances among the adjacent points are less than the domain range. If there are at least minP points, add these points to N; if point q does not belong to any cluster, add point q to T; S53. Repeat step S52 to continue checking unprocessed points in N until the current candidate set N is empty; S54. Repeat steps S51 to S53 until all points are classified into a cluster or marked as noise.
Citation Information
Patent Citations
Road passable area detection method based on three-dimensional laser radar
CN110244321A
Road boundary detection method based on four-line laser radar
CN111880197A
Multi-plane extraction method based on Euclidean clustering
CN112184736A
Multi-source sensing method and system for water surface unmanned equipment
WO2020237693A1
