A ground anomaly detection method, device and medium based on drivable area flatness estimation

Abstract

The present invention discloses a ground anomaly detection method, device and medium based on the flatness estimation of a drivable area. The method comprises obtaining point cloud data of a target area by using a laser radar sensor, dividing the target area into a plurality of cells, determining the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include the height value of the point cloud; determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell, wherein the first height difference is the height difference between the point cloud with the smallest height value and the point cloud with the largest height value in the cell; determining a cell whose second height difference is greater than a preset second threshold value as an abnormal cell, wherein the second height difference is the maximum value of the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells; determining that an abnormality exists in the ground area corresponding to the abnormal cell. The present invention can stably, accurately and reliably detect ground anomalies.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving, and in particular to a ground anomaly detection method, device and medium based on flatness estimation of a drivable area. Background Art

[0002] There are usually two methods for ground detection in the field of low-speed unmanned driving. The first ground detection method adopts the idea of ​​overall detection, that is, using point clouds or images to estimate the current drivable boundary as a whole, without performing ground anomaly detection or ground flatness estimation. The detection idea based on point clouds requires high accuracy of laser radar. The ranging accuracy of rotating laser radars on the market currently ranges from 2 to 5 cm. In addition to a series of errors such as vehicle movement, jitter, calibration, and positioning, the measurement accuracy of ground objects can be expanded to 5 to 10 cm, and the farther away from the vehicle itself, the lower its accuracy. As a result, this method cannot detect or detect low obstacles inaccurately when used in practice; for example, ground anomalies (such as changes of 10 cm) will be drowned out by measurement noise in the actual application of this detection method during actual detection, and this anomaly cannot be ignored for vehicle driving and needs to be discovered and avoided as soon as possible. Therefore, there are safety hazards.

[0003] In addition, the accuracy of ground detection using point clouds is greatly affected by the pitch angle of the vehicle, and the accuracy of ground height measurement decreases from near to far. Therefore, the detection method based on single-frame data has the problem of not being able to detect or detecting inaccurately at a distance, and once detection is made at a close distance, it is too late for the vehicle to avoid it.

[0004] The second ground detection method is to detect abnormalities on the current ground in real time, monitor bulges or depressions based on the currently collected image information and treat them as obstacles. For example, for low obstacles such as bricks on the ground, detection is performed based on features or deep learning methods based on the collected images. The idea of ​​deep learning based on images is difficult to cope with due to the large variety and different shapes of low obstacles. The currently popular large-model deep learning has strong generalization ability but low accuracy, and is also difficult to use in the detection of such tasks. Therefore, it is difficult for the above two ground detection methods to simultaneously achieve stable and effective detection of ground abnormalities.

[0005] The disclosure of the above background technology content is only used to assist in understanding the inventive concept and technical solution of the present invention. It does not necessarily belong to the prior art of the present application, nor does it necessarily provide technical guidance. In the absence of clear evidence that the above content has been disclosed before the filing date of the present application, the above background technology should not be used to evaluate the novelty and creativity of the present application. Summary of the invention

[0006] The purpose of the present invention is to provide a ground anomaly detection method, device and medium based on the estimation of the flatness of a drivable area, which can stably, accurately and reliably detect ground anomalies.

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

[0008] A ground anomaly detection method comprises the following steps:

[0009] Using a laser radar sensor to obtain point cloud data of a target area, dividing the target area into a plurality of cells, and determining the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include a height value of the point cloud;

[0010] Calculating a first height difference corresponding to each of the cells, where the first height difference is a height difference between a point cloud with a minimum height value and a point cloud with a maximum height value in the cell, and determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell;

[0011] Calculate the second height difference corresponding to each of the cells, and determine that the cell whose absolute value of the second height difference is greater than a preset second threshold is an abnormal cell, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the maximum height difference calculated value is the second height difference;

[0012] It is determined that an abnormality exists in the ground area corresponding to the abnormal cell.

[0013] Further, any one of the above-mentioned technical solutions or a combination of multiple technical solutions further includes determining whether the point clouds corresponding to two adjacent cells are spatially continuous, and if so, determining that the cell is a non-abnormal cell;

[0014] The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous:

[0015] Determine that one of two adjacent cells is a first cell and the other is a second cell;

[0016] Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane;

[0017] Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane;

[0018] If the angle between the first tangent plane and the second tangent plane is less than a preset third threshold, and the absolute value of the difference between the distance from the point cloud corresponding to the first cell to the second tangent plane and the distance from the second cell to the first tangent plane is less than a preset fourth threshold, then the point cloud space corresponding to the first cell and the second cell is continuous.

[0019] Furthermore, any one of the above-mentioned technical solutions or a combination of multiple technical solutions further includes the following judgment:

[0020] If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

[0021] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, surface fitting is performed on the point clouds corresponding to the first cell and the second cell in the following manner, and a first tangent plane corresponding to the first cell is determined according to the surface fitting result:

[0022] Based on the coordinates of the point clouds corresponding to the first cell and all its adjacent cells, the following surface fitting formula is used to perform surface fitting and obtain a fitting surface:

[0023] Z=AX 2 +BY 2 +CXY+DX+EY+F

[0024] Among them, A, B, C, D, E, F are surface parameters, and X, Y, Z are spatial coordinates;

[0025] Determine the coordinates of the target point of the first cell, the target point is the extreme point of the fitting surface in the first cell or the center point of the first cell, and solve the fitting surface at the target point (X 0 ,Y 0 ,Z 0 ) in the X and Y directions (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ), determine the normal of the cell as (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1);

[0026] According to the target point (X 0 ,Y 0 ,Z 0 ) and normal (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1) Determine the first tangent plane A 1 X+B 1 Y+C 1 Z+D1 =0, where A 1 , B 1 , C 1 , D 1 is the first tangent plane parameter, X, Y, Z are spatial coordinates, the target point is on the first tangent plane and the normal (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1)perpendicular to the first tangent plane.

[0027] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, the distance from the point cloud corresponding to the first cell to the second tangent plane is determined by the following method:

[0028] If the fitting surface has an extreme point in the first cell, the extreme point is determined as the target point; if the fitting surface does not have an extreme point in the first cell, the point cloud corresponding to the center of the first cell is determined as the target point;

[0029] Determine the second tangent plane as A 2 X+B 2 Y+C 2 Z+D 2 =0, where A 2 , B 2 , C 2 , D 2 is the second tangent plane parameter, X, Y, Z are the spatial coordinates;

[0030] The distance from the target point to the second tangent plane is calculated and used as the distance from the first cell to the second tangent plane.

[0031] Furthermore, any one of the above-mentioned technical solutions or a combination of multiple technical solutions further includes the following steps:

[0032] A scene map is established based on the drivable area and a coordinate system of the scene map is determined as a scene coordinate system, and the scene map is divided into a number of cells;

[0033] Predetermining and storing the categories of each cell in the scene map, wherein the categories of the cells include abnormal cells and non-abnormal cells;

[0034] Determine a position registration relationship between the scene coordinate system and the vehicle coordinate system, map the newly acquired point cloud data of the target area to the scene map according to the position registration relationship, and determine the category of each cell in the scene map corresponding to the target area;

[0035] If the category determination result of the cell is an abnormal cell or a non-abnormal cell, the category of the cell is updated.

[0036] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, if the category judgment result of the cell is a neutral cell, the category of the cell is determined to be the category of the cell currently stored in the scene map; or,

[0037] If the category judgment result of the cell is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

[0038] Furthermore, based on any one of the technical solutions or a combination of multiple technical solutions described above, after mapping the latest acquired point cloud data of the target area to the scene map according to the position registration relationship, it also includes, for each cell in the scene map corresponding to the target area, performing Kalman filtering on the point cloud data corresponding to the cell based on historical data and current data, and obtaining a first estimated coordinate and a second estimated coordinate of the point cloud corresponding to the cell, the first estimated coordinate being the coordinate of the point cloud with the largest height value in the cell, and the second estimated coordinate being the coordinate of the point cloud with the smallest height value in the cell.

[0039] Further, based on any one of the technical solutions or a combination of multiple technical solutions mentioned above, the first height difference corresponding to the cell is the height difference between the first estimated coordinate and the second estimated coordinate in the vertical direction; and / or,

[0040] The second height difference corresponding to the cell is calculated by determining the cell and its adjacent cells, calculating the height difference between the first estimated coordinates corresponding to the cell and the first estimated coordinates corresponding to each of its adjacent cells, and determining the height difference with the largest absolute value as the second height difference.

[0041] Furthermore, any one of the technical solutions or a combination of multiple technical solutions described above also includes, for each of the cells, performing surface fitting using the first estimated coordinates corresponding to the cell and all its adjacent cells, and determining whether the point clouds corresponding to the adjacent cells are spatially continuous.

[0042] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, if the absolute value of the second height difference is greater than a preset second threshold value and the second height difference is greater than zero, the cell is determined to be a protruding cell;

[0043] If the absolute value of the second height difference is greater than a preset second threshold and the second height difference is less than zero, it is determined that the cell is a concave cell.

[0044] According to another aspect of the present invention, the present invention provides a ground anomaly detection device, comprising a laser radar sensor and a processor, wherein the laser radar sensor is positioned to collect point cloud data of a target area; the processor is configured to receive the point cloud data and perform the following steps based on the point cloud data:

[0045] Divide the target area into a plurality of cells, and determine the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include the height value of the point cloud;

[0046] Calculating a first height difference corresponding to each of the cells, where the first height difference is a height difference between a point cloud with a minimum height value and a point cloud with a maximum height value in the cell, and determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell;

[0047] Calculate the second height difference corresponding to each of the cells, and determine that the cell whose absolute value of the second height difference is greater than a preset second threshold is an abnormal cell, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the maximum height difference calculated value is the second height difference;

[0048] It is determined that an abnormality exists in the ground area corresponding to the abnormal cell.

[0049] Furthermore, any one of the above-mentioned technical solutions or a combination of multiple technical solutions further includes:

[0050] Pre-establishing a scene map including a drivable area, wherein the scene map is configured with a scene coordinate system, and the scene map is divided into a plurality of cells;

[0051] Determine a position registration relationship between the vehicle coordinate system and the scene coordinate system based on the vehicle coordinate system and the current position of the vehicle in the scene map;

[0052] The processor is also configured to align the point cloud data of the currently acquired target area to the scene map according to the position alignment relationship between the scene coordinate system and the vehicle coordinate system, and determine the category of each cell in the scene map corresponding to the target area, and update the category of the cell if the category judgment result of the cell is an abnormal cell or a non-abnormal cell.

[0053] Further, any one of the above-mentioned technical solutions or a combination of multiple technical solutions also includes determining whether the point clouds corresponding to two adjacent cells are spatially continuous, and if so, determining that the cell is a non-abnormal cell; if the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, determining that the cell is a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell;

[0054] The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous:

[0055] Determine that one of two adjacent cells is a first cell and the other is a second cell;

[0056] Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane;

[0057] Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane;

[0058] If the angle between the first tangent plane and the second tangent plane is less than a preset third threshold, and the absolute value of the difference between the distance from the point cloud corresponding to the first cell to the second tangent plane and the distance from the second cell to the first tangent plane is less than a preset fourth threshold, then the point cloud space corresponding to the first cell and the second cell is continuous.

[0059] Further, based on any one of the above-mentioned technical solutions or a combination of multiple technical solutions, if the category judgment result of the cell is a neutral cell, the category of the cell is determined to be the category of the cell currently stored in the scene map; or,

[0060] If the category judgment result of the cell is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

[0061] According to another aspect of the present invention, the present invention provides a computer-readable storage medium for storing program instructions, wherein the program instructions are configured to call and execute the steps of the method described in any one of the above technical solutions or a combination of multiple technical solutions.

[0062] The beneficial effects brought by the technical solution provided by the present invention are as follows:

[0063] a. The present invention compares the first height difference corresponding to each cell, that is, the maximum height difference of the point cloud in the cell, with the first threshold, calculates the height difference between the cell and each of its adjacent cells, that is, the second height difference, and compares it with the second threshold, and judges the spatial continuity of the point cloud corresponding to each adjacent cell in the cell. Through the triple judgment, abnormal cells and non-abnormal cells can be more accurately and reliably identified, thereby improving the accuracy and reliability of ground anomaly detection;

[0064] b. The present invention establishes a large grid map that is compatible with the map of the entire unmanned driving area, rather than a local grid map based on the local coordinate system of the vehicle. By constructing a stable and fixed global grid map of the unmanned driving area, each cell corresponding to each cloud point data is tracked and predicted, and the uncertainty of the current data is suppressed by continuous observation. The point cloud data corresponding to each small grid, i.e., cell, is independently Kalman filtered, and the maximum and minimum values ​​corresponding to each cell are tracked and predicted in real time. Since the ground is stationary and locally continuous, even if the point cloud coordinates of the current frame have a certain deviation, they are still valid as the observation value of the current grid; all historical data are observed, continuously tracked and predicted, so as to achieve local stability, thereby solving the problem that one frame is unstable and multiple consecutive frames may also be unstable, thereby improving the reliability of ground anomaly detection results;

[0065] c. The present invention compares the angle between the first tangent plane and the second tangent plane with the third threshold, and the absolute value of the difference between the distance from the current cell to the second tangent plane and the distance from the adjacent cell to the first tangent plane with the fourth threshold, to judge the continuity, i.e., the flatness, of the cell and the adjacent cells. This method is different from the prior art in which local points are directly fitted, but a local quadratic surface is obtained, and then the tangent plane is obtained at the current cell as the plane equation of the cell. This method obtains a surface equation rather than an equation with "average meaning" obtained by fitting, which can better characterize the plane characteristics of the current cell, thereby improving the accuracy of ground anomaly detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0067] Figure 1 A flow chart of a ground anomaly detection method provided for an exemplary embodiment of the present invention;

[0068] Figure 2A flowchart of a ground anomaly detection method based on a scene map provided for an exemplary embodiment of the present invention;

[0069] Figure 3 A schematic block diagram of the principle of a ground anomaly detection method provided for an exemplary embodiment of the present invention;

[0070] Figure 4 A module diagram of a ground anomaly detection device based on a scene map provided as an exemplary embodiment of the present invention. DETAILED DESCRIPTION

[0071] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings 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. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0072] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, device, product or equipment that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or equipment.

[0073] In one embodiment of the present invention, a ground anomaly detection method is provided. Figures 1 to 3 , the method comprises the following steps:

[0074] Using a laser radar sensor to obtain point cloud data of a target area, dividing the target area into a plurality of cells, and determining the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include a height value of the point cloud;

[0075] Calculating a first height difference corresponding to each of the cells, where the first height difference is a height difference between a point cloud with a minimum height value and a point cloud with a maximum height value in the cell, and determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell;

[0076] Calculate the second height difference corresponding to each of the cells, and determine that the cell whose absolute value of the second height difference is greater than a preset second threshold is an abnormal cell, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the maximum height difference calculated value is the second height difference;

[0077] Determine that the ground area corresponding to the abnormal cell has an abnormality. In the present application, the abnormal cell includes a cell with an obstacle or a cell with a pothole, which reflects that the ground area corresponding to the abnormal cell has an obstacle or a pothole.

[0078] In this embodiment, it is also included to determine whether the point clouds corresponding to two adjacent cells are spatially continuous. If they are continuous, the cell is determined to be a non-abnormal cell. If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

[0079] The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous:

[0080] Determine that one of two adjacent cells is a first cell and the other is a second cell;

[0081] Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane;

[0082] Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane;

[0083] If the angle between the first tangent plane and the second tangent plane is less than a preset third threshold, and the absolute value of the difference between the distance between the point cloud corresponding to the first cell and the second tangent plane and the distance between the second cell and the first tangent plane is less than a preset fourth threshold, then the point cloud space corresponding to the first cell and the second cell is continuous. The first height difference corresponding to the cell The second height difference corresponding to the cell In one embodiment of the present invention, a ground anomaly detection method is provided. See Figure 2In this implementation, a scene map is established based on the drivable area and the coordinate system of the scene map is determined to be the scene coordinate system. The scene map is divided into several continuous cells of length × width S × S. For example, S can be 0.1m. It should be noted that the unmanned driving application scenario is of a fixed range, such as in a park, school or several streets, that is, the map boundary of the unmanned driving area is determinable. The present application is different from the prior art, which only uses real-time collected image / point cloud data for ground anomaly detection. Instead, it constructs a large-scale fixed scene map including the unmanned driving area, and uses the historical data of the scene map and the real-time collected point cloud / image data for ground anomaly detection.

[0084] Specifically, an image acquisition device can be used to obtain a map of the drivable area or to capture existing data to establish a scene map including the drivable area and determine its coordinate system as the scene coordinate system. The scene map is divided into a number of continuous S×S cells to form a global grid map. The categories of each cell in the scene map are predetermined, including abnormal cells and non-abnormal cells, and the types of each cell are stored as historical data. Preferably, the highest point coordinates and the lowest point coordinates in the cell are determined and stored as historical data.

[0085] Using a LiDAR sensor (mechanical, semi-solid, etc.) as the data source acquisition end, it does not directly detect ground anomalies or low obstacles based on single-frame data, but estimates the ground flatness in real time, and then detects the ground abnormal area based on this. The flatness here is a measure of the height difference between the ground area and its surrounding area. The smaller the height difference, the higher the flatness, and the local ground tends to be flat.

[0086] Determine the position registration relationship between the scene coordinate system and the vehicle coordinate system, register the most recently acquired point cloud data of the target area to the scene map according to the position registration relationship, and determine the category of each cell in the scene map corresponding to the target area.

[0087] In this embodiment, after the newly acquired point cloud data of the target area is registered to the scene map according to the position registration relationship, all cells of the point cloud data currently registered are loaded, and other cells on the scene map are not loaded, thereby reducing memory usage.

[0088] Although the prior art generally samples raster maps for ground detection, for example, Chinese patents with application numbers 2022102826716 and 2022115222413 both disclose two-dimensional rasterization of the ground / target area. However, the existing ground detection is based on the map / target area corresponding to the real-time detection data, and this processing method is greatly affected by the fluctuations and noise of data collection.

[0089] The grid map proposed in this application is based on the scene coordinate system. It is a large grid map that is compatible with the map of the entire unmanned driving area, rather than a local grid map based on the local coordinate system of the vehicle. In view of the conversion of the local single-frame point cloud to the map coordinate system, due to factors such as positioning accuracy, radar calibration accuracy, and jitter during the operation of the vehicle itself, the converted point cloud will be biased, and noise will appear in the point cloud due to the influence of lighting and reflectors. One way to deal with it is to use a point cloud matching algorithm such as ICP to correct the current point cloud coordinates, but this is a time-consuming operation and will not be done in an actual real-time system; another way to deal with it is through filtering suppression, but this is feasible in principle. In fact, what we want to detect is a small target. The radical point of the filter parameter setting even filters out ground anomalies, while the loose setting does not achieve the actual effect.

[0090] In view of the above shortcomings of the prior art, this patent does not emphasize noise filtering, but only performs height filtering to remove point clouds that are obviously higher than the ground; by constructing a global grid map, and tracking and predicting each cell therein, continuous observation is used to suppress the uncertainty of the current data. Specifically, an independent Kalman filter is performed on each small grid, that is, each cell is tracking and predicting its maximum and minimum values ​​in real time, and the observation value is the maximum and minimum value of each frame of the point cloud projected into the current grid. Since the ground is stationary and locally continuous, even if there is a certain deviation in the coordinates of the point cloud of the current frame, it is still valid as the observation value of the current grid. By using the observation of all historical data, continuous tracking and prediction, local stability can be achieved, thereby solving the problem of instability in one frame and possible instability in multiple consecutive frames, thereby improving the reliability of ground anomaly detection results.

[0091] For each cell in the scene map corresponding to the target area, that is, for the cell in the scene map where the point cloud falls, the point cloud data is first read frame by frame, and the point cloud is height-filtered to remove points that are a certain distance above the ground (for example, points with a height of not less than 0.5m). Then, based on historical data and current data (or based on multiple detection data), the cell is Kalman filtered to obtain the first estimated coordinates and the second estimated coordinates in the cell, the first estimated coordinates are the coordinates of the point cloud with the maximum height in the cell, and the second estimated coordinates are the coordinates of the point cloud with the minimum height in the cell.

[0092] Through Kalman filtering, the cells with point clouds falling into them are registered, and the coordinates of the point clouds with the maximum and minimum height values ​​in all current cells are counted (hereinafter referred to as the maximum value coordinates and the minimum value coordinates). This coordinate value is used as the observation value and Kalman filtering is performed on the point cloud in the cell, including two parts: prediction and estimation. The prediction equation and the estimation equation are as follows.

[0093] Prediction equation:

[0094] Estimated equation:

[0095] Among them, X is the estimator, P is the estimated variance matrix, Q k is the prediction error, Q z is the prediction error, K k is the Kalman gain matrix, Z k is the observed value, which corresponds to the maximum and minimum values ​​​​calculated in each frame in the cell, X k|k is the current estimated value, X k|k-1 is the current predicted value, X k-1|k-1 is the estimated value of the previous moment, P k|k-1 is the current propagation error, P k|k is the current estimation error, P k-1|k-1 is the estimation error at the previous moment.

[0096] The first height difference and the second height difference are calculated using the first estimated coordinates and the second estimated coordinates obtained by Kalman filtering in each cell.

[0097] The first height difference corresponding to the cell is the first estimated coordinate (x max ,y max ,z max ) and the second estimated coordinate (x min ,y min ,z min ), that is, the first height difference is z max -z min .

[0098] The second height difference corresponding to the cell is calculated by determining the cell and its adjacent cells, calculating the height difference between the first estimated coordinates corresponding to the cell and the first estimated coordinates corresponding to each of its adjacent cells, and determining the height difference with the largest absolute value as the second height difference.

[0099] Take the cell in the middle as an example. There are cells around the cell in the middle, that is, there are 8 adjacent cells around the cell in the middle (x min ,y min ,z min ). The first estimated coordinate corresponding to the middle cell is (x max_0 ,y max_0 ,z max_0 ), and its adjacent cell coordinates are (x max_i ,y max_i ,z max_i), i is 1-8. Calculate z respectively max_0 -z min_i and the absolute value |z max_0 -z min_i |The maximum calculated result is the second height difference.

[0100] If the first height difference corresponding to the cell is greater than a preset first threshold, it means that a "vertical surface" exists in the current cell, and the cell is determined to be a non-abnormal cell.

[0101] If the second height difference corresponding to the cell is greater than a preset second threshold, the cell is determined to be a non-abnormal cell. Typically, the second threshold is greater than the first threshold.

[0102] If the absolute value of the second height difference is greater than a preset second threshold and the second height difference is greater than zero, the cell is determined to be a convex cell. If the absolute value of the second height difference is greater than a preset second threshold and the second height difference is less than zero, the cell is determined to be a concave cell.

[0103] It is determined whether the point clouds corresponding to two adjacent cells are spatially continuous. If they are continuous, the cell is determined to be a non-abnormal cell. If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

[0104] In one embodiment of the present invention, it is determined whether the point cloud corresponding to the cell and the point cloud corresponding to any adjacent cell are not spatially continuous by:

[0105] Determine the adjacent cells of the cell, the number of the adjacent cells is multiple; for the middle cells, each cell has 8 adjacent cells; for the boundary cells, the adjacent cells can be padded to 8 by data filling, or only the adjacent cells can be determined to be 3, and surface fitting is performed for the cell and its 3 adjacent cells;

[0106] Based on the collected point cloud data, surface fitting is performed on the point clouds corresponding to the cell and its adjacent cells. In this embodiment, 9 first estimated coordinates corresponding to the cell and its adjacent cells (a total of 9 cells) are used to perform surface fitting, and the tangent plane of the cell is determined according to the surface fitting result and used as the first tangent plane;

[0107] Select any one of the adjacent cells as the current adjacent cell and perform the following steps:

[0108] Performing surface fitting on the current adjacent cell and its adjacent cells. In this embodiment, 9 first estimated coordinates corresponding to the current adjacent cell and its adjacent cells (a total of 9 cells) are also used for surface fitting, and a tangent plane of the current adjacent cell is determined according to the surface fitting result and used as a second tangent plane;

[0109] If the angle between the first tangent plane and the second tangent plane is less than a preset third threshold, and the absolute value of the difference between the distance from the cell to the second tangent plane and the distance from the current adjacent cell to the first tangent plane is less than a preset fourth threshold, then the point clouds corresponding to the cell and the current adjacent cell are spatially continuous; otherwise, the point clouds corresponding to the cell and the current adjacent cell are not spatially continuous;

[0110] If the point cloud corresponding to the cell and the point cloud corresponding to any of its adjacent cells are not spatially continuous, then it is determined that the point cloud corresponding to the cell and the point cloud corresponding to any of its adjacent cells are not spatially continuous, then the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

[0111] Table 1 Schematic diagram of cell arrangement

[0112] F1 F2 F3 F4 F5 F6 S1 S2 S3 F7 F8 S4 S0 S5 F9 F10 S6 S7 S8 F11 F12 F13 F14 F15 F16

[0113] As shown in Table 1, take the example of determining whether the point cloud corresponding to the target cell S0 has a point cloud corresponding to its spatially continuous adjacent cells. The eight adjacent cells S1-S8 of the target cell S0. Surface fitting is performed on the point cloud data corresponding to the cells S0-S8, and the tangent plane L0 corresponding to the target cell S0 is determined based on the fitting results.

[0114] For each of the cells, surface fitting is performed on the point cloud corresponding to the cell and its adjacent cells using the first estimated coordinates corresponding to the cell and the first estimated coordinates of each adjacent cell, and the tangent plane of the fitting surface corresponding to the cell is determined based on the surface fitting result and used as the first tangent plane.

[0115] Based on the 9 first estimated coordinates corresponding to the cell S0 and its adjacent cells, the following surface fitting formula is used to perform surface fitting:

[0116] Z=AX 2 +BY 2 +CXY+DX+EY+F

[0117] Among them, A, B, C, D, E, and F are surface parameters.

[0118] The present invention performs Kalman filtering on each cell respectively and obtains the maximum and minimum point cloud heights in each cell, which can not only quickly and accurately calculate the height difference of the cell itself; but also, when fitting the surface, only the first estimated coordinates corresponding to each cell need to be used, which can greatly save the amount of calculation and improve the detection efficiency.

[0119] After Kalman filtering, the first estimated coordinate corresponding to the cell S0 is determined to be (X 0 ,Y 0 ,Z 0 ), and solve the fitting surface in (X 0 ,Y 0 ,Z 0 ) in the X and Y directions (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ), determine the normal of the cell as (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1);

[0120] According to the first estimated coordinates (X 0 ,Y 0 ,Z 0 ) and normal (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1) Determine the first tangent plane A 1 X+B 1 Y+C 1 Z+D 1 =0, where A 1 2 +B 1 2 +C 1 2 =1.

[0121] Then, taking cell S1 as the center, its eight adjacent cells are determined to be F1-F3, F6, S2, F8, S4, and S0. Surface fitting is performed on the point clouds corresponding to cell S1 and its adjacent cells, and the tangent plane L1 of the fitting surface corresponding to the target cell S0 is determined based on the fitting results. This process is repeated to obtain the tangent planes L2-L8 corresponding to cells S2-S8 respectively.

[0122] The distribution calculates the angle between the cutting plane L0 and the cutting plane L1-L8. Taking the cutting plane L0 and the cutting plane L1 as an example, the cutting plane L0 is A 1 X+B 1 Y+C 1 Z+D 1 =0, the tangent plane L1 is A 2 X+B 2 Y+C 2 Z+D 2=0, where A 2 2 +B 2 2 +C 2 2 =1.

[0123] The angle α between the cutting plane L0 and the cutting plane L1 is calculated as follows: α = arccos(A 1 ×A 2 +B 1 ×B 2 +C 1 ×C 2 ).

[0124] The distances from the point cloud corresponding to the target cell S0 to the cutting planes L1-L8 and the distances from the point clouds corresponding to each adjacent cell S1-S8 to the cutting plane L0 are calculated respectively, so as to calculate the difference between the distance from the point cloud corresponding to the cell to the second cutting plane and the distance from the point cloud corresponding to the current adjacent cell to the first cutting plane.

[0125] Taking the target cell S0 and its adjacent cell S1 as an example, the first estimated coordinate corresponding to the target cell S0 is (X 0 ,Y 0 ,Z 0 ), the tangent plane L0 is A 1 X+B 1 Y+C 1 Z+D 1 = 0, the first estimated coordinate corresponding to cell S1 is (X 2 ,Y 2 ,Z 2 ), the tangent plane L1 is A 2 X+B 2 Y+C 2 Z+D 2 =0.

[0126] The distance d from the point cloud corresponding to the target cell S0 to the tangent plane L1 1 The calculation formula is:

[0127] d 1 =|A 2 X 0 +B 2 Y 0 +C 2 Z 0 |;

[0128] The distance d from the point cloud corresponding to the adjacent cell S1 to the tangent plane L0 2 The calculation formula is:

[0129] d 2=|A 1 X 2 +B 1 Y 2 +C 1 Z 2 |;

[0130] The absolute value of the difference between the distance between the point cloud corresponding to the cell S0 and the second tangent plane L1 and the distance between the point cloud corresponding to the current adjacent cell S1 and the first tangent plane L0 is |d 1 -d 2 |.

[0131] It should be noted that, for each cell, its corresponding first height difference, second height difference and whether there are spatially continuous adjacent point clouds can be calculated. If the cell satisfies that the first height difference is greater than the preset first threshold or the absolute value of the second height difference is greater than the preset second threshold, the cell is determined to be an abnormal cell and the category of the cell is updated. Usually, the second threshold is greater than the first threshold. Further, if the absolute value of the second height difference is greater than the preset second threshold and the second height difference is greater than zero, the cell is determined to be a raised cell. If the absolute value of the second height difference is greater than the preset second threshold and the second height difference is less than zero, the cell is determined to be a sunken cell. And the cell category is further marked as an obstacle or pothole.

[0132] For the remaining cells, determine whether the point clouds corresponding to the two adjacent cells are spatially continuous. If they are continuous, the cell is determined to be a non-abnormal cell; if the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell. It should be noted that the flatness of each cell can also be estimated first and then the first height difference and the second height difference can be calculated, or they can be calculated in parallel.

[0133] If the category judgment result of the cell is a neutral cell, the category of the cell is determined to be the category of the cell currently stored in the scene map.

[0134] Alternatively, more cautiously, if the category judgment result of the cell is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

[0135] The present invention detects points with obvious abnormalities on the ground surface by making an adjustment abnormality judgment between the point cloud corresponding to the cell and the point cloud corresponding to the neighborhood (usually 8 neighborhoods). The ground equation is calculated for all cells. The difference between the present application and the prior art is that it does not directly fit the local points, but obtains the local quadratic surface, and then calculates the tangent plane at the current cell, which is used as the plane equation of this cell. This method calculates the surface equation rather than the equation with "average meaning" obtained by fitting, which can better characterize the plane characteristics of the current cell. Adjacent cells solve the normal angle and plane distance through their corresponding surface equations, and use this as the judgment standard for whether two cells are continuous, cluster similar areas, and perform plane fitting on the local areas of the cluster, which is used as the basis for solving the ground slope.

[0136] Anomaly detection and clustering based on surface plane features can be seen as two types of methods. One is to find anomalies in the forward direction. The points found by this method must be anomalies. The other is to find continuous ground areas in the reverse direction. This is the area where vehicles can drive. Suspected anomalies outside these two methods need to be further observed and confirmed. Through the two methods of anomaly judgment and surface plane feature clustering, anomaly detection is performed continuously in time series to achieve the purpose of "continuously" and more accurately detecting anomalies.

[0137] The adjacent cells with similar sections, i.e., continuous point cloud space, are grouped together, and a plane fitting is performed on the local area to obtain a local fitting plane, and the fitting equation is:

[0138] Z=A 3 X+B 3 Y+C 3

[0139] Among them A 3 , B 3 , C 3 To fit the plane parameters locally, the angle between the normal and the ground normal (0, 0, 1) is calculated to deduce the slope of the local area from the surface. If the slope is less than a specified threshold (e.g., 1 degree), it is considered to be a flat ground area, otherwise it is judged to be a slope surface.

[0140] Cluster abnormal cells and send their outer envelope as an obstacle to planning control so that the vehicle can respond to road anomalies in a timely manner. If there is a slope area, its area information is sent to adjust the planning control strategy. After detecting the abnormal point and the flatness of the area, a simple concavity and convexity judgment is made through the 8-neighborhood to make a simple distinction between the abnormalities. Regardless of concavity and convexity, the vehicle cannot pass. The specific concavity and convexity category and further height and depth judgment must be done through visual deep learning, which is not the focus of this patent.

[0141] This application divides the entire driving area into cells and estimates each cell separately in real time, effectively reducing the impact of point cloud noise and solving the technical problem of inaccurate radar detection of ground anomalies. In addition, continuous frame data observation solves the problem of inability to effectively identify ground anomalies due to the sparse point cloud of mechanical 16-line laser radar, and can detect ground anomalies as early as possible to ensure vehicle driving safety.

[0142] In another embodiment of the present invention, after creating the scene map and dividing the scene map into several cells, the cells to be loaded for analysis are not determined by the current point cloud data falling into the cells of the scene map, but are determined in the following way.

[0143] According to the map range, the scene is divided into fixed-size blocks (for example, 10m×10m), and the coordinates of the upper left corner of the block are recorded, with the upper left corner of the block as the origin, the horizontal right as the positive X direction, and the horizontal downward as the positive Y direction. The block is the smallest unit of map scheduling, that is, when the vehicle drives to a certain point, all blocks that appear within a certain range of this point (R=60m) are loaded, and those that exceed the range are unloaded. The purpose of scheduling is to reduce memory usage, and the current block information can be saved for use the next time it is loaded. Determine that the cells contained in all currently loaded blocks are cells to be detected and analyzed. In this embodiment, based on the stable and fixed large grid map of the unmanned driving area, a two-level grid map design method is adopted, and the blocks are scheduled in the memory; each time the system is started, the previously constructed grid map will be loaded, and each time the system is shut down, the current grid map will be automatically saved, and only the blocks near the vehicle will be retained in the real-time system, so that the entire grid map will continue to evolve in each operation.

[0144] In another embodiment of the present invention, in the previous embodiment, the maximum value of the point cloud corresponding to each unit, i.e., the first estimated coordinate, and the minimum value of the point cloud, i.e., the second estimated coordinate, are determined by Kalman filtering, and the first estimated coordinates are used for surface fitting and calculation of the second height difference. In this embodiment, after removing the abnormal points, all the point cloud data in each unit cell are used for surface fitting.

[0145] In this embodiment, surface fitting is performed based on all point cloud data in a cell and its adjacent cells. The surface fitting method is the same as the above embodiment and will not be described in detail. The difference is that surface fitting is performed based on the point cloud of the cell and its adjacent cells and the extreme points of the corresponding fitting surface within the cell range are determined. If the extreme point exists, the extreme point with the largest height is determined as the target point, otherwise the center point of the cell is determined as the target point. The partial derivatives of the surface obtained by fitting in the X and Y directions at the target point are solved to determine the normal at the target point. The first tangent plane is determined using the target point coordinates and the normal at the target point. The second tangent plane is determined by the method of. The distance from the point cloud corresponding to the cell to the second tangent plane is the distance from the target point corresponding to the cell to the second tangent plane. The distance from the point cloud corresponding to the adjacent cell to the first tangent plane is the distance from the target point corresponding to the adjacent cell to the first tangent plane, which will not be described in detail.

[0146] In one embodiment of the present invention, a ground anomaly detection device is provided. Figure 4 The ground anomaly detection device includes a laser radar sensor, a positioning module, a processor, a display device and a prompt device.

[0147] The image information of the drivable area is collected in advance by using an image acquisition device and input into the processor. The processor establishes a scene map based on the image information, determines the scene coordinate system, divides the scene map into a number of continuous cells of length and width S×S, and determines the position registration relationship between the scene coordinate system and the vehicle coordinate system. Alternatively, a map of the drivable area is obtained by other means and input into the processor. Each cell is provided with a cell type. The determination of the first cell type can be sampled by manual inspection and marking, or by using a laser radar sensor to detect all drivable areas and determine it based on the detection results.

[0148] The lidar sensor is positioned to collect point cloud data of a target area and transmit it to the processor.

[0149] The processor is also configured to align / map the newly acquired point cloud data of the target area to the scene map according to the pre-established position alignment relationship between the scene map, the scene coordinate system and the vehicle coordinate system, and determine the category of each cell in the scene map corresponding to the target area, and update the category of the cell if the category judgment result of the cell is an abnormal cell or a non-abnormal cell.

[0150] Based on the coordinates of each point cloud in the current point cloud data, the cell in which it is located, and the historical data in the scene map, Kalman filtering is performed on each cell to determine the first estimated coordinates (maximum value of the point cloud) and the second estimated coordinates (minimum value of the point cloud) in each cell.

[0151] Using the first estimated coordinates and the second estimated coordinates, calculate the first height difference corresponding to each cell, that is, the height difference between the point cloud with the smallest height value and the point cloud with the largest height value in the cell, and determine that the cell whose first height difference is greater than a preset first threshold is an abnormal cell.

[0152] Using the first estimated coordinates and the second estimated coordinates, calculating the second height difference corresponding to each of the cells, determining the cells whose absolute values ​​of the second height difference are greater than a preset second threshold value as abnormal cells, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the maximum height difference calculated value is the second height difference;

[0153] For the cells that are not determined to be abnormal cells, it is determined whether the point clouds corresponding to the two adjacent cells are spatially continuous. If they are continuous, the cell is determined to be a non-abnormal cell. If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

[0154] The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous:

[0155] Determine that one of two adjacent cells is a first cell and the other is a second cell;

[0156] Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane;

[0157] Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane;

[0158] If the angle between the first tangent plane and the second tangent plane is less than a preset third threshold, and the absolute value of the difference between the distance from the point cloud corresponding to the first cell to the second tangent plane and the distance from the second cell to the first tangent plane is less than a preset fourth threshold, then the point cloud space corresponding to the first cell and the second cell is continuous.

[0159] If the cell category determination result is a neutral cell, the category of the cell is determined to be the category of the cell currently stored in the scene map. Alternatively, if the cell category determination result is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

[0160] In one embodiment of the present invention, a computer-readable storage medium is provided for storing program instructions, wherein the program instructions are configured to be called to execute the steps of the method described in any of the above embodiments.

[0161] It should be noted that the above-mentioned ground anomaly detection device, computer-readable storage medium embodiments and ground anomaly detection method embodiments belong to the same inventive concept, and the entire content of the ground anomaly detection method embodiment is incorporated into the ground anomaly detection device and computer-readable storage medium embodiments by reference.

[0162] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0163] The above is only a specific implementation of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A ground anomaly detection method, characterized in that: The following steps are involved: Using a laser radar sensor to obtain point cloud data of a target area, dividing the target area into a plurality of cells, and determining the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include a height value of the point cloud; Calculating a first height difference corresponding to each of the cells, where the first height difference is a height difference between a point cloud with a minimum height value and a point cloud with a maximum height value in the cell, and determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell; Calculate the second height difference corresponding to each of the cells, and determine that the cell whose absolute value of the second height difference is greater than a preset second threshold is an abnormal cell, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the calculated value of the height difference with the largest absolute value is the second height difference; It also includes determining whether the point clouds corresponding to two adjacent cells are spatially continuous, and if so, determining that the cells are non-abnormal cells; The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous: Determine that one of two adjacent cells is a first cell and the other is a second cell; Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane; Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane; If the angle between the first tangent plane and the second tangent plane is smaller than a preset third threshold, and the absolute value of the difference between the distance from the point cloud corresponding to the first cell to the second tangent plane and the distance from the point cloud corresponding to the second cell to the first tangent plane is smaller than a preset fourth threshold, then the point clouds corresponding to the first cell and the second cell are spatially continuous; Determining that an abnormality exists in the ground area corresponding to the abnormal cell; The distance from the point cloud corresponding to the cell to a plane is determined in the following manner: Based on the coordinates of the point clouds corresponding to the cell and all its adjacent cells, surface fitting is performed to obtain a fitting surface; If the fitting surface has an extreme point in the cell, the extreme point is determined as the target point; if the fitting surface does not have an extreme point in the cell, the point cloud corresponding to the center of the cell is determined as the target point; The distance from the target point to the plane is calculated and used as the distance from the point cloud corresponding to the cell to the plane.

2. The ground anomaly detection method according to claim 1, characterized in that: It also includes the following judgments: If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell.

3. The ground anomaly detection method according to claim 1, characterized in that: Performing surface fitting on the point clouds corresponding to the first cell and the second cell, and determining a first tangent plane corresponding to the first cell according to the surface fitting result: Based on the coordinates of the point clouds corresponding to the first cell and all its adjacent cells, the following surface fitting formula is used to perform surface fitting and obtain a fitting surface: Z=AX 2 +BY 2 +CXY+DX+EY+F Among them, A, B, C, D, E, F are surface parameters, and X, Y, Z are spatial coordinates; Determine the coordinates of the target point of the first cell, the target point is the extreme point of the fitting surface in the first cell or the center point of the first cell, and solve the partial derivatives (ΦZ) of the fitting surface in the X and Y directions at the target point (X0, Y0, Z0) X0,Y0|dx ,ΦZ X0,Y0|dy ), determine the normal of the cell as (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1); According to the target point (X0, Y0, Z0) and the normal (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1) Determine the first tangent plane A1X+B1Y+C1Z+D1=0, where A1, B1, C1, D1 are the first tangent plane parameters, X, Y, Z are the spatial coordinates, the target point is on the first tangent plane and the normal (ΦZ X0,Y0|dx ,ΦZ X0,Y0|dy ,1)perpendicular to the first tangent plane.

4. The ground anomaly detection method according to claim 3, characterized in that: The distance from the point cloud corresponding to the first cell to the second tangent plane is determined in the following manner: If the fitting surface has an extreme point in the first cell, the extreme point is determined as the target point; if the fitting surface does not have an extreme point in the first cell, the point cloud corresponding to the center of the first cell is determined as the target point; Determine the second tangent plane as A2X+B2Y+C2Z+D2=0, where A2, B2, C2, D2 are the second tangent plane parameters, and X, Y, Z are the spatial coordinates; The distance from the target point to the second tangent plane is calculated and used as the distance from the first cell to the second tangent plane.

5. The ground anomaly detection method according to claim 1, characterized in that: The following steps are also included: A scene map is established based on the drivable area and a coordinate system of the scene map is determined as a scene coordinate system, and the scene map is divided into a number of cells; Predetermining and storing the categories of each cell in the scene map, wherein the categories of the cells include abnormal cells and non-abnormal cells; Determine a position registration relationship between the scene coordinate system and the vehicle coordinate system, map the newly acquired point cloud data of the target area to the scene map according to the position registration relationship, and determine the category of each cell in the scene map corresponding to the target area; If the category determination result of the cell is an abnormal cell or a non-abnormal cell, the category of the cell is updated.

6. The ground anomaly detection method according to claim 5, characterized in that: If the category determination result of the cell is a neutral cell, determining the category of the cell to be the category of the cell currently stored in the scene map; or, If the category judgment result of the cell is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

7. The ground anomaly detection method according to claim 5, characterized in that: After mapping the most recently acquired point cloud data of the target area to the scene map according to the position registration relationship, it also includes, for each cell in the scene map corresponding to the target area, performing Kalman filtering on the point cloud data corresponding to the cell based on historical data and current data, and obtaining a first estimated coordinate and a second estimated coordinate of the point cloud corresponding to the cell, wherein the first estimated coordinate is the coordinate of the point cloud with the largest height value in the cell, and the second estimated coordinate is the coordinate of the point cloud with the smallest height value in the cell.

8. The ground anomaly detection method according to claim 7, characterized in that: The first height difference corresponding to the cell is a height difference between the first estimated coordinate and the second estimated coordinate in the vertical direction.

9. The ground anomaly detection method according to claim 7, characterized in that: The second height difference corresponding to the cell is calculated by determining the cell and its adjacent cells, calculating the height difference between the first estimated coordinates corresponding to the cell and the first estimated coordinates corresponding to each of its adjacent cells, and determining the height difference with the largest absolute value as the second height difference.

10. The ground anomaly detection method according to claim 8, characterized in that: The method also includes performing surface fitting for each of the cells using first estimated coordinates corresponding to the cell and all of its adjacent cells, and determining whether point clouds corresponding to adjacent cells are spatially continuous.

11. The ground anomaly detection method according to claim 1, characterized in that: If the absolute value of the second height difference is greater than a preset second threshold and the second height difference is greater than zero, determining that the cell is a protruding cell; If the absolute value of the second height difference is greater than a preset second threshold and the second height difference is less than zero, it is determined that the cell is a concave cell.

12. A ground anomaly detection device, characterized in that: The system comprises a laser radar sensor and a processor, wherein the laser radar sensor is configured to collect point cloud data of a target area; the processor is configured to receive the point cloud data and perform the following steps based on the point cloud data: Divide the target area into a plurality of cells, and determine the cell where the point cloud is located according to the coordinates of each point cloud in the point cloud data, wherein the coordinates of the point cloud include the height value of the point cloud; Calculating a first height difference corresponding to each of the cells, where the first height difference is a height difference between a point cloud with a minimum height value and a point cloud with a maximum height value in the cell, and determining a cell whose first height difference is greater than a preset first threshold value as an abnormal cell; Calculate the second height difference corresponding to each of the cells, and determine that the cell whose absolute value of the second height difference is greater than a preset second threshold is an abnormal cell, wherein the method for determining the second height difference comprises calculating the height difference between the point cloud with the largest height value in the cell and the point cloud with the largest height value in each of its adjacent cells, and determining that the calculated value of the height difference with the largest absolute value is the second height difference; It also includes determining whether the point clouds corresponding to two adjacent cells are spatially continuous, and if so, determining that the cells are non-abnormal cells; If the point cloud corresponding to the cell is not spatially continuous with the point cloud corresponding to any of its adjacent cells, the cell is determined to be a neutral cell, and the neutral cell is an abnormal cell or a non-abnormal cell; The following method is used to determine whether the point clouds corresponding to two adjacent cells are spatially continuous: Determine that one of two adjacent cells is a first cell and the other is a second cell; Performing surface fitting based on the coordinates of the point clouds corresponding to the first cell and all the adjacent cells of the first cell, and determining a tangent plane of the first cell according to the surface fitting result as the first tangent plane; Performing surface fitting based on the coordinates of the point clouds corresponding to the second cell and all the adjacent cells of the second cell, and determining a tangent plane of the second cell according to the surface fitting result as a second tangent plane; If the angle between the first tangent plane and the second tangent plane is smaller than a preset third threshold, and the absolute value of the difference between the distance from the point cloud corresponding to the first cell to the second tangent plane and the distance from the point cloud corresponding to the second cell to the first tangent plane is smaller than a preset fourth threshold, then the point clouds corresponding to the first cell and the second cell are spatially continuous; Determining that an abnormality exists in the ground area corresponding to the abnormal cell; The distance from the point cloud corresponding to the cell to a plane is determined in the following manner: Based on the coordinates of the point clouds corresponding to the cell and all its adjacent cells, surface fitting is performed to obtain a fitting surface; If the fitting surface has an extreme point in the cell, the extreme point is determined as the target point; if the fitting surface does not have an extreme point in the cell, the point cloud corresponding to the center of the cell is determined as the target point; The distance from the target point to the plane is calculated and used as the distance from the point cloud corresponding to the cell to the plane.

13. The ground anomaly detection device according to claim 12, characterized in that: Also includes: Pre-establishing a scene map including a drivable area, wherein the scene map is configured with a scene coordinate system, and the scene map is divided into a plurality of cells; Determine a position registration relationship between the vehicle coordinate system and the scene coordinate system based on the vehicle coordinate system and the current position of the vehicle in the scene map; The processor is also configured to align the point cloud data of the currently acquired target area to the scene map according to the position alignment relationship between the scene coordinate system and the vehicle coordinate system, and determine the category of each cell in the scene map corresponding to the target area, and update the category of the cell if the category judgment result of the cell is an abnormal cell or a non-abnormal cell.

14. The ground anomaly detection device according to claim 13, characterized in that: If the category determination result of the cell is a neutral cell, determining the category of the cell to be the category of the cell currently stored in the scene map; or, If the category judgment result of the cell is a neutral cell, the neutral cell is determined to be an abnormal cell and the category of the cell is updated.

15. A computer-readable storage medium for storing program instructions, characterized in that: The program instructions are configured to be called to execute the steps of the method according to any one of claims 1 to 11.

Images