A clustering-based method for dynamic obstacle filtering in laser data

Abstract

The present invention relates to a method for filtering dynamic obstacles of laser data based on clustering, comprising the following steps: S1, obtaining laser data and preprocessing; S2, clustering according to the laser data to complete the extraction of obstacles; S3, associating obstacles on the time axis according to the similarity of obstacles extracted from adjacent frames, realizing obstacle tracking, and obtaining the relative speed of obstacles in the laser sensor coordinate system; S4, obtaining the movement speed of the mobile robot in the map coordinate system, and judging the dynamic properties of the obstacles in the static environment in combination with the relative speed of the obstacles; S5, filtering out the laser data points corresponding to the dynamic obstacles in the laser data. Compared with the prior art, the present invention does not need to obtain visual information with the help of a camera, avoids additional recognition of dynamic obstacles, and realizes effective filtering of dynamic obstacles in laser data through flexible adjustment of relevant parameters.

Description

Technical Field

[0001] The invention relates to the field of mobile robots, and in particular to a clustering-based laser data dynamic obstacle filtering method. Background Art

[0002] Mobile robots are a widely-focused area of ​​research, involving a variety of technologies including SLAM, path planning, and obstacle avoidance. Considering the actual application scenarios, most of them have some dynamic obstacles. If dynamic obstacles are included in the map obtained by SLAM, and the instantaneous position state information of dynamic obstacles detected by sensors is represented in the map as fixed information, the relevant environmental information will be inaccurate, which will bring unnecessary troubles to subsequent path planning and obstacle avoidance.

[0003] Currently, most of the existing methods for extracting and tracking dynamic obstacles in the environment are based on the visual information provided by the camera. Based on the visual information, objects in the environment can be identified to obtain information such as the type of object and its position within the visual range, but this technical method generally has a large amount of calculation. In addition, for mobile robots in some indoor scenes, they can mainly rely on laser data to complete mapping, path planning, obstacle avoidance and other technologies. If the visual information provided by the camera is required to filter out dynamic obstacles, it is also necessary to complete the correspondence and calibration of laser data and visual information, which is complicated. Summary of the invention

[0004] The purpose of the present invention is to provide a clustering-based laser data dynamic obstacle filtering method, which completes obstacle filtering through laser data without the need for additional camera sensors, has a small amount of calculation, a fast calculation speed, and a simple and efficient method.

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

[0006] A clustering-based laser data dynamic obstacle filtering method comprises the following steps:

[0007] S1, acquiring laser data and preprocessing, wherein the laser data transmission and collection device is a laser sensor, and the laser sensor is arranged on the mobile robot;

[0008] S2. Clustering is performed based on the laser data to complete the extraction of obstacles;

[0009] S3, according to the similarity of obstacles extracted from adjacent frames, the obstacles are associated on the time axis to achieve obstacle tracking and obtain the relative speed of the obstacles in the laser sensor coordinate system;

[0010] S4, obtaining the movement speed of the mobile robot (i.e., the laser sensor) in the map coordinate system, and judging the dynamic properties of the obstacle in the static environment in combination with the relative speed of the obstacle;

[0011] S5. Filter out laser data points corresponding to dynamic obstacles in the laser data.

[0012] The preprocessing is specifically as follows: according to the actual ranging range parameter R of the laser sensor min and R max As well as the application scenario requirements, determine the effective laser ranging range, filter the laser data, and use the laser points within the effective laser ranging range as input for subsequent steps.

[0013] The step S2 comprises:

[0014] S21, based on the distance between laser points in a frame of laser data, points with similar distances are classified into the same point set by clustering, and each point set is approximately fitted with a line segment;

[0015] S22, merging the fitted line segments based on the concentrated distribution of the spatial positions;

[0016] S23. Based on the description of the merged line segments, a circle is used to describe the obstacle instead, and a new circle is constructed for the two intersecting circles based on the line segment connecting the centers of the circles. If the radius of the new circle is less than a preconfigured threshold, the two intersecting circles are replaced with the new circle for description, otherwise no replacement is performed.

[0017] When clustering the laser point cloud, d ij <d group +R i The point clouds of d are classified into the same point set to complete the initial division, where d ij is the distance between two laser points in the point cloud, d group is the object clustering distance parameter, R i d is the dynamic clustering distance compensation parameter, R i is the distance information obtained by the laser sensor, and d is the distance ratio parameter;

[0018] In order to avoid unreasonable intra-class spacing causing a point set to contain too large a range and include multiple objects, if the point set obtained by the initial division satisfies d max >d split +R j d, then the point set is split, where d max is the maximum distance between a point in the initial divided point set and the line connecting the two farthest points in the point set, d split is the split distance parameter, R j d is the dynamic clustering distance compensation parameter.

[0019] The least square method is used to approximate the line segment for each point set.

[0020] When merging the fitted line segments, the distance between the two line segments to be merged is considered and the approximate collinearity is judged. When the distance between the two line segments to be merged is less than the preconfigured threshold and meets the approximate collinearity condition, the two line segments are regarded as concentrated distribution, the corresponding point sets are merged, and approximate fitting is performed with a line segment based on the merged point set to complete the merger.

[0021] Based on the line segment description, a circle is used to describe the specific processing method instead:

[0022] An equilateral triangle is formed with the line segments as sides, and the center of the triangle is kept away from the origin of the laser frame;

[0023] A circle is constructed so that the equilateral triangle is inscribed in the circle, and the radius of the circle is increased according to the distribution of the point set.

[0024] The step S3 is specifically as follows:

[0025] S31, calculating the similarity cost of obstacles in adjacent frames based on obstacle information described by circles in two adjacent frames:

[0026]

[0027] Among them, c ij is the similarity cost between circle i and circle j in two adjacent frames, (x0, y0) is the position of the center of the circle, and r is the radius of the circle;

[0028] S32, for each obstacle, the obstacles are associated based on the obstacles corresponding to the minimum similarity cost in adjacent frames to achieve obstacle tracking;

[0029] S33. Combine the laser sensor frequency information and estimate the relative speed of the obstacle in the laser sensor coordinate system based on Kalman filter iteration.

[0030] The step S4 is specifically as follows: solving the movement speed of the mobile robot (i.e., the laser sensor) in the map coordinate system by means of sensor information fusion of the odometer and the IMU; determining the dynamic properties of the obstacle in the static environment in combination with the relative speed of the obstacle in the laser sensor coordinate system, and marking the dynamic obstacle.

[0031] The step S5 specifically includes filtering out the laser data points within the description circle corresponding to the dynamic obstacles marked as being present, thereby filtering out the dynamic obstacles in the laser data.

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

[0033] (1) In related technologies that rely on laser sensors as the main sensors, such as laser SLAM, path planning, etc., when they are applied in an environment with dynamic obstacles, the method described in the present invention does not require additional camera sensors, nor does it require processing of relatively complex visual information. It can extract and track dynamic obstacles by processing laser frame data, and finally effectively filter the laser data to reduce the impact of dynamic obstacles on laser SLAM and path planning.

[0034] (2) The present invention uses line segments and circles to fit and describe dynamic obstacles in each frame of laser data. The obstacle extraction method is simple, intuitive, and efficient, with good extraction effect, small calculation amount, and fast calculation speed.

[0035] (3) The present invention includes multiple adjustable parameters, which can be flexibly adjusted according to actual scenarios and specific needs, so that the present invention can achieve a dynamic obstacle filtering effect that meets the needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 The figure is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0037] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0038] A clustering-based method for filtering dynamic obstacles in laser data, such as Figure 1 As shown, the following steps are included:

[0039] S1, obtain laser data and perform preprocessing;

[0040] The laser data transmission and collection device is a laser sensor, and the laser sensor is arranged on the mobile robot.

[0041] The preprocessing is specifically as follows: according to the actual ranging range parameter R of the laser sensor min and R max (According to the laser sensor model, refer to relevant information) and application scenario requirements (such as further considering the size of the environmental range and the number of laser point cloud density requirements, etc.), determine the effective laser ranging range, filter the laser data, and use the laser points within the effective laser ranging range as input for subsequent steps. In one embodiment, due to installation problems, the laser point greater than R min There will be noise in the range, so it is necessary to increase R min In another embodiment, close to R max When the point cloud is too sparse, you can reduce R maxAdjust according to the actual application requirements and filter out the available results according to the smaller ranging range from the obtained ranging results.

[0042] S2. Clustering is performed based on the laser data to complete the extraction of obstacles;

[0043] S21, based on the distance between laser points in a frame of laser data, points with similar distances are classified into the same point set by clustering, and each point set is approximately fitted with a line segment;

[0044] When clustering laser point clouds, the following factors need to be considered for the inter-class distance: the distance d between two laser points in the point cloud ij , object clustering distance parameter d group Considering the characteristic that the farther the laser point cloud is measured, the sparser it is, that is, the larger the distance between laser points, the dynamic clustering distance compensation parameter R is set. i d, with laser ranging value R i increases with the increase of the distance ratio parameter, d group , d can be adjusted according to the actual application environment.

[0045] When clustering the laser point cloud, d ij <d group +R i The point cloud of d is classified into the same point set to complete the preliminary division.

[0046] Considering that the cluster point set obtained by the above method may be large, there is a situation where multiple objects with close distances are regarded as one object in the clustering process. Further, the point sets obtained by clustering according to the above method are split. First, the two points with the farthest distance in the point set obtained by the preliminary division are obtained and connected by a line, and then the farthest distance d between the point in the point set and the connecting line is obtained. max , correspondingly, the point is regarded as the farthest point. If the distance is too large, it satisfies d max >d split +R j d, then the point set is further split, d split is the split distance parameter, R j d is the dynamic clustering distance compensation parameter, which can be adjusted according to actual conditions.

[0047] According to the above splitting process, if the farthest point in the point set meets the further splitting conditions, the point set can be bounded by the farthest point, and the points on both sides of the farthest point can form new point sets. The above splitting process is recursively repeated for each point set until each point set does not meet the splitting conditions.

[0048] Furthermore, a least squares line segment is fitted for each point set, that is, the sum of the distances from each point in the point set to the fitting line segment is minimized.

[0049] S22, in order to use one line segment to describe an object as much as possible, the fitted line segments are merged based on the concentrated distribution of spatial positions;

[0050] When merging the fitted segments, the closest distance d0 between the adjacent endpoints of the two segments is first considered. If it is less than the set merging threshold d merge , we further consider fitting the point sets corresponding to the two line segments using the least squares method to obtain new line segments, and then calculate the maximum value of the distances from the endpoints of the two line segments to the new line segments (i.e., to determine the approximate collinearity). If the maximum value is still less than the set threshold d s , then the approximate collinearity condition is satisfied, and the two line segments are regarded as concentrated distributions, and the two line segments are merged, that is, the corresponding two point sets are merged and represented by a new line segment. In addition, in order to exclude environmental edges such as walls, it is also necessary to limit the length of the obtained line segments, and limit the length greater than the custom length threshold d max Finally, all the line segments obtained by processing a frame of laser data are stored and retained in a set.

[0051] S23. Based on the description of the merged line segments, a circle is used to describe the obstacle instead, so as to include all the laser point information corresponding to the obstacle with a relatively small description cost, thereby facilitating the positioning and restriction of the laser point; and a new circle is constructed for the two intersecting circles based on the line segment connecting the centers of the circles. If the radius of the new circle is less than a pre-configured threshold, the two intersecting circles are replaced by the new circle for description, otherwise no replacement is performed.

[0052] Compared with the line segment description, the circle can contain all the point data corresponding to the obstacle, which is convenient for clarifying the corresponding relationship of the laser points after clustering; compared with the point set description obtained by clustering, the circle can locate and limit the laser points corresponding to the obstacles within the range only by relying on the center position and radius of the circle, which is convenient for tracking.

[0053] Based on the line segment description, a circle is used to describe the specific processing method instead:

[0054] An equilateral triangle is formed with the line segments as sides, and the center of the triangle is kept away from the origin of the laser frame;

[0055] Construct a circle so that the equilateral triangle is inscribed in the circle, and appropriately increase the circle radius r according to the distribution of the point set d .

[0056] Furthermore, considering the occlusion and overlap between obstacles, the intersecting circles are appropriately merged. Connect the centers of the two intersecting circles and regard them as line segments. Use the circle description as a replacement method based on the line segment description to construct a new circle, obtain the center position, and then increase the radius of the circle by the radius length of the larger circle of the two intersecting circles. Even if the new circle is inscribed in the circle with the larger radius, if the radius length is less than Then replace the two intersecting circles with new circles for description.

[0057] S3, according to the similarity of obstacles extracted from adjacent frames, the obstacles are associated on the time axis to achieve obstacle tracking and obtain the relative speed of the obstacles in the laser sensor coordinate system;

[0058] S31, for each circle c in the laser frame i (i∈{1,...,n k}) and each circle c in the adjacent previous laser frame j (j∈{1,...,n k-1}) Calculate the similarity cost c ij :

[0059]

[0060] Among them, c ij is the similarity cost between circle i and circle j in two adjacent frames, (x0, y0) is the position of the circle center, and r is the radius of the circle; the smaller the calculated cost, the higher the similarity. The following cost matrix C can be obtained from this calculation:

[0061]

[0062] Each row corresponds to the cost calculation of a new obstacle for all old obstacles, and each column corresponds to the cost calculation of an old obstacle for all new obstacles.

[0063] S32, for each obstacle, the obstacles are associated based on the obstacles corresponding to the minimum similarity cost in adjacent frames to achieve obstacle tracking;

[0064] Take the minimum value of each column of the cost matrix and save the corresponding subscript to get n old_i , take the minimum value of each row and save the corresponding subscript to get n new_j , where if the minimum value is greater than the set threshold, the corresponding value will be marked as -1. This is further divided into four cases:

[0065] (1) No correlation, that is, n new_j = -1, thus not corresponding to the obstacle;

[0066] (2) One-to-one association, that is, n new_j =i;

[0067] (3) One-to-many association, that is, n old_a =j and n old_b =j, where a≠b, corresponding to the merging of obstacles;

[0068] (4) Many-to-one association, that is, n new_c =i and n new_d =i, where c≠d, corresponding to the splitting that can be considered as an obstacle.

[0069] Among them, the last three situations can all realize the tracking of obstacles.

[0070] S33. Combine the laser sensor frequency information and estimate the relative speed of the obstacle in the laser sensor coordinate system based on Kalman filter iteration.

[0071] After tracking the obstacle in the time correlation of the adjacent frames, the obstacle speed is further solved. Here, the Kalman filter method is used to achieve the optimal estimation of the state for each obstacle. First, the state equation is defined:

[0072]

[0073] For each obstacle, a state variable is defined. State transition matrix in t P is the sampling interval between two laser frames, which is related to the frequency of the laser sensor. Based on the above clustering, the obstacle position and radius information can be obtained by combining each frame of laser data, and the corresponding observation equation is defined as:

[0074]

[0075] The observation matrix

[0076] The time update and state update equations involved in the iterative derivation of the Kalman filter are respectively specified as follows in this problem:

[0077]

[0078]

[0079] Get the predicted value in the time update phase Update forecast value and the true value The covariance between Calculate the Kalman filter gain K in the state update phase k , and then calculate the correction value of the state quantity And update the covariance Pk , which is used for the next recursive deduction. Through the continuous iteration of the two steps of time update and state update by the Kalman filter, the speed information of each obstacle in the laser sensor coordinate system can be approximately obtained. The above one-to-one association and many-to-one association can be solved by the above method. For one-to-many association, the average value of the corresponding speeds can be used to approximate the speed of the new obstacle.

[0080] S4, obtaining the movement speed of the mobile robot (i.e., the laser sensor) in the map coordinate system, and judging the dynamic properties of the obstacle in the static environment in combination with the relative speed of the obstacle;

[0081] S41, fusing the sensor information of the odometer and the IMU through the extended Kalman filter to determine the position information and moving speed of the mobile robot in the map coordinate system; when the laser sensor is fixed on the mobile robot, the speeds of the two are the same and no further coordinate system conversion is required, so the moving speed of the laser sensor in the map coordinate system can be obtained;

[0082] The odometer has a simple structure and a high sampling frequency, and can measure the relative position of the mobile robot well in a short time. The IMU can obtain the posture information of the mobile robot through secondary integration of the three-axis accelerometer and the three-axis angular accelerometer. It has high accuracy in a short time and is suitable for providing posture estimation for the mobile robot.

[0083] S42. Taking the movement speed of the mobile robot (ie, the laser sensor) as a reference and combining the relative speed of the obstacle, determine the dynamic properties of the obstacle in the static environment (ie, whether it is a dynamic obstacle), and mark the dynamic obstacle.

[0084] S5. For each laser frame, filter out laser data points within the description circle corresponding to the dynamic obstacles, thereby filtering out the dynamic obstacles in the laser data.

[0085] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning, or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A clustering-based laser data dynamic obstacle filtering method, characterized in that: The following steps are involved: S1, acquiring laser data and preprocessing, wherein the laser data transmission and collection device is a laser sensor, and the laser sensor is arranged on the mobile robot; S2. Clustering is performed based on the laser data to complete the extraction of obstacles; S3, according to the similarity of obstacles extracted from adjacent frames, the obstacles are associated on the time axis to achieve obstacle tracking and obtain the relative speed of the obstacles in the laser sensor coordinate system; S4, obtaining the movement speed of the mobile robot in the map coordinate system, and judging the dynamic properties of the obstacle in the static environment in combination with the relative speed of the obstacle; S5, filtering out laser data points corresponding to dynamic obstacles in the laser data; The step S2 comprises: S21, based on the distance between laser points in a frame of laser data, points whose distance is less than a pre-configured threshold are classified into the same point set by clustering, and each point set is approximated by a line segment; S22, merging the fitted line segments based on the concentrated distribution of the spatial positions; S23, based on the description of the merged line segments, a circle is used to describe the obstacle instead, and a new circle is constructed for the two intersecting circles based on the line segment connecting the centers of the circles. If the radius of the new circle is less than a pre-configured threshold, the two intersecting circles are replaced with the new circle for description, otherwise no replacement is performed; Based on the line segment description, a circle is used to describe the specific processing method instead: An equilateral triangle is formed with the line segments as sides, and the center of the triangle is kept away from the origin of the laser frame; Constructing a circle so that the equilateral triangle is inscribed in the circle, and increasing the radius of the circle according to the distribution of the point set; The step S3 is specifically as follows: S31, calculating the similarity cost of obstacles in adjacent frames based on obstacle information described by circles in two adjacent frames: Among them, c ij is the similarity cost between circle i and circle j in two adjacent frames, (x0, y0) is the position of the center of the circle, and r is the radius of the circle; S32, for each obstacle, the obstacles are associated based on the obstacles corresponding to the minimum similarity cost in adjacent frames to achieve obstacle tracking; Take the minimum value of each column of the cost matrix and save the corresponding subscript to get n old_i , take the minimum value of each row and save the corresponding subscript to get n new_j , where if the minimum value is greater than the set threshold, the corresponding value will be marked as -1; this is further divided into four cases: 1) No correlation, that is, n new_j = -1, no response to the obstacle; 2) One-to-one association, that is, n new_j =i; 3) One-to-many association, that is, n old_d =j and n old_b =j, where a≠b, corresponding to the merging of obstacles; 4) Many-to-one association, that is, n new_c =i and n new_d =i, where c≠d, corresponding to the splitting considered as an obstacle; Among them, the tracking of obstacles is achieved through the last three situations; S33. Combine the laser sensor frequency information and estimate the relative speed of the obstacle in the laser sensor coordinate system based on Kalman filter iteration.

2. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: The preprocessing is specifically as follows: according to the actual ranging range parameter R of the laser sensor min and R max As well as the application scenario requirements, determine the effective laser ranging range, filter the laser data, and use the laser points within the effective laser ranging range as input for subsequent steps.

3. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: When clustering the laser point cloud, d ij <d group +R i The point clouds of d are classified into the same point set to complete the initial division, where d ij is the distance between two laser points in the point cloud, d group is the object clustering distance parameter, R i d is the dynamic clustering distance compensation parameter, R i is the distance information obtained by the laser sensor, and d is the distance ratio parameter; In order to avoid unreasonable intra-class spacing causing a point set to contain too large a range and include multiple objects, if the point set obtained by the initial division satisfies d max >d split +R j d, then the point set is split, where d max is the maximum distance between a point in the initial divided point set and the line connecting the two farthest points in the point set, d split is the split distance parameter, R j d is the dynamic clustering distance compensation parameter.

4. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: The least square method is used to approximate the line segment for each point set.

5. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: When merging the fitted line segments, the distance between the two line segments to be merged is considered and the approximate collinearity is judged. When the distance between the two line segments to be merged is less than the preconfigured threshold and meets the approximate collinearity condition, the two line segments are regarded as concentrated distribution, the corresponding point sets are merged, and approximate fitting is performed with a line segment based on the merged point set to complete the merger.

6. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: The step S4 is specifically as follows: solving the movement speed of the mobile robot in the map coordinate system by means of sensor information fusion of the odometer and the IMU; determining the dynamic properties of the obstacle in the static environment in combination with the relative speed of the obstacle in the laser sensor coordinate system, and marking the dynamic obstacle.

7. The method for dynamic obstacle removal of laser data based on clustering according to claim 1, characterized in that: The step S5 specifically includes filtering out the laser data points within the description circle corresponding to the dynamic obstacles marked as being present, thereby filtering out the dynamic obstacles in the laser data.

Images