A high-precision stair obstacle recognition and elimination method

Abstract

The application discloses a high-precision stair obstacle recognition and elimination method, which combines region growing and plane construction of stair obstacle recognition and elimination, belongs to the field of stair perception in environment perception of humanoid robots, and solves the problem that stair obstacles destroy the stair plane features, lead to inaccurate stair parameters obtained by the humanoid robots, and cause the problems of missing steps and falling. Firstly, the point cloud data is pretreated by combining voxel filtering and straight-through filtering; secondly, the point cloud data topology structure is established according to a KD-Tree algorithm, the point cloud normal vector is estimated by using the minimum principle of the sum of adjacent point projections, and the extraction of the stair horizontal plane is accurately completed; thirdly, the stair obstacle clustering condition is determined according to a region growing algorithm, the plane is constructed, and the number of points in the plane is analyzed to complete the fast recognition and elimination of the obstacles; and finally, the three-dimensional parameter estimation of the stair with obstacles and the stair with eliminated obstacles is carried out. The application improves the perception ability of the humanoid robots in a complex stair environment.

Description

Technical Field

[0001] This invention belongs to the field of indoor and outdoor dynamic environment perception for humanoid robots, specifically involving a method for humanoid robots to identify, remove, and estimate staircase parameters in urban buildings, rescue and disaster relief and other operational scenarios. Background Technology

[0002] In recent years, my country's robotics industry has developed rapidly. Among them, humanoid robots, with their excellent autonomous navigation and dynamic environmental perception capabilities, are considered typical representatives of intelligent robots. It is crucial to enhance the environmental perception, human-computer interaction, and behavior control capabilities of humanoid robots and break through their key technologies. Typically, humanoid robots need to move autonomously in complex environments consisting of underground spaces and multi-story buildings. Staircases are a classic example of such complex environments, and their diverse environments and confined spaces make staircase perception a significant challenge for humanoid robots' environmental awareness.

[0003] The urgent need for staircase perception manifests itself in several ways. First, with the arrival of an aging society, some elderly people or paralyzed patients who have lost their ability to walk independently due to muscle atrophy or other diseases are largely unable to manage activities such as climbing stairs and slopes independently. Second, stairs are a common feature in urban buildings and communities. To make full use of storage space and beautify the environment, people often pile up miscellaneous items and place flower pots and other decorations on stairs. In particular, during special rescue and disaster relief missions, obstacles such as bricks, wood blocks, and cardboard boxes may fall into stairwells in disaster areas. Such obstacles can damage the original features of the stairs, preventing humanoid robots from obtaining accurate three-dimensional parameters of the stairs. Therefore, humanoid robots need to analyze and process the staircase environment before they can complete the corresponding rescue tasks.

[0004] While some existing wheeled and tracked robots can climb stairs, their limitation lies in their inability to move horizontally within stairwells. In contrast, humanoid robots, possessing human-like features such as legs, arms, and torso, exhibit greater flexibility in complex environments. Early research focused primarily on stair recognition, with 2D image-based methods offering fast recognition speeds but failing to capture depth information. Therefore, point cloud processing methods are often used for stair parameter estimation, establishing 3D structural models of the stairs based on planar features combined with filtering and region segmentation. However, these methods lack versatility and cannot be applied to complex stair environments with obstacles. In real-world scenarios, dynamic and static obstacles disrupt the planar features of the stairs, such as unclear boundaries and altered interplanar spacing, preventing robots from accurately perceiving the 3D structural information and leading to tripping, missteps, and falls during movement. Summary of the Invention

[0005] The main objective of this invention is to propose a method for identifying and removing stair obstacles. This method can be applied to the environmental perception of complex indoor and outdoor staircases, accurately identify and remove stair obstacles, accurately perceive the three-dimensional information of the staircase based on the removal results, and finally obtain a rough estimate of the passable area of ​​the staircase.

[0006] This invention aims to solve the following problems:

[0007] 1. Traditional methods cannot effectively solve the problem that stair obstacles disrupt the planar features of the staircase, resulting in inaccurate three-dimensional parameters of the staircase obtained by the humanoid robot. This leads to problems such as tripping, misstepping, and falling during the humanoid robot's movement.

[0008] 2. Most existing methods are designed for barrier-free stair perception, which is not well adapted to complex stair scenarios, and rarely considers the impact of stair obstacles on the perception of three-dimensional stair information.

[0009] 3. To address the issue of poor algorithm adaptability, various types of obstacles are designed to simulate stair obstacle scenarios in different environments. Stair obstacles are eliminated and stair parameters are estimated, laying a solid foundation for gait planning of humanoid robots.

[0010] To address the aforementioned problems, this invention proposes a staircase obstacle recognition and removal algorithm based on region growing and planar construction. By artificially creating different types of obstacles, it simulates complex staircase scenarios from real life. The method first preprocesses the data using a filtering algorithm; secondly, it clusters the staircase planes using a region growing algorithm; thirdly, it constructs a plane to identify and remove obstacles; and finally, it obtains accurate three-dimensional staircase parameters, enabling the humanoid robot to effectively avoid obstacles while climbing stairs. Furthermore, it can calculate its foot lift height and step length based on the staircase's three-dimensional parameters, ultimately allowing it to successfully ascend the stairs.

[0011] The specific workflow of this invention is as follows:

[0012] Step 1: Create various obstacles and measure the size and other information of the stairs and obstacles. Use a depth camera to collect point clouds of the stair environment.

[0013] Step 2: Perform point cloud preprocessing by combining voxel filtering and pass-through filtering algorithms to filter out the background and obtain the point cloud of the staircase area.

[0014] Step 3: Based on the stair point cloud obtained in Step 2, use a KD tree to store the instance points in the point cloud space to establish a tree data structure for fast retrieval, and extract the horizontal plane point cloud of the stair according to the normal vector constraint.

[0015] Step 4: Based on the point cloud of the staircase horizontal plane obtained in Step 3, perform planar clustering using a region growing algorithm. The obstacle clustering is divided into two types: obstacles that are clustered individually and obstacles that are not clustered individually.

[0016] Step 5: Traverse each clustering plane. Based on the plane clustering results in Step 4, use the clustering point cloud threshold to remove obstacles in individual clusters, construct a plane, and use the principle of point mutation within the plane to remove obstacles in non-individual clusters.

[0017] Step 6: Based on the obstacle-free staircase planar point cloud obtained in Step 5, the OBB bounding box method is used to perceive the three-dimensional parameters of the staircase.

[0018] In step 3, after preprocessing the staircase point cloud in step 2, it is necessary to extract the horizontal plane point cloud of the staircase. For any point, the plane can be fitted using its surrounding neighboring points, and the corresponding normal vector of the plane can be obtained as the normal vector of that point. Considering that the calculation is very time-consuming when the point cloud data volume is large, in order to improve the efficiency of the neighbor search, a KD tree is used to establish the topology of the original point cloud, and Principal Component Analysis (PCA) is used to calculate the normal vector corresponding to each point cloud. There are two coordinate axes perpendicular to the horizontal plane of the stairs, and the normal vector of each point is denoted as A. i If the normal vector of coordinate axis 1 is B1 and the normal vector of coordinate axis 2 is B2, then A i With B 1 / 2 The cosine value is calculated as shown in formula (1):

[0019]

[0020] The similarity between two vectors is measured by taking the cosine of the angle between them. Considering that the two vectors are not necessarily perfectly perpendicular in reality, the angle is set to be between 60° and 120°, and the cosine value is specified to be between -0.5 and cos(θ). 1 / 2 Within 0.5, when both cos(θ)1 and cos(θ)2 meet the requirements, it is a point cloud of the horizontal plane, which is then extracted and displayed.

[0021] In step 4, after extracting the point cloud of the staircase horizontal plane, a region growing clustering algorithm is used to cluster the plane. The region growing clustering algorithm mainly includes two parts: determining the seed points and the region growing criteria. The seed points are determined using the minimum curvature point method within the surface, which effectively reduces overlapping segmentation. First, principal component analysis is used to solve for the curvature values ​​of the points in the surface, and the point cloud is denoted as R(P). i If there are h point clouds in total, then P i =(X i Y i Zi ), i = 1, 2, ... h, first define a plane equation as shown in formula (2):

[0022] AX + BY + CZ + D = 0 (2)

[0023] Where A, B, and C are three parameters of the plane, and D is the parameter of P. i Distance to the plane.

[0024] Calculate the k-nearest neighbor covariance matrix Q of any point in the point cloud, setting k=30, and calculate Q as shown in formula (3):

[0025]

[0026] in, Using Singular Value Decomposition (SVD) to solve for the eigenvalues ​​(λ1, λ2, λ3) and eigenvectors (α1, α2, α3) of Q, the minimum eigenvalue λ is found. min The corresponding eigenvector is denoted as α. min That is, P i If the normal vector is α, then α min = (A, B, C), then P i The corresponding curvature δ is given in formula (4):

[0027]

[0028] After determining the initial seed point for each segmented region, corresponding normal angle thresholds and curvature thresholds are set. The angle and curvature between the seed point and its neighboring points are compared. Finally, seed points that meet the thresholds are added to the seed point set, and the current seed point is updated. The normal angle threshold is selected by comprehensively considering multiple sets of data to find the optimal threshold. The curvature threshold is closely related to the actual object characteristics; objects with more prominent characteristics have a larger curvature threshold, while objects with gentler characteristics have a smaller curvature threshold. In this invention, the normal angle threshold is set to 30°, and the curvature threshold is set to 0.05°. The extracted staircase horizontal plane point cloud is input into a region growth clustering algorithm for growth. When the seed point sequence is cleared, it means that the growth of that region is complete. The above steps are repeated for the remaining points until all point clouds have been traversed. Different colors are used to represent different clustering plane results. There are two types of clustering results: the obstacle plane and the staircase plane each form a separate cluster, or the obstacle plane and the staircase plane are grouped into the same cluster.

[0029] In step 5, based on the planar point cloud obtained after region growing clustering, obstacle removal is first performed on individually clustered points, and then obstacle removal is performed on points not in individual clusters. Specifically, all planar point clouds are first input into the obstacle removal algorithm, each plane is traversed sequentially, and the number of point clouds in each cluster is counted using a histogram.

[0030] (1) Individual clustering obstacle removal

[0031] When there are no obstacles, the number of point clouds in the stepped horizontal plane clusters is almost uniform and all exceed a threshold. When an obstacle exists that forms its own cluster, the number of corresponding point clouds is checked against a given threshold. Point clouds exceeding the threshold are retained to remove the obstacle. Considering that the obstacle's volume is not large, half of the total number of points is taken, then the average is calculated, and a predetermined redundancy is subtracted from this average. The point cloud threshold is given by formula (5):

[0032]

[0033] Where N is the total number of planar clusters, X j Let be the number of point clouds in the j-th planar cluster.

[0034] (2) Removal of obstacles that are not clustered individually

[0035] After removing obstacles from individual clusters, obstacles from non-individual clusters are removed. Each plane is traversed sequentially. For a cluster plane, the plane is first constructed by defining a coordinate axis direction as the reference direction and finding the initial point corresponding to that coordinate axis direction. Then, the initial plane is constructed using this point and the normal vector of the reference direction, and its plane equation is shown in formula (6):

[0036] a(x-x0)+b(y-y0)+c(z-z0)=0 (6)

[0037] Where a, b, and c are the parameters of the normal vector defined for the constructed plane, and (x0, y0, z0) are the initial coordinates of the constructed plane.

[0038] The constructed plane is moved along the reference direction of the coordinate axis, rising 0.03m each time, and the number of interior points corresponding to the constructed plane at different height positions is calculated. Finally, a histogram is used to statistically analyze the number of interior points of the constructed plane. Based on the obstacle volume, σ is defined as the elimination factor, σ∈(1,1.5), then the threshold ε for the abrupt change in the point cloud before and after the constructed plane is determined. j As shown in formula (7):

[0039]

[0040] Among them, X j Let m be the number of point clouds in the j-th planar cluster. j It is the total number of constructing planes corresponding to the j-th planar cluster.

[0041] Ideally, the height of the constructed plane corresponding to the first jump is the horizontal plane boundary of the staircase within the cluster, and the point cloud above this position is considered to be an obstacle. The point cloud before the jump is saved to remove the obstacle point cloud.

[0042] In step 6, based on the point cloud of the stairs after obstacle removal in step 5, in order for the humanoid robot to calculate its foot lift height and step length in the passable area, it is necessary to obtain the three-dimensional parameter information of the stairs after removal. This invention considers a relatively moderate obstacle shape, which retains most of the step point cloud information after obstacle removal. Therefore, it considers using a rectangular bounding box to enclose the stairs and calculating the side length of the bounding box to obtain the stair parameter information. Specifically, the height information of the stairs is determined using the Euclidean distance from a point to a plane, and the width and depth information of the stairs are determined using the side length of an OBB bounding box. First, an OBB bounding box is used for each plane after obstacle removal, and the vertex coordinates of the bounding box are obtained. The width and depth of the stairs are calculated based on the distance between the vertex coordinates. The size and orientation of the OBB bounding box are determined based on the geometry of the object itself, and the box does not need to be perpendicular to the coordinate axes. This method is more accurate and compact than other methods.

[0043] The present invention has the following advantages:

[0044] (1) The present invention can solve the problem that when humanoid robots face stairs with obstacles, they may trip and fall due to inaccurate three-dimensional structural information of the stairs.

[0045] (2) The presence of stair obstacles will disrupt the planar features of the staircase. To address this problem, this invention proposes a new stair obstacle recognition and removal algorithm based on the abrupt change in the number of points in the horizontal plane of the staircase. This method can effectively remove different types of stair obstacles, providing a good prerequisite for humanoid robots to climb stairs and perceive stair parameters.

[0046] (3) The present invention can obtain relatively accurate three-dimensional structural information of the staircase after removing obstacles, which can provide better preconditions for the gait planning of humanoid robots. Attached Figure Description

[0047] Figure 1 Flowchart of the method for identifying and removing obstacles on stairs;

[0048] Figure 2 Point cloud processing images of stairs: (a) is the actual image of Example 1, (b) is the actual image of Example 2, (c) is the preprocessed image of Example 1, and (d) is the preprocessed image of Example 2.

[0049] Figure 3 The staircase horizontal plane processing diagram, (a) is a schematic diagram of the normal vector, and (b) is a diagram of the horizontal plane extraction effect;

[0050] Figure 4 A physical image of the staircase and a clustering diagram of the region growth: (a) is a physical image of Example 1, (b) is a physical image of Example 2, (c) is a clustering effect diagram of Example 1, and (d) is a clustering effect diagram of Example 2.

[0051] Figure 5 The diagram shows the obstacle removal process for the individual clustering of staircase data 1. (a) is a picture of the actual staircase, (b) is the overall clustering histogram, (c) is the clustering result diagram, and (d) is the obstacle removal result diagram.

[0052] Figure 6 The diagram shows the obstacle removal process for the individual clustering of staircase data 2. (a) is a picture of the actual staircase, (b) is the overall clustering histogram, (c) is the clustering result diagram, and (d) is the obstacle removal result diagram.

[0053] Figure 7 The non-individual clustering obstacle removal process diagram of staircase data 3 is shown in (a) as a picture of the actual staircase, (b) as a clustering histogram of the construction plane, (c) as a clustering result diagram, and (d) as an obstacle removal result diagram.

[0054] Figure 8 The non-individual clustering obstacle removal process diagram of staircase data 4 is shown in (a) as a picture of the actual staircase, (b) as a clustering histogram of the construction plane, (c) as a clustering result diagram, and (d) as an obstacle removal result diagram. Detailed Implementation

[0055] The following section provides a detailed explanation of this method, with reference to the accompanying drawings and examples.

[0056] Figure 1 The flowchart illustrates the method implementation. First, a depth camera is used to acquire point cloud data of the staircase environment. Downsampling the original point cloud not only filters out noise points but also reduces data volume and improves algorithm processing speed. Second, a normal vector estimation method is used to extract the horizontal plane of the staircase, and a region growing algorithm is used to cluster each region. Third, based on the clustering results, obstacles in individual clusters can be directly removed. By constructing a plane and analyzing the changes in the number of points within the plane, obstacles not in individual clusters are removed, resulting in an unobstructed passage area for the humanoid robot on the staircase. Finally, based on the obstacle removal results, the three-dimensional structural information of the staircase, including the height, width, and depth of the steps, is perceived. The specific steps are as follows:

[0057] 1. Point cloud preprocessing

[0058] (1) Voxel filtering

[0059] A voxel mesh is used to establish the topology of the original point cloud; the staircase point cloud is denoted as r(p i ), p iLet p be any point in space. i =(x i y i , z i ), where n is the number of point clouds in each voxel, l j The side length of each small voxel, v j Let V be the volume of each voxel.

[0060] v j =l jX l jY l jZ (8)

[0061] Among them, l jX l jY l jZ These represent the projections of small voxels onto the X, Y, and Z axes, respectively.

[0062] In each voxel, the centroid p of all points in that voxel is used. c To approximate other points in the voxel.

[0063] p c =(x c y c , z c (9)

[0064]

[0065] In this invention, voxels with a side length of 0.03 are used to downsample the point cloud, i.e., l j =0.03, which can not only filter out noise points in the point cloud, but also reduce the amount of data and improve the processing speed of the algorithm;

[0066] (2) Direct-through filtering

[0067] Even after voxel filtering, a complex background remains, so a pass-through filter is considered for further processing. Specifically, the points in the point cloud are traversed, and it is determined whether each point is within a specified value range. Points within the range are retained, and those outside the range are deleted, thus extracting the staircase point cloud. Threshold ranges in three axes are determined by considering the actual size of the staircase, the field of view of the depth camera, and the height of the depth camera above the ground, and point clouds within these ranges are extracted. In this invention, point clouds within the ranges of (-0.85m, 0.85m), (0m, 0.8m), and (0m, 1.3m) are selected along the X, Y, and Z axes, respectively. A schematic diagram of the staircase point cloud preprocessing is shown below. Figure 2 As shown in (c) and 2(d).

[0068] 2. Extraction of the horizontal plane of the staircase

[0069] Obstacle removal on stairs requires extracting the horizontal plane point cloud of the stairs. For any point, the plane can be fitted using its surrounding neighboring points, and the corresponding normal vector of the plane can be used as the normal vector for that point. Considering that the computation is very time-consuming when the point cloud data volume is large, a KD-tree is used to store the instance points in space to establish a tree-like data structure for fast retrieval in order to improve the efficiency of the neighbor search. The stair point cloud is denoted as R(P i )′,P i For each point in the space, use a KD-Tree to query point P. i The K nearest points are denoted as N(P) j ). Calculate P i normal vector K i , N(P j The sum of the distances from all points in the plane to the plane satisfies formula (11):

[0070]

[0071] In the formula, j = 1, 2, ..., K, P j For P i The j-th neighboring point, P o Let be the center point of the plane. The calculation method is shown in formula (12):

[0072]

[0073] According to Principal Component Analysis (PCA), finding a direction that minimizes the sum of the projections of all adjacent points along that direction means that the projection points along that direction are most concentrated and have the smallest variance. This direction can be considered the normal vector. is the eigenvector corresponding to the smallest eigenvalue found by PCA. This can be transformed into the eigenvalue decomposition of the covariance matrix, as shown in formula (13):

[0074]

[0075] In the formula, S is N(P) j The covariance matrix of ). A schematic diagram of its extracted normal vectors is shown below. Figure 3 As shown in (a).

[0076] By traversing the point cloud of the staircase, and based on the principle that the normal vector of the horizontal plane of the staircase is perpendicular to the normal vectors of the two coordinate axes, as shown in formula (1), the point cloud of the horizontal plane of the staircase is finally obtained. Figure 3 As shown in (b).

[0077] 3. Growth clustering in stairwell areas

[0078] The region containing the minimum curvature point is the smoothest region. Growing from this region effectively avoids over-segmentation and improves algorithm efficiency. By calculating the curvature value of each point, the point with the minimum curvature value is selected as the seed point for region growth.

[0079] Principal component analysis is used to determine the curvature of a point on a surface. The point cloud is denoted as R(P). i Define a plane equation as shown in formula (2), calculate the k-nearest neighbor covariance matrix of any point in its point cloud using formula (3), then use singular value decomposition (SVD) to solve for the eigenvalues ​​and eigenvectors of the covariance matrix, and find the minimum eigenvalue, then P i The corresponding curvature δ is given in formula (14):

[0080]

[0081] Where λ1, λ2, and λ3 are the eigenvalues ​​obtained from solving the covariance matrix, and λ min It is the smallest eigenvalue of the covariance matrix.

[0082] After determining the initial seed point, a multi-set threshold test method is used to determine the normal angle and curvature thresholds. The normal angle threshold is set to 30°, and the curvature threshold to 0.05. The angle and curvature of the seed point are compared with those of its neighboring points. Seed points that meet the thresholds are added to the seed point set, and the current seed point is updated. This process is repeated. The final clustering results are of two types: either the obstacle plane and the step plane each form a separate cluster, or the obstacle plane and the step plane are grouped into the same cluster. Figure 4 (c) and 4(d) are schematic diagrams of two results for clustering stair obstacles.

[0083] 4. Removal of obstacles on stairs

[0084] Based on the planar point cloud obtained after region growing clustering, obstacles in individual clusters are removed first, followed by obstacles in non-individual clusters. Specifically, all planar point clouds are first input into the obstacle removal algorithm, and each plane is traversed sequentially. The number of point clouds in each cluster plane is counted first. For obstacles in individual clusters, they can be directly removed using the planar cluster point cloud threshold, which is shown in formula (5). The removal effect is as follows: Figure 5 (d) and Figure 6 As shown in (d).

[0085] After identifying obstacles that belong to isolated clusters, obstacles that do not belong to isolated clusters are then removed. For obstacles that do not belong to isolated clusters, the planar clusters are traversed. First, coordinate axes are defined, with the positive direction of the coordinate axes defined as the normal vector F = (a, b, c) of the construction plane. Next, the initial point corresponding to the initial plane is found, defined as M0 = (x0, y0, z0). A construction plane is determined by a point plus a normal vector. Let M = (x, y, z) be any point on the construction plane. Then vector It is perpendicular to the plane normal vector F, and its dot product is 0, as shown in formula (15):

[0086]

[0087] The equation of the plane is given in formula (16):

[0088] a(x-x0)+b(y-y0)+c(z-z0)=0 (16)

[0089] The constructed plane is moved along the negative direction of the coordinate axis. Considering the large gap distance between horizontal planes, the constructed plane is raised by 0.03m each time. A threshold distance of 0.03m is set for the distance from a point to the plane; points less than this threshold are classified as interior points of the constructed plane. The number of interior points for each constructed plane is recorded sequentially. When the constructed plane and the staircase are on the same plane, the corresponding point cloud count is the number of interior points of that staircase. However, due to obstacle interference, there will be excess point cloud above the horizontal plane of the staircase. Histograms are used to statistically analyze the number of interior points of the constructed plane. Based on the obstacle volume, σ is defined as the elimination factor, σ∈(1,1.5). The threshold ε for the abrupt change in point cloud before and after the constructed plane is then determined. j As shown in formula (17):

[0090]

[0091] Among them, X j Let m be the number of point clouds in the j-th planar cluster. j It is the total number of constructing planes corresponding to the j-th planar cluster.

[0092] Ideally, the change in the point cloud before and after the construction plane, Δn, is greater than or equal to ε. j At this point, it is considered that the boundary between the staircase and the obstacle has been found. The non-obstacle point cloud is preserved to remove the obstacle. Figure 7 and Figure 8 In the examples shown, σ is always 1.2. The final removal result is as follows: Figure 7 (d) and Figure 8 As shown in (d).

Claims

1. A high-precision method for identifying and removing stair obstacles, characterized in that, It includes the following steps: Step 1: Create various obstacles and measure the dimensions of the stairs and obstacles; use a depth camera to collect point clouds of the stair environment. Step 2: Perform point cloud preprocessing by combining voxel filtering and pass-through filtering algorithms to filter out the background and obtain the point cloud of the staircase area; Step 3: Based on the stair point cloud obtained in Step 2, use a KD tree to store the instance points in the point cloud space to establish a tree data structure for fast retrieval, and extract the horizontal plane point cloud of the stair according to the normal vector constraint. Step 4: Based on the point cloud of the staircase horizontal plane obtained in Step 3, perform planar clustering using a region growing algorithm. The obstacle clustering is divided into two types: obstacles that are clustered individually and obstacles that are not clustered individually. Step 5: Traverse each clustering plane. Based on the plane clustering results in Step 4, use the clustering point cloud threshold to remove obstacles in individual clusters, construct a plane, and use the principle of point mutation within the plane to remove obstacles in non-individual clusters. Step 6: Based on the obstacle-free staircase planar point cloud obtained in Step 5, the OBB bounding box method is used to perceive the three-dimensional parameters of the staircase. Step 5 is as follows: Based on the planar point cloud obtained after region growing clustering, obstacle removal is first performed on the individually clustered obstacles, and then obstacle removal is performed on the non-individually clustered obstacles. Specifically, all planar point clouds are first input into the obstacle removal algorithm, each plane is traversed in turn, and the number of point clouds in each cluster is counted using histograms. (1) Individual clustering obstacle removal When there are no obstacles, the number of point clouds in the stepped horizontal plane clusters is almost uniform and all exceed the threshold. When there are obstacles in separate clusters, it is determined whether the number of corresponding point clouds is lower than the given threshold. Point clouds with a number higher than the threshold are retained to remove the obstacle. Considering that the obstacle volume is not large, half of the total number of points is taken and then averaged. A set redundancy is then subtracted from this average. , The point cloud threshold is given by formula (5): （5）； Where n is the total number of planar clusters, Let be the number of point clouds in the j-th planar cluster. ; (2) Removal of barriers that are not clustered individually After removing obstacles from individual clusters, obstacles from non-individual clusters are removed; each plane is traversed in turn. For a cluster plane, the plane needs to be constructed first. A coordinate axis direction is defined as the reference direction, and the initial point corresponding to the coordinate axis direction is found. Then, the initial plane is constructed using this point and the normal vector of the reference direction. Its plane equation is shown in formula (6): （6）； Where a, b, and c are the parameters of the normal vector defined for the constructed plane. These are the initial coordinates of the points used to construct the plane; The constructed plane is moved along the reference direction of the coordinate axis, rising 0.03m each time. A threshold distance of 0.03m is set between points and the plane; points less than this threshold are classified as interior points of the constructed plane. The number of interior points corresponding to different height positions is calculated. Finally, a histogram is used to statistically analyze the number of interior points of the constructed plane. This is based on the definition of obstacle volume. To eliminate factors, Then the threshold for the abrupt change in point cloud before and after constructing the plane. As shown in formula (7): （7）； in, Let be the number of point clouds in the j-th planar cluster. It is the total number of constructed planes corresponding to the j-th planar cluster; Ideally, the change in point cloud before and after constructing the plane... Greater than or equal to When the first jump occurs, the height position of the construction plane is the boundary of the horizontal plane of the stairs in the cluster. Points above this position are determined to be obstacles. The point cloud before the jump is saved to remove the obstacle point cloud.

2. The high-precision stair obstacle identification and removal method as described in claim 1, characterized in that, Step 3 is as follows: After preprocessing the staircase point cloud in step 2, it is necessary to extract the horizontal plane point cloud of the staircase. For any point, the plane is fitted using the surrounding neighboring points, and the corresponding normal vector of the plane is obtained as the normal vector of that point. Considering that the calculation is very time-consuming when the point cloud data volume is large, in order to improve the efficiency of the neighbor search, a KD tree is used to establish the topological structure of the original point cloud, and the normal vector corresponding to each point cloud is calculated using principal component analysis (PCA). There are two coordinate axes perpendicular to the horizontal plane of the stairs, and the normal vector of each point is denoted as... The normal vector of coordinate axis 1 is The normal vector of coordinate axis 2 is ,but and The cosine value is calculated as shown in formula (1): （1）； The similarity between two vectors is measured by the cosine of the angle between them. Considering that two vectors are not necessarily perfectly perpendicular in reality, the angle is set to a range of 60° to 120°, and the range of the cosine value is defined as follows: inside, when and When all requirements are met, the result is a point cloud on a horizontal plane, which is then extracted and displayed.

3. The high-precision stair obstacle identification and removal method as described in claim 1, characterized in that, Step 4 is as follows: After extracting the point cloud of the horizontal plane of the staircase, the region growing clustering algorithm is used to cluster the plane. The region growing clustering algorithm mainly includes two parts: the determination of seed points and the determination of region growing criteria. Seed points are determined by the minimum curvature point method in the surface, which can effectively reduce the phenomenon of overlapping segmentation. First, the curvature value of the midpoint of the surface is solved by the principal component analysis method. Let the point cloud space be denoted as If there are h point clouds in total, then , First, define a plane equation as shown in formula (2): （2）； Where A, B, and C are three parameters of the plane, and D is... Distance to the plane; Calculate the k-nearest neighbor covariance matrix Q of any point in the point cloud, setting k=30. The calculation of Q is as shown in formula (3): （3）； in, Singular value decomposition (SVD) is used to solve for the eigenvalues ​​of Q. and eigenvectors Then the smallest eigenvalue The corresponding eigenvector is denoted as That is The normal vector, then ,but Corresponding curvature See formula (4): （4）； After determining the initial seed point for each segmented region, corresponding normal angle thresholds and curvature thresholds are set, and the angle and curvature between the seed point and its neighboring points are compared. Finally, seed points that meet the thresholds are added to the seed point set, and the current seed point is updated. The normal angle threshold is selected by comprehensively selecting the optimal threshold after testing multiple sets of data. The curvature threshold is closely related to the actual object characteristics; objects with more obvious characteristics have a larger curvature threshold, while objects with gentler characteristics have a smaller curvature threshold. In this invention, the normal angle threshold is set to 30°, and the curvature threshold is set to 0.05°. The extracted staircase horizontal plane point cloud is input into the region growth clustering algorithm for growth operation. When the seed point sequence is cleared, it means that the growth of the region is complete. The above steps are repeated for the remaining points until all point clouds are traversed. Different colors are used to represent different clustering plane results. There are two types of clustering results: the obstacle plane and the staircase plane each form a cluster, and the obstacle plane and the staircase plane are grouped into the same cluster.

4. The high-precision stair obstacle identification and removal method as described in claim 1, characterized in that, Step 6 is as follows: The stairs are enclosed by a rectangular bounding box, and the side length of the bounding box is calculated to obtain the parameter information of the stairs. Specifically, the height of the stairs is determined by the Euclidean distance from the point to the plane, and the width and depth of the stairs are determined by the side length of the OBB bounding box. First, an OBB bounding box is used for each plane after removing obstacles and the vertex coordinates of the bounding box are obtained. The width and depth of the stairs are calculated based on the distance between the vertex coordinates.

Images