Multi-frame fusion stair detection method based on laser radar

Abstract

The invention relates to the technical field of robot and unmanned vehicle environment perception, in particular to a multi-frame fusion stair detection method based on a laser radar, and the method comprises the steps: obtaining laser radar point cloud and vehicle attitude data, carrying out the horizontal correction and cylinder filtering processing of the point cloud, and eliminating the interference; candidate stair point clouds are obtained through inter-frame matching, plane extraction and ground removal, multi-frame data are fused after clustering screening, and finally complete stair geometric information is output through parallel straight line feature recognition. A multi-frame fusion strategy and a feature reconstruction technology are adopted, the problems that in the prior art, environment interference is sensitive, plane fitting is not accurate, and a clustering threshold value is difficult to adjust are effectively solved, the stability and precision of stair detection are remarkably improved, and key parameters such as stair gradient, step size and orientation can be reliably output; accurate environment perception support is provided for automatic stair climbing control of unmanned equipment.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot and unmanned vehicle environment perception technology, and particularly relates to a multi-frame fusion stair detection method based on a laser radar. BACKGROUND

[0002] With the wide application of robots and unmanned vehicles, environment perception technology has become a key to autonomous navigation. As a high-precision sensor, a laser radar can realize modeling and recognition of the surrounding environment through point cloud data. However, in the stair detection task, the existing methods still face many challenges: due to the sparsity of laser radar point cloud and environmental interference (such as wall, ground noise, etc.), traditional algorithms are prone to miss detection, false detection, and are difficult to stably output the complete geometric information of the stairs (such as step height, width, slope, etc.), resulting in difficulties in planning and control of unmanned equipment in the process of climbing stairs.

[0003] To solve the above problems, the prior art such as the patent "All-terrain vehicle and automatic detection of stairs and climbing method thereof" (application number: CN202010910982.3) proposes a laser radar-based stair detection scheme. This technology removes distant sparse points and ground points, and then clusters and fits the planes to identify the planes that meet the stair characteristics. This method can detect stairs to some extent, but still has the following defects: first, indoor walls and other irrelevant planes are easily misjudged as stairs, increasing the computational burden and reducing the detection rate; second, the clustering threshold is difficult to adjust, and too small is prone to miss detection of steps, and too large introduces noise; in addition, when the laser radar is placed horizontally, the dense distribution of point clouds of the vertical plane of the steps will interfere with the plane fitting accuracy, especially when the number of laser lines increases, the computational complexity increases significantly, and the detection reliability decreases.

[0004] Therefore, how to overcome the problems of strong environmental interference, sensitive clustering threshold, and inaccurate plane fitting in the prior art to achieve high-robustness and high-precision stair detection has become a technical problem to be solved in the field. SUMMARY

[0005] The purpose of the present application is to provide a multi-frame fusion stair detection method based on a laser radar, which solves the problems of strong environmental interference, sensitive clustering threshold, and inaccurate plane fitting in the prior art stair detection method.

[0006] To achieve the above purpose, the present application provides a multi-frame fusion stair detection method based on a laser radar, comprising the following steps:

[0007] Obtaining laser radar original point cloud data and positioning information containing vehicle pitch angle and roll angle;

[0008] A point cloud correction conversion matrix is calculated according to the pitch angle and the roll angle, the original point cloud is horizontally corrected, and the original point cloud is converted to a horizontal state;

[0009] The point cloud in the horizontal state is subjected to cylinder filtering processing, the point cloud is divided according to a preset size of a cubic cylinder in a three-dimensional space, and each cylinder retains the highest point cloud data in height;

[0010] The point cloud filtered by the cylinder in the current frame is matched with the point cloud filtered by the cylinder in the last frame, and an inter-frame conversion matrix is calculated;

[0011] The point cloud subjected to the cylinder filtering and the correction processing is subjected to plane fitting to extract a plane point cloud, and the ground point cloud is removed, to obtain a candidate stair point cloud;

[0012] The candidate stair point cloud is subjected to clustering processing, and point cloud clusters conforming to the stair feature are screened through normal vector analysis;

[0013] The screened point cloud is subjected to inverse correction processing, and is restored to the current vehicle coordinate system, and is fused with the historical frame point cloud;

[0014] The fused point cloud is subjected to straight line fitting, parallel straight line groups are extracted, and whether the parallel straight line groups conform to the stair feature is judged;

[0015] When it is judged that the stair feature is present, the parallel line lengths are unified, the stair slope, the number of steps, the center point coordinates, the orientation, and the length are calculated, and the starting and ending steps are marked.

[0016] The point cloud in the horizontal state is subjected to cylinder filtering processing, the point cloud is divided according to a preset size of a cubic cylinder in a three-dimensional space, and each cylinder retains the highest point cloud data in height, and the specific steps include:

[0017] A mapping table storing indexes of each cubic cylinder and corresponding highest points is constructed;

[0018] Point cloud data in the horizontal state is traversed, and the horizontal index of each point in the cubic cylinder is calculated, wherein the horizontal index is obtained by dividing the plane coordinates of the point by the length of the bottom surface of the cubic cylinder;

[0019] Whether there is a cubic cylinder record corresponding to the index in the mapping table is queried;

[0020] If the record exists, the height values of the current point and the record point are compared, the point with a higher height is retained, and is updated to the mapping table;

[0021] If the record does not exist, the current point is added to the corresponding index position in the mapping table;

[0022] After the traversal is completed, all record points in the mapping table are extracted to form filtered point cloud data.

[0023] Wherein, the current frame column body filtered point cloud is matched with the last frame column body filtered point cloud, and the interframe conversion matrix is calculated, and the specific steps include:

[0024] Call the iterative closest point algorithm interface of the point cloud processing library;

[0025] The last frame point cloud is taken as the source point cloud, and the current frame point cloud is taken as the target point cloud to input the algorithm;

[0026] The optimal rigid transformation is obtained through iterative calculation, wherein the rigid transformation includes rotation and translation transformation;

[0027] The interframe conversion matrix representing the spatial transformation relationship between the two frames of point clouds is output.

[0028] Wherein, the plane fitting is performed on the point cloud after the column filtering and correction processing to extract the plane point cloud, and the ground point cloud is removed to obtain the candidate stair point cloud, and the specific steps include:

[0029] The plane segmentation object is constructed and the plane fitting parameters are set, wherein the parameters include the model type, the fitting method, the maximum iteration number and the distance threshold;

[0030] The point cloud after the column filtering processing is input into the plane segmentation object, and the random sample consensus plane fitting algorithm is executed;

[0031] The fitted plane point cloud is extracted as an initial plane point cloud set;

[0032] The secondary random sample consensus plane fitting is performed on the initial plane point cloud set, and the ground fitting dedicated parameters are set;

[0033] The point cloud data meeting the ground feature is identified and removed;

[0034] The remaining point cloud data is output as the candidate stair point cloud.

[0035] Wherein, the candidate stair point cloud is clustered, and the point cloud cluster meeting the stair feature is screened through the normal vector analysis, and the specific steps include:

[0036] The candidate stair point cloud is subjected to Euclidean clustering, and each plane is divided into an independent point cloud cluster;

[0037] The clustered point cloud cluster is extracted, the point cloud covariance matrix is calculated, and the PCA analysis is performed on the covariance matrix to obtain the point cloud cluster normal vector, and then the angle between the normal vector and the z-axis is calculated, and the point cloud cluster meeting the threshold is taken as the candidate point cloud cluster meeting the stair feature.

[0038] Wherein, the screened point cloud is subjected to inverse correction processing to restore to the current vehicle coordinate system, and is fused with the historical frame point cloud, and the specific steps include:

[0039] Invert the correction conversion matrix of the point cloud to obtain an inverse correction matrix;

[0040] Multiply the screened candidate stair point cloud by the inverse correction matrix to restore it to the original coordinate system of the vehicle;

[0041] Determine whether the historical frame candidate stair point cloud is empty, and if not, use the inter-frame conversion matrix to convert the historical frame point cloud to the current vehicle coordinate system, and merge the converted historical frame point cloud with the restored candidate stair point cloud of the current frame;

[0042] If the historical frame candidate stair point cloud is empty, directly use the restored candidate stair point cloud of the current frame;

[0043] Output the fused enhanced point cloud dataset.

[0044] Among them, the fused point cloud is fitted with a straight line, and whether it meets the stair feature is judged, and the specific steps include:

[0045] According to the set number of cycles, the RANSAC method is used to fit the straight line in the fused point cloud, and each cycle extracts the point cloud of the successfully fitted straight line, calculates and stores the orientation, center point, endpoint and length information of the straight line, removes the straight line point cloud from the fused point cloud, and enters the next cycle until the set number of cycles is completed or the early stop condition is triggered. The algorithm triggers the stop condition in advance when the number of point clouds is less than the threshold value;

[0046] Iterate through the straight line, and determine whether it is a parallel line according to the orientation of the straight line to obtain a plurality of parallel line arrays, and take the parallel line array with the largest number in the parallel line array as the candidate stair parallel line;

[0047] For the candidate stair parallel line, according to the number of matched parallel lines, the number of consecutive frames detected continuously, and the minimum length of the parallel line, if the threshold value is met, it is regarded as a stair parallel line.

[0048] The multi-frame fusion stair detection method based on laser radar provided by the application is used for a robot or unmanned vehicle equipped with a laser radar, and when the robot or unmanned vehicle encounters a stair obstacle and needs to climb over the stair obstacle, the laser radar is used in an environment where the performance of the laser radar is not limited. By extracting stair point clouds from the laser radar point cloud and then further extracting step point clouds, the position, height and width of the stair steps are calculated, automatic detection of parallel stairs is realized, complete stair information is provided, and automatic stair climbing of unmanned equipment is supported, thereby stably realizing a complete stair climbing process.

[0049] The application improves the stair detection rate by horizontal correction in the point cloud preprocessing stage according to the characteristics that the stairs are static and generally unchanged; introduces the Pillar Filter technology, divides the cubic column along the horizontal plane and retains the highest point, effectively eliminates the vertical wall / step interference and reduces the calculation amount; adopts the multi-frame fusion strategy to enhance the detection stability of the low-line laser radar; proposes a stair representation method based on parallel straight line groups, uses the characteristics that the point cloud after Pillar Filter presents multiple parallel straight lines, realizes fine step segmentation and start / end point positioning, and provides support for complex motion control. Compared with the simple clustering + plane fitting scheme of the prior art, the application significantly improves the robustness and detection accuracy in complex scenes through the spatial domain feature reconstruction and adaptive modeling mechanism. BRIEF DESCRIPTION OF DRAWINGS

[0050] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or prior art description will be briefly introduced as follows.

[0051] Figure 1 is a flowchart of the multi-frame fusion stair detection method based on laser radar of the application.

[0052] Figure 2 is a detection effect diagram before climbing stairs of the application.

[0053] Figure 3 is a detection effect diagram in the process of climbing stairs of the application.

[0054] Figure 4 is a detection effect diagram when the process of climbing stairs is almost finished of the application.

[0055] Figure 5 is a step flowchart of the multi-frame fusion stair detection method based on laser radar of the application. DETAILED DESCRIPTION

[0056] The embodiments of the application will be described in detail below, examples of which are shown in the drawings, the embodiments described below by referring to the drawings are exemplary and are intended to explain the application, and cannot be understood as a limitation of the application.

[0057] Please refer to Figures 1 to 5 , wherein, Figure 1 is a flowchart of the multi-frame fusion stair detection method based on laser radar of the application. Figure 2 is a detection effect diagram before climbing stairs of the application. Figure 3 is a detection effect diagram in the process of climbing stairs of the application. Figure 4 is a detection effect diagram when the process of climbing stairs is almost finished of the application. Figure 5 is a step flowchart of the multi-frame fusion stair detection method based on laser radar of the application.

[0058] The application provides a laser radar-based multi-frame fusion stair detection method, comprising the following steps:

[0059] S101: Obtain laser radar original point cloud data and positioning information containing vehicle pitch angle and roll angle;

[0060] S102: Calculate a point cloud correction conversion matrix according to the pitch angle and the roll angle, and perform horizontal correction on the original point cloud to convert the original point cloud to a horizontal state;

[0061] Specifically, the original point cloud and the positioning information are obtained, wherein the obtaining mode is different according to different platforms, and the application is also applicable to various different platforms, and different platforms have different obtaining modes, such as UDP network transmission and ros system topic subscription.

[0062] The self attitude angle pitch and roll in the positioning information are used to calculate a point cloud correction conversion matrix, and then the original point cloud is corrected by using the point cloud correction conversion matrix to convert the point cloud to a horizontal state. The significance of correction is that, based on the prior information that the stair is stationary and the stair step is usually composed of two faces perpendicular and parallel to the horizontal ground (when the vehicle is parallel to the horizontal ground, the x and y planes in the vehicle coordinate system are parallel to the horizontal ground), when the vehicle attitude, especially the roll and pitch angles, changes, the two faces of the stair step in the vehicle coordinate system are no longer perpendicular or parallel to the x and y planes. The correction conversion matrix calculated by using the vehicle attitude roll and pitch angles in the positioning information can restore the perpendicular or parallel relationship between the two faces of the stair step and the x and y planes, and ensure the extraction of subsequent stair point clouds.

[0063] S103: Perform cylinder filtering processing on the point cloud in the horizontal state, divide the point cloud in a three-dimensional space according to a preset size of a cuboid column, and retain the highest point cloud data in each column;

[0064] Specifically, the point cloud corrected to the horizontal state is received as input, the point cloud is divided in a three-dimensional space according to a cuboid column, the bottom surface length of the cuboid column can be configured, then only the highest point in each column is retained, and the new point cloud is obtained after the traversal is completed. The order of magnitude of the point cloud is reduced, and at the same time, the interference of the vertical wall point cloud on the subsequent extraction of the stair point cloud plane and the accumulation of the stair step point cloud in the vertical plane can be eliminated to some extent, so that the stair step point cloud is closer to a straight line distribution, and the success rate of subsequent stair point cloud straight line fitting can be improved.

[0065] S104: Perform inter-frame matching on the point cloud filtered by the column in the current frame and the point cloud filtered by the column in the previous frame, and calculate an inter-frame conversion matrix;

[0066] Specifically, the point cloud after the last frame pillar filter processing and the point cloud after the current frame pillar filter processing are matched by calling the interface of the point cloud data processing three-party library PCL library to obtain the conversion matrix of the last frame point cloud to the current frame, which provides a conversion basis for subsequent extraction of the stair point cloud and multi-frame fusion.

[0067] S105: Plane fitting is performed on the point cloud after the pillar filter and correction processing to extract the plane point cloud and remove the ground point cloud to obtain the candidate stair point cloud.

[0068] Specifically, after the pillar filter and correction processing, the point cloud distributed on the vertical plane will degenerate from a plane distribution to a curve or a straight line, and at this time, the stair point cloud is roughly in a plane state composed of multiple parallel lines. Based on this, the point cloud distributed on the plane is extracted by using the Random Sample Consensus (hereinafter referred to as RANSAC, which uses the idea of iterative random sampling and consensus voting: constantly trying to estimate the model with the least number of sample points, and then verifying whether the model is "understandable" for most data, that is, a certain number of point clouds are randomly selected, a plane is fitted from these point clouds, and then the distances of the remaining points to the plane are calculated. The point clouds with distances within a threshold are added to the original plane point cloud, and then a plane is fitted. Then, the loop is repeated, and after the stop condition (such as the maximum number of iterations) is reached, the plane of the last iteration is output as the result) plane fitting method, and the extracted point cloud usually contains the stair point cloud.

[0069] In the plane point cloud, in addition to the stair point cloud, the ground point cloud is usually included, and the ground point cloud is usually dense. To further extract the stair point cloud, eliminate interference, and reduce the computational load of subsequent operations, a ground removal operation is required. The ground point cloud is removed by using the RANSAC plane fitting method (the fitting method is consistent with the foregoing plane fitting, and the fitting parameters such as the distance threshold and the maximum number of iterations need to be set according to the actual application scenario).

[0070] S106: The candidate stair point cloud is clustered and processed, and the point cloud cluster meeting the stair characteristics is selected by normal vector analysis.

[0071] Specifically, after the pre-processing of the point cloud, the vertical plane point cloud and the horizontal ground point cloud are removed, and now there are also point clouds that are distributed in a plane at a certain angle with the horizontal ground, which includes the stair point cloud and may also include other plane point clouds. Based on this, the point cloud is clustered in Euclidean space, and each plane is divided into an independent point cloud cluster;

[0072] The clustered point cloud cluster is extracted, the point cloud covariance matrix is calculated, and the PCA analysis is performed on the covariance matrix to obtain the point cloud cluster normal vector. Then the angle between the normal vector and the z-axis is calculated, and the angle that meets the threshold value is judged as the candidate stair point cloud.

[0073] S107: Perform inverse correction processing on the screened point cloud to restore it to the current vehicle coordinate system and fuse it with the historical frame point cloud;

[0074] Specifically, the point cloud correction matrix is inverted, and then the point cloud is restored to the state before correction. The point cloud is corrected before to improve the success rate of stair point cloud detection. After the stair point cloud is extracted, it needs to be restored to the current vehicle coordinate system in order to calculate the stair information in the subsequent vehicle coordinate system;

[0075] The historical accumulated point cloud is converted to the current coordinate system using the inter-frame conversion matrix, and then fused with the current frame point cloud. The number of fused stair point clouds is larger, which improves the success rate of subsequent straight line fitting.

[0076] S108: Perform straight line fitting on the fused point cloud, extract parallel straight line groups and judge whether they meet the stair characteristics;

[0077] Specifically, according to the set number of cycles, the RANSAC method is used to fit straight lines in the fused point cloud. Each cycle extracts the point cloud of the successfully fitted straight line, calculates and stores the orientation, center point, end point, length, etc. of the straight line, removes the straight line point cloud from the fused point cloud, and enters the next cycle. The algorithm stops early when the number of point clouds is less than a threshold value;

[0078] Iterate through the straight lines to determine whether they are parallel lines based on their orientations to obtain a plurality of parallel straight line arrays. The largest parallel straight line array in the parallel straight line array is considered as the candidate stair parallel line;

[0079] For the candidate stair parallel line, judge it based on the number of matched parallel lines, the number of consecutive frames of continuous detection, and the minimum length of the parallel line. If it meets the threshold value requirement, it is considered as a stair parallel line.

[0080] S109: When the stair characteristics are judged, the parallel line length is unified, the stair slope, step number, center point coordinates, orientation, length are calculated, and the starting and ending steps are marked.

[0081] Specifically, for the stair parallel lines, first, according to the minimum length of the parallel lines, all parallel lines are unified to ensure that the center points of all parallel lines are on the same straight line. Then the slope of the stairs, the number of steps, the center point, the direction, the length and other information are calculated, and the starting and ending steps are judged;

[0082] The current frame of the stair in the vehicle coordinate system is taken as the result output.

[0083] Embodiment one:

[0084] The laser radar-based multi-frame fusion stair detection method specifically reasons M correct The steps are as follows:

[0085] S1, point cloud correction:

[0086] The self attitude angle in the positioning information: pitch, roll, pitch corresponds to the angle of rotation around the Y axis of the vehicle coordinate system, and roll corresponds to the angle of rotation around the X axis of the vehicle coordinate system. The point cloud correction rotation matrix is calculated

[0087] Then use the point cloud correction conversion matrix M correct The original point cloud P origin is corrected, and the point cloud is converted to a horizontal state to obtain the corrected point cloud P correct =M correct ·P origin .

[0088] S2, pillar filter (pillar filter):

[0089] A map is constructed to store the index of each cubic pillar and the maximum z value of the points belonging to the pillar region.

[0090] The point cloud P correct is traversed.

[0091] The index of each point in the pillar is calculated:

[0092] x-direction index: index x =x / pillar_size,

[0093] y-direction index: index y= y / pillar_size, where x, y are coordinate values of the point in the xy plane, and pillar_size is the edge length of the bottom surface of the cube column. The edge length pillar_size is set in combination with the number of laser radar point clouds, and the purpose of this filtering is to filter the point cloud in the z direction while retaining the density in the x and y directions to the greatest extent. Therefore, the edge length is usually 0.05 m or 0.1 m. If the edge length is too long, the filtered point cloud will be sparse in the x and y directions;

[0094] According to the index, the coordinate z value of the current point is compared with the value of the point in the pillar. If the current point is greater than the point in the pillar, the current point is used to replace the point in the pillar and stored in the map. If there is no point in the current pillar, the current point is directly stored in the map;

[0095] Traverse the map, and store the points in each pillar in the map into the filtered point cloud P filter ;

[0096] S3, inter-frame point cloud matching: when the point cloud P last processed by the pillar filter of the previous frame and the point cloud P filter processed by the pillar filter of the current frame are not empty, use the point cloud registration method to match P last and P filter to obtain the inter-frame conversion matrix M cur ;

[0097] S4, extracting planar point cloud: planar fitting is usually used to extract planar point cloud. The present application uses the random sample consensus algorithm RANSAC (Random Sample Consensus) to achieve the PCL library:

[0098] Construct a SACSegmentation object seg in the PCL library;

[0099] Pass the point cloud Points filter and the plane fitting parameters (including model type, fitting method, maximum iteration number, distance threshold, which can be configured according to the type of laser radar, installation height, and actual application scenario) into seg, and perform plane fitting;

[0100] Obtain the point cloud P plane belonging to the plane after fitting.

[0101] S5, removing the ground: the specific implementation is consistent with S4, but the input point cloud is the result P plane of S4, and the plane fitting parameters also need to be configured according to the actual scene to obtain the point cloud P non_ground ;

[0102] S6, point cloud clustering: there are many common point cloud clustering methods, such as density-based clustering method

[0103] DBSCAN (Density-Based Spatial Clustering of Applications with Noise), MeanShift clustering, RANSAC based on geometric features, Euclidean clustering, etc. The present application does not limit the clustering implementation method, and one of the adopted Euclidean clustering is implemented as follows:

[0104] Construct a KD tree object tree and pass in the point cloud P non_ground ;

[0105] Construct a Euclidean clustering object ec in the PCL library and perform clustering (the essence is connectivity analysis: define a distance threshold d (the maximum distance between two points considered as "adjacent"), if the actual Euclidean distance between two points ≤ d, they are considered to belong to the same connected component, and each component is a cluster);

[0106] Extract the index cluster_indices of each point cloud cluster in the point cloud P non_ground after clustering.

[0107] S7, determine the point cloud cluster that meets the stair feature, which is implemented as follows:

[0108] Iterate through cluster_indices:

[0109] Extract the point cloud of the point cloud cluster from P non_ground according to the index of each point cloud cluster, and obtain P cluster ;

[0110] Call the PCL library compute3DCentroid method to calculate the mean pc_mean of P cluster , and call the PCL library computeCovariance Matrix method to calculate the covariance matrix pc_cov of P cluster ;

[0111] Then use the PCL library to perform eigenvalue decomposition on the point cloud covariance matrix pc_cov to obtain the point cloud normal vector N, which is the eigenvector corresponding to the smallest eigenvalue;

[0112] Calculate the dot product dot_product between the point cloud normal vector N and the z-axis vector (0, 0, 1), and then calculate the included angle angle by inverse cosine of the dot product dot_product.

[0113] Determine if the included angle (angle) meets the angle threshold. If it does, then the point cloud P is... cluster Store candidate staircase point cloud P stair ;

[0114] After traversal, output the candidate stair point cloud P. stair .

[0115] S8, Point Cloud Inverse Correction, is implemented as follows:

[0116] Point cloud correction transformation matrix M correct To find the inverse, the inverse matrix M of the point cloud correction and transformation is obtained using the inverse method of the Eigen library, a third-party matrix computation library. correct_inverse ;

[0117] Then, the point cloud correction transformation inverse matrix M is used. correct_inverse Select staircase point cloud P stair By transforming back to the original state, i.e., in the current vehicle coordinate system, we obtain the restored candidate staircase point cloud.

[0118] P stair_revert =M correct_inverse ·P stair .

[0119] S9, Inter-frame point cloud fusion, specifically implemented as follows:

[0120] (1) When the candidate stair point cloud in the historical frame is not empty, use the inter-frame transformation matrix M cur Transform the candidate staircase point cloud from historical frames to the current vehicle coordinate system, and then compare it with P. stair_revert The candidate staircase point cloud P is obtained by fusion. stair_fusion If the candidate staircase point cloud in the historical frame is empty, no operation is performed (P). stair_fusion With P stair_revert Maintain consistency.

[0121] S10. Loop fitting of straight lines, specifically implemented as follows:

[0122] The initial non-linear point cloud P stair_fusion The algorithm iteratively fits a straight line (based on a set number of iterations, it uses the RANSAC method to iteratively fit a straight line in the fused point cloud. In each iteration, the point cloud containing the successfully fitted line is extracted, and information such as the line's orientation, center point, endpoints, and length is calculated and stored. The point cloud containing the line is then removed from the fused point cloud before proceeding to the next iteration. This process continues until the set number of iterations is completed or an early stopping condition is triggered. In this algorithm, if the number of point clouds is less than a threshold, the early stopping condition is triggered), resulting in the straight line point cloud P. line and non-linear point cloud P non_line ;

[0123] Traverse the straight line point cloud P line , find the maximum point max_x and the minimum point min_x in the x direction of the straight line point cloud by comparing the coordinate values of each point in the x direction; calculate the length length (i.e. the distance between two points) of the straight line according to the coordinates of max_x and min_x, the direction vector direction_vector of the straight line in the xy plane, then calculate the dot product of the direction vector direction_vector and the x-axis vector (1, 0, 0), then take the inverse cosine of the dot product to get the included angle angle, and then store the straight line center point centroid, length length, direction_vector and angle in the straight line information array line_infos;

[0124] Traverse the straight line point cloud P lines and the straight line information array line_infos as the result output.

[0125] S11, extract parallel straight lines, which are implemented as follows:

[0126] Construct a two-dimensional array parallel_lines with a one-dimensional size equal to the size of the array line_infos to store multiple parallel straight line arrays;

[0127] Traverse the straight line information array line_infos to match parallel straight lines;

[0128] If the current straight line has been successfully matched, skip it;

[0129] Store the current straight line in parallel_lines;

[0130] Traverse the remaining straight lines, compare the current straight line with each of the remaining straight lines, and if the angle difference between the two is less than a threshold value (the threshold value is set by referring to the point-to-straight line distance threshold value set when fitting the straight line, and a certain redundancy is given on this basis, and the reference threshold value used in the present application is 10°), it is determined that the two straight lines are parallel, the straight line that has been successfully matched with the current straight line is also put into parallel_lines, and the straight line is marked as having been successfully matched;

[0131] Take the straight line array with the largest number in the parallel straight line array parallel_lines as the candidate stair parallel straight line array parallel_line output.

[0132] S12, stair feature judgment, which is implemented as follows:

[0133] If the number of parallel straight lines in parallel_line is greater than a threshold value, it is considered that the detection result is reliable in quantity, and the processing continues;

[0134] Traverse parallel_line, and compare to obtain the length min_length of the shortest straight line in the parallel line;

[0135] If min_length is greater than a threshold value, it is considered that the detection result is reliable in length, and the continuous frame statistical quantity count is incremented by 1, otherwise count is set to 0.

[0136] If count is greater than a threshold value, it is considered that the detection result is stable, at which time it is considered that the current stable staircase is detected, and it is considered that parallel_line is a straight line of the staircase.

[0137] S13, staircase feature information calculation, which is specifically implemented as follows:

[0138] Uniform parallel line length, using the shortest straight line min_length_line in parallel_line, the intersection points of the vertical line vertical_line passing through the center point and end point of min_length_line with other straight lines are obtained, which are the center points and end points of the straight line after uniform length;

[0139] Calculate the slope of the staircase: connect the center points of the straight lines with the lowest and highest heights in the parallel line, and obtain the included angle of the connecting line with the x-axis in the xz plane, which is considered as the slope of the staircase in the present application;

[0140] Calculate the orientation of the staircase: connect the center points of the straight lines with the lowest and highest heights in the parallel line, and obtain the included angle of the connecting line with the x-axis in the xy plane, which is considered as the orientation of the staircase in the present application;

[0141] The number of current detected staircase steps is the number of straight lines in parallel_line; the center point is the center point of all straight lines, and the length is the length of the shortest straight line in parallel_line;

[0142] The lowest step of the first detected staircase is marked as the starting step, and is converted to the positioning coordinate system using the positioning information and stored as historical staircase information; the subsequently detected staircase is transferred to the positioning coordinate system and matched and fused with the historical staircase, and through comparison of each step of the historical staircase and the current staircase, if the distance between the straight lines representing the two steps (the distance between the two end points of the straight line is calculated respectively) is less than a threshold value, it is considered that it is the same step at different times, the step matched using the historical staircase is used, and the steps in the historical staircase that are not matched are added to the current staircase, thereby updating the historical staircase information, and then the matched and fused staircase is inversely transformed using the positioning information matrix M local_inverseIn the vehicle coordinate system, the endpoints of the straight line representing the stair step are all used

[0143] M local_inverse Rotation translation is performed; if the equipment is in the process of climbing stairs, the height is constantly rising, but the number of stair steps does not change for n consecutive frames (n is a hyperparameter and can be set), the last step is judged as the end step, and the current frame of the stair in the vehicle coordinate system is obtained. Then the calculated information such as stair slope, step number, center point, direction, length, starting and ending steps is uniformly summarized and output, and the stair detection is completed. The detection effects before, during and after climbing stairs are shown in Figures 2 to 4 : The coordinate axis represents the vehicle position, the green frame is the detected stair visualization result, and the yellow step represents the end step.

[0144] According to the characteristics that the stair is static and generally unchanged, the stair detection rate is improved by horizontal correction in the point cloud preprocessing stage; the PillarFilter technology is introduced, the cubic column is divided along the horizontal plane, and the highest point is retained, which effectively eliminates the vertical wall / step interference and reduces the calculation amount; a multi-frame fusion strategy is adopted to enhance the detection stability of the low-line laser radar; a stair representation method based on parallel straight line groups is proposed, which utilizes the characteristics of the point cloud after PillarFilter that presents multiple parallel straight lines, realizes fine step segmentation and starting and ending point positioning, and provides support for complex motion control. Compared with the existing simple clustering + plane fitting scheme, the application significantly improves the robustness and detection accuracy in complex scenes through the spatial domain feature reconstruction and adaptive modeling mechanism.

[0145] The above only discloses one or more preferred embodiments of the application, and cannot limit the scope of the application. Those skilled in the art can understand that all or part of the above-mentioned embodiments can be implemented, and equivalent changes made according to the claims of the application still fall within the scope of the application.

Claims

1. A method for detecting stairs based on multi-frame fusion of a laser radar, characterized in that, The method comprises the following steps: Obtain the laser radar original point cloud data and the positioning information containing the vehicle pitch angle and the roll angle; Calculate the point cloud correction conversion matrix according to the pitch angle and the roll angle, perform horizontal correction on the original point cloud, and convert the original point cloud to a horizontal state; Perform cylinder filtering processing on the point cloud in the horizontal state, divide the point cloud in a three-dimensional space according to a preset size of a cuboid cylinder, and reserve the highest point cloud data in each cylinder; Perform inter-frame matching on the point cloud filtered by the cylinder in the current frame and the point cloud filtered by the cylinder in the last frame, and calculate an inter-frame conversion matrix; Perform plane fitting on the point cloud filtered by the cylinder and the corrected processing, extract plane point cloud, and remove the ground point cloud to obtain candidate stair point cloud; Perform clustering processing on the candidate stair point cloud, and filter the point cloud cluster meeting the stair characteristics through normal vector analysis; Perform inverse correction processing on the filtered point cloud, restore it to the current vehicle coordinate system, and fuse it with the historical frame point cloud; Perform straight line fitting on the fused point cloud, extract parallel straight line groups, and judge whether they meet the stair characteristics; When it is judged as the stair characteristics, unify the parallel line length, calculate the stair slope, the step number, the center point coordinate, the direction, and the length, and mark the starting and ending steps.

2. The method of claim 1, wherein the laser-radar-based multi-frame fusion stair detection method is characterized by, Perform cylinder filtering processing on the point cloud in the horizontal state, divide the point cloud in a three-dimensional space according to a preset size of a cuboid cylinder, and reserve the highest point cloud data in each cylinder, and the specific steps comprise: Construct a mapping table for storing indexes of each cuboid cylinder and corresponding highest points; Iterate the point cloud data in the horizontal state, and calculate the horizontal index of each point in the cuboid cylinder, wherein the horizontal index is obtained by dividing the plane coordinates of the point by the length of the bottom surface of the cuboid cylinder; Query whether there is a cuboid cylinder record corresponding to the index in the mapping table; If there is a record, compare the height values of the current point and the record point, reserve the higher point, and update it to the mapping table; If there is no record, add the current point to the corresponding index position of the mapping table; After the iteration is completed, extract all record points in the mapping table to form the filtered point cloud data.

3. The laser-radar-based multi-frame fusion stair detection method of claim 2, wherein, Perform inter-frame matching on the point cloud filtered by the cylinder in the current frame and the point cloud filtered by the cylinder in the last frame, and calculate an inter-frame conversion matrix, and the specific steps comprise: Call the iterative closest point algorithm interface of the point cloud processing library; Input the last frame point cloud as the source point cloud and the current frame point cloud as the target point cloud into the algorithm; Obtain the optimal rigid transformation through iteration, wherein the rigid transformation comprises rotation and translation transformation; Output the inter-frame conversion matrix representing the spatial transformation relationship between the two frames of point cloud.

4. The method of claim 3, wherein the laser-radar-based multi-frame fusion stair detection method is characterized by, Perform plane fitting on the point cloud filtered by the cylinder and the corrected processing, extract plane point cloud, and remove the ground point cloud to obtain candidate stair point cloud, and the specific steps comprise: Construct a plane segmentation object and set plane fitting parameters, wherein the parameters comprise model type, fitting method, maximum iteration number and distance threshold; Input the point cloud filtered by the cylinder into the plane segmentation object, and execute the random sample consensus plane fitting algorithm; Extract the plane point cloud obtained by fitting as an initial plane point cloud set; Perform secondary random sample consensus plane fitting on the initial plane point cloud set, and set ground fitting dedicated parameters; Identify and remove point cloud data meeting the ground characteristics; Output the remaining point cloud data as a candidate stair point cloud.

5. The laser-radar-based multi-frame fusion stair detection method of claim 4, wherein, The candidate stair point cloud is clustered, and point cloud clusters meeting the stair feature are screened through normal vector analysis. The specific steps include: The candidate stair point cloud is subjected to Euclidean clustering, and each plane is divided into an independent point cloud cluster. The clustered point cloud cluster is extracted, the point cloud covariance matrix is calculated, the PCA analysis is performed on the covariance matrix, the point cloud cluster normal vector is obtained, and then the angle between the normal vector and the z-axis is calculated. The point cloud cluster meeting the threshold is regarded as a candidate point cloud cluster meeting the stair feature.

6. The laser-radar-based multi-frame fusion stair detection method of claim 5, wherein, The screened point cloud is subjected to inverse correction processing, restored to the current vehicle coordinate system, and fused with the historical frame point cloud. The specific steps include: The point cloud correction conversion matrix is inverted to obtain an inverse correction matrix. The screened candidate stair point cloud is multiplied by the inverse correction matrix to restore it to the original vehicle coordinate system. Determine whether the historical frame candidate stair point cloud is empty. If not, use the inter-frame conversion matrix to convert the historical frame point cloud to the current vehicle coordinate system, and merge the converted historical frame point cloud with the current frame restored candidate stair point cloud. If the historical frame candidate stair point cloud is empty, directly use the current frame restored candidate stair point cloud. Output the fused enhanced point cloud dataset.

7. The laser-radar-based multi-frame fusion stair detection method of claim 6, wherein, Straight lines are fitted to the fused point cloud, parallel straight line groups are extracted, and it is determined whether they meet the stair feature. The specific steps include: According to the set number of cycles, the RANSAC method is used to fit straight lines in the fused point cloud. Each cycle extracts the point cloud of the successfully fitted straight line, calculates and stores the orientation, center point, end point, and length information of the straight line, removes the straight line point cloud from the fused point cloud, and enters the next cycle until the set number of cycles is completed or the early stop condition is triggered. The algorithm triggers the stop condition early when the number of point clouds is less than a threshold. Traverse the straight lines to determine whether they are parallel lines according to their orientations, obtain several parallel line arrays, and regard the parallel line array with the largest number of parallel lines as a candidate stair parallel line. For the candidate stair parallel line, determine whether it meets the threshold requirements according to the number of matched parallel lines, the number of consecutive frames detected, and the minimum length of the parallel line. If it meets the threshold requirements, it is regarded as a stair parallel line.

Images