Two-dimensional slope map reconstruction method based on terrace inspection vehicle based on orthogonal two-dimensional point cloud

Abstract

The present invention discloses a two-dimensional slope map reconstruction method for a terrace inspection vehicle based on an orthogonal two-dimensional point cloud. The method determines whether the obstacle in front is a passable slope based on the environmental data of the terrace inspection vehicle represented by the orthogonal two-dimensional point cloud, and reconstructs the two-dimensional structural features of the passable slope into a global two-dimensional SLAM grid map. The steps are as follows: when the terrace inspection vehicle performs an inspection task, it determines whether there is an obstacle in front of the inspection vehicle based on its orthogonal two-dimensional point cloud distribution. If so, the obstacle and the ground point cloud are filtered and extracted, and a straight line fitting is performed on the obstacle point cloud and the ground point cloud. It is determined whether the obstacle is a passable slope based on the fitting degree and the angle of the fitted straight line. If so, the slope is mapped into the global two-dimensional SLAM grid map. The present invention provides a passable slope judgment and obstacle correction method for the terrace inspection vehicle when it crosses between terrace blocks, assisting the terrace inspection vehicle to complete the crossing between terrace blocks.

Description

Technical Field

[0001] The present invention relates to the field of agricultural intelligent environmental data perception and processing, and in particular to a two-dimensional slope map reconstruction method based on an orthogonal two-dimensional point cloud terrace inspection vehicle. Background Art

[0002] Agricultural science and technology are the core content and key support for agricultural modernization and a key indicator of agricultural development as a major modern industry. The Central Government's No. 1 Document for 2024 emphasizes the need to accelerate agricultural scientific and technological innovation, increase its contribution rate, enhance agricultural mechanization and equipment, and promote the development of agriculture towards digitalization, intelligence, and precision agriculture. By 2022, my country's agricultural science and technology contribution rate reached 62.4%, and the national comprehensive mechanization rate for crop cultivation and harvesting exceeded 73%.

[0003] As a modern agricultural equipment with functions such as autonomous cruising and farmland monitoring, agricultural automatic inspection vehicles are often used in vast, flat farmlands and artificially constructed greenhouses. They are an important manifestation of agricultural technology. However, the popularity of such equipment in terraced agriculture is relatively low.

[0004] There are two main reasons: First, the terraced fields have complex terrain and scattered plots, which makes it difficult for terraced field automatic inspection vehicles to select sensors and it is difficult to take into account both environmental adaptability and economic return rate. For example, the common environmental perception sensor of the inspection vehicle is a point cloud sensor, which emits measuring light and calculates the position of the environmental mapping point by measuring the light emission position and reception time. After traversing all the emission positions, it finally generates environmental point cloud data. Common point cloud sensors include three-dimensional point cloud sensors, which are composed of densely stacked two-dimensional point cloud sensors. The environmental point cloud data is obtained by rotating the sensor motor. Although powerful, its high price and high power consumption do not conform to the principle of low cost, low energy consumption and high efficiency of agricultural inspection vehicles. In contrast, a single two-dimensional point cloud sensor is cheap and has low power consumption, but it can only obtain planar point cloud data. Its perception ability is particularly insufficient when faced with complex environments, especially terraced agricultural environments. Secondly, due to the unique three-dimensional terrain characteristics of terraces, terraced automatic inspection vehicles not only need to travel within a single terrace block, but also need to have the ability to pass between different terrace blocks. For example, when passing between terraces, it is necessary to determine the passable slope based on the terrace inspection vehicle's own power limitations and environmental conditions to complete the crossing between terrace blocks.

[0005] In addition, during common terrace inspections, the terrace environment is first mapped to complete the map inspection path planning. The common practice is to install a two-dimensional point cloud sensor horizontally on the roof of the terrace inspection vehicle to obtain the horizontal point cloud mapping of the surrounding environment, control the movement of the terrace inspection vehicle in the environment to complete the horizontal plane point cloud collection of the global environment, and finally input the SLAM mapping algorithm (such as the Gmapping mapping algorithm, the Cartographer mapping algorithm, etc.) to complete the generation of the global two-dimensional SLAM grid map. However, as a three-dimensional structure, the terrace slope is usually used to collect environmental data. The horizontal point cloud sensor is not comprehensive in its contour point cloud acquisition. First, the horizontal point cloud sensor collects the horizontal tangent point cloud data of the slope and identifies it as an obstacle, which will cause the originally passable slope area to be misjudged as an obstacle. Second, in the process of constructing the global two-dimensional SLAM grid map, due to the fixed height of the horizontal point cloud sensor, it can only capture obstacle information that is parallel to its horizontal viewing angle, which makes the area below the horizontal point cloud sensor a blind spot. Summary of the Invention

[0006] The purpose of the present invention is to solve the above-mentioned defects in the prior art and to provide a two-dimensional slope map reconstruction method based on an orthogonal two-dimensional point cloud terrace inspection vehicle. The present invention is based on a low-cost orthogonal two-dimensional point cloud sensor environmental perception module, combined with the characteristics of the terrace environment and the SLAM grid map of the horizontal two-dimensional point cloud sensor, to provide a method for detecting and identifying passable slopes between terraces and reconstructing a two-dimensional SLAM grid map. This solution saves the high cost and energy consumption required for the use of three-dimensional point cloud sensors, and makes up for the blind spot below the horizontal level in the perception of a single horizontal point cloud sensor, and makes the terrace inspection vehicle suitable for completing the detection and identification of three-dimensional slopes and the reconstruction of two-dimensional SLAM grid maps in complex terrace environments.

[0007] The purpose of the present invention can be achieved by taking the following technical solutions:

[0008] A two-dimensional slope map reconstruction method based on an orthogonal two-dimensional point cloud terrace inspection vehicle comprises the following steps:

[0009] S1. The terrace inspection vehicle uses an orthogonal 2D point cloud environment perception solution to provide environmental data, and uses the global 2D SLAM grid map as the front map to determine whether there are obstacles in front of the terrace inspection vehicle based on the distribution characteristics of the orthogonal 2D point cloud.

[0010] S2. According to the distribution characteristics of the orthogonal two-dimensional point cloud, statistical filtering, median filtering and voxel filtering are sequentially performed on the orthogonal two-dimensional point cloud;

[0011] S3. Based on the point cloud features of the vertical point cloud in the filtered orthogonal two-dimensional point cloud, the vertical point cloud is divided into an aerial point cloud and a ground point cloud, and the aerial point cloud is segmented and clustered, and the point cloud of the obstacle ahead is extracted from the obtained clusters;

[0012] S4. Extracting a fitting line between the point cloud of the front obstacle and the ground point cloud, and determining whether the front obstacle is a slope structure based on the degree of fit of the fitting line of the point cloud of the front obstacle. If so, determining whether the front obstacle is a passable slope based on the angle between the fitting lines of the point cloud of the front obstacle and the ground point cloud;

[0013] S5. Generate a slope 2D SLAM grid map based on the passable slope features represented by the point cloud of the obstacle ahead, and overlay the generated slope 2D SLAM grid map onto the global 2D SLAM grid map to complete the map reconstruction.

[0014] Furthermore, the orthogonal two-dimensional point cloud environment perception solution is:

[0015] Two two-dimensional point cloud sensors are assembled in an orthogonal manner, respectively called the horizontal two-dimensional point cloud sensor and the vertical two-dimensional point cloud sensor. The horizontal two-dimensional point cloud sensor uses the positive half-axis direction of the z-axis of the world coordinate system as the vertical direction, the positive half-axis direction of the x-axis as the 0-degree direction, and the positive half-axis direction of the y-axis as the 90-degree direction. The horizontal two-dimensional point cloud sensor rotates clockwise to scan the environmental information of the xy plane; the vertical two-dimensional point cloud sensor uses the positive half-axis direction of the x-axis of the world coordinate system as the vertical direction, the negative half-axis direction of the z-axis as the 0-degree direction, and the positive half-axis direction of the y-axis as the 90-degree direction. The vertical two-dimensional point cloud sensor rotates clockwise to scan the environmental information of the yz plane.

[0016] Furthermore, the criteria for determining a passable slope are as follows:

[0017] When the determination coefficient of the fitted straight line of the front obstacle point cloud extracted in S3 is greater than the set determination coefficient threshold, and the angle between the fitted straight line of the front obstacle point cloud and the ground point cloud obtained in S3 is less than the set angle threshold, it is considered that the obstacle in front of the terrace inspection vehicle is a passable slope. Among them, the set determination coefficient threshold is usually limited by the system error of the point cloud sensor itself and the filter coefficient factors, and the set angle threshold is usually limited by the terrace inspection vehicle's own power and vehicle structure factors.

[0018] Furthermore, the two-dimensional SLAM grid map is defined as follows:

[0019] The terrace inspection vehicle uses a 2D SLAM mapping algorithm to generate a global 2D SLAM grid map while inspecting the terraced fields, based on the environmental point cloud mapping sensed by the lateral 2D point cloud sensor. This 2D SLAM mapping algorithm can be used, such as Gmapping or Cartographer.

[0020] Furthermore, the two-dimensional slope map reconstruction method based on the orthogonal two-dimensional point cloud terrace inspection vehicle is characterized in that the process of step S1 is as follows:

[0021] S101, dividing the orthogonal two-dimensional point cloud into a vertical two-dimensional point cloud and a horizontal two-dimensional point cloud, wherein the vertical two-dimensional point cloud refers to the environmental point cloud mapping obtained by the vertical two-dimensional point cloud sensor, and the horizontal two-dimensional point cloud refers to the environmental point cloud mapping obtained by the horizontal two-dimensional point cloud sensor; filtering out the height H of the ground obstacles that the terrace inspection vehicle can cross in the vertical two-dimensional point cloud g The following points, as well as the terrace inspection vehicle roof seat can ensure no collision height H c The above points are filtered out, and the width W of the left and right ends of the terrace inspection vehicle can be guaranteed to not collide in the horizontal two-dimensional point cloud. l and W r points outside;

[0022] S102. Count the number of points in the filtered vertical two-dimensional point cloud and the horizontal two-dimensional point cloud whose distance from the front endpoint of the terrace inspection vehicle is less than a set distance L. If the number of points is greater than the set threshold, it is considered that the terrace inspection vehicle has detected an obstacle at a distance L in front.

[0023] Furthermore, the method for detecting and identifying passable slopes and correcting maps based on an orthogonal two-dimensional point cloud terrace inspection vehicle is characterized in that the process of step S2 is as follows:

[0024] S201, performing statistical filtering on the orthogonal two-dimensional point cloud to remove system noise and abnormal discrete points;

[0025] S202, performing median filtering on the orthogonal two-dimensional point cloud to smooth the point cloud shape and retain the point cloud structure;

[0026] S203 : Perform voxel filtering on the vertical two-dimensional point cloud in the orthogonal two-dimensional point cloud to balance overly dense ground point clouds generated because the vertical two-dimensional point cloud sensor is too close to the ground.

[0027] Furthermore, the two-dimensional slope map reconstruction method based on the orthogonal two-dimensional point cloud terrace inspection vehicle is characterized in that the process of step S3 is as follows:

[0028] S301. Separate the vertical two-dimensional point cloud in the orthogonal two-dimensional point cloud into a ground point cloud and an aerial point cloud using the SMRF (Simple Morphological Filter) method, where the ground point cloud refers to the point cloud mapping of the environment ground obtained by the vertical two-dimensional point cloud sensor, and the aerial point cloud refers to the remaining point cloud mappings except the point cloud mapping of the environment ground;

[0029] S302, clustering the aerial point cloud using the DBSCAN clustering algorithm to divide the aerial point cloud into multiple clusters;

[0030] S303. Find the cluster represented by the obstacle in front of the terrace inspection vehicle in the segmented clusters, and the steps are as follows: calculate the center coordinates of each cluster, and the center coordinates of each cluster are represented by the average coordinates of all points in the cluster; calculate the Euclidean distance from the center coordinates of each cluster to the center of the vertical two-dimensional point cloud sensor; retain the cluster closest to the center of the vertical two-dimensional point cloud sensor and located in the detection angle range of 0 to 180° of the vertical two-dimensional point cloud sensor, and eliminate the remaining clusters, so as to obtain the point cloud of the obstacle in front.

[0031] Furthermore, the two-dimensional slope map reconstruction method based on the orthogonal two-dimensional point cloud terrace inspection vehicle is characterized in that the process of step S4 is as follows:

[0032] S401, considering the front obstacle point cloud and the ground point cloud in the vertical two-dimensional point cloud as linear distributions, and performing straight line fitting using the least squares method;

[0033] S402: Calculate the coefficient of determination of the straight line fitted to the obstacle point cloud, and use the coefficient of determination as a criterion for judging the degree of straight line fit. When the coefficient of determination is higher than the set standard, it is considered that the obstacle represented by the obstacle point cloud is a slope.

[0034] S403: Calculate the angle between the obstacle point cloud and the fitting line of the ground point cloud. If the angle is lower than a set standard and combined with S402, the obstacle is determined to be a slope structure, the obstacle is considered to be a passable slope.

[0035] Furthermore, the two-dimensional slope map reconstruction method based on the orthogonal two-dimensional point cloud terrace inspection vehicle is characterized in that the process of step S5 is as follows:

[0036] S501, obtaining the relative position of the terrace inspection vehicle and the global two-dimensional SLAM grid map according to the AMCL (Adaptive Monte Carlo Localization) algorithm;

[0037] S502, clustering the lateral two-dimensional point cloud in the orthogonal two-dimensional point cloud using the DBSCAN clustering algorithm to obtain multiple clusters;

[0038] S503. Find the cluster represented by the obstacle in front of the terraced field inspection vehicle in the clustered clusters. The steps are as follows: calculate the center coordinates of each cluster in S502, where the center coordinates of each cluster are represented by the average coordinates of all points in the cluster; calculate the Euclidean distance from the center coordinates of each cluster to the center of the horizontal two-dimensional point cloud sensor; retain the cluster closest to the center of the horizontal two-dimensional point cloud sensor and located in the detection angle range of -90 to 90° of the horizontal two-dimensional point cloud sensor, and eliminate the remaining clusters, thereby obtaining the point cloud of the width of the obstacle in front;

[0039] S504, converting the obtained front obstacle width point cloud coordinates into the global two-dimensional SLAM grid map coordinate system;

[0040] S505. Calculate the distance d between the intersection point a of the horizontal plane of the lateral two-dimensional point cloud sensor and the slope and the intersection point b of the slope and the ground based on the distance H between the center of the lateral two-dimensional point cloud sensor and the ground and the slope slope θ calculated in the previous step.

[0041] S506, generate a slope 2D SLAM grid map, specifically as follows: (1) Initialize a new 2D grid map G, and set the grid value of the whole map to mask1. The resolution r and size of G are the same as the original grid map. Figure 1 Map the points of the front obstacle width point cloud to G, and set the corresponding grid value to mask2; (2) Take the m points with the smallest angle and the m points with the largest angle in the horizontal two-dimensional point cloud sensor angular coordinate system in the front obstacle width point cloud, and calculate their average point coordinates respectively, which are recorded as p min and p max , draw two vertical lines, perpendicular to the width of the obstacle point cloud fitting line, the two vertical lines pass through p min and p max ; (3) Take the center of the horizontal two-dimensional point cloud sensor as the positive direction, and p min and p max Starting from the initial point, n points are taken, where n is obtained by rounding down the distance L in S102, and the step size is set to the resolution r of G. The points are mapped to the grid map G in sequence, and the corresponding grid values ​​are set to mask3;

[0042] S507. Overlay the grid map G onto the global two-dimensional SLAM grid map. For the grid values ​​in the new map, when the grid in G is marked as mask1, the grid value of the global two-dimensional SLAM grid map is taken; when the grid is marked as mask2, it is taken as 0, indicating no obstacle; when it is marked as mask3, it is taken as 1, indicating there is an obstacle.

[0043] The present invention has the following advantages and effects compared to the prior art:

[0044] 1) Based on orthogonal two-dimensional point cloud sensors, the present invention provides a low-cost sensor selection and combination solution for terrace inspection vehicles, taking into account the principles of low cost and high efficiency in agricultural technology needs, and realizing environmental perception in complex terrace environments.

[0045] 2) Based on the actual three-dimensional environment of terraced fields, the present invention studies the problem of crossing between terraced blocks during terraced field inspection, and proposes a method for crossing the passable slope between terraced fields based on orthogonal two-dimensional point clouds to assist terraced field inspection vehicles in completing the crossing behavior between terraced blocks.

[0046] 3) The present invention addresses the problem of defects in the two-dimensional mapping of slopes in the existing SLAM maps based on horizontal two-dimensional point clouds. The slopes on the map are corrected through the point cloud information provided by the orthogonal two-dimensional point cloud sensor, thereby eliminating false obstacles that are misjudged, so that passable slopes are no longer judged as obstacles, and the real obstacles in the blind spots are supplemented, assisting the terrace inspection vehicle to complete obstacle avoidance and passage. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a flow chart of a method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle disclosed in an embodiment of the present invention;

[0048] Figure 2 This is a diagram of an experimental scene when a terrace inspection vehicle encounters an obstacle in an embodiment of the present invention;

[0049] Figure 3 2. This is a diagram showing an installation scheme for an orthogonal two-dimensional laser radar for a terrace inspection vehicle according to an embodiment of the present invention;

[0050] Figure 4 This is a flowchart of point cloud noise removal and obstacle point cloud extraction in an embodiment of the present invention;

[0051] Figure 5 This is a flow chart of slope determination based on obstacle characteristics in an embodiment of the present invention;

[0052] Figure 6 This is a flowchart of two-dimensional SLAM grid map reconstruction in an embodiment of the present invention;

[0053] Figure 7 is the slope width point cloud and its normal extracted in the embodiment of the present invention;

[0054] Figure 8 is a newly generated two-dimensional SLAM grid map G in an embodiment of the present invention;

[0055] Figure 9 This is a comparison diagram of the slope two-dimensional SLAM grid map before and after correction in an embodiment of the present invention. DETAILED DESCRIPTION

[0056] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying 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 making creative efforts shall fall within the scope of protection of the present invention.

[0057] Example

[0058] This embodiment discloses a method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle. Figure 1 The specific steps are as follows:

[0059] S1, simulate a terrace inspection vehicle inspection task scene, and use the orthogonal 2D laser radar as the orthogonal 2D point cloud sensor, such as Figure 2 As shown in Figure 1, the scene mainly consists of three parts: obstacles, a terrace inspection vehicle, and a host computer. The terrace inspection vehicle uses an orthogonal 2D lidar to sense obstacles and surrounding environment information, and transmits the acquired point cloud data to the host computer via a wireless communication module for corresponding processing.

[0060] Among them, the installation scheme of the orthogonal two-dimensional laser radar for terrace inspection vehicles is as follows: Figure 3 As shown, two 2D laser radars are assembled orthogonally. The lateral 2D laser radar 301 rotates clockwise with the positive z-axis of the world coordinate system as the vertical direction and the positive x-axis as the zero-degree rotation direction. It scans the environment in the xy plane. The vertical 2D laser radar 302 scans the environment in the yz plane with the positive x-axis of the world coordinate system as the vertical direction and the negative z-axis as the zero-degree rotation direction. The chassis is designed to be a differential three-wheel drive, consisting of two main components: a central universal wheel 303 and a pair of drive wheels 304 on the left and right sides of the chassis. The universal wheel located in the middle serves as an auxiliary support to ensure the overall stable operation of the chassis, while the drive wheel has independent speed control capabilities. By adjusting the different speeds of the wheels on both sides, flexible steering of the chassis can be achieved.

[0061] Among them, the specific method of judging whether there is an obstacle in front of the inspection vehicle is as follows: after the terrace inspection vehicle collects the orthogonal two-dimensional laser point cloud data, it sends it to the host computer for processing. First, the height H of the ground obstacles that the terrace inspection vehicle can cross in the vertical two-dimensional laser radar is filtered out. gThe following point cloud and the terrace inspection vehicle roof seat can ensure no collision height H c The above point cloud is then filtered out, and the width W of the terrace inspection vehicle on the left and right sides that can ensure no collision in the lateral two-dimensional laser radar is removed. l and W r Point clouds other than H g 、H c 、W l and W r According to the actual size setting of the terrace inspection vehicle), the vertical two-dimensional laser radar remaining point cloud and the horizontal two-dimensional laser radar remaining point cloud are obtained; then the number of points in the vertical two-dimensional laser radar remaining point cloud and the horizontal two-dimensional laser radar remaining point cloud whose distance from the front endpoint of the terrace inspection vehicle is less than the set distance L is counted. If the number of points in the vertical two-dimensional laser radar remaining point cloud or the horizontal two-dimensional laser radar remaining point cloud is greater than the set threshold, it is considered that the terrace inspection vehicle has detected an obstacle at a distance L in front.

[0062] S2. Based on the characteristics of the orthogonal 2D laser point cloud, the orthogonal 2D laser point cloud is filtered. First, the orthogonal 2D laser point cloud is statistically filtered to remove its system noise and abnormal discrete points. The standard deviation multiplier threshold is set to 0.01, and the number of neighboring points of the local neighborhood point average is set to 1. Next, the orthogonal 2D laser point cloud is median filtered to smooth the point cloud shape and retain the point cloud structure. The neighborhood radius of the median filter is set to 0.05. Finally, the vertical 2D laser point cloud is voxel filtered to balance the overly dense ground point cloud caused by the vertical 2D laser radar being too close to the ground. The voxel size is set to 0.05.

[0063] S3. Separate the aerial point cloud and the ground point cloud based on the vertical two-dimensional laser point cloud features, segment and cluster the aerial two-dimensional point cloud to extract the point cloud of the obstacle ahead. The specific steps are as follows: Use the SMRF (Simple Morphological Filter) algorithm to separate the ground point cloud, set the elevation threshold to 0.01, the slope threshold to 0.001, and the maximum diameter of the opening operation to 5; Use the DBSCAN clustering algorithm to cluster the vertical two-dimensional point cloud, set the neighborhood radius to 0.5, and the minimum threshold within the neighborhood to 3; Find the cluster represented by the obstacle ahead of the terrace inspection vehicle in the segmented clusters, the steps are as follows: Calculate the center coordinates of each cluster, the center coordinates of each cluster are represented by the average coordinates of all points in the cluster; Calculate the Euclidean distance from the center coordinates of the cluster to the center of the vertical two-dimensional laser radar; Keep the cluster closest to the center of the vertical two-dimensional laser radar and located in the vertical two-dimensional laser radar detection angle range of 0 to 180°, and eliminate the remaining clusters to obtain the cluster represented by the obstacle ahead. The flow charts of S2 and S3 are as follows: Figure 4 shown.

[0064] S4. Fit the obstacle point cloud into a straight line, and determine whether it is a slope structure based on the degree of fit. If so, determine whether it is a passable slope based on its slope. The specific steps are as follows: the front obstacle point cloud and the ground point cloud in the vertical two-dimensional laser point cloud are regarded as linear distributions, and the least squares method is used for straight line fitting; the determination coefficient of the obstacle point cloud fitting line is calculated, and the determination coefficient is used as the standard for judging the straight line fit. When the determination coefficient is higher than the set standard, the obstacle represented by the obstacle point cloud is considered to be a slope; the angle between the obstacle point cloud and the ground point cloud fitting line is calculated. When the angle is lower than the set standard of 30° and the obstacle is judged to be a slope, the obstacle is considered to be a passable slope. The judgment process is as follows: Figure 5 shown.

[0065] S5. Based on the obtained passable slope features, cover them onto the two-dimensional SLAM grid map to complete the map reconstruction. The flowchart is as follows Figure 6 As shown, the specific steps are as follows: according to the AMCL (Adaptive Monte Carlo Localization) algorithm, the relative position of the terrace inspection vehicle and the global two-dimensional SLAM grid map is obtained; the DBSCAN clustering algorithm is used to cluster the horizontal two-dimensional laser point cloud; the cluster represented by the width point cloud of the obstacle in front of the terrace inspection vehicle is found in the cluster clusters after clustering, the steps are as follows: the center coordinates of each cluster are calculated, and the center coordinates of each cluster are represented by the average coordinates of all points in the cluster; the Euclidean distance from the center coordinates of the cluster to the center of the horizontal two-dimensional laser radar is calculated; the cluster closest to the center of the horizontal two-dimensional laser radar and located in the range of -90 to 90° of the horizontal two-dimensional laser radar detection angle is retained, and the remaining clusters are eliminated, thereby obtaining the cluster represented by the width point cloud of the obstacle in front; the coordinates of the point cloud of the width of the obstacle in front are converted to the two-dimensional SLAM grid map coordinate system; according to the distance H between the horizontal two-dimensional laser radar and the ground and the slope slope θ, the slope length n under the blind spot of the horizontal two-dimensional laser radar is obtained by the following formula:

[0066]

[0067] Next, a two-dimensional grid map of the slope structure is generated as follows: (1) Initialize a new two-dimensional grid map G, and set the full grid to mask1. The resolution r and size of G are the same as the original grid map. Figure 1 Map the points of the front obstacle width point cloud to G, and set the corresponding grid value to mask2; (2) Take the m points with the smallest angle and the m points with the largest angle in the horizontal two-dimensional laser radar angle coordinate system in the front obstacle width point cloud, and calculate their average point coordinates respectively, which are recorded as p min and p max, draw two vertical lines, perpendicular to the width of the obstacle point cloud fitting line, the two vertical lines pass through p min and p max , here m is set to 3, such as Figure 7 (3) Take the center of the horizontal two-dimensional point cloud sensor as the positive direction, and p min and p max As the initial point, we take n points and set the step size to the resolution r of G. We map the points to the grid map G in sequence and set the corresponding grid values ​​to mask3. At this time, the grid map G is drawn. Figure 8 As shown; (4) Overlay the grid map G onto the global 2D SLAM grid map. For the grid values ​​in the new map, when the grid in G is marked as mask1, the grid value of the global 2D SLAM grid map is taken; when the grid is marked as mask2, it is taken as 0, indicating no obstacle; when it is marked as mask3, it is taken as 1, indicating an obstacle. The comparison of the original global 2D SLAM grid map before and after coverage is as follows: Figure 9 As shown in the figure, the left side is the original global two-dimensional SLAM grid map, and the right side is the covered global two-dimensional SLAM grid map. It can be seen that this method can eliminate the originally misidentified slope obstacles in the global two-dimensional SLAM grid map and fill in the blind spots of the slope.

[0068] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A two-dimensional slope map reconstruction method based on an orthogonal two-dimensional point cloud terrace inspection vehicle, characterized in that: The method comprises the following steps: S1. The terrace inspection vehicle uses an orthogonal 2D point cloud environment perception solution to provide environmental data, and uses the global 2D SLAM grid map as the front map to determine whether there are obstacles in front of the terrace inspection vehicle based on the distribution characteristics of the orthogonal 2D point cloud. The process of step S1 is as follows: S101, dividing the orthogonal two-dimensional point cloud into a vertical two-dimensional point cloud and a horizontal two-dimensional point cloud, wherein the vertical two-dimensional point cloud refers to the environmental point cloud mapping obtained by the vertical two-dimensional point cloud sensor, and the horizontal two-dimensional point cloud refers to the environmental point cloud mapping obtained by the horizontal two-dimensional point cloud sensor; filtering out the height H of the ground obstacles that the terrace inspection vehicle can cross in the vertical two-dimensional point cloud g The following points, as well as the terrace inspection vehicle roof seat can ensure no collision height H c The above points are filtered out, and the width W of the left and right ends of the terrace inspection vehicle can be guaranteed to not collide in the horizontal two-dimensional point cloud. l and W r points outside; S102, counting the number of points in the filtered vertical two-dimensional point cloud and the horizontal two-dimensional point cloud whose distance from the front endpoint of the terrace inspection vehicle is less than a set distance L, and if the number of points is greater than a set threshold, it is considered that the terrace inspection vehicle has detected an obstacle at a distance L in front; S2. According to the distribution characteristics of the orthogonal two-dimensional point cloud, statistical filtering, median filtering and voxel filtering are sequentially performed on the orthogonal two-dimensional point cloud; S3. Based on the point cloud features of the vertical point cloud in the filtered orthogonal two-dimensional point cloud, the vertical point cloud is divided into an aerial point cloud and a ground point cloud, and the aerial point cloud is segmented and clustered, and the point cloud of the obstacle ahead is extracted from the obtained clusters; S4. Extracting a fitting line between the point cloud of the front obstacle and the ground point cloud, and determining whether the front obstacle is a slope structure based on the degree of fit of the fitting line of the point cloud of the front obstacle. If so, determining whether the front obstacle is a passable slope based on the angle between the fitting lines of the point cloud of the front obstacle and the ground point cloud; S5. Generate a slope 2D SLAM grid map based on the passable slope features represented by the point cloud of the obstacle ahead, and overlay the generated slope 2D SLAM grid map onto the global 2D SLAM grid map to complete the map reconstruction.

2. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 1 is characterized in that: The orthogonal two-dimensional point cloud environment perception solution is: Two two-dimensional point cloud sensors are assembled in an orthogonal manner, respectively called the horizontal two-dimensional point cloud sensor and the vertical two-dimensional point cloud sensor. The horizontal two-dimensional point cloud sensor uses the positive half-axis direction of the z-axis of the world coordinate system as the vertical direction, the positive half-axis direction of the x-axis as the 0-degree direction, and the positive half-axis direction of the y-axis as the 90-degree direction. The horizontal two-dimensional point cloud sensor rotates clockwise to scan the environmental information of the xy plane; the vertical two-dimensional point cloud sensor uses the positive half-axis direction of the x-axis of the world coordinate system as the vertical direction, the negative half-axis direction of the z-axis as the 0-degree direction, and the positive half-axis direction of the y-axis as the 90-degree direction. The vertical two-dimensional point cloud sensor rotates clockwise to scan the environmental information of the yz plane.

3. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 2 is characterized in that: The criteria for determining a passable slope are: When the determination coefficient of the straight line fitted by the front obstacle point cloud extracted in S3 is greater than the set determination coefficient threshold, and the angle between the fitting straight line of the front obstacle point cloud and the ground point cloud obtained in S3 is less than the set angle threshold, the obstacle in front of the terrace inspection vehicle is considered to be a passable slope.

4. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 3 is characterized in that: The global two-dimensional SLAM grid map is defined as: The terrace inspection vehicle uses a two-dimensional SLAM mapping algorithm to generate a global two-dimensional SLAM grid map when inspecting the terrace blocks based on the environmental point cloud mapping perceived by the horizontal two-dimensional point cloud sensor; the two-dimensional SLAM mapping algorithm uses the Gmapping mapping algorithm or the Cartographer mapping algorithm.

5. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 4 is characterized in that: The process of step S2 is as follows: S201, performing statistical filtering on the orthogonal two-dimensional point cloud to remove system noise and abnormal discrete points; S202, performing median filtering on the orthogonal two-dimensional point cloud to smooth the point cloud shape and retain the point cloud structure; S203 : Perform voxel filtering on the vertical two-dimensional point cloud in the orthogonal two-dimensional point cloud.

6. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 5 is characterized in that: The process of step S3 is as follows: S301, using the SMRF method to separate the vertical two-dimensional point cloud in the orthogonal two-dimensional point cloud into a ground point cloud and an aerial point cloud, wherein the ground point cloud refers to the point cloud mapping of the environment ground obtained by the vertical two-dimensional point cloud sensor, and the aerial point cloud refers to the remaining point cloud mappings except the point cloud mapping of the environment ground; S302, clustering the aerial point cloud using the DBSCAN clustering algorithm to divide the aerial point cloud into multiple clusters; S303. Find the cluster represented by the obstacle in front of the terrace inspection vehicle in the segmented clusters, and the steps are as follows: calculate the center coordinates of each cluster, and the center coordinates of each cluster are represented by the average coordinates of all points in the cluster; calculate the Euclidean distance from the center coordinates of each cluster to the center of the vertical two-dimensional point cloud sensor; retain the cluster closest to the center of the vertical two-dimensional point cloud sensor and located in the detection angle range of 0 to 180° of the vertical two-dimensional point cloud sensor, and eliminate the remaining clusters, so as to obtain the point cloud of the obstacle in front.

7. The method for reconstructing a two-dimensional slope map based on an orthogonal two-dimensional point cloud terrace inspection vehicle according to claim 6 is characterized in that: The process of step S4 is as follows: S401, considering the front obstacle point cloud and the ground point cloud in the vertical two-dimensional point cloud as linear distributions, and performing straight line fitting using the least squares method; S402: Calculate the coefficient of determination of the straight line fitted to the obstacle point cloud, and use the coefficient of determination as a criterion for judging the degree of straight line fit. When the coefficient of determination is higher than the set standard, it is considered that the obstacle represented by the obstacle point cloud is a slope. S403: Calculate the angle between the obstacle point cloud and the fitting line of the ground point cloud. If the angle is lower than a set standard and combined with S402, the obstacle is determined to be a slope structure, the obstacle is considered to be a passable slope.

Images