Obstacle extraction method, equipment and medium based on local plane fitting

Abstract

The present invention relates to a method, device, and medium for obstacle extraction based on local plane fitting. The method comprises the following steps: obtaining a three-dimensional point cloud of a target area; classifying the three-dimensional point cloud of the target area into ground points and non-ground points using a cloth-simulation filtering algorithm; clustering the non-ground points to obtain candidate obstacle clusters, and screening the candidate obstacle clusters to obtain a preliminary obstacle point cloud; performing plane fitting on the local ground near the preliminary obstacle point cloud using a least-squares plane fitting algorithm based on consistent random sampling, treating the points above the fitted local ground as the same obstacle point cloud; clustering the point clouds again, and selecting the cluster with the largest number of clustered points as the optimized obstacle point cloud; and measuring the optimized obstacle point cloud using a directed bounding box to obtain the position and morphological parameter information of each obstacle. Compared with existing technologies, the present invention has the advantages of high accuracy and good real-time performance.

Description

Technical Field

[0001] The present invention relates to the field of image processing, and in particular to a method, device and medium for extracting obstacles based on local plane fitting. Background Art

[0002] In the mission scenario of planetary surface patrol and exploration, due to the high communication delay and environmental uncertainty, the exploration vehicle needs to have strong autonomy, including autonomous navigation and autonomous obstacle avoidance capabilities.

[0003] Accurately obtaining information about the distribution and shape of obstacles is a prerequisite for the rover to perform autonomous obstacle avoidance. However, the currently commonly used obstacle detection algorithms use the rover's navigation camera images to identify and measure obstacles. They need to go through processes such as dense matching and triangulation to obtain a three-dimensional point cloud of the target area before performing obstacle detection processing. They have poor real-time performance and cannot meet the rover's goal of high-speed and long-distance detection. On the other hand, the currently commonly used obstacle detection algorithms also need to further improve their accuracy in identifying and extracting obstacles.

[0004] Therefore, it is urgent to design an obstacle extraction method with high accuracy and good real-time performance. Summary of the Invention

[0005] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a method, device and medium for obstacle extraction based on local plane fitting with high accuracy and good real-time performance.

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

[0007] According to a first aspect of the present invention, a method for extracting obstacles is provided, the method comprising the following steps:

[0008] Obtain the three-dimensional point cloud of the target area;

[0009] Using a cloth simulation filtering algorithm, classifying the three-dimensional point cloud of the target area into ground points and non-ground points;

[0010] Performing point cloud clustering on non-ground points to obtain candidate obstacle clusters, and screening the candidate obstacle clusters to obtain a preliminary obstacle point cloud;

[0011] A least squares plane fitting algorithm based on random sampling consistency is used to perform plane fitting on the local ground near the preliminary obstacle point cloud. The points above the local ground obtained by fitting are regarded as the same obstacle point cloud. After clustering the point cloud again, the cluster with the largest number of clustered points is regarded as the optimized obstacle point cloud.

[0012] The optimized obstacle point cloud is measured using a directed bounding box to obtain the position and morphological parameter information of each obstacle.

[0013] Preferably, the obtaining of the three-dimensional point cloud of the target area is specifically: collecting a depth image of the target area through a depth camera, calculating the corresponding three-dimensional coordinates of each valid pixel point in the depth image in combination with the internal parameters of the depth camera, and generating a three-dimensional point cloud of the target area.

[0014] Preferably, point cloud clustering is performed on the non-ground points, specifically: a Euclidean clustering algorithm is used to cluster the non-ground points.

[0015] Preferably, the candidate obstacle clusters are screened to obtain a preliminary obstacle point cloud, and the screening criteria used are: the cluster midpoint is above the ground; the number of cluster midpoints is greater than a given threshold; the maximum height of the cluster point cloud above the ground is greater than a given threshold.

[0016] Preferably, the method of taking the cluster with the largest number of points as the optimized obstacle point cloud further includes: repeatedly applying a screening criterion to the optimized obstacle point cloud to obtain a final obstacle point cloud.

[0017] Preferably, the least squares plane fitting algorithm based on random sampling consistency is used to perform plane fitting on the local ground near the preliminary obstacle point cloud, specifically:

[0018] For any initial obstacle point cloud cluster, calculate its maximum circumscribed circle radius r, obtain its surrounding ground point cloud data with cr as the radius, and fit the ground of the current part of the point cloud; where c is a constant coefficient set greater than 1.

[0019] Preferably, the local plane is fitted using a least squares plane fitting algorithm based on random sampling consistency, and the objective function expression is:

[0020]

[0021] Where: a, b, c, d are plane parameters; n is the number of points; (X i ,Y i ,Z i ) is the three-dimensional coordinate corresponding to point i.

[0022] Preferably, the use of a directed bounding box to measure the optimized obstacle point cloud to obtain the position and morphological parameter information of each obstacle is as follows:

[0023] Calculate the centroid of the optimized obstacle point cloud, perform de-centroiding, and construct the covariance matrix;

[0024] Perform singular value decomposition on the covariance matrix to obtain the eigenvector of the point cloud;

[0025] According to the eigenvector, the point cloud is projected into the feature space, and the ranges of the optimized obstacle point cloud in the X, Y, and Z directions in the feature space are obtained as the final obstacle measurement results.

[0026] According to a second aspect of the present invention, an electronic device is provided, comprising a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the program, any one of the methods described above is implemented.

[0027] According to a third aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored, and when the program is executed by a processor, any one of the methods described above is implemented.

[0028] Compared with the prior art, the present invention has the following beneficial effects:

[0029] 1) Based on the distinction between ground points and obstacles, the present invention adopts a least squares plane fitting algorithm based on random sampling consistency to perform plane fitting on the local ground near the obstacle point cloud, obtaining a more accurate ground than the initial ground, and the extracted obstacles are also more complete and accurate.

[0030] 2) The present invention adopts a cloth simulation filtering algorithm to classify the three-dimensional point cloud of the target area to effectively and accurately separate ground points from non-ground points.

[0031] 3) To address the problem of non-obstacle point clouds existing in non-ground points after fabric simulation filtering and separation, the present invention sets three screening criteria to retain the point cloud data of obstacles to the greatest extent.

[0032] 4) The present invention uses a depth camera as the data acquisition source. Combined with the camera's internal parameters, simple calculations are performed to obtain point cloud data, and then corresponding obstacle extraction and measurement are performed. The point cloud acquisition process has a small amount of calculation, which improves the calculation speed and is beneficial to real-time deployment on the detector. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a flow chart of the method of the present invention;

[0034] Figure 2 Examples of the RGB and depth images used and the generated point clouds; (a), (b), and (c) correspond to the original RGB image, depth image, and 3D point cloud (5-meter range), respectively.

[0035] Figure 3 Comparison diagram of the extraction effect of plane fitting secondary optimization; (a) and (b) are the extraction effects before and after the local plane fitting secondary optimization, respectively;

[0036] Figure 4This is the overall effect diagram of stone extraction in the embodiment;

[0037] Figure 5 Detailed diagram of the stone extraction effect in the embodiment; wherein (a), (b), and (c) correspond to three extraction angles respectively. DETAILED DESCRIPTION

[0038] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0039] Example

[0040] like Figure 1 As shown, this embodiment provides an obstacle extraction method based on local plane fitting, which includes the following steps:

[0041] S1. Obtain the 3D point cloud of the target area, specifically:

[0042] The depth image of the target area is collected by the depth camera. Combined with the internal parameters of the depth camera, the corresponding three-dimensional coordinates of each valid pixel in the depth image are calculated to generate a three-dimensional point cloud of the target area.

[0043] Specifically, given the intrinsic parameter f of the depth camera x ,f y ,c x ,c y , for a point (u i ,v i ), if the depth value corresponding to this point is D i , then its corresponding three-dimensional coordinates (X i ,Y i ,Z i ) can be expressed as the following formula. By calculating all valid points in the image, the three-dimensional point cloud of the target area can be obtained.

[0044]

[0045] S2. Use the cloth simulation filter (CSF) algorithm to classify the 3D point cloud of the target area into ground points and non-ground points, achieving accurate and effective separation of ground points and non-ground points.

[0046] Specifically, starting from a point, the clustering distance threshold is set according to the density of the point cloud and the size of the obstacle. Points less than this distance are considered to belong to the same cluster. After finding all nearby points, the above steps are repeated, and the point cloud is finally divided into several independent clusters.

[0047] S3. Use the Euclidean clustering algorithm to cluster the non-ground points to obtain candidate obstacle clusters, and then screen the candidate obstacle clusters to obtain a preliminary obstacle point cloud.

[0048] The specific screening criteria used are: the cluster midpoint is above the ground; the number of cluster midpoints is greater than a given threshold; the maximum height of the cluster point cloud above the ground is greater than a given threshold.

[0049] S4. Use the least squares plane fitting algorithm based on random sampling consistent RANSAC to perform plane fitting on the local ground near the obstacle point cloud. The points above the local ground obtained by fitting are regarded as the same stone obstacle point cloud. After clustering the point cloud again, the cluster with the largest number of clustered points is regarded as the optimized stone obstacle point cloud.

[0050] In this embodiment, for any initial obstacle point cloud cluster, its maximum circumscribed circle radius r is calculated, and its surrounding ground point cloud data is obtained with cr as the radius to fit the ground of the current part of the point cloud; where c is a constant coefficient set to be greater than 1.

[0051] Among them, the least squares plane fitting algorithm based on random sampling consistency is used to fit the local plane, and the objective function expression is:

[0052]

[0053] Where: a, b, c, d are plane parameters; n is the number of points; (X i ,Y i ,Z i ) is the three-dimensional coordinate corresponding to point i.

[0054] S5. Use a directed bounding box to measure the optimized stone obstacle point cloud to obtain the position and morphological parameter information of each stone obstacle, specifically:

[0055] Calculate the centroid of the optimized obstacle point cloud, perform de-centroiding, and construct the covariance matrix;

[0056] Perform singular value decomposition on the covariance matrix to obtain the eigenvector of the point cloud;

[0057] According to the eigenvector, the point cloud is projected into the feature space, and the ranges of the optimized obstacle point cloud in the X, Y, and Z directions in the feature space are obtained as the final obstacle measurement results.

[0058] Next, this embodiment takes stone obstacle detection as an example to introduce the obstacle extraction method in detail, including four main processes: generation of 3D point cloud, preliminary extraction of stone obstacles, optimization of stone obstacle extraction, and measurement of stone obstacles.

[0059] 3D point cloud generation: input the acquired depth image and camera intrinsic parameter f respectively x ,f y ,c x ,c y ,For each pixel point, first determine whether it is valid. If it is valid, calculate its corresponding three-dimensional coordinates according to the formula, and finally obtain the three-dimensional point cloud of the target area.

[0060] Initial extraction of rock obstacles: A completed 3D point cloud is input and first subjected to fabric-simulation filtering to separate ground points from non-ground points. Non-ground points are then Euclidean clustered, with a clustering distance of 0.05m for this task. The clustered point cloud is then filtered using the following criteria: First, the cluster midpoint must be above ground level to ensure that the result is a rock, not a subsurface impact crater. Second, the number of cluster midpoints must be greater than a given threshold to eliminate noise. Third, the maximum height of the clustered point cloud above the ground must be greater than a given threshold to exclude smaller rocks. In this example, the point cloud number threshold is set to 50, and the height threshold is set to 0.05m. After this initial extraction, a preliminary rock obstacle point cloud is obtained.

[0061] Optimizing Stone Obstacle Extraction: For each preliminary stone obstacle point cloud, we first obtain a ground point cloud within 1.3 times the radius of its maximum circumscribed circle. We then perform RANSAC least-squares plane fitting. The distance threshold to the plane is set to 0.005m, and the maximum number of iterations is 1000. After obtaining the corresponding plane for the candidate stone obstacle point cloud, we select points above the plane as the new stone obstacle point cloud. We then perform clustering, maintaining the clustering distance threshold at 0.05m. The cluster with the largest number of points after clustering becomes the optimized stone obstacle point cloud.

[0062] Stone obstacle measurement: Calculate the bounding box of the optimized stone obstacle point cloud, and use the length, width, and height of the box as the final morphological parameters of the stone.

[0063] The present invention can autonomously extract and measure obstacles on an extraterrestrial object exploration rover. The rich obstacle information obtained can be used by the rover for path planning and obstacle avoidance, reducing safety risks during the rover's movement. At the same time, it can also efficiently obtain the distribution and size information of stones around the rover for further scientific research.

[0064] The present invention can autonomously extract and measure obstacles on an extraterrestrial object exploration rover. The rich obstacle information obtained can be used by the rover for path planning and obstacle avoidance, reducing safety risks during the rover's movement. At the same time, it can also efficiently obtain the distribution and size information of stones around the rover for further scientific research.

[0065] The electronic device of the present invention includes a central processing unit (CPU), which can perform various appropriate actions and processes according to computer program instructions stored in a read-only memory (ROM) or loaded from a storage unit into a random access memory (RAM). In the RAM, various programs and data required for device operation can also be stored. The CPU, ROM, and RAM are connected to each other via a bus. An input / output (I / O) interface is also connected to the bus.

[0066] Many components in a device are connected to the I / O interface, including: input units, such as a keyboard and mouse; output units, such as various types of displays and speakers; storage units, such as magnetic disks and optical disks; and communication units, such as network cards, modems, and wireless communication transceivers. The communication unit allows the device to exchange information / data with other devices via computer networks such as the Internet and / or various telecommunication networks.

[0067] The processing unit performs the various methods and processes described above. For example, in some embodiments, the method can be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit. In some embodiments, part or all of the computer program can be loaded and / or installed on the device via a ROM and / or a communication unit. When the computer program is loaded into RAM and executed by the CPU, one or more steps of the method described above can be performed. Alternatively, in other embodiments, the CPU can be configured to execute the method in any other appropriate manner (e.g., by means of firmware).

[0068] The functions described above herein may be performed, at least in part, by one or more hardware logic components. For example, and without limitation, exemplary types of hardware logic components that may be used include: field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), and the like.

[0069] The program code for implementing the method of the present invention can be written in any combination of one or more programming languages. Such program code can be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device so that when the program code is executed by the processor or controller, the functions / operations specified in the flow chart and / or block diagram are implemented. The program code can be executed entirely on the machine, partially on the machine, as a stand-alone software package, partially on the machine and partially on a remote machine, or entirely on a remote machine or server.

[0070] In the context of the present invention, machine-readable medium can be a tangible medium that can contain or store a program for use with an instruction execution system, device or equipment or used in combination with an instruction execution system, device or equipment. Machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. Machine-readable medium can include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared or semiconductor systems, devices or equipment, or any suitable combination of the foregoing. More specific examples of machine-readable storage media can include electrical connections based on one or more lines, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0071] The present invention combines a cloth simulation filtering algorithm with a local plane fitting secondary optimization method. It can quickly extract stone obstacles from the three-dimensional point cloud generated by the depth image and measure their position and shape. It can be used by the exploration vehicle to perform operations such as path planning and obstacle avoidance, thereby improving the autonomy and safety of the exploration vehicle on the surface of extraterrestrial bodies.

[0072] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and such modifications or substitutions are intended to be within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. An obstacle extraction method based on local plane fitting, characterized in that: The method comprises the following steps: Obtain the three-dimensional point cloud of the target area; Using a cloth simulation filtering algorithm, classifying the three-dimensional point cloud of the target area into ground points and non-ground points; Performing point cloud clustering on non-ground points to obtain candidate obstacle clusters, and screening the candidate obstacle clusters to obtain a preliminary obstacle point cloud; A least squares plane fitting algorithm based on random sampling consistency is used to perform plane fitting on the local ground near the preliminary obstacle point cloud. The points above the local ground obtained by fitting are regarded as the same obstacle point cloud. After clustering the point cloud again, the cluster with the largest number of clustered points is regarded as the optimized obstacle point cloud. Use a directed bounding box to measure the optimized obstacle point cloud to obtain the position and morphological parameter information of each obstacle; The candidate obstacle clusters are screened to obtain a preliminary obstacle point cloud, and the screening criteria used are: the cluster center point is above the ground; the number of cluster center points is greater than a given threshold; the maximum height of the cluster point cloud above the ground is greater than a given threshold; The method further comprises: repeatedly applying a screening criterion to the optimized obstacle point cloud to obtain a final obstacle point cloud; The optimized obstacle point cloud is measured using a directed bounding box to obtain the position and morphological parameter information of each obstacle, specifically: Calculate the centroid of the optimized obstacle point cloud, perform de-centroiding, and construct the covariance matrix; Perform singular value decomposition on the covariance matrix to obtain the eigenvector of the point cloud; According to the eigenvector, the point cloud is projected into the feature space, and the ranges of the optimized obstacle point cloud in the X, Y, and Z directions in the feature space are obtained as the final obstacle measurement results.

2. The obstacle extraction method based on local plane fitting according to claim 1, characterized in that: The method of obtaining a three-dimensional point cloud of the target area specifically includes: collecting a depth image of the target area through a depth camera, calculating the corresponding three-dimensional coordinates of each valid pixel point in the depth image in combination with the depth camera internal parameters, and generating a three-dimensional point cloud of the target area.

3. The obstacle extraction method based on local plane fitting according to claim 1, characterized in that: Point cloud clustering is performed on non-ground points, specifically: a Euclidean clustering algorithm is used to cluster point clouds on non-ground points.

4. The obstacle extraction method based on local plane fitting according to claim 1, characterized in that: The least squares plane fitting algorithm based on random sampling consistency is used to perform plane fitting on the local ground near the preliminary obstacle point cloud, specifically: For any initial obstacle point cloud cluster, calculate its maximum circumscribed circle radius r, obtain its surrounding ground point cloud data with cr as the radius, and fit the ground of the current part of the point cloud; where c is a constant coefficient set greater than 1.

5. The obstacle extraction method based on local plane fitting according to claim 1, characterized in that: The local plane is fitted using the least squares plane fitting algorithm based on random sampling consistency, and the objective function expression is: Where: a, b, c, d are plane parameters; n is the number of points; (X i ,Y i ,Z i ) is the three-dimensional coordinate corresponding to point i.

6. An electronic device comprising a memory and a processor, wherein a computer program is stored in the memory, wherein: When the processor executes the program, the method according to any one of claims 1 to 5 is implemented.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 5 is implemented.

Images