Facade obstacle detection method based on three-dimensional laser point cloud data features

Abstract

The application discloses a facade obstacle detection method based on three-dimensional laser point cloud data features. Before the cleaning machine starts to move and clean, a point cloud coordinate system is established, and facade point cloud information is acquired; next, abnormal points are filtered, the correct corresponding relationship between the point cloud data and the real scene is acquired, and inherent parameters for coordinate system correction are acquired. After the cleaning machine starts to move, the rolling brush position of cleaning is acquired as ROI, then the ROI is subjected to target detection and segmentation through a random sample consensus algorithm, the point cloud of the obstacle on the facade is extracted, the point cloud of the same obstacle is merged through Euclidean clustering, and the position and size information of each obstacle are counted; finally, the obstacles are tracked through a Hungarian matching algorithm and Kalman filtering. The application can realize automatic detection of obstacles of different facade scenes such as ordinary straight wall surfaces and tunnel walls, the whole process does not need manual intervention, and is used for automatic obstacle avoidance of facade cleaning equipment.

Description

Technical Field

[0001] This invention relates to the field of intelligent environmental perception, and in particular to a method for detecting facade obstacles based on three-dimensional laser point cloud data features. Background Technology

[0002] Currently, tunnel wall cleaning is mainly done manually, which is time-consuming and labor-intensive, and poses significant safety hazards. Compared to traditional manual cleaning, wall washing machines can complete the work more efficiently and quickly. Manual operation of these machines is primarily carried out by a team of two: a driver and a worker. The driver operates the machine, while the worker sits in the control room and uses a remote control to operate the washing boom to clean the exterior surfaces of the tunnel walls. However, the complex operating environment of wall washing machines, with various obstacles frequently present on the tunnel and sound barrier facades, presents significant challenges and risks to the cleaning operation. If the cleaning worker is not focused or makes a mistake, the end of the boom may collide with the facade, causing impact damage to the boom system and the facade, and in severe cases, even leading to a traffic accident.

[0003] In automated cleaning operations, equipment based on LiDAR imaging can automatically adjust its cleaning strategy by sensing the distance and shape of the surrounding environment, avoiding collisions with obstacles and ensuring cleaning effectiveness. However, due to the different characteristics of walls in different scenarios, such as varying horizontal and vertical curvature and different wall transparency; when obstacles are small and have low light reflectivity due to their color and material, the obstacle information obtained by LiDAR is weak and sensitive to environmental changes; for walls with high transparency, such as sound barriers, most of the LiDAR beam will penetrate the glass, causing it to return to objects behind the sound barrier or resulting in no return signal; when vehicles are operating, the water mist sprayed will move in the direction of vehicle movement under the action of the roller brush, entering the LiDAR's field of view, and the dense water mist will reflect radar signals or float around obstacles, leading to inaccurate obstacle identification. Summary of the Invention

[0004] To address the technical problem of inaccurate obstacle identification caused by numerous interference signals in current automated cleaning operations, this invention proposes a method that can accurately identify various facade obstacles in different scenarios and under thin water mist conditions. When applied to facade cleaning equipment, the perception algorithm can obtain reliable and stable detection results.

[0005] To achieve the above-mentioned technical objectives, the technical solution of the present invention is as follows:

[0006] A method for detecting facade obstacles based on 3D laser point cloud data features includes the following steps:

[0007] S1. Before the wall-washing truck starts cleaning and moving, the point cloud of the scene where the wall-washing truck is located, including the ground and the wall, is collected by LiDAR. The correct correspondence between the point cloud and the real scene is obtained through external parameter calibration. Then, based on the spatial position transformation relationship between the planes, the inherent parameters used to correct the coordinates of the point cloud are obtained.

[0008] S2, after the wall-washing truck starts cleaning and moving, each frame of point cloud acquired during the movement is filtered and the coordinates are corrected based on the inherent parameters. The grid is divided according to the distribution. The grid features are used to characterize the point cloud, the region of interest (ROI) is extracted, and point cloud noise is filtered out.

[0009] S3, obtains the point cloud of the wall or facade through feature segmentation and filters it out from the region of interest, thereby obtaining the obstacle point cloud;

[0010] S4 performs obstacle point cloud clustering, calculates obstacle size and obtains obstacle bounding box information; after the initial clustering is completed, a secondary classification is performed based on the centroid distance to improve obstacle recognition accuracy.

[0011] S5 establishes tracking for each obstacle, performs matching based on distance between consecutive frames, and updates the matched obstacles using Kalman filtering; for different matching results, each obstacle is created, merged, or deleted to obtain the obstacle detection result.

[0012] The method for detecting facade obstacles based on three-dimensional lidar data, in step S1, includes the following steps for obtaining inherent parameters:

[0013] S101, Select multiple planes whose Euclidean distance exceeds the threshold, obtain the point cloud on these planes, and calculate the normal vector of this part of the point cloud as the normal vector of these planes;

[0014] S102, project the normal vectors of these planes onto the coordinate plane, and then calculate the angle between each projected normal vector and the real normal vector; before the wall-washing truck starts cleaning, collect multiple frames of point cloud data using a lidar, and take the average value of the angle results from multiple frames as the inherent parameter.

[0015] The method for detecting facade obstacles based on 3D LiDAR data, wherein step S101, calculating the normal vector of this part of the point cloud includes the following steps:

[0016] 1) Calculate the mean of the point cloud coordinates from the three coordinate dimensions in the XYZ coordinate system, obtain the centroid coordinates of the selected point cloud, and calculate the covariance matrix based on the centroid.

[0017] 2) Calculate the eigenvalues ​​and eigenvectors of the covariance matrix, and select the eigenvector corresponding to the smallest eigenvalue as the normal vector.

[0018] The facade obstacle detection method based on 3D laser point cloud data features, in step S2, extracting the region of interest (ROI) includes the following steps:

[0019] S201. Based on the movement data of the roller brush used for cleaning by the wall washing cart, the true position of the roller brush is obtained, and the position of the roller brush extending forward along the moving direction of the wall washing cart is taken as the ROI. In each frame of point cloud, a pass-through filter is used to remove point clouds with coordinates greater than the threshold, and a rotation transformation is performed according to the inherent parameters to obtain the point cloud of the ROI.

[0020] S202, Divide the grid in the XOZ plane, where the grid shape is a square with a given side length parameter;

[0021] S203: Traverse all grids and sort all points in the point cloud of each grid in ascending order of coordinates. If the coordinates of an element in the sorted array differ from the coordinates of the element in the previous position by more than a threshold, remove the points after that position in the grid; otherwise, do not process them.

[0022] In the facade obstacle detection method based on three-dimensional laser point cloud data features, in step S201, after obtaining the point cloud of ROI using a filtering algorithm, if the number of obtained point clouds is lower than the preset ratio threshold compared with the original point cloud, the point clouds of each plane, including the ground and the wall, of the current scene of the wall washing vehicle are re-collected, and the inherent parameters are re-acquired before step S2 is executed again.

[0023] The method for detecting facade obstacles based on three-dimensional laser point cloud data features, wherein step S3 includes:

[0024] S301, use the random sampling consistency method to separate the wall point cloud according to the plane fitting method of minimizing Euclidean distance, mark the identified plane point cloud; and repeat the process multiple times until the number of remaining point clouds is less than the preset ratio of the original point cloud, then treat the remaining point cloud as obstacle point cloud.

[0025] In the facade obstacle detection method based on three-dimensional laser point cloud data features, when step S301 is completed, if two planes are determined to exist based on geometric features, then when the distance between the two planes exceeds a threshold, and the number of point clouds on the plane that is farther from the origin of the laser radar exceeds the corresponding proportion of the original point cloud, the nearby point cloud is retained as a new working surface; otherwise, it is still considered to be the same plane.

[0026] The method for detecting facade obstacles based on three-dimensional laser point cloud data features, in step S301, when a curved wall appears, the curved wall is segmented along the curvature direction, and step S301 is performed on each segment to separate the obstacle. After all segments have separated the obstacle, voxel-based downsampling is used to reduce the number of point clouds, and finally the results of all segments are merged.

[0027] The facade obstacle detection method based on three-dimensional laser point cloud data features includes step S4, which involves calculating the obstacle size, including:

[0028] S401, Filter the obstacle point cloud obtained in step S3, select the corner point on the upper left side of the obstacle near the lidar as the starting point Pi, search for the k nearest neighbors of point Pi based on the point cloud distance, calculate the centroid of the set formed by Pi and the k nearest neighbors, calculate the distance of all points in the set to the centroid, and then calculate the mean and variance. Points in the set whose distance from the centroid exceeds the threshold are regarded as noise points and removed.

[0029] S402, use a clustering algorithm on the point set {P} obtained in S401 to obtain the point set {Ps} of each obstacle;

[0030] S403: Calculate the absolute value of the difference between the two extreme values ​​of each obstacle in the three coordinate directions, and use it as the length, width and height of the obstacle. For adjacent obstacles whose distance in the direction of the wall washer is less than the threshold, merge them and recalculate the length, width and height as a new obstacle.

[0031] The facade obstacle detection method based on three-dimensional laser point cloud data features includes step S5 as follows:

[0032] S501, If ​​it is not the starting frame, then the obstacles detected in the previous frame will be predicted according to the motion model;

[0033] S502, perform Hungarian matching between the obstacle prediction trajectory obtained in S501 and the obstacle detection in the current frame; for the matched trajectory and detection results, perform motion state integration using the state transition matrix, and update using Kalman filtering in combination with the observation results of the current frame.

[0034] S503: For the trajectory of an obstacle that existed in a previous frame but was not matched, mark its mismatch time. If the mismatch time exceeds the threshold, delete the trajectory. For the obstacle that exists in the current frame but was not matched with a previous trajectory, mark its existence time. If the existence time exceeds the threshold, add the obstacle as a new obstacle to the queue.

[0035] The technical advantages of this invention lie in its ability to accurately identify wall obstacles in different scenarios using RANSAC and Euclidean clustering, and the use of tracking filtering to ensure detection stability, providing reliable observation for subsequent path planning. This algorithm can operate in poorly lit tunnels and under adverse conditions where water mist obscures the operator's vision, ensuring obstacle avoidance safety during vehicle operation and improving work efficiency. It is of great significance for realizing unmanned and efficient operation of large, heavy-duty equipment. Attached Figure Description

[0036] Figure 1 This is a schematic diagram of the process of the present invention.

[0037] Figure 2 This is the original elevation point cloud map collected in an embodiment of the present invention.

[0038] Figure 3 This is a point cloud map showing the obstacle detection process in an embodiment of the present invention.

[0039] Figure 4 This is a cloud map of detection points for facade obstacles in an environment with water mist, as described in this invention. Detailed Implementation

[0040] The present invention will now be described in further detail with reference to specific embodiments and the accompanying drawings.

[0041] The steps in this embodiment are as follows: Figure 1 As shown. Here it is... Figure 2 Taking a frame of the facade work scene shown as an example, the facade obstacle detection method based on 3D LiDAR described in this example includes the following steps:

[0042] S1, calculate the plane normal vector, calculate the deviation between the true normal vector and the calculated normal vector, and perform a rotation transformation;

[0043] S101. Select a scene with a plane for LiDAR scanning. Select a portion of the point cloud on the plane based on the distance, and calculate the normal vector of this portion of the point cloud as the normal vector of the plane. The specific steps are as follows:

[0044] 1) Calculate the mean from the three coordinate dimensions to obtain the centroid coordinates of the selected point cloud, and calculate the covariance matrix based on the centroid;

[0045] 2) Calculate the eigenvalues ​​and eigenvectors of the covariance matrix, and select the eigenvector corresponding to the smallest eigenvalue as the plane normal vector;

[0046] S102: Calculate the angle between the projection of the plane normal vector onto the coordinate axis plane and the true normal vector of the coordinate axis plane, and take the average value of the results from multiple frames;

[0047] S103: Use pass-through filtering to remove point clouds with coordinates greater than a threshold, and perform rotation transformation according to this parameter in the processing of each frame.

[0048] S2, based on the data uploaded by the industrial control computer, use a pass-through filter to extract the area where the obstacle is located, and divide the area into grids with the wall as the projection surface. Within each grid, remove the small part of the points closest to the radar based on the distance from the radar.

[0049] S201: Based on the data uploaded by the industrial control computer, the coordinate direction is filtered to obtain the point cloud of the area where the obstacle is located. If the number of points in the filtered point cloud is less than a certain proportion of the original point cloud, it is considered a non-working area, an abnormal information is thrown, and the perception process is restarted.

[0050] S202: Divide the grid in the XOZ plane, and the grid shape is a square with a given side length parameter;

[0051] S203: Traverse all grids and sort the points in each grid in ascending order of their coordinates. If the coordinates of an element in the sorted array differ from the coordinates of the element in the previous position by more than a threshold, remove the points after that position in that grid; otherwise, do nothing.

[0052] S3: Extract and filter out wall point clouds based on point cloud coordinate features to obtain obstacle point clouds; if the wall is curved, detect it in segments along the curvature direction.

[0053] S301, Separate the wall point cloud based on the point cloud coordinate features, and perform multiple separation operations until the number of remaining point clouds is less than a certain proportion of the original point cloud. The specific steps are as follows:

[0054] 1) Randomly select multiple data points in the original point cloud and determine whether they are collinear. If they are collinear, reselect the data points; otherwise, calculate the plane equation of the plane P formed by the data points.

[0055] 2) Calculate the distance d from each point in the original point cloud to plane P;

[0056] 3) Set a threshold t. When d>t, it is considered invalid data and marked as invalid point. Otherwise, it is valid data and marked as valid data. After marking all data points, count the number of valid data points.

[0057] 4) Repeat steps 1-3 with k iterations. Select the plane with the most valid data from the k planes formed in the iterations; this plane is the fitting plane for the wall point cloud.

[0058] S302: If, during the separation process, there are two planes whose distance exceeds a threshold and the number of point clouds on the distant plane exceeds a certain proportion of the original point cloud, then the point cloud on the nearby plane is retained as a new working surface.

[0059] S303: For scenes with curved walls, obstacles are separated in segments along the curvature direction based on point cloud coordinate features, and finally the results of all segments are merged; to speed up the processing, voxel downsampling is used to reduce the number of point clouds before merging.

[0060] S4: Filter the obstacle point cloud obtained in S3, count the number of obstacles, and use the size of the largest bounding box of the obstacle as the size of the obstacle; merge adjacent obstacles with a distance less than the threshold.

[0061] S401: Select a corner point P from the obstacle point cloud obtained in S3. i Search point P using a KD tree i Find the k nearest neighbors of a point, calculate the centroid of the set formed by the point and its nearest neighbors, calculate the distance from each point to the centroid, and calculate the mean and variance. Points in the set whose distance from the centroid exceeds the threshold are considered noise points and are removed.

[0062] S402: Apply a clustering algorithm to the point set {P} obtained in S401 to obtain the point set {Ps} for each obstacle. The specific clustering algorithm steps are as follows:

[0063] 1) Construct a KD tree for the point set {P}, and randomly select a point P in the point set {P}. i And search for point P based on the KD tree. i k nearest neighbors;

[0064] 2) Solve for the nearest neighbor point to P i The distance is calculated and compared with a threshold. Points with a distance less than the threshold are marked as valid points of the same obstacle. Based on this point, k nearest neighbor points are searched until there are no points among the nearest neighbors with a distance less than the threshold. If the final number of valid points is less than the set threshold, it is not considered an obstacle.

[0065] 3) Repeat steps 1)-2) until all points have been clustered.

[0066] S403: Calculate the extreme value difference of each obstacle in each coordinate direction as its length, width and height. For adjacent obstacles whose distance in the forward direction is less than the threshold, merge them and recalculate their length, width and height as a new obstacle.

[0067] S5 establishes tracking for each obstacle, performs Hungarian matching between consecutive frames, and updates the filtering of successfully matched obstacles; previously existing but unmatched obstacles are predicted in position based on the motion model, and the obstacle is deleted if the number of consecutive matching failures exceeds a threshold; newly appearing obstacles are given a new trajectory.

[0068] S501: If it is not the starting frame, then the obstacles detected in the previous frame are predicted according to the motion model. The motion modeling process is as follows:

[0069] 1) Approximate the motion between two adjacent frames as uniform motion, and the equation of motion is:

[0070] X k+1 =FX k +w k

[0071] Where X k The target state represents the coordinates of the target's center point in the (x, y, z) coordinate dimensions and its velocity along the Z direction, w. k Let F be a Gaussian white noise sequence with zero mean, and let F be the state transition matrix.

[0072] 2) Use formula X k+1 =FX k Predict obstacles detected in the previous frame;

[0073] S502: Perform Hungarian matching between the predicted trajectory obtained in S501 and the obstacle detection results of the current frame; the specific steps are as follows:

[0074] 1) Calculate the distance between each pair of obstacles based on the vector formed by the length, width, height, and center point coordinates of the obstacles, and construct the cost matrix M;

[0075] 2) Initialize the matching set to empty, traverse the predicted trajectory Tr, and perform augmented path search for each trajectory starting with the obstacle corresponding to the smallest element value in the cost matrix M;

[0076] 3) If an augmenting path is found, proceed to the next search from that path; otherwise, keep that path as a match.

[0077] 4) After traversing all the trajectories, the resulting match is the final matching result.

[0078] For the matched trajectory and detection results, the state transition matrix F is integrated, and combined with the observation results of the current frame, Kalman filtering is used for updating. The specific steps are as follows:

[0079] 1) Let the observation equation be as follows:

[0080] Y k =HX k +v k

[0081]

[0082] Where Y k Let H be the observation vector (cx, cy, cz), and let H be the observation matrix.k R is a covariance matrix with zero mean. k Gaussian white noise sequence; w k and v k Unrelated;

[0083] 2) Calculate the prior state covariance matrix

[0084] 3) Calculate the Kalman gain

[0085] 4) Calculate the posterior state and the posterior state covariance matrix.

[0086] S503: For unmatched trajectories, i.e., trajectories of obstacles that existed in previous frames but were not matched in the current frame, mark their mismatch time T. death +1, if the mismatch time exceeds the threshold α, then delete the trajectory; for obstacles in the current frame that do not match previous trajectories, mark their existence time T. age +1, if the time exceeds the threshold β, then add the detection to the trajectory queue Tr.

[0087] According to embodiments of the present invention, the present invention also provides an electronic device and a computer-readable medium.

[0088] Electronic devices include:

[0089] One or more processors;

[0090] Storage device for storing one or more programs;

[0091] When the one or more programs are executed by the one or more processors, the one or more processors implement the aforementioned method.

[0092] In practical use, users can interact with servers, which are also electronic devices, via a network to receive or send messages. Terminal devices are generally various electronic devices equipped with a display and used through a human-computer interface, including but not limited to smartphones, tablets, laptops, and desktop computers. Various specific application software can be installed on these terminal devices as needed, including but not limited to web browsers, instant messaging software, social media platforms, and shopping apps.

[0093] A server is a network service provider that offers various services, such as a backend server that provides corresponding calculation services for facade point cloud data received from terminal devices. This allows for the calculation of the received facade point cloud data to obtain the size and location data of facade obstacles, and the return of the final detection data results to the terminal devices.

[0094] The status detection method provided in this embodiment is generally executed by the server. In practical applications, the terminal device can also directly perform status detection under necessary conditions.

[0095] Similarly, the computer-readable medium of the present invention stores a computer program thereon, which, when executed by a processor, implements a facade obstacle detection method based on three-dimensional laser point cloud data features according to an embodiment of the present invention.

Claims

1. A method for detecting facade obstacles based on three-dimensional laser point cloud data features, characterized in that, Includes the following steps: S1. Before the wall-washing truck starts cleaning and moving, the point cloud of the scene where the wall-washing truck is located, including the ground and the wall, is collected by LiDAR. The correct correspondence between the point cloud and the real scene is obtained through external parameter calibration. Then, based on the spatial position transformation relationship between the planes, the inherent parameters used to correct the coordinates of the point cloud are obtained. S2, after the wall-washing truck starts cleaning and moving, each frame of point cloud acquired during the movement is filtered and the coordinates are corrected based on the inherent parameters. The grid is divided according to the distribution. The grid features are used to characterize the point cloud, the region of interest (ROI) is extracted, and point cloud noise is filtered out. Specifically, based on the movement data of the roller brush used for cleaning by the wall washing cart, the actual position of the roller brush is obtained, and the position where the roller brush extends forward along the movement direction of the wall washing cart is taken as the ROI. S3, obtains the point cloud of the wall or facade through feature segmentation and filters it out from the region of interest, thereby obtaining the obstacle point cloud; S4 performs obstacle point cloud clustering, calculates obstacle size and obtains obstacle bounding box information; after the initial clustering is completed, a secondary classification is performed based on the centroid distance to improve obstacle recognition accuracy. S5 establishes tracking for each obstacle, performs matching based on distance between consecutive frames, and updates the matched obstacles using Kalman filtering; for different matching results, each obstacle is created, merged, or deleted to obtain the obstacle detection result.

2. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 1, characterized in that, Step S1, the steps for obtaining the intrinsic parameters include: S101, Select multiple planes whose Euclidean distance exceeds the threshold, obtain the point cloud on these planes, and calculate the normal vector of this part of the point cloud as the normal vector of these planes; S102, project the normal vectors of these planes onto the coordinate plane, and then calculate the angle between each projected normal vector and the real normal vector; before the wall-washing truck starts cleaning, collect multiple frames of point cloud data using a lidar, and take the average value of the angle results from multiple frames as the inherent parameter.

3. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 2, characterized in that, In step S101, calculating the normal vector of this part of the point cloud includes the following steps: 1) Calculate the mean of the point cloud coordinates from the three coordinate dimensions in the XYZ coordinate system, obtain the centroid coordinates of the selected point cloud, and calculate the covariance matrix based on the centroid. 2) Calculate the eigenvalues ​​and eigenvectors of the covariance matrix, and select the eigenvector corresponding to the smallest eigenvalue as the normal vector.

4. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 2, characterized in that, Step S2, extracting the region of interest (ROI) includes the following steps: S201: In each frame of point cloud, a pass-through filter is used to remove point clouds with coordinates greater than a threshold, and a rotation transformation is performed according to inherent parameters to obtain the point cloud of the ROI. S202, Divide the grid in the XOZ plane, where the grid shape is a square with a given side length parameter; S203: Traverse all grids and sort all points in the point cloud of each grid in ascending order of coordinates. If the coordinates of an element in the sorted array differ from the coordinates of the element in the previous position by more than a threshold, remove the points after that position in the grid; otherwise, do not process them.

5. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 4, characterized in that, In step S201, after obtaining the point cloud of the ROI using the filtering algorithm, if the number of points obtained is lower than the preset ratio threshold compared to the original point cloud, the point cloud of each plane, including the ground and the wall, of the current scene of the wall washing vehicle is re-collected, and the inherent parameters are re-acquired before step S2 is executed again.

6. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 3, characterized in that, Step S3 includes: S301, use the random sampling consistency method to separate the wall point cloud according to the plane fitting method of minimizing Euclidean distance, mark the identified plane point cloud; and repeat the process multiple times until the number of remaining point clouds is less than the preset ratio of the original point cloud, then treat the remaining point cloud as obstacle point cloud.

7. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 6, characterized in that, When step S301 is completed, if two planes are determined to exist based on geometric features, then when the distance between the two planes exceeds a threshold, and the number of point clouds on the plane that is farther from the origin of the lidar exceeds the corresponding proportion of the number of point clouds in the original plane, the point cloud on the closer plane is retained as a new working surface; otherwise, it is still considered to be the same plane.

8. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 6, characterized in that, When performing step S301, if a curved wall is encountered, the curved wall is segmented along the curvature direction, and step S301 is performed on each segment to separate the obstacle. After all segments have separated the obstacle, voxel-based downsampling is used to reduce the number of point clouds, and finally the results of all segments are merged.

9. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 6, characterized in that, In step S4, the obstacle dimensions are calculated, including: S401, Filter the obstacle point cloud obtained in step S3, select the corner point on the upper left side of the obstacle near the lidar as the starting point Pi, search for the k nearest neighbors of point Pi based on the point cloud distance, calculate the centroid of the set formed by Pi and the k nearest neighbors, calculate the distance of all points in the set to the centroid, and then calculate the mean and variance. Points in the set whose distance from the centroid exceeds the threshold are regarded as noise points and removed. S402, use a clustering algorithm on the point set {P} obtained in S401 to obtain the point set {Ps} of each obstacle; S403: Calculate the absolute value of the difference between the two extreme values ​​of each obstacle in the three coordinate directions, and use it as the length, width and height of the obstacle. For adjacent obstacles whose distance in the direction of the wall washer is less than the threshold, merge them and recalculate the length, width and height as a new obstacle.

10. The method for detecting facade obstacles based on three-dimensional laser point cloud data features according to claim 6, characterized in that, Step S5 includes: S501, If ​​it is not the starting frame, then the obstacles detected in the previous frame will be predicted according to the motion model; S502, perform Hungarian matching between the obstacle prediction trajectory obtained in S501 and the obstacle detection in the current frame; for the matched trajectory and detection results, perform motion state integration using the state transition matrix, and update using Kalman filtering in combination with the observation results of the current frame. S503: For the trajectory of an obstacle that existed in a previous frame but was not matched in the current frame, mark its mismatch time. If the mismatch time exceeds the threshold, delete the trajectory. For the obstacle that exists in the current frame but was not matched in a previous trajectory, mark its existence time. If the existence time exceeds the threshold, add the obstacle as a new obstacle to the queue.

Images