A stair detection method based on multi-frame point cloud sequence splicing

Abstract

The application discloses a stair detection method based on multi-frame point cloud sequence splicing, which comprises the following steps: firstly, a plane in a scene is segmented by a region growing method, then a plane meeting the stair standard is screened out according to the geometric features of the plane, and the stair structure is detected in the plane through graphical feature detection. After the stair is detected, the detection result is evaluated, the result of the stair detection is optimized according to the evaluation value, and the stair point cloud is spliced into the next frame of original point cloud for the detection of the next frame. The technical scheme of the application can improve the accuracy and completeness of the stair detection by using the multi-frame point cloud sequence splicing mode, can maintain the ability of detecting the stair in the case that the stair structure is partially blocked, and can improve the accuracy and stability of the stair detection by using the laser radar.

Description

Technical Field

[0001] This invention belongs to the field of environmental perception, and in particular relates to a staircase detection method based on multi-frame point cloud sequence stitching. Background Technology

[0002] Accessible area detection is one of the essential functions for autonomous vehicles to explore unknown environments. Accessible area information is a crucial prerequisite for global exploration planning and local path planning for obstacle avoidance and tracking the global path. Indoor spaces are mostly structured spaces with relatively flat floors, but they are also narrow and complex, requiring high accuracy in accessible area detection. For autonomous vehicles that need to cross floors in indoor three-dimensional spaces, stairwells must also be included in the scope of accessible area detection. Stairs have obvious geometric structural features, and stair detection algorithms need to extract point cloud features from LiDAR scans to accurately detect the location, orientation, and geometric information such as the height, width, and depth of the steps. Therefore, a method for detecting stairs based on multi-frame point cloud sequence stitching is designed, which can use stitched continuous multi-frame point clouds for stair detection and construct a highly complete stair model. Summary of the Invention

[0003] The purpose of this invention is to provide a staircase detection method based on multi-frame point cloud sequence stitching, so as to solve the problems existing in the prior art.

[0004] To achieve the above objectives, this invention provides a staircase detection method based on multi-frame point cloud sequence stitching, comprising:

[0005] The original point cloud is obtained, and the point cloud is segmented into planes using the region growing method. The segmented point cloud is then filtered to extract the point cloud of the staircase plane.

[0006] The point cloud of the staircase plane is filtered according to the geometric features of the staircase plane to obtain a plane that meets the staircase standard and then spliced ​​together to obtain the staircase graphic.

[0007] The staircase graphic of any two consecutive adjacent steps is initialized. The staircase graphic of the successfully initialized step is expanded. Each expansion searches for a plane point cloud that meets the staircase standard with a certain radius. If no plane that meets the staircase standard is found after expanding two consecutive steps, the expansion stops and the detection of the original point cloud of a single frame is completed. The detected plane point cloud is then stitched to the original point cloud of the next frame for detection of the next frame.

[0008] An evaluation function is constructed, and the evaluation value of each frame is calculated based on the evaluation function. If the evaluation value of the stair model up to the current frame is greater than that of the stair model in the previous frame, it is replaced, and the planar point cloud that meets the stair standard is stitched to the original point cloud until the motion stops.

[0009] Optionally, the process of obtaining the plane includes: downsampling the original point cloud using an octree, selecting a point in the downsampled point cloud as a seed point for growth, using the seed point as the center of the plane, using all points in the neighborhood of the seed point to locally fit the tangent plane, calculating the normal vector of the point cloud based on the fitting result, and obtaining the fitted plane based on the normal vector and the set of points in the neighborhood of the seed point; in the fitted plane, searching for the KNN point of the seed point, if the KNN point does not belong to any region, then creating a list for storage, and calculating the plane distance between the seed point and each KNN point in the list, if the plane distance does not exceed a preset value, then this KNN point is assigned to the current fitted plane and used as a new seed point to continue expanding the fitted plane, if it exceeds the preset value, then it is used as the seed point of a new plane for growth, until all planes are obtained through segmentation.

[0010] Optionally, the screening process includes setting stair standards, which include stair angles and stair geometry.

[0011] The PCA method is used to calculate the eigenvector of each plane. The angle between each plane and the Z-axis of the world coordinate system is calculated by comparing the eigenvector with the unit vector of the Z-axis of the world coordinate system. The angle is then compared with the preset value of the staircase angle to filter all planes by geometric angle.

[0012] Calculate the angle between the feature vector of each selected plane and the X-axis of the world coordinate system, construct a rotation matrix based on the angle, align the point cloud in the positive direction of the X-axis, and select planes that conform to the structure of the pedal and the upright and that conform to the preset geometric dimensions.

[0013] Optionally, planar point clouds that meet the preset geometric angle values ​​but have smaller geometric dimensions than the preset geometric dimensions can be stitched into the original point cloud of the next frame, and the stitched point cloud can be downsampled.

[0014] Optionally, initialization includes initialization of stair risers and stair treads; when expanding, the stair graphic is initialized multiple times.

[0015] Optionally, the expansion process includes: constructing an empty staircase graphic based on each element in the initialization result; expanding the successfully initialized staircase graphic along the staircase extension direction in units of one step; each expansion searches for the vertical plate plane point cloud and the tread plane point cloud that conform to the staircase standard with a certain radius; if found, adding the corresponding vertical plate or tread plane point cloud to the staircase graphic; wherein the staircase extension direction includes upward and downward.

[0016] Optionally, the evaluation function is constructed based on the upright evaluation function, the pedal evaluation function, the number of upright planes, and the number of pedals. The expression of the evaluation function is: S stair =S r ·nr +S t ·n t

[0017] In the formula, n r and n t S represents the number of detected stair model risers and treads. r S is the evaluation function for the vertical plate. t This is the pedal evaluation function.

[0018] Optionally, the evaluation function for the upright slab is the sum of three parts: the evaluation value of the upright slab step depth, the evaluation value of the upright slab step height, and the evaluation value of the upright slab staircase orientation.

[0019] The pedal evaluation function is the sum of the pedal step depth evaluation value and the pedal step height evaluation value.

[0020] The technical effects of this invention are as follows:

[0021] The technical solution of this invention uses a multi-frame point cloud sequence stitching method, which can improve the accuracy and completeness of stair detection, and can maintain the ability to detect stairs even when the stair structure is partially obscured, thereby improving the accuracy and stability of stair detection using lidar. Attached Figure Description

[0022] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings:

[0023] Figure 1 This is a schematic diagram of the process in an embodiment of the present invention. Detailed Implementation

[0024] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0025] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0026] Example 1

[0027] Staircases are passageways connecting different floors within a building. For autonomous vehicles to move across floors, they first need to detect the staircases, obtaining information such as their location and geometric dimensions. However, when LiDAR scans staircases, the staircase structure often obstructs each other, making it difficult for the LiDAR to scan the entire staircase.

[0028] like Figure 1 As shown, this embodiment provides a staircase detection method based on multi-frame point cloud sequence stitching, including:

[0029] First, the planes in the scene are segmented using region growing. Then, planes that meet the staircase criteria are selected based on their geometric features, and the staircase structure within them is detected using graphical feature analysis. After the staircase is detected, the detection results are evaluated, and the staircase detection results are optimized based on the evaluation values. Finally, the staircase point cloud is stitched into the original point cloud of the next frame for detection in the next frame.

[0030] Point cloud segmentation method based on region growing

[0031] Staircases have distinct structural features, and the point cloud can be segmented using the region growing method. Region growing refers to the process of expanding point cloud regions with similar characteristics into larger regions. Starting from a seed point, it grows like a plant, searching for points with similar attributes and incorporating them into the same region. As more point clouds are merged into the same region, the size of that region continuously increases. For staircase detection, the region growing method can be used for planar segmentation of the point cloud, grouping point clouds on the same plane into the same region.

[0032] First, an octree is used to process the original point cloud P. ori Downsampling is performed, and then a point in the downsampling point cloud is selected as the seed point for growth. Using this point as the center of the plane, the normal vector is calculated. The normal vector is an important geometric feature of the point cloud. This paper uses Principal Component Analysis (PCA) to solve for the point cloud's normal vector. This method uses all points in the selected point's neighborhood to locally fit the tangent plane, and calculates the point cloud's normal vector based on the fitting results. Let P be the currently calculated sample point. c P c ∈R 3 R 3 This represents the set of coordinate points in three-dimensional space. Its neighborhood set can be represented as N. b (p c The plane (p1, p2, p3, ..., p3) = {p1, p2, p3, ..., p3} can be quickly solved using an octree. The fitted plane can be represented as:

[0033]

[0034] Where H is the fitted plane, n is the unit normal vector of the fitted plane, and d c For sample point p c The Euclidean distance to the fitted plane H.

[0035] Through A covariance matrix can be constructed, and its minimum eigenvalue can be obtained by eigenvalue decomposition. The corresponding eigenvector is the normal vector n of the fitting plane H. Then, the K-Nearest Neighbor (KNN) of the seed point is searched. KNN is a simple classification method. For LiDAR point clouds, the algorithm often selects a point and then selects the k nearest points from the point cloud. The distance used in the calculation can be Euclidean distance, Manhattan distance, or any other distance metric that conforms to the triangle inequality. This paper uses Euclidean distance in 3D space as the distance metric for KNN. Since the point cloud has been preprocessed in the previous section, the KNN point of any point can be quickly found by traversing the index. If the KNN point of the seed point does not belong to any region, the KNN point is stored in a list, and the planar distance d is calculated for each point in the list. p Let p be the point currently being calculated. c The distance calculation method is as follows:

[0036] d p =(p n -p c )·n n

[0037] Where p n It is the center point of the plane, n n ∈R 3 Let be the normal vector of the currently growing plane. If the calculated planar distance d... p Within a certain threshold range, then p c Incorporate the current region and use it as the new seed point and plane center, find its K nearest neighbor, and begin a new round of growth. If p is calculated... c If the planar distance is outside the threshold range, then wait until the current region has finished growing, and use it as a seed point for growing a new region.

[0038] Staircase Plan Filtering Method

[0039] The staircase has two distinct planar structures: treads and risers. The algorithm extracts the point cloud of the staircase plane by filtering the point cloud after plane segmentation. The plane filtering only considers the attribute features of individual planes, ignoring the relationships between planes, because the computational complexity of filtering a single plane is O(n), while incorporating relationships between planes changes the computational complexity to O(n). 2 The point cloud used for stair detection comes from a global point cloud aligned with the world coordinate system. It can be preset in the global coordinate system. The stair treads are horizontal planes, and the risers are perpendicular to the treads. Therefore, the normal vectors of their planes are also perpendicular to each other.

[0040] According to my country's national standards for stair design, namely the "Code for Fire Protection Design of Buildings" (GB50016) and the "Code for Fire Protection Design of High-Rise Residential Buildings" (GB50045), the tread height of residential staircases shall not exceed 0.175m, the tread width shall not be less than 0.26m, and the clear width of the staircase shall not be less than 1.10m. The clear width of the staircase refers to the horizontal distance from the center of the wall to the center of the handrail. These three parameters correspond to the tread height h. s Step depth d s Step width W s .

[0041] During the screening process, the standards can be appropriately relaxed, limiting the maximum height of stair treads (i.e., the maximum width of the riser is 0.3m), the maximum depth of stair treads (i.e., the maximum width of the treads is 0.4m), and the minimum clear width of the stairs (i.e., the minimum length of the riser and tread) is 1m. Simultaneously, a 15° deviation is allowed in the angles of the riser and tread; that is, the angle between the normal vector of the riser plane and the ground is between -15° and 15°, and the angle between the normal vector of the tread plane and the ground is between 75° and 105°.

[0042] The PCA method is used to calculate the eigenvectors of each plane. First, the eigenvectors are compared with the world coordinate system z-axis unit vector v. z = (0, 0, 1) Calculate the angle between each plane and the z-axis of the world coordinate system.

[0043]

[0044] Filter out all planes that do not meet the geometric constraints of the staircase. Then calculate the eigenvectors of the filtered planes and the unit vector v along the x-axis in the world coordinate system. x Angle = (1, 0, 0)

[0045]

[0046] According to the included angle ψ p Construct rotation matrix

[0047]

[0048] Aligning the point cloud along the positive x-axis of the world coordinate system, the algorithm filters out the maximum and minimum values ​​of each axis of the planar point cloud to obtain the geometric parameters of the plane. Further filtering selects planes that conform to the geometric parameters, and adds the planar point clouds that conform to the treadle and upright structure to the point cloud set P respectively. t ={P t，1 P t，2 P t，3 ...} and P r ={P r，l P r，2 Pr，3 ...}, where each element is an independent planar point cloud, which is further examined as a possible plane of stairs.

[0049] Due to the location limitations of the autonomous vehicle or the mutual occlusion of the stair structure, the completeness of the stair point cloud obtained by LiDAR scanning is low. No planes meeting both the stair angle and geometric size standards are found in the current frame point cloud. To improve the success rate of stair detection when the stair structure point cloud is incomplete, the algorithm selects plane point clouds P whose angles meet the stair standard but whose geometric dimensions are smaller than the stair standard. out Stitched into the original point cloud of the next frame.

[0050] P ori ′=P ori ∪P out

[0051] P ori The image shows the original point cloud after stitching. By stitching together multiple frames of planar point clouds, the details of the staircase's planar structure are increased, improving the success rate of staircase detection. However, to prevent excessive overlap of point clouds and the addition of invalid details, the stitched point cloud needs to be downsampled again.

[0052] Graphical staircase detection method based on multi-frame point cloud sequence stitching

[0053] All staircases consist of a number of steps S i Stairs are composed of treads and uprights, a fixed structure that facilitates detection and expansion through graphical methods. However, stairs themselves have a three-dimensional spatial structure, and the structures can obstruct each other. Furthermore, LiDAR has scanning dead zones, meaning the stair structure in a single-frame LiDAR scan point cloud is often incomplete. This negatively impacts the accuracy and completeness of stair detection. Therefore, this paper proposes a stair detection method based on multi-frame point cloud sequence stitching. This method uses multi-frame LiDAR scan results to stitch together the stair point cloud, then detects the stairs and calculates the evaluation value of the detection results. Based on the evaluation value, the detection results are optimized to construct a complete and more accurate stair model.

[0054] In this paper, the direction of descent of a staircase is defined as the orientation 's' of the staircase. This is typically indicated by the height h of the stair steps. s Step depth d s Step width w sThe algorithm describes a staircase model using the staircase's orientation (s). First, the staircase shape needs to be initialized using the edges of two consecutive adjacent steps or treads, primarily by using the edges at the same position on these two adjacent steps or treads, such as the front and back edges, or the top and bottom edges. Given the three-dimensional structure of the staircase, edges may be obscured by the steps themselves; therefore, the algorithm needs to select different edges to initialize the staircase shape multiple times. For consecutive staircase steps, the algorithm uses the height difference (h) between the top edges of two consecutive steps. tt The height difference h between the bottom edge and the bottom edge bb For step height h s Perform two initializations.

[0055] The initialization results of the stair risers are as follows:

[0056] h s =h tt ∨h s =h bb

[0057] d s =|(c1-c2)·s|

[0058]

[0059] Where, n r Let c be the normal vector of the vertical plate, and c be the centroid of the planar point cloud, where c ∈ R. 3 If plane P = {p1, p2, p3, ..., p...} n},

[0060]

[0061] The graphs obtained from the two initializations are {h tt d s ,s} and {h bb， d s ,s}.

[0062] The same initialization strategy is used to initialize the treads of two consecutive adjacent steps. The algorithm uses the distance d between the front edges of two consecutive steps. ff The distance d between the back edge and the front edge bb For step depth d s Initialize the staircase by using the second eigenvector of the first tread and the second eigenvector of the second tread.

[0063] The initialization result using the stair treads is as follows:

[0064] h s =|(c1-c2)·0,0,1) T |

[0065] d s =d ff ∨d s =d bb

[0066] s=n t，1 ∨s=n t，2

[0067] Each time a staircase is detected, the staircase graphic is initialized multiple times. Because the 3D structure of the staircase can obscure each other during pedestrian and vehicle movement, initialization has a certain probability of failure. To improve the initialization success rate, the algorithm needs to allow for a certain margin in initialization according to the national standard for staircases. The standard for each parameter is as follows:

[0068] h s ∈[0.1m, 0.3m]

[0069] d s ∈[0.15m, 0.4m]

[0070]

[0071] If it is outside this range, the set of planes will not be initialized.

[0072] Since the height of the LiDAR installed on the autonomous vehicle typically does not exceed three steps, the initialization process based on the vertical plate is more important when climbing stairs, as the LiDAR on the autonomous vehicle has difficulty scanning two consecutive steps. However, when descending stairs, the LiDAR on the autonomous vehicle has difficulty scanning the vertical plate, and in this case, the step-based initialization process plays a major role in stair detection.

[0073] After initialization, for all successfully initialized stair structures J = {J1, J2, J3, ..., J...} n To expand upon this, first create an empty staircase shape G based on each element in the initialization result set. i (1≤i≤n), initialize the staircase graphic with the initialization result.

[0074] G i =J i

[0075] Then, expanding along the ascending direction of the stairs, one step at a time, each expansion searches the planar point cloud P of the vertical board with a certain radius. r And the planar point cloud P of the pedal t If a planar point cloud conforming to the staircase standard is found, the corresponding riser or tread structure point cloud is added to the staircase graphic.

[0076] Gi =G i ∪P r，k ∪P t，k

[0077] If no matching plane is found after expanding two consecutive steps, the upward expansion stops. Starting from the initial position, the staircase graphic is expanded downwards, following a similar process to upward expansion. The point cloud is searched for planar planes with a certain radius, one step at a time. If a plane matches the staircase structure requirements, it is added to the staircase structure. If no matching plane is found in the space between two consecutive steps, the staircase expansion stops, completing the staircase detection. The detected staircase planar point cloud is then stitched to the original point cloud P in the next frame. ori ′=P ori ∪P r ∪P t

[0078] The stitched point cloud is downsampled before proceeding to the next frame's detection process.

[0079] Because the algorithm uses stitched point clouds to perform multiple detections on the same staircase, an evaluation function is needed to assess the results of each detection and select the detection results with high scores to be included in the staircase model.

[0080] The algorithm incorporates the number of detected stair planes and the stair model parameters estimated through point cloud geometric properties to construct an evaluation function.

[0081] S stair =S r ·n r +S t ·n t

[0082] Where n r and n t These represent the number of detected stair model risers and treads, respectively.

[0083] S r For the evaluation function of the upright plate part,

[0084] S r =S rd +S rh +S rs

[0085] Evaluation value S of the depth of the vertical slab step rd Evaluation value S of the height of the upright step rh And the evaluation value S of the orientation of the vertical staircase. rs The algorithm consists of three parts. To ensure that the influence coefficients of the three parts are the same, all three parameters have been normalized. The evaluation value S of the slab step depth. rdThe calculation method is as follows:

[0086]

[0087] in The estimated step depth for the staircase model. To represent the staircase diagram model and the depth d of the i-th vertical step. s The coefficient of difference between them

[0088]

[0089] In the formula p est，i Let c be the coordinates of the i-th riser estimated based on the staircase model. i Let the centroid of the i-th vertical plate be obtained from point cloud measurements.

[0090]

[0091] In the formula p stair Let j be the coordinates of the top front edge of the first step in the staircase model, describing the current position of the riser relative to the step. The riser step height evaluation value S. rh The calculation method is as follows

[0092]

[0093] in The estimated step height for the staircase model. To represent the staircase model and the height h of the i-th riser step s The coefficient of difference between them. Since some risers are obscured, leading to inaccurate height parameters, directly incorporating the height estimated from the staircase model into the calculation of the difference coefficient is unreasonable. Therefore, we use whether the edge of each riser extends beyond the staircase model as a criterion to measure this difference:

[0094]

[0095] in, Let be the height of the upper edge of the i-th vertical plate plane. Let be the height of the lower edge of the i-th vertical plate plane.

[0096]

[0097]

[0098] and These are the heights of the upper and lower edges of the i-th vertical slab plane, estimated based on the staircase model. The vertical slab staircase orientation evaluation value S. rs The calculation method is as follows:

[0099]

[0100] in It is a coefficient that measures the difference between the model estimate and the orientation of a single vertical panel.

[0101]

[0102] In the formula, For the staircase orientation estimated using the staircase model, n i Let be the normal vector of the i-th vertical plate plane.

[0103] S t Let be the evaluation function for the pedal section.

[0104] S t =S td +S th

[0105] Evaluation value S of the tread step depth id Evaluation value S of step height th It consists of two parts, and similarly, in order to make the influence coefficients of the two parts the same, the three parameters of the algorithm are normalized.

[0106] Pedal step depth evaluation value S td The calculation method is the same as that for vertical panels:

[0107]

[0108] Similarly, due to the occlusion caused by the three-dimensional structure, the difference coefficient... The calculation uses the difference coefficient between the height of the upright board and the vertical board. In a similar way,

[0109]

[0110] in, The height of the rear edge of the i-th pedal plane. The height of the lower edge of the i-th pedal plane.

[0111]

[0112]

[0113] and These represent the depths of the rear and front edges of the i-th tread plane, estimated based on the staircase model. The tread step height evaluation value S... th The calculation method is as follows:

[0114]

[0115] The coefficient of difference is

[0116]

[0117] The algorithm calculates an evaluation value for the stairs detected in each frame, which serves as an indicator of the accuracy and completeness of the stairs detection in the current frame.

[0118] The algorithm re-detects stairs in the stitched point cloud. If the evaluation value of the same stair is higher than that of the previous frame, it indicates that the detected stair is more complete or the detection result is more accurate. The detection result of the current frame replaces the result of the previous frame, and the planar point cloud is stitched to the global point cloud again. For each detected stair, the overall stair space is stored in the form of a cube frame. The entire stair space is aligned using normal vectors, outliers are removed, and then the maximum and minimum values ​​of the x, y, and z axes of the aligned stair point cloud are calculated. Their differences are used as the length, width, and height parameters of the cube frame. When the same stair is misdetected as multiple stair spaces due to occlusion, they can be merged into one stair by using the adjacent or contained relationships of the cube spaces.

[0119] The above description is merely a preferred embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A staircase detection method based on multi-frame point cloud sequence stitching, characterized in that, Includes the following steps: The original point cloud is obtained, and the point cloud is segmented into planes using the region growing method. The segmented point cloud is then filtered to extract the point cloud of the staircase plane. The point cloud of the staircase plane is filtered according to the geometric features of the staircase plane to obtain a plane that meets the staircase standard and then spliced ​​together to obtain the staircase graphic. The staircase graphic of any two consecutive adjacent steps is initialized. The staircase graphic of the successfully initialized step is expanded. Each expansion searches for a plane point cloud that meets the staircase standard with a preset radius. If no plane that meets the staircase standard is found after expanding two consecutive steps, the expansion stops and the detection of the original point cloud of a single frame is completed. The detected plane point cloud is then stitched to the original point cloud of the next frame for detection of the next frame. An evaluation function is constructed, and the evaluation value of each frame is calculated based on the evaluation function. If the evaluation value of the stair model up to the current frame is greater than that of the stair model in the previous frame, it is replaced, and the planar point cloud that meets the stair standard is stitched to the original point cloud until the motion stops.

2. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 1, characterized in that, The process of obtaining the plane includes: downsampling the original point cloud using an octree; selecting a point in the downsampled point cloud as a seed point for growth; using the seed point as the center of the plane; using all points in the neighborhood of the seed point to locally fit the tangent plane; calculating the normal vector of the point cloud based on the fitting result; obtaining the fitted plane based on the normal vector and the set of points in the neighborhood of the seed point; in the fitted plane, searching for the KNN point of the seed point; if the KNN point does not belong to any region, creating a list for storage; and calculating the plane distance between the seed point and each KNN point in the list; if the plane distance does not exceed a preset value, it is included in the current fitted plane and used as a new seed point to continue expanding the fitted plane; if it exceeds the preset value, it is used as a seed point for growth of a new plane, until all planes are obtained through segmentation.

3. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 1, characterized in that, The screening process includes setting stair standards, which include stair angles and stair geometry. The PCA method is used to calculate the eigenvector of each plane. The angle between each plane and the Z-axis of the world coordinate system is calculated by comparing the eigenvector with the unit vector of the Z-axis of the world coordinate system. The angle is then compared with the preset value of the staircase angle to filter all planes by geometric angle. Calculate the angle between the feature vector of each selected plane and the X-axis of the world coordinate system, construct a rotation matrix based on the angle, align the point cloud in the positive direction of the X-axis, and select planes that conform to the structure of the pedal and the upright and that conform to the preset geometric dimensions.

4. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 3, characterized in that, The planar point cloud that meets the preset geometric angle but has a smaller geometric size than the preset geometric size is stitched into the original point cloud of the next frame, and the stitched point cloud is downsampled.

5. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 1, characterized in that, Initialization includes initialization of stair risers and stair treads; when expanding, the stair graphic is initialized multiple times.

6. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 1, characterized in that, The expansion process includes: constructing an empty staircase graphic based on each element in the initialization result; expanding the successfully initialized staircase graphic along the staircase extension direction in units of one step; searching for the vertical plate plane point cloud and the tread plane point cloud that meet the staircase standard with a preset radius for each expansion; if found, adding the corresponding vertical plate or tread plane point cloud to the staircase graphic; where the staircase extension direction includes upward and downward.

7. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 1, characterized in that, The evaluation function is constructed based on the upright evaluation function, the pedal evaluation function, the number of upright planes, and the number of pedals. The expression of the evaluation function is: S stair =S r ·n r +S t ·n t In the formula, n r and n t S represents the number of detected stair model risers and treads. r S is the evaluation function for the vertical plate. t This is the pedal evaluation function.

8. The staircase detection method based on multi-frame point cloud sequence stitching according to claim 7, characterized in that, The evaluation function for the vertical slab is the sum of three parts: the evaluation value of the vertical slab step depth, the evaluation value of the vertical slab step height, and the evaluation value of the vertical slab staircase orientation. The pedal evaluation function is the sum of the pedal step depth evaluation value and the pedal step height evaluation value.

Images